Add path profile to plan and move instruction and modify simple plan profile interface

[Levi-Armstrong]

This adds a path profile for both move and plan instruction to allows using different costs/constraints for the path to the waypoint versus the waypoint itself.

Updates the simple planners interface to provide next instruction.

[marip8]

Does this not duplicate the purpose of the composite instruction profile?

[Levi-Armstrong]

Does this not duplicate the purpose of the composite instruction profile?

I don't think so. The composite profile is a way to apply a set of cost and constraints to all instruction within the composite like velocity, acceleration, and jerk smoothing.

The path profile will be used for two purposes. The first is for the simple planner during the interpolation where it will assign the path profile as the waypoint profile for interpolated waypoints. The second is during execution where it would use the path profile when transitioning and the waypoint profile for the waypoint.

[marip8]

The first is for the simple planner during the interpolation where it will assign the path profile as the waypoint profile for interpolated waypoints.

If we're planning on doing the refactor proposed in #156, it seems like path_profile string would be a parameter that lives in a PlannerProfile or CompositeProfile since it would be applied to all waypoints or all waypoints inside a particular composite.

The second is during execution where it would use the path profile when transitioning and the waypoint profile for the waypoint.

Why couldn't an execution module just use the profile string in the composite instruction that holds a particular instruction for "transitions"?

I'm not necessarily opposed to this change, but it seems like the functionality you're looking for is already captured by the composite instruction profile string and the updated design concept for planner configuration

[Levi-Armstrong]

My original thought for the composite instruction profile was for solver parameters and for cost and constraints which require a series of instructions as input. Then Plan/Move profiles apply specifically to the instruction which would be the same for Path Profile which has been my separation used.

I put the graphic together below showing my comparison where the middle is my desired for the execution where each plan instruction in the original program has different profiles for the transition.

[marip8]

I see. Why don't you just nest each plan instruction in a second layer of composite instructions? That seems like the intuitive way to convey to the simple planner that anything generated for a particular waypoint should be independent of the other instructions. Then you could use the profile of the nested composite as the path profile

[mpowelson]

Looks fine overall to me. I like the concept. My main question would be if we should remove the PlanInstructionType and MoveInstructionType. Is there a case where this profile could not (or should not) be used to encode those types? It seems like this is just a more general case of those.

[Levi-Armstrong]

I see. Why don't you just nest each plan instruction in a second layer of composite instructions? That seems like the intuitive way to convey to the simple planner that anything generated for a particular waypoint should be independent of the other instructions. Then you could use the profile of the nested composite as the path profile

This would create complications at the motion planners due to the previous described separation between the composite profile and plan profile. Currently the OMPL, TrajOpt, Descartes and TrajOptIfopt will all fail if you have nested composites not that this could be changed but it expects a single composite of instructions. The challenge is how do you handle the sub composite instructions profiles relative to using the global composite profile. In the case of trajopt which one applies the velocity smoothing and collision avoidance. I think keeping this separation between the composite profile and plane profile along with only allowing a single composite with instructions as input to the motion planner simplifies things in the motion planners.

[Levi-Armstrong]

In addition, I wanted the command language to align with industrial robot programming and having a two profiles for a plan instructions does seem to align. In the case of Fanuc you have Termination Type and Motion Options where for our plan instruction the profile is the termination type and the path profile is the motion options.

[Levi-Armstrong]

Looks fine overall to me. I like the concept. My main question would be if we should remove the PlanInstructionType and MoveInstructionType. Is there a case where this profile could not (or should not) be used to encode those types? It seems like this is just a more general case of those.

This is a good observation. Based on issue #158, it aligns since the simple planner will be doing the interpolation and allow for more complicated interpolation approaches. Since we usually have different profiles for linear versus freespace move, we would move the linear/freespace type to the simple planner.

[marip8]

Nesting composites does have some tricky implications and it would be complicated to do. Maybe we think harder about that when we start refactoring the other planners. I suppose this is a better short-term alternative, but it feels like there should be a better way to do this. You might consider renaming from path_profile to something like child_profile or downstream_profile. If I understand correctly, the point of this new parameter is that it is the profile assigned children generated from a particular instruction. I think a prefix that describes this relationship more directly would make it more clear. I understand what you mean by the path_ prefix, but it seems like a specific interpretation of its actual purpose

[Levi-Armstrong]

I suppose this is a better short-term alternative, but it feels like there should be a better way to do this. You might consider renaming from path_profile to something like child_profile or downstream_profile. If I understand correctly, the point of this new parameter is that it is the profile assigned children generated from a particular instruction. I think a prefix that describes this relationship more directly would make it more clear. I understand what you mean by the path_ prefix, but it seems like a specific interpretation of its actual purpose

