【MoveIt】通过Plugin机制在MoveIt中集成算法

---

参考

• 古月居
• Plugin Interfaces
• Concepts
• ompl-moveit_interface
• ompl-ros_wiki
• moveit_tutorials_github官方代码—以线性插补算法为例
• ompl_ros

---

之前看到有通过安装源码的方式来集成自己的算法，其实大可不必，因为官方已经提供了插件的机制来实现自己的算法。可以在官方网站看到

我们就是要对planning interface进行操作。同时我们可以看到运动规划和碰撞检测并不是在一起的。
所以我们要继承planning interfaface里面的类。

大体上可以这样认为。

运动规划插件

MoveIt 通过插件界面与运动规划器合作。这允许 MoveIt 与多个库的不同运动规划器进行通信和使用，使 MoveIt 易于扩展。运动规划器的接口是通过 ROS 操作或服务（由move_group提供）。系统默认运动规划器move_group由 MoveIt 设置助手使用 OMPL 和 MoveIt 接口配置到 OMPL。
运动计划请求
运动计划请求明确规定了您希望运动规划者做什么。通常，您将要求运动规划器将手臂移动到其他位置（在关节空间中）或最终效应器移动到新姿势。默认情况下检查冲突（包括自冲突）。您可以将对象连接到最终效应器（或机器人的任何部分），例如，如果机器人拾取对象。这允许运动规划器在规划路径时考虑对象的运动。您还可以为运动规划器指定要检查的约束 - MoveIt 提供的内置约束是运动约束：

• 位置约束 - 将链接的位置限制为位于空间区域内
• 方向约束 - 将链接的方向限制在指定的滚动、俯仰或方向限制内
• 可见性约束 - 限制链路上的点位于特定传感器的可见性圆锥体内
• 关节约束 - 将关节限制在两个值之间
  用户指定的约束 - 您还可以使用用户定义的回调指定自己的约束。

运动计划结果

移动组节点将生成所需的轨迹，以响应您的移动计划请求。此轨迹将手臂（或任何一组关节）移动到所需位置。请注意，move_group 的结果是轨迹，而不仅仅是路径 - _move_group 将使用所需的最大速度和加速度（如果指定）生成遵循关节级别速度和加速度约束的轨迹。

运动规划管道：运动规划和计划请求适配器

完整的运动规划管道将运动规划器与其他组件（称为规划请求适配器）连接在一起。规划请求适配器允许预处理运动计划请求和后处理运动计划响应。预处理在多种情况下非常有用，例如，当机器人的启动状态略高于机器人的指定关节限制时。其他几个操作都需要后处理，例如，将为机器人生成的路径转换为时间参数化轨迹。MoveIt 提供了一组默认运动规划适配器，每个适配器都执行非常特定的功能。

• 修复启动状态绑定
  修复启动状态边界适配器将启动状态修复为在 URDF 中指定的联合限制内。在物理机器人的接头限制配置不正确的情况下，需要此适配器。然后，机器人可能最终出现一个或多个关节稍微超出其关节限制的配置。在这种情况下，运动规划器无法规划，因为它会认为起始状态超出联合限制。“FixStartStateBounds"规划请求适配器将"修复"启动状态，将之移动到联合限制。然而，这显然不是每次都正确的解决方案，例如，关节确实超出其关节限制大量。适配器的参数指定关节可以超出其限制的量，以使接头"可修复”。
• 修复工作空间绑定
  修复工作区绑定适配器将指定用于规划的默认工作区：大小为 10 m x 10 m x 10 m 的立方体。只有在对规划者的计划请求未填写这些字段时，才指定此工作区。
• 修复启动状态
  修复启动状态冲突适配器将尝试通过少量干扰关节值，在指定配置（在冲突中）附近对新的无碰撞配置进行采样。它将干扰值的量由"jiggle_fraction"参数指定，该参数控制扰动占关节总运动范围的百分比。此适配器的另一个参数指定适配器在放弃之前将采样多少随机扰动。
• 修复启动状态路径系统
  当运动计划的启动状态不遵守指定的路径约束时，将应用此适配器。它将尝试在机器人的当前配置之间规划一条路径，以遵守路径约束的新位置。新位置将作为规划的开始状态。
• 添加时间参数化
  运动规划器通常会生成"运动路径"，即不服从任何速度或加速度约束且未进行时间参数化的路径。此适配器将应用速度和加速度约束来"时间参数化"运动计划。

---

教程

• 主要参考 ->plugin_tutorial.rst
• lerp_example.cpp 源码

在本节中，我们将展示如何添加新的运动规划器到 MoveIt 作为插件。MoveIt 中的基类planning_interface任何新的规划器插件应继承的基类。出于演示目的，将创建一个线性插值规划器（lerp），用于规划联合空间中两个状态之间的运动。此规划器可以用作添加新规划器的起点，因为它包含必要的基础知识。本教程中设计的最终源文件可用：codedir：’<creating_moveit_plugins/lerp_motion_planner/src>"。下图显示了在 MoveIt 中添加新规划器的类之间的关系的简要整体视图。

