search

0aqz0 / neural-retargeting

Code for the paper "Kinematic Motion Retargeting via Neural Latent Optimization for Learning Sign Language", RAL with ICRA 2022
MIT License
34 stars 5 forks source link

Some problem while training yumi, it seems that the code is lack of yumi.urdf #2

Closed zzzbbs closed 1 year ago

zzzbbs commented 1 year ago

Hi,while reproducing the paper's code,I met some problems . I try to use the yumi_gym 's urdf , but it still went wrong.

Before Adding yumi_gym's urdf: Traceback (most recent call last): File "inference.py", line 73, in test_target = sorted([target for target in getattr(dataset, cfg.DATASET.TEST.TARGET_NAME)(root=cfg.DATASET.TEST.TARGET_PATH)], key=lambda target : target.skeleton_type) File "/home/zbs/Code/ImitationLearning/neural-retargeting/dataset.py", line 780, in init super(YumiAll, self).init(root, transform, pre_transform) File "/home/zbs/anaconda3/envs/mani_skill2/lib/python3.8/site-packages/torch_geometric/data/in_memory_dataset.py", line 60, in init super().init(root, transform, pre_transform, pre_filter) File "/home/zbs/anaconda3/envs/mani_skill2/lib/python3.8/site-packages/torch_geometric/data/dataset.py", line 86, in init self._process() File "/home/zbs/anaconda3/envs/mani_skill2/lib/python3.8/site-packages/torch_geometric/data/dataset.py", line 165, in _process self.process() File "/home/zbs/Code/ImitationLearning/neural-retargeting/dataset.py", line 818, in process data, slices = self.collate(data_list) File "/home/zbs/anaconda3/envs/mani_skill2/lib/python3.8/site-packages/torch_geometric/data/in_memory_dataset.py", line 120, in collate keys = data_list[0].keys IndexError: list index out of range

After Adding yumi_gym's urdf : Traceback (most recent call last): File "main.py", line 51, in train_target = sorted([target for target in getattr(dataset, cfg.DATASET.TRAIN.TARGET_NAME)(root=cfg.DATASET.TRAIN.TARGET_PATH)], key=lambda target : target.skeleton_type) File "/home/zbs/Code/ImitationLearning/neural-retargeting/dataset.py", line 780, in init super(YumiAll, self).init(root, transform, pre_transform) File "/home/zbs/anaconda3/envs/mani_skill2/lib/python3.8/site-packages/torch_geometric/data/in_memory_dataset.py", line 60, in init super().init(root, transform, pre_transform, pre_filter) File "/home/zbs/anaconda3/envs/mani_skill2/lib/python3.8/site-packages/torch_geometric/data/dataset.py", line 86, in init self._process() File "/home/zbs/anaconda3/envs/mani_skill2/lib/python3.8/site-packages/torch_geometric/data/dataset.py", line 165, in _process self.process() File "/home/zbs/Code/ImitationLearning/neural-retargeting/dataset.py", line 800, in process hand_data = hand2graph(file, InspireHand.hand_cfg) File "/home/zbs/Code/ImitationLearning/neural-retargeting/utils/urdf2graph.py", line 225, in hand2graph edge_attr.append(torch.Tensor(joints[child]['origin'])) KeyError: 'Link1111'

0aqz0 commented 1 year ago

Hi, you can use this URDF file. :smile: 

