All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
src/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: 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 x = rng_.gaussian(0, stdDev), y = rng_.gaussian(0, stdDev), z = rng_.gaussian(0, stdDev),
00122         theta = std::sqrt(x*x + y*y + z*z);
00123     if (theta < std::numeric_limits<double>::epsilon())
00124         space_->copyState(state, mean);
00125     else
00126     {
00127         SO3StateSpace::StateType q,
00128             *qs = static_cast<SO3StateSpace::StateType*>(state);
00129         const SO3StateSpace::StateType *qmu = static_cast<const SO3StateSpace::StateType*>(mean);
00130         double s = sin(theta / 2) / theta;
00131         q.w = cos(theta / 2);
00132         q.x = s * x;
00133         q.y = s * y;
00134         q.z = s * z;
00135         quaternionProduct(*qs, *qmu, q);
00136     }
00137 }
00138 
00139 unsigned int ompl::base::SO3StateSpace::getDimension(void) const
00140 {
00141     return 3;
00142 }
00143 
00144 double ompl::base::SO3StateSpace::getMaximumExtent(void) const
00145 {
00146     return .5 * boost::math::constants::pi<double>();
00147 }
00148 
00149 double ompl::base::SO3StateSpace::norm(const StateType *state) const
00150 {
00151     double nrmSqr = state->x * state->x + state->y * state->y + state->z * state->z + state->w * state->w;
00152     return (fabs(nrmSqr - 1.0) > std::numeric_limits<double>::epsilon()) ? sqrt(nrmSqr) : 1.0;
00153 }
00154 
00155 void ompl::base::SO3StateSpace::enforceBounds(State *state) const
00156 {
00157     StateType *qstate = static_cast<StateType*>(state);
00158     double nrm = norm(qstate);
00159     if (fabs(nrm) < MAX_QUATERNION_NORM_ERROR)
00160         qstate->setIdentity();
00161     else if (fabs(nrm - 1.0) > MAX_QUATERNION_NORM_ERROR)
00162     {
00163         qstate->x /= nrm;
00164         qstate->y /= nrm;
00165         qstate->z /= nrm;
00166         qstate->w /= nrm;
00167     }
00168 }
00169 
00170 bool ompl::base::SO3StateSpace::satisfiesBounds(const State *state) const
00171 {
00172     return fabs(norm(static_cast<const StateType*>(state)) - 1.0) < MAX_QUATERNION_NORM_ERROR;
00173 }
00174 
00175 void ompl::base::SO3StateSpace::copyState(State *destination, const State *source) const
00176 {
00177     const StateType *qsource = static_cast<const StateType*>(source);
00178     StateType *qdestination = static_cast<StateType*>(destination);
00179     qdestination->x = qsource->x;
00180     qdestination->y = qsource->y;
00181     qdestination->z = qsource->z;
00182     qdestination->w = qsource->w;
00183 }
00184 
00185 unsigned int ompl::base::SO3StateSpace::getSerializationLength(void) const
00186 {
00187     return sizeof(double) * 4;
00188 }
00189 
00190 void ompl::base::SO3StateSpace::serialize(void *serialization, const State *state) const
00191 {
00192     memcpy(serialization, &state->as<StateType>()->x, sizeof(double) * 4);
00193 }
00194 
00195 void ompl::base::SO3StateSpace::deserialize(State *state, const void *serialization) const
00196 {
00197     memcpy(&state->as<StateType>()->x, serialization, sizeof(double) * 4);
00198 }
00199 
00201 
00202 /*
00203 Based on code from :
00204 
00205 Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans  http://continuousphysics.com/Bullet/
00206 */
00207 namespace ompl
00208 {
00209     namespace base
00210     {
00211         static inline double arcLength(const State *state1, const State *state2)
00212         {
00213             const SO3StateSpace::StateType *qs1 = static_cast<const SO3StateSpace::StateType*>(state1);
00214             const SO3StateSpace::StateType *qs2 = static_cast<const SO3StateSpace::StateType*>(state2);
00215             double dq = fabs(qs1->x * qs2->x + qs1->y * qs2->y + qs1->z * qs2->z + qs1->w * qs2->w);
00216             if (dq > 1.0 - MAX_QUATERNION_NORM_ERROR)
00217                 return 0.0;
00218             else
00219                 return acos(dq);
00220         }
00221     }
00222 }
00224 
00225 double ompl::base::SO3StateSpace::distance(const State *state1, const State *state2) const
00226 {
00227     return arcLength(state1, state2);
00228 }
00229 
00230 bool ompl::base::SO3StateSpace::equalStates(const State *state1, const State *state2) const
00231 {
00232     return arcLength(state1, state2) < std::numeric_limits<double>::epsilon();
00233 }
00234 
00235 /*
00236 Based on code from :
00237 
00238 Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans  http://continuousphysics.com/Bullet/
00239 */
00240 void ompl::base::SO3StateSpace::interpolate(const State *from, const State *to, const double t, State *state) const
00241 {
00242     assert(fabs(norm(static_cast<const StateType*>(from)) - 1.0) < MAX_QUATERNION_NORM_ERROR);
00243     assert(fabs(norm(static_cast<const StateType*>(to)) - 1.0) < MAX_QUATERNION_NORM_ERROR);
00244 
00245     double theta = arcLength(from, to);
00246     if (theta > std::numeric_limits<double>::epsilon())
00247     {
00248         double d = 1.0 / sin(theta);
00249         double s0 = sin((1.0 - t) * theta);
00250         double s1 = sin(t * theta);
00251 
00252         const StateType *qs1 = static_cast<const StateType*>(from);
00253         const StateType *qs2 = static_cast<const StateType*>(to);
00254         StateType       *qr  = static_cast<StateType*>(state);
00255         double dq = qs1->x * qs2->x + qs1->y * qs2->y + qs1->z * qs2->z + qs1->w * qs2->w;
00256         if (dq < 0)  // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp
00257             s1 = -s1;
00258 
00259         qr->x = (qs1->x * s0 + qs2->x * s1) * d;
00260         qr->y = (qs1->y * s0 + qs2->y * s1) * d;
00261         qr->z = (qs1->z * s0 + qs2->z * s1) * d;
00262         qr->w = (qs1->w * s0 + qs2->w * s1) * d;
00263     }
00264     else
00265     {
00266         if (state != from)
00267             copyState(state, from);
00268     }
00269 }
00270 
00271 ompl::base::StateSamplerPtr ompl::base::SO3StateSpace::allocDefaultStateSampler(void) const
00272 {
00273     return StateSamplerPtr(new SO3StateSampler(this));
00274 }
00275 
00276 ompl::base::State* ompl::base::SO3StateSpace::allocState(void) const
00277 {
00278     return new StateType();
00279 }
00280 
00281 void ompl::base::SO3StateSpace::freeState(State *state) const
00282 {
00283     delete static_cast<StateType*>(state);
00284 }
00285 
00286 void ompl::base::SO3StateSpace::registerProjections(void)
00287 {
00288     class SO3DefaultProjection : public ProjectionEvaluator
00289     {
00290     public:
00291 
00292         SO3DefaultProjection(const StateSpace *space) : ProjectionEvaluator(space)
00293         {
00294         }
00295 
00296         virtual unsigned int getDimension(void) const
00297         {
00298             return 3;
00299         }
00300 
00301         virtual void defaultCellSizes(void)
00302         {
00303             cellSizes_.resize(3);
00304             cellSizes_[0] = boost::math::constants::pi<double>() / magic::PROJECTION_DIMENSION_SPLITS;
00305             cellSizes_[1] = boost::math::constants::pi<double>() / magic::PROJECTION_DIMENSION_SPLITS;
00306             cellSizes_[2] = boost::math::constants::pi<double>() / magic::PROJECTION_DIMENSION_SPLITS;
00307         }
00308 
00309         virtual void project(const State *state, EuclideanProjection &projection) const
00310         {
00311             projection(0) = state->as<SO3StateSpace::StateType>()->x;
00312             projection(1) = state->as<SO3StateSpace::StateType>()->y;
00313             projection(2) = state->as<SO3StateSpace::StateType>()->z;
00314         }
00315     };
00316 
00317     registerDefaultProjection(ProjectionEvaluatorPtr(dynamic_cast<ProjectionEvaluator*>(new SO3DefaultProjection(this))));
00318 }
00319 
00320 double* ompl::base::SO3StateSpace::getValueAddressAtIndex(State *state, const unsigned int index) const
00321 {
00322     return index < 4 ? &(state->as<StateType>()->x) + index : NULL;
00323 }
00324 
00325 void ompl::base::SO3StateSpace::printState(const State *state, std::ostream &out) const
00326 {
00327     out << "SO3State [";
00328     if (state)
00329     {
00330         const StateType *qstate = static_cast<const StateType*>(state);
00331         out << qstate->x << " " << qstate->y << " " << qstate->z << " " << qstate->w;
00332     }
00333     else
00334         out << "NULL";
00335     out << ']' << std::endl;
00336 }
00337 
00338 void ompl::base::SO3StateSpace::printSettings(std::ostream &out) const
00339 {
00340     out << "SO(3) state space '" << getName() << "' (represented using quaternions)" << std::endl;
00341 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines