BUG2（降速）解决：手工修改Moveit中的笛卡尔空间规划路径

问题描述：由于项目需要焊接，但是在使用moveit时遇到问题就是他的焊接轨迹规划算法自动规划出来的轨迹都是速度从0-0的路径，而且中间速度无法自定义修改，只能更改joint_limits.yaml文件修改全局的变量，因此需要直接修改plan变量。

还是基于aubo官网的功能包展开描述。

这里也给了一份焊接样本的轨迹路线程序，可以提供参考。

一般而言，我们默认使用moveit自带的ompl规划器进行运动规划，但这样就会面临两个问题：

1）BUG1:当速度过快或采样空间不合适时会导致笛卡尔空间的轨迹虽然数值上连续，但选解时，逆解数量大，导致机械臂关节空间不连续，使笛卡尔空间的运功轨迹不连续;

2) BUG2：降速问题，当使用moveit默认规划路径时，速度会按照默认的限制速度行进，无法通过调节降速比例因子来控制速度的大小；

针对BUG1，之前已经给出了解决方法就是更换求解器trac_IK或者IKFast等或者直接自己做逆解算法。（我使用的Trac_IK插件，更换方法详见这篇博文：ROS中MoveIt插件求解器更换-Trac_IK部分）

下面针对BUG2展开解决过程：

在Moveit插件中，运用规划算法时遵从的速度大小是有“joint_limit.yaml”文件内的定义决定的，所以解决的第一个方法很简单：

方法1：修改joint_limit.yaml文件

更改max_velocity:后面数值的大小即可，单位为rad/s，加速度单位为rad/s²；

缺点：这样一改的话最大速度成了这个，整体速度都下来了，有时候需要快速，因此对于本项目需要焊接阶段降速的情况还是没有什么卵用，整体效率就下降了。

方法2：更换轨迹规划器（本次采用了iptp的算法，有能力的可以自己搭建）

步骤1：在自己的demo文件里（就是最后要运行的文件，aubo工具包里有demo文件夹，我是在“MoveGroupInterface_To_Kinetic.cpp”文件的基础上修改的）

  首先添加头文件：

#include <moveit/robot_trajectory/robot_trajectory.h>
#include <moveit/trajectory_processing/iterative_time_parameterization.h>

 这里头文件“iterative_time_parameterization.h”需要下载，在moveit官网教程里面给了这部分：Time Parameterization — moveit_tutorials Kinetic documentation (ros.org)

 （下图中第一个就是iptp连接，代码在github中，可能需要科学上网。）

步骤二：在原先创建好规划路径之后填写下一行代码：（注意，一定要在computeCartesianPath（）函数之后）

robot_trajectory::RobotTrajectory rt(move_group.getCurrentState()->getRobotModel(),"manipulator_i5");

rt.setRobotTrajectoryMsg(*move_group.getCurrentState(),trajectory);

trajectory_processing::IterativeParabolicTimeParameterization iptp;

bool ItSuccess = iptp.computeTimeStamps(rt,0.1,0.1);

ROS_INFO("Computed time stamp %s",ItSuccess ? "SUCCESS" : "FAILED");

rt.getRobotTrajectoryMsg(trajectory);

解释：第一句是创建一个轨迹规划对象rt，其中参数“manipulator_i5”不能乱打，因为这个需要和其他文件中的变量对应；

第二句调用函数rt.setRobotTrajectoryMsg(*move_group.getCurrentState(),trajectory)，在这句话中rt被赋予了当前move_group运动规划组中的所有状态以及轨迹信息；

第三句创建了iptp轨迹规划模型；

第四句执行iptp.computeTimeStamps(rt,0.1,0.1);这里内部参数第一个“rt”是指之前创建的规划对象（内部包含当前轨迹规划组的信息，相当于一个中间桥梁），第二个参数0.1可以理解为速度因子，第三个参数0.1可以理解为加速度因子。

第五句就是借上一句返回值做判断，判断是否运力规划完成。

第六句，rt.getRobotTrajectoryMsg(trajectory);，更新rt中的轨迹；

至此，trajectory中包含的信息就是规划了速度和加速度之后的轨迹信息，然后继续运行后面的程序即可。

参考程序下载：MoveGroupInterface_To_Kinetic.cpp-C++文档类资源-CSDN文库

参考文章：

[1]ROS中的moveit_msgs.msg.DisplayTrajectory数据_唐_成的博客-CSDN博客

[2]sensor_msgs/JointState Documentation

[3]MoveIt Tutorials — moveit_tutorials Kinetic documentation

[4]planning_interface: move_group_interface.h Source File

[5]planning_interface: moveit::planning_interface::MoveGroupInterface::Plan Struct Reference

[6]ROS进阶——运动规划分析 - 古月居

[7]https://github.com/ros-planning/moveit_core/issues/141