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 
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;
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.
94 class HyperCubeValidityChecker : public ob::StateValidityChecker
95 {
96 public:
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 
119 protected:
120  int dimension_;
121 };
122 
123 static unsigned int numberRuns{0};
124 
125 void 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 
152 std::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 
186 void 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 
195 ob::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 
232 int 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);
243  ompl::geometric::SimpleSetup ss(space);
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 }
ompl::base::ParamSet::setParam
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...
Definition: GenericParam.cpp:114
ompl::base::ParamSet::hasParam
bool hasParam(const std::string &key) const
Check whether this set of parameters includes the parameter named key.
Definition: GenericParam.cpp:186
ompl::base::ParamSet
Maintain a set of parameters.
Definition: GenericParam.h:290
ompl::base::State
Definition of an abstract state.
Definition: State.h:114
ompl::base
This namespace contains sampling based planning routines shared by both planning under geometric cons...
Definition: ConstrainedSpaceInformation.h:87
ompl::geometric::SimpleSetup
Create the set of classes typically needed to solve a geometric problem.
Definition: SimpleSetup.h:127
ompl::base::StateSpace::StateType
ompl::base::State StateType
Define the type of state allocated by this space.
Definition: StateSpace.h:142
PlannerPtr
A shared pointer wrapper for ompl::base::Planner.
ompl::base::StateValidityChecker::StateValidityChecker
StateValidityChecker(SpaceInformation *si)
Constructor.
Definition: StateValidityChecker.h:158
ompl::base::StateValidityChecker::isValid
virtual bool isValid(const State *state) const =0
Return true if the state state is valid. Usually, this means at least collision checking....
ompl::tools::Benchmark
Benchmark a set of planners on a problem instance.
Definition: Benchmark.h:113
ompl::base::ScopedState
Definition of a scoped state.
Definition: ScopedState.h:121
ompl::base::StateValidityChecker
Abstract definition for a class checking the validity of states. The implementation of this class mus...
Definition: StateValidityChecker.h:155
ompl::toString
std::string toString(float val)
convert float to string using classic "C" locale semantics
Definition: String.cpp:82
ompl::tools::Benchmark::RunProperties
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:175
ompl::base::RealVectorBounds
The lower and upper bounds for an Rn space.
Definition: RealVectorBounds.h:112