diff --git a/.gitmodules b/.gitmodules
index 6df22de..efdfd5d 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -2,9 +2,6 @@
 [submodule "urdf_files/A1/unitree_ros"]
 	path = urdf_files/A1/unitree_ros
 	url = https://github.com/unitreerobotics/unitree_ros.git
-[submodule "urdf_files/HyQ/hyq-description"]
-	path = urdf_files/HyQ/hyq-description
-	url = https://github.com/iit-DLSLab/hyq-description.git
 [submodule "urdf_files/Go1/unitree_ros"]
 	path = urdf_files/Go1/unitree_ros
 	url = https://github.com/unitreerobotics/unitree_ros.git
diff --git a/tests/testGraphParser.py b/tests/testGraphParser.py
index 99c8558..2982857 100644
--- a/tests/testGraphParser.py
+++ b/tests/testGraphParser.py
@@ -13,12 +13,11 @@
 class TestNormalRobotGraph(unittest.TestCase):
 
     def setUp(self):
-        self.hyq_path = Path(
-            Path(__file__).cwd(), 'urdf_files', 'HyQ', 'hyq.urdf').absolute()
+        self.mini_cheetah_path = Path(
+            Path(__file__).cwd(), 'urdf_files', 'MiniCheetah', 'miniCheetah.urdf').absolute()
 
-        self.HyQ_URDF = NormalRobotGraph(self.hyq_path,
-                                         'package://hyq_description/',
-                                         'hyq-description')
+        self.mini_cheetah_URDF = NormalRobotGraph(self.mini_cheetah_path,
+                'package://yobotics_description/', 'mini-cheetah-gazebo-urdf/yobo_model/yobotics_description')
 
     def test_constructor(self):
         """
@@ -26,15 +25,15 @@ def test_constructor(self):
         """
 
         joint_names = [
-            'floating_base', 'lf_haa_joint', 'lf_hfe_joint', 'lf_kfe_joint',
-            'lf_foot_joint', 'rf_haa_joint', 'rf_hfe_joint', 'rf_kfe_joint',
-            'rf_foot_joint', 'lh_haa_joint', 'lh_hfe_joint', 'lh_kfe_joint',
-            'lh_foot_joint', 'rh_haa_joint', 'rh_hfe_joint', 'rh_kfe_joint',
-            'rh_foot_joint'
+            'floating_base', 
+            'FL_hip_joint', 'FL_thigh_joint', 'FL_calf_joint', 'FL_foot_fixed', 
+            'FR_hip_joint', 'FR_thigh_joint', 'FR_calf_joint', 'FR_foot_fixed', 
+            'RL_hip_joint', 'RL_thigh_joint', 'RL_calf_joint', 'RL_foot_fixed', 
+            'RR_hip_joint', 'RR_thigh_joint', 'RR_calf_joint', 'RR_foot_fixed', 
         ]
 
         edge_names_copy = copy.deepcopy(joint_names)
-        for i, node in enumerate(self.HyQ_URDF.nodes):
+        for i, node in enumerate(self.mini_cheetah_URDF.nodes):
             self.assertTrue(node.name in edge_names_copy)
             edge_names_copy.remove(node.name)
         self.assertEqual(0, len(edge_names_copy))