• PlannerConfigurationSettings -> yaml
• PlanningContext :规划算法的内容－》solve(做求解)
  • solve调用算法封装好的class。
• PlanningManager: 向moveit注册；－》register
  • decription.xml 描述插件，选择moveit接口
  • package.xml export
• 以上两个核心的基类都在github上－》核心基类代码—planning_interface.h
  

---

例程

相当于生成一个插件pkg。这里通过tree进行文件说明。

tree

├── CMakeLists.txt
├── include
│ └── lerp_interface
│ ├── lerp_interface.h（算法类封装）
│ └── lerp_planning_context.h（继承planning_interface里的planningContext, 定义核心借口）
├── launch
│ └── lerp_example.launch
├── lerp_interface_plugin_description.xml（对插件进行描述，生成路径等等）
├── lerp_planner.png
└── src
├── lerp_example.cpp
├── lerp_interface.cpp（用场景信息和req然后进行规划）
├── lerp_planner_manager.cpp（继承planning_interface里的planningManager,　实现初始化、实例化context、调用规划器，蔽障配置、返回respone）
└── lerp_planning_context.cpp（对算法类的调用，然后用srv格式作为返回）

不知为什么官方例程里没有package.xml文件。这个文件通过export向moveit声明插件。

---

接口源码分析

---

调用配置

通过配置moveit_config里的东西即可。
1、yaml定义全局变量，供算法使用。
2、launch文件（启动节点用）选择调用算法（修改piepline的调用默认值，以次乡下挖掘即可）->launch.xml

---

example源码解析

---

lerp_example运行结果

主要参考 ->plugin_tutorial.rst
在本小节中，我们将解释如何加载和使用我们之前创建的 lerp 规划器。为此，将创建一个名为"lerp_example.cpp的 ros 节点。第一步是通过以下代码行获取与请求的规划组以及规划场景相关的机器人的关节状态和组：

moveit::core::RobotStatePtr robot_state(new moveit::core::RobotState(robot_model)); //获取机器人状态
const moveit::core::JointModelGroup* joint_model_group = robot_state->getJointModelGroup(PLANNING_GROUP);//获取规划组名字
const std::vector<std::string>& joint_names = joint_model_group->getVariableNames();
planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model));//获取场景

下一步是使用插件加载规划器，并planner_plugin_name参数，并设置到我们创建的参数：

boost::scoped_ptr<pluginlib::ClassLoader<planning_interface::PlannerManager>> planner_plugin_loader;
planning_interface::PlannerManagerPtr planner_instance;
std::string planner_plugin_name =  "lerp_interface/LERPPlanner";
node_handle.setParam("planning_plugin", planner_plugin_name);

加载规划器后，是时候为运动规划问题设置开始和目标状态了。启动状态是设置为"启动"的机器人的req.start_state。另一方面，目标约束通过创建一个moveit_msgs：约束，使用目标状态和联合模型组。此约束被输入req.goal_constraint。以下代码演示如何执行这些步骤：

// Get the joint values of the start state and set them in request.start_state
std::vector<double> start_joint_values;
robot_state->copyJointGroupPositions(joint_model_group, start_joint_values);
req.start_state.joint_state.position = start_joint_values;

// Goal constraint
moveit::core::RobotState goal_state(robot_model);
std::vector<double> joint_values = { 0.8, 0.7, 1, 1.3, 1.9, 2.2, 3 };
goal_state.setJointGroupPositions(joint_model_group, joint_values);
moveit_msgs::Constraints joint_goal = kinematic_constraints::constructGoalConstraints(goal_state, joint_model_group);
req.goal_constraints.clear();
req.goal_constraints.push_back(joint_goal);

到目前为止，我们已经加载了规划器，并创建了运动规划问题的开始和目标状态，但我们还没有解决问题。通过创建计划上下文实例并调用其求解函数，通过给定的启动和目标状态信息解决联合状态中的运动规划问题。请记住，传递给此求解函数的响应应的类型应为 planning_interface：运动计划响应：

planning_interface::PlanningContextPtr context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_);

最后，要运行此节点，我们需要在启动lerp_example启动中启动。此启动文件启动演示.启动包panda_moveit_config通过传递 lerp 作为规划器的名称。然后，lerp_example启动，并加载lerp_planning.yaml，以将特定于 lerp 的参数设置为 ROS 参数服务器。

使用时将官方pkg直接放入ros的工作空间即可。官方是以franka emika panda为例，之后运行。

roslaunch panda_moveit_config demo.launch

这个时候默认的规划器还是ompl，运行。

rosrun moveit_lerp_motion_planner lerp_example 

此时规划器设为lerp，运行效果如下图：

---

问题

• example运行时有时候会出现　[ERROR] [1610070680.761221597]: Exception caught: ‘std::bad_alloc’
• 为什么没有pkg.xml