A、OMPL编译与安装

方式一：通过官网下载install-ompl-ubuntu.sh文件，运行后，自动编译安装 https://ompl.kavrakilab.org/installation.html，此方法在使用库文件时，cmakeconfig文件可能有问题，还是建议方法二。
方式二：通过Github下载源码，自己编译安装， https://github.com/ompl/ompl，
检查安装成功及 ROS中使用 https://blog.csdn.net/zghforever/article/details/106688410
官方教程 https://ompl.kavrakilab.org/tutorials.html

B、OMPL使用

B1、基本定义

求解方式
求解问题的过程中，根据是否使用ompl::geometric::SimpleSetup类，有两种方式，简单规划和完整规划，见代码部分C1。
求解过程：利用OMPL提供的类和方法来规划，基本过程包括以下几个部分
♥ 确认实际问题的状态空间（如SE(3)）
♥ 从OMPL提供的类中构建一个状态空间（如ompl::base::SE3StateSpace is appropriate）
♥ 设置状态空间的边界
♥ 定义状态有效性的判断方法
♥ 定义起点和终点的状态
♥ 求解和可视化
状态与状态空间

//状态与状态空间的基本定义ompl::base::StateSpacePtr space(new T());ompl::base::ScopedState<> state1(space);    //直接通过状态空间StateSpace初始化ompl::base::SpaceInformationPtr si(space);ompl::base::ScopedState<T> state2(si);       //通过SpaceInformation初始化
//举个栗子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; //state == backup, backup本质是State*, 作用是备份参数， 因此setX()等函数无效
//状态空间的合并//合并方式1ompl::base::CompoundStateSpace *cs = new ompl::base::CompoundStateSpace(); //利用CompoundStateSpace类来添加不同状态空间cs->addSubspace(ompl::base::StateSpacePtr(new ompl::base::SO2StateSpace()), 1.0);cs->addSubspace(ompl::base::StateSpacePtr(new ompl::base::SO3StateSpace()), 1.0);ompl::base::StateSpacePtr space(cs);//合并方式2// define the individual state spacesompl::base::StateSpacePtr so2(new ompl::base::SO2StateSpace());ompl::base::StateSpacePtr so3(new ompl::base::SO3StateSpace());ompl::base::StateSpacePtr space = so2 + so3;//虽然空间可以混合，但对于状态仍然需要指定具体的类型ompl::base::ScopedState<ompl::base::CompoundStateSpace> state(space);state->as<ompl::base::SO2StateSpace::StateType>(0)->setIdentity();

有效性检查
OMPL不提供具体检查方式，需要根据具体的问题具体确定。用户指定有效性检查的方式有两种，见代码部分C2
♥ 继承OMPL定义的两个抽象类
♥ 直接定义有效性检查函数

// 方法2：直接定义判断函数
bool myStateValidityCheckerFunction(const base::State *state)
{return ...;
}
//方法2：使用
base::SpaceInformationPtr si(space);
si->setStateValidityChecker(myStateValidityCheckerFunction);
si->setStateValidityCheckingResolution(0.03); // 3%
si->setup();

采样
采样的两种方式：基于ompl::base::StateSampler和基于ompl::base::ValidStateSampler。
♥ 基于ompl::base::StateSampler：生成状态空间的一个状态，生成状态附近的一个状态，生成高斯分布的一组状态。
♥ 基于ompl::base::ValidStateSampler：以ompl::base::StateSampler为基础，不断采样，直到采到一个有效状态或者到达迭代最大值，有效性通过ompl::base::SpaceInformation::isValid判断。
♥ OMPL提供了几个继承自ompl::base::ValidStateSampler的类，如下，可以使用这些类或继承ompl::base::ValidStateSampler重新编写采样器，基本使用方法如下，具体代码与解释见代码部分C3。


ompl::base::UniformValidStateSampler	默认采样器
ompl::base::ObstacleBasedValidStateSampler	采样一个有效状态和一个无效状态，在两者之间插值，返回最接近无效状态的一个有效的插值状态，即逼近障碍物
ompl::base::GaussianValidStateSampler	采样一对状态，第一个状态均匀随机采集，第二个状态在以第一个状态为中心的高斯分布上采集，两状态都有效或都无效，则再取一对，否则返回有效状态，也在逼近障碍物
ompl::base::MaximizeClearanceValidStateSampler	在默认采样器的基础上，出于安全考虑，最大化路径间隙，使得状态与障碍物之间的间隙最大化

