37 #ifndef OMPL_BASE_SPACES_SE3_STATE_SPACE_
38 #define OMPL_BASE_SPACES_SE3_STATE_SPACE_
40 #include "ompl/base/StateSpace.h"
41 #include "ompl/base/spaces/RealVectorStateSpace.h"
42 #include "ompl/base/spaces/SO3StateSpace.h"
65 return as<RealVectorStateSpace::StateType>(0)->values[0];
71 return as<RealVectorStateSpace::StateType>(0)->values[1];
77 return as<RealVectorStateSpace::StateType>(0)->values[2];
83 return *as<SO3StateSpace::StateType>(1);
89 return *as<SO3StateSpace::StateType>(1);
95 as<RealVectorStateSpace::StateType>(0)->values[0] = x;
101 as<RealVectorStateSpace::StateType>(0)->values[1] = y;
107 as<RealVectorStateSpace::StateType>(0)->values[2] = z;
111 void setXYZ(
double x,
double y,
double z)
129 virtual ~SE3StateSpace(
void)
136 as<RealVectorStateSpace>(0)->
setBounds(bounds);
142 return as<RealVectorStateSpace>(0)->
getBounds();
void setName(const std::string &name)
Set the name of the state space.
double getY(void) const
Get the Y component of the state.
int type_
A type assigned for this state space.
Definition of a compound state.
virtual State * allocState(void) const
Allocate a state that can store a point in the described space.
A boost shared pointer wrapper for ompl::base::StateSpace.
A state in SE(3): position = (x, y, z), quaternion = (x, y, z, w)
A state space representing SO(3). The internal representation is done with quaternions. The distance between states is the angle between quaternions and interpolation is done with slerp.
const RealVectorBounds & getBounds(void) const
Get the bounds for this state space.
virtual void registerProjections(void)
Register the projections for this state space. Usually, this is at least the default projection...
CompoundState StateType
Define the type of state allocated by this state space.
double getX(void) const
Get the X component of the state.
void setZ(double z)
Set the Z component of the state.
void setBounds(const RealVectorBounds &bounds)
Set the bounds of this state space. This defines the range of the space in which sampling is performe...
void setY(double y)
Set the Y component of the state.
A space to allow the composition of state spaces.
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.
ompl::base::SE3StateSpace
void setX(double x)
Set the X component of the state.
A state space representing Rn. The distance function is the L2 norm.
Definition of an abstract state.
void lock(void)
Lock this state space. This means no further spaces can be added as components. This function can be ...
void addSubspace(const StateSpacePtr &component, double weight)
Adds a new state space as part of the compound state space. For computing distances within the compou...
The lower and upper bounds for an Rn space.
virtual void freeState(State *state) const
Free the memory of the allocated state.
const SO3StateSpace::StateType & rotation(void) const
Get the rotation component of the state.
void setXYZ(double x, double y, double z)
Set the X, Y and Z components of the state.
A state space representing SE(3)
SO3StateSpace::StateType & rotation(void)
Get the rotation component of the state and allow changing it as well.
double getZ(void) const
Get the Z component of the state.