QuotientSpacePlanningHyperCubeBenchmark.cpp
1/*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2019, University of Stuttgart
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the University of Stuttgart nor the names
18 * of its contributors may be used to endorse or promote products
19 * derived from this software without specific prior written
20 * permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 * POSSIBILITY OF SUCH DAMAGE.
34 *********************************************************************/
35
36/* Author: Andreas Orthey */
37
38const double edgeWidth = 0.1;
39const double runtime_limit = 10;
40const double memory_limit = 4096 * 4096;
41const int run_count = 10;
42unsigned int curDim = 8;
43int numberPlanners = 0;
44
45#include "QuotientSpacePlanningCommon.h"
46#include <ompl/base/spaces/RealVectorStateSpace.h>
47
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>
58// #include <ompl/geometric/planners/prm/LazyPRM.h> //TODO: segfault?
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>
80
81#include <ompl/tools/benchmark/Benchmark.h>
82#include <ompl/util/String.h>
83
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>
88#include <fstream>
89
90// Only states near some edges of a hypercube are valid. The valid edges form a
91// narrow passage from (0,...,0) to (1,...,1). A state s is valid if there exists
92// a k s.t. (a) 0<=s[k]<=1, (b) for all i<k s[i]<=edgeWidth, and (c) for all i>k
93// s[i]>=1-edgewidth.
94class HyperCubeValidityChecker : public ob::StateValidityChecker
95{
96public:
97 HyperCubeValidityChecker(const ob::SpaceInformationPtr &si, int dimension)
98 : ob::StateValidityChecker(si), dimension_(dimension)
99 {
100 si->setStateValidityCheckingResolution(0.001);
101 }
102
103 bool isValid(const ob::State *state) const override
104 {
105 const auto *s = static_cast<const ob::RealVectorStateSpace::StateType *>(state);
106 bool foundMaxDim = false;
107
108 for (int i = dimension_ - 1; i >= 0; i--)
109 if (!foundMaxDim)
110 {
111 if ((*s)[i] > edgeWidth)
112 foundMaxDim = true;
113 }
114 else if ((*s)[i] < (1. - edgeWidth))
115 return false;
116 return true;
117 }
118
119protected:
120 int dimension_;
121};
122
123static unsigned int numberRuns{0};
124
125void PostRunEvent(const ob::PlannerPtr &planner, ot::Benchmark::RunProperties &run)
126{
127 static unsigned int pid = 0;
128
129 ob::SpaceInformationPtr si = planner->getSpaceInformation();
130 ob::ProblemDefinitionPtr pdef = planner->getProblemDefinition();
131
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"]);
135
136 bool solved = (time < runtime_limit);
137
138 std::cout << "Run " << pid << "/" << numberRuns << " [" << planner->getName() << "] "
139 << (solved ? "solved" : "FAILED") << "(time: " << time << ", states: " << states << ", memory: " << memory
140 << ")" << std::endl;
141 std::cout << std::string(80, '-') << std::endl;
142 pid++;
143}
144
145// Note: Number of all simplifications is
146// unsigned int numberSimplifications = std::pow(2, curDim - 1);
147// But here we will only create three simplifications, the trivial one, the
148// discrete one and a two-step simplifications, which we found worked well in
149// this experiment. You can experiment with finding better simplifications.
150// std::cout << "dimension: " << curDim << " simplifications:" << numberSimplifications << std::endl;
151
152std::vector<std::vector<int>> getHypercubeAdmissibleProjections(int dim)
153{
154 std::vector<std::vector<int>> projections;
155
156 // trivial: just configuration space
157 // discrete: use all admissible projections
158 std::vector<int> trivial{dim};
159 std::vector<int> discrete;
160 boost::push_back(discrete, boost::irange(2, dim + 1));
161
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);
166
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());
172
173 // std::cout << "Projections for dim " << dim << std::endl;
174 // for(unsigned int k = 0; k < projections.size(); k++){
175 // std::vector<int> pk = projections.at(k);
176 // std::cout << k << ": ";
177 // for(unsigned int j = 0; j < pk.size(); j++){
178 // std::cout << pk.at(j) << (j<pk.size()-1?",":"");
179 // }
180 // std::cout << std::endl;
181 // }
182
183 return projections;
184}
185
186void addPlanner(ompl::tools::Benchmark &benchmark, const ompl::base::PlannerPtr &planner, double range)
187{
188 ompl::base::ParamSet &params = planner->params();
189 if (params.hasParam(std::string("range")))
190 params.setParam(std::string("range"), ompl::toString(range));
191 benchmark.addPlanner(planner);
192 numberPlanners++;
193}
194
195ob::PlannerPtr GetQRRT(std::vector<int> sequenceLinks, ob::SpaceInformationPtr si)
196{
197 // ompl::msg::setLogLevel(ompl::msg::LOG_DEV2);
198 std::vector<ob::SpaceInformationPtr> si_vec;
199
200 for (unsigned int k = 0; k < sequenceLinks.size() - 1; k++)
201 {
202 int links = sequenceLinks.at(k);
203
204 auto spaceK(std::make_shared<ompl::base::RealVectorStateSpace>(links));
205 ompl::base::RealVectorBounds bounds(links);
206 bounds.setLow(0.);
207 bounds.setHigh(1.);
208 spaceK->setBounds(bounds);
209
210 auto siK = std::make_shared<ob::SpaceInformation>(spaceK);
211 siK->setStateValidityChecker(std::make_shared<HyperCubeValidityChecker>(siK, links));
212
213 spaceK->setup();
214 si_vec.push_back(siK);
215 }
216 si_vec.push_back(si);
217
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++)
221 {
222 int links = sequenceLinks.at(k);
223 qName += std::to_string(links) + ",";
224 }
225 qName += std::to_string(si->getStateDimension());
226 qName += "]";
227 std::cout << qName << std::endl;
228 planner->setName(qName);
229 return planner;
230}
231
232int main(int argc, char **argv)
233{
234 if (argc > 1)
235 {
236 curDim = std::atoi(argv[1]);
237 }
238
239 numberPlanners = 0;
240 double range = edgeWidth * 0.5;
241 auto space(std::make_shared<ompl::base::RealVectorStateSpace>(curDim));
242 ompl::base::RealVectorBounds bounds(curDim);
244 ob::SpaceInformationPtr si = ss.getSpaceInformation();
245 ob::ProblemDefinitionPtr pdef = ss.getProblemDefinition();
246 ompl::base::ScopedState<> start(space), goal(space);
247
248 bounds.setLow(0.);
249 bounds.setHigh(1.);
250 space->setBounds(bounds);
251 ss.setStateValidityChecker(std::make_shared<HyperCubeValidityChecker>(si, curDim));
252 for (unsigned int i = 0; i < curDim; ++i)
253 {
254 start[i] = 0.;
255 goal[i] = 1.;
256 }
257 ss.setStartAndGoalStates(start, goal);
258
259 ot::Benchmark benchmark(ss, "HyperCube");
260 benchmark.addExperimentParameter("num_dims", "INTEGER", std::to_string(curDim));
261
262 //############################################################################
263 // Load All Planner
264 //############################################################################
265 std::vector<std::vector<int>> admissibleProjections = getHypercubeAdmissibleProjections(curDim);
266 for (unsigned int k = 0; k < admissibleProjections.size(); k++)
267 {
268 std::vector<int> proj = admissibleProjections.at(k);
269 ob::PlannerPtr quotientSpacePlannerK = GetQRRT(proj, si);
270 addPlanner(benchmark, quotientSpacePlannerK, range);
271 }
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);
301 //############################################################################
302
303 printEstimatedTimeToCompletion(numberPlanners, run_count, runtime_limit);
304
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;
312
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());
316
317 printBenchmarkResults(benchmark);
318
319 return 0;
320}
Maintain a set of parameters.
Definition: GenericParam.h:226
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.
Definition: ScopedState.h:57
ompl::base::State StateType
Define the type of state allocated by this space.
Definition: StateSpace.h:78
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.
Definition: State.h:50
Create the set of classes typically needed to solve a geometric problem.
Definition: SimpleSetup.h:63
Benchmark a set of planners on a problem instance.
Definition: Benchmark.h:49
std::map< std::string, std::string > RunProperties
The data collected from a run of a planner is stored as key-value pairs.
Definition: Benchmark.h:79
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
Definition: String.cpp:82