38 const double edgeWidth = 0.1;
39 const double runtime_limit = 10;
40 const double memory_limit = 4096 * 4096;
41 const int run_count = 10;
42 unsigned int curDim = 8;
43 int numberPlanners = 0;
45 #include "QuotientSpacePlanningCommon.h"
46 #include <ompl/base/spaces/RealVectorStateSpace.h>
48 #include <ompl/geometric/planners/informedtrees/BITstar.h>
49 #include <ompl/geometric/planners/est/BiEST.h>
50 #include <ompl/geometric/planners/est/EST.h>
51 #include <ompl/geometric/planners/est/ProjEST.h>
52 #include <ompl/geometric/planners/fmt/BFMT.h>
53 #include <ompl/geometric/planners/fmt/FMT.h>
54 #include <ompl/geometric/planners/kpiece/BKPIECE1.h>
55 #include <ompl/geometric/planners/kpiece/KPIECE1.h>
56 #include <ompl/geometric/planners/kpiece/LBKPIECE1.h>
57 #include <ompl/geometric/planners/pdst/PDST.h>
59 #include <ompl/geometric/planners/prm/LazyPRMstar.h>
60 #include <ompl/geometric/planners/prm/PRM.h>
61 #include <ompl/geometric/planners/prm/PRMstar.h>
62 #include <ompl/geometric/planners/prm/SPARS.h>
63 #include <ompl/geometric/planners/prm/SPARStwo.h>
64 #include <ompl/geometric/planners/quotientspace/QRRT.h>
65 #include <ompl/geometric/planners/rrt/BiTRRT.h>
66 #include <ompl/geometric/planners/rrt/InformedRRTstar.h>
67 #include <ompl/geometric/planners/rrt/LazyRRT.h>
68 #include <ompl/geometric/planners/rrt/LBTRRT.h>
69 #include <ompl/geometric/planners/rrt/RRTConnect.h>
70 #include <ompl/geometric/planners/rrt/RRT.h>
71 #include <ompl/geometric/planners/rrt/RRTsharp.h>
72 #include <ompl/geometric/planners/rrt/RRTstar.h>
73 #include <ompl/geometric/planners/rrt/RRTXstatic.h>
74 #include <ompl/geometric/planners/rrt/SORRTstar.h>
75 #include <ompl/geometric/planners/rrt/TRRT.h>
76 #include <ompl/geometric/planners/sbl/pSBL.h>
77 #include <ompl/geometric/planners/sbl/SBL.h>
78 #include <ompl/geometric/planners/sst/SST.h>
79 #include <ompl/geometric/planners/stride/STRIDE.h>
81 #include <ompl/tools/benchmark/Benchmark.h>
82 #include <ompl/util/String.h>
84 #include <boost/math/constants/constants.hpp>
85 #include <boost/range/irange.hpp>
86 #include <boost/range/algorithm_ext/push_back.hpp>
87 #include <boost/format.hpp>
97 HyperCubeValidityChecker(
const ob::SpaceInformationPtr &si,
int dimension)
100 si->setStateValidityCheckingResolution(0.001);
106 bool foundMaxDim =
false;
108 for (
int i = dimension_ - 1; i >= 0; i--)
111 if ((*s)[i] > edgeWidth)
114 else if ((*s)[i] < (1. - edgeWidth))
123 static unsigned int numberRuns{0};
127 static unsigned int pid = 0;
129 ob::SpaceInformationPtr si = planner->getSpaceInformation();
130 ob::ProblemDefinitionPtr pdef = planner->getProblemDefinition();
132 unsigned int states = boost::lexical_cast<int>(run[
"graph states INTEGER"]);
133 double time = boost::lexical_cast<double>(run[
"time REAL"]);
134 double memory = boost::lexical_cast<double>(run[
"memory REAL"]);
136 bool solved = (time < runtime_limit);
138 std::cout <<
"Run " << pid <<
"/" << numberRuns <<
" [" << planner->getName() <<
"] "
139 << (solved ?
"solved" :
"FAILED") <<
"(time: " << time <<
", states: " << states <<
", memory: " << memory
141 std::cout << std::string(80,
'-') << std::endl;
152 std::vector<std::vector<int>> getHypercubeAdmissibleProjections(
int dim)
154 std::vector<std::vector<int>> projections;
158 std::vector<int> trivial{dim};
159 std::vector<int> discrete;
160 boost::push_back(discrete, boost::irange(2, dim + 1));
162 std::vector<int> twoStep;
163 boost::push_back(twoStep, boost::irange(2, dim + 1, 2));
164 if (twoStep.back() != dim)
165 twoStep.push_back(dim);
167 projections.push_back(trivial);
168 projections.push_back(discrete);
169 projections.push_back(twoStep);
170 auto last = std::unique(projections.begin(), projections.end());
171 projections.erase(last, projections.end());
189 if (params.
hasParam(std::string(
"range")))
191 benchmark.addPlanner(planner);
195 ob::PlannerPtr GetQRRT(std::vector<int> sequenceLinks, ob::SpaceInformationPtr si)
198 std::vector<ob::SpaceInformationPtr> si_vec;
200 for (
unsigned int k = 0; k < sequenceLinks.size() - 1; k++)
202 int links = sequenceLinks.at(k);
204 auto spaceK(std::make_shared<ompl::base::RealVectorStateSpace>(links));
208 spaceK->setBounds(bounds);
210 auto siK = std::make_shared<ob::SpaceInformation>(spaceK);
211 siK->setStateValidityChecker(std::make_shared<HyperCubeValidityChecker>(siK, links));
214 si_vec.push_back(siK);
216 si_vec.push_back(si);
218 auto planner = std::make_shared<og::QRRT>(si_vec);
219 std::string qName =
"QuotientSpaceRRT[";
220 for (
unsigned int k = 0; k < sequenceLinks.size() - 1; k++)
222 int links = sequenceLinks.at(k);
223 qName += std::to_string(links) +
",";
225 qName += std::to_string(si->getStateDimension());
227 std::cout << qName << std::endl;
228 planner->setName(qName);
232 int main(
int argc,
char **argv)
236 curDim = std::atoi(argv[1]);
240 double range = edgeWidth * 0.5;
241 auto space(std::make_shared<ompl::base::RealVectorStateSpace>(curDim));
244 ob::SpaceInformationPtr si = ss.getSpaceInformation();
245 ob::ProblemDefinitionPtr pdef = ss.getProblemDefinition();
250 space->setBounds(bounds);
251 ss.setStateValidityChecker(std::make_shared<HyperCubeValidityChecker>(si, curDim));
252 for (
unsigned int i = 0; i < curDim; ++i)
257 ss.setStartAndGoalStates(start, goal);
260 benchmark.addExperimentParameter(
"num_dims",
"INTEGER", std::to_string(curDim));
265 std::vector<std::vector<int>> admissibleProjections = getHypercubeAdmissibleProjections(curDim);
266 for (
unsigned int k = 0; k < admissibleProjections.size(); k++)
268 std::vector<int> proj = admissibleProjections.at(k);
269 ob::PlannerPtr quotientSpacePlannerK = GetQRRT(proj, si);
270 addPlanner(benchmark, quotientSpacePlannerK, range);
272 addPlanner(benchmark, std::make_shared<og::BITstar>(si), range);
273 addPlanner(benchmark, std::make_shared<og::EST>(si), range);
274 addPlanner(benchmark, std::make_shared<og::BiEST>(si), range);
275 addPlanner(benchmark, std::make_shared<og::ProjEST>(si), range);
276 addPlanner(benchmark, std::make_shared<og::FMT>(si), range);
277 addPlanner(benchmark, std::make_shared<og::BFMT>(si), range);
278 addPlanner(benchmark, std::make_shared<og::KPIECE1>(si), range);
279 addPlanner(benchmark, std::make_shared<og::BKPIECE1>(si), range);
280 addPlanner(benchmark, std::make_shared<og::LBKPIECE1>(si), range);
281 addPlanner(benchmark, std::make_shared<og::PDST>(si), range);
282 addPlanner(benchmark, std::make_shared<og::PRM>(si), range);
283 addPlanner(benchmark, std::make_shared<og::PRMstar>(si), range);
284 addPlanner(benchmark, std::make_shared<og::LazyPRMstar>(si), range);
285 addPlanner(benchmark, std::make_shared<og::SPARS>(si), range);
286 addPlanner(benchmark, std::make_shared<og::SPARStwo>(si), range);
287 addPlanner(benchmark, std::make_shared<og::RRT>(si), range);
288 addPlanner(benchmark, std::make_shared<og::RRTConnect>(si), range);
289 addPlanner(benchmark, std::make_shared<og::RRTsharp>(si), range);
290 addPlanner(benchmark, std::make_shared<og::RRTstar>(si), range);
291 addPlanner(benchmark, std::make_shared<og::RRTXstatic>(si), range);
292 addPlanner(benchmark, std::make_shared<og::LazyRRT>(si), range);
293 addPlanner(benchmark, std::make_shared<og::InformedRRTstar>(si), range);
294 addPlanner(benchmark, std::make_shared<og::TRRT>(si), range);
295 addPlanner(benchmark, std::make_shared<og::BiTRRT>(si), range);
296 addPlanner(benchmark, std::make_shared<og::LBTRRT>(si), range);
297 addPlanner(benchmark, std::make_shared<og::SORRTstar>(si), range);
298 addPlanner(benchmark, std::make_shared<og::SBL>(si), range);
299 addPlanner(benchmark, std::make_shared<og::SST>(si), range);
300 addPlanner(benchmark, std::make_shared<og::STRIDE>(si), range);
303 printEstimatedTimeToCompletion(numberPlanners, run_count, runtime_limit);
305 ot::Benchmark::Request request;
306 request.maxTime = runtime_limit;
307 request.maxMem = memory_limit;
308 request.runCount = run_count;
309 request.simplify =
false;
310 request.displayProgress =
false;
311 numberRuns = numberPlanners * run_count;
313 benchmark.setPostRunEvent(std::bind(&PostRunEvent, std::placeholders::_1, std::placeholders::_2));
314 benchmark.benchmark(request);
315 benchmark.saveResultsToFile(boost::str(boost::format(
"hypercube_%i.log") % curDim).c_str());
317 printBenchmarkResults(benchmark);
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.
Definition of a scoped state.
ompl::base::State StateType
Define the type of state allocated by this space.
Abstract definition for a class checking the validity of states. The implementation of this class mus...
virtual bool isValid(const State *state) const =0
Return true if the state state is valid. Usually, this means at least collision checking....
StateValidityChecker(SpaceInformation *si)
Constructor.
Definition of an abstract state.
Create the set of classes typically needed to solve a geometric problem.
This namespace contains sampling based planning routines shared by both planning under geometric cons...
std::string toString(float val)
convert float to string using classic "C" locale semantics