ConstrainedPlanningCommon.h
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2018, Rice University
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 the Rice University 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: Zachary Kingston */
36 
37 #ifndef OMPL_DEMO_CONSTRAINED_COMMON_
38 #define OMPL_DEMO_CONSTRAINED_COMMON_
39 
40 #include <iostream>
41 #include <fstream>
42 
43 #include <boost/format.hpp>
44 #include <boost/program_options.hpp>
45 
46 #include <ompl/geometric/SimpleSetup.h>
47 #include <ompl/geometric/PathGeometric.h>
48 
49 #include <ompl/base/Constraint.h>
50 #include <ompl/base/ConstrainedSpaceInformation.h>
51 #include <ompl/base/spaces/constraint/ConstrainedStateSpace.h>
52 #include <ompl/base/spaces/constraint/AtlasStateSpace.h>
53 #include <ompl/base/spaces/constraint/TangentBundleStateSpace.h>
54 #include <ompl/base/spaces/constraint/ProjectedStateSpace.h>
55 
56 #include <ompl/geometric/planners/rrt/RRT.h>
57 #include <ompl/geometric/planners/rrt/RRTConnect.h>
58 #include <ompl/geometric/planners/rrt/RRTstar.h>
59 #include <ompl/geometric/planners/est/EST.h>
60 #include <ompl/geometric/planners/est/BiEST.h>
61 #include <ompl/geometric/planners/est/ProjEST.h>
62 #include <ompl/geometric/planners/bitstar/BITstar.h>
63 #include <ompl/geometric/planners/prm/PRM.h>
64 #include <ompl/geometric/planners/prm/SPARS.h>
65 #include <ompl/geometric/planners/prm/SPARStwo.h>
66 #include <ompl/geometric/planners/kpiece/KPIECE1.h>
67 #include <ompl/geometric/planners/kpiece/BKPIECE1.h>
68 
69 #include <ompl/tools/benchmark/Benchmark.h>
70 
71 namespace po = boost::program_options;
72 namespace ob = ompl::base;
73 namespace og = ompl::geometric;
74 namespace om = ompl::magic;
75 namespace ot = ompl::tools;
76 
77 enum SPACE_TYPE
78 {
79  PJ = 0,
80  AT = 1,
81  TB = 2
82 };
83 
84 const std::string spaceStr[3] = {"PJ", "AT", "TB"};
85 
86 std::istream &operator>>(std::istream &in, enum SPACE_TYPE &type)
87 {
88  std::string token;
89  in >> token;
90  if (token == "PJ")
91  type = PJ;
92  else if (token == "AT")
93  type = AT;
94  else if (token == "TB")
95  type = TB;
96  else
97  in.setstate(std::ios_base::failbit);
98 
99  return in;
100 }
101 
102 void addSpaceOption(po::options_description &desc, enum SPACE_TYPE *space)
103 {
104  auto space_msg = "Choose which constraint handling methodology to use. One of:\n"
105  "PJ - Projection (Default), "
106  "AT - Atlas, "
107  "TB - Tangent Bundle.";
108 
109  desc.add_options()("space,s", po::value<enum SPACE_TYPE>(space), space_msg);
110 }
111 
112 enum PLANNER_TYPE
113 {
114  RRT,
115  RRT_I,
116  RRTConnect,
117  RRTConnect_I,
118  RRTstar,
119  EST,
120  BiEST,
121  ProjEST,
122  BITstar,
123  PRM,
124  SPARS,
125  KPIECE,
126  BKPIECE
127 };
128 
129 std::istream &operator>>(std::istream &in, enum PLANNER_TYPE &type)
130 {
131  std::string token;
132  in >> token;
133  if (token == "RRT")
134  type = RRT;
135  else if (token == "RRT_I")
136  type = RRT_I;
137  else if (token == "RRTConnect")
138  type = RRTConnect;
139  else if (token == "RRTConnect_I")
140  type = RRTConnect_I;
141  else if (token == "RRTstar")
142  type = RRTstar;
143  else if (token == "EST")
144  type = EST;
145  else if (token == "BiEST")
146  type = BiEST;
147  else if (token == "ProjEST")
148  type = ProjEST;
149  else if (token == "BITstar")
150  type = BITstar;
151  else if (token == "PRM")
152  type = PRM;
153  else if (token == "SPARS")
154  type = SPARS;
155  else if (token == "KPIECE")
156  type = KPIECE;
157  else if (token == "BKPIECE")
158  type = BKPIECE;
159  else
160  in.setstate(std::ios_base::failbit);
161 
162  return in;
163 }
164 
165 void addPlannerOption(po::options_description &desc, std::vector<enum PLANNER_TYPE> *planners)
166 {
167  auto planner_msg = "List of which motion planner to use (multiple if benchmarking, one if planning). Choose from:\n"
168  "RRT (Default), RRT_I, RRTConnect, RRTConnect_I, RRTstar, "
169  "EST, BiEST, ProjEST, "
170  "BITstar, "
171  "PRM, SPARS, "
172  "KPIECE, BKPIECE.";
173 
174  desc.add_options()("planner,p", po::value<std::vector<enum PLANNER_TYPE>>(planners)->multitoken(), planner_msg);
175 }
176 
178 {
179  double delta;
180  double lambda;
181  double tolerance;
182  double time;
183  unsigned int tries;
184  double range;
185 };
186 
187 void addConstrainedOptions(po::options_description &desc, struct ConstrainedOptions *options)
188 {
189  auto delta_msg = "Step-size for discrete geodesic on manifold.";
190  auto lambda_msg = "Maximum `wandering` allowed during traversal. Must be greater than 1.";
191  auto tolerance_msg = "Constraint satisfaction tolerance.";
192  auto time_msg = "Planning time allowed.";
193  auto tries_msg = "Maximum number sample tries per sample.";
194  auto range_msg = "Planner `range` value for planners that support this parameter. Automatically determined "
195  "otherwise (when 0).";
196 
197  desc.add_options()("delta,d", po::value<double>(&options->delta)->default_value(om::CONSTRAINED_STATE_SPACE_DELTA),
198  delta_msg);
199  desc.add_options()("lambda", po::value<double>(&options->lambda)->default_value(om::CONSTRAINED_STATE_SPACE_LAMBDA),
200  lambda_msg);
201  desc.add_options()("tolerance",
202  po::value<double>(&options->tolerance)->default_value(om::CONSTRAINT_PROJECTION_TOLERANCE),
203  tolerance_msg);
204  desc.add_options()("time", po::value<double>(&options->time)->default_value(5.), time_msg);
205  desc.add_options()(
206  "tries", po::value<unsigned int>(&options->tries)->default_value(om::CONSTRAINT_PROJECTION_MAX_ITERATIONS),
207  tries_msg);
208  desc.add_options()("range,r", po::value<double>(&options->range)->default_value(0), range_msg);
209 }
210 
212 {
213  double epsilon;
214  double rho;
215  double exploration;
216  double alpha;
217  bool bias;
218  bool separate;
219  unsigned int charts;
220 };
221 
222 void addAtlasOptions(po::options_description &desc, struct AtlasOptions *options)
223 {
224  auto epsilon_msg = "Maximum distance from an atlas chart to the manifold. Must be positive.";
225  auto rho_msg = "Maximum radius for an atlas chart. Must be positive.";
226  auto exploration_msg = "Value in [0, 1] which tunes balance of refinement and exploration in "
227  "atlas sampling.";
228  auto alpha_msg = "Maximum angle between an atlas chart and the manifold. Must be in [0, PI/2].";
229  auto bias_msg = "Sets whether the atlas should use frontier-biased chart sampling rather than "
230  "uniform.";
231  auto separate_msg = "Sets that the atlas should not compute chart separating halfspaces.";
232  auto charts_msg = "Maximum number of atlas charts that can be generated during one manifold "
233  "traversal.";
234 
235  desc.add_options()("epsilon", po::value<double>(&options->epsilon)->default_value(om::ATLAS_STATE_SPACE_EPSILON),
236  epsilon_msg);
237  desc.add_options()("rho",
238  po::value<double>(&options->rho)
239  ->default_value(om::CONSTRAINED_STATE_SPACE_DELTA * om::ATLAS_STATE_SPACE_RHO_MULTIPLIER),
240  rho_msg);
241  desc.add_options()("exploration",
242  po::value<double>(&options->exploration)->default_value(om::ATLAS_STATE_SPACE_EXPLORATION),
243  exploration_msg);
244  desc.add_options()("alpha", po::value<double>(&options->alpha)->default_value(om::ATLAS_STATE_SPACE_ALPHA),
245  alpha_msg);
246  desc.add_options()("bias", po::bool_switch(&options->bias)->default_value(false), bias_msg);
247  desc.add_options()("no-separate", po::bool_switch(&options->separate)->default_value(false), separate_msg);
248  desc.add_options()(
249  "charts",
250  po::value<unsigned int>(&options->charts)->default_value(om::ATLAS_STATE_SPACE_MAX_CHARTS_PER_EXTENSION),
251  charts_msg);
252 }
253 
255 {
256 public:
257  ConstrainedProblem(enum SPACE_TYPE type, ob::StateSpacePtr space_, ob::ConstraintPtr constraint_)
258  : type(type), space(std::move(space_)), constraint(std::move(constraint_))
259  {
260  // Combine the ambient state space and the constraint to create the
261  // constrained state space.
262  switch (type)
263  {
264  case PJ:
265  OMPL_INFORM("Using Projection-Based State Space!");
266  css = std::make_shared<ob::ProjectedStateSpace>(space, constraint);
267  csi = std::make_shared<ob::ConstrainedSpaceInformation>(css);
268  break;
269  case AT:
270  OMPL_INFORM("Using Atlas-Based State Space!");
271  css = std::make_shared<ob::AtlasStateSpace>(space, constraint);
272  csi = std::make_shared<ob::ConstrainedSpaceInformation>(css);
273  break;
274  case TB:
275  OMPL_INFORM("Using Tangent Bundle-Based State Space!");
276  css = std::make_shared<ob::TangentBundleStateSpace>(space, constraint);
277  csi = std::make_shared<ob::TangentBundleSpaceInformation>(css);
278  break;
279  }
280 
281  css->setup();
282  ss = std::make_shared<og::SimpleSetup>(csi);
283  }
284 
285  void setConstrainedOptions(struct ConstrainedOptions &opt)
286  {
287  c_opt = opt;
288 
289  constraint->setTolerance(opt.tolerance);
290  constraint->setMaxIterations(opt.tries);
291 
292  css->setDelta(opt.delta);
293  css->setLambda(opt.lambda);
294  }
295 
296  void setAtlasOptions(struct AtlasOptions &opt)
297  {
298  a_opt = opt;
299 
300  if (!(type == AT || type == TB))
301  return;
302 
303  auto &&atlas = css->as<ob::AtlasStateSpace>();
304  atlas->setExploration(opt.exploration);
305  atlas->setEpsilon(opt.epsilon);
306  atlas->setRho(opt.rho);
307  atlas->setAlpha(opt.alpha);
308  atlas->setMaxChartsPerExtension(opt.charts);
309 
310  if (opt.bias)
311  atlas->setBiasFunction([atlas](ompl::base::AtlasChart *c) -> double {
312  return (atlas->getChartCount() - c->getNeighborCount()) + 1;
313  });
314 
315  if (type == AT)
316  atlas->setSeparated(!opt.separate);
317 
318  atlas->setup();
319  }
320 
321  void setStartAndGoalStates(const Eigen::Ref<const Eigen::VectorXd> &start,
322  const Eigen::Ref<const Eigen::VectorXd> &goal)
323  {
324  // Create start and goal states (poles of the sphere)
325  ob::ScopedState<> sstart(css);
326  ob::ScopedState<> sgoal(css);
327 
328  sstart->as<ob::ConstrainedStateSpace::StateType>()->copy(start);
329  sgoal->as<ob::ConstrainedStateSpace::StateType>()->copy(goal);
330 
331  switch (type)
332  {
333  case AT:
334  case TB:
335  css->as<ob::AtlasStateSpace>()->anchorChart(sstart.get());
336  css->as<ob::AtlasStateSpace>()->anchorChart(sgoal.get());
337  break;
338  default:
339  break;
340  }
341 
342  // Setup problem
343  ss->setStartAndGoalStates(sstart, sgoal);
344  }
345 
346  template <typename _T>
347  std::shared_ptr<_T> createPlanner()
348  {
349  auto &&planner = std::make_shared<_T>(csi);
350  return planner;
351  }
352 
353  template <typename _T>
354  std::shared_ptr<_T> createPlannerIntermediate()
355  {
356  auto &&planner = std::make_shared<_T>(csi, true);
357  return planner;
358  }
359 
360  template <typename _T>
361  std::shared_ptr<_T> createPlannerRange()
362  {
363  auto &&planner = createPlanner<_T>();
364 
365  if (c_opt.range == 0)
366  {
367  if (type == AT || type == TB)
368  planner->setRange(css->as<ob::AtlasStateSpace>()->getRho_s());
369  }
370  else
371  planner->setRange(c_opt.range);
372 
373  return planner;
374  }
375 
376  template <typename _T>
377  std::shared_ptr<_T> createPlannerRange(bool intermediate)
378  {
379  auto &&planner = createPlannerIntermediate<_T>();
380 
381  if (c_opt.range == 0)
382  {
383  if (type == AT || type == TB)
384  planner->setRange(css->as<ob::AtlasStateSpace>()->getRho_s());
385  }
386  else
387  planner->setRange(c_opt.range);
388 
389  return planner;
390  }
391 
392  template <typename _T>
393  std::shared_ptr<_T> createPlannerRangeProj(const std::string &projection)
394  {
395  const bool isProj = projection != "";
396  auto &&planner = createPlannerRange<_T>();
397 
398  if (isProj)
399  planner->setProjectionEvaluator(projection);
400 
401  return planner;
402  }
403 
404  ob::PlannerPtr getPlanner(enum PLANNER_TYPE planner, const std::string &projection = "")
405  {
406  ob::PlannerPtr p;
407  switch (planner)
408  {
409  case RRT:
410  p = createPlannerRange<og::RRT>();
411  break;
412  case RRT_I:
413  p = createPlannerRange<og::RRT>(true);
414  break;
415  case RRTConnect:
416  p = createPlannerRange<og::RRTConnect>();
417  break;
418  case RRTConnect_I:
419  p = createPlannerRange<og::RRTConnect>(true);
420  break;
421  case RRTstar:
422  p = createPlannerRange<og::RRTstar>();
423  break;
424  case EST:
425  p = createPlannerRange<og::EST>();
426  break;
427  case BiEST:
428  p = createPlannerRange<og::BiEST>();
429  break;
430  case ProjEST:
431  p = createPlannerRangeProj<og::ProjEST>(projection);
432  break;
433  case BITstar:
434  p = createPlanner<og::BITstar>();
435  break;
436  case PRM:
437  p = createPlanner<og::PRM>();
438  break;
439  case SPARS:
440  p = createPlanner<og::SPARS>();
441  break;
442  case KPIECE:
443  p = createPlannerRangeProj<og::KPIECE1>(projection);
444  break;
445  case BKPIECE:
446  p = createPlannerRangeProj<og::BKPIECE1>(projection);
447  break;
448  }
449  return p;
450  }
451 
452  void setPlanner(enum PLANNER_TYPE planner, const std::string &projection = "")
453  {
454  pp = getPlanner(planner, projection);
455  ss->setPlanner(pp);
456  }
457 
458  ob::PlannerStatus solveOnce(bool output = false, const std::string &name = "ompl")
459  {
460  ss->setup();
461 
462  ob::PlannerStatus stat = ss->solve(c_opt.time);
463  std::cout << std::endl;
464 
465  if (stat)
466  {
467  // Get solution and validate
468  auto path = ss->getSolutionPath();
469  if (!path.check())
470  OMPL_WARN("Path fails check!");
471 
473  OMPL_WARN("Solution is approximate.");
474 
475  // Simplify solution and validate simplified solution path.
476  OMPL_INFORM("Simplifying solution...");
477  ss->simplifySolution(5.);
478 
479  auto simplePath = ss->getSolutionPath();
480  OMPL_INFORM("Simplified Path Length: %.3f -> %.3f", path.length(), simplePath.length());
481 
482  if (!simplePath.check())
483  OMPL_WARN("Simplified path fails check!");
484 
485  // Interpolate and validate interpolated solution path.
486  OMPL_INFORM("Interpolating path...");
487  path.interpolate();
488 
489  if (!path.check())
490  OMPL_WARN("Interpolated simplified path fails check!");
491 
492  OMPL_INFORM("Interpolating simplified path...");
493  simplePath.interpolate();
494 
495  if (!simplePath.check())
496  OMPL_WARN("Interpolated simplified path fails check!");
497 
498  if (output)
499  {
500  OMPL_INFORM("Dumping path to `%s_path.txt`.", name.c_str());
501  std::ofstream pathfile((boost::format("%1%_path.txt") % name).str());
502  path.printAsMatrix(pathfile);
503  pathfile.close();
504 
505  OMPL_INFORM("Dumping simplified path to `%s_simplepath.txt`.", name.c_str());
506  std::ofstream simplepathfile((boost::format("%1%_simplepath.txt") % name).str());
507  simplePath.printAsMatrix(simplepathfile);
508  simplepathfile.close();
509  }
510  }
511  else
512  OMPL_WARN("No solution found.");
513 
514  return stat;
515  }
516 
517  void setupBenchmark(std::vector<enum PLANNER_TYPE> &planners, const std::string &problem)
518  {
519  bench = new ot::Benchmark(*ss, problem);
520 
521  bench->addExperimentParameter("n", "INTEGER", std::to_string(constraint->getAmbientDimension()));
522  bench->addExperimentParameter("k", "INTEGER", std::to_string(constraint->getManifoldDimension()));
523  bench->addExperimentParameter("n - k", "INTEGER", std::to_string(constraint->getCoDimension()));
524  bench->addExperimentParameter("space", "INTEGER", std::to_string(type));
525 
526  request = ot::Benchmark::Request(c_opt.time, 2048, 100, 0.1, true, false, true, true);
527 
528  for (auto planner : planners)
529  bench->addPlanner(getPlanner(planner, problem));
530 
531  bench->setPreRunEvent([&](const ob::PlannerPtr &planner) {
532  if (type == AT || type == TB)
533  planner->getSpaceInformation()->getStateSpace()->as<ompl::base::AtlasStateSpace>()->clear();
534  else
535  planner->getSpaceInformation()->getStateSpace()->as<ompl::base::ConstrainedStateSpace>()->clear();
536 
537  planner->clear();
538  });
539  }
540 
541  void runBenchmark()
542  {
543  bench->benchmark(request);
544 
546  const std::string filename =
547  (boost::format("%1%_%2%_%3%_benchmark.log") % now % bench->getExperimentName() % spaceStr[type]).str();
548 
549  bench->saveResultsToFile(filename.c_str());
550  }
551 
552  void atlasStats()
553  {
554  // For atlas types, output information about size of atlas and amount of space explored
555  if (type == AT || type == TB)
556  {
557  auto at = css->as<ob::AtlasStateSpace>();
558  OMPL_INFORM("Atlas has %zu charts", at->getChartCount());
559  if (type == AT)
560  OMPL_INFORM("Atlas is approximately %.3f%% open", at->estimateFrontierPercent());
561  }
562  }
563 
564  void dumpGraph(const std::string &name)
565  {
566  OMPL_INFORM("Dumping planner graph to `%s_graph.graphml`.", name.c_str());
567  ob::PlannerData data(csi);
568  pp->getPlannerData(data);
569 
570  std::ofstream graphfile((boost::format("%1%_graph.graphml") % name).str());
571  data.printGraphML(graphfile);
572  graphfile.close();
573 
574  if (type == AT || type == TB)
575  {
576  OMPL_INFORM("Dumping atlas to `%s_atlas.ply`.", name.c_str());
577  std::ofstream atlasfile((boost::format("%1%_atlas.ply") % name).str());
578  css->as<ob::AtlasStateSpace>()->printPLY(atlasfile);
579  atlasfile.close();
580  }
581  }
582 
583  enum SPACE_TYPE type;
584 
585  ob::StateSpacePtr space;
586  ob::ConstraintPtr constraint;
587 
588  ob::ConstrainedStateSpacePtr css;
589  ob::ConstrainedSpaceInformationPtr csi;
590 
591  ob::PlannerPtr pp;
592 
594 
595  struct ConstrainedOptions c_opt;
596  struct AtlasOptions a_opt;
597 
598  ot::Benchmark *bench;
599  ot::Benchmark::Request request;
600 };
601 
602 #endif
const std::string & getExperimentName() const
Get the name of the experiment.
Definition: Benchmark.h:247
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:174
ConstrainedStateSpace encapsulating a planner-agnostic atlas algorithm for planning on a constraint m...
bool saveResultsToFile(const char *filename) const
Save the results of the benchmark to a file.
Definition: Benchmark.cpp:236
Definition of a scoped state.
Definition: ScopedState.h:56
A shared pointer wrapper for ompl::base::StateSpace.
Tangent space and bounding polytope approximating some patch of the manifold.
Definition: AtlasChart.h:52
A shared pointer wrapper for ompl::base::Constraint.
void addPlanner(const base::PlannerPtr &planner)
Add a planner to use.
Definition: Benchmark.h:253
This namespace includes magic constants used in various places in OMPL.
Definition: Constraint.h:51
Includes various tools such as self config, benchmarking, etc.
Benchmark a set of planners on a problem instance.
Definition: Benchmark.h:48
std::string as_string(const point &p)
Return string representation of point in time.
Definition: Time.h:90
StateType * get()
Returns a pointer to the contained state.
Definition: ScopedState.h:394
const ScopedState< T > & operator>>(const ScopedState< T > &from, ScopedState< Y > &to)
This is a fancy version of the assignment operator. It is a partial assignment, in some sense...
Definition: ScopedState.h:523
A shared pointer wrapper for ompl::geometric::SimpleSetup.
A shared pointer wrapper for ompl::base::Planner.
ompl::base::State StateType
Define the type of state allocated by this space.
Definition: StateSpace.h:78
void addExperimentParameter(const std::string &name, const std::string &type, const std::string &value)
Add an optional parameter&#39;s information to the benchmark output. Useful for aggregating results over ...
Definition: Benchmark.h:223
double getRho_s() const
Get the sampling radius.
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
This namespace contains sampling based planning routines shared by both planning under geometric cons...
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
void setPreRunEvent(const PreSetupEvent &event)
Set the event to be called before the run of a planner.
Definition: Benchmark.h:281
The planner found an approximate solution.
Definition: PlannerStatus.h:64
point now()
Get the current time point.
Definition: Time.h:70
A StateSpace that has a Constraint imposed upon it. Underlying space functions are passed to the ambi...
void setExploration(double exploration)
Set the exploration parameter, which tunes the balance of refinement (sampling within known regions) ...
This namespace contains code that is specific to planning under geometric constraints.
Definition: GeneticSearch.h:47
virtual void benchmark(const Request &req)
Benchmark the added planners on the defined problem. Repeated calls clear previously gathered data...
Definition: Benchmark.cpp:390
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68