<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- |    This document was autogenerated by xacro from movo.urdf.xacro                | -->
<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
<!-- =================================================================================== -->
<!--
Software License Agreement (BSD)
\file      movo.urdf.xacro
\authors   Patrick Hussey
\copyright Copyright (c) 2017, Kinova Robotics inc. All rights reserved.
Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:
    * Redistributions of source code must retain the above copyright notice,
      this list of conditions and the following disclaimer.
    * Redistributions in binary form must reproduce the above copyright notice,
      this list of conditions and the following disclaimer in the documentation
      and/or other materials provided with the distribution.
    * Neither the name of the copyright holder nor the names of its contributors
      may be used to endorse or promote products derived from this software
      without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 
LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 
A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 
CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 
EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 
PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 
PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 
LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 
NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-->
<robot name="movo" xmlns:xacro="http://ros.org/wiki/xacro">
  <!-- Included URDF/XACRO Files -->
  <gazebo>
    <plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control">
      <robotNamespace>/</robotNamespace>
    </plugin>
    <plugin filename="libgazebo_ros_force_based_move.so" name="object_controller">
      <robotNamespace>/</robotNamespace>
      <commandTopic>/movo/cmd_vel</commandTopic>
      <odometryTopic>/movo/odometry/wheel_odometry</odometryTopic>
      <odometryFrame>odom</odometryFrame>
      <odometryRate>100.0</odometryRate>
      <cmdVelTimeOut>0.2</cmdVelTimeOut>
      <robotBaseFrame>base_link</robotBaseFrame>
      <publishOdometryTf>0</publishOdometryTf>
      <yaw_velocity_p_gain>500.0</yaw_velocity_p_gain>
      <x_velocity_p_gain>10000.0</x_velocity_p_gain>
      <y_velocity_p_gain>10000.0</y_velocity_p_gain>
    </plugin>
    <plugin filename="libgazebo_ros_p3d.so" name="gazebo_ros_p3d">
      <bodyName>base_link</bodyName>
      <topicName>ground_truth_odom</topicName>
      <alwaysOn>true</alwaysOn>
      <updateRate>100.0</updateRate>
    </plugin>
  </gazebo>
  <!-- Base link is the center of the robot's chassis between the motors projected on the ground -->
  <link name="base_link"/>
  <gazebo reference="base_link">
    <mu1>0.0</mu1>
  </gazebo>
  <joint name="base_chassis_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0.0762"/>
    <parent link="base_link"/>
    <child link="base_chassis_link"/>
  </joint>
  <link name="base_chassis_link">
    <inertial>
      <mass value="44.2877"/>
      <origin xyz="-0.009506 -0.013039 0.032319"/>
      <inertia ixx="0.000165" ixy="0" ixz="0" iyx="0" iyy="0.000559" iyz="0" izx="0" izy="0" izz="0.000714"/>
    </inertial>
    <collision>
      <geometry>
        <mesh filename="package://movo_description/meshes/movo_components/collision/vector_base_collision.stl"/>
      </geometry>
    </collision>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://movo_description/meshes/movo_components/visual/vector_base.dae"/>
      </geometry>
    </visual>
  </link>
  <gazebo reference="base_chassis_link">
    <laserRetro>0</laserRetro>
  </gazebo>
  <gazebo reference="base_chassis_link">
    <mu1>0.0</mu1>
  </gazebo>
  <joint name="linear_actuator_fixed_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <parent link="base_chassis_link"/>
    <child link="linear_actuator_fixed_link"/>
  </joint>
  <link name="linear_actuator_fixed_link">
    <inertial>
      <mass value="15.877331"/>
      <origin xyz="0.013724 0.000002 0.509500"/>
      <inertia ixx="0.962473" ixy="0.000047" ixz="0.053722" iyx="0.000047" iyy="0.968691" iyz="-0.000058" izx="0.053722" izy="-0.000058" izz="0.098853"/>
    </inertial>
    <collision>
      <geometry>
        <mesh filename="package://movo_description/meshes/movo_components/collision/movo_fixed_body_collision.stl"/>
      </geometry>
    </collision>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://movo_description/meshes/movo_components/visual/movo_fixed_payload.dae"/>
      </geometry>
    </visual>
  </link>
  <gazebo reference="linear_actuator_fixed_link">
    <laserRetro>0</laserRetro>
  </gazebo>
  <joint name="linear_joint" type="prismatic">
    <parent link="linear_actuator_fixed_link"/>
    <child link="linear_actuator_link"/>
    <origin rpy="0 0 0" xyz="0.0771402 0 0.3938763"/>
    <axis xyz="0 0 1"/>
    <limit effort="1000" lower="0.0" upper="0.472" velocity="0.20"/>
    <dynamics damping="0.0" friction="0.0"/>
  </joint>
  <link name="linear_actuator_link">
    <visual>
      <geometry>
        <mesh filename="package://movo_description/meshes/movo_components/visual/movo_moving_payload.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://movo_description/meshes/movo_components/collision/movo_upper_body_collision.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="12.926242"/>
      <origin xyz="-0.075467 -0.004693 0.397754"/>
      <inertia ixx="0.300410" ixy="-0.010043" ixz="-0.054146" iyx="-0.010043" iyy="0.296460" iyz="-0.024977" izx="-0.054146" izy="-0.024977" izz="0.294778"/>
    </inertial>
  </link>
  <gazebo reference="linear_actuator_link">
    <laserRetro>0</laserRetro>
  </gazebo>
  <transmission name="linear_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="linear_joint">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="linear_motor">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <link name="sic_imu_frame"/>
  <joint name="sic_imu_joint" type="fixed">
    <origin rpy="3.14159 0 1.570795" xyz="0.0326421 0.0899448 0.0082008"/>
    <parent link="base_chassis_link"/>
    <child link="sic_imu_frame"/>
  </joint>
  <gazebo>
    <plugin filename="libhector_gazebo_ros_imu.so" name="sic_imu_controller">
      <alwaysOn>1</alwaysOn>
      <updateRate>50.0</updateRate>
      <bodyName>sic_imu_frame</bodyName>
      <topicName>/movo/feedback/sic_imu</topicName>
      <accelDrift>0.0005 0.0005 0.0005</accelDrift>
      <accelGaussianNoise>0.0005 0.0005 0.0005</accelGaussianNoise>
      <rateDrift>0.0005 0.0005 0.0005</rateDrift>
      <rateGaussianNoise>0.00005 0.00005 0.00005 </rateGaussianNoise>
      <headingDrift>0.00005</headingDrift>
      <headingGaussianNoise>0.00005</headingGaussianNoise>
    </plugin>
  </gazebo>
  <joint name="front_laser_joint" type="fixed">
    <origin rpy="3.14159 0 0" xyz="0.3375 0 0.1439106"/>
    <parent link="base_chassis_link"/>
    <child link="front_laser_link"/>
  </joint>
  <link name="front_laser_link">
    <visual>
      <geometry>
        <mesh filename="package://movo_description/meshes/sensors/visual/sick_tim.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <box size="0.1 0.1 0.1"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="front_laser_link">
    <sensor name="front_laser" type="ray">
      <pose>0 0 0 0 0 0</pose>
      <visualize>false</visualize>
      <update_rate>15</update_rate>
      <ray>
        <scan>
          <horizontal>
            <samples>720</samples>
            <resolution>1</resolution>
            <min_angle>-2.0</min_angle>
            <max_angle>2.0</max_angle>
          </horizontal>
        </scan>
        <range>
          <min>0.1</min>
          <max>25.0</max>
          <resolution>0.01</resolution>
        </range>
        <noise>
          <type>gaussian</type>
          <mean>0.0</mean>
          <stddev>0.01</stddev>
        </noise>
      </ray>
      <plugin filename="libgazebo_ros_laser.so" name="front_laser_node">
        <topicName>/movo/front_scan</topicName>
        <frameName>front_laser_link</frameName>
      </plugin>
    </sensor>
  </gazebo>
  <joint name="rear_laser_joint" type="fixed">
    <origin rpy="3.14159 0 3.14159" xyz="-0.3375 0 0.1439106"/>
    <parent link="base_chassis_link"/>
    <child link="rear_laser_link"/>
  </joint>
  <link name="rear_laser_link">
    <visual>
      <geometry>
        <mesh filename="package://movo_description/meshes/sensors/visual/sick_tim.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <box size="0.1 0.1 0.1"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="rear_laser_link">
    <sensor name="rear_laser" type="ray">
      <pose>0 0 0 0 0 0</pose>
      <visualize>false</visualize>
      <update_rate>15</update_rate>
      <ray>
        <scan>
          <horizontal>
            <samples>720</samples>
            <resolution>1</resolution>
            <min_angle>-2.0</min_angle>
            <max_angle>2.0</max_angle>
          </horizontal>
        </scan>
        <range>
          <min>0.1</min>
          <max>25.0</max>
          <resolution>0.01</resolution>
        </range>
        <noise>
          <type>gaussian</type>
          <mean>0.0</mean>
          <stddev>0.01</stddev>
        </noise>
      </ray>
      <plugin filename="libgazebo_ros_laser.so" name="rear_laser_node">
        <topicName>/movo/rear_scan</topicName>
        <frameName>rear_laser_link</frameName>
      </plugin>
    </sensor>
  </gazebo>
  <joint name="pan_base_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0.19735 0 0.3449888"/>
    <parent link="linear_actuator_link"/>
    <child link="pan_base_link"/>
  </joint>
  <link name="pan_base_link">
    <visual>
      <geometry>
        <mesh filename="package://movo_description/meshes/pan_tilt/visual/pan_tilt_base.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://movo_description/meshes/pan_tilt/collision/pan_tilt_base.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.271282"/>
      <origin xyz="0.000098 -0.000007 0.034804"/>
      <inertia ixx="0.000158" ixy="0.000000" ixz="-0.000001" iyx="0.000000" iyy="0.000151" iyz="0.000000" izx="-0.000001" izy="0.000000" izz="0.000150"/>
    </inertial>
  </link>
  <joint name="pan_joint" type="revolute">
    <origin rpy="3.14159 0 0" xyz="0 0 0.05525"/>
    <axis xyz="0 0 1"/>
    <parent link="pan_base_link"/>
    <child link="pan_link"/>
    <limit effort="7.0" lower="-1.570796326794897" upper="1.570796326794897" velocity="0.907571211"/>
  </joint>
  <transmission name="pan_trans" type="SimpleTransmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="pan_joint">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="pan_actuator">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <link name="pan_link">
    <visual>
      <geometry>
        <mesh filename="package://movo_description/meshes/pan_tilt/visual/pan_link.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://movo_description/meshes/pan_tilt/collision/pan_link.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.470548"/>
      <origin xyz="-0.000036 0.021015 -0.052191"/>
      <inertia ixx="0.000957" ixy="0.000000" ixz="0.000000" iyx="0.000000" iyy="0.000805" iyz="-0.000328" izx="0.000000" izy="-0.000328" izz="0.000353"/>
    </inertial>
  </link>
  <joint name="tilt_joint" type="revolute">
    <origin rpy="-1.570795 1.570795 0" xyz="0 0.026 -0.09075"/>
    <axis xyz="0 0 1"/>
    <parent link="pan_link"/>
    <child link="tilt_link"/>
    <limit effort="7.0" lower="-1.570796326794897" upper="1.570796326794897" velocity="0.907571211"/>
  </joint>
  <transmission name="tilt_trans" type="SimpleTransmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="tilt_joint">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="tilt_actuator">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <link name="tilt_link">
    <visual>
      <geometry>
        <mesh filename="package://movo_description/meshes/pan_tilt/visual/tilt_link.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://movo_description/meshes/pan_tilt/collision/tilt_link.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.434513"/>
      <origin xyz="0.037380 0.005753 -0.019367"/>
      <inertia ixx="0.000418" ixy="0.000123" ixz="-0.000065" iyx="0.000123" iyy="0.001134" iyz="-0.000003" izx="-0.000065" izy="-0.000003" izz="0.000967"/>
    </inertial>
  </link>
  <joint name="kinect2_joint" type="fixed">
    <origin rpy="-1.570795 0 -1.570795" xyz="0.05825 -0.0105875 -0.026"/>
    <parent link="tilt_link"/>
    <child link="kinect2_link"/>
  </joint>
  <link name="kinect2_link">
    <inertial>
      <mass value="1.4"/>
      <origin xyz="-0.000036 0.021015 -0.052191"/>
      <inertia ixx="0.0006835" ixy="-0.000042" ixz="0.000010" iyx="-0.000042" iyy="0.001197" iyz="0.000006" izx="0.000010" izy="0.000006" izz="0.007020"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="package://movo_description/meshes/sensors/visual/kinect_one.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://movo_description/meshes/sensors/collision/kinect_one_collision.stl"/>
      </geometry>
    </collision>
  </link>
  <link name="kinect2_ir_link"/>
  <joint name="kinect2_ir_joint" type="fixed">
    <parent link="kinect2_link"/>
    <child link="kinect2_ir_link"/>
    <origin rpy="0 0 0" xyz="0.0245 -0.03325  0.04201"/>
  </joint>
  <link name="kinect2_ir_optical_frame"/>
  <joint name="kinect2_ir_optical_frame_joint" type="fixed">
    <origin rpy="-1.570795 0 -1.570795" xyz="0 0 0"/>
    <parent link="kinect2_ir_link"/>
    <child link="kinect2_ir_optical_frame"/>
  </joint>
  <link name="kinect2_rgb_link"/>
  <joint name="kinect2_rgb_joint" type="fixed">
    <parent link="kinect2_ir_link"/>
    <child link="kinect2_rgb_link"/>
    <origin rpy="0 0 0" xyz="0 -0.06175  0"/>
  </joint>
  <link name="kinect2_rgb_optical_frame"/>
  <joint name="kinect2_rgb_optical_joint" type="fixed">
    <parent link="kinect2_rgb_link"/>
    <child link="kinect2_rgb_optical_frame"/>
    <origin rpy="-1.570795 0 -1.570795" xyz="0 0 0"/>
  </joint>
  <gazebo reference="kinect2_ir_link">
    <sensor name="kinect2_ir_sensor" type="depth">
      <always_on>true</always_on>
      <update_rate>30.0</update_rate>
      <camera>
        <horizontal_fov>1.22172944444</horizontal_fov>
        <image>
          <format>L8</format>
          <width>512</width>
          <height>424</height>
        </image>
        <clip>
          <near>0.01</near>
          <far>100</far>
        </clip>
      </camera>
      <plugin filename="libgazebo_ros_openni_kinect.so" name="kinect2_ir_link_controller">
        <baseline>0.2</baseline>
        <alwaysOn>true</alwaysOn>
        <updateRate>1.0</updateRate>
        <cameraName>kinect2/sd/image_ir</cameraName>
        <imageTopicName>/kinect2/sd/image_ir</imageTopicName>
        <cameraInfoTopicName>/kinect2/sd/camera_info</cameraInfoTopicName>
        <depthImageTopicName>/kinect2/sd/image_depth</depthImageTopicName>
        <depthImageCameraInfoTopicName>/kinect2/sd/depth/camera_info</depthImageCameraInfoTopicName>
        <pointCloudTopicName>/kinect2/sd/points</pointCloudTopicName>
        <frameName>kinect2_ir_optical_frame</frameName>
        <pointCloudCutoff>0.1</pointCloudCutoff>
        <pointCloudCutoffMax>12.0</pointCloudCutoffMax>
        <distortionK1>0.0</distortionK1>
        <distortionK2>0.0</distortionK2>
        <distortionK3>0.0</distortionK3>
        <distortionT1>0.0</distortionT1>
        <distortionT2>0.0</distortionT2>
        <CxPrime>0</CxPrime>
        <Cx>0</Cx>
        <Cy>0</Cy>
        <focalLength>0</focalLength>
        <hackBaseline>0</hackBaseline>
      </plugin>
    </sensor>
    <material value="Gazebo/Red"/>
  </gazebo>
  <joint name="right_kinova_joint" type="fixed">
    <parent link="linear_actuator_link"/>
    <child link="right_base_link"/>
    <origin rpy="-1.570796325 0 -1.570796325" xyz="0.0261046 -0.13335 0.2799937"/>
  </joint>
  <link name="right_base_link">
    <visual>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/jaco/visual/base.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/jaco/collision/base.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.649618"/>
      <origin xyz="-0.000146 0.0 0.031039"/>
      <inertia ixx="0.000652" ixy="-0.000000" ixz="0.000001" iyx="0.000000" iyy="0.000646" iyz="0.000000" izx="0.000001" izy="0.000000" izz="0.000480"/>
    </inertial>
  </link>
  <gazebo reference="right_base_link">
    <implicitSpringDamper>1</implicitSpringDamper>
    <kp>1000000.0</kp>
    <kd>100.0</kd>
    <mu1>1.0</mu1>
    <mu2>1.0</mu2>
    <maxVel>100.0</maxVel>
    <minDepth>0.001</minDepth>
  </gazebo>
  <joint name="right_shoulder_pan_joint" type="revolute">
    <parent link="right_base_link"/>
    <child link="right_shoulder_link"/>
    <origin rpy="0 3.14159265 0" xyz="0 0 0.0625"/>
    <axis xyz="0 0 1"/>
    <limit effort="18000.0" lower="-3.14159265" upper="3.14159265" velocity="0.691150384"/>
    <dynamics damping="0.01" friction="0.0"/>
  </joint>
  <link name="right_shoulder_link">
    <visual>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/jaco/visual/shoulder_7dof.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/jaco/collision/shoulder_7dof.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.824524"/>
      <origin xyz="0.000018 -0.004667 -0.060609"/>
      <inertia ixx="0.002561" ixy="0.000000" ixz="0.000001" iyx="0.000000" iyy="0.002653" iyz="0.000212" izx="0.000001" izy="0.000212" izz="0.000524"/>
    </inertial>
  </link>
  <gazebo reference="right_shoulder_link">
    <implicitSpringDamper>1</implicitSpringDamper>
    <kp>1000000.0</kp>
    <kd>100.0</kd>
    <mu1>1.0</mu1>
    <mu2>1.0</mu2>
    <maxVel>100.0</maxVel>
    <minDepth>0.001</minDepth>
  </gazebo>
  <joint name="right_shoulder_lift_joint" type="revolute">
    <parent link="right_shoulder_link"/>
    <child link="right_arm_half_1_link"/>
    <origin rpy="1.570796325 0 0" xyz="0 0 -0.11875"/>
    <axis xyz="0 0 1"/>
    <limit effort="30000.5" lower="-2.238" upper="2.238" velocity="0.691150384"/>
    <dynamics damping="0.01" friction="0.0"/>
  </joint>
  <link name="right_arm_half_1_link">
    <visual>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/jaco/visual/arm_half_1.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/jaco/collision/arm_half_1.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.8447"/>
      <origin xyz="0 -0.103563213 0"/>
      <inertia ixx="0.000435" ixy="0.000002" ixz="0.000231" iyx="0.000002" iyy="0.034947" iyz="0.000000" izx="0.000231" izy="0.000000" izz="0.035198"/>
    </inertial>
  </link>
  <gazebo reference="right_arm_half_1_link">
    <implicitSpringDamper>1</implicitSpringDamper>
    <kp>1000000.0</kp>
    <kd>100.0</kd>
    <mu1>1.0</mu1>
    <mu2>1.0</mu2>
    <maxVel>100.0</maxVel>
    <minDepth>0.001</minDepth>
  </gazebo>
  <joint name="right_arm_half_joint" type="revolute">
    <parent link="right_arm_half_1_link"/>
    <child link="right_arm_half_2_link"/>
    <origin rpy="-1.570796325 0 0" xyz="0 -0.205 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="30000.5" lower="-3.14159265" upper="3.14159265" velocity="0.691150384"/>
    <dynamics damping="0.01" friction="0.0"/>
  </joint>
  <link name="right_arm_half_2_link">
    <visual>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/jaco/visual/arm_half_2.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/jaco/collision/arm_half_2.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.8447"/>
      <origin xyz="0 0 -0.1022447445"/>
      <inertia ixx="0.000435" ixy="0.000002" ixz="0.000231" iyx="0.000002" iyy="0.034947" iyz="0.000000" izx="0.000231" izy="0.000000" izz="0.035198"/>
    </inertial>
  </link>
  <gazebo reference="right_arm_half_2_link">
    <implicitSpringDamper>1</implicitSpringDamper>
    <kp>1000000.0</kp>
    <kd>100.0</kd>
    <mu1>1.0</mu1>
    <mu2>1.0</mu2>
    <maxVel>100.0</maxVel>
    <minDepth>0.001</minDepth>
  </gazebo>
  <joint name="right_elbow_joint" type="revolute">
    <parent link="right_arm_half_2_link"/>
    <child link="right_forearm_link"/>
    <origin rpy="-1.570796325 0 0" xyz="0 0 -0.205"/>
    <axis xyz="0 0 1"/>
    <limit effort="18000.0" lower="-2.809" upper="2.809" velocity="0.691150384"/>
    <dynamics damping="0.01" friction="0.0"/>
  </joint>
  <link name="right_forearm_link">
    <visual>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/jaco/visual/forearm_7dof.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/jaco/collision/forearm_7dof.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.674254"/>
      <origin xyz="0 0.081 -0.0086"/>
      <inertia ixx="0.005453" ixy="0.000003" ixz="0.000000" iyx="0.000003" iyy="0.000305" iyz="0.000087" izx="0.000000" izy="0.000087" izz="0.005578"/>
    </inertial>
  </link>
  <joint name="right_wrist_spherical_1_joint" type="revolute">
    <parent link="right_forearm_link"/>
    <child link="right_wrist_spherical_1_link"/>
    <!--  <origin xyz="0 0.20725 0 " rpy="${wrist_spherical_1_joint_rpy}" /> -->
    <origin rpy="1.570796325 3.14159265 0" xyz="0 0.2073 -0.0114"/>
    <axis xyz="0 0 1"/>
    <limit effort="60000.8" lower="-3.14159265" upper="3.14159265" velocity="0.921533845"/>
    <dynamics damping="0.01" friction="0.0"/>
  </joint>
  <link name="right_wrist_spherical_1_link">
    <visual>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/jaco/visual/wrist_spherical_1.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/jaco/collision/wrist_spherical_1.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.463"/>
      <origin xyz="0 0.0028848942 -0.0541932613"/>
      <inertia ixx="0.000392" ixy="0.000000" ixz="0.000000" iyx="0.000000" iyy="0.000347" iyz="0.000101" izx="0.000000" izy="0.000101" izz="0.000233"/>
    </inertial>
  </link>
  <gazebo reference="right_wrist_spherical_1_link">
    <implicitSpringDamper>1</implicitSpringDamper>
    <kp>1000000.0</kp>
    <kd>100.0</kd>
    <mu1>1.0</mu1>
    <mu2>1.0</mu2>
    <maxVel>100.0</maxVel>
    <minDepth>0.001</minDepth>
  </gazebo>
  <joint name="right_wrist_spherical_2_joint" type="revolute">
    <parent link="right_wrist_spherical_1_link"/>
    <child link="right_wrist_spherical_2_link"/>
    <origin rpy="-1.570796325 0 0" xyz="0 0 -0.10375 "/>
    <axis xyz="0 0 1"/>
    <limit effort="60000.8" lower="-2.78" upper="2.78" velocity="0.921533845"/>
    <dynamics damping="0.01" friction="0.0"/>
  </joint>
  <link name="right_wrist_spherical_2_link">
    <visual>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/jaco/visual/wrist_spherical_2.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/jaco/collision/wrist_spherical_2.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.463"/>
      <origin xyz="0 0.0497208855 -0.0028562765"/>
      <inertia ixx="0.000392" ixy="0.000000" ixz="0.000000" iyx="0.000000" iyy="0.000347" iyz="0.000101" izx="0.000000" izy="0.000101" izz="0.000233"/>
    </inertial>
  </link>
  <gazebo reference="right_wrist_spherical_2_link">
    <implicitSpringDamper>1</implicitSpringDamper>
    <kp>1000000.0</kp>
    <kd>100.0</kd>
    <mu1>1.0</mu1>
    <mu2>1.0</mu2>
    <maxVel>100.0</maxVel>
    <minDepth>0.001</minDepth>
  </gazebo>
  <joint name="right_wrist_3_joint" type="revolute">
    <parent link="right_wrist_spherical_2_link"/>
    <child link="right_wrist_3_link"/>
    <origin rpy="1.570796325 3.14159265 0" xyz="0 0.10375 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="60000.8" lower="-3.14159265" upper="3.14159265" velocity="0.921533845"/>
    <dynamics damping="0.01" friction="0.0"/>
  </joint>
  <link name="right_wrist_3_link">
    <visual>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/jaco/visual/kinova_actuator_gearing.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/jaco/collision/kinova_actuator_gearing.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.190782"/>
      <origin xyz="-0.00082 0.000048 -0.216121"/>
      <inertia ixx="0.000048" ixy="0.000000" ixz="0.000000" iyx="0.000000" iyy="0.000049" iyz="0.000000" izx="0.000000" izy="0.000000" izz="0.000072"/>
    </inertial>
  </link>
  <gazebo reference="right_wrist_3_link">
    <implicitSpringDamper>1</implicitSpringDamper>
    <kp>1000000.0</kp>
    <kd>100.0</kd>
    <mu1>1.0</mu1>
    <mu2>1.0</mu2>
    <maxVel>100.0</maxVel>
    <minDepth>0.001</minDepth>
  </gazebo>
  <joint name="right_ee_fixed_joint" type="fixed">
    <parent link="right_wrist_3_link"/>
    <child link="right_ee_link"/>
    <origin rpy="3.14159265 1.570796325 0" xyz="0 0 -0.1600"/>
  </joint>
  <link name="right_ee_link"/>
  <transmission name="right_shoulder_pan_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="right_shoulder_pan_joint">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="right_shoulder_pan_motor">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="right_shoulder_lift_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="right_shoulder_lift_joint">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="right_shoulder_lift_motor">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="right_half_arm_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="right_arm_half_joint">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="right_arm_half_motor">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="right_elbow_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="right_elbow_joint">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="right_elbow_motor">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="right_wrist_spherical_1_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="right_wrist_spherical_1_joint">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="right_wrist_spherical_1_motor">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="right_wrist_spherical_2_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="right_wrist_spherical_2_joint">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="right_wrist_spherical_2_motor">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="right_wrist_3_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="right_wrist_3_joint">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="right_wrist_3_motor">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <joint name="left_kinova_joint" type="fixed">
    <parent link="linear_actuator_link"/>
    <child link="left_base_link"/>
    <origin rpy="-1.570796325 0 -1.570796325" xyz="0.0261046 0.13335 0.2799937"/>
  </joint>
  <link name="left_base_link">
    <visual>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/jaco/visual/base.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/jaco/collision/base.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.649618"/>
      <origin xyz="-0.000146 0.0 0.031039"/>
      <inertia ixx="0.000652" ixy="-0.000000" ixz="0.000001" iyx="0.000000" iyy="0.000646" iyz="0.000000" izx="0.000001" izy="0.000000" izz="0.000480"/>
    </inertial>
  </link>
  <gazebo reference="left_base_link">
    <implicitSpringDamper>1</implicitSpringDamper>
    <kp>1000000.0</kp>
    <kd>100.0</kd>
    <mu1>1.0</mu1>
    <mu2>1.0</mu2>
    <maxVel>100.0</maxVel>
    <minDepth>0.001</minDepth>
  </gazebo>
  <joint name="left_shoulder_pan_joint" type="revolute">
    <parent link="left_base_link"/>
    <child link="left_shoulder_link"/>
    <origin rpy="0 3.14159265 0" xyz="0 0 0.0625"/>
    <axis xyz="0 0 1"/>
    <limit effort="18000.0" lower="-3.14159265" upper="3.14159265" velocity="0.691150384"/>
    <dynamics damping="0.01" friction="0.0"/>
  </joint>
  <link name="left_shoulder_link">
    <visual>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/jaco/visual/shoulder_7dof.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/jaco/collision/shoulder_7dof.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.824524"/>
      <origin xyz="0.000018 -0.004667 -0.060609"/>
      <inertia ixx="0.002561" ixy="0.000000" ixz="0.000001" iyx="0.000000" iyy="0.002653" iyz="0.000212" izx="0.000001" izy="0.000212" izz="0.000524"/>
    </inertial>
  </link>
  <gazebo reference="left_shoulder_link">
    <implicitSpringDamper>1</implicitSpringDamper>
    <kp>1000000.0</kp>
    <kd>100.0</kd>
    <mu1>1.0</mu1>
    <mu2>1.0</mu2>
    <maxVel>100.0</maxVel>
    <minDepth>0.001</minDepth>
  </gazebo>
  <joint name="left_shoulder_lift_joint" type="revolute">
    <parent link="left_shoulder_link"/>
    <child link="left_arm_half_1_link"/>
    <origin rpy="1.570796325 0 0" xyz="0 0 -0.11875"/>
    <axis xyz="0 0 1"/>
    <limit effort="30000.5" lower="-2.238" upper="2.238" velocity="0.691150384"/>
    <dynamics damping="0.01" friction="0.0"/>
  </joint>
  <link name="left_arm_half_1_link">
    <visual>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/jaco/visual/arm_half_1.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/jaco/collision/arm_half_1.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.8447"/>
      <origin xyz="0 -0.103563213 0"/>
      <inertia ixx="0.000435" ixy="0.000002" ixz="0.000231" iyx="0.000002" iyy="0.034947" iyz="0.000000" izx="0.000231" izy="0.000000" izz="0.035198"/>
    </inertial>
  </link>
  <gazebo reference="left_arm_half_1_link">
    <implicitSpringDamper>1</implicitSpringDamper>
    <kp>1000000.0</kp>
    <kd>100.0</kd>
    <mu1>1.0</mu1>
    <mu2>1.0</mu2>
    <maxVel>100.0</maxVel>
    <minDepth>0.001</minDepth>
  </gazebo>
  <joint name="left_arm_half_joint" type="revolute">
    <parent link="left_arm_half_1_link"/>
    <child link="left_arm_half_2_link"/>
    <origin rpy="-1.570796325 0 0" xyz="0 -0.205 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="30000.5" lower="-3.14159265" upper="3.14159265" velocity="0.691150384"/>
    <dynamics damping="0.01" friction="0.0"/>
  </joint>
  <link name="left_arm_half_2_link">
    <visual>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/jaco/visual/arm_half_2.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/jaco/collision/arm_half_2.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.8447"/>
      <origin xyz="0 0 -0.1022447445"/>
      <inertia ixx="0.000435" ixy="0.000002" ixz="0.000231" iyx="0.000002" iyy="0.034947" iyz="0.000000" izx="0.000231" izy="0.000000" izz="0.035198"/>
    </inertial>
  </link>
  <gazebo reference="left_arm_half_2_link">
    <implicitSpringDamper>1</implicitSpringDamper>
    <kp>1000000.0</kp>
    <kd>100.0</kd>
    <mu1>1.0</mu1>
    <mu2>1.0</mu2>
    <maxVel>100.0</maxVel>
    <minDepth>0.001</minDepth>
  </gazebo>
  <joint name="left_elbow_joint" type="revolute">
    <parent link="left_arm_half_2_link"/>
    <child link="left_forearm_link"/>
    <origin rpy="-1.570796325 0 0" xyz="0 0 -0.205"/>
    <axis xyz="0 0 1"/>
    <limit effort="18000.0" lower="-2.809" upper="2.809" velocity="0.691150384"/>
    <dynamics damping="0.01" friction="0.0"/>
  </joint>
  <link name="left_forearm_link">
    <visual>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/jaco/visual/forearm_7dof.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/jaco/collision/forearm_7dof.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.674254"/>
      <origin xyz="0 0.081 -0.0086"/>
      <inertia ixx="0.005453" ixy="0.000003" ixz="0.000000" iyx="0.000003" iyy="0.000305" iyz="0.000087" izx="0.000000" izy="0.000087" izz="0.005578"/>
    </inertial>
  </link>
  <joint name="left_wrist_spherical_1_joint" type="revolute">
    <parent link="left_forearm_link"/>
    <child link="left_wrist_spherical_1_link"/>
    <!--  <origin xyz="0 0.20725 0 " rpy="${wrist_spherical_1_joint_rpy}" /> -->
    <origin rpy="1.570796325 3.14159265 0" xyz="0 0.2073 -0.0114"/>
    <axis xyz="0 0 1"/>
    <limit effort="60000.8" lower="-3.14159265" upper="3.14159265" velocity="0.921533845"/>
    <dynamics damping="0.01" friction="0.0"/>
  </joint>
  <link name="left_wrist_spherical_1_link">
    <visual>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/jaco/visual/wrist_spherical_1.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/jaco/collision/wrist_spherical_1.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.463"/>
      <origin xyz="0 0.0028848942 -0.0541932613"/>
      <inertia ixx="0.000392" ixy="0.000000" ixz="0.000000" iyx="0.000000" iyy="0.000347" iyz="0.000101" izx="0.000000" izy="0.000101" izz="0.000233"/>
    </inertial>
  </link>
  <gazebo reference="left_wrist_spherical_1_link">
    <implicitSpringDamper>1</implicitSpringDamper>
    <kp>1000000.0</kp>
    <kd>100.0</kd>
    <mu1>1.0</mu1>
    <mu2>1.0</mu2>
    <maxVel>100.0</maxVel>
    <minDepth>0.001</minDepth>
  </gazebo>
  <joint name="left_wrist_spherical_2_joint" type="revolute">
    <parent link="left_wrist_spherical_1_link"/>
    <child link="left_wrist_spherical_2_link"/>
    <origin rpy="-1.570796325 0 0" xyz="0 0 -0.10375 "/>
    <axis xyz="0 0 1"/>
    <limit effort="60000.8" lower="-2.78" upper="2.78" velocity="0.921533845"/>
    <dynamics damping="0.01" friction="0.0"/>
  </joint>
  <link name="left_wrist_spherical_2_link">
    <visual>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/jaco/visual/wrist_spherical_2.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/jaco/collision/wrist_spherical_2.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.463"/>
      <origin xyz="0 0.0497208855 -0.0028562765"/>
      <inertia ixx="0.000392" ixy="0.000000" ixz="0.000000" iyx="0.000000" iyy="0.000347" iyz="0.000101" izx="0.000000" izy="0.000101" izz="0.000233"/>
    </inertial>
  </link>
  <gazebo reference="left_wrist_spherical_2_link">
    <implicitSpringDamper>1</implicitSpringDamper>
    <kp>1000000.0</kp>
    <kd>100.0</kd>
    <mu1>1.0</mu1>
    <mu2>1.0</mu2>
    <maxVel>100.0</maxVel>
    <minDepth>0.001</minDepth>
  </gazebo>
  <joint name="left_wrist_3_joint" type="revolute">
    <parent link="left_wrist_spherical_2_link"/>
    <child link="left_wrist_3_link"/>
    <origin rpy="1.570796325 3.14159265 0" xyz="0 0.10375 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="60000.8" lower="-3.14159265" upper="3.14159265" velocity="0.921533845"/>
    <dynamics damping="0.01" friction="0.0"/>
  </joint>
  <link name="left_wrist_3_link">
    <visual>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/jaco/visual/kinova_actuator_gearing.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/jaco/collision/kinova_actuator_gearing.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.190782"/>
      <origin xyz="-0.00082 0.000048 -0.216121"/>
      <inertia ixx="0.000048" ixy="0.000000" ixz="0.000000" iyx="0.000000" iyy="0.000049" iyz="0.000000" izx="0.000000" izy="0.000000" izz="0.000072"/>
    </inertial>
  </link>
  <gazebo reference="left_wrist_3_link">
    <implicitSpringDamper>1</implicitSpringDamper>
    <kp>1000000.0</kp>
    <kd>100.0</kd>
    <mu1>1.0</mu1>
    <mu2>1.0</mu2>
    <maxVel>100.0</maxVel>
    <minDepth>0.001</minDepth>
  </gazebo>
  <joint name="left_ee_fixed_joint" type="fixed">
    <parent link="left_wrist_3_link"/>
    <child link="left_ee_link"/>
    <origin rpy="3.14159265 1.570796325 0" xyz="0 0 -0.1600"/>
  </joint>
  <link name="left_ee_link"/>
  <transmission name="left_shoulder_pan_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="left_shoulder_pan_joint">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="left_shoulder_pan_motor">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="left_shoulder_lift_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="left_shoulder_lift_joint">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="left_shoulder_lift_motor">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="left_half_arm_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="left_arm_half_joint">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="left_arm_half_motor">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="left_elbow_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="left_elbow_joint">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="left_elbow_motor">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="left_wrist_spherical_1_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="left_wrist_spherical_1_joint">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="left_wrist_spherical_1_motor">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="left_wrist_spherical_2_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="left_wrist_spherical_2_joint">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="left_wrist_spherical_2_motor">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="left_wrist_3_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="left_wrist_3_joint">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="left_wrist_3_motor">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <joint name="right_kinova_robotiq_coupler_joint" type="fixed">
    <parent link="right_ee_link"/>
    <child link="right_robotiq_coupler_link"/>
    <origin rpy="-3.14159265 1.570796325 0" xyz="-0.16 0 0"/>
  </joint>
  <link name="right_robotiq_coupler_link">
    <visual>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/robotiq/visual/kinova_robotiq_coupler.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/robotiq/collision/kinova_robotiq_coupler.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.1"/>
      <origin xyz="0.0 0.0 0.0"/>
      <inertia ixx="0.000001" ixy="0.000000" ixz="0.000000" iyx="0.000000" iyy="0.000001" iyz="0.000000" izx="0.000000" izy="0.000000" izz="0.000001"/>
    </inertial>
  </link>
  <gazebo reference="right_robotiq_coupler_link">
    <implicitSpringDamper>1</implicitSpringDamper>
    <mu1>100000</mu1>
    <mu2>100000</mu2>
    <kp>100000000.0</kp>
    <kd>1.0</kd>
  </gazebo>
  <joint name="right_gripper_base_joint" type="fixed">
    <parent link="right_robotiq_coupler_link"/>
    <child link="right_gripper_base_link"/>
    <origin rpy="-1.570796325 1.570796325 0.0" xyz="0 0 -0.05105"/>
  </joint>
  <link name="right_gripper_base_link">
    <visual>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/robotiq/visual/robotiq_85_base_link.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/robotiq/collision/robotiq_85_base_link.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.636951"/>
      <origin xyz="0.0 0.0 0.0"/>
      <inertia ixx="0.000380" ixy="0.000000" ixz="0.000000" iyx="0.000000" iyy="0.001110" iyz="0.000000" izx="0.000000" izy="0.000000" izz="0.001171"/>
    </inertial>
  </link>
  <gazebo reference="right_gripper_base_link">
    <implicitSpringDamper>1</implicitSpringDamper>
    <mu1>100000</mu1>
    <mu2>100000</mu2>
    <kp>100000000.0</kp>
    <kd>1.0</kd>
  </gazebo>
  <joint name="right_gripper_finger1_joint" type="revolute">
    <parent link="right_gripper_base_link"/>
    <child link="right_gripper_finger1_knuckle_link"/>
    <axis xyz="0 0 1"/>
    <origin rpy="3.14159265 0.0 0.0" xyz="0.05490451627 0.03060114443 0.0"/>
    <limit effort="1000" lower="0.0" upper="0.804" velocity="2.0"/>
  </joint>
  <joint name="right_gripper_finger2_joint" type="revolute">
    <parent link="right_gripper_base_link"/>
    <child link="right_gripper_finger2_knuckle_link"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0.0 0.0 0.0" xyz="0.05490451627 -0.03060114443 0.0"/>
    <limit effort="1000" lower="-3.14" upper="3.14" velocity="100.0"/>
    <mimic joint="right_gripper_finger1_joint"/>
  </joint>
  <link name="right_gripper_finger1_knuckle_link">
    <visual>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/robotiq/visual/robotiq_85_knuckle_link.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/robotiq/collision/robotiq_85_knuckle_link.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.018491"/>
      <origin xyz="0.0 0.0 0.0"/>
      <inertia ixx="0.000009" ixy="-0.000001" ixz="0.000000" iyx="-0.000001" iyy="0.000001" iyz="0.000000" izx="0.000000" izy="0.000000" izz="0.000010"/>
    </inertial>
  </link>
  <gazebo reference="right_gripper_finger1_knuckle_link">
    <implicitSpringDamper>1</implicitSpringDamper>
    <mu1>100000</mu1>
    <mu2>100000</mu2>
    <kp>100000000.0</kp>
    <kd>1.0</kd>
  </gazebo>
  <link name="right_gripper_finger2_knuckle_link">
    <visual>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/robotiq/visual/robotiq_85_knuckle_link.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/robotiq/collision/robotiq_85_knuckle_link.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.018491"/>
      <origin xyz="0.0 0.0 0.0"/>
      <inertia ixx="0.000009" ixy="-0.000001" ixz="0.000000" iyx="-0.000001" iyy="0.000001" iyz="0.000000" izx="0.000000" izy="0.000000" izz="0.000010"/>
    </inertial>
  </link>
  <gazebo reference="right_gripper_finger2_knuckle_link">
    <implicitSpringDamper>1</implicitSpringDamper>
    <mu1>100000</mu1>
    <mu2>100000</mu2>
    <kp>100000000.0</kp>
    <kd>1.0</kd>
  </gazebo>
  <joint name="right_gripper_finger1_finger_joint" type="fixed">
    <parent link="right_gripper_finger1_knuckle_link"/>
    <child link="right_gripper_finger1_finger_link"/>
    <origin rpy="0 0 0" xyz="-0.00408552455 -0.03148604435 0.0"/>
  </joint>
  <joint name="right_gripper_finger2_finger_joint" type="fixed">
    <parent link="right_gripper_finger2_knuckle_link"/>
    <child link="right_gripper_finger2_finger_link"/>
    <origin rpy="0 0 0" xyz="-0.00408552455 -0.03148604435 0.0"/>
  </joint>
  <link name="right_gripper_finger1_finger_link">
    <visual>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/robotiq/visual/robotiq_85_finger_link.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/robotiq/collision/robotiq_85_finger_link.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.027309"/>
      <origin xyz="0.0 0.0 0.0"/>
      <inertia ixx="0.000003" ixy="-0.000002" ixz="0.000000" iyx="-0.000002" iyy="0.000021" iyz="0.000000" izx="0.000000" izy="0.000000" izz="0.000020"/>
    </inertial>
  </link>
  <gazebo reference="right_gripper_finger1_finger_link">
    <implicitSpringDamper>1</implicitSpringDamper>
    <mu1>100000</mu1>
    <mu2>100000</mu2>
    <kp>100000000.0</kp>
    <kd>1.0</kd>
  </gazebo>
  <link name="right_gripper_finger2_finger_link">
    <visual>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/robotiq/visual/robotiq_85_finger_link.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/robotiq/collision/robotiq_85_finger_link.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.027309"/>
      <origin xyz="0.0 0.0 0.0"/>
      <inertia ixx="0.000003" ixy="-0.000002" ixz="0.000000" iyx="-0.000002" iyy="0.000021" iyz="0.000000" izx="0.000000" izy="0.000000" izz="0.000020"/>
    </inertial>
  </link>
  <gazebo reference="right_gripper_finger2_finger_link">
    <implicitSpringDamper>1</implicitSpringDamper>
    <mu1>100000</mu1>
    <mu2>100000</mu2>
    <kp>100000000.0</kp>
    <kd>1.0</kd>
  </gazebo>
  <joint name="right_gripper_finger1_inner_knuckle_joint" type="continuous">
    <parent link="right_gripper_base_link"/>
    <child link="right_gripper_finger1_inner_knuckle_link"/>
    <axis xyz="0 0 1"/>
    <origin rpy="3.14159265 0.0 0.0" xyz="0.06142 0.0127 0"/>
    <limit effort="0.1" lower="-3.14" upper="3.14" velocity="100.0"/>
    <mimic joint="right_gripper_finger1_joint" offset="0"/>
  </joint>
  <joint name="right_gripper_finger2_inner_knuckle_joint" type="continuous">
    <parent link="right_gripper_base_link"/>
    <child link="right_gripper_finger2_inner_knuckle_link"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 0" xyz="0.06142 -0.0127 0"/>
    <limit effort="0.1" lower="-3.14" upper="3.14" velocity="100.0"/>
    <mimic joint="right_gripper_finger1_joint" offset="0"/>
  </joint>
  <link name="right_gripper_finger1_inner_knuckle_link">
    <visual>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/robotiq/visual/robotiq_85_inner_knuckle_link.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/robotiq/collision/robotiq_85_inner_knuckle_link.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.029951"/>
      <origin xyz="0.0 0.0 0.0"/>
      <inertia ixx="0.000039" ixy="0.000000" ixz="0.000000" iyx="0.000000" iyy="0.000005" iyz="0.000000" izx="0.000000" izy="0.000000" izz="0.000035"/>
    </inertial>
  </link>
  <gazebo reference="right_gripper_finger1_inner_knuckle_link">
    <implicitSpringDamper>1</implicitSpringDamper>
    <mu1>100000</mu1>
    <mu2>100000</mu2>
    <kp>100000000.0</kp>
    <kd>1.0</kd>
  </gazebo>
  <link name="right_gripper_finger2_inner_knuckle_link">
    <visual>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/robotiq/visual/robotiq_85_inner_knuckle_link.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/robotiq/collision/robotiq_85_inner_knuckle_link.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.029951"/>
      <origin xyz="0.0 0.0 0.0"/>
      <inertia ixx="0.000039" ixy="0.000000" ixz="0.000000" iyx="0.000000" iyy="0.000005" iyz="0.000000" izx="0.000000" izy="0.000000" izz="0.000035"/>
    </inertial>
  </link>
  <gazebo reference="right_gripper_finger2_inner_knuckle_link">
    <implicitSpringDamper>1</implicitSpringDamper>
    <mu1>100000</mu1>
    <mu2>100000</mu2>
    <kp>100000000.0</kp>
    <kd>1.0</kd>
  </gazebo>
  <joint name="right_gripper_finger1_finger_tip_joint" type="continuous">
    <parent link="right_gripper_finger1_inner_knuckle_link"/>
    <child link="right_gripper_finger1_finger_tip_link"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0.0 0.0 0.0" xyz="0.04303959807 -0.03759940821 0.0"/>
    <limit effort="0.1" lower="-3.14" upper="3.14" velocity="100.0"/>
    <mimic joint="right_gripper_finger1_joint" multiplier="-1"/>
  </joint>
  <joint name="right_gripper_finger2_finger_tip_joint" type="continuous">
    <parent link="right_gripper_finger2_inner_knuckle_link"/>
    <child link="right_gripper_finger2_finger_tip_link"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0.0 0.0 0.0" xyz="0.04303959807 -0.03759940821  0.0"/>
    <limit effort="0.1" lower="-3.14" upper="3.14" velocity="100.0"/>
    <mimic joint="right_gripper_finger1_joint" multiplier="-1"/>
  </joint>
  <link name="right_gripper_finger1_finger_tip_link">
    <visual>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/robotiq/visual/robotiq_85_finger_tip_link.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/robotiq/collision/robotiq_85_finger_tip_link.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.019555"/>
      <origin xyz="0.0 0.0 0.0"/>
      <inertia ixx="0.000002" ixy="0.000000" ixz="0.000000" iyx="0.000000" iyy="0.000005" iyz="0.000000" izx="0.000000" izy="0.000000" izz="0.000006"/>
    </inertial>
  </link>
  <gazebo reference="right_gripper_finger1_finger_tip_link">
    <implicitSpringDamper>1</implicitSpringDamper>
    <mu1>100000</mu1>
    <mu2>100000</mu2>
    <kp>100000000.0</kp>
    <kd>1.0</kd>
  </gazebo>
  <link name="right_gripper_finger2_finger_tip_link">
    <visual>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/robotiq/visual/robotiq_85_finger_tip_link.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/robotiq/collision/robotiq_85_finger_tip_link.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.019555"/>
      <origin xyz="0.0 0.0 0.0"/>
      <inertia ixx="0.000002" ixy="0.000000" ixz="0.000000" iyx="0.000000" iyy="0.000005" iyz="0.000000" izx="0.000000" izy="0.000000" izz="0.000006"/>
    </inertial>
  </link>
  <gazebo reference="right_gripper_finger2_finger_tip_link">
    <implicitSpringDamper>1</implicitSpringDamper>
    <mu1>100000</mu1>
    <mu2>100000</mu2>
    <kp>100000000.0</kp>
    <kd>1.0</kd>
  </gazebo>
  <transmission name="right_gripper_finger1_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="right_gripper_finger1_joint">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="right_gripper_finger1_motor">
      <mechanicalReduction>1</mechanicalReduction>
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo>
    <!-- loading plugin: mimic joints works in gazebo now -->
    <plugin filename="libroboticsgroup_gazebo_mimic_joint_plugin.so" name="right_mimic_robotiq_85_1">
      <joint>right_gripper_finger1_joint</joint>
      <mimicJoint>right_gripper_finger2_joint</mimicJoint>
      <!--multiplier>1.0</multiplier>
                <offset>0.0</offset-->
    </plugin>
    <plugin filename="libroboticsgroup_gazebo_mimic_joint_plugin.so" name="right_mimic_robotiq_85_2">
      <joint>right_gripper_finger1_joint</joint>
      <mimicJoint>right_gripper_finger2_inner_knuckle_joint</mimicJoint>
      <!--multiplier>1.0</multiplier>
                <offset>0.0</offset-->
    </plugin>
    <plugin filename="libroboticsgroup_gazebo_mimic_joint_plugin.so" name="right_mimic_robotiq_85_3">
      <joint>right_gripper_finger1_joint</joint>
      <mimicJoint>right_gripper_finger1_inner_knuckle_joint</mimicJoint>
      <!--multiplier>1.0</multiplier>
                <offset>0.0</offset-->
    </plugin>
    <plugin filename="libroboticsgroup_gazebo_mimic_joint_plugin.so" name="right_mimic_robotiq_85_4">
      <joint>right_gripper_finger1_joint</joint>
      <mimicJoint>right_gripper_finger1_finger_tip_joint</mimicJoint>
      <multiplier>-1.0</multiplier>
      <offset>0.0</offset>
    </plugin>
    <plugin filename="libroboticsgroup_gazebo_mimic_joint_plugin.so" name="right_mimic_robotiq_85_5">
      <joint>right_gripper_finger1_joint</joint>
      <mimicJoint>right_gripper_finger2_finger_tip_joint</mimicJoint>
      <multiplier>-1.0</multiplier>
      <offset>0.0</offset>
    </plugin>
  </gazebo>
  <joint name="left_kinova_robotiq_coupler_joint" type="fixed">
    <parent link="left_ee_link"/>
    <child link="left_robotiq_coupler_link"/>
    <origin rpy="-3.14159265 1.570796325 0" xyz="-0.16 0 0"/>
  </joint>
  <link name="left_robotiq_coupler_link">
    <visual>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/robotiq/visual/kinova_robotiq_coupler.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/robotiq/collision/kinova_robotiq_coupler.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.1"/>
      <origin xyz="0.0 0.0 0.0"/>
      <inertia ixx="0.000001" ixy="0.000000" ixz="0.000000" iyx="0.000000" iyy="0.000001" iyz="0.000000" izx="0.000000" izy="0.000000" izz="0.000001"/>
    </inertial>
  </link>
  <gazebo reference="left_robotiq_coupler_link">
    <implicitSpringDamper>1</implicitSpringDamper>
    <mu1>100000</mu1>
    <mu2>100000</mu2>
    <kp>100000000.0</kp>
    <kd>1.0</kd>
  </gazebo>
  <joint name="left_gripper_base_joint" type="fixed">
    <parent link="left_robotiq_coupler_link"/>
    <child link="left_gripper_base_link"/>
    <origin rpy="-1.570796325 1.570796325 0.0" xyz="0 0 -0.05105"/>
  </joint>
  <link name="left_gripper_base_link">
    <visual>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/robotiq/visual/robotiq_85_base_link.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/robotiq/collision/robotiq_85_base_link.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.636951"/>
      <origin xyz="0.0 0.0 0.0"/>
      <inertia ixx="0.000380" ixy="0.000000" ixz="0.000000" iyx="0.000000" iyy="0.001110" iyz="0.000000" izx="0.000000" izy="0.000000" izz="0.001171"/>
    </inertial>
  </link>
  <gazebo reference="left_gripper_base_link">
    <implicitSpringDamper>1</implicitSpringDamper>
    <mu1>100000</mu1>
    <mu2>100000</mu2>
    <kp>100000000.0</kp>
    <kd>1.0</kd>
  </gazebo>
  <joint name="left_gripper_finger1_joint" type="revolute">
    <parent link="left_gripper_base_link"/>
    <child link="left_gripper_finger1_knuckle_link"/>
    <axis xyz="0 0 1"/>
    <origin rpy="3.14159265 0.0 0.0" xyz="0.05490451627 0.03060114443 0.0"/>
    <limit effort="1000" lower="0.0" upper="0.804" velocity="2.0"/>
  </joint>
  <joint name="left_gripper_finger2_joint" type="revolute">
    <parent link="left_gripper_base_link"/>
    <child link="left_gripper_finger2_knuckle_link"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0.0 0.0 0.0" xyz="0.05490451627 -0.03060114443 0.0"/>
    <limit effort="1000" lower="-3.14" upper="3.14" velocity="100.0"/>
    <mimic joint="left_gripper_finger1_joint"/>
  </joint>
  <link name="left_gripper_finger1_knuckle_link">
    <visual>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/robotiq/visual/robotiq_85_knuckle_link.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/robotiq/collision/robotiq_85_knuckle_link.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.018491"/>
      <origin xyz="0.0 0.0 0.0"/>
      <inertia ixx="0.000009" ixy="-0.000001" ixz="0.000000" iyx="-0.000001" iyy="0.000001" iyz="0.000000" izx="0.000000" izy="0.000000" izz="0.000010"/>
    </inertial>
  </link>
  <gazebo reference="left_gripper_finger1_knuckle_link">
    <implicitSpringDamper>1</implicitSpringDamper>
    <mu1>100000</mu1>
    <mu2>100000</mu2>
    <kp>100000000.0</kp>
    <kd>1.0</kd>
  </gazebo>
  <link name="left_gripper_finger2_knuckle_link">
    <visual>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/robotiq/visual/robotiq_85_knuckle_link.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/robotiq/collision/robotiq_85_knuckle_link.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.018491"/>
      <origin xyz="0.0 0.0 0.0"/>
      <inertia ixx="0.000009" ixy="-0.000001" ixz="0.000000" iyx="-0.000001" iyy="0.000001" iyz="0.000000" izx="0.000000" izy="0.000000" izz="0.000010"/>
    </inertial>
  </link>
  <gazebo reference="left_gripper_finger2_knuckle_link">
    <implicitSpringDamper>1</implicitSpringDamper>
    <mu1>100000</mu1>
    <mu2>100000</mu2>
    <kp>100000000.0</kp>
    <kd>1.0</kd>
  </gazebo>
  <joint name="left_gripper_finger1_finger_joint" type="fixed">
    <parent link="left_gripper_finger1_knuckle_link"/>
    <child link="left_gripper_finger1_finger_link"/>
    <origin rpy="0 0 0" xyz="-0.00408552455 -0.03148604435 0.0"/>
  </joint>
  <joint name="left_gripper_finger2_finger_joint" type="fixed">
    <parent link="left_gripper_finger2_knuckle_link"/>
    <child link="left_gripper_finger2_finger_link"/>
    <origin rpy="0 0 0" xyz="-0.00408552455 -0.03148604435 0.0"/>
  </joint>
  <link name="left_gripper_finger1_finger_link">
    <visual>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/robotiq/visual/robotiq_85_finger_link.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/robotiq/collision/robotiq_85_finger_link.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.027309"/>
      <origin xyz="0.0 0.0 0.0"/>
      <inertia ixx="0.000003" ixy="-0.000002" ixz="0.000000" iyx="-0.000002" iyy="0.000021" iyz="0.000000" izx="0.000000" izy="0.000000" izz="0.000020"/>
    </inertial>
  </link>
  <gazebo reference="left_gripper_finger1_finger_link">
    <implicitSpringDamper>1</implicitSpringDamper>
    <mu1>100000</mu1>
    <mu2>100000</mu2>
    <kp>100000000.0</kp>
    <kd>1.0</kd>
  </gazebo>
  <link name="left_gripper_finger2_finger_link">
    <visual>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/robotiq/visual/robotiq_85_finger_link.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/robotiq/collision/robotiq_85_finger_link.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.027309"/>
      <origin xyz="0.0 0.0 0.0"/>
      <inertia ixx="0.000003" ixy="-0.000002" ixz="0.000000" iyx="-0.000002" iyy="0.000021" iyz="0.000000" izx="0.000000" izy="0.000000" izz="0.000020"/>
    </inertial>
  </link>
  <gazebo reference="left_gripper_finger2_finger_link">
    <implicitSpringDamper>1</implicitSpringDamper>
    <mu1>100000</mu1>
    <mu2>100000</mu2>
    <kp>100000000.0</kp>
    <kd>1.0</kd>
  </gazebo>
  <joint name="left_gripper_finger1_inner_knuckle_joint" type="continuous">
    <parent link="left_gripper_base_link"/>
    <child link="left_gripper_finger1_inner_knuckle_link"/>
    <axis xyz="0 0 1"/>
    <origin rpy="3.14159265 0.0 0.0" xyz="0.06142 0.0127 0"/>
    <limit effort="0.1" lower="-3.14" upper="3.14" velocity="100.0"/>
    <mimic joint="left_gripper_finger1_joint" offset="0"/>
  </joint>
  <joint name="left_gripper_finger2_inner_knuckle_joint" type="continuous">
    <parent link="left_gripper_base_link"/>
    <child link="left_gripper_finger2_inner_knuckle_link"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 0" xyz="0.06142 -0.0127 0"/>
    <limit effort="0.1" lower="-3.14" upper="3.14" velocity="100.0"/>
    <mimic joint="left_gripper_finger1_joint" offset="0"/>
  </joint>
  <link name="left_gripper_finger1_inner_knuckle_link">
    <visual>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/robotiq/visual/robotiq_85_inner_knuckle_link.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/robotiq/collision/robotiq_85_inner_knuckle_link.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.029951"/>
      <origin xyz="0.0 0.0 0.0"/>
      <inertia ixx="0.000039" ixy="0.000000" ixz="0.000000" iyx="0.000000" iyy="0.000005" iyz="0.000000" izx="0.000000" izy="0.000000" izz="0.000035"/>
    </inertial>
  </link>
  <gazebo reference="left_gripper_finger1_inner_knuckle_link">
    <implicitSpringDamper>1</implicitSpringDamper>
    <mu1>100000</mu1>
    <mu2>100000</mu2>
    <kp>100000000.0</kp>
    <kd>1.0</kd>
  </gazebo>
  <link name="left_gripper_finger2_inner_knuckle_link">
    <visual>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/robotiq/visual/robotiq_85_inner_knuckle_link.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/robotiq/collision/robotiq_85_inner_knuckle_link.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.029951"/>
      <origin xyz="0.0 0.0 0.0"/>
      <inertia ixx="0.000039" ixy="0.000000" ixz="0.000000" iyx="0.000000" iyy="0.000005" iyz="0.000000" izx="0.000000" izy="0.000000" izz="0.000035"/>
    </inertial>
  </link>
  <gazebo reference="left_gripper_finger2_inner_knuckle_link">
    <implicitSpringDamper>1</implicitSpringDamper>
    <mu1>100000</mu1>
    <mu2>100000</mu2>
    <kp>100000000.0</kp>
    <kd>1.0</kd>
  </gazebo>
  <joint name="left_gripper_finger1_finger_tip_joint" type="continuous">
    <parent link="left_gripper_finger1_inner_knuckle_link"/>
    <child link="left_gripper_finger1_finger_tip_link"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0.0 0.0 0.0" xyz="0.04303959807 -0.03759940821 0.0"/>
    <limit effort="0.1" lower="-3.14" upper="3.14" velocity="100.0"/>
    <mimic joint="left_gripper_finger1_joint" multiplier="-1"/>
  </joint>
  <joint name="left_gripper_finger2_finger_tip_joint" type="continuous">
    <parent link="left_gripper_finger2_inner_knuckle_link"/>
    <child link="left_gripper_finger2_finger_tip_link"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0.0 0.0 0.0" xyz="0.04303959807 -0.03759940821  0.0"/>
    <limit effort="0.1" lower="-3.14" upper="3.14" velocity="100.0"/>
    <mimic joint="left_gripper_finger1_joint" multiplier="-1"/>
  </joint>
  <link name="left_gripper_finger1_finger_tip_link">
    <visual>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/robotiq/visual/robotiq_85_finger_tip_link.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/robotiq/collision/robotiq_85_finger_tip_link.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.019555"/>
      <origin xyz="0.0 0.0 0.0"/>
      <inertia ixx="0.000002" ixy="0.000000" ixz="0.000000" iyx="0.000000" iyy="0.000005" iyz="0.000000" izx="0.000000" izy="0.000000" izz="0.000006"/>
    </inertial>
  </link>
  <gazebo reference="left_gripper_finger1_finger_tip_link">
    <implicitSpringDamper>1</implicitSpringDamper>
    <mu1>100000</mu1>
    <mu2>100000</mu2>
    <kp>100000000.0</kp>
    <kd>1.0</kd>
  </gazebo>
  <link name="left_gripper_finger2_finger_tip_link">
    <visual>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/robotiq/visual/robotiq_85_finger_tip_link.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://movo_description/meshes/manipulation/robotiq/collision/robotiq_85_finger_tip_link.stl"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.019555"/>
      <origin xyz="0.0 0.0 0.0"/>
      <inertia ixx="0.000002" ixy="0.000000" ixz="0.000000" iyx="0.000000" iyy="0.000005" iyz="0.000000" izx="0.000000" izy="0.000000" izz="0.000006"/>
    </inertial>
  </link>
  <gazebo reference="left_gripper_finger2_finger_tip_link">
    <implicitSpringDamper>1</implicitSpringDamper>
    <mu1>100000</mu1>
    <mu2>100000</mu2>
    <kp>100000000.0</kp>
    <kd>1.0</kd>
  </gazebo>
  <transmission name="left_gripper_finger1_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="left_gripper_finger1_joint">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="left_gripper_finger1_motor">
      <mechanicalReduction>1</mechanicalReduction>
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </actuator>
  </transmission>
  <gazebo>
    <!-- loading plugin: mimic joints works in gazebo now -->
    <plugin filename="libroboticsgroup_gazebo_mimic_joint_plugin.so" name="left_mimic_robotiq_85_1">
      <joint>left_gripper_finger1_joint</joint>
      <mimicJoint>left_gripper_finger2_joint</mimicJoint>
      <!--multiplier>1.0</multiplier>
                <offset>0.0</offset-->
    </plugin>
    <plugin filename="libroboticsgroup_gazebo_mimic_joint_plugin.so" name="left_mimic_robotiq_85_2">
      <joint>left_gripper_finger1_joint</joint>
      <mimicJoint>left_gripper_finger2_inner_knuckle_joint</mimicJoint>
      <!--multiplier>1.0</multiplier>
                <offset>0.0</offset-->
    </plugin>
    <plugin filename="libroboticsgroup_gazebo_mimic_joint_plugin.so" name="left_mimic_robotiq_85_3">
      <joint>left_gripper_finger1_joint</joint>
      <mimicJoint>left_gripper_finger1_inner_knuckle_joint</mimicJoint>
      <!--multiplier>1.0</multiplier>
                <offset>0.0</offset-->
    </plugin>
    <plugin filename="libroboticsgroup_gazebo_mimic_joint_plugin.so" name="left_mimic_robotiq_85_4">
      <joint>left_gripper_finger1_joint</joint>
      <mimicJoint>left_gripper_finger1_finger_tip_joint</mimicJoint>
      <multiplier>-1.0</multiplier>
      <offset>0.0</offset>
    </plugin>
    <plugin filename="libroboticsgroup_gazebo_mimic_joint_plugin.so" name="left_mimic_robotiq_85_5">
      <joint>left_gripper_finger1_joint</joint>
      <mimicJoint>left_gripper_finger2_finger_tip_joint</mimicJoint>
      <multiplier>-1.0</multiplier>
      <offset>0.0</offset>
    </plugin>
  </gazebo>
</robot>
