bool ompl_interface::ConstrainedGoalSampler::stateValidityCallback(ob::State* new_goal,
robot_state::RobotState const* state,
const robot_model::JointModelGroup* jmg,
const double* jpos, bool verbose) const
{
// we copy the state to not change the seed state
robot_state::RobotState solution_state(*state);
solution_state.setJointGroupPositions(jmg, jpos);
solution_state.update();
return checkStateValidity(new_goal, solution_state, verbose);
}
bool ompl_interface::ConstrainedGoalSampler::sampleUsingConstraintSampler(const ob::GoalLazySamples *gls, ob::State *new_goal)
{
// moveit::Profiler::ScopedBlock sblock("ConstrainedGoalSampler::sampleUsingConstraintSampler");
unsigned int max_attempts = planning_context_->getMaximumGoalSamplingAttempts();
unsigned int attempts_so_far = gls->samplingAttemptsCount();
// terminate after too many attempts
if (attempts_so_far >= max_attempts)
return false;
// terminate after a maximum number of samples
if (gls->getStateCount() >= planning_context_->getMaximumGoalSamples())
return false;
// terminate the sampling thread when a solution has been found
if (planning_context_->getOMPLSimpleSetup().getProblemDefinition()->hasSolution())
return false;
unsigned int max_attempts_div2 = max_attempts/2;
for (unsigned int a = gls->samplingAttemptsCount() ; a < max_attempts && gls->isSampling() ; ++a)
{
bool verbose = false;
if (gls->getStateCount() == 0 && a >= max_attempts_div2)
if (verbose_display_ < 1)
{
verbose = true;
verbose_display_++;
}
if (constraint_sampler_)
{
// makes the constraint sampler also perform a validity callback
robot_state::GroupStateValidityCallbackFn gsvcf = boost::bind(&ompl_interface::ConstrainedGoalSampler::stateValidityCallback,
this,
new_goal,
_1, // pointer to state
_2, // const* joint model group
_3, // double* of joint positions
verbose);
constraint_sampler_->setGroupStateValidityCallback( gsvcf );
if (constraint_sampler_->project(work_state_, planning_context_->getMaximumStateSamplingAttempts()))
{
work_state_.update();
if (kinematic_constraint_set_->decide(work_state_, verbose).satisfied)
{
if (checkStateValidity(new_goal, work_state_, verbose))
return true;
}
else
{
invalid_sampled_constraints_++;
if (!warned_invalid_samples_ && invalid_sampled_constraints_ >= (attempts_so_far * 8) / 10)
{
warned_invalid_samples_ = true;
logWarn("More than 80%% of the sampled goal states fail to satisfy the constraints imposed on the goal sampler. Is the constrained sampler working correctly?");
}
}
}
}
else
{
default_sampler_->sampleUniform(new_goal);
if (static_cast<const StateValidityChecker*>(si_->getStateValidityChecker().get())->isValid(new_goal, verbose))
{
planning_context_->getOMPLStateSpace()->copyToRobotState(work_state_, new_goal);
if (kinematic_constraint_set_->decide(work_state_, verbose).satisfied)
return true;
}
}
}
return false;
}
