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;
Tangent space and bounding polytope approximating some patch of the manifold.
ConstrainedStateSpace encapsulating a planner-agnostic atlas algorithm for planning on a constraint m...
void clear() override
Reset the space (except for anchor charts).
double getRho_s() const
Get the sampling radius.
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...
virtual void clear()
Clear any allocated memory from the state space.
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
void printGraphML(std::ostream &out=std::cout) const
Writes a GraphML file of this structure to the given stream.
Definition of a scoped state.
StateType * get()
Returns a pointer to the contained state.
ompl::base::State StateType
Define the type of state allocated by this space.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
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....
This namespace contains sampling based planning routines shared by both planning under geometric cons...
This namespace contains code that is specific to planning under geometric constraints.
This namespace includes magic constants used in various places in OMPL.
point now()
Get the current time point.
std::string as_string(const point &p)
Return string representation of point in time.
A class to store the exit status of Planner::solve()
@ APPROXIMATE_SOLUTION
The planner found an approximate solution.