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