@@ -44,28 +43,28 @@ def test_constructor(self):
         # Additionally, links with multiple children joints get one
         # edge for each child.
         desired_edges = [
-            RobotGraph.Edge('trunk_to_lf_haa_joint', "floating_base",
-                            "lf_haa_joint", None),
-            RobotGraph.Edge('trunk_to_lh_haa_joint', "floating_base",
-                            "lh_haa_joint", None),
-            RobotGraph.Edge('trunk_to_rf_haa_joint', "floating_base",
-                            "rf_haa_joint", None),
-            RobotGraph.Edge('trunk_to_rh_haa_joint', "floating_base",
-                            "rh_haa_joint", None),
-            RobotGraph.Edge('lf_hipassembly', "lf_haa_joint", "lf_hfe_joint", None),
-            RobotGraph.Edge('lf_upperleg', "lf_hfe_joint", "lf_kfe_joint", None),
-            RobotGraph.Edge('lf_lowerleg', "lf_kfe_joint", "lf_foot_joint", None),
-            RobotGraph.Edge('rf_hipassembly', "rf_haa_joint", "rf_hfe_joint", None),
-            RobotGraph.Edge('rf_upperleg', "rf_hfe_joint", "rf_kfe_joint", None),
-            RobotGraph.Edge('rf_lowerleg', "rf_kfe_joint", "rf_foot_joint", None),
-            RobotGraph.Edge('lh_hipassembly', "lh_haa_joint", "lh_hfe_joint", None),
-            RobotGraph.Edge('lh_upperleg', "lh_hfe_joint", "lh_kfe_joint", None),
-            RobotGraph.Edge('lh_lowerleg', "lh_kfe_joint", "lh_foot_joint", None),
-            RobotGraph.Edge('rh_hipassembly', "rh_haa_joint", "rh_hfe_joint", None),
-            RobotGraph.Edge('rh_upperleg', "rh_hfe_joint", "rh_kfe_joint", None),
-            RobotGraph.Edge('rh_lowerleg', "rh_kfe_joint", "rh_foot_joint", None)
+            RobotGraph.Edge('trunk_to_FL_hip_joint', "floating_base",
+                            "FL_hip_joint", None),
+            RobotGraph.Edge('trunk_to_FR_hip_joint', "floating_base",
+                            "FR_hip_joint", None),
+            RobotGraph.Edge('trunk_to_RL_hip_joint', "floating_base",
+                            "RL_hip_joint", None),
+            RobotGraph.Edge('trunk_to_RR_hip_joint', "floating_base",
+                            "RR_hip_joint", None),
+            RobotGraph.Edge('FL_hip', "FL_hip_joint", "FL_thigh_joint", None),
+            RobotGraph.Edge('FL_thigh', "FL_thigh_joint", "FL_calf_joint", None),
+            RobotGraph.Edge('FL_calf', "FL_calf_joint", "FL_foot_fixed", None),
+            RobotGraph.Edge('FR_hip', "FR_hip_joint", "FR_thigh_joint", None),
+            RobotGraph.Edge('FR_thigh', "FR_thigh_joint", "FR_calf_joint", None),
+            RobotGraph.Edge('FR_calf', "FR_calf_joint", "FR_foot_fixed", None),
+            RobotGraph.Edge('RL_hip', "RL_hip_joint", "RL_thigh_joint", None),
+            RobotGraph.Edge('RL_thigh', "RL_thigh_joint", "RL_calf_joint", None),
+            RobotGraph.Edge('RL_calf', "RL_calf_joint", "RL_foot_fixed", None),
+            RobotGraph.Edge('RR_hip', "RR_hip_joint", "RR_thigh_joint", None),
+            RobotGraph.Edge('RR_thigh', "RR_thigh_joint", "RR_calf_joint", None),
+            RobotGraph.Edge('RR_calf', "RR_calf_joint", "RR_foot_fixed", None)
         ]
-        for i, edge in enumerate(self.HyQ_URDF.edges):
+        for i, edge in enumerate(self.mini_cheetah_URDF.edges):
             match_found = False
             for j, desired_edge in enumerate(desired_edges):
                 if edge.name == desired_edge.name:
@@ -89,7 +88,7 @@ def test_constructor(self):
         ]
         edge_names_copy = copy.deepcopy(joint_names)
         num_matches = 0
-        for i, node in enumerate(self.HyQ_URDF.nodes):
+        for i, node in enumerate(self.mini_cheetah_URDF.nodes):
             for j, node_des in enumerate(edge_names_copy):
                 if (node.name == node_des):
                     self.assertEqual(node.get_node_type(), des_node_type[j])
@@ -103,15 +102,15 @@ def test_constructor(self):
         # is stored for one of the Nodes.
         # ==================
         node_found = False
-        for i, node in enumerate(self.HyQ_URDF.nodes):
-            if node.name == "rh_kfe_joint":
+        for i, node in enumerate(self.mini_cheetah_URDF.nodes):
+            if node.name == "RL_hip_joint":
                 # Test name information
                 joint: urchin.Joint= node.joint
-                self.assertEqual("rh_kfe_joint", joint.name)
+                self.assertEqual("RL_hip_joint", joint.name)
 
                 # Test joint information
