Working with States and State Spaces

本教程大概意思应该是：如何使用状态与状态空间对象、以及两者之间的关系，状态空间是整个规划算法的描述基础，我觉得很重要。

1 Allocating memory for states(如何创建对象)

状态对象的内存分配或者说如何创建一个状态对象

1.1 The simple version

从官方提供的两个代码可以看出

1. StateSpace：是基本的状态空间对象
2. SpaceInformation：是在StateSpace的基础上进一步构建的(从名字可以看出是带有信息的状态空间，因此包含的内容最起码有StateSpace)
3. ScopedState：是一种状态，状态是来自状态空间的所以其在StateSpace的基础上构建的(从名字可以看出是一定范围内的某个状态，这个范围就是状态空间)
4. StateSpace的创建是通过动态分配内存实现的
5. SpaceInformation 与ScopedState 是其他方式构造的。
6. 模板参数 T：表示其他各种实际的状态空间类型，比如$R^3,SE(3),SO(3)$等等，从这里可以看出StateSpace 一定是其他派生的状态空间的基类。

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

或者使用

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

上面状态的产生会根据状态空间自动的分配对应的状态，除了使用构函数外也可以使用=和==这样的赋值操作。如果模板T被明确定义，那么会将实际状态空间表示的状态转换成模板指定的状态对象T::StateType，这个过程中对于=会使用StateSpace::copyState()，对于==会使用StateSpace::equalStates()

这段话描述了状态生成的机制，状态的产生最好使用状态空间进行，并且两者并不是完全对应的，可以通过模板参数进行转换。(可是从一个状态空间拿的状态就应该是对应的状态类型啊，所以感觉没啥应用场景啊。这里暂且直到吧)

1.2 The expert version

官方提供的代码，高级版的内存分配，从代码中可以看出：

1. 状态的生成取决于一个定义好的状态空间。
2. 状态的生成可以使用状态空间的allocState()方法来分配内存(很奇怪啊，状态与状态空间看起来明明是两个类对象，但是状态的生成却要由状态空间对象的方法提供)
3. 生成的状态具体是什么属性(free还是怎么样)也是由状态空间的提供的方法完成

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

下面解释了我上面的一个疑问(哎，自己在那瞎想，结果官方有解释)：

1. 状态是一个抽象基类，因此不能直接分配内存，而是要依据子类对象
2. 状态空间的产生成功的提供了这个子类对象信息
3. 因此使用状态空间对象创建状态对象
4. 使用SpaceInformation::allocState() 比StateSpace::allocState() 更好，因为后面的规划器(planner)使用的正是SpaceInformation对象进行建立的。

2 Working with states

2.1 Simple version

1. 使用状态的时候推荐使用ScopedState对象
2. ScopedState能够自动维护其状态，比如：该状态超过了状态空间范围
3. 可以使用模板参数来明确对应的状态类型，当没有模板参数的时候其内部维护的是一个ompl::base::State*

感觉T::StateType才是真实的状态对象，ScopedState是包含该对象的新对象，他能够自动维护 “是否在状态空间” 这个一个状态

下面代码基本上是描述了状态创建的几个情况：

1. 由构造函数创建
2. 由=创建
3. 由状态空间的allocState方法创建

// 状态空间对象
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
// 使用状态空间的 allocState创建状态
// 这里space是SE2StateSpace，而返回的对象是 State，不知道能不能写成 SE2StateSpace
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");

使用CompoundStateSpace 与 ScopedState的联合使用

// 创建一个复合空间对象，注意直接使用new的哦
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
// 用StateSpace包装这个对象，或者说创建一个新的指针只不过换了个名字
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.
// ScopedState的构造需要StateSpace对象，这里应该可以使用CompoundStateSpace对象吧
ompl::base::ScopedState<ompl::base::CompoundStateSpace> state(space);
// 把state看成是SO2StateSpace::StateType并对其第一个参数进行操作
state->as<ompl::base::SO2StateSpace::StateType>(0)->setIdentity();

使用运算符号构建复合空间并与ScopedState的联合使用:

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

从复合空间中提取想要的状态，比如：从SE(2)中提取位置信息

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

简单总结：

1. 复合空间的产生可以使用：
   1. 使用CompoundStateSpace对象构造
   2. 使用StateSpace与 运算操作构造
   3. 使用已经定义好的复合空间
2. 复合空间中提取状态、或操作：
   1. 先提取复合空间的复合状态，然后用as对复合状态提取或者操作
   2. 先创建所要的对象，然后用<<,>>操作来从复合状态进行提取

3. Operators for States and State Spaces

状态与状态空间的基本操作：

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

构造状态的使用引入状态空间的运算

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的X构造sXZW的X，而ZW没有构造
sX >> sXZW;
 
// the following line will initialize the X component of sXY with
// the X component of sXZW;
// sXY的X构造sXZW的X
sXY << sXZW;
 
// the following line will initialize both components of sZX, using
// the X and Z components of sXZW;
// sZX的Z,X构造sXZW的Z,W
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.
// 级联操作(^)将构造X+Y状态空间并产生对应状态
bool eq = (sX ^ sY) == sXY;