Moveit + Gazebo实现联合仿真：ABB yumi双臂机器人（ 一、基本仿真搭建及简单运动实现 ）_moveit+gazebo仿真

---

前言

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

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

---

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

搭建环境

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

实现目标

机器人选择：yumi机器人

本文选用ABB公司yumi双臂机器人，一方面因为就读院校拥有一台yumi实机，也是实验室为数不多的双臂机器人，故对其相对熟悉；另一方面ABB公司毕竟为机器人大厂，网络开源资料相对充足，且yumi确是比较知名的双臂机器人，方便在实现的过程中参考网络资料。（图片来自网络）

仿真目标

基于基本仿真平台实现简单的运动规划执行（定点运动、绕八字等）；
基于双臂协同仿真优化运动实现效果；
另外由于npy喜欢玩蛋仔派对，遂也考虑使用yumi机器人模仿蛋仔游戏中一段人物舞蹈，比较契合机器人的双臂特点，实现难度适中，对自己也有不错的锻炼效果。

流程安排

本文计划完整记录整个实现流程，故整体分为两个部分：基本仿真实现和双臂协同仿真实现。前者方便读者用来作简单机械臂仿真参考，后者具体实现双臂协同目标。主要步骤包括如下：

• 基本仿真实现

  • 1、模型导入
  • 2、Moveit配置
  • 3、Moveit、Gazebo建立连接
  • 4、机器人运动代码实现（走直线、绕八字）

• 双臂协同仿真实现

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

一、基本仿真实现

本部分基本实现 yumi 机器人在 rviz 和 gazebo 中的通讯建立，最终可以通过moveit的运动规划来实现机器人在 gazebo 中做简单动作（如走直线、绕八字）。建议读者在学习 Moveit 教程后（或学习过程中）参考本文，效果更佳。

---

开始之前创建 yumi_ws 工作空间

mkdir yumi_ws/src
cd yumi/src
catkin_make
1
2
3

模型导入

ROS 中描述机器人模型通常使用 urdf 或 xacro 文件，两者均是标记语言，其中 urdf 相对原始， 而 xacro 对 urdf 作了一定优化，使用宏定义等方式有效减少了 urdf 文件代码的冗余性，实际使用中对二者的选择限制并不明显，本文中更多使用前者。
关于 urdf 文件，一般有以下几种方式获得：

• solidworks绘制模型导出
• 网络开源 urdf 文件
• 3、机器人运动代码实现（绕八字、跳舞）

对于使用现有的产品级机器人建议直接从网络上下载相关机器人资源，本文使用的 yumi 机器人在可从ABB官方开源的 github 项目中下载，也可从网友上传的资源中下载。
需要的资源包括如下：

其中 urdf 文件夹下存放 yumi.urdf 文件（模型文件），meshe 文件夹中为 yumi 机器人的贴图文件（会在 yumi.urdf 中被调用）
在工作空间中创建 yumi_description 功能包（标准命名 robot_description，添加 urdf 、xacro依赖，添加上述相关文件即可。
网络常见的资源通常也为 yumi_description 功能包形式，一般可以直接下载使用。
此处可以 cd 到 yumi.urdf 所在目录下输入：

check_urdf yumi.urdf
1

对机器人模型作简单检查，正常情况终端显示：

也可以使用

urdf_to_graphiz yumi_urdf
1

在当前目录下生成pdf文件用来直观认识机器人模型结构，检查无误后本步完成。

---

注意：网络上的机器人模型文件众多，但有不少存在细微错误无法用于后续的Moveit+Gazebo通信建立并且较难发觉（在后文中仍会提到，此处仅作提醒），不一定要使用ABB官方提供的文件，有时经过网友实践检验的反而更佳。

Moveit配置

打开终端运行

rosrun moveit_setup_assistant moveit_setup_assistant
1

启动 Moveit 配置工具，按照流程完成Moveit配置：

1. 点击新建Moveit Configuration Package ，Browse到机器人urdf文件，点击 Load Files 键：
   
   有些朋友会遇到弹窗报错机器人模型无法显示情况，一般建议cd 到yumi_description 所在工作空间下重新尝试（记得source环境变量）
2. 由于我们期望实现双臂运动，故在运动规划组处添加左右两臂两个 group，如下：
   
   由于本项目运动规划不涉及末端执行操作，所以group末端选到gripper_x_base 即可。
