MorseStateSpace.h
MORSE State. This is a compound state that allows accessing the properties of the bodies the state sp...
Definition: MorseStateSpace.h:54
const MorseEnvironmentPtr & getEnvironment() const
Get the MORSE environment this state space corresponds to.
Definition: MorseStateSpace.h:86
A boost shared pointer wrapper for ompl::base::StateSampler.
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...
Definition: MorseStateSpace.cpp:141
void readState(State *state) const
Read the parameters of the MORSE bodies and store them in state.
Definition: MorseStateSpace.cpp:156
CompoundState StateType
Define the type of state allocated by this state space.
Definition: StateSpace.h:549
unsigned int getNrBodies() const
Get the number of bodies this state space represents.
Definition: MorseStateSpace.h:92
StateSamplerPtr allocStateSampler() const
Allocate an instance of the state sampler for this space. This sampler will be allocated with the sam...
Definition: MorseStateSpace.cpp:151
State * allocState() const
Allocate a state that can store a point in the described space.
Definition: MorseStateSpace.cpp:128
void freeState(State *state) const
Free the memory of the allocated state.
Definition: MorseStateSpace.cpp:135
A space to allow the composition of state spaces.
Definition: StateSpace.h:544
StateSamplerPtr allocDefaultStateSampler() const
Allocate an instance of the default uniform state sampler for this space.
Definition: MorseStateSpace.cpp:146
void writeState(const State *state) const
Set the parameters of the MORSE bodies to be the ones read from state.
Definition: MorseStateSpace.cpp:167
MorseEnvironmentPtr env_
Representation of the MORSE parameters OMPL needs to plan.
Definition: MorseStateSpace.h:131
bool satisfiesBounds(const State *state) const
This function checks whether a state satisfies its bounds.
Definition: MorseStateSpace.cpp:96
The lower and upper bounds for an Rn space.
Definition: RealVectorBounds.h:48
void setLinearVelocityBounds(const RealVectorBounds &bounds)
Set the bounds for each of the linear velocity subspaces.
Definition: MorseStateSpace.cpp:116
void setPositionBounds(const RealVectorBounds &bounds)
Set the bounds for each of the position subspaces.
Definition: MorseStateSpace.cpp:110
A boost shared pointer wrapper for ompl::base::MorseEnvironment.
void copyState(State *destination, const State *source) const
Copy a state to another. The memory of source and destination should NOT overlap. ...
Definition: MorseStateSpace.cpp:91
void setAngularVelocityBounds(const RealVectorBounds &bounds)
Set the bounds for each of the angular velocity subspaces.
Definition: MorseStateSpace.cpp:122
MorseStateSpace(const MorseEnvironmentPtr &env, double positionWeight=1.0, double linVelWeight=0.5, double angVelWeight=0.5, double orientationWeight=1.0)
Construct a state space representing MORSE states.
Definition: MorseStateSpace.cpp:44