How are Cartesian waypoints handled in planners like Trajopt and OMPL?

[marip8]

ik_problem-2022-07-28_18.18.27.mp4

Problem

In the above planning problem, I am using the Trajopt planning pipeline to create a motion plan of linear moves through through four Cartesian waypoints. I am using a gradient-based IK solver for this particular robot (KDL LMA), and it seems like part of the planning pipeline is choosing IK solutions with different joint configurations for those Cartesian waypoints to produce a seed for Trajopt (even though the robot does not appear to be on the boundary of a configuration change). This almost always causes the planning to fail due to iteration limit. If I change from linear moves to freespace moves, planning succeeds (see video above) and it becomes apparent that the joint configurations differ between waypoints

Questions

• How are the joint solutions chosen for Cartesian waypoints before they are sent to planners like Trajopt (or OMPL)? Does the simple planner do this or something else?
• What seed is used for the IK solver?
  • It seems like the IK solver seed might be the state of the environment given in the planning request. If this is the case, I would have thought my IK solver would have produced consistent configurations
• Does this problem go away if I use an IK solver that returns multiple solutions (e.g. OPW kinematics)? If so, how are multiple IK solutions handled differently than a solver that returns a single solution?

[Levi-Armstrong]

Is this first running through the simple planner?

[marip8]

Yes; it's using the Trajopt taskflow that has a simple planner step (in this case using an IK-enabled LVS profile). I also see that it has many more than 4 intermediate waypoints in the Trajopt log

[Levi-Armstrong]

I have mostly used descartes first to generate the seed in this situation. I believe the simple planner may need some work to generate a good seed for this case.

[marip8]

Same here. I think we could make a few simple changes to improve this significantly. The easiest thing to do would probably be to use the IK solution from one waypoint as the seed for the subsequent waypoint. This should help enforce configuration consistency

[Levi-Armstrong]

I thought I had updated it to do that recently but something could be broken.

[marip8]

For reference here is where the comes from. Per our discussion yesterday, we should eliminate the Cartesian-to-joint and Cartesian-to-Cartesian case because the first Cartesian waypoint should always be resolved to a joint waypoint using IK