-                np.testing.assert_array_equal(np.array([[1.0, 0.0, 0.0, 0.35], [0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 1.0]]), joint.origin)
-                self.assertEqual(0.1, joint.dynamics.damping)
+                np.testing.assert_array_equal(np.array([[1.0, 0.0, 0.0, -0.196], [0.0, 1.0, 0.0, 0.049664], [0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 1.0]]), joint.origin)
+                self.assertEqual(0.0, joint.dynamics.damping)
                 self.assertEqual(0.0, joint.dynamics.friction)
 
                 node_found = True
@@ -120,16 +119,16 @@ def test_constructor(self):
         self.assertTrue(node_found)
         
         edge_found = False
-        for i, edge in enumerate(self.HyQ_URDF.edges):
-            if edge.name == "lh_hipassembly":
+        for i, edge in enumerate(self.mini_cheetah_URDF.edges):
+            if edge.name == "FL_calf":
                 # Test name information
                 link: urchin.Link = edge.link
-                self.assertEqual("lh_hipassembly", link.name)
+                self.assertEqual("FL_calf", link.name)
 
                 # Test inertial information
-                np.testing.assert_array_equal(np.array([[1.0, 0.0, 0.0, 0.04263], [0.0, 1.0, 0.0, -0.0], [0.0, 0.0, 1.0, -0.16931], [0.0, 0.0, 0.0, 1.0]]), link.inertial.origin)
-                self.assertEqual(2.93, link.inertial.mass)
-                np.testing.assert_array_equal(np.array([[0.05071, 4e-05, 0.00159], [4e-05, 0.05486, -5e-05], [0.00159, -5e-05, 0.00571]]), link.inertial.inertia)
+                np.testing.assert_array_equal(np.array([[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 1.0]]), link.inertial.origin)
+                self.assertEqual(0.064, link.inertial.mass)
+                np.testing.assert_array_equal(np.array([[0.000214698666667, 0.0, 0.0], [0.0, 0.000214698666667, 0.0], [0.0, 0.0, 2.73066666667e-06]]), link.inertial.inertia)
 
                 edge_found = True
                 break
@@ -141,21 +140,21 @@ def test_get_connections_to_link(self):
         Check that we can properly find the connections to the links in the library.
         """
 
-        edge_parent, edge_children = self.HyQ_URDF.get_connections_to_link(
+        edge_parent, edge_children = self.mini_cheetah_URDF.get_connections_to_link(
             urchin.Link("base_link", None, None, None))
         self.assertEqual(edge_parent, None)
         self.assertSequenceEqual(edge_children, ["floating_base"])
 
-        edge_parent, edge_children = self.HyQ_URDF.get_connections_to_link(
+        edge_parent, edge_children = self.mini_cheetah_URDF.get_connections_to_link(
             urchin.Link("trunk", None, None, None))
         self.assertEqual(edge_parent, "floating_base")
         self.assertSequenceEqual(
             edge_children,
-            ["lf_haa_joint", "rf_haa_joint", "lh_haa_joint", "rh_haa_joint"])
+            ["RL_hip_joint", "FL_hip_joint", "RR_hip_joint", "FR_hip_joint"])
 
-        edge_parent, edge_children = self.HyQ_URDF.get_connections_to_link(
-            urchin.Link("lf_foot", None, None, None))
-        self.assertEqual(edge_parent, "lf_foot_joint")
+        edge_parent, edge_children = self.mini_cheetah_URDF.get_connections_to_link(
+            urchin.Link("FL_foot", None, None, None))
+        self.assertEqual(edge_parent, "FL_foot_fixed")
         self.assertSequenceEqual(edge_children, [])
 
     def test_create_updated_urdf_file(self):
@@ -165,14 +164,14 @@ def test_create_updated_urdf_file(self):
         """
 
         # Delete the urdf file
-        hyq_path_updated = self.hyq_path.parent / "hyq_updated.urdf"
-        os.remove(str(hyq_path_updated))
-        self.assertFalse(os.path.exists(hyq_path_updated))
+        mini_cheetah_path_updated = self.mini_cheetah_path.parent / "miniCheetah_updated.urdf"
+        os.remove(str(mini_cheetah_path_updated))
+        self.assertFalse(os.path.exists(mini_cheetah_path_updated))
 
         # Rebuild it
