Moveit+Gazebo：搭建双臂仿真平台（方案二）

已于 2023-02-06 18:49:55 修改
阅读量2k 收藏 43
点赞数 4
文章标签： 机器人 ubuntu linux
于 2023-02-06 18:43:43 首次发布
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.youkuaiyun.com/magic_invaid/article/details/128906171
版权

      演示视频：Moveit+Gazebo：搭建双臂仿真平台（方案二）_哔哩哔哩_bilibili

  实现效果如上图所示，有两个rviz界面分别展示两条臂的运动，gazebo中同步rviz中的运动。

        先说一下方案二的大致思路：在两个命名空间下启动两个move group节点分别控制左臂和右臂，然后启动gazebo并添加需要的控制器，最后编写中间节点实现moveit和gazebo的通信。

        不能通过之前单臂moveit+gazebo的方法（见往期博文）实现moveit+gazebo的联合仿真，因为在两个命名空间下，moveit和gazebo的通信受到影响，需要手动建立moveit到gazebo的通信。

        方案二具有自身的优点：能够并行的控制两个机械臂，解决了方案一中两个机械臂的规划总是同时开始同时结束的问题。

        方案二也具有问题，两个机械臂的move group节点是互相不知道对方的存在的，这可能导致碰撞的问题。

        下面开始介绍方案二的探索实现过程。

        首先正常启动机械臂 moveit_config包中的demo.launch

        通过命令rqt_graph查看节点图

         打开demo.launch文件，在图中所示的地方添加<group>标签，主要目的是添加命名空间namespace，使得可以启动多个move group节点。

        再次启动demo.launch文件，命令roslaunch right_arm_moveit_config demo.launch

        再次查看节点图，可以看到，demo.launch 中启动的所有节点都在right_arm命名空间下正常运行。这就说明同时启动左臂和右臂的move group是可行的。

 那么对于左臂我们也进行上述操作，同时启动左臂和右臂的demo.launch后，查看节点图。

        这里可以看到，除了/tf话题，其他节点和话题都在各自的命名空间下正常运行，互相没有影响，对于/tf话题，这里我的解决方法是左臂和右臂的URDF或XACRO中各自link和joint不能相同，相同就会使move group不能分辨，可以在编写urdf或xacro时在每个臂的link和joint前都加上right_arm或left_arm的前缀，在rviz中进行规划，可以看到两臂都能正常规划，二者没有互相影响，可以实现并行的控制。 

        这里给出左臂和右臂的urdf文件，除此之外还需要安装franka-description包，安装该包的操作以及用moveit setup assistant的操作这里不再赘述，有很多教程都方便查找，当然最推荐的是moveit的官方教程。

<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- |    This document was autogenerated by xacro from left_arm.urdf.xacro               | -->
<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
<!-- =================================================================================== -->
<robot name="left_arm">
  <!-- sub-link defining detailed meshes for collision with environment -->
  <link name="left_arm_link0">
    <visual>
      <geometry>
        <mesh filename="package://franka_description/meshes/visual/link0.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://franka_description/meshes/collision/link0.stl"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="-0.041018 -0.00014 0.049974"/>
      <mass value="0.629769"/>
      <inertia ixx="0.00315" ixy="8.2904e-07" ixz="0.00015" iyy="0.00388" iyz="8.2299e-06" izz="0.004285"/>
    </inertial>
  </link>
  <!-- sub-link defining coarse geometries of real robot's internal self-collision -->
  <link name="left_arm_link0_sc">
  </link>
  <!-- fixed joint between both sub-links -->
  <joint name="left_arm_link0_sc_joint" type="fixed">
    <origin rpy="0 0 0"/>
    <parent link="left_arm_link0"/>
    <child link="left_arm_link0_sc"/>
  </joint>
  <!-- sub-link defining detailed meshes for collision with environment -->
  <link name="left_arm_link1">
    <visual>
      <geometry>
        <mesh filename="package://franka_description/meshes/visual/link1.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://franka_description/meshes/collision/link1.stl"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.003875 0.002081 -0.04762"/>
      <mass value="4.970684"/>
      <inertia ixx="0.70337" ixy="-0.000139" ixz="0.006772" iyy="0.70661" iyz="0.019169" izz="0.009117"/>
    </inertial>
  </link>
  <!-- sub-link defining coarse geometries of real robot's internal self-collision -->
  <link name="left_arm_link1_sc">
  </link>
  <!-- fixed joint between both sub-links -->
  <joint name="left_arm_link1_sc_joint" type="fixed">
    <origin rpy="0 0 0"/>
    <parent link="left_arm_link1"/>
    <child link="left_arm_link1_sc"/>
  </joint>
  <joint name="left_arm_joint1" type="revolute">
    <origin rpy="0 0 0" xyz="0 0 0.333"/>
    <parent link="left_arm_link0"/>
    <child link="left_arm_link1"/>
    <axis xyz="0 0 1"/>
    <limit effort="87.0" lower="-2.8973" upper="2.8973" velocity="2.175"/>
    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
    <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
  </joint>
  <!-- sub-link defining detailed meshes for collision with environment -->
  <link name="left_arm_link2">
    <visual>
      <geometry>
        <mesh filename="package://franka_description/meshes/visual/link2.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://franka_description/meshes/collision/link2.stl"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="-0.003141 -0.02872  0.003495"/>
      <mass value="0.646926"/>
      <inertia ixx="0.007962" ixy="-0.003925" ixz="0.010254" iyy="0.02811" iyz="0.000704" izz="0.025995"/>
    </inertial>
  </link>
  <!-- sub-link defining coarse geometries of real robot's internal self-collision -->
  <link name="left_arm_link2_sc">
  </link>
  <!-- fixed joint between both sub-links -->
  <joint name="left_arm_link2_sc_joint" type="fixed">
    <origin rpy="0 0 0"/>
    <parent link="left_arm_link2"/>
    <child link="left_arm_link2_sc"/>
  </joint>
  <joint name="left_arm_joint2" type="revolute">
    <origin rpy="-1.5707963267948966 0 0" xyz="0 0 0"/>
    <parent link="left_arm_link1"/>
    <child link="left_arm_link2"/>
    <axis xyz="0 0 1"/>
    <limit effort="87.0" lower="-1.7628" upper="1.7628" velocity="2.175"/>
    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-1.7628" soft_upper_limit="1.7628"/>
    <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
  </joint>
  <!-- sub-link defining detailed meshes for collision with environment -->
  <link name="left_arm_link3">
    <visual>
      <geometry>
        <mesh filename="package://franka_description/meshes/visual/link3.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://franka_description/meshes/collision/link3.stl"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="2.7518e-02 3.9252e-02 -6.6502e-02"/>
      <mass value="3.228604"/>
      <inertia ixx="0.037242" ixy="-0.004761" ixz="-0.011396" iyy="0.036155" iyz="-0.012805" izz="0.01083"/>
    </inertial>
  </link>
  <!-- sub-link defining coarse geometries of real robot's internal self-collision -->
  <link name="left_arm_link3_sc">
  </link>
  <!-- fixed joint between both sub-links -->
  <joint name="left_arm_link3_sc_joint" type="fixed">
    <origin rpy="0 0 0"/>
    <parent link="left_arm_link3"/>
    <child link="left_arm_link3_sc"/>
  </joint>
  <joint name="left_arm_joint3" type="revolute">
    <origin rpy="1.5707963267948966 0 0" xyz="0 -0.316 0"/>
    <parent link="left_arm_link2"/>
    <child link="left_arm_link3"/>
    <axis xyz="0 0 1"/>
    <limit effort="87.0" lower="-2.8973" upper="2.8973" velocity="2.175"/>
    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
    <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
  </joint>
  <!-- sub-link defining detailed meshes for collision with environment -->
  <link name="left_arm_link4">
    <visual>
      <geometry>
        <mesh filename="package://franka_description/meshes/visual/link4.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://franka_description/meshes/collision/link4.stl"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="-5.317e-02 1.04419e-01 2.7454e-02"/>
      <mass value="3.587895"/>
      <inertia ixx="0.025853" ixy="0.007796" ixz="-0.001332" iyy="0.019552" iyz="0.008641" izz="0.028323"/>
    </inertial>
  </link>
  <!-- sub-link defining coarse geometries of real robot's internal self-collision -->
  <link name="left_arm_link4_sc">
  </link>
  <!-- fixed joint between both sub-links -->
  <joint name="left_arm_link4_sc_joint" type="fixed">
    <origin rpy="0 0 0"/>
    <parent link="left_arm_link4"/>
    <child link="left_arm_link4_sc"/>
  </joint>
  <joint name="left_arm_joint4" type="revolute">
    <origin rpy="1.5707963267948966 0 0" xyz="0.0825 0 0"/>
    <parent link="left_arm_link3"/>
    <child link="left_arm_link4"/>
    <axis xyz="0 0 1"/>
    <limit effort="87.0" lower="-3.0718" upper="-0.0698" velocity="2.175"/>
    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-3.0718" soft_upper_limit="-0.0698"/>
    <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
  </joint>
  <!-- sub-link defining detailed meshes for collision with environment -->
  <link name="left_arm_link5">
    <visual>
      <geometry>
        <mesh filename="package://franka_description/meshes/visual/link5.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://franka_description/meshes/collision/link5.stl"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="-1.1953e-02 4.1065e-02 -3.8437e-02"/>
      <mass value="1.225946"/>
      <inertia ixx="0.035549" ixy="-0.002117" ixz="-0.004037" iyy="0.029474" iyz="0.000229" izz="0.008627"/>
    </inertial>
  </link>
  <!-- sub-link defining coarse geometries of real robot's internal self-collision -->
  <link name="left_arm_link5_sc">
  </link>
  <!-- fixed joint between both sub-links -->
  <joint name="left_arm_link5_sc_joint" type="fixed">
    <origin rpy="0 0 0"/>
    <parent link="left_arm_link5"/>
    <child link="left_arm_link5_sc"/>
  </joint>
  <joint name="left_arm_joint5" type="revolute">
    <origin rpy="-1.5707963267948966 0 0" xyz="-0.0825 0.384 0"/>
    <parent link="left_arm_link4"/>
    <child link="left_arm_link5"/>
    <axis xyz="0 0 1"/>
    <limit effort="12.0" lower="-2.8973" upper="2.8973" velocity="2.61"/>
    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
    <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
  </joint>
  <!-- sub-link defining detailed meshes for collision with environment -->
  <link name="left_arm_link6">
    <visual>
      <geometry>
        <mesh filename="package://franka_description/meshes/visual/link6.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://franka_description/meshes/collision/link6.stl"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="6.0149e-02 -1.4117e-02 -1.0517e-02"/>
      <mass value="1.666555"/>
      <inertia ixx="0.001964" ixy="0.000109" ixz="-0.001158" iyy="0.004354" iyz="0.000341" izz="0.005433"/>
    </inertial>
  </link>
  <!-- sub-link defining coarse geometries of real robot's internal self-collision -->
  <link name="left_arm_link6_sc">
  </link>
  <!-- fixed joint between both sub-links -->
  <joint name="left_arm_link6_sc_joint" type="fixed">
    <origin rpy="0 0 0"/>
    <parent link="left_arm_link6"/>
    <child link="left_arm_link6_sc"/>
  </joint>
  <joint name="left_arm_joint6" type="revolute">
    <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
    <parent link="left_arm_link5"/>
    <child link="left_arm_link6"/>
    <axis xyz="0 0 1"/>
    <limit effort="12.0" lower="-0.0175" upper="3.7525" velocity="2.61"/>
    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-0.0175" soft_upper_limit="3.7525"/>
    <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
  </joint>
  <!-- sub-link defining detailed meshes for collision with environment -->
  <link name="left_arm_link7">
    <visual>
      <geometry>
        <mesh filename="package://franka_description/meshes/visual/link7.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://franka_description/meshes/collision/link7.stl"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="1.0517e-02 -4.252e-03 6.1597e-02"/>
      <mass value="0.735522"/>
      <inertia ixx="0.012516" ixy="-0.000428" ixz="-0.001196" iyy="0.010027" iyz="-0.000741" izz="0.004815"/>
    </inertial>
  </link>
  <!-- sub-link defining coarse geometries of real robot's internal self-collision -->
  <link name="left_arm_link7_sc">
  </link>
  <!-- fixed joint between both sub-links -->
  <joint name="left_arm_link7_sc_joint" type="fixed">
    <origin rpy="0 0 0.7853981633974483"/>
    <parent link="left_arm_link7"/>
    <child link="left_arm_link7_sc"/>
  </joint>
  <joint name="left_arm_joint7" type="revolute">
    <origin rpy="1.5707963267948966 0 0" xyz="0.088 0 0"/>
    <parent link="left_arm_link6"/>
    <child link="left_arm_link7"/>
    <axis xyz="0 0 1"/>
    <limit effort="12.0" lower="-2.8973" upper="2.8973" velocity="2.61"/>
    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
    <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
  </joint>
  <link name="left_arm_link8"/>
  <joint name="left_arm_joint8" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0.107"/>
    <parent link="left_arm_link7"/>
    <child link="left_arm_link8"/>
  </joint>
  <!-- Gazebo requires a joint to a link called "world" for statically mounted robots -->
  <link name="world"/>
  <joint name="world_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <parent link="world"/>
    <child link="left_arm_link0"/>
  </joint>
  <gazebo reference="left_arm_joint1">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="left_arm_joint1_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="left_arm_joint1">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="left_arm_joint1_motor">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo reference="left_arm_joint2">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="left_arm_joint2_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="left_arm_joint2">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="left_arm_joint2_motor">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo reference="left_arm_joint3">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="left_arm_joint3_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="left_arm_joint3">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="left_arm_joint3_motor">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo reference="left_arm_joint4">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="left_arm_joint4_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="left_arm_joint4">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="left_arm_joint4_motor">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo reference="left_arm_joint5">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="left_arm_joint5_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="left_arm_joint5">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="left_arm_joint5_motor">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo reference="left_arm_joint6">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="left_arm_joint6_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="left_arm_joint6">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="left_arm_joint6_motor">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo reference="left_arm_joint7">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="left_arm_joint7_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="left_arm_joint7">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="left_arm_joint7_motor">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo reference="left_arm_joint1">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="left_arm_joint1_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="left_arm_joint1">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="left_arm_joint1_motor">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo reference="left_arm_joint2">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="left_arm_joint2_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="left_arm_joint2">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="left_arm_joint2_motor">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo reference="left_arm_joint3">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="left_arm_joint3_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="left_arm_joint3">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="left_arm_joint3_motor">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo reference="left_arm_joint4">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="left_arm_joint4_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="left_arm_joint4">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="left_arm_joint4_motor">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo reference="left_arm_joint5">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="left_arm_joint5_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="left_arm_joint5">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="left_arm_joint5_motor">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo reference="left_arm_joint6">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="left_arm_joint6_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="left_arm_joint6">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="left_arm_joint6_motor">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo reference="left_arm_joint7">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="left_arm_joint7_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="left_arm_joint7">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="left_arm_joint7_motor">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo reference="left_arm_joint1">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="left_arm_joint1_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="left_arm_joint1">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="left_arm_joint1_motor">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo reference="left_arm_joint2">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="left_arm_joint2_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="left_arm_joint2">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="left_arm_joint2_motor">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo reference="left_arm_joint3">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="left_arm_joint3_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="left_arm_joint3">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="left_arm_joint3_motor">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo reference="left_arm_joint4">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="left_arm_joint4_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="left_arm_joint4">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="left_arm_joint4_motor">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo reference="left_arm_joint5">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="left_arm_joint5_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="left_arm_joint5">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="left_arm_joint5_motor">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo reference="left_arm_joint6">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="left_arm_joint6_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="left_arm_joint6">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="left_arm_joint6_motor">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo reference="left_arm_joint7">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="left_arm_joint7_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="left_arm_joint7">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="left_arm_joint7_motor">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <transmission name="left_arm_franka_state">
    <type>franka_hw/FrankaStateInterface</type>
    <joint name="left_arm_joint1">
      <hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface>
    </joint>
    <joint name="left_arm_joint2">
      <hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface>
    </joint>
    <joint name="left_arm_joint3">
      <hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface>
    </joint>
    <joint name="left_arm_joint4">
      <hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface>
    </joint>
    <joint name="left_arm_joint5">
      <hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface>
    </joint>
    <joint name="left_arm_joint6">
      <hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface>
    </joint>
    <joint name="left_arm_joint7">
      <hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface>
    </joint>
    <actuator name="left_arm_motor1">
      <hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface>
    </actuator>
    <actuator name="left_arm_motor2">
      <hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface>
    </actuator>
    <actuator name="left_arm_motor3">
      <hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface>
    </actuator>
    <actuator name="left_arm_motor4">
      <hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface>
    </actuator>
    <actuator name="left_arm_motor5">
      <hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface>
    </actuator>
    <actuator name="left_arm_motor6">
      <hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface>
    </actuator>
    <actuator name="left_arm_motor7">
      <hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface>
    </actuator>
  </transmission>
  <transmission name="left_arm_franka_model">
    <type>franka_hw/FrankaModelInterface</type>
    <joint name="left_arm_joint1">
      <role>root</role>
      <hardwareInterface>franka_hw/FrankaModelInterface</hardwareInterface>
    </joint>
    <joint name="left_arm_joint8">
      <role>tip</role>
      <hardwareInterface>franka_hw/FrankaModelInterface</hardwareInterface>
    </joint>
    <actuator name="left_arm_joint1_motor_root">
      <hardwareInterface>franka_hw/FrankaModelInterface</hardwareInterface>
    </actuator>
    <actuator name="left_arm_joint8_motor_tip">
      <hardwareInterface>franka_hw/FrankaModelInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo>
    <plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control">
      <controlPeriod>0.001</controlPeriod>
      <robotSimType>franka_gazebo/FrankaHWSim</robotSimType>
    </plugin>
    <self_collide>true</self_collide>
  </gazebo>
