OMPL 学习笔记

为3D的钢体结构进行几何规划

构建刚体的几何规划有两种方式，可以先定义一个ompl::geometric::SimpleSetup，也可以不定义它，但在后续的ompl::base::SpaceInformation 和 ompl::base::ProblemDefinition传入一些初始化的值。

官方说，给刚体规划几何路径需要有以下几个步骤：

1. 确定一个规划空间，即SE(3)
2. 选择一个可用的相关状态，或编写一个相关状态，也是一个SE(3)，可用ompl::base::SE3StateSpace。
3. 由于SE(3)具备旋转量，所以我们需要定义它的边界
4. 定义有效的状态
5. 确定起始点和目标点的表达

具体做法是：

namespace ob = ompl::base;
namespace og = ompl::geometric;

然后定义一个验证状态有效性的方法：

bool isStateValid(const ob::State *state)

然后定义一个空间实例（使用ompl::geometric::SimpleSetup的版本）：

void planWithSimpleSetup()
{
    // construct the state space we are planning in
    auto space(std::make_shared<ob::SE3StateSpace>());
    
    // 设置上下限
    ob::RealVectorBounds bounds(3);
    bounds.setLow(-1);
    bounds.setHigh(1);
    space->setBounds(bounds);

    // 调用 SimpleSetup,这个方法会在内部声明ompl::base::SpaceInformation和 ompl::base::ProblemDefinition
    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);

    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);
    }
}

定义一个空间实例（不使用ompl::geometric::SimpleSetup的版本）：

void planWithSimpleSetup()
{
    // construct the state space we are planning in
    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);

    // 声明一个随机的初始状态
    ob::ScopedState<> start(space);
    start.random();

    // 声明一个随机的结束状态
    ob::ScopedState<> goal(space);
    goal.random();

    auto pdef(std::make_shared<ob::ProblemDefinition>(si));
    
    planner->setProblemDefinition(pdef);

    planner->setup();

    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);
    }
}

给机器人关节空间进行OMPL规划

关节空间的OMPL规划是一个简单问题，因为它还不涉及周围的物体状态和碰撞，只是拿来做演示和测试用的。

在关节空间下，一般可以通过以下几个步骤来规划路径：

1. 确定空间类型。OMPL里面有一些已经实现过的空间类型，都在ompl/base/spaces/下，关节空间由于是若干个关节值，所以可以用RealVectorStateSpace来作为规划的空间。
2. 确定可行范围并设置。一般关节是具备运动范围的，比如每根轴都有自己的旋转范围，2、3轴可能会存在一个干涉范围，这些干涉都要加入规划。具体来说就是写一个函数，接收ompl::base::state(OMPL库在求解过程中用到的类型),然后我们要自己把这个state转回关节值，再做关节判断，如超行程、干涉等。
3. 定义问题和空间信息。这一部分官方说用og::SimpleSetup就行，也可以分两步，先声明ob::SpaceInformation,再定义一个ob::ProblemDefinition也行。

• 给空间信息添加规划器。上一步用到的ob::SpaceInformation可以设置单独的规划器，这些规划器就是RRT\RRT*之类的，先给规划器设置一些基本参数，如规划器类型和采样步长等，然后将其设置到ob::SpaceInformation就行了。

4. 执行路径规划。
5. 获取规划结果。

总的伪代码结果如下：

bool PathPlanner::isStateValid(const ob::State* state)
{
	const ob::RealVectorStateSpace::StateType* state2D =
		state->as<ob::RealVectorStateSpace::StateType>();

    // 在这可以把关节值拿出来一一校验，也可以通过外部的库做碰撞检测来确定该state是否可用。
	return true;
}

std::vector<std::vector<double>> path_points; // 存储路径点的容器

// 创建小数型数组的状态空间
std::shared_ptr<ob::RealVectorStateSpace> space(std::make_shared<ob::RealVectorStateSpace>(DOF));

// 设置空间的范围
ob::RealVectorBounds bounds(DOF);
bounds.setLow(0, jogRange["J1Min"]);
bounds.setHigh(0, jogRange["J1Max"]);
bounds.setLow(1, jogRange["J2Min"]);
bounds.setHigh(1, jogRange["J2Max"]);
bounds.setLow(2, jogRange["J3Min"]);
bounds.setHigh(2, jogRange["J3Max"]);
bounds.setLow(3, jogRange["J4Min"]);
bounds.setHigh(3, jogRange["J4Max"]);
bounds.setLow(4, jogRange["J5Min"]);
bounds.setHigh(4, jogRange["J5Max"]);
bounds.setLow(5, jogRange["J6Min"]);
bounds.setHigh(5, jogRange["J6Max"]);
space->setBounds(bounds);

// 创建空间信息对象
og::SimpleSetup ss(space);

ss.setStateValidityChecker([this](const ob::State* state) { return isStateValid(state); });

ob::ScopedState<ob::RealVectorStateSpace> start(space);
start[0] = 0;
start[1] = 0;
start[2] = 0;
start[3] = 0;
start[4] = -90;
start[5] = 0;

ob::ScopedState<ob::RealVectorStateSpace> end(space);
end[0] = 90;
end[1] = 0;
end[2] = 0;
end[3] = 0;
end[4] = -90;
end[5] = 0;
    
