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; }