</robot>


<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- |    This document was autogenerated by xacro from right_arm.urdf.xacro               | -->
<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
<!-- =================================================================================== -->
<robot name="right_arm">
  <!-- sub-link defining detailed meshes for collision with environment -->
  <link name="right_arm_link0">
    <visual>
      <geometry>
        <mesh filename="package://franka_description/meshes/visual/link0.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://franka_description/meshes/collision/link0.stl"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="-0.041018 -0.00014 0.049974"/>
      <mass value="0.629769"/>
      <inertia ixx="0.00315" ixy="8.2904e-07" ixz="0.00015" iyy="0.00388" iyz="8.2299e-06" izz="0.004285"/>
    </inertial>
  </link>
  <!-- sub-link defining coarse geometries of real robot's internal self-collision -->
  <link name="right_arm_link0_sc">
  </link>
  <!-- fixed joint between both sub-links -->
  <joint name="right_arm_link0_sc_joint" type="fixed">
    <origin rpy="0 0 0"/>
    <parent link="right_arm_link0"/>
    <child link="right_arm_link0_sc"/>
  </joint>
  <!-- sub-link defining detailed meshes for collision with environment -->
  <link name="right_arm_link1">
    <visual>
      <geometry>
        <mesh filename="package://franka_description/meshes/visual/link1.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://franka_description/meshes/collision/link1.stl"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.003875 0.002081 -0.04762"/>
      <mass value="4.970684"/>
      <inertia ixx="0.70337" ixy="-0.000139" ixz="0.006772" iyy="0.70661" iyz="0.019169" izz="0.009117"/>
    </inertial>
  </link>
  <!-- sub-link defining coarse geometries of real robot's internal self-collision -->
  <link name="right_arm_link1_sc">
  </link>
  <!-- fixed joint between both sub-links -->
  <joint name="right_arm_link1_sc_joint" type="fixed">
    <origin rpy="0 0 0"/>
    <parent link="right_arm_link1"/>
    <child link="right_arm_link1_sc"/>
  </joint>
  <joint name="right_arm_joint1" type="revolute">
    <origin rpy="0 0 0" xyz="0 0 0.333"/>
    <parent link="right_arm_link0"/>
    <child link="right_arm_link1"/>
    <axis xyz="0 0 1"/>
    <limit effort="87.0" lower="-2.8973" upper="2.8973" velocity="2.175"/>
    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
    <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
  </joint>
  <!-- sub-link defining detailed meshes for collision with environment -->
  <link name="right_arm_link2">
    <visual>
      <geometry>
        <mesh filename="package://franka_description/meshes/visual/link2.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://franka_description/meshes/collision/link2.stl"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="-0.003141 -0.02872  0.003495"/>
      <mass value="0.646926"/>
      <inertia ixx="0.007962" ixy="-0.003925" ixz="0.010254" iyy="0.02811" iyz="0.000704" izz="0.025995"/>
    </inertial>
  </link>
  <!-- sub-link defining coarse geometries of real robot's internal self-collision -->
  <link name="right_arm_link2_sc">
  </link>
  <!-- fixed joint between both sub-links -->
  <joint name="right_arm_link2_sc_joint" type="fixed">
    <origin rpy="0 0 0"/>
    <parent link="right_arm_link2"/>
    <child link="right_arm_link2_sc"/>
  </joint>
  <joint name="right_arm_joint2" type="revolute">
    <origin rpy="-1.5707963267948966 0 0" xyz="0 0 0"/>
    <parent link="right_arm_link1"/>
    <child link="right_arm_link2"/>
    <axis xyz="0 0 1"/>
    <limit effort="87.0" lower="-1.7628" upper="1.7628" velocity="2.175"/>
    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-1.7628" soft_upper_limit="1.7628"/>
    <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
  </joint>
  <!-- sub-link defining detailed meshes for collision with environment -->
  <link name="right_arm_link3">
    <visual>
      <geometry>
        <mesh filename="package://franka_description/meshes/visual/link3.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://franka_description/meshes/collision/link3.stl"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="2.7518e-02 3.9252e-02 -6.6502e-02"/>
      <mass value="3.228604"/>
      <inertia ixx="0.037242" ixy="-0.004761" ixz="-0.011396" iyy="0.036155" iyz="-0.012805" izz="0.01083"/>
    </inertial>
  </link>
  <!-- sub-link defining coarse geometries of real robot's internal self-collision -->
  <link name="right_arm_link3_sc">
  </link>
  <!-- fixed joint between both sub-links -->
  <joint name="right_arm_link3_sc_joint" type="fixed">
    <origin rpy="0 0 0"/>
    <parent link="right_arm_link3"/>
    <child link="right_arm_link3_sc"/>
  </joint>
  <joint name="right_arm_joint3" type="revolute">
    <origin rpy="1.5707963267948966 0 0" xyz="0 -0.316 0"/>
    <parent link="right_arm_link2"/>
    <child link="right_arm_link3"/>
    <axis xyz="0 0 1"/>
    <limit effort="87.0" lower="-2.8973" upper="2.8973" velocity="2.175"/>
    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
    <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
  </joint>
  <!-- sub-link defining detailed meshes for collision with environment -->
  <link name="right_arm_link4">
    <visual>
      <geometry>
        <mesh filename="package://franka_description/meshes/visual/link4.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://franka_description/meshes/collision/link4.stl"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="-5.317e-02 1.04419e-01 2.7454e-02"/>
      <mass value="3.587895"/>
      <inertia ixx="0.025853" ixy="0.007796" ixz="-0.001332" iyy="0.019552" iyz="0.008641" izz="0.028323"/>
    </inertial>
  </link>
  <!-- sub-link defining coarse geometries of real robot's internal self-collision -->
  <link name="right_arm_link4_sc">
  </link>
  <!-- fixed joint between both sub-links -->
  <joint name="right_arm_link4_sc_joint" type="fixed">
    <origin rpy="0 0 0"/>
    <parent link="right_arm_link4"/>
    <child link="right_arm_link4_sc"/>
  </joint>
  <joint name="right_arm_joint4" type="revolute">
    <origin rpy="1.5707963267948966 0 0" xyz="0.0825 0 0"/>
    <parent link="right_arm_link3"/>
    <child link="right_arm_link4"/>
    <axis xyz="0 0 1"/>
    <limit effort="87.0" lower="-3.0718" upper="-0.0698" velocity="2.175"/>
    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-3.0718" soft_upper_limit="-0.0698"/>
    <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
  </joint>
  <!-- sub-link defining detailed meshes for collision with environment -->
  <link name="right_arm_link5">
    <visual>
      <geometry>
        <mesh filename="package://franka_description/meshes/visual/link5.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://franka_description/meshes/collision/link5.stl"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="-1.1953e-02 4.1065e-02 -3.8437e-02"/>
      <mass value="1.225946"/>
      <inertia ixx="0.035549" ixy="-0.002117" ixz="-0.004037" iyy="0.029474" iyz="0.000229" izz="0.008627"/>
    </inertial>
  </link>
  <!-- sub-link defining coarse geometries of real robot's internal self-collision -->
  <link name="right_arm_link5_sc">
  </link>
  <!-- fixed joint between both sub-links -->
  <joint name="right_arm_link5_sc_joint" type="fixed">
    <origin rpy="0 0 0"/>
    <parent link="right_arm_link5"/>
    <child link="right_arm_link5_sc"/>
  </joint>
  <joint name="right_arm_joint5" type="revolute">
    <origin rpy="-1.5707963267948966 0 0" xyz="-0.0825 0.384 0"/>
    <parent link="right_arm_link4"/>
    <child link="right_arm_link5"/>
    <axis xyz="0 0 1"/>
    <limit effort="12.0" lower="-2.8973" upper="2.8973" velocity="2.61"/>
    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
    <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
  </joint>
  <!-- sub-link defining detailed meshes for collision with environment -->
  <link name="right_arm_link6">
    <visual>
      <geometry>
        <mesh filename="package://franka_description/meshes/visual/link6.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://franka_description/meshes/collision/link6.stl"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="6.0149e-02 -1.4117e-02 -1.0517e-02"/>
      <mass value="1.666555"/>
      <inertia ixx="0.001964" ixy="0.000109" ixz="-0.001158" iyy="0.004354" iyz="0.000341" izz="0.005433"/>
    </inertial>
  </link>
  <!-- sub-link defining coarse geometries of real robot's internal self-collision -->
  <link name="right_arm_link6_sc">
  </link>
  <!-- fixed joint between both sub-links -->
  <joint name="right_arm_link6_sc_joint" type="fixed">
    <origin rpy="0 0 0"/>
    <parent link="right_arm_link6"/>
    <child link="right_arm_link6_sc"/>
  </joint>
  <joint name="right_arm_joint6" type="revolute">
    <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
    <parent link="right_arm_link5"/>
    <child link="right_arm_link6"/>
    <axis xyz="0 0 1"/>
    <limit effort="12.0" lower="-0.0175" upper="3.7525" velocity="2.61"/>
    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-0.0175" soft_upper_limit="3.7525"/>
    <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
  </joint>
  <!-- sub-link defining detailed meshes for collision with environment -->
  <link name="right_arm_link7">
    <visual>
      <geometry>
        <mesh filename="package://franka_description/meshes/visual/link7.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://franka_description/meshes/collision/link7.stl"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="1.0517e-02 -4.252e-03 6.1597e-02"/>
      <mass value="0.735522"/>
      <inertia ixx="0.012516" ixy="-0.000428" ixz="-0.001196" iyy="0.010027" iyz="-0.000741" izz="0.004815"/>
    </inertial>
  </link>
  <!-- sub-link defining coarse geometries of real robot's internal self-collision -->
  <link name="right_arm_link7_sc">
  </link>
  <!-- fixed joint between both sub-links -->
  <joint name="right_arm_link7_sc_joint" type="fixed">
    <origin rpy="0 0 0.7853981633974483"/>
    <parent link="right_arm_link7"/>
    <child link="right_arm_link7_sc"/>
  </joint>
  <joint name="right_arm_joint7" type="revolute">
    <origin rpy="1.5707963267948966 0 0" xyz="0.088 0 0"/>
    <parent link="right_arm_link6"/>
    <child link="right_arm_link7"/>
    <axis xyz="0 0 1"/>
    <limit effort="12.0" lower="-2.8973" upper="2.8973" velocity="2.61"/>
    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
    <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
  </joint>
  <link name="right_arm_link8"/>
  <joint name="right_arm_joint8" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0.107"/>
    <parent link="right_arm_link7"/>
    <child link="right_arm_link8"/>
  </joint>
  <!-- Gazebo requires a joint to a link called "world" for statically mounted robots -->
  <link name="world"/>
  <joint name="world_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <parent link="world"/>
    <child link="right_arm_link0"/>
  </joint>
  <gazebo reference="right_arm_joint1">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="right_arm_joint1_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="right_arm_joint1">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="right_arm_joint1_motor">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo reference="right_arm_joint2">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="right_arm_joint2_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="right_arm_joint2">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="right_arm_joint2_motor">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo reference="right_arm_joint3">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="right_arm_joint3_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="right_arm_joint3">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="right_arm_joint3_motor">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo reference="right_arm_joint4">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="right_arm_joint4_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="right_arm_joint4">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="right_arm_joint4_motor">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo reference="right_arm_joint5">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="right_arm_joint5_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="right_arm_joint5">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="right_arm_joint5_motor">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo reference="right_arm_joint6">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="right_arm_joint6_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="right_arm_joint6">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="right_arm_joint6_motor">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo reference="right_arm_joint7">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="right_arm_joint7_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="right_arm_joint7">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="right_arm_joint7_motor">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo reference="right_arm_joint1">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="right_arm_joint1_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="right_arm_joint1">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="right_arm_joint1_motor">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo reference="right_arm_joint2">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="right_arm_joint2_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="right_arm_joint2">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="right_arm_joint2_motor">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo reference="right_arm_joint3">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="right_arm_joint3_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="right_arm_joint3">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="right_arm_joint3_motor">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo reference="right_arm_joint4">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="right_arm_joint4_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="right_arm_joint4">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="right_arm_joint4_motor">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo reference="right_arm_joint5">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="right_arm_joint5_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="right_arm_joint5">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="right_arm_joint5_motor">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo reference="right_arm_joint6">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="right_arm_joint6_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="right_arm_joint6">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="right_arm_joint6_motor">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo reference="right_arm_joint7">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="right_arm_joint7_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="right_arm_joint7">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="right_arm_joint7_motor">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo reference="right_arm_joint1">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="right_arm_joint1_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="right_arm_joint1">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="right_arm_joint1_motor">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo reference="right_arm_joint2">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="right_arm_joint2_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="right_arm_joint2">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="right_arm_joint2_motor">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo reference="right_arm_joint3">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="right_arm_joint3_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="right_arm_joint3">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="right_arm_joint3_motor">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo reference="right_arm_joint4">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="right_arm_joint4_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="right_arm_joint4">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="right_arm_joint4_motor">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo reference="right_arm_joint5">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="right_arm_joint5_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="right_arm_joint5">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="right_arm_joint5_motor">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo reference="right_arm_joint6">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="right_arm_joint6_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="right_arm_joint6">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="right_arm_joint6_motor">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo reference="right_arm_joint7">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="right_arm_joint7_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="right_arm_joint7">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="right_arm_joint7_motor">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <transmission name="right_arm_franka_state">
    <type>franka_hw/FrankaStateInterface</type>
    <joint name="right_arm_joint1">
      <hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface>
    </joint>
    <joint name="right_arm_joint2">
      <hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface>
    </joint>
    <joint name="right_arm_joint3">
      <hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface>
    </joint>
    <joint name="right_arm_joint4">
      <hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface>
    </joint>
    <joint name="right_arm_joint5">
      <hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface>
    </joint>
    <joint name="right_arm_joint6">
      <hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface>
    </joint>
    <joint name="right_arm_joint7">
      <hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface>
    </joint>
    <actuator name="right_arm_motor1">
      <hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface>
    </actuator>
    <actuator name="right_arm_motor2">
      <hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface>
    </actuator>
    <actuator name="right_arm_motor3">
      <hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface>
    </actuator>
    <actuator name="right_arm_motor4">
      <hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface>
    </actuator>
    <actuator name="right_arm_motor5">
      <hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface>
    </actuator>
    <actuator name="right_arm_motor6">
      <hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface>
    </actuator>
    <actuator name="right_arm_motor7">
      <hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface>
    </actuator>
  </transmission>
  <transmission name="right_arm_franka_model">
    <type>franka_hw/FrankaModelInterface</type>
    <joint name="right_arm_joint1">
      <role>root</role>
      <hardwareInterface>franka_hw/FrankaModelInterface</hardwareInterface>
    </joint>
    <joint name="right_arm_joint8">
      <role>tip</role>
      <hardwareInterface>franka_hw/FrankaModelInterface</hardwareInterface>
    </joint>
    <actuator name="right_arm_joint1_motor_root">
      <hardwareInterface>franka_hw/FrankaModelInterface</hardwareInterface>
    </actuator>
    <actuator name="right_arm_joint8_motor_tip">
      <hardwareInterface>franka_hw/FrankaModelInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo>
    <plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control">
      <controlPeriod>0.001</controlPeriod>
      <robotSimType>franka_gazebo/FrankaHWSim</robotSimType>
    </plugin>
    <self_collide>true</self_collide>
  </gazebo>
