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 
49 
50 #include <ompl/base/samplers/UniformValidStateSampler.h>
51 #include <ompl/base/goals/GoalLazySamples.h>
52 #include <ompl/tools/config/SelfConfig.h>
53 #include <ompl/base/spaces/SE3StateSpace.h>
54 #include <ompl/datastructures/PDF.h>
55 
56 #include "ompl/base/objectives/PathLengthOptimizationObjective.h"
57 #include "ompl/base/objectives/MechanicalWorkOptimizationObjective.h"
58 #include "ompl/base/objectives/MinimaxObjective.h"
59 #include "ompl/base/objectives/StateCostIntegralObjective.h"
60 #include "ompl/base/objectives/MaximizeMinClearanceObjective.h"
61 
64  : planning_interface::PlanningContext(name, spec.state_space_->getJointModelGroup()->getName())
65  , spec_(spec)
66  , complete_initial_robot_state_(spec.state_space_->getRobotModel())
67  , ompl_simple_setup_(spec.ompl_simple_setup_)
68  , ompl_benchmark_(*ompl_simple_setup_)
69  , ompl_parallel_plan_(ompl_simple_setup_->getProblemDefinition())
70  , ptc_(NULL)
71  , last_plan_time_(0.0)
72  , last_simplify_time_(0.0)
73  , max_goal_samples_(0)
74  , max_state_sampling_attempts_(0)
75  , max_goal_sampling_attempts_(0)
76  , max_planning_threads_(0)
77  , max_solution_segment_length_(0.0)
78  , minimum_waypoint_count_(0)
79  , use_state_validity_cache_(true)
80  , simplify_solutions_(true)
81 {
83  ompl_simple_setup_->getStateSpace()->computeSignature(space_signature_);
84  ompl_simple_setup_->getStateSpace()->setStateSamplerAllocator(
86 }
87 
89 {
90  if (!spec_.state_space_)
91  {
92  logError("No state space is configured yet");
93  return;
94  }
95  ob::ProjectionEvaluatorPtr pe = getProjectionEvaluator(peval);
96  if (pe)
97  spec_.state_space_->registerDefaultProjection(pe);
98 }
99 
100 ompl::base::ProjectionEvaluatorPtr
102 {
103  if (peval.find_first_of("link(") == 0 && peval[peval.length() - 1] == ')')
104  {
105  std::string link_name = peval.substr(5, peval.length() - 6);
106  if (getRobotModel()->hasLinkModel(link_name))
107  return ob::ProjectionEvaluatorPtr(new ProjectionEvaluatorLinkPose(this, link_name));
108  else
109  logError("Attempted to set projection evaluator with respect to position of link '%s', but that link is not "
110  "known to the kinematic model.",
111  link_name.c_str());
112  }
113  else if (peval.find_first_of("joints(") == 0 && peval[peval.length() - 1] == ')')
114  {
115  std::string joints = peval.substr(7, peval.length() - 8);
116  boost::replace_all(joints, ",", " ");
117  std::vector<unsigned int> j;
118  std::stringstream ss(joints);
119  while (ss.good() && !ss.eof())
120  {
121  std::string v;
122  ss >> v >> std::ws;
123  if (getJointModelGroup()->hasJointModel(v))
124  {
125  unsigned int vc = getJointModelGroup()->getJointModel(v)->getVariableCount();
126  if (vc > 0)
127  {
128  int idx = getJointModelGroup()->getVariableGroupIndex(v);
129  for (int q = 0; q < vc; ++q)
130  j.push_back(idx + q);
131  }
132  else
133  logWarn("%s: Ignoring joint '%s' in projection since it has 0 DOF", name_.c_str(), v.c_str());
134  }
135  else
136  logError("%s: Attempted to set projection evaluator with respect to value of joint '%s', but that joint is not "
137  "known to the group '%s'.",
138  name_.c_str(), v.c_str(), getGroupName().c_str());
139  }
140  if (j.empty())
141  logError("%s: No valid joints specified for joint projection", name_.c_str());
142  else
143  return ob::ProjectionEvaluatorPtr(new ProjectionEvaluatorJointValue(this, j));
144  }
145  else
146  logError("Unable to allocate projection evaluator based on description: '%s'", peval.c_str());
147  return ob::ProjectionEvaluatorPtr();
148 }
149 
150 ompl::base::StateSamplerPtr
152 {
153  if (spec_.state_space_.get() != ss)
154  {
155  logError("%s: Attempted to allocate a state sampler for an unknown state space", name_.c_str());
156  return ompl::base::StateSamplerPtr();
157  }
158 
159  logDebug("%s: Allocating a new state sampler (attempts to use path constraints)", name_.c_str());
160 
161  if (path_constraints_)
162  {
164  {
165  const ConstraintApproximationPtr& ca =
166  spec_.constraints_library_->getConstraintApproximation(path_constraints_msg_);
167  if (ca)
168  {
169  ompl::base::StateSamplerAllocator c_ssa = ca->getStateSamplerAllocator(path_constraints_msg_);
170  if (c_ssa)
171  {
172  ompl::base::StateSamplerPtr res = c_ssa(ss);
173  if (res)
174  {
175  logInform("%s: Using precomputed state sampler (approximated constraint space) for constraint '%s'",
176  name_.c_str(), path_constraints_msg_.name.c_str());
177  return res;
178  }
179  }
180  }
181  }
182 
183  constraint_samplers::ConstraintSamplerPtr cs;
186  path_constraints_->getAllConstraints());
187 
188  if (cs)
189  {
190  logInform("%s: Allocating specialized state sampler for state space", name_.c_str());
191  return ob::StateSamplerPtr(new ConstrainedSampler(this, cs));
192  }
193  }
194  logDebug("%s: Allocating default state sampler for state space", name_.c_str());
195  return ss->allocDefaultStateSampler();
196 }
197 
199 {
200  // convert the input state to the corresponding OMPL state
201  ompl::base::ScopedState<> ompl_start_state(spec_.state_space_);
202  spec_.state_space_->copyToOMPLState(ompl_start_state.get(), getCompleteInitialRobotState());
203  ompl_simple_setup_->setStartState(ompl_start_state);
204  ompl_simple_setup_->setStateValidityChecker(ob::StateValidityCheckerPtr(new StateValidityChecker(this)));
205 
207  {
208  const ConstraintApproximationPtr& ca =
209  spec_.constraints_library_->getConstraintApproximation(path_constraints_msg_);
210  if (ca)
211  {
212  getOMPLStateSpace()->setInterpolationFunction(ca->getInterpolationFunction());
213  logInform("Using precomputed interpolation states");
214  }
215  }
216 
217  useConfig();
218  if (ompl_simple_setup_->getGoal())
219  ompl_simple_setup_->setup();
220 }
221 
223 {
224  const std::map<std::string, std::string>& config = spec_.config_;
225  if (config.empty())
226  return;
227  std::map<std::string, std::string> cfg = config;
228 
229  // set the projection evaluator
230  std::map<std::string, std::string>::iterator it = cfg.find("projection_evaluator");
231  if (it != cfg.end())
232  {
233  setProjectionEvaluator(boost::trim_copy(it->second));
234  cfg.erase(it);
235  }
236 
237  if (cfg.empty())
238  return;
239 
240  std::string optimizer;
241  ompl::base::OptimizationObjectivePtr objective;
242  it = cfg.find("optimization_objective");
243  if (it == cfg.end())
244  {
245  optimizer = "PathLengthOptimizationObjective";
246  logDebug("No optimization objective specified, defaulting to %s", optimizer.c_str());
247  }
248  else
249  {
250  optimizer = it->second;
251  cfg.erase(it);
252  }
253 
254  if (optimizer == "PathLengthOptimizationObjective")
255  {
256  objective.reset(new ompl::base::PathLengthOptimizationObjective(ompl_simple_setup_->getSpaceInformation()));
257  }
258  else if (optimizer == "MinimaxObjective")
259  {
260  objective.reset(new ompl::base::MinimaxObjective(ompl_simple_setup_->getSpaceInformation()));
261  }
262  else if (optimizer == "StateCostIntegralObjective")
263  {
264  objective.reset(new ompl::base::StateCostIntegralObjective(ompl_simple_setup_->getSpaceInformation()));
265  }
266  else if (optimizer == "MechanicalWorkOptimizationObjective")
267  {
268  objective.reset(new ompl::base::MechanicalWorkOptimizationObjective(ompl_simple_setup_->getSpaceInformation()));
269  }
270  else if (optimizer == "MaximizeMinClearanceObjective")
271  {
272  objective.reset(new ompl::base::MaximizeMinClearanceObjective(ompl_simple_setup_->getSpaceInformation()));
273  }
274  else
275  {
276  objective.reset(new ompl::base::PathLengthOptimizationObjective(ompl_simple_setup_->getSpaceInformation()));
277  }
278 
279  ompl_simple_setup_->setOptimizationObjective(objective);
280 
281  // remove the 'type' parameter; the rest are parameters for the planner itself
282  it = cfg.find("type");
283  if (it == cfg.end())
284  {
285  if (name_ != getGroupName())
286  logWarn("%s: Attribute 'type' not specified in planner configuration", name_.c_str());
287  }
288  else
289  {
290  std::string type = it->second;
291  cfg.erase(it);
292  ompl_simple_setup_->setPlannerAllocator(
293  boost::bind(spec_.planner_selector_(type), _1, name_ != getGroupName() ? name_ : "", spec_));
294  logInform("Planner configuration '%s' will use planner '%s'. Additional configuration parameters will be set when "
295  "the planner is constructed.",
296  name_.c_str(), type.c_str());
297  }
298 
299  // call the setParams() after setup(), so we know what the params are
300  ompl_simple_setup_->getSpaceInformation()->setup();
301  ompl_simple_setup_->getSpaceInformation()->params().setParams(cfg, true);
302  // call setup() again for possibly new param values
303  ompl_simple_setup_->getSpaceInformation()->setup();
304 }
305 
306 void ompl_interface::ModelBasedPlanningContext::setPlanningVolume(const moveit_msgs::WorkspaceParameters& wparams)
307 {
308  if (wparams.min_corner.x == wparams.max_corner.x && wparams.min_corner.x == 0.0 &&
309  wparams.min_corner.y == wparams.max_corner.y && wparams.min_corner.y == 0.0 &&
310  wparams.min_corner.z == wparams.max_corner.z && wparams.min_corner.z == 0.0)
311  logWarn("It looks like the planning volume was not specified.");
312 
313  logDebug("%s: Setting planning volume (affects SE2 & SE3 joints only) to x = [%f, %f], y = [%f, %f], z = [%f, %f]",
314  name_.c_str(), wparams.min_corner.x, wparams.max_corner.x, wparams.min_corner.y, wparams.max_corner.y,
315  wparams.min_corner.z, wparams.max_corner.z);
316 
317  spec_.state_space_->setPlanningVolume(wparams.min_corner.x, wparams.max_corner.x, wparams.min_corner.y,
318  wparams.max_corner.y, wparams.min_corner.z, wparams.max_corner.z);
319 }
320 
322 {
323  ompl_simple_setup_->simplifySolution(timeout);
324  last_simplify_time_ = ompl_simple_setup_->getLastSimplificationTime();
325 }
326 
328 {
329  if (ompl_simple_setup_->haveSolutionPath())
330  {
331  og::PathGeometric& pg = ompl_simple_setup_->getSolutionPath();
332  pg.interpolate(
333  std::max((unsigned int)floor(0.5 + pg.length() / max_solution_segment_length_), minimum_waypoint_count_));
334  }
335 }
336 
337 void ompl_interface::ModelBasedPlanningContext::convertPath(const ompl::geometric::PathGeometric& pg,
339 {
340  robot_state::RobotState ks = complete_initial_robot_state_;
341  for (std::size_t i = 0; i < pg.getStateCount(); ++i)
342  {
343  spec_.state_space_->copyToRobotState(ks, pg.getState(i));
344  traj.addSuffixWayPoint(ks, 0.0);
345  }
346 }
347 
349 {
350  traj.clear();
351  if (!ompl_simple_setup_->haveSolutionPath())
352  return false;
353  convertPath(ompl_simple_setup_->getSolutionPath(), traj);
354  return true;
355 }
356 
358 {
359  if (ompl_simple_setup_->getStateValidityChecker())
360  static_cast<StateValidityChecker*>(ompl_simple_setup_->getStateValidityChecker().get())->setVerbose(flag);
361 }
362 
364 {
365  // ******************* set up the goal representation, based on goal constraints
366 
367  std::vector<ob::GoalPtr> goals;
368  for (std::size_t i = 0; i < goal_constraints_.size(); ++i)
369  {
370  constraint_samplers::ConstraintSamplerPtr cs;
373  goal_constraints_[i]->getAllConstraints());
374  if (cs)
375  {
376  ob::GoalPtr g = ob::GoalPtr(new ConstrainedGoalSampler(this, goal_constraints_[i], cs));
377  goals.push_back(g);
378  }
379  }
380 
381  if (!goals.empty())
382  return goals.size() == 1 ? goals[0] : ompl::base::GoalPtr(new GoalSampleableRegionMux(goals));
383  else
384  logError("Unable to construct goal representation");
385 
386  return ob::GoalPtr();
387 }
388 
390  const robot_state::RobotState& complete_initial_robot_state)
391 {
392  complete_initial_robot_state_ = complete_initial_robot_state;
394 }
395 
397 {
398  ompl_simple_setup_->clear();
399  ompl_simple_setup_->clearStartStates();
400  ompl_simple_setup_->setGoal(ob::GoalPtr());
401  ompl_simple_setup_->setStateValidityChecker(ob::StateValidityCheckerPtr());
402  path_constraints_.reset();
403  goal_constraints_.clear();
404  getOMPLStateSpace()->setInterpolationFunction(InterpolationFunction());
405 }
406 
407 bool ompl_interface::ModelBasedPlanningContext::setPathConstraints(const moveit_msgs::Constraints& path_constraints,
408  moveit_msgs::MoveItErrorCodes* error)
409 {
410  // ******************* set the path constraints to use
412  path_constraints_->add(path_constraints, getPlanningScene()->getTransforms());
413  path_constraints_msg_ = path_constraints;
414 
415  return true;
416 }
417 
419  const std::vector<moveit_msgs::Constraints>& goal_constraints, const moveit_msgs::Constraints& path_constraints,
420  moveit_msgs::MoveItErrorCodes* error)
421 {
422  // ******************* check if the input is correct
423  goal_constraints_.clear();
424  for (std::size_t i = 0; i < goal_constraints.size(); ++i)
425  {
426  moveit_msgs::Constraints constr = kinematic_constraints::mergeConstraints(goal_constraints[i], path_constraints);
427  kinematic_constraints::KinematicConstraintSetPtr kset(
429  kset->add(constr, getPlanningScene()->getTransforms());
430  if (!kset->empty())
431  goal_constraints_.push_back(kset);
432  }
433 
434  if (goal_constraints_.empty())
435  {
436  logWarn("%s: No goal constraints specified. There is no problem to solve.", name_.c_str());
437  if (error)
438  error->val = moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS;
439  return false;
440  }
441 
442  ob::GoalPtr goal = constructGoal();
443  ompl_simple_setup_->setGoal(goal);
444  if (goal)
445  return true;
446  else
447  return false;
448 }
449 
450 bool ompl_interface::ModelBasedPlanningContext::benchmark(double timeout, unsigned int count,
451  const std::string& filename)
452 {
453  ompl_benchmark_.clearPlanners();
454  ompl_simple_setup_->setup();
455  ompl_benchmark_.addPlanner(ompl_simple_setup_->getPlanner());
456  ompl_benchmark_.setExperimentName(getRobotModel()->getName() + "_" + getGroupName() + "_" +
457  getPlanningScene()->getName() + "_" + name_);
458 
459  ot::Benchmark::Request req;
460  req.maxTime = timeout;
461  req.runCount = count;
462  req.displayProgress = true;
463  req.saveConsoleOutput = false;
464  ompl_benchmark_.benchmark(req);
465  return filename.empty() ? ompl_benchmark_.saveResultsToFile() : ompl_benchmark_.saveResultsToFile(filename.c_str());
466 }
467 
469 {
470  bool gls = ompl_simple_setup_->getGoal()->hasType(ob::GOAL_LAZY_SAMPLES);
471  if (gls)
472  static_cast<ob::GoalLazySamples*>(ompl_simple_setup_->getGoal().get())->startSampling();
473  else
474  // we know this is a GoalSampleableMux by elimination
475  static_cast<GoalSampleableRegionMux*>(ompl_simple_setup_->getGoal().get())->startSampling();
476 }
477 
479 {
480  bool gls = ompl_simple_setup_->getGoal()->hasType(ob::GOAL_LAZY_SAMPLES);
481  if (gls)
482  static_cast<ob::GoalLazySamples*>(ompl_simple_setup_->getGoal().get())->stopSampling();
483  else
484  // we know this is a GoalSampleableMux by elimination
485  static_cast<GoalSampleableRegionMux*>(ompl_simple_setup_->getGoal().get())->stopSampling();
486 }
487 
489 {
490  // clear previously computed solutions
491  ompl_simple_setup_->getProblemDefinition()->clearSolutionPaths();
492  const ob::PlannerPtr planner = ompl_simple_setup_->getPlanner();
493  if (planner)
494  planner->clear();
495  startSampling();
496  ompl_simple_setup_->getSpaceInformation()->getMotionValidator()->resetMotionCounter();
497 }
498 
500 {
501  stopSampling();
502  int v = ompl_simple_setup_->getSpaceInformation()->getMotionValidator()->getValidMotionCount();
503  int iv = ompl_simple_setup_->getSpaceInformation()->getMotionValidator()->getInvalidMotionCount();
504  logDebug("There were %d valid motions and %d invalid motions.", v, iv);
505 
506  if (ompl_simple_setup_->getProblemDefinition()->hasApproximateSolution())
507  logWarn("Computed solution is approximate");
508 }
509 
511 {
512  if (solve(request_.allowed_planning_time, request_.num_planning_attempts))
513  {
514  double ptime = getLastPlanTime();
515  if (simplify_solutions_ && ptime < request_.allowed_planning_time)
516  {
517  simplifySolution(request_.allowed_planning_time - ptime);
518  ptime += getLastSimplifyTime();
519  }
521 
522  // fill the response
523  logDebug("%s: Returning successful solution with %lu states", getName().c_str(),
524  getOMPLSimpleSetup()->getSolutionPath().getStateCount());
525 
528  res.planning_time_ = ptime;
529  return true;
530  }
531  else
532  {
533  logInform("Unable to solve the planning problem");
534  res.error_code_.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED;
535  return false;
536  }
537 }
538 
540 {
541  if (solve(request_.allowed_planning_time, request_.num_planning_attempts))
542  {
543  res.trajectory_.reserve(3);
544 
545  // add info about planned solution
546  double ptime = getLastPlanTime();
547  res.processing_time_.push_back(ptime);
548  res.description_.push_back("plan");
549  res.trajectory_.resize(res.trajectory_.size() + 1);
551  getSolutionPath(*res.trajectory_.back());
552 
553  // simplify solution if time remains
554  if (simplify_solutions_ && ptime < request_.allowed_planning_time)
555  {
556  simplifySolution(request_.allowed_planning_time - ptime);
557  res.processing_time_.push_back(getLastSimplifyTime());
558  res.description_.push_back("simplify");
559  res.trajectory_.resize(res.trajectory_.size() + 1);
561  getSolutionPath(*res.trajectory_.back());
562  }
563 
564  ompl::time::point start_interpolate = ompl::time::now();
566  res.processing_time_.push_back(ompl::time::seconds(ompl::time::now() - start_interpolate));
567  res.description_.push_back("interpolate");
568  res.trajectory_.resize(res.trajectory_.size() + 1);
570  getSolutionPath(*res.trajectory_.back());
571 
572  // fill the response
573  logDebug("%s: Returning successful solution with %lu states", getName().c_str(),
574  getOMPLSimpleSetup()->getSolutionPath().getStateCount());
575  return true;
576  }
577  else
578  {
579  logInform("Unable to solve the planning problem");
580  res.error_code_.val = moveit_msgs::MoveItErrorCodes::PLANNING_FAILED;
581  return false;
582  }
583 }
584 
585 bool ompl_interface::ModelBasedPlanningContext::solve(double timeout, unsigned int count)
586 {
587  moveit::tools::Profiler::ScopedBlock sblock("PlanningContext:Solve");
588  ompl::time::point start = ompl::time::now();
589  preSolve();
590 
591  bool result = false;
592  if (count <= 1)
593  {
594  logDebug("%s: Solving the planning problem once...", name_.c_str());
595  ob::PlannerTerminationCondition ptc =
596  ob::timedPlannerTerminationCondition(timeout - ompl::time::seconds(ompl::time::now() - start));
598  result = ompl_simple_setup_->solve(ptc) == ompl::base::PlannerStatus::EXACT_SOLUTION;
599  last_plan_time_ = ompl_simple_setup_->getLastPlanComputationTime();
601  }
602  else
603  {
604  logDebug("%s: Solving the planning problem %u times...", name_.c_str(), count);
605  ompl_parallel_plan_.clearHybridizationPaths();
606  if (count <= max_planning_threads_)
607  {
608  ompl_parallel_plan_.clearPlanners();
609  if (ompl_simple_setup_->getPlannerAllocator())
610  for (unsigned int i = 0; i < count; ++i)
611  ompl_parallel_plan_.addPlannerAllocator(ompl_simple_setup_->getPlannerAllocator());
612  else
613  for (unsigned int i = 0; i < count; ++i)
614  ompl_parallel_plan_.addPlanner(ompl::tools::SelfConfig::getDefaultPlanner(ompl_simple_setup_->getGoal()));
615 
616  ob::PlannerTerminationCondition ptc =
617  ob::timedPlannerTerminationCondition(timeout - ompl::time::seconds(ompl::time::now() - start));
619  result = ompl_parallel_plan_.solve(ptc, 1, count, true) == ompl::base::PlannerStatus::EXACT_SOLUTION;
620  last_plan_time_ = ompl::time::seconds(ompl::time::now() - start);
622  }
623  else
624  {
625  ob::PlannerTerminationCondition ptc =
626  ob::timedPlannerTerminationCondition(timeout - ompl::time::seconds(ompl::time::now() - start));
628  int n = count / max_planning_threads_;
629  result = true;
630  for (int i = 0; i < n && ptc() == false; ++i)
631  {
632  ompl_parallel_plan_.clearPlanners();
633  if (ompl_simple_setup_->getPlannerAllocator())
634  for (unsigned int i = 0; i < max_planning_threads_; ++i)
635  ompl_parallel_plan_.addPlannerAllocator(ompl_simple_setup_->getPlannerAllocator());
636  else
637  for (unsigned int i = 0; i < max_planning_threads_; ++i)
638  ompl_parallel_plan_.addPlanner(ompl::tools::SelfConfig::getDefaultPlanner(ompl_simple_setup_->getGoal()));
639  bool r = ompl_parallel_plan_.solve(ptc, 1, count, true) == ompl::base::PlannerStatus::EXACT_SOLUTION;
640  result = result && r;
641  }
642  n = count % max_planning_threads_;
643  if (n && ptc() == false)
644  {
645  ompl_parallel_plan_.clearPlanners();
646  if (ompl_simple_setup_->getPlannerAllocator())
647  for (int i = 0; i < n; ++i)
648  ompl_parallel_plan_.addPlannerAllocator(ompl_simple_setup_->getPlannerAllocator());
649  else
650  for (int i = 0; i < n; ++i)
651  ompl_parallel_plan_.addPlanner(ompl::tools::SelfConfig::getDefaultPlanner(ompl_simple_setup_->getGoal()));
652  bool r = ompl_parallel_plan_.solve(ptc, 1, count, true) == ompl::base::PlannerStatus::EXACT_SOLUTION;
653  result = result && r;
654  }
655  last_plan_time_ = ompl::time::seconds(ompl::time::now() - start);
657  }
658  }
659 
660  postSolve();
661 
662  return result;
663 }
664 
665 void ompl_interface::ModelBasedPlanningContext::registerTerminationCondition(const ob::PlannerTerminationCondition& ptc)
666 {
667  boost::mutex::scoped_lock slock(ptc_lock_);
668  ptc_ = &ptc;
669 }
670 
672 {
673  boost::mutex::scoped_lock slock(ptc_lock_);
674  ptc_ = NULL;
675 }
676 
678 {
679  boost::mutex::scoped_lock slock(ptc_lock_);
680  if (ptc_)
681  ptc_->terminate();
682  return true;
683 }
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
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 ...
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
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)
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 Sat Apr 21 2018 02:55:34