ompl::base::StateManifoldPtr manifold(new T()); ompl::base::ScopedState<> state(manifold);
ompl::base::SpaceInformationPtr si(manifold); ompl::base::ScopedState<T> state(si);
ompl::base::SpaceInformationPtr si(manifold); 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 manifolds are also used.
ompl::base::StateManifoldPtr manifold(new ompl::base::SE2StateManifold()); ompl::base::ScopedState<ompl::base::SE2StateManifold> state(manifold); 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 = manifold->allocState(); // this will copy the content of abstractState to state and // cast it internall as ompl::base::SE2StateManifold::StateType state = abstractState; // restore state to it's original value state = backup; if (state != backup) throw ompl::Exception("This should never happen");
ompl::base::CompoundStateManifold *cm = new ompl::base::CompoundStateManifold(); cm->addSubManifold(ompl::base::StateManifoldPtr(new ompl::base::SO2StateManifold()), 1.0); cm->addSubManifold(ompl::base::StateManifoldPtr(new ompl::base::SO3StateManifold()), 1.0); // put the pointer to the manifold in a shared pointer ompl::base::StateManifoldPtr manifold(cm); // 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::CompoundManifold> state(manifold); state->as<ompl::base::SO2StateManifold::StateType>(0)->setIdentity();
// define the individual manifolds ompl::base::StateManifoldPtr so2(new ompl::base::SO2StateManifold()); ompl::base::StateManifoldPtr so3(new ompl::base::SO3StateManifold()); // construct a compound manifold using the overloaded operator+ ompl::base::StateManifoldPtr manifold = 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::CompoundManifold> state(manifold); state->as<ompl::base::SO2StateManifold::StateType>(0)->setIdentity();
ompl::base::ScopedState<> state(manifold); std::cout << state;
// an SE2 manifold is in fact a compound manifold consisting of R^2 and SO2 ompl::base::StateManifoldPtr manifold(new ompl::base::SE2StateManifold()); // define a full state for this manifold ompl::base::ScopedState<ompl::base::SE2StateManifold> fullState(manifold); // set the state to a random value fullState.random(); // construct a state that corresponds to the position component of SE2 ompl::base::ScopedState<> pos(manifold->as<ompl::base::SE2StateManifold>()->getSubManifold(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::StateManifoldPtr manifold(new ompl::base::RealVectorStateManifold(1)); ompl::base::State *state = manifold->allocState(); state->as<ompl::base::RealVectorStateManifold::StateType>()->values[0] = 0.1; ompl::base::State *copy = manifold->allocState(); manifold->copyState(copy, state); if (!manifold->equalStates(copy, state)) throw ompl::Exception("This should not happen"); manifold->freeState(state); manifold->freeState(copy);
These operators are intended to simplify code that manipulates states and manifolds. They rely on the fact that manifolds have unique names. Here are some examples for using these operators:
// Assume X, Y, Z, W are state manifold instances, none of // which inherits from ompl::base::CompoundStateManifold. // Denote a compound manifold as C[...], where "..." is the // list of submanifolds. ompl::base::StateManifoldPtr X; ompl::base::StateManifoldPtr Y; ompl::base::StateManifoldPtr Z; ompl::base::StateManifoldPtr W; // the following line will construct a manifold C1 = C[X, Y] ompl::base::StateManifoldPtr C1 = X + Y; // the following line will construct a manifold C2 = C[X, Y, Z] ompl::base::StateManifoldPtr C2 = C1 + Z; // the following line will leave C2 as C[X, Y, Z] ompl::base::StateManifoldPtr C2 = C1 + C2; // the following line will construct a manifold C2 = C[X, Y, Z, W] ompl::base::StateManifoldPtr C2 = C2 + W; // the following line will construct a manifold C3 = C[X, Z, Y] ompl::base::StateManifoldPtr C3 = X + Z + Y; // the following line will construct a manifold C4 = C[Z, W] ompl::base::StateManifoldPtr C4 = C2 - C1; // the following line will construct a manifold C5 = W ompl::base::StateManifoldPtr C5 = C2 - C3; // the following line will construct an empty manifold C6 = C[] ompl::base::StateManifoldPtr C6 = X - X; // the following line will construct an empty manifold C7 = Y ompl::base::StateManifoldPtr C7 = Y + C6;
These manifolds 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 manifolds 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 manifold X + Y and a state // from that manifold containing the information from sX and sY. Since sXY is // constructed from the manifold X + Y, the two are comparable. bool eq = (sX ^ sY) == sXY;