37 #include <boost/algorithm/string/trim.hpp>
38 #include <boost/algorithm/string/split.hpp>
52 #include <ompl/config.h>
53 #include <ompl/base/samplers/UniformValidStateSampler.h>
54 #include <ompl/base/goals/GoalLazySamples.h>
55 #include <ompl/tools/config/SelfConfig.h>
56 #include <ompl/base/spaces/SE3StateSpace.h>
57 #include <ompl/datastructures/PDF.h>
59 #if OMPL_VERSION_VALUE < 1005000
60 #include <ompl/base/PlannerTerminationCondition.h>
64 #include <ompl/base/terminationconditions/IterationTerminationCondition.h>
65 #include <ompl/base/terminationconditions/CostConvergenceTerminationCondition.h>
68 #include "ompl/base/objectives/PathLengthOptimizationObjective.h"
69 #include "ompl/base/objectives/MechanicalWorkOptimizationObjective.h"
70 #include "ompl/base/objectives/MinimaxObjective.h"
71 #include "ompl/base/objectives/StateCostIntegralObjective.h"
72 #include "ompl/base/objectives/MaximizeMinClearanceObjective.h"
73 #include <ompl/geometric/planners/prm/LazyPRM.h>
77 constexpr
char LOGNAME[] =
"model_based_planning_context";
84 , complete_initial_robot_state_(spec.state_space_->getRobotModel())
85 , ompl_simple_setup_(spec.ompl_simple_setup_)
86 , ompl_benchmark_(*ompl_simple_setup_)
87 , ompl_parallel_plan_(ompl_simple_setup_->getProblemDefinition())
89 , last_plan_time_(0.0)
90 , last_simplify_time_(0.0)
91 , max_goal_samples_(0)
92 , max_state_sampling_attempts_(0)
93 , max_goal_sampling_attempts_(0)
94 , max_planning_threads_(0)
95 , max_solution_segment_length_(0.0)
96 , minimum_waypoint_count_(0)
97 , multi_query_planning_enabled_(false)
98 , simplify_solutions_(true)
109 loadConstraintApproximations(nh);
110 if (!use_constraints_approximations)
112 setConstraintsApproximations(ConstraintsLibraryPtr());
114 complete_initial_robot_state_.update();
115 ompl_simple_setup_->getStateSpace()->computeSignature(space_signature_);
116 ompl_simple_setup_->getStateSpace()->setStateSamplerAllocator(
117 [
this](
const ompl::base::StateSpace* ss) {
return allocPathConstrainedSampler(ss); });
120 ompl::base::ScopedState<> ompl_start_state(spec_.state_space_);
121 spec_.state_space_->copyToOMPLState(ompl_start_state.get(), getCompleteInitialRobotState());
122 ompl_simple_setup_->setStartState(ompl_start_state);
123 ompl_simple_setup_->setStateValidityChecker(ob::StateValidityCheckerPtr(
new StateValidityChecker(
this)));
125 if (path_constraints_ && constraints_library_)
127 const ConstraintApproximationPtr& constraint_approx =
128 constraints_library_->getConstraintApproximation(path_constraints_msg_);
129 if (constraint_approx)
131 getOMPLStateSpace()->setInterpolationFunction(constraint_approx->getInterpolationFunction());
137 if (ompl_simple_setup_->getGoal())
138 ompl_simple_setup_->setup();
143 if (!spec_.state_space_)
148 ob::ProjectionEvaluatorPtr projection_eval = getProjectionEvaluator(peval);
150 spec_.state_space_->registerDefaultProjection(projection_eval);
153 ompl::base::ProjectionEvaluatorPtr
156 if (peval.find_first_of(
"link(") == 0 && peval[peval.length() - 1] ==
')')
158 std::string link_name = peval.substr(5, peval.length() - 6);
159 if (getRobotModel()->hasLinkModel(link_name))
163 "Attempted to set projection evaluator with respect to position of link '%s', "
164 "but that link is not known to the kinematic model.",
167 else if (peval.find_first_of(
"joints(") == 0 && peval[peval.length() - 1] ==
')')
169 std::string joints = peval.substr(7, peval.length() - 8);
170 boost::replace_all(joints,
",",
" ");
171 std::vector<unsigned int> j;
172 std::stringstream ss(joints);
173 while (ss.good() && !ss.eof())
176 ss >> joint >> std::ws;
177 if (getJointModelGroup()->hasJointModel(joint))
179 unsigned int variable_count = getJointModelGroup()->getJointModel(joint)->getVariableCount();
180 if (variable_count > 0)
182 int idx = getJointModelGroup()->getVariableGroupIndex(joint);
183 for (
unsigned int q = 0; q < variable_count; ++q)
184 j.push_back(idx + q);
192 "%s: Attempted to set projection evaluator with respect to value of joint "
193 "'%s', but that joint is not known to the group '%s'.",
194 name_.c_str(), joint.c_str(), getGroupName().c_str());
202 ROS_ERROR_NAMED(
LOGNAME,
"Unable to allocate projection evaluator based on description: '%s'", peval.c_str());
203 return ob::ProjectionEvaluatorPtr();
206 ompl::base::StateSamplerPtr
209 if (spec_.state_space_.get() != state_space)
211 ROS_ERROR_NAMED(
LOGNAME,
"%s: Attempted to allocate a state sampler for an unknown state space", name_.c_str());
212 return ompl::base::StateSamplerPtr();
215 ROS_DEBUG_NAMED(
LOGNAME,
"%s: Allocating a new state sampler (attempts to use path constraints)", name_.c_str());
217 if (path_constraints_)
219 if (constraints_library_)
221 const ConstraintApproximationPtr& constraint_approx =
222 constraints_library_->getConstraintApproximation(path_constraints_msg_);
223 if (constraint_approx)
225 ompl::base::StateSamplerAllocator state_sampler_allocator =
226 constraint_approx->getStateSamplerAllocator(path_constraints_msg_);
227 if (state_sampler_allocator)
229 ompl::base::StateSamplerPtr state_sampler = state_sampler_allocator(state_space);
233 "%s: Using precomputed state sampler (approximated constraint space) for constraint '%s'",
234 name_.c_str(), path_constraints_msg_.name.c_str());
235 return state_sampler;
241 constraint_samplers::ConstraintSamplerPtr constraint_sampler;
242 if (spec_.constraint_sampler_manager_)
243 constraint_sampler = spec_.constraint_sampler_manager_->selectSampler(getPlanningScene(), getGroupName(),
244 path_constraints_->getAllConstraints());
246 if (constraint_sampler)
253 return state_space->allocDefaultStateSampler();
258 const std::map<std::string, std::string>& config = spec_.config_;
261 std::map<std::string, std::string> cfg = config;
264 auto it = cfg.find(
"longest_valid_segment_fraction");
266 if (it != cfg.end() || max_solution_segment_length_ != 0.0)
269 double longest_valid_segment_fraction_config = (it != cfg.end())
272 double longest_valid_segment_fraction_final = longest_valid_segment_fraction_config;
273 if (max_solution_segment_length_ > 0.0)
277 longest_valid_segment_fraction_final = std::min(
278 longest_valid_segment_fraction_config,
279 max_solution_segment_length_ / spec_.state_space_->getMaximumExtent()
289 it = cfg.find(
"projection_evaluator");
292 setProjectionEvaluator(boost::trim_copy(it->second));
299 std::string optimizer;
300 ompl::base::OptimizationObjectivePtr objective;
301 it = cfg.find(
"optimization_objective");
304 optimizer = it->second;
307 if (optimizer ==
"PathLengthOptimizationObjective")
310 std::make_shared<ompl::base::PathLengthOptimizationObjective>(ompl_simple_setup_->getSpaceInformation());
312 else if (optimizer ==
"MinimaxObjective")
314 objective = std::make_shared<ompl::base::MinimaxObjective>(ompl_simple_setup_->getSpaceInformation());
316 else if (optimizer ==
"StateCostIntegralObjective")
318 objective = std::make_shared<ompl::base::StateCostIntegralObjective>(ompl_simple_setup_->getSpaceInformation());
320 else if (optimizer ==
"MechanicalWorkOptimizationObjective")
323 std::make_shared<ompl::base::MechanicalWorkOptimizationObjective>(ompl_simple_setup_->getSpaceInformation());
325 else if (optimizer ==
"MaximizeMinClearanceObjective")
328 std::make_shared<ompl::base::MaximizeMinClearanceObjective>(ompl_simple_setup_->getSpaceInformation());
333 std::make_shared<ompl::base::PathLengthOptimizationObjective>(ompl_simple_setup_->getSpaceInformation());
336 ompl_simple_setup_->setOptimizationObjective(objective);
340 it = cfg.find(
"multi_query_planning_enabled");
342 multi_query_planning_enabled_ = boost::lexical_cast<bool>(it->second);
345 it = cfg.find(
"interpolate");
348 interpolate_ = boost::lexical_cast<bool>(it->second);
353 it = cfg.find(
"hybridize");
356 hybridize_ = boost::lexical_cast<bool>(it->second);
361 it = cfg.find(
"type");
364 if (name_ != getGroupName())
365 ROS_WARN_NAMED(
LOGNAME,
"%s: Attribute 'type' not specified in planner configuration", name_.c_str());
369 std::string type = it->second;
371 const std::string planner_name = getGroupName() +
"/" + name_;
372 ompl_simple_setup_->setPlannerAllocator(
373 [planner_name, &spec = this->spec_, allocator = spec_.planner_selector_(type)](
374 const ompl::base::SpaceInformationPtr& si) { return allocator(si, planner_name, spec); });
376 "Planner configuration '%s' will use planner '%s'. "
377 "Additional configuration parameters will be set when the planner is constructed.",
378 name_.c_str(), type.c_str());
382 ompl_simple_setup_->getSpaceInformation()->setup();
383 ompl_simple_setup_->getSpaceInformation()->params().setParams(cfg,
true);
385 ompl_simple_setup_->getSpaceInformation()->setup();
390 if (wparams.min_corner.x == wparams.max_corner.x && wparams.min_corner.x == 0.0 &&
391 wparams.min_corner.y == wparams.max_corner.y && wparams.min_corner.y == 0.0 &&
392 wparams.min_corner.z == wparams.max_corner.z && wparams.min_corner.z == 0.0)
396 "%s: Setting planning volume (affects SE2 & SE3 joints only) to x = [%f, %f], y = "
397 "[%f, %f], z = [%f, %f]",
398 name_.c_str(), wparams.min_corner.x, wparams.max_corner.x, wparams.min_corner.y, wparams.max_corner.y,
399 wparams.min_corner.z, wparams.max_corner.z);
401 spec_.state_space_->setPlanningVolume(wparams.min_corner.x, wparams.max_corner.x, wparams.min_corner.y,
402 wparams.max_corner.y, wparams.min_corner.z, wparams.max_corner.z);
407 ompl::time::point
start = ompl::time::now();
408 ob::PlannerTerminationCondition ptc = constructPlannerTerminationCondition(timeout,
start);
409 registerTerminationCondition(ptc);
410 ompl_simple_setup_->simplifySolution(ptc);
411 last_simplify_time_ = ompl_simple_setup_->getLastSimplificationTime();
412 unregisterTerminationCondition();
417 if (ompl_simple_setup_->haveSolutionPath())
419 og::PathGeometric& pg = ompl_simple_setup_->getSolutionPath();
423 unsigned int eventual_states = 1;
424 std::vector<ompl::base::State*> states = pg.getStates();
425 for (
size_t i = 0; i < states.size() - 1; i++)
427 eventual_states += ompl_simple_setup_->getStateSpace()->validSegmentCount(states[i], states[i + 1]);
430 if (eventual_states < minimum_waypoint_count_)
433 pg.interpolate(minimum_waypoint_count_);
447 for (std::size_t i = 0; i < pg.getStateCount(); ++i)
449 spec_.state_space_->copyToRobotState(ks, pg.getState(i));
457 if (ompl_simple_setup_->haveSolutionPath())
458 convertPath(ompl_simple_setup_->getSolutionPath(), traj);
459 return ompl_simple_setup_->haveSolutionPath();
464 if (ompl_simple_setup_->getStateValidityChecker())
465 static_cast<StateValidityChecker*
>(ompl_simple_setup_->getStateValidityChecker().get())->setVerbose(flag);
472 std::vector<ob::GoalPtr> goals;
473 for (kinematic_constraints::KinematicConstraintSetPtr& goal_constraint : goal_constraints_)
475 constraint_samplers::ConstraintSamplerPtr constraint_sampler;
476 if (spec_.constraint_sampler_manager_)
477 constraint_sampler = spec_.constraint_sampler_manager_->selectSampler(getPlanningScene(), getGroupName(),
478 goal_constraint->getAllConstraints());
479 if (constraint_sampler)
482 goals.push_back(goal);
491 return ob::GoalPtr();
494 ompl::base::PlannerTerminationCondition
496 const ompl::time::point& start)
498 auto it = spec_.config_.find(
"termination_condition");
499 if (it == spec_.config_.end())
500 return ob::timedPlannerTerminationCondition(timeout - ompl::time::seconds(ompl::time::now() -
start));
501 std::string termination_string = it->second;
502 std::vector<std::string> termination_and_params;
503 boost::split(termination_and_params, termination_string, boost::is_any_of(
"[ ,]"));
505 if (termination_and_params.empty())
510 else if (termination_and_params[0] ==
"Iteration")
512 if (termination_and_params.size() > 1)
513 return ob::plannerOrTerminationCondition(
514 ob::timedPlannerTerminationCondition(timeout - ompl::time::seconds(ompl::time::now() -
start)),
515 ob::IterationTerminationCondition(std::stoul(termination_and_params[1])));
520 #if OMPL_VERSION_VALUE >= 1005000
523 else if (termination_and_params[0] ==
"CostConvergence")
525 std::size_t solutions_window = 10u;
527 if (termination_and_params.size() > 1)
529 solutions_window = std::stoul(termination_and_params[1]);
530 if (termination_and_params.size() > 2)
533 return ob::plannerOrTerminationCondition(
534 ob::timedPlannerTerminationCondition(timeout - ompl::time::seconds(ompl::time::now() -
start)),
535 ob::CostConvergenceTerminationCondition(ompl_simple_setup_->getProblemDefinition(), solutions_window,
epsilon));
541 else if (termination_and_params[0] ==
"ExactSolution")
543 return ob::plannerOrTerminationCondition(
544 ob::timedPlannerTerminationCondition(timeout - ompl::time::seconds(ompl::time::now() -
start)),
545 ob::exactSolnPlannerTerminationCondition(ompl_simple_setup_->getProblemDefinition()));
551 return ob::plannerAlwaysTerminatingCondition();
557 complete_initial_robot_state_ = complete_initial_robot_state;
558 complete_initial_robot_state_.
update();
563 if (!multi_query_planning_enabled_)
564 ompl_simple_setup_->clear();
566 #if OMPL_VERSION_VALUE >= 1005000
573 auto planner =
dynamic_cast<ompl::geometric::LazyPRM*
>(ompl_simple_setup_->getPlanner().get());
574 if (planner !=
nullptr)
575 planner->clearValidity();
578 ompl_simple_setup_->clearStartStates();
579 ompl_simple_setup_->setGoal(ob::GoalPtr());
580 ompl_simple_setup_->setStateValidityChecker(ob::StateValidityCheckerPtr());
581 path_constraints_.reset();
582 goal_constraints_.clear();
587 moveit_msgs::MoveItErrorCodes* )
590 path_constraints_ = std::make_shared<kinematic_constraints::KinematicConstraintSet>(getRobotModel());
591 path_constraints_->add(path_constraints, getPlanningScene()->getTransforms());
592 path_constraints_msg_ = path_constraints;
598 const std::vector<moveit_msgs::Constraints>& goal_constraints,
const moveit_msgs::Constraints& path_constraints,
599 moveit_msgs::MoveItErrorCodes* error)
602 goal_constraints_.clear();
603 for (
const moveit_msgs::Constraints& goal_constraint : goal_constraints)
606 kinematic_constraints::KinematicConstraintSetPtr kset(
608 kset->add(constr, getPlanningScene()->getTransforms());
610 goal_constraints_.push_back(kset);
613 if (goal_constraints_.empty())
615 ROS_WARN_NAMED(
LOGNAME,
"%s: No goal constraints specified. There is no problem to solve.", name_.c_str());
617 error->val = moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS;
621 ob::GoalPtr goal = constructGoal();
622 ompl_simple_setup_->setGoal(goal);
623 return static_cast<bool>(goal);
627 const std::string& filename)
629 ompl_benchmark_.clearPlanners();
630 ompl_simple_setup_->setup();
631 ompl_benchmark_.addPlanner(ompl_simple_setup_->getPlanner());
632 ompl_benchmark_.setExperimentName(getRobotModel()->
getName() +
"_" + getGroupName() +
"_" +
633 getPlanningScene()->
getName() +
"_" + name_);
635 ot::Benchmark::Request req;
636 req.maxTime = timeout;
637 req.runCount = count;
638 req.displayProgress =
true;
639 req.saveConsoleOutput =
false;
640 ompl_benchmark_.benchmark(req);
641 return filename.empty() ? ompl_benchmark_.saveResultsToFile() : ompl_benchmark_.saveResultsToFile(filename.c_str());
646 bool gls = ompl_simple_setup_->getGoal()->hasType(ob::GOAL_LAZY_SAMPLES);
648 static_cast<ob::GoalLazySamples*
>(ompl_simple_setup_->getGoal().get())->startSampling();
656 bool gls = ompl_simple_setup_->getGoal()->hasType(ob::GOAL_LAZY_SAMPLES);
658 static_cast<ob::GoalLazySamples*
>(ompl_simple_setup_->getGoal().get())->stopSampling();
667 ompl_simple_setup_->getProblemDefinition()->clearSolutionPaths();
668 const ob::PlannerPtr planner = ompl_simple_setup_->getPlanner();
669 if (planner && !multi_query_planning_enabled_)
672 ompl_simple_setup_->getSpaceInformation()->getMotionValidator()->resetMotionCounter();
678 int v = ompl_simple_setup_->getSpaceInformation()->getMotionValidator()->getValidMotionCount();
679 int iv = ompl_simple_setup_->getSpaceInformation()->getMotionValidator()->getInvalidMotionCount();
685 res.
error_code_ = solve(request_.allowed_planning_time, request_.num_planning_attempts);
686 if (res.
error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
688 double ptime = getLastPlanTime();
689 if (simplify_solutions_)
691 simplifySolution(request_.allowed_planning_time - ptime);
692 ptime += getLastSimplifyTime();
696 interpolateSolution();
700 getOMPLSimpleSetup()->getSolutionPath().getStateCount());
702 res.
trajectory_ = std::make_shared<robot_trajectory::RobotTrajectory>(getRobotModel(), getGroupName());
716 res.
error_code_ = solve(request_.allowed_planning_time, request_.num_planning_attempts);
717 if (res.
error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
722 double ptime = getLastPlanTime();
726 res.
trajectory_.back() = std::make_shared<robot_trajectory::RobotTrajectory>(getRobotModel(), getGroupName());
730 if (simplify_solutions_)
732 simplifySolution(request_.allowed_planning_time - ptime);
736 res.
trajectory_.back() = std::make_shared<robot_trajectory::RobotTrajectory>(getRobotModel(), getGroupName());
742 ompl::time::point start_interpolate = ompl::time::now();
743 interpolateSolution();
744 res.
processing_time_.push_back(ompl::time::seconds(ompl::time::now() - start_interpolate));
747 res.
trajectory_.back() = std::make_shared<robot_trajectory::RobotTrajectory>(getRobotModel(), getGroupName());
752 getOMPLSimpleSetup()->getSolutionPath().getStateCount());
765 ompl::time::point
start = ompl::time::now();
768 moveit_msgs::MoveItErrorCodes result;
769 result.val = moveit_msgs::MoveItErrorCodes::FAILURE;
770 ob::PlannerTerminationCondition ptc = constructPlannerTerminationCondition(timeout,
start);
771 registerTerminationCondition(ptc);
772 if (count <= 1 || multi_query_planning_enabled_)
775 result.val = errorCode(ompl_simple_setup_->solve(ptc));
776 last_plan_time_ = ompl_simple_setup_->getLastPlanComputationTime();
781 ompl_parallel_plan_.clearHybridizationPaths();
783 auto plan_parallel = [
this, &ptc](
unsigned int num_planners) {
784 ompl_parallel_plan_.clearPlanners();
785 if (ompl_simple_setup_->getPlannerAllocator())
786 for (
unsigned int i = 0; i < num_planners; ++i)
787 ompl_parallel_plan_.addPlannerAllocator(ompl_simple_setup_->getPlannerAllocator());
789 for (
unsigned int i = 0; i < num_planners; ++i)
790 ompl_parallel_plan_.addPlanner(ompl::tools::SelfConfig::getDefaultPlanner(ompl_simple_setup_->getGoal()));
792 return errorCode(ompl_parallel_plan_.solve(ptc, 1, num_planners, hybridize_));
795 if (count <= max_planning_threads_)
797 result.val = plan_parallel(count);
801 int n = count / max_planning_threads_;
802 for (
int i = 0; i < n && result.val != moveit_msgs::MoveItErrorCodes::SUCCESS && !ptc(); ++i)
803 result.val = plan_parallel(max_planning_threads_);
804 if (result.val != moveit_msgs::MoveItErrorCodes::SUCCESS && !ptc())
805 result.val = plan_parallel(count % max_planning_threads_);
807 last_plan_time_ = ompl::time::seconds(ompl::time::now() -
start);
809 unregisterTerminationCondition();
816 std::unique_lock<std::mutex> slock(ptc_lock_);
822 std::unique_lock<std::mutex> slock(ptc_lock_);
828 auto result = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED;
829 switch (ompl::base::PlannerStatus::StatusType(status))
831 case ompl::base::PlannerStatus::UNKNOWN:
833 result = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED;
835 case ompl::base::PlannerStatus::INVALID_START:
837 result = moveit_msgs::MoveItErrorCodes::START_STATE_INVALID;
839 case ompl::base::PlannerStatus::INVALID_GOAL:
841 result = moveit_msgs::MoveItErrorCodes::GOAL_STATE_INVALID;
843 case ompl::base::PlannerStatus::UNRECOGNIZED_GOAL_TYPE:
845 result = moveit_msgs::MoveItErrorCodes::UNRECOGNIZED_GOAL_TYPE;
847 case ompl::base::PlannerStatus::TIMEOUT:
849 result = moveit_msgs::MoveItErrorCodes::TIMED_OUT;
851 case ompl::base::PlannerStatus::APPROXIMATE_SOLUTION:
853 result = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED;
855 case ompl::base::PlannerStatus::EXACT_SOLUTION:
856 result = moveit_msgs::MoveItErrorCodes::SUCCESS;
858 case ompl::base::PlannerStatus::CRASH:
860 result = moveit_msgs::MoveItErrorCodes::CRASH;
862 case ompl::base::PlannerStatus::ABORT:
864 result = moveit_msgs::MoveItErrorCodes::ABORT;
869 result = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED;
876 std::unique_lock<std::mutex> slock(ptc_lock_);
884 std::string constraint_path;
885 if (nh.
getParam(
"constraint_approximations_path", constraint_path))
887 constraints_library_->saveConstraintApproximations(constraint_path);
890 ROS_WARN_NAMED(
LOGNAME,
"ROS param 'constraint_approximations' not found. Unable to save constraint approximations");
896 std::string constraint_path;
897 if (nh.
getParam(
"constraint_approximations_path", constraint_path))
899 constraints_library_->loadConstraintApproximations(constraint_path);
900 std::stringstream ss;
901 constraints_library_->printConstraintApproximations(ss);