</robot>

        下一步我们来解决gazebo，先给出launch文件,这个launch文件的主要作用就是启动gazebo，加载机器人的模型，加载控制器。

<?xml version="1.0"?>
<launch>
    <!-- Launch empty Gazebo world -->
    <include file="$(find gazebo_ros)/launch/empty_world.launch">
        <arg name="use_sim_time" value="true" />
        <arg name="gui" value="true" />
        <arg name="paused" value="false" />
        <arg name="debug" value="false" />
    </include>
    
    <param name="robot_description" command="$(find xacro)/xacro  '$(find panda_dual_arms)/robot_description/dual_panda_without_hand.urdf'" />
    <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen" args="-urdf -param robot_description -model panda_dual_arms" />

    <rosparam file="$(find panda_dual_arms)/config/dual_arms_gazebo_controller.yaml" command="load" />
    <node name="controller_spawner" pkg="controller_manager" type="spawner"  args="joint_state_controller right_arm_trajectory_controller left_arm_trajectory_controller" respawn="false" output="screen" />

    <!-- Robot state publisher -->
    <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
        <param name="publish_frequency" type="double" value="50.0" />
        <param name="tf_prefix" type="string" value="" />
    </node>

</launch>

        注意这里加载的机器人模型是在一个文件中编写了双臂(包括一个简单的工作台)，而之前moveit setup assitant生成的moveit config包，我们用了两个文件编写两个机械臂的urdf。

下面给出这里需要的文件。

