search

StanfordVL / OmniGibson

OmniGibson: a platform for accelerating Embodied AI research built upon NVIDIA's Omniverse engine. Join our Discord for support: https://discord.gg/bccR5vGFEx
https://behavior.stanford.edu/omnigibson/
MIT License
522 stars 54 forks source link

Negative mesh scale trips pose assertion after setting xform properties when importing new robot #801

Open cgokmen opened 4 months ago

cgokmen commented 4 months ago

@bohanwu was trying to import a robot today using the tutorial and ran into the below assertion error:

assert np.allclose(new_position, current_position, atol=1e-4) and np.allclose(r1, r2, atol=1e-4), \
    f"{self.prim_path}: old_pos: {current_position}, new_pos: {new_position}, " \
    f"old_orn: {current_orientation}, new_orn: {new_orientation}"

He says the old orn is different from the new orn for almost all the links, for example:

Issue stems from there being a negative scale in the URDF file's meshes, which we do not support. We need to instead replace these with positive-scaled URDF tags that point to already-scaled copies of the obj files. This can easily be done with a script, e.g. 

import trimesh

obj = trimesh.load("file.obj")
scale = np.eye(4)
scale[1, 1] = -1   # this is assuming it's the Y axis thats negativelys caled in the URDF
obj.apply_transform(scale)
obj.export("file_scaled.obj")

Can we introduce a script that automatically performs this on a URDF and then imports it into OmniGibson? Can we also assert nonnegative scale prior to setting xform properties?

cgokmen commented 4 months ago

Example file:

<?xml version="1.0" encoding="utf-8"?>
<!-- =================================================================================== -->
<!-- |    This document was autogenerated by xacro from /opt/pal/ferrum/share/tiago_dual_description/robots/tiago_dual.urdf.xacro | -->
<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
<!-- =================================================================================== -->
<!--
  Copyright (c) 2019, PAL Robotics, S.L.
  All rights reserved.

  This work is licensed under the Creative Commons Attribution-NonCommercial-NoDerivs 3.0 Unported License.
  To view a copy of this license, visit http://creativecommons.org/licenses/by-nc-nd/3.0/ or send a letter to
  Creative Commons, 444 Castro Street, Suite 900, Mountain View, California, 94041, USA.
