ompl/base/spaces/src/SO3StateSpace.cpp
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2010, Rice University 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of the Rice University nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 *********************************************************************/ 00034 00035 /* Author: Mark Moll, Ioan Sucan */ 00036 00037 #include "ompl/base/spaces/SO3StateSpace.h" 00038 #include <algorithm> 00039 #include <limits> 00040 #include <cmath> 00041 #include "ompl/tools/config/MagicConstants.h" 00042 #include <boost/math/constants/constants.hpp> 00043 #include <boost/version.hpp> 00044 #include <boost/assert.hpp> 00045 00046 #ifndef BOOST_ASSERT_MSG 00047 #define BOOST_ASSERT_MSG(expr, msg) assert(expr) 00048 #endif 00049 00050 #if BOOST_VERSION < 105000 00051 namespace boost{ namespace math{ namespace constants{ 00052 BOOST_DEFINE_MATH_CONSTANT(root_three, 1.732050807568877293527446341505872366, 1.73205080756887729352744634150587236694280525381038062805580697945193301690880003708114618675724857567562614142, 0) 00053 }}} 00054 #endif 00055 00056 static const double MAX_QUATERNION_NORM_ERROR = 1e-9; 00057 00059 namespace ompl 00060 { 00061 namespace base 00062 { 00063 static inline void computeAxisAngle(SO3StateSpace::StateType &q, double ax, double ay, double az, double angle) 00064 { 00065 double norm = std::sqrt(ax * ax + ay * ay + az * az); 00066 if (norm < MAX_QUATERNION_NORM_ERROR) 00067 q.setIdentity(); 00068 else 00069 { 00070 double half_angle = angle / 2.0; 00071 double s = sin(half_angle) / norm; 00072 q.x = s * ax; 00073 q.y = s * ay; 00074 q.z = s * az; 00075 q.w = cos(half_angle); 00076 } 00077 } 00078 00079 /* Standard quaternion multiplication: q = q0 * q1 */ 00080 static inline void quaternionProduct(SO3StateSpace::StateType &q, const SO3StateSpace::StateType &q0, const SO3StateSpace::StateType &q1) 00081 { 00082 q.x = q0.w * q1.x + q0.x * q1.w + q0.y * q1.z - q0.z * q1.y; 00083 q.y = q0.w * q1.y + q0.y * q1.w + q0.z * q1.x - q0.x * q1.z; 00084 q.z = q0.w * q1.z + q0.z * q1.w + q0.x * q1.y - q0.y * q1.x; 00085 q.w = q0.w * q1.w - q0.x * q1.x - q0.y * q1.y - q0.z * q1.z; 00086 } 00087 00088 inline double quaternionNormSquared(const SO3StateSpace::StateType &q) 00089 { 00090 return q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w; 00091 } 00092 } 00093 } 00095 00096 void ompl::base::SO3StateSpace::StateType::setAxisAngle(double ax, double ay, double az, double angle) 00097 { 00098 computeAxisAngle(*this, ax, ay, az, angle); 00099 } 00100 00101 void ompl::base::SO3StateSpace::StateType::setIdentity() 00102 { 00103 x = y = z = 0.0; 00104 w = 1.0; 00105 } 00106 00107 void ompl::base::SO3StateSampler::sampleUniform(State *state) 00108 { 00109 rng_.quaternion(&state->as<SO3StateSpace::StateType>()->x); 00110 } 00111 00112 void ompl::base::SO3StateSampler::sampleUniformNear(State *state, const State *near, const double distance) 00113 { 00114 if (distance >= .25 * boost::math::constants::pi<double>()) 00115 { 00116 sampleUniform(state); 00117 return; 00118 } 00119 double d = rng_.uniform01(); 00120 SO3StateSpace::StateType q, 00121 *qs = static_cast<SO3StateSpace::StateType*>(state); 00122 const SO3StateSpace::StateType *qnear = static_cast<const SO3StateSpace::StateType*>(near); 00123 computeAxisAngle(q, rng_.gaussian01(), rng_.gaussian01(), rng_.gaussian01(), 00124 2. * pow(d, boost::math::constants::third<double>()) * distance); 00125 quaternionProduct(*qs, *qnear, q); 00126 } 00127 00128 void ompl::base::SO3StateSampler::sampleGaussian(State *state, const State * mean, const double stdDev) 00129 { 00130 // The standard deviation of the individual components of the tangent 00131 // perturbation needs to be scaled so that the expected quaternion distance 00132 // between the sampled state and the mean state is stdDev. The factor 2 is 00133 // due to the way we define distance (see also Matt Mason's lecture notes 00134 // on quaternions at 00135 // http://www.cs.cmu.edu/afs/cs/academic/class/16741-s07/www/lecture7.pdf). 00136 // The 1/sqrt(3) factor is necessary because the distribution in the tangent 00137 // space is a 3-dimensional Gaussian, so that the *length* of a tangent 00138 // vector needs to be scaled by 1/sqrt(3). 00139 double rotDev = (2. * stdDev) / boost::math::constants::root_three<double>(); 00140 00141 // CDF of N(0, 1.17) at -pi/4 is approx. .25, so there's .25 probability 00142 // weight in each tail. Since the maximum distance in SO(3) is pi/2, we're 00143 // essentially as likely to sample a state within distance [0, pi/4] as 00144 // within distance [pi/4, pi/2]. With most weight in the tails (that wrap 00145 // around in case of quaternions) we might as well sample uniformly. 00146 if (rotDev > 1.17) 00147 { 00148 sampleUniform(state); 00149 return; 00150 } 00151 00152 00153 double x = rng_.gaussian(0, rotDev), y = rng_.gaussian(0, rotDev), z = rng_.gaussian(0, rotDev), 00154 theta = std::sqrt(x*x + y*y + z*z); 00155 if (theta < std::numeric_limits<double>::epsilon()) 00156 space_->copyState(state, mean); 00157 else 00158 { 00159 SO3StateSpace::StateType q, 00160 *qs = static_cast<SO3StateSpace::StateType*>(state); 00161 const SO3StateSpace::StateType *qmu = static_cast<const SO3StateSpace::StateType*>(mean); 00162 double half_theta = theta / 2.0; 00163 double s = sin(half_theta) / theta; 00164 q.w = cos(half_theta); 00165 q.x = s * x; 00166 q.y = s * y; 00167 q.z = s * z; 00168 quaternionProduct(*qs, *qmu, q); 00169 } 00170 } 00171 00172 unsigned int ompl::base::SO3StateSpace::getDimension() const 00173 { 00174 return 3; 00175 } 00176 00177 double ompl::base::SO3StateSpace::getMaximumExtent() const 00178 { 00179 return .5 * boost::math::constants::pi<double>(); 00180 } 00181 00182 double ompl::base::SO3StateSpace::getMeasure() const 00183 { 00184 // half of the surface area of a unit 4-sphere 00185 return boost::math::constants::pi<double>() * boost::math::constants::pi<double>(); 00186 } 00187 00188 double ompl::base::SO3StateSpace::norm(const StateType *state) const 00189 { 00190 double nrmSqr = quaternionNormSquared(*state); 00191 return (fabs(nrmSqr - 1.0) > std::numeric_limits<double>::epsilon()) ? std::sqrt(nrmSqr) : 1.0; 00192 } 00193 00194 void ompl::base::SO3StateSpace::enforceBounds(State *state) const 00195 { 00196 // see http://stackoverflow.com/questions/11667783/quaternion-and-normalization/12934750#12934750 00197 StateType *qstate = static_cast<StateType*>(state); 00198 double nrmsq = quaternionNormSquared(*qstate); 00199 double error = std::abs(1.0 - nrmsq); 00200 const double epsilon = 2.107342e-08; 00201 if (error < epsilon) 00202 { 00203 double scale = 2.0 / (1.0 + nrmsq); 00204 qstate->x *= scale; 00205 qstate->y *= scale; 00206 qstate->z *= scale; 00207 qstate->w *= scale; 00208 } 00209 else 00210 { 00211 if (nrmsq < 1e-6) 00212 qstate->setIdentity(); 00213 else 00214 { 00215 double scale = 1.0 / std::sqrt(nrmsq); 00216 qstate->x *= scale; 00217 qstate->y *= scale; 00218 qstate->z *= scale; 00219 qstate->w *= scale; 00220 } 00221 } 00222 } 00223 00224 bool ompl::base::SO3StateSpace::satisfiesBounds(const State *state) const 00225 { 00226 return fabs(norm(static_cast<const StateType*>(state)) - 1.0) < MAX_QUATERNION_NORM_ERROR; 00227 } 00228 00229 void ompl::base::SO3StateSpace::copyState(State *destination, const State *source) const 00230 { 00231 const StateType *qsource = static_cast<const StateType*>(source); 00232 StateType *qdestination = static_cast<StateType*>(destination); 00233 qdestination->x = qsource->x; 00234 qdestination->y = qsource->y; 00235 qdestination->z = qsource->z; 00236 qdestination->w = qsource->w; 00237 } 00238 00239 unsigned int ompl::base::SO3StateSpace::getSerializationLength() const 00240 { 00241 return sizeof(double) * 4; 00242 } 00243 00244 void ompl::base::SO3StateSpace::serialize(void *serialization, const State *state) const 00245 { 00246 memcpy(serialization, &state->as<StateType>()->x, sizeof(double) * 4); 00247 } 00248 00249 void ompl::base::SO3StateSpace::deserialize(State *state, const void *serialization) const 00250 { 00251 memcpy(&state->as<StateType>()->x, serialization, sizeof(double) * 4); 00252 } 00253 00255 00256 /* 00257 Based on code from : 00258 00259 Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ 00260 */ 00261 namespace ompl 00262 { 00263 namespace base 00264 { 00265 static inline double arcLength(const State *state1, const State *state2) 00266 { 00267 const SO3StateSpace::StateType *qs1 = static_cast<const SO3StateSpace::StateType*>(state1); 00268 const SO3StateSpace::StateType *qs2 = static_cast<const SO3StateSpace::StateType*>(state2); 00269 double dq = fabs(qs1->x * qs2->x + qs1->y * qs2->y + qs1->z * qs2->z + qs1->w * qs2->w); 00270 if (dq > 1.0 - MAX_QUATERNION_NORM_ERROR) 00271 return 0.0; 00272 else 00273 return acos(dq); 00274 } 00275 } 00276 } 00278 00279 double ompl::base::SO3StateSpace::distance(const State *state1, const State *state2) const 00280 { 00281 BOOST_ASSERT_MSG(satisfiesBounds(state1) && satisfiesBounds(state2), 00282 "The states passed to SO3StateSpace::distance are not within bounds. Call " 00283 "SO3StateSpace::enforceBounds() in, e.g., ompl::control::ODESolver::PostPropagationEvent, " 00284 "ompl::control::StatePropagator, or ompl::base::StateValidityChecker"); 00285 return arcLength(state1, state2); 00286 } 00287 00288 bool ompl::base::SO3StateSpace::equalStates(const State *state1, const State *state2) const 00289 { 00290 return arcLength(state1, state2) < std::numeric_limits<double>::epsilon(); 00291 } 00292 00293 /* 00294 Based on code from : 00295 00296 Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ 00297 */ 00298 void ompl::base::SO3StateSpace::interpolate(const State *from, const State *to, const double t, State *state) const 00299 { 00300 assert(fabs(norm(static_cast<const StateType*>(from)) - 1.0) < MAX_QUATERNION_NORM_ERROR); 00301 assert(fabs(norm(static_cast<const StateType*>(to)) - 1.0) < MAX_QUATERNION_NORM_ERROR); 00302 00303 double theta = arcLength(from, to); 00304 if (theta > std::numeric_limits<double>::epsilon()) 00305 { 00306 double d = 1.0 / sin(theta); 00307 double s0 = sin((1.0 - t) * theta); 00308 double s1 = sin(t * theta); 00309 00310 const StateType *qs1 = static_cast<const StateType*>(from); 00311 const StateType *qs2 = static_cast<const StateType*>(to); 00312 StateType *qr = static_cast<StateType*>(state); 00313 double dq = qs1->x * qs2->x + qs1->y * qs2->y + qs1->z * qs2->z + qs1->w * qs2->w; 00314 if (dq < 0) // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp 00315 s1 = -s1; 00316 00317 qr->x = (qs1->x * s0 + qs2->x * s1) * d; 00318 qr->y = (qs1->y * s0 + qs2->y * s1) * d; 00319 qr->z = (qs1->z * s0 + qs2->z * s1) * d; 00320 qr->w = (qs1->w * s0 + qs2->w * s1) * d; 00321 } 00322 else 00323 { 00324 if (state != from) 00325 copyState(state, from); 00326 } 00327 } 00328 00329 ompl::base::StateSamplerPtr ompl::base::SO3StateSpace::allocDefaultStateSampler() const 00330 { 00331 return StateSamplerPtr(new SO3StateSampler(this)); 00332 } 00333 00334 ompl::base::State* ompl::base::SO3StateSpace::allocState() const 00335 { 00336 return new StateType(); 00337 } 00338 00339 void ompl::base::SO3StateSpace::freeState(State *state) const 00340 { 00341 delete static_cast<StateType*>(state); 00342 } 00343 00344 void ompl::base::SO3StateSpace::registerProjections() 00345 { 00346 class SO3DefaultProjection : public ProjectionEvaluator 00347 { 00348 public: 00349 00350 SO3DefaultProjection(const StateSpace *space) : ProjectionEvaluator(space) 00351 { 00352 } 00353 00354 virtual unsigned int getDimension() const 00355 { 00356 return 3; 00357 } 00358 00359 virtual void defaultCellSizes() 00360 { 00361 cellSizes_.resize(3); 00362 cellSizes_[0] = boost::math::constants::pi<double>() / magic::PROJECTION_DIMENSION_SPLITS; 00363 cellSizes_[1] = boost::math::constants::pi<double>() / magic::PROJECTION_DIMENSION_SPLITS; 00364 cellSizes_[2] = boost::math::constants::pi<double>() / magic::PROJECTION_DIMENSION_SPLITS; 00365 bounds_.resize(3); 00366 bounds_.setLow(-1.0); 00367 bounds_.setHigh(1.0); 00368 } 00369 00370 virtual void project(const State *state, EuclideanProjection &projection) const 00371 { 00372 projection(0) = state->as<SO3StateSpace::StateType>()->x; 00373 projection(1) = state->as<SO3StateSpace::StateType>()->y; 00374 projection(2) = state->as<SO3StateSpace::StateType>()->z; 00375 } 00376 }; 00377 00378 registerDefaultProjection(ProjectionEvaluatorPtr(dynamic_cast<ProjectionEvaluator*>(new SO3DefaultProjection(this)))); 00379 } 00380 00381 double* ompl::base::SO3StateSpace::getValueAddressAtIndex(State *state, const unsigned int index) const 00382 { 00383 return index < 4 ? &(state->as<StateType>()->x) + index : NULL; 00384 } 00385 00386 void ompl::base::SO3StateSpace::printState(const State *state, std::ostream &out) const 00387 { 00388 out << "SO3State ["; 00389 if (state) 00390 { 00391 const StateType *qstate = static_cast<const StateType*>(state); 00392 out << qstate->x << " " << qstate->y << " " << qstate->z << " " << qstate->w; 00393 } 00394 else 00395 out << "NULL"; 00396 out << ']' << std::endl; 00397 } 00398 00399 void ompl::base::SO3StateSpace::printSettings(std::ostream &out) const 00400 { 00401 out << "SO(3) state space '" << getName() << "' (represented using quaternions)" << std::endl; 00402 }