[OMPL] also check constraints in StateValidityCallback (#3586) (#3611)

mergify[bot] · MatthijsBurgh · web-flow · commit 684bccdc2b91 · 2025-11-17T12:29:46.000-07:00
(cherry picked from commit ca2bb1b) Co-authored-by: Matthijs van der Burgh <MatthijsBurgh@outlook.com>
diff --git a/moveit_planners/ompl/ompl_interface/src/detail/constrained_goal_sampler.cpp b/moveit_planners/ompl/ompl_interface/src/detail/constrained_goal_sampler.cpp
@@ -89,7 +89,8 @@ bool ConstrainedGoalSampler::stateValidityCallback(ob::State* new_goal, const mo
   moveit::core::RobotState solution_state(*state);
   solution_state.setJointGroupPositions(jmg, jpos);
   solution_state.update();
-  return checkStateValidity(new_goal, solution_state, verbose);
+  return checkStateValidity(new_goal, solution_state, verbose) &&
+         kinematic_constraint_set_->decide(solution_state, verbose).satisfied;
 }
 
 bool ConstrainedGoalSampler::sampleUsingConstraintSampler(const ob::GoalLazySamples* gls, ob::State* new_goal)

Original file line number	Diff line number	Diff line change
`@@ -89,7 +89,8 @@ bool ConstrainedGoalSampler::stateValidityCallback(ob::State* new_goal, const mo`
`89`	`89`	`moveit::core::RobotState solution_state(*state);`
`90`	`90`	`solution_state.setJointGroupPositions(jmg, jpos);`
`91`	`91`	`solution_state.update();`
`92`		`- return checkStateValidity(new_goal, solution_state, verbose);`
	`92`	`+ return checkStateValidity(new_goal, solution_state, verbose) &&`
	`93`	`+ kinematic_constraint_set_->decide(solution_state, verbose).satisfied;`
`93`	`94`	`}`
`94`	`95`
`95`	`96`	`bool ConstrainedGoalSampler::sampleUsingConstraintSampler(const ob::GoalLazySamples* gls, ob::State* new_goal)`