OMPL 入门Tutorial 3：使用状态和状态空间(Working with States and State Spaces)

以下内容为OMPL官方Tutorial Working with States and State Spaces的翻译

为状态分配内存

简易版本：

ompl::base::StateSpacePtr space(new T());
ompl::base::ScopedState<> state(space);

或者

ompl::base::SpaceInformationPtr si(space);
ompl::base::ScopedState<T> state(si);

ompl::base::ScopedState类会做必需的内存操作来从正确的状态空间中分配一个状态。这是除了ompl内部方法之外的推荐的分配状态的方法。ompl提供了一些方便的运算符，如=和==。如果给定了state space类型T，那么维持的状态被转换为T::StateType。操作符"=“会用ompl::base::StateSpace::copyState()，操作符”=="会调用ompl::base::StateSpace::equalStates().

高阶版本

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

一个状态的结构取决于状态空间的定义。ompl::base::State类型只是一个为其他状态空间抽象的基类。因此，状态不能够被直接分配，而是需要通过状态空间的分配机制：ompl::base::StateSpace::allocState()。释放状态可以用ompl::base::SpaceInformation::freeState()。为了方便操作，也定义了 ompl::base::SpaceInformation::allocState() 和 ompl::base::SpaceInformation::freeState()。SpaceInformation这种方式更好，因为能确保是与规划用的状态空间相同。这是最底层的状态操作，只推荐高阶用户使用。

Working with states

为了让“状态”在设置起点终点的时候更好用，我们需要能够与他的内容交互。这里默认用户了解“为状态分配内存”和“状态与状态空间的操作”（即本篇另外两部分）

简易版本：

状态的推荐用法是用ompl::base::ScopedState。给定一个状态空间，这个类会从这个空间中分配一个状态。当离开这个类的作用域的时候，这个内部维护的状态也就被释放了，ompl::base::ScopedState是一个模板类，继承自T::StateType，其中T是一个状态空间类型。这允许他将维护的状态转换为一个期望的类型，从而展示出作为T::StateType的功能。如果缺省了模板参数，内部的状态被设为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);

// backup的内容从state拷贝而来，但这里backup的内部状态仍然是State*，因此setX()不能用。
ompl::base::ScopedState<> backup = state;

ompl::base::State *abstractState = space->allocState();

// 这会将abstractState的内容拷贝到state，并内部转换为ompl::base::SE2StateSpace::StateType
state = abstractState;
 
// 把state恢复到最初的状态
state = backup;
 
if (state != backup)
   throw ompl::Exception("This should never happen");

将 ompl::base::ScopedState 与 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);
 
// 将状态空间的指针放到智能指针里
ompl::base::StateSpacePtr space(cs);

// 这里仍然需要手动将状态的内容转换为我们期望的内容，所以其实ompl::base::ScopedState这里只内部转换了一次
ompl::base::ScopedState<ompl::base::CompoundStateSpace> state(space);
state->as<ompl::base::SO2StateSpace::StateType>(0)->setIdentity();

上述代码块等价于（其实就是演示了一下复合状态空间的不同构造方式）

// 定义单独的状态空间
ompl::base::StateSpacePtr so2(new ompl::base::SO2StateSpace());
ompl::base::StateSpacePtr so3(new ompl::base::SO3StateSpace());
 
// 用重载的+运算符建立一个复合状态空间
ompl::base::StateSpacePtr space = so2 + so3;
 
ompl::base::ScopedState<ompl::base::CompoundStateSpace> state(space);
state->as<ompl::base::SO2StateSpace::StateType>(0)->setIdentity();

状态也可以用streams打印出来

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

有时会需要提取部分状态，或者只给定部分状态，尤其是在使用复合状态空间时。

// 一个SE2 state space 其实是R^2 and SO2组成的复合状态空间
ompl::base::StateSpacePtr space(new ompl::base::SE2StateSpace());
// 为状态空间定义一个完整的状态
ompl::base::ScopedState<ompl::base::SE2StateSpace> fullState(space);
// 随即设定状态值
fullState.random();
 
// 建立一个状态对应于SE2的位置分量 
ompl::base::ScopedState<> pos(space->as<ompl::base::SE2StateSpace>()->getSubspace(0));
 
// 复制position
pos << fullState;
 
// 同样，也可以这么做
fullState >> pos;
 
// 如果我们修改了pos，也可以这样反向设置full state
pos >> fullState;

高阶版本：

对于一个T类型的状态空间类型，ompl::base::StateSpace::allocState()的结果可以被转换为T::StateType*来获取状态的内部成员变量。为了简化这个用法，定义了ompl::base::State::as()函数

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);

更多关于复制状态的信息见Advanced methods for copying states

状态与状态空间的操作

这些操作符是为了简化操作状态和状态空间而定义的。他们依赖于状态空间名字各不相同。下面是一些使用示例

// 假设 X, Y, Z, W 为状态空间实例，都不继承自ompl::base::CompoundStateSpace.
// 将复合状态空间表示为 C[...],"..."代表子空间
 
ompl::base::StateSpacePtr X;
ompl::base::StateSpacePtr Y;
ompl::base::StateSpacePtr Z;
ompl::base::StateSpacePtr W;
 
// C1 = C[X, Y]
ompl::base::StateSpacePtr C1 = X + Y;
// C2 = C[X, Y, Z]
ompl::base::StateSpacePtr C2 = C1 + Z;
// C2 = C[X, Y, Z]
ompl::base::StateSpacePtr C2 = C1 + C2;
// C2 = C[X, Y, Z, W]
ompl::base::StateSpacePtr C2 = C2 + W;
// C3 = C[X, Z, Y]
ompl::base::StateSpacePtr C3 = X + Z + Y;
// C4 = C[Z, W]
ompl::base::StateSpacePtr C4 = C2 - C1;
// C5 = W
ompl::base::StateSpacePtr C5 = C2 - C3;
// C6 = C[]
ompl::base::StateSpacePtr C6 = X - X;
// C7 = Y
ompl::base::StateSpacePtr C7 = Y + C6;

状态空间可以用于操作状态

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);
 
// 将状态sX的内容复制到sXZW对应的位置，而Z和W则不会被影响
sX >> sXZW;
 
// 将状态sXY的X初始化为sXZW的X
sXY << sXZW;
 
// 将状态sZX的X, Z初始化为sXZW的X, Z
sZX << sXZW;

// 对比sX和sY的联结(concatenation)与sXY对比。联结即通过X+Y构造的状态空间，其中的状态包含sX和sY的信息
bool eq = (sX ^ sY) == sXY;