通过ROS控制真实机械臂(7)

在之前的move_group界面中，当点击plan and execute之后，move_group就会帮我们规划出一条通往指定位姿的轨迹，发布在follow_joint_trajectory上，通过rostopic echo /redwall_arm/follow_joint_trajctory/goal,可以看到其中包含了positions: velocities: accelerations: time_from_start的路径点，称之为p[],v[],a[],T[]，数了一下，一共16个路径点，实际的点不是16，有些没有显示出来！！！

这个消息中就包含了路径规划后的所有路点信息，也就是描述机械臂应该以什么样的姿态运动到目标位置。关于MoveIt的运动规划算法， 运动规划器根据环境计算得出一条空间轨迹，但是这条轨迹只包含位置关系，并没有告诉机器人应该以怎样的速度、加速度运动 于是，就要经过AddTimeParameterization这个模块， 为这条空间轨迹进行速度、加速度约束，并且计算运动到每个路点的时间。 可以去看MoveIt中AddTimeParameterization模块的的代码实.这个算法的全称是 Time-Optimal Path Parameterization Algorithm，简称TOPP。参考链接：http://www.guyuehome.com/752?replytocom=48315

根据时间最优的原则，输出所有点的速度、加速度、时间信息。其中存在的一个关键问题就是加速度存在抖动。MoveIt把路径规划后的消息发到下层的控制器，控制器当然要根据所有的路点完成中间状态完成插补运算，如果用多项式插补，在已知所有路点速度、加速度、时间的条件下，需要使用一个五阶多项式来描述（参见《机器人学导论》或https://blog.csdn.net/qq_26565435/article/details/94545986）

下图所示是用五次多项式插补的一条路径，可以看到速度加速都是平滑的，按照这样的插补运算，确实可以完成插补的任务，但是由于加速度曲线完全拟合，在实际的控制中表现成加速度抖动问题，也就是传说中的”龙格现象“，这种抖动已经远远超过了机器人的加速度限制图片来源：https://blog.csdn.net/libing403/article/details/78715418

所以采用三次样条插补算法： 三次多项式实际是速度规划里面常说的PVT算法。PVT 模 式 是 指 位 置 — 速 度 — 时 间（Position-Velocity-Time）模式。PVT模式是一种简单又有效的运动控制模式，用户只需要给定离散点的位置、速度和时间，运动控制卡的插补算法将会生成一条连续、平滑的运动路径。三次样条所产生的效果大致：从位置曲线和速度曲线上来看是平滑的，加速度也不存在五次多项式那样的抖动，但是也引入了新的问题就是加速度不连续，对于一些应用的动力学和惯性载荷会产生一些不期待的影响。

图片来源https://blog.csdn.net/libing403/article/details/78698322

接下来看三次样条的具体实现方法：每个轨迹点都有 positions[], velocities[], accelerations[],  time_from_start []。古月老师源码要输入的是 两个数组，x时间，y位置。因此需要把轨迹点中每个关节的所有位置取出来放到相应的数组里。时间点的数组可以共用一个。

头文件cubicSpline.h：

使用上一篇博客的action服务端节点：redwall_arm_server.cpp ，订阅move_group发布的follow_joint_trajectory动作，获取所有的路径点信息，然后经行三次样条插补。