From 9f10e91f3aa21177695077cbe240d21934f4847a Mon Sep 17 00:00:00 2001
From: Guilhem Saurel <guilhem.saurel@laas.fr>
Date: Fri, 21 May 2021 19:28:00 +0200
Subject: [PATCH] tiago_dual: add generated robot description

---
 .../tiago_description/robots/tiago_dual.urdf  | 5625 +++++++++++++++++
 1 file changed, 5625 insertions(+)
 create mode 100644 robots/tiago_description/robots/tiago_dual.urdf

diff --git a/robots/tiago_description/robots/tiago_dual.urdf b/robots/tiago_description/robots/tiago_dual.urdf
new file mode 100644
index 0000000..7995de6
--- /dev/null
+++ b/robots/tiago_description/robots/tiago_dual.urdf
@@ -0,0 +1,5625 @@
+<?xml version="1.0" ?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from src/tiago_dual_robot/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-->
+  <gazebo>
+    <plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control">
+      <ns/>
+      <robotSimType>pal_hardware_gazebo/PalHardwareGazebo</robotSimType>
+      <robotNamespace/>
+      <controlPeriod>0.001</controlPeriod>
+    </plugin>
+    <plugin filename="libgazebo_ros_p3d.so" name="gazebo_ros_p3d">
+      <bodyName>base_footprint</bodyName>
+      <topicName>ground_truth_odom</topicName>
+      <alwaysOn>true</alwaysOn>
+      <updateRate>100.0</updateRate>
+    </plugin>
+  </gazebo>
+  <gazebo>
+    <plugin filename="libgazebo_world_odometry.so" name="gazebo_ros_odometry">
+      <frameName>base_footprint</frameName>
+      <topicName>floating_base_pose_simulated</topicName>
+    </plugin>
+  </gazebo>
+  <gazebo reference="base_imu_link">
+    <!-- this is expected to be reparented to pelvis with appropriate offset
+         when imu_link is reduced by fixed joint reduction -->
+    <!-- todo: this is working with gazebo 1.4, need to write a unit test -->
+    <sensor name="imu_sensor" type="imu">
+      <always_on>1</always_on>
+      <update_rate>1000.0</update_rate>
+      <imu>
+        <noise>
+          <type>gaussian</type>
+          <!-- Noise parameters from Boston Dynamics
+               (http://gazebosim.org/wiki/Sensor_noise):
+                 rates (rad/s): mean=0, stddev=2e-4
+                 accels (m/s/s): mean=0, stddev=1.7e-2
+                 rate bias (rad/s): 5e-6 - 1e-5
+                 accel bias (m/s/s): 1e-1
+               Experimentally, simulation provide rates with noise of
+               about 1e-3 rad/s and accels with noise of about 1e-1 m/s/s.
+               So we don't expect to see the noise unless number of inner iterations
+               are increased.
+
+               We will add bias.  In this model, bias is sampled once for rates
+               and once for accels at startup; the sign (negative or positive)
+               of each bias is then switched with equal probability.  Thereafter,
+               the biases are fixed additive offsets.  We choose
+               bias means and stddevs to produce biases close to the provided
+               data. -->
+          <rate>
+            <mean>0.0</mean>
+            <stddev>2e-4</stddev>
+            <bias_mean>0.0000075</bias_mean>
+            <bias_stddev>0.0000008</bias_stddev>
+          </rate>
+          <accel>
+            <mean>0.0</mean>
+            <stddev>1.7e-2</stddev>
+            <bias_mean>0.1</bias_mean>
+            <bias_stddev>0.001</bias_stddev>
+          </accel>
+        </noise>
+      </imu>
+    </sensor>
+  </gazebo>
+  <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_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="package://pmb2_description/meshes/sensors/sick_tim551.stl"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+  </link>
+  <joint name="base_laser_joint" type="fixed">
+    <axis xyz="0 1 0"/>
+    <origin rpy="0 0 0" xyz="0.202 0.0 -0.004"/>
+    <parent link="base_link"/>
+    <child link="base_laser_link"/>
+  </joint>
+  <gazebo reference="base_laser_link">
+    <sensor name="base_laser" type="ray">
+      <pose>0 0 0 0 0 0</pose>
+      <update_rate>10</update_rate>
+      <visualize>true</visualize>
+      <ray>
+        <scan>
+          <horizontal>
+            <!-- 818 (270/0.33) steps in 270deg fov -->
+            <samples>666.5185185185185</samples>
+            <resolution>1</resolution>
+            <!-- not the sensor resolution; just 1 -->
+            <min_angle>-1.9198621771937625</min_angle>
+            <max_angle>1.9198621771937625</max_angle>
+          </horizontal>
+        </scan>
+        <range>
+          <min>0.05</min>
+          <max>25.0</max>
+          <resolution>0.001</resolution>
+        </range>
+      </ray>
+      <plugin filename="libgazebo_ros_laser.so" name="gazebo_ros_sick_laser">
+        <gaussianNoise>0.06</gaussianNoise>
+        <alwaysOn>true</alwaysOn>
+        <updateRate>10</updateRate>
+        <topicName>scan_raw</topicName>
+        <frameName>base_laser_link</frameName>
+      </plugin>
+    </sensor>
+    <material>Gazebo/DarkGrey</material>
+  </gazebo>
+  <link name="base_sonar_03_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <mass value="0.001"/>
+      <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://pmb2_description/meshes/sensors/srf05.stl"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+  </link>
+  <joint name="base_sonar_03_joint" type="fixed">
+    <origin rpy="0 0 2.356194490192345" xyz="-0.183 0.183 0.173"/>
+    <axis xyz="0 0 1"/>
+    <parent link="base_link"/>
+    <child link="base_sonar_03_link"/>
+  </joint>
+  <gazebo reference="base_sonar_03_link">
+    <sensor name="base_sonar_03" type="ray">
+      <pose>0 0 0 0 0 0</pose>
+      <update_rate>5</update_rate>
+      <visualize>false</visualize>
+      <ray>
+        <scan>
+          <horizontal>
+            <samples>5</samples>
+            <resolution>1</resolution>
+            <min_angle>-0.25</min_angle>
+            <max_angle>0.25</max_angle>
+          </horizontal>
+          <vertical>
+            <samples>5</samples>
+            <resolution>1</resolution>
+            <min_angle>-0.25</min_angle>
+            <max_angle>0.25</max_angle>
+          </vertical>
+        </scan>
+        <range>
+          <min>0.02</min>
+          <max>1.0</max>
+          <resolution>0.01</resolution>
+        </range>
+      </ray>
+      <plugin filename="libgazebo_ros_range.so" name="gazebo_ros_range">
+        <gaussianNoise>0.005</gaussianNoise>
+        <alwaysOn>true</alwaysOn>
+        <updateRate>5</updateRate>
+        <topicName>sonar_base</topicName>
+        <frameName>base_sonar_03_link</frameName>
+        <minRange>0.02</minRange>
+        <maxRange>1.0</maxRange>
+        <fov>0.5</fov>
+        <radiation>ultrasound</radiation>
+      </plugin>
+    </sensor>
+    <material>Gazebo/DarkGrey</material>
+  </gazebo>
+  <link name="base_sonar_02_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <mass value="0.001"/>
+      <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://pmb2_description/meshes/sensors/srf05.stl"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+  </link>
+  <joint name="base_sonar_02_joint" type="fixed">
+    <origin rpy="0 0 3.141592653589793" xyz="-0.259 0.0 0.173"/>
+    <axis xyz="0 0 1"/>
+    <parent link="base_link"/>
+    <child link="base_sonar_02_link"/>
+  </joint>
+  <gazebo reference="base_sonar_02_link">
+    <sensor name="base_sonar_02" type="ray">
+      <pose>0 0 0 0 0 0</pose>
+      <update_rate>5</update_rate>
+      <visualize>false</visualize>
+      <ray>
+        <scan>
+          <horizontal>
+            <samples>5</samples>
+            <resolution>1</resolution>
+            <min_angle>-0.25</min_angle>
+            <max_angle>0.25</max_angle>
+          </horizontal>
+          <vertical>
+            <samples>5</samples>
+            <resolution>1</resolution>
+            <min_angle>-0.25</min_angle>
+            <max_angle>0.25</max_angle>
+          </vertical>
+        </scan>
+        <range>
+          <min>0.02</min>
+          <max>1.0</max>
+          <resolution>0.01</resolution>
+        </range>
+      </ray>
+      <plugin filename="libgazebo_ros_range.so" name="gazebo_ros_range">
+        <gaussianNoise>0.005</gaussianNoise>
+        <alwaysOn>true</alwaysOn>
+        <updateRate>5</updateRate>
+        <topicName>sonar_base</topicName>
+        <frameName>base_sonar_02_link</frameName>
+        <minRange>0.02</minRange>
+        <maxRange>1.0</maxRange>
+        <fov>0.5</fov>
+        <radiation>ultrasound</radiation>
+      </plugin>
+    </sensor>
+    <material>Gazebo/DarkGrey</material>
+  </gazebo>
+  <link name="base_sonar_01_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <mass value="0.001"/>
+      <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://pmb2_description/meshes/sensors/srf05.stl"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+  </link>
+  <joint name="base_sonar_01_joint" type="fixed">
+    <origin rpy="0 0 3.9269908169872414" xyz="-0.183 -0.183 0.173"/>
+    <axis xyz="0 0 1"/>
+    <parent link="base_link"/>
+    <child link="base_sonar_01_link"/>
+  </joint>
+  <gazebo reference="base_sonar_01_link">
+    <sensor name="base_sonar_01" type="ray">
+      <pose>0 0 0 0 0 0</pose>
+      <update_rate>5</update_rate>
+      <visualize>false</visualize>
+      <ray>
+        <scan>
+          <horizontal>
+            <samples>5</samples>
+            <resolution>1</resolution>
+            <min_angle>-0.25</min_angle>
+            <max_angle>0.25</max_angle>
+          </horizontal>
+          <vertical>
+            <samples>5</samples>
+            <resolution>1</resolution>
+            <min_angle>-0.25</min_angle>
+            <max_angle>0.25</max_angle>
+          </vertical>
+        </scan>
+        <range>
+          <min>0.02</min>
+          <max>1.0</max>
+          <resolution>0.01</resolution>
+        </range>
+      </ray>
+      <plugin filename="libgazebo_ros_range.so" name="gazebo_ros_range">
+        <gaussianNoise>0.005</gaussianNoise>
+        <alwaysOn>true</alwaysOn>
+        <updateRate>5</updateRate>
+        <topicName>sonar_base</topicName>
+        <frameName>base_sonar_01_link</frameName>
+        <minRange>0.02</minRange>
+        <maxRange>1.0</maxRange>
+        <fov>0.5</fov>
+        <radiation>ultrasound</radiation>
+      </plugin>
+    </sensor>
+    <material>Gazebo/DarkGrey</material>
+  </gazebo>
+  <link name="base_mic_front_left_link"/>
+  <joint name="base_mic_front_left_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0.23182 0.03902 0.195"/>
+    <child link="base_mic_front_left_link"/>
+    <parent link="base_link"/>
+  </joint>
+  <link name="base_mic_front_right_link"/>
+  <joint name="base_mic_front_right_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0.23182 -0.03902 0.195"/>
+    <child link="base_mic_front_right_link"/>
+    <parent link="base_link"/>
+  </joint>
+  <link name="base_mic_back_left_link"/>
+  <joint name="base_mic_back_left_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="-0.1082 -0.0599 0.195"/>
+    <child link="base_mic_back_left_link"/>
+    <parent link="base_link"/>
+  </joint>
+  <link name="base_mic_back_right_link"/>
+  <joint name="base_mic_back_right_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="-0.1082 -0.13789 0.195"/>
+    <child link="base_mic_back_right_link"/>
+    <parent link="base_link"/>
+  </joint>
+  <!-- Base -->
+  <link name="base_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="-0.00000106 0.00000109 0.0000387"/>
+      <mass value="28.26649"/>
+      <inertia ixx="0.465408937" ixy="0.002160024" ixz="-0.001760255" iyy="0.483193291" iyz="-0.000655952" izz="0.550939703"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://pmb2_description/meshes/base/base.stl"/>
+      </geometry>
+      <material name="White"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://pmb2_description/meshes/base/base_collision.stl"/>
+      </geometry>
+      <material name="White"/>
+    </collision>
+  </link>
+  <gazebo reference="base_link">
+    <material>Gazebo/White</material>
+  </gazebo>
+  <!-- Base footprint -->
+  <link name="base_footprint"/>
+  <joint name="base_footprint_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0.0985"/>
+    <child link="base_link"/>
+    <parent link="base_footprint"/>
+  </joint>
+  <!-- gazebo parameters are given here for base_link thru base_footprint -->
+  <gazebo reference="base_footprint">
+    <kp>100000000.0</kp>
+    <kd>10.0</kd>
+    <mu1>0.1</mu1>
+    <mu2>0.1</mu2>
+    <fdir1>1 0 0</fdir1>
+    <maxVel>10.0</maxVel>
+    <minDepth>0.0005</minDepth>
+    <laserRetro>0</laserRetro>
+  </gazebo>
+  <link name="base_cover_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.22414000000 0.00002000000 0.04681000000"/>
+      <mass value="0.25821000000"/>
+      <inertia ixx="0.00023719188" ixy="0.00000006119" ixz="-0.00000011799" iyy="0.00028335052" iyz="0.00000000774" izz="0.00038497063"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://pmb2_description/meshes/base/base_ring.stl"/>
+      </geometry>
+      <material name="Orange"/>
+    </visual>
+  </link>
+  <joint name="cover_joint" type="fixed">
+    <parent link="base_footprint"/>
+    <child link="base_cover_link"/>
+    <origin rpy="0 0 0" xyz="0 0 0.13"/>
+  </joint>
+  <gazebo reference="base_cover_link">
+    <material>Gazebo/Orange</material>
+  </gazebo>
+  <link name="base_antenna_left_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <mass value="0.000001"/>
+      <inertia ixx="0.00000002371" ixy="0.00000006119" ixz="0.00000001179" iyy="0.00000002833" iyz="0.00000000774" izz="0.00000003849"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://pmb2_description/meshes/objects/antenna.stl"/>
+      </geometry>
+      <material name="Black"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0.085"/>
+      <geometry>
+        <cylinder length="0.17" radius="0.005"/>
+      </geometry>
+      <material name="Black"/>
+    </collision>
+  </link>
+  <gazebo reference="base_antenna_left_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <joint name="base_antenna_left_joint" type="fixed">
+    <parent link="base_link"/>
+    <child link="base_antenna_left_link"/>
+    <origin rpy="0 0 0" xyz="-0.201 0.1062 0.195"/>
+  </joint>
+  <link name="base_antenna_right_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <mass value="0.000001"/>
+      <inertia ixx="0.00000002371" ixy="0.00000006119" ixz="0.00000001179" iyy="0.00000002833" iyz="0.00000000774" izz="0.00000003849"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://pmb2_description/meshes/objects/antenna.stl"/>
+      </geometry>
+      <material name="Black"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0.085"/>
+      <geometry>
+        <cylinder length="0.17" radius="0.005"/>
+      </geometry>
+      <material name="Black"/>
+    </collision>
+  </link>
+  <gazebo reference="base_antenna_right_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <joint name="base_antenna_right_joint" type="fixed">
+    <parent link="base_link"/>
+    <child link="base_antenna_right_link"/>
+    <origin rpy="0 0 0" xyz="-0.201 -0.1062 0.195"/>
+  </joint>
+  <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.5707963267948966" xyz="0.10171 0 0.11282"/>
+  </joint>
+  <gazebo reference="base_imu_link">
+    <sensor name="base_imu_sensor" type="imu">
+      <always_on>1</always_on>
+      <update_rate>100.0</update_rate>
+      <imu>
+        <noise>
+          <type>gaussian</type>
+          <rate>
+            <mean>0.0</mean>
+            <stddev>2e-4</stddev>
+            <bias_mean>0.0000075</bias_mean>
+            <bias_stddev>0.0000008</bias_stddev>
+          </rate>
+          <accel>
+            <mean>0.0</mean>
+            <stddev>1.7e-2</stddev>
+            <bias_mean>0.1</bias_mean>
+            <bias_stddev>0.001</bias_stddev>
+          </accel>
+        </noise>
+      </imu>
+    </sensor>
+  </gazebo>
+  <link name="wheel_right_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 -0.00207"/>
+      <mass value="1.82362"/>
+      <inertia ixx="0.00499743171" ixy="4.464e-08" ixz="-0.00000002245" iyy="0.00499741733" iyz="-1.64e-09" izz="0.00839239692"/>
+    </inertial>
+    <visual>
+      <origin rpy="-0.0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://pmb2_description/meshes/wheels/wheel.stl"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <sphere radius="0.0985"/>
+        <!-- length="${width}"/> -->
+      </geometry>
+    </collision>
+  </link>
+  <link name="suspension_right_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 -0.02"/>
+      <mass value="10"/>
+      <inertia ixx="1.0" ixy="0" ixz="0" iyy="1.0" iyz="0" izz="1.0"/>
+    </inertial>
+    <kinematic>0</kinematic>
+    <gravity>1</gravity>
+  </link>
+  <joint name="suspension_right_joint" type="prismatic">
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="100.0" lower="-0.005" upper="0.005" velocity="1.0"/>
+    <child link="suspension_right_link"/>
+    <parent link="base_link"/>
+    <calibration rising="0.0"/>
+    <dynamics damping="100"/>
+    <safety_controller k_position="0" k_velocity="10" soft_lower_limit="-0.005" soft_upper_limit="0.005"/>
+  </joint>
+  <joint name="wheel_right_joint" type="continuous">
+    <parent link="suspension_right_link"/>
+    <child link="wheel_right_link"/>
+    <origin rpy="-1.5707963267948966 0 0" xyz="0 -0.2022 0.0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="6.0" velocity="10.152284263959391"/>
+  </joint>
+  <transmission name="wheel_right_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <actuator name="wheel_right_motor">
+      <mechanicalReduction>1.0</mechanicalReduction>
+    </actuator>
+    <joint name="wheel_right_joint">
+      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
+      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+    </joint>
+  </transmission>
+  <!-- Contact model for the wheel surface -->
+  <gazebo reference="wheel_right_link">
+    <kp>1000000.0</kp>
+    <kd>100.0</kd>
+    <!--
+      <mu1>10.0</mu1>
+      <mu2>10.0</mu2>
+      -->
+    <mu1>100000000000000.0</mu1>
+    <mu2>100000000000000.0</mu2>
+    <!--<fdir1>1 0 0</fdir1>-->
+    <maxVel>1.0</maxVel>
+    <minDepth>0.002</minDepth>
+    <material>Gazebo/DarkGrey</material>
+  </gazebo>
+  <gazebo reference="wheel_{side}_joint">
+    <implicitSpringDamper>1</implicitSpringDamper>
+  </gazebo>
+  <link name="wheel_left_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0.00207"/>
+      <mass value="1.82362"/>
+      <inertia ixx="0.00499743171" ixy="-4.464e-08" ixz="-0.00000002245" iyy="0.00499741733" iyz="1.64e-09" izz="0.00839239692"/>
+    </inertial>
+    <visual>
+      <origin rpy="-3.141592653589793 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://pmb2_description/meshes/wheels/wheel.stl"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <sphere radius="0.0985"/>
+        <!-- length="${width}"/> -->
+      </geometry>
+    </collision>
+  </link>
+  <link name="suspension_left_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 -0.02"/>
+      <mass value="10"/>
+      <inertia ixx="1.0" ixy="0" ixz="0" iyy="1.0" iyz="0" izz="1.0"/>
+    </inertial>
+    <kinematic>0</kinematic>
+    <gravity>1</gravity>
+  </link>
+  <joint name="suspension_left_joint" type="prismatic">
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="100.0" lower="-0.005" upper="0.005" velocity="1.0"/>
+    <child link="suspension_left_link"/>
+    <parent link="base_link"/>
+    <calibration rising="0.0"/>
+    <dynamics damping="100"/>
+    <safety_controller k_position="0" k_velocity="10" soft_lower_limit="-0.005" soft_upper_limit="0.005"/>
+  </joint>
+  <joint name="wheel_left_joint" type="continuous">
+    <parent link="suspension_left_link"/>
+    <child link="wheel_left_link"/>
+    <origin rpy="-1.5707963267948966 0 0" xyz="0 0.2022 0.0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="6.0" velocity="10.152284263959391"/>
+  </joint>
+  <transmission name="wheel_left_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <actuator name="wheel_left_motor">
+      <mechanicalReduction>1.0</mechanicalReduction>
+    </actuator>
+    <joint name="wheel_left_joint">
+      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
+      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+    </joint>
+  </transmission>
+  <!-- Contact model for the wheel surface -->
+  <gazebo reference="wheel_left_link">
+    <kp>1000000.0</kp>
+    <kd>100.0</kd>
+    <!--
+      <mu1>10.0</mu1>
+      <mu2>10.0</mu2>
+      -->
+    <mu1>100000000000000.0</mu1>
+    <mu2>100000000000000.0</mu2>
+    <!--<fdir1>1 0 0</fdir1>-->
+    <maxVel>1.0</maxVel>
+    <minDepth>0.002</minDepth>
+    <material>Gazebo/DarkGrey</material>
+  </gazebo>
+  <gazebo reference="wheel_{side}_joint">
+    <implicitSpringDamper>1</implicitSpringDamper>
+  </gazebo>
+  <link name="caster_front_right_1_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="-0.00634599 0.00005347 -0.02037519"/>
+      <mass value="0.05144761"/>
+      <inertia ixx="0.00001513" ixy="-0.00000003" ixz="0.00000267" iyy="0.00001309" iyz="-0.00000005" izz="0.00001198"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://pmb2_description/meshes/wheels/caster_1.stl"/>
+      </geometry>
+      <material name="Grey"/>
+    </visual>
+    <!--collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <cylinder radius="${radius}" length="${width}"/>
+        </geometry>
+      </collision-->
+  </link>
+  <link name="caster_front_right_2_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <mass value="0.08855784"/>
+      <inertia ixx="0.00001075" ixy="0" ixz="0" iyy="0.00001541" iyz="0" izz="0.00001075"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://pmb2_description/meshes/wheels/caster_2.stl"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <sphere radius="0.025"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="caster_front_right_1_joint" type="continuous">
+    <parent link="base_link"/>
+    <child link="caster_front_right_1_link"/>
+    <origin rpy="0 0 0" xyz="0.1695 -0.102 -0.0335"/>
+    <axis xyz="0 0 1"/>
+    <dynamics damping="0.005" friction="0.0"/>
+  </joint>
+  <joint name="caster_front_right_2_joint" type="continuous">
+    <parent link="caster_front_right_1_link"/>
+    <child link="caster_front_right_2_link"/>
+    <origin rpy="-1.5707963267948966 0 0" xyz="-0.016 0.0000 -0.040"/>
+    <axis xyz="0 0 1"/>
+  </joint>
+  <!-- Contact model for the caster wheel surface -->
+  <gazebo reference="caster_front_right_1_link">
+    <kp>100000000.0</kp>
+    <kd>10.0</kd>
+    <mu1>0.1</mu1>
+    <mu2>0.1</mu2>
+    <!--<fdir1>1 0 0</fdir1>-->
+    <maxVel>10.0</maxVel>
+    <minDepth>0.0005</minDepth>
+    <material>Gazebo/DarkGrey</material>
+  </gazebo>
+  <gazebo reference="caster_front_right_2_link">
+    <kp>10000000.0</kp>
+    <kd>10.0</kd>
+    <mu1>0.1</mu1>
+    <mu2>0.1</mu2>
+    <!--<fdir1>1 0 0</fdir1>-->
+    <maxVel>10.0</maxVel>
+    <minDepth>0.0005</minDepth>
+    <material>Gazebo/DarkGrey</material>
+  </gazebo>
+  <gazebo reference="caster_{side}_1_joint">
+    <implicitSpringDamper>0</implicitSpringDamper>
+  </gazebo>
+  <gazebo reference="caster_{side}_2_joint">
+    <implicitSpringDamper>0</implicitSpringDamper>
+  </gazebo>
+  <link name="caster_front_left_1_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="-0.00634599 0.00005347 -0.02037519"/>
+      <mass value="0.05144761"/>
+      <inertia ixx="0.00001513" ixy="-0.00000003" ixz="0.00000267" iyy="0.00001309" iyz="-0.00000005" izz="0.00001198"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://pmb2_description/meshes/wheels/caster_1.stl"/>
+      </geometry>
+      <material name="Grey"/>
+    </visual>
+    <!--collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <cylinder radius="${radius}" length="${width}"/>
+        </geometry>
+      </collision-->
+  </link>
+  <link name="caster_front_left_2_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <mass value="0.08855784"/>
+      <inertia ixx="0.00001075" ixy="0" ixz="0" iyy="0.00001541" iyz="0" izz="0.00001075"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://pmb2_description/meshes/wheels/caster_2.stl"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <sphere radius="0.025"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="caster_front_left_1_joint" type="continuous">
+    <parent link="base_link"/>
+    <child link="caster_front_left_1_link"/>
+    <origin rpy="0 0 0" xyz="0.1695 0.102 -0.0335"/>
+    <axis xyz="0 0 1"/>
+    <dynamics damping="0.005" friction="0.0"/>
+  </joint>
+  <joint name="caster_front_left_2_joint" type="continuous">
+    <parent link="caster_front_left_1_link"/>
+    <child link="caster_front_left_2_link"/>
+    <origin rpy="-1.5707963267948966 0 0" xyz="-0.016 0.0000 -0.040"/>
+    <axis xyz="0 0 1"/>
+  </joint>
+  <!-- Contact model for the caster wheel surface -->
+  <gazebo reference="caster_front_left_1_link">
+    <kp>100000000.0</kp>
+    <kd>10.0</kd>
+    <mu1>0.1</mu1>
+    <mu2>0.1</mu2>
+    <!--<fdir1>1 0 0</fdir1>-->
+    <maxVel>10.0</maxVel>
+    <minDepth>0.0005</minDepth>
+    <material>Gazebo/DarkGrey</material>
+  </gazebo>
+  <gazebo reference="caster_front_left_2_link">
+    <kp>10000000.0</kp>
+    <kd>10.0</kd>
+    <mu1>0.1</mu1>
+    <mu2>0.1</mu2>
+    <!--<fdir1>1 0 0</fdir1>-->
+    <maxVel>10.0</maxVel>
+    <minDepth>0.0005</minDepth>
+    <material>Gazebo/DarkGrey</material>
+  </gazebo>
+  <gazebo reference="caster_{side}_1_joint">
+    <implicitSpringDamper>0</implicitSpringDamper>
+  </gazebo>
+  <gazebo reference="caster_{side}_2_joint">
+    <implicitSpringDamper>0</implicitSpringDamper>
+  </gazebo>
+  <link name="caster_back_right_1_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="-0.00634599 0.00005347 -0.02037519"/>
+      <mass value="0.05144761"/>
+      <inertia ixx="0.00001513" ixy="-0.00000003" ixz="0.00000267" iyy="0.00001309" iyz="-0.00000005" izz="0.00001198"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://pmb2_description/meshes/wheels/caster_1.stl"/>
+      </geometry>
+      <material name="Grey"/>
+    </visual>
+    <!--collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <cylinder radius="${radius}" length="${width}"/>
+        </geometry>
+      </collision-->
+  </link>
+  <link name="caster_back_right_2_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <mass value="0.08855784"/>
+      <inertia ixx="0.00001075" ixy="0" ixz="0" iyy="0.00001541" iyz="0" izz="0.00001075"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://pmb2_description/meshes/wheels/caster_2.stl"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <sphere radius="0.025"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="caster_back_right_1_joint" type="continuous">
+    <parent link="base_link"/>
+    <child link="caster_back_right_1_link"/>
+    <origin rpy="0 0 0" xyz="-0.17350000000000002 -0.102 -0.0335"/>
+    <axis xyz="0 0 1"/>
+    <dynamics damping="0.005" friction="0.0"/>
+  </joint>
+  <joint name="caster_back_right_2_joint" type="continuous">
+    <parent link="caster_back_right_1_link"/>
+    <child link="caster_back_right_2_link"/>
+    <origin rpy="-1.5707963267948966 0 0" xyz="-0.016 0.0000 -0.040"/>
+    <axis xyz="0 0 1"/>
+  </joint>
+  <!-- Contact model for the caster wheel surface -->
+  <gazebo reference="caster_back_right_1_link">
+    <kp>100000000.0</kp>
+    <kd>10.0</kd>
+    <mu1>0.1</mu1>
+    <mu2>0.1</mu2>
+    <!--<fdir1>1 0 0</fdir1>-->
+    <maxVel>10.0</maxVel>
+    <minDepth>0.0005</minDepth>
+    <material>Gazebo/DarkGrey</material>
+  </gazebo>
+  <gazebo reference="caster_back_right_2_link">
+    <kp>10000000.0</kp>
+    <kd>10.0</kd>
+    <mu1>0.1</mu1>
+    <mu2>0.1</mu2>
+    <!--<fdir1>1 0 0</fdir1>-->
+    <maxVel>10.0</maxVel>
+    <minDepth>0.0005</minDepth>
+    <material>Gazebo/DarkGrey</material>
+  </gazebo>
+  <gazebo reference="caster_{side}_1_joint">
+    <implicitSpringDamper>0</implicitSpringDamper>
+  </gazebo>
+  <gazebo reference="caster_{side}_2_joint">
+    <implicitSpringDamper>0</implicitSpringDamper>
+  </gazebo>
+  <link name="caster_back_left_1_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="-0.00634599 0.00005347 -0.02037519"/>
+      <mass value="0.05144761"/>
+      <inertia ixx="0.00001513" ixy="-0.00000003" ixz="0.00000267" iyy="0.00001309" iyz="-0.00000005" izz="0.00001198"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://pmb2_description/meshes/wheels/caster_1.stl"/>
+      </geometry>
+      <material name="Grey"/>
+    </visual>
+    <!--collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <cylinder radius="${radius}" length="${width}"/>
+        </geometry>
+      </collision-->
+  </link>
+  <link name="caster_back_left_2_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <mass value="0.08855784"/>
+      <inertia ixx="0.00001075" ixy="0" ixz="0" iyy="0.00001541" iyz="0" izz="0.00001075"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://pmb2_description/meshes/wheels/caster_2.stl"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <sphere radius="0.025"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="caster_back_left_1_joint" type="continuous">
+    <parent link="base_link"/>
+    <child link="caster_back_left_1_link"/>
+    <origin rpy="0 0 0" xyz="-0.17350000000000002 0.102 -0.0335"/>
+    <axis xyz="0 0 1"/>
+    <dynamics damping="0.005" friction="0.0"/>
+  </joint>
+  <joint name="caster_back_left_2_joint" type="continuous">
+    <parent link="caster_back_left_1_link"/>
+    <child link="caster_back_left_2_link"/>
+    <origin rpy="-1.5707963267948966 0 0" xyz="-0.016 0.0000 -0.040"/>
+    <axis xyz="0 0 1"/>
+  </joint>
+  <!-- Contact model for the caster wheel surface -->
+  <gazebo reference="caster_back_left_1_link">
+    <kp>100000000.0</kp>
+    <kd>10.0</kd>
+    <mu1>0.1</mu1>
+    <mu2>0.1</mu2>
+    <!--<fdir1>1 0 0</fdir1>-->
+    <maxVel>10.0</maxVel>
+    <minDepth>0.0005</minDepth>
+    <material>Gazebo/DarkGrey</material>
+  </gazebo>
+  <gazebo reference="caster_back_left_2_link">
+    <kp>10000000.0</kp>
+    <kd>10.0</kd>
+    <mu1>0.1</mu1>
+    <mu2>0.1</mu2>
+    <!--<fdir1>1 0 0</fdir1>-->
+    <maxVel>10.0</maxVel>
+    <minDepth>0.0005</minDepth>
+    <material>Gazebo/DarkGrey</material>
+  </gazebo>
+  <gazebo reference="caster_{side}_1_joint">
+    <implicitSpringDamper>0</implicitSpringDamper>
+  </gazebo>
+  <gazebo reference="caster_{side}_2_joint">
+    <implicitSpringDamper>0</implicitSpringDamper>
+  </gazebo>
+  <!-- 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="package://tiago_dual_description/meshes/torso/torso_fix.stl" 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>
+  <gazebo reference="torso_fixed_column_link">
+    <material>Gazebo/DarkGrey</material>
+  </gazebo>
+  <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="package://tiago_dual_description/meshes/torso/torso_lift_dual_arm.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="White"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://tiago_description/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="package://tiago_description/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="package://tiago_description/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="package://tiago_description/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>
+  <!-- includes -->
+  <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="package://tiago_description/meshes/head/head_1.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="White"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0.0" xyz="0.0 0 0"/>
+      <geometry>
+        <mesh filename="package://tiago_description/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.3089969389957472" upper="1.3089969389957472" velocity="3.0"/>
+    <dynamics damping="0.5" friction="1.0"/>
+    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-1.2389969389957471" soft_upper_limit="1.2389969389957471"/>
+  </joint>
+  <gazebo reference="head_1_link">
+    <mu1>0.9</mu1>
+    <mu2>0.9</mu2>
+  </gazebo>
+  <gazebo reference="head_1_joint">
+    <implicitSpringDamper>1</implicitSpringDamper>
+  </gazebo>
+  <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</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="package://tiago_description/meshes/head/head_2.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="White"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://tiago_description/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.5707963267948966 0 0" xyz="0.005 0.0 0.098"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.77" lower="-1.0471975511965976" upper="0.7853981633974483" velocity="3.0"/>
+    <dynamics damping="0.5" friction="1.0"/>
+    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-0.9771975511965976" soft_upper_limit="0.7153981633974482"/>
+  </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</offset>
+      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+  </transmission>
+  <!--************************-->
+  <!--        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="package://tiago_description/meshes/arm/arm_1.stl" scale="1 -1 -1"/>
+      </geometry>
+      <material name="White"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://tiago_description/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.141592653589793 0 1.5707963267948966" xyz="0.02556 0.19 -0.171"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="43.0" lower="-1.1780972450961724" upper="1.5707963267948966" velocity="1.95"/>
+    <dynamics damping="1.0" friction="1.0"/>
+    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-1.1080972450961724" soft_upper_limit="1.5007963267948965"/>
+  </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="package://tiago_description/meshes/arm/arm_2.stl" 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="package://tiago_description/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.5707963267948966 0.0 0.0" xyz="0.125 -0.0195 0.031"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="43.0" lower="-1.1780972450961724" upper="1.5707963267948966" velocity="1.95"/>
+    <dynamics damping="1.0" friction="1.0"/>
+    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-1.1080972450961724" soft_upper_limit="1.5007963267948965"/>
+  </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="package://tiago_description/meshes/arm/arm_3.stl" scale="1 1 -1"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://tiago_description/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.5707963267948966 3.141592653589793 -1.5707963267948966" xyz="0.0895 0.0 -0.0015"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="26" lower="-0.7853981633974483" upper="3.9269908169872414" velocity="2.35"/>
+    <dynamics damping="1.0" friction="1.0"/>
+    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-0.7153981633974482" soft_upper_limit="3.8569908169872416"/>
+  </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="package://tiago_description/meshes/arm/arm_4.stl" scale="-1 -1 -1"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://tiago_description/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.5707963267948966 -1.5707963267948966 0.0" xyz="-0.02 -0.027 0.222"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="26" lower="-0.39269908169872414" upper="2.356194490192345" velocity="2.35"/>
+    <dynamics damping="1.0" friction="1.0"/>
+    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-0.32269908169872413" soft_upper_limit="2.286194490192345"/>
+  </joint>
+  <gazebo reference="arm_left_1_link">
+    <mu1>0.9</mu1>
+    <mu2>0.9</mu2>
+    <material>Gazebo/DarkGrey</material>
+  </gazebo>
+  <gazebo reference="arm_left_2_link">
+    <mu1>0.9</mu1>
+    <mu2>0.9</mu2>
+    <material>Gazebo/DarkGrey</material>
+  </gazebo>
+  <gazebo reference="arm_left_3_link">
+    <mu1>0.9</mu1>
+    <mu2>0.9</mu2>
+    <material>Gazebo/DarkGrey</material>
+  </gazebo>
+  <gazebo reference="arm_left_4_link">
+    <mu1>0.9</mu1>
+    <mu2>0.9</mu2>
+    <material>Gazebo/DarkGrey</material>
+  </gazebo>
+  <gazebo reference="arm_left_1_joint">
+    <implicitSpringDamper>1</implicitSpringDamper>
+  </gazebo>
+  <gazebo reference="arm_left_2_joint">
+    <implicitSpringDamper>1</implicitSpringDamper>
+  </gazebo>
+  <gazebo reference="arm_left_3_joint">
+    <implicitSpringDamper>1</implicitSpringDamper>
+  </gazebo>
+  <gazebo reference="arm_left_4_joint">
+    <implicitSpringDamper>1</implicitSpringDamper>
+  </gazebo>
+  <!--************************-->
+  <!--        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="package://tiago_description/meshes/arm/arm_5-wrist-2010.stl" scale="-1 -1 -1"/>
+      </geometry>
+      <material name="LightGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://tiago_description/meshes/arm/arm_5_collision-wrist-2010.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.5707963267948966 -1.5707963267948966 -1.5707963267948966" xyz="0.162 -0.02 -0.027"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="3" lower="-2.0943951023931953" upper="2.0943951023931953" velocity="1.95"/>
+    <dynamics damping="1.0" friction="1.0"/>
+    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-2.0743951023931952" soft_upper_limit="2.0743951023931952"/>
+  </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="package://tiago_description/meshes/arm/arm_6-wrist-2010.stl" scale="-1 1 1"/>
+      </geometry>
+      <material name="LightGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://tiago_description/meshes/arm/arm_6_collision-wrist-2010.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.5707963267948966 -1.5707963267948966" xyz="0 0 -0.15"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="6.6" lower="-1.413716694115407" upper="1.413716694115407" velocity="1.76"/>
+    <dynamics damping="1.0" friction="1.0"/>
+    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-1.3937166941154069" soft_upper_limit="1.3937166941154069"/>
+  </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.5707963267948966 0.0 1.5707963267948966" xyz="0 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="6.6" lower="-2.0943951023931953" upper="2.0943951023931953" velocity="1.76"/>
+    <dynamics damping="1.0" friction="1.0"/>
+    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-2.0743951023931952" soft_upper_limit="2.0743951023931952"/>
+  </joint>
+  <gazebo reference="arm_left_5_link">
+    <mu1>0.9</mu1>
+    <mu2>0.9</mu2>
+  </gazebo>
+  <gazebo reference="arm_left_6_link">
+    <mu1>0.9</mu1>
+    <mu2>0.9</mu2>
+  </gazebo>
+  <gazebo reference="arm_left_7_link">
+    <mu1>0.9</mu1>
+    <mu2>0.9</mu2>
+  </gazebo>
+  <gazebo reference="arm_left_5_joint">
+    <implicitSpringDamper>1</implicitSpringDamper>
+  </gazebo>
+  <gazebo reference="arm_left_6_joint">
+    <implicitSpringDamper>1</implicitSpringDamper>
+  </gazebo>
+  <gazebo reference="arm_left_7_joint">
+    <implicitSpringDamper>1</implicitSpringDamper>
+    <provideFeedback>1</provideFeedback>
+  </gazebo>
+  <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</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</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</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="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 1.5707963267948966 0" xyz="0.001 0 0"/>
+      <geometry>
+        <cylinder length="0.005" radius="0.005"/>
+      </geometry>
+      <material name="LightGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 1.5707963267948966 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.5707963267948966 1.5707963267948966 3.141592653589793" 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</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</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</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</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="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0"/>
+    </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.5707963267948966 0 1.5707963267948966" xyz="0.00785 0 0"/>
+  </joint>
+  <!--***********************-->
+  <!--       FT TOOL         -->
+  <!--***********************-->
+  <link name="wrist_left_ft_tool_link">
+    <inertial>
+      <mass value="0.1"/>
+      <inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 1.5707963267948966 0" xyz="0.0 0 0"/>
+      <geometry>
+        <cylinder length="0.00975" radius="0.025"/>
+      </geometry>
+      <material name="FlatBlack"/>
+    </visual>
+    <collision>
+      <origin rpy="0 1.5707963267948966 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.5707963267948966 -1.5707963267948966 0" xyz="0 0 0.012725"/>
+  </joint>
+  <!-- Tool Link -->
+  <link name="hand_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="hand_left_tool_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <parent link="wrist_left_ft_tool_link"/>
+    <child link="hand_left_tool_link"/>
+    <axis xyz="0 0 0"/>
+  </joint>
+  <joint name="hand_left_palm_joint" type="fixed">
+    <parent link="wrist_left_ft_tool_link"/>
+    <child link="hand_left_palm_link"/>
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+  </joint>
+  <link name="hand_left_palm_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0641 -0.0022 0.0021"/>
+      <!-- NOTE: Less than total palm mass of 0.598kg because actuator masses are specified separately -->
+      <mass value="0.4"/>
+      <inertia ixx="0.000305100" ixy="0.000005037" ixz="0.000015302" iyy="0.000811920" iyz="0.000007622" izz="0.000655851"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/palm.stl" scale="1 1 -1"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/palm_collision.stl" scale="1 1 -1"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="hand_left_palm_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <joint name="hand_left_thumb_joint" type="revolute">
+    <parent link="hand_left_palm_link"/>
+    <child link="hand_left_thumb_link"/>
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="-2.6179938779914944" upper="6.283185307179586" velocity="3.35"/>
+    <dynamics damping="0.5" friction="0.0"/>
+    <!-- TODO: Tune -->
+    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-2.6179938779914944" soft_upper_limit="6.283185307179586"/>
+  </joint>
+  <link name="hand_left_thumb_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <mass value="0.05"/>
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!--To stabilize simulation-->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <sphere radius="0.001"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <sphere radius="0.001"/>
+      </geometry>
+    </collision>
+  </link>
+  <transmission name="hand_left_thumb_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <actuator name="hand_left_thumb_motor">
+      <mechanicalReduction>1.0</mechanicalReduction>
+    </actuator>
+    <joint name="hand_left_thumb_joint">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+  </transmission>
+  <joint name="hand_left_index_joint" type="revolute">
+    <parent link="hand_left_palm_link"/>
+    <child link="hand_left_index_link"/>
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="-3.0543261909900767" upper="6.806784082777885" velocity="3.35"/>
+    <dynamics damping="0.5" friction="0.0"/>
+    <!-- TODO: Tune -->
+    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-3.0543261909900767" soft_upper_limit="6.806784082777885"/>
+  </joint>
+  <link name="hand_left_index_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <mass value="0.05"/>
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!--To stabilize simulation-->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <sphere radius="0.001"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <sphere radius="0.001"/>
+      </geometry>
+    </collision>
+  </link>
+  <transmission name="hand_left_index_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <actuator name="hand_left_index_motor">
+      <mechanicalReduction>1.0</mechanicalReduction>
+    </actuator>
+    <joint name="hand_left_index_joint">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+  </transmission>
+  <joint name="hand_left_mrl_joint" type="revolute">
+    <parent link="hand_left_palm_link"/>
+    <child link="hand_left_mrl_link"/>
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="-2.530727415391778" upper="9.250245035569947" velocity="3.56"/>
+    <dynamics damping="0.5" friction="0.0"/>
+    <!-- TODO: Tune -->
+    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-2.530727415391778" soft_upper_limit="9.250245035569947"/>
+  </joint>
+  <link name="hand_left_mrl_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <mass value="0.05"/>
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!--To stabilize simulation-->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <sphere radius="0.001"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <sphere radius="0.001"/>
+      </geometry>
+    </collision>
+  </link>
+  <transmission name="hand_left_mrl_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <actuator name="hand_left_mrl_motor">
+      <mechanicalReduction>1.0</mechanicalReduction>
+    </actuator>
+    <joint name="hand_left_mrl_joint">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+  </transmission>
+  <joint name="hand_left_thumb_abd_joint" type="revolute">
+    <parent link="hand_left_palm_link"/>
+    <child link="hand_left_thumb_abd_link"/>
+    <origin rpy="0 1.1344640137963142 0" xyz="0.0901 0.0134 -0.038"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="0.0" upper="1.5707963267948966" velocity="2.0"/>
+    <dynamics damping="0.5" friction="0.0"/>
+  </joint>
+  <link name="hand_left_thumb_abd_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="-0.0132 0.0037 -0.0017"/>
+      <mass value="0.014"/>
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!-- To stabilize simulation -->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/thumb_abd_collision.stl" scale="1 1 -1"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/thumb_abd_collision.stl" scale="1 -1 1"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="hand_left_thumb_abd_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <joint name="hand_left_thumb_virtual_1_joint" type="revolute">
+    <parent link="hand_left_thumb_abd_link"/>
+    <child link="hand_left_thumb_virtual_1_link"/>
+    <origin rpy="1.1442378576074825 0 0" xyz="0.0225 0.003 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+    <dynamics damping="0.5" friction="0.0"/>
+  </joint>
+  <link name="hand_left_thumb_virtual_1_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+      <mass value="0.01"/>
+      <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!-- To stabilize simulation -->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+      <geometry>
+        <box size="0.013 0.002 0.012"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.013 0.002 0.012"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="hand_left_thumb_virtual_1_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <joint name="hand_left_thumb_flex_1_joint" type="revolute">
+    <parent link="hand_left_thumb_virtual_1_link"/>
+    <child link="hand_left_thumb_flex_1_link"/>
+    <origin rpy="0 0 0" xyz="0.013 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+    <dynamics damping="0.5" friction="0.0"/>
+  </joint>
+  <link name="hand_left_thumb_flex_1_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0047 0.0031 0.0"/>
+      <mass value="0.009"/>
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!-- To stabilize simulation -->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="hand_left_thumb_flex_1_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <joint name="hand_left_thumb_virtual_2_joint" type="revolute">
+    <parent link="hand_left_thumb_flex_1_link"/>
+    <child link="hand_left_thumb_virtual_2_link"/>
+    <origin rpy="0 0 0" xyz="0.009 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+    <dynamics damping="0.5" friction="0.0"/>
+  </joint>
+  <link name="hand_left_thumb_virtual_2_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+      <mass value="0.01"/>
+      <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!-- To stabilize simulation -->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+      <geometry>
+        <box size="0.013 0.002 0.012"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.013 0.002 0.012"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="hand_left_thumb_virtual_2_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <joint name="hand_left_thumb_flex_2_joint" type="revolute">
+    <parent link="hand_left_thumb_virtual_2_link"/>
+    <child link="hand_left_thumb_flex_2_link"/>
+    <origin rpy="0 0 0" xyz="0.013 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+    <dynamics damping="0.5" friction="0.0"/>
+  </joint>
+  <link name="hand_left_thumb_flex_2_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0096 0.003 0.0"/>
+      <mass value="0.015"/>
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!-- To stabilize simulation -->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_flex_tip_collision.stl"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_flex_tip_collision.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="hand_left_thumb_flex_2_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <joint name="hand_left_index_abd_joint" type="revolute">
+    <parent link="hand_left_palm_link"/>
+    <child link="hand_left_index_abd_link"/>
+    <origin rpy="1.5707963267948966 0.5235987755982988 0" xyz="0.1265 0.0042 -0.0265"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="-0.5235987755982988" upper="0.5235987755982988" velocity="2.0"/>
+    <dynamics damping="0.5" friction="0.0"/>
+  </joint>
+  <link name="hand_left_index_abd_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="-0.0018 0.0 0.0028"/>
+      <mass value="0.018"/>
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!-- To stabilize simulation -->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_abd_collision.stl" scale="1 1 -1"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_abd_collision.stl" scale="1 1 -1"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="hand_left_index_abd_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <joint name="hand_left_index_virtual_1_joint" type="revolute">
+    <parent link="hand_left_index_abd_link"/>
+    <child link="hand_left_index_virtual_1_link"/>
+    <origin rpy="-1.5707963267948966 0 0" xyz="0.009 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+    <dynamics damping="0.5" friction="0.0"/>
+  </joint>
+  <link name="hand_left_index_virtual_1_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+      <mass value="0.01"/>
+      <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!-- To stabilize simulation -->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+      <geometry>
+        <box size="0.013 0.002 0.012"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.013 0.002 0.012"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="hand_left_index_virtual_1_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <joint name="hand_left_index_flex_1_joint" type="revolute">
+    <parent link="hand_left_index_virtual_1_link"/>
+    <child link="hand_left_index_flex_1_link"/>
+    <origin rpy="0 0 0" xyz="0.013 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+    <dynamics damping="0.5" friction="0.0"/>
+  </joint>
+  <link name="hand_left_index_flex_1_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0047 0.0031 0.0"/>
+      <mass value="0.009"/>
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!-- To stabilize simulation -->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="hand_left_index_flex_1_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <joint name="hand_left_index_virtual_2_joint" type="revolute">
+    <parent link="hand_left_index_flex_1_link"/>
+    <child link="hand_left_index_virtual_2_link"/>
+    <origin rpy="0 0 0" xyz="0.009 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+    <dynamics damping="0.5" friction="0.0"/>
+  </joint>
+  <link name="hand_left_index_virtual_2_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+      <mass value="0.01"/>
+      <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!-- To stabilize simulation -->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+      <geometry>
+        <box size="0.013 0.002 0.012"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.013 0.002 0.012"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="hand_left_index_virtual_2_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <joint name="hand_left_index_flex_2_joint" type="revolute">
+    <parent link="hand_left_index_virtual_2_link"/>
+    <child link="hand_left_index_flex_2_link"/>
+    <origin rpy="0 0 0" xyz="0.013 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+    <dynamics damping="0.5" friction="0.0"/>
+  </joint>
+  <link name="hand_left_index_flex_2_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0047 0.0031 0.0"/>
+      <mass value="0.009"/>
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!-- To stabilize simulation -->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="hand_left_index_flex_2_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <joint name="hand_left_index_virtual_3_joint" type="revolute">
+    <parent link="hand_left_index_flex_2_link"/>
+    <child link="hand_left_index_virtual_3_link"/>
+    <origin rpy="0 0 0" xyz="0.009 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+    <dynamics damping="0.5" friction="0.0"/>
+  </joint>
+  <link name="hand_left_index_virtual_3_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+      <mass value="0.01"/>
+      <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!-- To stabilize simulation -->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+      <geometry>
+        <box size="0.013 0.002 0.012"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.013 0.002 0.012"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="hand_left_index_virtual_3_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <joint name="hand_left_index_flex_3_joint" type="revolute">
+    <parent link="hand_left_index_virtual_3_link"/>
+    <child link="hand_left_index_flex_3_link"/>
+    <origin rpy="0 0 0" xyz="0.013 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+    <dynamics damping="0.5" friction="0.0"/>
+  </joint>
+  <link name="hand_left_index_flex_3_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0096 0.003 0.0"/>
+      <mass value="0.015"/>
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!-- To stabilize simulation -->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_flex_tip_collision.stl"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_flex_tip_collision.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="hand_left_index_flex_3_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <joint name="hand_left_middle_abd_joint" type="revolute">
+    <parent link="hand_left_palm_link"/>
+    <child link="hand_left_middle_abd_link"/>
+    <origin rpy="1.5707963267948966 0 0" xyz="0.1335 0.0042 -0.007"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="-0.5235987755982988" upper="0.5235987755982988" velocity="2.0"/>
+    <dynamics damping="0.5" friction="0.0"/>
+  </joint>
+  <link name="hand_left_middle_abd_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="-0.0018 0.0 0.0028"/>
+      <mass value="0.018"/>
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!-- To stabilize simulation -->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_abd_collision.stl" scale="1 1 -1"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_abd_collision.stl" scale="1 1 -1"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="hand_left_middle_abd_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <joint name="hand_left_middle_virtual_1_joint" type="revolute">
+    <parent link="hand_left_middle_abd_link"/>
+    <child link="hand_left_middle_virtual_1_link"/>
+    <origin rpy="-1.5707963267948966 0 0" xyz="0.009 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+    <dynamics damping="0.5" friction="0.0"/>
+  </joint>
+  <link name="hand_left_middle_virtual_1_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+      <mass value="0.01"/>
+      <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!-- To stabilize simulation -->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+      <geometry>
+        <box size="0.013 0.002 0.012"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.013 0.002 0.012"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="hand_left_middle_virtual_1_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <joint name="hand_left_middle_flex_1_joint" type="revolute">
+    <parent link="hand_left_middle_virtual_1_link"/>
+    <child link="hand_left_middle_flex_1_link"/>
+    <origin rpy="0 0 0" xyz="0.013 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+    <dynamics damping="0.5" friction="0.0"/>
+  </joint>
+  <link name="hand_left_middle_flex_1_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0047 0.0031 0.0"/>
+      <mass value="0.009"/>
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!-- To stabilize simulation -->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="hand_left_middle_flex_1_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <joint name="hand_left_middle_virtual_2_joint" type="revolute">
+    <parent link="hand_left_middle_flex_1_link"/>
+    <child link="hand_left_middle_virtual_2_link"/>
+    <origin rpy="0 0 0" xyz="0.009 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+    <dynamics damping="0.5" friction="0.0"/>
+  </joint>
+  <link name="hand_left_middle_virtual_2_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+      <mass value="0.01"/>
+      <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!-- To stabilize simulation -->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+      <geometry>
+        <box size="0.013 0.002 0.012"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.013 0.002 0.012"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="hand_left_middle_virtual_2_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <joint name="hand_left_middle_flex_2_joint" type="revolute">
+    <parent link="hand_left_middle_virtual_2_link"/>
+    <child link="hand_left_middle_flex_2_link"/>
+    <origin rpy="0 0 0" xyz="0.013 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+    <dynamics damping="0.5" friction="0.0"/>
+  </joint>
+  <link name="hand_left_middle_flex_2_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0047 0.0031 0.0"/>
+      <mass value="0.009"/>
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!-- To stabilize simulation -->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="hand_left_middle_flex_2_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <joint name="hand_left_middle_virtual_3_joint" type="revolute">
+    <parent link="hand_left_middle_flex_2_link"/>
+    <child link="hand_left_middle_virtual_3_link"/>
+    <origin rpy="0 0 0" xyz="0.009 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+    <dynamics damping="0.5" friction="0.0"/>
+  </joint>
+  <link name="hand_left_middle_virtual_3_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+      <mass value="0.01"/>
+      <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!-- To stabilize simulation -->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+      <geometry>
+        <box size="0.013 0.002 0.012"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.013 0.002 0.012"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="hand_left_middle_virtual_3_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <joint name="hand_left_middle_flex_3_joint" type="revolute">
+    <parent link="hand_left_middle_virtual_3_link"/>
+    <child link="hand_left_middle_flex_3_link"/>
+    <origin rpy="0 0 0" xyz="0.013 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+    <dynamics damping="0.5" friction="0.0"/>
+  </joint>
+  <link name="hand_left_middle_flex_3_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0096 0.003 0.0"/>
+      <mass value="0.015"/>
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!-- To stabilize simulation -->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_flex_tip_collision.stl"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_flex_tip_collision.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="hand_left_middle_flex_3_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <joint name="hand_left_ring_abd_joint" type="revolute">
+    <parent link="hand_left_palm_link"/>
+    <child link="hand_left_ring_abd_link"/>
+    <origin rpy="1.5707963267948966 -0.2617993877991494 0" xyz="0.1265 0.0042 0.0125"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="-0.5235987755982988" upper="0.5235987755982988" velocity="2.0"/>
+    <dynamics damping="0.5" friction="0.0"/>
+  </joint>
+  <link name="hand_left_ring_abd_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="-0.0018 0.0 0.0028"/>
+      <mass value="0.018"/>
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!-- To stabilize simulation -->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_abd_collision.stl" scale="1 1 -1"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_abd_collision.stl" scale="1 1 -1"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="hand_left_ring_abd_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <joint name="hand_left_ring_virtual_1_joint" type="revolute">
+    <parent link="hand_left_ring_abd_link"/>
+    <child link="hand_left_ring_virtual_1_link"/>
+    <origin rpy="-1.5707963267948966 0 0" xyz="0.009 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+    <dynamics damping="0.5" friction="0.0"/>
+  </joint>
+  <link name="hand_left_ring_virtual_1_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+      <mass value="0.01"/>
+      <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!-- To stabilize simulation -->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+      <geometry>
+        <box size="0.013 0.002 0.012"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.013 0.002 0.012"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="hand_left_ring_virtual_1_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <joint name="hand_left_ring_flex_1_joint" type="revolute">
+    <parent link="hand_left_ring_virtual_1_link"/>
+    <child link="hand_left_ring_flex_1_link"/>
+    <origin rpy="0 0 0" xyz="0.013 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+    <dynamics damping="0.5" friction="0.0"/>
+  </joint>
+  <link name="hand_left_ring_flex_1_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0047 0.0031 0.0"/>
+      <mass value="0.009"/>
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!-- To stabilize simulation -->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="hand_left_ring_flex_1_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <joint name="hand_left_ring_virtual_2_joint" type="revolute">
+    <parent link="hand_left_ring_flex_1_link"/>
+    <child link="hand_left_ring_virtual_2_link"/>
+    <origin rpy="0 0 0" xyz="0.009 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+    <dynamics damping="0.5" friction="0.0"/>
+  </joint>
+  <link name="hand_left_ring_virtual_2_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+      <mass value="0.01"/>
+      <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!-- To stabilize simulation -->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+      <geometry>
+        <box size="0.013 0.002 0.012"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.013 0.002 0.012"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="hand_left_ring_virtual_2_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <joint name="hand_left_ring_flex_2_joint" type="revolute">
+    <parent link="hand_left_ring_virtual_2_link"/>
+    <child link="hand_left_ring_flex_2_link"/>
+    <origin rpy="0 0 0" xyz="0.013 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+    <dynamics damping="0.5" friction="0.0"/>
+  </joint>
+  <link name="hand_left_ring_flex_2_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0047 0.0031 0.0"/>
+      <mass value="0.009"/>
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!-- To stabilize simulation -->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="hand_left_ring_flex_2_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <joint name="hand_left_ring_virtual_3_joint" type="revolute">
+    <parent link="hand_left_ring_flex_2_link"/>
+    <child link="hand_left_ring_virtual_3_link"/>
+    <origin rpy="0 0 0" xyz="0.009 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+    <dynamics damping="0.5" friction="0.0"/>
+  </joint>
+  <link name="hand_left_ring_virtual_3_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+      <mass value="0.01"/>
+      <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!-- To stabilize simulation -->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+      <geometry>
+        <box size="0.013 0.002 0.012"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.013 0.002 0.012"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="hand_left_ring_virtual_3_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <joint name="hand_left_ring_flex_3_joint" type="revolute">
+    <parent link="hand_left_ring_virtual_3_link"/>
+    <child link="hand_left_ring_flex_3_link"/>
+    <origin rpy="0 0 0" xyz="0.013 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+    <dynamics damping="0.5" friction="0.0"/>
+  </joint>
+  <link name="hand_left_ring_flex_3_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0096 0.003 0.0"/>
+      <mass value="0.015"/>
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!-- To stabilize simulation -->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_flex_tip_collision.stl"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_flex_tip_collision.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="hand_left_ring_flex_3_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <joint name="hand_left_little_abd_joint" type="revolute">
+    <parent link="hand_left_palm_link"/>
+    <child link="hand_left_little_abd_link"/>
+    <origin rpy="1.5707963267948966 -0.5235987755982988 0" xyz="0.1165 0.0042 0.032"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="-0.5235987755982988" upper="0.5235987755982988" velocity="2.0"/>
+    <dynamics damping="0.5" friction="0.0"/>
+  </joint>
+  <link name="hand_left_little_abd_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="-0.0018 0.0 0.0028"/>
+      <mass value="0.018"/>
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!-- To stabilize simulation -->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_abd_collision.stl" scale="1 1 -1"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_abd_collision.stl" scale="1 1 -1"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="hand_left_little_abd_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <joint name="hand_left_little_virtual_1_joint" type="revolute">
+    <parent link="hand_left_little_abd_link"/>
+    <child link="hand_left_little_virtual_1_link"/>
+    <origin rpy="-1.5707963267948966 0 0" xyz="0.009 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+    <dynamics damping="0.5" friction="0.0"/>
+  </joint>
+  <link name="hand_left_little_virtual_1_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+      <mass value="0.01"/>
+      <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!-- To stabilize simulation -->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+      <geometry>
+        <box size="0.013 0.002 0.012"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.013 0.002 0.012"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="hand_left_little_virtual_1_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <joint name="hand_left_little_flex_1_joint" type="revolute">
+    <parent link="hand_left_little_virtual_1_link"/>
+    <child link="hand_left_little_flex_1_link"/>
+    <origin rpy="0 0 0" xyz="0.013 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+    <dynamics damping="0.5" friction="0.0"/>
+  </joint>
+  <link name="hand_left_little_flex_1_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0047 0.0031 0.0"/>
+      <mass value="0.009"/>
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!-- To stabilize simulation -->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="hand_left_little_flex_1_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <joint name="hand_left_little_virtual_2_joint" type="revolute">
+    <parent link="hand_left_little_flex_1_link"/>
+    <child link="hand_left_little_virtual_2_link"/>
+    <origin rpy="0 0 0" xyz="0.009 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+    <dynamics damping="0.5" friction="0.0"/>
+  </joint>
+  <link name="hand_left_little_virtual_2_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+      <mass value="0.01"/>
+      <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!-- To stabilize simulation -->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+      <geometry>
+        <box size="0.013 0.002 0.012"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.013 0.002 0.012"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="hand_left_little_virtual_2_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <joint name="hand_left_little_flex_2_joint" type="revolute">
+    <parent link="hand_left_little_virtual_2_link"/>
+    <child link="hand_left_little_flex_2_link"/>
+    <origin rpy="0 0 0" xyz="0.013 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+    <dynamics damping="0.5" friction="0.0"/>
+  </joint>
+  <link name="hand_left_little_flex_2_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0047 0.0031 0.0"/>
+      <mass value="0.009"/>
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!-- To stabilize simulation -->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="hand_left_little_flex_2_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <joint name="hand_left_little_virtual_3_joint" type="revolute">
+    <parent link="hand_left_little_flex_2_link"/>
+    <child link="hand_left_little_virtual_3_link"/>
+    <origin rpy="0 0 0" xyz="0.009 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+    <dynamics damping="0.5" friction="0.0"/>
+  </joint>
+  <link name="hand_left_little_virtual_3_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+      <mass value="0.01"/>
+      <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!-- To stabilize simulation -->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+      <geometry>
+        <box size="0.013 0.002 0.012"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.013 0.002 0.012"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="hand_left_little_virtual_3_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <joint name="hand_left_little_flex_3_joint" type="revolute">
+    <parent link="hand_left_little_virtual_3_link"/>
+    <child link="hand_left_little_flex_3_link"/>
+    <origin rpy="0 0 0" xyz="0.013 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+    <dynamics damping="0.5" friction="0.0"/>
+  </joint>
+  <link name="hand_left_little_flex_3_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0096 0.003 0.0"/>
+      <mass value="0.015"/>
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!-- To stabilize simulation -->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_flex_tip_collision.stl"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_flex_tip_collision.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="hand_left_little_flex_3_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <!--Abductor-->
+  <transmission name="hand_left_thumb_abd_trans">
+    <type>hey5_transmissions/Hey5Reducer</type>
+    <joint name="hand_left_thumb_abd_joint">
+      <saturationLower>0.0</saturationLower>
+      <saturationUpper>1.5707963267948966</saturationUpper>
+      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+    </joint>
+    <actuator name="hand_left_thumb_motor">
+      <mechanicalReduction>0.6544984694978736</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <!--First flexor-->
+  <transmission name="hand_left_thumb_virtual_1_trans">
+    <type>hey5_transmissions/Hey5Reducer</type>
+    <joint name="hand_left_thumb_virtual_1_joint">
+      <saturationLower>0.0</saturationLower>
+      <deadBandUpper>2.0</deadBandUpper>
+      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+    </joint>
+    <actuator name="hand_left_thumb_motor">
+      <mechanicalReduction>0.16299357854713997</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="hand_left_thumb_flex_1_trans">
+    <type>hey5_transmissions/Hey5Reducer</type>
+    <joint name="hand_left_thumb_flex_1_joint">
+      <saturationLower>0.0</saturationLower>
+      <deadBandUpper>2.0</deadBandUpper>
+      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+    </joint>
+    <actuator name="hand_left_thumb_motor">
+      <mechanicalReduction>0.16299357854713997</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <!--Second flexor-->
+  <transmission name="hand_left_thumb_virtual_2_trans">
+    <type>hey5_transmissions/Hey5Reducer</type>
+    <joint name="hand_left_thumb_virtual_2_joint">
+      <saturationLower>0.0</saturationLower>
+      <deadBandUpper>2.0</deadBandUpper>
+      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+    </joint>
+    <actuator name="hand_left_thumb_motor">
+      <mechanicalReduction>0.16299357854713997</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="hand_left_thumb_flex_2_trans">
+    <type>hey5_transmissions/Hey5Reducer</type>
+    <joint name="hand_left_thumb_flex_2_joint">
+      <saturationLower>0.0</saturationLower>
+      <deadBandUpper>2.0</deadBandUpper>
+      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+    </joint>
+    <actuator name="hand_left_thumb_motor">
+      <mechanicalReduction>0.16299357854713997</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <!--Abductor-->
+  <transmission name="hand_left_index_abd_trans">
+    <type>hey5_transmissions/Hey5Reducer</type>
+    <joint name="hand_left_index_abd_joint">
+      <saturationLower>0.0</saturationLower>
+      <saturationUpper>0.5235987755982988</saturationUpper>
+      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+    </joint>
+    <actuator name="hand_left_index_motor">
+      <mechanicalReduction>0.49866550056980846</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <!--First flexor-->
+  <transmission name="hand_left_index_virtual_1_trans">
+    <type>hey5_transmissions/Hey5Reducer</type>
+    <joint name="hand_left_index_virtual_1_joint">
+      <saturationLower>0.0</saturationLower>
+      <deadBandUpper>0.7</deadBandUpper>
+      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+    </joint>
+    <actuator name="hand_left_index_motor">
+      <mechanicalReduction>0.10288861079760754</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="hand_left_index_flex_1_trans">
+    <type>hey5_transmissions/Hey5Reducer</type>
+    <joint name="hand_left_index_flex_1_joint">
+      <saturationLower>0.0</saturationLower>
+      <deadBandUpper>0.7</deadBandUpper>
+      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+    </joint>
+    <actuator name="hand_left_index_motor">
+      <mechanicalReduction>0.10288861079760754</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <!--Second flexor-->
+  <transmission name="hand_left_index_virtual_2_trans">
+    <type>hey5_transmissions/Hey5Reducer</type>
+    <joint name="hand_left_index_virtual_2_joint">
+      <saturationLower>0.0</saturationLower>
+      <deadBandUpper>0.7</deadBandUpper>
+      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+    </joint>
+    <actuator name="hand_left_index_motor">
+      <mechanicalReduction>0.10288861079760754</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="hand_left_index_flex_2_trans">
+    <type>hey5_transmissions/Hey5Reducer</type>
+    <joint name="hand_left_index_flex_2_joint">
+      <saturationLower>0.0</saturationLower>
+      <deadBandUpper>0.7</deadBandUpper>
+      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+    </joint>
+    <actuator name="hand_left_index_motor">
+      <mechanicalReduction>0.10288861079760754</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <!--Third flexor-->
+  <transmission name="hand_left_index_virtual_3_trans">
+    <type>hey5_transmissions/Hey5Reducer</type>
+    <joint name="hand_left_index_virtual_3_joint">
+      <saturationLower>0.0</saturationLower>
+      <deadBandUpper>0.7</deadBandUpper>
+      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+    </joint>
+    <actuator name="hand_left_index_motor">
+      <mechanicalReduction>0.10288861079760754</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="hand_left_index_flex_3_trans">
+    <type>hey5_transmissions/Hey5Reducer</type>
+    <joint name="hand_left_index_flex_3_joint">
+      <saturationLower>0.0</saturationLower>
+      <deadBandUpper>0.7</deadBandUpper>
+      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+    </joint>
+    <actuator name="hand_left_index_motor">
+      <mechanicalReduction>0.10288861079760754</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <!--Abductor-->
+  <transmission name="hand_left_middle_abd_trans">
+    <type>hey5_transmissions/Hey5Reducer</type>
+    <joint name="hand_left_middle_abd_joint">
+      <saturationLower>0.0</saturationLower>
+      <saturationUpper>0.0</saturationUpper>
+      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+    </joint>
+    <actuator name="hand_left_mrl_motor">
+      <mechanicalReduction>1.0</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <!--First flexor-->
+  <transmission name="hand_left_middle_virtual_1_trans">
+    <type>hey5_transmissions/Hey5Reducer</type>
+    <joint name="hand_left_middle_virtual_1_joint">
+      <saturationLower>0.0</saturationLower>
+      <deadBandUpper>0.0</deadBandUpper>
+      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+    </joint>
+    <actuator name="hand_left_mrl_motor">
+      <mechanicalReduction>0.06792452830188679</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="hand_left_middle_flex_1_trans">
+    <type>hey5_transmissions/Hey5Reducer</type>
+    <joint name="hand_left_middle_flex_1_joint">
+      <saturationLower>0.0</saturationLower>
+      <deadBandUpper>0.0</deadBandUpper>
+      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+    </joint>
+    <actuator name="hand_left_mrl_motor">
+      <mechanicalReduction>0.06792452830188679</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <!--Second flexor-->
+  <transmission name="hand_left_middle_virtual_2_trans">
+    <type>hey5_transmissions/Hey5Reducer</type>
+    <joint name="hand_left_middle_virtual_2_joint">
+      <saturationLower>0.0</saturationLower>
+      <deadBandUpper>0.0</deadBandUpper>
+      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+    </joint>
+    <actuator name="hand_left_mrl_motor">
+      <mechanicalReduction>0.06792452830188679</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="hand_left_middle_flex_2_trans">
+    <type>hey5_transmissions/Hey5Reducer</type>
+    <joint name="hand_left_middle_flex_2_joint">
+      <saturationLower>0.0</saturationLower>
+      <deadBandUpper>0.0</deadBandUpper>
+      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+    </joint>
+    <actuator name="hand_left_mrl_motor">
+      <mechanicalReduction>0.06792452830188679</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <!--Third flexor-->
+  <transmission name="hand_left_middle_virtual_3_trans">
+    <type>hey5_transmissions/Hey5Reducer</type>
+    <joint name="hand_left_middle_virtual_3_joint">
+      <saturationLower>0.0</saturationLower>
+      <deadBandUpper>0.0</deadBandUpper>
+      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+    </joint>
+    <actuator name="hand_left_mrl_motor">
+      <mechanicalReduction>0.06792452830188679</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="hand_left_middle_flex_3_trans">
+    <type>hey5_transmissions/Hey5Reducer</type>
+    <joint name="hand_left_middle_flex_3_joint">
+      <saturationLower>0.0</saturationLower>
+      <deadBandUpper>0.0</deadBandUpper>
+      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+    </joint>
+    <actuator name="hand_left_mrl_motor">
+      <mechanicalReduction>0.06792452830188679</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <!--Abductor-->
+  <transmission name="hand_left_ring_abd_trans">
+    <type>hey5_transmissions/Hey5Reducer</type>
+    <joint name="hand_left_ring_abd_joint">
+      <saturationLower>-0.2617993877991494</saturationLower>
+      <saturationUpper>0.0</saturationUpper>
+      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+    </joint>
+    <actuator name="hand_left_mrl_motor">
+      <mechanicalReduction>-0.23271056693257727</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <!--First flexor-->
+  <transmission name="hand_left_ring_virtual_1_trans">
+    <type>hey5_transmissions/Hey5Reducer</type>
+    <joint name="hand_left_ring_virtual_1_joint">
+      <saturationLower>0.0</saturationLower>
+      <deadBandUpper>0.75</deadBandUpper>
+      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+    </joint>
+    <actuator name="hand_left_mrl_motor">
+      <mechanicalReduction>0.07391769626507355</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="hand_left_ring_flex_1_trans">
+    <type>hey5_transmissions/Hey5Reducer</type>
+    <joint name="hand_left_ring_flex_1_joint">
+      <saturationLower>0.0</saturationLower>
+      <deadBandUpper>0.75</deadBandUpper>
+      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+    </joint>
+    <actuator name="hand_left_mrl_motor">
+      <mechanicalReduction>0.07391769626507355</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <!--Second flexor-->
+  <transmission name="hand_left_ring_virtual_2_trans">
+    <type>hey5_transmissions/Hey5Reducer</type>
+    <joint name="hand_left_ring_virtual_2_joint">
+      <saturationLower>0.0</saturationLower>
+      <deadBandUpper>0.75</deadBandUpper>
+      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+    </joint>
+    <actuator name="hand_left_mrl_motor">
+      <mechanicalReduction>0.07391769626507355</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="hand_left_ring_flex_2_trans">
+    <type>hey5_transmissions/Hey5Reducer</type>
+    <joint name="hand_left_ring_flex_2_joint">
+      <saturationLower>0.0</saturationLower>
+      <deadBandUpper>0.75</deadBandUpper>
+      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+    </joint>
+    <actuator name="hand_left_mrl_motor">
+      <mechanicalReduction>0.07391769626507355</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <!--Third flexor-->
+  <transmission name="hand_left_ring_virtual_3_trans">
+    <type>hey5_transmissions/Hey5Reducer</type>
+    <joint name="hand_left_ring_virtual_3_joint">
+      <saturationLower>0.0</saturationLower>
+      <deadBandUpper>0.75</deadBandUpper>
+      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+    </joint>
+    <actuator name="hand_left_mrl_motor">
+      <mechanicalReduction>0.07391769626507355</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="hand_left_ring_flex_3_trans">
+    <type>hey5_transmissions/Hey5Reducer</type>
+    <joint name="hand_left_ring_flex_3_joint">
+      <saturationLower>0.0</saturationLower>
+      <deadBandUpper>0.75</deadBandUpper>
+      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+    </joint>
+    <actuator name="hand_left_mrl_motor">
+      <mechanicalReduction>0.07391769626507355</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <!--Abductor-->
+  <transmission name="hand_left_little_abd_trans">
+    <type>hey5_transmissions/Hey5Reducer</type>
+    <joint name="hand_left_little_abd_joint">
+      <saturationLower>-0.5235987755982988</saturationLower>
+      <saturationUpper>0.0</saturationUpper>
+      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+    </joint>
+    <actuator name="hand_left_mrl_motor">
+      <mechanicalReduction>-0.46542113386515455</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <!--First flexor-->
+  <transmission name="hand_left_little_virtual_1_trans">
+    <type>hey5_transmissions/Hey5Reducer</type>
+    <joint name="hand_left_little_virtual_1_joint">
+      <saturationLower>0.0</saturationLower>
+      <deadBandUpper>0.75</deadBandUpper>
+      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+    </joint>
+    <actuator name="hand_left_mrl_motor">
+      <mechanicalReduction>0.07391769626507355</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="hand_left_little_flex_1_trans">
+    <type>hey5_transmissions/Hey5Reducer</type>
+    <joint name="hand_left_little_flex_1_joint">
+      <saturationLower>0.0</saturationLower>
+      <deadBandUpper>0.75</deadBandUpper>
+      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+    </joint>
+    <actuator name="hand_left_mrl_motor">
+      <mechanicalReduction>0.07391769626507355</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <!--Second flexor-->
+  <transmission name="hand_left_little_virtual_2_trans">
+    <type>hey5_transmissions/Hey5Reducer</type>
+    <joint name="hand_left_little_virtual_2_joint">
+      <saturationLower>0.0</saturationLower>
+      <deadBandUpper>0.75</deadBandUpper>
+      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+    </joint>
+    <actuator name="hand_left_mrl_motor">
+      <mechanicalReduction>0.07391769626507355</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="hand_left_little_flex_2_trans">
+    <type>hey5_transmissions/Hey5Reducer</type>
+    <joint name="hand_left_little_flex_2_joint">
+      <saturationLower>0.0</saturationLower>
+      <deadBandUpper>0.75</deadBandUpper>
+      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+    </joint>
+    <actuator name="hand_left_mrl_motor">
+      <mechanicalReduction>0.07391769626507355</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <!--Third flexor-->
+  <transmission name="hand_left_little_virtual_3_trans">
+    <type>hey5_transmissions/Hey5Reducer</type>
+    <joint name="hand_left_little_virtual_3_joint">
+      <saturationLower>0.0</saturationLower>
+      <deadBandUpper>0.75</deadBandUpper>
+      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+    </joint>
+    <actuator name="hand_left_mrl_motor">
+      <mechanicalReduction>0.07391769626507355</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="hand_left_little_flex_3_trans">
+    <type>hey5_transmissions/Hey5Reducer</type>
+    <joint name="hand_left_little_flex_3_joint">
+      <saturationLower>0.0</saturationLower>
+      <deadBandUpper>0.75</deadBandUpper>
+      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+    </joint>
+    <actuator name="hand_left_mrl_motor">
+      <mechanicalReduction>0.07391769626507355</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <link name="hand_left_grasping_frame"/>
+  <joint name="hand_left_grasping_fixed_joint" type="fixed">
+    <parent link="hand_left_palm_link"/>
+    <child link="hand_left_grasping_frame"/>
+    <axis xyz="0 0 1"/>
+    <origin rpy="0 0 0" xyz="0.13 0.02 0"/>
+  </joint>
+  <gazebo>
+    <plugin filename="libgazebo_underactuated_finger.so" name="gazebo_hand_left_thumb">
+      <actuatedJoint>hand_left_thumb_joint</actuatedJoint>
+      <virtualJoint>
+        <name>hand_left_thumb_abd_joint</name>
+        <scale_factor>2.0</scale_factor>
+      </virtualJoint>
+      <virtualJoint>
+        <name>hand_left_thumb_flex_1_joint</name>
+        <scale_factor>2.5</scale_factor>
+      </virtualJoint>
+      <virtualJoint>
+        <name>hand_left_thumb_virtual_1_joint</name>
+        <scale_factor>3.0</scale_factor>
+      </virtualJoint>
+      <virtualJoint>
+        <name>hand_left_thumb_flex_2_joint</name>
+        <scale_factor>3.5</scale_factor>
+      </virtualJoint>
+      <virtualJoint>
+        <name>hand_left_thumb_virtual_2_joint</name>
+        <scale_factor>3.0</scale_factor>
+      </virtualJoint>
+    </plugin>
+    <plugin filename="libgazebo_underactuated_finger.so" name="gazebo_hand_left_index">
+      <actuatedJoint>hand_left_index_joint</actuatedJoint>
+      <virtualJoint>
+        <name>hand_left_index_abd_joint</name>
+        <scale_factor>5.0</scale_factor>
+      </virtualJoint>
+      <virtualJoint>
+        <name>hand_left_index_flex_1_joint</name>
+        <scale_factor>2.5</scale_factor>
+      </virtualJoint>
+      <virtualJoint>
+        <name>hand_left_index_virtual_1_joint</name>
+        <scale_factor>3.0</scale_factor>
+      </virtualJoint>
+      <virtualJoint>
+        <name>hand_left_index_flex_2_joint</name>
+        <scale_factor>3.5</scale_factor>
+      </virtualJoint>
+      <virtualJoint>
+        <name>hand_left_index_virtual_2_joint</name>
+        <scale_factor>3.5</scale_factor>
+      </virtualJoint>
+      <virtualJoint>
+        <name>hand_left_index_flex_3_joint</name>
+        <scale_factor>4.0</scale_factor>
+      </virtualJoint>
+    </plugin>
+    <plugin filename="libgazebo_underactuated_finger.so" name="gazebo_hand_left_middle">
+      <actuatedJoint>hand_left_mrl_joint</actuatedJoint>
+      <virtualJoint>
+        <name>hand_left_middle_flex_1_joint</name>
+        <scale_factor>2.5</scale_factor>
+      </virtualJoint>
+      <virtualJoint>
+        <name>hand_left_middle_virtual_1_joint</name>
+        <scale_factor>3.0</scale_factor>
+      </virtualJoint>
+      <virtualJoint>
+        <name>hand_left_middle_flex_2_joint</name>
+        <scale_factor>3.5</scale_factor>
+      </virtualJoint>
+      <virtualJoint>
+        <name>hand_left_middle_virtual_2_joint</name>
+        <scale_factor>3.5</scale_factor>
+      </virtualJoint>
+      <virtualJoint>
+        <name>hand_left_middle_flex_3_joint</name>
+        <scale_factor>4.0</scale_factor>
+      </virtualJoint>
+    </plugin>
+    <plugin filename="libgazebo_underactuated_finger.so" name="gazebo_hand_left_ring">
+      <actuatedJoint>hand_left_mrl_joint</actuatedJoint>
+      <virtualJoint>
+        <name>hand_left_ring_flex_1_joint</name>
+        <scale_factor>2.5</scale_factor>
+      </virtualJoint>
+      <virtualJoint>
+        <name>hand_left_ring_virtual_1_joint</name>
+        <scale_factor>3.0</scale_factor>
+      </virtualJoint>
+      <virtualJoint>
+        <name>hand_left_ring_flex_2_joint</name>
+        <scale_factor>3.5</scale_factor>
+      </virtualJoint>
+      <virtualJoint>
+        <name>hand_left_ring_virtual_2_joint</name>
+        <scale_factor>3.5</scale_factor>
+      </virtualJoint>
+      <virtualJoint>
+        <name>hand_left_ring_flex_3_joint</name>
+        <scale_factor>4.0</scale_factor>
+      </virtualJoint>
+    </plugin>
+    <plugin filename="libgazebo_underactuated_finger.so" name="gazebo_hand_left_little">
+      <actuatedJoint>hand_left_mrl_joint</actuatedJoint>
+      <virtualJoint>
+        <name>hand_left_little_flex_1_joint</name>
+        <scale_factor>2.5</scale_factor>
+      </virtualJoint>
+      <virtualJoint>
+        <name>hand_left_little_virtual_1_joint</name>
+        <scale_factor>3.0</scale_factor>
+      </virtualJoint>
+      <virtualJoint>
+        <name>hand_left_little_flex_2_joint</name>
+        <scale_factor>3.5</scale_factor>
+      </virtualJoint>
+      <virtualJoint>
+        <name>hand_left_little_virtual_2_joint</name>
+        <scale_factor>3.5</scale_factor>
+      </virtualJoint>
+      <virtualJoint>
+        <name>hand_left_little_flex_3_joint</name>
+        <scale_factor>4.0</scale_factor>
+      </virtualJoint>
+    </plugin>
+  </gazebo>
+  <joint name="hand_left_safety_box_joint" type="fixed">
+    <parent link="wrist_left_ft_tool_link"/>
+    <child link="hand_left_safety_box"/>
+    <origin rpy="0 0 0" xyz="0.1 0.02 -0.02"/>
+  </joint>
+  <link name="hand_left_safety_box">
+    <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>
+    <!--
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <box size="0.2 0.115 0.18" />
+        </geometry>
+      </visual>
+      -->
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.2 0.115 0.18"/>
+      </geometry>
+    </collision>
+  </link>
+  <!--************************-->
+  <!--        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="package://tiago_description/meshes/arm/arm_1.stl" scale="1 -1 1"/>
+      </geometry>
+      <material name="White"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://tiago_description/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.5707963267948966" xyz="0.02556 -0.19 -0.171"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="43.0" lower="-1.1780972450961724" upper="1.5707963267948966" velocity="1.95"/>
+    <dynamics damping="1.0" friction="1.0"/>
+    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-1.1080972450961724" soft_upper_limit="1.5007963267948965"/>
+  </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="package://tiago_description/meshes/arm/arm_2.stl" 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="package://tiago_description/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.5707963267948966 0.0 0.0" xyz="0.125 -0.0195 -0.031"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="43.0" lower="-1.1780972450961724" upper="1.5707963267948966" velocity="1.95"/>
+    <dynamics damping="1.0" friction="1.0"/>
+    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-1.1080972450961724" soft_upper_limit="1.5007963267948965"/>
+  </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="package://tiago_description/meshes/arm/arm_3.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://tiago_description/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.5707963267948966 0.0 1.5707963267948966" xyz="0.0895 0.0 -0.0015"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="26" lower="-0.7853981633974483" upper="3.9269908169872414" velocity="2.35"/>
+    <dynamics damping="1.0" friction="1.0"/>
+    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-0.7153981633974482" soft_upper_limit="3.8569908169872416"/>
+  </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="package://tiago_description/meshes/arm/arm_4.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://tiago_description/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.5707963267948966 -1.5707963267948966 0.0" xyz="-0.02 -0.027 -0.222"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="26" lower="-0.39269908169872414" upper="2.356194490192345" velocity="2.35"/>
+    <dynamics damping="1.0" friction="1.0"/>
+    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-0.32269908169872413" soft_upper_limit="2.286194490192345"/>
+  </joint>
+  <gazebo reference="arm_right_1_link">
+    <mu1>0.9</mu1>
+    <mu2>0.9</mu2>
+    <material>Gazebo/DarkGrey</material>
+  </gazebo>
+  <gazebo reference="arm_right_2_link">
+    <mu1>0.9</mu1>
+    <mu2>0.9</mu2>
+    <material>Gazebo/DarkGrey</material>
+  </gazebo>
+  <gazebo reference="arm_right_3_link">
+    <mu1>0.9</mu1>
+    <mu2>0.9</mu2>
+    <material>Gazebo/DarkGrey</material>
+  </gazebo>
+  <gazebo reference="arm_right_4_link">
+    <mu1>0.9</mu1>
+    <mu2>0.9</mu2>
+    <material>Gazebo/DarkGrey</material>
+  </gazebo>
+  <gazebo reference="arm_right_1_joint">
+    <implicitSpringDamper>1</implicitSpringDamper>
+  </gazebo>
+  <gazebo reference="arm_right_2_joint">
+    <implicitSpringDamper>1</implicitSpringDamper>
+  </gazebo>
+  <gazebo reference="arm_right_3_joint">
+    <implicitSpringDamper>1</implicitSpringDamper>
+  </gazebo>
+  <gazebo reference="arm_right_4_joint">
+    <implicitSpringDamper>1</implicitSpringDamper>
+  </gazebo>
+  <!--************************-->
+  <!--        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="package://tiago_description/meshes/arm/arm_5-wrist-2010.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="LightGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://tiago_description/meshes/arm/arm_5_collision-wrist-2010.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.5707963267948966 -1.5707963267948966 -1.5707963267948966" xyz="-0.162 0.02 0.027"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="3" lower="-2.0943951023931953" upper="2.0943951023931953" velocity="1.95"/>
+    <dynamics damping="1.0" friction="1.0"/>
+    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-2.0743951023931952" soft_upper_limit="2.0743951023931952"/>
+  </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="package://tiago_description/meshes/arm/arm_6-wrist-2010.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="LightGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://tiago_description/meshes/arm/arm_6_collision-wrist-2010.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.5707963267948966 -1.5707963267948966" xyz="0 0 0.15"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="6.6" lower="-1.413716694115407" upper="1.413716694115407" velocity="1.76"/>
+    <dynamics damping="1.0" friction="1.0"/>
+    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-1.3937166941154069" soft_upper_limit="1.3937166941154069"/>
+  </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.5707963267948966 0.0 1.5707963267948966" xyz="0 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="6.6" lower="-2.0943951023931953" upper="2.0943951023931953" velocity="1.76"/>
+    <dynamics damping="1.0" friction="1.0"/>
+    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-2.0743951023931952" soft_upper_limit="2.0743951023931952"/>
+  </joint>
+  <gazebo reference="arm_right_5_link">
+    <mu1>0.9</mu1>
+    <mu2>0.9</mu2>
+  </gazebo>
+  <gazebo reference="arm_right_6_link">
+    <mu1>0.9</mu1>
+    <mu2>0.9</mu2>
+  </gazebo>
+  <gazebo reference="arm_right_7_link">
+    <mu1>0.9</mu1>
+    <mu2>0.9</mu2>
+  </gazebo>
+  <gazebo reference="arm_right_5_joint">
+    <implicitSpringDamper>1</implicitSpringDamper>
+  </gazebo>
+  <gazebo reference="arm_right_6_joint">
+    <implicitSpringDamper>1</implicitSpringDamper>
+  </gazebo>
+  <gazebo reference="arm_right_7_joint">
+    <implicitSpringDamper>1</implicitSpringDamper>
+    <provideFeedback>1</provideFeedback>
+  </gazebo>
+  <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</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</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</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="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 1.5707963267948966 0" xyz="0.001 0 0"/>
+      <geometry>
+        <cylinder length="0.005" radius="0.005"/>
+      </geometry>
+      <material name="LightGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 1.5707963267948966 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.5707963267948966 -1.5707963267948966 3.141592653589793" 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</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</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</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</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="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0"/>
+    </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.5707963267948966 0 1.5707963267948966" xyz="0.00785 0 0"/>
+  </joint>
+  <!--***********************-->
+  <!--       FT TOOL         -->
+  <!--***********************-->
+  <link name="wrist_right_ft_tool_link">
+    <inertial>
+      <mass value="0.1"/>
+      <inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 1.5707963267948966 0" xyz="0.0 0 0"/>
+      <geometry>
+        <cylinder length="0.00975" radius="0.025"/>
+      </geometry>
+      <material name="FlatBlack"/>
+    </visual>
+    <collision>
+      <origin rpy="0 1.5707963267948966 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.5707963267948966 -1.5707963267948966 0" xyz="0 0 0.012725"/>
+  </joint>
+  <!-- Tool Link -->
+  <link name="hand_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="hand_right_tool_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <parent link="wrist_right_ft_tool_link"/>
+    <child link="hand_right_tool_link"/>
+    <axis xyz="0 0 0"/>
+  </joint>
+  <joint name="hand_right_palm_joint" type="fixed">
+    <parent link="wrist_right_ft_tool_link"/>
+    <child link="hand_right_palm_link"/>
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+  </joint>
+  <link name="hand_right_palm_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0641 -0.0022 0.0021"/>
+      <!-- NOTE: Less than total palm mass of 0.598kg because actuator masses are specified separately -->
+      <mass value="0.4"/>
+      <inertia ixx="0.000305100" ixy="0.000005037" ixz="0.000015302" iyy="0.000811920" iyz="0.000007622" izz="0.000655851"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/palm.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/palm_collision.stl" scale="1 1 1"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="hand_right_palm_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <joint name="hand_right_thumb_joint" type="revolute">
+    <parent link="hand_right_palm_link"/>
+    <child link="hand_right_thumb_link"/>
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="-2.6179938779914944" upper="6.283185307179586" velocity="3.35"/>
+    <dynamics damping="0.5" friction="0.0"/>
+    <!-- TODO: Tune -->
+    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-2.6179938779914944" soft_upper_limit="6.283185307179586"/>
+  </joint>
+  <link name="hand_right_thumb_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <mass value="0.05"/>
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!--To stabilize simulation-->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <sphere radius="0.001"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <sphere radius="0.001"/>
+      </geometry>
+    </collision>
+  </link>
+  <transmission name="hand_right_thumb_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <actuator name="hand_right_thumb_motor">
+      <mechanicalReduction>1.0</mechanicalReduction>
+    </actuator>
+    <joint name="hand_right_thumb_joint">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+  </transmission>
+  <joint name="hand_right_index_joint" type="revolute">
+    <parent link="hand_right_palm_link"/>
+    <child link="hand_right_index_link"/>
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="-3.0543261909900767" upper="6.806784082777885" velocity="3.35"/>
+    <dynamics damping="0.5" friction="0.0"/>
+    <!-- TODO: Tune -->
+    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-3.0543261909900767" soft_upper_limit="6.806784082777885"/>
+  </joint>
+  <link name="hand_right_index_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <mass value="0.05"/>
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!--To stabilize simulation-->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <sphere radius="0.001"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <sphere radius="0.001"/>
+      </geometry>
+    </collision>
+  </link>
+  <transmission name="hand_right_index_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <actuator name="hand_right_index_motor">
+      <mechanicalReduction>1.0</mechanicalReduction>
+    </actuator>
+    <joint name="hand_right_index_joint">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+  </transmission>
+  <joint name="hand_right_mrl_joint" type="revolute">
+    <parent link="hand_right_palm_link"/>
+    <child link="hand_right_mrl_link"/>
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="-2.530727415391778" upper="9.250245035569947" velocity="3.56"/>
+    <dynamics damping="0.5" friction="0.0"/>
+    <!-- TODO: Tune -->
+    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-2.530727415391778" soft_upper_limit="9.250245035569947"/>
+  </joint>
+  <link name="hand_right_mrl_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <mass value="0.05"/>
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!--To stabilize simulation-->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <sphere radius="0.001"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <sphere radius="0.001"/>
+      </geometry>
+    </collision>
+  </link>
+  <transmission name="hand_right_mrl_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <actuator name="hand_right_mrl_motor">
+      <mechanicalReduction>1.0</mechanicalReduction>
+    </actuator>
+    <joint name="hand_right_mrl_joint">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+  </transmission>
+  <joint name="hand_right_thumb_abd_joint" type="revolute">
+    <parent link="hand_right_palm_link"/>
+    <child link="hand_right_thumb_abd_link"/>
+    <origin rpy="0 -1.1344640137963142 0" xyz="0.0901 0.0134 0.038"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="0.0" upper="1.5707963267948966" velocity="2.0"/>
+    <dynamics damping="0.5" friction="0.0"/>
+  </joint>
+  <link name="hand_right_thumb_abd_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="-0.0132 0.0037 -0.0017"/>
+      <mass value="0.014"/>
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!-- To stabilize simulation -->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/thumb_abd_collision.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/thumb_abd_collision.stl" scale="1 1 1"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="hand_right_thumb_abd_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <joint name="hand_right_thumb_virtual_1_joint" type="revolute">
+    <parent link="hand_right_thumb_abd_link"/>
+    <child link="hand_right_thumb_virtual_1_link"/>
+    <origin rpy="-1.1442378576074825 0 0" xyz="0.0225 0.003 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+    <dynamics damping="0.5" friction="0.0"/>
+  </joint>
+  <link name="hand_right_thumb_virtual_1_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+      <mass value="0.01"/>
+      <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!-- To stabilize simulation -->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+      <geometry>
+        <box size="0.013 0.002 0.012"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.013 0.002 0.012"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="hand_right_thumb_virtual_1_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <joint name="hand_right_thumb_flex_1_joint" type="revolute">
+    <parent link="hand_right_thumb_virtual_1_link"/>
+    <child link="hand_right_thumb_flex_1_link"/>
+    <origin rpy="0 0 0" xyz="0.013 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+    <dynamics damping="0.5" friction="0.0"/>
+  </joint>
+  <link name="hand_right_thumb_flex_1_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0047 0.0031 0.0"/>
+      <mass value="0.009"/>
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!-- To stabilize simulation -->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="hand_right_thumb_flex_1_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <joint name="hand_right_thumb_virtual_2_joint" type="revolute">
+    <parent link="hand_right_thumb_flex_1_link"/>
+    <child link="hand_right_thumb_virtual_2_link"/>
+    <origin rpy="0 0 0" xyz="0.009 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+    <dynamics damping="0.5" friction="0.0"/>
+  </joint>
+  <link name="hand_right_thumb_virtual_2_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+      <mass value="0.01"/>
+      <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!-- To stabilize simulation -->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+      <geometry>
+        <box size="0.013 0.002 0.012"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.013 0.002 0.012"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="hand_right_thumb_virtual_2_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <joint name="hand_right_thumb_flex_2_joint" type="revolute">
+    <parent link="hand_right_thumb_virtual_2_link"/>
+    <child link="hand_right_thumb_flex_2_link"/>
+    <origin rpy="0 0 0" xyz="0.013 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+    <dynamics damping="0.5" friction="0.0"/>
+  </joint>
+  <link name="hand_right_thumb_flex_2_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0096 0.003 0.0"/>
+      <mass value="0.015"/>
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!-- To stabilize simulation -->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_flex_tip_collision.stl"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_flex_tip_collision.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="hand_right_thumb_flex_2_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <joint name="hand_right_index_abd_joint" type="revolute">
+    <parent link="hand_right_palm_link"/>
+    <child link="hand_right_index_abd_link"/>
+    <origin rpy="-1.5707963267948966 -0.5235987755982988 0" xyz="0.1265 0.0042 0.0265"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="-0.5235987755982988" upper="0.5235987755982988" velocity="2.0"/>
+    <dynamics damping="0.5" friction="0.0"/>
+  </joint>
+  <link name="hand_right_index_abd_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="-0.0018 0.0 0.0028"/>
+      <mass value="0.018"/>
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!-- To stabilize simulation -->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_abd_collision.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_abd_collision.stl" scale="1 1 1"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="hand_right_index_abd_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <joint name="hand_right_index_virtual_1_joint" type="revolute">
+    <parent link="hand_right_index_abd_link"/>
+    <child link="hand_right_index_virtual_1_link"/>
+    <origin rpy="1.5707963267948966 0 0" xyz="0.009 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+    <dynamics damping="0.5" friction="0.0"/>
+  </joint>
+  <link name="hand_right_index_virtual_1_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+      <mass value="0.01"/>
+      <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!-- To stabilize simulation -->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+      <geometry>
+        <box size="0.013 0.002 0.012"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.013 0.002 0.012"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="hand_right_index_virtual_1_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <joint name="hand_right_index_flex_1_joint" type="revolute">
+    <parent link="hand_right_index_virtual_1_link"/>
+    <child link="hand_right_index_flex_1_link"/>
+    <origin rpy="0 0 0" xyz="0.013 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+    <dynamics damping="0.5" friction="0.0"/>
+  </joint>
+  <link name="hand_right_index_flex_1_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0047 0.0031 0.0"/>
+      <mass value="0.009"/>
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!-- To stabilize simulation -->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="hand_right_index_flex_1_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <joint name="hand_right_index_virtual_2_joint" type="revolute">
+    <parent link="hand_right_index_flex_1_link"/>
+    <child link="hand_right_index_virtual_2_link"/>
+    <origin rpy="0 0 0" xyz="0.009 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+    <dynamics damping="0.5" friction="0.0"/>
+  </joint>
+  <link name="hand_right_index_virtual_2_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+      <mass value="0.01"/>
+      <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!-- To stabilize simulation -->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+      <geometry>
+        <box size="0.013 0.002 0.012"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.013 0.002 0.012"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="hand_right_index_virtual_2_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <joint name="hand_right_index_flex_2_joint" type="revolute">
+    <parent link="hand_right_index_virtual_2_link"/>
+    <child link="hand_right_index_flex_2_link"/>
+    <origin rpy="0 0 0" xyz="0.013 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+    <dynamics damping="0.5" friction="0.0"/>
+  </joint>
+  <link name="hand_right_index_flex_2_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0047 0.0031 0.0"/>
+      <mass value="0.009"/>
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!-- To stabilize simulation -->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="hand_right_index_flex_2_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <joint name="hand_right_index_virtual_3_joint" type="revolute">
+    <parent link="hand_right_index_flex_2_link"/>
+    <child link="hand_right_index_virtual_3_link"/>
+    <origin rpy="0 0 0" xyz="0.009 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+    <dynamics damping="0.5" friction="0.0"/>
+  </joint>
+  <link name="hand_right_index_virtual_3_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+      <mass value="0.01"/>
+      <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!-- To stabilize simulation -->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+      <geometry>
+        <box size="0.013 0.002 0.012"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.013 0.002 0.012"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="hand_right_index_virtual_3_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <joint name="hand_right_index_flex_3_joint" type="revolute">
+    <parent link="hand_right_index_virtual_3_link"/>
+    <child link="hand_right_index_flex_3_link"/>
+    <origin rpy="0 0 0" xyz="0.013 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+    <dynamics damping="0.5" friction="0.0"/>
+  </joint>
+  <link name="hand_right_index_flex_3_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0096 0.003 0.0"/>
+      <mass value="0.015"/>
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!-- To stabilize simulation -->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_flex_tip_collision.stl"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_flex_tip_collision.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="hand_right_index_flex_3_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <joint name="hand_right_middle_abd_joint" type="revolute">
+    <parent link="hand_right_palm_link"/>
+    <child link="hand_right_middle_abd_link"/>
+    <origin rpy="-1.5707963267948966 0 0" xyz="0.1335 0.0042 0.007"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="-0.5235987755982988" upper="0.5235987755982988" velocity="2.0"/>
+    <dynamics damping="0.5" friction="0.0"/>
+  </joint>
+  <link name="hand_right_middle_abd_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="-0.0018 0.0 0.0028"/>
+      <mass value="0.018"/>
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!-- To stabilize simulation -->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_abd_collision.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_abd_collision.stl" scale="1 1 1"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="hand_right_middle_abd_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <joint name="hand_right_middle_virtual_1_joint" type="revolute">
+    <parent link="hand_right_middle_abd_link"/>
+    <child link="hand_right_middle_virtual_1_link"/>
+    <origin rpy="1.5707963267948966 0 0" xyz="0.009 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+    <dynamics damping="0.5" friction="0.0"/>
+  </joint>
+  <link name="hand_right_middle_virtual_1_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+      <mass value="0.01"/>
+      <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!-- To stabilize simulation -->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+      <geometry>
+        <box size="0.013 0.002 0.012"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.013 0.002 0.012"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="hand_right_middle_virtual_1_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <joint name="hand_right_middle_flex_1_joint" type="revolute">
+    <parent link="hand_right_middle_virtual_1_link"/>
+    <child link="hand_right_middle_flex_1_link"/>
+    <origin rpy="0 0 0" xyz="0.013 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+    <dynamics damping="0.5" friction="0.0"/>
+  </joint>
+  <link name="hand_right_middle_flex_1_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0047 0.0031 0.0"/>
+      <mass value="0.009"/>
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!-- To stabilize simulation -->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="hand_right_middle_flex_1_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <joint name="hand_right_middle_virtual_2_joint" type="revolute">
+    <parent link="hand_right_middle_flex_1_link"/>
+    <child link="hand_right_middle_virtual_2_link"/>
+    <origin rpy="0 0 0" xyz="0.009 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+    <dynamics damping="0.5" friction="0.0"/>
+  </joint>
+  <link name="hand_right_middle_virtual_2_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+      <mass value="0.01"/>
+      <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!-- To stabilize simulation -->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+      <geometry>
+        <box size="0.013 0.002 0.012"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.013 0.002 0.012"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="hand_right_middle_virtual_2_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <joint name="hand_right_middle_flex_2_joint" type="revolute">
+    <parent link="hand_right_middle_virtual_2_link"/>
+    <child link="hand_right_middle_flex_2_link"/>
+    <origin rpy="0 0 0" xyz="0.013 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+    <dynamics damping="0.5" friction="0.0"/>
+  </joint>
+  <link name="hand_right_middle_flex_2_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0047 0.0031 0.0"/>
+      <mass value="0.009"/>
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!-- To stabilize simulation -->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="hand_right_middle_flex_2_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <joint name="hand_right_middle_virtual_3_joint" type="revolute">
+    <parent link="hand_right_middle_flex_2_link"/>
+    <child link="hand_right_middle_virtual_3_link"/>
+    <origin rpy="0 0 0" xyz="0.009 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+    <dynamics damping="0.5" friction="0.0"/>
+  </joint>
+  <link name="hand_right_middle_virtual_3_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+      <mass value="0.01"/>
+      <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!-- To stabilize simulation -->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+      <geometry>
+        <box size="0.013 0.002 0.012"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.013 0.002 0.012"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="hand_right_middle_virtual_3_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <joint name="hand_right_middle_flex_3_joint" type="revolute">
+    <parent link="hand_right_middle_virtual_3_link"/>
+    <child link="hand_right_middle_flex_3_link"/>
+    <origin rpy="0 0 0" xyz="0.013 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+    <dynamics damping="0.5" friction="0.0"/>
+  </joint>
+  <link name="hand_right_middle_flex_3_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0096 0.003 0.0"/>
+      <mass value="0.015"/>
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!-- To stabilize simulation -->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_flex_tip_collision.stl"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_flex_tip_collision.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="hand_right_middle_flex_3_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <joint name="hand_right_ring_abd_joint" type="revolute">
+    <parent link="hand_right_palm_link"/>
+    <child link="hand_right_ring_abd_link"/>
+    <origin rpy="-1.5707963267948966 0.2617993877991494 0" xyz="0.1265 0.0042 -0.0125"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="-0.5235987755982988" upper="0.5235987755982988" velocity="2.0"/>
+    <dynamics damping="0.5" friction="0.0"/>
+  </joint>
+  <link name="hand_right_ring_abd_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="-0.0018 0.0 0.0028"/>
+      <mass value="0.018"/>
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!-- To stabilize simulation -->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_abd_collision.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_abd_collision.stl" scale="1 1 1"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="hand_right_ring_abd_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <joint name="hand_right_ring_virtual_1_joint" type="revolute">
+    <parent link="hand_right_ring_abd_link"/>
+    <child link="hand_right_ring_virtual_1_link"/>
+    <origin rpy="1.5707963267948966 0 0" xyz="0.009 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+    <dynamics damping="0.5" friction="0.0"/>
+  </joint>
+  <link name="hand_right_ring_virtual_1_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+      <mass value="0.01"/>
+      <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!-- To stabilize simulation -->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+      <geometry>
+        <box size="0.013 0.002 0.012"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.013 0.002 0.012"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="hand_right_ring_virtual_1_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <joint name="hand_right_ring_flex_1_joint" type="revolute">
+    <parent link="hand_right_ring_virtual_1_link"/>
+    <child link="hand_right_ring_flex_1_link"/>
+    <origin rpy="0 0 0" xyz="0.013 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+    <dynamics damping="0.5" friction="0.0"/>
+  </joint>
+  <link name="hand_right_ring_flex_1_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0047 0.0031 0.0"/>
+      <mass value="0.009"/>
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!-- To stabilize simulation -->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="hand_right_ring_flex_1_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <joint name="hand_right_ring_virtual_2_joint" type="revolute">
+    <parent link="hand_right_ring_flex_1_link"/>
+    <child link="hand_right_ring_virtual_2_link"/>
+    <origin rpy="0 0 0" xyz="0.009 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+    <dynamics damping="0.5" friction="0.0"/>
+  </joint>
+  <link name="hand_right_ring_virtual_2_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+      <mass value="0.01"/>
+      <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!-- To stabilize simulation -->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+      <geometry>
+        <box size="0.013 0.002 0.012"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.013 0.002 0.012"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="hand_right_ring_virtual_2_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <joint name="hand_right_ring_flex_2_joint" type="revolute">
+    <parent link="hand_right_ring_virtual_2_link"/>
+    <child link="hand_right_ring_flex_2_link"/>
+    <origin rpy="0 0 0" xyz="0.013 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+    <dynamics damping="0.5" friction="0.0"/>
+  </joint>
+  <link name="hand_right_ring_flex_2_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0047 0.0031 0.0"/>
+      <mass value="0.009"/>
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!-- To stabilize simulation -->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="hand_right_ring_flex_2_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <joint name="hand_right_ring_virtual_3_joint" type="revolute">
+    <parent link="hand_right_ring_flex_2_link"/>
+    <child link="hand_right_ring_virtual_3_link"/>
+    <origin rpy="0 0 0" xyz="0.009 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+    <dynamics damping="0.5" friction="0.0"/>
+  </joint>
+  <link name="hand_right_ring_virtual_3_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+      <mass value="0.01"/>
+      <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!-- To stabilize simulation -->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+      <geometry>
+        <box size="0.013 0.002 0.012"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.013 0.002 0.012"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="hand_right_ring_virtual_3_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <joint name="hand_right_ring_flex_3_joint" type="revolute">
+    <parent link="hand_right_ring_virtual_3_link"/>
+    <child link="hand_right_ring_flex_3_link"/>
+    <origin rpy="0 0 0" xyz="0.013 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+    <dynamics damping="0.5" friction="0.0"/>
+  </joint>
+  <link name="hand_right_ring_flex_3_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0096 0.003 0.0"/>
+      <mass value="0.015"/>
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!-- To stabilize simulation -->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_flex_tip_collision.stl"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_flex_tip_collision.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="hand_right_ring_flex_3_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <joint name="hand_right_little_abd_joint" type="revolute">
+    <parent link="hand_right_palm_link"/>
+    <child link="hand_right_little_abd_link"/>
+    <origin rpy="-1.5707963267948966 0.5235987755982988 0" xyz="0.1165 0.0042 -0.032"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="-0.5235987755982988" upper="0.5235987755982988" velocity="2.0"/>
+    <dynamics damping="0.5" friction="0.0"/>
+  </joint>
+  <link name="hand_right_little_abd_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="-0.0018 0.0 0.0028"/>
+      <mass value="0.018"/>
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!-- To stabilize simulation -->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_abd_collision.stl" scale="1 1 1"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_abd_collision.stl" scale="1 1 1"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="hand_right_little_abd_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <joint name="hand_right_little_virtual_1_joint" type="revolute">
+    <parent link="hand_right_little_abd_link"/>
+    <child link="hand_right_little_virtual_1_link"/>
+    <origin rpy="1.5707963267948966 0 0" xyz="0.009 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+    <dynamics damping="0.5" friction="0.0"/>
+  </joint>
+  <link name="hand_right_little_virtual_1_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+      <mass value="0.01"/>
+      <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!-- To stabilize simulation -->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+      <geometry>
+        <box size="0.013 0.002 0.012"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.013 0.002 0.012"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="hand_right_little_virtual_1_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <joint name="hand_right_little_flex_1_joint" type="revolute">
+    <parent link="hand_right_little_virtual_1_link"/>
+    <child link="hand_right_little_flex_1_link"/>
+    <origin rpy="0 0 0" xyz="0.013 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+    <dynamics damping="0.5" friction="0.0"/>
+  </joint>
+  <link name="hand_right_little_flex_1_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0047 0.0031 0.0"/>
+      <mass value="0.009"/>
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!-- To stabilize simulation -->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="hand_right_little_flex_1_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <joint name="hand_right_little_virtual_2_joint" type="revolute">
+    <parent link="hand_right_little_flex_1_link"/>
+    <child link="hand_right_little_virtual_2_link"/>
+    <origin rpy="0 0 0" xyz="0.009 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+    <dynamics damping="0.5" friction="0.0"/>
+  </joint>
+  <link name="hand_right_little_virtual_2_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+      <mass value="0.01"/>
+      <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!-- To stabilize simulation -->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+      <geometry>
+        <box size="0.013 0.002 0.012"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.013 0.002 0.012"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="hand_right_little_virtual_2_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <joint name="hand_right_little_flex_2_joint" type="revolute">
+    <parent link="hand_right_little_virtual_2_link"/>
+    <child link="hand_right_little_flex_2_link"/>
+    <origin rpy="0 0 0" xyz="0.013 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+    <dynamics damping="0.5" friction="0.0"/>
+  </joint>
+  <link name="hand_right_little_flex_2_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0047 0.0031 0.0"/>
+      <mass value="0.009"/>
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!-- To stabilize simulation -->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="hand_right_little_flex_2_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <joint name="hand_right_little_virtual_3_joint" type="revolute">
+    <parent link="hand_right_little_flex_2_link"/>
+    <child link="hand_right_little_virtual_3_link"/>
+    <origin rpy="0 0 0" xyz="0.009 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+    <dynamics damping="0.5" friction="0.0"/>
+  </joint>
+  <link name="hand_right_little_virtual_3_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+      <mass value="0.01"/>
+      <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!-- To stabilize simulation -->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
+      <geometry>
+        <box size="0.013 0.002 0.012"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.013 0.002 0.012"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="hand_right_little_virtual_3_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <joint name="hand_right_little_flex_3_joint" type="revolute">
+    <parent link="hand_right_little_virtual_3_link"/>
+    <child link="hand_right_little_flex_3_link"/>
+    <origin rpy="0 0 0" xyz="0.013 0 0"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="2.0" lower="0.0" upper="0.7853981633974483" velocity="2.0"/>
+    <dynamics damping="0.5" friction="0.0"/>
+  </joint>
+  <link name="hand_right_little_flex_3_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.0096 0.003 0.0"/>
+      <mass value="0.015"/>
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+      <!-- To stabilize simulation -->
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_flex_tip_collision.stl"/>
+      </geometry>
+      <material name="Hey5DarkGrey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hey5_description/meshes/finger_flex_tip_collision.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="hand_right_little_flex_3_link">
+    <material>Gazebo/Black</material>
+  </gazebo>
+  <!--Abductor-->
+  <transmission name="hand_right_thumb_abd_trans">
+    <type>hey5_transmissions/Hey5Reducer</type>
+    <joint name="hand_right_thumb_abd_joint">
+      <saturationLower>0.0</saturationLower>
+      <saturationUpper>1.5707963267948966</saturationUpper>
+      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+    </joint>
+    <actuator name="hand_right_thumb_motor">
+      <mechanicalReduction>0.6544984694978736</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <!--First flexor-->
+  <transmission name="hand_right_thumb_virtual_1_trans">
+    <type>hey5_transmissions/Hey5Reducer</type>
+    <joint name="hand_right_thumb_virtual_1_joint">
+      <saturationLower>0.0</saturationLower>
+      <deadBandUpper>2.0</deadBandUpper>
+      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+    </joint>
+    <actuator name="hand_right_thumb_motor">
+      <mechanicalReduction>0.16299357854713997</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="hand_right_thumb_flex_1_trans">
+    <type>hey5_transmissions/Hey5Reducer</type>
+    <joint name="hand_right_thumb_flex_1_joint">
+      <saturationLower>0.0</saturationLower>
+      <deadBandUpper>2.0</deadBandUpper>
+      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+    </joint>
+    <actuator name="hand_right_thumb_motor">
+      <mechanicalReduction>0.16299357854713997</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <!--Second flexor-->
+  <transmission name="hand_right_thumb_virtual_2_trans">
+    <type>hey5_transmissions/Hey5Reducer</type>
+    <joint name="hand_right_thumb_virtual_2_joint">
+      <saturationLower>0.0</saturationLower>
+      <deadBandUpper>2.0</deadBandUpper>
+      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+    </joint>
+    <actuator name="hand_right_thumb_motor">
+      <mechanicalReduction>0.16299357854713997</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="hand_right_thumb_flex_2_trans">
+    <type>hey5_transmissions/Hey5Reducer</type>
+    <joint name="hand_right_thumb_flex_2_joint">
+      <saturationLower>0.0</saturationLower>
+      <deadBandUpper>2.0</deadBandUpper>
+      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+    </joint>
+    <actuator name="hand_right_thumb_motor">
+      <mechanicalReduction>0.16299357854713997</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <!--Abductor-->
+  <transmission name="hand_right_index_abd_trans">
+    <type>hey5_transmissions/Hey5Reducer</type>
+    <joint name="hand_right_index_abd_joint">
+      <saturationLower>0.0</saturationLower>
+      <saturationUpper>0.5235987755982988</saturationUpper>
+      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+    </joint>
+    <actuator name="hand_right_index_motor">
+      <mechanicalReduction>0.49866550056980846</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <!--First flexor-->
+  <transmission name="hand_right_index_virtual_1_trans">
+    <type>hey5_transmissions/Hey5Reducer</type>
+    <joint name="hand_right_index_virtual_1_joint">
+      <saturationLower>0.0</saturationLower>
+      <deadBandUpper>0.7</deadBandUpper>
+      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+    </joint>
+    <actuator name="hand_right_index_motor">
+      <mechanicalReduction>0.10288861079760754</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="hand_right_index_flex_1_trans">
+    <type>hey5_transmissions/Hey5Reducer</type>
+    <joint name="hand_right_index_flex_1_joint">
+      <saturationLower>0.0</saturationLower>
+      <deadBandUpper>0.7</deadBandUpper>
+      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+    </joint>
+    <actuator name="hand_right_index_motor">
+      <mechanicalReduction>0.10288861079760754</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <!--Second flexor-->
+  <transmission name="hand_right_index_virtual_2_trans">
+    <type>hey5_transmissions/Hey5Reducer</type>
+    <joint name="hand_right_index_virtual_2_joint">
+      <saturationLower>0.0</saturationLower>
+      <deadBandUpper>0.7</deadBandUpper>
+      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+    </joint>
+    <actuator name="hand_right_index_motor">
+      <mechanicalReduction>0.10288861079760754</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="hand_right_index_flex_2_trans">
+    <type>hey5_transmissions/Hey5Reducer</type>
+    <joint name="hand_right_index_flex_2_joint">
+      <saturationLower>0.0</saturationLower>
+      <deadBandUpper>0.7</deadBandUpper>
+      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+    </joint>
+    <actuator name="hand_right_index_motor">
+      <mechanicalReduction>0.10288861079760754</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <!--Third flexor-->
+  <transmission name="hand_right_index_virtual_3_trans">
+    <type>hey5_transmissions/Hey5Reducer</type>
+    <joint name="hand_right_index_virtual_3_joint">
+      <saturationLower>0.0</saturationLower>
+      <deadBandUpper>0.7</deadBandUpper>
+      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+    </joint>
+    <actuator name="hand_right_index_motor">
+      <mechanicalReduction>0.10288861079760754</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="hand_right_index_flex_3_trans">
+    <type>hey5_transmissions/Hey5Reducer</type>
+    <joint name="hand_right_index_flex_3_joint">
+      <saturationLower>0.0</saturationLower>
+      <deadBandUpper>0.7</deadBandUpper>
+      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+    </joint>
+    <actuator name="hand_right_index_motor">
+      <mechanicalReduction>0.10288861079760754</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <!--Abductor-->
+  <transmission name="hand_right_middle_abd_trans">
+    <type>hey5_transmissions/Hey5Reducer</type>
+    <joint name="hand_right_middle_abd_joint">
+      <saturationLower>0.0</saturationLower>
+      <saturationUpper>0.0</saturationUpper>
+      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+    </joint>
+    <actuator name="hand_right_mrl_motor">
+      <mechanicalReduction>1.0</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <!--First flexor-->
+  <transmission name="hand_right_middle_virtual_1_trans">
+    <type>hey5_transmissions/Hey5Reducer</type>
+    <joint name="hand_right_middle_virtual_1_joint">
+      <saturationLower>0.0</saturationLower>
+      <deadBandUpper>0.0</deadBandUpper>
+      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+    </joint>
+    <actuator name="hand_right_mrl_motor">
+      <mechanicalReduction>0.06792452830188679</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="hand_right_middle_flex_1_trans">
+    <type>hey5_transmissions/Hey5Reducer</type>
+    <joint name="hand_right_middle_flex_1_joint">
+      <saturationLower>0.0</saturationLower>
+      <deadBandUpper>0.0</deadBandUpper>
+      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+    </joint>
+    <actuator name="hand_right_mrl_motor">
+      <mechanicalReduction>0.06792452830188679</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <!--Second flexor-->
+  <transmission name="hand_right_middle_virtual_2_trans">
+    <type>hey5_transmissions/Hey5Reducer</type>
+    <joint name="hand_right_middle_virtual_2_joint">
+      <saturationLower>0.0</saturationLower>
+      <deadBandUpper>0.0</deadBandUpper>
+      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+    </joint>
+    <actuator name="hand_right_mrl_motor">
+      <mechanicalReduction>0.06792452830188679</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="hand_right_middle_flex_2_trans">
+    <type>hey5_transmissions/Hey5Reducer</type>
+    <joint name="hand_right_middle_flex_2_joint">
+      <saturationLower>0.0</saturationLower>
+      <deadBandUpper>0.0</deadBandUpper>
+      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+    </joint>
+    <actuator name="hand_right_mrl_motor">
+      <mechanicalReduction>0.06792452830188679</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <!--Third flexor-->
+  <transmission name="hand_right_middle_virtual_3_trans">
+    <type>hey5_transmissions/Hey5Reducer</type>
+    <joint name="hand_right_middle_virtual_3_joint">
+      <saturationLower>0.0</saturationLower>
+      <deadBandUpper>0.0</deadBandUpper>
+      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+    </joint>
+    <actuator name="hand_right_mrl_motor">
+      <mechanicalReduction>0.06792452830188679</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="hand_right_middle_flex_3_trans">
+    <type>hey5_transmissions/Hey5Reducer</type>
+    <joint name="hand_right_middle_flex_3_joint">
+      <saturationLower>0.0</saturationLower>
+      <deadBandUpper>0.0</deadBandUpper>
+      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+    </joint>
+    <actuator name="hand_right_mrl_motor">
+      <mechanicalReduction>0.06792452830188679</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <!--Abductor-->
+  <transmission name="hand_right_ring_abd_trans">
+    <type>hey5_transmissions/Hey5Reducer</type>
+    <joint name="hand_right_ring_abd_joint">
+      <saturationLower>-0.2617993877991494</saturationLower>
+      <saturationUpper>0.0</saturationUpper>
+      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+    </joint>
+    <actuator name="hand_right_mrl_motor">
+      <mechanicalReduction>-0.23271056693257727</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <!--First flexor-->
+  <transmission name="hand_right_ring_virtual_1_trans">
+    <type>hey5_transmissions/Hey5Reducer</type>
+    <joint name="hand_right_ring_virtual_1_joint">
+      <saturationLower>0.0</saturationLower>
+      <deadBandUpper>0.75</deadBandUpper>
+      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+    </joint>
+    <actuator name="hand_right_mrl_motor">
+      <mechanicalReduction>0.07391769626507355</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="hand_right_ring_flex_1_trans">
+    <type>hey5_transmissions/Hey5Reducer</type>
+    <joint name="hand_right_ring_flex_1_joint">
+      <saturationLower>0.0</saturationLower>
+      <deadBandUpper>0.75</deadBandUpper>
+      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+    </joint>
+    <actuator name="hand_right_mrl_motor">
+      <mechanicalReduction>0.07391769626507355</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <!--Second flexor-->
+  <transmission name="hand_right_ring_virtual_2_trans">
+    <type>hey5_transmissions/Hey5Reducer</type>
+    <joint name="hand_right_ring_virtual_2_joint">
+      <saturationLower>0.0</saturationLower>
+      <deadBandUpper>0.75</deadBandUpper>
+      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+    </joint>
+    <actuator name="hand_right_mrl_motor">
+      <mechanicalReduction>0.07391769626507355</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="hand_right_ring_flex_2_trans">
+    <type>hey5_transmissions/Hey5Reducer</type>
+    <joint name="hand_right_ring_flex_2_joint">
+      <saturationLower>0.0</saturationLower>
+      <deadBandUpper>0.75</deadBandUpper>
+      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+    </joint>
+    <actuator name="hand_right_mrl_motor">
+      <mechanicalReduction>0.07391769626507355</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <!--Third flexor-->
+  <transmission name="hand_right_ring_virtual_3_trans">
+    <type>hey5_transmissions/Hey5Reducer</type>
+    <joint name="hand_right_ring_virtual_3_joint">
+      <saturationLower>0.0</saturationLower>
+      <deadBandUpper>0.75</deadBandUpper>
+      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+    </joint>
+    <actuator name="hand_right_mrl_motor">
+      <mechanicalReduction>0.07391769626507355</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="hand_right_ring_flex_3_trans">
+    <type>hey5_transmissions/Hey5Reducer</type>
+    <joint name="hand_right_ring_flex_3_joint">
+      <saturationLower>0.0</saturationLower>
+      <deadBandUpper>0.75</deadBandUpper>
+      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+    </joint>
+    <actuator name="hand_right_mrl_motor">
+      <mechanicalReduction>0.07391769626507355</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <!--Abductor-->
+  <transmission name="hand_right_little_abd_trans">
+    <type>hey5_transmissions/Hey5Reducer</type>
+    <joint name="hand_right_little_abd_joint">
+      <saturationLower>-0.5235987755982988</saturationLower>
+      <saturationUpper>0.0</saturationUpper>
+      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+    </joint>
+    <actuator name="hand_right_mrl_motor">
+      <mechanicalReduction>-0.46542113386515455</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <!--First flexor-->
+  <transmission name="hand_right_little_virtual_1_trans">
+    <type>hey5_transmissions/Hey5Reducer</type>
+    <joint name="hand_right_little_virtual_1_joint">
+      <saturationLower>0.0</saturationLower>
+      <deadBandUpper>0.75</deadBandUpper>
+      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+    </joint>
+    <actuator name="hand_right_mrl_motor">
+      <mechanicalReduction>0.07391769626507355</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="hand_right_little_flex_1_trans">
+    <type>hey5_transmissions/Hey5Reducer</type>
+    <joint name="hand_right_little_flex_1_joint">
+      <saturationLower>0.0</saturationLower>
+      <deadBandUpper>0.75</deadBandUpper>
+      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+    </joint>
+    <actuator name="hand_right_mrl_motor">
+      <mechanicalReduction>0.07391769626507355</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <!--Second flexor-->
+  <transmission name="hand_right_little_virtual_2_trans">
+    <type>hey5_transmissions/Hey5Reducer</type>
+    <joint name="hand_right_little_virtual_2_joint">
+      <saturationLower>0.0</saturationLower>
+      <deadBandUpper>0.75</deadBandUpper>
+      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+    </joint>
+    <actuator name="hand_right_mrl_motor">
+      <mechanicalReduction>0.07391769626507355</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="hand_right_little_flex_2_trans">
+    <type>hey5_transmissions/Hey5Reducer</type>
+    <joint name="hand_right_little_flex_2_joint">
+      <saturationLower>0.0</saturationLower>
+      <deadBandUpper>0.75</deadBandUpper>
+      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+    </joint>
+    <actuator name="hand_right_mrl_motor">
+      <mechanicalReduction>0.07391769626507355</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <!--Third flexor-->
+  <transmission name="hand_right_little_virtual_3_trans">
+    <type>hey5_transmissions/Hey5Reducer</type>
+    <joint name="hand_right_little_virtual_3_joint">
+      <saturationLower>0.0</saturationLower>
+      <deadBandUpper>0.75</deadBandUpper>
+      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+    </joint>
+    <actuator name="hand_right_mrl_motor">
+      <mechanicalReduction>0.07391769626507355</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <transmission name="hand_right_little_flex_3_trans">
+    <type>hey5_transmissions/Hey5Reducer</type>
+    <joint name="hand_right_little_flex_3_joint">
+      <saturationLower>0.0</saturationLower>
+      <deadBandUpper>0.75</deadBandUpper>
+      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
+    </joint>
+    <actuator name="hand_right_mrl_motor">
+      <mechanicalReduction>0.07391769626507355</mechanicalReduction>
+    </actuator>
+  </transmission>
+  <link name="hand_right_grasping_frame"/>
+  <joint name="hand_right_grasping_fixed_joint" type="fixed">
+    <parent link="hand_right_palm_link"/>
+    <child link="hand_right_grasping_frame"/>
+    <axis xyz="0 0 1"/>
+    <origin rpy="0 0 0" xyz="0.13 0.02 0"/>
+  </joint>
+  <gazebo>
+    <plugin filename="libgazebo_underactuated_finger.so" name="gazebo_hand_right_thumb">
+      <actuatedJoint>hand_right_thumb_joint</actuatedJoint>
+      <virtualJoint>
+        <name>hand_right_thumb_abd_joint</name>
+        <scale_factor>2.0</scale_factor>
+      </virtualJoint>
+      <virtualJoint>
+        <name>hand_right_thumb_flex_1_joint</name>
+        <scale_factor>2.5</scale_factor>
+      </virtualJoint>
+      <virtualJoint>
+        <name>hand_right_thumb_virtual_1_joint</name>
+        <scale_factor>3.0</scale_factor>
+      </virtualJoint>
+      <virtualJoint>
+        <name>hand_right_thumb_flex_2_joint</name>
+        <scale_factor>3.5</scale_factor>
+      </virtualJoint>
+      <virtualJoint>
+        <name>hand_right_thumb_virtual_2_joint</name>
+        <scale_factor>3.0</scale_factor>
+      </virtualJoint>
+    </plugin>
+    <plugin filename="libgazebo_underactuated_finger.so" name="gazebo_hand_right_index">
+      <actuatedJoint>hand_right_index_joint</actuatedJoint>
+      <virtualJoint>
+        <name>hand_right_index_abd_joint</name>
+        <scale_factor>5.0</scale_factor>
+      </virtualJoint>
+      <virtualJoint>
+        <name>hand_right_index_flex_1_joint</name>
+        <scale_factor>2.5</scale_factor>
+      </virtualJoint>
+      <virtualJoint>
+        <name>hand_right_index_virtual_1_joint</name>
+        <scale_factor>3.0</scale_factor>
+      </virtualJoint>
+      <virtualJoint>
+        <name>hand_right_index_flex_2_joint</name>
+        <scale_factor>3.5</scale_factor>
+      </virtualJoint>
+      <virtualJoint>
+        <name>hand_right_index_virtual_2_joint</name>
+        <scale_factor>3.5</scale_factor>
+      </virtualJoint>
+      <virtualJoint>
+        <name>hand_right_index_flex_3_joint</name>
+        <scale_factor>4.0</scale_factor>
+      </virtualJoint>
+    </plugin>
+    <plugin filename="libgazebo_underactuated_finger.so" name="gazebo_hand_right_middle">
+      <actuatedJoint>hand_right_mrl_joint</actuatedJoint>
+      <virtualJoint>
+        <name>hand_right_middle_flex_1_joint</name>
+        <scale_factor>2.5</scale_factor>
+      </virtualJoint>
+      <virtualJoint>
+        <name>hand_right_middle_virtual_1_joint</name>
+        <scale_factor>3.0</scale_factor>
+      </virtualJoint>
+      <virtualJoint>
+        <name>hand_right_middle_flex_2_joint</name>
+        <scale_factor>3.5</scale_factor>
+      </virtualJoint>
+      <virtualJoint>
+        <name>hand_right_middle_virtual_2_joint</name>
+        <scale_factor>3.5</scale_factor>
+      </virtualJoint>
+      <virtualJoint>
+        <name>hand_right_middle_flex_3_joint</name>
+        <scale_factor>4.0</scale_factor>
+      </virtualJoint>
+    </plugin>
+    <plugin filename="libgazebo_underactuated_finger.so" name="gazebo_hand_right_ring">
+      <actuatedJoint>hand_right_mrl_joint</actuatedJoint>
+      <virtualJoint>
+        <name>hand_right_ring_flex_1_joint</name>
+        <scale_factor>2.5</scale_factor>
+      </virtualJoint>
+      <virtualJoint>
+        <name>hand_right_ring_virtual_1_joint</name>
+        <scale_factor>3.0</scale_factor>
+      </virtualJoint>
+      <virtualJoint>
+        <name>hand_right_ring_flex_2_joint</name>
+        <scale_factor>3.5</scale_factor>
+      </virtualJoint>
+      <virtualJoint>
+        <name>hand_right_ring_virtual_2_joint</name>
+        <scale_factor>3.5</scale_factor>
+      </virtualJoint>
+      <virtualJoint>
+        <name>hand_right_ring_flex_3_joint</name>
+        <scale_factor>4.0</scale_factor>
+      </virtualJoint>
+    </plugin>
+    <plugin filename="libgazebo_underactuated_finger.so" name="gazebo_hand_right_little">
+      <actuatedJoint>hand_right_mrl_joint</actuatedJoint>
+      <virtualJoint>
+        <name>hand_right_little_flex_1_joint</name>
+        <scale_factor>2.5</scale_factor>
+      </virtualJoint>
+      <virtualJoint>
+        <name>hand_right_little_virtual_1_joint</name>
+        <scale_factor>3.0</scale_factor>
+      </virtualJoint>
+      <virtualJoint>
+        <name>hand_right_little_flex_2_joint</name>
+        <scale_factor>3.5</scale_factor>
+      </virtualJoint>
+      <virtualJoint>
+        <name>hand_right_little_virtual_2_joint</name>
+        <scale_factor>3.5</scale_factor>
+      </virtualJoint>
+      <virtualJoint>
+        <name>hand_right_little_flex_3_joint</name>
+        <scale_factor>4.0</scale_factor>
+      </virtualJoint>
+    </plugin>
+  </gazebo>
+  <joint name="hand_right_safety_box_joint" type="fixed">
+    <parent link="wrist_right_ft_tool_link"/>
+    <child link="hand_right_safety_box"/>
+    <origin rpy="0 0 0" xyz="0.1 0.02 0.02"/>
+  </joint>
+  <link name="hand_right_safety_box">
+    <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>
+    <!--
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <box size="0.2 0.115 0.18" />
+        </geometry>
+      </visual>
+      -->
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.2 0.115 0.18"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- 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>
+</robot>
+
-- 
GitLab