Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Fix tests for pybullet #151

Merged
merged 2 commits into from
Jan 6, 2023
Merged

Fix tests for pybullet #151

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
610e48d
anymal: set 0 mass to camera links
nim65s Jan 6, 2023
255c3a5
allegro has no mass informations
nim65s Jan 6, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
144 changes: 126 additions & 18 deletions robots/anymal_c_simple_description/urdf/anymal.urdf
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -211,31 +211,55 @@
<origin rpy="0.0 0.0 0.0" xyz="0.0255 0.0175 0.0"/>
</joint>
<!-- Camera parent link -->
<link name="depth_camera_front_camera_parent"/>
<link name="depth_camera_front_camera_parent">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="0"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
</link>
<!-- Depth optical frame joint -->
<joint name="depth_camera_front_camera_parent_to_depth_optical_frame" type="fixed">
<parent link="depth_camera_front_camera_parent"/>
<child link="depth_camera_front_depth_optical_frame"/>
<origin rpy="-1.57079632679 0.0 -1.57079632679" xyz="0.0 0.0 0.0"/>
</joint>
<!-- Depth optical frame link -->
<link name="depth_camera_front_depth_optical_frame"/>
<link name="depth_camera_front_depth_optical_frame">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="0"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
</link>
<!-- Camera color frame joint -->
<joint name="depth_camera_front_camera_parent_to_color_frame" type="fixed">
<parent link="depth_camera_front_camera_parent"/>
<child link="depth_camera_front_color_frame"/>
<origin rpy="0 0 0" xyz="0 0.015 0"/>
</joint>
<!-- Camera color frame link -->
<link name="depth_camera_front_color_frame"/>
<link name="depth_camera_front_color_frame">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="0"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
</link>
<!-- Camera color optical joint -->
<joint name="depth_camera_front_color_frame_to_color_optical_frame" type="fixed">
<parent link="depth_camera_front_color_frame"/>
<child link="depth_camera_front_color_optical_frame"/>
<origin rpy="-1.57079632679 0.0 -1.57079632679" xyz="0.0 0.0 0.0"/>
</joint>
<!-- Camera color optical link -->
<link name="depth_camera_front_color_optical_frame"/>
<link name="depth_camera_front_color_optical_frame">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="0"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
</link>
<!-- Camera joint -->
<!-- Is located in the center of the mounting points. -->
<joint name="face_front_to_wide_angle_camera_front_camera" type="fixed">
Expand Down Expand Up @@ -264,7 +288,13 @@
<origin rpy="-1.57079632679 0.0 -1.57079632679" xyz="0.0 0.0 0.0"/>
</joint>
<!-- Camera parent link -->
<link name="wide_angle_camera_front_camera_parent"/>
<link name="wide_angle_camera_front_camera_parent">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="0"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
</link>
<!-- Fixed joint base face -->
<joint name="base_face_rear" type="fixed">
<parent link="base"/>
Expand Down Expand Up @@ -332,31 +362,55 @@
<origin rpy="0.0 0.0 0.0" xyz="0.0255 0.0175 0.0"/>
</joint>
<!-- Camera parent link -->
<link name="depth_camera_rear_camera_parent"/>
<link name="depth_camera_rear_camera_parent">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="0"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
</link>
<!-- Depth optical frame joint -->
<joint name="depth_camera_rear_camera_parent_to_depth_optical_frame" type="fixed">
<parent link="depth_camera_rear_camera_parent"/>
<child link="depth_camera_rear_depth_optical_frame"/>
<origin rpy="-1.57079632679 0.0 -1.57079632679" xyz="0.0 0.0 0.0"/>
</joint>
<!-- Depth optical frame link -->
<link name="depth_camera_rear_depth_optical_frame"/>
<link name="depth_camera_rear_depth_optical_frame">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="0"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
</link>
<!-- Camera color frame joint -->
<joint name="depth_camera_rear_camera_parent_to_color_frame" type="fixed">
<parent link="depth_camera_rear_camera_parent"/>
<child link="depth_camera_rear_color_frame"/>
<origin rpy="0 0 0" xyz="0 0.015 0"/>
</joint>
<!-- Camera color frame link -->
<link name="depth_camera_rear_color_frame"/>
<link name="depth_camera_rear_color_frame">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="0"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
</link>
<!-- Camera color optical joint -->
<joint name="depth_camera_rear_color_frame_to_color_optical_frame" type="fixed">
<parent link="depth_camera_rear_color_frame"/>
<child link="depth_camera_rear_color_optical_frame"/>
<origin rpy="-1.57079632679 0.0 -1.57079632679" xyz="0.0 0.0 0.0"/>
</joint>
<!-- Camera color optical link -->
<link name="depth_camera_rear_color_optical_frame"/>
<link name="depth_camera_rear_color_optical_frame">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="0"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
</link>
<!-- Camera joint -->
<!-- Is located in the center of the mounting points. -->
<joint name="face_rear_to_wide_angle_camera_rear_camera" type="fixed">
Expand Down Expand Up @@ -385,7 +439,13 @@
<origin rpy="-1.57079632679 0.0 -1.57079632679" xyz="0.0 0.0 0.0"/>
</joint>
<!-- Camera parent link -->
<link name="wide_angle_camera_rear_camera_parent"/>
<link name="wide_angle_camera_rear_camera_parent">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="0"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
</link>
<!-- Fixed joint base battery -->
<joint name="base_battery" type="fixed">
<parent link="base"/>
Expand Down Expand Up @@ -475,31 +535,55 @@
<origin rpy="0.0 0.0 0.0" xyz="0.0255 0.0175 0.0"/>
</joint>
<!-- Camera parent link -->
<link name="depth_camera_left_camera_parent"/>
<link name="depth_camera_left_camera_parent">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="0"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
</link>
<!-- Depth optical frame joint -->
<joint name="depth_camera_left_camera_parent_to_depth_optical_frame" type="fixed">
<parent link="depth_camera_left_camera_parent"/>
<child link="depth_camera_left_depth_optical_frame"/>
<origin rpy="-1.57079632679 0.0 -1.57079632679" xyz="0.0 0.0 0.0"/>
</joint>
<!-- Depth optical frame link -->
<link name="depth_camera_left_depth_optical_frame"/>
<link name="depth_camera_left_depth_optical_frame">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="0"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
</link>
<!-- Camera color frame joint -->
<joint name="depth_camera_left_camera_parent_to_color_frame" type="fixed">
<parent link="depth_camera_left_camera_parent"/>
<child link="depth_camera_left_color_frame"/>
<origin rpy="0 0 0" xyz="0 0.015 0"/>
</joint>
<!-- Camera color frame link -->
<link name="depth_camera_left_color_frame"/>
<link name="depth_camera_left_color_frame">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="0"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
</link>
<!-- Camera color optical joint -->
<joint name="depth_camera_left_color_frame_to_color_optical_frame" type="fixed">
<parent link="depth_camera_left_color_frame"/>
<child link="depth_camera_left_color_optical_frame"/>
<origin rpy="-1.57079632679 0.0 -1.57079632679" xyz="0.0 0.0 0.0"/>
</joint>
<!-- Camera color optical link -->
<link name="depth_camera_left_color_optical_frame"/>
<link name="depth_camera_left_color_optical_frame">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="0"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
</link>
<!-- Camera joint -->
<!-- Is located between the two back screw holes at ground level. -->
<joint name="base_to_depth_camera_right_camera" type="fixed">
Expand Down Expand Up @@ -529,31 +613,55 @@
<origin rpy="0.0 0.0 0.0" xyz="0.0255 0.0175 0.0"/>
</joint>
<!-- Camera parent link -->
<link name="depth_camera_right_camera_parent"/>
<link name="depth_camera_right_camera_parent">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="0"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
</link>
<!-- Depth optical frame joint -->
<joint name="depth_camera_right_camera_parent_to_depth_optical_frame" type="fixed">
<parent link="depth_camera_right_camera_parent"/>
<child link="depth_camera_right_depth_optical_frame"/>
<origin rpy="-1.57079632679 0.0 -1.57079632679" xyz="0.0 0.0 0.0"/>
</joint>
<!-- Depth optical frame link -->
<link name="depth_camera_right_depth_optical_frame"/>
<link name="depth_camera_right_depth_optical_frame">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="0"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
</link>
<!-- Camera color frame joint -->
<joint name="depth_camera_right_camera_parent_to_color_frame" type="fixed">
<parent link="depth_camera_right_camera_parent"/>
<child link="depth_camera_right_color_frame"/>
<origin rpy="0 0 0" xyz="0 0.015 0"/>
</joint>
<!-- Camera color frame link -->
<link name="depth_camera_right_color_frame"/>
<link name="depth_camera_right_color_frame">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="0"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
</link>
<!-- Camera color optical joint -->
<joint name="depth_camera_right_color_frame_to_color_optical_frame" type="fixed">
<parent link="depth_camera_right_color_frame"/>
<child link="depth_camera_right_color_optical_frame"/>
<origin rpy="-1.57079632679 0.0 -1.57079632679" xyz="0.0 0.0 0.0"/>
</joint>
<!-- Camera color optical link -->
<link name="depth_camera_right_color_optical_frame"/>
<link name="depth_camera_right_color_optical_frame">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="0"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
</link>
<!-- parent to cage joint, located between mounting plate on trunk and the cage -->
<joint name="base_to_lidar_cage" type="fixed">
<parent link="base"/>
Expand Down
8 changes: 4 additions & 4 deletions unittest/test_load.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,13 +11,13 @@


class RobotTestCase(unittest.TestCase):
def check(self, name, expected_nq, expected_nv, one_kg_bodies=[]):
def check(self, name, expected_nq, expected_nv, one_kg_bodies=[], mass=True):
"""Helper function for the real tests"""
robot, _, urdf, _ = load_full(name, display=False)
self.assertEqual(robot.model.nq, expected_nq)
self.assertEqual(robot.model.nv, expected_nv)
self.assertTrue(hasattr(robot, "q0"))
if pybullet:
if pybullet and mass:
self.check_pybullet(urdf, one_kg_bodies)

def check_pybullet(self, urdf, one_kg_bodies):
Expand Down Expand Up @@ -90,10 +90,10 @@ def test_panda(self):
self.check("panda", 9, 9)

def test_allegro_right(self):
self.check("allegro_right_hand", 16, 16)
self.check("allegro_right_hand", 16, 16, mass=False)

def test_allegro_left(self):
self.check("allegro_left_hand", 16, 16)
self.check("allegro_left_hand", 16, 16, mass=False)

def test_romeo(self):
self.check("romeo", 62, 61)
Expand Down