3. 分别为双臂添加 home_pose 。
4. 完成其他配置后在 src 目录下Generate Package 名为 yumi_moveit_config (标准命名 robot_moveit_config）。
5. 配置完成后可在终端输入

roslaunch yumi_moveit_config demo.launch 
1

正常情况会启动Riz仿真环境，检查是否可以对两个 group 分别做运动 plan 和 execcute：

在 planning Group 处切换规划组。检查无误，Moveit 配置完成。

Moveit、Gazebo建立连接（重要）

参考网络教程，此部分需要完成多个文件的修改、添加，由于网络资料介绍、ROS版本等参差不一，整个实现中绝大多数报错来自此部分，需要读者格外注意。

---

准备工作：src 下新建 yumi_gazebo 功能包，包含 参数配置config 文件夹和 启动文件 launch 文件夹。

建立 Gazebo 仿真环境启动文件： yumi_gazebo_world.launch

<launch>

  <!-- these are the arguments you can pass this launch file, for example paused:=true -->
  <arg name="paused" default="false"/>
  <arg name="use_sim_time" default="true"/>
  <arg name="gui" default="true"/>
  <arg name="headless" default="false"/>
  <arg name="debug" default="false"/>

  <!-- 用于启动整个Gazebo仿真环境 -->
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="debug" value="$(arg debug)" />
    <arg name="gui" value="$(arg gui)" />
    <arg name="paused" value="$(arg paused)"/>
    <arg name="use_sim_time" value="$(arg use_sim_time)"/>
    <arg name="headless" value="$(arg headless)"/>
  </include>
  
<!-- 用于保存机器人模型路径 -->
  <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find yumi_description)/urdf/yumi.urdf'" /> 

<!-- 用于将机器人模型放置在gazebo中 -->
  <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
	args="-urdf -model yumi -param robot_description"/> 
</launch>

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26

终端运行：

roslaunch yumi_gazebo yumi_gazebo_world.launch 
1

启动gazebo仿真环境，正常情况为机器人在gazebo中正常显示，终端没有报错。
注意：常见的终端报错内容有：

• 提示某个关节不存在——机器人模型有误，建议检查改正或直接替换其他urdf文件，这个错误很容易不重视，因为实际中可能出现不存在的关节为finger关节，由于运动规划组中不涉及末端，所以一开始作者选择忽略本错误
• 机器人模型未出现，终端报错：

