ZPC8210的博客

【代码】MoveIt Doucument。

将地图的长宽各缩小为原来的十分之一后，将其划分为100个相等的网格，障碍物占据的网格数量与这100个网格的比例即为障碍物的分布情况。如图1为重选父节点示意图，其中橙色实线为更新的路径，蓝色虚线为舍去路径，在单位圆内存在某一节点，使得X_new选择这个节点为父节点的距离更近且无碰撞，即可更新。图2为节点重连，在单位圆内，X_1选择X_new为父节点的距离比X_1选择X_2为父节点的距离更短且无碰撞，即可更新。步长过大时，会增加Q_new与 Q_nearest之间的路径经过障碍物的概率，降低采样的成功率。

简介RRT 算法（快速扩展随机树，rapidly exploring random tree）是一种随机性算法，它可以直接应用于非完整约束系统的规划，不需进行路径转换，所以它的算法复杂度较小，尤为适用于高维多自由度的系统。缺点是得到的路径质量不是很好。其思想是快速扩张一群像树一样的路径以探索（填充）空间的大部分区域，伺机找到可行的路径。RRT 的基本步骤是：1. 起点作为一颗种子，从它开始生长枝丫；2. 在机器人的“构型”空间中，生成一个随机点X；

通过上述过程，使用MoveIt中的RRT算法进行机械臂的轨迹规划、插值和控制。首先，通过配置RRT算法的参数进行规划；然后，使用插值技术优化轨迹；最后，将规划和优化后的轨迹发送到控制器以执行。这样可以确保机械臂以平滑、准确的路径进行运动。

自从看了交大邱博的路径规划的一个视频之后（如下其中之一的截图），才知道这里面包含的方法有很多，才发现ROS是多么的强大，帮我这种小硕铺了一条康庄大道，很多东西直接拿来就用了。当然对于大神来说，折腾总是在路上的，看了那么多算法，虽然很多都不适用于机械臂这种高维空间的路径规划，但是还是免不了想试试看自己导入一些算法。

我们可以看到，本次运动规划使用了LBKPIECE算法，并且使用了4线程并行规划，规划时间为0.063719秒。在中间的下来菜单里面有很多的具体算法，之后你可以尝试不同的算法，看看他们的区别。同样的，你也可以在MotionPlanning模块下的Planning子模块写的Query子模块里面设置随机的或者预设的目标位置。的时候，就代表启动成功了。接下来可以尝试不同的OMPL算法，不同的目标位姿和不同的环境。因为OMPL是采样算法，由于其随机采样的特性，每次的路径是不同的，如下图。终于，到了运动规划的时候了。

打开刚刚创建好的lwr_moveit_config文件夹，我们发现有config和launch两个文件夹。ompl_planning.yaml：这里是配置OMPL各种算法的各种参数。3.1 MoveIt!先看config，里面有。3.3 Launch文件。3.2 SRDF文件。

但是对于刚体机器人来说，有些肢体之间是不可能发生碰撞的，比如原本就相邻的肢体，比如类人机器人的脚和头。下方我们可以看到有'Base Link'和'Tip Link'，我们选择'lwr_arm_0_link'作为Base，选择'lwr_arm_7_link'作为Tip. 然后点击Save，这样一个规划组群就创建好了。同样的，我们可以再创建一个手的组群(Hand)，这一次我们用Add Links，然后选择'lwr_arm_7_link'。终端控制器，就是机械臂的手，以后用来在工作环境中直接控制的部位。

我们这里讲的，有别于一般来说，path planning用于无人车/无人机领域，而motion planning主要用于机械臂，类人机器人领域。当然了，这两者没有本质的区别，理论上说MoveIt!和OMPL同样可以用于无人车无人机的规划，但不免有些杀鸡用牛刀的感觉。两者规划的空间维度不同，导致他们的难易程度不同。举例说明，如果不考虑速度加速度，只考虑位置的话，无人车轨迹规划维度是3 (x,y,和角度), 无人机是6 (x,y,z,和另外3个量确定空间的旋转角度)。

在 MoveIt 中，最简单的用户界面是通过 MoveGroupInterface 类https://github.com/moveit/moveit2/blob/main/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h。

MoveItCpp是一个新的高级接口，是一个统一的C++ API，不需要使用ROS的动作、服务和消息来访问MoveIt2核心功能，并且是现有的一个替代（不是完全替代），我们将此接口推荐给需要更多实时控制的高级用户或工业应用。该接口是PickNik Robotics公司为许多商业应用程序开发的。

例如，一个运动规划的采样pipeline可能包含了一个初始的由OMPL产生的规划，然后由CHOMP来优化产生比单独使用OMPL或CHOMP更好的路径。类似地，使用Planning Adapter的概念，依赖于机器人的操作环境，其他的运动规划器也可以进行混合和匹配。为了使用Industrial_trajectory_filters包，我们首先先要了解moveit中的Planning Request Adapters的概念。完成这样的配置后，再由Moveit生成的轨迹就是固定时间步进的均匀采样点。

事实上，我的 xmate7_pro 机械臂是以 1ms 为控制周期进行控制指令的下发和机械臂关节状态的反馈的，因此，决不能直接把 MoveIt 规划完的轨迹数据直接在每个控制周期发送给机械臂执行；说了这么多，想要强调的就是，在把轨迹指令发给机械臂之前，一定要好好将 MoveIt 规划后的轨迹数据绘制出来看一下是否符合真实机械臂的使用要求，也要弄清楚机械臂的控制周期。然后，根据实际机械臂的控制周期，将经重采样处理之后的对应各个周期的路点数据发给机械臂执行，这样才能保证机械臂的安全性以及轨迹执行正确性。

如果对规划出来的轨迹不是很满意，我们可以在plan和execute之间对轨迹进行修改。这里以删除轨迹中的中间点为例，对轨迹进行重定义。接着调用//每gap个元素删除一次，保留首尾= 0)//尾元素保留break;elsei++;count++;

每次添加新节点时，它会重新检查并优化节点间的连接，以保证路径是最短的。路径优化：对于新添加的节点，检查树中是否存在其他节点可以通过连接到新节点来获得更优的路径。如果存在，更新这些节点的父节点为新节点，从而优化路径。RRT-Connect 是对 RRT 的一种改进，通过同时扩展两棵树（从起点和终点分别扩展），并尝试连接这两棵树，从而加速路径的找到过程。扩展：从最近节点向采样点方向扩展一定距离，形成一个新的节点，如果这个新节点不与障碍物碰撞，就将其加入到树中。如果足够接近，则认为找到了一条可行的路径。

【代码】move_group_interface_tutorial中文注释。

完整的cpp代码在https://github.com/ros-planning/moveit_tutorials/blob/master/doc/move_group_interface/src/move_group_interface_tutorial.cpp中查看，接下来将会逐步分析各项代码的功能。默认值是10%（0.1），如果你想要让机器人移动的更快，可以在机器人的 moveit_config 的 joint _ limits.yaml 文件中设置你想要的默认值，或者在代码中设定明显的因素。