//基本使用方式
//***************************定义状态分配器（函数）***************************
ompl::base::ValidStateSamplerPtr allocOBValidStateSampler(const ompl::base::SpaceInformation *si)
{// we can perform any additional setup / configuration of a sampler here,// but there is nothing to tweak in case of the ObstacleBasedValidStateSampler.return std::make_shared<ompl::base::ObstacleBasedValidStateSampler>(si);
}
//***************************使用***************************
ss.getSpaceInformation()->setValidStateSamplerAllocator(allocOBValidStateSampler);

B2、路径可视化

OMPL不提供路径可视化工具，直接一点的方式是作为矩阵打印输出，保存成文本文件。
OMPL提供两种路径：ompl::geometric::PathGeometric 和 ompl::control::PathControl，两个类可用成员函数printAsMatrix()打印，前者每行打印一个状态，后者每行除了状态，还有控制指令和控制间隔时间。
打印方式如下：

bool solved = ss.solve(20.0);
if (solved)
{// if ss is a ompl::geometric::SimpleSetup objectss.getSolutionPath().printAsMatrix(std::cout);// if ss is a ompl::control::SimpleSetup objectss.getSolutionPath().asGeometric().printAsMatrix(std::cout);
}

B3、API Overview

C、代码附录

C1、Geometric Planning for a Rigid Body in 3D

//注：这是在ROS里面写的，虽然这一部分跟ROS暂时没关系。
#include <ros/ros.h>
#include <iostream>#include <ompl/base/spaces/SE3StateSpace.h>//使用ompl::geometric::SimpleSetup类
#include <ompl/geometric/SimpleSetup.h>
//不使用ompl::geometric::SimpleSetup类
#include <ompl/base/SpaceInformation.h>
#include <ompl/geometric/planners/rrt/RRTConnect.h>namespace ob = ompl::base;
namespace og = ompl::geometric;//状态检查函数
bool isStateValid(const ob::State *state)
{// cast the abstract state type to the type we expect// 将抽象状态类型转换为我们期望的类型const auto *se3state = state->as<ob::SE3StateSpace::StateType>();// extract the first component of the state and cast it to what we expect // 提取状态的第一个组件并将其转换为我们所期望的const auto *pos = se3state->as<ob::RealVectorStateSpace::StateType>(0);// extract the second component of the state and cast it to what we expect// 提取状态的第二个组件并将其转换为我们所期望的const auto *rot = se3state->as<ob::SO3StateSpace::StateType>(1);// check validity of state defined by pos & rot// return a value that is always true but uses the two variables we define, so we avoid compiler warnings// 返回一个始终为true但使用我们定义的两个变量的值return (const void*)rot != (const void*)pos;
}//简单规划：使用ompl::geometric::SimpleSetup类
void planWithSimpleSetup()
{// 状态空间：构建auto space(std::make_shared<ob::SE3StateSpace>());// 状态空间：边界ob::RealVectorBounds bounds(3);bounds.setLow(-1);bounds.setHigh(1);space->setBounds(bounds);// 配置类：构建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);// this call is optional, but we put it in to get more output informationss.setup();ss.print();// attempt to solve the problem within one second of planning timeob::PlannerStatus solved = ss.solve(1.0);if (solved){std::cout << "Found solution:" << std::endl;// print the path to screenss.simplifySolution();ss.getSolutionPath().print(std::cout);}elsestd::cout << "No solution found" << std::endl;
}//完整规划：不使用ompl::geometric::SimpleSetup类
void plan()
{// 状态空间：构建auto space(std::make_shared<ob::SE3StateSpace>());// 状态空间：边界ob::RealVectorBounds bounds(3);bounds.setLow(-1);bounds.setHigh(1);space->setBounds(bounds);// 空间信息：构建auto si(std::make_shared<ob::SpaceInformation>(space));// 空间信息：设置状态有效性检查器si->setStateValidityChecker(isStateValid);// 问题实例：构建auto pdef(std::make_shared<ob::ProblemDefinition>(si));// 问题实例：设置起终点ob::ScopedState<> start(space);start.random();ob::ScopedState<> goal(space);goal.random();pdef->setStartAndGoalStates(start, goal);// 规划方法：构建auto planner(std::make_shared<og::RRTConnect>(si));// 规划方法：实际问题planner->setProblemDefinition(pdef);// 规划方法：初始化planner->setup();// 打印空间信息和问题实例si->printSettings(std::cout);pdef->print(std::cout);// 求解ob::PlannerStatus solved = planner->ob::Planner::solve(1.0);if (solved){// get the goal representation from the problem definition (not the same as the goal state)// and inquire about the found pathob::PathPtr path = pdef->getSolutionPath();std::cout << "Found solution:" << std::endl;// print the path to screenpath->print(std::cout);}elsestd::cout << "No solution found" << std::endl;
}int main(int argc, char** argv)
{ros::init(argc, argv, "rigid_body_planning");ros::NodeHandle nh;std::cout << "OMPL version: " << OMPL_VERSION << std::endl;plan();std::cout << std::endl << std::endl;planWithSimpleSetup(); return 0;
}

