Loading...
Searching...
No Matches
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
53struct 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.
62using Environment = std::vector<Segment>;
63
64// simply use a random projection
65class KinematicChainProjector : public ompl::base::ProjectionEvaluator
66{
67public:
68 KinematicChainProjector(const ompl::base::StateSpace *space) : ompl::base::ProjectionEvaluator(space)
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
84protected:
85 ompl::base::ProjectionMatrix projectionMatrix_;
86};
87
88class KinematicChainSpace : public ompl::base::RealVectorStateSpace
89{
90public:
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 }
99
100 void registerProjections() override
101 {
102 registerDefaultProjection(std::make_shared<KinematicChainProjector>(this));
103 }
104
105 double distance(const ompl::base::State *state1, const ompl::base::State *state2) const override
106 {
107 const auto *cstate1 = state1->as<StateType>();
108 const auto *cstate2 = state2->as<StateType>();
109 double theta1 = 0., theta2 = 0., dx = 0., dy = 0., dist = 0.;
110
111 for (unsigned int i = 0; i < dimension_; ++i)
112 {
113 theta1 += cstate1->values[i];
114 theta2 += cstate2->values[i];
115 dx += cos(theta1) - cos(theta2);
116 dy += sin(theta1) - sin(theta2);
117 dist += sqrt(dx * dx + dy * dy);
118 }
119
120 return dist * linkLength_;
121 }
122
123 void enforceBounds(ompl::base::State *state) const override
124 {
125 auto *statet = state->as<StateType>();
126
127 for (unsigned int i = 0; i < dimension_; ++i)
128 {
129 double v = fmod(statet->values[i], 2.0 * boost::math::constants::pi<double>());
130 if (v < -boost::math::constants::pi<double>())
131 v += 2.0 * boost::math::constants::pi<double>();
132 else if (v >= boost::math::constants::pi<double>())
133 v -= 2.0 * boost::math::constants::pi<double>();
134 statet->values[i] = v;
135 }
136 }
137
138 bool equalStates(const ompl::base::State *state1, const ompl::base::State *state2) const override
139 {
140 bool flag = true;
141 const auto *cstate1 = state1->as<StateType>();
142 const auto *cstate2 = state2->as<StateType>();
143
144 for (unsigned int i = 0; i < dimension_ && flag; ++i)
145 flag &= fabs(cstate1->values[i] - cstate2->values[i]) < std::numeric_limits<double>::epsilon() * 2.0;
146
147 return flag;
148 }
149
150 void interpolate(const ompl::base::State *from, const ompl::base::State *to, const double t,
151 ompl::base::State *state) const override
152 {
153 const auto *fromt = from->as<StateType>();
154 const auto *tot = to->as<StateType>();
155 auto *statet = state->as<StateType>();
156
157 for (unsigned int i = 0; i < dimension_; ++i)
158 {
159 double diff = tot->values[i] - fromt->values[i];
160 if (fabs(diff) <= boost::math::constants::pi<double>())
161 statet->values[i] = fromt->values[i] + diff * t;
162 else
163 {
164 if (diff > 0.0)
165 diff = 2.0 * boost::math::constants::pi<double>() - diff;
166 else
167 diff = -2.0 * boost::math::constants::pi<double>() - diff;
168
169 statet->values[i] = fromt->values[i] - diff * t;
170 if (statet->values[i] > boost::math::constants::pi<double>())
171 statet->values[i] -= 2.0 * boost::math::constants::pi<double>();
172 else if (statet->values[i] < -boost::math::constants::pi<double>())
173 statet->values[i] += 2.0 * boost::math::constants::pi<double>();
174 }
175 }
176 }
177
178 double linkLength() const
179 {
180 return linkLength_;
181 }
182
183 const Environment *environment() const
184 {
185 return environment_;
186 }
187
188protected:
189 double linkLength_;
190 Environment *environment_;
191};
192
193class KinematicChainValidityChecker : public ompl::base::StateValidityChecker
194{
195public:
196 KinematicChainValidityChecker(const ompl::base::SpaceInformationPtr &si) : ompl::base::StateValidityChecker(si)
197 {
198 }
199
200 bool isValid(const ompl::base::State *state) const override
201 {
202 const KinematicChainSpace *space = si_->getStateSpace()->as<KinematicChainSpace>();
203 const auto *s = state->as<KinematicChainSpace::StateType>();
204
205 return isValidImpl(space, s);
206 }
207
208protected:
209 bool isValidImpl(const KinematicChainSpace *space, const KinematicChainSpace::StateType *s) const
210 {
211 unsigned int n = si_->getStateDimension();
212 Environment segments;
213 double linkLength = space->linkLength();
214 double theta = 0., x = 0., y = 0., xN, yN;
215
216 segments.reserve(n + 1);
217 for (unsigned int i = 0; i < n; ++i)
218 {
219 theta += s->values[i];
220 xN = x + cos(theta) * linkLength;
221 yN = y + sin(theta) * linkLength;
222 segments.emplace_back(x, y, xN, yN);
223 x = xN;
224 y = yN;
225 }
226 xN = x + cos(theta) * 0.001;
227 yN = y + sin(theta) * 0.001;
228 segments.emplace_back(x, y, xN, yN);
229 return selfIntersectionTest(segments) && environmentIntersectionTest(segments, *space->environment());
230 }
231
232 // return true iff env does *not* include a pair of intersecting segments
233 bool selfIntersectionTest(const Environment &env) const
234 {
235 for (unsigned int i = 0; i < env.size(); ++i)
236 for (unsigned int j = i + 1; j < env.size(); ++j)
237 if (intersectionTest(env[i], env[j]))
238 return false;
239 return true;
240 }
241 // return true iff no segment in env0 intersects any segment in env1
242 bool environmentIntersectionTest(const Environment &env0, const Environment &env1) const
243 {
244 for (const auto &i : env0)
245 for (const auto &j : env1)
246 if (intersectionTest(i, j))
247 return false;
248 return true;
249 }
250 // return true iff segment s0 intersects segment s1
251 bool intersectionTest(const Segment &s0, const Segment &s1) const
252 {
253 // adopted from:
254 // http://stackoverflow.com/questions/563198/how-do-you-detect-where-two-line-segments-intersect/1201356#1201356
255 double s10_x = s0.x1 - s0.x0;
256 double s10_y = s0.y1 - s0.y0;
257 double s32_x = s1.x1 - s1.x0;
258 double s32_y = s1.y1 - s1.y0;
259 double denom = s10_x * s32_y - s32_x * s10_y;
260 if (fabs(denom) < std::numeric_limits<double>::epsilon())
261 return false; // Collinear
262 bool denomPositive = denom > 0;
263
264 double s02_x = s0.x0 - s1.x0;
265 double s02_y = s0.y0 - s1.y0;
266 double s_numer = s10_x * s02_y - s10_y * s02_x;
267 if ((s_numer < std::numeric_limits<float>::epsilon()) == denomPositive)
268 return false; // No collision
269 double t_numer = s32_x * s02_y - s32_y * s02_x;
270 if ((t_numer < std::numeric_limits<float>::epsilon()) == denomPositive)
271 return false; // No collision
272 if (((s_numer - denom > -std::numeric_limits<float>::epsilon()) == denomPositive) ||
273 ((t_numer - denom > std::numeric_limits<float>::epsilon()) == denomPositive))
274 return false; // No collision
275 return true;
276 }
277};
278
279Environment createHornEnvironment(unsigned int d, double eps)
280{
281 std::ofstream envFile(boost::str(boost::format("environment_%i.dat") % d));
282 std::vector<Segment> env;
283 double w = 1. / (double)d, x = w, y = -eps, xN, yN, theta = 0.,
284 scale = w * (1. + boost::math::constants::pi<double>() * eps);
285
286 envFile << x << " " << y << std::endl;
287 for (unsigned int i = 0; i < d - 1; ++i)
288 {
289 theta += boost::math::constants::pi<double>() / (double)d;
290 xN = x + cos(theta) * scale;
291 yN = y + sin(theta) * scale;
292 env.emplace_back(x, y, xN, yN);
293 x = xN;
294 y = yN;
295 envFile << x << " " << y << std::endl;
296 }
297
298 theta = 0.;
299 x = w;
300 y = eps;
301 envFile << x << " " << y << std::endl;
302 scale = w * (1.0 - boost::math::constants::pi<double>() * eps);
303 for (unsigned int i = 0; i < d - 1; ++i)
304 {
305 theta += boost::math::constants::pi<double>() / d;
306 xN = x + cos(theta) * scale;
307 yN = y + sin(theta) * scale;
308 env.emplace_back(x, y, xN, yN);
309 x = xN;
310 y = yN;
311 envFile << x << " " << y << std::endl;
312 }
313 envFile.close();
314 return env;
315}
316
317Environment createEmptyEnvironment(unsigned int d)
318{
319 std::ofstream envFile(boost::str(boost::format("environment_%i.dat") % d));
320 std::vector<Segment> env;
321 envFile.close();
322 return env;
323}
324
325#endif
unsigned int getDimension() const override
Return the dimension of the projection defined by this evaluator.
void project(const ompl::base::State *state, Eigen::Ref< Eigen::VectorXd > projection) const override
Compute the projection as an array of double values.
bool equalStates(const ompl::base::State *state1, const ompl::base::State *state2) const override
Checks whether two states are equal.
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 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-...
void registerProjections() override
Register the projections for this state space. Usually, this is at least the default projection....
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....
bool isValid(const ompl::base::State *state) const override
Return true if the state state is valid. Usually, this means at least collision checking....
Abstract definition for a class computing projections to Rn. Implicit integer grids are imposed on th...
const StateSpace * space_
The state space this projection operates on.
A projection matrix – it allows multiplication of real vectors by a specified matrix....
The lower and upper bounds for an Rn space.
double * values
The value of the actual vector in Rn
A state space representing Rn. The distance function is the L2 norm.
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...
unsigned int getStateDimension() const
Return the dimension of the state space.
Representation of a space in which planning can be performed. Topology specific sampling,...
Definition StateSpace.h:71
virtual unsigned int getDimension() const =0
Get the dimension of the space (not the dimension of the surrounding ambient space)
void registerDefaultProjection(const ProjectionEvaluatorPtr &projection)
Register the default projection for this state space.
Abstract definition for a class checking the validity of states. The implementation of this class mus...
SpaceInformation * si_
The instance of space information this state validity checker operates on.
Definition of an abstract state.
Definition State.h:50
const T * as() const
Cast this instance to a desired type.
Definition State.h:66