37 #include <ompl/base/spaces/RealVectorStateSpace.h>
38 #include <ompl/geometric/planners/rrt/RRT.h>
39 #include <ompl/geometric/planners/kpiece/KPIECE1.h>
40 #include <ompl/geometric/planners/est/EST.h>
41 #include <ompl/geometric/planners/prm/PRM.h>
42 #include <ompl/geometric/planners/stride/STRIDE.h>
43 #include <ompl/tools/benchmark/Benchmark.h>
44 #include <ompl/util/String.h>
46 #include <boost/math/constants/constants.hpp>
47 #include <boost/format.hpp>
51 const double edgeWidth = 0.1;
61 bool foundMaxDim =
false;
63 for (
int i = ndim - 1; i >= 0; i--)
66 if ((*s)[i] > edgeWidth)
69 else if ((*s)[i] < (1. - edgeWidth))
77 if (params.
hasParam(std::string(
"range")))
79 benchmark.addPlanner(planner);
82 int main(
int argc,
char **argv)
85 ndim = std::stoul(argv[1]);
87 double range = edgeWidth * 0.5;
88 auto space(std::make_shared<ompl::base::RealVectorStateSpace>(ndim));
95 space->setBounds(bounds);
96 ss.setStateValidityChecker(&isStateValid);
97 ss.getSpaceInformation()->setStateValidityCheckingResolution(0.001);
98 for(
unsigned int i = 0; i < ndim; ++i)
103 ss.setStartAndGoalStates(start, goal);
106 double runtime_limit = 1000, memory_limit = 4096;
110 b.addExperimentParameter(
"num_dims",
"INTEGER", std::to_string(ndim));
112 addPlanner(b, std::make_shared<ompl::geometric::STRIDE>(ss.getSpaceInformation()), range);
113 addPlanner(b, std::make_shared<ompl::geometric::EST>(ss.getSpaceInformation()), range);
114 addPlanner(b, std::make_shared<ompl::geometric::KPIECE1>(ss.getSpaceInformation()), range);
115 addPlanner(b, std::make_shared<ompl::geometric::RRT>(ss.getSpaceInformation()), range);
116 addPlanner(b, std::make_shared<ompl::geometric::PRM>(ss.getSpaceInformation()), range);
117 b.benchmark(request);
118 b.saveResultsToFile(boost::str(boost::format(
"hypercube_%i.log") % ndim).c_str());
Maintain a set of parameters.
bool hasParam(const std::string &key) const
Check whether this set of parameters includes the parameter named key.
bool setParam(const std::string &key, const std::string &value)
Algorithms in OMPL often have parameters that can be set externally. While each algorithm will have t...
A shared pointer wrapper for ompl::base::Planner.
The lower and upper bounds for an Rn space.
The definition of a state in Rn
Definition of a scoped state.
Definition of an abstract state.
Create the set of classes typically needed to solve a geometric problem.
std::string toString(float val)
convert float to string using classic "C" locale semantics