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
00046 void ompl::base::SO3StateSpace::StateType::setAxisAngle(double ax, double ay, double az, double angle)
00047 {
00048 double norm = sqrt(ax * ax + ay * ay + az * az);
00049 if (norm < MAX_QUATERNION_NORM_ERROR)
00050 setIdentity();
00051 else
00052 {
00053 double s = sin(angle / 2.0);
00054 x = s * ax / norm;
00055 y = s * ay / norm;
00056 z = s * az / norm;
00057 w = cos(angle / 2.0);
00058 }
00059 }
00060
00061 void ompl::base::SO3StateSpace::StateType::setIdentity(void)
00062 {
00063 x = y = z = 0.0;
00064 w = 1.0;
00065 }
00066
00067 void ompl::base::SO3StateSampler::sampleUniform(State *state)
00068 {
00069 rng_.quaternion(&state->as<SO3StateSpace::StateType>()->x);
00070 }
00071
00072 void ompl::base::SO3StateSampler::sampleUniformNear(State *state, const State * , const double )
00073 {
00075 sampleUniform(state);
00076 }
00077
00078 void ompl::base::SO3StateSampler::sampleGaussian(State *state, const State * , const double )
00079 {
00081 sampleUniform(state);
00082 }
00083
00084
00085 unsigned int ompl::base::SO3StateSpace::getDimension(void) const
00086 {
00087 return 3;
00088 }
00089
00090 double ompl::base::SO3StateSpace::getMaximumExtent(void) const
00091 {
00092 return boost::math::constants::pi<double>();
00093 }
00094
00095 double ompl::base::SO3StateSpace::norm(const StateType *state) const
00096 {
00097 double nrmSqr = state->x * state->x + state->y * state->y + state->z * state->z + state->w * state->w;
00098 return (fabs(nrmSqr - 1.0) > std::numeric_limits<double>::epsilon()) ? sqrt(nrmSqr) : 1.0;
00099 }
00100
00101 void ompl::base::SO3StateSpace::enforceBounds(State *state) const
00102 {
00103 StateType *qstate = static_cast<StateType*>(state);
00104 double nrm = norm(qstate);
00105 if (fabs(nrm - 1.0) > MAX_QUATERNION_NORM_ERROR)
00106 {
00107 qstate->x /= nrm;
00108 qstate->y /= nrm;
00109 qstate->z /= nrm;
00110 qstate->w /= nrm;
00111 }
00112 else
00113 qstate->setIdentity();
00114 }
00115
00116 bool ompl::base::SO3StateSpace::satisfiesBounds(const State *state) const
00117 {
00118 return fabs(norm(static_cast<const StateType*>(state)) - 1.0) < MAX_QUATERNION_NORM_ERROR;
00119 }
00120
00121 void ompl::base::SO3StateSpace::copyState(State *destination, const State *source) const
00122 {
00123 const StateType *qsource = static_cast<const StateType*>(source);
00124 StateType *qdestination = static_cast<StateType*>(destination);
00125 qdestination->x = qsource->x;
00126 qdestination->y = qsource->y;
00127 qdestination->z = qsource->z;
00128 qdestination->w = qsource->w;
00129 }
00130
00131
00132
00133
00134
00135
00136
00137 double ompl::base::SO3StateSpace::distance(const State *state1, const State *state2) const
00138 {
00139 const StateType *qs1 = static_cast<const StateType*>(state1);
00140 const StateType *qs2 = static_cast<const StateType*>(state2);
00141 double dq = fabs(qs1->x * qs2->x + qs1->y * qs2->y + qs1->z * qs2->z + qs1->w * qs2->w);
00142 if (dq > 1.0 - MAX_QUATERNION_NORM_ERROR)
00143 return 0.0;
00144 else
00145 return acos(dq) * 2.0;
00146 }
00147
00148 bool ompl::base::SO3StateSpace::equalStates(const State *state1, const State *state2) const
00149 {
00150 return distance(state1, state2) < std::numeric_limits<double>::epsilon() * 2.0;
00151 }
00152
00153
00154
00155
00156
00157
00158 void ompl::base::SO3StateSpace::interpolate(const State *from, const State *to, const double t, State *state) const
00159 {
00160 assert(norm(static_cast<const StateType*>(from)) - 1.0 < MAX_QUATERNION_NORM_ERROR);
00161 assert(norm(static_cast<const StateType*>(to)) - 1.0 < MAX_QUATERNION_NORM_ERROR);
00162
00163 double theta = distance(from, to) / 2.0;
00164 if (theta > std::numeric_limits<double>::epsilon())
00165 {
00166 double d = 1.0 / sin(theta);
00167 double s0 = sin((1.0 - t) * theta);
00168 double s1 = sin(t * theta);
00169
00170 const StateType *qs1 = static_cast<const StateType*>(from);
00171 const StateType *qs2 = static_cast<const StateType*>(to);
00172 StateType *qr = static_cast<StateType*>(state);
00173 double dq = qs1->x * qs2->x + qs1->y * qs2->y + qs1->z * qs2->z + qs1->w * qs2->w;
00174 if (dq < 0)
00175 s1 = -s1;
00176
00177 qr->x = (qs1->x * s0 + qs2->x * s1) * d;
00178 qr->y = (qs1->y * s0 + qs2->y * s1) * d;
00179 qr->z = (qs1->z * s0 + qs2->z * s1) * d;
00180 qr->w = (qs1->w * s0 + qs2->w * s1) * d;
00181 }
00182 else
00183 {
00184 if (state != from)
00185 copyState(state, from);
00186 }
00187 }
00188
00189 ompl::base::StateSamplerPtr ompl::base::SO3StateSpace::allocStateSampler(void) const
00190 {
00191 return StateSamplerPtr(new SO3StateSampler(this));
00192 }
00193
00194 ompl::base::State* ompl::base::SO3StateSpace::allocState(void) const
00195 {
00196 return new StateType();
00197 }
00198
00199 void ompl::base::SO3StateSpace::freeState(State *state) const
00200 {
00201 delete static_cast<StateType*>(state);
00202 }
00203
00204 void ompl::base::SO3StateSpace::registerProjections(void)
00205 {
00206 class SO3DefaultProjection : public ProjectionEvaluator
00207 {
00208 public:
00209
00210 SO3DefaultProjection(const StateSpace *space) : ProjectionEvaluator(space)
00211 {
00212 }
00213
00214 virtual unsigned int getDimension(void) const
00215 {
00216 return 3;
00217 }
00218
00219 virtual void defaultCellSizes(void)
00220 {
00221 cellSizes_.resize(3);
00222 cellSizes_[0] = boost::math::constants::pi<double>() / magic::PROJECTION_DIMENSION_SPLITS;
00223 cellSizes_[1] = boost::math::constants::pi<double>() / magic::PROJECTION_DIMENSION_SPLITS;
00224 cellSizes_[2] = boost::math::constants::pi<double>() / magic::PROJECTION_DIMENSION_SPLITS;
00225 }
00226
00227 virtual void project(const State *state, EuclideanProjection &projection) const
00228 {
00229 projection(0) = state->as<SO3StateSpace::StateType>()->x;
00230 projection(1) = state->as<SO3StateSpace::StateType>()->y;
00231 projection(2) = state->as<SO3StateSpace::StateType>()->z;
00232 }
00233 };
00234
00235 registerDefaultProjection(ProjectionEvaluatorPtr(dynamic_cast<ProjectionEvaluator*>(new SO3DefaultProjection(this))));
00236 }
00237
00238 double* ompl::base::SO3StateSpace::getValueAddressAtIndex(State *state, const unsigned int index) const
00239 {
00240 return index < 4 ? &(state->as<StateType>()->x) + index : NULL;
00241 }
00242
00243 void ompl::base::SO3StateSpace::printState(const State *state, std::ostream &out) const
00244 {
00245 out << "SO3State [";
00246 if (state)
00247 {
00248 const StateType *qstate = static_cast<const StateType*>(state);
00249 out << qstate->x << " " << qstate->y << " " << qstate->z << " " << qstate->w;
00250 }
00251 else
00252 out << "NULL";
00253 out << ']' << std::endl;
00254 }
00255
00256 void ompl::base::SO3StateSpace::printSettings(std::ostream &out) const
00257 {
00258 out << "SO(3) state space '" << getName() << "' (represented using quaternions)" << std::endl;
00259 }