<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- |    This document was autogenerated by xacro from dual_panda_without_hand.urdf.xacro | -->
<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
<!-- =================================================================================== -->
<robot name="panda_multiple_arms">
  <link name="world"/>
  <!-- box shaped table as base for the 2 Pandas -->
  <link name="base">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0.5"/>
      <geometry>
        <box size="1 2 1"/>
      </geometry>
      <material name="White">
        <color rgba="1.0 1.0 1.0 1.0"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.5"/>
      <geometry>
        <box size="1 2 1"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <mass value="10.0"/>
      <inertia ixx="0.001" ixy="0.0" ixz="0.001" iyy="0.0" iyz="0.0" izz="0.001"/>
    </inertial>
  </link>
  <joint name="base_to_world" type="fixed">
    <parent link="world"/>
    <child link="base"/>
    <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
  </joint>
  <joint name="d_right_arm_joint_base" type="fixed">
    <parent link="base"/>
    <child link="d_right_arm_link0"/>
    <origin rpy="0 0 0" xyz="0 -0.5 1"/>
  </joint>
  <!-- sub-link defining detailed meshes for collision with environment -->
  <link name="d_right_arm_link0">
    <visual>
      <geometry>
        <mesh filename="package://franka_description/meshes/visual/link0.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://franka_description/meshes/collision/link0.stl"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="-0.041018 -0.00014 0.049974"/>
      <mass value="0.629769"/>
      <inertia ixx="0.00315" ixy="8.2904e-07" ixz="0.00015" iyy="0.00388" iyz="8.2299e-06" izz="0.004285"/>
    </inertial>
  </link>
  <!-- sub-link defining coarse geometries of real robot's internal self-collision -->
  <link name="d_right_arm_link0_sc">
  </link>
  <!-- fixed joint between both sub-links -->
  <joint name="d_right_arm_link0_sc_joint" type="fixed">
    <origin rpy="0 0 0"/>
    <parent link="d_right_arm_link0"/>
    <child link="d_right_arm_link0_sc"/>
  </joint>
  <!-- sub-link defining detailed meshes for collision with environment -->
  <link name="d_right_arm_link1">
    <visual>
      <geometry>
        <mesh filename="package://franka_description/meshes/visual/link1.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://franka_description/meshes/collision/link1.stl"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.003875 0.002081 -0.04762"/>
      <mass value="4.970684"/>
      <inertia ixx="0.70337" ixy="-0.000139" ixz="0.006772" iyy="0.70661" iyz="0.019169" izz="0.009117"/>
    </inertial>
  </link>
  <!-- sub-link defining coarse geometries of real robot's internal self-collision -->
  <link name="d_right_arm_link1_sc">
  </link>
  <!-- fixed joint between both sub-links -->
  <joint name="d_right_arm_link1_sc_joint" type="fixed">
    <origin rpy="0 0 0"/>
    <parent link="d_right_arm_link1"/>
    <child link="d_right_arm_link1_sc"/>
  </joint>
  <joint name="d_right_arm_joint1" type="revolute">
    <origin rpy="0 0 0" xyz="0 0 0.333"/>
    <parent link="d_right_arm_link0"/>
    <child link="d_right_arm_link1"/>
    <axis xyz="0 0 1"/>
    <limit effort="87.0" lower="-2.8973" upper="2.8973" velocity="2.175"/>
    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
    <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
  </joint>
  <!-- sub-link defining detailed meshes for collision with environment -->
  <link name="d_right_arm_link2">
    <visual>
      <geometry>
        <mesh filename="package://franka_description/meshes/visual/link2.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://franka_description/meshes/collision/link2.stl"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="-0.003141 -0.02872  0.003495"/>
      <mass value="0.646926"/>
      <inertia ixx="0.007962" ixy="-0.003925" ixz="0.010254" iyy="0.02811" iyz="0.000704" izz="0.025995"/>
    </inertial>
  </link>
  <!-- sub-link defining coarse geometries of real robot's internal self-collision -->
  <link name="d_right_arm_link2_sc">
  </link>
  <!-- fixed joint between both sub-links -->
  <joint name="d_right_arm_link2_sc_joint" type="fixed">
    <origin rpy="0 0 0"/>
    <parent link="d_right_arm_link2"/>
    <child link="d_right_arm_link2_sc"/>
  </joint>
  <joint name="d_right_arm_joint2" type="revolute">
    <origin rpy="-1.5707963267948966 0 0" xyz="0 0 0"/>
    <parent link="d_right_arm_link1"/>
    <child link="d_right_arm_link2"/>
    <axis xyz="0 0 1"/>
    <limit effort="87.0" lower="-1.7628" upper="1.7628" velocity="2.175"/>
    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-1.7628" soft_upper_limit="1.7628"/>
    <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
  </joint>
  <!-- sub-link defining detailed meshes for collision with environment -->
  <link name="d_right_arm_link3">
    <visual>
      <geometry>
        <mesh filename="package://franka_description/meshes/visual/link3.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://franka_description/meshes/collision/link3.stl"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="2.7518e-02 3.9252e-02 -6.6502e-02"/>
      <mass value="3.228604"/>
      <inertia ixx="0.037242" ixy="-0.004761" ixz="-0.011396" iyy="0.036155" iyz="-0.012805" izz="0.01083"/>
    </inertial>
  </link>
  <!-- sub-link defining coarse geometries of real robot's internal self-collision -->
  <link name="d_right_arm_link3_sc">
  </link>
  <!-- fixed joint between both sub-links -->
  <joint name="d_right_arm_link3_sc_joint" type="fixed">
    <origin rpy="0 0 0"/>
    <parent link="d_right_arm_link3"/>
    <child link="d_right_arm_link3_sc"/>
  </joint>
  <joint name="d_right_arm_joint3" type="revolute">
    <origin rpy="1.5707963267948966 0 0" xyz="0 -0.316 0"/>
    <parent link="d_right_arm_link2"/>
    <child link="d_right_arm_link3"/>
    <axis xyz="0 0 1"/>
    <limit effort="87.0" lower="-2.8973" upper="2.8973" velocity="2.175"/>
    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
    <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
  </joint>
  <!-- sub-link defining detailed meshes for collision with environment -->
  <link name="d_right_arm_link4">
    <visual>
      <geometry>
        <mesh filename="package://franka_description/meshes/visual/link4.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://franka_description/meshes/collision/link4.stl"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="-5.317e-02 1.04419e-01 2.7454e-02"/>
      <mass value="3.587895"/>
      <inertia ixx="0.025853" ixy="0.007796" ixz="-0.001332" iyy="0.019552" iyz="0.008641" izz="0.028323"/>
    </inertial>
  </link>
  <!-- sub-link defining coarse geometries of real robot's internal self-collision -->
  <link name="d_right_arm_link4_sc">
  </link>
  <!-- fixed joint between both sub-links -->
  <joint name="d_right_arm_link4_sc_joint" type="fixed">
    <origin rpy="0 0 0"/>
    <parent link="d_right_arm_link4"/>
    <child link="d_right_arm_link4_sc"/>
  </joint>
  <joint name="d_right_arm_joint4" type="revolute">
    <origin rpy="1.5707963267948966 0 0" xyz="0.0825 0 0"/>
    <parent link="d_right_arm_link3"/>
    <child link="d_right_arm_link4"/>
    <axis xyz="0 0 1"/>
    <limit effort="87.0" lower="-3.0718" upper="-0.0698" velocity="2.175"/>
    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-3.0718" soft_upper_limit="-0.0698"/>
    <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
  </joint>
  <!-- sub-link defining detailed meshes for collision with environment -->
  <link name="d_right_arm_link5">
    <visual>
      <geometry>
        <mesh filename="package://franka_description/meshes/visual/link5.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://franka_description/mes<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- |    This document was autogenerated by xacro from dual_panda_without_hand.urdf.xacro | -->
