OMPL 入门Tutorial 1:三维刚体的几何规划(Geometric Planning for a Rigid Body in 3D)

以下内容为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);
    }
}
  • 1
    点赞
  • 7
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值