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 index 34afabd9ae..6c658a3a80 100644 --- 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)