Loading...
Searching...
No Matches
SO3StateSpace.cpp
1/*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2010, 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: Mark Moll, Ioan Sucan */
36
37#include "ompl/base/spaces/SO3StateSpace.h"
38#include <algorithm>
39#include <limits>
40#include <cmath>
41#include "ompl/tools/config/MagicConstants.h"
42#include <boost/math/constants/constants.hpp>
43#include <boost/assert.hpp>
44
45using namespace boost::math::double_constants;
46
47static const double MAX_QUATERNION_NORM_ERROR = 1e-9;
48
50namespace ompl
51{
52 namespace base
53 {
54 static inline void computeAxisAngle(SO3StateSpace::StateType &q, double ax, double ay, double az, double angle)
55 {
56 double norm = std::sqrt(ax * ax + ay * ay + az * az);
57 if (norm < MAX_QUATERNION_NORM_ERROR)
58 q.setIdentity();
59 else
60 {
61 double half_angle = angle / 2.0;
62 double s = sin(half_angle) / norm;
63 q.x = s * ax;
64 q.y = s * ay;
65 q.z = s * az;
66 q.w = cos(half_angle);
67 }
68 }
69
70 /* Standard quaternion multiplication: q = q0 * q1 */
71 static inline void quaternionProduct(SO3StateSpace::StateType &q, const SO3StateSpace::StateType &q0,
72 const SO3StateSpace::StateType &q1)
73 {
74 q.x = q0.w * q1.x + q0.x * q1.w + q0.y * q1.z - q0.z * q1.y;
75 q.y = q0.w * q1.y + q0.y * q1.w + q0.z * q1.x - q0.x * q1.z;
76 q.z = q0.w * q1.z + q0.z * q1.w + q0.x * q1.y - q0.y * q1.x;
77 q.w = q0.w * q1.w - q0.x * q1.x - q0.y * q1.y - q0.z * q1.z;
78 }
79
80 inline double quaternionNormSquared(const SO3StateSpace::StateType &q)
81 {
82 return q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w;
83 }
84 } // namespace base
85} // namespace ompl
87
88void ompl::base::SO3StateSpace::StateType::setAxisAngle(double ax, double ay, double az, double angle)
89{
90 computeAxisAngle(*this, ax, ay, az, angle);
91}
92
94{
95 x = y = z = 0.0;
96 w = 1.0;
97}
98
100{
101 rng_.quaternion(&state->as<SO3StateSpace::StateType>()->x);
102}
103
105{
106 if (distance >= .25 * pi)
107 {
108 sampleUniform(state);
109 return;
110 }
111 double d = rng_.uniform01();
112 SO3StateSpace::StateType q, *qs = static_cast<SO3StateSpace::StateType *>(state);
113 const auto *qnear = static_cast<const SO3StateSpace::StateType *>(near);
114 computeAxisAngle(q, rng_.gaussian01(), rng_.gaussian01(), rng_.gaussian01(),
115 2. * pow(d, third) * distance);
116 quaternionProduct(*qs, *qnear, q);
117}
118
119void ompl::base::SO3StateSampler::sampleGaussian(State *state, const State *mean, const double stdDev)
120{
121 // The standard deviation of the individual components of the tangent
122 // perturbation needs to be scaled so that the expected quaternion distance
123 // between the sampled state and the mean state is stdDev. The factor 2 is
124 // due to the way we define distance (see also Matt Mason's lecture notes
125 // on quaternions at
126 // http://www.cs.cmu.edu/afs/cs/academic/class/16741-s07/www/lectures/Lecture8.pdf).
127 // The 1/sqrt(3) factor is necessary because the distribution in the tangent
128 // space is a 3-dimensional Gaussian, so that the *length* of a tangent
129 // vector needs to be scaled by 1/sqrt(3).
130 double rotDev = (2. * stdDev) / root_three;
131
132 // CDF of N(0, 1.17) at -pi/4 is approx. .25, so there's .25 probability
133 // weight in each tail. Since the maximum distance in SO(3) is pi/2, we're
134 // essentially as likely to sample a state within distance [0, pi/4] as
135 // within distance [pi/4, pi/2]. With most weight in the tails (that wrap
136 // around in case of quaternions) we might as well sample uniformly.
137 if (rotDev > 1.17)
138 {
139 sampleUniform(state);
140 return;
141 }
142
143 double x = rng_.gaussian(0, rotDev), y = rng_.gaussian(0, rotDev), z = rng_.gaussian(0, rotDev),
144 theta = std::sqrt(x * x + y * y + z * z);
145 if (theta < std::numeric_limits<double>::epsilon())
146 space_->copyState(state, mean);
147 else
148 {
149 SO3StateSpace::StateType q, *qs = static_cast<SO3StateSpace::StateType *>(state);
150 const auto *qmu = static_cast<const SO3StateSpace::StateType *>(mean);
151 double half_theta = theta / 2.0;
152 double s = sin(half_theta) / theta;
153 q.w = cos(half_theta);
154 q.x = s * x;
155 q.y = s * y;
156 q.z = s * z;
157 quaternionProduct(*qs, *qmu, q);
158 }
159}
160
162{
163 return 3;
164}
165
167{
168 return .5 * pi;
169}
170
172{
173 // half of the surface area of a unit 3-sphere
174 return pi * pi;
175}
176
178{
179 double nrmSqr = quaternionNormSquared(*state);
180 return (fabs(nrmSqr - 1.0) > std::numeric_limits<double>::epsilon()) ? std::sqrt(nrmSqr) : 1.0;
181}
182
184{
185 // see http://stackoverflow.com/questions/11667783/quaternion-and-normalization/12934750#12934750
186 auto *qstate = static_cast<StateType *>(state);
187 double nrmsq = quaternionNormSquared(*qstate);
188 double error = std::abs(1.0 - nrmsq);
189 const double epsilon = 2.107342e-08;
190 if (error < epsilon)
191 {
192 double scale = 2.0 / (1.0 + nrmsq);
193 qstate->x *= scale;
194 qstate->y *= scale;
195 qstate->z *= scale;
196 qstate->w *= scale;
197 }
198 else
199 {
200 if (nrmsq < 1e-6)
201 qstate->setIdentity();
202 else
203 {
204 double scale = 1.0 / std::sqrt(nrmsq);
205 qstate->x *= scale;
206 qstate->y *= scale;
207 qstate->z *= scale;
208 qstate->w *= scale;
209 }
210 }
211}
212
214{
215 return fabs(norm(static_cast<const StateType *>(state)) - 1.0) < MAX_QUATERNION_NORM_ERROR;
216}
217
218void ompl::base::SO3StateSpace::copyState(State *destination, const State *source) const
219{
220 const auto *qsource = static_cast<const StateType *>(source);
221 auto *qdestination = static_cast<StateType *>(destination);
222 qdestination->x = qsource->x;
223 qdestination->y = qsource->y;
224 qdestination->z = qsource->z;
225 qdestination->w = qsource->w;
226}
227
229{
230 return sizeof(double) * 4;
231}
232
233void ompl::base::SO3StateSpace::serialize(void *serialization, const State *state) const
234{
235 memcpy(serialization, &state->as<StateType>()->x, sizeof(double) * 4);
236}
237
238void ompl::base::SO3StateSpace::deserialize(State *state, const void *serialization) const
239{
240 memcpy(&state->as<StateType>()->x, serialization, sizeof(double) * 4);
241}
242
244
245/*
246Based on code from :
247
248Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/
249*/
250namespace ompl
251{
252 namespace base
253 {
254 static inline double arcLength(const State *state1, const State *state2)
255 {
256 const auto *qs1 = static_cast<const SO3StateSpace::StateType *>(state1);
257 const auto *qs2 = static_cast<const SO3StateSpace::StateType *>(state2);
258 double dq = fabs(qs1->x * qs2->x + qs1->y * qs2->y + qs1->z * qs2->z + qs1->w * qs2->w);
259 if (dq > 1.0 - MAX_QUATERNION_NORM_ERROR)
260 return 0.0;
261 return acos(dq);
262 }
263 } // namespace base
264} // namespace ompl
266
267double ompl::base::SO3StateSpace::distance(const State *state1, const State *state2) const
268{
269 BOOST_ASSERT_MSG(satisfiesBounds(state1) && satisfiesBounds(state2), "The states passed to SO3StateSpace::distance "
270 "are not within bounds. Call "
271 "SO3StateSpace::enforceBounds() in, e.g., "
272 "ompl::control::ODESolver::"
273 "PostPropagationEvent, "
274 "ompl::control::StatePropagator, or "
275 "ompl::base::StateValidityChecker");
276 return arcLength(state1, state2);
277}
278
279bool ompl::base::SO3StateSpace::equalStates(const State *state1, const State *state2) const
280{
281 return arcLength(state1, state2) < std::numeric_limits<double>::epsilon();
282}
283
284/*
285Based on code from :
286
287Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/
288*/
289void ompl::base::SO3StateSpace::interpolate(const State *from, const State *to, const double t, State *state) const
290{
291 assert(fabs(norm(static_cast<const StateType *>(from)) - 1.0) < MAX_QUATERNION_NORM_ERROR);
292 assert(fabs(norm(static_cast<const StateType *>(to)) - 1.0) < MAX_QUATERNION_NORM_ERROR);
293
294 double theta = arcLength(from, to);
295 if (theta > std::numeric_limits<double>::epsilon())
296 {
297 double d = 1.0 / sin(theta);
298 double s0 = sin((1.0 - t) * theta);
299 double s1 = sin(t * theta);
300
301 const auto *qs1 = static_cast<const StateType *>(from);
302 const auto *qs2 = static_cast<const StateType *>(to);
303 auto *qr = static_cast<StateType *>(state);
304 double dq = qs1->x * qs2->x + qs1->y * qs2->y + qs1->z * qs2->z + qs1->w * qs2->w;
305 if (dq < 0) // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp
306 s1 = -s1;
307
308 qr->x = (qs1->x * s0 + qs2->x * s1) * d;
309 qr->y = (qs1->y * s0 + qs2->y * s1) * d;
310 qr->z = (qs1->z * s0 + qs2->z * s1) * d;
311 qr->w = (qs1->w * s0 + qs2->w * s1) * d;
312 }
313 else
314 {
315 if (state != from)
316 copyState(state, from);
317 }
318}
319
321{
322 return std::make_shared<SO3StateSampler>(this);
323}
324
329
331{
332 delete static_cast<StateType *>(state);
333}
334
336{
337 class SO3DefaultProjection : public ProjectionEvaluator
338 {
339 public:
340 SO3DefaultProjection(const StateSpace *space) : ProjectionEvaluator(space)
341 {
342 }
343
344 unsigned int getDimension() const override
345 {
346 return 3;
347 }
348
349 void defaultCellSizes() override
350 {
351 cellSizes_.resize(3);
352 cellSizes_[0] = pi / magic::PROJECTION_DIMENSION_SPLITS;
353 cellSizes_[1] = pi / magic::PROJECTION_DIMENSION_SPLITS;
354 cellSizes_[2] = pi / magic::PROJECTION_DIMENSION_SPLITS;
355 bounds_.resize(3);
356 bounds_.setLow(-1.0);
357 bounds_.setHigh(1.0);
358 }
359
360 void project(const State *state, Eigen::Ref<Eigen::VectorXd> projection) const override
361 {
362 projection(0) = state->as<SO3StateSpace::StateType>()->x;
363 projection(1) = state->as<SO3StateSpace::StateType>()->y;
364 projection(2) = state->as<SO3StateSpace::StateType>()->z;
365 }
366 };
367
368 registerDefaultProjection(std::make_shared<SO3DefaultProjection>(this));
369}
370
371double *ompl::base::SO3StateSpace::getValueAddressAtIndex(State *state, const unsigned int index) const
372{
373 return index < 4 ? &(state->as<StateType>()->x) + index : nullptr;
374}
375
376void ompl::base::SO3StateSpace::printState(const State *state, std::ostream &out) const
377{
378 out << "SO3State [";
379 if (state != nullptr)
380 {
381 const auto *qstate = static_cast<const StateType *>(state);
382 out << qstate->x << " " << qstate->y << " " << qstate->z << " " << qstate->w;
383 }
384 else
385 out << "nullptr";
386 out << ']' << std::endl;
387}
388
389void ompl::base::SO3StateSpace::printSettings(std::ostream &out) const
390{
391 out << "SO(3) state space '" << getName() << "' (represented using quaternions)" << std::endl;
392}
Abstract definition for a class computing projections to Rn. Implicit integer grids are imposed on th...
void sampleGaussian(State *state, const State *mean, double stdDev) override
Sample a state such that the expected distance between mean and state is stdDev.
void sampleUniformNear(State *state, const State *near, double distance) override
To sample unit quaternions uniformly within some given distance, we sample a 3-vector from the R^3 ta...
void sampleUniform(State *state) override
Sample a state.
The definition of a state in SO(3) represented as a unit quaternion.
double x
X component of quaternion vector.
double w
scalar component of quaternion
double z
Z component of quaternion vector.
void setIdentity()
Set the state to identity – no rotation.
double y
Y component of quaternion vector.
void setAxisAngle(double ax, double ay, double az, double angle)
Set the quaternion from axis-angle representation. The angle is given in radians.
void copyState(State *destination, const State *source) const override
Copy a state to another. The memory of source and destination should NOT overlap.
unsigned int getSerializationLength() const override
Get the number of chars in the serialization of a state in this space.
State * allocState() const override
Allocate a state that can store a point in the described space.
double norm(const StateType *state) const
Compute the norm of a state.
void printSettings(std::ostream &out) const override
Print the settings for this state space to a stream.
StateSamplerPtr allocDefaultStateSampler() const override
Allocate an instance of the default uniform state sampler for this space.
void registerProjections() override
Register the projections for this state space. Usually, this is at least the default projection....
double distance(const State *state1, const State *state2) const override
Computes distance between two states. This function satisfies the properties of a metric if isMetricS...
void serialize(void *serialization, const State *state) const override
Write the binary representation of state to serialization.
unsigned int getDimension() const override
Get the dimension of the space (not the dimension of the surrounding ambient space)
bool equalStates(const State *state1, const State *state2) const override
Checks whether two states are equal.
double * getValueAddressAtIndex(State *state, unsigned int index) const override
Many states contain a number of double values. This function provides a means to get the memory addre...
double getMaximumExtent() const override
Get the maximum value a call to distance() can return (or an upper bound). For unbounded state spaces...
void printState(const State *state, std::ostream &out) const override
Print a state to a stream.
void enforceBounds(State *state) const override
Bring the state within the bounds of the state space. For unbounded spaces this function can be a no-...
double getMeasure() const override
Get a measure of the space (this can be thought of as a generalization of volume)
void deserialize(State *state, const void *serialization) const override
Read the binary representation of a state from serialization and write it to state.
void freeState(State *state) const override
Free the memory of the allocated state.
void interpolate(const State *from, const State *to, double t, 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 satisfiesBounds(const State *state) const override
Check if a state is inside the bounding box. For unbounded spaces this function can always return tru...
A shared pointer wrapper for ompl::base::StateSampler.
Representation of a space in which planning can be performed. Topology specific sampling,...
Definition StateSpace.h:71
void registerDefaultProjection(const ProjectionEvaluatorPtr &projection)
Register the default projection for this state space.
const std::string & getName() const
Get the name of the state space.
Definition of an abstract state.
Definition State.h:50
const T * as() const
Cast this instance to a desired type.
Definition State.h:66
Main namespace. Contains everything in this library.