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 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines