Remove reference to HyQ submodule which has been deleted off github, update test cases that used it

Author: DanielChaseButterfield

.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

tests/testGraphParser.py

@@ -13,28 +13,27 @@
 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):
         """
         Check that the constructor properly assigns all of the links and joints to a node/edge.
         """
 
         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,26 +164,26 @@ 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):
         """
         Check if all the indexes of the nodes in the dictionary
         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))