KinematicChainBenchmark.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2013, Rice University
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 Rice University nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: Bryant Gipson, Mark Moll */
36 
37 #include <ompl/base/spaces/SO2StateSpace.h>
38 #include <ompl/geometric/planners/rrt/RRT.h>
39 #include <ompl/geometric/planners/kpiece/KPIECE1.h>
40 #include <ompl/geometric/planners/est/EST.h>
41 #include <ompl/geometric/planners/prm/PRM.h>
42 #include <ompl/geometric/planners/stride/STRIDE.h>
43 #include <ompl/tools/benchmark/Benchmark.h>
44 
45 #include <boost/math/constants/constants.hpp>
46 #include <boost/format.hpp>
47 #include <fstream>
48 
49 // a 2D line segment
50 struct Segment
51 {
52  Segment(double p0_x, double p0_y, double p1_x, double p1_y)
53  : x0(p0_x), y0(p0_y), x1(p1_x), y1(p1_y)
54  {
55  }
56  double x0, y0, x1, y1;
57 };
58 
59 // the robot and environment are modeled both as a vector of segments.
60 using Environment = std::vector<Segment>;
61 
62 // simply use a random projection
63 class KinematicChainProjector : public ompl::base::ProjectionEvaluator
64 {
65 public:
66  KinematicChainProjector(const ompl::base::StateSpace *space)
67  : ompl::base::ProjectionEvaluator(space)
68  {
69  int dimension = std::max(2, (int)ceil(log((double) space->getDimension())));
70  projectionMatrix_.computeRandom(space->getDimension(), dimension);
71  }
72  unsigned int getDimension() const override
73  {
74  return projectionMatrix_.mat.size1();
75  }
76  void project(const ompl::base::State *state, ompl::base::EuclideanProjection &projection) const override
77  {
78  std::vector<double> v(space_->getDimension());
79  space_->copyToReals(v, state);
80  projectionMatrix_.project(&v[0], projection);
81  }
82 protected:
83  ompl::base::ProjectionMatrix projectionMatrix_;
84 };
85 
86 
87 class KinematicChainSpace : public ompl::base::CompoundStateSpace
88 {
89 public:
90  KinematicChainSpace(unsigned int numLinks, double linkLength, Environment *env = nullptr)
91  : ompl::base::CompoundStateSpace(), linkLength_(linkLength), environment_(env)
92  {
93  for (unsigned int i = 0; i < numLinks; ++i)
94  addSubspace(std::make_shared<ompl::base::SO2StateSpace>(), 1.);
95  lock();
96  }
97 
98  void registerProjections() override
99  {
100  registerDefaultProjection(std::make_shared<KinematicChainProjector>(this));
101  }
102 
103  double distance(const ompl::base::State *state1, const ompl::base::State *state2) const override
104  {
105  const StateType *cstate1 = state1->as<StateType>();
106  const StateType *cstate2 = state2->as<StateType>();
107  double theta1 = 0., theta2 = 0., dx = 0., dy = 0., dist = 0.;
108 
109  for (unsigned int i = 0; i < getSubspaceCount(); ++i)
110  {
111  theta1 += cstate1->as<ompl::base::SO2StateSpace::StateType>(i)->value;
112  theta2 += cstate2->as<ompl::base::SO2StateSpace::StateType>(i)->value;
113  dx += cos(theta1) - cos(theta2);
114  dy += sin(theta1) - sin(theta2);
115  dist += sqrt(dx * dx + dy * dy);
116  }
117  return dist * linkLength_;
118  }
119  double linkLength() const
120  {
121  return linkLength_;
122  }
123  const Environment* environment() const
124  {
125  return environment_;
126  }
127 
128 protected:
129  double linkLength_;
130  Environment* environment_;
131 };
132 
133 
134 class KinematicChainValidityChecker : public ompl::base::StateValidityChecker
135 {
136 public:
137  KinematicChainValidityChecker(const ompl::base::SpaceInformationPtr &si)
138  : ompl::base::StateValidityChecker(si)
139  {
140  }
141 
142  bool isValid(const ompl::base::State *state) const override
143  {
144  const KinematicChainSpace* space = si_->getStateSpace()->as<KinematicChainSpace>();
146  unsigned int n = si_->getStateDimension();
147  Environment segments;
148  double linkLength = space->linkLength();
149  double theta = 0., x = 0., y = 0., xN, yN;
150 
151  segments.reserve(n + 1);
152  for(unsigned int i = 0; i < n; ++i)
153  {
154  theta += s->as<ompl::base::SO2StateSpace::StateType>(i)->value;
155  xN = x + cos(theta) * linkLength;
156  yN = y + sin(theta) * linkLength;
157  segments.emplace_back(x, y, xN, yN);
158  x = xN;
159  y = yN;
160  }
161  xN = x + cos(theta) * 0.001;
162  yN = y + sin(theta) * 0.001;
163  segments.emplace_back(x, y, xN, yN);
164  return selfIntersectionTest(segments)
165  && environmentIntersectionTest(segments, *space->environment());
166  }
167 
168 protected:
169  // return true iff env does *not* include a pair of intersecting segments
170  bool selfIntersectionTest(const Environment& env) const
171  {
172  for (unsigned int i = 0; i < env.size(); ++i)
173  for (unsigned int j = i + 1; j < env.size(); ++j)
174  if (intersectionTest(env[i], env[j]))
175  return false;
176  return true;
177  }
178  // return true iff no segment in env0 intersects any segment in env1
179  bool environmentIntersectionTest(const Environment& env0, const Environment& env1) const
180  {
181  for (const auto & i : env0)
182  for (const auto & j : env1)
183  if (intersectionTest(i, j))
184  return false;
185  return true;
186  }
187  // return true iff segment s0 intersects segment s1
188  bool intersectionTest(const Segment& s0, const Segment& s1) const
189  {
190  // adopted from:
191  // http://stackoverflow.com/questions/563198/how-do-you-detect-where-two-line-segments-intersect/1201356#1201356
192  double s10_x = s0.x1 - s0.x0;
193  double s10_y = s0.y1 - s0.y0;
194  double s32_x = s1.x1 - s1.x0;
195  double s32_y = s1.y1 - s1.y0;
196  double denom = s10_x * s32_y - s32_x * s10_y;
197  if (fabs(denom) < std::numeric_limits<double>::epsilon())
198  return false; // Collinear
199  bool denomPositive = denom > 0;
200 
201  double s02_x = s0.x0 - s1.x0;
202  double s02_y = s0.y0 - s1.y0;
203  double s_numer = s10_x * s02_y - s10_y * s02_x;
204  if ((s_numer < std::numeric_limits<float>::epsilon()) == denomPositive)
205  return false; // No collision
206  double t_numer = s32_x * s02_y - s32_y * s02_x;
207  if ((t_numer < std::numeric_limits<float>::epsilon()) == denomPositive)
208  return false; // No collision
209  if (((s_numer - denom > -std::numeric_limits<float>::epsilon()) == denomPositive)
210  || ((t_numer - denom > std::numeric_limits<float>::epsilon()) == denomPositive))
211  return false; // No collision
212  return true;
213  }
214 };
215 
216 
217 Environment createHornEnvironment(unsigned int d, double eps)
218 {
219  std::ofstream envFile("environment.dat");
220  std::vector<Segment> env;
221  double w = 1. / (double)d, x = w, y = -eps, xN, yN, theta = 0.,
222  scale = w * (1. + boost::math::constants::pi<double>() * eps);
223 
224  envFile << x << " " << y << std::endl;
225  for(unsigned int i = 0; i < d - 1; ++i)
226  {
227  theta += boost::math::constants::pi<double>() / (double) d;
228  xN = x + cos(theta) * scale;
229  yN = y + sin(theta) * scale;
230  env.emplace_back(x, y, xN, yN);
231  x = xN;
232  y = yN;
233  envFile << x << " " << y << std::endl;
234  }
235 
236  theta = 0.;
237  x = w;
238  y = eps;
239  envFile << x << " " << y << std::endl;
240  scale = w * (1.0 - boost::math::constants::pi<double>() * eps);
241  for(unsigned int i = 0; i < d - 1; ++i)
242  {
243  theta += boost::math::constants::pi<double>() / d;
244  xN = x + cos(theta) * scale;
245  yN = y + sin(theta) * scale;
246  env.emplace_back(x, y, xN, yN);
247  x = xN;
248  y = yN;
249  envFile << x << " " << y << std::endl;
250  }
251  envFile.close();
252  return env;
253 }
254 
255 
256 int main(int argc, char **argv)
257 {
258  if (argc < 2)
259  {
260  std::cout << "Usage:\n" << argv[0] << " <num_links>\n";
261  exit(0);
262  }
263 
264  unsigned int numLinks = boost::lexical_cast<unsigned int>(std::string(argv[1]));
265  Environment env = createHornEnvironment(numLinks, log((double)numLinks) / (double)numLinks);
266  auto chain(std::make_shared<KinematicChainSpace>(numLinks, 1. / (double)numLinks, &env));
268 
269  ss.setStateValidityChecker(std::make_shared<KinematicChainValidityChecker>(ss.getSpaceInformation()));
270 
271  ompl::base::ScopedState<> start(chain), goal(chain);
272  std::vector<double> startVec(numLinks, boost::math::constants::pi<double>() / (double)numLinks);
273  std::vector<double> goalVec(numLinks, 0.);
274 
275  startVec[0] = 0.;
276  goalVec[0] = boost::math::constants::pi<double>() - .001;
277  chain->setup();
278  chain->copyFromReals(start.get(), startVec);
279  chain->copyFromReals(goal.get(), goalVec);
280  ss.setStartAndGoalStates(start, goal);
281 
282  // SEKRIT BONUS FEATURE:
283  // if you specify a second command line argument, it will solve the
284  // problem just once with STRIDE and print out the solution path.
285  if (argc > 2)
286  {
287  ss.setPlanner(std::make_shared<ompl::geometric::STRIDE>(ss.getSpaceInformation()));
288  ss.setup();
289  ss.print();
290  ss.solve(3600);
291  ss.simplifySolution();
292 
293  ompl::geometric::PathGeometric path = ss.getSolutionPath();
294  std::vector<double> v;
295  for(unsigned int i = 0; i < path.getStateCount(); ++i)
296  {
297  chain->copyToReals(v, path.getState(i));
298  std::copy(v.begin(), v.end(), std::ostream_iterator<double>(std::cout, " "));
299  std::cout << std::endl;
300  }
301  exit(0);
302  }
303 
304  // by default, use the Benchmark class
305  double runtime_limit = 60, memory_limit = 1024;
306  int run_count = 20;
307  ompl::tools::Benchmark::Request request(runtime_limit, memory_limit, run_count, 0.5);
308  ompl::tools::Benchmark b(ss, "KinematicChain");
309  b.addExperimentParameter("num_links", "INTEGER", std::to_string(numLinks));
310 
311  b.addPlanner(std::make_shared<ompl::geometric::STRIDE>(ss.getSpaceInformation()));
312  b.addPlanner(std::make_shared<ompl::geometric::EST>(ss.getSpaceInformation()));
313  b.addPlanner(std::make_shared<ompl::geometric::KPIECE1>(ss.getSpaceInformation()));
314  b.addPlanner(std::make_shared<ompl::geometric::RRT>(ss.getSpaceInformation()));
315  b.addPlanner(std::make_shared<ompl::geometric::PRM>(ss.getSpaceInformation()));
316  b.benchmark(request);
317  b.saveResultsToFile(boost::str(boost::format("kinematic_%i.log") % numLinks).c_str());
318 
319  exit(0);
320 }
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
Definition of a scoped state.
Definition: ScopedState.h:56
virtual unsigned int getDimension() const =0
Get the dimension of the space (not the dimension of the surrounding ambient space) ...
std::size_t getStateCount() const
Get the number of states (way-points) that make up this path.
base::State * getState(unsigned int index)
Get the state located at index along the path.
Benchmark a set of planners on a problem instance.
Definition: Benchmark.h:48
T * as()
Cast this instance to a desired type.
Definition: StateSpace.h:87
The definition of a state in SO(2)
Definition: SO2StateSpace.h:67
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
Create the set of classes typically needed to solve a geometric problem.
Definition: SimpleSetup.h:63
const T * as() const
Cast this instance to a desired type.
Definition: State.h:66
A space to allow the composition of state spaces.
Definition: StateSpace.h:573
Abstract definition for a class checking the validity of states. The implementation of this class mus...
A shared pointer wrapper for ompl::base::SpaceInformation.
Representation of a space in which planning can be performed. Topology specific sampling, interpolation and distance are defined.
Definition: StateSpace.h:70
boost::numeric::ublas::vector< double > EuclideanProjection
The datatype for state projections. This class contains a real vector.
Definition of an abstract state.
Definition: State.h:49
A projection matrix – it allows multiplication of real vectors by a specified matrix. The matrix can also be randomly generated.
Representation of a benchmark request.
Definition: Benchmark.h:156
Definition of a geometric path.
Definition: PathGeometric.h:60
Abstract definition for a class computing projections to Rn. Implicit integer grids are imposed on th...