[FEATURE] Add parsing of joint friction and damping for URDF files. (Genesis-Embodied-AI#1833)

Author: duburcqa

genesis/utils/urdf.py

@@ -218,7 +218,6 @@ def parse_urdf(morph, surface):
             j_info["n_qs"] = 1
             j_info["n_dofs"] = 1
             j_info["init_qpos"] = np.zeros(1)
-
         elif joint.joint_type == "continuous":
             j_info["dofs_motion_ang"] = np.array([joint.axis])
             j_info["dofs_motion_vel"] = np.zeros((1, 3))
@@ -229,7 +228,6 @@ def parse_urdf(morph, surface):
             j_info["n_qs"] = 1
             j_info["n_dofs"] = 1
             j_info["init_qpos"] = np.zeros(1)
-
         elif joint.joint_type == "prismatic":
             j_info["dofs_motion_ang"] = np.zeros((1, 3))
             j_info["dofs_motion_vel"] = np.array([joint.axis])
@@ -247,7 +245,6 @@ def parse_urdf(morph, surface):
             j_info["n_qs"] = 1
             j_info["n_dofs"] = 1
             j_info["init_qpos"] = np.zeros(1)
-
         elif joint.joint_type == "floating":
             j_info["dofs_motion_ang"] = np.eye(6, 3, -3)
             j_info["dofs_motion_vel"] = np.eye(6, 3)
@@ -258,28 +255,30 @@ def parse_urdf(morph, surface):
             j_info["n_qs"] = 7
             j_info["n_dofs"] = 6
             j_info["init_qpos"] = np.concatenate([gu.zero_pos(), gu.identity_quat()])
-
         else:
             gs.raise_exception(f"Unsupported URDF joint type: {joint.joint_type}")
 
-        j_info["dofs_invweight"] = np.full((j_info["n_dofs"],), fill_value=-1.0)
-        j_info["dofs_frictionloss"] = np.zeros(j_info["n_dofs"])
         j_info["sol_params"] = gu.default_solver_params()
-        j_info["dofs_kp"] = gu.default_dofs_kp(j_info["n_dofs"])
-        j_info["dofs_kv"] = gu.default_dofs_kv(j_info["n_dofs"])
-        j_info["dofs_force_range"] = np.tile([-np.inf, np.inf], (j_info["n_dofs"], 1))
+        j_info["dofs_invweight"] = np.full((j_info["n_dofs"],), fill_value=-1.0)
 
-        j_info["dofs_damping"] = np.zeros(j_info["n_dofs"])
+        joint_friction, joint_damping = 0.0, 0.0
+        if joint.dynamics is not None:
+            joint_friction, joint_damping = joint.dynamics.friction, joint.dynamics.damping
+        j_info["dofs_frictionloss"] = np.full(j_info["n_dofs"], joint_friction)
+        j_info["dofs_damping"] = np.full(j_info["n_dofs"], joint_damping)
         j_info["dofs_armature"] = np.zeros(j_info["n_dofs"])
         if joint.joint_type not in ("floating", "fixed") and morph.default_armature is not None:
             j_info["dofs_armature"] = np.full((j_info["n_dofs"],), morph.default_armature)
 
+        j_info["dofs_kp"] = gu.default_dofs_kp(j_info["n_dofs"])
+        j_info["dofs_kv"] = gu.default_dofs_kv(j_info["n_dofs"])
         if joint.safety_controller is not None:
             if joint.safety_controller.k_position is not None:
                 j_info["dofs_kp"] = np.tile(joint.safety_controller.k_position, j_info["n_dofs"])
             if joint.safety_controller.k_velocity is not None:
                 j_info["dofs_kv"] = np.tile(joint.safety_controller.k_velocity, j_info["n_dofs"])
 
+        j_info["dofs_force_range"] = np.tile([-np.inf, np.inf], (j_info["n_dofs"], 1))
         if joint.limit is not None and joint.limit.effort is not None:
             j_info["dofs_force_range"] = np.tile([-joint.limit.effort, joint.limit.effort], (j_info["n_dofs"], 1))
 

tests/test_rigid_physics.py

@@ -222,7 +222,7 @@ def chain_capsule_hinge_capsule(asset_tmp_path):
     return _build_chain_capsule_hinge(asset_tmp_path, enable_mesh=False)
 
 
-def _build_multi_pendulum(n):
+def _build_multi_pendulum(n, joint_damping, joint_friction):
     """Generate an URDF model of a multi-link pendulum with n segments."""
     urdf = ET.Element("robot", name="multi_pendulum")
 
@@ -238,6 +238,7 @@ def _build_multi_pendulum(n):
         ET.SubElement(joint, "parent", link=parent_link)
         ET.SubElement(joint, "child", link=f"PendulumArm_{i}")
         ET.SubElement(joint, "limit", effort=str(100.0 * (n - i)), velocity="30.0")
+        ET.SubElement(joint, "dynamics", damping=str(joint_damping), friction=str(joint_friction))
 
         # Arm link
         arm = ET.SubElement(urdf, "link", name=f"PendulumArm_{i}")
@@ -276,14 +277,19 @@ def _build_multi_pendulum(n):
     return urdf
 
 
+@pytest.fixture
+def pendulum_with_joint_dynamics(joint_damping, joint_friction):
+    return _build_multi_pendulum(n=1, joint_damping=joint_damping, joint_friction=joint_friction)
+
+
 @pytest.fixture(scope="session")
-def pendulum(asset_tmp_path):
-    return _build_multi_pendulum(n=1)
+def pendulum():
+    return _build_multi_pendulum(n=1, joint_damping=0.0, joint_friction=0.0)
 
 
 @pytest.fixture(scope="session")
-def double_pendulum(asset_tmp_path):
-    return _build_multi_pendulum(n=2)
+def double_pendulum():
+    return _build_multi_pendulum(n=2, joint_damping=0.0, joint_friction=0.0)
 
 
 @pytest.fixture(scope="session")
@@ -2302,6 +2308,25 @@ def test_urdf_mimic(show_viewer, tol):
     assert_allclose(gs_qpos[-1], gs_qpos[-2], tol=tol)
 
 
+@pytest.mark.required
+@pytest.mark.parametrize("model_name", ["pendulum_with_joint_dynamics"])
+@pytest.mark.parametrize("joint_damping, joint_friction", [(1.0, 2.0)])
+@pytest.mark.parametrize("backend", [gs.cpu])
+def test_urdf_joint_dynamics(joint_damping, joint_friction, xml_path):
+    scene = gs.Scene()
+    robot = scene.add_entity(
+        gs.morphs.URDF(
+            file=xml_path,
+            pos=(0, 0, 0.8),
+            convexify=True,
+        ),
+    )
+    assert_allclose(robot.joints[0].dofs_damping, 0.0, tol=gs.EPS)
+    assert_allclose(robot.joints[1].dofs_damping, joint_damping, tol=gs.EPS)
+    assert_allclose(robot.joints[0].dofs_frictionloss, 0.0, tol=gs.EPS)
+    assert_allclose(robot.joints[1].dofs_frictionloss, joint_friction, tol=gs.EPS)
+
+
 @pytest.mark.required
 @pytest.mark.parametrize("backend", [gs.cpu])
 def test_gravity(show_viewer, tol):