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 typedef std::vector<Segment> Environment;
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  virtual unsigned int getDimension(void) const
73  {
74  return projectionMatrix_.mat.size1();
75  }
76  void project(const ompl::base::State *state, ompl::base::EuclideanProjection &projection) const
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 = NULL)
91  : ompl::base::CompoundStateSpace(), linkLength_(linkLength), environment_(env)
92  {
93  for (unsigned int i = 0; i < numLinks; ++i)
95  lock();
96  }
97 
98  void registerProjections()
99  {
101  new KinematicChainProjector(this)));
102  }
103 
104  double distance(const ompl::base::State *state1, const ompl::base::State *state2) const
105  {
106  const StateType *cstate1 = state1->as<StateType>();
107  const StateType *cstate2 = state2->as<StateType>();
108  double theta1 = 0., theta2 = 0., dx = 0., dy = 0., dist = 0.;
109 
110  for (unsigned int i = 0; i < getSubspaceCount(); ++i)
111  {
112  theta1 += cstate1->as<ompl::base::SO2StateSpace::StateType>(i)->value;
113  theta2 += cstate2->as<ompl::base::SO2StateSpace::StateType>(i)->value;
114  dx += cos(theta1) - cos(theta2);
115  dy += sin(theta1) - sin(theta2);
116  dist += sqrt(dx * dx + dy * dy);
117  }
118  return dist * linkLength_;
119  }
120  double linkLength() const
121  {
122  return linkLength_;
123  }
124  const Environment* environment() const
125  {
126  return environment_;
127  }
128 
129 protected:
130  double linkLength_;
131  Environment* environment_;
132 };
133 
134 
135 class KinematicChainValidityChecker : public ompl::base::StateValidityChecker
136 {
137 public:
138  KinematicChainValidityChecker(const ompl::base::SpaceInformationPtr &si)
139  : ompl::base::StateValidityChecker(si)
140  {
141  }
142 
143  bool isValid(const ompl::base::State *state) const
144  {
145  const KinematicChainSpace* space = si_->getStateSpace()->as<KinematicChainSpace>();
147  unsigned int n = si_->getStateDimension();
148  Environment segments;
149  double linkLength = space->linkLength();
150  double theta = 0., x = 0., y = 0., xN, yN;
151 
152  segments.reserve(n + 1);
153  for(unsigned int i = 0; i < n; ++i)
154  {
155  theta += s->as<ompl::base::SO2StateSpace::StateType>(i)->value;
156  xN = x + cos(theta) * linkLength;
157  yN = y + sin(theta) * linkLength;
158  segments.push_back(Segment(x, y, xN, yN));
159  x = xN;
160  y = yN;
161  }
162  xN = x + cos(theta) * 0.001;
163  yN = y + sin(theta) * 0.001;
164  segments.push_back(Segment(x, y, xN, yN));
165  return selfIntersectionTest(segments)
166  && environmentIntersectionTest(segments, *space->environment());
167  }
168 
169 protected:
170  // return true iff env does *not* include a pair of intersecting segments
171  bool selfIntersectionTest(const Environment& env) const
172  {
173  for (unsigned int i = 0; i < env.size(); ++i)
174  for (unsigned int j = i + 1; j < env.size(); ++j)
175  if (intersectionTest(env[i], env[j]))
176  return false;
177  return true;
178  }
179  // return true iff no segment in env0 intersects any segment in env1
180  bool environmentIntersectionTest(const Environment& env0, const Environment& env1) const
181  {
182  for (unsigned int i = 0; i < env0.size(); ++i)
183  for (unsigned int j = 0; j < env1.size(); ++j)
184  if (intersectionTest(env0[i], env1[j]))
185  return false;
186  return true;
187  }
188  // return true iff segment s0 intersects segment s1
189  bool intersectionTest(const Segment& s0, const Segment& s1) const
190  {
191  // adopted from:
192  // http://stackoverflow.com/questions/563198/how-do-you-detect-where-two-line-segments-intersect/1201356#1201356
193  double s10_x = s0.x1 - s0.x0;
194  double s10_y = s0.y1 - s0.y0;
195  double s32_x = s1.x1 - s1.x0;
196  double s32_y = s1.y1 - s1.y0;
197  double denom = s10_x * s32_y - s32_x * s10_y;
198  if (fabs(denom) < std::numeric_limits<double>::epsilon())
199  return false; // Collinear
200  bool denomPositive = denom > 0;
201 
202  double s02_x = s0.x0 - s1.x0;
203  double s02_y = s0.y0 - s1.y0;
204  double s_numer = s10_x * s02_y - s10_y * s02_x;
205  if ((s_numer < std::numeric_limits<float>::epsilon()) == denomPositive)
206  return false; // No collision
207  double t_numer = s32_x * s02_y - s32_y * s02_x;
208  if ((t_numer < std::numeric_limits<float>::epsilon()) == denomPositive)
209  return false; // No collision
210  if (((s_numer - denom > -std::numeric_limits<float>::epsilon()) == denomPositive)
211  || ((t_numer - denom > std::numeric_limits<float>::epsilon()) == denomPositive))
212  return false; // No collision
213  return true;
214  }
215 };
216 
217 
218 Environment createHornEnvironment(unsigned int d, double eps)
219 {
220  std::ofstream envFile("environment.dat");
221  std::vector<Segment> env;
222  double w = 1. / (double)d, x = w, y = -eps, xN, yN, theta = 0.,
223  scale = w * (1. + boost::math::constants::pi<double>() * eps);
224 
225  envFile << x << " " << y << std::endl;
226  for(unsigned int i = 0; i < d - 1; ++i)
227  {
228  theta += boost::math::constants::pi<double>() / (double) d;
229  xN = x + cos(theta) * scale;
230  yN = y + sin(theta) * scale;
231  env.push_back(Segment(x, y, xN, yN));
232  x = xN;
233  y = yN;
234  envFile << x << " " << y << std::endl;
235  }
236 
237  theta = 0.;
238  x = w;
239  y = eps;
240  envFile << x << " " << y << std::endl;
241  scale = w * (1.0 - boost::math::constants::pi<double>() * eps);
242  for(unsigned int i = 0; i < d - 1; ++i)
243  {
244  theta += boost::math::constants::pi<double>() / d;
245  xN = x + cos(theta) * scale;
246  yN = y + sin(theta) * scale;
247  env.push_back(Segment(x, y, xN, yN));
248  x = xN;
249  y = yN;
250  envFile << x << " " << y << std::endl;
251  }
252  envFile.close();
253  return env;
254 }
255 
256 
257 int main(int argc, char **argv)
258 {
259  if (argc < 2)
260  {
261  std::cout << "Usage:\n" << argv[0] << " <num_links>\n";
262  exit(0);
263  }
264 
265  unsigned int numLinks = boost::lexical_cast<unsigned int>(std::string(argv[1]));
266  Environment env = createHornEnvironment(numLinks, log((double)numLinks) / (double)numLinks);
267  ompl::base::StateSpacePtr chain(new KinematicChainSpace(numLinks, 1. / (double)numLinks, &env));
269 
270  ss.setStateValidityChecker(ompl::base::StateValidityCheckerPtr(
271  new KinematicChainValidityChecker(ss.getSpaceInformation())));
272 
273  ompl::base::ScopedState<> start(chain), goal(chain);
274  std::vector<double> startVec(numLinks, boost::math::constants::pi<double>() / (double)numLinks);
275  std::vector<double> goalVec(numLinks, 0.);
276 
277  startVec[0] = 0.;
278  goalVec[0] = boost::math::constants::pi<double>() - .001;
279  chain->setup();
280  chain->copyFromReals(start.get(), startVec);
281  chain->copyFromReals(goal.get(), goalVec);
282  ss.setStartAndGoalStates(start, goal);
283 
284  // SEKRIT BONUS FEATURE:
285  // if you specify a second command line argument, it will solve the
286  // problem just once with STRIDE and print out the solution path.
287  if (argc > 2)
288  {
289  ss.setPlanner(ompl::base::PlannerPtr(new ompl::geometric::STRIDE(ss.getSpaceInformation())));
290  ss.setup();
291  ss.print();
292  ss.solve(3600);
293  ss.simplifySolution();
294 
295  ompl::geometric::PathGeometric path = ss.getSolutionPath();
296  std::vector<double> v;
297  for(unsigned int i = 0; i < path.getStateCount(); ++i)
298  {
299  chain->copyToReals(v, path.getState(i));
300  std::copy(v.begin(), v.end(), std::ostream_iterator<double>(std::cout, " "));
301  std::cout << std::endl;
302  }
303  exit(0);
304  }
305 
306  // by default, use the Benchmark class
307  double runtime_limit = 60, memory_limit = 1024;
308  int run_count = 20;
309  ompl::tools::Benchmark::Request request(runtime_limit, memory_limit, run_count, 0.5);
310  ompl::tools::Benchmark b(ss, boost::str(boost::format("KinematicChain%i") % numLinks));
311 
312  b.addPlanner(ompl::base::PlannerPtr(new ompl::geometric::STRIDE(ss.getSpaceInformation())));
313  b.addPlanner(ompl::base::PlannerPtr(new ompl::geometric::EST(ss.getSpaceInformation())));
314  b.addPlanner(ompl::base::PlannerPtr(new ompl::geometric::KPIECE1(ss.getSpaceInformation())));
315  b.addPlanner(ompl::base::PlannerPtr(new ompl::geometric::RRT(ss.getSpaceInformation())));
316  b.addPlanner(ompl::base::PlannerPtr(new ompl::geometric::PRM(ss.getSpaceInformation())));
317  b.benchmark(request);
318  b.saveResultsToFile(boost::str(boost::format("kinematic_%i.log") % numLinks).c_str());
319 
320  exit(0);
321 }
Search Tree with Resolution Independent Density Estimation.
Definition: STRIDE.h:80
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:104
Definition of a scoped state.
Definition: ScopedState.h:56
A boost shared pointer wrapper for ompl::base::StateSpace.
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) ...
ProjectionEvaluator(const StateSpace *space)
Construct a projection evaluator for a specific state space.
STL namespace.
CompoundState StateType
Define the type of state allocated by this state space.
Definition: StateSpace.h:549
virtual void registerProjections()
Register the projections for this state space. Usually, this is at least the default projection...
Definition: StateSpace.cpp:227
void registerDefaultProjection(const ProjectionEvaluatorPtr &projection)
Register the default projection for this state space.
Definition: StateSpace.cpp:737
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
std::size_t getStateCount() const
Get the number of states (way-points) that make up this path.
The definition of a state in SO(2)
Definition: SO2StateSpace.h:70
A boost shared pointer wrapper for ompl::base::StateValidityChecker.
const StateSpace * space_
The state space this projection operates on.
Main namespace. Contains everything in this library.
Definition: Cost.h:42
Create the set of classes typically needed to solve a geometric problem.
Definition: SimpleSetup.h:65
A boost shared pointer wrapper for ompl::base::Planner.
Kinematic Planning by Interior-Exterior Cell Exploration.
Definition: KPIECE1.h:74
A space to allow the composition of state spaces.
Definition: StateSpace.h:544
Abstract definition for a class checking the validity of states. The implementation of this class mus...
StateValidityChecker(SpaceInformation *si)
Constructor.
A boost shared pointer wrapper for ompl::base::ProjectionEvaluator.
Rapidly-exploring Random Trees.
Definition: RRT.h:65
A boost 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:73
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:50
virtual double distance(const State *state1, const State *state2) const
Computes distance between two states. This function satisfies the properties of a metric if isMetricS...
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...
Definition: StateSpace.cpp:855
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:154
const StateSpacePtr & getStateSpace() const
Return the instance of the used state space.
Probabilistic RoadMap planner.
Definition: PRM.h:83
unsigned int getStateDimension() const
Return the dimension of the state space.
unsigned int getSubspaceCount() const
Get the number of state spaces that make up the compound state space.
Definition: StateSpace.cpp:888
A state space representing SO(2). The distance function and interpolation take into account angle wra...
Definition: SO2StateSpace.h:65
SpaceInformation * si_
The instance of space information this state validity checker operates on.
CompoundStateSpace()
Construct an empty compound state space.
Definition: StateSpace.cpp:839
Definition of a geometric path.
Definition: PathGeometric.h:60
const T * as() const
Cast this instance to a desired type.
Definition: State.h:74
Expansive Space Trees.
Definition: EST.h:73
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:319
const T * as(const unsigned int index) const
Cast a component of this instance to a desired type.
Definition: State.h:109
Abstract definition for a class computing projections to Rn. Implicit integer grids are imposed on th...