State space representing OpenDE states. More...
#include <ompl/extensions/opende/OpenDEStateSpace.h>

Classes | |
class | StateType |
OpenDE State. This is a compound state that allows accessing the properties of the bodies the state space is constructed for. More... | |
Public Types | |
enum | { STATE_COLLISION_KNOWN_BIT = 0, STATE_COLLISION_VALUE_BIT = 1, STATE_VALIDITY_KNOWN_BIT = 2, STATE_VALIDITY_VALUE_BIT = 3 } |
Public Member Functions | |
OpenDEStateSpace (const OpenDEEnvironmentPtr &env, double positionWeight=1.0, double linVelWeight=0.5, double angVelWeight=0.5, double orientationWeight=1.0) | |
Construct a state space representing OpenDE states. | |
const OpenDEEnvironmentPtr & | getEnvironment () const |
Get the OpenDE environment this state space corresponds to. | |
unsigned int | getNrBodies () const |
Get the number of bodies state is maintained for. | |
void | setDefaultBounds () |
By default, the volume bounds enclosing the geometry of the environment are computed to include all objects in the spaces collision checking is performed (env.collisionSpaces_). The linear and angular velocity bounds are set as -1 to 1 for each dimension. | |
void | setVolumeBounds (const base::RealVectorBounds &bounds) |
Set the bounds for each of the position subspaces. | |
void | setLinearVelocityBounds (const base::RealVectorBounds &bounds) |
Set the bounds for each of the linear velocity subspaces. | |
void | setAngularVelocityBounds (const base::RealVectorBounds &bounds) |
Set the bounds for each of the angular velocity subspaces. | |
virtual void | readState (base::State *state) const |
Read the parameters of the OpenDE bodies and store them in state. | |
virtual void | writeState (const base::State *state) const |
Set the parameters of the OpenDE bodies to be the ones read from state. The code will technically work if this function is called from multiple threads simultaneously, but the results are unpredictable. | |
bool | satisfiesBoundsExceptRotation (const StateType *state) const |
This is a convenience function provided for optimization purposes. It checks whether a state satisfies its bounds. Typically, in the process of simulation the rotations remain valid (very slightly out of bounds), so there is no point in updating or checking them. This function checks all other bounds (position, linear and agular velocities) | |
virtual base::State * | allocState () const |
Allocate a state that can store a point in the described space. | |
virtual void | freeState (base::State *state) const |
Free the memory of the allocated state. | |
virtual void | copyState (base::State *destination, const base::State *source) const |
Copy a state to another. The memory of source and destination should NOT overlap. | |
virtual void | interpolate (const base::State *from, const base::State *to, const double t, base::State *state) const |
Computes the state that lies at time t in [0, 1] on the segment that connects from state to to state. The memory location of state is not required to be different from the memory of either from or to. | |
virtual base::StateSamplerPtr | allocDefaultStateSampler () const |
Allocate an instance of the default uniform state sampler for this space. | |
virtual base::StateSamplerPtr | allocStateSampler () const |
Allocate an instance of the state sampler for this space. This sampler will be allocated with the sampler allocator that was previously specified by setStateSamplerAllocator() or, if no sampler allocator was specified, allocDefaultStateSampler() is called. | |
virtual bool | evaluateCollision (const base::State *source) const |
Fill the OpenDEStateSpace::STATE_COLLISION_VALUE_BIT of StateType::collision member of a state, if unspecified. Return the value value of that bit. | |
Protected Attributes | |
OpenDEEnvironmentPtr | env_ |
Representation of the OpenDE parameters OMPL needs to plan. |
Detailed Description
State space representing OpenDE states.
Definition at line 51 of file OpenDEStateSpace.h.
Member Enumeration Documentation
anonymous enum |
- Enumerator:
STATE_COLLISION_KNOWN_BIT Index of bit in StateType::collision indicating whether it is known if a state is in collision or not. Initially this is 0. The value of this bit is updated by OpenDEStateSpace::evaluateCollision() and OpenDEControlSpace::propagate().
STATE_COLLISION_VALUE_BIT Index of bit in StateType::collision indicating whether a state is in collision or not. Initially the value of this field is unspecified. The value gains meaning (1 or 0) when OpenDEStateSpace::STATE_COLLISION_KNOWN_BIT becomes 1. The value of this bit is updated by OpenDEStateSpace::evaluateCollision() and OpenDEControlSpace::propagate(). A value of 1 implies that there is no collision for which OpenDEEnvironment::isValidCollision() returns false.
STATE_VALIDITY_KNOWN_BIT Index of bit in StateType::collision indicating whether it is known if a state is in valid or not. Initially this is 0. The value of this bit is updated by OpenDEStateValidityChecker::isValid(). This bit is only used if the OpenDEStateValidityChecker is used.
STATE_VALIDITY_VALUE_BIT Index of bit in StateType::collision indicating whether a state is valid or not. Initially the value of this field is unspecified. The value gains meaning (1 or 0) when OpenDEStateSpace::STATE_VALIDITY_KNOWN_BIT becomes 1. The value of this bit is updated by OpenDEEnvironment::isValid(). A value of 1 implies that a state is valid. This bit is only used if the OpenDEStateValidityChecker is used.
Definition at line 55 of file OpenDEStateSpace.h.
Constructor & Destructor Documentation
ompl::control::OpenDEStateSpace::OpenDEStateSpace | ( | const OpenDEEnvironmentPtr & | env, |
double | positionWeight = 1.0 , |
||
double | linVelWeight = 0.5 , |
||
double | angVelWeight = 0.5 , |
||
double | orientationWeight = 1.0 |
||
) |
Construct a state space representing OpenDE states.
This will be a compound state space with 4 components for each body in env.stateBodies_. The 4 subspaces constructed for each body are: position (R3), linear velocity (R3), angular velocity (R3) and orientation (SO(3)). Default bounds are set by calling setDefaultBounds().
- Parameters:
-
env the environment to construct the state space for positionWeight the weight to pass to CompoundStateSpace::addSubspace() for position subspaces linVelWeight the weight to pass to CompoundStateSpace::addSubspace() for linear velocity subspaces angVelWeight the weight to pass to CompoundStateSpace::addSubspace() for angular velocity subspaces orientationWeight the weight to pass to CompoundStateSpace::addSubspace() for orientation subspaces
Definition at line 43 of file OpenDEStateSpace.cpp.
Member Function Documentation
void ompl::control::OpenDEStateSpace::copyState | ( | base::State * | destination, |
const base::State * | source | ||
) | const [virtual] |
Copy a state to another. The memory of source and destination should NOT overlap.
- Note:
- For more advanced state copying methods (partial copy, for example), see Advanced methods for copying states.
Reimplemented from base::CompoundStateSpace.
Definition at line 150 of file OpenDEStateSpace.cpp.
The documentation for this class was generated from the following files:
- ompl/extensions/opende/OpenDEStateSpace.h
- ompl/extensions/opende/src/OpenDEStateSpace.cpp