<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
<!-- =================================================================================== -->
<robot name="panda_multiple_arms">
  <link name="world"/>
  <!-- box shaped table as base for the 2 Pandas -->
  <link name="base">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0.5"/>
      <geometry>
        <box size="1 2 1"/>
      </geometry>
      <material name="White">
        <color rgba="1.0 1.0 1.0 1.0"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.5"/>
      <geometry>
        <box size="1 2 1"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <mass value="10.0"/>
      <inertia ixx="0.001" ixy="0.0" ixz="0.001" iyy="0.0" iyz="0.0" izz="0.001"/>
    </inertial>
  </link>
  <joint name="base_to_world" type="fixed">
    <parent link="world"/>
    <child link="base"/>
    <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
  </joint>
  <joint name="d_right_arm_joint_base" type="fixed">
    <parent link="base"/>
    <child link="d_right_arm_link0"/>
    <origin rpy="0 0 0" xyz="0 -0.5 1"/>
  </joint>
  <!-- sub-link defining detailed meshes for collision with environment -->
  <link name="d_right_arm_link0">
    <visual>
      <geometry>
        <mesh filename="package://franka_description/meshes/visual/link0.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://franka_description/meshes/collision/link0.stl"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="-0.041018 -0.00014 0.049974"/>
      <mass value="0.629769"/>
      <inertia ixx="0.00315" ixy="8.2904e-07" ixz="0.00015" iyy="0.00388" iyz="8.2299e-06" izz="0.004285"/>
    </inertial>
  </link>
  <!-- sub-link defining coarse geometries of real robot's internal self-collision -->
  <link name="d_right_arm_link0_sc">
  </link>
  <!-- fixed joint between both sub-links -->
  <joint name="d_right_arm_link0_sc_joint" type="fixed">
    <origin rpy="0 0 0"/>
    <parent link="d_right_arm_link0"/>
    <child link="d_right_arm_link0_sc"/>
  </joint>
  <!-- sub-link defining detailed meshes for collision with environment -->
  <link name="d_right_arm_link1">
    <visual>
      <geometry>
        <mesh filename="package://franka_description/meshes/visual/link1.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://franka_description/meshes/collision/link1.stl"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.003875 0.002081 -0.04762"/>
      <mass value="4.970684"/>
      <inertia ixx="0.70337" ixy="-0.000139" ixz="0.006772" iyy="0.70661" iyz="0.019169" izz="0.009117"/>
    </inertial>
  </link>
  <!-- sub-link defining coarse geometries of real robot's internal self-collision -->
  <link name="d_right_arm_link1_sc">
  </link>
  <!-- fixed joint between both sub-links -->
  <joint name="d_right_arm_link1_sc_joint" type="fixed">
    <origin rpy="0 0 0"/>
    <parent link="d_right_arm_link1"/>
    <child link="d_right_arm_link1_sc"/>
  </joint>
  <joint name="d_right_arm_joint1" type="revolute">
    <origin rpy="0 0 0" xyz="0 0 0.333"/>
    <parent link="d_right_arm_link0"/>
    <child link="d_right_arm_link1"/>
    <axis xyz="0 0 1"/>
    <limit effort="87.0" lower="-2.8973" upper="2.8973" velocity="2.175"/>
    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
    <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
  </joint>
  <!-- sub-link defining detailed meshes for collision with environment -->
  <link name="d_right_arm_link2">
    <visual>
      <geometry>
        <mesh filename="package://franka_description/meshes/visual/link2.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://franka_description/meshes/collision/link2.stl"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="-0.003141 -0.02872  0.003495"/>
      <mass value="0.646926"/>
      <inertia ixx="0.007962" ixy="-0.003925" ixz="0.010254" iyy="0.02811" iyz="0.000704" izz="0.025995"/>
    </inertial>
  </link>
  <!-- sub-link defining coarse geometries of real robot's internal self-collision -->
  <link name="d_right_arm_link2_sc">
  </link>
  <!-- fixed joint between both sub-links -->
  <joint name="d_right_arm_link2_sc_joint" type="fixed">
    <origin rpy="0 0 0"/>
    <parent link="d_right_arm_link2"/>
    <child link="d_right_arm_link2_sc"/>
  </joint>
  <joint name="d_right_arm_joint2" type="revolute">
    <origin rpy="-1.5707963267948966 0 0" xyz="0 0 0"/>
    <parent link="d_right_arm_link1"/>
    <child link="d_right_arm_link2"/>
    <axis xyz="0 0 1"/>
    <limit effort="87.0" lower="-1.7628" upper="1.7628" velocity="2.175"/>
    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-1.7628" soft_upper_limit="1.7628"/>
    <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
  </joint>
  <!-- sub-link defining detailed meshes for collision with environment -->
  <link name="d_right_arm_link3">
    <visual>
      <geometry>
        <mesh filename="package://franka_description/meshes/visual/link3.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://franka_description/meshes/collision/link3.stl"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="2.7518e-02 3.9252e-02 -6.6502e-02"/>
      <mass value="3.228604"/>
      <inertia ixx="0.037242" ixy="-0.004761" ixz="-0.011396" iyy="0.036155" iyz="-0.012805" izz="0.01083"/>
    </inertial>
  </link>
  <!-- sub-link defining coarse geometries of real robot's internal self-collision -->
  <link name="d_right_arm_link3_sc">
  </link>
  <!-- fixed joint between both sub-links -->
  <joint name="d_right_arm_link3_sc_joint" type="fixed">
    <origin rpy="0 0 0"/>
    <parent link="d_right_arm_link3"/>
    <child link="d_right_arm_link3_sc"/>
  </joint>
  <joint name="d_right_arm_joint3" type="revolute">
    <origin rpy="1.5707963267948966 0 0" xyz="0 -0.316 0"/>
    <parent link="d_right_arm_link2"/>
    <child link="d_right_arm_link3"/>
    <axis xyz="0 0 1"/>
    <limit effort="87.0" lower="-2.8973" upper="2.8973" velocity="2.175"/>
    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
    <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
  </joint>
  <!-- sub-link defining detailed meshes for collision with environment -->
  <link name="d_right_arm_link4">
    <visual>
      <geometry>
        <mesh filename="package://franka_description/meshes/visual/link4.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://franka_description/meshes/collision/link4.stl"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="-5.317e-02 1.04419e-01 2.7454e-02"/>
      <mass value="3.587895"/>
      <inertia ixx="0.025853" ixy="0.007796" ixz="-0.001332" iyy="0.019552" iyz="0.008641" izz="0.028323"/>
    </inertial>
  </link>
  <!-- sub-link defining coarse geometries of real robot's internal self-collision -->
  <link name="d_right_arm_link4_sc">
  </link>
  <!-- fixed joint between both sub-links -->
  <joint name="d_right_arm_link4_sc_joint" type="fixed">
    <origin rpy="0 0 0"/>
    <parent link="d_right_arm_link4"/>
    <child link="d_right_arm_link4_sc"/>
  </joint>
  <joint name="d_right_arm_joint4" type="revolute">
    <origin rpy="1.5707963267948966 0 0" xyz="0.0825 0 0"/>
    <parent link="d_right_arm_link3"/>
    <child link="d_right_arm_link4"/>
    <axis xyz="0 0 1"/>
    <limit effort="87.0" lower="-3.0718" upper="-0.0698" velocity="2.175"/>
    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-3.0718" soft_upper_limit="-0.0698"/>
    <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
  </joint>
  <!-- sub-link defining detailed meshes for collision with environment -->
  <link name="d_right_arm_link5">
    <visual>
      <geometry>
        <mesh filename="package://franka_description/meshes/visual/link5.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://franka_description/meshes/collision/link5.stl"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="-1.1953e-02 4.1065e-02 -3.8437e-02"/>
      <mass value="1.225946"/>
      <inertia ixx="0.035549" ixy="-0.002117" ixz="-0.004037" iyy="0.029474" iyz="0.000229" izz="0.008627"/>
    </inertial>
  </link>
  <!-- sub-link defining coarse geometries of real robot's internal self-collision -->
  <link name="d_right_arm_link5_sc">
  </link>
  <!-- fixed joint between both sub-links -->
  <joint name="d_right_arm_link5_sc_joint" type="fixed">
    <origin rpy="0 0 0"/>
    <parent link="d_right_arm_link5"/>
    <child link="d_right_arm_link5_sc"/>
  </joint>
  <joint name="d_right_arm_joint5" type="revolute">
    <origin rpy="-1.5707963267948966 0 0" xyz="-0.0825 0.384 0"/>
    <parent link="d_right_arm_link4"/>
    <child link="d_right_arm_link5"/>
    <axis xyz="0 0 1"/>
    <limit effort="12.0" lower="-2.8973" upper="2.8973" velocity="2.61"/>
    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
    <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
  </joint>
  <!-- sub-link defining detailed meshes for collision with environment -->
  <link name="d_right_arm_link6">
    <visual>
      <geometry>
        <mesh filename="package://franka_description/meshes/visual/link6.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://franka_description/meshes/collision/link6.stl"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="6.0149e-02 -1.4117e-02 -1.0517e-02"/>
      <mass value="1.666555"/>
      <inertia ixx="0.001964" ixy="0.000109" ixz="-0.001158" iyy="0.004354" iyz="0.000341" izz="0.005433"/>
    </inertial>
  </link>
  <!-- sub-link defining coarse geometries of real robot's internal self-collision -->
  <link name="d_right_arm_link6_sc">
  </link>
  <!-- fixed joint between both sub-links -->
  <joint name="d_right_arm_link6_sc_joint" type="fixed">
    <origin rpy="0 0 0"/>
    <parent link="d_right_arm_link6"/>
    <child link="d_right_arm_link6_sc"/>
  </joint>
  <joint name="d_right_arm_joint6" type="revolute">
    <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
    <parent link="d_right_arm_link5"/>
    <child link="d_right_arm_link6"/>
    <axis xyz="0 0 1"/>
    <limit effort="12.0" lower="-0.0175" upper="3.7525" velocity="2.61"/>
    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-0.0175" soft_upper_limit="3.7525"/>
    <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
  </joint>
  <!-- sub-link defining detailed meshes for collision with environment -->
  <link name="d_right_arm_link7">
    <visual>
      <geometry>
        <mesh filename="package://franka_description/meshes/visual/link7.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://franka_description/meshes/collision/link7.stl"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="1.0517e-02 -4.252e-03 6.1597e-02"/>
      <mass value="0.735522"/>
      <inertia ixx="0.012516" ixy="-0.000428" ixz="-0.001196" iyy="0.010027" iyz="-0.000741" izz="0.004815"/>
    </inertial>
  </link>
  <!-- sub-link defining coarse geometries of real robot's internal self-collision -->
  <link name="d_right_arm_link7_sc">
  </link>
  <!-- fixed joint between both sub-links -->
  <joint name="d_right_arm_link7_sc_joint" type="fixed">
    <origin rpy="0 0 0.7853981633974483"/>
    <parent link="d_right_arm_link7"/>
    <child link="d_right_arm_link7_sc"/>
  </joint>
  <joint name="d_right_arm_joint7" type="revolute">
    <origin rpy="1.5707963267948966 0 0" xyz="0.088 0 0"/>
    <parent link="d_right_arm_link6"/>
    <child link="d_right_arm_link7"/>
    <axis xyz="0 0 1"/>
    <limit effort="12.0" lower="-2.8973" upper="2.8973" velocity="2.61"/>
    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
    <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
  </joint>
  <link name="d_right_arm_link8"/>
  <joint name="d_right_arm_joint8" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0.107"/>
    <parent link="d_right_arm_link7"/>
    <child link="d_right_arm_link8"/>
  </joint>
  <joint name="d_left_arm_joint_base" type="fixed">
    <parent link="base"/>
    <child link="d_left_arm_link0"/>
    <origin rpy="0 0 0" xyz="0 0.5 1"/>
  </joint>
  <!-- sub-link defining detailed meshes for collision with environment -->
  <link name="d_left_arm_link0">
    <visual>
      <geometry>
        <mesh filename="package://franka_description/meshes/visual/link0.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://franka_description/meshes/collision/link0.stl"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="-0.041018 -0.00014 0.049974"/>
      <mass value="0.629769"/>
      <inertia ixx="0.00315" ixy="8.2904e-07" ixz="0.00015" iyy="0.00388" iyz="8.2299e-06" izz="0.004285"/>
    </inertial>
  </link>
  <!-- sub-link defining coarse geometries of real robot's internal self-collision -->
  <link name="d_left_arm_link0_sc">
  </link>
  <!-- fixed joint between both sub-links -->
  <joint name="d_left_arm_link0_sc_joint" type="fixed">
    <origin rpy="0 0 0"/>
    <parent link="d_left_arm_link0"/>
    <child link="d_left_arm_link0_sc"/>
  </joint>
  <!-- sub-link defining detailed meshes for collision with environment -->
  <link name="d_left_arm_link1">
    <visual>
      <geometry>
        <mesh filename="package://franka_description/meshes/visual/link1.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://franka_description/meshes/collision/link1.stl"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.003875 0.002081 -0.04762"/>
      <mass value="4.970684"/>
      <inertia ixx="0.70337" ixy="-0.000139" ixz="0.006772" iyy="0.70661" iyz="0.019169" izz="0.009117"/>
    </inertial>
  </link>
  <!-- sub-link defining coarse geometries of real robot's internal self-collision -->
  <link name="d_left_arm_link1_sc">
  </link>
  <!-- fixed joint between both sub-links -->
  <joint name="d_left_arm_link1_sc_joint" type="fixed">
    <origin rpy="0 0 0"/>
    <parent link="d_left_arm_link1"/>
    <child link="d_left_arm_link1_sc"/>
  </joint>
  <joint name="d_left_arm_joint1" type="revolute">
    <origin rpy="0 0 0" xyz="0 0 0.333"/>
    <parent link="d_left_arm_link0"/>
    <child link="d_left_arm_link1"/>
    <axis xyz="0 0 1"/>
    <limit effort="87.0" lower="-2.8973" upper="2.8973" velocity="2.175"/>
    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
    <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
  </joint>
  <!-- sub-link defining detailed meshes for collision with environment -->
  <link name="d_left_arm_link2">
    <visual>
      <geometry>
        <mesh filename="package://franka_description/meshes/visual/link2.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://franka_description/meshes/collision/link2.stl"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="-0.003141 -0.02872  0.003495"/>
      <mass value="0.646926"/>
      <inertia ixx="0.007962" ixy="-0.003925" ixz="0.010254" iyy="0.02811" iyz="0.000704" izz="0.025995"/>
    </inertial>
  </link>
  <!-- sub-link defining coarse geometries of real robot's internal self-collision -->
  <link name="d_left_arm_link2_sc">
  </link>
  <!-- fixed joint between both sub-links -->
  <joint name="d_left_arm_link2_sc_joint" type="fixed">
    <origin rpy="0 0 0"/>
    <parent link="d_left_arm_link2"/>
    <child link="d_left_arm_link2_sc"/>
  </joint>
  <joint name="d_left_arm_joint2" type="revolute">
    <origin rpy="-1.5707963267948966 0 0" xyz="0 0 0"/>
    <parent link="d_left_arm_link1"/>
    <child link="d_left_arm_link2"/>
    <axis xyz="0 0 1"/>
    <limit effort="87.0" lower="-1.7628" upper="1.7628" velocity="2.175"/>
    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-1.7628" soft_upper_limit="1.7628"/>
    <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
  </joint>
  <!-- sub-link defining detailed meshes for collision with environment -->
  <link name="d_left_arm_link3">
    <visual>
      <geometry>
        <mesh filename="package://franka_description/meshes/visual/link3.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://franka_description/meshes/collision/link3.stl"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="2.7518e-02 3.9252e-02 -6.6502e-02"/>
      <mass value="3.228604"/>
      <inertia ixx="0.037242" ixy="-0.004761" ixz="-0.011396" iyy="0.036155" iyz="-0.012805" izz="0.01083"/>
    </inertial>
  </link>
  <!-- sub-link defining coarse geometries of real robot's internal self-collision -->
  <link name="d_left_arm_link3_sc">
  </link>
  <!-- fixed joint between both sub-links -->
  <joint name="d_left_arm_link3_sc_joint" type="fixed">
    <origin rpy="0 0 0"/>
    <parent link="d_left_arm_link3"/>
    <child link="d_left_arm_link3_sc"/>
  </joint>
  <joint name="d_left_arm_joint3" type="revolute">
    <origin rpy="1.5707963267948966 0 0" xyz="0 -0.316 0"/>
    <parent link="d_left_arm_link2"/>
    <child link="d_left_arm_link3"/>
    <axis xyz="0 0 1"/>
    <limit effort="87.0" lower="-2.8973" upper="2.8973" velocity="2.175"/>
    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
    <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
  </joint>
  <!-- sub-link defining detailed meshes for collision with environment -->
  <link name="d_left_arm_link4">
    <visual>
      <geometry>
        <mesh filename="package://franka_description/meshes/visual/link4.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://franka_description/meshes/collision/link4.stl"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="-5.317e-02 1.04419e-01 2.7454e-02"/>
      <mass value="3.587895"/>
      <inertia ixx="0.025853" ixy="0.007796" ixz="-0.001332" iyy="0.019552" iyz="0.008641" izz="0.028323"/>
    </inertial>
  </link>
  <!-- sub-link defining coarse geometries of real robot's internal self-collision -->
  <link name="d_left_arm_link4_sc">
  </link>
  <!-- fixed joint between both sub-links -->
  <joint name="d_left_arm_link4_sc_joint" type="fixed">
    <origin rpy="0 0 0"/>
    <parent link="d_left_arm_link4"/>
    <child link="d_left_arm_link4_sc"/>
  </joint>
  <joint name="d_left_arm_joint4" type="revolute">
    <origin rpy="1.5707963267948966 0 0" xyz="0.0825 0 0"/>
    <parent link="d_left_arm_link3"/>
    <child link="d_left_arm_link4"/>
    <axis xyz="0 0 1"/>
    <limit effort="87.0" lower="-3.0718" upper="-0.0698" velocity="2.175"/>
    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-3.0718" soft_upper_limit="-0.0698"/>
    <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
  </joint>
  <!-- sub-link defining detailed meshes for collision with environment -->
  <link name="d_left_arm_link5">
    <visual>
      <geometry>
        <mesh filename="package://franka_description/meshes/visual/link5.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://franka_description/meshes/collision/link5.stl"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="-1.1953e-02 4.1065e-02 -3.8437e-02"/>
      <mass value="1.225946"/>
      <inertia ixx="0.035549" ixy="-0.002117" ixz="-0.004037" iyy="0.029474" iyz="0.000229" izz="0.008627"/>
    </inertial>
  </link>
  <!-- sub-link defining coarse geometries of real robot's internal self-collision -->
  <link name="d_left_arm_link5_sc">
  </link>
  <!-- fixed joint between both sub-links -->
  <joint name="d_left_arm_link5_sc_joint" type="fixed">
    <origin rpy="0 0 0"/>
    <parent link="d_left_arm_link5"/>
    <child link="d_left_arm_link5_sc"/>
  </joint>
  <joint name="d_left_arm_joint5" type="revolute">
    <origin rpy="-1.5707963267948966 0 0" xyz="-0.0825 0.384 0"/>
    <parent link="d_left_arm_link4"/>
    <child link="d_left_arm_link5"/>
    <axis xyz="0 0 1"/>
    <limit effort="12.0" lower="-2.8973" upper="2.8973" velocity="2.61"/>
    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
    <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
  </joint>
  <!-- sub-link defining detailed meshes for collision with environment -->
  <link name="d_left_arm_link6">
    <visual>
      <geometry>
        <mesh filename="package://franka_description/meshes/visual/link6.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://franka_description/meshes/collision/link6.stl"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="6.0149e-02 -1.4117e-02 -1.0517e-02"/>
      <mass value="1.666555"/>
      <inertia ixx="0.001964" ixy="0.000109" ixz="-0.001158" iyy="0.004354" iyz="0.000341" izz="0.005433"/>
    </inertial>
  </link>
  <!-- sub-link defining coarse geometries of real robot's internal self-collision -->
  <link name="d_left_arm_link6_sc">
  </link>
  <!-- fixed joint between both sub-links -->
  <joint name="d_left_arm_link6_sc_joint" type="fixed">
    <origin rpy="0 0 0"/>
    <parent link="d_left_arm_link6"/>
    <child link="d_left_arm_link6_sc"/>
  </joint>
  <joint name="d_left_arm_joint6" type="revolute">
    <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
    <parent link="d_left_arm_link5"/>
    <child link="d_left_arm_link6"/>
    <axis xyz="0 0 1"/>
    <limit effort="12.0" lower="-0.0175" upper="3.7525" velocity="2.61"/>
    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-0.0175" soft_upper_limit="3.7525"/>
    <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
  </joint>
  <!-- sub-link defining detailed meshes for collision with environment -->
  <link name="d_left_arm_link7">
    <visual>
      <geometry>
        <mesh filename="package://franka_description/meshes/visual/link7.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://franka_description/meshes/collision/link7.stl"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="1.0517e-02 -4.252e-03 6.1597e-02"/>
      <mass value="0.735522"/>
      <inertia ixx="0.012516" ixy="-0.000428" ixz="-0.001196" iyy="0.010027" iyz="-0.000741" izz="0.004815"/>
    </inertial>
  </link>
  <!-- sub-link defining coarse geometries of real robot's internal self-collision -->
  <link name="d_left_arm_link7_sc">
  </link>
  <!-- fixed joint between both sub-links -->
  <joint name="d_left_arm_link7_sc_joint" type="fixed">
    <origin rpy="0 0 0.7853981633974483"/>
    <parent link="d_left_arm_link7"/>
    <child link="d_left_arm_link7_sc"/>
  </joint>
  <joint name="d_left_arm_joint7" type="revolute">
    <origin rpy="1.5707963267948966 0 0" xyz="0.088 0 0"/>
    <parent link="d_left_arm_link6"/>
    <child link="d_left_arm_link7"/>
    <axis xyz="0 0 1"/>
    <limit effort="12.0" lower="-2.8973" upper="2.8973" velocity="2.61"/>
    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
    <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
  </joint>
  <link name="d_left_arm_link8"/>
  <joint name="d_left_arm_joint8" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0.107"/>
    <parent link="d_left_arm_link7"/>
    <child link="d_left_arm_link8"/>
  </joint>
  <gazebo reference="d_right_arm_joint1">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="d_right_arm_joint1_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="d_right_arm_joint1">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="d_right_arm_joint1_motor">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo reference="d_right_arm_joint2">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="d_right_arm_joint2_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="d_right_arm_joint2">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="d_right_arm_joint2_motor">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo reference="d_right_arm_joint3">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="d_right_arm_joint3_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="d_right_arm_joint3">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="d_right_arm_joint3_motor">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo reference="d_right_arm_joint4">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="d_right_arm_joint4_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="d_right_arm_joint4">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="d_right_arm_joint4_motor">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo reference="d_right_arm_joint5">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="d_right_arm_joint5_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="d_right_arm_joint5">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="d_right_arm_joint5_motor">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo reference="d_right_arm_joint6">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="d_right_arm_joint6_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="d_right_arm_joint6">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="d_right_arm_joint6_motor">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo reference="d_right_arm_joint7">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="d_right_arm_joint7_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="d_right_arm_joint7">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="d_right_arm_joint7_motor">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo reference="d_left_arm_joint1">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="d_left_arm_joint1_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="d_left_arm_joint1">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="d_left_arm_joint1_motor">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo reference="d_left_arm_joint2">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="d_left_arm_joint2_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="d_left_arm_joint2">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="d_left_arm_joint2_motor">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo reference="d_left_arm_joint3">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="d_left_arm_joint3_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="d_left_arm_joint3">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="d_left_arm_joint3_motor">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo reference="d_left_arm_joint4">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="d_left_arm_joint4_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="d_left_arm_joint4">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="d_left_arm_joint4_motor">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo reference="d_left_arm_joint5">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="d_left_arm_joint5_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="d_left_arm_joint5">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="d_left_arm_joint5_motor">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo reference="d_left_arm_joint6">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="d_left_arm_joint6_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="d_left_arm_joint6">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="d_left_arm_joint6_motor">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo reference="d_left_arm_joint7">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="d_left_arm_joint7_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="d_left_arm_joint7">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="d_left_arm_joint7_motor">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <!-- load ros_control plugin -->
  <gazebo>
    <plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control"/>
  </gazebo>
