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  : 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 auto *cstate1 = state1->as<StateType>();
106  const auto *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>();
145  const auto *s = state->as<KinematicChainSpace::StateType>();
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(boost::str(boost::format("environment_%i.dat") % d));
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  auto 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  std::ofstream pathfile(
294  boost::str(boost::format("kinematic_path_%i.dat") % numLinks).c_str());
295  ss.getSolutionPath().printAsMatrix(pathfile);
296  exit(0);
297  }
298 
299  // by default, use the Benchmark class
300  double runtime_limit = 60, memory_limit = 1024;
301  int run_count = 20;
302  ompl::tools::Benchmark::Request request(runtime_limit, memory_limit, run_count, 0.5);
303  ompl::tools::Benchmark b(ss, "KinematicChain");
304  b.addExperimentParameter("num_links", "INTEGER", std::to_string(numLinks));
305 
306  b.addPlanner(std::make_shared<ompl::geometric::STRIDE>(ss.getSpaceInformation()));
307  b.addPlanner(std::make_shared<ompl::geometric::EST>(ss.getSpaceInformation()));
308  b.addPlanner(std::make_shared<ompl::geometric::KPIECE1>(ss.getSpaceInformation()));
309  b.addPlanner(std::make_shared<ompl::geometric::RRT>(ss.getSpaceInformation()));
310  b.addPlanner(std::make_shared<ompl::geometric::PRM>(ss.getSpaceInformation()));
311  b.benchmark(request);
312  b.saveResultsToFile(boost::str(boost::format("kinematic_%i.log") % numLinks).c_str());
313 
314  exit(0);
315 }
unsigned int getStateDimension() const
Return the dimension of the state space.
const StateSpacePtr & getStateSpace() const
Return the instance of the used state space.
double distance(const State *state1, const State *state2) const override
Computes distance between two states. This function satisfies the properties of a metric if isMetricS...
unsigned int getSubspaceCount() const
Get the number of state spaces that make up the compound state space.
virtual unsigned int getDimension() const =0
Return the dimension of the projection defined by this evaluator.
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
void lock()
Lock this state space. This means no further spaces can be added as components. This function can be ...
virtual unsigned int getDimension() const =0
Get the dimension of the space (not the dimension of the surrounding ambient space) ...
virtual void registerProjections()
Register the projections for this state space. Usually, this is at least the default projection...
Definition: StateSpace.cpp:244
void registerDefaultProjection(const ProjectionEvaluatorPtr &projection)
Register the default projection for this state space.
Benchmark a set of planners on a problem instance.
Definition: Benchmark.h:48
The definition of a state in SO(2)
Definition: SO2StateSpace.h:67
void copyToReals(std::vector< double > &reals, const State *source) const
Copy all the real values from a state source to the array reals using getValueAddressAtLocation() ...
Definition: StateSpace.cpp:340
const StateSpace * space_
The state space this projection operates on.
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
ompl::base::CompoundState StateType
Define the type of state allocated by this state space.
Definition: StateSpace.h:577
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...
StateValidityChecker(SpaceInformation *si)
Constructor.
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.
void addSubspace(const StateSpacePtr &component, double weight)
Adds a new state space as part of the compound state space. For computing distances within the compou...
virtual void project(const State *state, EuclideanProjection &projection) const =0
Compute the projection as an array of double values.
virtual bool isValid(const State *state) const =0
Return true if the state state is valid. Usually, this means at least collision checking. If it is possible that ompl::base::StateSpace::interpolate() or ompl::control::ControlSpace::propagate() return states that are outside of bounds, this function should also make a call to ompl::base::SpaceInformation::satisfiesBounds().
Representation of a benchmark request.
Definition: Benchmark.h:156
SpaceInformation * si_
The instance of space information this state validity checker operates on.
const T * as(unsigned int index) const
Cast a component of this instance to a desired type.
Definition: State.h:95
Abstract definition for a class computing projections to Rn. Implicit integer grids are imposed on th...