坦白说，这个OMPL系列实际上就是翻译官方教程。我以前也翻译过一些库的官方教程，总是开工后才发现网络上已经有前辈做过了，如ROS、Moveit！等中文博客资料多如牛毛，不再需要什么教程，于是就不怎么做这种重复工作了。最近学习OMPL，发现虽然这个库用途广泛，但有关的中文教程却少得不像话。考虑到运动规划也是一个不小的研究领域，针对小白的入门介绍还是有必要的。于是简单翻译了官网教程，结合了我的踩坑过程发出来，希望对大家有些许帮助。如果相关的其它资料，也希望大家分享学习。
1 回顾
上一篇文章中，我们介绍了OMPL的常用基础类。在图1中，类的颜色说明了其在运动规划程序中的必要性。在一个完整的运动规划程序中，有些类是必需实例化的，有些类是可选择的。那么该如何选用这些接口来编写运动规划程序呢？
可以看到，紫色的类是必需实例化的，浅蓝色的是可选的。还有一种深蓝色的类，一般来说也需要实例化，但OMPL提供了一种简化方法SimpleSetup，如果使用了该类，那么深蓝色的类就可不必实例化。
2 代码分析
下面我们通过一个简单的SE(3)单刚体的运动规划程序来说明OMPL库的用法。代码来自于示例程序ompl-1.4.2-Source/demos/RigidBodyPlanning.cp，也可见链接。分析如下：
2.1 状态有效性检查
需要特别说明的是，OPML库中本身不包含任何有效性检查（或者叫碰撞检测）功能，只提供了StateValidityChecker接口，供使用者自己实现或调用别的碰撞检测库。这是为了增强库的灵活性。
在这里，我们实现了一个伪碰撞检查，对于任何输入的状态state，返回的结果都是真（无任何碰撞）：
bool isStateValid(const ob::State *state)
{
// 将传入的状态转化到所需的状态空间中
const auto *se3state = state->as<ob::SE3StateSpace::StateType>();
// 提取状态的第一组值，即位置向量
const auto *pos = se3state->as<ob::RealVectorStateSpace::StateType>(0);
// 提取状态的第二组值，即姿态四元数
const auto *rot = se3state->as<ob::SO3StateSpace::StateType>(1);
// 检查位置和姿态的有效性
// 这是一个伪有效性检查，始终无碰撞。下面表达式只是为了同时调用rot和pos以防止编译警告
return (const void*)rot != (const void*)pos;
}
2.2 完整规划（不使用SimpleSetup）
plan()函数是运动规划程序的完整实现，建议对照图1理解。
1.首先定义状态空间StateSpace的类型，并设置空间的边界：
void plan()
{
// 创建一个状态SE(3)空间
auto space(std::make_shared<ob::SE3StateSpace>());
// 设置SE(3)空间中，位置的边界
ob::RealVectorBounds bounds(3);
bounds.setLow(-1);
bounds.setHigh(1);
space->setBounds(bounds);
2.接着实例化空间信息类SpaceInformation，其中包含状态空间和状态有效性检查器（由2.1步定义）
// 通过状态空间实例化SpaceInformation
auto si(std::make_shared<ob::SpaceInformation>(space));
// 设置状态有效性检查器StateValidityChecker
si->setStateValidityChecker(isStateValid);
3.接下来定义规划问题类ProblemDefinition，其中包含起始和目标状态：
// 创建一个随机的起始状态
ob::ScopedState<> start(space);
start.random()；
// 创建一个随机的目标状态
ob::ScopedState<> goal(space);
goal.random();
// 实例化规划问题
auto pdef(std::make_shared<ob::ProblemDefinition>(si));
// 将起始和目标状态定义到问题中
pdef->setStartAndGoalStates(start, goal);
4.下一步，定义规划器类Planner
// 通过状态空间创建规划器
auto planner(std::make_shared<og::RRTConnect>(si));
// 在规划器中设置需要解决的问题
planner->setProblemDefinition(pdef);
// 执行设置步骤
planner->setup();
5.最后，我们执行规划器。如果规划成功，就打印规划结果：
// 执行规划器以解决所设置的问题，其中规划时间设置为1.0秒
ob::PlannerStatus solved = planner->ob::Planner::solve(1.0);
if (solved)
{
// 获取规划结果
ob::PathPtr path = pdef->getSolutionPath();
std::cout << "Found solution:" << std::endl;
// 打印规划结果
path->print(std::cout);
}
else
std::cout << "No solution found" << std::endl;
}
2.3 简单规划（使用SimpleSetup）
planWithSimpleSetup()函数是运动规划的简化实现，同样建议对照图1理解。
如果使用简单规划方法SimpleSetup类，那么2.2节中的2-4步所实例化的类都可省略，替换为如下代码。可以看到，所有的信息都直接在SimpleSetup中进行设置了。
// 定义SimpleSetup类
og::SimpleSetup ss(space);
// 设置状态有效性检查器
ss.setStateValidityChecker([](const ob::State *state) { return isStateValid(state); });
// 创建并设置随机的起始和目标状态
ob::ScopedState<> start(space);
start.random();
ob::ScopedState<> goal(space);
goal.random();
ss.setStartAndGoalStates(start, goal);
// 执行规划以解决所设置的问题，其中规划时间设置为1.0秒
ob::PlannerStatus solved = ss.solve(1.0);
通过如下代码（以代替2.2节第5步）打印规划结果：
if (solved)
{
std::cout << "Found solution:" << std::endl;
ss.simplifySolution();
ss.getSolutionPath().print(std::cout);
}
else
std::cout << "No solution found" << std::endl;
}
3 规划结果分析
如果已经成功安装并编译了ompl库（如何安装？见【OMPL】安装与介绍），在ompl-1.4.2-Source/build/Release/bin/中将会看到大量示例可执行文件，在该目录下打开终端，运行：
./demo_RigidBodyPlanning
输出了许多信息，其中较重要的信息分析如下：
1.以下信息展示了第2.2-3步所随机生成的起始状态和目标状态
Start states:
Compound state [
RealVectorState [0.672119 0.0562133 -0.210078]
SO3State [0.705713 -0.70384 -0.00999784 -0.0804889]
]
Goal state, threshold = 2.22045e-16, memory address = 0x55ad319e6ce0, state =
Compound state [
RealVectorState [0.255424 0.466844 0.873504]
SO3State [0.0492216 -0.424305 0.108009 -0.897706]
]
2.以下信息说明了规划器的相关信息：采用了RRTConnect规划算法，即双向RRT算法（什么是RRT算法？其中起始端的树上得到了2个状态（包括起始状态），目标端的树上得到了4个状态：
Info: RRTConnect: Starting planning with 1 states already in datastructure
Info: RRTConnect: Created 6 states (2 start + 4 goal)
3.以下信息展示了运动规划的结果，其中位置用三维向量表示，姿态用四元数表示
Found solution:
Geometric path with 5 states
Compound state [
RealVectorState [0.672119 0.0562133 -0.210078]
SO3State [0.705713 -0.70384 -0.00999784 -0.0804889]
]
Compound state [
RealVectorState [0.226027 0.0796226 -0.286067]
SO3State [0.35916 -0.816641 -0.390001 -0.228036]
]
Compound state [
RealVectorState [0.22898 0.118522 -0.169579]
SO3State [0.340513 -0.811687 -0.350399 -0.320056]
]
Compound state [
RealVectorState [0.242202 0.292683 0.351962]
SO3State [0.217148 -0.688656 -0.135052 -0.678499]
]
Compound state [
RealVectorState [0.255424 0.466844 0.873504]
SO3State [0.0492216 -0.424305 0.108009 -0.897706]
]
将规划结果可视化（位置点连线，姿态用rgb参考系表示），可以看出：
1.路径明显分为两段（用蓝色和橙色标出），这是RRTConnect算法的双向性的体现；
2.由于我们使用了伪碰撞检测，也就是说空间中并不存在障碍。而路径却出现了转折，说明RRT算法只能找到可行解而非最优解；
规划结果可视化
参考资料
[1] OMPL官方文档
