Moveit + Gazebo实现联合仿真：ABB yumi双臂机器人（ 二、双臂协同运动实现 ）

前言

在学习Moveit相关教程后开始尝试动手搭建双臂机器人仿真平台，整个实现过程中多次碰壁，考虑当前网络上关于双臂机器人的教程解答相对较少并不乏谬误，于是将实现流程、注意事项及个人总结分享在网络，本文不做过多教程类解释（如有需要可参考Moveit官方教程或其他博主资源），仅以记录形式方便大家和未来的自己参考。

文中代码多仅呈现关键部分，未做完整展示，项目完整代码可参考 https://github.com/Qiaozs/Moveit-Gazebo-for-yumi-ABB。

---

部分图片资源及参考资料来自网络，如有侵权，请联系删除。

搭建环境

ubuntu20.04
ROS-noetic
提前安装好ROS对应版本下的Moveit和Gazebo软件

流程安排

在本项目上一部分中，已经完成了yumi及机器人在Moveit和Gazebo中的通讯建立，并且实现了简单的运动规划执行，为了进一步发挥双臂机器人结构优势，贴合实际控制场景，本篇着重于实现yumi机器人双臂协同运动。

• 双臂协同仿真实现
  • 1、Moveit配置
  • 2、Moveit、Gazebo建立连接
  • 3、机器人运动代码实现（绕八字、跳舞）

二、双臂协同仿真

为了区别于之前的简单仿真平台，在主目录下新建 2yumi_ws 工作空间：

mkdir -p 2yumi_ws/src
cd 2yumi
catkin_make

模型导入部分与前述相同，在工作空间下新建yumi_description 功能包，把需要的文件复制进来即可。
注意：细心的朋友可能发现这样做导致不同的工作空间下有了重名功能包，目前尚无影响，但在之后的launch文件启动时有可能启动之前工作空间中的同名文件，并且不容易发现，为了避免这种情况，提醒读者在系统.bashrc文件中更新环境变量使本工作空间位于最新位置。

Moveit配置

为了实现双臂同时规划运动，需要在Moveit中修改部分配置内容。
可以直接将之前的yumi_moveit_config功能包复制到当前工作空间，cd 到当前工作空间下执行

rosrun moveit_setup_assistant moveit_setup_assistant 

这一次选择“Edit Existing Moveit Configuration Package"，Browse到复制好的yumi_moveit_config功能包。
需要修改的部分是Planning Group部分，添加一个group名为dual_arm，将之前的arm_l，arm_r设置为其subgroup:

可以在Robot Poses中为dual_arm也添加一个初始位置。其余部分基本不用修改，直接跳到Configuration Files生成功能包，Moveit会提示是否覆盖部分文件，直接一路OK即可。

Moveit、Gazebo建立连接

由于新添加的move_group其实是之前双臂的父组，而关于双臂通讯建立的所有内容已在前述中完成，基本不用修改，所以将yumi_gazebo 功能包之间复制进来即可。

机器人运动代码实现（绕八字、跳舞）

定点

首先通过定点运动仿真熟悉双臂机器人的运动规划代码实现：

# 伪代码
         # 初始化需要使用move group控制的机械臂中的arm group
        dual_arm = moveit_commander.MoveGroupCommander('dual_arm')

        dual_arm.set_pose_target(target_pose_l_1, end_effector_link_l)
        dual_arm.set_pose_target(target_pose_r_1, end_effector_link_r)
        dual_arm.go()
        rospy.loginfo("左臂第 1 个位置：")
        show_pose(arm_l)
        rospy.loginfo("右臂第 1 个位置：")
        show_pose(arm_r)

为了明确双臂中每个手臂的运动，仍需要给两臂单独设置末端执行器和末端位姿，但是所有操作可以均在dual_arm下进行。
可以看到机器人已经可以正常双臂同时运动，下一步可以开始规划更多动作。

绕八字

在双臂运动中实现绕八字仍然需要使用笛卡尔空间轨迹规划，以往实现方式是：