ss.setStartAndGoalStates(start, end);
auto planner(std::make_shared<og::RRT>(ss.getSpaceInformation()));
planner->setRange(0.5);
ss.setPlanner(planner);


ob::PlannerStatus solved = ss.solve(1);

if (solved)
{
    // 获取解决方案路径
    og::PathGeometric path = ss.getSolutionPath(); 
    //ss.simplifySolution();
    path.interpolate();

    // 提取路径中的所有状态点
    std::vector<ob::State*> states = path.getStates();

    // 将每个状态转换为关节角度向量
    for (ob::State* state : states) {
        const auto* real_state = state->as<ob::RealVectorStateSpace::StateType>();
        std::vector<double> joint_angles;

        for (unsigned int i = 0; i < DOF; ++i) {
            joint_angles.push_back(real_state->values[i]);
        }
        path_points.push_back(joint_angles);
    }

    // 打印路径信息，由于我没有添加障碍物，路径在关节空间是一条直线
    std::cout << "Path has " << path_points.size() << " waypoints" << std::endl;
    path.print(std::cout);
}

上述这个写法和实际相差还是比较远的，因为是关节空间不好表示障碍物的描述，除非提前把障碍我空间内的关节通过逆解算出来，或者直接用碰撞检测来判断是否碰撞，这就需要很多额外的信息了。

OMPL官方还提供了ODE的控制规划，比如二轮差速小车。这部分我的理解是这样的：

Q1: 什么是基于控制的规划？它和普通的几何路径规划有什么区别

A1:​​ 核心区别在于“规划什么”和“如何保证可行性”？
​​几何路径规划​​：规划对象是机器人的​​状态​​（如位置XYZ、关节角度）。它假设机器人可以在状态空间中“瞬移”，只关心找到一条无碰撞的几何路径，不关心机器人怎么走过去。好比在地图上画一条忽略车辆物理规律的理想连线。​基于控制的规划​​：规划对象是​​控制指令​​（如速度、转向角）。它通过机器人的​​动力学模型（微分方程）​​ 来模拟“施加某个控制指令后，机器人会怎么运动”，从而生成路径。这条路经的每一步都严格遵循物理规律。好比制定详细的驾驶指令（方向盘打多少、油门踩多久），让真实车辆沿着可行轨迹行驶。

​​Q2: ODESolver（常微分方程求解器）在其中扮演什么角色？​​

​​A2:​​ ODESolver是​​状态传播​​的核心执行器。它的任务是：当规划器采样到一个控制指令（如“左轮1m/s，右轮0.5m/s，持续0.1秒”）后，ODESolver会根据您提供的机器人微分方程模型，通过​​数值积分​​计算出执行该指令后机器人的​​新状态​​。
​​输入​​：当前状态 + 控制指令 + 持续时间。
​​输出​​：新的状态。
​​作用​​：相当于一个高保真的“物理模拟器”，确保规划器探索的每一步运动都是物理上可实现的。

Q3: 它是如何解决像“差速小车不能横向移动”这类动力学约束的？​​

​​A3:​​ 它不是事后“修正”或“替代”非法移动，而是​​从根本上杜绝​​其产生。
因为ODESolver提供的微分方程本身就​​没有描述横向移动的项​​。规划器通过ODESolver进行状态传播时，​​根本不会生成​​包含横向移动的新状态。
规划器探索的整个状态空间，都被限制在由动力学方程定义的“可行流形”上。那些需要违反约束才能到达的状态（如瞬时侧移），从一开始就不会在探索树中出现。

​Q4: 规划器采样控制指令，如果采样到“让小车朝反方向走”这种物理可行但逻辑错误的指令怎么办？​​

​​A4:​​ 规划器不是盲目接受所有采样结果，它有一个​​评估和选择机制​​。
​​采样​​：随机采样一个控制指令。
​​传播​​：通过ODESolver得到新状态。
​​评估​​：计算新状态与目标状态的“代价”（如距离）。让小车离目标更远的指令，代价就高。
​​选择​​：规划器会倾向于选择并保留那些​​代价更低​​（即更接近目标）的轨迹分支，而抛弃或较少探索代价高的分支。
通过这种“优胜劣汰”的机制，即使采样空间包含一些“傻”指令，规划器也能逐渐收敛到一条有效的路径。
​

​Q5: 所以，ODE是直接对控制命令做采样的，而不是几何路径，对吧?

​​A5: 这是理解基于控制规划的关键。
​​几何规划器​​：直接在​​状态空间​​（如XYθ坐标）采样，然后尝试用几何线连接。
​​基于控制的规划器​​：直接在​​控制空间​​（如速度v和角速度ω）采样，然后通过​​ODE求解器​​将控制指令“翻译”成状态空间中的一段​​轨迹​​。规划的本质从“找点连线”变成了“通过试驾来找出一条能开过去的路线”。
​

官方这里也给了很多Demo

后续的想法

机器人关节空间规划和末端执行器的规划是不一样的概念，关节空间规划虽然好控制速度加速、以及转角，但这样不适合控制末端执行器的稳定性，可能插值出来的结果是抖动的。如果要做末端执行器的连续控制，需要编写一个底层的机器人运动学解析库，配合Mujoco的物理仿真，在OMPL做采样的时候每次都对采样结果做验证，直接在笛卡尔空间采样出一条不干涉、可达且末端连续的路径。