</robot>
hes/collision/link5.stl"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="-1.1953e-02 4.1065e-02 -3.8437e-02"/>
      <mass value="1.225946"/>
      <inertia ixx="0.035549" ixy="-0.002117" ixz="-0.004037" iyy="0.029474" iyz="0.000229" izz="0.008627"/>
    </inertial>
  </link>
  <!-- sub-link defining coarse geometries of real robot's internal self-collision -->
  <link name="d_right_arm_link5_sc">
  </link>
  <!-- fixed joint between both sub-links -->
  <joint name="d_right_arm_link5_sc_joint" type="fixed">
    <origin rpy="0 0 0"/>
    <parent link="d_right_arm_link5"/>
    <child link="d_right_arm_link5_sc"/>
  </joint>
  <joint name="d_right_arm_joint5" type="revolute">
    <origin rpy="-1.5707963267948966 0 0" xyz="-0.0825 0.384 0"/>
    <parent link="d_right_arm_link4"/>
    <child link="d_right_arm_link5"/>
    <axis xyz="0 0 1"/>
    <limit effort="12.0" lower="-2.8973" upper="2.8973" velocity="2.61"/>
    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
    <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
  </joint>
  <!-- sub-link defining detailed meshes for collision with environment -->
  <link name="d_right_arm_link6">
    <visual>
      <geometry>
        <mesh filename="package://franka_description/meshes/visual/link6.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://franka_description/meshes/collision/link6.stl"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="6.0149e-02 -1.4117e-02 -1.0517e-02"/>
      <mass value="1.666555"/>
      <inertia ixx="0.001964" ixy="0.000109" ixz="-0.001158" iyy="0.004354" iyz="0.000341" izz="0.005433"/>
    </inertial>
  </link>
  <!-- sub-link defining coarse geometries of real robot's internal self-collision -->
  <link name="d_right_arm_link6_sc">
  </link>
  <!-- fixed joint between both sub-links -->
  <joint name="d_right_arm_link6_sc_joint" type="fixed">
    <origin rpy="0 0 0"/>
    <parent link="d_right_arm_link6"/>
    <child link="d_right_arm_link6_sc"/>
  </joint>
  <joint name="d_right_arm_joint6" type="revolute">
    <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
    <parent link="d_right_arm_link5"/>
    <child link="d_right_arm_link6"/>
    <axis xyz="0 0 1"/>
    <limit effort="12.0" lower="-0.0175" upper="3.7525" velocity="2.61"/>
    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-0.0175" soft_upper_limit="3.7525"/>
    <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
  </joint>
  <!-- sub-link defining detailed meshes for collision with environment -->
  <link name="d_right_arm_link7">
    <visual>
      <geometry>
        <mesh filename="package://franka_description/meshes/visual/link7.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://franka_description/meshes/collision/link7.stl"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="1.0517e-02 -4.252e-03 6.1597e-02"/>
      <mass value="0.735522"/>
      <inertia ixx="0.012516" ixy="-0.000428" ixz="-0.001196" iyy="0.010027" iyz="-0.000741" izz="0.004815"/>
    </inertial>
  </link>
  <!-- sub-link defining coarse geometries of real robot's internal self-collision -->
  <link name="d_right_arm_link7_sc">
  </link>
  <!-- fixed joint between both sub-links -->
  <joint name="d_right_arm_link7_sc_joint" type="fixed">
    <origin rpy="0 0 0.7853981633974483"/>
    <parent link="d_right_arm_link7"/>
    <child link="d_right_arm_link7_sc"/>
  </joint>
  <joint name="d_right_arm_joint7" type="revolute">
    <origin rpy="1.5707963267948966 0 0" xyz="0.088 0 0"/>
    <parent link="d_right_arm_link6"/>
    <child link="d_right_arm_link7"/>
    <axis xyz="0 0 1"/>
    <limit effort="12.0" lower="-2.8973" upper="2.8973" velocity="2.61"/>
    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
    <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
  </joint>
  <link name="d_right_arm_link8"/>
  <joint name="d_right_arm_joint8" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0.107"/>
    <parent link="d_right_arm_link7"/>
    <child link="d_right_arm_link8"/>
  </joint>
  <joint name="d_left_arm_joint_base" type="fixed">
    <parent link="base"/>
    <child link="d_left_arm_link0"/>
    <origin rpy="0 0 0" xyz="0 0.5 1"/>
  </joint>
  <!-- sub-link defining detailed meshes for collision with environment -->
  <link name="d_left_arm_link0">
    <visual>
      <geometry>
        <mesh filename="package://franka_description/meshes/visual/link0.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://franka_description/meshes/collision/link0.stl"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="-0.041018 -0.00014 0.049974"/>
      <mass value="0.629769"/>
      <inertia ixx="0.00315" ixy="8.2904e-07" ixz="0.00015" iyy="0.00388" iyz="8.2299e-06" izz="0.004285"/>
    </inertial>
  </link>
  <!-- sub-link defining coarse geometries of real robot's internal self-collision -->
  <link name="d_left_arm_link0_sc">
  </link>
  <!-- fixed joint between both sub-links -->
  <joint name="d_left_arm_link0_sc_joint" type="fixed">
    <origin rpy="0 0 0"/>
    <parent link="d_left_arm_link0"/>
    <child link="d_left_arm_link0_sc"/>
  </joint>
  <!-- sub-link defining detailed meshes for collision with environment -->
  <link name="d_left_arm_link1">
    <visual>
      <geometry>
        <mesh filename="package://franka_description/meshes/visual/link1.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://franka_description/meshes/collision/link1.stl"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.003875 0.002081 -0.04762"/>
      <mass value="4.970684"/>
      <inertia ixx="0.70337" ixy="-0.000139" ixz="0.006772" iyy="0.70661" iyz="0.019169" izz="0.009117"/>
    </inertial>
  </link>
  <!-- sub-link defining coarse geometries of real robot's internal self-collision -->
  <link name="d_left_arm_link1_sc">
  </link>
  <!-- fixed joint between both sub-links -->
  <joint name="d_left_arm_link1_sc_joint" type="fixed">
    <origin rpy="0 0 0"/>
    <parent link="d_left_arm_link1"/>
    <child link="d_left_arm_link1_sc"/>
  </joint>
  <joint name="d_left_arm_joint1" type="revolute">
    <origin rpy="0 0 0" xyz="0 0 0.333"/>
    <parent link="d_left_arm_link0"/>
    <child link="d_left_arm_link1"/>
    <axis xyz="0 0 1"/>
    <limit effort="87.0" lower="-2.8973" upper="2.8973" velocity="2.175"/>
    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
    <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
  </joint>
  <!-- sub-link defining detailed meshes for collision with environment -->
  <link name="d_left_arm_link2">
    <visual>
      <geometry>
        <mesh filename="package://franka_description/meshes/visual/link2.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://franka_description/meshes/collision/link2.stl"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="-0.003141 -0.02872  0.003495"/>
      <mass value="0.646926"/>
      <inertia ixx="0.007962" ixy="-0.003925" ixz="0.010254" iyy="0.02811" iyz="0.000704" izz="0.025995"/>
    </inertial>
  </link>
  <!-- sub-link defining coarse geometries of real robot's internal self-collision -->
  <link name="d_left_arm_link2_sc">
  </link>
  <!-- fixed joint between both sub-links -->
  <joint name="d_left_arm_link2_sc_joint" type="fixed">
    <origin rpy="0 0 0"/>
    <parent link="d_left_arm_link2"/>
    <child link="d_left_arm_link2_sc"/>
  </joint>
  <joint name="d_left_arm_joint2" type="revolute">
    <origin rpy="-1.5707963267948966 0 0" xyz="0 0 0"/>
    <parent link="d_left_arm_link1"/>
    <child link="d_left_arm_link2"/>
    <axis xyz="0 0 1"/>
    <limit effort="87.0" lower="-1.7628" upper="1.7628" velocity="2.175"/>
    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-1.7628" soft_upper_limit="1.7628"/>
    <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
  </joint>
  <!-- sub-link defining detailed meshes for collision with environment -->
  <link name="d_left_arm_link3">
    <visual>
      <geometry>
        <mesh filename="package://franka_description/meshes/visual/link3.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://franka_description/meshes/collision/link3.stl"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="2.7518e-02 3.9252e-02 -6.6502e-02"/>
      <mass value="3.228604"/>
      <inertia ixx="0.037242" ixy="-0.004761" ixz="-0.011396" iyy="0.036155" iyz="-0.012805" izz="0.01083"/>
    </inertial>
  </link>
  <!-- sub-link defining coarse geometries of real robot's internal self-collision -->
  <link name="d_left_arm_link3_sc">
  </link>
  <!-- fixed joint between both sub-links -->
  <joint name="d_left_arm_link3_sc_joint" type="fixed">
    <origin rpy="0 0 0"/>
    <parent link="d_left_arm_link3"/>
    <child link="d_left_arm_link3_sc"/>
  </joint>
  <joint name="d_left_arm_joint3" type="revolute">
    <origin rpy="1.5707963267948966 0 0" xyz="0 -0.316 0"/>
    <parent link="d_left_arm_link2"/>
    <child link="d_left_arm_link3"/>
    <axis xyz="0 0 1"/>
    <limit effort="87.0" lower="-2.8973" upper="2.8973" velocity="2.175"/>
    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
    <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
  </joint>
  <!-- sub-link defining detailed meshes for collision with environment -->
  <link name="d_left_arm_link4">
    <visual>
      <geometry>
        <mesh filename="package://franka_description/meshes/visual/link4.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://franka_description/meshes/collision/link4.stl"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="-5.317e-02 1.04419e-01 2.7454e-02"/>
      <mass value="3.587895"/>
      <inertia ixx="0.025853" ixy="0.007796" ixz="-0.001332" iyy="0.019552" iyz="0.008641" izz="0.028323"/>
    </inertial>
  </link>
  <!-- sub-link defining coarse geometries of real robot's internal self-collision -->
  <link name="d_left_arm_link4_sc">
  </link>
  <!-- fixed joint between both sub-links -->
  <joint name="d_left_arm_link4_sc_joint" type="fixed">
    <origin rpy="0 0 0"/>
    <parent link="d_left_arm_link4"/>
    <child link="d_left_arm_link4_sc"/>
  </joint>
  <joint name="d_left_arm_joint4" type="revolute">
    <origin rpy="1.5707963267948966 0 0" xyz="0.0825 0 0"/>
    <parent link="d_left_arm_link3"/>
    <child link="d_left_arm_link4"/>
    <axis xyz="0 0 1"/>
    <limit effort="87.0" lower="-3.0718" upper="-0.0698" velocity="2.175"/>
    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-3.0718" soft_upper_limit="-0.0698"/>
    <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
  </joint>
  <!-- sub-link defining detailed meshes for collision with environment -->
  <link name="d_left_arm_link5">
    <visual>
      <geometry>
        <mesh filename="package://franka_description/meshes/visual/link5.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://franka_description/meshes/collision/link5.stl"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="-1.1953e-02 4.1065e-02 -3.8437e-02"/>
      <mass value="1.225946"/>
      <inertia ixx="0.035549" ixy="-0.002117" ixz="-0.004037" iyy="0.029474" iyz="0.000229" izz="0.008627"/>
    </inertial>
  </link>
  <!-- sub-link defining coarse geometries of real robot's internal self-collision -->
  <link name="d_left_arm_link5_sc">
  </link>
  <!-- fixed joint between both sub-links -->
  <joint name="d_left_arm_link5_sc_joint" type="fixed">
    <origin rpy="0 0 0"/>
    <parent link="d_left_arm_link5"/>
    <child link="d_left_arm_link5_sc"/>
  </joint>
  <joint name="d_left_arm_joint5" type="revolute">
    <origin rpy="-1.5707963267948966 0 0" xyz="-0.0825 0.384 0"/>
    <parent link="d_left_arm_link4"/>
    <child link="d_left_arm_link5"/>
    <axis xyz="0 0 1"/>
    <limit effort="12.0" lower="-2.8973" upper="2.8973" velocity="2.61"/>
    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
    <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
  </joint>
  <!-- sub-link defining detailed meshes for collision with environment -->
  <link name="d_left_arm_link6">
    <visual>
      <geometry>
        <mesh filename="package://franka_description/meshes/visual/link6.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://franka_description/meshes/collision/link6.stl"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="6.0149e-02 -1.4117e-02 -1.0517e-02"/>
      <mass value="1.666555"/>
      <inertia ixx="0.001964" ixy="0.000109" ixz="-0.001158" iyy="0.004354" iyz="0.000341" izz="0.005433"/>
    </inertial>
  </link>
  <!-- sub-link defining coarse geometries of real robot's internal self-collision -->
  <link name="d_left_arm_link6_sc">
  </link>
  <!-- fixed joint between both sub-links -->
  <joint name="d_left_arm_link6_sc_joint" type="fixed">
    <origin rpy="0 0 0"/>
    <parent link="d_left_arm_link6"/>
    <child link="d_left_arm_link6_sc"/>
  </joint>
  <joint name="d_left_arm_joint6" type="revolute">
    <origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
    <parent link="d_left_arm_link5"/>
    <child link="d_left_arm_link6"/>
    <axis xyz="0 0 1"/>
    <limit effort="12.0" lower="-0.0175" upper="3.7525" velocity="2.61"/>
    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-0.0175" soft_upper_limit="3.7525"/>
    <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
  </joint>
  <!-- sub-link defining detailed meshes for collision with environment -->
  <link name="d_left_arm_link7">
    <visual>
      <geometry>
        <mesh filename="package://franka_description/meshes/visual/link7.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://franka_description/meshes/collision/link7.stl"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="1.0517e-02 -4.252e-03 6.1597e-02"/>
      <mass value="0.735522"/>
      <inertia ixx="0.012516" ixy="-0.000428" ixz="-0.001196" iyy="0.010027" iyz="-0.000741" izz="0.004815"/>
    </inertial>
  </link>
  <!-- sub-link defining coarse geometries of real robot's internal self-collision -->
  <link name="d_left_arm_link7_sc">
  </link>
  <!-- fixed joint between both sub-links -->
  <joint name="d_left_arm_link7_sc_joint" type="fixed">
    <origin rpy="0 0 0.7853981633974483"/>
    <parent link="d_left_arm_link7"/>
    <child link="d_left_arm_link7_sc"/>
  </joint>
  <joint name="d_left_arm_joint7" type="revolute">
    <origin rpy="1.5707963267948966 0 0" xyz="0.088 0 0"/>
    <parent link="d_left_arm_link6"/>
    <child link="d_left_arm_link7"/>
    <axis xyz="0 0 1"/>
    <limit effort="12.0" lower="-2.8973" upper="2.8973" velocity="2.61"/>
    <safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
    <dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
  </joint>
  <link name="d_left_arm_link8"/>
  <joint name="d_left_arm_joint8" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0.107"/>
    <parent link="d_left_arm_link7"/>
    <child link="d_left_arm_link8"/>
  </joint>
  <gazebo reference="d_right_arm_joint1">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="d_right_arm_joint1_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="d_right_arm_joint1">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="d_right_arm_joint1_motor">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo reference="d_right_arm_joint2">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="d_right_arm_joint2_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="d_right_arm_joint2">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="d_right_arm_joint2_motor">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo reference="d_right_arm_joint3">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="d_right_arm_joint3_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="d_right_arm_joint3">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="d_right_arm_joint3_motor">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo reference="d_right_arm_joint4">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="d_right_arm_joint4_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="d_right_arm_joint4">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="d_right_arm_joint4_motor">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo reference="d_right_arm_joint5">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="d_right_arm_joint5_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="d_right_arm_joint5">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="d_right_arm_joint5_motor">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo reference="d_right_arm_joint6">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="d_right_arm_joint6_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="d_right_arm_joint6">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="d_right_arm_joint6_motor">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo reference="d_right_arm_joint7">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="d_right_arm_joint7_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="d_right_arm_joint7">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="d_right_arm_joint7_motor">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo reference="d_left_arm_joint1">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="d_left_arm_joint1_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="d_left_arm_joint1">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="d_left_arm_joint1_motor">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo reference="d_left_arm_joint2">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="d_left_arm_joint2_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="d_left_arm_joint2">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="d_left_arm_joint2_motor">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo reference="d_left_arm_joint3">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="d_left_arm_joint3_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="d_left_arm_joint3">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="d_left_arm_joint3_motor">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo reference="d_left_arm_joint4">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="d_left_arm_joint4_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="d_left_arm_joint4">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="d_left_arm_joint4_motor">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo reference="d_left_arm_joint5">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="d_left_arm_joint5_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="d_left_arm_joint5">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="d_left_arm_joint5_motor">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo reference="d_left_arm_joint6">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="d_left_arm_joint6_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="d_left_arm_joint6">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="d_left_arm_joint6_motor">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo reference="d_left_arm_joint7">
    <!-- Needed for ODE to output external wrenches on joints -->
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <transmission name="d_left_arm_joint7_transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="d_left_arm_joint7">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="d_left_arm_joint7_motor">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <!-- load ros_control plugin -->
  <gazebo>
    <plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control"/>
  </gazebo>
