<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- |    This document was autogenerated by xacro from tiago_pro.urdf.xacro           | -->
<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
<!-- =================================================================================== -->
<!--

  Copyright (c) 2024 PAL Robotics S.L. All rights reserved.
  Licensed under the Apache License, Version 2.0 (the "License");
  you may not use this file except in compliance with the License.
  You may obtain a copy of the License at

      http://www.apache.org/licenses/LICENSE-2.0

  Unless required by applicable law or agreed to in writing, software
  distributed under the License is distributed on an "AS IS" BASIS,
  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  See the License for the specific language governing permissions and
  limitations under the License.
-->
<robot name="tiago_pro">
  <!-- ARGUMENTS -->
  <!-- PROPERTIES -->
  <!-- Compatibility check within TC and ft sensors-->
  <!-- <xacro:if value="${ft_sensor_right == 'ati' and wrist_model_right=='straight-wrist'}">
    <xacro:error_ati_not_compatible_with_straight_wrist />
  </xacro:if>
  <xacro:if value="${ft_sensor_left == 'ati' and wrist_model_left=='straight-wrist'}">
    <xacro:error_ati_not_compatible_with_straight_wrist />
  </xacro:if>
  <xacro:if value="${ft_sensor_right == 'rokubi' and wrist_model_right=='spherical-wrist'}">
    <xacro:error_rokubi_not_compatible_with_spherical_wrist />
  </xacro:if>
  <xacro:if value="${ft_sensor_left == 'rokubi' and wrist_model_left=='spherical-wrist'}">
    <xacro:error_rokubi_not_compatible_with_spherical_wrist />
  </xacro:if>
  <xacro:if value="${ft_sensor_right == 'rokubi' and tool_changer_right}">
    <xacro:error_rokubi_not_compatible_with_tc />
  </xacro:if>
  <xacro:if value="${ft_sensor_left == 'rokubi' and tool_changer_left}">
    <xacro:error_rokubi_not_compatible_with_tc />
  </xacro:if>
  <xacro:if value="${not tool_changer_right and wrist_model_right=='spherical-wrist'}">
    <xacro:error_spherical_wrist_always_comes_with_tc />
  </xacro:if>
  <xacro:if value="${not tool_changer_left and wrist_model_left=='spherical-wrist'}">
    <xacro:error_spherical_wrist_always_comes_with_tc />
  </xacro:if>
  <xacro:if value="${not tool_changer_left and ft_sensor_left == 'ati'}">
    <xacro:error_ati_always_comes_with_tc />
  </xacro:if>
  <xacro:if value="${not tool_changer_right and ft_sensor_right == 'ati'}">
    <xacro:error_ati_always_comes_with_tc />
  </xacro:if> -->
  <!-- INCLUDES -->
  <!-- PROPERTIES -->
  <!--Constant parameters-->
  <material name="aluminum">
    <color rgba="0.5 0.5 0.5 1"/>
  </material>
  <material name="plastic">
    <color rgba="0.1 0.1 0.1 1"/>
  </material>
  <!-- EYE FRAMES -->
  <link name="sellion_link"/>
  <joint name="sellion_joint" type="fixed">
    <origin rpy="-1.5707963267948966 0 0" xyz="0.04 0.09205 0"/>
    <child link="sellion_link"/>
    <parent link="head_2_link"/>
  </joint>
  <!-- <link name="eye_right_link"/>

  <joint name="eye_right_joint" type="fixed">
    <origin xyz="0.0 -0.1 0" rpy="0 0 0"/>
    <child link="eye_right_link" />
    <parent link="sellion_link"/>
  </joint>

  <link name="eye_left_link"/>

  <joint name="eye_left_joint" type="fixed">
    <origin xyz="0 0.1 0" rpy="0 0 0"/>
    <child link="eye_left_link" />
    <parent link="sellion_link"/>
  </joint> -->
  <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>
  <material name="Black">
    <color rgba="0.0 0.0 0.0 1.0"/>
  </material>
  <material name="FlatBlack">
    <color rgba="0.09 0.09 0.09 1.0"/>
  </material>
  <material name="Orange">
    <color rgba="1.0 0.487 0.0 1.0"/>
  </material>
  <material name="pal/Orange">
    <color rgba="0.91373 0.61568 0.29411 1.0"/>
  </material>
  <material name="pal/DarkGrey">
    <color rgba="0.29804 0.29804 0.29804 1"/>
  </material>
  <!-- ROBOT DEFINITION -->
  <link name="base_rear_laser_link" type="laser">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.02559000000 -0.00056000000 -0.05732000000"/>
      <mass value="0.28922000000"/>
      <inertia ixx="0.00002628919" ixy="0.00000024298" ixz="-0.00000368129" iyy="0.00003374542" iyz="-0.00000001330" izz="0.00005832599"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/sensors/sick_tim551.stl" scale="1 1 1"/>
      </geometry>
      <material name="DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="0.01" radius="0.01"/>
      </geometry>
    </collision>
  </link>
  <joint name="base_rear_laser_joint" type="fixed">
    <axis xyz="0 1 0"/>
    <origin rpy="-3.141592653589793 0 2.356194490192345" xyz="-0.27512 0.18297 0.13244"/>
    <parent link="base_link"/>
    <child link="base_rear_laser_link"/>
  </joint>
  <gazebo reference="base_rear_laser_link">
    <sensor name="base_rear_laser" type="gpu_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>818.0</samples>
            <resolution>1</resolution>
            <!-- not the sensor resolution; just 1 -->
            <min_angle>-2.356194490192345</min_angle>
            <max_angle>2.356194490192345</max_angle>
          </horizontal>
        </scan>
        <range>
          <min>0.05</min>
          <max>25.0</max>
          <resolution>0.001</resolution>
        </range>
        <noise>
          <type>gaussian</type>
          <mean>0.0</mean>
          <stddev>0.01</stddev>
        </noise>
      </ray>
      <plugin filename="libgazebo_ros_ray_sensor.so" name="base_rear_laser">
        <ros>
          <remapping>~/out:=scan_rear_raw</remapping>
        </ros>
        <output_type>sensor_msgs/LaserScan</output_type>
        <frame_name>base_rear_laser_link</frame_name>
      </plugin>
    </sensor>
    <material>Gazebo/DarkGrey</material>
  </gazebo>
  <link name="base_front_laser_link" type="laser">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.02559000000 -0.00056000000 -0.05732000000"/>
      <mass value="0.28922000000"/>
      <inertia ixx="0.00002628919" ixy="0.00000024298" ixz="-0.00000368129" iyy="0.00003374542" iyz="-0.00000001330" izz="0.00005832599"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/sensors/sick_tim551.stl" scale="1 1 1"/>
      </geometry>
      <material name="DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="0.01" radius="0.01"/>
      </geometry>
    </collision>
  </link>
  <joint name="base_front_laser_joint" type="fixed">
    <axis xyz="0 1 0"/>
    <origin rpy="-3.141592653589793 0 -0.7853981633974483" xyz="0.27512 -0.18297 0.13244"/>
    <parent link="base_link"/>
    <child link="base_front_laser_link"/>
  </joint>
  <gazebo reference="base_front_laser_link">
    <sensor name="base_front_laser" type="gpu_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>818.0</samples>
            <resolution>1</resolution>
            <!-- not the sensor resolution; just 1 -->
            <min_angle>-2.356194490192345</min_angle>
            <max_angle>2.356194490192345</max_angle>
          </horizontal>
        </scan>
        <range>
          <min>0.05</min>
          <max>25.0</max>
          <resolution>0.001</resolution>
        </range>
        <noise>
          <type>gaussian</type>
          <mean>0.0</mean>
          <stddev>0.01</stddev>
        </noise>
      </ray>
      <plugin filename="libgazebo_ros_ray_sensor.so" name="base_front_laser">
        <ros>
          <remapping>~/out:=scan_front_raw</remapping>
        </ros>
        <output_type>sensor_msgs/LaserScan</output_type>
        <frame_name>base_front_laser_link</frame_name>
      </plugin>
    </sensor>
    <material>Gazebo/DarkGrey</material>
  </gazebo>
  <!-- Virtual Base Laser Link -->
  <joint name="virtual_base_laser_joint" type="fixed">
    <parent link="base_link"/>
    <child link="virtual_base_laser_link"/>
    <origin rpy="0 0 0" xyz="0 0 0.13244"/>
  </joint>
  <link name="virtual_base_laser_link"/>
  <link name="base_imu_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.287 0 0.092163"/>
  </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>
      <plugin filename="libgazebo_ros_imu_sensor.so" name="base_imu">
        <ros>
          <!-- publish to /imu/data -->
          <remapping>~/out:=base_imu</remapping>
        </ros>
      </plugin>
    </sensor>
  </gazebo>
  <!-- Base -->
  <link name="base_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0026742 0.00027459 0.086363"/>
      <mass value="34.047"/>
      <inertia ixx="0.22777" ixy="0.00035979" ixz="0.00015269" iyy="0.29991" iyz="1.7872E-05" izz="0.46036"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/omni_base/base/base_link.stl"/>
      </geometry>
      <material name="">
        <color rgba="0.75294 0.75294 0.75294 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.18925"/>
      <geometry>
        <box size="0.717 0.497 0.0845"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.132"/>
      <geometry>
        <box size="0.58 0.39 0.03"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.0335"/>
      <geometry>
        <box size="0.705 0.48 0.167"/>
      </geometry>
    </collision>
  </link>
  <!-- Base footprint -->
  <link name="base_footprint"/>
  <joint name="base_footprint_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0.0762"/>
    <child link="base_link"/>
    <parent link="base_footprint"/>
  </joint>
  <!-- Docking link -->
  <link name="base_dock_link">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.0037082 1.5226E-18 -4.1633E-17"/>
      <mass value="0.032242"/>
      <inertia ixx="2.1302E-05" ixy="-1.0461E-21" ixz="6.6555E-22" iyy="1.2058E-05" iyz="-1.9453E-21" izz="1.0058E-05"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/omni_base/base/base_dock_link.stl"/>
      </geometry>
      <material name="">
        <color rgba="0.25098 0.25098 0.25098 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/omni_base/base/base_dock_link.stl"/>
      </geometry>
    </collision>
  </link>
  <joint name="base_dock" type="fixed">
    <origin rpy="0 0 0" xyz="0.3535 0 0.0596"/>
    <parent link="base_link"/>
    <child link="base_dock_link"/>
    <axis xyz="0 0 0"/>
  </joint>
  <link name="base_antenna_right_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.027807 -0.0085125 0.015138"/>
      <mass value="0.02311"/>
      <inertia ixx="2.3107E-06" ixy="5.0914e-06" ixz="8.5605E-08" iyy="1.7384E-05" iyz="-2.6206e-08" izz="1.8542E-05"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/omni_base/base/base_antena_link.stl" scale="1 1 1"/>
      </geometry>
      <material name="">
        <color rgba="0.75294 0.75294 0.75294 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/omni_base/base/base_antena_link.stl" scale="1 1 1"/>
      </geometry>
    </collision>
  </link>
  <joint name="base_antenna_right_joint" type="fixed">
    <origin rpy="0 0 0" xyz="-0.2015 -0.13 0.2165"/>
    <parent link="base_link"/>
    <child link="base_antenna_right_link"/>
    <axis xyz="0 0 0"/>
  </joint>
  <gazebo reference="base_antenna_right_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <link name="base_antenna_left_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.027807 0.0085125 0.015138"/>
      <mass value="0.02311"/>
      <inertia ixx="2.3107E-06" ixy="-5.0914e-06" ixz="8.5605E-08" iyy="1.7384E-05" iyz="2.6206e-08" izz="1.8542E-05"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/omni_base/base/base_antena_link.stl" scale="1 -1 1"/>
      </geometry>
      <material name="">
        <color rgba="0.75294 0.75294 0.75294 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/omni_base/base/base_antena_link.stl" scale="1 -1 1"/>
      </geometry>
    </collision>
  </link>
  <joint name="base_antenna_left_joint" type="fixed">
    <origin rpy="0 0 0" xyz="-0.2015 0.13 0.2165"/>
    <parent link="base_link"/>
    <child link="base_antenna_left_link"/>
    <axis xyz="0 0 0"/>
  </joint>
  <gazebo reference="base_antenna_left_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <link name="suspension_front_right_link">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.058182 -0.0043908 -0.0057695"/>
      <mass value="1.2879"/>
      <inertia ixx="0.0015947" ixy="-0.00013183" ixz="1.7315E-05" iyy="0.00060158" iyz="-6.0731e-05" izz="0.0015861"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/omni_base/wheels/suspension_front_link.stl" scale="1 1.0 1"/>
      </geometry>
      <material name="">
        <color rgba="0.79216 0.81961 0.93333 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/omni_base/wheels/suspension_front_link.stl" scale="1 1.0 1"/>
      </geometry>
    </collision>
    <kinematic>0</kinematic>
    <gravity>1</gravity>
  </link>
  <!-- Changed joint type to fixed joint. It is observed that the robot is loosing contact points due to some issues with the Gazebo Physics Engine. This makes the robot drift in localization when static due to added noise -->
  <joint name="suspension_front_right_joint" type="fixed">
    <origin rpy="0 0 -1.5707963267948966" xyz="0.244 -0.1725 0"/>
    <parent link="base_link"/>
    <child link="suspension_front_right_link"/>
    <axis xyz="0 0 1"/>
  </joint>
  <link name="wheel_front_right_link">
    <inertial>
      <origin rpy="0 0 0" xyz="5.4922e-09 6.9775e-05 0.011144"/>
      <mass value="0.70767"/>
      <inertia ixx="0.00035492" ixy="7.5685e-13" ixz="3.8795E-11" iyy="0.00035529" iyz="-2.0284e-07" izz="0.00040131"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/omni_base/wheels/wheel_link.stl" scale="1 1 1.0"/>
      </geometry>
      <material name="DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="0.05" radius="0.0762"/>
      </geometry>
    </collision>
    <surface>
      <friction>
        <ode>
          <mu1>100000.0</mu1>
          <mu2>100000.0</mu2>
        </ode>
      </friction>
      <contact>
        <ode>
          <kp>1e+5</kp>
          <kd>1</kd>
          <soft_cfm>0</soft_cfm>
          <soft_erp>0.2</soft_erp>
          <minDepth>0.002</minDepth>
          <maxVel>0</maxVel>
        </ode>
      </contact>
    </surface>
  </link>
  <joint name="wheel_front_right_joint" type="continuous">
    <origin rpy="-1.5707963267948966 0 1.5707963267948966" xyz="0.050675 0 0"/>
    <parent link="suspension_front_right_link"/>
    <child link="wheel_front_right_link"/>
    <axis xyz="0 0 1"/>
    <limit effort="6.0" velocity="13.123359580052492"/>
    <dynamics damping="1.0" friction="2.0"/>
  </joint>
  <!-- Contact model for the wheel surface -->
  <gazebo reference="wheel_front_right_link">
    <kp>1000000.0</kp>
    <kd>1000.0</kd>
    <mu1>0.0</mu1>
    <mu2>0.0</mu2>
    <fdir1>1 0 0</fdir1>
    <maxVel>1.0</maxVel>
    <minDepth>0.00</minDepth>
    <implicitSpringDamper>1</implicitSpringDamper>
    <material>Gazebo/Grey</material>
  </gazebo>
  <link name="suspension_front_left_link">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.058182 0.0043908 -0.0057695"/>
      <mass value="1.2879"/>
      <inertia ixx="0.0015947" ixy="0.00013183" ixz="1.7315E-05" iyy="0.00060158" iyz="6.0731e-05" izz="0.0015861"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/omni_base/wheels/suspension_front_link.stl" scale="1 -1.0 1"/>
      </geometry>
      <material name="">
        <color rgba="0.79216 0.81961 0.93333 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/omni_base/wheels/suspension_front_link.stl" scale="1 -1.0 1"/>
      </geometry>
    </collision>
    <kinematic>0</kinematic>
    <gravity>1</gravity>
  </link>
  <!-- Changed joint type to fixed joint. It is observed that the robot is loosing contact points due to some issues with the Gazebo Physics Engine. This makes the robot drift in localization when static due to added noise -->
  <joint name="suspension_front_left_joint" type="fixed">
    <origin rpy="0 0 1.5707963267948966" xyz="0.244 0.1725 0"/>
    <parent link="base_link"/>
    <child link="suspension_front_left_link"/>
    <axis xyz="0 0 1"/>
  </joint>
  <link name="wheel_front_left_link">
    <inertial>
      <origin rpy="0 0 0" xyz="5.4922e-09 -6.9775e-05 -0.011144"/>
      <mass value="0.70767"/>
      <inertia ixx="0.00035492" ixy="7.5685e-13" ixz="3.8795E-11" iyy="0.00035529" iyz="-2.0284e-07" izz="0.00040131"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/omni_base/wheels/wheel_link.stl" scale="1 1 -1.0"/>
      </geometry>
      <material name="DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="0.05" radius="0.0762"/>
      </geometry>
    </collision>
    <surface>
      <friction>
        <ode>
          <mu1>100000.0</mu1>
          <mu2>100000.0</mu2>
        </ode>
      </friction>
      <contact>
        <ode>
          <kp>1e+5</kp>
          <kd>1</kd>
          <soft_cfm>0</soft_cfm>
          <soft_erp>0.2</soft_erp>
          <minDepth>0.002</minDepth>
          <maxVel>0</maxVel>
        </ode>
      </contact>
    </surface>
  </link>
  <joint name="wheel_front_left_joint" type="continuous">
    <origin rpy="-1.5707963267948966 0 -1.5707963267948966" xyz="0.050675 0 0"/>
    <parent link="suspension_front_left_link"/>
    <child link="wheel_front_left_link"/>
    <axis xyz="0 0 1"/>
    <limit effort="6.0" velocity="13.123359580052492"/>
    <dynamics damping="1.0" friction="2.0"/>
  </joint>
  <!-- Contact model for the wheel surface -->
  <gazebo reference="wheel_front_left_link">
    <kp>1000000.0</kp>
    <kd>1000.0</kd>
    <mu1>0.0</mu1>
    <mu2>0.0</mu2>
    <fdir1>1 0 0</fdir1>
    <maxVel>1.0</maxVel>
    <minDepth>0.00</minDepth>
    <implicitSpringDamper>1</implicitSpringDamper>
    <material>Gazebo/Grey</material>
  </gazebo>
  <link name="suspension_rear_right_link">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.058182 0.0043908 -0.0057695"/>
      <mass value="1.2879"/>
      <inertia ixx="0.0015947" ixy="0.00013183" ixz="1.7315E-05" iyy="0.00060158" iyz="6.0731e-05" izz="0.0015861"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/omni_base/wheels/suspension_rear_link.stl" scale="1 1.0 1"/>
      </geometry>
      <material name="">
        <color rgba="0.79216 0.81961 0.93333 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/omni_base/wheels/suspension_rear_link.stl" scale="1 1.0 1"/>
      </geometry>
    </collision>
    <kinematic>0</kinematic>
    <gravity>1</gravity>
  </link>
  <!-- Changed joint type to fixed joint. It is observed that the robot is loosing contact points due to some issues with the Gazebo Physics Engine. This makes the robot drift in localization when static due to added noise -->
  <joint name="suspension_rear_right_joint" type="fixed">
    <origin rpy="0 0 -1.5707963267948966" xyz="-0.244 -0.1725 0"/>
    <parent link="base_link"/>
    <child link="suspension_rear_right_link"/>
    <axis xyz="0 0 1"/>
  </joint>
  <link name="wheel_rear_right_link">
    <inertial>
      <origin rpy="0 0 0" xyz="-5.4922e-09 -6.9775e-05 0.011144"/>
      <mass value="0.70767"/>
      <inertia ixx="0.00035492" ixy="-7.5685e-13" ixz="3.8795E-11" iyy="0.00035529" iyz="2.0284e-07" izz="0.00040131"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/omni_base/wheels/wheel_link.stl" scale="1 1 1.0"/>
      </geometry>
      <material name="DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="0.05" radius="0.0762"/>
      </geometry>
    </collision>
    <surface>
      <friction>
        <ode>
          <mu1>100000.0</mu1>
          <mu2>100000.0</mu2>
        </ode>
      </friction>
      <contact>
        <ode>
          <kp>1e+5</kp>
          <kd>1</kd>
          <soft_cfm>0</soft_cfm>
          <soft_erp>0.2</soft_erp>
          <minDepth>0.002</minDepth>
          <maxVel>0</maxVel>
        </ode>
      </contact>
    </surface>
  </link>
  <joint name="wheel_rear_right_joint" type="continuous">
    <origin rpy="-1.5707963267948966 0 1.5707963267948966" xyz="0.050675 0 0"/>
    <parent link="suspension_rear_right_link"/>
    <child link="wheel_rear_right_link"/>
    <axis xyz="0 0 1"/>
    <limit effort="6.0" velocity="13.123359580052492"/>
    <dynamics damping="1.0" friction="2.0"/>
  </joint>
  <!-- Contact model for the wheel surface -->
  <gazebo reference="wheel_rear_right_link">
    <kp>1000000.0</kp>
    <kd>1000.0</kd>
    <mu1>0.0</mu1>
    <mu2>0.0</mu2>
    <fdir1>1 0 0</fdir1>
    <maxVel>1.0</maxVel>
    <minDepth>0.00</minDepth>
    <implicitSpringDamper>1</implicitSpringDamper>
    <material>Gazebo/Grey</material>
  </gazebo>
  <link name="suspension_rear_left_link">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.058182 -0.0043908 -0.0057695"/>
      <mass value="1.2879"/>
      <inertia ixx="0.0015947" ixy="-0.00013183" ixz="1.7315E-05" iyy="0.00060158" iyz="-6.0731e-05" izz="0.0015861"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/omni_base/wheels/suspension_rear_link.stl" scale="1 -1.0 1"/>
      </geometry>
      <material name="">
        <color rgba="0.79216 0.81961 0.93333 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/omni_base/wheels/suspension_rear_link.stl" scale="1 -1.0 1"/>
      </geometry>
    </collision>
    <kinematic>0</kinematic>
    <gravity>1</gravity>
  </link>
  <!-- Changed joint type to fixed joint. It is observed that the robot is loosing contact points due to some issues with the Gazebo Physics Engine. This makes the robot drift in localization when static due to added noise -->
  <joint name="suspension_rear_left_joint" type="fixed">
    <origin rpy="0 0 1.5707963267948966" xyz="-0.244 0.1725 0"/>
    <parent link="base_link"/>
    <child link="suspension_rear_left_link"/>
    <axis xyz="0 0 1"/>
  </joint>
  <link name="wheel_rear_left_link">
    <inertial>
      <origin rpy="0 0 0" xyz="-5.4922e-09 6.9775e-05 -0.011144"/>
      <mass value="0.70767"/>
      <inertia ixx="0.00035492" ixy="-7.5685e-13" ixz="3.8795E-11" iyy="0.00035529" iyz="2.0284e-07" izz="0.00040131"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/omni_base/wheels/wheel_link.stl" scale="1 1 -1.0"/>
      </geometry>
      <material name="DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="0.05" radius="0.0762"/>
      </geometry>
    </collision>
    <surface>
      <friction>
        <ode>
          <mu1>100000.0</mu1>
          <mu2>100000.0</mu2>
        </ode>
      </friction>
      <contact>
        <ode>
          <kp>1e+5</kp>
          <kd>1</kd>
          <soft_cfm>0</soft_cfm>
          <soft_erp>0.2</soft_erp>
          <minDepth>0.002</minDepth>
          <maxVel>0</maxVel>
        </ode>
      </contact>
    </surface>
  </link>
  <joint name="wheel_rear_left_joint" type="continuous">
    <origin rpy="-1.5707963267948966 0 -1.5707963267948966" xyz="0.050675 0 0"/>
    <parent link="suspension_rear_left_link"/>
    <child link="wheel_rear_left_link"/>
    <axis xyz="0 0 1"/>
    <limit effort="6.0" velocity="13.123359580052492"/>
    <dynamics damping="1.0" friction="2.0"/>
  </joint>
  <!-- Contact model for the wheel surface -->
  <gazebo reference="wheel_rear_left_link">
    <kp>1000000.0</kp>
    <kd>1000.0</kd>
    <mu1>0.0</mu1>
    <mu2>0.0</mu2>
    <fdir1>1 0 0</fdir1>
    <maxVel>1.0</maxVel>
    <minDepth>0.00</minDepth>
    <implicitSpringDamper>1</implicitSpringDamper>
    <material>Gazebo/Grey</material>
  </gazebo>
  <!-- Materials -->
  <gazebo reference="base_link">
    <material>Gazebo/White</material>
  </gazebo>
  <!-- base_link through 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>
  <!-- Odometry plugin for ground truth -->
  <gazebo>
    <plugin filename="libgazebo_ros_planar_move.so" name="gazebo_ros_planar_move">
      <ros>
        <remapping>cmd_vel:=mobile_base_controller/cmd_vel_unstamped</remapping>
        <remapping>odom:=mobile_base_controller/odom</remapping>
      </ros>
      <update_rate>100</update_rate>
      <publish_rate>1000</publish_rate>
      <publish_odom>true</publish_odom>
      <publish_odom_tf>false</publish_odom_tf>
      <odometry_frame>odom</odometry_frame>
      <robot_base_frame>base_footprint</robot_base_frame>
      <covariance_x>0.0001</covariance_x>
      <covariance_y>0.0001</covariance_y>
      <covariance_yaw>0.01</covariance_yaw>
    </plugin>
  </gazebo>
  <!-- Connection with the base -->
  <link name="torso_base_link">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.043624513712351 -5.47054746000779E-05 0.0277532809798974"/>
      <mass value="1.00223030186675"/>
      <inertia ixx="0.00660602943010985" ixy="3.32767644990319E-05" ixz="-0.000408545705396379" iyy="0.00690807320580873" iyz="8.29177467923023E-06" izz="0.0122016808119362"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/torso/torso_fixed.stl"/>
      </geometry>
      <material name="pal/DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/torso/torso_fixed_collision.stl"/>
      </geometry>
    </collision>
  </link>
  <joint name="torso_fixed_joint" type="fixed">
    <origin rpy="0 0 0" xyz="-0.0245 0 0.215"/>
    <parent link="base_link"/>
    <child link="torso_base_link"/>
    <axis xyz="0 0 0"/>
  </joint>
  <gazebo reference="torso_base_link">
    <material>Gazebo/DarkGrey</material>
  </gazebo>
  <!-- Fixed column -->
  <link name="torso_fixed_column_link">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0.275"/>
      <geometry>
        <box size="0.18 0.2 0.55"/>
      </geometry>
      <material name="pal/DarkGrey"/>
    </visual>
  </link>
  <joint name="torso_fixed_column_joint" type="fixed">
    <origin rpy="0 0 0" xyz="-0.028 0 0.205"/>
    <parent link="base_link"/>
    <child link="torso_fixed_column_link"/>
    <axis xyz="0 0 0"/>
  </joint>
  <gazebo reference="torso_fixed_column_link">
    <material>Gazebo/DarkGrey</material>
  </gazebo>
  <!-- Lift torso -->
  <link name="torso_lift_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="3.24266045177967"/>
      <inertia ixx="0.0295369723560087" ixy="-7.85101789883696E-07" ixz="-8.58576921315497E-05" iyy="0.0170989395656347" iyz="-8.17944373020342E-06" izz="0.0290055792681799"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/torso/torso_lift_link.stl"/>
      </geometry>
      <material name="">
        <color rgba="0.894117647058823 0.913725490196078 0.929411764705882 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/torso/torso_lift_link_collision.stl"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="0 0 -1.5707963267949" xyz="-0.226436438571948 3.12797385809451E-05 0.00524038105676683"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/torso/laptop_tray_collision.stl"/>
      </geometry>
    </collision>
  </link>
  <joint name="torso_lift_joint" type="prismatic">
    <origin rpy="0 0 0" xyz="0 0 0.5545"/>
    <parent link="torso_base_link"/>
    <child link="torso_lift_link"/>
    <axis xyz="0 0 1"/>
    <limit effort="2000" lower="0" upper="0.35" velocity="0.035"/>
    <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>
  <gazebo reference="torso_lift_joint">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Head mounting link -->
  <link name="torso_head_link">
    <inertial>
      <origin rpy="0 0 0" xyz="3.22658566531686E-16 -5.67885640993557E-18 -0.00811522365001816"/>
      <mass value="0.19541960582329"/>
      <inertia ixx="0.000439179732174765" ixy="5.76286393008048E-20" ixz="1.09094748941927E-18" iyy="0.000498456551929403" iyz="-1.60059406264224E-19" izz="0.000881920896131913"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0.01085"/>
      <geometry>
        <box size="0.11 0.092 0.0217"/>
      </geometry>
      <material name="">
        <color rgba="0.298039215686275 0.298039215686275 0.298039215686275 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.01085"/>
      <geometry>
        <box size="0.11 0.092 0.0217"/>
      </geometry>
    </collision>
  </link>
  <joint name="torso_head_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0.0399999999999999"/>
    <parent link="torso_lift_link"/>
    <child link="torso_head_link"/>
    <axis xyz="0 0 0"/>
  </joint>
  <!-- Torso arm left link-->
  <link name="torso_arm_left_link"/>
  <joint name="torso_arm_left_joint" type="fixed">
    <origin rpy="-2.06788770984932 0 -1.22173047639603" xyz="0.0898413194899453 0.159181203216899 -0.0920547375491735"/>
    <parent link="torso_lift_link"/>
    <child link="torso_arm_left_link"/>
    <axis xyz="0 0 0"/>
  </joint>
  <!-- Torso arm right link -->
  <link name="torso_arm_right_link"/>
  <joint name="torso_arm_right_joint" type="fixed">
    <origin rpy="-2.06788770984932 0 -1.91986217719377" xyz="0.0898413194899453 -0.1591812032169 -0.0920547375491731"/>
    <parent link="torso_lift_link"/>
    <child link="torso_arm_right_link"/>
    <axis xyz="0 0 0"/>
  </joint>
  <!-- Head Link 1-->
  <link name="head_1_link">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.0037459 -4.6685E-05 0.1055"/>
      <mass value="0.15566"/>
      <inertia ixx="0.0005208" ixy="3.0453E-07" ixz="-6.1005E-05" iyy="0.00035021" iyz="5.8627E-07" izz="0.00033341"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/head/head_1_link.stl"/>
      </geometry>
      <material name="">
        <color rgba="0.29804 0.29804 0.29804 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/head/head_1_link_collision.stl"/>
      </geometry>
    </collision>
  </link>
  <joint name="head_1_joint" type="revolute">
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <parent link="torso_head_link"/>
    <child link="head_1_link"/>
    <axis xyz="0 0 1"/>
    <limit effort="5.197" lower="-1.309" upper="1.309" velocity="3.0"/>
    <dynamics damping="0.5" friction="1.0"/>
    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-1.159" soft_upper_limit="1.159"/>
  </joint>
  <!-- Head link 2-->
  <link name="head_2_link">
    <inertial>
      <origin rpy="1.5707963267948966 0 0" xyz="-0.00875103486217405 0.114127350051331 -0.000389070804431699"/>
      <mass value="0.846359357446667"/>
      <inertia ixx="0.00623850186261437" ixy="-0.000550112126953165" ixz="-9.403405403969E-06" iyy="0.0050320204467172" iyz="-5.68886635279132E-06" izz="0.0024981099013485"/>
    </inertial>
    <visual>
      <origin rpy="0 3.141592653589793 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/head/head_2_link.stl"/>
      </geometry>
      <material name="">
        <color rgba="1 1 1 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 3.141592653589793 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/head/head_2_link_collision.stl"/>
      </geometry>
    </collision>
  </link>
  <joint name="head_2_joint" type="revolute">
    <origin rpy="1.5707963267948966 0 0" xyz="0.0425 0 0.10585"/>
    <parent link="head_1_link"/>
    <child link="head_2_link"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.77" lower="-1.0472" upper="0.34907" velocity="3.0"/>
    <dynamics damping="0.5" friction="1.0"/>
    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-0.8971999999999999" soft_upper_limit="0.19907"/>
  </joint>
  <gazebo reference="head_1_link">
    <material>Gazebo/DarkGrey</material>
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo reference="head_2_link">
    <material>Gazebo/White</material>
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo reference="head_1_joint">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="head_2_joint">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- Cameras-->
  <!-- camera body, with origin at bottom screw mount -->
  <joint name="head_front_camera_joint" type="fixed">
    <origin rpy="-1.5708 0 0" xyz="0.0416 0.1555 0"/>
    <parent link="head_2_link"/>
    <child link="head_front_camera_bottom_screw_frame"/>
  </joint>
  <link name="head_front_camera_bottom_screw_frame"/>
  <joint name="head_front_camera_link_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0.010600000000000002 0.0175 0.0125"/>
    <parent link="head_front_camera_bottom_screw_frame"/>
    <child link="head_front_camera_link"/>
  </joint>
  <link name="head_front_camera_link">
    <visual>
      <origin rpy="1.5707963267948966 0 1.5707963267948966" xyz="0.0043 -0.0175 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/sensors/d435_simplified.stl"/>
      </geometry>
      <material name="aluminum"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="-0.008324999999999999 -0.0175 0"/>
      <geometry>
        <box size="0.02505 0.09 0.025"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.075"/>
      <origin xyz="0.0019 -0.01244 0.00025 "/>
      <inertia ixx="0.00005" ixy="0.0" ixz="0.0" iyy="0.00001" iyz="0.0" izz="0.00005"/>
    </inertial>
  </link>
  <gazebo reference="head_front_camera_link">
    <self_collide>0</self_collide>
    <enable_wind>0</enable_wind>
    <kinematic>0</kinematic>
    <gravity>1</gravity>
    <!--<mu>1</mu>-->
    <mu2>1</mu2>
    <fdir1>0 0 0</fdir1>
    <!--<slip1>0</slip1>
      <slip2>0</slip2>-->
    <kp>1e+13</kp>
    <kd>1</kd>
    <!--<max_vel>0.01</max_vel>
      <min_depth>0</min_depth>-->
    <sensor name="head_front_cameracolor" type="camera">
      <camera name="head_front_camera">
        <horizontal_fov>1.2112585008840648</horizontal_fov>
        <image>
          <width>640</width>
          <height>480</height>
          <format>RGB_INT8</format>
        </image>
        <clip>
          <near>0.1</near>
          <far>100</far>
        </clip>
        <noise>
          <type>gaussian</type>
          <mean>0.0</mean>
          <stddev>0.007</stddev>
        </noise>
      </camera>
      <always_on>1</always_on>
      <update_rate>15</update_rate>
      <visualize>0</visualize>
    </sensor>
    <sensor name="head_front_cameraired1" type="camera">
      <camera name="head_front_camera">
        <horizontal_fov>1.4870205226991688</horizontal_fov>
        <image>
          <width>1280</width>
          <height>720</height>
          <format>L_INT8</format>
        </image>
        <clip>
          <near>0.1</near>
          <far>100</far>
        </clip>
        <noise>
          <type>gaussian</type>
          <mean>0.0</mean>
          <stddev>0.05</stddev>
        </noise>
      </camera>
      <always_on>0</always_on>
      <update_rate>15</update_rate>
      <visualize>0</visualize>
    </sensor>
    <sensor name="head_front_cameraired2" type="camera">
      <camera name="head_front_camera">
        <horizontal_fov>1.4870205226991688</horizontal_fov>
        <image>
          <width>1280</width>
          <height>720</height>
          <format>L_INT8</format>
        </image>
        <clip>
          <near>0.1</near>
          <far>100</far>
        </clip>
        <noise>
          <type>gaussian</type>
          <mean>0.0</mean>
          <stddev>0.05</stddev>
        </noise>
      </camera>
      <always_on>0</always_on>
      <update_rate>15</update_rate>
      <visualize>0</visualize>
    </sensor>
    <sensor name="head_front_cameradepth" type="depth">
      <camera name="head_front_camera">
        <horizontal_fov>1.4870205226991688</horizontal_fov>
        <image>
          <width>1280</width>
          <height>720</height>
        </image>
        <clip>
          <near>0.1</near>
          <far>100</far>
        </clip>
        <noise>
          <type>gaussian</type>
          <mean>0.0</mean>
          <stddev>0.100</stddev>
        </noise>
      </camera>
      <always_on>1</always_on>
      <update_rate>15</update_rate>
      <visualize>0</visualize>
    </sensor>
  </gazebo>
  <gazebo>
    <plugin filename="librealsense_gazebo_plugin.so" name="head_front_camera">
      <prefix>head_front_camera</prefix>
      <depthUpdateRate>15.0</depthUpdateRate>
      <colorUpdateRate>15.0</colorUpdateRate>
      <infraredUpdateRate>15.0</infraredUpdateRate>
      <depthTopicName>depth/image_raw</depthTopicName>
      <colorTopicName>color/image_raw</colorTopicName>
      <infrared1TopicName>infra1/image_raw</infrared1TopicName>
      <infrared2TopicName>infra2/image_raw</infrared2TopicName>
      <colorOpticalframeName>head_front_camera_color_optical_frame</colorOpticalframeName>
      <depthOpticalframeName>head_front_camera_depth_optical_frame</depthOpticalframeName>
      <infrared1OpticalframeName>head_front_camera_infra1_ir_optical_frame</infrared1OpticalframeName>
      <infrared2OpticalframeName>head_front_camera_infra2_ir_optical_frame</infrared2OpticalframeName>
      <rangeMinDepth>0.2</rangeMinDepth>
      <rangeMaxDepth>10.0</rangeMaxDepth>
      <pointCloud>True</pointCloud>
      <pointCloudTopicName>depth/color/points</pointCloudTopicName>
      <pointCloudCutoff>0.25</pointCloudCutoff>
      <pointCloudCutoffMax>9.0</pointCloudCutoffMax>
    </plugin>
  </gazebo>
  <!-- PROPERTIES -->
  <!-- Compatibility check with wrist model -->
  <!-- <xacro:if value="${arm_type == 'tiago-sea' and wrist_model=='spherical-wrist'}">
      <xacro:error_wrist_not_supported />
    </xacro:if>
    <xacro:if value="${arm_type == 'tiago-sea-dual' and wrist_model=='spherical-wrist'}">
      <xacro:error_wrist_not_supported />
    </xacro:if> -->
  <!-- INCLUDES -->
  <!-- ARM DEFINITION -->
  <!-- ARM 1 LINK -->
  <link name="arm_left_1_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0043916 0.016184 0.15576"/>
      <mass value="1.1325"/>
      <inertia ixx="0.0025497" ixy="2.6206E-05" ixz="6.2082E-05" iyy="0.0023517" iyz="-0.00020617" izz="0.00097131"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/arm_tiago_pro/arm_1_link.stl" scale="1 1 1"/>
      </geometry>
      <material name="pal/DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/arm_tiago_pro/arm_1_link_collision.stl" scale="1 1 1"/>
      </geometry>
    </collision>
  </link>
  <!-- ARM 1 JOINT -->
  <joint name="arm_left_1_joint" type="revolute">
    <origin rpy="-2.0679 0.0 -1.2217" xyz="0.08976 0.15918 -0.092008"/>
    <parent link="torso_lift_link"/>
    <child link="arm_left_1_link"/>
    <axis xyz="0 0 1"/>
    <limit effort="43.0" lower="-0.5235987755982988" upper="4.71238898038469" velocity="2.5"/>
    <dynamics damping="1.0" friction="1.0"/>
    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-0.4535987755982988" soft_upper_limit="4.642388980384689"/>
  </joint>
  <!-- ARM 2 LINK -->
  <link name="arm_left_2_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.01786 -0.11956 -0.032097"/>
      <mass value="1.0436"/>
      <inertia ixx="0.0015021" ixy="0.00011264" ixz="1.0692E-05" iyy="0.00088017" iyz="-0.00015843" izz="0.0016021"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/arm_tiago_pro/arm_2_link.stl" scale="1 1 1"/>
      </geometry>
      <material name="pal/DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/arm_tiago_pro/arm_2_link_collision.stl" scale="1 1 1"/>
      </geometry>
    </collision>
  </link>
  <!-- ARM 2 JOINT -->
  <joint name="arm_left_2_joint" type="revolute">
    <origin rpy="-1.57079632679489 0 -1.5707963267949" xyz="0.0371099999999995 0.02 0.18"/>
    <parent link="arm_left_1_link"/>
    <child link="arm_left_2_link"/>
    <axis xyz="0 0 1"/>
    <limit effort="43.0" lower="-2.443460952792061" upper="1.1344640137963142" velocity="2.5"/>
    <dynamics damping="1.0" friction="1.0"/>
    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-2.3734609527920614" soft_upper_limit="1.0644640137963142"/>
  </joint>
  <!-- ARM 3 LINK -->
  <link name="arm_left_3_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.016184 -0.0043916 0.13565"/>
      <mass value="1.1325"/>
      <inertia ixx="0.0023517" ixy="-2.6206E-05" ixz="-0.00020617" iyy="0.0025497" iyz="-6.2082E-05" izz="0.00097131"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/arm_tiago_pro/arm_3_link.stl" scale="1 1 1"/>
      </geometry>
      <material name="pal/DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/arm_tiago_pro/arm_3_link_collision.stl" scale="1 1 1"/>
      </geometry>
    </collision>
  </link>
  <!-- ARM 3 JOINT -->
  <joint name="arm_left_3_joint" type="revolute">
    <origin rpy="1.5707963267949 0 0" xyz="0.019999999999999 -0.17011 -0.037110000000001"/>
    <parent link="arm_left_2_link"/>
    <child link="arm_left_3_link"/>
    <axis xyz="0 0 1"/>
    <limit effort="26" lower="-2.6179938779914944" upper="2.6179938779914944" velocity="2.5"/>
    <dynamics damping="1.0" friction="1.0"/>
    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-2.5479938779914946" soft_upper_limit="2.5479938779914946"/>
  </joint>
  <!-- ARM 4 LINK -->
  <link name="arm_left_4_link">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.01786 0.11956 -0.032097"/>
      <mass value="1.0436"/>
      <inertia ixx="0.0015021" ixy="0.00011264" ixz="-1.0692E-05" iyy="0.00088017" iyz="0.00015843" izz="0.0016021"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/arm_tiago_pro/arm_4_link.stl" scale="1 1 1"/>
      </geometry>
      <material name="pal/DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/arm_tiago_pro/arm_4_link_collision.stl" scale="1 1 1"/>
      </geometry>
    </collision>
  </link>
  <!-- ARM 4 JOINT -->
  <joint name="arm_left_4_joint" type="revolute">
    <origin rpy="1.57079632679489 0 0" xyz="0.0199999999999993 -0.0371100000000008 0.15989"/>
    <parent link="arm_left_3_link"/>
    <child link="arm_left_4_link"/>
    <axis xyz="0 0 1"/>
    <limit effort="26" lower="-2.443460952792061" upper="1.1344640137963142" velocity="2.5"/>
    <dynamics damping="1.0" friction="1.0"/>
    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-2.3734609527920614" soft_upper_limit="1.0644640137963142"/>
  </joint>
  <!-- ARM 5 LINK -->
  <link name="arm_left_5_link">
    <inertial>
      <origin rpy="0 0 0" xyz="1.1053306160504E-05 -0.027586177392457 0.067845360860401"/>
      <mass value="0.310763344920608"/>
      <inertia ixx="0.001401450896582" ixy="9.16506225733395E-08" ixz="-9.26253840796548E-08" iyy="0.001399994200261" iyz="0.000327712970358816" izz="0.000402007894416599"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/arm_tiago_pro/arm_5_link.stl" scale="1 1 1"/>
      </geometry>
      <material name="pal/DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/arm_tiago_pro/arm_5_link_collision.stl" scale="1 1 1"/>
      </geometry>
    </collision>
  </link>
  <!-- ARM 5 JOINT -->
  <joint name="arm_left_5_joint" type="revolute">
    <!-- Check new limits -->
    <origin rpy="-1.5707963267949 3.14 0" xyz="-0.0199999999999971 0.17011 -0.037110000000002"/>
    <parent link="arm_left_4_link"/>
    <child link="arm_left_5_link"/>
    <axis xyz="0 0 1"/>
    <limit effort="26" lower="-3.6651914291880923" upper="1.5707963267948966" velocity="2.5"/>
    <dynamics damping="1.0" friction="1.0"/>
    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-3.5951914291880924" soft_upper_limit="1.5007963267948965"/>
  </joint>
  <!-- ARM 6 LINK -->
  <link name="arm_left_6_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.031765231605914 -0.006515108863227 -0.040804517366422"/>
      <mass value="1.34587360139295"/>
      <inertia ixx="0.001098855044306" ixy="0.000130003468892" ixz="-5.88209416784623E-06" iyy="0.001164948561169" iyz="4.20092116284985E-07" izz="0.001189837857495"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/arm_tiago_pro/arm_6_link.stl" scale="1 1 1"/>
      </geometry>
      <material name="pal/DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/arm_tiago_pro/arm_6_link_collision.stl" scale="1 1 1"/>
      </geometry>
    </collision>
  </link>
  <!-- ARM 6 JOINT -->
  <joint name="arm_left_6_joint" type="revolute">
    <origin rpy="-1.5708 0 3.14" xyz="0 -0.0422 0.1654"/>
    <parent link="arm_left_5_link"/>
    <child link="arm_left_6_link"/>
    <axis xyz="0 0 -1"/>
    <limit effort="26" lower="-1.8849555921538759" upper="3.001966313430247" velocity="2.5"/>
    <dynamics damping="1.0" friction="1.0"/>
    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-1.8149555921538758" soft_upper_limit="2.931966313430247"/>
  </joint>
  <!-- ARM 7 LINK -->
  <link name="arm_left_7_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.00174 -0.00223 0.0083"/>
      <mass value="0.12399"/>
      <inertia ixx="0.00008" ixy="0.0" ixz="0.0" iyy="0.00006" iyz="0.0" izz="0.00012"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/arm_tiago_pro/arm_7_link.stl" scale="1 1 1"/>
      </geometry>
      <material name="pal/DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="0.0366" radius="0.0375"/>
      </geometry>
    </collision>
  </link>
  <!-- ARM 7 JOINT -->
  <joint name="arm_left_7_joint" type="revolute">
    <origin rpy="1.5708 0 0" xyz="0.07 -0.0534 -0.0422"/>
    <parent link="arm_left_6_link"/>
    <child link="arm_left_7_link"/>
    <axis xyz="0 0 1"/>
    <limit effort="26" lower="-2.6179938779914944" upper="2.6179938779914944" velocity="2.5"/>
    <dynamics damping="1.0" friction="1.0"/>
    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-2.5479938779914946" soft_upper_limit="2.5479938779914946"/>
  </joint>
  <!--***********************-->
  <!--        TOOL           -->
  <!--***********************-->
  <link name="arm_left_tool_link"/>
  <joint name="arm_left_tool_joint" type="fixed">
    <parent link="arm_left_7_link"/>
    <child link="arm_left_tool_link"/>
    <origin rpy="0 0 0" xyz="0 0 0.017"/>
  </joint>
  <gazebo reference="arm_left_1_link">
    <material>Gazebo/DarkGrey</material>
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo reference="arm_left_2_link">
    <material>Gazebo/DarkGrey</material>
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo reference="arm_left_3_link">
    <material>Gazebo/DarkGrey</material>
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo reference="arm_left_4_link">
    <material>Gazebo/DarkGrey</material>
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo reference="arm_left_5_link">
    <material>Gazebo/DarkGrey</material>
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo reference="arm_left_6_link">
    <material>Gazebo/DarkGrey</material>
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo reference="arm_left_7_link">
    <material>Gazebo/DarkGrey</material>
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo reference="arm_left_tool_link">
    <material>Gazebo/DarkGrey</material>
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </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>
  <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>
  <!-- Base link -->
  <link name="gripper_left_base_link">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.000111195944040731 -5.20000467719152E-05 0.04682714296137"/>
      <mass value="0.351833020899102"/>
      <inertia ixx="0.000212702588837237" ixy="-1.5724450163606E-07" ixz="4.66935789777261E-07" iyy="0.000159938323491697" iyz="-1.90446642900145E-07" izz="0.000254149142086845"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/pal_pro_gripper/base_link_tc.stl"/>
      </geometry>
      <material name="pal/DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/pal_pro_gripper/base_link_tc.stl"/>
      </geometry>
    </collision>
  </link>
  <joint name="gripper_left_base_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
    <parent link="arm_left_tool_link"/>
    <child link="gripper_left_base_link"/>
    <axis xyz="0 0 0"/>
  </joint>
  <!-- Inner finger -->
  <link name="gripper_left_inner_finger_left_link">
    <inertial>
      <origin rpy="0 0 0" xyz="-1.7347E-18 0.024759 0"/>
      <mass value="0.018619"/>
      <inertia ixx="4.8806E-06" ixy="-7.6762E-22" ixz="1.0244E-22" iyy="2.5471E-07" iyz="-1.1841E-21" izz="4.9573E-06"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/pal_pro_gripper/inner_finger.stl"/>
      </geometry>
      <material name="">
        <color rgba="0.752941176470588 0.752941176470588 0.752941176470588 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/pal_pro_gripper/inner_finger.stl"/>
      </geometry>
    </collision>
  </link>
  <joint name="gripper_left_inner_finger_left_joint" type="revolute">
    <origin rpy="1.5708 0.83776 -1.5708" xyz="0 -0.013977 0.078832"/>
    <parent link="gripper_left_base_link"/>
    <child link="gripper_left_inner_finger_left_link"/>
    <axis xyz="0 0 1"/>
    <limit effort="0.1" lower="0" upper="0.8028514559173916" velocity="2"/>
    <mimic joint="gripper_left_finger_joint" multiplier="1" offset="0"/>
  </joint>
  <!-- Outer finger -->
  <link name="gripper_left_outer_finger_left_link">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.00128298420356912 0.0143974756929021 1.5011142385525E-15"/>
      <mass value="0.0159887772377711"/>
      <inertia ixx="5.98786243781872E-06" ixy="-5.21728921919786E-07" ixz="-7.65398863989897E-20" iyy="4.87250835241547E-07" iyz="-7.03393901384231E-19" izz="6.14972693188202E-06"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/pal_pro_gripper/outer_finger.stl"/>
      </geometry>
      <material name="">
        <color rgba="0.752941176470588 0.752941176470588 0.752941176470588 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/pal_pro_gripper/outer_finger.stl"/>
      </geometry>
    </collision>
  </link>
  <joint name="gripper_left_finger_joint" type="revolute">
    <origin rpy="1.5708 0.83776 -1.5708" xyz="0 -0.0288 0.0743"/>
    <parent link="gripper_left_base_link"/>
    <child link="gripper_left_outer_finger_left_link"/>
    <axis xyz="0 0 1"/>
    <limit effort="0.1" lower="0" upper="0.8028514559173916" velocity="2"/>
    <dynamics damping="0.03"/>
  </joint>
  <!-- Fingertip -->
  <link name="gripper_left_fingertip_left_link">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.000864586242839444 0.0119290372347422 -2.732444563276E-11"/>
      <mass value="0.0335029606041878"/>
      <inertia ixx="3.6391786422032E-06" ixy="3.4052204633865E-07" ixz="-1.41918626749374E-14" iyy="1.70950026855986E-06" iyz="-4.24736685249643E-14" izz="2.84428135741107E-06"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/pal_pro_gripper/fingertip.stl"/>
      </geometry>
      <material name="">
        <color rgba="0.752941176470588 0.752941176470588 0.752941176470588 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/pal_pro_gripper/fingertip.stl"/>
      </geometry>
    </collision>
  </link>
  <joint name="gripper_left_fingertip_left_joint" type="revolute">
    <origin rpy="0 0 0.83776" xyz="0 0.05 0"/>
    <parent link="gripper_left_inner_finger_left_link"/>
    <child link="gripper_left_fingertip_left_link"/>
    <axis xyz="0 0 1"/>
    <limit effort="0.1" lower="-0.8028514559173916" upper="0" velocity="2"/>
    <mimic joint="gripper_left_finger_joint" multiplier="-1" offset="0"/>
  </joint>
  <!-- Inner finger -->
  <link name="gripper_left_inner_finger_right_link">
    <inertial>
      <origin rpy="0 0 0" xyz="-1.7347E-18 0.024759 0"/>
      <mass value="0.018619"/>
      <inertia ixx="4.8806E-06" ixy="-7.6762E-22" ixz="1.0244E-22" iyy="2.5471E-07" iyz="-1.1841E-21" izz="4.9573E-06"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/pal_pro_gripper/inner_finger.stl"/>
      </geometry>
      <material name="">
        <color rgba="0.752941176470588 0.752941176470588 0.752941176470588 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/pal_pro_gripper/inner_finger.stl"/>
      </geometry>
    </collision>
  </link>
  <joint name="gripper_left_inner_finger_right_joint" type="revolute">
    <origin rpy="1.5708 0.83776 1.5708" xyz="0 0.013977 0.078832"/>
    <parent link="gripper_left_base_link"/>
    <child link="gripper_left_inner_finger_right_link"/>
    <axis xyz="0 0 1"/>
    <limit effort="0.1" lower="0" upper="0.8028514559173916" velocity="2"/>
    <mimic joint="gripper_left_finger_joint" multiplier="1" offset="0"/>
  </joint>
  <!-- Outer finger -->
  <link name="gripper_left_outer_finger_right_link">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.00128298420356912 0.0143974756929021 1.5011142385525E-15"/>
      <mass value="0.0159887772377711"/>
      <inertia ixx="5.98786243781872E-06" ixy="-5.21728921919786E-07" ixz="-7.65398863989897E-20" iyy="4.87250835241547E-07" iyz="-7.03393901384231E-19" izz="6.14972693188202E-06"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/pal_pro_gripper/outer_finger.stl"/>
      </geometry>
      <material name="">
        <color rgba="0.752941176470588 0.752941176470588 0.752941176470588 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/pal_pro_gripper/outer_finger.stl"/>
      </geometry>
    </collision>
  </link>
  <joint name="gripper_left_outer_finger_right_joint" type="revolute">
    <origin rpy="1.5708 0.83776 1.5708" xyz="0 0.0288 0.0743"/>
    <parent link="gripper_left_base_link"/>
    <child link="gripper_left_outer_finger_right_link"/>
    <axis xyz="0 0 1"/>
    <limit effort="0.1" lower="0" upper="0.8028514559173916" velocity="2"/>
    <mimic joint="gripper_left_finger_joint" multiplier="1" offset="0"/>
  </joint>
  <!-- Fingertip -->
  <link name="gripper_left_fingertip_right_link">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.000864586242839444 0.0119290372347422 -2.732444563276E-11"/>
      <mass value="0.0335029606041878"/>
      <inertia ixx="3.6391786422032E-06" ixy="3.4052204633865E-07" ixz="-1.41918626749374E-14" iyy="1.70950026855986E-06" iyz="-4.24736685249643E-14" izz="2.84428135741107E-06"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/pal_pro_gripper/fingertip.stl"/>
      </geometry>
      <material name="">
        <color rgba="0.752941176470588 0.752941176470588 0.752941176470588 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/pal_pro_gripper/fingertip.stl"/>
      </geometry>
    </collision>
  </link>
  <joint name="gripper_left_fingertip_right_joint" type="revolute">
    <origin rpy="0 0 0.83776" xyz="0 0.05 0"/>
    <parent link="gripper_left_inner_finger_right_link"/>
    <child link="gripper_left_fingertip_right_link"/>
    <axis xyz="0 0 1"/>
    <limit effort="0.1" lower="-0.8028514559173916" upper="0" velocity="2"/>
    <mimic joint="gripper_left_finger_joint" multiplier="-1" offset="0"/>
  </joint>
  <!-- Gazebo config -->
  <gazebo reference="gripper_left_inner_finger_left_link">
    <material>Gazebo/White</material>
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo reference="gripper_left_inner_finger_right_link">
    <material>Gazebo/White</material>
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo reference="gripper_left_outer_finger_left_link">
    <material>Gazebo/White</material>
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo reference="gripper_left_outer_finger_right_link">
    <material>Gazebo/White</material>
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo reference="gripper_left_fingertip_right_link">
    <material>Gazebo/White</material>
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo reference="gripper_left_fingertip_left_link">
    <material>Gazebo/White</material>
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo reference="gripper_left_base_link">
    <material>Gazebo/DarkGrey</material>
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo reference="gripper_left_finger_joint">
    <implicitSpringDamper>1</implicitSpringDamper>
    <provideFeedback>1</provideFeedback>
  </gazebo>
  <gazebo reference="gripper_left_finger_joint">
    <implicitSpringDamper>1</implicitSpringDamper>
    <provideFeedback>1</provideFeedback>
  </gazebo>
  <!-- PAL Pro Prototype -->
  <!-- <xacro:if value="${type == 'pal-pro-gripper-prototype'}">
      <xacro:include filename="$(find pal_pro_gripper_description)/urdf/pro_gripper_prototype.urdf.xacro" />
      <xacro:pal_pro_gripper_prototype name="${name}" parent="${parent}">
        <origin xyz="0.052 0.0 0.0" rpy="0 ${90 * deg_to_rad} 0"/>
      </xacro:pal_pro_gripper_prototype>
    </xacro:if> -->
  <!-- Custom -->
  <!-- <xacro:if value="${type == 'custom' and not is_dual}">
    <xacro:include filename="$(find custom_ee_description)/urdf/end-effector.urdf.xacro" />
    <xacro:end_effector parent="${parent}">
      <origin xyz="0.0 0 0" rpy="0 0 0"/>
    </xacro:end_effector>
  </xacro:if> -->
  <!-- Custom dual-->
  <!-- <xacro:if value="${type == 'custom' and is_dual}"> -->
  <!-- If right -->
  <!-- <xacro:if value="${reflect == 1}">
    <xacro:include filename="$(find custom_dual_ee_description)/urdf/end-effector-right.urdf.xacro" />
    <xacro:end_effector_right parent="${parent}">
      <origin xyz="0.0 0 0" rpy="0 0 0"/>
    </xacro:end_effector_right>
    </xacro:if> -->
  <!-- If left -->
  <!-- <xacro:if value="${reflect == -1}">
    <xacro:include filename="$(find custom_dual_ee_description)/urdf/end-effector-left.urdf.xacro" />
    <xacro:end_effector_left parent="${parent}">
      <origin xyz="0.0 0 0" rpy="0 0 0"/>
    </xacro:end_effector_left>
    </xacro:if>
  </xacro:if> -->
  <!-- PROPERTIES -->
  <!-- Compatibility check with wrist model -->
  <!-- <xacro:if value="${arm_type == 'tiago-sea' and wrist_model=='spherical-wrist'}">
      <xacro:error_wrist_not_supported />
    </xacro:if>
    <xacro:if value="${arm_type == 'tiago-sea-dual' and wrist_model=='spherical-wrist'}">
      <xacro:error_wrist_not_supported />
    </xacro:if> -->
  <!-- INCLUDES -->
  <!-- ARM DEFINITION -->
  <!-- ARM 1 LINK -->
  <link name="arm_right_1_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0043916 0.016184 0.15576"/>
      <mass value="1.1325"/>
      <inertia ixx="0.0025497" ixy="2.6206E-05" ixz="6.2082E-05" iyy="0.0023517" iyz="-0.00020617" izz="0.00097131"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/arm_tiago_pro/arm_1_link.stl" scale="1 1 1"/>
      </geometry>
      <material name="pal/DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/arm_tiago_pro/arm_1_link_collision.stl" scale="1 1 1"/>
      </geometry>
    </collision>
  </link>
  <!-- ARM 1 JOINT -->
  <joint name="arm_right_1_joint" type="revolute">
    <origin rpy="-2.0679 -0.0 -1.9198899999999999" xyz="0.08976 -0.15918 -0.092008"/>
    <parent link="torso_lift_link"/>
    <child link="arm_right_1_link"/>
    <axis xyz="0 0 1"/>
    <limit effort="43.0" lower="-4.71238898038469" upper="0.5235987755982988" velocity="2.5"/>
    <dynamics damping="1.0" friction="1.0"/>
    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-4.642388980384689" soft_upper_limit="0.4535987755982988"/>
  </joint>
  <!-- ARM 2 LINK -->
  <link name="arm_right_2_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.01786 -0.11956 -0.032097"/>
      <mass value="1.0436"/>
      <inertia ixx="0.0015021" ixy="0.00011264" ixz="1.0692E-05" iyy="0.00088017" iyz="-0.00015843" izz="0.0016021"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/arm_tiago_pro/arm_2_link.stl" scale="1 1 1"/>
      </geometry>
      <material name="pal/DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/arm_tiago_pro/arm_2_link_collision.stl" scale="1 1 1"/>
      </geometry>
    </collision>
  </link>
  <!-- ARM 2 JOINT -->
  <joint name="arm_right_2_joint" type="revolute">
    <origin rpy="-1.57079632679489 0 -1.5707963267949" xyz="0.0371099999999995 0.02 0.18"/>
    <parent link="arm_right_1_link"/>
    <child link="arm_right_2_link"/>
    <axis xyz="0 0 1"/>
    <limit effort="43.0" lower="-2.443460952792061" upper="1.1344640137963142" velocity="2.5"/>
    <dynamics damping="1.0" friction="1.0"/>
    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-2.3734609527920614" soft_upper_limit="1.0644640137963142"/>
  </joint>
  <!-- ARM 3 LINK -->
  <link name="arm_right_3_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.016184 -0.0043916 0.13565"/>
      <mass value="1.1325"/>
      <inertia ixx="0.0023517" ixy="-2.6206E-05" ixz="-0.00020617" iyy="0.0025497" iyz="-6.2082E-05" izz="0.00097131"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/arm_tiago_pro/arm_3_link.stl" scale="1 1 1"/>
      </geometry>
      <material name="pal/DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/arm_tiago_pro/arm_3_link_collision.stl" scale="1 1 1"/>
      </geometry>
    </collision>
  </link>
  <!-- ARM 3 JOINT -->
  <joint name="arm_right_3_joint" type="revolute">
    <origin rpy="1.5707963267949 0 0" xyz="0.019999999999999 -0.17011 -0.037110000000001"/>
    <parent link="arm_right_2_link"/>
    <child link="arm_right_3_link"/>
    <axis xyz="0 0 1"/>
    <limit effort="26" lower="-2.6179938779914944" upper="2.6179938779914944" velocity="2.5"/>
    <dynamics damping="1.0" friction="1.0"/>
    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-2.5479938779914946" soft_upper_limit="2.5479938779914946"/>
  </joint>
  <!-- ARM 4 LINK -->
  <link name="arm_right_4_link">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.01786 0.11956 -0.032097"/>
      <mass value="1.0436"/>
      <inertia ixx="0.0015021" ixy="0.00011264" ixz="-1.0692E-05" iyy="0.00088017" iyz="0.00015843" izz="0.0016021"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/arm_tiago_pro/arm_4_link.stl" scale="1 1 1"/>
      </geometry>
      <material name="pal/DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/arm_tiago_pro/arm_4_link_collision.stl" scale="1 1 1"/>
      </geometry>
    </collision>
  </link>
  <!-- ARM 4 JOINT -->
  <joint name="arm_right_4_joint" type="revolute">
    <origin rpy="1.57079632679489 0 0" xyz="0.0199999999999993 -0.0371100000000008 0.15989"/>
    <parent link="arm_right_3_link"/>
    <child link="arm_right_4_link"/>
    <axis xyz="0 0 1"/>
    <limit effort="26" lower="-2.443460952792061" upper="1.1344640137963142" velocity="2.5"/>
    <dynamics damping="1.0" friction="1.0"/>
    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-2.3734609527920614" soft_upper_limit="1.0644640137963142"/>
  </joint>
  <!-- ARM 5 LINK -->
  <link name="arm_right_5_link">
    <inertial>
      <origin rpy="0 0 0" xyz="1.1053306160504E-05 -0.027586177392457 0.067845360860401"/>
      <mass value="0.310763344920608"/>
      <inertia ixx="0.001401450896582" ixy="9.16506225733395E-08" ixz="-9.26253840796548E-08" iyy="0.001399994200261" iyz="0.000327712970358816" izz="0.000402007894416599"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/arm_tiago_pro/arm_5_link.stl" scale="1 1 1"/>
      </geometry>
      <material name="pal/DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/arm_tiago_pro/arm_5_link_collision.stl" scale="1 1 1"/>
      </geometry>
    </collision>
  </link>
  <!-- ARM 5 JOINT -->
  <joint name="arm_right_5_joint" type="revolute">
    <!-- Check new limits -->
    <origin rpy="-1.5707963267949 3.14 0" xyz="-0.0199999999999971 0.17011 -0.037110000000002"/>
    <parent link="arm_right_4_link"/>
    <child link="arm_right_5_link"/>
    <axis xyz="0 0 1"/>
    <limit effort="26" lower="-1.5707963267948966" upper="3.6651914291880923" velocity="2.5"/>
    <dynamics damping="1.0" friction="1.0"/>
    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-1.5007963267948965" soft_upper_limit="3.5951914291880924"/>
  </joint>
  <!-- ARM 6 LINK -->
  <link name="arm_right_6_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.031765231605914 -0.006515108863227 -0.040804517366422"/>
      <mass value="1.34587360139295"/>
      <inertia ixx="0.001098855044306" ixy="0.000130003468892" ixz="-5.88209416784623E-06" iyy="0.001164948561169" iyz="4.20092116284985E-07" izz="0.001189837857495"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/arm_tiago_pro/arm_6_link.stl" scale="1 1 1"/>
      </geometry>
      <material name="pal/DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/arm_tiago_pro/arm_6_link_collision.stl" scale="1 1 1"/>
      </geometry>
    </collision>
  </link>
  <!-- ARM 6 JOINT -->
  <joint name="arm_right_6_joint" type="revolute">
    <origin rpy="-1.5708 0 3.14" xyz="0 -0.0422 0.1654"/>
    <parent link="arm_right_5_link"/>
    <child link="arm_right_6_link"/>
    <axis xyz="0 0 -1"/>
    <limit effort="26" lower="-1.8849555921538759" upper="3.001966313430247" velocity="2.5"/>
    <dynamics damping="1.0" friction="1.0"/>
    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-1.8149555921538758" soft_upper_limit="2.931966313430247"/>
  </joint>
  <!-- ARM 7 LINK -->
  <link name="arm_right_7_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.00174 -0.00223 0.0083"/>
      <mass value="0.12399"/>
      <inertia ixx="0.00008" ixy="0.0" ixz="0.0" iyy="0.00006" iyz="0.0" izz="0.00012"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/arm_tiago_pro/arm_7_link.stl" scale="1 1 1"/>
      </geometry>
      <material name="pal/DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="0.0366" radius="0.0375"/>
      </geometry>
    </collision>
  </link>
  <!-- ARM 7 JOINT -->
  <joint name="arm_right_7_joint" type="revolute">
    <origin rpy="1.5708 0 0" xyz="0.07 -0.0534 -0.0422"/>
    <parent link="arm_right_6_link"/>
    <child link="arm_right_7_link"/>
    <axis xyz="0 0 1"/>
    <limit effort="26" lower="-2.6179938779914944" upper="2.6179938779914944" velocity="2.5"/>
    <dynamics damping="1.0" friction="1.0"/>
    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-2.5479938779914946" soft_upper_limit="2.5479938779914946"/>
  </joint>
  <!--***********************-->
  <!--        TOOL           -->
  <!--***********************-->
  <link name="arm_right_tool_link"/>
  <joint name="arm_right_tool_joint" type="fixed">
    <parent link="arm_right_7_link"/>
    <child link="arm_right_tool_link"/>
    <origin rpy="0 0 0" xyz="0 0 0.017"/>
  </joint>
  <gazebo reference="arm_right_1_link">
    <material>Gazebo/DarkGrey</material>
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo reference="arm_right_2_link">
    <material>Gazebo/DarkGrey</material>
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo reference="arm_right_3_link">
    <material>Gazebo/DarkGrey</material>
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo reference="arm_right_4_link">
    <material>Gazebo/DarkGrey</material>
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo reference="arm_right_5_link">
    <material>Gazebo/DarkGrey</material>
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo reference="arm_right_6_link">
    <material>Gazebo/DarkGrey</material>
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo reference="arm_right_7_link">
    <material>Gazebo/DarkGrey</material>
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo reference="arm_right_tool_link">
    <material>Gazebo/DarkGrey</material>
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </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>
  <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>
  <!-- Base link -->
  <link name="gripper_right_base_link">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.000111195944040731 -5.20000467719152E-05 0.04682714296137"/>
      <mass value="0.351833020899102"/>
      <inertia ixx="0.000212702588837237" ixy="-1.5724450163606E-07" ixz="4.66935789777261E-07" iyy="0.000159938323491697" iyz="-1.90446642900145E-07" izz="0.000254149142086845"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/pal_pro_gripper/base_link_tc.stl"/>
      </geometry>
      <material name="pal/DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/pal_pro_gripper/base_link_tc.stl"/>
      </geometry>
    </collision>
  </link>
  <joint name="gripper_right_base_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
    <parent link="arm_right_tool_link"/>
    <child link="gripper_right_base_link"/>
    <axis xyz="0 0 0"/>
  </joint>
  <!-- Inner finger -->
  <link name="gripper_right_inner_finger_left_link">
    <inertial>
      <origin rpy="0 0 0" xyz="-1.7347E-18 0.024759 0"/>
      <mass value="0.018619"/>
      <inertia ixx="4.8806E-06" ixy="-7.6762E-22" ixz="1.0244E-22" iyy="2.5471E-07" iyz="-1.1841E-21" izz="4.9573E-06"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/pal_pro_gripper/inner_finger.stl"/>
      </geometry>
      <material name="">
        <color rgba="0.752941176470588 0.752941176470588 0.752941176470588 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/pal_pro_gripper/inner_finger.stl"/>
      </geometry>
    </collision>
  </link>
  <joint name="gripper_right_inner_finger_left_joint" type="revolute">
    <origin rpy="1.5708 0.83776 -1.5708" xyz="0 -0.013977 0.078832"/>
    <parent link="gripper_right_base_link"/>
    <child link="gripper_right_inner_finger_left_link"/>
    <axis xyz="0 0 1"/>
    <limit effort="0.1" lower="0" upper="0.8028514559173916" velocity="2"/>
    <mimic joint="gripper_right_finger_joint" multiplier="1" offset="0"/>
  </joint>
  <!-- Outer finger -->
  <link name="gripper_right_outer_finger_left_link">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.00128298420356912 0.0143974756929021 1.5011142385525E-15"/>
      <mass value="0.0159887772377711"/>
      <inertia ixx="5.98786243781872E-06" ixy="-5.21728921919786E-07" ixz="-7.65398863989897E-20" iyy="4.87250835241547E-07" iyz="-7.03393901384231E-19" izz="6.14972693188202E-06"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/pal_pro_gripper/outer_finger.stl"/>
      </geometry>
      <material name="">
        <color rgba="0.752941176470588 0.752941176470588 0.752941176470588 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/pal_pro_gripper/outer_finger.stl"/>
      </geometry>
    </collision>
  </link>
  <joint name="gripper_right_finger_joint" type="revolute">
    <origin rpy="1.5708 0.83776 -1.5708" xyz="0 -0.0288 0.0743"/>
    <parent link="gripper_right_base_link"/>
    <child link="gripper_right_outer_finger_left_link"/>
    <axis xyz="0 0 1"/>
    <limit effort="0.1" lower="0" upper="0.8028514559173916" velocity="2"/>
    <dynamics damping="0.03"/>
  </joint>
  <!-- Fingertip -->
  <link name="gripper_right_fingertip_left_link">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.000864586242839444 0.0119290372347422 -2.732444563276E-11"/>
      <mass value="0.0335029606041878"/>
      <inertia ixx="3.6391786422032E-06" ixy="3.4052204633865E-07" ixz="-1.41918626749374E-14" iyy="1.70950026855986E-06" iyz="-4.24736685249643E-14" izz="2.84428135741107E-06"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/pal_pro_gripper/fingertip.stl"/>
      </geometry>
      <material name="">
        <color rgba="0.752941176470588 0.752941176470588 0.752941176470588 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/pal_pro_gripper/fingertip.stl"/>
      </geometry>
    </collision>
  </link>
  <joint name="gripper_right_fingertip_left_joint" type="revolute">
    <origin rpy="0 0 0.83776" xyz="0 0.05 0"/>
    <parent link="gripper_right_inner_finger_left_link"/>
    <child link="gripper_right_fingertip_left_link"/>
    <axis xyz="0 0 1"/>
    <limit effort="0.1" lower="-0.8028514559173916" upper="0" velocity="2"/>
    <mimic joint="gripper_right_finger_joint" multiplier="-1" offset="0"/>
  </joint>
  <!-- Inner finger -->
  <link name="gripper_right_inner_finger_right_link">
    <inertial>
      <origin rpy="0 0 0" xyz="-1.7347E-18 0.024759 0"/>
      <mass value="0.018619"/>
      <inertia ixx="4.8806E-06" ixy="-7.6762E-22" ixz="1.0244E-22" iyy="2.5471E-07" iyz="-1.1841E-21" izz="4.9573E-06"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/pal_pro_gripper/inner_finger.stl"/>
      </geometry>
      <material name="">
        <color rgba="0.752941176470588 0.752941176470588 0.752941176470588 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/pal_pro_gripper/inner_finger.stl"/>
      </geometry>
    </collision>
  </link>
  <joint name="gripper_right_inner_finger_right_joint" type="revolute">
    <origin rpy="1.5708 0.83776 1.5708" xyz="0 0.013977 0.078832"/>
    <parent link="gripper_right_base_link"/>
    <child link="gripper_right_inner_finger_right_link"/>
    <axis xyz="0 0 1"/>
    <limit effort="0.1" lower="0" upper="0.8028514559173916" velocity="2"/>
    <mimic joint="gripper_right_finger_joint" multiplier="1" offset="0"/>
  </joint>
  <!-- Outer finger -->
  <link name="gripper_right_outer_finger_right_link">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.00128298420356912 0.0143974756929021 1.5011142385525E-15"/>
      <mass value="0.0159887772377711"/>
      <inertia ixx="5.98786243781872E-06" ixy="-5.21728921919786E-07" ixz="-7.65398863989897E-20" iyy="4.87250835241547E-07" iyz="-7.03393901384231E-19" izz="6.14972693188202E-06"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/pal_pro_gripper/outer_finger.stl"/>
      </geometry>
      <material name="">
        <color rgba="0.752941176470588 0.752941176470588 0.752941176470588 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/pal_pro_gripper/outer_finger.stl"/>
      </geometry>
    </collision>
  </link>
  <joint name="gripper_right_outer_finger_right_joint" type="revolute">
    <origin rpy="1.5708 0.83776 1.5708" xyz="0 0.0288 0.0743"/>
    <parent link="gripper_right_base_link"/>
    <child link="gripper_right_outer_finger_right_link"/>
    <axis xyz="0 0 1"/>
    <limit effort="0.1" lower="0" upper="0.8028514559173916" velocity="2"/>
    <mimic joint="gripper_right_finger_joint" multiplier="1" offset="0"/>
  </joint>
  <!-- Fingertip -->
  <link name="gripper_right_fingertip_right_link">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.000864586242839444 0.0119290372347422 -2.732444563276E-11"/>
      <mass value="0.0335029606041878"/>
      <inertia ixx="3.6391786422032E-06" ixy="3.4052204633865E-07" ixz="-1.41918626749374E-14" iyy="1.70950026855986E-06" iyz="-4.24736685249643E-14" izz="2.84428135741107E-06"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/pal_pro_gripper/fingertip.stl"/>
      </geometry>
      <material name="">
        <color rgba="0.752941176470588 0.752941176470588 0.752941176470588 1"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_pro_description/meshes/pal_pro_gripper/fingertip.stl"/>
      </geometry>
    </collision>
  </link>
  <joint name="gripper_right_fingertip_right_joint" type="revolute">
    <origin rpy="0 0 0.83776" xyz="0 0.05 0"/>
    <parent link="gripper_right_inner_finger_right_link"/>
    <child link="gripper_right_fingertip_right_link"/>
    <axis xyz="0 0 1"/>
    <limit effort="0.1" lower="-0.8028514559173916" upper="0" velocity="2"/>
    <mimic joint="gripper_right_finger_joint" multiplier="-1" offset="0"/>
  </joint>
  <!-- Gazebo config -->
  <gazebo reference="gripper_right_inner_finger_left_link">
    <material>Gazebo/White</material>
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo reference="gripper_right_inner_finger_right_link">
    <material>Gazebo/White</material>
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo reference="gripper_right_outer_finger_left_link">
    <material>Gazebo/White</material>
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo reference="gripper_right_outer_finger_right_link">
    <material>Gazebo/White</material>
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo reference="gripper_right_fingertip_right_link">
    <material>Gazebo/White</material>
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo reference="gripper_right_fingertip_left_link">
    <material>Gazebo/White</material>
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo reference="gripper_right_base_link">
    <material>Gazebo/DarkGrey</material>
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo reference="gripper_right_finger_joint">
    <implicitSpringDamper>1</implicitSpringDamper>
    <provideFeedback>1</provideFeedback>
  </gazebo>
  <gazebo reference="gripper_right_finger_joint">
    <implicitSpringDamper>1</implicitSpringDamper>
    <provideFeedback>1</provideFeedback>
  </gazebo>
  <!-- PAL Pro Prototype -->
  <!-- <xacro:if value="${type == 'pal-pro-gripper-prototype'}">
      <xacro:include filename="$(find pal_pro_gripper_description)/urdf/pro_gripper_prototype.urdf.xacro" />
      <xacro:pal_pro_gripper_prototype name="${name}" parent="${parent}">
        <origin xyz="0.052 0.0 0.0" rpy="0 ${90 * deg_to_rad} 0"/>
      </xacro:pal_pro_gripper_prototype>
    </xacro:if> -->
  <!-- Custom -->
  <!-- <xacro:if value="${type == 'custom' and not is_dual}">
    <xacro:include filename="$(find custom_ee_description)/urdf/end-effector.urdf.xacro" />
    <xacro:end_effector parent="${parent}">
      <origin xyz="0.0 0 0" rpy="0 0 0"/>
    </xacro:end_effector>
  </xacro:if> -->
  <!-- Custom dual-->
  <!-- <xacro:if value="${type == 'custom' and is_dual}"> -->
  <!-- If right -->
  <!-- <xacro:if value="${reflect == 1}">
    <xacro:include filename="$(find custom_dual_ee_description)/urdf/end-effector-right.urdf.xacro" />
    <xacro:end_effector_right parent="${parent}">
      <origin xyz="0.0 0 0" rpy="0 0 0"/>
    </xacro:end_effector_right>
    </xacro:if> -->
  <!-- If left -->
  <!-- <xacro:if value="${reflect == -1}">
    <xacro:include filename="$(find custom_dual_ee_description)/urdf/end-effector-left.urdf.xacro" />
    <xacro:end_effector_left parent="${parent}">
      <origin xyz="0.0 0 0" rpy="0 0 0"/>
    </xacro:end_effector_left>
    </xacro:if>
  </xacro:if> -->
  <!-- TODO-->
  <!--Constant parameters-->
  <!-- CAN system -->
  <ros2_control name="ros2_control_tiago_pro_system_can" rw_rate="100" type="system">
    <hardware>
      <plugin>robot_control/RobotControl</plugin>
      <param name="group">can</param>
    </hardware>
    <joint name="wheel_front_right_joint">
      <command_interface name="velocity">
        <param name="min">-1</param>
        <param name="max">1</param>
      </command_interface>
      <state_interface name="position"/>
    </joint>
    <transmission name="wheel_front_right_trans">
      <plugin>transmission_interface/SimpleTransmission</plugin>
      <actuator name="wheel_front_right_actuator" role="actuator1"/>
      <joint name="wheel_front_right_joint" role="joint1">
        <mechanical_reduction>1.0</mechanical_reduction>
      </joint>
    </transmission>
    <joint name="wheel_front_left_joint">
      <command_interface name="velocity">
        <param name="min">-1</param>
        <param name="max">1</param>
      </command_interface>
      <state_interface name="position"/>
    </joint>
    <transmission name="wheel_front_left_trans">
      <plugin>transmission_interface/SimpleTransmission</plugin>
      <actuator name="wheel_front_left_actuator" role="actuator1"/>
      <joint name="wheel_front_left_joint" role="joint1">
        <mechanical_reduction>1.0</mechanical_reduction>
      </joint>
    </transmission>
    <joint name="wheel_rear_right_joint">
      <command_interface name="velocity">
        <param name="min">-1</param>
        <param name="max">1</param>
      </command_interface>
      <state_interface name="position"/>
    </joint>
    <transmission name="wheel_rear_right_trans">
      <plugin>transmission_interface/SimpleTransmission</plugin>
      <actuator name="wheel_rear_right_actuator" role="actuator1"/>
      <joint name="wheel_rear_right_joint" role="joint1">
        <mechanical_reduction>1.0</mechanical_reduction>
      </joint>
    </transmission>
    <joint name="wheel_rear_left_joint">
      <command_interface name="velocity">
        <param name="min">-1</param>
        <param name="max">1</param>
      </command_interface>
      <state_interface name="position"/>
    </joint>
    <transmission name="wheel_rear_left_trans">
      <plugin>transmission_interface/SimpleTransmission</plugin>
      <actuator name="wheel_rear_left_actuator" role="actuator1"/>
      <joint name="wheel_rear_left_joint" role="joint1">
        <mechanical_reduction>1.0</mechanical_reduction>
      </joint>
    </transmission>
    <!-- IMU -->
    <!-- <xacro:ros2_control_imu name="base_imu_sensor"/> -->
  </ros2_control>
  <!-- Dynamixels system -->
  <ros2_control name="ros2_control_tiago_pro_system_dynamixel" rw_rate="100" type="system">
    <hardware>
      <plugin>robot_control/RobotControl</plugin>
      <param name="group">dynamixel</param>
    </hardware>
    <joint name="head_1_joint">
      <command_interface name="position"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>
    <transmission name="head_1_trans">
      <plugin>transmission_interface/SimpleTransmission</plugin>
      <actuator name="head_1_actuator" role="actuator1"/>
      <joint name="head_1_joint" role="joint1">
        <offset>0.0</offset>
        <mechanicalReduction>1</mechanicalReduction>
      </joint>
    </transmission>
    <joint name="head_2_joint">
      <command_interface name="position"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>
    <transmission name="head_2_trans">
      <plugin>transmission_interface/SimpleTransmission</plugin>
      <actuator name="head_2_actuator" role="actuator1"/>
      <joint name="head_2_joint" role="joint1">
        <offset>0.0</offset>
        <mechanicalReduction>1</mechanicalReduction>
      </joint>
    </transmission>
  </ros2_control>
  <ros2_control name="ros2_control_tiago_pro_system_ethercat" rw_rate="1000" type="system">
    <hardware>
      <plugin>robot_control/RobotControl</plugin>
      <param name="group">ethercat</param>
    </hardware>
    <joint name="arm_left_1_joint">
      <command_interface name="position"/>
      <command_interface name="velocity"/>
      <command_interface name="effort"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>
    <transmission name="arm_left_1_trans">
      <plugin>transmission_interface/SimpleTransmission</plugin>
      <actuator name="arm_left_1_actuator" role="actuator1"/>
      <joint name="arm_left_1_joint" role="joint1">
        <offset>0.0</offset>
        <mechanical_reduction>1.0</mechanical_reduction>
      </joint>
    </transmission>
    <joint name="arm_left_2_joint">
      <command_interface name="position"/>
      <command_interface name="velocity"/>
      <command_interface name="effort"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>
    <transmission name="arm_left_2_trans">
      <plugin>transmission_interface/SimpleTransmission</plugin>
      <actuator name="arm_left_2_actuator" role="actuator1"/>
      <joint name="arm_left_2_joint" role="joint1">
        <offset>0.0</offset>
        <mechanical_reduction>1.0</mechanical_reduction>
      </joint>
    </transmission>
    <joint name="arm_left_3_joint">
      <command_interface name="position"/>
      <command_interface name="velocity"/>
      <command_interface name="effort"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>
    <transmission name="arm_left_3_trans">
      <plugin>transmission_interface/SimpleTransmission</plugin>
      <actuator name="arm_left_3_actuator" role="actuator1"/>
      <joint name="arm_left_3_joint" role="joint1">
        <offset>0.0</offset>
        <mechanical_reduction>1.0</mechanical_reduction>
      </joint>
    </transmission>
    <joint name="arm_left_4_joint">
      <command_interface name="position"/>
      <command_interface name="velocity"/>
      <command_interface name="effort"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>
    <transmission name="arm_left_4_trans">
      <plugin>transmission_interface/SimpleTransmission</plugin>
      <actuator name="arm_left_4_actuator" role="actuator1"/>
      <joint name="arm_left_4_joint" role="joint1">
        <offset>0.0</offset>
        <mechanical_reduction>1.0</mechanical_reduction>
      </joint>
    </transmission>
    <joint name="arm_left_5_joint">
      <command_interface name="position"/>
      <command_interface name="velocity"/>
      <command_interface name="effort"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>
    <transmission name="arm_left_5_trans">
      <plugin>transmission_interface/SimpleTransmission</plugin>
      <actuator name="arm_left_5_actuator" role="actuator1"/>
      <joint name="arm_left_5_joint" role="joint1">
        <offset>0.0</offset>
        <mechanical_reduction>1.0</mechanical_reduction>
      </joint>
    </transmission>
    <joint name="arm_left_6_joint">
      <command_interface name="position"/>
      <command_interface name="velocity"/>
      <command_interface name="effort"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>
    <transmission name="arm_left_6_trans">
      <plugin>transmission_interface/SimpleTransmission</plugin>
      <actuator name="arm_left_6_actuator" role="actuator1"/>
      <joint name="arm_left_6_joint" role="joint1">
        <offset>0.0</offset>
        <mechanical_reduction>1.0</mechanical_reduction>
      </joint>
    </transmission>
    <joint name="arm_left_7_joint">
      <command_interface name="position"/>
      <command_interface name="velocity"/>
      <command_interface name="effort"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>
    <transmission name="arm_left_7_trans">
      <plugin>transmission_interface/SimpleTransmission</plugin>
      <actuator name="arm_left_7_actuator" role="actuator1"/>
      <joint name="arm_left_7_joint" role="joint1">
        <offset>0.0</offset>
        <mechanical_reduction>1.0</mechanical_reduction>
      </joint>
    </transmission>
    <joint name="gripper_left_finger_joint">
      <command_interface name="position"/>
      <state_interface name="position">
        <param name="initial_value">0.15</param>
      </state_interface>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>
    <joint name="gripper_left_outer_finger_right_joint">
      <param name="mimic">gripper_left_finger_joint</param>
      <param name="multiplier">1</param>
    </joint>
    <joint name="gripper_left_inner_finger_right_joint">
      <param name="mimic">gripper_left_finger_joint</param>
      <param name="multiplier">1</param>
    </joint>
    <joint name="gripper_left_fingertip_right_joint">
      <param name="mimic">gripper_left_finger_joint</param>
      <param name="multiplier">-1</param>
    </joint>
    <joint name="gripper_left_inner_finger_left_joint">
      <param name="mimic">gripper_left_finger_joint</param>
      <param name="multiplier">1</param>
    </joint>
    <joint name="gripper_left_fingertip_left_joint">
      <param name="mimic">gripper_left_finger_joint</param>
      <param name="multiplier">-1</param>
    </joint>
    <transmission name="gripper_left_finger_trans">
      <plugin>transmission_interface/SimpleTransmission</plugin>
      <actuator name="gripper_left_finger_actuator" role="actuator1"/>
      <joint name="gripper_left_finger_joint" role="joint1">
        <mechanical_reduction>1</mechanical_reduction>
      </joint>
    </transmission>
    <joint name="arm_right_1_joint">
      <command_interface name="position"/>
      <command_interface name="velocity"/>
      <command_interface name="effort"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>
    <transmission name="arm_right_1_trans">
      <plugin>transmission_interface/SimpleTransmission</plugin>
      <actuator name="arm_right_1_actuator" role="actuator1"/>
      <joint name="arm_right_1_joint" role="joint1">
        <offset>0.0</offset>
        <mechanical_reduction>1.0</mechanical_reduction>
      </joint>
    </transmission>
    <joint name="arm_right_2_joint">
      <command_interface name="position"/>
      <command_interface name="velocity"/>
      <command_interface name="effort"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>
    <transmission name="arm_right_2_trans">
      <plugin>transmission_interface/SimpleTransmission</plugin>
      <actuator name="arm_right_2_actuator" role="actuator1"/>
      <joint name="arm_right_2_joint" role="joint1">
        <offset>0.0</offset>
        <mechanical_reduction>1.0</mechanical_reduction>
      </joint>
    </transmission>
    <joint name="arm_right_3_joint">
      <command_interface name="position"/>
      <command_interface name="velocity"/>
      <command_interface name="effort"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>
    <transmission name="arm_right_3_trans">
      <plugin>transmission_interface/SimpleTransmission</plugin>
      <actuator name="arm_right_3_actuator" role="actuator1"/>
      <joint name="arm_right_3_joint" role="joint1">
        <offset>0.0</offset>
        <mechanical_reduction>1.0</mechanical_reduction>
      </joint>
    </transmission>
    <joint name="arm_right_4_joint">
      <command_interface name="position"/>
      <command_interface name="velocity"/>
      <command_interface name="effort"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>
    <transmission name="arm_right_4_trans">
      <plugin>transmission_interface/SimpleTransmission</plugin>
      <actuator name="arm_right_4_actuator" role="actuator1"/>
      <joint name="arm_right_4_joint" role="joint1">
        <offset>0.0</offset>
        <mechanical_reduction>1.0</mechanical_reduction>
      </joint>
    </transmission>
    <joint name="arm_right_5_joint">
      <command_interface name="position"/>
      <command_interface name="velocity"/>
      <command_interface name="effort"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>
    <transmission name="arm_right_5_trans">
      <plugin>transmission_interface/SimpleTransmission</plugin>
      <actuator name="arm_right_5_actuator" role="actuator1"/>
      <joint name="arm_right_5_joint" role="joint1">
        <offset>0.0</offset>
        <mechanical_reduction>1.0</mechanical_reduction>
      </joint>
    </transmission>
    <joint name="arm_right_6_joint">
      <command_interface name="position"/>
      <command_interface name="velocity"/>
      <command_interface name="effort"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>
    <transmission name="arm_right_6_trans">
      <plugin>transmission_interface/SimpleTransmission</plugin>
      <actuator name="arm_right_6_actuator" role="actuator1"/>
      <joint name="arm_right_6_joint" role="joint1">
        <offset>0.0</offset>
        <mechanical_reduction>1.0</mechanical_reduction>
      </joint>
    </transmission>
    <joint name="arm_right_7_joint">
      <command_interface name="position"/>
      <command_interface name="velocity"/>
      <command_interface name="effort"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>
    <transmission name="arm_right_7_trans">
      <plugin>transmission_interface/SimpleTransmission</plugin>
      <actuator name="arm_right_7_actuator" role="actuator1"/>
      <joint name="arm_right_7_joint" role="joint1">
        <offset>0.0</offset>
        <mechanical_reduction>1.0</mechanical_reduction>
      </joint>
    </transmission>
    <joint name="gripper_right_finger_joint">
      <command_interface name="position"/>
      <state_interface name="position">
        <param name="initial_value">0.15</param>
      </state_interface>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>
    <joint name="gripper_right_outer_finger_right_joint">
      <param name="mimic">gripper_right_finger_joint</param>
      <param name="multiplier">1</param>
    </joint>
    <joint name="gripper_right_inner_finger_right_joint">
      <param name="mimic">gripper_right_finger_joint</param>
      <param name="multiplier">1</param>
    </joint>
    <joint name="gripper_right_fingertip_right_joint">
      <param name="mimic">gripper_right_finger_joint</param>
      <param name="multiplier">-1</param>
    </joint>
    <joint name="gripper_right_inner_finger_left_joint">
      <param name="mimic">gripper_right_finger_joint</param>
      <param name="multiplier">1</param>
    </joint>
    <joint name="gripper_right_fingertip_left_joint">
      <param name="mimic">gripper_right_finger_joint</param>
      <param name="multiplier">-1</param>
    </joint>
    <transmission name="gripper_right_finger_trans">
      <plugin>transmission_interface/SimpleTransmission</plugin>
      <actuator name="gripper_right_finger_actuator" role="actuator1"/>
      <joint name="gripper_right_finger_joint" role="joint1">
        <mechanical_reduction>1</mechanical_reduction>
      </joint>
    </transmission>
    <joint name="torso_lift_joint">
      <command_interface name="position"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>
    <transmission name="torso_lift_trans">
      <plugin>transmission_interface/SimpleTransmission</plugin>
      <actuator name="torso_lift_actuator" role="actuator1"/>
      <joint name="torso_lift_joint" role="joint1">
        <mechanical_reduction>1.0</mechanical_reduction>
        <offset>0.0</offset>
      </joint>
    </transmission>
  </ros2_control>
</robot>