C2、有效性检查方式

//方法1：继承OMPL定义的抽象类 base::StateValidityChecker
class myStateValidityCheckerClass : public base::StateValidityChecker
{public:myStateValidityCheckerClass(const base::SpaceInformationPtr &si) :base::StateValidityChecker(si){}virtual bool isValid(const base::State *state) const{return ...;}
};
//方法1：使用
base::SpaceInformationPtr si(space);
si->setStateValidityChecker(std::make_shared<myStateValidityCheckerClass>(si));
si->setup();//方法1：继承OMPL定义的抽象类 base::MotionValidator
class myMotionValidator : public base::MotionValidator
{public:// implement checkMotion()
};
//方法1：使用
base::SpaceInformationPtr si(space);
si->setMotionValidator(std::make_shared<myMotionValidator>(si));
si->setup();// 方法2：直接定义判断函数
bool myStateValidityCheckerFunction(const base::State *state)
{return ...;
}
//方法2：使用
base::SpaceInformationPtr si(space);
si->setStateValidityChecker(myStateValidityCheckerFunction);
si->setStateValidityCheckingResolution(0.03); // 3%
si->setup();

C3、采样方式

1 已有采样器

不能直接在SimpleSetup或者SpaceInformation类中配置采样方式，需要通过定义下面类型的函数，输入ompl::base::SpaceInformation，返回ompl::base::ValidStateSamplerPtr。

//***************************定义状态分配器（函数）***************************
ompl::base::ValidStateSamplerPtr allocOBValidStateSampler(const ompl::base::SpaceInformation *si)
{// we can perform any additional setup / configuration of a sampler here,// but there is nothing to tweak in case of the ObstacleBasedValidStateSampler.return std::make_shared<ompl::base::ObstacleBasedValidStateSampler>(si);
}
//***************************使用***************************
ss.getSpaceInformation()->setValidStateSamplerAllocator(allocOBValidStateSampler);

2 自定义采样器

继承ompl::base::ValidStateSampler重新编写采样器，继承后的类与OMPL提供的类的使用方法相同。