I am not fully understanding the suggested remaining. The path profile (I originally had transition profile) applies during the transition from the previous plan instruction to the current plan instruction where the profile is defined. This applies to anything upstream of the current instruction until the previous plan instruction. Do you like transition profile better?

[marip8]

The path profile (I originally had transition profile) applies during the transition from the previous plan instruction to the current plan instruction where the profile is defined.

Yes, but it is the "current" instruction that controls the profile of any "transition" instruction generated from it (i.e. its children in a sense). It doesn't matter if the generated instructions get placed before or after the instruction used to define their properties, so the prefixes upstream_ and downstream_ aren't the best. Even the "transition" concept you're describing is only one interpretation of how this new parameter could be used. I could write my own motion planner that adds waypoints in a completely different way and still use this parameter to assign their profiles. The common purpose of this new parameter is to be the profile for any instruction that gets generated from the original instruction (regardless of what is done with those new instructions). I think the naming should reflect that (i.e. child_profile or something equivalent), rather than reflecting what is being done with the generated instructions (i.e. making a "transition" or "path" for interpolation purposes)

[Levi-Armstrong]

Yes, but it is the "current" instruction that controls the profile of any "transition" instruction generated from it (i.e. its children in a sense). It doesn't matter if the generated instructions get placed before or after the instruction used to define their properties, so the prefixes upstream_ and downstream_ aren't the best. Even the "transition" concept you're describing is only one interpretation of how this new parameter could be used. I could write my own motion planner that adds waypoints in a completely different way and still use this parameter to assign their profiles. The common purpose of this new parameter is to be the profile for any instruction that gets generated from the original instruction (regardless of what is done with those new instructions). I think the naming should reflect that (i.e. child_profile or something equivalent), rather than reflecting what is being done with the generated instructions (i.e. making a "transition" or "path" for interpolation purposes)

I see your point of view and would agree if the primary intent was for interpolation, but that is not the intent of the path profile. The path profile would still be needed regardless of interpolation because the primary function is to define different set of cost/constraints on the path taken versus the termination waypoint similar to industrial robots. The interpolation is only done because the current motion planners are discrete planners and the interpolated points would still have a need for the path profile. If we every updated trajopt to operate on splines, then interpolation would not be need but the path profile is still needed.

[Levi-Armstrong]

The path profile will eventually replace the plan_type as @mpowelson suggested providing more flexibility instead of the hard coded LINEAR, FREESPACE, and CIRCULAR.

[codecov]

Codecov Report

Merging #159 (483aa2a) into master (4242422) will increase coverage by 0.19%.
The diff coverage is 66.66%.

❗ Current head 483aa2a differs from pull request most recent head 7e19fef. Consider uploading reports for the commit 7e19fef to get more accurate results

@@            Coverage Diff             @@
##           master     #159      +/-   ##
==========================================
+ Coverage   66.33%   66.53%   +0.19%     
==========================================
  Files         199      199              
  Lines        9407     9478      +71     
==========================================
+ Hits         6240     6306      +66     
- Misses       3167     3172       +5     

Impacted Files	Coverage Δ
...lude/tesseract_command_language/move_instruction.h	100.00% <ø> (+33.33%)	⬆️
...lude/tesseract_command_language/plan_instruction.h	100.00% <ø> (ø)
...le/simple_planner_fixed_size_assign_plan_profile.h	100.00% <ø> (ø)
...e/profile/simple_planner_fixed_size_plan_profile.h	100.00% <ø> (ø)
...le/profile/simple_planner_lvs_no_ik_plan_profile.h	100.00% <ø> (ø)
...s/simple/profile/simple_planner_lvs_plan_profile.h	100.00% <ø> (ø)
...n_planners/simple/profile/simple_planner_profile.h	33.33% <0.00%> (-66.67%)	⬇️
...ion_planners/simple/profile/simple_planner_utils.h	100.00% <ø> (ø)
...act_motion_planners/simple/simple_motion_planner.h	100.00% <ø> (ø)
.../simple_planner_fixed_size_assign_plan_profile.cpp	65.71% <ø> (ø)
... and 15 more

[marip8]

The path profile would still be needed regardless of interpolation because the primary function is to define different set of cost/constraints on the path taken versus the termination waypoint similar to industrial robots. The interpolation is only done because the current motion planners are discrete planners and the interpolated points would still have a need for the path profile. If we every updated trajopt to operate on splines, then interpolation would not be need but the path profile is still needed.

Okay; my concern primarily about semantics now and it doesn't really make a practical difference. We should at least document the intent of the path_profile with the content of the discussion above to make it clear to users what this is for.