Skip to content

Commit

Permalink
Remove reference to HyQ submodule which has been deleted off github, …
Browse files Browse the repository at this point in the history 
…update test cases that used it
  • Loading branch information
@DanielChaseButterfield
DanielChaseButterfield committed Dec 15, 2024
1 parent 45df95b commit 131b911
Show file tree
Hide file tree
Showing 5 changed files with 67 additions and 965 deletions.
3 changes: 0 additions & 3 deletions .gitmodules
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
135 changes: 67 additions & 68 deletions tests/testGraphParser.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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))
Expand All @@ -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:
Expand All @@ -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])
Expand All @@ -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
Expand All @@ -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
Expand All @@ -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):
Expand All @@ -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)
Expand All @@ -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)
Expand All @@ -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)
Expand All @@ -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):
"""
Expand All @@ -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))
Expand All @@ -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))
Expand Down
1 change: 0 additions & 1 deletion urdf_files/HyQ/hyq-description
Submodule hyq-description deleted from f95d65
Binary file removed urdf_files/HyQ/hyq.pdf
View file
Binary file not shown.
Loading

0 comments on commit 131b911

Please sign in to comment.