Clarification and proposed revision for the operation of the simple (i.e., interpolating) planner

[marip8]

The simple planner seems to have somewhat non-standard outputs/operation relative to the other motion planners. I think it would be beneficial to first clarify what exactly the simple planner currently does (and merge it into the documentation).

I think it would also be beneficial to revise how these planners work, such that they could be used directly as stand-alone "planners" as well as seeds for other planners. This would be beneficial for many applications where niave interpolated trajectories could be valid, and it would be beneficial for visualizing seed trajectories (e.g., in Rviz) for other planners.

Current operation (for all but fixed size assign)

For LINEAR moves:

• Joint/state waypoint to joint/state waypoint
  • Convert both poses to Cartesian poses (using FK)
  • Interpolate in Cartesian space
  • Return list of move instructions to Cartesian waypoints
    • The seeds of these Cartesian waypoints are the interpolated states between start and end joint pose
• Joint/state waypoint to Cartesian waypoint
  • Convert start pose to Cartesian pose (using FK)
  • Interpolate in Cartesian space
  • Return list of move instructions to Cartesian waypoints
    • The seeds of the Cartesian waypoints are all set to the values of the start joint pose
• Cartesian waypoint to joint/state waypoint
  • Convert end pose to Cartesian pose (using FK)
  • Interpolate in Cartesian space
  • Return list of move instructions to Cartesian waypoints
    • The seeds of the Cartesian waypoints are all set to the values of the end joint pose
• Cartesian waypoint to Cartesian waypoint
  • Interpolate in Cartesian space
  • Return list of move instructions to Cartesian waypoints
    • The seeds of the Cartesian waypoints are all set to an input joint state (presumably the current state of the environment)

For Freespace moves:

• Joint/state waypoint to joint/state waypoint
  • Interpolate in joint space
  • Return list of move instructions to joint waypoints
• Joint/state waypoint to Cartesian waypoint
  • Convert Cartesian waypoint to a joint waypoint by extracting its seed pose or computing IK (if the seed pose does not exist)
    • This new joint waypoint is only used to determine the number of interpolation steps
  • Return list of move instructions to joint waypoints
    • Start state is simply copied n times
• Cartesian waypoint to joint waypoint
  • Convert Cartesian waypoint to a joint waypoint by extracting its seed pose or computing IK (if the seed pose does not exist)
    • This new joint waypoint is only used to determine the number of interpolation steps
  • Return list of move instructions to joint waypoints
    • End joint state is simply copied n times
• Cartesian waypoint to Cartesian waypoint
  • Convert both Cartesian waypoints to joint waypoints by extracting their seed poses or computing IK (if the seed poses do not exist)
    • These new joint states are only used to determine the number of interpolation steps
  • Return list of move instructions to joint waypoints
    • The input seed state (presumably taken from the current state of the environment) is simply copied n times

Proposed revised operation

For LINEAR moves:

• Joint/state waypoint to joint/state waypoint
  • Convert both poses to Cartesian poses (using FK)
  • Interpolate in Cartesian space
  • Solve IK at each waypoint, using the solution of the previous waypoint as the seed
  • Return list of move instructions to state waypoints
• Joint/state waypoint to Cartesian waypoint
  • Convert start pose to Cartesian pose (using FK)
  • Interpolate in Cartesian space
  • Solve IK at each waypoint, using the solution of the previous waypoint as the seed
  • Return list of move instructions to state waypoints
• Cartesian waypoint to joint/state waypoint
  • Convert end pose to Cartesian pose (using FK)
  • Interpolate in Cartesian space
  • Solve IK at each waypoint, using the solution of the previous waypoint as the seed
  • Return list of move instructions to state waypoints
• Cartesian waypoint to Cartesian waypoint
  • Interpolate in Cartesian space
  • Solve IK at each waypoint, using the solution of the previous waypoint as the seed
  • Return list of move instructions to state waypoints

For Freespace moves:

• Joint/state waypoint to joint/state waypoint
  • Interpolate in joint space
  • Return list of move instructions to state waypoints
• Joint/state waypoint to Cartesian waypoint
  • Convert Cartesian waypoint to a joint pose by computing IK (using its seed pose)
  • Interpolate in joint space
  • Return list of move instructions to state waypoints
• Cartesian waypoint to joint waypoint
  • Convert Cartesian waypoint to a joint pose by computing IK (using its seed pose)
  • Interpolate in joint space
  • Return list of move instructions to state waypoints
• Cartesian waypoint to Cartesian waypoint
  • Convert both Cartesian waypoints to joint waypoints by solving IK (using their seed poses)
  • Interpolate in joint space
  • Return list of move instructions to state waypoints

Discussion points

• This change standardizes the output of the simple planner into a list of state waypoints that represent a valid trajectory (to be collision checked and time parameterized by subsequent task steps)
• How to use the output as a seed for another planner?
  • We would create tasks (e.g., ConvertToCartesianWaypoint or ConvertToJointWaypoint -- similar to the FormatAsInput task) that run after the simple planner but before other planners to convert the output into an input that the other planners expect
• Don't these proposed changes involve more (and potentially unnecessary) computational effort
  • Yes; particularly for LINEAR moves the proposed change would involve potentially many IK solves. However, the benefit is that you would know up front whether all intermediate waypoints have an IK solution, rather than waiting for another planner (e.g., Descartes) to fail. It would additionally allow you to use the output of the simple planner directly as a trajectory, which cannot really be done in tesseract now (except for the joint state to to joint state case). We can always add another simple planner implementation that does not do this for applications where this performance penalty is a serious problem
• How would we handle the case of the fixed size assign planning profile?
  • I think this would be separate instance of a motion planner since its operation is significantly different than the other interpolating planners