<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- |    This document was autogenerated by xacro from /home/dbellicoso/catkin_ws/src/anymal_beth/anymal_beth_description/urdf/anymal_beth.urdf.xacro | -->
<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
<!-- =================================================================================== -->
<!--Copyright (C) 2015 by Dario Bellicoso                                    -->
<!--bellicosodario@gmail.com                                                 -->
<!--                                                                         -->
<!--This program is free software; you can redistribute it and/or modify     -->
<!--it under the terms of the Lesser GNU General Public License as published -->
<!--by the Free Software Foundation; either version 3 of the License, or     -->
<!--(at your option) any later version.                                      -->
<!--                                                                         -->
<!--This program is distributed in the hope that it will be useful,          -->
<!--but WITHOUT ANY WARRANTY; without even the implied warranty of           -->
<!--MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the             -->
<!--Lesser GNU General Public License for more details.                      -->
<!--                                                                         -->
<!--You should have received a copy of the Lesser GNU General Public License -->
<!--along with this program. If not, see <http://www.gnu.org/licenses/>.     -->
<!-- This file contains the description of the ANYmal Beth 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>
    <gazebo>
        <plugin filename="libanymal_gazebo.so" name="gazebo_ros_control">
            <robotNamespace>anymal</robotNamespace>
            <!-- Currently unused -->
            <robotDescription>quadruped_description</robotDescription>
            <robotBaseLink>base</robotBaseLink>
            <statePublisherRate>400.0</statePublisherRate>
        </plugin>
    </gazebo>
    <!-- Material for the visual primitives -->
    <material name="anymal_black_material">
        <color rgba="0.3 0.3 0.3 1"/>
    </material>
     <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="anymal_base_2.obj" scale="0.001 0.001     0.001"/>
            </geometry>
            <material name="anymal_black_material"/>
        </visual>
        <collision>
            <!-- Main Body -->
            <origin rpy="0 0 0" xyz="0 0 0.08"/>
            <geometry>
                <box size="0.531 0.232 0.26"/>
            </geometry>
        </collision>
        <inertial>
            <origin rpy="0 0 0" xyz="0.022 -0.01 0.01"/>
            <mass value="21.39"/>
            <inertia ixx="0.268388530624" ixy="0.000775392455422" ixz="-0.0151848534451" iyy="0.884235660795" iyz="-0.000989297489507" izz="0.829158678306"/>
        </inertial>
    </link>
    <!-- Xacro:Properties -->
    <!-- [m] -->
    <!-- [Nm] -->
    <link name="LF_HIP">
        <visual>
            <origin rpy="3.14159265359 1.5707963268 3.14159265359" xyz="0 0 0"/>
            <geometry>
                <mesh filename="anymal_hip.obj" scale="0.001 0.001     0.001"/>
            </geometry>
            <material name="anymal_material"/>
        </visual>
        <collision>
            <origin rpy="1.57079632679 0 0" xyz="0.0585 0.0315 0.0"/>
            <geometry>
                <cylinder length="0.08" radius="0.04"/>
            </geometry>
        </collision>
        <inertial>
            <origin rpy="0 0 0" xyz="0.0542 -0.0136 0.0"/>
            <mass value="1.1064"/>
            <inertia ixx="0.00123301508523" ixy="-6.4285329973e-05" ixz="6.9990104e-08" iyy="0.000993231183395" iyz="2.327401345e-06" izz="0.00142160349098"/>
        </inertial>
    </link>
    <!-- Hip joint -->
    <joint name="LF_HAA" type="revolute">
        <parent link="base"/>
        <child link="LF_HIP"/>
        <origin xyz="0.2629 0.115 0.0"/>
        <axis xyz="1 0 0"/>
        <limit effort="80" lower="-6.28" upper="6.28" velocity="15"/>
        <dynamics damping="0.0" friction="0.0"/>
    </joint>
    <!-- Xacro:Properties -->
    <!-- [m] -->
    <!-- [Nm] -->
    <link name="LF_THIGH">
        <visual>
            <origin rpy="0 0 0" xyz="0 0 0"/>
            <geometry>
                <mesh filename="anymal_thigh.obj" scale="0.001 0.001     0.001"/>
            </geometry>
            <material name="anymal_material"/>
        </visual>
        <!--collision>
            <origin rpy="0.145 0 0" xyz="0 0.02 -0.1"/>
            <geometry>
                <cylinder length="0.2" radius="0.01"/>
            </geometry>
        </collision-->
        <collision>
            <origin rpy="1.57079632679 0 0" xyz="0.0 0.07 -0.25"/>
            <geometry>
                <cylinder length="0.08" radius="0.04"/>
            </geometry>
        </collision>
        <inertial>
            <origin rpy="0 0 0" xyz="0.0008 0.0545 -0.1975"/>
            <mass value="1.51"/>
            <inertia ixx="0.0147571628275" ixy="-1.7996507882e-05" ixz="0.000205877679383" iyy="0.0144188925719" iyz="-0.00181291342366" izz="0.00178449075964"/>
        </inertial>
    </link>
    <!-- Thigh joint -->
    <joint name="LF_HFE" type="revolute">
        <parent link="LF_HIP"/>
        <child link="LF_THIGH"/>
        <origin xyz="0.0585                    0.0315                    0.0"/>
        <axis xyz="0 1 0"/>
        <limit effort="80" lower="-6.28" upper="6.28" velocity="15"/>
        <dynamics damping="0.0" friction="0.0"/>
    </joint>
    <!-- Xacro:Properties -->
    <!-- [m] -->
    <!-- [Nm] -->
    <link name="LF_SHANK">
        <visual>
            <origin rpy="0 0 0.0" xyz="0 0 0"/>
            <geometry>
                <mesh filename="anymal_shank_2_LF.obj" scale="0.001 0.001     0.001"/>
            </geometry>
            <material name="anymal_material"/>
        </visual>
        <!--collision>
            <origin rpy="0 -0.9 -0.2" xyz="0.04 -0.02 -0.04"/>
            <geometry>
                <cylinder length="0.13" radius="0.015"/>
            </geometry>
        </collision-->
        <collision>
            <origin rpy="-0.0 0.34 0" xyz="0.06 -0.03 -0.18"/>
            <geometry>
                <cylinder length="0.19" radius="0.015"/>
            </geometry>
        </collision>
        <inertial>
            <origin rpy="0 0 0" xyz="0.016355414056 -0.00562219106 -0.064350437219"/>
            <mass value="0.318"/>
            <inertia ixx="0.0037693717909" ixy="-7.7334500762e-05" ixz="-0.000152069372983" iyy="0.00400446467635" iyz="-0.0003218356083" izz="0.000418471691904"/>
        </inertial>
    </link>
    <!-- Shank joint -->
    <joint name="LF_KFE" type="revolute">
        <parent link="LF_THIGH"/>
        <child link="LF_SHANK"/>
        <origin xyz="0.0 0.111 -0.25"/>
        <axis xyz="0 1 0"/>
        <limit effort="80" lower="-6.28" upper="6.28" velocity="15"/>
        <dynamics damping="0.0" friction="0.0"/>
    </joint>
    <!-- Foot mount link -->
    <link name="LF_FOOT_MOUNT">
        <visual>
            <origin rpy="0 0 0" xyz="0 0 0"/>
            <geometry>
                <mesh filename="anymal_optoforce.obj" scale="0.001 0.001     0.001"/>
            </geometry>
            <material name="anymal_material"/>
        </visual>
        <collision>
            <origin rpy="0 0 0" xyz="0.0 0.0 0"/>
            <geometry>
                <sphere radius="0.03"/>
            </geometry>
        </collision>
        <inertial>
            <origin rpy="0 0 0" xyz="-5.552058e-06 7.1133124e-05 -0.021388647779"/>
            <mass value="0.1922685"/>
            <inertia ixx="8.3086410294e-05" ixy="4.16901601e-07" ixz="3.99054799e-07" iyy="8.2860209667e-05" iyz="-3.45712252e-07" izz="8.1948124086e-05"/>
        </inertial>
    </link>
    <!-- Foot mount joint / ankle -->
    <joint name="LF_ANKLE_JOINT" type="fixed">
        <parent link="LF_SHANK"/>
        <child link="LF_FOOT_MOUNT"/>
        <origin rpy="0.323 0.549 0" xyz="0.0 -0.016 -0.31"/>
    </joint>
    <!-- Xacro:Properties -->
    <!-- [m] -->
    <!-- Gazebo customization -->
    <gazebo reference="LF_FOOT_SENSOR">
        <kp>1000000.0</kp>
        <kd>100.0</kd>
        <mu1>0.8</mu1>
        <mu2>0.8</mu2>
    </gazebo>
    <gazebo reference="LF_MOUNT_TO_SENSOR"/>
    <!-- Xacro:Properties -->
    <!-- [m] -->
    <!-- [Nm] -->
    <link name="RF_HIP">
        <visual>
            <origin rpy="0 -1.57079632679 3.14159265359" xyz="0 0 0"/>
            <geometry>
                <mesh filename="anymal_hip.obj" scale="0.001 0.001     0.001"/>
            </geometry>
            <material name="anymal_material"/>
        </visual>
        <collision>
            <origin rpy="1.57079632679 0 0" xyz="0.0585 -0.0315 0.0"/>
            <geometry>
                <cylinder length="0.08" radius="0.04"/>
            </geometry>
        </collision>
        <inertial>
            <origin rpy="0 0 0" xyz="0.0542 0.0136 0.0"/>
            <mass value="1.1064"/>
            <inertia ixx="0.00123301508523" ixy="6.4285329973e-05" ixz="6.9990104e-08" iyy="0.000993231183395" iyz="-2.327401345e-06" izz="0.00142160349098"/>
        </inertial>
    </link>
    <!-- Hip joint -->
    <joint name="RF_HAA" type="revolute">
        <parent link="base"/>
        <child link="RF_HIP"/>
        <origin xyz="0.2629 -0.115 0.0"/>
        <axis xyz="1 0 0"/>
        <limit effort="80" lower="-6.28" upper="6.28" velocity="15"/>
        <dynamics damping="0.0" friction="0.0"/>
    </joint>
    <!-- Xacro:Properties -->
    <!-- [m] -->
    <!-- [Nm] -->
    <link name="RF_THIGH">
        <visual>
            <origin rpy="0 0 3.14159265359" xyz="0 0 0"/>
            <geometry>
                <mesh filename="anymal_thigh.obj" scale="0.001 0.001     0.001"/>
            </geometry>
            <material name="anymal_material"/>
        </visual>
        <!--collision>
            <origin rpy="-0.145 0 0" xyz="0 -0.02 -0.1"/>
            <geometry>
                <cylinder length="0.2" radius="0.01"/>
            </geometry>
        </collision-->
        <collision>
            <origin rpy="1.57079632679 0 0" xyz="0.0 -0.07 -0.25"/>
            <geometry>
                <cylinder length="0.08" radius="0.04"/>
            </geometry>
        </collision>
        <inertial>
            <origin rpy="0 0 0" xyz="0.0008 -0.0545 -0.1975"/>
            <mass value="1.51"/>
            <inertia ixx="0.0147571628275" ixy="1.7996507882e-05" ixz="0.000205877679383" iyy="0.0144188925719" iyz="0.00181291342366" izz="0.00178449075964"/>
        </inertial>
    </link>
    <!-- Thigh joint -->
    <joint name="RF_HFE" type="revolute">
        <parent link="RF_HIP"/>
        <child link="RF_THIGH"/>
        <origin xyz="0.0585                    -0.0315                    0.0"/>
        <axis xyz="0 1 0"/>
        <limit effort="80" lower="-6.28" upper="6.28" velocity="15"/>
        <dynamics damping="0.0" friction="0.0"/>
    </joint>
    <!-- Xacro:Properties -->
    <!-- [m] -->
    <!-- [Nm] -->
    <link name="RF_SHANK">
        <visual>
            <origin rpy="3.14159265359 3.14159265359 3.14159265359" xyz="0 0 0"/>
            <geometry>
                <mesh filename="anymal_shank_2_RF.obj" scale="0.001 0.001     0.001"/>
            </geometry>
            <material name="anymal_material"/>
        </visual>
        <!--collision>
            <origin rpy="0 -0.9 0.2" xyz="0.04 0.02 -0.04"/>
            <geometry>
                <cylinder length="0.13" radius="0.015"/>
            </geometry>
        </collision-->
        <collision>
            <origin rpy="0.0 0.34 0" xyz="0.06 0.03 -0.18"/>
            <geometry>
                <cylinder length="0.19" radius="0.015"/>
            </geometry>
        </collision>
        <inertial>
            <origin rpy="0 0 0" xyz="0.016355414056 0.00562219106 -0.064350437219"/>
            <mass value="0.318"/>
            <inertia ixx="0.0037693717909" ixy="7.7334500762e-05" ixz="-0.000152069372983" iyy="0.00400446467635" iyz="0.0003218356083" izz="0.000418471691904"/>
        </inertial>
    </link>
    <!-- Shank joint -->
    <joint name="RF_KFE" type="revolute">
        <parent link="RF_THIGH"/>
        <child link="RF_SHANK"/>
        <origin xyz="0.0 -0.111 -0.25"/>
        <axis xyz="0 1 0"/>
        <limit effort="80" lower="-6.28" upper="6.28" velocity="15"/>
        <dynamics damping="0.0" friction="0.0"/>
    </joint>
    <!-- Foot mount link -->
    <link name="RF_FOOT_MOUNT">
        <visual>
            <origin rpy="0 0 0" xyz="0 0 0"/>
            <geometry>
                <mesh filename="anymal_optoforce.obj" scale="0.001 0.001     0.001"/>
            </geometry>
            <material name="anymal_material"/>
        </visual>
        <collision>
            <origin rpy="0 0 0" xyz="0.0 0.0 0"/>
            <geometry>
                <sphere radius="0.03"/>
            </geometry>
        </collision>
        <inertial>
            <origin rpy="0 0 0" xyz="-5.552058e-06 -7.1133124e-05 -0.021388647779"/>
            <mass value="0.1922685"/>
            <inertia ixx="8.3086410294e-05" ixy="-4.16901601e-07" ixz="3.99054799e-07" iyy="8.2860209667e-05" iyz="3.45712252e-07" izz="8.1948124086e-05"/>
        </inertial>
    </link>
    <!-- Foot mount joint / ankle -->
    <joint name="RF_ANKLE_JOINT" type="fixed">
        <parent link="RF_SHANK"/>
        <child link="RF_FOOT_MOUNT"/>
        <origin rpy="-0.323 0.549 0" xyz="0.0 0.016 -0.31"/>
    </joint>
    <!-- Xacro:Properties -->
    <!-- [m] -->
    <!-- Gazebo customization -->
    <gazebo reference="RF_FOOT_SENSOR">
        <kp>1000000.0</kp>
        <kd>100.0</kd>
        <mu1>0.8</mu1>
        <mu2>0.8</mu2>
    </gazebo>
    <gazebo reference="RF_MOUNT_TO_SENSOR"/>
    <!-- Xacro:Properties -->
    <!-- [m] -->
    <!-- [Nm] -->
    <link name="LH_HIP">
        <visual>
            <origin rpy="0 -1.5707963268 0" xyz="0 0 0"/>
            <geometry>
                <mesh filename="anymal_hip.obj" scale="0.001 0.001     0.001"/>
            </geometry>
            <material name="anymal_material"/>
        </visual>
        <collision>
            <origin rpy="-1.57079632679 0 0" xyz="-0.0585 0.0315 0.0"/>
            <geometry>
                <cylinder length="0.08" radius="0.04"/>
            </geometry>
        </collision>
        <inertial>
            <origin rpy="0 0 0" xyz="-0.0542 -0.0136 0.0"/>
            <mass value="1.1064"/>
            <inertia ixx="0.00123301508523" ixy="6.4285329973e-05" ixz="-6.9990104e-08" iyy="0.000993231183395" iyz="2.327401345e-06" izz="0.00142160349098"/>
        </inertial>
    </link>
    <!-- Hip joint -->
    <joint name="LH_HAA" type="revolute">
        <parent link="base"/>
        <child link="LH_HIP"/>
        <origin xyz="-0.2629 0.115 0.0"/>
        <axis xyz="1 0 0"/>
        <limit effort="80" lower="-6.28" upper="6.28" velocity="15"/>
        <dynamics damping="0.0" friction="0.0"/>
    </joint>
    <!-- Xacro:Properties -->
    <!-- [m] -->
    <!-- [Nm] -->
    <link name="LH_THIGH">
        <visual>
            <origin rpy="0 0 0" xyz="0 0 0"/>
            <geometry>
                <mesh filename="anymal_thigh.obj" scale="0.001 0.001     0.001"/>
            </geometry>
            <material name="anymal_material"/>
        </visual>
        <!--collision>
            <origin rpy="0.145 0 0" xyz="0 0.02 -0.1"/>
            <geometry>
                <cylinder length="0.2" radius="0.01"/>
            </geometry>
        </collision-->
        <collision>
            <origin rpy="1.57079632679 0 0" xyz="0.0 0.07 -0.25"/>
            <geometry>
                <cylinder length="0.08" radius="0.04"/>
            </geometry>
        </collision>
        <inertial>
            <origin rpy="0 0 0" xyz="-0.0008 0.0545 -0.1975"/>
            <mass value="1.51"/>
            <inertia ixx="0.0147571628275" ixy="1.7996507882e-05" ixz="-0.000205877679383" iyy="0.0144188925719" iyz="-0.00181291342366" izz="0.00178449075964"/>
        </inertial>
    </link>
    <!-- Thigh joint -->
    <joint name="LH_HFE" type="revolute">
        <parent link="LH_HIP"/>
        <child link="LH_THIGH"/>
        <origin xyz="-0.0585                    0.0315                    0.0"/>
        <axis xyz="0 1 0"/>
        <limit effort="80" lower="-6.28" upper="6.28" velocity="15"/>
        <dynamics damping="0.0" friction="0.0"/>
    </joint>
    <!-- Xacro:Properties -->
    <!-- [m] -->
    <!-- [Nm] -->
    <link name="LH_SHANK">
        <visual>
            <origin rpy="0 0 0" xyz="0 0 0"/>
            <geometry>
                <mesh filename="anymal_shank_2_LH.obj" scale="0.001 0.001     0.001"/>
            </geometry>
            <material name="anymal_material"/>
        </visual>
        <!--collision>
            <origin rpy="0 0.9 0.2" xyz="-0.04 -0.02 -0.04"/>
            <geometry>
                <cylinder length="0.13" radius="0.015"/>
            </geometry>
        </collision-->
        <collision>
            <origin rpy="-0.0 -0.34 0" xyz="-0.06 -0.03 -0.18"/>
            <geometry>
                <cylinder length="0.19" radius="0.015"/>
            </geometry>
        </collision>
        <inertial>
            <origin rpy="0 0 0" xyz="-0.016355414056 -0.00562219106 -0.064350437219"/>
            <mass value="0.318"/>
            <inertia ixx="0.0037693717909" ixy="7.7334500762e-05" ixz="0.000152069372983" iyy="0.00400446467635" iyz="-0.0003218356083" izz="0.000418471691904"/>
        </inertial>
    </link>
    <!-- Shank joint -->
    <joint name="LH_KFE" type="revolute">
        <parent link="LH_THIGH"/>
        <child link="LH_SHANK"/>
        <origin xyz="-0.0 0.111 -0.25"/>
        <axis xyz="0 1 0"/>
        <limit effort="80" lower="-6.28" upper="6.28" velocity="15"/>
        <dynamics damping="0.0" friction="0.0"/>
    </joint>
    <!-- Foot mount link -->
    <link name="LH_FOOT_MOUNT">
        <visual>
            <origin rpy="0 0 0" xyz="0 0 0"/>
            <geometry>
                <mesh filename="anymal_optoforce.obj" scale="0.001 0.001     0.001"/>
            </geometry>
            <material name="anymal_material"/>
        </visual>
        <collision>
            <origin rpy="0 0 0" xyz="0.0 0.0 0"/>
            <geometry>
                <sphere radius="0.03"/>
            </geometry>
        </collision>
        <inertial>
            <origin rpy="0 0 0" xyz="5.552058e-06 7.1133124e-05 -0.021388647779"/>
            <mass value="0.1922685"/>
            <inertia ixx="8.3086410294e-05" ixy="-4.16901601e-07" ixz="-3.99054799e-07" iyy="8.2860209667e-05" iyz="-3.45712252e-07" izz="8.1948124086e-05"/>
        </inertial>
    </link>
    <!-- Foot mount joint / ankle -->
    <joint name="LH_ANKLE_JOINT" type="fixed">
        <parent link="LH_SHANK"/>
        <child link="LH_FOOT_MOUNT"/>
        <origin rpy="0.323 -0.549 0" xyz="-0.0 -0.016 -0.31"/>
    </joint>
    <!-- Xacro:Properties -->
    <!-- [m] -->
    <!-- Gazebo customization -->
    <gazebo reference="LH_FOOT_SENSOR">
        <kp>1000000.0</kp>
        <kd>100.0</kd>
        <mu1>0.8</mu1>
        <mu2>0.8</mu2>
    </gazebo>
    <gazebo reference="LH_MOUNT_TO_SENSOR"/>
    <!-- Xacro:Properties -->
    <!-- [m] -->
    <!-- [Nm] -->
    <link name="RH_HIP">
        <visual>
            <origin rpy="1.5707963268 1.5707963268 -1.5707963268" xyz="0 0 0"/>
            <geometry>
                <mesh filename="anymal_hip.obj" scale="0.001 0.001     0.001"/>
            </geometry>
            <material name="anymal_material"/>
        </visual>
        <collision>
            <origin rpy="-1.57079632679 0 0" xyz="-0.0585 -0.0315 0.0"/>
            <geometry>
                <cylinder length="0.08" radius="0.04"/>
            </geometry>
        </collision>
        <inertial>
            <origin rpy="0 0 0" xyz="-0.0542 0.0136 0.0"/>
            <mass value="1.1064"/>
            <inertia ixx="0.00123301508523" ixy="-6.4285329973e-05" ixz="-6.9990104e-08" iyy="0.000993231183395" iyz="-2.327401345e-06" izz="0.00142160349098"/>
        </inertial>
    </link>
    <!-- Hip joint -->
    <joint name="RH_HAA" type="revolute">
        <parent link="base"/>
        <child link="RH_HIP"/>
        <origin xyz="-0.2629 -0.115 0.0"/>
        <axis xyz="1 0 0"/>
        <limit effort="80" lower="-6.28" upper="6.28" velocity="15"/>
        <dynamics damping="0.0" friction="0.0"/>
    </joint>
    <!-- Xacro:Properties -->
    <!-- [m] -->
    <!-- [Nm] -->
    <link name="RH_THIGH">
        <visual>
            <origin rpy="0 0 3.14159265359" xyz="0 0 0"/>
            <geometry>
                <mesh filename="anymal_thigh.obj" scale="0.001 0.001     0.001"/>
            </geometry>
            <material name="anymal_material"/>
        </visual>
        <!--collision>
            <origin rpy="-0.145 0 0" xyz="0 -0.02 -0.1"/>
            <geometry>
                <cylinder length="0.2" radius="0.01"/>
            </geometry>
        </collision-->
        <collision>
            <origin rpy="1.57079632679 0 0" xyz="0.0 -0.07 -0.25"/>
            <geometry>
                <cylinder length="0.08" radius="0.04"/>
            </geometry>
        </collision>
        <inertial>
            <origin rpy="0 0 0" xyz="-0.0008 -0.0545 -0.1975"/>
            <mass value="1.51"/>
            <inertia ixx="0.0147571628275" ixy="-1.7996507882e-05" ixz="-0.000205877679383" iyy="0.0144188925719" iyz="0.00181291342366" izz="0.00178449075964"/>
        </inertial>
    </link>
    <!-- Thigh joint -->
    <joint name="RH_HFE" type="revolute">
        <parent link="RH_HIP"/>
        <child link="RH_THIGH"/>
        <origin xyz="-0.0585                    -0.0315                    0.0"/>
        <axis xyz="0 1 0"/>
        <limit effort="80" lower="-6.28" upper="6.28" velocity="15"/>
        <dynamics damping="0.0" friction="0.0"/>
    </joint>
    <!-- Xacro:Properties -->
    <!-- [m] -->
    <!-- [Nm] -->
    <link name="RH_SHANK">
        <visual>
            <origin rpy="0 0 0" xyz="0 0 0"/>
            <geometry>
                <mesh filename="anymal_shank_2_RH.obj" scale="0.001 0.001     0.001"/>
            </geometry>
            <material name="anymal_material"/>
        </visual>
        <!--collision>
            <origin rpy="0 0.9 -0.2" xyz="-0.04 0.02 -0.04"/>
            <geometry>
                <cylinder length="0.13" radius="0.015"/>
            </geometry>
        </collision-->
        <collision>
            <origin rpy="0.0 -0.34 0" xyz="-0.06 0.03 -0.18"/>
            <geometry>
                <cylinder length="0.19" radius="0.015"/>
            </geometry>
        </collision>
        <inertial>
            <origin rpy="0 0 0" xyz="-0.016355414056 0.00562219106 -0.064350437219"/>
            <mass value="0.318"/>
            <inertia ixx="0.0037693717909" ixy="-7.7334500762e-05" ixz="0.000152069372983" iyy="0.00400446467635" iyz="0.0003218356083" izz="0.000418471691904"/>
        </inertial>
    </link>
    <!-- Shank joint -->
    <joint name="RH_KFE" type="revolute">
        <parent link="RH_THIGH"/>
        <child link="RH_SHANK"/>
        <origin xyz="-0.0 -0.111 -0.25"/>
        <axis xyz="0 1 0"/>
        <limit effort="80" lower="-6.28" upper="6.28" velocity="15"/>
        <dynamics damping="0.0" friction="0.0"/>
    </joint>
    <!-- Foot mount link -->
    <link name="RH_FOOT_MOUNT">
        <visual>
            <origin rpy="0 0 0" xyz="0 0 0"/>
            <geometry>
                <mesh filename="anymal_optoforce.obj" scale="0.001 0.001     0.001"/>
            </geometry>
            <material name="anymal_material"/>
        </visual>
        <collision>
            <origin rpy="0 0 0" xyz="0.0 0.0 0"/>
            <geometry>
                <sphere radius="0.03"/>
            </geometry>
        </collision>
        <inertial>
            <origin rpy="0 0 0" xyz="5.552058e-06 -7.1133124e-05 -0.021388647779"/>
            <mass value="0.1922685"/>
            <inertia ixx="8.3086410294e-05" ixy="4.16901601e-07" ixz="-3.99054799e-07" iyy="8.2860209667e-05" iyz="3.45712252e-07" izz="8.1948124086e-05"/>
        </inertial>
    </link>
    <!-- Foot mount joint / ankle -->
    <joint name="RH_ANKLE_JOINT" type="fixed">
        <parent link="RH_SHANK"/>
        <child link="RH_FOOT_MOUNT"/>
        <origin rpy="-0.323 -0.549 0" xyz="-0.0 0.016 -0.31"/>
    </joint>
    <!-- Xacro:Properties -->
    <!-- [m] -->
    <!-- Gazebo customization -->
    <gazebo reference="RH_FOOT_SENSOR">
        <kp>1000000.0</kp>
        <kd>100.0</kd>
        <mu1>0.8</mu1>
        <mu2>0.8</mu2>
    </gazebo>
    <gazebo reference="RH_MOUNT_TO_SENSOR"/>
    <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="3.14159265359 0 0" xyz="0.0245 -0.04255 0.1857"/>
    </joint>
    <gazebo reference="imu_link">
        <material>Gazebo/Orange</material>
        <mu1>0.8</mu1>
        <mu2>0.8</mu2>
    </gazebo>
    <gazebo reference="imu_joint">
    </gazebo>
    <gazebo>
        <plugin filename="libhector_gazebo_ros_imu.so" name="imu_plugin">
            <robotNamespace>sensors</robotNamespace>
            <bodyName>imu_link</bodyName>
            <frameId>/imu_link</frameId>
            <topicName>imu</topicName>
            <biasTopicName>imu/bias</biasTopicName>
            <serviceName>imu/calibrate</serviceName>
            <updateRate>400</updateRate>
        </plugin>
    </gazebo>
    <!-- Omniview -->
    <!-- <xacro:include filename="$(find anymal_beth_description)/urdf/omniview/anymal_beth_omniview_macro.urdf.xacro"/> -->
    <!-- <xacro:anymal_beth_omniview_mesh link_name="omniview_mesh_front" parent_name="base" joint_name="omniview_mesh_front_joint"> -->
    <!-- <origin xyz="0.3312 0.0 0.025" rpy="-1.57079632679 0.0 0.0"/> -->
    <!-- </xacro:anymal_beth_omniview_mesh> -->
    <!-- <xacro:anymal_beth_omniview_mesh link_name="omniview_mesh_back" parent_name="base" joint_name="omniview_mesh_back_joint"> -->
    <!-- <origin xyz="-0.3312 0.0 0.025" rpy="-1.57079632679 0.0 3.14159265359"/> -->
    <!-- </xacro:anymal_beth_omniview_mesh> -->
    <!--<link name="static_lidar_front"/>-->
    <!--<joint name="base_to_static_lidar_front" type="fixed">-->
        <!--<origin rpy="1.57079632679 0.0 1.57079632679" xyz="0.2 0.0 0.1415"/>-->
        <!--<parent link="base"/>-->
        <!--<child link="static_lidar_front"/>-->
        <!--<origin rpy="0 0 0" xyz="0 0 0"/>-->
    <!--</joint>-->
    <!-- <xacro:anymal_lidar_fix name="static_lidar_back" parent="base" dynamixel_lidar_offset="0.1515">
        <origin xyz="-0.2 0.0 0.1415" rpy="1.57079632679 0.0 -1.57079632679"/>
      </xacro:anymal_lidar_fix> -->
</robot>
