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 
177 struct ConstrainedOptions
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 
211 struct AtlasOptions
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 
254 class ConstrainedProblem
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 std::move(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 std::move(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 std::move(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 std::move(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 std::move(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 
593  og::SimpleSetupPtr ss;
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
void setPreRunEvent(const PreSetupEvent &event)
Set the event to be called before the run of a planner.
Definition: Benchmark.h:373
Includes various tools such as self config, benchmarking, etc.
The planner found an approximate solution.
This namespace contains code that is specific to planning under geometric constraints.
Definition: GeneticSearch.h:77
This namespace contains sampling based planning routines shared by both planning under geometric cons...
double getRho_s() const
Get the sampling radius.
point now()
Get the current time point.
Definition: Time.h:119
StateType * get()
Returns a pointer to the contained state.
Definition: ScopedState.h:485
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:67
ConstrainedStateSpace encapsulating a planner-agnostic atlas algorithm for planning on a constraint m...
ompl::base::State StateType
Define the type of state allocated by this space.
Definition: StateSpace.h:139
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:235
void addPlanner(const base::PlannerPtr &planner)
Add a planner to use.
Definition: Benchmark.h:345
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:583
void addExperimentParameter(const std::string &name, const std::string &type, const std::string &value)
Add an optional parameter's information to the benchmark output. Useful for aggregating results over ...
Definition: Benchmark.h:315
A class to store the exit status of Planner::solve()
void setExploration(double exploration)
Set the exploration parameter, which tunes the balance of refinement (sampling within known regions) ...
A StateSpace that has a Constraint imposed upon it. Underlying space functions are passed to the ambi...
Benchmark a set of planners on a problem instance.
Definition: Benchmark.h:109
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:65
Tangent space and bounding polytope approximating some patch of the manifold.
Definition: AtlasChart.h:113
virtual void benchmark(const Request &req)
Benchmark the added planners on the defined problem. Repeated calls clear previously gathered data.
Definition: Benchmark.cpp:352
bool saveResultsToFile(const char *filename) const
Save the results of the benchmark to a file.
Definition: Benchmark.cpp:198
Definition of a scoped state.
Definition: ScopedState.h:117
std::string as_string(const point &p)
Return string representation of point in time.
Definition: Time.h:139
const std::string & getExperimentName() const
Get the name of the experiment.
Definition: Benchmark.h:339
This namespace includes magic constants used in various places in OMPL.
Definition: Constraint.h:81