Use the Cartesian waypoint seed when solving IK in the OMPL move profile#763
Use the Cartesian waypoint seed when solving IK in the OMPL move profile#763munich35 wants to merge 2 commits into
Conversation
OMPLRealVectorMoveProfile solved start/goal states for CartesianWaypoints by calling calcInvKin with a hardcoded zero seed, ignoring any seed state attached to the waypoint (e.g. populated by MinLengthTask's interpolation). For seed-dependent solvers that return a single solution (e.g. the KDL-LMA plugin), the zero-seed solution can land outside the joint limits; calcInvKin's limit filter then empties the goal set and the planner aborts with "All goal states are either in collision or outside limits", even though the waypoint is reachable and its seed would solve cleanly. Honor the waypoint's attached seed (CartesianWaypointPoly::getSeed()) as the IK seed when it is present and sized to the manipulator, falling back to the previous zero seed otherwise. The seed is threaded through the KinGroupIKInput applyStartStates/applyGoalStates overloads via an optional trailing parameter, so the existing API and behavior for callers that pass no seed are unchanged. Fixes tesseract-robotics#762 Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
|
@munich35 It looks like the OMPL unit tests are failing which is interesting. Let me know what you find. |
The initial version used the Cartesian waypoint's attached seed exclusively. A seed produced upstream without collision awareness (e.g. the simple planner's interpolation states, which generateInterpolatedProgram attaches to Cartesian waypoints) can steer a single-solution IK solver onto a branch that is in collision; applyGoalStates then rejected the only solution and threw 'All goal states are either in collision or outside limits' before any planner ran - the OMPLFreespaceCartesianGoalPlannerUnit failure seen identically across all eight configurators (link_4 vs box_attached, distance -0.008). The seeded pass now runs first and a second pass with the legacy zero seed recovers the previous behavior whenever the seeded pass contributes no valid state, so a reachable goal/start is never lost to an unlucky seed. Applied symmetrically to applyGoalStates and applyStartStates. Full OMPL unit suite passes locally: 25/25 across all eight configurators.
|
Found it — the failure was real and caused by the change, thanks for flagging it. Root cause: Fix pushed (a812033): the waypoint seed is now used preferentially, not exclusively — the seeded IK pass runs first, and a second pass with the legacy zero seed recovers the previous behavior whenever the seeded pass contributes no valid state. Applied symmetrically to Local verification: full OMPL unit suite green — 25/25 across all eight configurators (SBL, PRM, PRMstar, LazyPRMstar, EST, BKPIECE1, KPIECE1, RRTConnect), including the two Cartesian start/goal tests and the motion validator test. |
2 participants
Summary
OMPLRealVectorMoveProfilenow uses the seed state attached to aCartesianWaypointwhen solving IK for start/goal states, instead of always seedingcalcInvKinwith a hardcoded zero vector.Root cause
In
applyStartStates/applyGoalStates(theKinGroupIKInputoverloads),calcInvKinwas called withEigen::VectorXd::Zero(dof)even when the waypoint carried a seed (CartesianWaypointPoly::getSeed()populated, e.g. byMinLengthTask's interpolation). For seed-dependent solvers that return a single solution (e.g. the KDL-LMA plugin), the zero-seed solution can converge to an out-of-limits branch of the solution manifold.calcInvKin's joint-limit filter then discards it, the goal-state set ends up empty, and the planner aborts with:The failure is deterministic: programs whose LINEAR
StateWaypoints were converted to seededCartesianWaypoints byMinLengthTaskfail even though every state is reachable and within limits — the seed that would make IK succeed is on the waypoint but was never consulted.Fix
KinGroupIKInputapplyStartStates/applyGoalStatesoverloads via an optional trailingconst Eigen::VectorXd& seedparameter (defaulted, so the public API and existing zero-seed behavior are unchanged for callers that pass nothing).cur_wp.getSeed().positionwhencur_wp.hasSeed().This also resolves the pre-existing
@todo Need to also provide the seed instruction to use hereinapplyStartStates.Notes for review
getSeed().positionis ordered bygetSeed().joint_names. This follows the same convention already relied on elsewhere for Cartesian-waypoint seeds (e.g.simple_motion_planner.cppand the simple-planner assign profiles usegetSeed().positiondirectly), so no reordering is applied here. The size guard makes a mismatched seed a safe no-op (falls back to zero); a seed is a hint, so a mis-ordered one degrades gracefully rather than breaking correctness.Verification
An equivalent seed-honoring patch was validated in our downstream application on 0.34 (the previously-failing
MinLengthTask-> OMPL -> KDL-LMA programs planned successfully, with no regressions observed elsewhere); this PR ports that approach to currentmaster. The seed-selection logic and the call-site expressions were compile-checked in isolation against Eigen. A fullcolconbuild against currentmaster(0.35.0) was not run in this environment. Happy to add a unit test if you'd like.Closes #762