Allocating memory for states

The simple version

ompl::base::StateSpacePtr space(new T());

or

ompl::base::SpaceInformationPtr si(space);

The ompl::base::ScopedState class will do the necessary memory operations to allocate a state from the correct state space. This is the recommended way of allocating states for code other than ompl internals. Convenience operators such as = and == are provided. If a type T is provided, where T is a state space type, the maintained state is cast as T::StateType. operator= will use ompl::base::StateSpace::copyState() and operator== will use ompl::base::StateSpace::equalStates().

The expert version

ompl::base::SpaceInformationPtr si(space);
ompl::base::State* state = si->allocState();
...
si->freeState(state);

The structure of a state depends on the state space specification. The ompl::base::State type is just an abstract base for the states of other state spaces. For this reason, states cannot be allocated directly, but through the allocation mechanism of the state space: ompl::base::StateSpace::allocState(). States are to be freed using ompl::base::StateSpace::freeState(). For convenience, ompl::base::SpaceInformation::allocState() and ompl::base::SpaceInformation::freeState() are defined as well. Using the calls from the ompl::base::SpaceInformation class is better since they certainly use the same state space as the one used for planning. This is the lowest level of operating on states and only recommended for expert users.

See Working with states for how to fill the contents of the allocated states.

Working with states

In order for states to be useful in setting start (or goal) positions, accessing their content is needed. It is assumed the reader is familiar with Allocating memory for states. Furthermore, operators on states and state spaces are also used.

Simple version

The recommended use of states is with ompl::base::ScopedState. Given the instance of a state space, this class will allocate a state from that space. The internally maintained state is freed when the instance of ompl::base::ScopedState goes out of scope. ompl::base::ScopedState is a templated class and it inherits from T::StateType, where T is a state space type. This allows it to cast the state it maintains to the desired type and thus exhibit the functionality (the same members) as T::StateType. If no template argument is specified, the internal state is kept as ompl::base::State*.

ompl::base::StateSpacePtr space(new ompl::base::SE2StateSpace());
ompl::base::ScopedState<ompl::base::SE2StateSpace> state(space);
state->setX(0.1);
state->setY(0.2);
state->setYaw(0.0);
 
ompl::base::ScopedState<> backup = state;
// backup maintains its internal state as State*, so setX() is not available.
// the content of backup is copied from state
 
ompl::base::State *abstractState = space->allocState();
 
 
// this will copy the content of abstractState to state and
// cast it internally as ompl::base::SE2StateSpace::StateType
state = abstractState;
 
// restore state to it's original value
state = backup;
 
if (state != backup)
   throw ompl::Exception("This should never happen");

Combining ompl::base::ScopedState with ompl::base::CompoundStateSpace:

ompl::base::CompoundStateSpace *cs = new ompl::base::CompoundStateSpace();
cs->addSubspace(ompl::base::StateSpacePtr(new ompl::base::SO2StateSpace()), 1.0);
cs->addSubspace(ompl::base::StateSpacePtr(new ompl::base::SO3StateSpace()), 1.0);
 
// put the pointer to the state space in a shared pointer
ompl::base::StateSpacePtr space(cs);
 
// the ompl::base::ScopedState helps only with one cast here, since we still need to
// manually cast the components of the state to what we want them to be.
ompl::base::ScopedState<ompl::base::CompoundStateSpace> state(space);
state->as<ompl::base::SO2StateSpace::StateType>(0)->setIdentity();

The code above can be equivalently written as:

// define the individual state spaces
ompl::base::StateSpacePtr so2(new ompl::base::SO2StateSpace());
ompl::base::StateSpacePtr so3(new ompl::base::SO3StateSpace());
 
// construct a compound state space using the overloaded operator+
ompl::base::StateSpacePtr space = so2 + so3;
 
// the ompl::base::ScopedState helps only with one cast here, since we still need to
// manually cast the components of the state to what we want them to be.
ompl::base::ScopedState<ompl::base::CompoundStateSpace> state(space);
state->as<ompl::base::SO2StateSpace::StateType>(0)->setIdentity();

States can also be printed to streams:

ompl::base::ScopedState<> state(space);

