以下内容为OMPL官方Tutorial Geometric Planning for a Rigid Body in 3D的翻译
本教程描述如何为一个3D空间中的刚体(rigid body)做规划。有两种方式:用/不用ompl::geometric::SimpleSetup 类。区别在于对后者而言ompl::base::SpaceInformation 和 ompl::base::ProblemDefinition需要被实例化。而且,planner也需要被明确地实例化。相比之下,更推荐用ompl::geometric::SimpleSetup,因为更不容易出现bug而且完全没有在功能层面施加任何限制。
为三维刚体配置几何规划器需要以下步骤:
- 明确规划的空间:SE(3)
- 从可行的状态空间中选择一个,或者自己设置一个。对于SE(3)而言,ompl::base::SE3StateSpace即可
- 由于SE(3)包含一个 R 3 R^3 R3部分,所以需要设定边界
- 定义可行状态
- 定义起点和终点表示
这些步骤完成后,对问题的定义基本就完成了。示例如下:
用ompl::geometric::SimpleSetup 类
// 设定命名空间
namespace ob = ompl::base;
namespace og = ompl::geometric;
// 声明状态可行检测函数
bool isStateValid(const ob::State *state);
void planWithSimpleSetup()
{
// 首先建立一个规划的状态空间
auto space(std::make_shared<ob::SE3StateSpace>());
// 设定三维边界
ob::RealVectorBounds bounds(3);
bounds.setLow(-1);
bounds.setHigh(1);
space->setBounds(bounds);
// 实例化一个SimpleSetup类,这个类的内部会实例化SpaceInfomation和ProblemDefinition
og::SimpleSetup ss(space);
// 设定状态可行检测(state validity checking)
ss.setStateValidityChecker([](const ob::State *state) { return isStateValid(state); });
// 设定一个随机的起始状态和目标状态,并放置到SimpleSetup中
ob::ScopedState<> start(space);
start.random();
ob::ScopedState<> goal(space);
goal.random();
ss.setStartAndGoalStates(start, goal);
// 接下来就可以尝试求解。这一命令会调用ompl::geometric::SimpleSetup::setup()并创建一个默认的规划器(因为没有指定),并且调用ompl::base::Planner::setup(),这个函数又调用ompl::base::SpaceInfomation::Setup()。这一系列调用会计算一些运行时参数,例如可行状态检测(state validity checking)的分辨率。然后从ompl::base::PlannerStatus返回一个描述“是否在给定时间(秒)内找到解”的值
ob::PlannerStatus solved = ss.solve(1.0);
// 如果找到解了,我们就可以选择simplify他然后输出他
if (solved)
{
std::cout << "Found solution:" << std::endl;
// print the path to screen
ss.simplifySolution();
ss.getSolutionPath().print(std::cout);
}
}
不用ompl::geometric::SimpleSetup 类
// 设定命名空间
namespace ob = ompl::base;
namespace og = ompl::geometric;
// 声明状态可行检测函数
bool isStateValid(const ob::State *state);
void plan()
{
// 首先建立一个规划的状态空间
auto space(std::make_shared<ob::SE3StateSpace>());
// 设定三维边界
ob::RealVectorBounds bounds(3);
bounds.setLow(-1);
bounds.setHigh(1);
space->setBounds(bounds);
// 为状态空间实例化一个ompl::base::SpaceInfomation类
auto si(std::make_shared<ob::SpaceInformation>(space));
// 设定状态可行检测(state validity checking)
si->setStateValidityChecker(isStateValid);
// 设定一个随机的起始状态和目标状态
ob::ScopedState<> start(space);
start.random();
ob::ScopedState<> goal(space);
goal.random();
// 创建一个ompl::base::ProblemDefinition实例对象,并设定其实和目标状态
auto pdef(std::make_shared<ob::ProblemDefinition>(si));
pdef->setStartAndGoalStates(start, goal);
// 创建一个规划器实例
auto planner(std::make_shared<og::RRTConnect>(si));
// 告诉规划器我们所要解决的问题
planner->setProblemDefinition(pdef);
// 确保按照上述顺序完成了所有的配置。接下来也是计算可行状态检测(state validity checking)的分辨率。
planner->setup();
// 接下来就可以尝试求解。这一命令会从ompl::base::PlannerStatus返回一个描述“是否在给定时间(秒)内找到解”的值
ob::PlannerStatus solved = planner->ob::Planner::solve(1.0);
// 如果找到解了,我们就可以选择simplify他然后输出他
if (solved)
{
// 从problem definition中获取target的表示,与goal stateButong,并且查询找到的路径
// and inquire about the found path
ob::PathPtr path = pdef->getSolutionPath();
std::cout << "Found solution:" << std::endl;
// 打印路径
path->print(std::cout);
}
}