<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- |    This document was autogenerated by xacro from yumi.urdf.xacro                | -->
<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
<!-- =================================================================================== -->
<robot name="yumi" xmlns:xacro="http://www.ros.org/wiki/xacro">
  <material name="Black">
    <color rgba="0.0 0.0 0.0 1.0"/>
  </material>
  <material name="Blue">
    <color rgba="0.0 0.0 0.8 1.0"/>
  </material>
  <material name="Green">
    <color rgba="0.0 0.8 0.0 1.0"/>
  </material>
  <material name="Grey">
    <color rgba="0.5 0.5 0.5 1.0"/>
  </material>
  <material name="Light_Grey">
    <color rgba="0.8 0.8 0.8 1.0"/>
  </material>
  <material name="Ivory">
    <color rgba="1.0 1.0 0.94 1.0"/>
  </material>
  <material name="Orange">
    <color rgba="1.0 0.423529411765 0.0392156862745 1.0"/>
  </material>
  <material name="Brown">
    <color rgba="0.870588235294 0.811764705882 0.764705882353 1.0"/>
  </material>
  <material name="Red">
    <color rgba="0.8 0.0 0.0 1.0"/>
  </material>
  <material name="White">
    <color rgba="1.0 1.0 1.0 1.0"/>
  </material>
  <gazebo>
    <plugin filename="libgazebo_ros_control.so" name="gazebo_ros_controller">
      <robotNamespace>/yumi</robotNamespace>
    </plugin>
  </gazebo>
  <link name="yumi_base_link"/>
  <joint name="yumi_base_link_to_body" type="fixed">
    <parent link="yumi_base_link"/>
    <child link="yumi_body"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
  </joint>
  <link name="yumi_body">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="15"/>
      <inertia ixx="0.5" ixy="0" ixz="0" iyy="0.6" iyz="0" izz="0.3"/>
    </inertial>
    <!-- <visual> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/body.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Light_Grey"/> -->
    <!-- </visual> -->
    <!-- <collision> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/coarse/body.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Light_Grey"/> -->
    <!-- </collision> -->
  </link>
  <!--///////////////////////////////////// RIGHT ARM ////////////////////////////////  -->
  <!-- joint between body and link_1_r -->
  <joint name="yumi_joint_1_r" type="revolute">
    <parent link="yumi_body"/>
    <child link="yumi_link_1_r"/>
    <origin rpy="-0.9795   -0.5682   -2.3155" xyz="0.05355 -0.0725 0.41492"/>
    <axis xyz="0 0 1"/>
    <limit effort="300" lower="-2.94087978961" upper="2.94087978961" velocity="3.14159265359"/>
    <dynamics damping="0.5"/>
  </joint>
  <link name="yumi_link_1_r">
    <inertial>
      <origin rpy="0 0 0" xyz="0 -0.03 0.12"/>
      <mass value="2"/>
      <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
    </inertial>
    <!-- <visual> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/link_1.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Grey"/> -->
    <!-- </visual> -->
    <!-- <collision> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/coarse/link_1.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Grey"/> -->
    <!-- </collision> -->
  </link>
  <!-- joint between link_1_r and link_2_r -->
  <joint name="yumi_joint_2_r" type="revolute">
    <parent link="yumi_link_1_r"/>
    <child link="yumi_link_2_r"/>
    <origin rpy="1.57079632679 0.0 0.0" xyz="0.03 0.0 0.1"/>
    <axis xyz="0 0 1"/>
    <limit effort="300" lower="-2.50454747661" upper="0.759218224618" velocity="3.14159265359"/>
    <dynamics damping="0.5"/>
  </joint>
  <link name="yumi_link_2_r">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="2"/>
      <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
    </inertial>
    <!-- <visual> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/link_2.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Grey"/> -->
    <!-- </visual> -->
    <!-- <collision> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/coarse/link_2.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Grey"/> -->
    <!-- </collision> -->
  </link>
  <!-- joint between link_2_r and link_3_r -->
  <joint name="yumi_joint_7_r" type="revolute">
    <parent link="yumi_link_2_r"/>
    <child link="yumi_link_3_r"/>
    <origin rpy="-1.57079632679 0.0 0.0" xyz="-0.03 0.17283 0.0"/>
    <axis xyz="0 0 1"/>
    <limit effort="300" lower="-2.94087978961" upper="2.94087978961" velocity="3.14159265359"/>
    <dynamics damping="0.5"/>
  </joint>
  <link name="yumi_link_3_r">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="2"/>
      <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
    </inertial>
    <!-- <visual> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/link_3.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Grey"/> -->
    <!-- </visual> -->
    <!-- <collision> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/coarse/link_3.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Grey"/> -->
    <!-- </collision> -->
  </link>
  <!-- joint between link_3_r and link_4_r -->
  <joint name="yumi_joint_3_r" type="revolute">
    <parent link="yumi_link_3_r"/>
    <child link="yumi_link_4_r"/>
    <origin rpy="1.57079632679 -1.57079632679 0.0" xyz="-0.04188 0.0 0.07873"/>
    <axis xyz="0 0 1"/>
    <limit effort="300" lower="-2.15548162621" upper="1.3962634016" velocity="3.14159265359"/>
    <dynamics damping="0.5"/>
  </joint>
  <link name="yumi_link_4_r">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="2"/>
      <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
    </inertial>
    <!-- <visual> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/link_4.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Grey"/> -->
    <!-- </visual> -->
    <!-- <collision> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/coarse/link_4.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Grey"/> -->
    <!-- </collision> -->
  </link>
  <!-- joint between link_4_r and link_5_r -->
  <joint name="yumi_joint_4_r" type="revolute">
    <parent link="yumi_link_4_r"/>
    <child link="yumi_link_5_r"/>
    <origin rpy="-1.57079632679 0.0 0.0" xyz="0.0405 0.16461 0.0"/>
    <axis xyz="0 0 1"/>
    <limit effort="300" lower="-5.06145483078" upper="5.06145483078" velocity="6.98131700798"/>
    <dynamics damping="0.5"/>
  </joint>
  <link name="yumi_link_5_r">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="2"/>
      <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
    </inertial>
    <!-- <visual> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/link_5.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Grey"/> -->
    <!-- </visual> -->
    <!-- <collision> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/coarse/link_5.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Grey"/> -->
    <!-- </collision> -->
  </link>
  <!-- joint between link_5_r and link_6_r -->
  <joint name="yumi_joint_5_r" type="revolute">
    <parent link="yumi_link_5_r"/>
    <child link="yumi_link_6_r"/>
    <origin rpy="1.57079632679 0.0 0.0" xyz="-0.027 0 0.10039"/>
    <axis xyz="0 0 1"/>
    <limit effort="300" lower="-1.53588974176" upper="2.40855436775" velocity="6.98131700798"/>
    <dynamics damping="0.5"/>
  </joint>
  <link name="yumi_link_6_r">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="2"/>
      <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
    </inertial>
    <!-- <visual> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/link_6.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Grey"/> -->
    <!-- </visual> -->
    <!-- <collision> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/coarse/link_6.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Grey"/> -->
    <!-- </collision> -->
  </link>
  <!-- joint between link_6_r and link_7_r -->
  <joint name="yumi_joint_6_r" type="revolute">
    <parent link="yumi_link_6_r"/>
    <child link="yumi_link_7_r"/>
    <origin rpy="-1.57079632679 0.0 0.0" xyz="0.027 0.029 0.0"/>
    <axis xyz="0 0 1"/>
    <limit effort="300" lower="-3.99680398707" upper="3.99680398707" velocity="6.98131700798"/>
    <dynamics damping="0.5"/>
  </joint>
  <link name="yumi_link_7_r">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="2"/>
      <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
    </inertial>
    <!-- <visual> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/link_7.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Grey"/> -->
    <!-- </visual> -->
    <!-- <collision> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/coarse/link_7.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Grey"/> -->
    <!-- </collision> -->
  </link>
  <!--///////////////////////////////////// LEFT ARM ////////////////////////////////  -->
  <!-- joint between body and link_1_l -->
  <joint name="yumi_joint_1_l" type="revolute">
    <parent link="yumi_body"/>
    <child link="yumi_link_1_l"/>
    <origin rpy="0.9781 -0.5716 2.3180" xyz="0.05355 0.07250 0.41492"/>
    <axis xyz="0 0 1"/>
    <limit effort="300" lower="-2.94087978961" upper="2.94087978961" velocity="3.14159265359"/>
    <dynamics damping="0.5"/>
  </joint>
  <link name="yumi_link_1_l">
    <inertial>
      <origin rpy="0 0 0" xyz="0 -0.03 0.12"/>
      <mass value="2"/>
      <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
    </inertial>
    <!-- <visual> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/link_1.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Grey"/> -->
    <!-- </visual> -->
    <!-- <collision> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/coarse/link_1.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Grey"/> -->
    <!-- </collision> -->
  </link>
  <!-- joint between link_1_l and link_2_l -->
  <joint name="yumi_joint_2_l" type="revolute">
    <parent link="yumi_link_1_l"/>
    <child link="yumi_link_2_l"/>
    <origin rpy="1.57079632679 0.0 0.0" xyz="0.03 0.0 0.1"/>
    <axis xyz="0 0 1"/>
    <limit effort="300" lower="-2.50454747661" upper="0.759218224618" velocity="3.14159265359"/>
    <dynamics damping="0.5"/>
  </joint>
  <link name="yumi_link_2_l">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="2"/>
      <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
    </inertial>
    <!-- <visual> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/link_2.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Grey"/> -->
    <!-- </visual> -->
    <!-- <collision> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/coarse/link_2.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Grey"/> -->
    <!-- </collision> -->
  </link>
  <!-- joint between link_2_l and link_3_l -->
  <joint name="yumi_joint_7_l" type="revolute">
    <parent link="yumi_link_2_l"/>
    <child link="yumi_link_3_l"/>
    <origin rpy="-1.57079632679 0.0 0.0" xyz="-0.03 0.17283 0.0"/>
    <axis xyz="0 0 1"/>
    <limit effort="300" lower="-2.94087978961" upper="2.94087978961" velocity="3.14159265359"/>
    <dynamics damping="0.5"/>
  </joint>
  <link name="yumi_link_3_l">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="2"/>
      <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
    </inertial>
    <!-- <visual> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/link_3.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Grey"/> -->
    <!-- </visual> -->
    <!-- <collision> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/coarse/link_3.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Grey"/> -->
    <!-- </collision> -->
  </link>
  <!-- joint between link_3_l and link_4_l -->
  <joint name="yumi_joint_3_l" type="revolute">
    <parent link="yumi_link_3_l"/>
    <child link="yumi_link_4_l"/>
    <origin rpy="1.57079632679 -1.57079632679 0.0" xyz="-0.04188 0.0 0.07873"/>
    <axis xyz="0 0 1"/>
    <limit effort="300" lower="-2.15548162621" upper="1.3962634016" velocity="3.14159265359"/>
    <dynamics damping="0.5"/>
  </joint>
  <link name="yumi_link_4_l">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="2"/>
      <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
    </inertial>
    <!-- <visual> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/link_4.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Grey"/> -->
    <!-- </visual> -->
    <!-- <collision> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/coarse/link_4.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Grey"/> -->
    <!-- </collision> -->
  </link>
  <!-- joint between link_4_l and link_5_l -->
  <joint name="yumi_joint_4_l" type="revolute">
    <parent link="yumi_link_4_l"/>
    <child link="yumi_link_5_l"/>
    <origin rpy="-1.57079632679 0.0 0.0" xyz="0.0405 0.16461 0.0"/>
    <axis xyz="0 0 1"/>
    <limit effort="300" lower="-5.06145483078" upper="5.06145483078" velocity="6.98131700798"/>
    <dynamics damping="0.5"/>
  </joint>
  <link name="yumi_link_5_l">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="2"/>
      <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
    </inertial>
    <!-- <visual> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/link_5.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Grey"/> -->
    <!-- </visual> -->
    <!-- <collision> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/coarse/link_5.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Grey"/> -->
    <!-- </collision> -->
  </link>
  <!-- joint between link_5_l and link_6_l -->
  <joint name="yumi_joint_5_l" type="revolute">
    <parent link="yumi_link_5_l"/>
    <child link="yumi_link_6_l"/>
    <origin rpy="1.57079632679 0.0 0.0" xyz="-0.027 0 0.10039"/>
    <axis xyz="0 0 1"/>
    <limit effort="300" lower="-1.53588974176" upper="2.40855436775" velocity="6.98131700798"/>
    <dynamics damping="0.5"/>
  </joint>
  <link name="yumi_link_6_l">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="2"/>
      <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
    </inertial>
    <!-- <visual> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/link_6.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Grey"/> -->
    <!-- </visual> -->
    <!-- <collision> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/coarse/link_6.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Grey"/> -->
    <!-- </collision> -->
  </link>
  <!-- joint between link_6_l and link_7_l -->
  <joint name="yumi_joint_6_l" type="revolute">
    <parent link="yumi_link_6_l"/>
    <child link="yumi_link_7_l"/>
    <origin rpy="-1.57079632679 0.0 0.0" xyz="0.027 0.029 0.0"/>
    <axis xyz="0 0 1"/>
    <limit effort="300" lower="-3.99680398707" upper="3.99680398707" velocity="6.98131700798"/>
    <dynamics damping="0.5"/>
  </joint>
  <link name="yumi_link_7_l">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="2"/>
      <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
    </inertial>
    <!-- <visual> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/link_7.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Grey"/> -->
    <!-- </visual> -->
    <!-- <collision> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/coarse/link_7.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Grey"/> -->
    <!-- </collision> -->
  </link>
  <!-- Gazebo reference frame -->
  <gazebo reference="yumi_body">
    <material>Gazebo/Ivory</material>
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
  </gazebo>
  <transmission name="yumi_tran_1_r">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="yumi_joint_1_r">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="yumi_motor_1_r">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="yumi_tran_2_r">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="yumi_joint_2_r">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="yumi_motor_2_r">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="yumi_tran_3_r">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="yumi_joint_3_r">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="yumi_motor_3_r">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="yumi_tran_4_r">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="yumi_joint_4_r">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="yumi_motor_4_r">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="yumi_tran_5_r">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="yumi_joint_5_r">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="yumi_motor_5_r">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="yumi_tran_6_r">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="yumi_joint_6_r">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="yumi_motor_6_r">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="yumi_tran_7_r">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="yumi_joint_7_r">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="yumi_motor_7_r">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="yumi_tran_1_l">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="yumi_joint_1_l">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="yumi_motor_1_l">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="yumi_tran_2_l">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="yumi_joint_2_l">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="yumi_motor_2_l">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="yumi_tran_3_l">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="yumi_joint_3_l">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="yumi_motor_3_l">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="yumi_tran_4_l">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="yumi_joint_4_l">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="yumi_motor_4_l">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="yumi_tran_5_l">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="yumi_joint_5_l">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="yumi_motor_5_l">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="yumi_tran_6_l">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="yumi_joint_6_l">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="yumi_motor_6_l">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="yumi_tran_7_l">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="yumi_joint_7_l">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="yumi_motor_7_l">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <!--joint between {parent} and base-->
  <joint name="yumi_link_7_r_joint" type="fixed">
    <origin rpy="0 0 3.14159265359" xyz="0 0 0.007"/>
    <parent link="yumi_link_7_r"/>
    <child link="gripper_r_base"/>
  </joint>
  <link name="gripper_r_base">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="0.2"/>
      <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
    </inertial>
    <!-- <visual> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/gripper/base.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Light_Grey"/> -->
    <!-- </visual> -->
    <!-- <collision> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/gripper/coarse/base.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Light_Grey"/> -->
    <!-- </collision> -->
  </link>
  <!-- joint between base and right finger -->
  <joint name="gripper_r_joint" type="prismatic">
    <parent link="gripper_r_base"/>
    <child link="gripper_r_finger_r"/>
    <origin rpy="0 0 0" xyz="0 -0.0065 0.0837"/>
    <axis xyz="-1 0 0"/>
    <limit effort="20" lower="0" upper="0.025" velocity="2"/>
    <dynamics damping="1.0"/>
  </joint>
  <link name="gripper_r_finger_r">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="0.01"/>
      <inertia ixx="0.000001" ixy="0" ixz="0" iyy="0.000001" iyz="0" izz="0.000001"/>
    </inertial>
    <!-- <visual> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/gripper/finger.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Blue"/> -->
    <!-- </visual> -->
    <!-- <collision> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/gripper/coarse/finger.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Blue"/> -->
    <!-- </collision> -->
  </link>
  <!-- joint between base and left finger -->
  <joint name="gripper_r_joint_m" type="prismatic">
    <parent link="gripper_r_base"/>
    <child link="gripper_r_finger_l"/>
    <origin rpy="0 0 3.14159265359" xyz="0 0.0065 0.0837"/>
    <axis xyz="-1 0 0"/>
    <limit effort="20" lower="0" upper="0.025" velocity="2"/>
    <dynamics damping="1.0"/>
    <mimic joint="gripper_r_joint"/>
  </joint>
  <link name="gripper_r_finger_l">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="0.01"/>
      <inertia ixx="0.000001" ixy="0" ixz="0" iyy="0.000001" iyz="0" izz="0.000001"/>
    </inertial>
    <!-- <visual> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/gripper/finger.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Blue"/> -->
    <!-- </visual> -->
    <!-- <collision> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/gripper/coarse/finger.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Blue"/> -->
    <!-- </collision> -->
  </link>
  <!-- Plugin for the mimic o/c joint - AFAIK, that shouldn't be necessary any more starting with Gazebo 3.0 -->
  <gazebo>
    <plugin filename="libgazebo_mimic_plugin.so" name="mimic_plugin">
      <joint>gripper_r_joint</joint>
      <mimicJoint>gripper_r_joint_m</mimicJoint>
      <multiplier>1.0</multiplier>
    </plugin>
  </gazebo>
  <!-- body -->
  <gazebo reference="gripper_r_base">
    <material>Gazebo/Light_Grey</material>
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
  </gazebo>
  <transmission name="gripper_r_tran">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="gripper_r_joint">
      <hardwareInterface>hardware_interface/EffortJoinInterface</hardwareInterface>
    </joint>
    <actuator name="gripper_r_motor">
      <hardwareInterface>hardware_interface/EffortJoinInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <!--joint between {parent} and base-->
  <joint name="yumi_link_7_l_joint" type="fixed">
    <origin rpy="0 0 3.14159265359" xyz="0 0 0.007"/>
    <parent link="yumi_link_7_l"/>
    <child link="gripper_l_base"/>
  </joint>
  <link name="gripper_l_base">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="0.2"/>
      <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
    </inertial>
    <!-- <visual> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/gripper/base.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Light_Grey"/> -->
    <!-- </visual> -->
    <!-- <collision> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/gripper/coarse/base.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Light_Grey"/> -->
    <!-- </collision> -->
  </link>
  <!-- joint between base and right finger -->
  <joint name="gripper_l_joint" type="prismatic">
    <parent link="gripper_l_base"/>
    <child link="gripper_l_finger_r"/>
    <origin rpy="0 0 0" xyz="0 -0.0065 0.0837"/>
    <axis xyz="-1 0 0"/>
    <limit effort="20" lower="0" upper="0.025" velocity="2"/>
    <dynamics damping="1.0"/>
  </joint>
  <link name="gripper_l_finger_r">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="0.01"/>
      <inertia ixx="0.000001" ixy="0" ixz="0" iyy="0.000001" iyz="0" izz="0.000001"/>
    </inertial>
    <!-- <visual> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/gripper/finger.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Blue"/> -->
    <!-- </visual> -->
    <!-- <collision> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/gripper/coarse/finger.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Blue"/> -->
    <!-- </collision> -->
  </link>
  <!-- joint between base and left finger -->
  <joint name="gripper_l_joint_m" type="prismatic">
    <parent link="gripper_l_base"/>
    <child link="gripper_l_finger_l"/>
    <origin rpy="0 0 3.14159265359" xyz="0 0.0065 0.0837"/>
    <axis xyz="-1 0 0"/>
    <limit effort="20" lower="0" upper="0.025" velocity="2"/>
    <dynamics damping="1.0"/>
    <mimic joint="gripper_l_joint"/>
  </joint>
  <link name="gripper_l_finger_l">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="0.01"/>
      <inertia ixx="0.000001" ixy="0" ixz="0" iyy="0.000001" iyz="0" izz="0.000001"/>
    </inertial>
    <!-- <visual> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/gripper/finger.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Blue"/> -->
    <!-- </visual> -->
    <!-- <collision> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/gripper/coarse/finger.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Blue"/> -->
    <!-- </collision> -->
  </link>
  <!-- Plugin for the mimic o/c joint - AFAIK, that shouldn't be necessary any more starting with Gazebo 3.0 -->
  <gazebo>
    <plugin filename="libgazebo_mimic_plugin.so" name="mimic_plugin">
      <joint>gripper_l_joint</joint>
      <mimicJoint>gripper_l_joint_m</mimicJoint>
      <multiplier>1.0</multiplier>
    </plugin>
  </gazebo>
  <!-- body -->
  <gazebo reference="gripper_l_base">
    <material>Gazebo/Light_Grey</material>
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
  </gazebo>
  <transmission name="gripper_l_tran">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="gripper_l_joint">
      <hardwareInterface>hardware_interface/EffortJoinInterface</hardwareInterface>
    </joint>
    <actuator name="gripper_l_motor">
      <hardwareInterface>hardware_interface/EffortJoinInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <link name="world"/>
  <joint name="world_joint" type="fixed">
    <parent link="world"/>
    <child link="yumi_base_link"/>
    <origin rpy="0 0 0" xyz="0 0 0.1"/>
  </joint>
