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
bool equalStates(const ompl::base::State *state1, const ompl::base::State *state2) const override
Checks whether two states are equal.
A shared pointer wrapper for ompl::base::SpaceInformation.
Representation of a space in which planning can be performed. Topology specific sampling,...
Definition: StateSpace.h:134
void registerProjections() override
Register the projections for this state space. Usually, this is at least the default projection....
void computeRandom(unsigned int from, unsigned int to, const std::vector< double > &scale)
Wrapper for ComputeRandom(from, to, scale)
Definition of an abstract state.
Definition: State.h:113
unsigned int getStateDimension() const
Return the dimension of the state space.
A projection matrix – it allows multiplication of real vectors by a specified matrix....
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 setHigh(double value)
Set the upper bound in each dimension to a specific value.
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:329
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
@ STATE_SPACE_SO2
ompl::base::SO2StateSpace
void setLow(double value)
Set the lower bound in each dimension to a specific value.
const T * as() const
Cast this instance to a desired type.
Definition: State.h:162
bool isValid(const ompl::base::State *state) const override
Return true if the state state is valid. Usually, this means at least collision checking....
virtual unsigned int getDimension() const =0
Get the dimension of the space (not the dimension of the surrounding ambient space)
A state space representing Rn. The distance function is the L2 norm.
unsigned int getDimension() const override
Return the dimension of the projection defined by this evaluator.
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...
void registerDefaultProjection(const ProjectionEvaluatorPtr &projection)
Register the default projection for this state space.
Definition: StateSpace.cpp:752
unsigned int dimension_
The dimension of the space.
void setBounds(const RealVectorBounds &bounds)
Set the bounds of this state space. This defines the range of the space in which sampling is performe...
Abstract definition for a class computing projections to Rn. Implicit integer grids are imposed on th...
int type_
A type assigned for this state space.
Definition: StateSpace.h:595
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....
SpaceInformation * si_
The instance of space information this state validity checker operates on.
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-...
const StateSpacePtr & getStateSpace() const
Return the instance of the used state space.
void project(const ompl::base::State *state, Eigen::Ref< Eigen::VectorXd > projection) const override
Compute the projection as an array of double values.
const StateSpace * space_
The state space this projection operates on.
Abstract definition for a class checking the validity of states. The implementation of this class mus...
Matrix mat
Projection matrix.
The lower and upper bounds for an Rn space.