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 using namespace boost::math::double_constants;
46 
47 static const double MAX_QUATERNION_NORM_ERROR = 1e-9;
48 
50 namespace 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 
88 void 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 
104 void ompl::base::SO3StateSampler::sampleUniformNear(State *state, const State *near, const double distance)
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 
119 void 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 
177 double ompl::base::SO3StateSpace::norm(const StateType *state) const
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 
218 void 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 
233 void ompl::base::SO3StateSpace::serialize(void *serialization, const State *state) const
234 {
235  memcpy(serialization, &state->as<StateType>()->x, sizeof(double) * 4);
236 }
237 
238 void 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 /*
246 Based on code from :
247 
248 Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/
249 */
250 namespace 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 
267 double 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 
279 bool 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 /*
285 Based on code from :
286 
287 Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/
288 */
289 void 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 
326 {
327  return new StateType();
328 }
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 
371 double *ompl::base::SO3StateSpace::getValueAddressAtIndex(State *state, const unsigned int index) const
372 {
373  return index < 4 ? &(state->as<StateType>()->x) + index : nullptr;
374 }
375 
376 void 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 
389 void ompl::base::SO3StateSpace::printSettings(std::ostream &out) const
390 {
391  out << "SO(3) state space '" << getName() << "' (represented using quaternions)" << std::endl;
392 }
void printState(const State *state, std::ostream &out) const override
Print a state to a stream.
void printSettings(std::ostream &out) const override
Print the settings for this state space 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 distance(const State *state1, const State *state2) const override
Computes distance between two states. This function satisfies the properties of a metric if isMetricS...
Representation of a space in which planning can be performed. Topology specific sampling,...
Definition: StateSpace.h:134
double getMeasure() const override
Get a measure of the space (this can be thought of as a generalization of volume)
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....
Definition of an abstract state.
Definition: State.h:113
void deserialize(State *state, const void *serialization) const override
Read the binary representation of a state from serialization and write it to state.
The definition of a state in SO(3) represented as a unit quaternion.
unsigned int getSerializationLength() const override
Get the number of chars in the serialization of a state in this space.
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...
const T * as() const
Cast this instance to a desired type.
Definition: State.h:162
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 registerProjections() override
Register the projections for this state space. Usually, this is at least the default projection....
void freeState(State *state) const override
Free the memory of the allocated state.
double getMaximumExtent() const override
Get the maximum value a call to distance() can return (or an upper bound). For unbounded state spaces...
State * allocState() const override
Allocate a state that can store a point in the described space.
double z
Z component of quaternion vector.
Abstract definition for a class computing projections to Rn. Implicit integer grids are imposed on th...
unsigned int getDimension() const override
Get the dimension of the space (not the dimension of the surrounding ambient space)
void serialize(void *serialization, const State *state) const override
Write the binary representation of state to serialization.
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 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...
bool equalStates(const State *state1, const State *state2) const override
Checks whether two states are equal.
void setAxisAngle(double ax, double ay, double az, double angle)
Set the quaternion from axis-angle representation. The angle is given in radians.
double x
X component of quaternion vector.
A shared pointer wrapper for ompl::base::StateSampler.
StateSamplerPtr allocDefaultStateSampler() const override
Allocate an instance of the default uniform state sampler for this space.
double norm(const StateType *state) const
Compute the norm of a state.
double w
scalar component of quaternion
double y
Y component of quaternion vector.
void sampleUniform(State *state) override
Sample a state.
void setIdentity()
Set the state to identity – no rotation.
Main namespace. Contains everything in this library.
void copyState(State *destination, const State *source) const override
Copy a state to another. The memory of source and destination should NOT overlap.