，
场景
动画
一、动画脚本分析（以机器人1为例说明），定义了以下函数：
路径可视化（输入关节空间路径点，通过设置关节位置，读取末端位置得到相邻关节配置的末端路径段，添加到绘图对象以显示路径）
获取关节配置差异值（在处理旋转关节时，考虑到了周期性，将关节差异限制在[-pi，pi]范围内）
应用关节位置（根据关节位置数组设置各关节的目标关节位置）
生成路径长度表（获取从起始关节配置到第i个关节配置的加权距离 数组并返回）
获取目标位姿局部平移后的位姿矩阵
验证关节配置是否引起碰撞的回调函数（保存当前关节配置，设置关节位置到新的关节配置，检查碰撞对是否发生碰撞，恢复之前保存的关节配置，返回碰撞检查结果）
寻找无碰撞的配置（设置仿真环境目标位姿矩阵，获取当前配置，设置关节范围[关节下限+范围]数组，同步模拟环境到IK环境，复制IK环境，设置IK环境的关节范围，在指定的IK环境中查找无碰撞配置，擦除复制的IK环境，返回找到的无碰撞配置）
寻找多个无碰撞的配置（在给定矩阵位置附近搜索多个无碰撞机器人配置，尝试次数和找到指定个数无碰撞配置）
获取当前机器人配置
将指定配置应用到机器人
获取两个配置在关节配置空间的加权距离
获取关节空间路径加权距离的总长度
在设定关节极限情况下寻找起始关节配置和目标关节配置之间的路径（设置关节上下限位，创建OMPL任务，设置OMPL算法，创建状态空间数组即关节空间数组，设置OMPL的关节状态空间和碰撞检查对，设置OMPL的起始状态和多个关节配置目标状态即接近目标位姿矩阵的无碰撞关节配置，设置OMPL任务并进行规划准备，执行OMPL计算，如果路径计算成功，则可视化路径并获取关节空间加权路径距离总长度，销毁OMPL任务，返回关节空间路径及总长度）
找到起始配置到多个近似目标关节配置的关节空间路径以及关节空间路径长度表
生成IK路径（保存当前关节配置，将机械臂配置设置为起始配置，设置目标位姿到仿真场景中，根据是否忽略碰撞，选择是否传递验证回调函数，将当前仿真状态同步到指定的逆运动学环境和逆运动学组中，生成逆运动学路径，恢复原始关节配置和模拟步进状态，将返回的路径重新组织为一维数组并返回路径及其关节空间路径累积长度数组）
执行运动（获取仿真时间步长，获取关节最大速度限制并存储在表中，设置速度修正因子来调整速度以获得更平滑的运动，开始修正速度因子，设置初始位置速度加速度以及目标位置和速度，生成一个 Ruckig 运动规划器（Ruckig motion planner）的句柄，执行Ruckig路径生成，计算每个关节路径段各关节实际关节速度与关节速度上限的比率，如果相对速度比率大于1，则意味着实际关节速度超过了最大允许关节速度，修正速度因子后，再来一遍，通过不断修正速度因子，直到相对速度比率小于1.001结束速度因子修正，之后再次使用Ruckig算法生成路径并应用关节位置，最后移除Ruckig路径规划器）。
机器人1主程序：使用运动规划靠近第一个杯子->使用IK靠近第一个杯子->抓取->请求第二个机器人开始规划并运动->使用IK将杯中内容倒入另一个杯子->使用IK返回到倒水之前的配置->使用IK将杯子放回桌子上->松开手->使用IK后退一点->返回到初始配置。
二、 Ruckig运动规划器
参考网址
https://pypi.com.cn/project/ruckig/ ruckig - 机器人和机器的瞬时运动生成。_PyPI中文网
https://github.com/pantor/ruckig GitHub - pantor/ruckig: Motion Generation for Robots and Machines. Real-time. Jerk-constrained. Time-optimal.
https://blog.csdn.net/yuanshenqiang/article/details/128716684 Ros中的轨迹规划模块_ros 轨迹规划-CSDN博客
作者陈晓永：智能装备专业高级工程师，软件工程师。机器人自动化产线仿真动画制作
The End