All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
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 = static_cast<const StateType*>(state1);
107  const StateType *cstate2 = static_cast<const StateType*>(state2);
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->components[i]->as<ompl::base::SO2StateSpace::StateType>()->value;
113  theta2 += cstate2->components[i]->as<ompl::base::SO2StateSpace::StateType>()->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 = static_cast<const KinematicChainSpace*>(si_->getStateSpace().get());
146  const KinematicChainSpace::StateType *s = static_cast<const KinematicChainSpace::StateType*>(state);
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 += static_cast<ompl::base::SO2StateSpace::StateType*>((*s)[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 = 7200, memory_limit = 1024;
308  int run_count = 20;
309  ompl::tools::Benchmark::Request request(runtime_limit, memory_limit, run_count);
310  ompl::tools::Benchmark b(ss, "KinematicChain");
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
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
const StateSpacePtr & getStateSpace(void) const
Return the instance of the used state space.
Definition of a scoped state.
Definition: ScopedState.h:56
A boost shared pointer wrapper for ompl::base::StateSpace.
ProjectionEvaluator(const StateSpace *space)
Construct a projection evaluator for a specific state space.
virtual unsigned int getDimension(void) const =0
Get the dimension of the space (not the dimension of the surrounding ambient space) ...
unsigned int getStateDimension(void) const
Return the dimension of the state space.
CompoundState StateType
Define the type of state allocated by this state space.
Definition: StateSpace.h:543
virtual unsigned int getDimension(void) const =0
Return the dimension of the projection defined by this evaluator.
void registerDefaultProjection(const ProjectionEvaluatorPtr &projection)
Register the default projection for this state space.
Definition: StateSpace.cpp:726
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(void) 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 T * as(void) const
Cast this instance to a desired type.
Definition: State.h:74
virtual void registerProjections(void)
Register the projections for this state space. Usually, this is at least the default projection...
Definition: StateSpace.cpp:216
const StateSpace * space_
The state space this projection operates on.
Create the set of classes typically needed to solve a geometric problem.
Definition: SimpleSetup.h:64
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:538
Abstract definition for a class checking the validity of states. The implementation of this class mus...
StateValidityChecker(SpaceInformation *si)
Constructor.
unsigned int getSubspaceCount(void) const
Get the number of state spaces that make up the compound state space.
Definition: StateSpace.cpp:872
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...
CompoundStateSpace(void)
Construct an empty compound state space.
Definition: StateSpace.cpp:823
void lock(void)
Lock this state space. This means no further spaces can be added as components. This function can be ...
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:839
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:151
Probabilistic RoadMap planner.
Definition: PRM.h:83
State ** components
The components that make up a compound state.
Definition: State.h:135
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.
Definition of a geometric path.
Definition: PathGeometric.h:55
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:308
Abstract definition for a class computing projections to Rn. Implicit integer grids are imposed on th...