MorseStateSpace.cpp
44 ompl::base::MorseStateSpace::MorseStateSpace(const MorseEnvironmentPtr &env, double positionWeight, double linVelWeight,
103 if (!components_[i]->satisfiesBounds(state->as<CompoundStateSpace::StateType>()->components[i]))
141 void ompl::base::MorseStateSpace::interpolate(const State *from, const State *to, const double t, State *state) const
void setName(const std::string &name)
Set the name of the state space.
Definition: StateSpace.cpp:203
MORSE State. This is a compound state that allows accessing the properties of the bodies the state sp...
Definition: MorseStateSpace.h:54
virtual void copyState(State *destination, const State *source) const
Copy a state to another. The memory of source and destination should NOT overlap. ...
Definition: StateSpace.cpp:1014
virtual StateSamplerPtr allocDefaultStateSampler() const
Allocate an instance of the default uniform state sampler for this space.
Definition: StateSpace.cpp:1102
A boost shared pointer wrapper for ompl::base::StateSpace.
A boost shared pointer wrapper for ompl::base::StateSampler.
void lock()
Lock this state space. This means no further spaces can be added as components. This function can be ...
Definition: StateSpace.cpp:1146
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
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.
Definition: SO3StateSpace.h:84
CompoundState StateType
Define the type of state allocated by this state space.
Definition: StateSpace.h:549
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
void allocStateComponents(CompoundState *state) const
Allocate the state components. Called by allocState(). Usually called by derived state spaces...
Definition: StateSpace.cpp:1130
const StateSpacePtr & getSubspace(const unsigned int index) const
Get a specific subspace from the compound state space.
Definition: StateSpace.cpp:893
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
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...
Definition: StateSpace.cpp:1093
A space to allow the composition of state spaces.
Definition: StateSpace.h:544
The definition of a state in SO(3) represented as a unit quaternion.
Definition: SO3StateSpace.h:94
A state space representing Rn. The distance function is the L2 norm.
Definition: RealVectorStateSpace.h:75
StateSamplerPtr allocDefaultStateSampler() const
Allocate an instance of the default uniform state sampler for this space.
Definition: MorseStateSpace.cpp:146
virtual void freeState(State *state) const
Free the memory of the allocated state.
Definition: StateSpace.cpp:1137
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
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...
Definition: StateSpace.cpp:855
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
A space representing discrete states; i.e. there are a small number of discrete states the system can...
Definition: DiscreteStateSpace.h:69
Number of state space types; To add new types, use values that are larger than the count...
Definition: StateSpaceTypes.h:75
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-...
Definition: StateSpace.cpp:998
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
std::vector< StateSpacePtr > components_
The state spaces that make up the compound state space.
Definition: StateSpace.h:707
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