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"), std::to_string(range));
78 benchmark.addPlanner(planner);
81 int main(
int argc,
char **argv)
84 ndim = boost::lexical_cast<size_t>(argv[1]);
86 double range = edgeWidth * 0.5;
87 auto space(std::make_shared<ompl::base::RealVectorStateSpace>(ndim));
94 space->setBounds(bounds);
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;
109 b.addExperimentParameter(
"num_dims",
"INTEGER", std::to_string(ndim));
111 addPlanner(b, std::make_shared<ompl::geometric::STRIDE>(ss.getSpaceInformation()), range);
112 addPlanner(b, std::make_shared<ompl::geometric::EST>(ss.getSpaceInformation()), range);
113 addPlanner(b, std::make_shared<ompl::geometric::KPIECE1>(ss.getSpaceInformation()), range);
114 addPlanner(b, std::make_shared<ompl::geometric::RRT>(ss.getSpaceInformation()), range);
115 addPlanner(b, std::make_shared<ompl::geometric::PRM>(ss.getSpaceInformation()), range);
116 b.benchmark(request);
117 b.saveResultsToFile(boost::str(boost::format(
"hypercube_%i.log") % ndim).c_str());