std::cout << state;

Sometimes it may be useful to extract parts of a state, or assign only to some parts of a state, especially when using compound state spaces:

// an SE2 state space is in fact a compound state space consisting of R^2 and SO2
ompl::base::StateSpacePtr space(new ompl::base::SE2StateSpace());
// define a full state for this state space
ompl::base::ScopedState<ompl::base::SE2StateSpace> fullState(space);
// set the state to a random value
fullState.random();
 
// construct a state that corresponds to the position component of SE2
ompl::base::ScopedState<> pos(space->as<ompl::base::SE2StateSpace>()->getSubspace(0));
 
// copy the position
pos << fullState;
 
// equivalently, this can be done too:
fullState >> pos;
 
// if we now modify pos somehow, we can set it back in the full state:
pos >> fullState;

Expert version

For a state space type of type T, the result of ompl::base::StateSpace::allocState() can be casted to T::StateType* to gain access to the state's members. To ease this functionality, the ompl::base::State::as() functions have been defined.

ompl::base::StateSpacePtr space(new ompl::base::RealVectorStateSpace(1));
ompl::base::State *state = space->allocState();
state->as<ompl::base::RealVectorStateSpace::StateType>()->values[0] = 0.1;
ompl::base::State *copy = space->allocState();
space->copyState(copy, state);
if (!space->equalStates(copy, state))
   throw ompl::Exception("This should not happen");
space->freeState(state);
space->freeState(copy);

See Advanced methods for copying states for more information on copying states.

Operators for States and State Spaces

These operators are intended to simplify code that manipulates states and state spaces. They rely on the fact that state spaces have unique names. Here are some examples for using these operators:

// Assume X, Y, Z, W are state space instances, none of
// which inherits from ompl::base::CompoundStateSpace.
// Denote a compound state space as C[...], where "..." is the
// list of subspaces.
 
ompl::base::StateSpacePtr X;
ompl::base::StateSpacePtr Y;
ompl::base::StateSpacePtr Z;
ompl::base::StateSpacePtr W;
 
// the following line will construct a state space C1 = C[X, Y]
ompl::base::StateSpacePtr C1 = X + Y;
 
// the following line will construct a state space C2 = C[X, Y, Z]
ompl::base::StateSpacePtr C2 = C1 + Z;
 
// the following line will leave C2 as C[X, Y, Z]
ompl::base::StateSpacePtr C2 = C1 + C2;
 
// the following line will construct a state space C2 = C[X, Y, Z, W]
ompl::base::StateSpacePtr C2 = C2 + W;
 
// the following line will construct a state space C3 = C[X, Z, Y]
ompl::base::StateSpacePtr C3 = X + Z + Y;
 
// the following line will construct a state space C4 = C[Z, W]
ompl::base::StateSpacePtr C4 = C2 - C1;
 
// the following line will construct a state space C5 = W
ompl::base::StateSpacePtr C5 = C2 - C3;
 
// the following line will construct an empty state space C6 = C[]
ompl::base::StateSpacePtr C6 = X - X;
 
// the following line will construct a state space C7 = Y
ompl::base::StateSpacePtr C7 = Y + C6;

These state spaces can be used when operating with states:

ompl::base::ScopedState<> sX(X);
ompl::base::ScopedState<> sXY(X + Y);
ompl::base::ScopedState<> sY(Y);
ompl::base::ScopedState<> sZX(Z + X);
ompl::base::ScopedState<> sXZW(X + Z + W);
 
// the following line will copy the content of the state sX to
// the corresponding locations in sXZW. The components of the state
// corresponding to the Z and W state spaces are not touched
sX >> sXZW;
 
// the following line will initialize the X component of sXY with
// the X component of sXZW;
sXY << sXZW;
 
// the following line will initialize both components of sZX, using
// the X and Z components of sXZW;
sZX << sXZW;
 
// the following line compares the concatenation of states sX and sY with sXY
// the concatenation will automatically construct the state space X + Y and a state
// from that state space containing the information from sX and sY. Since sXY is
// constructed from the state space X + Y, the two are comparable.
bool eq = (sX ^ sY) == sXY;