Loading...
Searching...
No Matches
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
45const unsigned int numLinks = 15;
46const double runtime_limit = 5;
47const double memory_limit = 4096;
48const int run_count = 2;
49
50const double linkLength = 1.0 / numLinks;
51
52using namespace ompl::base;
53std::vector<Environment> envs;
54
55Environment createCustomHornEnvironment(unsigned int d)
56{
57 double narrowPassageWidth = log((double)d) / (double)d;
58 return createHornEnvironment(d, narrowPassageWidth);
59}
60
61PlannerPtr 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
96std::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
129int 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::base::Planner.
A shared pointer wrapper for ompl::base::ProblemDefinition.
Definition of a scoped state.
Definition ScopedState.h:57
A shared pointer wrapper for ompl::base::SpaceInformation.
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
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition Console.h:68
This namespace contains sampling based planning routines shared by both planning under geometric cons...
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
Representation of a benchmark request.
Definition Benchmark.h:157
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:180
bool displayProgress
flag indicating whether progress is to be displayed or not; true by default
Definition Benchmark.h:187
double maxMem
the maximum amount of memory a planner is allowed to use (MB); 4096.0 by default
Definition Benchmark.h:176
double maxTime
the maximum amount of time a planner is allowed to run (seconds); 5.0 by default
Definition Benchmark.h:173
bool simplify
flag indicating whether simplification should be applied to path; true by default
Definition Benchmark.h:194