<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- |    This document was autogenerated by xacro from hyq_no_sensors.urdf.xacro      | -->
<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
<!-- =================================================================================== -->
<robot name="hyq" xmlns:xacro="http://ros.org/wiki/xacro">
  <material name="white">
    <color rgba="1 1 1 1"/>
  </material>
  <material name="black">
    <color rgba="0 0 0 1"/>
  </material>
  <!-- This argument allows us to load joint sensors that measure the internal wrenches -->
  <!-- The following included files set up definitions of parts of the robot body -->
  <!-- kin limits (sign ref is RH leg)-->
  <!-- torque limits 120-->
  <!-- standard distances -->
  <!-- Inertial values -->
  <!-- Trunk with hydraulic system -->
  <!-- Hip assembly -->
  <!-- Upper leg -->
  <!-- Lower leg -->
  <!-- TODO checks the imu properties -->
  <!-- Microstrain kvh1775 IMU -->
  <!-- Microstrain 3dmgx425 IMU -->
  <!-- Microstrain 3dmgx325 IMU -->
  <!-- PTU unit -->
  <!-- Asus Xtion Camera Location (Nominally 0) -->
  <!-- Kinect Camera Location (Nominally 0) -->
  <!-- Bumblebee mass and lengths for computing inertia tensor -->
  <!-- Velodyne Lidar Location (Nominally 0) -->
  <!-- HighDef Camera Location (Nominally 0) -->
  <!-- HyQ trunk -->
  <!-- Things that are needed only for Gazebo (not the physical robot). These include sensor and controller plugin specifications -->
  <!-- ================================ Trunk ================================ -->
  <!-- The xacro macro xacro:hyq_base contains: base and trunk -->
  <!-- HyQ legs -->
  <!-- Things that are needed only for Gazebo (not the physical robot). These include sensor and controller plugin specifications -->
  <!-- ================================== Leg ================================== -->
  <!-- The xacro macro xacro:hyq_leg contains: hip assembly, upper and lower leg -->
  <!-- HyQ sensors -->
  <!-- ============================ IMU microstrain ============================== -->
  <!-- The xacro macro xacro:microstrain_3dmgx425_imu: imu frame -->
  <!-- generic simulator_gazebo plugins for starting mechanism control, ros time -->
  <gazebo>
    <plugin filename="libgazebo_ros_control.so" name="gazebo_ros_controller_manager">
      <robotNamespace>/hyq</robotNamespace>
      <!--			<robotSimType>dls_gazebo_interface/DLSHWSim</robotSimType>-->
      <controlPeriod>0.004</controlPeriod>
    </plugin>
    <plugin filename="libgazebo_ros_p3d.so" name="gazebo_ros_p3d">
      <robotNamespace>/hyq</robotNamespace>
      <bodyName>trunk</bodyName>
      <topicName>ground_truth</topicName>
      <frameName>world</frameName>
      <xyzOffsets>0 0 0</xyzOffsets>
      <rpyOffsets>0 0 0</rpyOffsets>
      <gaussianNoise>0</gaussianNoise>
      <alwaysOn>true</alwaysOn>
      <updateRate>250.0</updateRate>
    </plugin>
  </gazebo>
  <!-- Now we can start using the macros included above to define the actual HyQ -->
  <!-- The first use of a macro. This one was defined in trunk.urdf.xacro above.
