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 auto *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 auto *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  auto *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 auto *qsource = static_cast<const StateType *>(source);
219  auto *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 auto *qs1 = static_cast<const SO3StateSpace::StateType *>(state1);
255  const auto *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  return acos(dq);
260  }
261  }
262 }
264 
265 double ompl::base::SO3StateSpace::distance(const State *state1, const State *state2) const
266 {
267  BOOST_ASSERT_MSG(satisfiesBounds(state1) && satisfiesBounds(state2),
268  "The states passed to SO3StateSpace::distance are not within bounds. Call "
269  "SO3StateSpace::enforceBounds() in, e.g., ompl::control::ODESolver::PostPropagationEvent, "
270  "ompl::control::StatePropagator, or ompl::base::StateValidityChecker");
271  return arcLength(state1, state2);
272 }
273 
274 bool ompl::base::SO3StateSpace::equalStates(const State *state1, const State *state2) const
275 {
276  return arcLength(state1, state2) < std::numeric_limits<double>::epsilon();
277 }
278 
279 /*
280 Based on code from :
281 
282 Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/
283 */
284 void ompl::base::SO3StateSpace::interpolate(const State *from, const State *to, const double t, State *state) const
285 {
286  assert(fabs(norm(static_cast<const StateType *>(from)) - 1.0) < MAX_QUATERNION_NORM_ERROR);
287  assert(fabs(norm(static_cast<const StateType *>(to)) - 1.0) < MAX_QUATERNION_NORM_ERROR);
288 
289  double theta = arcLength(from, to);
290  if (theta > std::numeric_limits<double>::epsilon())
291  {
292  double d = 1.0 / sin(theta);
293  double s0 = sin((1.0 - t) * theta);
294  double s1 = sin(t * theta);
295 
296  const auto *qs1 = static_cast<const StateType *>(from);
297  const auto *qs2 = static_cast<const StateType *>(to);
298  auto *qr = static_cast<StateType *>(state);
299  double dq = qs1->x * qs2->x + qs1->y * qs2->y + qs1->z * qs2->z + qs1->w * qs2->w;
300  if (dq < 0) // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp
301  s1 = -s1;
302 
303  qr->x = (qs1->x * s0 + qs2->x * s1) * d;
304  qr->y = (qs1->y * s0 + qs2->y * s1) * d;
305  qr->z = (qs1->z * s0 + qs2->z * s1) * d;
306  qr->w = (qs1->w * s0 + qs2->w * s1) * d;
307  }
308  else
309  {
310  if (state != from)
311  copyState(state, from);
312  }
313 }
314 
316 {
317  return std::make_shared<SO3StateSampler>(this);
318 }
319 
321 {
322  return new StateType();
323 }
324 
326 {
327  delete static_cast<StateType *>(state);
328 }
329 
331 {
332  class SO3DefaultProjection : public ProjectionEvaluator
333  {
334  public:
335  SO3DefaultProjection(const StateSpace *space) : ProjectionEvaluator(space)
336  {
337  }
338 
339  unsigned int getDimension() const override
340  {
341  return 3;
342  }
343 
344  void defaultCellSizes() override
345  {
346  cellSizes_.resize(3);
347  cellSizes_[0] = boost::math::constants::pi<double>() / magic::PROJECTION_DIMENSION_SPLITS;
348  cellSizes_[1] = boost::math::constants::pi<double>() / magic::PROJECTION_DIMENSION_SPLITS;
349  cellSizes_[2] = boost::math::constants::pi<double>() / magic::PROJECTION_DIMENSION_SPLITS;
350  bounds_.resize(3);
351  bounds_.setLow(-1.0);
352  bounds_.setHigh(1.0);
353  }
354 
355  void project(const State *state, EuclideanProjection &projection) const override
356  {
357  projection(0) = state->as<SO3StateSpace::StateType>()->x;
358  projection(1) = state->as<SO3StateSpace::StateType>()->y;
359  projection(2) = state->as<SO3StateSpace::StateType>()->z;
360  }
361  };
362 
363  registerDefaultProjection(std::make_shared<SO3DefaultProjection>(this));
364 }
365 
366 double *ompl::base::SO3StateSpace::getValueAddressAtIndex(State *state, const unsigned int index) const
367 {
368  return index < 4 ? &(state->as<StateType>()->x) + index : nullptr;
369 }
370 
371 void ompl::base::SO3StateSpace::printState(const State *state, std::ostream &out) const
372 {
373  out << "SO3State [";
374  if (state != nullptr)
375  {
376  const auto *qstate = static_cast<const StateType *>(state);
377  out << qstate->x << " " << qstate->y << " " << qstate->z << " " << qstate->w;
378  }
379  else
380  out << "nullptr";
381  out << ']' << std::endl;
382 }
383 
384 void ompl::base::SO3StateSpace::printSettings(std::ostream &out) const
385 {
386  out << "SO(3) state space '" << getName() << "' (represented using quaternions)" << std::endl;
387 }
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 serialize(void *serialization, const State *state) const override
Write the binary representation of state to serialization.
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. ...
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...
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
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
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...
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...
void sampleGaussian(State *state, const State *mean, double stdDev) override
Sample a state such that the expected distance between mean and state is stdDev.
State * allocState() const override
Allocate a state that can store a point in the described space.
unsigned int getSerializationLength() const override
Get the number of chars in the serialization of a state in this space.
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...
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...