【OMPL】单刚体运动规划演示(2)

1198
0
2020年4月17日 16时39分

坦白说,这个OMPL系列实际上就是翻译官方教程。我以前也翻译过一些库的官方教程,总是开工后才发现网络上已经有前辈做过了,如ROS、Moveit!等中文博客资料多如牛毛,不再需要什么教程,于是就不怎么做这种重复工作了。最近学习OMPL,发现虽然这个库用途广泛,但有关的中文教程却少得不像话。考虑到运动规划也是一个不小的研究领域,针对小白的入门介绍还是有必要的。于是简单翻译了官网教程,结合了我的踩坑过程发出来,希望对大家有些许帮助。如果相关的其它资料,也希望大家分享学习。

 

1 回顾

上一篇文章中,我们介绍了OMPL的常用基础类。在图1中,类的颜色说明了其在运动规划程序中的必要性。在一个完整的运动规划程序中,有些类是必需实例化的,有些类是可选择的。那么该如何选用这些接口来编写运动规划程序呢?

 

可以看到,紫色的类是必需实例化的,浅蓝色的是可选的。还有一种深蓝色的类,一般来说也需要实例化,但OMPL提供了一种简化方法SimpleSetup,如果使用了该类,那么深蓝色的类就可不必实例化。

 

【OMPL】单刚体运动规划演示(2)插图

 

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算法只能找到可行解而非最优解;

【OMPL】单刚体运动规划演示(2)插图(1)

规划结果可视化

 

参考资料

[1] OMPL官方文档

发表评论

后才能评论