Fix joint limit enforcement discarded in OMPL JointWaypoint start/goal registration

NickNair · Levi-Armstrong · commit c54f2e8c87af · 2026-07-13T16:32:13.000-05:00
diff --git a/motion_planners/ompl/src/profile/ompl_real_vector_move_profile.cpp b/motion_planners/ompl/src/profile/ompl_real_vector_move_profile.cpp
@@ -335,7 +335,7 @@ void OMPLRealVectorMoveProfile::applyGoalStates(ompl::geometric::SimpleSetup& si
 
   ompl::base::ScopedState<> goal_state(simple_setup.getStateSpace());
   for (unsigned i = 0; i < dof; ++i)
-    goal_state[i] = joint_waypoint[i];
+    goal_state[i] = solution[static_cast<Eigen::Index>(i)];
 
   simple_setup.setGoalState(goal_state);
 }
@@ -444,7 +444,7 @@ void OMPLRealVectorMoveProfile::applyStartStates(ompl::geometric::SimpleSetup& s
 
   ompl::base::ScopedState<> start_state(simple_setup.getStateSpace());
   for (unsigned i = 0; i < dof; ++i)
-    start_state[i] = joint_waypoint[i];
+    start_state[i] = solution[static_cast<Eigen::Index>(i)];
 
   simple_setup.addStartState(start_state);
 }

Original file line number	Diff line number	Diff line change
`@@ -335,7 +335,7 @@ void OMPLRealVectorMoveProfile::applyGoalStates(ompl::geometric::SimpleSetup& si`
`335`	`335`
`336`	`336`	`ompl::base::ScopedState<> goal_state(simple_setup.getStateSpace());`
`337`	`337`	`for (unsigned i = 0; i < dof; ++i)`
`338`		`- goal_state[i] = joint_waypoint[i];`
	`338`	`+ goal_state[i] = solution[static_cast<Eigen::Index>(i)];`
`339`	`339`
`340`	`340`	`simple_setup.setGoalState(goal_state);`
`341`	`341`	`}`
`@@ -444,7 +444,7 @@ void OMPLRealVectorMoveProfile::applyStartStates(ompl::geometric::SimpleSetup& s`
`444`	`444`
`445`	`445`	`ompl::base::ScopedState<> start_state(simple_setup.getStateSpace());`
`446`	`446`	`for (unsigned i = 0; i < dof; ++i)`
`447`		`- start_state[i] = joint_waypoint[i];`
	`447`	`+ start_state[i] = solution[static_cast<Eigen::Index>(i)];`
`448`	`448`
`449`	`449`	`simple_setup.addStartState(start_state);`
`450`	`450`	`}`