<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- |    This URDF was edited to fix inertias.                                        | -->
<!-- =================================================================================== -->
<!-- This file contains the description of the ANYmal B robot. -->
<robot name="anymal"
  xmlns:xacro="http://www.ros.org/wiki/xacro">
  <material name="black">
    <color rgba="0.0 0.0 0.0 1.0"/>
  </material>
  <material name="blue">
    <color rgba="0.0 0.0 0.8 1.0"/>
  </material>
  <material name="green">
    <color rgba="0.0 0.8 0.0 1.0"/>
  </material>
  <material name="grey">
    <color rgba="0.2 0.2 0.2 1.0"/>
  </material>
  <material name="light_grey">
    <color rgba="0.4 0.4 0.4 1.0"/>
  </material>
  <material name="orange">
    <color rgba="1.0 0.423529411765 0.0392156862745 1.0"/>
  </material>
  <material name="brown">
    <color rgba="0.870588235294 0.811764705882 0.764705882353 1.0"/>
  </material>
  <material name="red">
    <color rgba="0.8 0.0 0.0 1.0"/>
  </material>
  <material name="white">
    <color rgba="1.0 1.0 1.0 1.0"/>
  </material>
  <!-- Material for the visual primitives -->
  <material name="anymal_material">
    <color rgba="0.7 0.7 0.7 1."/>
  </material>
  <!-- Xacro:Properties -->
  <!-- [m] -->
  <!-- Base link -->
  <link name="base">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/anymal_b_simple_description/meshes/base/anymal_base.dae" scale="1 1 1"/>
      </geometry>
      <material name="anymal_material"/>
    </visual>
    <collision>
      <!-- Main Body -->
      <origin rpy="0 0 0" xyz="0 0 0.08"/>
      <geometry>
        <box size="0.531 0.27 0.24"/>
      </geometry>
    </collision>
    <collision>
      <!-- HAA actuators -->
      <origin rpy="0 1.57079632679 0" xyz="0.227 0.116 0"/>
      <geometry>
        <cylinder length="0.1" radius="0.05"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="0 1.57079632679 0" xyz="0.227 -0.116 0"/>
      <geometry>
        <cylinder length="0.1" radius="0.05"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="0 1.57079632679 0" xyz="-0.227 0.116 0"/>
      <geometry>
        <cylinder length="0.1" radius="0.05"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="0 1.57079632679 0" xyz="-0.227 -0.116 0"/>
      <geometry>
        <cylinder length="0.1" radius="0.05"/>
      </geometry>
    </collision>
    <collision>
      <!-- Belly plate front bump -->
      <origin rpy="0 0 0" xyz="0.2155 0.0 -0.09"/>
      <geometry>
        <box size="0.1 0.1 0.07"/>
      </geometry>
    </collision>
    <collision>
      <!-- Belly plate hind bump-->
      <origin rpy="0 0 0" xyz="-0.2155 0.0 -0.09"/>
      <geometry>
        <box size="0.1 0.1 0.07"/>
      </geometry>
    </collision>
    <collision>
      <!-- Belly plate middle bump-->
      <origin rpy="0 0 0" xyz="0 0 -0.09"/>
      <geometry>
        <box size="0.531 0.02 0.07"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="1e-6"/>
      <inertia ixx="1e-6" ixy="1e-6" ixz="1e-6" iyy="1e-6" iyz="1e-6" izz="1e-6"/>
    </inertial>
  </link>
  <!-- Fixed joint to add dummy inertia link -->
  <joint name="base_to_base_inertia" type="fixed">
    <parent link="base"/>
    <child link="base_inertia"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
  </joint>
  <!-- Dummy inertia link, because KDL cannot have inertia on the base link -->
  <link name="base_inertia">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.001960558279 -0.001413217745 0.050207125344"/>
      <mass value="16.793507758"/>
      <inertia ixx="0.217391101503" ixy="-0.00132873239126" ixz="-0.00228200226173" iyy="0.639432546734" iyz="-0.00138078263145" izz="0.62414077654"/>
    </inertial>
  </link>
  <!-- Xacro:Properties -->
  <!-- [kg * m^2] -->
  <!-- [A] -->
  <link name="LF_HIP">
    <visual>
      <origin rpy="0 0 0.0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/anymal_b_simple_description/meshes/hip/anymal_hip_l.dae" scale="1 1 1"/>
      </geometry>
      <material name="anymal_material"/>
    </visual>
    <collision>
      <!-- Protector -->
      <origin rpy="1.57079632679 0 0" xyz="0.0635 -0.009 0.0"/>
      <geometry>
        <cylinder length="0.1" radius="0.08"/>
      </geometry>
    </collision>
    <collision>
      <!-- Heatfins -->
      <origin rpy="1.57079632679 0 0" xyz="0.0635 -0.074 0.0"/>
      <geometry>
        <cylinder length="0.03" radius="0.045"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.064516258147 -0.003787101702 -0.000152184388"/>
      <mass value="1.42462064"/>
      <inertia ixx="0.00243023349564" ixy="-1.53023971e-05" ixz="-2.1819095354e-05" iyy="0.00230257239103" iyz="2.6473021273e-05" izz="0.0019806759227"/>
    </inertial>
  </link>
  <!-- Hip joint -->
  <joint name="LF_HAA" type="revolute">
    <parent link="base"/>
    <child link="LF_HIP"/>
    <origin xyz="0.277 0.116 0.0"/>
    <axis xyz="1 0 0"/>
    <limit command_effort="40" current="10" effort="80" gear_velocity="10" lower="-9.42" upper="9.42" velocity="15"/>
    <dynamics damping="0.0" friction="0.0"/>
  </joint>
  <!-- Xacro:Properties -->
  <!-- [m] -->
  <!-- [A] -->
  <link name="LF_THIGH">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/anymal_b_simple_description/meshes/thigh/anymal_thigh.dae" scale="1 1 1"/>
      </geometry>
      <material name="anymal_material"/>
    </visual>
    <collision>
      <!-- thigh with heat fins -->
      <origin rpy="0.145 0 0" xyz="0 0.035 -0.125"/>
      <geometry>
        <box size="0.08 0.04 0.25"/>
      </geometry>
    </collision>
    <collision>
      <!-- KFE actuator -->
      <origin rpy="1.57079632679 0 0" xyz="0.0 0.069 -0.25"/>
      <geometry>
        <cylinder length="0.12" radius="0.06"/>
      </geometry>
    </collision>
    <collision>
      <!-- upper protector -->
      <origin rpy="1.71579632679 0 0" xyz="0 -0.005 0"/>
      <geometry>
        <cylinder length="0.12" radius="0.066"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="-0.003897968082 0.054226618537 -0.214583373795"/>
      <mass value="1.634976467"/>
      <inertia ixx="0.0120367944369" ixy="6.762065206e-05" ixz="0.000287806340448" iyy="0.0120643637939" iyz="-0.00140610131218" izz="0.00249422574881"/>
    </inertial>
  </link>
  <!-- Thigh joint -->
  <joint name="LF_HFE" type="revolute">
    <parent link="LF_HIP"/>
    <child link="LF_THIGH"/>
    <origin xyz="0.0635                    0.041                    0.0"/>
    <axis xyz="0 1 0"/>
    <limit command_effort="40" current="10" effort="80" gear_velocity="10" lower="-9.42" upper="9.42" velocity="15.0"/>
    <dynamics damping="0.0" friction="0.0"/>
  </joint>
  <!-- Xacro:Properties -->
  <!-- [m] -->
  <!-- [A] -->
  <link name="LF_SHANK">
    <visual>
      <origin rpy="0 0 0.0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/anymal_b_simple_description/meshes/shank/anymal_shank_l.dae" scale="1 1 1"/>
      </geometry>
      <material name="anymal_material"/>
    </visual>
    <collision>
      <origin rpy="0 1.57079632679 0" xyz="0.065 -0.015 0.01"/>
      <geometry>
        <box size="0.08 0.07 0.13"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.030816858139 -0.004617229294 0.000893125713"/>
      <mass value="0.207204302"/>
      <inertia ixx="0.0002104880248" ixy="-5.6750980345e-05" ixz="1.0127699391e-05" iyy="0.000676270210023" iyz="-8.22869024e-07" izz="0.000545032674924"/>
    </inertial>
  </link>
  <!-- Shank joint -->
  <joint name="LF_KFE" type="revolute">
    <parent link="LF_THIGH"/>
    <child link="LF_SHANK"/>
    <origin xyz="0.0 0.109 -0.25"/>
    <axis xyz="0 1 0"/>
    <limit command_effort="40" current="10" effort="80" gear_velocity="10" lower="-9.42" upper="9.42" velocity="15.0"/>
    <dynamics damping="0.0" friction="0.0"/>
  </joint>
  <!-- Xacro:Properties -->
  <!-- [m] -->
  <!-- Shank to Adapter joint -->
  <joint name="LF_SHANK_TO_ADAPTER" type="fixed">
    <parent link="LF_SHANK"/>
    <child link="LF_ADAPTER"/>
    <origin rpy="0.0 0.0 0.0" xyz="0.1 -0.02 0.0"/>
  </joint>
  <!-- Adapter link -->
  <link name="LF_ADAPTER">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0.032"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/anymal_b_simple_description/meshes/foot/anymal_foot.dae" scale="1 1 1"/>
      </geometry>
      <material name="anymal_material"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 -0.160625"/>
      <geometry>
        <cylinder length="0.32125" radius="0.015"/>
      </geometry>
      <material name="anymal_material"/>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="-8.66e-10 -1.472e-09 -0.244345749188"/>
      <mass value="0.140170767"/>
      <inertia ixx="0.00159938741862" ixy="-9.32e-13" ixz="1.039e-11" iyy="0.00159938741932" iyz="1.7563e-11" izz="5.4423177329e-05"/>
    </inertial>
  </link>
  <!-- Adapter to Foot joint -->
  <joint name="LF_ADAPTER_TO_FOOT" type="fixed">
    <parent link="LF_ADAPTER"/>
    <child link="LF_FOOT"/>
    <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 -0.32125"/>
  </joint>
  <!-- Foot link -->
  <link name="LF_FOOT">
    <collision>
      <origin xyz="0 0 0.02325"/>
      <geometry>
        <sphere radius="0.031"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.001"/>
      <inertia ixx="4.90e-07" ixy="0.0" ixz="0.0" iyy="4.90e-07" iyz="0.0" izz="4.90e-07"/>
      <!-- for a sphere: 2/5 m * r**2 -->
    </inertial>
    <!-- Bullet specific parameters -->
    <contact>
      <lateral_friction value="1.0"/>
      <stiffness value="300000"/>
      <damping value="15000"/>
      <!--<restitution value="0.5"/>-->
    </contact>
  </link>
  <!-- Gazebo customization -->
  <gazebo reference="LF_FOOT">
    <kp>1000000.0</kp>
    <kd>100.0</kd>
    <mu1>0.8</mu1>
    <mu2>0.8</mu2>
  </gazebo>
  <!-- Xacro:Properties -->
  <!-- [kg * m^2] -->
  <!-- [A] -->
  <link name="RF_HIP">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/anymal_b_simple_description/meshes/hip/anymal_hip_r.dae" scale="1 1 1"/>
      </geometry>
      <material name="anymal_material"/>
    </visual>
    <collision>
      <!-- Protector -->
      <origin rpy="1.57079632679 0 0" xyz="0.0635 0.009 0.0"/>
      <geometry>
        <cylinder length="0.1" radius="0.08"/>
      </geometry>
    </collision>
    <collision>
      <!-- Heatfins -->
      <origin rpy="1.57079632679 0 0" xyz="0.0635 0.074 0.0"/>
      <geometry>
        <cylinder length="0.03" radius="0.045"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.064516258147 0.003787101702 -0.000152184388"/>
      <mass value="1.42462064"/>
      <inertia ixx="0.00243023349564" ixy="1.53023971e-05" ixz="-2.1819095354e-05" iyy="0.00230257239103" iyz="-2.6473021273e-05" izz="0.0019806759227"/>
    </inertial>
  </link>
  <!-- Hip joint -->
  <joint name="RF_HAA" type="revolute">
    <parent link="base"/>
    <child link="RF_HIP"/>
    <origin xyz="0.277 -0.116 0.0"/>
    <axis xyz="1 0 0"/>
    <limit command_effort="40" current="10" effort="80" gear_velocity="10" lower="-9.42" upper="9.42" velocity="15"/>
    <dynamics damping="0.0" friction="0.0"/>
  </joint>
  <!-- Xacro:Properties -->
  <!-- [m] -->
  <!-- [A] -->
  <link name="RF_THIGH">
    <visual>
      <origin rpy="3.1416 3.1416 0.0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/anymal_b_simple_description/meshes/thigh/anymal_thigh.dae" scale="1 1 1"/>
      </geometry>
      <material name="anymal_material"/>
    </visual>
    <collision>
      <!-- thigh with heat fins -->
      <origin rpy="-0.145 0 0" xyz="0 -0.035 -0.125"/>
      <geometry>
        <box size="0.08 0.04 0.25"/>
      </geometry>
    </collision>
    <collision>
      <!-- KFE actuator -->
      <origin rpy="1.57079632679 0 0" xyz="0.0 -0.069 -0.25"/>
      <geometry>
        <cylinder length="0.12" radius="0.06"/>
      </geometry>
    </collision>
    <collision>
      <!-- upper protector -->
      <origin rpy="-1.71579632679 0 0" xyz="0 0.005 0"/>
      <geometry>
        <cylinder length="0.12" radius="0.066"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="-0.003897968082 -0.054226618537 -0.214583373795"/>
      <mass value="1.634976467"/>
      <inertia ixx="0.0120367944369" ixy="-6.762065206e-05" ixz="0.000287806340448" iyy="0.0120643637939" iyz="0.00140610131218" izz="0.00249422574881"/>
    </inertial>
  </link>
  <!-- Thigh joint -->
  <joint name="RF_HFE" type="revolute">
    <parent link="RF_HIP"/>
    <child link="RF_THIGH"/>
    <origin xyz="0.0635                    -0.041                    0.0"/>
    <axis xyz="0 1 0"/>
    <limit command_effort="40" current="10" effort="80" gear_velocity="10" lower="-9.42" upper="9.42" velocity="15.0"/>
    <dynamics damping="0.0" friction="0.0"/>
  </joint>
  <!-- Xacro:Properties -->
  <!-- [m] -->
  <!-- [A] -->
  <link name="RF_SHANK">
    <visual>
      <origin rpy="0 0 0.0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/anymal_b_simple_description/meshes/shank/anymal_shank_r.dae" scale="1 1 1"/>
      </geometry>
      <material name="anymal_material"/>
    </visual>
    <collision>
      <origin rpy="0 1.57079632679 0" xyz="0.065 0.015 0.01"/>
      <geometry>
        <box size="0.08 0.07 0.13"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.030816858139 0.004617229294 0.000893125713"/>
      <mass value="0.207204302"/>
      <inertia ixx="0.0002104880248" ixy="5.6750980345e-05" ixz="1.0127699391e-05" iyy="0.000676270210023" iyz="8.22869024e-07" izz="0.000545032674924"/>
    </inertial>
  </link>
  <!-- Shank joint -->
  <joint name="RF_KFE" type="revolute">
    <parent link="RF_THIGH"/>
    <child link="RF_SHANK"/>
    <origin xyz="0.0 -0.109 -0.25"/>
    <axis xyz="0 1 0"/>
    <limit command_effort="40" current="10" effort="80" gear_velocity="10" lower="-9.42" upper="9.42" velocity="15.0"/>
    <dynamics damping="0.0" friction="0.0"/>
  </joint>
  <!-- Xacro:Properties -->
  <!-- [m] -->
  <!-- Shank to Adapter joint -->
  <joint name="RF_SHANK_TO_ADAPTER" type="fixed">
    <parent link="RF_SHANK"/>
    <child link="RF_ADAPTER"/>
    <origin rpy="-0.0 0.0 -0.0" xyz="0.1 0.02 0.0"/>
  </joint>
  <!-- Adapter link -->
  <link name="RF_ADAPTER">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0.032"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/anymal_b_simple_description/meshes/foot/anymal_foot.dae" scale="1 1 1"/>
      </geometry>
      <material name="anymal_material"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 -0.160625"/>
      <geometry>
        <cylinder length="0.32125" radius="0.015"/>
      </geometry>
      <material name="anymal_material"/>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="-8.66e-10 -1.472e-09 -0.244345749188"/>
      <mass value="0.140170767"/>
      <inertia ixx="0.00159938741862" ixy="-9.32e-13" ixz="1.039e-11" iyy="0.00159938741932" iyz="1.7563e-11" izz="5.4423177329e-05"/>
    </inertial>
  </link>
  <!-- Adapter to Foot joint -->
  <joint name="RF_ADAPTER_TO_FOOT" type="fixed">
    <parent link="RF_ADAPTER"/>
    <child link="RF_FOOT"/>
    <origin rpy="-0.0 0.0 -0.0" xyz="0.0 -0.0 -0.32125"/>
  </joint>
  <!-- Foot link -->
  <link name="RF_FOOT">
    <collision>
      <origin xyz="0 0 0.02325"/>
      <geometry>
        <sphere radius="0.031"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.001"/>
      <inertia ixx="4.90e-07" ixy="0.0" ixz="0.0" iyy="4.90e-07" iyz="0.0" izz="4.90e-07"/>
      <!-- for a sphere: 2/5 m * r**2 -->
    </inertial>
    <!-- Bullet specific parameters -->
    <contact>
      <lateral_friction value="1.0"/>
      <stiffness value="300000"/>
      <damping value="15000"/>
      <!--<restitution value="0.5"/>-->
    </contact>
  </link>
  <!-- Gazebo customization -->
  <gazebo reference="RF_FOOT">
    <kp>1000000.0</kp>
    <kd>100.0</kd>
    <mu1>0.8</mu1>
    <mu2>0.8</mu2>
  </gazebo>
  <!-- Xacro:Properties -->
  <!-- [kg * m^2] -->
  <!-- [A] -->
  <link name="LH_HIP">
    <visual>
      <origin rpy="0 0 -3.14159265359" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/anymal_b_simple_description/meshes/hip/anymal_hip_r.dae" scale="1 1 1"/>
      </geometry>
      <material name="anymal_material"/>
    </visual>
    <collision>
      <!-- Protector -->
      <origin rpy="-1.57079632679 0 0" xyz="-0.0635 -0.009 0.0"/>
      <geometry>
        <cylinder length="0.1" radius="0.08"/>
      </geometry>
    </collision>
    <collision>
      <!-- Heatfins -->
      <origin rpy="-1.57079632679 0 0" xyz="-0.0635 -0.074 0.0"/>
      <geometry>
        <cylinder length="0.03" radius="0.045"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="-0.064516258147 -0.003787101702 -0.000152184388"/>
      <mass value="1.42462064"/>
      <inertia ixx="0.00243023349564" ixy="1.53023971e-05" ixz="2.1819095354e-05" iyy="0.00230257239103" iyz="2.6473021273e-05" izz="0.0019806759227"/>
    </inertial>
  </link>
  <!-- Hip joint -->
  <joint name="LH_HAA" type="revolute">
    <parent link="base"/>
    <child link="LH_HIP"/>
    <origin xyz="-0.277 0.116 0.0"/>
    <axis xyz="1 0 0"/>
    <limit command_effort="40" current="10" effort="80" gear_velocity="10" lower="-9.42" upper="9.42" velocity="15"/>
    <dynamics damping="0.0" friction="0.0"/>
  </joint>
  <!-- Xacro:Properties -->
  <!-- [m] -->
  <!-- [A] -->
  <link name="LH_THIGH">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/anymal_b_simple_description/meshes/thigh/anymal_thigh.dae" scale="1 1 1"/>
      </geometry>
      <material name="anymal_material"/>
    </visual>
    <collision>
      <!-- thigh with heat fins -->
      <origin rpy="0.145 0 0" xyz="0 0.035 -0.125"/>
      <geometry>
        <box size="0.08 0.04 0.25"/>
      </geometry>
    </collision>
    <collision>
      <!-- KFE actuator -->
      <origin rpy="1.57079632679 0 0" xyz="0.0 0.069 -0.25"/>
      <geometry>
        <cylinder length="0.12" radius="0.06"/>
      </geometry>
    </collision>
    <collision>
      <!-- upper protector -->
      <origin rpy="1.71579632679 0 0" xyz="0 -0.005 0"/>
      <geometry>
        <cylinder length="0.12" radius="0.066"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.003897968082 0.054226618537 -0.214583373795"/>
      <mass value="1.634976467"/>
      <inertia ixx="0.0120367944369" ixy="-6.762065206e-05" ixz="-0.000287806340448" iyy="0.0120643637939" iyz="-0.00140610131218" izz="0.00249422574881"/>
    </inertial>
  </link>
  <!-- Thigh joint -->
  <joint name="LH_HFE" type="revolute">
    <parent link="LH_HIP"/>
    <child link="LH_THIGH"/>
    <origin xyz="-0.0635                    0.041                    0.0"/>
    <axis xyz="0 1 0"/>
    <limit command_effort="40" current="10" effort="80" gear_velocity="10" lower="-9.42" upper="9.42" velocity="15.0"/>
    <dynamics damping="0.0" friction="0.0"/>
  </joint>
  <!-- Xacro:Properties -->
  <!-- [m] -->
  <!-- [A] -->
  <link name="LH_SHANK">
    <visual>
      <origin rpy="0 0 -3.14159265359" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/anymal_b_simple_description/meshes/shank/anymal_shank_r.dae" scale="1 1 1"/>
      </geometry>
      <material name="anymal_material"/>
    </visual>
    <collision>
      <origin rpy="0 1.57079632679 0" xyz="-0.065 -0.015 0.01"/>
      <geometry>
        <box size="0.08 0.07 0.13"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="-0.030816858139 -0.004617229294 0.000893125713"/>
      <mass value="0.207204302"/>
      <inertia ixx="0.0002104880248" ixy="5.6750980345e-05" ixz="-1.0127699391e-05" iyy="0.000676270210023" iyz="-8.22869024e-07" izz="0.000545032674924"/>
    </inertial>
  </link>
  <!-- Shank joint -->
  <joint name="LH_KFE" type="revolute">
    <parent link="LH_THIGH"/>
    <child link="LH_SHANK"/>
    <origin xyz="-0.0 0.109 -0.25"/>
    <axis xyz="0 1 0"/>
    <limit command_effort="40" current="10" effort="80" gear_velocity="10" lower="-9.42" upper="9.42" velocity="15.0"/>
    <dynamics damping="0.0" friction="0.0"/>
  </joint>
  <!-- Xacro:Properties -->
  <!-- [m] -->
  <!-- Shank to Adapter joint -->
  <joint name="LH_SHANK_TO_ADAPTER" type="fixed">
    <parent link="LH_SHANK"/>
    <child link="LH_ADAPTER"/>
    <origin rpy="0.0 -0.0 -0.0" xyz="-0.1 -0.02 0.0"/>
  </joint>
  <!-- Adapter link -->
  <link name="LH_ADAPTER">
    <visual>
      <origin rpy="0 0 -3.14159265359" xyz="0 0 0.032"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/anymal_b_simple_description/meshes/foot/anymal_foot.dae" scale="1 1 1"/>
      </geometry>
      <material name="anymal_material"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 -0.160625"/>
      <geometry>
        <cylinder length="0.32125" radius="0.015"/>
      </geometry>
      <material name="anymal_material"/>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="-8.66e-10 -1.472e-09 -0.244345749188"/>
      <mass value="0.140170767"/>
      <inertia ixx="0.00159938741862" ixy="-9.32e-13" ixz="1.039e-11" iyy="0.00159938741932" iyz="1.7563e-11" izz="5.4423177329e-05"/>
    </inertial>
  </link>
  <!-- Adapter to Foot joint -->
  <joint name="LH_ADAPTER_TO_FOOT" type="fixed">
    <parent link="LH_ADAPTER"/>
    <child link="LH_FOOT"/>
    <origin rpy="0.0 -0.0 -0.0" xyz="-0.0 0.0 -0.32125"/>
  </joint>
  <!-- Foot link -->
  <link name="LH_FOOT">
    <collision>
      <origin xyz="0 0 0.02325"/>
      <geometry>
        <sphere radius="0.031"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.001"/>
      <inertia ixx="4.90e-07" ixy="0.0" ixz="0.0" iyy="4.90e-07" iyz="0.0" izz="4.90e-07"/>
      <!-- for a sphere: 2/5 m * r**2 -->
    </inertial>
    <!-- Bullet specific parameters -->
    <contact>
      <lateral_friction value="1.0"/>
      <stiffness value="300000"/>
      <damping value="15000"/>
      <!--<restitution value="0.5"/>-->
    </contact>
  </link>
  <!-- Gazebo customization -->
  <gazebo reference="LH_FOOT">
    <kp>1000000.0</kp>
    <kd>100.0</kd>
    <mu1>0.8</mu1>
    <mu2>0.8</mu2>
  </gazebo>
  <!-- Xacro:Properties -->
  <!-- [kg * m^2] -->
  <!-- [A] -->
  <link name="RH_HIP">
    <visual>
      <origin rpy="0 0 -3.14159265359" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/anymal_b_simple_description/meshes/hip/anymal_hip_l.dae" scale="1 1 1"/>
      </geometry>
      <material name="anymal_material"/>
    </visual>
    <collision>
      <!-- Protector -->
      <origin rpy="-1.57079632679 0 0" xyz="-0.0635 0.009 0.0"/>
      <geometry>
        <cylinder length="0.1" radius="0.08"/>
      </geometry>
    </collision>
    <collision>
      <!-- Heatfins -->
      <origin rpy="-1.57079632679 0 0" xyz="-0.0635 0.074 0.0"/>
      <geometry>
        <cylinder length="0.03" radius="0.045"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="-0.064516258147 0.003787101702 -0.000152184388"/>
      <mass value="1.42462064"/>
      <inertia ixx="0.00243023349564" ixy="-1.53023971e-05" ixz="2.1819095354e-05" iyy="0.00230257239103" iyz="-2.6473021273e-05" izz="0.0019806759227"/>
    </inertial>
  </link>
  <!-- Hip joint -->
  <joint name="RH_HAA" type="revolute">
    <parent link="base"/>
    <child link="RH_HIP"/>
    <origin xyz="-0.277 -0.116 0.0"/>
    <axis xyz="1 0 0"/>
    <limit command_effort="40" current="10" effort="80" gear_velocity="10" lower="-9.42" upper="9.42" velocity="15"/>
    <dynamics damping="0.0" friction="0.0"/>
  </joint>
  <!-- Xacro:Properties -->
  <!-- [m] -->
  <!-- [A] -->
  <link name="RH_THIGH">
    <visual>
      <origin rpy="0 0 -3.14159265359" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/anymal_b_simple_description/meshes/thigh/anymal_thigh.dae" scale="1 1 1"/>
      </geometry>
      <material name="anymal_material"/>
    </visual>
    <collision>
      <!-- thigh with heat fins -->
      <origin rpy="-0.145 0 0" xyz="0 -0.035 -0.125"/>
      <geometry>
        <box size="0.08 0.04 0.25"/>
      </geometry>
    </collision>
    <collision>
      <!-- KFE actuator -->
      <origin rpy="1.57079632679 0 0" xyz="0.0 -0.069 -0.25"/>
      <geometry>
        <cylinder length="0.12" radius="0.06"/>
      </geometry>
    </collision>
    <collision>
      <!-- upper protector -->
      <origin rpy="-1.71579632679 0 0" xyz="0 0.005 0"/>
      <geometry>
        <cylinder length="0.12" radius="0.066"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="0.003897968082 -0.054226618537 -0.214583373795"/>
      <mass value="1.634976467"/>
      <inertia ixx="0.0120367944369" ixy="6.762065206e-05" ixz="-0.000287806340448" iyy="0.0120643637939" iyz="0.00140610131218" izz="0.00249422574881"/>
    </inertial>
  </link>
  <!-- Thigh joint -->
  <joint name="RH_HFE" type="revolute">
    <parent link="RH_HIP"/>
    <child link="RH_THIGH"/>
    <origin xyz="-0.0635                    -0.041                    0.0"/>
    <axis xyz="0 1 0"/>
    <limit command_effort="40" current="10" effort="80" gear_velocity="10" lower="-9.42" upper="9.42" velocity="15.0"/>
    <dynamics damping="0.0" friction="0.0"/>
  </joint>
  <!-- Xacro:Properties -->
  <!-- [m] -->
  <!-- [A] -->
  <link name="RH_SHANK">
    <visual>
      <origin rpy="0 0 -3.14159265359" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/anymal_b_simple_description/meshes/shank/anymal_shank_l.dae" scale="1 1 1"/>
      </geometry>
      <material name="anymal_material"/>
    </visual>
    <collision>
      <origin rpy="0 1.57079632679 0" xyz="-0.065 0.015 0.01"/>
      <geometry>
        <box size="0.08 0.07 0.13"/>
      </geometry>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="-0.030816858139 0.004617229294 0.000893125713"/>
      <mass value="0.207204302"/>
      <inertia ixx="0.0002104880248" ixy="-5.6750980345e-05" ixz="-1.0127699391e-05" iyy="0.000676270210023" iyz="8.22869024e-07" izz="0.000545032674924"/>
    </inertial>
  </link>
  <!-- Shank joint -->
  <joint name="RH_KFE" type="revolute">
    <parent link="RH_THIGH"/>
    <child link="RH_SHANK"/>
    <origin xyz="-0.0 -0.109 -0.25"/>
    <axis xyz="0 1 0"/>
    <limit command_effort="40" current="10" effort="80" gear_velocity="10" lower="-9.42" upper="9.42" velocity="15.0"/>
    <dynamics damping="0.0" friction="0.0"/>
  </joint>
  <!-- Xacro:Properties -->
  <!-- [m] -->
  <!-- Shank to Adapter joint -->
  <joint name="RH_SHANK_TO_ADAPTER" type="fixed">
    <parent link="RH_SHANK"/>
    <child link="RH_ADAPTER"/>
    <origin rpy="-0.0 -0.0 0.0" xyz="-0.1 0.02 0.0"/>
  </joint>
  <!-- Adapter link -->
  <link name="RH_ADAPTER">
    <visual>
      <origin rpy="0 0 -3.14159265359" xyz="0 0 0.032"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/anymal_b_simple_description/meshes/foot/anymal_foot.dae" scale="1 1 1"/>
      </geometry>
      <material name="anymal_material"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 -0.160625"/>
      <geometry>
        <cylinder length="0.32125" radius="0.015"/>
      </geometry>
      <material name="anymal_material"/>
    </collision>
    <inertial>
      <origin rpy="0 0 0" xyz="-8.66e-10 -1.472e-09 -0.244345749188"/>
      <mass value="0.140170767"/>
      <inertia ixx="0.00159938741862" ixy="-9.32e-13" ixz="1.039e-11" iyy="0.00159938741932" iyz="1.7563e-11" izz="5.4423177329e-05"/>
    </inertial>
  </link>
  <!-- Adapter to Foot joint -->
  <joint name="RH_ADAPTER_TO_FOOT" type="fixed">
    <parent link="RH_ADAPTER"/>
    <child link="RH_FOOT"/>
    <origin rpy="-0.0 -0.0 0.0" xyz="-0.0 -0.0 -0.32125"/>
  </joint>
  <!-- Foot link -->
  <link name="RH_FOOT">
    <collision>
      <origin xyz="0 0 0.02325"/>
      <geometry>
        <sphere radius="0.031"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.001"/>
      <inertia ixx="4.90e-07" ixy="0.0" ixz="0.0" iyy="4.90e-07" iyz="0.0" izz="4.90e-07"/>
      <!-- for a sphere: 2/5 m * r**2 -->
    </inertial>
    <!-- Bullet specific parameters -->
    <contact>
      <lateral_friction value="1.0"/>
      <stiffness value="300000"/>
      <damping value="15000"/>
      <!--<restitution value="0.5"/>-->
    </contact>
  </link>
  <!-- Gazebo customization -->
  <gazebo reference="RH_FOOT">
    <kp>1000000.0</kp>
    <kd>100.0</kd>
    <mu1>0.8</mu1>
    <mu2>0.8</mu2>
  </gazebo>
  <link name="imu_link">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0.011"/>
      <geometry>
        <box size="0.058 0.058 0.022"/>
      </geometry>
      <material name="orange"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.011"/>
      <geometry>
        <box size="0.058 0.058 0.022"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.05"/>
      <origin xyz="0 0 0"/>
      <inertia ixx="1.60333333333e-05" ixy="0.0" ixz="0.0" iyy="2.80333333333e-05" iyz="0.0" izz="1.60333333333e-05"/>
    </inertial>
  </link>
  <joint name="imu_joint" type="fixed">
    <parent link="base"/>
    <child link="imu_link"/>
    <origin rpy="0 3.14159265359 0" xyz="0.038 0.06245 0.1837"/>
  </joint>

  <link name="jaco_front_hatch_support_v2">
    <!-- TODO: Proper inertia -->
    <inertial>
      <mass value="0.205"/>
      <inertia ixx="4.90e-07" ixy="0.0" ixz="0.0" iyy="4.90e-07" iyz="0.0" izz="4.90e-07"/>
      <!-- for a sphere: 2/5 m * r**2 -->
    </inertial>
    <visual>
      <geometry>
        <mesh filename="package://example-robot-data/robots/kinova_description/meshes/jaco_front_hatch_support_v2.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin xyz="0 0 0.02325"/>
      <geometry>
        <sphere radius="0.031"/>
      </geometry>
    </collision>
  </link>
  <joint name="base_to_jaco_support" type="fixed">
    <parent link="base"/>
    <child link="jaco_front_hatch_support_v2"/>
    <origin rpy="0 0 0" xyz="0.2692 0.00 0.06713"/>
  </joint>

  <link name="jaco_mounting_block">
    <!-- TODO: Proper inertia -->
    <inertial>
      <mass value="0.1741"/><!-- Volume = 0.000064 m^3; aluminium density (alloy 6061) = 2720 kg/m^3 -->
      <inertia ixx="4.90e-07" ixy="0.0" ixz="0.0" iyy="4.90e-07" iyz="0.0" izz="4.90e-07"/>
      <!-- for a sphere: 2/5 m * r**2 -->
    </inertial>
    <visual>
      <geometry>
        <mesh filename="package://example-robot-data/robots/kinova_description/meshes/jaco_mounting_block.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin xyz="0 0 0.02325"/>
      <geometry>
        <sphere radius="0.031"/>
      </geometry>
    </collision>
  </link>
  <joint name="jaco_support_to_jaco_mounting_block" type="fixed">
    <parent link="jaco_front_hatch_support_v2"/>
    <child link="jaco_mounting_block"/>
    <origin rpy="0 0 0" xyz="0.045 0.00 -0.007626"/>
  </joint>

  <link name="j2s6s200_link_base">
    <visual>
      <geometry>
        <mesh filename="package://example-robot-data/robots/kinova_description/meshes/base.dae"/>
      </geometry>
      <material name="carbon_fiber">
        <color rgba="0.3 0.3 0.3 1"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://example-robot-data/robots/kinova_description/meshes/base.dae"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.46784"/>
      <origin rpy="0 0 0" xyz="0 0 0.1255"/>
      <inertia ixx="0.000951270861568" ixy="0" ixz="0" iyy="0.000951270861568" iyz="0" izz="0.000374272"/>
    </inertial>
  </link>
  <joint name="jaco_mounting_block_to_j2s6s200_link_base" type="fixed">
    <parent link="jaco_mounting_block"/>
    <child link="j2s6s200_link_base"/>
    <axis xyz="0 0 0"/>
    <limit effort="0" lower="0" upper="0" velocity="0"/>
    <origin rpy="0 0 1.57079632679" xyz="0.000 0.000 0.000"/>
    <dynamics damping="0.0" friction="0.0"/>
  </joint>
  <link name="j2s6s200_link_1">
    <visual>
      <geometry>
        <mesh filename="package://example-robot-data/robots/kinova_description/meshes/shoulder.dae"/>
      </geometry>
      <material name="carbon_fiber">
        <color rgba="0.3 0.3 0.3 1"/>
      </material>
    </visual>
    <visual>
      <geometry>
        <mesh filename="package://example-robot-data/robots/kinova_description/meshes/ring_big.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://example-robot-data/robots/kinova_description/meshes/shoulder.dae"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.7477"/>
      <origin xyz="0 -0.002 -0.0605"/>
      <inertia ixx="0.00152031725204" ixy="0" ixz="0" iyy="0.00152031725204" iyz="0" izz="0.00059816"/>
    </inertial>
  </link>
  <joint name="j2s6s200_joint_1" type="revolute"><!-- continuous -->
    <parent link="j2s6s200_link_base"/>
    <child link="j2s6s200_link_1"/>
    <axis xyz="0 0 1"/>
    <limit effort="40" lower="-6.28318530718" upper="6.28318530718" velocity="0.628318530718"/>
    <origin rpy="0 3.14159265359 0" xyz="0 0 0.15675"/>
    <dynamics damping="0.0" friction="0.0"/>
  </joint>
  <transmission name="j2s6s200_joint_1_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="j2s6s200_joint_1">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="j2s6s200_joint_1_actuator">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <mechanicalReduction>160</mechanicalReduction>
    </actuator>
  </transmission>
  <link name="j2s6s200_link_2">
    <visual>
      <geometry>
        <mesh filename="package://example-robot-data/robots/kinova_description/meshes/arm.dae"/>
      </geometry>
      <material name="carbon_fiber">
        <color rgba="0.3 0.3 0.3 1"/>
      </material>
    </visual>
    <visual>
      <geometry>
        <mesh filename="package://example-robot-data/robots/kinova_description/meshes/ring_big.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://example-robot-data/robots/kinova_description/meshes/arm.dae"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.99"/>
      <origin xyz="0 -0.2065 -0.01"/>
      <inertia ixx="0.010502207991" ixy="0" ixz="0" iyy="0.000792" iyz="0" izz="0.010502207991"/>
    </inertial>
  </link>
  <joint name="j2s6s200_joint_2" type="revolute">
    <parent link="j2s6s200_link_1"/>
    <child link="j2s6s200_link_2"/>
    <axis xyz="0 0 1"/>
    <limit effort="80" lower="0.820304748437" upper="5.46288055874" velocity="0.628318530718"/>
    <origin rpy="-1.57079632679 0 3.14159265359" xyz="0 0.0016 -0.11875"/>
    <dynamics damping="0.0" friction="0.0"/>
  </joint>
  <transmission name="j2s6s200_joint_2_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="j2s6s200_joint_2">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="j2s6s200_joint_2_actuator">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <mechanicalReduction>160</mechanicalReduction>
    </actuator>
  </transmission>
  <link name="j2s6s200_link_3">
    <visual>
      <geometry>
        <mesh filename="package://example-robot-data/robots/kinova_description/meshes/forearm.dae"/>
      </geometry>
      <material name="carbon_fiber">
        <color rgba="0.3 0.3 0.3 1"/>
      </material>
    </visual>
    <visual>
      <geometry>
        <mesh filename="package://example-robot-data/robots/kinova_description/meshes/ring_big.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://example-robot-data/robots/kinova_description/meshes/forearm.dae"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.6763"/>
      <origin xyz="0 0.081 -0.0086"/>
      <inertia ixx="0.00142022431908" ixy="0" ixz="0" iyy="0.000304335" iyz="0" izz="0.00142022431908"/>
    </inertial>
  </link>
  <joint name="j2s6s200_joint_3" type="revolute">
    <parent link="j2s6s200_link_2"/>
    <child link="j2s6s200_link_3"/>
    <axis xyz="0 0 1"/>
    <limit effort="40" lower="0.331612557879" upper="5.9515727493" velocity="0.628318530718"/>
    <origin rpy="0 3.14159265359 0" xyz="0 -0.410 0"/>
    <dynamics damping="0.0" friction="0.0"/>
  </joint>
  <transmission name="j2s6s200_joint_3_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="j2s6s200_joint_3">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="j2s6s200_joint_3_actuator">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <mechanicalReduction>160</mechanicalReduction>
    </actuator>
  </transmission>
  <link name="j2s6s200_link_4">
    <visual>
      <geometry>
        <mesh filename="package://example-robot-data/robots/kinova_description/meshes/wrist_spherical_1.dae"/>
      </geometry>
      <material name="carbon_fiber">
        <color rgba="0.3 0.3 0.3 1"/>
      </material>
    </visual>
    <visual>
      <geometry>
        <mesh filename="package://example-robot-data/robots/kinova_description/meshes/ring_small.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://example-robot-data/robots/kinova_description/meshes/wrist_spherical_1.dae"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.463"/>
      <origin xyz="0 0.0028848942 -0.0541932613"/>
      <inertia ixx="0.0004321316048" ixy="0" ixz="0" iyy="0.0004321316048" iyz="0" izz="9.26e-05"/>
    </inertial>
  </link>
  <joint name="j2s6s200_joint_4" type="revolute"><!-- continuous -->
    <parent link="j2s6s200_link_3"/>
    <child link="j2s6s200_link_4"/>
    <axis xyz="0 0 1"/>
    <limit effort="20" lower="-6.28318530718" upper="6.28318530718" velocity="0.837758040957"/>
    <origin rpy="-1.57079632679 0 3.14159265359" xyz="0 0.2073 -0.0114"/>
    <dynamics damping="0.0" friction="0.0"/>
  </joint>
  <transmission name="j2s6s200_joint_4_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="j2s6s200_joint_4">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="j2s6s200_joint_4_actuator">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <mechanicalReduction>160</mechanicalReduction>
    </actuator>
  </transmission>
  <link name="j2s6s200_link_5">
    <visual>
      <geometry>
        <mesh filename="package://example-robot-data/robots/kinova_description/meshes/wrist_spherical_2.dae"/>
      </geometry>
      <material name="carbon_fiber">
        <color rgba="0.3 0.3 0.3 1"/>
      </material>
    </visual>
    <visual>
      <geometry>
        <mesh filename="package://example-robot-data/robots/kinova_description/meshes/ring_small.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://example-robot-data/robots/kinova_description/meshes/wrist_spherical_2.dae"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.463"/>
      <origin xyz="0 0.0497208855 -0.0028562765"/>
      <inertia ixx="0.0004321316048" ixy="0" ixz="0" iyy="9.26e-05" iyz="0" izz="0.0004321316048"/>
    </inertial>
  </link>
  <joint name="j2s6s200_joint_5" type="revolute">
    <parent link="j2s6s200_link_4"/>
    <child link="j2s6s200_link_5"/>
    <axis xyz="0 0 1"/>
    <limit effort="20" lower="0.523598775598" upper="5.75958653158" velocity="0.837758040957"/>
    <origin rpy="1.57079632679 0 3.14159265359" xyz="0 0 -0.10375"/>
    <dynamics damping="0.0" friction="0.0"/>
  </joint>
  <transmission name="j2s6s200_joint_5_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="j2s6s200_joint_5">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="j2s6s200_joint_5_actuator">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <mechanicalReduction>160</mechanicalReduction>
    </actuator>
  </transmission>
  <link name="j2s6s200_link_6">
    <visual>
      <geometry>
        <mesh filename="package://example-robot-data/robots/kinova_description/meshes/hand_2finger.dae"/>
      </geometry>
      <material name="carbon_fiber">
        <color rgba="0.3 0.3 0.3 1"/>
      </material>
    </visual>
    <visual>
      <geometry>
        <mesh filename="package://example-robot-data/robots/kinova_description/meshes/ring_small.dae"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://example-robot-data/robots/kinova_description/meshes/hand_2finger.dae"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.99"/>
      <origin xyz="0 0 -0.06"/>
      <inertia ixx="0.0003453236187" ixy="0" ixz="0" iyy="0.0003453236187" iyz="0" izz="0.0005816"/>
    </inertial>
  </link>
  <joint name="j2s6s200_joint_6" type="revolute"><!-- revolute -->
    <parent link="j2s6s200_link_5"/>
    <child link="j2s6s200_link_6"/>
    <axis xyz="0 0 1"/>
    <limit effort="20" lower="-6.28318530718" upper="6.28318530718" velocity="0.837758040957"/>
    <origin rpy="-1.57079632679 0 3.14159265359" xyz="0 0.10375 0"/>
    <dynamics damping="0.0" friction="0.0"/>
  </joint>
  <transmission name="j2s6s200_joint_6_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="j2s6s200_joint_6">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="j2s6s200_joint_6_actuator">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <mechanicalReduction>160</mechanicalReduction>
    </actuator>
  </transmission>
  <link name="j2s6s200_end_effector">
    <inertial>
      <mass value="0.001"/>
      <inertia ixx="4.90e-07" ixy="0.0" ixz="0.0" iyy="4.90e-07" iyz="0.0" izz="4.90e-07"/>
      <!-- for a sphere: 2/5 m * r**2 -->
    </inertial>
  </link>
  <joint name="j2s6s200_joint_end_effector" type="fixed">
    <parent link="j2s6s200_link_6"/>
    <child link="j2s6s200_end_effector"/>
    <axis xyz="0 0 0"/>
    <limit effort="2000" lower="0" upper="0" velocity="1"/>
    <!-- <origin rpy="3.14159265359 0 1.57079632679" xyz="0 0 -0.1600"/> -->
    <origin rpy="3.14159265359 1.57079632679 0" xyz="0 0 -0.1600"/>
  </joint>
  <link name="j2s6s200_link_finger_1">
    <visual>
      <geometry>
        <mesh filename="package://example-robot-data/robots/kinova_description/meshes/finger_proximal.dae"/>
      </geometry>
      <material name="carbon_fiber">
        <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://example-robot-data/robots/kinova_description/meshes/finger_proximal.dae"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.01"/>
      <origin xyz="0.022 0 0"/>
      <inertia ixx="7.8999684e-07" ixy="0" ixz="0" iyy="7.8999684e-07" iyz="0" izz="8e-08"/>
    </inertial>
  </link>
  <joint name="j2s6s200_joint_finger_1" type="fixed">
    <parent link="j2s6s200_link_6"/>
    <child link="j2s6s200_link_finger_1"/>
    <axis xyz="0 0 1"/>
    <origin rpy="-1.570796327 .649262481663582 1.57079632679490" xyz="-0.0025 0.03095 -0.11482"/>
    <limit effort="2" lower="0" upper="1.51" velocity="1"/>
  </joint>
  <transmission name="j2s6s200_joint_finger_1_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="j2s6s200_joint_finger_1">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="j2s6s200_joint_finger_1_actuator">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <link name="j2s6s200_link_finger_tip_1">
    <visual>
      <geometry>
        <mesh filename="package://example-robot-data/robots/kinova_description/meshes/finger_distal.dae"/>
      </geometry>
      <material name="carbon_fiber">
        <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://example-robot-data/robots/kinova_description/meshes/finger_distal.dae"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.01"/>
      <origin xyz="0.022 0 0"/>
      <inertia ixx="7.8999684e-07" ixy="0" ixz="0" iyy="7.8999684e-07" iyz="0" izz="8e-08"/>
    </inertial>
  </link>
  <joint name="j2s6s200_joint_finger_tip_1" type="fixed">
    <parent link="j2s6s200_link_finger_1"/>
    <child link="j2s6s200_link_finger_tip_1"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 0" xyz="0.044 -0.003 0"/>
    <limit effort="2" lower="0" upper="2" velocity="1"/>
  </joint>
  <transmission name="j2s6s200_joint_finger_tip_1_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="j2s6s200_joint_finger_tip_1">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="j2s6s200_joint_finger_tip_1_actuator">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <link name="j2s6s200_link_finger_2">
    <visual>
      <geometry>
        <mesh filename="package://example-robot-data/robots/kinova_description/meshes/finger_proximal.dae"/>
      </geometry>
      <material name="carbon_fiber">
        <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://example-robot-data/robots/kinova_description/meshes/finger_proximal.dae"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.01"/>
      <origin xyz="0.022 0 0"/>
      <inertia ixx="7.8999684e-07" ixy="0" ixz="0" iyy="7.8999684e-07" iyz="0" izz="8e-08"/>
    </inertial>
  </link>
  <joint name="j2s6s200_joint_finger_2" type="fixed">
    <parent link="j2s6s200_link_6"/>
    <child link="j2s6s200_link_finger_2"/>
    <axis xyz="0 0 1"/>
    <origin rpy="-1.570796327 .649262481663582 -1.57079632679490" xyz="-0.0025 -0.03095 -0.11482"/>
    <limit effort="2" lower="0" upper="1.51" velocity="1"/>
  </joint>
  <transmission name="j2s6s200_joint_finger_2_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="j2s6s200_joint_finger_2">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="j2s6s200_joint_finger_2_actuator">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <link name="j2s6s200_link_finger_tip_2">
    <visual>
      <geometry>
        <mesh filename="package://example-robot-data/robots/kinova_description/meshes/finger_distal.dae"/>
      </geometry>
      <material name="carbon_fiber">
        <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://example-robot-data/robots/kinova_description/meshes/finger_distal.dae"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.01"/>
      <origin xyz="0.022 0 0"/>
      <inertia ixx="7.8999684e-07" ixy="0" ixz="0" iyy="7.8999684e-07" iyz="0" izz="8e-08"/>
    </inertial>
  </link>
  <joint name="j2s6s200_joint_finger_tip_2" type="fixed">
    <parent link="j2s6s200_link_finger_2"/>
    <child link="j2s6s200_link_finger_tip_2"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 0" xyz="0.044 -0.003 0"/>
    <limit effort="2" lower="0" upper="2" velocity="1"/>
  </joint>
  <transmission name="j2s6s200_joint_finger_tip_2_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="j2s6s200_joint_finger_tip_2">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="j2s6s200_joint_finger_tip_2_actuator">
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
</robot>
