00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
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
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
00112
00113
00114
00115
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
00204
00205
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
00237
00238
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)
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 }