-        RobotGraph(self.hyq_path, 'package://hyq_description/',
-                   'hyq-description')
-        self.assertTrue(os.path.exists(hyq_path_updated))
+        RobotGraph(self.mini_cheetah_path, 'package://yobotics_description/', 
+                'mini-cheetah-gazebo-urdf/yobo_model/yobotics_description')
+        self.assertTrue(os.path.exists(mini_cheetah_path_updated))
 
     def test_get_node_name_to_index_dict(self):
         """
@@ -180,11 +179,11 @@ def test_get_node_name_to_index_dict(self):
         are unique.
         """
 
-        key = list(self.HyQ_URDF.get_node_name_to_index_dict())
+        key = list(self.mini_cheetah_URDF.get_node_name_to_index_dict())
         get_nodes_index = []
 
         for key in key:
-            index = self.HyQ_URDF.get_node_name_to_index_dict()[key]
+            index = self.mini_cheetah_URDF.get_node_name_to_index_dict()[key]
             get_nodes_index.append(index)
 
         self.assertTrue(pd.Index(get_nodes_index).is_unique)
@@ -195,12 +194,12 @@ def test_get_node_index_to_name_dict(self):
         index_to_name dict and the name_to_index dict are consistent.
         """
 
-        index_to_name = list(self.HyQ_URDF.get_node_index_to_name_dict())
-        name_to_index = list(self.HyQ_URDF.get_node_name_to_index_dict())
+        index_to_name = list(self.mini_cheetah_URDF.get_node_index_to_name_dict())
+        name_to_index = list(self.mini_cheetah_URDF.get_node_name_to_index_dict())
         get_nodes_index = []
 
         for key in name_to_index:
-            index = self.HyQ_URDF.get_node_name_to_index_dict()[key]
+            index = self.mini_cheetah_URDF.get_node_name_to_index_dict()[key]
             get_nodes_index.append(index)
 
         self.assertEqual(index_to_name, get_nodes_index)
@@ -210,7 +209,7 @@ def test_get_edge_index_matrix(self):
         Check the dimensionality of the edge matrix.
         """
 
-        edge_matrix = self.HyQ_URDF.get_edge_index_matrix()
+        edge_matrix = self.mini_cheetah_URDF.get_edge_index_matrix()
 
         self.assertEqual(edge_matrix.shape[0], 2)
         self.assertEqual(edge_matrix.shape[1], 32)
@@ -220,7 +219,7 @@ def test_get_num_nodes(self):
         Check that the number of nodes are correct. 
         """
 
-        self.assertEqual(self.HyQ_URDF.get_num_nodes(), 17)
+        self.assertEqual(self.mini_cheetah_URDF.get_num_nodes(), 17)
 
     def test_get_edge_connections_to_name_dict(self):
         """
@@ -230,13 +229,13 @@ def test_get_edge_connections_to_name_dict(self):
         """
 
         connections_to_name = list(
-            self.HyQ_URDF.get_edge_connections_to_name_dict())
+            self.mini_cheetah_URDF.get_edge_connections_to_name_dict())
         name_to_connections = list(
-            self.HyQ_URDF.get_edge_name_to_connections_dict())
+            self.mini_cheetah_URDF.get_edge_name_to_connections_dict())
 
         result = []
         for key in name_to_connections:
-            connections = self.HyQ_URDF.get_edge_name_to_connections_dict(
+            connections = self.mini_cheetah_URDF.get_edge_name_to_connections_dict(
             )[key]
             for i in range(connections.shape[1]):
                 real_reshaped = np.squeeze(connections[:, i].reshape(1, -1))
@@ -252,12 +251,12 @@ def test_get_edge_name_to_connections_dict(self):
         """
 
         name_to_connections = list(
-            self.HyQ_URDF.get_edge_name_to_connections_dict())
+            self.mini_cheetah_URDF.get_edge_name_to_connections_dict())
         all_connections = []
 
         # Get all connections from dictionary
         for key in name_to_connections:
