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