1. 绘制期望轨迹，用一数组存储轨迹点序列
2. 使用compute_cartesian_path函数得到运动规划plan
3. 使用move_group.execute()执行该plan

但在此处这样的方案就会遇到问题：

1. 当前的期望轨迹不再是单一的一个圆，而是空间中两个圆，使用一个数组逐个采样记录数据点没办法连成期望的轨迹效果，即便连成轨迹，无法在compute_cartesian_path中明确两臂分别执行哪条轨迹。
2. 如果仍然单独绘制两条轨迹，单独得到两个轨迹规划plan，没有办法使用dual_arm执行，因为每一条plan中制定相关的7个关节运动，moveit没有提供同时execute两个plan的方案 。
3. 如果仍然单独执行每条plan，效果仍是左右臂先后运动，无法同时规划运动。

经过查找moveit相关函数接口定义，特别是compute_cartesian_path得到的plan数据结构，给出如下一种解决方案：

1. 仍然分别设定左右臂期望轨迹，使用compute_cartesian_path得到两个plan_l,plan_r.
2. 由于清楚两plan指定的机械臂关节并不相同，所以考虑将两plan加和，这样在一个plan中就包含了所有关节的规划信息。

双臂轨迹加和实现

具体实现函数如下：

def plan_plus(plan_l,plan_r):
        # 创建一个新的轨迹
        dual_arm_plan = RobotTrajectory()
        dual_arm_plan.joint_trajectory.joint_names = plan_l.joint_trajectory.joint_names + plan_r.joint_trajectory.joint_names
        # 确保左臂和右臂轨迹有相同数量的点，如果没有，可能需要将两者的轨迹点数匹配起来
        assert len(plan_l.joint_trajectory.points) == len(plan_r.joint_trajectory.points)

        # 合并轨迹点数据
        for i in range(len(plan_l.joint_trajectory.points)):
          new_point = JointTrajectoryPoint()
          # 注意：确保positions, velocities, accelerations等数据结构的长度和顺序是正确的
          new_point.positions = plan_l.joint_trajectory.points[i].positions + plan_r.joint_trajectory.points[i].positions
          new_point.time_from_start = plan_l.joint_trajectory.points[i].time_from_start
          dual_arm_plan.joint_trajectory.points.append(new_point) 

        return dual_arm_plan

如此一来，将分别得到的plan_l,plan_r.加和为dual_arm_plan，就能轻松交给dual_arm执行，实现双臂同时进行笛卡尔空间下运动（此处为八字运动）

        # # circle_move 函数，参数为：move_group,target_pose,半径，圈数，方向（顺时针1，逆时针0），起始位置（圆心左1，圆心上2，圆心右3，圆心下4）
       plan_l=circle_move_yz(arm_l,target_pose_l_1,0.1,1,1,1)
       plan_r=circle_move_yz(arm_r,target_pose_r_1,0.1,1,0,3)    
       dual_arm_plan=plan_plus(plan_l,plan_r)
       dual_arm.execute(dual_arm_plan)

简单修改圆形轨迹代码同样可以实现椭圆运动：

注意：虽然通过这种方式实现了双臂同时运动，但是有一个明显的问题：由于两个plan是分开规划得到的，moveit考虑双臂碰撞的功能不能再发挥效果，因为在规划一条plan时另一手臂并不处在运动位置。所以使用这种方法需要读者自行确保双臂运动不会发生碰撞。

自定义运动实现

有时候控制者对机械臂运动有独特期望，不能用现有方式实现，比如模拟一段跳舞动作，对机械臂运动的关节状态比较关注。
通常情况下可以在Rviz界面中直接调整各个关节的角度值：

也可以在仿真界面中拖动末端小球直接设置末端位姿，同时得到各关节角度：

