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 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
00194
00195
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
00227
00228
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)
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 }