Use the Cartesian waypoint seed when solving IK in the OMPL move profile

[munich35]

Summary

OMPLRealVectorMoveProfile now uses the seed state attached to a CartesianWaypoint when solving IK for start/goal states, instead of always seeding calcInvKin with a hardcoded zero vector.

Root cause

In applyStartStates / applyGoalStates (the KinGroupIKInput overloads), calcInvKin was called with Eigen::VectorXd::Zero(dof) even when the waypoint carried a seed (CartesianWaypointPoly::getSeed() populated, e.g. by MinLengthTask'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:

All goal states are either in collision or outside limits

The failure is deterministic: programs whose LINEAR StateWaypoints were converted to seeded CartesianWaypoints by MinLengthTask fail 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

• Thread the waypoint's seed into the two KinGroupIKInput applyStartStates/applyGoalStates overloads via an optional trailing const Eigen::VectorXd& seed parameter (defaulted, so the public API and existing zero-seed behavior are unchanged for callers that pass nothing).
• At the call sites, pass cur_wp.getSeed().position when cur_wp.hasSeed().
• Inside, the seed is only used when its size matches the manipulator DOF; otherwise it falls back to the previous zero seed.

This also resolves the pre-existing @todo Need to also provide the seed instruction to use here in applyStartStates.

Notes for review

• Seed ordering: getSeed().position is ordered by getSeed().joint_names. This follows the same convention already relied on elsewhere for Cartesian-waypoint seeds (e.g. simple_motion_planner.cpp and the simple-planner assign profiles use getSeed().position directly), 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.
• Scope: this implements the "honor the attached seed" part only. A multi-random-restart fallback for the no-seed / no-analytic-IK case (mentioned in the issue) would be a reasonable follow-up but is intentionally left out to keep this change minimal.

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 current master. The seed-selection logic and the call-site expressions were compile-checked in isolation against Eigen. A full colcon build against current master (0.35.0) was not run in this environment. Happy to add a unit test if you'd like.

Closes #762

[Levi-Armstrong]

@munich35 It looks like the OMPL unit tests are failing which is interesting. Let me know what you find.

[munich35]

Found it — the failure was real and caused by the change, thanks for flagging it.

Root cause: generateInterpolatedProgram attaches the simple planner's final interpolation state as the Cartesian waypoint's seed — and that seed is produced without collision awareness. In OMPLFreespaceCartesianGoalPlannerUnit the seeded IK (single-solution solver) converges onto a branch where link_4 is ~8 mm inside the test box (Solution: 0 Links: box_attached, link_4 Distance: -0.008); the lone solution gets rejected, GoalStates ends up empty, and createSetup throws before any planner runs — which is why all eight configurators failed identically.

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 applyGoalStates and applyStartStates, so a reachable goal/start is never lost to an unlucky seed while the original issue (#762 — a zero seed converging out-of-limits and losing a reachable goal) stays fixed.

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.