将两种方法结合可以：拖动小球确定期望机械臂姿态，读取各关节角度，在python中使用set_joint_value_target()函数设置机械臂末端姿态。
在实际操作中，由于yumi机器人双臂至少14个关节，每确定一个位置需要读入多个关节角度，挨个位置修改工作量巨大，为了增加效率，可以使用Moveit提供的get_current_joint_values函数，用于在Rviz确定末端姿态后，自动读出各关节角度数值，复制粘贴传入给set_joint_value_target即可，具体实现如下(get_joint.py文件)：

        pose_1 = dual_arm.get_current_joint_values()
        rospy.loginfo(str(pose_1[0])+" ,"+
                      str(pose_1[1])+" ,"+
                      str(pose_1[2])+" ,"+
                      str(pose_1[3])+" ,"+
                      str(pose_1[4])+" ,"+
                      str(pose_1[5])+" ,"+
                      str(pose_1[6])+" ,"+
                      str(pose_1[7])+" ,"+
                      str(pose_1[8])+" ,"+
                      str(pose_1[9])+" ,"+
                      str(pose_1[10])+" ,"+
                      str(pose_1[11])+" ,"+
                      str(pose_1[12])+" ,"+
                      str(pose_1[13]))

理论上为了实现自定义运动效果，只要多取路径点，依次执行实现即可：

        dual_arm.set_joint_value_target(pose_1) 
        dual_arm.go()
        dual_arm.set_joint_value_target(pose_2) 
        dual_arm.go()
        ...

但这样得到的运动效果有一个明显问题：每个路径点处机械臂会停止运动一下，原因是机械臂需要在到达当前路径点后才开始规划到下一目标点的运动plan，使得整个运动十分卡顿。

连续轨迹拼接函数

为了解决这一问题，本文给出的方法是：首先得到每一段运动的轨迹规划，自行将各段轨迹拼接成一个完整轨迹，使得机器人一次性完成所有轨迹。
根据Moveit中plan()函数及RobotTrajectory对象源码，实现轨迹拼接的方法如下：

def plan_plus(plan1, plan2):  

    # plan1和plan2是已经规划好的两个轨迹  
    # new_plan将是包含两个轨迹的新的轨迹  
    # 创建一个新的RobotTrajectory实例  
    new_plan = RobotTrajectory()  
    # 确保plan1和plan2使用相同的关节名称  
    if plan1.joint_trajectory.joint_names != plan2.joint_trajectory.joint_names:  
        raise ValueError("Joint names do not match between plans.")  
    new_plan.joint_trajectory.joint_names = plan1.joint_trajectory.joint_names  

    # 将第一个plan的点添加到new_plan中  
    for point in plan1.joint_trajectory.points:  
        new_plan.joint_trajectory.points.append(deepcopy(point))   

    # 获取第一个plan的最后时间点  
    last_time_from_start = plan1.joint_trajectory.points[-1].time_from_start  
    # 将第二个plan的点添加到new_plan中，并调整时间  
    for point in plan2.joint_trajectory.points:  
        new_point = deepcopy(point)  
        # 使用 last_time_from_start 作为偏移量  
        new_time_from_start = Duration(last_time_from_start.to_sec() + point.time_from_start.to_sec()+0.01)  # 0.001 是缓冲时间 
        # 确保 new_time_from_start 大于 new_plan 的最后一个时间戳（如果 new_plan 不为空）  
        new_point.time_from_start = new_time_from_start  
        new_plan.joint_trajectory.points.append(new_point)   
    return new_plan
   

注意，本函数只是初步完成了轨迹点的拼接，考虑了时间戳的匹配，但并没有对各轨迹速度和加速度处理，形成的运动效果消除了原有相邻轨迹之间的计算时间，但整个轨迹仍可以进一步变平滑（设置初、末速度不为零等方法）。

---

作为示例，本文定义多个路径点，为每个点设定关节期望角度，将整个轨迹拼接，模拟蛋仔派对中人物舞蹈动作，基本实现效果如下：

总结

至此，本文以yumi机器人作为研究范例，构建起了双臂机器人的Moveit与Gazebo联合仿真平台，并实现了Moveit运动规划的基本代码。针对常见的运动规划问题，如轨迹拼接、双臂轨迹协同等，初步提出了有效的解决方案，未来基于此平台可做更多实用性开发。