All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator
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: 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 
00044 static const double MAX_QUATERNION_NORM_ERROR = 1e-9;
00045 
00047 namespace ompl
00048 {
00049     namespace base
00050     {
00051         static inline void computeAxisAngle(SO3StateSpace::StateType &q, double ax, double ay, double az, double angle)
00052         {
00053             double norm = sqrt(ax * ax + ay * ay + az * az);
00054             if (norm < MAX_QUATERNION_NORM_ERROR)
00055                 q.setIdentity();
00056             else
00057             {
00058                 double s = sin(angle / 2.0);
00059                 q.x = s * ax / norm;
00060                 q.y = s * ay / norm;
00061                 q.z = s * az / norm;
00062                 q.w = cos(angle / 2.0);
00063             }
00064         }
00065 
00066         /* Standard quaternion multiplication: q = q0 * q1 */
00067         static inline void quaternionProduct(SO3StateSpace::StateType &q, const SO3StateSpace::StateType& q0, const SO3StateSpace::StateType& q1)
00068         {
00069             q.x = q0.w*q1.x + q0.x*q1.w + q0.y*q1.z - q0.z*q1.y;
00070             q.y = q0.w*q1.y + q0.y*q1.w + q0.z*q1.x - q0.x*q1.z;
00071             q.z = q0.w*q1.z + q0.z*q1.w + q0.x*q1.y - q0.y*q1.x;
00072             q.w = q0.w*q1.w - q0.x*q1.x - q0.y*q1.y - q0.z*q1.z;
00073         }
00074     }
00075 }
00077 
00078 void ompl::base::SO3StateSpace::StateType::setAxisAngle(double ax, double ay, double az, double angle)
00079 {
00080     computeAxisAngle(*this, ax, ay, az, angle);
00081 }
00082 
00083 void ompl::base::SO3StateSpace::StateType::setIdentity(void)
00084 {
00085     x = y = z = 0.0;
00086     w = 1.0;
00087 }
00088 
00089 void ompl::base::SO3StateSampler::sampleUniform(State *state)
00090 {
00091     rng_.quaternion(&state->as<SO3StateSpace::StateType>()->x);
00092 }
00093 
00094 void ompl::base::SO3StateSampler::sampleUniformNear(State *state, const State *near, const double distance)
00095 {
00096     if (distance >= .25 * boost::math::constants::pi<double>())
00097     {
00098         sampleUniform(state);
00099         return;
00100     }
00101     double d = rng_.uniform01();
00102     SO3StateSpace::StateType q,
00103         *qs = static_cast<SO3StateSpace::StateType*>(state);
00104     const SO3StateSpace::StateType *qnear = static_cast<const SO3StateSpace::StateType*>(near);
00105     computeAxisAngle(q, rng_.gaussian01(), rng_.gaussian01(), rng_.gaussian01(), 2.*pow(d,1./3.)*distance);
00106     quaternionProduct(*qs, *qnear, q);
00107 }
00108 
00109 void ompl::base::SO3StateSampler::sampleGaussian(State *state, const State * mean, const double stdDev)
00110 {
00111     // CDF of N(0, 1.17) at -pi/4 is approx. .25, so there's .25 probability
00112     // weight in each tail. Since the maximum distance in SO(3) is pi/2, we're
00113     // essentially as likely to sample a state within distance [0, pi/4] as
00114     // within distance [pi/4, pi/2]. With most weight in the tails (that wrap
00115     // around in case of quaternions) we might as well sample uniformly.
00116     if (stdDev > 1.17)
00117     {
00118         sampleUniform(state);
00119         return;
00120     }
00121     double d = rng_.gaussian01();
00122     SO3StateSpace::StateType q,
00123         *qs = static_cast<SO3StateSpace::StateType*>(state);
00124     const SO3StateSpace::StateType *qmu = static_cast<const SO3StateSpace::StateType*>(mean);
00125     q.setAxisAngle(rng_.gaussian01(), rng_.gaussian01(), rng_.gaussian01(), 2.*pow(d,1./3.)*stdDev);
00126     quaternionProduct(*qs, *qmu, q);
00127 }
00128 
00129 unsigned int ompl::base::SO3StateSpace::getDimension(void) const
00130 {
00131     return 3;
00132 }
00133 
00134 double ompl::base::SO3StateSpace::getMaximumExtent(void) const
00135 {
00136     return .5 * boost::math::constants::pi<double>();
00137 }
00138 
00139 double ompl::base::SO3StateSpace::norm(const StateType *state) const
00140 {
00141     double nrmSqr = state->x * state->x + state->y * state->y + state->z * state->z + state->w * state->w;
00142     return (fabs(nrmSqr - 1.0) > std::numeric_limits<double>::epsilon()) ? sqrt(nrmSqr) : 1.0;
00143 }
00144 
00145 void ompl::base::SO3StateSpace::enforceBounds(State *state) const
00146 {
00147     StateType *qstate = static_cast<StateType*>(state);
00148     double nrm = norm(qstate);
00149     if (fabs(nrm - 1.0) > MAX_QUATERNION_NORM_ERROR)
00150     {
00151         qstate->x /= nrm;
00152         qstate->y /= nrm;
00153         qstate->z /= nrm;
00154         qstate->w /= nrm;
00155     }
00156     else
00157         qstate->setIdentity();
00158 }
00159 
00160 bool ompl::base::SO3StateSpace::satisfiesBounds(const State *state) const
00161 {
00162     return fabs(norm(static_cast<const StateType*>(state)) - 1.0) < MAX_QUATERNION_NORM_ERROR;
00163 }
00164 
00165 void ompl::base::SO3StateSpace::copyState(State *destination, const State *source) const
00166 {
00167     const StateType *qsource = static_cast<const StateType*>(source);
00168     StateType *qdestination = static_cast<StateType*>(destination);
00169     qdestination->x = qsource->x;
00170     qdestination->y = qsource->y;
00171     qdestination->z = qsource->z;
00172     qdestination->w = qsource->w;
00173 }
00174 
00175 unsigned int ompl::base::SO3StateSpace::getSerializationLength(void) const
00176 {
00177     return sizeof(double) * 4;
00178 }
00179 
00180 void ompl::base::SO3StateSpace::serialize(void *serialization, const State *state) const
00181 {
00182     memcpy(serialization, &state->as<StateType>()->x, sizeof(double) * 4);
00183 }
00184 
00185 void ompl::base::SO3StateSpace::deserialize(State *state, const void *serialization) const
00186 {
00187     memcpy(&state->as<StateType>()->x, serialization, sizeof(double) * 4);
00188 }
00189 
00191 
00192 /*
00193 Based on code from :
00194 
00195 Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans  http://continuousphysics.com/Bullet/
00196 */
00197 namespace ompl
00198 {
00199     namespace base
00200     {
00201         static inline double arcLength(const State *state1, const State *state2)
00202         {
00203             const SO3StateSpace::StateType *qs1 = static_cast<const SO3StateSpace::StateType*>(state1);
00204             const SO3StateSpace::StateType *qs2 = static_cast<const SO3StateSpace::StateType*>(state2);
00205             double dq = fabs(qs1->x * qs2->x + qs1->y * qs2->y + qs1->z * qs2->z + qs1->w * qs2->w);
00206             if (dq > 1.0 - MAX_QUATERNION_NORM_ERROR)
00207                 return 0.0;
00208             else
00209                 return acos(dq);
00210         }
00211     }
00212 }
00214 
00215 double ompl::base::SO3StateSpace::distance(const State *state1, const State *state2) const
00216 {
00217     return arcLength(state1, state2);
00218 }
00219 
00220 bool ompl::base::SO3StateSpace::equalStates(const State *state1, const State *state2) const
00221 {
00222     return arcLength(state1, state2) < std::numeric_limits<double>::epsilon();
00223 }
00224 
00225 /*
00226 Based on code from :
00227 
00228 Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans  http://continuousphysics.com/Bullet/
00229 */
00230 void ompl::base::SO3StateSpace::interpolate(const State *from, const State *to, const double t, State *state) const
00231 {
00232     assert(fabs(norm(static_cast<const StateType*>(from)) - 1.0) < MAX_QUATERNION_NORM_ERROR);
00233     assert(fabs(norm(static_cast<const StateType*>(to)) - 1.0) < MAX_QUATERNION_NORM_ERROR);
00234 
00235     double theta = arcLength(from, to);
00236     if (theta > std::numeric_limits<double>::epsilon())
00237     {
00238         double d = 1.0 / sin(theta);
00239         double s0 = sin((1.0 - t) * theta);
00240         double s1 = sin(t * theta);
00241 
00242         const StateType *qs1 = static_cast<const StateType*>(from);
00243         const StateType *qs2 = static_cast<const StateType*>(to);
00244         StateType       *qr  = static_cast<StateType*>(state);
00245         double dq = qs1->x * qs2->x + qs1->y * qs2->y + qs1->z * qs2->z + qs1->w * qs2->w;
00246         if (dq < 0)  // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp
00247             s1 = -s1;
00248 
00249         qr->x = (qs1->x * s0 + qs2->x * s1) * d;
00250         qr->y = (qs1->y * s0 + qs2->y * s1) * d;
00251         qr->z = (qs1->z * s0 + qs2->z * s1) * d;
00252         qr->w = (qs1->w * s0 + qs2->w * s1) * d;
00253     }
00254     else
00255     {
00256         if (state != from)
00257             copyState(state, from);
00258     }
00259 }
00260 
00261 ompl::base::StateSamplerPtr ompl::base::SO3StateSpace::allocDefaultStateSampler(void) const
00262 {
00263     return StateSamplerPtr(new SO3StateSampler(this));
00264 }
00265 
00266 ompl::base::State* ompl::base::SO3StateSpace::allocState(void) const
00267 {
00268     return new StateType();
00269 }
00270 
00271 void ompl::base::SO3StateSpace::freeState(State *state) const
00272 {
00273     delete static_cast<StateType*>(state);
00274 }
00275 
00276 void ompl::base::SO3StateSpace::registerProjections(void)
00277 {
00278     class SO3DefaultProjection : public ProjectionEvaluator
00279     {
00280     public:
00281 
00282         SO3DefaultProjection(const StateSpace *space) : ProjectionEvaluator(space)
00283         {
00284         }
00285 
00286         virtual unsigned int getDimension(void) const
00287         {
00288             return 3;
00289         }
00290 
00291         virtual void defaultCellSizes(void)
00292         {
00293             cellSizes_.resize(3);
00294             cellSizes_[0] = boost::math::constants::pi<double>() / magic::PROJECTION_DIMENSION_SPLITS;
00295             cellSizes_[1] = boost::math::constants::pi<double>() / magic::PROJECTION_DIMENSION_SPLITS;
00296             cellSizes_[2] = boost::math::constants::pi<double>() / magic::PROJECTION_DIMENSION_SPLITS;
00297         }
00298 
00299         virtual void project(const State *state, EuclideanProjection &projection) const
00300         {
00301             projection(0) = state->as<SO3StateSpace::StateType>()->x;
00302             projection(1) = state->as<SO3StateSpace::StateType>()->y;
00303             projection(2) = state->as<SO3StateSpace::StateType>()->z;
00304         }
00305     };
00306 
00307     registerDefaultProjection(ProjectionEvaluatorPtr(dynamic_cast<ProjectionEvaluator*>(new SO3DefaultProjection(this))));
00308 }
00309 
00310 double* ompl::base::SO3StateSpace::getValueAddressAtIndex(State *state, const unsigned int index) const
00311 {
00312     return index < 4 ? &(state->as<StateType>()->x) + index : NULL;
00313 }
00314 
00315 void ompl::base::SO3StateSpace::printState(const State *state, std::ostream &out) const
00316 {
00317     out << "SO3State [";
00318     if (state)
00319     {
00320         const StateType *qstate = static_cast<const StateType*>(state);
00321         out << qstate->x << " " << qstate->y << " " << qstate->z << " " << qstate->w;
00322     }
00323     else
00324         out << "NULL";
00325     out << ']' << std::endl;
00326 }
00327 
00328 void ompl::base::SO3StateSpace::printSettings(std::ostream &out) const
00329 {
00330     out << "SO(3) state space '" << getName() << "' (represented using quaternions)" << std::endl;
00331 }