//***************************定义子类***************************
namespace ob = ompl::base;
namespace og = ompl::geometric;// This is a problem-specific sampler that automatically generates valid
// states; it doesn't need to call SpaceInformation::isValid. This is an
// example of constrained sampling. If you can explicitly describe the set valid
// states and can draw samples from it, then this is typically much more
// efficient than generating random samples from the entire state space and
// checking for validity.
class MyValidStateSampler : public ob::ValidStateSampler
{public:MyValidStateSampler(const ob::SpaceInformation *si) : ValidStateSampler(si){name_ = "my sampler";}// Generate a sample in the valid part of the R^3 state space// Valid states satisfy the following constraints:// -1<= x,y,z <=1// if .25 <= z <= .5, then |x|>.8 and |y|>.8bool sample(ob::State *state) override{double* val = static_cast<ob::RealVectorStateSpace::StateType*>(state)->values;double z = rng_.uniformReal(-1,1);if (z>.25 && z<.5){double x = rng_.uniformReal(0,1.8), y = rng_.uniformReal(0,.2);switch(rng_.uniformInt(0,3)){case 0: val[0]=x-1;  val[1]=y-1;  break;case 1: val[0]=x-.8; val[1]=y+.8; break;case 2: val[0]=y-1;  val[1]=x-1;  break;case 3: val[0]=y+.8; val[1]=x-.8; break;}}else{val[0] = rng_.uniformReal(-1,1);val[1] = rng_.uniformReal(-1,1);}val[2] = z;assert(si_->isValid(state));return true;}// We don't need this in the example below.bool sampleNear(ob::State* /*state*/, const ob::State* /*near*/, const double /*distance*/) override{throw ompl::Exception("MyValidStateSampler::sampleNear", "not implemented");return false;}
protected:ompl::RNG rng_;
};
//***************************定义状态分配器（函数）***************************
ob::ValidStateSamplerPtr allocMyValidStateSampler(const ob::SpaceInformation *si)
{return std::make_shared<MyValidStateSampler>(si);
}
//***************************使用***************************
ss.getSpaceInformation()->setValidStateSamplerAllocator(allocMyValidStateSampler);

运动规划学习笔记4——探索OMPL相关推荐

深蓝学院-机器人运动规划学习笔记-第一章
第一课移动机器人运动规划 Motion planning for mobile robots Introduction Course outline Typical planning methods ...
ROS运动规划学习六---dwa_local_planner
文章目录前言一.dwa_local_planner结构二.setPlan.initialize.isGoalReached 三.computeVelocityCommands() 总结前言在 ...
ROS运动规划学习三---move_base
文章目录前言一.move_base工程结构二.move_base简要分析 1.MoveBase类的声明---move_base.h 2.MoveBase类的实现---move_base.cpp ...
最优控制和轨迹规划学习笔记包含多个实际案例主要思路是使用优化算法来找到车辆的最佳路径
最优控制和轨迹规划学习笔记包含多个实际案例倒立摆上翻控制满足车辆运动学约束的路径规划离散点参考线优化 lattice横向距离规划这段代码包含了三个程序,我们将分别对它们进行详细的分析. 最速 ...
rk3288 img打包工具_【个人开源】机器人运动规划学习工具箱使用说明
最近的课题与机器人的运动规划有关.回顾过去的学习经历,深感机械出身的我们编程实践与总结能力实在太弱,以前的一些课题就拿matlab随便写一写m文件出个结果了事,许多后来发现有用的课程或作业,要么找不到 ...
TWAIN学习笔记006 探索TWAIN之DS连接及扫描
前一篇中我们已经成功连接了DSM并找到了所有可用的DS.本文中我们试着连接其中一个DS并完成一次扫描. 第一步,选择并连接一个DS,将TWAIN状态由3转到4. 注:本文选用了测试用的DS - TWA ...
Java学习笔记：探索yzk18-commons库
文件读写等类 1.乱码:用A标准保存文件,用B标准读取文件. 2.帖子:用记事本写一个文本文件,后缀名改为exe,有没有可能造出一个能运行的程序出来. 1.培训机构就是想让你们没有自学能力. 2.尽量 ...
ROS运动规划学习五---global_planner
文章目录前言一.global_planner功能包结构二.planner_core 1.执行过程 2. calculatePotentials() 3. getPlanFromPotential ...
深蓝学院-运动规划重点笔记
文章目录基于图搜索的方法 Dijkstra A* JPS(Jump Point Search) 基于采样的方法概率路图 RRT RRT* Kinodynamic-RRT* Anytime-RRT* ...

运动规划学习笔记4——探索OMPL

运动规划学习笔记4——探索OMPL

A、OMPL编译与安装

B、OMPL使用

B1、基本定义

B2、路径可视化

B3、API Overview

C、代码附录

C1、Geometric Planning for a Rigid Body in 3D

C2、有效性检查方式

C3、采样方式

1 已有采样器

2 自定义采样器

运动规划学习笔记4——探索OMPL相关推荐

最新文章

热门文章