model_based_planning_context.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2012, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
37 #include <boost/algorithm/string/trim.hpp>
38 
50 
51 #include <ompl/base/samplers/UniformValidStateSampler.h>
52 #include <ompl/base/goals/GoalLazySamples.h>
53 #include <ompl/tools/config/SelfConfig.h>
54 #include <ompl/base/spaces/SE3StateSpace.h>
55 #include <ompl/datastructures/PDF.h>
56 
57 #include "ompl/base/objectives/PathLengthOptimizationObjective.h"
58 #include "ompl/base/objectives/MechanicalWorkOptimizationObjective.h"
59 #include "ompl/base/objectives/MinimaxObjective.h"
60 #include "ompl/base/objectives/StateCostIntegralObjective.h"
61 #include "ompl/base/objectives/MaximizeMinClearanceObjective.h"
62 
65  : planning_interface::PlanningContext(name, spec.state_space_->getJointModelGroup()->getName())
66  , spec_(spec)
67  , complete_initial_robot_state_(spec.state_space_->getRobotModel())
68  , ompl_simple_setup_(spec.ompl_simple_setup_)
69  , ompl_benchmark_(*ompl_simple_setup_)
70  , ompl_parallel_plan_(ompl_simple_setup_->getProblemDefinition())
71  , ptc_(NULL)
72  , last_plan_time_(0.0)
73  , last_simplify_time_(0.0)
74  , max_goal_samples_(0)
75  , max_state_sampling_attempts_(0)
76  , max_goal_sampling_attempts_(0)
77  , max_planning_threads_(0)
78  , max_solution_segment_length_(0.0)
79  , minimum_waypoint_count_(0)
80  , use_state_validity_cache_(true)
81  , simplify_solutions_(true)
82 {
84  ompl_simple_setup_->getStateSpace()->computeSignature(space_signature_);
85  ompl_simple_setup_->getStateSpace()->setStateSamplerAllocator(
87 }
88 
90 {
91  if (!spec_.state_space_)
92  {
93  ROS_ERROR_NAMED("model_based_planning_context", "No state space is configured yet");
94  return;
95  }
96  ob::ProjectionEvaluatorPtr pe = getProjectionEvaluator(peval);
97  if (pe)
98  spec_.state_space_->registerDefaultProjection(pe);
99 }
100 
101 ompl::base::ProjectionEvaluatorPtr
103 {
104  if (peval.find_first_of("link(") == 0 && peval[peval.length() - 1] == ')')
105  {
106  std::string link_name = peval.substr(5, peval.length() - 6);
107  if (getRobotModel()->hasLinkModel(link_name))
108  return ob::ProjectionEvaluatorPtr(new ProjectionEvaluatorLinkPose(this, link_name));
109  else
110  ROS_ERROR_NAMED("model_based_planning_context",
111  "Attempted to set projection evaluator with respect to position of link '%s', "
112  "but that link is not known to the kinematic model.",
113  link_name.c_str());
114  }
115  else if (peval.find_first_of("joints(") == 0 && peval[peval.length() - 1] == ')')
116  {
117  std::string joints = peval.substr(7, peval.length() - 8);
118  boost::replace_all(joints, ",", " ");
119  std::vector<unsigned int> j;
120  std::stringstream ss(joints);
121  while (ss.good() && !ss.eof())
122  {
123  std::string v;
124  ss >> v >> std::ws;
125  if (getJointModelGroup()->hasJointModel(v))
126  {
127  unsigned int vc = getJointModelGroup()->getJointModel(v)->getVariableCount();
128  if (vc > 0)
129  {
130  int idx = getJointModelGroup()->getVariableGroupIndex(v);
131  for (unsigned int q = 0; q < vc; ++q)
132  j.push_back(idx + q);
133  }
134  else
135  ROS_WARN_NAMED("model_based_planning_context", "%s: Ignoring joint '%s' in projection since it has 0 DOF",
136  name_.c_str(), v.c_str());
137  }
138  else
139  ROS_ERROR_NAMED("model_based_planning_context",
140  "%s: Attempted to set projection evaluator with respect to value of joint "
141  "'%s', but that joint is not known to the group '%s'.",
142  name_.c_str(), v.c_str(), getGroupName().c_str());
143  }
144  if (j.empty())
145  ROS_ERROR_NAMED("model_based_planning_context", "%s: No valid joints specified for joint projection",
146  name_.c_str());
147  else
148  return ob::ProjectionEvaluatorPtr(new ProjectionEvaluatorJointValue(this, j));
149  }
150  else
151  ROS_ERROR_NAMED("model_based_planning_context",
152  "Unable to allocate projection evaluator based on description: '%s'", peval.c_str());
153  return ob::ProjectionEvaluatorPtr();
154 }
155 
156 ompl::base::StateSamplerPtr
158 {
159  if (spec_.state_space_.get() != ss)
160  {
161  ROS_ERROR_NAMED("model_based_planning_context",
162  "%s: Attempted to allocate a state sampler for an unknown state space", name_.c_str());
163  return ompl::base::StateSamplerPtr();
164  }
165 
166  ROS_DEBUG_NAMED("model_based_planning_context",
167  "%s: Allocating a new state sampler (attempts to use path constraints)", name_.c_str());
168 
169  if (path_constraints_)
170  {
172  {
173  const ConstraintApproximationPtr& ca =
174  spec_.constraints_library_->getConstraintApproximation(path_constraints_msg_);
175  if (ca)
176  {
177  ompl::base::StateSamplerAllocator c_ssa = ca->getStateSamplerAllocator(path_constraints_msg_);
178  if (c_ssa)
179  {
180  ompl::base::StateSamplerPtr res = c_ssa(ss);
181  if (res)
182  {
183  ROS_INFO_NAMED("model_based_planning_context",
184  "%s: Using precomputed state sampler (approximated constraint space) for constraint '%s'",
185  name_.c_str(), path_constraints_msg_.name.c_str());
186  return res;
187  }
188  }
189  }
190  }
191 
192  constraint_samplers::ConstraintSamplerPtr cs;
195  path_constraints_->getAllConstraints());
196 
197  if (cs)
198  {
199  ROS_INFO_NAMED("model_based_planning_context", "%s: Allocating specialized state sampler for state space",
200  name_.c_str());
201  return ob::StateSamplerPtr(new ConstrainedSampler(this, cs));
202  }
203  }
204  ROS_DEBUG_NAMED("model_based_planning_context", "%s: Allocating default state sampler for state space",
205  name_.c_str());
206  return ss->allocDefaultStateSampler();
207 }
208 
210 {
211  // convert the input state to the corresponding OMPL state
212  ompl::base::ScopedState<> ompl_start_state(spec_.state_space_);
213  spec_.state_space_->copyToOMPLState(ompl_start_state.get(), getCompleteInitialRobotState());
214  ompl_simple_setup_->setStartState(ompl_start_state);
215  ompl_simple_setup_->setStateValidityChecker(ob::StateValidityCheckerPtr(new StateValidityChecker(this)));
216 
218  {
219  const ConstraintApproximationPtr& ca =
220  spec_.constraints_library_->getConstraintApproximation(path_constraints_msg_);
221  if (ca)
222  {
223  getOMPLStateSpace()->setInterpolationFunction(ca->getInterpolationFunction());
224  ROS_INFO_NAMED("model_based_planning_context", "Using precomputed interpolation states");
225  }
226  }
227 
228  useConfig();
229  if (ompl_simple_setup_->getGoal())
230  ompl_simple_setup_->setup();
231 }
232 
234 {
235  const std::map<std::string, std::string>& config = spec_.config_;
236  if (config.empty())
237  return;
238  std::map<std::string, std::string> cfg = config;
239 
240  // set the distance between waypoints when interpolating and collision checking.
241  std::map<std::string, std::string>::iterator it = cfg.find("longest_valid_segment_fraction");
242  // If one of the two variables is set.
243  if (it != cfg.end() || max_solution_segment_length_ != 0.0)
244  {
245  // clang-format off
246  double longest_valid_segment_fraction_config = (it != cfg.end())
247  ? moveit::core::toDouble(it->second) // value from config file if there
248  : 0.01; // default value in OMPL.
249  double longest_valid_segment_fraction_final = longest_valid_segment_fraction_config;
251  {
252  // If this parameter is specified too, take the most conservative of the two variables,
253  // i.e. the one that uses the shorter segment length.
254  longest_valid_segment_fraction_final = std::min(
255  longest_valid_segment_fraction_config,
256  max_solution_segment_length_ / spec_.state_space_->getMaximumExtent()
257  );
258  }
259  // clang-format on
260 
261  // convert to string using no locale
262  cfg["longest_valid_segment_fraction"] = moveit::core::toString(longest_valid_segment_fraction_final);
263  }
264 
265  // set the projection evaluator
266  it = cfg.find("projection_evaluator");
267  if (it != cfg.end())
268  {
269  setProjectionEvaluator(boost::trim_copy(it->second));
270  cfg.erase(it);
271  }
272 
273  if (cfg.empty())
274  return;
275 
276  std::string optimizer;
277  ompl::base::OptimizationObjectivePtr objective;
278  it = cfg.find("optimization_objective");
279  if (it == cfg.end())
280  {
281  optimizer = "PathLengthOptimizationObjective";
282  ROS_DEBUG_NAMED("model_based_planning_context", "No optimization objective specified, defaulting to %s",
283  optimizer.c_str());
284  }
285  else
286  {
287  optimizer = it->second;
288  cfg.erase(it);
289  }
290 
291  if (optimizer == "PathLengthOptimizationObjective")
292  {
293  objective.reset(new ompl::base::PathLengthOptimizationObjective(ompl_simple_setup_->getSpaceInformation()));
294  }
295  else if (optimizer == "MinimaxObjective")
296  {
297  objective.reset(new ompl::base::MinimaxObjective(ompl_simple_setup_->getSpaceInformation()));
298  }
299  else if (optimizer == "StateCostIntegralObjective")
300  {
301  objective.reset(new ompl::base::StateCostIntegralObjective(ompl_simple_setup_->getSpaceInformation()));
302  }
303  else if (optimizer == "MechanicalWorkOptimizationObjective")
304  {
305  objective.reset(new ompl::base::MechanicalWorkOptimizationObjective(ompl_simple_setup_->getSpaceInformation()));
306  }
307  else if (optimizer == "MaximizeMinClearanceObjective")
308  {
309  objective.reset(new ompl::base::MaximizeMinClearanceObjective(ompl_simple_setup_->getSpaceInformation()));
310  }
311  else
312  {
313  objective.reset(new ompl::base::PathLengthOptimizationObjective(ompl_simple_setup_->getSpaceInformation()));
314  }
315 
316  ompl_simple_setup_->setOptimizationObjective(objective);
317 
318  // remove the 'type' parameter; the rest are parameters for the planner itself
319  it = cfg.find("type");
320  if (it == cfg.end())
321  {
322  if (name_ != getGroupName())
323  ROS_WARN_NAMED("model_based_planning_context", "%s: Attribute 'type' not specified in planner configuration",
324  name_.c_str());
325  }
326  else
327  {
328  std::string type = it->second;
329  cfg.erase(it);
330  ompl_simple_setup_->setPlannerAllocator(
331  boost::bind(spec_.planner_selector_(type), _1, name_ != getGroupName() ? name_ : "", spec_));
332  ROS_INFO_NAMED("model_based_planning_context",
333  "Planner configuration '%s' will use planner '%s'. "
334  "Additional configuration parameters will be set when the planner is constructed.",
335  name_.c_str(), type.c_str());
336  }
337 
338  // call the setParams() after setup(), so we know what the params are
339  ompl_simple_setup_->getSpaceInformation()->setup();
340  ompl_simple_setup_->getSpaceInformation()->params().setParams(cfg, true);
341  // call setup() again for possibly new param values
342  ompl_simple_setup_->getSpaceInformation()->setup();
343 }
344 
345 void ompl_interface::ModelBasedPlanningContext::setPlanningVolume(const moveit_msgs::WorkspaceParameters& wparams)
346 {
347  if (wparams.min_corner.x == wparams.max_corner.x && wparams.min_corner.x == 0.0 &&
348  wparams.min_corner.y == wparams.max_corner.y && wparams.min_corner.y == 0.0 &&
349  wparams.min_corner.z == wparams.max_corner.z && wparams.min_corner.z == 0.0)
350  ROS_WARN_NAMED("model_based_planning_context", "It looks like the planning volume was not specified.");
351 
352  ROS_DEBUG_NAMED("model_based_planning_context",
353  "%s: Setting planning volume (affects SE2 & SE3 joints only) to x = [%f, %f], y = "
354  "[%f, %f], z = [%f, %f]",
355  name_.c_str(), wparams.min_corner.x, wparams.max_corner.x, wparams.min_corner.y, wparams.max_corner.y,
356  wparams.min_corner.z, wparams.max_corner.z);
357 
358  spec_.state_space_->setPlanningVolume(wparams.min_corner.x, wparams.max_corner.x, wparams.min_corner.y,
359  wparams.max_corner.y, wparams.min_corner.z, wparams.max_corner.z);
360 }
361 
363 {
364  ompl_simple_setup_->simplifySolution(timeout);
365  last_simplify_time_ = ompl_simple_setup_->getLastSimplificationTime();
366 }
367 
369 {
370  if (ompl_simple_setup_->haveSolutionPath())
371  {
372  og::PathGeometric& pg = ompl_simple_setup_->getSolutionPath();
373 
374  // Find the number of states that will be in the interpolated solution.
375  // This is what interpolate() does internally.
376  unsigned int eventual_states = 1;
377  std::vector<ompl::base::State*> states = pg.getStates();
378  for (size_t i = 0; i < states.size() - 1; i++)
379  {
380  eventual_states += ompl_simple_setup_->getStateSpace()->validSegmentCount(states[i], states[i + 1]);
381  }
382 
383  if (eventual_states < minimum_waypoint_count_)
384  {
385  // If that's not enough states, use the minimum amount instead.
386  pg.interpolate(minimum_waypoint_count_);
387  }
388  else
389  {
390  // Interpolate the path to have as the exact states that are checked when validating motions.
391  pg.interpolate();
392  }
393  }
394 }
395 
396 void ompl_interface::ModelBasedPlanningContext::convertPath(const ompl::geometric::PathGeometric& pg,
398 {
399  robot_state::RobotState ks = complete_initial_robot_state_;
400  for (std::size_t i = 0; i < pg.getStateCount(); ++i)
401  {
402  spec_.state_space_->copyToRobotState(ks, pg.getState(i));
403  traj.addSuffixWayPoint(ks, 0.0);
404  }
405 }
406 
408 {
409  traj.clear();
410  if (!ompl_simple_setup_->haveSolutionPath())
411  return false;
412  convertPath(ompl_simple_setup_->getSolutionPath(), traj);
413  return true;
414 }
415 
417 {
418  if (ompl_simple_setup_->getStateValidityChecker())
419  static_cast<StateValidityChecker*>(ompl_simple_setup_->getStateValidityChecker().get())->setVerbose(flag);
420 }
421 
423 {
424  // ******************* set up the goal representation, based on goal constraints
425 
426  std::vector<ob::GoalPtr> goals;
427  for (std::size_t i = 0; i < goal_constraints_.size(); ++i)
428  {
429  constraint_samplers::ConstraintSamplerPtr cs;
432  goal_constraints_[i]->getAllConstraints());
433  if (cs)
434  {
435  ob::GoalPtr g = ob::GoalPtr(new ConstrainedGoalSampler(this, goal_constraints_[i], cs));
436  goals.push_back(g);
437  }
438  }
439 
440  if (!goals.empty())
441  return goals.size() == 1 ? goals[0] : ompl::base::GoalPtr(new GoalSampleableRegionMux(goals));
442  else
443  ROS_ERROR_NAMED("model_based_planning_context", "Unable to construct goal representation");
444 
445  return ob::GoalPtr();
446 }
447 
449  const robot_state::RobotState& complete_initial_robot_state)
450 {
451  complete_initial_robot_state_ = complete_initial_robot_state;
453 }
454 
456 {
457  ompl_simple_setup_->clear();
458  ompl_simple_setup_->clearStartStates();
459  ompl_simple_setup_->setGoal(ob::GoalPtr());
460  ompl_simple_setup_->setStateValidityChecker(ob::StateValidityCheckerPtr());
461  path_constraints_.reset();
462  goal_constraints_.clear();
463  getOMPLStateSpace()->setInterpolationFunction(InterpolationFunction());
464 }
465 
466 bool ompl_interface::ModelBasedPlanningContext::setPathConstraints(const moveit_msgs::Constraints& path_constraints,
467  moveit_msgs::MoveItErrorCodes* error)
468 {
469  // ******************* set the path constraints to use
471  path_constraints_->add(path_constraints, getPlanningScene()->getTransforms());
472  path_constraints_msg_ = path_constraints;
473 
474  return true;
475 }
476 
478  const std::vector<moveit_msgs::Constraints>& goal_constraints, const moveit_msgs::Constraints& path_constraints,
479  moveit_msgs::MoveItErrorCodes* error)
480 {
481  // ******************* check if the input is correct
482  goal_constraints_.clear();
483  for (std::size_t i = 0; i < goal_constraints.size(); ++i)
484  {
485  moveit_msgs::Constraints constr = kinematic_constraints::mergeConstraints(goal_constraints[i], path_constraints);
486  kinematic_constraints::KinematicConstraintSetPtr kset(
488  kset->add(constr, getPlanningScene()->getTransforms());
489  if (!kset->empty())
490  goal_constraints_.push_back(kset);
491  }
492 
493  if (goal_constraints_.empty())
494  {
495  ROS_WARN_NAMED("model_based_planning_context", "%s: No goal constraints specified. There is no problem to solve.",
496  name_.c_str());
497  if (error)
498  error->val = moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS;
499  return false;
500  }
501 
502  ob::GoalPtr goal = constructGoal();
503  ompl_simple_setup_->setGoal(goal);
504  if (goal)
505  return true;
506  else
507  return false;
508 }
509 
510 bool ompl_interface::ModelBasedPlanningContext::benchmark(double timeout, unsigned int count,
511  const std::string& filename)
512 {
513  ompl_benchmark_.clearPlanners();
514  ompl_simple_setup_->setup();
515  ompl_benchmark_.addPlanner(ompl_simple_setup_->getPlanner());
516  ompl_benchmark_.setExperimentName(getRobotModel()->getName() + "_" + getGroupName() + "_" +
517  getPlanningScene()->getName() + "_" + name_);
518 
519  ot::Benchmark::Request req;
520  req.maxTime = timeout;
521  req.runCount = count;
522  req.displayProgress = true;
523  req.saveConsoleOutput = false;
524  ompl_benchmark_.benchmark(req);
525  return filename.empty() ? ompl_benchmark_.saveResultsToFile() : ompl_benchmark_.saveResultsToFile(filename.c_str());
526 }
527 
529 {
530  bool gls = ompl_simple_setup_->getGoal()->hasType(ob::GOAL_LAZY_SAMPLES);
531  if (gls)
532  static_cast<ob::GoalLazySamples*>(ompl_simple_setup_->getGoal().get())->startSampling();
533  else
534  // we know this is a GoalSampleableMux by elimination
535  static_cast<GoalSampleableRegionMux*>(ompl_simple_setup_->getGoal().get())->startSampling();
536 }
537 
539 {
540  bool gls = ompl_simple_setup_->getGoal()->hasType(ob::GOAL_LAZY_SAMPLES);
541  if (gls)
542  static_cast<ob::GoalLazySamples*>(ompl_simple_setup_->getGoal().get())->stopSampling();
543  else
544  // we know this is a GoalSampleableMux by elimination
545  static_cast<GoalSampleableRegionMux*>(ompl_simple_setup_->getGoal().get())->stopSampling();
546 }
547 
549 {
550  // clear previously computed solutions
551  ompl_simple_setup_->getProblemDefinition()->clearSolutionPaths();
552  const ob::PlannerPtr planner = ompl_simple_setup_->getPlanner();
553  if (planner)
554  planner->clear();
555  startSampling();
556  ompl_simple_setup_->getSpaceInformation()->getMotionValidator()->resetMotionCounter();
557 }
558 
560 {
561  stopSampling();
562  int v = ompl_simple_setup_->getSpaceInformation()->getMotionValidator()->getValidMotionCount();
563  int iv = ompl_simple_setup_->getSpaceInformation()->getMotionValidator()->getInvalidMotionCount();
564  ROS_DEBUG_NAMED("model_based_planning_context", "There were %d valid motions and %d invalid motions.", v, iv);
565 
566  if (ompl_simple_setup_->getProblemDefinition()->hasApproximateSolution())
567  ROS_WARN_NAMED("model_based_planning_context", "Computed solution is approximate");
568 }
569 
571 {
572  if (solve(request_.allowed_planning_time, request_.num_planning_attempts))
573  {
574  double ptime = getLastPlanTime();
576  {
577  simplifySolution(request_.allowed_planning_time - ptime);
578  ptime += getLastSimplifyTime();
579  }
581 
582  // fill the response
583  ROS_DEBUG_NAMED("model_based_planning_context", "%s: Returning successful solution with %lu states",
584  getName().c_str(), getOMPLSimpleSetup()->getSolutionPath().getStateCount());
585 
588  res.planning_time_ = ptime;
589  return true;
590  }
591  else
592  {
593  ROS_INFO_NAMED("model_based_planning_context", "Unable to solve the planning problem");
594  res.error_code_.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED;
595  return false;
596  }
597 }
598 
600 {
601  if (solve(request_.allowed_planning_time, request_.num_planning_attempts))
602  {
603  res.trajectory_.reserve(3);
604 
605  // add info about planned solution
606  double ptime = getLastPlanTime();
607  res.processing_time_.push_back(ptime);
608  res.description_.push_back("plan");
609  res.trajectory_.resize(res.trajectory_.size() + 1);
611  getSolutionPath(*res.trajectory_.back());
612 
613  // simplify solution if time remains
615  {
616  simplifySolution(request_.allowed_planning_time - ptime);
617  res.processing_time_.push_back(getLastSimplifyTime());
618  res.description_.push_back("simplify");
619  res.trajectory_.resize(res.trajectory_.size() + 1);
621  getSolutionPath(*res.trajectory_.back());
622  }
623 
624  ompl::time::point start_interpolate = ompl::time::now();
626  res.processing_time_.push_back(ompl::time::seconds(ompl::time::now() - start_interpolate));
627  res.description_.push_back("interpolate");
628  res.trajectory_.resize(res.trajectory_.size() + 1);
630  getSolutionPath(*res.trajectory_.back());
631 
632  // fill the response
633  ROS_DEBUG_NAMED("model_based_planning_context", "%s: Returning successful solution with %lu states",
634  getName().c_str(), getOMPLSimpleSetup()->getSolutionPath().getStateCount());
635  return true;
636  }
637  else
638  {
639  ROS_INFO_NAMED("model_based_planning_context", "Unable to solve the planning problem");
640  res.error_code_.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED;
641  return false;
642  }
643 }
644 
645 bool ompl_interface::ModelBasedPlanningContext::solve(double timeout, unsigned int count)
646 {
647  moveit::tools::Profiler::ScopedBlock sblock("PlanningContext:Solve");
648  ompl::time::point start = ompl::time::now();
649  preSolve();
650 
651  bool result = false;
652  if (count <= 1)
653  {
654  ROS_DEBUG_NAMED("model_based_planning_context", "%s: Solving the planning problem once...", name_.c_str());
655  ob::PlannerTerminationCondition ptc =
656  ob::timedPlannerTerminationCondition(timeout - ompl::time::seconds(ompl::time::now() - start));
658  result = ompl_simple_setup_->solve(ptc) == ompl::base::PlannerStatus::EXACT_SOLUTION;
659  last_plan_time_ = ompl_simple_setup_->getLastPlanComputationTime();
661  }
662  else
663  {
664  ROS_DEBUG_NAMED("model_based_planning_context", "%s: Solving the planning problem %u times...", name_.c_str(),
665  count);
666  ompl_parallel_plan_.clearHybridizationPaths();
667  if (count <= max_planning_threads_)
668  {
669  ompl_parallel_plan_.clearPlanners();
670  if (ompl_simple_setup_->getPlannerAllocator())
671  for (unsigned int i = 0; i < count; ++i)
672  ompl_parallel_plan_.addPlannerAllocator(ompl_simple_setup_->getPlannerAllocator());
673  else
674  for (unsigned int i = 0; i < count; ++i)
675  ompl_parallel_plan_.addPlanner(ompl::tools::SelfConfig::getDefaultPlanner(ompl_simple_setup_->getGoal()));
676 
677  ob::PlannerTerminationCondition ptc =
678  ob::timedPlannerTerminationCondition(timeout - ompl::time::seconds(ompl::time::now() - start));
680  result = ompl_parallel_plan_.solve(ptc, 1, count, true) == ompl::base::PlannerStatus::EXACT_SOLUTION;
681  last_plan_time_ = ompl::time::seconds(ompl::time::now() - start);
683  }
684  else
685  {
686  ob::PlannerTerminationCondition ptc =
687  ob::timedPlannerTerminationCondition(timeout - ompl::time::seconds(ompl::time::now() - start));
689  int n = count / max_planning_threads_;
690  result = true;
691  for (int i = 0; i < n && ptc() == false; ++i)
692  {
693  ompl_parallel_plan_.clearPlanners();
694  if (ompl_simple_setup_->getPlannerAllocator())
695  for (unsigned int i = 0; i < max_planning_threads_; ++i)
696  ompl_parallel_plan_.addPlannerAllocator(ompl_simple_setup_->getPlannerAllocator());
697  else
698  for (unsigned int i = 0; i < max_planning_threads_; ++i)
699  ompl_parallel_plan_.addPlanner(ompl::tools::SelfConfig::getDefaultPlanner(ompl_simple_setup_->getGoal()));
700  bool r = ompl_parallel_plan_.solve(ptc, 1, count, true) == ompl::base::PlannerStatus::EXACT_SOLUTION;
701  result = result && r;
702  }
703  n = count % max_planning_threads_;
704  if (n && ptc() == false)
705  {
706  ompl_parallel_plan_.clearPlanners();
707  if (ompl_simple_setup_->getPlannerAllocator())
708  for (int i = 0; i < n; ++i)
709  ompl_parallel_plan_.addPlannerAllocator(ompl_simple_setup_->getPlannerAllocator());
710  else
711  for (int i = 0; i < n; ++i)
712  ompl_parallel_plan_.addPlanner(ompl::tools::SelfConfig::getDefaultPlanner(ompl_simple_setup_->getGoal()));
713  bool r = ompl_parallel_plan_.solve(ptc, 1, count, true) == ompl::base::PlannerStatus::EXACT_SOLUTION;
714  result = result && r;
715  }
716  last_plan_time_ = ompl::time::seconds(ompl::time::now() - start);
718  }
719  }
720 
721  postSolve();
722 
723  return result;
724 }
725 
726 void ompl_interface::ModelBasedPlanningContext::registerTerminationCondition(const ob::PlannerTerminationCondition& ptc)
727 {
728  boost::mutex::scoped_lock slock(ptc_lock_);
729  ptc_ = &ptc;
730 }
731 
733 {
734  boost::mutex::scoped_lock slock(ptc_lock_);
735  ptc_ = NULL;
736 }
737 
739 {
740  boost::mutex::scoped_lock slock(ptc_lock_);
741  if (ptc_)
742  ptc_->terminate();
743  return true;
744 }
double last_simplify_time_
the time spent simplifying the last plan
robot_trajectory::RobotTrajectoryPtr trajectory_
ot::Benchmark ompl_benchmark_
the OMPL tool for benchmarking planners
#define ROS_INFO_NAMED(name,...)
#define ROS_WARN_NAMED(name,...)
double toDouble(const std::string &s)
std::string getName(void *handle)
const og::SimpleSetupPtr & getOMPLSimpleSetup() const
const robot_model::JointModelGroup * getJointModelGroup() const
void setPlanningVolume(const moveit_msgs::WorkspaceParameters &wparams)
const robot_state::RobotState & getCompleteInitialRobotState() const
const planning_scene::PlanningSceneConstPtr & getPlanningScene() const
moveit_msgs::MoveItErrorCodes error_code_
unsigned int max_planning_threads_
when planning in parallel, this is the maximum number of threads to use at one time ...
#define ROS_DEBUG_NAMED(name,...)
kinematic_constraints::KinematicConstraintSetPtr path_constraints_
ModelBasedPlanningContext(const std::string &name, const ModelBasedPlanningContextSpecification &spec)
double last_plan_time_
the time spent computing the last plan
const ob::PlannerTerminationCondition * ptc_
ot::ParallelPlan ompl_parallel_plan_
tool used to compute multiple plans in parallel; this uses the problem definition maintained by ompl_...
moveit_msgs::MoveItErrorCodes error_code_
virtual ob::ProjectionEvaluatorPtr getProjectionEvaluator(const std::string &peval) const
virtual ob::StateSamplerPtr allocPathConstrainedSampler(const ompl::base::StateSpace *ss) const
og::SimpleSetupPtr ompl_simple_setup_
the OMPL planning context; this contains the problem definition and the planner used ...
ModelBasedPlanningContextSpecification spec_
const std::string & getGroupName() const
std::string toString(double d)
bool setGoalConstraints(const std::vector< moveit_msgs::Constraints > &goal_constraints, const moveit_msgs::Constraints &path_constraints, moveit_msgs::MoveItErrorCodes *error)
void registerTerminationCondition(const ob::PlannerTerminationCondition &ptc)
constraint_samplers::ConstraintSamplerManagerPtr constraint_sampler_manager_
bool benchmark(double timeout, unsigned int count, const std::string &filename="")
std::vector< kinematic_constraints::KinematicConstraintSetPtr > goal_constraints_
void convertPath(const og::PathGeometric &pg, robot_trajectory::RobotTrajectory &traj) const
std::vector< robot_trajectory::RobotTrajectoryPtr > trajectory_
const std::string & getName() const
bool setPathConstraints(const moveit_msgs::Constraints &path_constraints, moveit_msgs::MoveItErrorCodes *error)
#define ROS_ERROR_NAMED(name,...)
moveit_msgs::Constraints mergeConstraints(const moveit_msgs::Constraints &first, const moveit_msgs::Constraints &second)
An interface for a OMPL state validity checker.
bool getSolutionPath(robot_trajectory::RobotTrajectory &traj) const
void addSuffixWayPoint(const robot_state::RobotState &state, double dt)
const robot_model::RobotModelConstPtr & getRobotModel() const
r
virtual bool solve(planning_interface::MotionPlanResponse &res)
boost::function< bool(const ompl::base::State *from, const ompl::base::State *to, const double t, ompl::base::State *state)> InterpolationFunction
void setCompleteInitialState(const robot_state::RobotState &complete_initial_robot_state)
const ModelBasedStateSpacePtr & getOMPLStateSpace() const


ompl
Author(s): Ioan Sucan
autogenerated on Mon Jun 10 2019 14:08:33