运动规划学习笔记1——探索OMPL
A、OMPL编译与安装
B、OMPL使用
B1、基本定义
B2、路径可视化
B3、API Overview
C、代码附录
C1、Geometric Planning for a Rigid Body in 3D
C2、有效性检查方式
C3、采样方式
1 已有采样器
2 自定义采样器
在这里分享了运动规划方面的一些知识，属于《路径规划算法学习笔记》系列的扩展系列，其中内容可能存在不完善和错误之处，如有读者发现，欢迎批评指正。
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()等函数无效
//状态空间的合并
	//合并方式1
	ompl::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 spaces
	ompl::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();
  1. 有效性检查
    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();
  2. 采样
    采样的两种方式：基于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。
  3. //基本使用方式
//***************************定义状态分配器（函数）***************************
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);
  4. 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 object
    ss.getSolutionPath().printAsMatrix(std::cout);
 
    // if ss is a ompl::control::SimpleSetup object
    ss.getSolutionPath().asGeometric().printAsMatrix(std::cout);
}
    B3、API Overview
  5. 在这里插入图片描述
  6. C、代码附录C1、Geometric Planning for a Rigid Body in 3D
  7. //注：这是在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 information
	ss.setup();
	ss.print();

	// attempt to solve the problem within one second of planning time
	ob::PlannerStatus solved = ss.solve(1.0);

	if (solved)
	{
		std::cout << "Found solution:" << std::endl;
		// print the path to screen
		ss.simplifySolution();
		ss.getSolutionPath().print(std::cout);
	}
	else
		std::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 path
		ob::PathPtr path = pdef->getSolutionPath();
		std::cout << "Found solution:" << std::endl;

		// print the path to screen
		path->print(std::cout);
	}
	else
		std::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;
}
  8. C2、有效性检查方式
  9. //方法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();
  10. 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);

  11. 2 自定义采样器

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

  12. //***************************定义子类***************************
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|>.8
    bool 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);