</robot>
zzzbbs commented 1 year ago

Hi, you can use this URDF file. smile

<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- |    This document was autogenerated by xacro from yumi.urdf.xacro                | -->
<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
<!-- =================================================================================== -->
<robot name="yumi" xmlns:xacro="http://www.ros.org/wiki/xacro">
  <material name="Black">
    <color rgba="0.0 0.0 0.0 1.0"/>
  </material>
  <material name="Blue">
    <color rgba="0.0 0.0 0.8 1.0"/>
  </material>
  <material name="Green">
    <color rgba="0.0 0.8 0.0 1.0"/>
  </material>
  <material name="Grey">
    <color rgba="0.5 0.5 0.5 1.0"/>
  </material>
  <material name="Light_Grey">
    <color rgba="0.8 0.8 0.8 1.0"/>
  </material>
  <material name="Ivory">
    <color rgba="1.0 1.0 0.94 1.0"/>
  </material>
  <material name="Orange">
    <color rgba="1.0 0.423529411765 0.0392156862745 1.0"/>
  </material>
  <material name="Brown">
    <color rgba="0.870588235294 0.811764705882 0.764705882353 1.0"/>
  </material>
  <material name="Red">
    <color rgba="0.8 0.0 0.0 1.0"/>
  </material>
  <material name="White">
    <color rgba="1.0 1.0 1.0 1.0"/>
  </material>
  <gazebo>
    <plugin filename="libgazebo_ros_control.so" name="gazebo_ros_controller">
      <robotNamespace>/yumi</robotNamespace>
    </plugin>
  </gazebo>
  <link name="yumi_base_link"/>
  <joint name="yumi_base_link_to_body" type="fixed">
    <parent link="yumi_base_link"/>
    <child link="yumi_body"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
  </joint>
  <link name="yumi_body">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="15"/>
      <inertia ixx="0.5" ixy="0" ixz="0" iyy="0.6" iyz="0" izz="0.3"/>
    </inertial>
    <!-- <visual> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/body.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Light_Grey"/> -->
    <!-- </visual> -->
    <!-- <collision> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/coarse/body.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Light_Grey"/> -->
    <!-- </collision> -->
  </link>
  <!--///////////////////////////////////// RIGHT ARM ////////////////////////////////  -->
  <!-- joint between body and link_1_r -->
  <joint name="yumi_joint_1_r" type="revolute">
    <parent link="yumi_body"/>
    <child link="yumi_link_1_r"/>
    <origin rpy="-0.9795   -0.5682   -2.3155" xyz="0.05355 -0.0725 0.41492"/>
    <axis xyz="0 0 1"/>
    <limit effort="300" lower="-2.94087978961" upper="2.94087978961" velocity="3.14159265359"/>
    <dynamics damping="0.5"/>
  </joint>
  <link name="yumi_link_1_r">
    <inertial>
      <origin rpy="0 0 0" xyz="0 -0.03 0.12"/>
      <mass value="2"/>
      <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
    </inertial>
    <!-- <visual> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/link_1.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Grey"/> -->
    <!-- </visual> -->
    <!-- <collision> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/coarse/link_1.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Grey"/> -->
    <!-- </collision> -->
  </link>
  <!-- joint between link_1_r and link_2_r -->
  <joint name="yumi_joint_2_r" type="revolute">
    <parent link="yumi_link_1_r"/>
    <child link="yumi_link_2_r"/>
    <origin rpy="1.57079632679 0.0 0.0" xyz="0.03 0.0 0.1"/>
    <axis xyz="0 0 1"/>
    <limit effort="300" lower="-2.50454747661" upper="0.759218224618" velocity="3.14159265359"/>
    <dynamics damping="0.5"/>
  </joint>
  <link name="yumi_link_2_r">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="2"/>
      <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
    </inertial>
    <!-- <visual> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/link_2.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Grey"/> -->
    <!-- </visual> -->
    <!-- <collision> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/coarse/link_2.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Grey"/> -->
    <!-- </collision> -->
  </link>
  <!-- joint between link_2_r and link_3_r -->
  <joint name="yumi_joint_7_r" type="revolute">
    <parent link="yumi_link_2_r"/>
    <child link="yumi_link_3_r"/>
    <origin rpy="-1.57079632679 0.0 0.0" xyz="-0.03 0.17283 0.0"/>
    <axis xyz="0 0 1"/>
    <limit effort="300" lower="-2.94087978961" upper="2.94087978961" velocity="3.14159265359"/>
    <dynamics damping="0.5"/>
  </joint>
  <link name="yumi_link_3_r">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="2"/>
      <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
    </inertial>
    <!-- <visual> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/link_3.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Grey"/> -->
    <!-- </visual> -->
    <!-- <collision> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/coarse/link_3.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Grey"/> -->
    <!-- </collision> -->
  </link>
  <!-- joint between link_3_r and link_4_r -->
  <joint name="yumi_joint_3_r" type="revolute">
    <parent link="yumi_link_3_r"/>
    <child link="yumi_link_4_r"/>
    <origin rpy="1.57079632679 -1.57079632679 0.0" xyz="-0.04188 0.0 0.07873"/>
    <axis xyz="0 0 1"/>
    <limit effort="300" lower="-2.15548162621" upper="1.3962634016" velocity="3.14159265359"/>
    <dynamics damping="0.5"/>
  </joint>
  <link name="yumi_link_4_r">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="2"/>
      <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
    </inertial>
    <!-- <visual> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/link_4.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Grey"/> -->
    <!-- </visual> -->
    <!-- <collision> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/coarse/link_4.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Grey"/> -->
    <!-- </collision> -->
  </link>
  <!-- joint between link_4_r and link_5_r -->
  <joint name="yumi_joint_4_r" type="revolute">
    <parent link="yumi_link_4_r"/>
    <child link="yumi_link_5_r"/>
    <origin rpy="-1.57079632679 0.0 0.0" xyz="0.0405 0.16461 0.0"/>
    <axis xyz="0 0 1"/>
    <limit effort="300" lower="-5.06145483078" upper="5.06145483078" velocity="6.98131700798"/>
    <dynamics damping="0.5"/>
  </joint>
  <link name="yumi_link_5_r">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="2"/>
      <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
    </inertial>
    <!-- <visual> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/link_5.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Grey"/> -->
    <!-- </visual> -->
    <!-- <collision> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/coarse/link_5.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Grey"/> -->
    <!-- </collision> -->
  </link>
  <!-- joint between link_5_r and link_6_r -->
  <joint name="yumi_joint_5_r" type="revolute">
    <parent link="yumi_link_5_r"/>
    <child link="yumi_link_6_r"/>
    <origin rpy="1.57079632679 0.0 0.0" xyz="-0.027 0 0.10039"/>
    <axis xyz="0 0 1"/>
    <limit effort="300" lower="-1.53588974176" upper="2.40855436775" velocity="6.98131700798"/>
    <dynamics damping="0.5"/>
  </joint>
  <link name="yumi_link_6_r">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="2"/>
      <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
    </inertial>
    <!-- <visual> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/link_6.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Grey"/> -->
    <!-- </visual> -->
    <!-- <collision> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/coarse/link_6.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Grey"/> -->
    <!-- </collision> -->
  </link>
  <!-- joint between link_6_r and link_7_r -->
  <joint name="yumi_joint_6_r" type="revolute">
    <parent link="yumi_link_6_r"/>
    <child link="yumi_link_7_r"/>
    <origin rpy="-1.57079632679 0.0 0.0" xyz="0.027 0.029 0.0"/>
    <axis xyz="0 0 1"/>
    <limit effort="300" lower="-3.99680398707" upper="3.99680398707" velocity="6.98131700798"/>
    <dynamics damping="0.5"/>
  </joint>
  <link name="yumi_link_7_r">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="2"/>
      <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
    </inertial>
    <!-- <visual> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/link_7.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Grey"/> -->
    <!-- </visual> -->
    <!-- <collision> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/coarse/link_7.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Grey"/> -->
    <!-- </collision> -->
  </link>
  <!--///////////////////////////////////// LEFT ARM ////////////////////////////////  -->
  <!-- joint between body and link_1_l -->
  <joint name="yumi_joint_1_l" type="revolute">
    <parent link="yumi_body"/>
    <child link="yumi_link_1_l"/>
    <origin rpy="0.9781 -0.5716 2.3180" xyz="0.05355 0.07250 0.41492"/>
    <axis xyz="0 0 1"/>
    <limit effort="300" lower="-2.94087978961" upper="2.94087978961" velocity="3.14159265359"/>
    <dynamics damping="0.5"/>
  </joint>
  <link name="yumi_link_1_l">
    <inertial>
      <origin rpy="0 0 0" xyz="0 -0.03 0.12"/>
      <mass value="2"/>
      <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
    </inertial>
    <!-- <visual> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/link_1.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Grey"/> -->
    <!-- </visual> -->
    <!-- <collision> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/coarse/link_1.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Grey"/> -->
    <!-- </collision> -->
  </link>
  <!-- joint between link_1_l and link_2_l -->
  <joint name="yumi_joint_2_l" type="revolute">
    <parent link="yumi_link_1_l"/>
    <child link="yumi_link_2_l"/>
    <origin rpy="1.57079632679 0.0 0.0" xyz="0.03 0.0 0.1"/>
    <axis xyz="0 0 1"/>
    <limit effort="300" lower="-2.50454747661" upper="0.759218224618" velocity="3.14159265359"/>
    <dynamics damping="0.5"/>
  </joint>
  <link name="yumi_link_2_l">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="2"/>
      <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
    </inertial>
    <!-- <visual> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/link_2.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Grey"/> -->
    <!-- </visual> -->
    <!-- <collision> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/coarse/link_2.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Grey"/> -->
    <!-- </collision> -->
  </link>
  <!-- joint between link_2_l and link_3_l -->
  <joint name="yumi_joint_7_l" type="revolute">
    <parent link="yumi_link_2_l"/>
    <child link="yumi_link_3_l"/>
    <origin rpy="-1.57079632679 0.0 0.0" xyz="-0.03 0.17283 0.0"/>
    <axis xyz="0 0 1"/>
    <limit effort="300" lower="-2.94087978961" upper="2.94087978961" velocity="3.14159265359"/>
    <dynamics damping="0.5"/>
  </joint>
  <link name="yumi_link_3_l">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="2"/>
      <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
    </inertial>
    <!-- <visual> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/link_3.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Grey"/> -->
    <!-- </visual> -->
    <!-- <collision> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/coarse/link_3.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Grey"/> -->
    <!-- </collision> -->
  </link>
  <!-- joint between link_3_l and link_4_l -->
  <joint name="yumi_joint_3_l" type="revolute">
    <parent link="yumi_link_3_l"/>
    <child link="yumi_link_4_l"/>
    <origin rpy="1.57079632679 -1.57079632679 0.0" xyz="-0.04188 0.0 0.07873"/>
    <axis xyz="0 0 1"/>
    <limit effort="300" lower="-2.15548162621" upper="1.3962634016" velocity="3.14159265359"/>
    <dynamics damping="0.5"/>
  </joint>
  <link name="yumi_link_4_l">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="2"/>
      <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
    </inertial>
    <!-- <visual> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/link_4.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Grey"/> -->
    <!-- </visual> -->
    <!-- <collision> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/coarse/link_4.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Grey"/> -->
    <!-- </collision> -->
  </link>
  <!-- joint between link_4_l and link_5_l -->
  <joint name="yumi_joint_4_l" type="revolute">
    <parent link="yumi_link_4_l"/>
    <child link="yumi_link_5_l"/>
    <origin rpy="-1.57079632679 0.0 0.0" xyz="0.0405 0.16461 0.0"/>
    <axis xyz="0 0 1"/>
    <limit effort="300" lower="-5.06145483078" upper="5.06145483078" velocity="6.98131700798"/>
    <dynamics damping="0.5"/>
  </joint>
  <link name="yumi_link_5_l">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="2"/>
      <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
    </inertial>
    <!-- <visual> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/link_5.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Grey"/> -->
    <!-- </visual> -->
    <!-- <collision> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/coarse/link_5.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Grey"/> -->
    <!-- </collision> -->
  </link>
  <!-- joint between link_5_l and link_6_l -->
  <joint name="yumi_joint_5_l" type="revolute">
    <parent link="yumi_link_5_l"/>
    <child link="yumi_link_6_l"/>
    <origin rpy="1.57079632679 0.0 0.0" xyz="-0.027 0 0.10039"/>
    <axis xyz="0 0 1"/>
    <limit effort="300" lower="-1.53588974176" upper="2.40855436775" velocity="6.98131700798"/>
    <dynamics damping="0.5"/>
  </joint>
  <link name="yumi_link_6_l">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="2"/>
      <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
    </inertial>
    <!-- <visual> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/link_6.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Grey"/> -->
    <!-- </visual> -->
    <!-- <collision> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/coarse/link_6.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Grey"/> -->
    <!-- </collision> -->
  </link>
  <!-- joint between link_6_l and link_7_l -->
  <joint name="yumi_joint_6_l" type="revolute">
    <parent link="yumi_link_6_l"/>
    <child link="yumi_link_7_l"/>
    <origin rpy="-1.57079632679 0.0 0.0" xyz="0.027 0.029 0.0"/>
    <axis xyz="0 0 1"/>
    <limit effort="300" lower="-3.99680398707" upper="3.99680398707" velocity="6.98131700798"/>
    <dynamics damping="0.5"/>
  </joint>
  <link name="yumi_link_7_l">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="2"/>
      <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
    </inertial>
    <!-- <visual> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/link_7.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Grey"/> -->
    <!-- </visual> -->
    <!-- <collision> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/coarse/link_7.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Grey"/> -->
    <!-- </collision> -->
  </link>
  <!-- Gazebo reference frame -->
  <gazebo reference="yumi_body">
    <material>Gazebo/Ivory</material>
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
  </gazebo>
  <transmission name="yumi_tran_1_r">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="yumi_joint_1_r">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="yumi_motor_1_r">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="yumi_tran_2_r">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="yumi_joint_2_r">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="yumi_motor_2_r">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="yumi_tran_3_r">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="yumi_joint_3_r">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="yumi_motor_3_r">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="yumi_tran_4_r">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="yumi_joint_4_r">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="yumi_motor_4_r">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="yumi_tran_5_r">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="yumi_joint_5_r">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="yumi_motor_5_r">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="yumi_tran_6_r">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="yumi_joint_6_r">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="yumi_motor_6_r">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="yumi_tran_7_r">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="yumi_joint_7_r">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="yumi_motor_7_r">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="yumi_tran_1_l">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="yumi_joint_1_l">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="yumi_motor_1_l">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="yumi_tran_2_l">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="yumi_joint_2_l">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="yumi_motor_2_l">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="yumi_tran_3_l">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="yumi_joint_3_l">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="yumi_motor_3_l">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="yumi_tran_4_l">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="yumi_joint_4_l">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="yumi_motor_4_l">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="yumi_tran_5_l">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="yumi_joint_5_l">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="yumi_motor_5_l">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="yumi_tran_6_l">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="yumi_joint_6_l">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="yumi_motor_6_l">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="yumi_tran_7_l">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="yumi_joint_7_l">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="yumi_motor_7_l">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <!--joint between {parent} and base-->
  <joint name="yumi_link_7_r_joint" type="fixed">
    <origin rpy="0 0 3.14159265359" xyz="0 0 0.007"/>
    <parent link="yumi_link_7_r"/>
    <child link="gripper_r_base"/>
  </joint>
  <link name="gripper_r_base">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="0.2"/>
      <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
    </inertial>
    <!-- <visual> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/gripper/base.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Light_Grey"/> -->
    <!-- </visual> -->
    <!-- <collision> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/gripper/coarse/base.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Light_Grey"/> -->
    <!-- </collision> -->
  </link>
  <!-- joint between base and right finger -->
  <joint name="gripper_r_joint" type="prismatic">
    <parent link="gripper_r_base"/>
    <child link="gripper_r_finger_r"/>
    <origin rpy="0 0 0" xyz="0 -0.0065 0.0837"/>
    <axis xyz="-1 0 0"/>
    <limit effort="20" lower="0" upper="0.025" velocity="2"/>
    <dynamics damping="1.0"/>
  </joint>
  <link name="gripper_r_finger_r">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="0.01"/>
      <inertia ixx="0.000001" ixy="0" ixz="0" iyy="0.000001" iyz="0" izz="0.000001"/>
    </inertial>
    <!-- <visual> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/gripper/finger.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Blue"/> -->
    <!-- </visual> -->
    <!-- <collision> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/gripper/coarse/finger.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Blue"/> -->
    <!-- </collision> -->
  </link>
  <!-- joint between base and left finger -->
  <joint name="gripper_r_joint_m" type="prismatic">
    <parent link="gripper_r_base"/>
    <child link="gripper_r_finger_l"/>
    <origin rpy="0 0 3.14159265359" xyz="0 0.0065 0.0837"/>
    <axis xyz="-1 0 0"/>
    <limit effort="20" lower="0" upper="0.025" velocity="2"/>
    <dynamics damping="1.0"/>
    <mimic joint="gripper_r_joint"/>
  </joint>
  <link name="gripper_r_finger_l">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="0.01"/>
      <inertia ixx="0.000001" ixy="0" ixz="0" iyy="0.000001" iyz="0" izz="0.000001"/>
    </inertial>
    <!-- <visual> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/gripper/finger.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Blue"/> -->
    <!-- </visual> -->
    <!-- <collision> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/gripper/coarse/finger.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Blue"/> -->
    <!-- </collision> -->
  </link>
  <!-- Plugin for the mimic o/c joint - AFAIK, that shouldn't be necessary any more starting with Gazebo 3.0 -->
  <gazebo>
    <plugin filename="libgazebo_mimic_plugin.so" name="mimic_plugin">
      <joint>gripper_r_joint</joint>
      <mimicJoint>gripper_r_joint_m</mimicJoint>
      <multiplier>1.0</multiplier>
    </plugin>
  </gazebo>
  <!-- body -->
  <gazebo reference="gripper_r_base">
    <material>Gazebo/Light_Grey</material>
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
  </gazebo>
  <transmission name="gripper_r_tran">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="gripper_r_joint">
      <hardwareInterface>hardware_interface/EffortJoinInterface</hardwareInterface>
    </joint>
    <actuator name="gripper_r_motor">
      <hardwareInterface>hardware_interface/EffortJoinInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <!--joint between {parent} and base-->
  <joint name="yumi_link_7_l_joint" type="fixed">
    <origin rpy="0 0 3.14159265359" xyz="0 0 0.007"/>
    <parent link="yumi_link_7_l"/>
    <child link="gripper_l_base"/>
  </joint>
  <link name="gripper_l_base">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="0.2"/>
      <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
    </inertial>
    <!-- <visual> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/gripper/base.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Light_Grey"/> -->
    <!-- </visual> -->
    <!-- <collision> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/gripper/coarse/base.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Light_Grey"/> -->
    <!-- </collision> -->
  </link>
  <!-- joint between base and right finger -->
  <joint name="gripper_l_joint" type="prismatic">
    <parent link="gripper_l_base"/>
    <child link="gripper_l_finger_r"/>
    <origin rpy="0 0 0" xyz="0 -0.0065 0.0837"/>
    <axis xyz="-1 0 0"/>
    <limit effort="20" lower="0" upper="0.025" velocity="2"/>
    <dynamics damping="1.0"/>
  </joint>
  <link name="gripper_l_finger_r">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="0.01"/>
      <inertia ixx="0.000001" ixy="0" ixz="0" iyy="0.000001" iyz="0" izz="0.000001"/>
    </inertial>
    <!-- <visual> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/gripper/finger.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Blue"/> -->
    <!-- </visual> -->
    <!-- <collision> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/gripper/coarse/finger.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Blue"/> -->
    <!-- </collision> -->
  </link>
  <!-- joint between base and left finger -->
  <joint name="gripper_l_joint_m" type="prismatic">
    <parent link="gripper_l_base"/>
    <child link="gripper_l_finger_l"/>
    <origin rpy="0 0 3.14159265359" xyz="0 0.0065 0.0837"/>
    <axis xyz="-1 0 0"/>
    <limit effort="20" lower="0" upper="0.025" velocity="2"/>
    <dynamics damping="1.0"/>
    <mimic joint="gripper_l_joint"/>
  </joint>
  <link name="gripper_l_finger_l">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="0.01"/>
      <inertia ixx="0.000001" ixy="0" ixz="0" iyy="0.000001" iyz="0" izz="0.000001"/>
    </inertial>
    <!-- <visual> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/gripper/finger.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Blue"/> -->
    <!-- </visual> -->
    <!-- <collision> -->
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <!-- <geometry> -->
        <!-- <mesh filename="package://yumi_description/meshes/gripper/coarse/finger.stl"/> -->
      <!-- </geometry> -->
      <!-- <material name="Blue"/> -->
    <!-- </collision> -->
  </link>
  <!-- Plugin for the mimic o/c joint - AFAIK, that shouldn't be necessary any more starting with Gazebo 3.0 -->
  <gazebo>
    <plugin filename="libgazebo_mimic_plugin.so" name="mimic_plugin">
      <joint>gripper_l_joint</joint>
      <mimicJoint>gripper_l_joint_m</mimicJoint>
      <multiplier>1.0</multiplier>
    </plugin>
  </gazebo>
  <!-- body -->
  <gazebo reference="gripper_l_base">
    <material>Gazebo/Light_Grey</material>
    <mu1>0.2</mu1>
    <mu2>0.2</mu2>
  </gazebo>
  <transmission name="gripper_l_tran">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="gripper_l_joint">
      <hardwareInterface>hardware_interface/EffortJoinInterface</hardwareInterface>
    </joint>
    <actuator name="gripper_l_motor">
      <hardwareInterface>hardware_interface/EffortJoinInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <link name="world"/>
  <joint name="world_joint" type="fixed">
    <parent link="world"/>
    <child link="yumi_base_link"/>
    <origin rpy="0 0 0" xyz="0 0 0.1"/>
  </joint>
</robot>

Thank you very much for your reply!! This is really a greak work on solving the problem of human motion to different robot architecture. I am now doing the same work. Could you provide a example or suggestion on how to using NAO or PEPPER in this project ? I would be most grateful if you could give me some suggestion~