From da0fae0270b89ee82791ca5076e8f81f36e85329 Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Thu, 17 Sep 2026 08:42:25 -0700 Subject: [PATCH 1/2] Restore primitive gripper contacts for Franka Lift and Reorient --- .../fix-franka-lift-primitive-contacts.rst | 9 +++ .../core/lift/config/franka/franka_env_cfg.py | 9 ++- .../isaaclab_tasks/core/lift/mdp/utils.py | 6 +- .../test/core/test_lift_env_cfg.py | 66 +++++++++++++++---- 4 files changed, 73 insertions(+), 17 deletions(-) create mode 100644 source/isaaclab_tasks/changelog.d/fix-franka-lift-primitive-contacts.rst diff --git a/source/isaaclab_tasks/changelog.d/fix-franka-lift-primitive-contacts.rst b/source/isaaclab_tasks/changelog.d/fix-franka-lift-primitive-contacts.rst new file mode 100644 index 000000000000..87e06a4ab933 --- /dev/null +++ b/source/isaaclab_tasks/changelog.d/fix-franka-lift-primitive-contacts.rst @@ -0,0 +1,9 @@ +Fixed +^^^^^ + +* Restored hand-capsule and finger-pad contacts for rigid Franka Lift and Reorient by + disabling the primitive asset's arm capsules through a task-local spawn override. + This replaced the convex-hull selection, which also enabled arm and hand mesh contacts. +* Made Lift/Reorient reset clearance ignore colliders with disabled collision, preventing + inactive arm capsules from blocking reset sampling. Reset acceptance now follows enabled + collision geometry; disabled arm meshes are no longer used as clearance proxies. diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka/franka_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka/franka_env_cfg.py index b244c84dcab0..e287cf605d49 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka/franka_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka/franka_env_cfg.py @@ -10,7 +10,7 @@ from isaaclab.managers import RewardTermCfg as RewTerm from isaaclab.managers import SceneEntityCfg from isaaclab.sensors import ContactSensorCfg -from isaaclab.sim import MeshCapsuleCfg, MeshCuboidCfg, MeshSphereCfg +from isaaclab.sim import MeshCapsuleCfg, MeshCuboidCfg, MeshSphereCfg, UsdPhysicsCollisionCfg from isaaclab.utils import configclass from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR @@ -24,8 +24,11 @@ # legacy asset so the upstream franka tasks keep their demos and baselines. FRANKA_PANDA_LIFT_CFG = FRANKA_PANDA_CFG.copy() FRANKA_PANDA_LIFT_CFG.spawn.usd_path = f"{ISAACLAB_NUCLEUS_DIR}/Robots/FrankaEmika/franka_panda.usda" -# Reset clearance was calibrated for these arm meshes; the asset's primitive colliders intersect the ground. -FRANKA_PANDA_LIFT_CFG.spawn.variants = {"Colliders": "convex_hulls"} +# Preserve hand/finger-only contacts after the asset added arm capsules to its primitive variant. +FRANKA_PANDA_LIFT_CFG.spawn.variants = {"Colliders": "primitives"} +FRANKA_PANDA_LIFT_CFG.spawn.collision_props = { + "/Geometry/.*/link[0-7]_capsule(_[0-9]+)?": [UsdPhysicsCollisionCfg(collision_enabled=False)], +} FRANKA_PANDA_LIFT_CFG.actuators = { # Inspired by libfranka's joint_impedance_control.cpp. ``actuator_velocity_limit`` # remains the soft task-limit snapshot; ``joint_velocity_limit`` is the diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/lift/mdp/utils.py b/source/isaaclab_tasks/isaaclab_tasks/core/lift/mdp/utils.py index 907f347f2689..e7111f539d07 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/lift/mdp/utils.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/lift/mdp/utils.py @@ -277,7 +277,7 @@ def farthest_point_sampling( def collect_collision_meshes(root_prim, owner_frame_fn: Callable) -> dict[int, trimesh.Trimesh]: - """Collect collision meshes under ``root_prim``, grouped in caller-selected frames.""" + """Collect enabled collision meshes under ``root_prim``, grouped in caller-selected frames.""" import trimesh from pxr import UsdPhysics @@ -287,7 +287,9 @@ def collect_collision_meshes(root_prim, owner_frame_fn: Callable) -> dict[int, t mesh_types = PRIMITIVE_MESH_TYPES + ["Mesh"] mesh_prims = sim_utils.get_all_matching_child_prims( root_prim.GetPath(), - lambda prim: prim.GetTypeName() in mesh_types and prim.HasAPI(UsdPhysics.CollisionAPI), + lambda prim: prim.GetTypeName() in mesh_types + and prim.HasAPI(UsdPhysics.CollisionAPI) + and UsdPhysics.CollisionAPI(prim).GetCollisionEnabledAttr().Get(), ) meshes_by_owner: dict[int, list[trimesh.Trimesh]] = {} diff --git a/source/isaaclab_tasks/test/core/test_lift_env_cfg.py b/source/isaaclab_tasks/test/core/test_lift_env_cfg.py index eb6de255122b..019570fa0458 100644 --- a/source/isaaclab_tasks/test/core/test_lift_env_cfg.py +++ b/source/isaaclab_tasks/test/core/test_lift_env_cfg.py @@ -10,10 +10,10 @@ import pytest import torch -from pxr import Usd +from pxr import Usd, UsdGeom, UsdPhysics from isaaclab.managers import CommandTerm -from isaaclab.sim import select_usd_variants +from isaaclab.sim import apply_collision_properties, select_usd_variants, use_stage from isaaclab_tasks.core.lift import mdp from isaaclab_tasks.core.lift.config.franka.franka_env_cfg import FrankaLiftEnvCfg, FrankaReorientEnvCfg @@ -23,6 +23,7 @@ DeformableUniformPoseCommand, ObjectUniformPoseCommand, ) +from isaaclab_tasks.core.lift.mdp.utils import collect_collision_meshes from isaaclab_tasks.utils.hydra import resolve_presets @@ -63,26 +64,67 @@ def test_franka_soft_robot_physics_variant_matches_backend( @pytest.mark.parametrize("cfg_type", [FrankaLiftEnvCfg, FrankaReorientEnvCfg]) -def test_franka_rigid_tasks_select_collision_meshes_for_reset_clearance(cfg_type) -> None: - """Reset validation keeps the original arm meshes when the asset defaults to capsules.""" +def test_franka_rigid_tasks_keep_only_hand_and_finger_contacts(cfg_type) -> None: + """Arm overrides must preserve hand/finger contacts nested below the arm links.""" cfg = cfg_type() stage = Usd.Stage.CreateInMemory() robot = stage.DefinePrim("/Robot", "Xform") colliders = robot.GetVariantSets().AddVariantSet("Colliders") - for selection, prim_path, prim_type in ( - ("convex_hulls", "/Robot/link1_c/link1_c", "Mesh"), - ("primitives", "/Robot/link1_capsule", "Capsule"), - ): + for selection in ("convex_hulls", "primitives"): colliders.AddVariant(selection) colliders.SetVariantSelection(selection) with colliders.GetVariantEditContext(): - stage.DefinePrim(prim_path, prim_type) - colliders.SetVariantSelection("primitives") + link_path = "/Robot/Geometry" + for index in range(8): + link_path += f"/panda_link{index}" + names = [f"link{index}_capsule"] + if index in (5, 7): + names = [f"link{index}_capsule_0", f"link{index}_capsule_1"] + if selection == "convex_hulls": + names = [f"link{index}_c"] + for name in names: + prim = stage.DefinePrim(f"{link_path}/{name}", "Capsule" if selection == "primitives" else "Mesh") + UsdPhysics.CollisionAPI.Apply(prim) + hand_path = f"{link_path}/panda_hand" + for suffix, shape in ( + ("hand_capsule", "Capsule"), + ("panda_leftfinger/left_finger_pad", "Cube"), + ("panda_rightfinger/right_finger_pad", "Cube"), + ): + UsdPhysics.CollisionAPI.Apply(stage.DefinePrim(f"{hand_path}/{suffix}", shape)) + colliders.SetVariantSelection("convex_hulls") select_usd_variants("/Robot", cfg.scene.robot.spawn.variants or {}, stage=stage) + for pattern, fragments in (cfg.scene.robot.spawn.collision_props or {}).items(): + assert apply_collision_properties(f"/Robot{pattern}", fragments, stage=stage) + + collision_prims = [prim for prim in stage.Traverse() if prim.HasAPI(UsdPhysics.CollisionAPI)] + enabled = [prim for prim in collision_prims if UsdPhysics.CollisionAPI(prim).GetCollisionEnabledAttr().Get()] + disabled = [prim for prim in collision_prims if not UsdPhysics.CollisionAPI(prim).GetCollisionEnabledAttr().Get()] + assert {(prim.GetName(), prim.GetTypeName()) for prim in enabled} == { + ("hand_capsule", "Capsule"), + ("left_finger_pad", "Cube"), + ("right_finger_pad", "Cube"), + } + assert len(disabled) == 10 + assert all(prim.GetTypeName() == "Capsule" for prim in disabled) + - assert stage.GetPrimAtPath("/Robot/link1_c/link1_c").IsValid() - assert not stage.GetPrimAtPath("/Robot/link1_capsule").IsValid() +def test_reset_clearance_ignores_disabled_collision_geometry() -> None: + """Disabled colliders and visual-only geometry must not reject reset candidates.""" + stage = Usd.Stage.CreateInMemory() + root = stage.DefinePrim("/Object", "Xform") + for name, enabled in (("default_enabled", None), ("explicit_enabled", True), ("disabled", False)): + prim = UsdGeom.Cube.Define(stage, f"/Object/{name}").GetPrim() + collision = UsdPhysics.CollisionAPI.Apply(prim) + if enabled is not None: + collision.CreateCollisionEnabledAttr(enabled) + UsdGeom.Cube.Define(stage, "/Object/visual_only") + + with use_stage(stage): + meshes = collect_collision_meshes(root, lambda prim: (prim.GetName(), root)) + + assert set(meshes) == {"default_enabled", "explicit_enabled"} def test_camera_normalization_is_stationary() -> None: From f3bbb8f03e139f24caaa9b2f1ad019b4b0d317b2 Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Thu, 17 Sep 2026 08:55:30 -0700 Subject: [PATCH 2/2] Keep Franka task regression coverage minimal --- .../test/core/test_lift_env_cfg.py | 50 +------------------ 1 file changed, 1 insertion(+), 49 deletions(-) diff --git a/source/isaaclab_tasks/test/core/test_lift_env_cfg.py b/source/isaaclab_tasks/test/core/test_lift_env_cfg.py index 019570fa0458..aa3a62921985 100644 --- a/source/isaaclab_tasks/test/core/test_lift_env_cfg.py +++ b/source/isaaclab_tasks/test/core/test_lift_env_cfg.py @@ -13,10 +13,9 @@ from pxr import Usd, UsdGeom, UsdPhysics from isaaclab.managers import CommandTerm -from isaaclab.sim import apply_collision_properties, select_usd_variants, use_stage +from isaaclab.sim import use_stage from isaaclab_tasks.core.lift import mdp -from isaaclab_tasks.core.lift.config.franka.franka_env_cfg import FrankaLiftEnvCfg, FrankaReorientEnvCfg from isaaclab_tasks.core.lift.config.franka_soft.franka_soft_env_cfg import FrankaSoftEnvCfg from isaaclab_tasks.core.lift.mdp.commands.pose_commands import ( CableUniformPoseCommand, @@ -63,53 +62,6 @@ def test_franka_soft_robot_physics_variant_matches_backend( assert cfg.scene.robot.spawn.variants == {"Physics": expected_physics} -@pytest.mark.parametrize("cfg_type", [FrankaLiftEnvCfg, FrankaReorientEnvCfg]) -def test_franka_rigid_tasks_keep_only_hand_and_finger_contacts(cfg_type) -> None: - """Arm overrides must preserve hand/finger contacts nested below the arm links.""" - cfg = cfg_type() - stage = Usd.Stage.CreateInMemory() - robot = stage.DefinePrim("/Robot", "Xform") - colliders = robot.GetVariantSets().AddVariantSet("Colliders") - for selection in ("convex_hulls", "primitives"): - colliders.AddVariant(selection) - colliders.SetVariantSelection(selection) - with colliders.GetVariantEditContext(): - link_path = "/Robot/Geometry" - for index in range(8): - link_path += f"/panda_link{index}" - names = [f"link{index}_capsule"] - if index in (5, 7): - names = [f"link{index}_capsule_0", f"link{index}_capsule_1"] - if selection == "convex_hulls": - names = [f"link{index}_c"] - for name in names: - prim = stage.DefinePrim(f"{link_path}/{name}", "Capsule" if selection == "primitives" else "Mesh") - UsdPhysics.CollisionAPI.Apply(prim) - hand_path = f"{link_path}/panda_hand" - for suffix, shape in ( - ("hand_capsule", "Capsule"), - ("panda_leftfinger/left_finger_pad", "Cube"), - ("panda_rightfinger/right_finger_pad", "Cube"), - ): - UsdPhysics.CollisionAPI.Apply(stage.DefinePrim(f"{hand_path}/{suffix}", shape)) - colliders.SetVariantSelection("convex_hulls") - - select_usd_variants("/Robot", cfg.scene.robot.spawn.variants or {}, stage=stage) - for pattern, fragments in (cfg.scene.robot.spawn.collision_props or {}).items(): - assert apply_collision_properties(f"/Robot{pattern}", fragments, stage=stage) - - collision_prims = [prim for prim in stage.Traverse() if prim.HasAPI(UsdPhysics.CollisionAPI)] - enabled = [prim for prim in collision_prims if UsdPhysics.CollisionAPI(prim).GetCollisionEnabledAttr().Get()] - disabled = [prim for prim in collision_prims if not UsdPhysics.CollisionAPI(prim).GetCollisionEnabledAttr().Get()] - assert {(prim.GetName(), prim.GetTypeName()) for prim in enabled} == { - ("hand_capsule", "Capsule"), - ("left_finger_pad", "Cube"), - ("right_finger_pad", "Cube"), - } - assert len(disabled) == 10 - assert all(prim.GetTypeName() == "Capsule" for prim in disabled) - - def test_reset_clearance_ignores_disabled_collision_geometry() -> None: """Disabled colliders and visual-only geometry must not reject reset candidates.""" stage = Usd.Stage.CreateInMemory()