All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
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: 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 
44 static const double MAX_QUATERNION_NORM_ERROR = 1e-9;
45 
47 namespace ompl
48 {
49  namespace base
50  {
51  static inline void computeAxisAngle(SO3StateSpace::StateType &q, double ax, double ay, double az, double angle)
52  {
53  double norm = sqrt(ax * ax + ay * ay + az * az);
54  if (norm < MAX_QUATERNION_NORM_ERROR)
55  q.setIdentity();
56  else
57  {
58  double s = sin(angle / 2.0);
59  q.x = s * ax / norm;
60  q.y = s * ay / norm;
61  q.z = s * az / norm;
62  q.w = cos(angle / 2.0);
63  }
64  }
65 
66  /* Standard quaternion multiplication: q = q0 * q1 */
67  static inline void quaternionProduct(SO3StateSpace::StateType &q, const SO3StateSpace::StateType& q0, const SO3StateSpace::StateType& q1)
68  {
69  q.x = q0.w*q1.x + q0.x*q1.w + q0.y*q1.z - q0.z*q1.y;
70  q.y = q0.w*q1.y + q0.y*q1.w + q0.z*q1.x - q0.x*q1.z;
71  q.z = q0.w*q1.z + q0.z*q1.w + q0.x*q1.y - q0.y*q1.x;
72  q.w = q0.w*q1.w - q0.x*q1.x - q0.y*q1.y - q0.z*q1.z;
73  }
74  }
75 }
77 
78 void ompl::base::SO3StateSpace::StateType::setAxisAngle(double ax, double ay, double az, double angle)
79 {
80  computeAxisAngle(*this, ax, ay, az, angle);
81 }
82 
84 {
85  x = y = z = 0.0;
86  w = 1.0;
87 }
88 
90 {
91  rng_.quaternion(&state->as<SO3StateSpace::StateType>()->x);
92 }
93 
94 void ompl::base::SO3StateSampler::sampleUniformNear(State *state, const State *near, const double distance)
95 {
96  if (distance >= .25 * boost::math::constants::pi<double>())
97  {
98  sampleUniform(state);
99  return;
100  }
101  double d = rng_.uniform01();
103  *qs = static_cast<SO3StateSpace::StateType*>(state);
104  const SO3StateSpace::StateType *qnear = static_cast<const SO3StateSpace::StateType*>(near);
105  computeAxisAngle(q, rng_.gaussian01(), rng_.gaussian01(), rng_.gaussian01(), 2.*pow(d,1./3.)*distance);
106  quaternionProduct(*qs, *qnear, q);
107 }
108 
109 void ompl::base::SO3StateSampler::sampleGaussian(State *state, const State * mean, const double stdDev)
110 {
111  // CDF of N(0, 1.17) at -pi/4 is approx. .25, so there's .25 probability
112  // weight in each tail. Since the maximum distance in SO(3) is pi/2, we're
113  // essentially as likely to sample a state within distance [0, pi/4] as
114  // within distance [pi/4, pi/2]. With most weight in the tails (that wrap
115  // around in case of quaternions) we might as well sample uniformly.
116  if (stdDev > 1.17)
117  {
118  sampleUniform(state);
119  return;
120  }
121  double x = rng_.gaussian(0, stdDev), y = rng_.gaussian(0, stdDev), z = rng_.gaussian(0, stdDev),
122  theta = std::sqrt(x*x + y*y + z*z);
123  if (theta < std::numeric_limits<double>::epsilon())
124  space_->copyState(state, mean);
125  else
126  {
128  *qs = static_cast<SO3StateSpace::StateType*>(state);
129  const SO3StateSpace::StateType *qmu = static_cast<const SO3StateSpace::StateType*>(mean);
130  double s = sin(theta / 2) / theta;
131  q.w = cos(theta / 2);
132  q.x = s * x;
133  q.y = s * y;
134  q.z = s * z;
135  quaternionProduct(*qs, *qmu, q);
136  }
137 }
138 
140 {
141  return 3;
142 }
143 
145 {
146  return .5 * boost::math::constants::pi<double>();
147 }
148 
149 double ompl::base::SO3StateSpace::norm(const StateType *state) const
150 {
151  double nrmSqr = state->x * state->x + state->y * state->y + state->z * state->z + state->w * state->w;
152  return (fabs(nrmSqr - 1.0) > std::numeric_limits<double>::epsilon()) ? sqrt(nrmSqr) : 1.0;
153 }
154 
156 {
157  StateType *qstate = static_cast<StateType*>(state);
158  double nrm = norm(qstate);
159  if (fabs(nrm) < MAX_QUATERNION_NORM_ERROR)
160  qstate->setIdentity();
161  else if (fabs(nrm - 1.0) > MAX_QUATERNION_NORM_ERROR)
162  {
163  qstate->x /= nrm;
164  qstate->y /= nrm;
165  qstate->z /= nrm;
166  qstate->w /= nrm;
167  }
168 }
169 
171 {
172  return fabs(norm(static_cast<const StateType*>(state)) - 1.0) < MAX_QUATERNION_NORM_ERROR;
173 }
174 
175 void ompl::base::SO3StateSpace::copyState(State *destination, const State *source) const
176 {
177  const StateType *qsource = static_cast<const StateType*>(source);
178  StateType *qdestination = static_cast<StateType*>(destination);
179  qdestination->x = qsource->x;
180  qdestination->y = qsource->y;
181  qdestination->z = qsource->z;
182  qdestination->w = qsource->w;
183 }
184 
186 {
187  return sizeof(double) * 4;
188 }
189 
190 void ompl::base::SO3StateSpace::serialize(void *serialization, const State *state) const
191 {
192  memcpy(serialization, &state->as<StateType>()->x, sizeof(double) * 4);
193 }
194 
195 void ompl::base::SO3StateSpace::deserialize(State *state, const void *serialization) const
196 {
197  memcpy(&state->as<StateType>()->x, serialization, sizeof(double) * 4);
198 }
199 
201 
202 /*
203 Based on code from :
204 
205 Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/
206 */
207 namespace ompl
208 {
209  namespace base
210  {
211  static inline double arcLength(const State *state1, const State *state2)
212  {
213  const SO3StateSpace::StateType *qs1 = static_cast<const SO3StateSpace::StateType*>(state1);
214  const SO3StateSpace::StateType *qs2 = static_cast<const SO3StateSpace::StateType*>(state2);
215  double dq = fabs(qs1->x * qs2->x + qs1->y * qs2->y + qs1->z * qs2->z + qs1->w * qs2->w);
216  if (dq > 1.0 - MAX_QUATERNION_NORM_ERROR)
217  return 0.0;
218  else
219  return acos(dq);
220  }
221  }
222 }
224 
225 double ompl::base::SO3StateSpace::distance(const State *state1, const State *state2) const
226 {
227  return arcLength(state1, state2);
228 }
229 
230 bool ompl::base::SO3StateSpace::equalStates(const State *state1, const State *state2) const
231 {
232  return arcLength(state1, state2) < std::numeric_limits<double>::epsilon();
233 }
234 
235 /*
236 Based on code from :
237 
238 Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/
239 */
240 void ompl::base::SO3StateSpace::interpolate(const State *from, const State *to, const double t, State *state) const
241 {
242  assert(fabs(norm(static_cast<const StateType*>(from)) - 1.0) < MAX_QUATERNION_NORM_ERROR);
243  assert(fabs(norm(static_cast<const StateType*>(to)) - 1.0) < MAX_QUATERNION_NORM_ERROR);
244 
245  double theta = arcLength(from, to);
246  if (theta > std::numeric_limits<double>::epsilon())
247  {
248  double d = 1.0 / sin(theta);
249  double s0 = sin((1.0 - t) * theta);
250  double s1 = sin(t * theta);
251 
252  const StateType *qs1 = static_cast<const StateType*>(from);
253  const StateType *qs2 = static_cast<const StateType*>(to);
254  StateType *qr = static_cast<StateType*>(state);
255  double dq = qs1->x * qs2->x + qs1->y * qs2->y + qs1->z * qs2->z + qs1->w * qs2->w;
256  if (dq < 0) // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp
257  s1 = -s1;
258 
259  qr->x = (qs1->x * s0 + qs2->x * s1) * d;
260  qr->y = (qs1->y * s0 + qs2->y * s1) * d;
261  qr->z = (qs1->z * s0 + qs2->z * s1) * d;
262  qr->w = (qs1->w * s0 + qs2->w * s1) * d;
263  }
264  else
265  {
266  if (state != from)
267  copyState(state, from);
268  }
269 }
270 
272 {
273  return StateSamplerPtr(new SO3StateSampler(this));
274 }
275 
277 {
278  return new StateType();
279 }
280 
282 {
283  delete static_cast<StateType*>(state);
284 }
285 
287 {
288  class SO3DefaultProjection : public ProjectionEvaluator
289  {
290  public:
291 
292  SO3DefaultProjection(const StateSpace *space) : ProjectionEvaluator(space)
293  {
294  }
295 
296  virtual unsigned int getDimension(void) const
297  {
298  return 3;
299  }
300 
301  virtual void defaultCellSizes(void)
302  {
303  cellSizes_.resize(3);
304  cellSizes_[0] = boost::math::constants::pi<double>() / magic::PROJECTION_DIMENSION_SPLITS;
305  cellSizes_[1] = boost::math::constants::pi<double>() / magic::PROJECTION_DIMENSION_SPLITS;
306  cellSizes_[2] = boost::math::constants::pi<double>() / magic::PROJECTION_DIMENSION_SPLITS;
307  }
308 
309  virtual void project(const State *state, EuclideanProjection &projection) const
310  {
311  projection(0) = state->as<SO3StateSpace::StateType>()->x;
312  projection(1) = state->as<SO3StateSpace::StateType>()->y;
313  projection(2) = state->as<SO3StateSpace::StateType>()->z;
314  }
315  };
316 
317  registerDefaultProjection(ProjectionEvaluatorPtr(dynamic_cast<ProjectionEvaluator*>(new SO3DefaultProjection(this))));
318 }
319 
320 double* ompl::base::SO3StateSpace::getValueAddressAtIndex(State *state, const unsigned int index) const
321 {
322  return index < 4 ? &(state->as<StateType>()->x) + index : NULL;
323 }
324 
325 void ompl::base::SO3StateSpace::printState(const State *state, std::ostream &out) const
326 {
327  out << "SO3State [";
328  if (state)
329  {
330  const StateType *qstate = static_cast<const StateType*>(state);
331  out << qstate->x << " " << qstate->y << " " << qstate->z << " " << qstate->w;
332  }
333  else
334  out << "NULL";
335  out << ']' << std::endl;
336 }
337 
338 void ompl::base::SO3StateSpace::printSettings(std::ostream &out) const
339 {
340  out << "SO(3) state space '" << getName() << "' (represented using quaternions)" << std::endl;
341 }