MultiLevelPlanningKinematicChain.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 #include "MultiLevelPlanningCommon.h"
39 #include "../KinematicChain.h"
40 #include <ompl/base/SpaceInformation.h>
41 #include <ompl/tools/benchmark/Benchmark.h>
42 #include <boost/range/irange.hpp>
43 #include <boost/range/algorithm_ext/push_back.hpp>
44 
45 const unsigned int numLinks = 15;
46 const double runtime_limit = 5;
47 const double memory_limit = 4096;
48 const int run_count = 2;
49 
50 const double linkLength = 1.0 / numLinks;
51 
52 using namespace ompl::base;
53 std::vector<Environment> envs;
54 
55 Environment createCustomHornEnvironment(unsigned int d)
56 {
57  double narrowPassageWidth = log((double)d) / (double)d;
58  return createHornEnvironment(d, narrowPassageWidth);
59 }
60 
61 PlannerPtr GetQRRT(std::vector<int> sequenceLinks, SpaceInformationPtr si)
62 {
63  std::vector<SpaceInformationPtr> si_vec;
64 
65  for (unsigned int k = 0; k < sequenceLinks.size(); k++)
66  {
67  auto links = sequenceLinks.at(k);
68  assert(links < numLinks);
69 
70  OMPL_INFORM("Create MultiLevel Chain with %d links.", links);
71  auto spaceK(std::make_shared<KinematicChainSpace>(links, linkLength, &envs.at(links)));
72 
73  auto siK = std::make_shared<SpaceInformation>(spaceK);
74  siK->setStateValidityChecker(std::make_shared<KinematicChainValidityChecker>(siK));
75  spaceK->setup();
76  si_vec.push_back(siK);
77  }
78 
79  OMPL_INFORM("Add Original Chain with %d links.", numLinks);
80  si_vec.push_back(si);
81 
82  auto planner = std::make_shared<ompl::multilevel::QRRT>(si_vec);
83 
84  std::string qName = "QRRT[";
85  for (unsigned int k = 0; k < sequenceLinks.size(); k++)
86  {
87  int links = sequenceLinks.at(k);
88  qName += std::to_string(links) + ",";
89  }
90  qName += std::to_string(numLinks);
91  qName += "]";
92  planner->setName(qName);
93  return planner;
94 }
95 
96 std::vector<std::vector<int>> getAdmissibleProjections(int dim)
97 {
98  std::vector<std::vector<int>> projections;
99  std::vector<int> discrete;
100  boost::push_back(discrete, boost::irange(2, dim + 1));
101 
102  std::vector<int> twoStep;
103  boost::push_back(twoStep, boost::irange(2, dim + 1, 2));
104 
105  if (twoStep.back() != dim)
106  twoStep.push_back(dim);
107 
108  projections.push_back(twoStep);
109  projections.push_back(discrete);
110 
111  auto last = std::unique(projections.begin(), projections.end());
112  projections.erase(last, projections.end());
113 
114  std::cout << "Projections for dim " << dim << std::endl;
115  for (unsigned int k = 0; k < projections.size(); k++)
116  {
117  std::vector<int> pk = projections.at(k);
118  std::cout << k << ": ";
119  for (unsigned int j = 0; j < pk.size(); j++)
120  {
121  std::cout << pk.at(j) << (j < pk.size() - 1 ? "," : "");
122  }
123  std::cout << std::endl;
124  }
125 
126  return projections;
127 }
128 
129 int main()
130 {
131  Environment env = createCustomHornEnvironment(numLinks);
132  OMPL_INFORM("Original Chain has %d links", numLinks);
133  for (unsigned int k = 0; k < numLinks; k++)
134  {
135  envs.push_back(createCustomHornEnvironment((k > 0 ? k : 1)));
136  }
137 
138  auto chain(std::make_shared<KinematicChainSpace>(numLinks, linkLength, &env));
140 
141  ss.setStateValidityChecker(std::make_shared<KinematicChainValidityChecker>(ss.getSpaceInformation()));
142 
143  ompl::base::ScopedState<> start(chain), goal(chain);
144  std::vector<double> startVec(numLinks, boost::math::constants::pi<double>() / (double)numLinks);
145  std::vector<double> goalVec(numLinks, 0);
146  startVec[0] = 0.;
147  goalVec[0] = boost::math::constants::pi<double>() - .001;
148  chain->setup();
149  chain->copyFromReals(start.get(), startVec);
150  chain->copyFromReals(goal.get(), goalVec);
151  ss.setStartAndGoalStates(start, goal);
152 
153  ompl::tools::Benchmark benchmark(ss, "KinematicChain");
154  benchmark.addExperimentParameter("num_links", "INTEGER", std::to_string(numLinks));
155 
156  //############################################################################
157  // Compare QRRT with different QuotientSpace sequences to other OMPL planner
158  //############################################################################
159  SpaceInformationPtr si = ss.getSpaceInformation();
160  ProblemDefinitionPtr pdef = ss.getProblemDefinition();
161 
162  addPlanner(benchmark, std::make_shared<ompl::geometric::STRIDE>(si));
163  addPlanner(benchmark, std::make_shared<ompl::geometric::KPIECE1>(si));
164  addPlanner(benchmark, std::make_shared<ompl::geometric::EST>(si));
165  addPlanner(benchmark, std::make_shared<ompl::geometric::PRM>(si));
166 
167  std::vector<std::vector<int>> admissibleProjections = getAdmissibleProjections(numLinks - 1);
168  for (unsigned int k = 0; k < admissibleProjections.size(); k++)
169  {
170  std::vector<int> proj = admissibleProjections.at(k);
171  PlannerPtr plannerK = GetQRRT(proj, si);
172  addPlanner(benchmark, plannerK);
173  }
174 
175  printEstimatedTimeToCompletion(numberPlanners, run_count, runtime_limit);
176 
178  request.maxTime = runtime_limit;
179  request.maxMem = memory_limit;
180  request.runCount = run_count;
181  request.simplify = false;
182  request.displayProgress = false;
183  numberRuns = numberPlanners * run_count;
184 
185  benchmark.setPostRunEvent(std::bind(&PostRunEvent, std::placeholders::_1, std::placeholders::_2));
186 
187  benchmark.benchmark(request);
188  benchmark.saveResultsToFile(boost::str(boost::format("kinematic_%i.log") % numLinks).c_str());
189 
190  printBenchmarkResults(benchmark);
191  return 0;
192 }
A shared pointer wrapper for ompl::control::SpaceInformation.
This namespace contains sampling based planning routines shared by both planning under geometric cons...
Create the set of classes typically needed to solve a geometric problem.
Definition: SimpleSetup.h:126
void log(const char *file, int line, LogLevel level, const char *m,...)
Root level logging function. This should not be invoked directly, but rather used via a logging macro...
Definition: Console.cpp:120
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
bool simplify
flag indicating whether simplification should be applied to path; true by default
Definition: Benchmark.h:290
A shared pointer wrapper for ompl::base::Planner.
unsigned int runCount
the number of times to run each planner; 100 by default If set to 0, then run each planner as many ti...
Definition: Benchmark.h:276
bool displayProgress
flag indicating whether progress is to be displayed or not; true by default
Definition: Benchmark.h:283
Representation of a benchmark request.
Definition: Benchmark.h:252
A shared pointer wrapper for ompl::base::ProblemDefinition.
Benchmark a set of planners on a problem instance.
Definition: Benchmark.h:112
double maxMem
the maximum amount of memory a planner is allowed to use (MB); 4096.0 by default
Definition: Benchmark.h:272
Definition of a scoped state.
Definition: ScopedState.h:120
double maxTime
the maximum amount of time a planner is allowed to run (seconds); 5.0 by default
Definition: Benchmark.h:269