OMPL官方教程学习Working with States and State Spaces
Working with States and State Spaces
本教程大概意思应该是：如何使用状态与状态空间对象、以及两者之间的关系，状态空间是整个规划算法的描述基础，我觉得很重要。
1 Allocating memory for states(如何创建对象)
状态对象的内存分配或者说如何创建一个状态对象
1.1 The simple version
从官方提供的两个代码可以看出
- StateSpace ：是基本的状态空间对象
- SpaceInformation ：是在StateSpace 的基础上进一步构建的(从名字可以看出是带有信息的状态空间，因此包含的内容最起码有StateSpace)
- ScopedState ：是一种状态，状态是来自状态空间的所以其在StateSpace 的基础上构建的(从名字可以看出是一定范围内的某个状态，这个范围就是状态空间)
- StateSpace 的创建是通过动态分配内存实现的
- SpaceInformation 与ScopedState 是其他方式构造的。
- 模板参数 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()
这段话描述了状态生成的机制，状态的产生最好使用状态空间进行，并且两者并不是完全对应的，可以通过模板参数进行转换。(可是从一个状态空间拿的状态就应该是对应的状态类型啊，所以感觉没啥应用场景啊。这里暂且直到吧)
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().
1.2 The expert version
官方提供的代码，高级版的内存分配，从代码中可以看出：
- 状态的生成取决于一个定义好的状态空间。
- 状态的生成可以使用状态空间的allocState()方法 来分配内存(很奇怪啊，状态与状态空间看起来明明是两个类对象，但是状态的生成却要由状态空间对象的方法提供)
- 生成的状态具体是什么属性(free还是怎么样)也是由状态空间的提供的方法 完成
ompl::base::SpaceInformationPtr si(space);
ompl::base::State* state = si->allocState();
...
si->freeState(state);下面解释了我上面的一个疑问(哎，自己在那瞎想，结果官方有解释)：
- 状态是一个抽象基类，因此不能直接分配内存，而是要依据子类对象
- 状态空间的产生成功的提供了这个子类对象信息
- 因此使用状态空间对象创建状态对象
- 使用SpaceInformation::allocState() 比StateSpace::allocState() 更好，因为后面的规划器(planner)使用的正是SpaceInformation 对象进行建立的。
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.
2 Working with states
2.1 Simple version
- 使用状态的时候推荐使用ScopedState 对象
- ScopedState 能够自动维护其状态，比如：该状态超过了状态空间范围
- 可以使用模板参数来明确对应的状态类型，当没有模板参数的时候其内部维护的是一个ompl::base::State *
感觉T::StateType才是真实的状态对象，ScopedState是包含该对象的新对象，他能够自动维护 “是否在状态空间” 这个一个状态
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*.
下面代码基本上是描述了状态创建的几个情况：
- 由构造函数创建
- 由
=创建 - 由状态空间的
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. 使用CompoundStateSpace 对象构造
2. 使用StateSpace 与 运算操作 构造
3. 使用已经定义好的复合空间 -
复合空间中提取状态、或操作：
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;