</robot>

再给出加载机械臂控制器的yaml文件

right_arm_trajectory_controller.yaml：

right_arm_trajectory_controller:
    type: "position_controllers/JointTrajectoryController"
    joints:
        - right_arm_joint1
        - right_arm_joint2
        - right_arm_joint3
        - right_arm_joint4
        - right_arm_joint5
        - right_arm_joint6
        - right_arm_joint7
    constraints:
        goal_time: 0.6
        stopped_velocity_tolerance: 0.05
        right_arm_joint1: {trajectory: 0.1, goal: 0.1}
        right_arm_joint2: {trajectory: 0.1, goal: 0.1}
        right_arm_joint3: {trajectory: 0.1, goal: 0.1}
        right_arm_joint4: {trajectory: 0.1, goal: 0.1}
        right_arm_joint5: {trajectory: 0.1, goal: 0.1}
        right_arm_joint6: {trajectory: 0.1, goal: 0.1}
        right_arm_joint7: {trajectory: 0.1, goal: 0.1}
    stop_trajectory_duration: 0.5
    state_publish_rate:  25
    action_monitor_rate: 10
    
right_hand_controller:
    type: "effort_controllers/JointTrajectoryController"
    joints:
        - right_arm_finger_joint1
    gains:
        right_arm_finger_joint1:  {p: 50.0, d: 1.0, i: 0.01, i_clamp: 1.0}

left_arm_trajectory_controller.yaml

    left_arm_trajectory_controller:
        type: "position_controllers/JointTrajectoryController"
        joints:
            - left_arm_joint1
            - left_arm_joint2
            - left_arm_joint3
            - left_arm_joint4
            - left_arm_joint5
            - left_arm_joint6
            - left_arm_joint7
        constraints:
            goal_time: 0.6
            stopped_velocity_tolerance: 0.05
            left_arm_joint1: {trajectory: 0.1, goal: 0.1}
            left_arm_joint2: {trajectory: 0.1, goal: 0.1}
            left_arm_joint3: {trajectory: 0.1, goal: 0.1}
            left_arm_joint4: {trajectory: 0.1, goal: 0.1}
            left_arm_joint5: {trajectory: 0.1, goal: 0.1}
            left_arm_joint6: {trajectory: 0.1, goal: 0.1}
            left_arm_joint7: {trajectory: 0.1, goal: 0.1}
        stop_trajectory_duration: 0.5
        state_publish_rate:  25
        action_monitor_rate: 10
        
    left_hand_controller:
        type: "effort_controllers/JointTrajectoryController"
        joints:
            - left_arm_finger_joint1
        gains:
            left_arm_finger_joint1:  {p: 50.0, d: 1.0, i: 0.01, i_clamp: 1.0}

启动launch文件效果如下所示：

 这里报错是因为没有添加pid参数，忽略即可，没有影响。

通过命令rostopic list，可以看到，控制器添加成功后，可以通过图上所示的话题。

        控制器的作用就是为moveit提供一个/follow_joint_trajectory的接口，使得moveit可以通过action控制机械臂，但在两个命名空间下，move group就无法识别到接口，使得我们需要自己完成从moveit到gazebo的通信。

         最后一步就是实现moveit到gazebo的通信，首先任意启动一个臂的demo.launch，另开一个终端通过命令rostopic list ， 可以看到如下图所示的topic，再通过命令rostopic echo /left_arm/execute_trajectory/goal监听此话题

         当我们在rviz界面中进行机械臂的规划时（也就是 点击execute时）可以看到监听的终端展示的机械臂关节轨迹信息，将这些信息收集并通过gazebo加载控制器的acction接口同步发送，即可建立moveit到gazebo的通信。

通过命令rqt，详细查看这两个topic的信息，可以看到，下图中框出的部分是相同类型的，因此，我们就可以通过手动编写节点，获取到move group中规划得到的轨迹信息，再通过action向gazebo中的控制新发送goal，使得moveit和gazebo之间建立通信。

具体代码：

#!/usr/bin/env python
# -*- coding: utf-8 -*-
from trajectory_msgs.msg import *
from control_msgs.msg import *
from moveit_msgs.msg import *
import rospy
import actionlib


JOINT_NAMES = ['d_left_arm_joint1', 'd_left_arm_joint2', 'd_left_arm_joint3',
               'd_left_arm_joint4', 'd_left_arm_joint5', 'd_left_arm_joint6','d_left_arm_joint7']
 
def callback(goal):

        print(goal.goal.trajectory.joint_trajectory)

        #agoal就是我们向发送的关节运动数据，实例化为FollowJointTrajectoryGoal()类
        agoal = FollowJointTrajectoryGoal()
 
        #goal当中的trajectory就是我们要操作的，其余的Header之类的不用管
        agoal.trajectory = JointTrajectory()

        agoal.trajectory = goal.goal.trajectory.joint_trajectory
        #agoal.trajectory底下一共还有两个成员，分别是joint_names和points，先给joint_names赋值
        agoal.trajectory.joint_names = JOINT_NAMES

        mediator_client.send_goal(agoal)
 
