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>
45 #include <boost/math/constants/constants.hpp>
46 #include <boost/format.hpp>
50 const double edgeWidth = 0.1;
60 bool foundMaxDim =
false;
62 for (
int i = ndim - 1; i >= 0; i--)
65 if ((*s)[i] > edgeWidth)
68 else if ((*s)[i] < (1. - edgeWidth))
76 if (params.
hasParam(std::string(
"range")))
77 params.
setParam(std::string(
"range"), boost::lexical_cast<std::string>(range));
81 int main(
int argc,
char **argv)
84 ndim = boost::lexical_cast<
size_t>(argv[1]);
86 double range = edgeWidth * 0.5;
95 ss.setStateValidityChecker(&isStateValid);
96 ss.getSpaceInformation()->setStateValidityCheckingResolution(0.001);
97 for(
unsigned int i = 0; i < ndim; ++i)
102 ss.setStartAndGoalStates(start, goal);
105 double runtime_limit = 1000, memory_limit = 4096;
115 b.benchmark(request);
116 b.saveResultsToFile(boost::str(boost::format(
"hypercube_%i.log") % ndim).c_str());
Search Tree with Resolution Independent Density Estimation.
bool hasParam(const std::string &key) const
Check whether this set of parameters includes the parameter named key.
Definition of a scoped state.
A boost shared pointer wrapper for ompl::base::StateSpace.
Maintain a set of parameters.
Create the set of classes typically needed to solve a geometric problem.
A boost shared pointer wrapper for ompl::base::Planner.
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...
Kinematic Planning by Interior-Exterior Cell Exploration.
Rapidly-exploring Random Trees.
A state space representing Rn. The distance function is the L2 norm.
Definition of an abstract state.
The lower and upper bounds for an Rn space.
Probabilistic RoadMap planner.
The definition of a state in Rn