37 #include "ompl/base/spaces/SO3StateSpace.h"
41 #include "ompl/tools/config/MagicConstants.h"
42 #include <boost/math/constants/constants.hpp>
43 #include <boost/version.hpp>
44 #include <boost/assert.hpp>
46 #ifndef BOOST_ASSERT_MSG
47 #define BOOST_ASSERT_MSG(expr, msg) assert(expr)
50 #if BOOST_VERSION < 105000
51 namespace boost{
namespace math{
namespace constants{
52 BOOST_DEFINE_MATH_CONSTANT(root_three, 1.732050807568877293527446341505872366, 1.73205080756887729352744634150587236694280525381038062805580697945193301690880003708114618675724857567562614142, 0)
56 static const double MAX_QUATERNION_NORM_ERROR = 1e-9;
65 double norm = sqrt(ax * ax + ay * ay + az * az);
66 if (norm < MAX_QUATERNION_NORM_ERROR)
70 double s = sin(angle / 2.0);
74 q.w = cos(angle / 2.0);
81 q.x = q0.w*q1.x + q0.x*q1.w + q0.y*q1.z - q0.z*q1.y;
82 q.y = q0.w*q1.y + q0.y*q1.w + q0.z*q1.x - q0.x*q1.z;
83 q.z = q0.w*q1.z + q0.z*q1.w + q0.x*q1.y - q0.y*q1.x;
84 q.w = q0.w*q1.w - q0.x*q1.x - q0.y*q1.y - q0.z*q1.z;
92 computeAxisAngle(*
this, ax, ay, az, angle);
108 if (distance >= .25 * boost::math::constants::pi<double>())
110 sampleUniform(state);
113 double d = rng_.uniform01();
117 computeAxisAngle(q, rng_.gaussian01(), rng_.gaussian01(), rng_.gaussian01(),
118 2. * pow(d, boost::math::constants::third<double>()) *
distance);
119 quaternionProduct(*qs, *qnear, q);
133 double rotDev = (2. * stdDev) / boost::math::constants::root_three<double>();
142 sampleUniform(state);
147 double x = rng_.gaussian(0, rotDev), y = rng_.gaussian(0, rotDev), z = rng_.gaussian(0, rotDev),
148 theta = std::sqrt(x*x + y*y + z*z);
149 if (theta < std::numeric_limits<double>::epsilon())
150 space_->copyState(state, mean);
156 double s = sin(theta / 2) / theta;
157 q.
w = cos(theta / 2);
161 quaternionProduct(*qs, *qmu, q);
172 return .5 * boost::math::constants::pi<double>();
177 double nrmSqr = state->
x * state->
x + state->
y * state->
y + state->
z * state->
z + state->
w * state->
w;
178 return (fabs(nrmSqr - 1.0) > std::numeric_limits<double>::epsilon()) ? sqrt(nrmSqr) : 1.0;
184 double nrm =
norm(qstate);
185 if (fabs(nrm) < MAX_QUATERNION_NORM_ERROR)
187 else if (fabs(nrm - 1.0) > MAX_QUATERNION_NORM_ERROR)
198 return fabs(
norm(static_cast<const StateType*>(state)) - 1.0) < MAX_QUATERNION_NORM_ERROR;
205 qdestination->
x = qsource->
x;
206 qdestination->
y = qsource->
y;
207 qdestination->
z = qsource->
z;
208 qdestination->
w = qsource->
w;
213 return sizeof(double) * 4;
218 memcpy(serialization, &state->
as<
StateType>()->
x,
sizeof(
double) * 4);
223 memcpy(&state->
as<
StateType>()->
x, serialization,
sizeof(
double) * 4);
237 static inline double arcLength(
const State *state1,
const State *state2)
241 double dq = fabs(qs1->
x * qs2->
x + qs1->
y * qs2->
y + qs1->
z * qs2->
z + qs1->
w * qs2->
w);
242 if (dq > 1.0 - MAX_QUATERNION_NORM_ERROR)
254 "The states passed to SO3StateSpace::distance are not within bounds. Call "
255 "SO3StateSpace::enforceBounds() in, e.g., ompl::control::ODESolver::PostPropagationEvent, "
256 "ompl::control::StatePropagator, or ompl::base::StateValidityChecker");
257 return arcLength(state1, state2);
262 return arcLength(state1, state2) < std::numeric_limits<double>::epsilon();
272 assert(fabs(
norm(static_cast<const StateType*>(from)) - 1.0) < MAX_QUATERNION_NORM_ERROR);
273 assert(fabs(
norm(static_cast<const StateType*>(to)) - 1.0) < MAX_QUATERNION_NORM_ERROR);
275 double theta = arcLength(from, to);
276 if (theta > std::numeric_limits<double>::epsilon())
278 double d = 1.0 / sin(theta);
279 double s0 = sin((1.0 - t) * theta);
280 double s1 = sin(t * theta);
285 double dq = qs1->
x * qs2->
x + qs1->
y * qs2->
y + qs1->
z * qs2->
z + qs1->
w * qs2->
w;
289 qr->
x = (qs1->
x * s0 + qs2->
x * s1) * d;
290 qr->
y = (qs1->
y * s0 + qs2->
y * s1) * d;
291 qr->
z = (qs1->
z * s0 + qs2->
z * s1) * d;
292 qr->
w = (qs1->
w * s0 + qs2->
w * s1) * d;
331 virtual void defaultCellSizes(
void)
333 cellSizes_.resize(3);
338 bounds_.setLow(-1.0);
339 bounds_.setHigh(1.0);
355 return index < 4 ? &(state->
as<
StateType>()->x) + index : NULL;
364 out << qstate->
x <<
" " << qstate->
y <<
" " << qstate->
z <<
" " << qstate->
w;
368 out <<
']' << std::endl;
373 out <<
"SO(3) state space '" <<
getName() <<
"' (represented using quaternions)" << std::endl;
virtual void serialize(void *serialization, const State *state) const
Write the binary representation of state to serialization.
virtual double * getValueAddressAtIndex(State *state, const unsigned int index) const
Many states contain a number of double values. This function provides a means to get the memory addre...
virtual double getMaximumExtent(void) const
Get the maximum value a call to distance() can return (or an upper bound). For unbounded state spaces...
virtual State * allocState(void) const
Allocate a state that can store a point in the described space.
virtual bool satisfiesBounds(const State *state) const
Check if a state is inside the bounding box. For unbounded spaces this function can always return tru...
virtual void printState(const State *state, std::ostream &out) const
Print a state to a stream.
A boost shared pointer wrapper for ompl::base::StateSampler.
virtual unsigned int getDimension(void) const
Get the dimension of the space (not the dimension of the surrounding ambient space) ...
virtual double distance(const State *state1, const State *state2) const
Computes distance between two states. This function satisfies the properties of a metric if isMetricS...
virtual void printSettings(std::ostream &out) const
Print the settings for this state space to a stream.
virtual bool equalStates(const State *state1, const State *state2) const
Checks whether two states are equal.
void registerDefaultProjection(const ProjectionEvaluatorPtr &projection)
Register the default projection for this state space.
virtual void enforceBounds(State *state) const
Bring the state within the bounds of the state space. For unbounded spaces this function can be a no-...
State StateType
Define the type of state allocated by this space.
const T * as(void) const
Cast this instance to a desired type.
double w
scalar component of quaternion
virtual void deserialize(State *state, const void *serialization) const
Read the binary representation of a state from serialization and write it to state.
virtual StateSamplerPtr allocDefaultStateSampler(void) const
Allocate an instance of the default uniform state sampler for this space.
virtual void interpolate(const State *from, const State *to, const double t, State *state) const
Computes the state that lies at time t in [0, 1] on the segment that connects from state to to state...
virtual void freeState(State *state) const
Free the memory of the allocated state.
The definition of a state in SO(3) represented as a unit quaternion.
const std::string & getName(void) const
Get the name of the state space.
State space sampler for SO(3), using quaternion representation.
A boost shared pointer wrapper for ompl::base::ProjectionEvaluator.
static const double PROJECTION_DIMENSION_SPLITS
When the cell sizes for a projection are automatically computed, this value defines the number of par...
Representation of a space in which planning can be performed. Topology specific sampling, interpolation and distance are defined.
virtual void copyState(State *destination, const State *source) const
Copy a state to another. The memory of source and destination should NOT overlap. ...
boost::numeric::ublas::vector< double > EuclideanProjection
The datatype for state projections. This class contains a real vector.
virtual void registerProjections(void)
Register the projections for this state space. Usually, this is at least the default projection...
Definition of an abstract state.
void setAxisAngle(double ax, double ay, double az, double angle)
Set the quaternion from axis-angle representation.
virtual void sampleUniform(State *state)
Sample a state.
void setIdentity(void)
Set the state to identity – no rotation.
double z
Z component of quaternion vector.
virtual void sampleUniformNear(State *state, const State *near, const double distance)
To sample unit quaternions uniformly within some given distance, we sample a 3-vector from the R^3 ta...
double y
Y component of quaternion vector.
double norm(const StateType *state) const
Compute the norm of a state.
virtual unsigned int getSerializationLength(void) const
Get the number of chars in the serialization of a state in this space.
virtual void sampleGaussian(State *state, const State *mean, const double stdDev)
Sample a state such that the expected distance between mean and state is stdDev.
double x
X component of quaternion vector.
Abstract definition for a class computing projections to Rn. Implicit integer grids are imposed on th...