def mediator():
        global mediator_client
        #初始化ros节点
        rospy.init_node("left_mediator")          
        #实例化一个action的类，命名为client，与上述client对应，话题为right_arm_trajectory_controller/follow_joint_trajectory，消息类型为FollowJointTrajectoryAction
        mediator_client = actionlib.SimpleActionClient('left_arm_trajectory_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
        print("----------Waiting for server-------------")
        #等待server
        mediator_client.wait_for_server()
        print("----------Connect to server--------------")
        mediator_subscriber = rospy.Subscriber("left_arm/execute_trajectory/goal",ExecuteTrajectoryActionGoal,callback)
        rospy.spin()
 
if __name__ == "__main__":
          mediator()

Moveit + Gazebo实现联合仿真：ABB yumi双臂机器人（ 二、双臂协同运动实现 ）
2201_75624690的博客
05-05 3723
实现Moveit+Gazebo中yumi机器人双臂协同运动。
Moveit + Gazebo:搭建双臂仿真平台（方案一）
magic_invaid的博客
01-09 6274
环境ubuntu20.04 ROS-noetic国内少有搭建Moveit和Gazebo联合仿真的教程，对于搭建双臂等复杂的仿真平台更是鲜有资料，因此想要把自己的见解分享出来供大家参考，共同提高。本文提出了两种方法实现Moveit 对双臂的规划，并在gazebo中进行仿真。
Moveit + Gazebo实现联合仿真：ABB yumi双臂机器人（ 一、基本仿真搭建及简单运动实现 ）
2201_75624690的博客
05-05 5367
基于Moveit+Gazebo实现yumi双臂机器人基本仿真及简单运动
Ros-melodic: Moveit+Gazebo
A person does not really understand something until after teaching it to a computer. Donald.E.Knuth
02-11 186
【代码】Ros-melodic: Moveit+Gazebo。
Franka机械臂：学习记录1---moveit与gazebo
qq_55055349的博客
12-03 2580
其中，“moveit_controller_manager”的值为“ros_control”，一般来说，其值可以分为三种，"fake"、“simple”和“ros_control”，好像是只使用moveit而不仿真时用fake，而“ros_control”代表的是用户可以自定义的控制器（所以好像也可以自定义这个控制器的名字，但是“ros_control”是moveit给用户默认生成的，只是具体内容需要用户自己填写）。“Add Links”的一个示例，名字同上图也有点区别，也是因为这是之前配置时截的图。
双臂采用moveit和gazebo联合仿真配置文件
weixin_72595939的博客
12-06 1264
2、去改moveit的double_arm_moveit_controller_manager.launch.xml添加controllers配置。5、在gazebo中创建launch文件运行4中建立的yaml(double_arm_trajectory_controller.launch)launch文件创建double_arm_gazebo_states.launch 去运行yaml文件。config内创建double_arm_gazebo_joint_states.yaml。
Moveit搭建机械臂仿真平台记录
magic_invaid的博客
11-29 1212
moveit的学习过程
PROBOT_G603双臂GAZEBO+MoveIt！仿真中配置手眼相机和夹爪
weixin_52849265的博客
08-02 2183
PROBOT_G603双臂GAZEBO+MoveIt！仿真中配置手眼相机和夹爪问题描述安装双臂模型修改urdf文件Gripper文件修改Gripper添加到机械臂上配置MoveIt!配置moveit助手修改ros_controller.yaml修改运行文件几点问题- moveit规划的时候gazebo中模型动但是rviz中不动- gazebo中机械臂基座和地面不固定- gazebo中机械臂抖动总结 问题描述 课题需要搭建双臂仿真模型，其中一个机械臂上有相机，两个机械臂上都需要带有夹爪，GAZEBO仿真环境搭
基于C/C++和ROS的MOVEIT+GAZEBO仿真系统搭建+机械臂开发+源码+硬件+设计图纸+视频演示（高分优秀项目）
05-13
基于C/C++和ROS的MOVEIT+GAZEBO仿真系统搭建+机械臂开发+源码+硬件+设计图纸+视频演示，适合毕业设计、课程设计、项目开发。项目源码已经过严格测试，可以放心参考并在此基础上延申使用~ 项目简介： 电机及驱动 ...
MoveIt+ Gazebo 联合仿真环境搭建（巨简单版）
weixin_44666829的博客
03-25 6701
moveit + gazebo 联合仿真。
Moveit+gazebo联合仿真，最终实现Pyhon接口控制机械臂运动及夹爪的张开闭合
青春要不限量的博客
04-22 680
Moveit+-gazebo联合仿真，最终实现Pyhon接口控制机械臂运动及夹爪的张开闭合
10.Moveit+gazebo仿真
wzfafabga的博客
05-17 696
注意三个控制器关系，Follow Joint Trajectory发布轨迹，Joint Trajectory Controller接收轨迹，Joint State Controller反馈关节位置。 一、c800 1.进入ros_controllers.yaml,将命名空间对应好，要不无法接收。 修改前，controller_gazebo是名字是自动生成的： 修改后： 目的是解决，二者的对接问题： 2.进入ros_controllers.launch,把控制器接口...
＜PLC＞＜视觉＞＜机器人＞基于海康威视视觉检测和UR机械臂，如何实现N点标定？
用沸腾的热血，支付我们的人生吧！
05-16 285
本文介绍了如何使用UR机器人携带海康威视工业相机，通过9点标定方法实现定位检测。文章详细描述了标定流程，包括安装海康威视视觉软件VMS、创建流程、获取图像源、高精度特征提取、位置修正、直线检测、线线测量以及N点标定等步骤。标定完成后，生成了标定文件，用于后续的定位检测。文章还提到了标定转换模块的使用，以及在实际应用中可能遇到的机械臂坐标系与相机像素坐标系方向不一致的问题，并给出了相应的解决方案。本文为工业自动化领域的视觉应用提供了实用的参考和指导。
DeepSeek赋能电商，智能客服机器人破解大型活动人力困境
微刻
05-17 751
影刀RPA的核心功能包括数据抓取、解析、校验和自动填表，还支持可视化流程设计器、Python和JavaScript脚本接入，以及流程录制等功能，帮助用户快速搭建自动化流程。影刀RPA结合AI技术，支持机器视觉、自然语言处理等高级功能，进一步提升自动化能力。影刀RPA广泛应用于电商、金融、制造等行业，帮助客户实现订单处理、客户数据录入、财务对账等任务的自动化。影刀实战，影刀证书快速获取，影刀功能定制
宇树科技申请 “机器人牌照” 商标，剑指机器人领域新高度​
RUZHUA的博客
05-16 564
从 2013-2016 年王兴兴开发全自由度高性能四足机器人 xdog，到 2017 年重构的四足机器人 laikago 问世，再到 2019 年的 aliengo 四足机器人、2020 年的教育酷玩四足机器人 a1、2021 年的伴随仿生机器人 go1、2023 年的全新一代消费级四足机器人 unitree go2 和首款通用人形机器人 h1，以及 2024 年的 unitree g1 人形智能体，宇树科技不断推陈出新，在机器人研发领域成果丰硕。在市场竞争方面，这一举措有望为宇树科技带来新的优势。
基于STM32的多传感器融合的设施农业小型搬运机器人避障控制系统设计
weixin_42300449的博客
05-18 426
针对设施农业场景中狭窄通道、障碍物多样（如农机具、作物植株、水管）的特点，设计一款基于 STM32 的小型搬运机器人避障控制系统。系统通过多传感器融合实现 360° 环境感知，采用模糊 PID 控制算法实现平滑避障，满足温室、大棚等场景的搬运需求。通过在温室环境测试，系统可实现对直径≥5cm 障碍物的有效检测（检测距离误差≤5%），避障转向半径≤30cm，平均功耗≤15W。后续可通过增加激光雷达（如 TFmini）提升远距离障碍物检测能力，结合 SLAM 算法实现路径规划。
上肢康复机器人设计与临床应用研究
最新发布
2301_78600126的博客
05-20 938
本文针对神经系统疾病导致的上肢运动功能障碍，提出了一种上肢康复机器人的设计方案。该机器人旨在通过精准的运动控制和生物反馈机制，提供高效、标准化的康复训练。文章详细阐述了机器人的设计方法，包括机械系统设计、控制系统设计以及临床验证与效果评估。机械系统采用5自由度串联构型，通过D-H参数法建立运动学模型，确保末端执行器可达工作空间直径≥1.2m。控制系统采用自适应阻抗控制和运动意图识别算法，实现训练模式的平滑切换和动作识别。临床验证表明，机器人辅助训练可显著提升运动功能恢复速度，并降低肌肉痉挛发生率。文章还探讨
机器人手臂相机应用介绍
qq_39640794的博客
05-19 1275
手臂相机为固定在机器人手臂上而非末端关节轴上，通常手臂相机是指安装在4轴机器人的J2关节轴上或者6轴机器人的J5关节轴上。手臂相机安装示例。
1.1 Epson机器人常用指令1-Print函数、RobotInfo$
qq_34047402的博客
05-19 315
3. CX, CY, CZ可以返回点的XYZ坐标值。1.Print函数（将数据输出到指定的文件、通信端口、显示装置）RealPos 函数，用于返回指定机器人当前位置。CurPos:返回机器人的当前的动作目标位置。Agl 是返回指定关节的角度的函数。CX 是返回指定点数据X坐标值。CY 是返回指定点数据Y坐标值。CZ 是返回指定点数据Z坐标值。CU 是返回指定点数据U坐标值。CV 是返回指定点数据V坐标值。CR 是返回指定点数据R坐标值。CS 是返回指定点数据S坐标值。CT 是返回指定点数据T坐标值。
abb yumi
03-27
<think>好的，我现在需要处理用户关于ABB YuMi机器人的查询。首先，用户提供了三个引用资料，我需要仔细分析这些内容来构建回答。 首先，引用[1]提到YuMi是协作式双臂机器人，用于缓解劳动力短缺，每个手臂有7个轴，工作范围0.56米，负载38公斤，专为制造业设计。这些是基本的技术参数和应用场景，应该放在介绍的前面部分。 接下来，引用[2]是关于在Gazebo中的仿真环境，提供了模型下载链接。这说明YuMi有仿真资源，用户可能在研究如何模拟其行为，这部分可以作为技术支持或开发环境的内容。 然后，引用[3]详细说明了使用Moveit和Gazebo进行双臂协同运动的仿真，包括绕八字和跳舞等动作。这展示了YuMi的高级功能，即双臂协作能力，这也是其区别于其他机器人的重要特点，需要重点强调。 现在需要将信息整合成结构清晰的回答。根据用户的要求，回答需要分点，使用中文，数学公式正确，同时生成相关问题。用户可能对技术细节、应用案例、仿真方法感兴趣，所以相关问题需要覆盖这些方面。 需要注意引用标识的位置，确保每个引用的信息末尾自然添加。例如，提到负载能力时引用[1]，提到仿真时引用[2]和[3]。 还要检查是否有遗漏的信息，比如YuMi的推出时间（2015年），每个手臂的轴数，这些细节能增加回答的专业性。同时，双臂协同运动的具体实现方法，如Moveit配置和通讯建立，可能对技术用户有帮助，需要简要提及。 最后，生成的相关问题应围绕用户可能进一步想了解的内容，如技术参数、应用领域、仿真方法、与其他机器人的对比，以及维护成本等。这些问题应覆盖不同层次的需求，从基础到高级应用。 现在组织内容，确保每个部分简明扼要，符合格式要求，正确使用引用标记，并自然流畅。</think>### ABB YuMi 协作式双臂机器人介绍 1. **基础信息与设计特点** ABB YuMi（IRB 14000）是2015年推出的协作式双臂机器人，专为精密制造业设计，尤其适用于电子元件组装等需要高灵活性的场景。其核心特点包括： - **双臂协同**：每个手臂拥有7个运动轴，可模拟人类手臂的复杂扭转动作，支持同步操作[^1]。 - **负载能力**：单臂最大负载为38kg，工作范围约0.56米，适合轻量化精密任务[^1]。 - **安全性**：内置力传感器和视觉系统，可在无物理围栏环境下与人协同工作[^1]。 2. **技术实现与仿真支持** YuMi的仿真开发资源丰富，支持在Gazebo等平台进行建模与控制： - **Gazebo仿真环境**：用户可通过公开的模型文件快速搭建仿真场景（下载链接见引用[2]）[^2]。 - **Moveit协同控制**：结合Moveit框架可实现双臂协同运动规划，例如绕八字轨迹或舞蹈动作（需配置ROS驱动与通信模块）[^3]。 3. **应用场景与案例** - **电子装配**：例如电路板元件安装、小型设备组装。 - **医疗设备生产**：在洁净环境中执行无菌操作。 - **教育培训**：作为机器人协同算法开发的教学工具。
评论 10
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值