process has died [pid 29282, exit code 2, cmd /opt/ros/noetic/lib/controller
1

机器人link质量或转动惯量设定有误，即也是urdf文件问题。

• 终端报错：

[spawn_model-4] process has died [pid 19622, exit code 1, cmd /opt/ros/noetic/lib/gazebo_ros/spawn_m
1

解决方法：删除urdf文件中的中文内容，或者前往主目录下/usr/lib/python2.7/xml/etree/ElementTree.py修改不识别中文的编码问题，可参考此处，但后者相对麻烦，建议修改前者。

• 终端报错：
  
  网络上统一意见是忽略本错误，实践中的确不影响仿真进行。

---

其余还可能遇到各种错误，网络上的报错解决方案也五花八门，修改起来费时费力，但作者的经验是：当遇到报错且存在上述第一条报错时，优先解决这一条，其他的有可能会随之解决。

配置Gazebo端轨迹接收执行文件（关节轨迹控制器）

---

从此处开始，以下全部文件均默认自行新建

---

参数配置文件：yumi_trajectory_control.yaml

yumi:
  arm_l_controller:
    type: "position_controllers/JointTrajectoryController"
    joints:
      - yumi_joint_1_l
      - yumi_joint_2_l
      - yumi_joint_7_l
      - yumi_joint_3_l
      - yumi_joint_4_l
      - yumi_joint_5_l
      - yumi_joint_6_l
    gains:
      yumi_joint_1_l: { p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0 }
      yumi_joint_2_l: { p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0 }
      yumi_joint_7_l: { p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0 }
      yumi_joint_3_l: { p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0 }
      yumi_joint_4_l: { p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0 }
      yumi_joint_5_l: { p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0 }
      yumi_joint_6_l: { p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0 }

  arm_r_controller:
    type: "position_controllers/JointTrajectoryController"
    joints:
      - yumi_joint_1_r
      - yumi_joint_2_r
      - yumi_joint_7_r
      - yumi_joint_3_r
      - yumi_joint_4_r
      - yumi_joint_5_r
      - yumi_joint_6_r
    gains:
      yumi_joint_1_r: { p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0 }
      yumi_joint_2_r: { p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0 }
      yumi_joint_7_r: { p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0 }
      yumi_joint_3_r: { p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0 }
      yumi_joint_4_r: { p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0 }
      yumi_joint_5_r: { p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0 }
      yumi_joint_6_r: { p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0 }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38

着重注意：

• yaml文件第一行为命名空间，此处为yumi，需要与其他文件中的命名空间统一，否则无法建立通信。
• 文件第二行为规划组控制器名称，同样不能随便定义，可以参考yumi_moveit_config/fake_controller.yaml中的命名方式（此为moveit配置自动生成，一般不会出错），同样需要与其他文件中的命名方式统一，否则无法建立通信。

控制器启动文件yumi_trajectory_controller.launch

<launch>

    <rosparam file="$(find yumi_gazebo)/config/yumi_trajectory_control.yaml" command="load"/>

    <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
          output="screen" ns="/yumi" args="arm_l_controller arm_r_controller"/>
</launch>

1
2
3
4
5
6
7
8

注意点：

1. node中的ns参数代表上述提到的命名空间，以 /yumi 形式传入
2. node中的name参数可写作"controller_spawner"者"arm_controller_spawner"，影响不大
3. node中args传入运动规划组控制器，此处是多臂机器人，不同规划组之间空格隔开即可

配置Gazebo端轨迹接收执行文件（关节状态控制器）

参数配置文件：yumi_joint_states.yaml

yumi:
  # Publish all joint states -----------------------------------
  joint_state_controller:
    type: joint_state_controller/JointStateController
    publish_rate: 50
1
2
3
4
5

控制器启动文件yumi_gazebo_states.launch

<launch>
    <!-- 将关节控制器的配置参数加载到参数服务器中 -->
    <rosparam file="$(find yumi_gazebo)/config/yumi_gazebo_joint_states.yaml" command="load"/>

    <node name="joint_controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
          output="screen" ns="/yumi" args="joint_state_controller" />

    <!-- 运行robot_state_publisher节点，发布tf  -->
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
        respawn="false" output="screen">
        <remap from="/joint_states" to="/yumi/joint_states" />
    </node>
</launch>
1
2
3
4
5
6
7
8
9
10
11
12
13

有些教程中将关节状态控制器和关节轨迹控制器写在一起，同样也可以实现期望效果。

Moveit端配置

参数配置文件：controllers_gazebo.yaml

controller_manager_ns: controller_manager
controller_list:
  - name: yumi/arm_l_controller
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    default: true
    joints:
      - yumi_joint_1_l
      - yumi_joint_2_l
      - yumi_joint_7_l
      - yumi_joint_3_l
      - yumi_joint_4_l
      - yumi_joint_5_l
      - yumi_joint_6_l

  - name: yumi/arm_r_controller
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    default: true
    joints:
      - yumi_joint_1_r
      - yumi_joint_2_r
      - yumi_joint_7_r
      - yumi_joint_3_r
      - yumi_joint_4_r
      - yumi_joint_5_r
      - yumi_joint_6_r
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27

name处写作yumi/arm_l_controller,即命名空间/运动规划组控制器，有些教程中不强调写命名空间即，只写作arm_l_controller，实际中也是有无均可，但可以给自己起到提醒作用。
控制器启动文件yumi_moveit_controller_manager.launch.xml

<launch>
  <!-- Set the param that trajectory_execution_manager needs to find the controller plugin -->
  <arg name="moveit_controller_manager" default="moveit_simple_controller_manager/MoveItSimpleControllerManager" />
  <param name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/>

  <!-- load controller_list -->
  <!-- Arbotix -->
  <!-- <rosparam file="$(find marm_moveit_config)/config/controllers.yaml"/> -->
  <!-- Gazebo -->
  <rosparam file="$(find yumi_moveit_config)/config/controllers_gazebo.yaml"/>
</launch>
1
2
3
4
5
6
7
8
9
10
11

Moveit启动文件

moveit_planning_execution.launch

<launch>
 # The planning and execution components of MoveIt! configured to 
 # publish the current configuration of the robot (simulated or real)
 # and the current state of the world as seen by the planner
 <include file="$(find yumi_moveit_config)/launch/move_group.launch">
  <arg name="publish_monitored_planning_scene" value="true" />
 </include>

 # The visualization component of MoveIt!
 <include file="$(find yumi_moveit_config)/launch/moveit_rviz.launch">
  <arg name="config" value="true" />
 </include>



  <!-- We do not have a robot connected, so publish fake joint states -->
  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
    <param name="/use_gui" value="false"/> 
    <rosparam param="/source_list">[/yumi/joint_states]</rosparam>
  </node>

</launch>
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22

最终启动文件

本文件用于一次性启动上述的各个文件:yumi_bringup_moveit.launch

<launch>
  
    <!-- Launch Gazebo  -->
    <include file="$(find yumi_gazebo)/launch/yumi_gazebo_world.launch" />

    <!-- ros_control arm launch file -->
    <include file="$(find yumi_gazebo)/launch/yumi_gazebo_states.launch" />   

    <!-- ros_control trajectory control dof arm launch file -->
    <include file="$(find yumi_gazebo)/launch/yumi_trajectory_controller.launch" />

    <!-- moveit launch file -->
    <include file="$(find yumi_moveit_config)/launch/moveit_planning_execution.launch" />

</launch>
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15

从上到下依次启动了四个launch文件。

注意

以上流程为参照相关教程实现，但是参考教程实现过程中可能会遇到如下问题：

1. 教程提到的文件不存在——许多网友在复现教程或是查找资料时都会遇到“自己没有名称一致的文件”的问题，一般这种情况直接自行新建，只要保证各launch文件之间依次按计划启动即没问题，最好明白各文件之间的关系。
   举一个典型的情况：yumi_moveit_controller_manager.launch.xml（下简称y）文件在之前版本的Moveit中是随配置直接生成，而在作者使用的noetic版本的ROS中疑似被"simple_moveit_controller_manager.launch.xml"（下简称s）取代，这种情况可以选择自行新建y文件；也可以不新建，使用s文件；还有一种稳妥的方式，即新建y文件，并保证s文件内容与其一致。
   同样的情况还出现在“controlers_gazebo.yaml"和
   "simple_moveit_controllers.yaml"中，需要引起读者注意。
2. 终端报错：
   如果机器人模型文件无误则问题出在命名空间/运动规划组名称不统一，注意检查之前强调步骤有无疏漏。
3. 终端报错：
   
   是moveit_rviz.launch文件问题，可将文件直接修改为如下：

<launch>

  <arg name="debug" default="false" />
  <arg unless="$(arg debug)" name="launch_prefix" value="" />
  <arg     if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args" />

  <arg name="config" default="false" />
  <arg unless="$(arg config)" name="command_args" default="" />
  <arg     if="$(arg config)" name="command_args" default="-d $(find yumi_moveit_config)/launch/moveit.rviz" />

  <node name="$(anon rviz)" launch-prefix="$(arg launch_prefix)" pkg="rviz" type="rviz" respawn="false"
	args="$(arg command_args)" output="screen">
    <rosparam command="load" file="$(find yumi_moveit_config)/config/kinematics.yaml"/>
  </node>

</launch>
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16

4. 修改上述问题后仍然无法实现moveit+gazebo联动，可将trajectory_execution.launch.xml文件中的最后一行 pass_all_args="true"删除，重新尝试运行。
5. 如仍无法实现，再在网络资源中查找报错解决方法，但需要注意控制变量，参考过程中加入自身思考，本部分配置过程有一定复杂性，需要花费一段时间解决。

至此成功实现Moveit+Gazebo的联合仿真通讯建立，下一步可以通过编写运动规划代码来交给Moveit做运动规划，并在Gazebo中呈现。

机器人运动代码实现（定点、绕八字）

Moveit做运动规划的好处在于其提供了运动学求解工具，并且会自动进行碰撞检测。使用Moveit做运动规划一般有三种方式：Rviz界面拖动规划、Python编程控制、C++编程控制。由于Python代码相对简洁易懂，所以本文均使用Python作为编程语言。
在基本仿真中主要实现了定点运动、绕八字运动和椭圆运动。全部代码见开源文档，此处只做部分展示。

定点运动

实现定点运动有两种方案：1、设定关节角度；2、设定末端位姿
此处对设定末端姿态方式做如下展示：
末端姿态使用xyz坐标和四元数设定，此处为了避免设定多位置带来的代码冗余，定义一姿态设定函数，arm.go()等运动命令封装在move()函数中：

def set_pose(x,y,z,ox,oy,oz,w):
        reference_frame = 'world'
        target_pose = PoseStamped()
        target_pose.header.frame_id = reference_frame
        target_pose.header.stamp = rospy.Time.now()     
        target_pose.pose.position.x = x
        target_pose.pose.position.y = y
        target_pose.pose.position.z = z
        target_pose.pose.orientation.x = ox
        target_pose.pose.orientation.y = oy
        target_pose.pose.orientation.z = oz
        target_pose.pose.orientation.w = w
        return target_pose
1
2
3
4
5
6
7
8
9
10
11
12
13

随后标定目标位置执行：

        # 标定要到达的位置点
        target_pose_l_1=set_pose(0.5,0.4,0.5,  0, 0.7071068, 0, 0.7071068 )
        target_pose_r_1=set_pose(0.5,-0.2,0.5, 0.7071068, 0, 0.7071068, 0 )
        target_pose_l_2=set_pose(0.5,0.2,0.5,  0, 0.7071068, 0, 0.7071068 )
        target_pose_r_2=set_pose(0.5,-0.4,0.5, 0.7071068, 0, 0.7071068, 0 )
        target_pose_l_3=set_pose(0.3,0.5,0.5,  0, 0, 0, 1 )
        target_pose_r_3=set_pose(0.3,-0.5,0.5,  0, 0, 0,1 )
 
        # 设置机械臂终端运动的目标位姿并规划执行运动
        rospy.loginfo("左臂第 1 个位置：")
        move(arm_l,target_pose_l_1, end_effector_link_l)
        rospy.loginfo("右臂第 1 个位置：")
        move(arm_r,target_pose_r_1, end_effector_link_r)
        rospy.loginfo("左臂第 2 个位置：")
        move(arm_l,target_pose_l_2, end_effector_link_l)
        rospy.loginfo("右臂第 2 个位置：")
        move(arm_r,target_pose_r_2, end_effector_link_r)
        rospy.loginfo("左臂第 3 个位置：")
        move(arm_l,target_pose_l_3, end_effector_link_l)
        rospy.loginfo("右臂第 3 个位置：")
        move(arm_r,target_pose_r_3, end_effector_link_r)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21

实际操作中，机械臂末端四元数的设置相对麻烦，需要频繁得计算确定，此处给出一种相对简单方案：
可以在Rviz界面中拖动机械臂调整末端位姿，在python代码中调用move_group.get_current_pose() 函数用来提取当前机械臂位姿信息，实现方式如下（可见于get_pose.py文件)

def show_pose(move_group,arm_l,arm_r):
     current_pose = move_group.get_current_pose()   
     rospy.loginfo(str(current_pose.pose.position.x)+","+
                   str(current_pose.pose.position.y)+","+
                   str(current_pose.pose.position.z)+","+
                   str(current_pose.pose.orientation.x)+","+
                   str(current_pose.pose.orientation.y)+","+
                   str(current_pose.pose.orientation.z)+","+
                   str(current_pose.pose.orientation.w))
1
2
3
4
5
6
7
8
9

终端输出的位姿信息可以直接用于设定期望末端位姿。
设定若干目标位姿后可实现如下效果：

注意：可以看到代码中分左右臂两部分编写，实际运行中，yumi机器人双臂也为先后运行，即无法做到同步运动，这是因为Moveit没有办法同时规划两个运动，即便使用两个终端同时运行两个脚本文件也无法实现（亲测，强行同时规划两个运动时，机器人双臂出现争夺现象且均无法正常运动），如此仿真完全无法发挥yumi双臂机器人优势，于是本文在下一章节给出双臂同时规划的方案。

绕八字运动

区别于定点运动，八字运动强调轨迹的连续性、确定性，所以使用笛卡尔空间轨迹规划法，沿期望轨迹连续采点，plan出整体的轨迹随后execute。此处为了实现圆周运动，使用简单的三角函数原理，同样若要画椭圆运动，对三角函数系数稍作修改即可。
现定义circle_move_yz函数用来实现机械臂圆周运动：

# circle_move 函数，参数为：move_group,target_pose,半径，圈数，方向（顺时针1，逆时针0），起始位置（圆心左1，圆心上2，圆心右3，圆心下4）
def circle_move_yz(move_group,target_pose,radius,circles,direction,start_pose):
     # 初始化路点列表
        waypoints = []       
        # 将圆弧上的路径点加入列表
        # waypoints.append(target_pose.pose)
        centerA = target_pose.pose.position.y # 圆心
        centerB = target_pose.pose.position.z
                                           
        for th in numpy.arange(0, circles*6.28, 0.02):
            if start_pose == 1:
             a = 3.14
            if start_pose == 2:
             a = 4.71
            if start_pose == 3:
             a = 0
            if start_pose == 4:
             a = 1.57 
            if direction == 0:# 逆时针
                target_pose.pose.position.y = centerA + radius * math.cos(th+a)
                target_pose.pose.position.z = centerB + radius * math.sin(th+a)
                wpose = deepcopy(target_pose.pose)
                waypoints.append(deepcopy(wpose))
            if direction ==1:# 顺时针
                target_pose.pose.position.y = centerA + radius * math.cos(th+a)
                target_pose.pose.position.z = centerB - radius * math.sin(th+a)
                wpose = deepcopy(target_pose.pose)
                waypoints.append(deepcopy(wpose))
       
        fraction = 0.0   #路径规划覆盖率
        maxtries= 100   #最大尝试规划次数
        attempts = 0     #已经尝试规划次数

        # 设置机器臂当前的状态作为运动初始状态
        move_group.set_start_state_to_current_state()
 
        # 尝试规划一条笛卡尔空间下的路径，依次通过所有路点，完成圆弧轨迹
        while fraction < 1.0 and attempts < maxtries:
            (plan, fraction) = move_group.compute_cartesian_path (
                                    waypoints,   # waypoint poses，路点列表
                                    0.01,        # eef_step，终端步进值
                                    0.0,         # jump_threshold，跳跃阈值
                                    True)        # avoid_collisions，避障规划
            
            # 尝试次数累加
            attempts += 1         
            # 打印运动规划进程
            if attempts % 10 == 0:
                rospy.loginfo("Still trying after " + str(attempts) + " attempts...")
        
        # 如果路径规划成功（覆盖率100%）,则开始控制机械臂运动
        if fraction == 1.0:
            rospy.loginfo("Path computed successfully. Moving the arm.")
            move_group.execute(plan)
            rospy.loginfo("Path execution complete.")
        # 如果路径规划失败，则打印失败信息
        else:
            rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.")  
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58

双臂分别设置末端执行器、末端位置，调用两次圆周函数模拟八字运动：

        target_pose_l_1=set_pose(0.5,0.4,0.5,  0, 0.7071068, 0, 0.7071068 )
        target_pose_r_1=set_pose(0.5,-0.2,0.5, 0.7071068, 0, 0.7071068, 0 )
        target_pose_l_2=set_pose(0.5,0.2,0.5,  0, 0.7071068, 0, 0.7071068 )
        target_pose_r_2=set_pose(0.5,-0.4,0.5, 0.7071068, 0, 0.7071068, 0 )
 
        # 设置机械臂终端运动的目标位姿
        arm_l.set_pose_target(target_pose_l_1, end_effector_link_l)
        arm_l.go()
        arm_r.set_pose_target(target_pose_r_1, end_effector_link_r)
        arm_r.go()

        # circle_move 函数，参数为：move_group,target_pose,半径，圈数，方向（顺时针1，逆时针0），起始位置（圆心左1，圆心上2，圆心右3，圆心下4）
        circle_move_yz(arm_l,target_pose_l_1,0.1,1,0,1)
        circle_move_yz(arm_r,target_pose_r_1,0.1,1,1,1)   
        circle_move_yz(arm_l,target_pose_l_2,0.1,1,1,3)
        circle_move_yz(arm_r,target_pose_r_2,0.1,1,0,3)   
        
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17

为函数选择合适的参数后实现效果如下（两臂交替运动完成两个8字效果）：

由于基本仿真无法同时规划运动两臂，所以本代码实现的效果是：左臂运动一周，右臂运动一周，重复一次各换另一圆周。

绕椭圆运动

对前述画圆函数稍作修改即可实现画椭圆效果：

至此，已经完成了yumi双臂机器人在Moveit和Gazebo中的通讯建立，实现两臂单独运动基于基本仿真平台对典型运动规划做尝试，下一章中将实现双臂协同工作。

二、双臂协同仿真

双臂协同仿真部分