KinematicChain.h
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 #ifndef OMPL_DEMO_KINEMATIC_CHAIN_
38 #define OMPL_DEMO_KINEMATIC_CHAIN_
39 
40 #include <ompl/base/spaces/RealVectorStateSpace.h>
41 #include <ompl/geometric/planners/rrt/RRT.h>
42 #include <ompl/geometric/planners/kpiece/KPIECE1.h>
43 #include <ompl/geometric/planners/est/EST.h>
44 #include <ompl/geometric/planners/prm/PRM.h>
45 #include <ompl/geometric/planners/stride/STRIDE.h>
46 #include <ompl/tools/benchmark/Benchmark.h>
47 
48 #include <boost/math/constants/constants.hpp>
49 #include <boost/format.hpp>
50 #include <fstream>
51 
52 // a 2D line segment
53 struct Segment
54 {
55  Segment(double p0_x, double p0_y, double p1_x, double p1_y) : x0(p0_x), y0(p0_y), x1(p1_x), y1(p1_y)
56  {
57  }
58  double x0, y0, x1, y1;
59 };
60 
61 // the robot and environment are modeled both as a vector of segments.
62 using Environment = std::vector<Segment>;
63 
64 // simply use a random projection
66 {
67 public:
69  {
70  int dimension = std::max(2, (int)ceil(log((double)space->getDimension())));
71  projectionMatrix_.computeRandom(space->getDimension(), dimension);
72  }
73  unsigned int getDimension() const override
74  {
75  return projectionMatrix_.mat.rows();
76  }
77  void project(const ompl::base::State *state, Eigen::Ref<Eigen::VectorXd> projection) const override
78  {
79  std::vector<double> v(space_->getDimension());
80  space_->copyToReals(v, state);
81  projectionMatrix_.project(&v[0], projection);
82  }
83 
84 protected:
85  ompl::base::ProjectionMatrix projectionMatrix_;
86 };
87 
89 {
90 public:
91  KinematicChainSpace(unsigned int numLinks, double linkLength, Environment *env = nullptr)
92  : ompl::base::RealVectorStateSpace(numLinks), linkLength_(linkLength), environment_(env)
93  {
94  ompl::base::RealVectorBounds bounds(numLinks);
95  bounds.setLow(-boost::math::constants::pi<double>());
96  bounds.setHigh(boost::math::constants::pi<double>());
97  setBounds(bounds);
98 
100  }
101 
102  void registerProjections() override
103  {
104  registerDefaultProjection(std::make_shared<KinematicChainProjector>(this));
105  }
106 
107  double distance(const ompl::base::State *state1, const ompl::base::State *state2) const override
108  {
109  const auto *cstate1 = state1->as<StateType>();
110  const auto *cstate2 = state2->as<StateType>();
111  double theta1 = 0., theta2 = 0., dx = 0., dy = 0., dist = 0.;
112 
113  for (unsigned int i = 0; i < dimension_; ++i)
114  {
115  theta1 += cstate1->values[i];
116  theta2 += cstate2->values[i];
117  dx += cos(theta1) - cos(theta2);
118  dy += sin(theta1) - sin(theta2);
119  dist += sqrt(dx * dx + dy * dy);
120  }
121 
122  return dist * linkLength_;
123  }
124 
125  void enforceBounds(ompl::base::State *state) const override
126  {
127  auto *statet = state->as<StateType>();
128 
129  for (unsigned int i = 0; i < dimension_; ++i)
130  {
131  double v = fmod(statet->values[i], 2.0 * boost::math::constants::pi<double>());
132  if (v < -boost::math::constants::pi<double>())
133  v += 2.0 * boost::math::constants::pi<double>();
134  else if (v >= boost::math::constants::pi<double>())
135  v -= 2.0 * boost::math::constants::pi<double>();
136  statet->values[i] = v;
137  }
138  }
139 
140  bool equalStates(const ompl::base::State *state1, const ompl::base::State *state2) const override
141  {
142  bool flag = true;
143  const auto *cstate1 = state1->as<StateType>();
144  const auto *cstate2 = state2->as<StateType>();
145 
146  for (unsigned int i = 0; i < dimension_ && flag; ++i)
147  flag &= fabs(cstate1->values[i] - cstate2->values[i]) < std::numeric_limits<double>::epsilon() * 2.0;
148 
149  return flag;
150  }
151 
152  void interpolate(const ompl::base::State *from, const ompl::base::State *to, const double t,
153  ompl::base::State *state) const override
154  {
155  const auto *fromt = from->as<StateType>();
156  const auto *tot = to->as<StateType>();
157  auto *statet = state->as<StateType>();
158 
159  for (unsigned int i = 0; i < dimension_; ++i)
160  {
161  double diff = tot->values[i] - fromt->values[i];
162  if (fabs(diff) <= boost::math::constants::pi<double>())
163  statet->values[i] = fromt->values[i] + diff * t;
164  else
165  {
166  if (diff > 0.0)
167  diff = 2.0 * boost::math::constants::pi<double>() - diff;
168  else
169  diff = -2.0 * boost::math::constants::pi<double>() - diff;
170 
171  statet->values[i] = fromt->values[i] - diff * t;
172  if (statet->values[i] > boost::math::constants::pi<double>())
173  statet->values[i] -= 2.0 * boost::math::constants::pi<double>();
174  else if (statet->values[i] < -boost::math::constants::pi<double>())
175  statet->values[i] += 2.0 * boost::math::constants::pi<double>();
176  }
177  }
178  }
179 
180  double linkLength() const
181  {
182  return linkLength_;
183  }
184 
185  const Environment *environment() const
186  {
187  return environment_;
188  }
189 
190 protected:
191  double linkLength_;
192  Environment *environment_;
193 };
194 
196 {
197 public:
199  {
200  }
201 
202  bool isValid(const ompl::base::State *state) const override
203  {
204  const KinematicChainSpace *space = si_->getStateSpace()->as<KinematicChainSpace>();
205  const auto *s = state->as<KinematicChainSpace::StateType>();
206 
207  return isValidImpl(space, s);
208  }
209 
210 protected:
211  bool isValidImpl(const KinematicChainSpace *space, const KinematicChainSpace::StateType *s) const
212  {
213  unsigned int n = si_->getStateDimension();
214  Environment segments;
215  double linkLength = space->linkLength();
216  double theta = 0., x = 0., y = 0., xN, yN;
217 
218  segments.reserve(n + 1);
219  for (unsigned int i = 0; i < n; ++i)
220  {
221  theta += s->values[i];
222  xN = x + cos(theta) * linkLength;
223  yN = y + sin(theta) * linkLength;
224  segments.emplace_back(x, y, xN, yN);
225  x = xN;
226  y = yN;
227  }
228  xN = x + cos(theta) * 0.001;
229  yN = y + sin(theta) * 0.001;
230  segments.emplace_back(x, y, xN, yN);
231  return selfIntersectionTest(segments) && environmentIntersectionTest(segments, *space->environment());
232  }
233 
234  // return true iff env does *not* include a pair of intersecting segments
235  bool selfIntersectionTest(const Environment &env) const
236  {
237  for (unsigned int i = 0; i < env.size(); ++i)
238  for (unsigned int j = i + 1; j < env.size(); ++j)
239  if (intersectionTest(env[i], env[j]))
240  return false;
241  return true;
242  }
243  // return true iff no segment in env0 intersects any segment in env1
244  bool environmentIntersectionTest(const Environment &env0, const Environment &env1) const
245  {
246  for (const auto &i : env0)
247  for (const auto &j : env1)
248  if (intersectionTest(i, j))
249  return false;
250  return true;
251  }
252  // return true iff segment s0 intersects segment s1
253  bool intersectionTest(const Segment &s0, const Segment &s1) const
254  {
255  // adopted from:
256  // http://stackoverflow.com/questions/563198/how-do-you-detect-where-two-line-segments-intersect/1201356#1201356
257  double s10_x = s0.x1 - s0.x0;
258  double s10_y = s0.y1 - s0.y0;
259  double s32_x = s1.x1 - s1.x0;
260  double s32_y = s1.y1 - s1.y0;
261  double denom = s10_x * s32_y - s32_x * s10_y;
262  if (fabs(denom) < std::numeric_limits<double>::epsilon())
263  return false; // Collinear
264  bool denomPositive = denom > 0;
265 
266  double s02_x = s0.x0 - s1.x0;
267  double s02_y = s0.y0 - s1.y0;
268  double s_numer = s10_x * s02_y - s10_y * s02_x;
269  if ((s_numer < std::numeric_limits<float>::epsilon()) == denomPositive)
270  return false; // No collision
271  double t_numer = s32_x * s02_y - s32_y * s02_x;
272  if ((t_numer < std::numeric_limits<float>::epsilon()) == denomPositive)
273  return false; // No collision
274  if (((s_numer - denom > -std::numeric_limits<float>::epsilon()) == denomPositive) ||
275  ((t_numer - denom > std::numeric_limits<float>::epsilon()) == denomPositive))
276  return false; // No collision
277  return true;
278  }
279 };
280 
281 Environment createHornEnvironment(unsigned int d, double eps)
282 {
283  std::ofstream envFile(boost::str(boost::format("environment_%i.dat") % d));
284  std::vector<Segment> env;
285  double w = 1. / (double)d, x = w, y = -eps, xN, yN, theta = 0.,
286  scale = w * (1. + boost::math::constants::pi<double>() * eps);
287 
288  envFile << x << " " << y << std::endl;
289  for (unsigned int i = 0; i < d - 1; ++i)
290  {
291  theta += boost::math::constants::pi<double>() / (double)d;
292  xN = x + cos(theta) * scale;
293  yN = y + sin(theta) * scale;
294  env.emplace_back(x, y, xN, yN);
295  x = xN;
296  y = yN;
297  envFile << x << " " << y << std::endl;
298  }
299 
300  theta = 0.;
301  x = w;
302  y = eps;
303  envFile << x << " " << y << std::endl;
304  scale = w * (1.0 - boost::math::constants::pi<double>() * eps);
305  for (unsigned int i = 0; i < d - 1; ++i)
306  {
307  theta += boost::math::constants::pi<double>() / d;
308  xN = x + cos(theta) * scale;
309  yN = y + sin(theta) * scale;
310  env.emplace_back(x, y, xN, yN);
311  x = xN;
312  y = yN;
313  envFile << x << " " << y << std::endl;
314  }
315  envFile.close();
316  return env;
317 }
318 
319 Environment createEmptyEnvironment(unsigned int d)
320 {
321  std::ofstream envFile(boost::str(boost::format("environment_%i.dat") % d));
322  std::vector<Segment> env;
323  envFile.close();
324  return env;
325 }
326 
327 #endif
unsigned int getStateDimension() const
Return the dimension of the state space.
int type_
A type assigned for this state space.
Definition: StateSpace.h:531
const StateSpacePtr & getStateSpace() const
Return the instance of the used state space.
void registerProjections() override
Register the projections for this state space. Usually, this is at least the default projection...
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
void project(const ompl::base::State *state, Eigen::Ref< Eigen::VectorXd > projection) const override
Compute the projection as an array of double values.
void computeRandom(unsigned int from, unsigned int to, const std::vector< double > &scale)
Wrapper for ComputeRandom(from, to, scale)
void enforceBounds(ompl::base::State *state) const override
Bring the state within the bounds of the state space. For unbounded spaces this function can be a no-...
bool equalStates(const ompl::base::State *state1, const ompl::base::State *state2) const override
Checks whether two states are equal.
virtual unsigned int getDimension() const =0
Get the dimension of the space (not the dimension of the surrounding ambient space) ...
void project(const double *from, Eigen::Ref< Eigen::VectorXd > to) const
Multiply the vector from by the contained projection matrix to obtain the vector to.
void interpolate(const ompl::base::State *from, const ompl::base::State *to, const double t, ompl::base::State *state) const override
Computes the state that lies at time t in [0, 1] on the segment that connects from state to to state...
void registerDefaultProjection(const ProjectionEvaluatorPtr &projection)
Register the default projection for this state space.
double distance(const ompl::base::State *state1, const ompl::base::State *state2) const override
Computes distance between two states. This function satisfies the properties of a metric if isMetricS...
virtual 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:328
const StateSpace * space_
The state space this projection operates on.
unsigned int dimension_
The dimension of the space.
const T * as() const
Cast this instance to a desired type.
Definition: State.h:66
Matrix mat
Projection matrix.
bool isValid(const ompl::base::State *state) const override
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().
Abstract definition for a class checking the validity of states. The implementation of this class mus...
A shared pointer wrapper for ompl::base::SpaceInformation.
A state space representing Rn. The distance function is the L2 norm.
Representation of a space in which planning can be performed. Topology specific sampling, interpolation and distance are defined.
Definition: StateSpace.h:70
Definition of an abstract state.
Definition: State.h:49
ompl::base::SO2StateSpace
A projection matrix – it allows multiplication of real vectors by a specified matrix. The matrix can also be randomly generated.
unsigned int getDimension() const override
Return the dimension of the projection defined by this evaluator.
The lower and upper bounds for an Rn space.
SpaceInformation * si_
The instance of space information this state validity checker operates on.
Abstract definition for a class computing projections to Rn. Implicit integer grids are imposed on th...
void setBounds(const RealVectorBounds &bounds)
Set the bounds of this state space. This defines the range of the space in which sampling is performe...