A macro like this will expand to a set of link and joint definitions, and to additional
Gazebo-related extensions (sensor plugins, etc). The macro takes an argument, name,
that equals "base", and uses it to generate names for its component links and joints
(e.g., base_link). The included origin block is also an argument to the macro. By convention,
the origin block defines where the component is w.r.t its parent (in this case the parent
is the world frame). For more, see http://ros.org/wiki/xacro -->
  <!-- body -->
  <!-- trunk -->
  <!-- Floating-base Joint -->
  <joint name="floating_base" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <parent link="base_link"/>
    <child link="trunk"/>
  </joint>
  <!-- Links -->
  <!-- Footprint link -->
  <link name="base_link">
    <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>
    <visual>
      <geometry>
        <cylinder length="0.01" radius="0.01"/>
      </geometry>
    </visual>
  </link>
  <!-- Trunk link -->
  <link name="trunk">
    <inertial>
      <origin xyz="0.056 0.0215 0.00358"/>
      <mass value="60.96"/>
      <inertia ixx="1.5725937" ixy="0.028375" ixz="-0.203139" iyy="8.5015928" iyz="-0.004462" izz="9.1954911"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="package://example-robot-data/robots/hyq_description/meshes/trunk/trunk.dae" scale="1 1 1"/>
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="package://example-robot-data/robots/hyq_description/meshes/trunk/trunk.dae" scale="1 1 1"/>
      </geometry>
    </collision>
  </link>
  <!-- Extensions -->
  <gazebo reference="trunk">
    <mu1>1.5</mu1>
    <mu2>1.5</mu2>
  </gazebo>
  <!-- LF leg -->
  <!-- Joints -->
  <!-- Hip assembly joint -->
  <joint name="lf_haa_joint" type="revolute">
    <origin rpy="0 1.57079632679 3.14159265359" xyz="0.3735 0.207 0"/>
    <parent link="trunk"/>
    <child link="lf_hipassembly"/>
    <axis xyz="0 0 1"/>
    <limit effort="150" lower="-1.2217304764" upper="0.436332312999" velocity="12.0"/>
    <dynamics damping="0.1" friction="0"/>
    <safety_controller k_position="50" k_velocity="10" soft_lower_limit="-1.2217304764" soft_upper_limit="0.436332312999"/>
  </joint>
  <!-- Upper leg joint -->
  <joint name="lf_hfe_joint" type="revolute">
    <origin rpy="1.57079632679 0 0" xyz="0.08 0 0"/>
    <parent link="lf_hipassembly"/>
    <child link="lf_upperleg"/>
    <axis xyz="0 0 1"/>
    <limit effort="150" lower="-0.872664625997" upper="1.2217304764" velocity="12.0"/>
    <dynamics damping="0.1" friction="0"/>
    <safety_controller k_position="50" k_velocity="10" soft_lower_limit="-0.872664625997" soft_upper_limit="1.2217304764"/>
  </joint>
  <!-- Lower leg joint -->
  <joint name="lf_kfe_joint" type="revolute">
    <origin rpy="0 0 0" xyz="0.35 0 0"/>
    <parent link="lf_upperleg"/>
    <child link="lf_lowerleg"/>
    <axis xyz="0 0 1"/>
    <limit effort="150" lower="-2.44346095279" upper="-0.349065850399" velocity="12.0"/>
    <dynamics damping="0.1" friction="0"/>
    <safety_controller k_position="50" k_velocity="10" soft_lower_limit="-2.44346095279" soft_upper_limit="-0.349065850399"/>
    <!-- Foot joint -->
  </joint>
  <joint name="lf_foot_joint" type="fixed">
    <origin rpy="1.57079632679 0 -1.57079632679" xyz="0.346 0 0"/>
    <parent link="lf_lowerleg"/>
    <child link="lf_foot"/>
  </joint>
  <!-- Links -->
  <!-- Hip assembly link -->
  <link name="lf_hipassembly">
    <inertial>
      <origin xyz="0.04263         0.0         0.16931"/>
      <mass value="2.93"/>
      <inertia ixx="0.05071" ixy="-4e-05" ixz="-0.00159" iyy="0.05486" iyz="-5e-05" izz="0.00571"/>
    </inertial>
    <visual>
      <origin rpy="0.0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/hyq_description/meshes/leg/hipassembly.dae" scale="1 1 1"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0.0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/hyq_description/meshes/leg/hipassembly.dae" scale="1 1 1"/>
      </geometry>
    </collision>
  </link>
  <!-- Upper leg link -->
  <link name="lf_upperleg">
    <inertial>
      <origin xyz="0.15074 -0.02625 0.0"/>
      <mass value="2.638"/>
      <inertia ixx="0.00368" ixy="-0.00302" ixz="0.0001" iyy="0.02719" iyz="2e-05" izz="0.02811"/>
    </inertial>
    <visual>
      <origin rpy="0.0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/hyq_description/meshes/leg/upperleg.dae" scale="1 1 1"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0.0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/hyq_description/meshes/leg/upperleg.dae" scale="1 1 1"/>
      </geometry>
    </collision>
  </link>
  <!-- Lower leg link -->
  <link name="lf_lowerleg">
    <inertial>
      <origin xyz="0.1254 4e-05 -0.0001"/>
      <mass value="0.881"/>
      <inertia ixx="0.00047" ixy="6e-05" ixz="-1e-05" iyy="0.01256" iyz="-0.0" izz="0.01233"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="package://example-robot-data/robots/hyq_description/meshes/leg/lowerleg.dae" scale="1 1 1"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <origin rpy="0 -1.57079632679  0" xyz="0.173 0 0"/>
      <geometry>
        <cylinder length="0.346" radius="0.02"/>
      </geometry>
    </collision>
  </link>
  <!-- Foot link -->
  <link name="lf_foot">
    <contact>
      <lateral_friction value="1"/>
      <stiffness value="30000"/>
      <damping value="1000"/>
    </contact>
    <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>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <sphere radius="0.02175"/>
      </geometry>
    </collision>
    <visual>
      <geometry>
        <sphere radius="0.02175"/>
      </geometry>
      <material name="black"/>
    </visual>
  </link>
  <!-- Extensions -->
  <!-- Hip assembly -->
  <transmission name="lf_haa_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="lf_haa_joint">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="lf_haa_motor">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <!-- Upper leg -->
  <transmission name="lf_hfe_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="lf_hfe_joint">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="lf_hfe_motor">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <!-- Lower leg -->
  <transmission name="lf_kfe_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="lf_kfe_joint">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="lf_kfe_motor">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <gazebo reference="lf_haa_joint">
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <gazebo reference="lf_kfe_joint">
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <gazebo reference="lf_hfe_joint">
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <gazebo reference="lf_upperleg">
    <kp>1000000.0</kp>
    <kd>100.0</kd>
    <mu1>1.5</mu1>
    <mu2>1.5</mu2>
    <fdir1>1 0 0</fdir1>
    <maxVel>1.0</maxVel>
    <minDepth>0.00</minDepth>
  </gazebo>
  <gazebo reference="lf_lowerleg">
    <kp>1000000.0</kp>
    <kd>100.0</kd>
    <mu1>1.0</mu1>
    <mu2>1.0</mu2>
    <maxVel>1.0</maxVel>
    <maxContacts>1</maxContacts>
    <sensor name="lf_shin_contact_sensor" type="contact">
      <always_on>true</always_on>
      <update_rate>250.0</update_rate>
      <contact>
        <collision>lf_lowerleg_collision</collision>
      </contact>
      <plugin filename="libgazebo_ros_bumper.so" name="lf_shin_bumper">
        <bumperTopicName>/hyq/lf_shin_bumper</bumperTopicName>
        <frameName>base_link</frameName>
      </plugin>
    </sensor>
  </gazebo>
  <gazebo reference="lf_foot">
    <kp>1000000.0</kp>
    <kd>100.0</kd>
    <mu1>1.0</mu1>
    <mu2>1.0</mu2>
    <maxVel>1.0</maxVel>
    <maxContacts>1</maxContacts>
    <sensor name="lf_foot_contact_sensor" type="contact">
      <always_on>true</always_on>
      <update_rate>250.0</update_rate>
      <contact>
        <collision>lf_lowerleg_fixed_joint_lump__lf_foot_collision_1</collision>
      </contact>
      <plugin filename="libgazebo_ros_bumper.so" name="lf_foot_bumper">
        <bumperTopicName>/hyq/lf_foot_bumper</bumperTopicName>
        <frameName>base_link</frameName>
        <robotNamespace>hyq</robotNamespace>
      </plugin>
    </sensor>
    <material>Gazebo/Black</material>
  </gazebo>
  <!-- RF leg -->
  <!-- Joints -->
  <!-- Hip assembly joint -->
  <joint name="rf_haa_joint" type="revolute">
    <origin rpy="0 1.57079632679 0" xyz="0.3735 -0.207 0"/>
    <parent link="trunk"/>
    <child link="rf_hipassembly"/>
    <axis xyz="0 0 1"/>
    <limit effort="150" lower="-1.2217304764" upper="0.436332312999" velocity="12.0"/>
    <dynamics damping="0.1" friction="0"/>
    <safety_controller k_position="50" k_velocity="10" soft_lower_limit="-1.2217304764" soft_upper_limit="0.436332312999"/>
  </joint>
  <!-- Upper leg joint -->
  <joint name="rf_hfe_joint" type="revolute">
    <origin rpy="-1.57079632679 0 0" xyz="0.08 0 0"/>
    <parent link="rf_hipassembly"/>
    <child link="rf_upperleg"/>
    <axis xyz="0 0 1"/>
    <limit effort="150" lower="-0.872664625997" upper="1.2217304764" velocity="12.0"/>
    <dynamics damping="0.1" friction="0"/>
    <safety_controller k_position="50" k_velocity="10" soft_lower_limit="-0.872664625997" soft_upper_limit="1.2217304764"/>
  </joint>
  <!-- Lower leg joint -->
  <joint name="rf_kfe_joint" type="revolute">
    <origin rpy="0 0 0" xyz="0.35 0 0"/>
    <parent link="rf_upperleg"/>
    <child link="rf_lowerleg"/>
    <axis xyz="0 0 1"/>
    <limit effort="150" lower="-2.44346095279" upper="-0.349065850399" velocity="12.0"/>
    <dynamics damping="0.1" friction="0"/>
    <safety_controller k_position="50" k_velocity="10" soft_lower_limit="-2.44346095279" soft_upper_limit="-0.349065850399"/>
    <!-- Foot joint -->
  </joint>
  <joint name="rf_foot_joint" type="fixed">
    <origin rpy="1.57079632679 0 -1.57079632679" xyz="0.346 0 0"/>
    <parent link="rf_lowerleg"/>
    <child link="rf_foot"/>
  </joint>
  <!-- Links -->
  <!-- Hip assembly link -->
  <link name="rf_hipassembly">
    <inertial>
      <origin xyz="0.04263         -0.0         -0.16931"/>
      <mass value="2.93"/>
      <inertia ixx="0.05071" ixy="4e-05" ixz="0.00159" iyy="0.05486" iyz="-5e-05" izz="0.00571"/>
    </inertial>
    <visual>
      <origin rpy="3.14159265359 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/hyq_description/meshes/leg/hipassembly.dae" scale="1 1 1"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="3.14159265359 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/hyq_description/meshes/leg/hipassembly.dae" scale="1 1 1"/>
      </geometry>
    </collision>
  </link>
  <!-- Upper leg link -->
  <link name="rf_upperleg">
    <inertial>
      <origin xyz="0.15074 -0.02625 0.0"/>
      <mass value="2.638"/>
      <inertia ixx="0.00368" ixy="-0.00302" ixz="0.0001" iyy="0.02719" iyz="2e-05" izz="0.02811"/>
    </inertial>
    <visual>
      <origin rpy="0.0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/hyq_description/meshes/leg/upperleg.dae" scale="1 1 1"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0.0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/hyq_description/meshes/leg/upperleg.dae" scale="1 1 1"/>
      </geometry>
    </collision>
  </link>
  <!-- Lower leg link -->
  <link name="rf_lowerleg">
    <inertial>
      <origin xyz="0.1254 4e-05 -0.0001"/>
      <mass value="0.881"/>
      <inertia ixx="0.00047" ixy="6e-05" ixz="-1e-05" iyy="0.01256" iyz="-0.0" izz="0.01233"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="package://example-robot-data/robots/hyq_description/meshes/leg/lowerleg.dae" scale="1 1 1"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <origin rpy="0 -1.57079632679  0" xyz="0.173 0 0"/>
      <geometry>
        <cylinder length="0.346" radius="0.02"/>
      </geometry>
    </collision>
  </link>
  <!-- Foot link -->
  <link name="rf_foot">
    <contact>
      <lateral_friction value="1"/>
      <stiffness value="30000"/>
      <damping value="1000"/>
    </contact>
    <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>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <sphere radius="0.02175"/>
      </geometry>
    </collision>
    <visual>
      <geometry>
        <sphere radius="0.02175"/>
      </geometry>
      <material name="black"/>
    </visual>
  </link>
  <!-- Extensions -->
  <!-- Hip assembly -->
  <transmission name="rf_haa_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="rf_haa_joint">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="rf_haa_motor">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <!-- Upper leg -->
  <transmission name="rf_hfe_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="rf_hfe_joint">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="rf_hfe_motor">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <!-- Lower leg -->
  <transmission name="rf_kfe_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="rf_kfe_joint">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="rf_kfe_motor">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <gazebo reference="rf_haa_joint">
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <gazebo reference="rf_kfe_joint">
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <gazebo reference="rf_hfe_joint">
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <gazebo reference="rf_upperleg">
    <kp>1000000.0</kp>
    <kd>100.0</kd>
    <mu1>1.5</mu1>
    <mu2>1.5</mu2>
    <fdir1>1 0 0</fdir1>
    <maxVel>1.0</maxVel>
    <minDepth>0.00</minDepth>
  </gazebo>
  <gazebo reference="rf_lowerleg">
    <kp>1000000.0</kp>
    <kd>100.0</kd>
    <mu1>1.0</mu1>
    <mu2>1.0</mu2>
    <maxVel>1.0</maxVel>
    <maxContacts>1</maxContacts>
    <sensor name="rf_shin_contact_sensor" type="contact">
      <always_on>true</always_on>
      <update_rate>250.0</update_rate>
      <contact>
        <collision>rf_lowerleg_collision</collision>
      </contact>
      <plugin filename="libgazebo_ros_bumper.so" name="rf_shin_bumper">
        <bumperTopicName>/hyq/rf_shin_bumper</bumperTopicName>
        <frameName>base_link</frameName>
      </plugin>
    </sensor>
  </gazebo>
  <gazebo reference="rf_foot">
    <kp>1000000.0</kp>
    <kd>100.0</kd>
    <mu1>1.0</mu1>
    <mu2>1.0</mu2>
    <maxVel>1.0</maxVel>
    <maxContacts>1</maxContacts>
    <sensor name="rf_foot_contact_sensor" type="contact">
      <always_on>true</always_on>
      <update_rate>250.0</update_rate>
      <contact>
        <collision>rf_lowerleg_fixed_joint_lump__rf_foot_collision_1</collision>
      </contact>
      <plugin filename="libgazebo_ros_bumper.so" name="rf_foot_bumper">
        <bumperTopicName>/hyq/rf_foot_bumper</bumperTopicName>
        <frameName>base_link</frameName>
        <robotNamespace>hyq</robotNamespace>
      </plugin>
    </sensor>
    <material>Gazebo/Black</material>
  </gazebo>
  <!-- LH leg -->
  <!-- Joints -->
  <!-- Hip assembly joint -->
  <joint name="lh_haa_joint" type="revolute">
    <origin rpy="0 1.57079632679 3.14159265359" xyz="-0.3735 0.207 0"/>
    <parent link="trunk"/>
    <child link="lh_hipassembly"/>
    <axis xyz="0 0 1"/>
    <limit effort="150" lower="-1.2217304764" upper="0.436332312999" velocity="12.0"/>
    <dynamics damping="0.1" friction="0"/>
    <safety_controller k_position="50" k_velocity="10" soft_lower_limit="-1.2217304764" soft_upper_limit="0.436332312999"/>
  </joint>
  <!-- Upper leg joint -->
  <joint name="lh_hfe_joint" type="revolute">
    <origin rpy="1.57079632679 0 0" xyz="0.08 0 0"/>
    <parent link="lh_hipassembly"/>
    <child link="lh_upperleg"/>
    <axis xyz="0 0 1"/>
    <limit effort="150" lower="-1.2217304764" upper="0.872664625997" velocity="12.0"/>
    <dynamics damping="0.1" friction="0"/>
    <safety_controller k_position="50" k_velocity="10" soft_lower_limit="-1.2217304764" soft_upper_limit="0.872664625997"/>
  </joint>
  <!-- Lower leg joint -->
  <joint name="lh_kfe_joint" type="revolute">
    <origin rpy="0 0 0" xyz="0.35 0 0"/>
    <parent link="lh_upperleg"/>
    <child link="lh_lowerleg"/>
    <axis xyz="0 0 1"/>
    <limit effort="150" lower="0.349065850399" upper="2.44346095279" velocity="12.0"/>
    <dynamics damping="0.1" friction="0"/>
    <safety_controller k_position="50" k_velocity="10" soft_lower_limit="0.349065850399" soft_upper_limit="2.44346095279"/>
    <!-- Foot joint -->
  </joint>
  <joint name="lh_foot_joint" type="fixed">
    <origin rpy="1.57079632679 0 -1.57079632679" xyz="0.346 0 0"/>
    <parent link="lh_lowerleg"/>
    <child link="lh_foot"/>
  </joint>
  <!-- Links -->
  <!-- Hip assembly link -->
  <link name="lh_hipassembly">
    <inertial>
      <origin xyz="0.04263         -0.0         -0.16931"/>
      <mass value="2.93"/>
      <inertia ixx="0.05071" ixy="4e-05" ixz="0.00159" iyy="0.05486" iyz="-5e-05" izz="0.00571"/>
    </inertial>
    <visual>
      <origin rpy="3.14159265359 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/hyq_description/meshes/leg/hipassembly.dae" scale="1 1 1"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="3.14159265359 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/hyq_description/meshes/leg/hipassembly.dae" scale="1 1 1"/>
      </geometry>
    </collision>
  </link>
  <!-- Upper leg link -->
  <link name="lh_upperleg">
    <inertial>
      <origin xyz="0.15074 0.02625 0.0"/>
      <mass value="2.638"/>
      <inertia ixx="0.00368" ixy="0.00302" ixz="-0.0001" iyy="0.02719" iyz="2e-05" izz="0.02811"/>
    </inertial>
    <visual>
      <origin rpy="3.14159265359 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/hyq_description/meshes/leg/upperleg.dae" scale="1 1 1"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="3.14159265359 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/hyq_description/meshes/leg/upperleg.dae" scale="1 1 1"/>
      </geometry>
    </collision>
  </link>
  <!-- Lower leg link -->
  <link name="lh_lowerleg">
    <inertial>
      <origin xyz="0.1254 -4e-05 0.0001"/>
      <mass value="0.881"/>
      <inertia ixx="0.00047" ixy="-6e-05" ixz="1e-05" iyy="0.01256" iyz="-0.0" izz="0.01233"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="package://example-robot-data/robots/hyq_description/meshes/leg/lowerleg.dae" scale="1 1 1"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <origin rpy="0 -1.57079632679  0" xyz="0.173 0 0"/>
      <geometry>
        <cylinder length="0.346" radius="0.02"/>
      </geometry>
    </collision>
  </link>
  <!-- Foot link -->
  <link name="lh_foot">
    <contact>
      <lateral_friction value="1"/>
      <stiffness value="30000"/>
      <damping value="1000"/>
    </contact>
    <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>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <sphere radius="0.02175"/>
      </geometry>
    </collision>
    <visual>
      <geometry>
        <sphere radius="0.02175"/>
      </geometry>
      <material name="black"/>
    </visual>
  </link>
  <!-- Extensions -->
  <!-- Hip assembly -->
  <transmission name="lh_haa_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="lh_haa_joint">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="lh_haa_motor">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <!-- Upper leg -->
  <transmission name="lh_hfe_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="lh_hfe_joint">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="lh_hfe_motor">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <!-- Lower leg -->
  <transmission name="lh_kfe_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="lh_kfe_joint">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="lh_kfe_motor">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <gazebo reference="lh_haa_joint">
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <gazebo reference="lh_kfe_joint">
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <gazebo reference="lh_hfe_joint">
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <gazebo reference="lh_upperleg">
    <kp>1000000.0</kp>
    <kd>100.0</kd>
    <mu1>1.5</mu1>
    <mu2>1.5</mu2>
    <fdir1>1 0 0</fdir1>
    <maxVel>1.0</maxVel>
    <minDepth>0.00</minDepth>
  </gazebo>
  <gazebo reference="lh_lowerleg">
    <kp>1000000.0</kp>
    <kd>100.0</kd>
    <mu1>1.0</mu1>
    <mu2>1.0</mu2>
    <maxVel>1.0</maxVel>
    <maxContacts>1</maxContacts>
    <sensor name="lh_shin_contact_sensor" type="contact">
      <always_on>true</always_on>
      <update_rate>250.0</update_rate>
      <contact>
        <collision>lh_lowerleg_collision</collision>
      </contact>
      <plugin filename="libgazebo_ros_bumper.so" name="lh_shin_bumper">
        <bumperTopicName>/hyq/lh_shin_bumper</bumperTopicName>
        <frameName>base_link</frameName>
      </plugin>
    </sensor>
  </gazebo>
  <gazebo reference="lh_foot">
    <kp>1000000.0</kp>
    <kd>100.0</kd>
    <mu1>1.0</mu1>
    <mu2>1.0</mu2>
    <maxVel>1.0</maxVel>
    <maxContacts>1</maxContacts>
    <sensor name="lh_foot_contact_sensor" type="contact">
      <always_on>true</always_on>
      <update_rate>250.0</update_rate>
      <contact>
        <collision>lh_lowerleg_fixed_joint_lump__lh_foot_collision_1</collision>
      </contact>
      <plugin filename="libgazebo_ros_bumper.so" name="lh_foot_bumper">
        <bumperTopicName>/hyq/lh_foot_bumper</bumperTopicName>
        <frameName>base_link</frameName>
        <robotNamespace>hyq</robotNamespace>
      </plugin>
    </sensor>
    <material>Gazebo/Black</material>
  </gazebo>
  <!-- RH leg -->
  <!-- Joints -->
  <!-- Hip assembly joint -->
  <joint name="rh_haa_joint" type="revolute">
    <origin rpy="0 1.57079632679 0" xyz="-0.3735 -0.207 0"/>
    <parent link="trunk"/>
    <child link="rh_hipassembly"/>
    <axis xyz="0 0 1"/>
    <limit effort="150" lower="-1.2217304764" upper="0.436332312999" velocity="12.0"/>
    <dynamics damping="0.1" friction="0"/>
    <safety_controller k_position="50" k_velocity="10" soft_lower_limit="-1.2217304764" soft_upper_limit="0.436332312999"/>
  </joint>
  <!-- Upper leg joint -->
  <joint name="rh_hfe_joint" type="revolute">
    <origin rpy="-1.57079632679 0 0" xyz="0.08 0 0"/>
    <parent link="rh_hipassembly"/>
    <child link="rh_upperleg"/>
    <axis xyz="0 0 1"/>
    <limit effort="150" lower="-1.2217304764" upper="0.872664625997" velocity="12.0"/>
    <dynamics damping="0.1" friction="0"/>
    <safety_controller k_position="50" k_velocity="10" soft_lower_limit="-1.2217304764" soft_upper_limit="0.872664625997"/>
  </joint>
  <!-- Lower leg joint -->
  <joint name="rh_kfe_joint" type="revolute">
    <origin rpy="0 0 0" xyz="0.35 0 0"/>
    <parent link="rh_upperleg"/>
    <child link="rh_lowerleg"/>
    <axis xyz="0 0 1"/>
    <limit effort="150" lower="0.349065850399" upper="2.44346095279" velocity="12.0"/>
    <dynamics damping="0.1" friction="0"/>
    <safety_controller k_position="50" k_velocity="10" soft_lower_limit="0.349065850399" soft_upper_limit="2.44346095279"/>
    <!-- Foot joint -->
  </joint>
  <joint name="rh_foot_joint" type="fixed">
    <origin rpy="1.57079632679 0 -1.57079632679" xyz="0.346 0 0"/>
    <parent link="rh_lowerleg"/>
    <child link="rh_foot"/>
  </joint>
  <!-- Links -->
  <!-- Hip assembly link -->
  <link name="rh_hipassembly">
    <inertial>
      <origin xyz="0.04263         0.0         0.16931"/>
      <mass value="2.93"/>
      <inertia ixx="0.05071" ixy="-4e-05" ixz="-0.00159" iyy="0.05486" iyz="-5e-05" izz="0.00571"/>
    </inertial>
    <visual>
      <origin rpy="0.0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/hyq_description/meshes/leg/hipassembly.dae" scale="1 1 1"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0.0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/hyq_description/meshes/leg/hipassembly.dae" scale="1 1 1"/>
      </geometry>
    </collision>
  </link>
  <!-- Upper leg link -->
  <link name="rh_upperleg">
    <inertial>
      <origin xyz="0.15074 0.02625 0.0"/>
      <mass value="2.638"/>
      <inertia ixx="0.00368" ixy="0.00302" ixz="-0.0001" iyy="0.02719" iyz="2e-05" izz="0.02811"/>
    </inertial>
    <visual>
      <origin rpy="3.14159265359 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/hyq_description/meshes/leg/upperleg.dae" scale="1 1 1"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="3.14159265359 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/hyq_description/meshes/leg/upperleg.dae" scale="1 1 1"/>
      </geometry>
    </collision>
  </link>
  <!-- Lower leg link -->
  <link name="rh_lowerleg">
    <inertial>
      <origin xyz="0.1254 -4e-05 0.0001"/>
      <mass value="0.881"/>
      <inertia ixx="0.00047" ixy="-6e-05" ixz="1e-05" iyy="0.01256" iyz="-0.0" izz="0.01233"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="package://example-robot-data/robots/hyq_description/meshes/leg/lowerleg.dae" scale="1 1 1"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <origin rpy="0 -1.57079632679  0" xyz="0.173 0 0"/>
      <geometry>
        <cylinder length="0.346" radius="0.02"/>
      </geometry>
    </collision>
  </link>
  <!-- Foot link -->
  <link name="rh_foot">
    <contact>
      <lateral_friction value="1"/>
      <stiffness value="30000"/>
      <damping value="1000"/>
    </contact>
    <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>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <sphere radius="0.02175"/>
      </geometry>
    </collision>
    <visual>
      <geometry>
        <sphere radius="0.02175"/>
      </geometry>
      <material name="black"/>
    </visual>
  </link>
  <!-- Extensions -->
  <!-- Hip assembly -->
  <transmission name="rh_haa_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="rh_haa_joint">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="rh_haa_motor">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <!-- Upper leg -->
  <transmission name="rh_hfe_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="rh_hfe_joint">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="rh_hfe_motor">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <!-- Lower leg -->
  <transmission name="rh_kfe_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="rh_kfe_joint">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="rh_kfe_motor">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <gazebo reference="rh_haa_joint">
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <gazebo reference="rh_kfe_joint">
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <gazebo reference="rh_hfe_joint">
    <provideFeedback>true</provideFeedback>
  </gazebo>
  <gazebo reference="rh_upperleg">
    <kp>1000000.0</kp>
    <kd>100.0</kd>
    <mu1>1.5</mu1>
    <mu2>1.5</mu2>
    <fdir1>1 0 0</fdir1>
    <maxVel>1.0</maxVel>
    <minDepth>0.00</minDepth>
  </gazebo>
  <gazebo reference="rh_lowerleg">
    <kp>1000000.0</kp>
    <kd>100.0</kd>
    <mu1>1.0</mu1>
    <mu2>1.0</mu2>
    <maxVel>1.0</maxVel>
    <maxContacts>1</maxContacts>
    <sensor name="rh_shin_contact_sensor" type="contact">
      <always_on>true</always_on>
      <update_rate>250.0</update_rate>
      <contact>
        <collision>rh_lowerleg_collision</collision>
      </contact>
      <plugin filename="libgazebo_ros_bumper.so" name="rh_shin_bumper">
        <bumperTopicName>/hyq/rh_shin_bumper</bumperTopicName>
        <frameName>base_link</frameName>
      </plugin>
    </sensor>
  </gazebo>
  <gazebo reference="rh_foot">
    <kp>1000000.0</kp>
    <kd>100.0</kd>
    <mu1>1.0</mu1>
    <mu2>1.0</mu2>
    <maxVel>1.0</maxVel>
    <maxContacts>1</maxContacts>
    <sensor name="rh_foot_contact_sensor" type="contact">
      <always_on>true</always_on>
      <update_rate>250.0</update_rate>
      <contact>
        <collision>rh_lowerleg_fixed_joint_lump__rh_foot_collision_1</collision>
      </contact>
      <plugin filename="libgazebo_ros_bumper.so" name="rh_foot_bumper">
        <bumperTopicName>/hyq/rh_foot_bumper</bumperTopicName>
        <frameName>base_link</frameName>
        <robotNamespace>hyq</robotNamespace>
      </plugin>
    </sensor>
    <material>Gazebo/Black</material>
  </gazebo>
  <!-- IMU trunk -->
  <!-- Joint -->
  <joint name="trunk_imu_joint" type="fixed">
    <origin rpy="0 -3.14159265359 0" xyz="0.290 0.000 0.0999214"/>
    <parent link="trunk"/>
    <child link="trunk_imu"/>
  </joint>
  <!-- Link -->
  <link name="trunk_imu">
    <inertial>
      <origin xyz="-0.00020  0.00010  0.00110"/>
      <mass value="0.018"/>
      <inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0.00485 0 -0.00105"/>
      <geometry>
        <box size="0.0329 0.0244 0.0111"/>
      </geometry>
      <material name="black">
        <color rgba="0 0 0 0.9"/>
      </material>
    </visual>
  </link>
  <!-- IMU Gazebo plugin definition -->
  <gazebo>
    <plugin filename="libgazebo_ros_imu.so" name="trunk_imu_controller">
      <alwaysOn>true</alwaysOn>
      <updateRate>250</updateRate>
      <bodyName>trunk_imu</bodyName>
      <topicName>trunk_imu/data</topicName>
      <gaussianNoise>4e-08</gaussianNoise>
      <xyzOffset>0 0 0</xyzOffset>
      <rpyOffset>0 0 0</rpyOffset>
      <serviceName>/trunk_imu_imu</serviceName>
    </plugin>
  </gazebo>
  <!-- Imu definition required for DLS hardware interface of ros control -->
  <gazebo reference="trunk_imu">
    <sensor name="trunk_imu" type="imu">
      <always_on>1</always_on>
      <update_rate>250</update_rate>
      <imu>
        <noise>
          <type>gaussian</type>
          <rate>
            <mean>0</mean>
            <stddev>0.0002</stddev>
            <bias_mean>0.00087</bias_mean>
            <bias_stddev>8e-07</bias_stddev>
          </rate>
          <accel>
            <mean>0</mean>
            <stddev>0.017</stddev>
            <bias_mean>0.002</bias_mean>
            <bias_stddev>0.001</bias_stddev>
          </accel>
        </noise>
      </imu>
    </sensor>
  </gazebo>
</robot>
