37 #ifndef OMPL_DEMO_CONSTRAINED_COMMON_
38 #define OMPL_DEMO_CONSTRAINED_COMMON_
43 #include <boost/format.hpp>
44 #include <boost/program_options.hpp>
46 #include <ompl/geometric/SimpleSetup.h>
47 #include <ompl/geometric/PathGeometric.h>
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>
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/informedtrees/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>
69 #include <ompl/tools/benchmark/Benchmark.h>
71 namespace po = boost::program_options;
84 const std::string spaceStr[3] = {
"PJ",
"AT",
"TB"};
86 std::istream &
operator>>(std::istream &in,
enum SPACE_TYPE &type)
92 else if (token ==
"AT")
94 else if (token ==
"TB")
97 in.setstate(std::ios_base::failbit);
102 void addSpaceOption(po::options_description &desc,
enum SPACE_TYPE *space)
104 auto space_msg =
"Choose which constraint handling methodology to use. One of:\n"
105 "PJ - Projection (Default), "
107 "TB - Tangent Bundle.";
109 desc.add_options()(
"space,s", po::value<enum SPACE_TYPE>(space), space_msg);
129 std::istream &
operator>>(std::istream &in,
enum PLANNER_TYPE &type)
135 else if (token ==
"RRT_I")
137 else if (token ==
"RRTConnect")
139 else if (token ==
"RRTConnect_I")
141 else if (token ==
"RRTstar")
143 else if (token ==
"EST")
145 else if (token ==
"BiEST")
147 else if (token ==
"ProjEST")
149 else if (token ==
"BITstar")
151 else if (token ==
"PRM")
153 else if (token ==
"SPARS")
155 else if (token ==
"KPIECE")
157 else if (token ==
"BKPIECE")
160 in.setstate(std::ios_base::failbit);
165 void addPlannerOption(po::options_description &desc, std::vector<enum PLANNER_TYPE> *planners)
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, "
174 desc.add_options()(
"planner,p", po::value<std::vector<enum PLANNER_TYPE>>(planners)->multitoken(), planner_msg);
187 void addConstrainedOptions(po::options_description &desc,
struct ConstrainedOptions *options)
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).";
197 desc.add_options()(
"delta,d", po::value<double>(&options->delta)->default_value(om::CONSTRAINED_STATE_SPACE_DELTA),
199 desc.add_options()(
"lambda", po::value<double>(&options->lambda)->default_value(om::CONSTRAINED_STATE_SPACE_LAMBDA),
201 desc.add_options()(
"tolerance",
202 po::value<double>(&options->tolerance)->default_value(om::CONSTRAINT_PROJECTION_TOLERANCE),
204 desc.add_options()(
"time", po::value<double>(&options->time)->default_value(5.), time_msg);
206 "tries", po::value<unsigned int>(&options->tries)->default_value(om::CONSTRAINT_PROJECTION_MAX_ITERATIONS),
208 desc.add_options()(
"range,r", po::value<double>(&options->range)->default_value(0), range_msg);
222 void addAtlasOptions(po::options_description &desc,
struct AtlasOptions *options)
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 "
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 "
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 "
235 desc.add_options()(
"epsilon", po::value<double>(&options->epsilon)->default_value(om::ATLAS_STATE_SPACE_EPSILON),
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),
241 desc.add_options()(
"exploration",
242 po::value<double>(&options->exploration)->default_value(om::ATLAS_STATE_SPACE_EXPLORATION),
244 desc.add_options()(
"alpha", po::value<double>(&options->alpha)->default_value(om::ATLAS_STATE_SPACE_ALPHA),
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);
250 po::value<unsigned int>(&options->charts)->default_value(om::ATLAS_STATE_SPACE_MAX_CHARTS_PER_EXTENSION),
257 ConstrainedProblem(
enum SPACE_TYPE type, ob::StateSpacePtr space_, ob::ConstraintPtr constraint_)
258 : type(type), space(std::move(space_)), constraint(std::move(constraint_))
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);
271 css = std::make_shared<ob::AtlasStateSpace>(space, constraint);
272 csi = std::make_shared<ob::ConstrainedSpaceInformation>(css);
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);
282 ss = std::make_shared<og::SimpleSetup>(csi);
289 constraint->setTolerance(opt.tolerance);
290 constraint->setMaxIterations(opt.tries);
292 css->setDelta(opt.delta);
293 css->setLambda(opt.lambda);
300 if (!(type == AT || type == TB))
305 atlas->setEpsilon(opt.epsilon);
306 atlas->setRho(opt.rho);
307 atlas->setAlpha(opt.alpha);
308 atlas->setMaxChartsPerExtension(opt.charts);
312 return (atlas->getChartCount() - c->getNeighborCount()) + 1;
316 atlas->setSeparated(!opt.separate);
321 void setStartAndGoalStates(
const Eigen::Ref<const Eigen::VectorXd> &start,
322 const Eigen::Ref<const Eigen::VectorXd> &goal)
343 ss->setStartAndGoalStates(sstart, sgoal);
346 template <
typename _T>
347 std::shared_ptr<_T> createPlanner()
349 auto &&planner = std::make_shared<_T>(csi);
350 return std::move(planner);
353 template <
typename _T>
354 std::shared_ptr<_T> createPlannerIntermediate()
356 auto &&planner = std::make_shared<_T>(csi,
true);
357 return std::move(planner);
360 template <
typename _T>
361 std::shared_ptr<_T> createPlannerRange()
363 auto &&planner = createPlanner<_T>();
365 if (c_opt.range == 0)
367 if (type == AT || type == TB)
371 planner->setRange(c_opt.range);
373 return std::move(planner);
376 template <
typename _T>
377 std::shared_ptr<_T> createPlannerRange(
bool )
379 auto &&planner = createPlannerIntermediate<_T>();
381 if (c_opt.range == 0)
383 if (type == AT || type == TB)
387 planner->setRange(c_opt.range);
389 return std::move(planner);
392 template <
typename _T>
393 std::shared_ptr<_T> createPlannerRangeProj(
const std::string &projection)
395 const bool isProj = projection !=
"";
396 auto &&planner = createPlannerRange<_T>();
399 planner->setProjectionEvaluator(projection);
401 return std::move(planner);
404 ob::PlannerPtr getPlanner(
enum PLANNER_TYPE planner,
const std::string &projection =
"")
410 p = createPlannerRange<og::RRT>();
413 p = createPlannerRange<og::RRT>(
true);
416 p = createPlannerRange<og::RRTConnect>();
419 p = createPlannerRange<og::RRTConnect>(
true);
422 p = createPlannerRange<og::RRTstar>();
425 p = createPlannerRange<og::EST>();
428 p = createPlannerRange<og::BiEST>();
431 p = createPlannerRangeProj<og::ProjEST>(projection);
434 p = createPlanner<og::BITstar>();
437 p = createPlanner<og::PRM>();
440 p = createPlanner<og::SPARS>();
443 p = createPlannerRangeProj<og::KPIECE1>(projection);
446 p = createPlannerRangeProj<og::BKPIECE1>(projection);
452 void setPlanner(
enum PLANNER_TYPE planner,
const std::string &projection =
"")
454 pp = getPlanner(planner, projection);
458 ob::PlannerStatus solveOnce(
bool output =
false,
const std::string &name =
"ompl")
463 std::cout << std::endl;
468 auto path = ss->getSolutionPath();
477 ss->simplifySolution(5.);
479 auto simplePath = ss->getSolutionPath();
480 OMPL_INFORM(
"Simplified Path Length: %.3f -> %.3f", path.length(), simplePath.length());
482 if (!simplePath.check())
483 OMPL_WARN(
"Simplified path fails check!");
490 OMPL_WARN(
"Interpolated simplified path fails check!");
493 simplePath.interpolate();
495 if (!simplePath.check())
496 OMPL_WARN(
"Interpolated simplified path fails check!");
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);
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();
517 void setupBenchmark(std::vector<enum PLANNER_TYPE> &planners,
const std::string &problem)
526 request = ot::Benchmark::Request(c_opt.time, 2048, 100, 0.1,
true,
false,
true);
528 for (
auto planner : planners)
529 bench->
addPlanner(getPlanner(planner, problem));
532 if (type == AT || type == TB)
546 const std::string filename =
547 (boost::format(
"%1%_%2%_%3%_benchmark.log") % now % bench->
getExperimentName() % spaceStr[type]).str();
555 if (type == AT || type == TB)
558 OMPL_INFORM(
"Atlas has %zu charts", at->getChartCount());
560 OMPL_INFORM(
"Atlas is approximately %.3f%% open", at->estimateFrontierPercent());
564 void dumpGraph(
const std::string &name)
566 OMPL_INFORM(
"Dumping planner graph to `%s_graph.graphml`.", name.c_str());
568 pp->getPlannerData(data);
570 std::ofstream graphfile((boost::format(
"%1%_graph.graphml") % name).str());
574 if (type == AT || type == TB)
576 OMPL_INFORM(
"Dumping atlas to `%s_atlas.ply`.", name.c_str());
577 std::ofstream atlasfile((boost::format(
"%1%_atlas.ply") % name).str());
583 enum SPACE_TYPE type;
585 ob::StateSpacePtr space;
586 ob::ConstraintPtr constraint;
588 ob::ConstrainedStateSpacePtr css;
589 ob::ConstrainedSpaceInformationPtr csi;
593 og::SimpleSetupPtr ss;
599 ot::Benchmark::Request request;