Motion Planner Restructure

[marip8]

Various proposals for improving understandability/modularity of the motion planner packages:

• Combine core elements of tesseract_motion_planners and tesseract_process_managers into a single package
  • Interface definitions (motion planner, profile, task, task generator, task flow generator, etc.)
  • Common planning objects (profile dictionary, planning server, etc.)
  • Generic implementations of interfaces (simple planner?, graph task flow generator, etc.)
• Create separate packages for planner implementations (simple?, Descartes, OMPL, TrajOpt)
• Move task and task flow generation code out of tesseract_process_managers into planner specific packages

[Levi-Armstrong]

I am on board.

[Levi-Armstrong]

We had a meeting this week and I identified some items to pre-restructure to hopefully simplify the restructuring. There several things that complicate the current motion planning framework and the following are few things that should simplify things.

1. The MoveInstruction and PlanInstruction are identical. The plan is to remove the plan instruction and only use the move instruction.
2. The second is the program and seed have two different structures. This was to distinguish between the origin program instructions and interpolated instruction. The plan is to make the seed be a flat structure where the interpolated instructions are a different type to allow distinguishing between the two. The InterpolatedMoveInstruction will derive from the plan instruction.
3. The second is each task operates on a single set of data or a subset of that data. A data storage structure will be implemented so each task operated on a single composite and outputs a single composite structure.

[Levi-Armstrong]

@marip8, @marrts, @mpowelson I have been trying to update the type erasure implementation of the instruction and waypoint to support similar capability of inheritance but with everything I have tried I end up getting weird runtime crashes. Do any of you have any objections to switching this to use inheritance? The base classes already exist within the type erasure definition so I do not think it will be a significant effort.

[mpowelson]

What do you mean by similar capability of inheritance? Are you saying that instead of making completely new instruction types you want the ability to inherit from existing ones?

In general, I don't have any major objection if you have compelling reasons to do it. This will be a bit full circle. I seem to remember type erasure cleaning up a lot of things in the data storage when you switched to that, but maybe that isn't such a big deal if we move towards a centralized data store.

[Levi-Armstrong]

What do you mean by similar capability of inheritance? Are you saying that instead of making completely new instruction types you want the ability to inherit from existing ones?

Right now you cannot use your own CartesianWaypoint, PlanInstruction, etc. Currenlty our type erasure functions the same way as std::any which uses type index to determine if you can cast to a type which does not work for derived types. The following test would fail because the type index stores the type that was stored in the type erasure type. Then if you call the .as<> using the base or derived type opposite of what was used when assigning the type erasure it would fail. The casting has to use a static_cast which is why I added the type index because it was very hard to track down if a incorrect type was assigned to to the type erasure. If the use of type index was remove this derived types would work, but there is now checking now if an incorrect type was assigned.

https://github.com/Levi-Armstrong/tesseract_planning/blob/36641b17c8cde43e7435c01361ae6772c4728c4c/tesseract_command_language/test/cartesian_waypoint_unit.cpp#L56-L64

[Levi-Armstrong]

Well it might be possible. I found the following.

https://akrzemi1.wordpress.com/2013/12/06/type-erasure-part-ii/
http://stlab.adobe.com/poly_8hpp_source.html

[Levi-Armstrong]

@marip8 Now that we only have the MoveInstruction and the only difference between the input composite and the result composite is the waypoint is a StateWaypointPoly. I propose updating the MoveInstruction to have a InputWaypoint and OutputWaypoint to eliminate the need to have both a input and output data structure. This will reduce coping a lot of data when the only thing changing is the waypoint. Also it is nice that the input is located with the result to avoid searching a separate data structure. What do you think?

  WaypointPoly& getInputWaypoint();
  const WaypointPoly& getInputWaypoint() const;
  void assignInputCartesianWaypoint(CartesianWaypointPoly waypoint);
  void assignInputJointWaypoint(JointWaypointPoly waypoint);
  void assignInputStateWaypoint(StateWaypointPoly waypoint);

  StateWaypointPoly& getOutputWaypoint();
  const StateWaypointPoly& getOutputWaypoint() const;
  void assignOutputWaypoint(StateWaypointPoly waypoint);