-->
<robot name="tiago_dual">
  <material name="Hey5DarkGrey">
    <color rgba="0.1 0.1 0.1 1.0"/>
  </material>
  <!--TODO: Check-->
  <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.7 0.7 0.7 1.0"/>
  </material>
  <material name="LightGrey">
    <color rgba="0.9 0.9 0.9 1.0"/>
  </material>
  <material name="DarkGrey">
    <color rgba="0.1 0.1 0.1 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>
  <material name="Yellow">
    <color rgba="0.96 0.88 0.14 1.0"/>
  </material>
  <link name="base_rear_laser_link" type="laser">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.02559000000 -0.00056000000 -0.05732000000"/>
      <mass value="0.28922000000"/>
      <inertia ixx="0.00002628919" ixy="0.00000024298" ixz="-0.00000368129" iyy="0.00003374542" iyz="-0.00000001330" izz="0.00005832599"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/sensors/sick_tim551.obj"/>
      </geometry>
      <material name="DarkGrey"/>
    </visual>
  </link>
  <joint name="base_rear_laser_joint" type="fixed">
    <axis xyz="0 1 0"/>
    <origin rpy="-3.14159265359 0 2.35619449019" xyz="-0.27512 0.18297 0.13244"/>
    <parent link="base_link"/>
    <child link="base_rear_laser_link"/>
  </joint>
  <link name="base_front_laser_link" type="laser">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.02559000000 -0.00056000000 -0.05732000000"/>
      <mass value="0.28922000000"/>
      <inertia ixx="0.00002628919" ixy="0.00000024298" ixz="-0.00000368129" iyy="0.00003374542" iyz="-0.00000001330" izz="0.00005832599"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/sensors/sick_tim551.obj"/>
      </geometry>
      <material name="DarkGrey"/>
    </visual>
  </link>
  <joint name="base_front_laser_joint" type="fixed">
    <axis xyz="0 1 0"/>
    <origin rpy="-3.14159265359 0 -0.785398163397" xyz="0.27512 -0.18297 0.13244"/>
    <parent link="base_link"/>
    <child link="base_front_laser_link"/>
  </joint>
  <!-- Virtual Base Laser Link -->
  <joint name="virtual_base_laser_joint" type="fixed">
    <parent link="base_link"/>
    <child link="virtual_base_laser_link"/>
    <origin rpy="0 0 0" xyz="0 0 0.13244"/>
  </joint>
  <link name="virtual_base_laser_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="0.0001"/>
      <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
    </inertial>
  </link>
  <!-- Base footprint -->
  <link name="base_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0026742 0.00027459 0.086363"/>
      <mass value="34.047"/>
      <inertia ixx="0.22777" ixy="0.00035979" ixz="0.00015269" iyy="0.29991" iyz="1.7872E-05" izz="0.46036"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/base/base_link.obj"/>
      </geometry>
      <material name="">
        <color rgba="0.75294 0.75294 0.75294 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/base/base_link.obj"/>
      </geometry>
    </collision>
  </link>
  <link name="base_footprint"/>
  <joint name="base_footprint_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0.0762"/>
    <child link="base_link"/>
    <parent link="base_footprint"/>
  </joint>
  <!-- gazebo parameters are given here for base_link through base_footprint -->
  <link name="base_imu_link">
    <inertial>
      <mass value="0.01"/>
      <origin xyz="0 0 0"/>
      <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>
          <box size="0.01 0.01 0.01"/>
        </geometry>
      </visual-->
  </link>
  <joint name="base_imu_joint" type="fixed">
    <parent link="base_link"/>
    <child link="base_imu_link"/>
    <origin rpy="0 0 1.57079632679" xyz="-0.287 0 0.092163"/>
  </joint>
  <!-- Docking link -->
  <link name="base_dock_link">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.0037082 1.5226E-18 -4.1633E-17"/>
      <mass value="0.032242"/>
      <inertia ixx="2.1302E-05" ixy="-1.0461E-21" ixz="6.6555E-22" iyy="1.2058E-05" iyz="-1.9453E-21" izz="1.0058E-05"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/base/base_dock_link.obj"/>
      </geometry>
      <material name="">
        <color rgba="0.25098 0.25098 0.25098 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/base/base_dock_link.obj"/>
      </geometry>
    </collision>
  </link>
  <joint name="base_dock" type="fixed">
    <origin rpy="0 0 0" xyz="0.3535 0 0.0596"/>
    <parent link="base_link"/>
    <child link="base_dock_link"/>
    <axis xyz="0 0 0"/>
  </joint>
  <link name="base_antenna_right_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.027807 -0.0085125 0.015138"/>
      <mass value="0.02311"/>
      <inertia ixx="2.3107E-06" ixy="5.0914e-06" ixz="8.5605E-08" iyy="1.7384E-05" iyz="-2.6206e-08" izz="1.8542E-05"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/base/base_antena_link.obj" scale="1 1 1"/>
      </geometry>
      <material name="">
        <color rgba="0.75294 0.75294 0.75294 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/base/base_antena_link.obj" scale="1 1 1"/>
      </geometry>
    </collision>
  </link>
  <joint name="base_antenna_right_joint" type="fixed">
    <origin rpy="0 0 0" xyz="-0.2015 -0.13 0.2165"/>
    <parent link="base_link"/>
    <child link="base_antenna_right_link"/>
    <axis xyz="0 0 0"/>
  </joint>
  <link name="base_antenna_left_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.027807 0.0085125 0.015138"/>
      <mass value="0.02311"/>
      <inertia ixx="2.3107E-06" ixy="-5.0914e-06" ixz="8.5605E-08" iyy="1.7384E-05" iyz="2.6206e-08" izz="1.8542E-05"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/base/base_antena_link.obj" scale="1 -1 1"/>
      </geometry>
      <material name="">
        <color rgba="0.75294 0.75294 0.75294 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/base/base_antena_link.obj" scale="1 -1 1"/>
      </geometry>
    </collision>
  </link>
  <joint name="base_antenna_left_joint" type="fixed">
    <origin rpy="0 0 0" xyz="-0.2015 0.13 0.2165"/>
    <parent link="base_link"/>
    <child link="base_antenna_left_link"/>
    <axis xyz="0 0 0"/>
  </joint>
  <link name="suspension_front_right_link">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.058182 -0.0043908 -0.0057695"/>
      <mass value="1.2879"/>
      <inertia ixx="0.0015947" ixy="-0.00013183" ixz="1.7315E-05" iyy="0.00060158" iyz="-6.0731e-05" izz="0.0015861"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/wheels/suspension_front_link.obj" scale="1 1.0 1"/>
      </geometry>
      <material name="">
        <color rgba="0.79216 0.81961 0.93333 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/wheels/suspension_front_link.obj" scale="1 1.0 1"/>
      </geometry>
    </collision>
    <kinematic>0</kinematic>
    <gravity>1</gravity>
  </link>
  <joint name="suspension_front_right_joint" type="fixed">
    <origin rpy="0 0 -1.57079632679" xyz="0.244 -0.1725 0"/>
    <parent link="base_link"/>
    <child link="suspension_front_right_link"/>
    <axis xyz="0 0 1"/>
    <calibration rising="0.0"/>
    <dynamics damping="100"/>
    <limit effort="100" lower="-0.01" upper="0.01" velocity="1.0"/>
  </joint>
  <link name="wheel_front_right_link">
    <inertial>
      <origin rpy="0 0 0" xyz="5.4922e-09 6.9775e-05 0.011144"/>
      <mass value="0.70767"/>
      <inertia ixx="0.00035492" ixy="7.5685e-13" ixz="3.8795E-11" iyy="0.00035529" iyz="-2.0284e-07" izz="0.00040131"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/wheels/wheel_link.obj" scale="1 1 1.0"/>
      </geometry>
      <material name="DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="0.05" radius="0.0762"/>
      </geometry>
    </collision>
  </link>
  <joint name="wheel_front_right_joint" type="fixed">
    <origin rpy="-1.57079632679 0 1.57079632679" xyz="0.050675 0 0"/>
    <parent link="suspension_front_right_link"/>
    <child link="wheel_front_right_link"/>
    <axis xyz="0 0 1"/>
    <limit effort="6.0" velocity="13.1233595801"/>
  </joint>
  <transmission name="wheel_front_right_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="wheel_front_right_motor">
      <mechanicalReduction>1.0</mechanicalReduction>
    </actuator>
    <joint name="wheel_front_right_joint">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
  </transmission>
  <!-- Contact model for the wheel surface -->
  <link name="suspension_front_left_link">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.058182 0.0043908 -0.0057695"/>
      <mass value="1.2879"/>
      <inertia ixx="0.0015947" ixy="0.00013183" ixz="1.7315E-05" iyy="0.00060158" iyz="6.0731e-05" izz="0.0015861"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/wheels/suspension_front_link.obj" scale="1 -1.0 1"/>
      </geometry>
      <material name="">
        <color rgba="0.79216 0.81961 0.93333 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/wheels/suspension_front_link.obj" scale="1 -1.0 1"/>
      </geometry>
    </collision>
    <kinematic>0</kinematic>
    <gravity>1</gravity>
  </link>
  <joint name="suspension_front_left_joint" type="fixed">
    <origin rpy="0 0 1.57079632679" xyz="0.244 0.1725 0"/>
    <parent link="base_link"/>
    <child link="suspension_front_left_link"/>
    <axis xyz="0 0 1"/>
    <calibration rising="0.0"/>
    <dynamics damping="100"/>
    <limit effort="100" lower="-0.01" upper="0.01" velocity="1.0"/>
  </joint>
  <link name="wheel_front_left_link">
    <inertial>
      <origin rpy="0 0 0" xyz="5.4922e-09 -6.9775e-05 -0.011144"/>
      <mass value="0.70767"/>
      <inertia ixx="0.00035492" ixy="7.5685e-13" ixz="3.8795E-11" iyy="0.00035529" iyz="-2.0284e-07" izz="0.00040131"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/wheels/wheel_link.obj" scale="1 1 -1.0"/>
      </geometry>
      <material name="DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="0.05" radius="0.0762"/>
      </geometry>
    </collision>
  </link>
  <joint name="wheel_front_left_joint" type="fixed">
    <origin rpy="-1.57079632679 0 -1.57079632679" xyz="0.050675 0 0"/>
    <parent link="suspension_front_left_link"/>
    <child link="wheel_front_left_link"/>
    <axis xyz="0 0 1"/>
    <limit effort="6.0" velocity="13.1233595801"/>
  </joint>
  <transmission name="wheel_front_left_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="wheel_front_left_motor">
      <mechanicalReduction>1.0</mechanicalReduction>
    </actuator>
    <joint name="wheel_front_left_joint">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
  </transmission>
  <!-- Contact model for the wheel surface -->
  <link name="suspension_rear_right_link">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.058182 0.0043908 -0.0057695"/>
      <mass value="1.2879"/>
      <inertia ixx="0.0015947" ixy="0.00013183" ixz="1.7315E-05" iyy="0.00060158" iyz="6.0731e-05" izz="0.0015861"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/wheels/suspension_rear_link.obj" scale="1 1.0 1"/>
      </geometry>
      <material name="">
        <color rgba="0.79216 0.81961 0.93333 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/wheels/suspension_rear_link.obj" scale="1 1.0 1"/>
      </geometry>
    </collision>
    <kinematic>0</kinematic>
    <gravity>1</gravity>
  </link>
  <joint name="suspension_rear_right_joint" type="fixed">
    <origin rpy="0 0 -1.57079632679" xyz="-0.244 -0.1725 0"/>
    <parent link="base_link"/>
    <child link="suspension_rear_right_link"/>
    <axis xyz="0 0 1"/>
    <calibration rising="0.0"/>
    <dynamics damping="100"/>
    <limit effort="100" lower="-0.01" upper="0.01" velocity="1.0"/>
  </joint>
  <link name="wheel_rear_right_link">
    <inertial>
      <origin rpy="0 0 0" xyz="-5.4922e-09 -6.9775e-05 0.011144"/>
      <mass value="0.70767"/>
      <inertia ixx="0.00035492" ixy="-7.5685e-13" ixz="3.8795E-11" iyy="0.00035529" iyz="2.0284e-07" izz="0.00040131"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/wheels/wheel_link.obj" scale="1 1 1.0"/>
      </geometry>
      <material name="DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="0.05" radius="0.0762"/>
      </geometry>
    </collision>
  </link>
  <joint name="wheel_rear_right_joint" type="fixed">
    <origin rpy="-1.57079632679 0 1.57079632679" xyz="0.050675 0 0"/>
    <parent link="suspension_rear_right_link"/>
    <child link="wheel_rear_right_link"/>
    <axis xyz="0 0 1"/>
    <limit effort="6.0" velocity="13.1233595801"/>
  </joint>
  <transmission name="wheel_rear_right_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="wheel_rear_right_motor">
      <mechanicalReduction>1.0</mechanicalReduction>
    </actuator>
    <joint name="wheel_rear_right_joint">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
  </transmission>
  <!-- Contact model for the wheel surface -->
  <link name="suspension_rear_left_link">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.058182 -0.0043908 -0.0057695"/>
      <mass value="1.2879"/>
      <inertia ixx="0.0015947" ixy="-0.00013183" ixz="1.7315E-05" iyy="0.00060158" iyz="-6.0731e-05" izz="0.0015861"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/wheels/suspension_rear_link.obj" scale="1 -1.0 1"/>
      </geometry>
      <material name="">
        <color rgba="0.79216 0.81961 0.93333 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/wheels/suspension_rear_link.obj" scale="1 -1.0 1"/>
      </geometry>
    </collision>
    <kinematic>0</kinematic>
    <gravity>1</gravity>
  </link>
  <joint name="suspension_rear_left_joint" type="fixed">
    <origin rpy="0 0 1.57079632679" xyz="-0.244 0.1725 0"/>
    <parent link="base_link"/>
    <child link="suspension_rear_left_link"/>
    <axis xyz="0 0 1"/>
    <calibration rising="0.0"/>
    <dynamics damping="100"/>
    <limit effort="100" lower="-0.01" upper="0.01" velocity="1.0"/>
  </joint>
  <link name="wheel_rear_left_link">
    <inertial>
      <origin rpy="0 0 0" xyz="-5.4922e-09 6.9775e-05 -0.011144"/>
      <mass value="0.70767"/>
      <inertia ixx="0.00035492" ixy="-7.5685e-13" ixz="3.8795E-11" iyy="0.00035529" iyz="2.0284e-07" izz="0.00040131"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/wheels/wheel_link.obj" scale="1 1 -1.0"/>
      </geometry>
      <material name="DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="0.05" radius="0.0762"/>
      </geometry>
    </collision>
  </link>
  <joint name="wheel_rear_left_joint" type="fixed">
    <origin rpy="-1.57079632679 0 -1.57079632679" xyz="0.050675 0 0"/>
    <parent link="suspension_rear_left_link"/>
    <child link="wheel_rear_left_link"/>
    <axis xyz="0 0 1"/>
    <limit effort="6.0" velocity="13.1233595801"/>
  </joint>
  <transmission name="wheel_rear_left_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="wheel_rear_left_motor">
      <mechanicalReduction>1.0</mechanicalReduction>
    </actuator>
    <joint name="wheel_rear_left_joint">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
  </transmission>
  <!-- Contact model for the wheel surface -->
  <!-- Bottom fixed part of column -->
  <link name="torso_fixed_link">
    <inertial>
      <origin rpy="0.00000 0.00000 0.00000" xyz="0.02435 -0.00464 0.15900"/>
      <mass value="14.01360"/>
      <inertia ixx="0.38341100000" ixy="0.00036400000" ixz="-0.02438900000" iyy="0.40577300000" iyz="-0.00078200000" izz="0.07699800000"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/torso/torso_fix.obj" scale="1 1 1"/>
      </geometry>
      <material name="LightGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.085 0.0 0.02"/>
      <geometry>
        <box size="0.4 0.25 0.04"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.05"/>
      <geometry>
        <box size="0.25 0.25 0.1"/>
      </geometry>
    </collision>
  </link>
  <joint name="torso_fixed_joint" type="fixed">
    <origin rpy="0 0 0" xyz="-0.062 0.0 0.193"/>
    <parent link="base_link"/>
    <child link="torso_fixed_link"/>
    <axis xyz="0 0 0"/>
  </joint>
  <!-- Middle extending  -->
  <link name="torso_fixed_column_link">
    <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.275"/>
      <geometry>
        <box size="0.18 0.2 0.55"/>
      </geometry>
      <material name="DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.275"/>
      <geometry>
        <box size="0.23 0.23 0.55"/>
      </geometry>
      <material name="DarkGrey"/>
    </collision>
  </link>
  <joint name="torso_fixed_column_joint" type="fixed">
    <origin rpy="0 0 0" xyz="-0.062 0 0.195"/>
    <parent link="base_link"/>
    <child link="torso_fixed_column_link"/>
    <axis xyz="0 0 0"/>
  </joint>
  <link name="torso_lift_link">
    <inertial>
      <origin rpy="0.00000 0.00000 0.00000" xyz="0.04361 -0.00116 -0.17334"/>
      <mass value="9.594213"/>
      <inertia ixx="0.326395" ixy="-0.000844" ixz="-0.018905" iyy="0.235332" iyz="0.009042" izz="0.176168"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/torso/torso_lift_dual_arm.obj" scale="1 1 1"/>
      </geometry>
      <material name="White"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/torso/torso_lift_collision_core.dae" scale="1 1 1"/>
      </geometry>
      <material name="LightGrey"/>
    </collision>
    <!-- left shoulder collision-->
    <collision>
      <origin rpy="0 0 0" xyz="-0.131 0.17 -0.02556"/>
      <geometry>
        <mesh filename="meshes/torso/torso_lift_collision_shoulder.dae" scale="1 1 1"/>
      </geometry>
      <material name="LightGrey"/>
    </collision>
    <!-- right shoulder collision-->
    <collision>
      <origin rpy="0 0 0" xyz="-0.131 -0.19 -0.02556"/>
      <geometry>
        <mesh filename="meshes/torso/torso_lift_collision_shoulder.dae" scale="1 1 1"/>
      </geometry>
      <material name="LightGrey"/>
    </collision>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/torso/torso_lift_collision_neck.dae" scale="1 1 1"/>
      </geometry>
      <material name="LightGrey"/>
    </collision>
  </link>
  <joint name="torso_lift_joint" type="prismatic">
    <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.597"/>
    <parent link="torso_fixed_link"/>
    <child link="torso_lift_link"/>
    <axis xyz="0 0 1"/>
    <limit effort="2000" lower="0" upper="0.35" velocity="0.07"/>
    <calibration rising="0.0"/>
    <dynamics damping="1000"/>
    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="0.0" soft_upper_limit="0.35"/>
  </joint>
  <transmission name="torso_lift_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="torso_lift_motor">
      <mechanicalReduction>1.0</mechanicalReduction>
    </actuator>
    <joint name="torso_lift_joint">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
  </transmission>
  <link name="head_1_link">
    <inertial>
      <origin rpy="0.00000 0.00000 0.00000" xyz="-0.00508 0.00237 0.07229"/>
      <mass value="0.62220"/>
      <inertia ixx="0.00119200000" ixy="-0.00000700000" ixz="0.00009600000" iyy="0.00140200000" iyz="0.00000400000" izz="0.00088900000"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/head/head_1.obj" scale="1 1 1"/>
      </geometry>
      <material name="White"/>
    </visual>
    <collision>
      <origin rpy="0 0 0.0" xyz="0.0 0 0"/>
      <geometry>
        <mesh filename="meshes/head/head_1_collision.dae" scale="1 1 1"/>
      </geometry>
      <material name="White"/>
    </collision>
  </link>
  <joint name="head_1_joint" type="revolute">
    <parent link="torso_lift_link"/>
    <child link="head_1_link"/>
    <origin rpy="0 0 0" xyz="0.182 0.0 0.0"/>
    <axis xyz="0 0 1"/>
    <limit effort="5.197" lower="-1.308996939" upper="1.308996939" velocity="3.0"/>
    <dynamics damping="0.5" friction="1.0"/>
    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-1.238996939" soft_upper_limit="1.238996939"/>
  </joint>
  <transmission name="head_1_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="head_1_motor">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
    <joint name="head_1_joint">
      <offset>0.0</offset>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
  </transmission>
  <link name="head_2_link">
    <inertial>
      <origin rpy="0.00000 0.00000 0.00000" xyz="0.04700 0.05542 0.00022"/>
      <mass value="0.66123"/>
      <inertia ixx="0.00462000000" ixy="0.00068900000" ixz="0.00000600000" iyy="0.00486100000" iyz="0.00000600000" izz="0.00313200000"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/head/head_2.obj" scale="1 1 1"/>
      </geometry>
      <material name="White"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/head/head_2_collision.dae" scale="1 1 1"/>
      </geometry>
    </collision>
  </link>
  <joint name="head_2_joint" type="revolute">
    <parent link="head_1_link"/>
    <child link="head_2_link"/>
    <origin rpy="1.57079632679 0 0" xyz="0.005 0.0 0.098"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.77" lower="-1.0471975512" upper="0.785398163397" velocity="3.0"/>
    <dynamics damping="0.5" friction="1.0"/>
    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-0.977197551197" soft_upper_limit="0.715398163397"/>
  </joint>
  <transmission name="head_2_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="head_2_motor">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
    <joint name="head_2_joint">
      <offset>0.0</offset>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
  </transmission>
  <!-- frames in the center of the camera -->
  <joint name="xtion_joint" type="fixed">
    <origin rpy="-1.57079632679 0.0 0.0" xyz="0.0908 0.08 0.0"/>
    <parent link="head_2_link"/>
    <child link="xtion_link"/>
  </joint>
  <link name="xtion_link">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.00936000000 -0.00003000000 -0.00273000000"/>
      <mass value="0.21970000000"/>
      <inertia ixx="0.00000429247" ixy="0.00000000000" ixz="0.00000002565" iyy="0.00000008027" iyz="0.00000000000" izz="0.00000427339"/>
    </inertial>
    <!--
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <mesh filename="meshes/sensors/xtion_pro_live/xtion_pro_live.dae" />
        </geometry>
        <material name="Grey">
          <color rgba="0.5 0.5 0.5 1"/>
        </material>
      </visual>
      <collision>
        <origin xyz="-0.01 0.0025 0" rpy="0 0 0"/>
        <geometry>
          <box size="0.04 0.185 0.03"/>
        </geometry>
      </collision>
      -->
  </link>
  <joint name="xtion_optical_joint" type="fixed">
    <origin rpy="-1.57079632679 0.0 -1.57079632679" xyz="0 0 0"/>
    <parent link="xtion_link"/>
    <child link="xtion_optical_frame"/>
  </joint>
  <link name="xtion_optical_frame">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="0.001"/>
      <inertia ixx="0.00001" ixy="0" ixz="0" iyy="0.00001" iyz="0" izz="0.00001"/>
    </inertial>
  </link>
  <!-- frame of camera origin provided by Orbbec, need this intermediate frame for backwards compatibility with xtion_pro_live
         to reuse same camera origin -->
  <joint name="xtion_orbbec_aux_joint" type="fixed">
    <!-- This is the real origin as in the robot solidworks, it must match: 0.1015 0.095 0.008 -->
    <!-- Assuming we have 0.0908 0.08 0.0 from default camera_position, and rotated -90 deg on x.-->
    <!-- Pending confirmation from Orbbec about the position of the frames, going with old defaults-->
    <!-- <origin xyz="0.0107 0.008 0.015" rpy="0 0 0" />-->
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <parent link="xtion_link"/>
    <child link="xtion_orbbec_aux_joint_frame"/>
  </joint>
  <link name="xtion_orbbec_aux_joint_frame">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="0.001"/>
      <inertia ixx="0.00001" ixy="0" ixz="0" iyy="0.00001" iyz="0" izz="0.00001"/>
    </inertial>
  </link>
  <!-- frames of the depth sensor -->
  <joint name="xtion_depth_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0.0 0.025 0.0"/>
    <parent link="xtion_rgb_frame"/>
    <child link="xtion_depth_frame"/>
  </joint>
  <link name="xtion_depth_frame">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="0.001"/>
      <inertia ixx="0.00001" ixy="0" ixz="0" iyy="0.00001" iyz="0" izz="0.00001"/>
    </inertial>
  </link>
  <joint name="xtion_depth_optical_joint" type="fixed">
    <origin rpy="-1.57079632679 0.0 -1.57079632679" xyz="0 0 0"/>
    <parent link="xtion_depth_frame"/>
    <child link="xtion_depth_optical_frame"/>
  </joint>
  <link name="xtion_depth_optical_frame">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="0.001"/>
      <inertia ixx="0.00001" ixy="0" ixz="0" iyy="0.00001" iyz="0" izz="0.00001"/>
    </inertial>
  </link>
  <!-- frames of the rgb sensor -->
  <joint name="xtion_rgb_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0.0 0.022 0.0"/>
    <parent link="xtion_orbbec_aux_joint_frame"/>
    <child link="xtion_rgb_frame"/>
  </joint>
  <link name="xtion_rgb_frame">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="0.001"/>
      <inertia ixx="0.00001" ixy="0" ixz="0" iyy="0.00001" iyz="0" izz="0.00001"/>
    </inertial>
  </link>
  <joint name="xtion_rgb_optical_joint" type="fixed">
    <origin rpy="-1.57079632679 0.0 -1.57079632679" xyz="0 0 0"/>
    <parent link="xtion_rgb_frame"/>
    <child link="xtion_rgb_optical_frame"/>
  </joint>
  <link name="xtion_rgb_optical_frame">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="0.001"/>
      <inertia ixx="0.00001" ixy="0" ixz="0" iyy="0.00001" iyz="0" izz="0.00001"/>
    </inertial>
  </link>
  <!--************************-->
  <!--        SHOULDER        -->
  <!--************************-->
  <link name="arm_left_1_link">
    <inertial>
      <!--<origin xyz="0.061191 ${reflect * -0.022397} -0.012835" rpy="0.00000 0.00000 0.00000"/>-->
      <origin rpy="0.00000 0.00000 0.00000" xyz="0.061191 -0.022397 0.012835"/>
      <mass value="1.563428"/>
      <inertia ixx="0.002471" ixy="0.001809" ixz="-0.001202" iyy="0.006132" iyz="-0.000494" izz="0.006704"/>
    </inertial>
    <!-- M90 module -->
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/arm/arm_1.obj" scale="1 -1 -1"/>
      </geometry>
      <material name="White"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/arm/arm_1_collision.dae" scale="1 -1 -1"/>
      </geometry>
    </collision>
  </link>
  <joint name="arm_left_1_joint" type="revolute">
    <parent link="torso_lift_link"/>
    <child link="arm_left_1_link"/>
    <origin rpy="3.14159265359 0 1.57079632679" xyz="0.02556 0.19 -0.171"/>
    <axis xyz="0 0 1"/>
    <limit effort="43.0" lower="-1.1780972451" upper="1.57079632679" velocity="1.95"/>
    <dynamics damping="1.0" friction="1.0"/>
    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-1.1080972451" soft_upper_limit="1.50079632679"/>
  </joint>
  <link name="arm_left_2_link">
    <inertial>
      <origin rpy="0.00000 0.00000 0.00000" xyz="0.030432 0.000229 0.005942"/>
      <mass value="1.800397"/>
      <inertia ixx="0.001809" ixy="-1.5e-05" ixz="-0.000254" iyy="0.004333" iyz="-1.4e-05" izz="0.004341"/>
    </inertial>
    <visual>
      <!-- <origin xyz="0 0 0" rpy="${180 * deg_to_rad} ${0 * deg_to_rad} ${-90 * deg_to_rad}" /> -->
      <origin rpy="0.0 0.0 0.0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/arm/arm_2.obj" scale="1 1 -1"/>
      </geometry>
      <material name="DarkGrey"/>
    </visual>
    <collision>
      <!-- <origin xyz="0 0 0" rpy="${180 * deg_to_rad} ${0 * deg_to_rad} ${-90 * deg_to_rad}" /> -->
      <origin rpy="0.0 0.0 0.0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/arm/arm_2_collision.dae" scale="1 1 -1"/>
      </geometry>
    </collision>
  </link>
  <joint name="arm_left_2_joint" type="revolute">
    <parent link="arm_left_1_link"/>
    <child link="arm_left_2_link"/>
    <origin rpy="1.57079632679 0.0 0.0" xyz="0.125 -0.0195 0.031"/>
    <axis xyz="0 0 1"/>
    <limit effort="43.0" lower="-1.1780972451" upper="1.57079632679" velocity="1.95"/>
    <dynamics damping="1.0" friction="1.0"/>
    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-1.1080972451" soft_upper_limit="1.50079632679"/>
  </joint>
  <link name="arm_left_3_link">
    <inertial>
      <origin rpy="0.00000 0.00000 0.00000" xyz="0.007418 -0.004361 0.134194"/>
      <mass value="1.8"/>
      <inertia ixx="0.019541" ixy="-0.000105" ixz="-0.001717" iyy="0.019936" iyz="-0.000852" izz="0.002864"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/arm/arm_3.obj" scale="1 1 -1"/>
      </geometry>
      <material name="DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/arm/arm_3_collision.dae" scale="1 1 -1"/>
      </geometry>
    </collision>
  </link>
  <joint name="arm_left_3_joint" type="revolute">
    <parent link="arm_left_2_link"/>
    <child link="arm_left_3_link"/>
    <origin rpy="-1.57079632679 3.14159265359 -1.57079632679" xyz="0.0895 0.0 -0.0015"/>
    <axis xyz="0 0 1"/>
    <limit effort="26" lower="-0.785398163397" upper="3.92699081699" velocity="2.35"/>
    <dynamics damping="1.0" friction="1.0"/>
    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-0.715398163397" soft_upper_limit="3.85699081699"/>
  </joint>
  <!--************************-->
  <!--        ELBOW           -->
  <!--************************-->
  <link name="arm_left_4_link">
    <inertial>
      <origin rpy="0.00000 0.00000 0.00000" xyz="0.095658 -0.014666 -0.018133"/>
      <mass value="1.432697"/>
      <inertia ixx="0.002040" ixy="-0.000725" ixz="0.00115" iyy="0.008424" iyz="-0.000204" izz="0.008686"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/arm/arm_4.obj" scale="-1 -1 -1"/>
      </geometry>
      <material name="DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/arm/arm_4_collision.dae" scale="-1 -1 -1"/>
      </geometry>
    </collision>
  </link>
  <joint name="arm_left_4_joint" type="revolute">
    <parent link="arm_left_3_link"/>
    <child link="arm_left_4_link"/>
    <origin rpy="1.57079632679 -1.57079632679 0.0" xyz="-0.02 -0.027 0.222"/>
    <axis xyz="0 0 1"/>
    <limit effort="26" lower="-0.392699081699" upper="2.35619449019" velocity="2.35"/>
    <dynamics damping="1.0" friction="1.0"/>
    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-0.322699081699" soft_upper_limit="2.28619449019"/>
  </joint>
  <!--************************-->
  <!--        WRIST           -->
  <!--************************-->
  <!--************************-->
  <!--        WRIST           -->
  <!--************************-->
  <link name="arm_left_5_link">
    <inertial>
      <origin rpy="0.00000 0.00000 0.00000" xyz="0.001078 -0.000151 0.077173"/>
      <mass value="1.655819"/>
      <inertia ixx="0.00613800000" ixy="-0.00000400000" ixz="0.00015600000" iyy="0.00564000000" iyz="0.00002200000" izz="0.00179700000"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/arm/arm_5-wrist-2017.obj" scale="-1 -1 -1"/>
      </geometry>
      <material name="LightGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/arm/arm_5_collision-wrist-2017.dae" scale="-1 -1 -1"/>
      </geometry>
    </collision>
  </link>
  <joint name="arm_left_5_joint" type="revolute">
    <parent link="arm_left_4_link"/>
    <child link="arm_left_5_link"/>
    <origin rpy="1.57079632679 -1.57079632679 -1.57079632679" xyz="0.162 -0.02 -0.027"/>
    <axis xyz="0 0 1"/>
    <limit effort="3" lower="-2.09439510239" upper="2.09439510239" velocity="1.95"/>
    <dynamics damping="1.0" friction="1.0"/>
    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-2.07439510239" soft_upper_limit="2.07439510239"/>
  </joint>
  <link name="arm_left_6_link">
    <inertial>
      <origin rpy="0.00000 0.00000 0.00000" xyz="-0.000153 -0.003122 0.000183"/>
      <mass value="0.370063"/>
      <inertia ixx="0.00021500000" ixy="0.00000000000" ixz="0.00000000000" iyy="0.00017000000" iyz="0.00000200000" izz="0.00017400000"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/arm/arm_6-wrist-2017.obj" scale="-1 1 1"/>
      </geometry>
      <material name="LightGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/arm/arm_6_collision-wrist-2017.dae" scale="-1 1 1"/>
      </geometry>
    </collision>
  </link>
  <joint name="arm_left_6_joint" type="revolute">
    <parent link="arm_left_5_link"/>
    <child link="arm_left_6_link"/>
    <origin rpy="0.0 -1.57079632679 -1.57079632679" xyz="0 0 -0.15"/>
    <axis xyz="0 0 1"/>
    <limit effort="6.6" lower="-1.41371669412" upper="1.41371669412" velocity="1.76"/>
    <dynamics damping="1.0" friction="1.0"/>
    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-1.39371669412" soft_upper_limit="1.39371669412"/>
  </joint>
  <link name="arm_left_7_link">
    <inertial>
      <origin rpy="0.00000 0.00000 0.00000" xyz="0.000010 -0.000003 -0.007774"/>
      <mass value="0.234252"/>
      <inertia ixx="0.00014200000" ixy="0.00000000000" ixz="0.00000000000" iyy="0.00011300000" iyz="0.00000000000" izz="0.00005200000"/>
    </inertial>
  </link>
  <joint name="arm_left_7_joint" type="revolute">
    <parent link="arm_left_6_link"/>
    <child link="arm_left_7_link"/>
    <origin rpy="1.57079632679 0.0 1.57079632679" xyz="0 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="6.6" lower="-2.09439510239" upper="2.09439510239" velocity="1.76"/>
    <dynamics damping="1.0" friction="1.0"/>
    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-2.07439510239" soft_upper_limit="2.07439510239"/>
  </joint>
  <transmission name="arm_left_5_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="arm_left_5_motor">
      <mechanicalReduction>1.0</mechanicalReduction>
    </actuator>
    <joint name="arm_left_5_joint">
      <offset>0.0</offset>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
  </transmission>
  <transmission name="arm_left_wrist_trans">
    <type>transmission_interface/DifferentialTransmission</type>
    <actuator name="arm_left_6_motor">
      <role>actuator1</role>
      <mechanicalReduction>-1.0</mechanicalReduction>
    </actuator>
    <actuator name="arm_left_7_motor">
      <role>actuator2</role>
      <mechanicalReduction>1.0</mechanicalReduction>
    </actuator>
    <joint name="arm_left_6_joint">
      <role>joint1</role>
      <offset>0.0</offset>
      <mechanicalReduction>1.0</mechanicalReduction>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <ignoreTransmissionAbsoluteEncoder>1</ignoreTransmissionAbsoluteEncoder>
    </joint>
    <joint name="arm_left_7_joint">
      <role>joint2</role>
      <offset>0.0</offset>
      <mechanicalReduction>1</mechanicalReduction>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <ignoreTransmissionAbsoluteEncoder>1</ignoreTransmissionAbsoluteEncoder>
    </joint>
  </transmission>
  <!--***********************-->
  <!--        TOOL           -->
  <!--***********************-->
  <link name="arm_left_tool_link">
    <inertial>
      <mass value="0.1"/>
      <inertia ixx="8.33E-07" ixy="0.0" ixz="0.0" iyy="8.33E-07" iyz="0.0" izz="1.25E-06"/>
    </inertial>
    <visual>
      <origin rpy="0 1.57079632679 0" xyz="0.001 0 0"/>
      <geometry>
        <cylinder length="0.005" radius="0.005"/>
      </geometry>
      <material name="LightGrey"/>
    </visual>
    <collision>
      <origin rpy="0 1.57079632679 0" xyz="0.001 0 0"/>
      <geometry>
        <cylinder length="0.005" radius="0.005"/>
      </geometry>
    </collision>
  </link>
  <joint name="arm_left_tool_joint" type="fixed">
    <parent link="arm_left_7_link"/>
    <child link="arm_left_tool_link"/>
    <origin rpy="1.57079632679 1.57079632679 3.14159265359" xyz="0 0 -0.046"/>
  </joint>
  <transmission name="arm_left_1_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="arm_left_1_motor">
      <mechanicalReduction>1.0</mechanicalReduction>
    </actuator>
    <joint name="arm_left_1_joint">
      <offset>0.0</offset>
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
  </transmission>
  <transmission name="arm_left_2_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="arm_left_2_motor">
      <mechanicalReduction>1.0</mechanicalReduction>
    </actuator>
    <joint name="arm_left_2_joint">
      <offset>0.0</offset>
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
  </transmission>
  <transmission name="arm_left_3_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="arm_left_3_motor">
      <mechanicalReduction>1.0</mechanicalReduction>
    </actuator>
    <joint name="arm_left_3_joint">
      <offset>0.0</offset>
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
  </transmission>
  <transmission name="arm_left_4_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="arm_left_4_motor">
      <mechanicalReduction>1.0</mechanicalReduction>
    </actuator>
    <joint name="arm_left_4_joint">
      <offset>0.0</offset>
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
  </transmission>
  <!--************************-->
  <!--        ft sensor       -->
  <!--************************-->
  <link name="wrist_left_ft_link">
    <inertial>
      <mass value="0.1"/>
      <inertia ixx="1.6E-05" ixy="-2.0E-06" ixz="0.0" iyy="1.6E-05" iyz="0.0" izz="2.7E-05"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="0.0157" radius="0.0225"/>
      </geometry>
      <material name="LightGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="0.0157" radius="0.0225"/>
      </geometry>
    </collision>
  </link>
  <joint name="wrist_left_ft_joint" type="fixed">
    <parent link="arm_left_tool_link"/>
    <child link="wrist_left_ft_link"/>
    <origin rpy="1.57079632679 0 1.57079632679" xyz="0.00785 0 0"/>
  </joint>
  <!--***********************-->
  <!--       FT TOOL         -->
  <!--***********************-->
  <link name="wrist_left_ft_tool_link">
    <inertial>
      <mass value="0.1"/>
      <inertia ixx="1.64E-05" ixy="0.0" ixz="0.0" iyy="1.64E-05" iyz="0.0" izz="3.125E-05"/>
    </inertial>
    <visual>
      <origin rpy="0 1.57079632679 0" xyz="0.0 0 0"/>
      <geometry>
        <cylinder length="0.00975" radius="0.025"/>
      </geometry>
      <material name="FlatBlack"/>
    </visual>
    <collision>
      <origin rpy="0 1.57079632679 0" xyz="0.0 0 0"/>
      <geometry>
        <cylinder length="0.00975" radius="0.025"/>
      </geometry>
    </collision>
  </link>
  <joint name="wrist_left_tool_joint" type="fixed">
    <parent link="wrist_left_ft_link"/>
    <child link="wrist_left_ft_tool_link"/>
    <origin rpy="-1.57079632679 -1.57079632679 0" xyz="0 0 0.012725"/>
  </joint>
  <link name="gripper_left_tool_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="0.0001"/>
      <inertia ixx="0.00001" ixy="0.0" ixz="0.0" iyy="0.00001" iyz="0.0" izz="0.00001"/>
    </inertial>
  </link>
  <joint name="gripper_left_tool_joint" type="fixed">
    <origin rpy="1.57079632679 1.57079632679 -1.57079632679" xyz="0.01 0 0"/>
    <parent link="wrist_left_ft_tool_link"/>
    <child link="gripper_left_tool_link"/>
    <axis xyz="0 0 0"/>
  </joint>
  <link name="gripper_left_link">
    <inertial>
      <origin rpy="0.0 0.0 0.0" xyz="-0.000294 0.000291 -0.033086"/>
      <mass value="0.602660"/>
      <inertia ixx="0.000693" ixy="0.000045" ixz="-0.000000" iyy="0.001024" iyz="0.000000" izz="0.000672"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/grippers/gripper_link.obj"/>
      </geometry>
      <material name="White"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/grippers/gripper_link_collision.stl"/>
      </geometry>
    </collision>
  </link>
  <joint name="gripper_left_joint" type="fixed">
    <origin rpy="1.57079632679 1.57079632679 -1.57079632679" xyz="0.01 0 0"/>
    <parent link="wrist_left_ft_tool_link"/>
    <child link="gripper_left_link"/>
    <axis xyz="0 0 0"/>
  </joint>
  <link name="gripper_left_right_finger_link">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.004470 0.010766 -0.121013"/>
      <mass value="0.109980"/>
      <inertia ixx="0.000232" ixy="-0.000005" ixz="-0.000032" iyy="0.000224" iyz="0.000036" izz="0.000029"/>
    </inertial>
    <visual>
      <origin rpy="0 0 3.1415" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/grippers/gripper_finger_link.obj" scale="-1 -1 1"/>
      </geometry>
      <material name="Black"/>
    </visual>
    <collision>
      <origin rpy="0 0 3.1415" xyz="0.005 0 -0.165"/>
      <geometry>
        <box size="0.01 0.05 0.11"/>
      </geometry>
    </collision>
  </link>
  <joint name="gripper_left_right_finger_joint" type="prismatic">
    <origin rpy="0 0 0" xyz="0. 0 0"/>
    <parent link="gripper_left_link"/>
    <child link="gripper_left_right_finger_link"/>
    <axis xyz="1 0 0"/>
    <limit effort="16" lower="0.0" upper="0.045" velocity="0.05"/>
    <calibration rising="0.0"/>
    <dynamics damping="80"/>
    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="0.001" soft_upper_limit="0.044"/>
  </joint>
  <link name="gripper_left_left_finger_link">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.004470 0.010766 -0.121013"/>
      <mass value="0.109980"/>
      <inertia ixx="0.000232" ixy="-0.000005" ixz="-0.000032" iyy="0.000224" iyz="0.000036" izz="0.000029"/>
    </inertial>
    <visual>
      <origin rpy="0 0 3.1415" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/grippers/gripper_finger_link.obj"/>
      </geometry>
      <material name="Black"/>
    </visual>
    <collision>
      <origin rpy="0 0 3.1415" xyz="-0.005 0 -0.165"/>
      <geometry>
        <box size="0.01 0.05 0.11"/>
      </geometry>
    </collision>
  </link>
  <joint name="gripper_left_left_finger_joint" type="prismatic">
    <origin rpy="0 0 0" xyz="0. 0 0"/>
    <parent link="gripper_left_link"/>
    <child link="gripper_left_left_finger_link"/>
    <axis xyz="-1 0 0"/>
    <limit effort="16" lower="0" upper="0.045" velocity="0.05"/>
    <calibration rising="0.0"/>
    <dynamics damping="80"/>
    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="0.001" soft_upper_limit="0.044"/>
  </joint>
  <!-- Grasping frame -->
  <joint name="gripper_left_grasping_frame_joint" type="fixed">
    <parent link="gripper_left_link"/>
    <child link="gripper_left_grasping_frame"/>
    <origin rpy="-1.5708 1.5708 0" xyz="0 0 -0.12"/>
  </joint>
  <link name="gripper_left_grasping_frame">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="0.0001"/>
      <inertia ixx="0.00001" ixy="0.0" ixz="0.0" iyy="0.00001" iyz="0.0" izz="0.00001"/>
    </inertial>
  </link>
  <transmission name="gripper_left_left_finger_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="gripper_left_left_finger_motor">
      <mechanicalReduction>1.0</mechanicalReduction>
    </actuator>
    <joint name="gripper_left_left_finger_joint">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
  </transmission>
  <transmission name="gripper_left_right_finger_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="gripper_left_right_finger_motor">
      <mechanicalReduction>1.0</mechanicalReduction>
    </actuator>
    <joint name="gripper_left_right_finger_joint">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
  </transmission>
  <!--************************-->
  <!--        SHOULDER        -->
  <!--************************-->
  <link name="arm_right_1_link">
    <inertial>
      <!--<origin xyz="0.061191 ${reflect * -0.022397} -0.012835" rpy="0.00000 0.00000 0.00000"/>-->
      <origin rpy="0.00000 0.00000 0.00000" xyz="0.061191 -0.022397 -0.012835"/>
      <mass value="1.563428"/>
      <inertia ixx="0.002471" ixy="-0.001809" ixz="-0.001202" iyy="0.006132" iyz="0.000494" izz="0.006704"/>
    </inertial>
    <!-- M90 module -->
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/arm/arm_1.obj" scale="1 -1 1"/>
      </geometry>
      <material name="White"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/arm/arm_1_collision.dae" scale="1 -1 1"/>
      </geometry>
    </collision>
  </link>
  <joint name="arm_right_1_joint" type="revolute">
    <parent link="torso_lift_link"/>
    <child link="arm_right_1_link"/>
    <origin rpy="0 0 -1.57079632679" xyz="0.02556 -0.19 -0.171"/>
    <axis xyz="0 0 1"/>
    <limit effort="43.0" lower="-1.1780972451" upper="1.57079632679" velocity="1.95"/>
    <dynamics damping="1.0" friction="1.0"/>
    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-1.1080972451" soft_upper_limit="1.50079632679"/>
  </joint>
  <link name="arm_right_2_link">
    <inertial>
      <origin rpy="0.00000 0.00000 0.00000" xyz="0.030432 0.000229 -0.005942"/>
      <mass value="1.800397"/>
      <inertia ixx="0.001809" ixy="1.5e-05" ixz="0.000254" iyy="0.004333" iyz="1.4e-05" izz="0.004341"/>
    </inertial>
    <visual>
      <!-- <origin xyz="0 0 0" rpy="${180 * deg_to_rad} ${0 * deg_to_rad} ${-90 * deg_to_rad}" /> -->
      <origin rpy="0.0 0.0 0.0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/arm/arm_2.obj" scale="1 1 1"/>
      </geometry>
      <material name="DarkGrey"/>
    </visual>
    <collision>
      <!-- <origin xyz="0 0 0" rpy="${180 * deg_to_rad} ${0 * deg_to_rad} ${-90 * deg_to_rad}" /> -->
      <origin rpy="0.0 0.0 0.0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/arm/arm_2_collision.dae" scale="1 1 1"/>
      </geometry>
    </collision>
  </link>
  <joint name="arm_right_2_joint" type="revolute">
    <parent link="arm_right_1_link"/>
    <child link="arm_right_2_link"/>
    <origin rpy="-1.57079632679 0.0 0.0" xyz="0.125 -0.0195 -0.031"/>
    <axis xyz="0 0 1"/>
    <limit effort="43.0" lower="-1.1780972451" upper="1.57079632679" velocity="1.95"/>
    <dynamics damping="1.0" friction="1.0"/>
    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-1.1080972451" soft_upper_limit="1.50079632679"/>
  </joint>
  <link name="arm_right_3_link">
    <inertial>
      <origin rpy="0.00000 0.00000 0.00000" xyz="0.007418 -0.004361 -0.134194"/>
      <mass value="1.8"/>
      <inertia ixx="0.019541" ixy="-0.000105" ixz="-0.001717" iyy="0.019936" iyz="0.000852" izz="0.002864"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/arm/arm_3.obj" scale="1 1 1"/>
      </geometry>
      <material name="DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/arm/arm_3_collision.dae" scale="1 1 1"/>
      </geometry>
    </collision>
  </link>
  <joint name="arm_right_3_joint" type="revolute">
    <parent link="arm_right_2_link"/>
    <child link="arm_right_3_link"/>
    <origin rpy="-1.57079632679 0.0 1.57079632679" xyz="0.0895 0.0 -0.0015"/>
    <axis xyz="0 0 1"/>
    <limit effort="26" lower="-0.785398163397" upper="3.92699081699" velocity="2.35"/>
    <dynamics damping="1.0" friction="1.0"/>
    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-0.715398163397" soft_upper_limit="3.85699081699"/>
  </joint>
  <!--************************-->
  <!--        ELBOW           -->
  <!--************************-->
  <link name="arm_right_4_link">
    <inertial>
      <origin rpy="0.00000 0.00000 0.00000" xyz="-0.095658 0.014666 0.018133"/>
      <mass value="1.432697"/>
      <inertia ixx="0.002040" ixy="0.000725" ixz="-0.00115" iyy="0.008424" iyz="-0.000204" izz="0.008686"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/arm/arm_4.obj" scale="1 1 1"/>
      </geometry>
      <material name="DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/arm/arm_4_collision.dae" scale="1 1 1"/>
      </geometry>
    </collision>
  </link>
  <joint name="arm_right_4_joint" type="revolute">
    <parent link="arm_right_3_link"/>
    <child link="arm_right_4_link"/>
    <origin rpy="-1.57079632679 -1.57079632679 0.0" xyz="-0.02 -0.027 -0.222"/>
    <axis xyz="0 0 1"/>
    <limit effort="26" lower="-0.392699081699" upper="2.35619449019" velocity="2.35"/>
    <dynamics damping="1.0" friction="1.0"/>
    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-0.322699081699" soft_upper_limit="2.28619449019"/>
  </joint>
  <!--************************-->
  <!--        WRIST           -->
  <!--************************-->
  <!--************************-->
  <!--        WRIST           -->
  <!--************************-->
  <link name="arm_right_5_link">
    <inertial>
      <origin rpy="0.00000 0.00000 0.00000" xyz="0.001078 -0.000151 -0.077173"/>
      <mass value="1.655819"/>
      <inertia ixx="0.00613800000" ixy="-0.00000400000" ixz="0.00015600000" iyy="0.00564000000" iyz="0.00002200000" izz="0.00179700000"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/arm/arm_5-wrist-2017.obj" scale="1 1 1"/>
      </geometry>
      <material name="LightGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/arm/arm_5_collision-wrist-2017.dae" scale="1 1 1"/>
      </geometry>
    </collision>
  </link>
  <joint name="arm_right_5_joint" type="revolute">
    <parent link="arm_right_4_link"/>
    <child link="arm_right_5_link"/>
    <origin rpy="1.57079632679 -1.57079632679 -1.57079632679" xyz="-0.162 0.02 0.027"/>
    <axis xyz="0 0 1"/>
    <limit effort="3" lower="-2.09439510239" upper="2.09439510239" velocity="1.95"/>
    <dynamics damping="1.0" friction="1.0"/>
    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-2.07439510239" soft_upper_limit="2.07439510239"/>
  </joint>
  <link name="arm_right_6_link">
    <inertial>
      <origin rpy="0.00000 0.00000 0.00000" xyz="-0.000153 -0.003122 0.000183"/>
      <mass value="0.370063"/>
      <inertia ixx="0.00021500000" ixy="0.00000000000" ixz="0.00000000000" iyy="0.00017000000" iyz="0.00000200000" izz="0.00017400000"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/arm/arm_6-wrist-2017.obj" scale="1 1 1"/>
      </geometry>
      <material name="LightGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/arm/arm_6_collision-wrist-2017.dae" scale="1 1 1"/>
      </geometry>
    </collision>
  </link>
  <joint name="arm_right_6_joint" type="revolute">
    <parent link="arm_right_5_link"/>
    <child link="arm_right_6_link"/>
    <origin rpy="0.0 -1.57079632679 -1.57079632679" xyz="0 0 0.15"/>
    <axis xyz="0 0 1"/>
    <limit effort="6.6" lower="-1.41371669412" upper="1.41371669412" velocity="1.76"/>
    <dynamics damping="1.0" friction="1.0"/>
    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-1.39371669412" soft_upper_limit="1.39371669412"/>
  </joint>
  <link name="arm_right_7_link">
    <inertial>
      <origin rpy="0.00000 0.00000 0.00000" xyz="0.000010 -0.000003 -0.007774"/>
      <mass value="0.234252"/>
      <inertia ixx="0.00014200000" ixy="0.00000000000" ixz="0.00000000000" iyy="0.00011300000" iyz="0.00000000000" izz="0.00005200000"/>
    </inertial>
  </link>
  <joint name="arm_right_7_joint" type="revolute">
    <parent link="arm_right_6_link"/>
    <child link="arm_right_7_link"/>
    <origin rpy="1.57079632679 0.0 1.57079632679" xyz="0 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="6.6" lower="-2.09439510239" upper="2.09439510239" velocity="1.76"/>
    <dynamics damping="1.0" friction="1.0"/>
    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-2.07439510239" soft_upper_limit="2.07439510239"/>
  </joint>
  <transmission name="arm_right_5_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="arm_right_5_motor">
      <mechanicalReduction>1.0</mechanicalReduction>
    </actuator>
    <joint name="arm_right_5_joint">
      <offset>0.0</offset>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
  </transmission>
  <transmission name="arm_right_wrist_trans">
    <type>transmission_interface/DifferentialTransmission</type>
    <actuator name="arm_right_6_motor">
      <role>actuator1</role>
      <mechanicalReduction>-1.0</mechanicalReduction>
    </actuator>
    <actuator name="arm_right_7_motor">
      <role>actuator2</role>
      <mechanicalReduction>1.0</mechanicalReduction>
    </actuator>
    <joint name="arm_right_6_joint">
      <role>joint1</role>
      <offset>0.0</offset>
      <mechanicalReduction>1.0</mechanicalReduction>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <ignoreTransmissionAbsoluteEncoder>1</ignoreTransmissionAbsoluteEncoder>
    </joint>
    <joint name="arm_right_7_joint">
      <role>joint2</role>
      <offset>0.0</offset>
      <mechanicalReduction>-1</mechanicalReduction>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <ignoreTransmissionAbsoluteEncoder>1</ignoreTransmissionAbsoluteEncoder>
    </joint>
  </transmission>
  <!--***********************-->
  <!--        TOOL           -->
  <!--***********************-->
  <link name="arm_right_tool_link">
    <inertial>
      <mass value="0.1"/>
      <inertia ixx="8.33E-07" ixy="0.0" ixz="0.0" iyy="8.33E-07" iyz="0.0" izz="1.25E-06"/>
    </inertial>
    <visual>
      <origin rpy="0 1.57079632679 0" xyz="0.001 0 0"/>
      <geometry>
        <cylinder length="0.005" radius="0.005"/>
      </geometry>
      <material name="LightGrey"/>
    </visual>
    <collision>
      <origin rpy="0 1.57079632679 0" xyz="0.001 0 0"/>
      <geometry>
        <cylinder length="0.005" radius="0.005"/>
      </geometry>
    </collision>
  </link>
  <joint name="arm_right_tool_joint" type="fixed">
    <parent link="arm_right_7_link"/>
    <child link="arm_right_tool_link"/>
    <origin rpy="1.57079632679 -1.57079632679 3.14159265359" xyz="0 0 0.046"/>
  </joint>
  <transmission name="arm_right_1_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="arm_right_1_motor">
      <mechanicalReduction>1.0</mechanicalReduction>
    </actuator>
    <joint name="arm_right_1_joint">
      <offset>0.0</offset>
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
  </transmission>
  <transmission name="arm_right_2_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="arm_right_2_motor">
      <mechanicalReduction>1.0</mechanicalReduction>
    </actuator>
    <joint name="arm_right_2_joint">
      <offset>0.0</offset>
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
  </transmission>
  <transmission name="arm_right_3_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="arm_right_3_motor">
      <mechanicalReduction>1.0</mechanicalReduction>
    </actuator>
    <joint name="arm_right_3_joint">
      <offset>0.0</offset>
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
  </transmission>
  <transmission name="arm_right_4_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="arm_right_4_motor">
      <mechanicalReduction>1.0</mechanicalReduction>
    </actuator>
    <joint name="arm_right_4_joint">
      <offset>0.0</offset>
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
  </transmission>
  <!--************************-->
  <!--        ft sensor       -->
  <!--************************-->
  <link name="wrist_right_ft_link">
    <inertial>
      <mass value="0.1"/>
      <inertia ixx="1.6E-05" ixy="-2.0E-06" ixz="0.0" iyy="1.6E-05" iyz="0.0" izz="2.7E-05"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="0.0157" radius="0.0225"/>
      </geometry>
      <material name="LightGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="0.0157" radius="0.0225"/>
      </geometry>
    </collision>
  </link>
  <joint name="wrist_right_ft_joint" type="fixed">
    <parent link="arm_right_tool_link"/>
    <child link="wrist_right_ft_link"/>
    <origin rpy="1.57079632679 0 1.57079632679" xyz="0.00785 0 0"/>
  </joint>
  <!--***********************-->
  <!--       FT TOOL         -->
  <!--***********************-->
  <link name="wrist_right_ft_tool_link">
    <inertial>
      <mass value="0.1"/>
      <inertia ixx="1.64E-05" ixy="0.0" ixz="0.0" iyy="1.64E-05" iyz="0.0" izz="3.125E-05"/>
    </inertial>
    <visual>
      <origin rpy="0 1.57079632679 0" xyz="0.0 0 0"/>
      <geometry>
        <cylinder length="0.00975" radius="0.025"/>
      </geometry>
      <material name="FlatBlack"/>
    </visual>
    <collision>
      <origin rpy="0 1.57079632679 0" xyz="0.0 0 0"/>
      <geometry>
        <cylinder length="0.00975" radius="0.025"/>
      </geometry>
    </collision>
  </link>
  <joint name="wrist_right_tool_joint" type="fixed">
    <parent link="wrist_right_ft_link"/>
    <child link="wrist_right_ft_tool_link"/>
    <origin rpy="-1.57079632679 -1.57079632679 0" xyz="0 0 0.012725"/>
  </joint>
  <link name="gripper_right_tool_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="0.0001"/>
      <inertia ixx="0.00001" ixy="0.0" ixz="0.0" iyy="0.00001" iyz="0.0" izz="0.00001"/>
    </inertial>
  </link>
  <joint name="gripper_right_tool_joint" type="fixed">
    <origin rpy="1.57079632679 1.57079632679 -1.57079632679" xyz="0.01 0 0"/>
    <parent link="wrist_right_ft_tool_link"/>
    <child link="gripper_right_tool_link"/>
    <axis xyz="0 0 0"/>
  </joint>
  <link name="gripper_right_link">
    <inertial>
      <origin rpy="0.0 0.0 0.0" xyz="-0.000294 0.000291 -0.033086"/>
      <mass value="0.602660"/>
      <inertia ixx="0.000693" ixy="0.000045" ixz="-0.000000" iyy="0.001024" iyz="0.000000" izz="0.000672"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/grippers/gripper_link.obj"/>
      </geometry>
      <material name="White"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/grippers/gripper_link_collision.stl"/>
      </geometry>
    </collision>
  </link>
  <joint name="gripper_right_joint" type="fixed">
    <origin rpy="1.57079632679 1.57079632679 -1.57079632679" xyz="0.01 0 0"/>
    <parent link="wrist_right_ft_tool_link"/>
    <child link="gripper_right_link"/>
    <axis xyz="0 0 0"/>
  </joint>
  <link name="gripper_right_right_finger_link">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.004470 0.010766 -0.121013"/>
      <mass value="0.109980"/>
      <inertia ixx="0.000232" ixy="-0.000005" ixz="-0.000032" iyy="0.000224" iyz="0.000036" izz="0.000029"/>
    </inertial>
    <visual>
      <origin rpy="0 0 3.1415" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/grippers/gripper_finger_link.obj" scale="-1 -1 1"/>
      </geometry>
      <material name="Black"/>
    </visual>
    <collision>
      <origin rpy="0 0 3.1415" xyz="0.005 0 -0.165"/>
      <geometry>
        <box size="0.01 0.05 0.11"/>
      </geometry>
    </collision>
  </link>
  <joint name="gripper_right_right_finger_joint" type="prismatic">
    <origin rpy="0 0 0" xyz="0. 0 0"/>
    <parent link="gripper_right_link"/>
    <child link="gripper_right_right_finger_link"/>
    <axis xyz="1 0 0"/>
    <limit effort="16" lower="0.0" upper="0.045" velocity="0.05"/>
    <calibration rising="0.0"/>
    <dynamics damping="80"/>
    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="0.001" soft_upper_limit="0.044"/>
  </joint>
  <link name="gripper_right_left_finger_link">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.004470 0.010766 -0.121013"/>
      <mass value="0.109980"/>
      <inertia ixx="0.000232" ixy="-0.000005" ixz="-0.000032" iyy="0.000224" iyz="0.000036" izz="0.000029"/>
    </inertial>
    <visual>
      <origin rpy="0 0 3.1415" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/grippers/gripper_finger_link.obj"/>
      </geometry>
      <material name="Black"/>
    </visual>
    <collision>
      <origin rpy="0 0 3.1415" xyz="-0.005 0 -0.165"/>
      <geometry>
        <box size="0.01 0.05 0.11"/>
      </geometry>
    </collision>
  </link>
  <joint name="gripper_right_left_finger_joint" type="prismatic">
    <origin rpy="0 0 0" xyz="0. 0 0"/>
    <parent link="gripper_right_link"/>
    <child link="gripper_right_left_finger_link"/>
    <axis xyz="-1 0 0"/>
    <limit effort="16" lower="0" upper="0.045" velocity="0.05"/>
    <calibration rising="0.0"/>
    <dynamics damping="80"/>
    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="0.001" soft_upper_limit="0.044"/>
  </joint>
  <!-- Grasping frame -->
  <joint name="gripper_right_grasping_frame_joint" type="fixed">
    <parent link="gripper_right_link"/>
    <child link="gripper_right_grasping_frame"/>
    <origin rpy="-1.5708 1.5708 0" xyz="0 0 -0.12"/>
  </joint>
  <link name="gripper_right_grasping_frame">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="0.0001"/>
      <inertia ixx="0.00001" ixy="0.0" ixz="0.0" iyy="0.00001" iyz="0.0" izz="0.00001"/>
    </inertial>
  </link>
  <transmission name="gripper_right_left_finger_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="gripper_right_left_finger_motor">
      <mechanicalReduction>1.0</mechanicalReduction>
    </actuator>
    <joint name="gripper_right_left_finger_joint">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
  </transmission>
  <transmission name="gripper_right_right_finger_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="gripper_right_right_finger_motor">
      <mechanicalReduction>1.0</mechanicalReduction>
    </actuator>
    <joint name="gripper_right_right_finger_joint">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
  </transmission>
  <!-- RGBD Laser Link -->
  <joint name="rgbd_laser_joint" type="fixed">
    <parent link="base_footprint"/>
    <child link="rgbd_laser_link"/>
    <origin rpy="0 0 0" xyz="-0.9 0 0"/>
  </joint>
  <link name="rgbd_laser_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="0.0001"/>
      <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
    </inertial>
  </link>
  <!-- Eyes (added manually for iG) -->
  <link name="eyes">
      <inertial>
      <mass value="0.001"/>
      <origin xyz="0 0 0"/>
      <inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
    </inertial>
  </link>
  <joint name="eye_joint" type="fixed">
   <origin rpy="0 0 0" xyz="0 0 0"/>
    <parent link="xtion_depth_frame"/>
    <child link="eyes"/>
  </joint>
</robot>