ompl::base::StateSpacePtr space(new T()); ompl::base::ScopedState<> state(space);
ompl::base::SpaceInformationPtr si(space); ompl::base::ScopedState<T> state(si);
ompl::base::SpaceInformationPtr si(space); ompl::base::State* state = si->allocState(); ... si->freeState(state);
See Operating with states for how to fill the contents of the allocated 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.
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 internall 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");
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();
// 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();
ompl::base::ScopedState<> state(space); std::cout << state;
// 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;
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);
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 an empty 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;