-            connections = self.HyQ_URDF.get_edge_name_to_connections_dict(
+            connections = self.mini_cheetah_URDF.get_edge_name_to_connections_dict(
             )[key]
             for i in range(connections.shape[1]):
                 real_reshaped = np.squeeze(connections[:, i].reshape(1, -1))
diff --git a/urdf_files/HyQ/hyq-description b/urdf_files/HyQ/hyq-description
deleted file mode 160000
index f95d651..0000000
--- a/urdf_files/HyQ/hyq-description
+++ /dev/null
@@ -1 +0,0 @@
-Subproject commit f95d651be787ea5369b7fa354f3e896eae741067
diff --git a/urdf_files/HyQ/hyq.pdf b/urdf_files/HyQ/hyq.pdf
deleted file mode 100644
index 07da419..0000000
Binary files a/urdf_files/HyQ/hyq.pdf and /dev/null differ
diff --git a/urdf_files/HyQ/hyq.urdf b/urdf_files/HyQ/hyq.urdf
deleted file mode 100644
index 95feec8..0000000
--- a/urdf_files/HyQ/hyq.urdf
+++ /dev/null
@@ -1,893 +0,0 @@
-<?xml version="1.0" encoding="utf-8"?>
-<!-- =================================================================================== -->
-<!-- |    This document was autogenerated by xacro from src/hyq-description/robots/hyq.urdf.xacro | -->
-<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
-<!-- =================================================================================== -->
-<robot name="hyq">
-  <gazebo>
-    <plugin filename="libgazebo_ros_control.so" name="gazebo_ros_controller_manager">
-      <robotNamespace>/hyq</robotNamespace>
-      <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
-      <legacyModeNS>true</legacyModeNS>
-      <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 -->
-  <!-- Floating-base Joint -->
-  <!-- <link name="world"/>
-  <joint name="floating_base_joint" type="floating">
-    <origin rpy="0 0 0" xyz="0 0 0"/>
-    <parent link="world"/>
-    <child link="base_link"/>
-  </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">
-    <visual>
-      <geometry>
-        <cylinder length="0.01" radius="0.01"/>
-      </geometry>
-    </visual>
-  </link>
-  <!-- Trunk link -->
-  <link name="trunk">
-    <inertial>
-      <origin xyz="0.01 0.0 0.00358"/>
-      <mass value="58.96"/>
-      <inertia ixx="1.5725937" ixy="0.028375" ixz="-0.203139" iyy="8.5015928" iyz="-0.004462" izz="9.1954911"/>
-    </inertial>
-    <visual>
-      <!-- value manually found with RVIZ to correctly align the trunk to the hip assemblies -->
-      <origin xyz="0 0 -0.0082"/>
-      <geometry>
-        <mesh filename="package://hyq_description/meshes/trunk/trunk.dae" scale="1 1 1"/>
-      </geometry>
-    </visual>
-    <collision>
-      <!-- value manually found with RVIZ to correctly align the trunk to the hip assemblies -->
-      <origin xyz="0 0 -0.0082"/>
-      <geometry>
-        <mesh filename="package://hyq_description/meshes/trunk/trunk.dae" scale="1 1 1"/>
-      </geometry>
-    </collision>
-  </link>
-  <gazebo reference="trunk">
-    <mu1>1.5</mu1>
-    <mu2>1.5</mu2>
-  </gazebo>
-  <!-- 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.341 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://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://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://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://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://hyq_description/meshes/leg/lowerleg.dae" scale="1 1 1"/>
-      </geometry>
-      <material name="white"/>
-    </visual>
-    <collision>
-      <origin rpy="0 -1.57079632679  0" xyz="0.1705 0 0"/>
-      <geometry>
-        <cylinder length="0.341" radius="0.02"/>
-      </geometry>
-    </collision>
-  </link>
-  <!-- Foot link -->
-  <link name="lf_foot">
-    <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>
-  <!-- Hip assembly -->
-  <transmission name="lf_haa_trans">
-    <type>transmission_interface/SimpleTransmission</type>
-    <joint name="lf_haa_joint">
-      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-    </joint>
-    <actuator name="lf_haa_motor">
-      <mechanicalReduction>1</mechanicalReduction>
-    </actuator>
-  </transmission>
-  <!-- Upper leg -->
-  <transmission name="lf_hfe_trans">
-    <type>transmission_interface/SimpleTransmission</type>
-    <joint name="lf_hfe_joint">
-      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-    </joint>
-    <actuator name="lf_hfe_motor">
-      <mechanicalReduction>1</mechanicalReduction>
-    </actuator>
-  </transmission>
-  <!-- Lower leg -->
-  <transmission name="lf_kfe_trans">
-    <type>transmission_interface/SimpleTransmission</type>
-    <joint name="lf_kfe_joint">
-      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-    </joint>
-    <actuator name="lf_kfe_motor">
-      <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>
-      <!-- the sensor readings in terms of force torques are expressed in the lowerleg frame
-                because gazebo lumps the fake footframe with the real parent link, the normals are in the world frame-->
-      <plugin filename="libgazebo_ros_bumper.so" name="lf_shin_bumper">
-        <bumperTopicName>/hyq/lf_shin_bumper</bumperTopicName>
-      </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>
-      <!-- the sensor readings in terms of force torques are expressed in the lowerleg frame
-                                because gazebo lumps the fake footframe with the real parent link, the normals are in the world frame-->
-      <plugin filename="libgazebo_ros_bumper.so" name="lf_foot_bumper">
-        <bumperTopicName>/hyq/lf_foot_bumper</bumperTopicName>
-        <robotNamespace>hyq</robotNamespace>
-      </plugin>
-    </sensor>
-    <material>Gazebo/Black</material>
-  </gazebo>
-  <!-- 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.341 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://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://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://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://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://hyq_description/meshes/leg/lowerleg.dae" scale="1 1 1"/>
-      </geometry>
-      <material name="white"/>
-    </visual>
-    <collision>
-      <origin rpy="0 -1.57079632679  0" xyz="0.1705 0 0"/>
-      <geometry>
-        <cylinder length="0.341" radius="0.02"/>
-      </geometry>
-    </collision>
-  </link>
-  <!-- Foot link -->
-  <link name="rf_foot">
-    <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>
-  <!-- Hip assembly -->
-  <transmission name="rf_haa_trans">
-    <type>transmission_interface/SimpleTransmission</type>
-    <joint name="rf_haa_joint">
-      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-    </joint>
-    <actuator name="rf_haa_motor">
-      <mechanicalReduction>1</mechanicalReduction>
-    </actuator>
-  </transmission>
-  <!-- Upper leg -->
-  <transmission name="rf_hfe_trans">
-    <type>transmission_interface/SimpleTransmission</type>
-    <joint name="rf_hfe_joint">
-      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-    </joint>
-    <actuator name="rf_hfe_motor">
-      <mechanicalReduction>1</mechanicalReduction>
-    </actuator>
-  </transmission>
-  <!-- Lower leg -->
-  <transmission name="rf_kfe_trans">
-    <type>transmission_interface/SimpleTransmission</type>
-    <joint name="rf_kfe_joint">
-      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-    </joint>
-    <actuator name="rf_kfe_motor">
-      <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>
-      <!-- the sensor readings in terms of force torques are expressed in the lowerleg frame
-                because gazebo lumps the fake footframe with the real parent link, the normals are in the world frame-->
-      <plugin filename="libgazebo_ros_bumper.so" name="rf_shin_bumper">
-        <bumperTopicName>/hyq/rf_shin_bumper</bumperTopicName>
-      </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>
-      <!-- the sensor readings in terms of force torques are expressed in the lowerleg frame
-                                because gazebo lumps the fake footframe with the real parent link, the normals are in the world frame-->
-      <plugin filename="libgazebo_ros_bumper.so" name="rf_foot_bumper">
-        <bumperTopicName>/hyq/rf_foot_bumper</bumperTopicName>
-        <robotNamespace>hyq</robotNamespace>
-      </plugin>
-    </sensor>
-    <material>Gazebo/Black</material>
-  </gazebo>
-  <!-- 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.341 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://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://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://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://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://hyq_description/meshes/leg/lowerleg.dae" scale="1 1 1"/>
-      </geometry>
-      <material name="white"/>
-    </visual>
-    <collision>
-      <origin rpy="0 -1.57079632679  0" xyz="0.1705 0 0"/>
-      <geometry>
-        <cylinder length="0.341" radius="0.02"/>
-      </geometry>
-    </collision>
-  </link>
-  <!-- Foot link -->
-  <link name="lh_foot">
-    <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>
-  <!-- Hip assembly -->
-  <transmission name="lh_haa_trans">
-    <type>transmission_interface/SimpleTransmission</type>
-    <joint name="lh_haa_joint">
-      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-    </joint>
-    <actuator name="lh_haa_motor">
-      <mechanicalReduction>1</mechanicalReduction>
-    </actuator>
-  </transmission>
-  <!-- Upper leg -->
-  <transmission name="lh_hfe_trans">
-    <type>transmission_interface/SimpleTransmission</type>
-    <joint name="lh_hfe_joint">
-      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-    </joint>
-    <actuator name="lh_hfe_motor">
-      <mechanicalReduction>1</mechanicalReduction>
-    </actuator>
-  </transmission>
-  <!-- Lower leg -->
-  <transmission name="lh_kfe_trans">
-    <type>transmission_interface/SimpleTransmission</type>
-    <joint name="lh_kfe_joint">
-      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-    </joint>
-    <actuator name="lh_kfe_motor">
-      <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>
-      <!-- the sensor readings in terms of force torques are expressed in the lowerleg frame
-                because gazebo lumps the fake footframe with the real parent link, the normals are in the world frame-->
-      <plugin filename="libgazebo_ros_bumper.so" name="lh_shin_bumper">
-        <bumperTopicName>/hyq/lh_shin_bumper</bumperTopicName>
-      </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>
-      <!-- the sensor readings in terms of force torques are expressed in the lowerleg frame
-                                because gazebo lumps the fake footframe with the real parent link, the normals are in the world frame-->
-      <plugin filename="libgazebo_ros_bumper.so" name="lh_foot_bumper">
-        <bumperTopicName>/hyq/lh_foot_bumper</bumperTopicName>
-        <robotNamespace>hyq</robotNamespace>
-      </plugin>
-    </sensor>
-    <material>Gazebo/Black</material>
-  </gazebo>
-  <!-- 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.341 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://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://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://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://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://hyq_description/meshes/leg/lowerleg.dae" scale="1 1 1"/>
-      </geometry>
-      <material name="white"/>
-    </visual>
-    <collision>
-      <origin rpy="0 -1.57079632679  0" xyz="0.1705 0 0"/>
-      <geometry>
-        <cylinder length="0.341" radius="0.02"/>
-      </geometry>
-    </collision>
-  </link>
-  <!-- Foot link -->
-  <link name="rh_foot">
-    <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>
-  <!-- Hip assembly -->
-  <transmission name="rh_haa_trans">
-    <type>transmission_interface/SimpleTransmission</type>
-    <joint name="rh_haa_joint">
-      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-    </joint>
-    <actuator name="rh_haa_motor">
-      <mechanicalReduction>1</mechanicalReduction>
-    </actuator>
-  </transmission>
-  <!-- Upper leg -->
-  <transmission name="rh_hfe_trans">
-    <type>transmission_interface/SimpleTransmission</type>
-    <joint name="rh_hfe_joint">
-      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-    </joint>
-    <actuator name="rh_hfe_motor">
-      <mechanicalReduction>1</mechanicalReduction>
-    </actuator>
-  </transmission>
-  <!-- Lower leg -->
-  <transmission name="rh_kfe_trans">
-    <type>transmission_interface/SimpleTransmission</type>
-    <joint name="rh_kfe_joint">
-      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
-    </joint>
-    <actuator name="rh_kfe_motor">
-      <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>
-      <!-- the sensor readings in terms of force torques are expressed in the lowerleg frame
-                because gazebo lumps the fake footframe with the real parent link, the normals are in the world frame-->
-      <plugin filename="libgazebo_ros_bumper.so" name="rh_shin_bumper">
-        <bumperTopicName>/hyq/rh_shin_bumper</bumperTopicName>
-      </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>
-      <!-- the sensor readings in terms of force torques are expressed in the lowerleg frame
-                                because gazebo lumps the fake footframe with the real parent link, the normals are in the world frame-->
-      <plugin filename="libgazebo_ros_bumper.so" name="rh_foot_bumper">
-        <bumperTopicName>/hyq/rh_foot_bumper</bumperTopicName>
-        <robotNamespace>hyq</robotNamespace>
-      </plugin>
-    </sensor>
-    <material>Gazebo/Black</material>
-  </gazebo>
-</robot>