From b6374c6940cc6e29fa650ee12135b477908100e6 Mon Sep 17 00:00:00 2001 From: Maximilian Krause Date: Tue, 29 Sep 2026 19:12:55 +0200 Subject: [PATCH] Add flat Franka asset and backend body-resolution support --- ...aximiliank-direct-body-prim-resolution.rst | 4 + source/isaaclab/isaaclab/sim/utils/queries.py | 8 ++ source/isaaclab/test/sim/test_cloner.py | 8 ++ .../maximiliank-franka-flat-config.minor.rst | 14 ++ .../isaaclab_assets/__init__.pyi | 6 + .../isaaclab_assets/robots/__init__.pyi | 6 + .../isaaclab_assets/robots/franka.py | 124 +++++++++++++++--- .../test/test_valid_configs.py | 20 +++ ...miliank-franka-material-body-selection.rst | 4 + .../assets/articulation/articulation.py | 8 +- .../isaaclab_newton/envs/mdp/events.py | 10 +- .../test/assets/test_articulation.py | 52 ++++---- ...iliank-frame-transformer-nested-bodies.rst | 5 + .../frame_transformer/frame_transformer.py | 10 +- .../test/assets/test_articulation.py | 34 ++++- .../test/sensors/test_frame_transformer.py | 42 ++++++ ...iliank-frame-transformer-nested-bodies.rst | 5 + .../frame_transformer/frame_transformer.py | 12 +- .../test/assets/test_articulation.py | 26 ++-- .../test/sensors/test_frame_transformer.py | 39 ++++++ 20 files changed, 360 insertions(+), 77 deletions(-) create mode 100644 source/isaaclab/changelog.d/maximiliank-direct-body-prim-resolution.rst create mode 100644 source/isaaclab_assets/changelog.d/maximiliank-franka-flat-config.minor.rst create mode 100644 source/isaaclab_newton/changelog.d/maximiliank-franka-material-body-selection.rst create mode 100644 source/isaaclab_ov/changelog.d/maximiliank-frame-transformer-nested-bodies.rst create mode 100644 source/isaaclab_physx/changelog.d/maximiliank-frame-transformer-nested-bodies.rst diff --git a/source/isaaclab/changelog.d/maximiliank-direct-body-prim-resolution.rst b/source/isaaclab/changelog.d/maximiliank-direct-body-prim-resolution.rst new file mode 100644 index 000000000000..4ead32834c86 --- /dev/null +++ b/source/isaaclab/changelog.d/maximiliank-direct-body-prim-resolution.rst @@ -0,0 +1,4 @@ +Fixed +^^^^^ + +* Fixed direct rigid-body path queries so nested rigid-body descendants are not selected when the requested body matches. diff --git a/source/isaaclab/isaaclab/sim/utils/queries.py b/source/isaaclab/isaaclab/sim/utils/queries.py index 0994b049c44c..b73ae060289a 100644 --- a/source/isaaclab/isaaclab/sim/utils/queries.py +++ b/source/isaaclab/isaaclab/sim/utils/queries.py @@ -387,6 +387,7 @@ def resolve_matching_prims_from_source( env_regex_ns: str = "/World/envs/env_[^/]+", raise_if_no_matches: bool = True, traverse_instance_prims: bool = True, + prefer_direct_matches: bool = False, ) -> list[tuple[Usd.Prim, str]]: """Resolve matching prims from a single(source) instance when multiple instances are present. @@ -402,6 +403,8 @@ def resolve_matching_prims_from_source( env_regex_ns: Namespace pattern that marks one instance root when no clone plan applies. raise_if_no_matches: Whether to raise if no prim matches ``path_expr``. Defaults to True. traverse_instance_prims: Whether to traverse instance prims when applying ``predicate``. + prefer_direct_matches: Prefer matching prims named by ``path_expr`` over their matching descendants. + Descendants remain a fallback when no direct prim satisfies ``predicate``. Returns: A list of ``(source_prim, destination_expr)`` pairs. Empty only when @@ -481,6 +484,11 @@ def resolve_matching_prims_from_source( unique_matches.setdefault(child_path, (child, dest + child_path[len(source_path) :])) results = list(unique_matches.values()) + if prefer_direct_matches: + direct_matches = [pair for pair in results if re.fullmatch(path_expr, pair[0].GetPath().pathString)] + if direct_matches: + results = direct_matches + if expected_num_matches is not None and len(results) != expected_num_matches: raise RuntimeError(f"Expected {expected_num_matches} prims at '{path_expr}', found {len(results)}.") if raise_if_no_matches and not results: diff --git a/source/isaaclab/test/sim/test_cloner.py b/source/isaaclab/test/sim/test_cloner.py index 51bfc38077f8..7ea2acef280c 100644 --- a/source/isaaclab/test/sim/test_cloner.py +++ b/source/isaaclab/test/sim/test_cloner.py @@ -285,3 +285,11 @@ def test_resolve_matching_prims_from_source(sim, with_clone_plan): "/World/envs/env_[^/]+/Robot/foo/bar", "/World/envs/env_[^/]+/Robot/other/bar", ] + + matches = queries.resolve_matching_prims_from_source( + r"/World/envs/env_[^/]+/Robot/foo", + predicate=lambda prim: prim.GetName() in {"foo", "bar"}, + expected_num_matches=1, + prefer_direct_matches=True, + ) + assert [prim.GetPath().pathString for prim, _ in matches] == ["/World/envs/env_0/Robot/foo"] diff --git a/source/isaaclab_assets/changelog.d/maximiliank-franka-flat-config.minor.rst b/source/isaaclab_assets/changelog.d/maximiliank-franka-flat-config.minor.rst new file mode 100644 index 000000000000..6618848f9c25 --- /dev/null +++ b/source/isaaclab_assets/changelog.d/maximiliank-franka-flat-config.minor.rst @@ -0,0 +1,14 @@ +Added +^^^^^ + +* Added ``FRANKA_PANDA_FLAT_CFG`` and ``FRANKA_PANDA_FLAT_HIGH_PD_CFG`` for the shared flat Franka + asset. These configurations selected gripper-only colliders and the ``panda_arm`` actuator group; + select the ``Physics`` variant for the target backend. + +Deprecated +^^^^^^^^^^ + +* Deprecated ``FRANKA_PANDA_CFG``, ``FRANKA_PANDA_HIGH_PD_CFG``, and + ``FRANKA_PANDA_MENAGERIE_CFG``. Their previous asset and actuator contracts remained available + during the deprecation window. Use the corresponding ``FRANKA_PANDA_FLAT_*`` configuration to + migrate to the shared flat asset, and replace shoulder/forearm actuator overrides with ``panda_arm``. diff --git a/source/isaaclab_assets/isaaclab_assets/__init__.pyi b/source/isaaclab_assets/isaaclab_assets/__init__.pyi index 2c491a499d7d..b38edb800a5f 100644 --- a/source/isaaclab_assets/isaaclab_assets/__init__.pyi +++ b/source/isaaclab_assets/isaaclab_assets/__init__.pyi @@ -22,7 +22,10 @@ __all__ = [ "GR1T2_CFG", "GR1T2_HIGH_PD_CFG", "FRANKA_PANDA_CFG", + "FRANKA_PANDA_FLAT_CFG", + "FRANKA_PANDA_FLAT_HIGH_PD_CFG", "FRANKA_PANDA_HIGH_PD_CFG", + "FRANKA_PANDA_LEGACY_CFG", "FRANKA_PANDA_MENAGERIE_CFG", "FRANKA_ROBOTIQ_GRIPPER_CFG", "FOURBAR_POLE_CFG", @@ -89,7 +92,10 @@ from .robots import ( GR1T2_CFG, GR1T2_HIGH_PD_CFG, FRANKA_PANDA_CFG, + FRANKA_PANDA_FLAT_CFG, + FRANKA_PANDA_FLAT_HIGH_PD_CFG, FRANKA_PANDA_HIGH_PD_CFG, + FRANKA_PANDA_LEGACY_CFG, FRANKA_PANDA_MENAGERIE_CFG, FRANKA_ROBOTIQ_GRIPPER_CFG, FOURBAR_POLE_CFG, diff --git a/source/isaaclab_assets/isaaclab_assets/robots/__init__.pyi b/source/isaaclab_assets/isaaclab_assets/robots/__init__.pyi index 023c8e801230..e54481a15ee5 100644 --- a/source/isaaclab_assets/isaaclab_assets/robots/__init__.pyi +++ b/source/isaaclab_assets/isaaclab_assets/robots/__init__.pyi @@ -22,7 +22,10 @@ __all__ = [ "GR1T2_CFG", "GR1T2_HIGH_PD_CFG", "FRANKA_PANDA_CFG", + "FRANKA_PANDA_FLAT_CFG", + "FRANKA_PANDA_FLAT_HIGH_PD_CFG", "FRANKA_PANDA_HIGH_PD_CFG", + "FRANKA_PANDA_LEGACY_CFG", "FRANKA_PANDA_MENAGERIE_CFG", "FRANKA_ROBOTIQ_GRIPPER_CFG", "FOURBAR_POLE_CFG", @@ -82,7 +85,10 @@ from .cassie import CASSIE_CFG from .fourier import GR1T2_CFG, GR1T2_HIGH_PD_CFG from .franka import ( FRANKA_PANDA_CFG, + FRANKA_PANDA_FLAT_CFG, + FRANKA_PANDA_FLAT_HIGH_PD_CFG, FRANKA_PANDA_HIGH_PD_CFG, + FRANKA_PANDA_LEGACY_CFG, FRANKA_PANDA_MENAGERIE_CFG, FRANKA_ROBOTIQ_GRIPPER_CFG, ) diff --git a/source/isaaclab_assets/isaaclab_assets/robots/franka.py b/source/isaaclab_assets/isaaclab_assets/robots/franka.py index c8749fab4fdd..11849e12b234 100644 --- a/source/isaaclab_assets/isaaclab_assets/robots/franka.py +++ b/source/isaaclab_assets/isaaclab_assets/robots/franka.py @@ -7,14 +7,23 @@ The following configurations are available: -* :obj:`FRANKA_PANDA_CFG`: Franka Emika Panda robot with Panda hand -* :obj:`FRANKA_PANDA_MENAGERIE_CFG`: Franka Emika Panda robot converted from MuJoCo Menagerie -* :obj:`FRANKA_PANDA_HIGH_PD_CFG`: Franka Emika Panda robot with Panda hand with stiffer PD control +* :obj:`FRANKA_PANDA_FLAT_CFG`: Shared flat asset for maintained Franka tasks +* :obj:`FRANKA_PANDA_FLAT_HIGH_PD_CFG`: Flat asset with stiffer PD control +* :obj:`FRANKA_PANDA_LEGACY_CFG`: Legacy Franka Emika Panda asset configuration +* :obj:`FRANKA_PANDA_CFG`: Deprecated alias retaining the legacy asset and actuators +* :obj:`FRANKA_PANDA_HIGH_PD_CFG`: Deprecated legacy high-PD configuration +* :obj:`FRANKA_PANDA_MENAGERIE_CFG`: Deprecated nested-instance Menagerie configuration * :obj:`FRANKA_ROBOTIQ_GRIPPER_CFG`: Franka robot with Robotiq_2f_85 gripper +The old public names retain their original configuration contracts for the deprecation window. Use +``FRANKA_PANDA_FLAT_CFG`` for new code and select its ``Physics`` variant for the chosen backend. + Reference: https://github.com/frankaemika/franka_ros """ +import warnings +from typing import TYPE_CHECKING + from isaaclab_newton.sim.schemas import NewtonArticulationCfg from isaaclab_physx.sim.schemas import PhysxArticulationCfg, PhysxRigidBodyCfg @@ -28,7 +37,7 @@ # Configuration ## -FRANKA_PANDA_CFG = ArticulationCfg( +FRANKA_PANDA_LEGACY_CFG = ArticulationCfg( spawn=sim_utils.UsdFileCfg( usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Robots/FrankaEmika/Legacy/panda_instanceable.usd", activate_contact_sensors=False, @@ -77,45 +86,120 @@ }, soft_joint_pos_limit_factor=1.0, ) -"""Configuration of Franka Emika Panda robot.""" +"""Configuration of the legacy Franka Emika Panda robot asset.""" -FRANKA_PANDA_MENAGERIE_CFG = clone(FRANKA_PANDA_CFG) -FRANKA_PANDA_MENAGERIE_CFG.spawn.usd_path = f"{ISAACLAB_NUCLEUS_DIR}/Robots/FrankaEmika/franka_panda.usda" -FRANKA_PANDA_MENAGERIE_CFG.actuators = { +FRANKA_PANDA_FLAT_CFG = clone(FRANKA_PANDA_LEGACY_CFG) +FRANKA_PANDA_FLAT_CFG.spawn.usd_path = f"{ISAACLAB_NUCLEUS_DIR}/Robots/FrankaEmika/franka_panda.usda" +FRANKA_PANDA_FLAT_CFG.spawn.variants = {"Physics": "physx", "Colliders": "gripper_only"} +next( + props for props in FRANKA_PANDA_FLAT_CFG.spawn.articulation_props if isinstance(props, PhysxArticulationCfg) +).enabled_self_collisions = False +next( + props for props in FRANKA_PANDA_FLAT_CFG.spawn.articulation_props if isinstance(props, NewtonArticulationCfg) +).self_collision_enabled = False +FRANKA_PANDA_FLAT_CFG.actuators = { "panda_arm": ImplicitActuatorCfg( joint_names_expr=["panda_joint[1-7]"], + joint_effort_limit={"panda_joint[1-4]": 100.0, "panda_joint[5-7]": 12.0}, joint_velocity_limit={"panda_joint[1-4]": 20.0, "panda_joint[5-7]": 25.0}, stiffness=None, damping=None, + viscous_friction=0.0, ), "panda_hand": ImplicitActuatorCfg( - joint_names_expr=["panda_finger_joint.*"], + joint_names_expr=["panda_finger_joint1"], + joint_effort_limit=200.0, stiffness=None, damping=None, + viscous_friction=0.0, + ), + "panda_finger2_passive": ImplicitActuatorCfg( + joint_names_expr=["panda_finger_joint2"], + joint_effort_limit=200.0, + stiffness=0.0, + damping=0.0, + viscous_friction=0.0, ), } -"""Configuration of the MuJoCo Menagerie-derived Franka Emika Panda robot. +"""Configuration of the Franka Emika Panda robot. -The converted model has different inertial and drive authoring from the legacy asset used by -:attr:`FRANKA_PANDA_CFG`. The solver velocity limits provide consistent behavior across physics -backends, while the arm and hand retain their USD-authored drives. +The flat asset contains PhysX and MuJoCo physics variants and gripper-only, primitive, and convex-hull +collider variants. The gripper-only collider variant is the default. Explicit solver properties keep +the actuator contract consistent across physics payloads. Only the leading finger has an active drive; +the authored mimic constraint moves the passive follower. The standalone configuration selects the PhysX +payload by default; direct Newton consumers must select the ``mujoco`` physics variant explicitly. """ -FRANKA_PANDA_HIGH_PD_CFG = clone(FRANKA_PANDA_CFG) -FRANKA_PANDA_HIGH_PD_CFG.spawn.rigid_props.disable_gravity = True -FRANKA_PANDA_HIGH_PD_CFG.actuators["panda_shoulder"].stiffness = 400.0 -FRANKA_PANDA_HIGH_PD_CFG.actuators["panda_shoulder"].damping = 80.0 -FRANKA_PANDA_HIGH_PD_CFG.actuators["panda_forearm"].stiffness = 400.0 -FRANKA_PANDA_HIGH_PD_CFG.actuators["panda_forearm"].damping = 80.0 +FRANKA_PANDA_FLAT_HIGH_PD_CFG = clone(FRANKA_PANDA_FLAT_CFG) +FRANKA_PANDA_FLAT_HIGH_PD_CFG.spawn.rigid_props.disable_gravity = True +FRANKA_PANDA_FLAT_HIGH_PD_CFG.actuators["panda_arm"].stiffness = 400.0 +FRANKA_PANDA_FLAT_HIGH_PD_CFG.actuators["panda_arm"].damping = 80.0 """Configuration of Franka Emika Panda robot with stiffer PD control. This configuration is useful for task-space control using differential IK. """ -FRANKA_ROBOTIQ_GRIPPER_CFG = clone(FRANKA_PANDA_CFG) +_FRANKA_PANDA_COMPAT_CFG = clone(FRANKA_PANDA_LEGACY_CFG) + + +_FRANKA_PANDA_LEGACY_HIGH_PD_CFG = clone(FRANKA_PANDA_LEGACY_CFG) +_FRANKA_PANDA_LEGACY_HIGH_PD_CFG.spawn.rigid_props.disable_gravity = True +for actuator in ("panda_shoulder", "panda_forearm"): + _FRANKA_PANDA_LEGACY_HIGH_PD_CFG.actuators[actuator].stiffness = 400.0 + _FRANKA_PANDA_LEGACY_HIGH_PD_CFG.actuators[actuator].damping = 80.0 + + +_FRANKA_PANDA_NESTED_MENAGERIE_CFG = clone(FRANKA_PANDA_LEGACY_CFG) +_FRANKA_PANDA_NESTED_MENAGERIE_CFG.spawn.usd_path = ( + f"{ISAACLAB_NUCLEUS_DIR}/Robots/FrankaEmika/franka_panda_nestedInstance.usda" +) +_FRANKA_PANDA_NESTED_MENAGERIE_CFG.actuators = { + "panda_arm": ImplicitActuatorCfg( + joint_names_expr=["panda_joint[1-7]"], + joint_velocity_limit={"panda_joint[1-4]": 20.0, "panda_joint[5-7]": 25.0}, + stiffness=None, + damping=None, + ), + "panda_hand": ImplicitActuatorCfg( + joint_names_expr=["panda_finger_joint.*"], + stiffness=None, + damping=None, + ), +} + + +_DEPRECATED_FRANKA_CFGS = { + "FRANKA_PANDA_CFG": (_FRANKA_PANDA_COMPAT_CFG, "FRANKA_PANDA_FLAT_CFG"), + "FRANKA_PANDA_HIGH_PD_CFG": (_FRANKA_PANDA_LEGACY_HIGH_PD_CFG, "FRANKA_PANDA_FLAT_HIGH_PD_CFG"), + "FRANKA_PANDA_MENAGERIE_CFG": (_FRANKA_PANDA_NESTED_MENAGERIE_CFG, "FRANKA_PANDA_FLAT_CFG"), +} + +if TYPE_CHECKING: + FRANKA_PANDA_CFG: ArticulationCfg + FRANKA_PANDA_HIGH_PD_CFG: ArticulationCfg + FRANKA_PANDA_MENAGERIE_CFG: ArticulationCfg + + +def __getattr__(name: str) -> ArticulationCfg: + if name not in _DEPRECATED_FRANKA_CFGS: + raise AttributeError(f"module {__name__!r} has no attribute {name!r}") + cfg, replacement = _DEPRECATED_FRANKA_CFGS[name] + warnings.warn( + f"{name} is deprecated; use {replacement} for the flat Franka asset.", + FutureWarning, + stacklevel=2, + ) + return cfg + + +def __dir__() -> list[str]: + return sorted(set(globals()) | _DEPRECATED_FRANKA_CFGS.keys()) + + +FRANKA_ROBOTIQ_GRIPPER_CFG = clone(FRANKA_PANDA_LEGACY_CFG) FRANKA_ROBOTIQ_GRIPPER_CFG.spawn.usd_path = f"{ISAAC_NUCLEUS_DIR}/Robots/FrankaRobotics/FrankaPanda/franka.usd" FRANKA_ROBOTIQ_GRIPPER_CFG.spawn.variants = {"Gripper": "Robotiq_2F_85"} FRANKA_ROBOTIQ_GRIPPER_CFG.spawn.rigid_props.disable_gravity = True diff --git a/source/isaaclab_assets/test/test_valid_configs.py b/source/isaaclab_assets/test/test_valid_configs.py index 3832e196def1..b267e4862bd4 100644 --- a/source/isaaclab_assets/test/test_valid_configs.py +++ b/source/isaaclab_assets/test/test_valid_configs.py @@ -19,6 +19,26 @@ from isaaclab.test.utils import DeviceScope, test_devices import isaaclab_assets as lab_assets # noqa: F401 +import isaaclab_assets.robots.franka as franka_assets + + +def test_franka_legacy_configs_warn_and_keep_their_contract() -> None: + """Deprecated Franka names retain their previous asset and actuator contracts.""" + with pytest.warns(FutureWarning, match="FRANKA_PANDA_CFG.*deprecated"): + legacy_cfg = franka_assets.FRANKA_PANDA_CFG + assert legacy_cfg.spawn.usd_path.endswith("/Legacy/panda_instanceable.usd") + assert set(legacy_cfg.actuators) == {"panda_shoulder", "panda_forearm", "panda_hand"} + + with pytest.warns(FutureWarning, match="FRANKA_PANDA_HIGH_PD_CFG.*deprecated"): + high_pd_cfg = franka_assets.FRANKA_PANDA_HIGH_PD_CFG + assert high_pd_cfg.spawn.usd_path == legacy_cfg.spawn.usd_path + assert high_pd_cfg.actuators["panda_shoulder"].stiffness == 400.0 + assert high_pd_cfg.actuators["panda_forearm"].stiffness == 400.0 + + with pytest.warns(FutureWarning, match="FRANKA_PANDA_MENAGERIE_CFG.*deprecated"): + menagerie_cfg = franka_assets.FRANKA_PANDA_MENAGERIE_CFG + assert menagerie_cfg.spawn.usd_path.endswith("/franka_panda_nestedInstance.usda") + assert set(menagerie_cfg.actuators) == {"panda_arm", "panda_hand"} @pytest.fixture(scope="module") diff --git a/source/isaaclab_newton/changelog.d/maximiliank-franka-material-body-selection.rst b/source/isaaclab_newton/changelog.d/maximiliank-franka-material-body-selection.rst new file mode 100644 index 000000000000..f3763ed81629 --- /dev/null +++ b/source/isaaclab_newton/changelog.d/maximiliank-franka-material-body-selection.rst @@ -0,0 +1,4 @@ +Fixed +^^^^^ + +* Fixed Newton articulation material randomization for bodies whose shapes are interleaved in the native shape array. diff --git a/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation.py b/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation.py index e905d1adb0e7..66d8c4bb07d1 100644 --- a/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation.py +++ b/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation.py @@ -271,7 +271,7 @@ def num_bodies(self) -> int: @property def num_shapes_per_body(self) -> list[int]: - """Number of collision shapes per body in public body-name order. + """Number of shapes per body in public body-name order. Each element corresponds to the body at the same index in :attr:`body_names`. Backend-order counts are cached; a nonidentity body @@ -287,11 +287,11 @@ def num_shapes_per_body(self) -> list[int]: @property def backend_num_shapes_per_body(self) -> list[int]: - """Number of collision shapes per body in active backend solver-view order. + """Number of shapes per body in active backend solver-view order. Each element corresponds to the body at the same index in - :attr:`backend_body_names`, matching the shape axis of the backend - solver arrays. The counts are cached on first access. Use + :attr:`backend_body_names`. Shapes belonging to one body need not be + contiguous in the backend shape arrays. The counts are cached on first access. Use :attr:`num_shapes_per_body` for public body order. Returns: diff --git a/source/isaaclab_newton/isaaclab_newton/envs/mdp/events.py b/source/isaaclab_newton/isaaclab_newton/envs/mdp/events.py index e7c5faa973af..4724037e0869 100644 --- a/source/isaaclab_newton/isaaclab_newton/envs/mdp/events.py +++ b/source/isaaclab_newton/isaaclab_newton/envs/mdp/events.py @@ -63,14 +63,10 @@ def __init__(self, cfg: EventTermCfg, env: ManagerBasedEnv) -> None: self._restitution_binding = asset._root_view.get_attribute("shape_material_restitution", model)[:, 0] # type: ignore if isinstance(asset, assets.Articulation) and asset_cfg.body_ids != slice(None): - # Shape counts use backend body order. - num_shapes_per_body = asset.backend_num_shapes_per_body - shape_indices_list = [] backend_body_ids = asset.map_body_ids_to_backend(asset_cfg.body_ids) - for body_id in backend_body_ids: - start_idx = sum(num_shapes_per_body[:body_id]) - end_idx = start_idx + num_shapes_per_body[body_id] - shape_indices_list.extend(range(start_idx, end_idx)) + shape_indices_list = [ + shape for body_id in backend_body_ids for shape in asset.root_view.body_shapes[body_id] + ] self._shape_indices = torch.tensor(shape_indices_list, dtype=torch.long) else: self._shape_indices = torch.arange(self._friction_binding.shape[1], dtype=torch.long) diff --git a/source/isaaclab_newton/test/assets/test_articulation.py b/source/isaaclab_newton/test/assets/test_articulation.py index f0f4b5da6dbc..9cc6e1787eac 100644 --- a/source/isaaclab_newton/test/assets/test_articulation.py +++ b/source/isaaclab_newton/test/assets/test_articulation.py @@ -66,9 +66,14 @@ ## # Pre-defined configs ## -from isaaclab_assets import ANYMAL_C_CFG, FRANKA_PANDA_CFG, FRANKA_PANDA_HIGH_PD_CFG # isort:skip +from isaaclab_assets import ANYMAL_C_CFG, FRANKA_PANDA_FLAT_CFG, FRANKA_PANDA_FLAT_HIGH_PD_CFG # isort:skip from isaaclab_assets.robots.shadow_hand import SHADOW_HAND_NEWTON_CFG +_FRANKA_PANDA_NEWTON_CFG = FRANKA_PANDA_FLAT_CFG.copy() +_FRANKA_PANDA_NEWTON_CFG.spawn.variants = {"Physics": "mujoco", "Colliders": "gripper_only"} +_FRANKA_PANDA_HIGH_PD_NEWTON_CFG = FRANKA_PANDA_FLAT_HIGH_PD_CFG.copy() +_FRANKA_PANDA_HIGH_PD_NEWTON_CFG.spawn.variants = {"Physics": "mujoco", "Colliders": "gripper_only"} + SIM_CFGs = { "humanoid": SimulationCfg( physics=NewtonCfg( @@ -227,7 +232,7 @@ def generate_articulation_cfg( actuators={"body": ImplicitActuatorCfg(joint_names_expr=[".*"], stiffness=stiffness, damping=damping)}, ) elif articulation_type == "panda": - articulation_cfg = FRANKA_PANDA_CFG + articulation_cfg = _FRANKA_PANDA_NEWTON_CFG elif articulation_type == "anymal": articulation_cfg = ANYMAL_C_CFG elif articulation_type == "shadow_hand": @@ -438,7 +443,7 @@ def generate_articulation( def _setup_franka_at_home_pose(sim, *, zero_actuator_pd: bool = False, disable_gravity: bool = True): """Build a Franka articulation at its configured home pose. - Constructs :data:`FRANKA_PANDA_HIGH_PD_CFG`, optionally zeroes the + Constructs :data:`_FRANKA_PANDA_HIGH_PD_NEWTON_CFG`, optionally zeroes the arm-actuator PD gains, resets the simulator, and teleports the arm joints to :attr:`default_joint_pos` (the env reset path that normally does this is not invoked for standalone tests, so the @@ -447,23 +452,21 @@ def _setup_franka_at_home_pose(sim, *, zero_actuator_pd: bool = False, disable_g Args: sim: The simulation context to use. - zero_actuator_pd: If True, sets the panda_shoulder/panda_forearm - actuator stiffness and damping to zero. Used by the OSC test + zero_actuator_pd: If True, sets the ``panda_arm`` actuator stiffness + and damping to zero. Used by the OSC test so OSC's joint-effort output is not opposed by the implicit-PD's residual ``kp·(target − q)``. disable_gravity: Per-body gravity flag written to the spawn config. - :data:`FRANKA_PANDA_HIGH_PD_CFG` ships with gravity disabled; + :data:`_FRANKA_PANDA_HIGH_PD_NEWTON_CFG` ships with gravity disabled; pass False for tests where the arm must feel scene gravity. Returns: Tuple of ``(robot, ee_frame_idx, ee_jacobi_idx, arm_joint_ids)``. """ - cfg = replace(clone(FRANKA_PANDA_HIGH_PD_CFG), prim_path="/World/Env_[^/]*/Robot") + cfg = replace(_FRANKA_PANDA_HIGH_PD_NEWTON_CFG, prim_path="/World/Env_[^/]*/Robot") if zero_actuator_pd: - cfg.actuators["panda_shoulder"].stiffness = 0.0 - cfg.actuators["panda_shoulder"].damping = 0.0 - cfg.actuators["panda_forearm"].stiffness = 0.0 - cfg.actuators["panda_forearm"].damping = 0.0 + cfg.actuators["panda_arm"].stiffness = 0.0 + cfg.actuators["panda_arm"].damping = 0.0 cfg.spawn.rigid_props.disable_gravity = disable_gravity sim_utils.create_prim("/World/Env_0", "Xform", translation=(0.0, 0.0, 0.0)) clone_plan_from_env_0(CloneCfg(clone_template="/World/Env_{}"), (cfg,), 1, 0.0) @@ -1738,7 +1741,6 @@ def test_out_of_range_default_joint_state(sim, device, articulation_type, state_ quantity = "positions" if state_field == "joint_pos" else "velocities" replicate(sim.get_clone_plan()) with pytest.raises(ValueError, match=f"default {quantity} out of the limits"): - replicate(sim.get_clone_plan()) sim.reset() @@ -2511,29 +2513,31 @@ def test_write_joint_viscous_friction_to_sim(sim, num_articulations, device, art Static joint friction writes also propagate directly to the Newton model. """ - articulation_cfg = generate_articulation_cfg(articulation_type) - articulation_cfg.actuators["panda_shoulder"].viscous_friction = 0.25 + articulation_cfg = clone(generate_articulation_cfg(articulation_type)) + articulation_cfg.actuators["panda_arm"].viscous_friction = 0.25 + articulation_cfg.actuators["panda_arm"].damping = 2.0 articulation, _ = generate_articulation(articulation_cfg, num_articulations, device) replicate(sim.get_clone_plan()) sim.reset() - shoulder_joint_ids = articulation.actuators["panda_shoulder"].joint_indices - expected_viscous_friction = torch.full((articulation.num_instances, 4), 0.25, device=device) + arm_joint_ids = articulation.actuators["panda_arm"].joint_indices + expected_viscous_friction = torch.full((articulation.num_instances, len(arm_joint_ids)), 0.25, device=device) torch.testing.assert_close( - articulation.data.joint_viscous_friction_coeff.torch[:, shoulder_joint_ids], expected_viscous_friction + articulation.data.joint_viscous_friction_coeff.torch[:, arm_joint_ids], expected_viscous_friction ) torch.testing.assert_close( wp.to_torch(articulation.root_view.get_attribute("joint_damping", SimulationManager.get_model()))[ - :, 0, shoulder_joint_ids + :, 0, arm_joint_ids ], expected_viscous_friction, ) - expected_pd_damping = torch.full_like(expected_viscous_friction, 4.0) - torch.testing.assert_close(articulation.data.joint_damping.torch[:, shoulder_joint_ids], expected_pd_damping) + expected_pd_damping = torch.full_like(expected_viscous_friction, 2.0) + assert not torch.allclose(expected_pd_damping, expected_viscous_friction) + torch.testing.assert_close(articulation.data.joint_damping.torch[:, arm_joint_ids], expected_pd_damping) torch.testing.assert_close( wp.to_torch(articulation.root_view.get_attribute("joint_target_kd", SimulationManager.get_model()))[ - :, 0, shoulder_joint_ids + :, 0, arm_joint_ids ], expected_pd_damping, ) @@ -2840,7 +2844,7 @@ def test_heterogeneous_scene_per_view_shapes(sim, device, add_ground_plane, arti # per-articulation shape gate without that pre-existing quirk. num_per_type = 1 - franka_cfg = replace(FRANKA_PANDA_CFG, prim_path="/World/Env_[^/]*/Franka") + franka_cfg = replace(_FRANKA_PANDA_NEWTON_CFG, prim_path="/World/Env_[^/]*/Franka") anymal_cfg = replace(ANYMAL_C_CFG, prim_path="/World/Env_[^/]*/Anymal") anymal_cfg.init_state.pos = (0.0, 5.0, anymal_cfg.init_state.pos[2]) @@ -3142,7 +3146,7 @@ def test_get_gravity_compensation_forces_static_equilibrium(sim, num_articulatio # gravity-comp signal. Default Franka cfg has stiffness=80 / damping=4 # which would absorb gravity through PD bias and hide accessor bugs. cfg = replace(base_cfg, actuators={"all": ImplicitActuatorCfg(joint_names_expr=[".*"], stiffness=0.0, damping=0.0)}) - # FRANKA_PANDA_CFG has rigid_props.disable_gravity=False already, but be + # FRANKA_PANDA_FLAT_CFG has rigid_props.disable_gravity=False already, but be # defensive — gravity must be ON for τ_gc to have anything to cancel. cfg = replace(cfg, spawn=replace(cfg.spawn, rigid_props=replace(cfg.spawn.rigid_props, disable_gravity=False))) @@ -3160,7 +3164,7 @@ def test_get_gravity_compensation_forces_static_equilibrium(sim, num_articulatio articulation.write_joint_velocity_to_sim_index(velocity=default_qd) articulation.update(sim.cfg.dt) - # Default joint pose from FRANKA_PANDA_CFG bends the elbow + # Default joint pose from FRANKA_PANDA_FLAT_CFG bends the elbow # (joint2=-0.569, joint4=-2.81, joint6=3.04) so several links carry a # gravity load — τ_gc is non-trivial in this configuration. A natural- # hang pose (all zeros) would produce near-zero τ_gc and make this diff --git a/source/isaaclab_ov/changelog.d/maximiliank-frame-transformer-nested-bodies.rst b/source/isaaclab_ov/changelog.d/maximiliank-frame-transformer-nested-bodies.rst new file mode 100644 index 000000000000..02a80fb24471 --- /dev/null +++ b/source/isaaclab_ov/changelog.d/maximiliank-frame-transformer-nested-bodies.rst @@ -0,0 +1,5 @@ +Fixed +^^^^^ + +* Fixed frame-transformer path expressions that directly match a rigid body from also selecting + nested rigid-body descendants. diff --git a/source/isaaclab_ov/isaaclab_ov/sensors/frame_transformer/frame_transformer.py b/source/isaaclab_ov/isaaclab_ov/sensors/frame_transformer/frame_transformer.py index 0dec1c3c1368..f5a001c43eff 100644 --- a/source/isaaclab_ov/isaaclab_ov/sensors/frame_transformer/frame_transformer.py +++ b/source/isaaclab_ov/isaaclab_ov/sensors/frame_transformer/frame_transformer.py @@ -176,7 +176,11 @@ def has_rigid_body_api(prim) -> bool: return bool(prim.HasAPI(UsdPhysics.RigidBodyAPI)) matches = resolve_matching_prims_from_source( - prim_path, predicate=has_rigid_body_api, raise_if_no_matches=False + prim_path, + predicate=has_rigid_body_api, + expected_num_matches=1 if frame_type == "source" else None, + raise_if_no_matches=False, + prefer_direct_matches=True, ) if not matches: raise ValueError( @@ -296,8 +300,8 @@ def has_rigid_body_api(prim) -> bool: # -- target frames: use relative prim path for unique identification self._target_frame_body_names = [self._get_relative_body_path(prim_path) for prim_path in sorted_prim_paths] - # -- source frame: use relative prim path for unique identification - self._source_frame_body_name = self._get_relative_body_path(self.cfg.prim_path) + # -- source frame: retain the concrete body resolved from the configured expression + self._source_frame_body_name = tracked_body_names[0] source_frame_index = self._target_frame_body_names.index(self._source_frame_body_name) # Only remove source frame from tracked bodies if it is not also a target frame diff --git a/source/isaaclab_ov/test/assets/test_articulation.py b/source/isaaclab_ov/test/assets/test_articulation.py index 40cea80c376a..0c7a57456c27 100644 --- a/source/isaaclab_ov/test/assets/test_articulation.py +++ b/source/isaaclab_ov/test/assets/test_articulation.py @@ -98,7 +98,7 @@ ## # Pre-defined configs ## -from isaaclab_assets import ANYMAL_C_CFG, CARTPOLE_CFG, FRANKA_PANDA_CFG, SHADOW_HAND_PHYSX_CFG # isort:skip +from isaaclab_assets import ANYMAL_C_CFG, CARTPOLE_CFG, FRANKA_PANDA_FLAT_CFG, SHADOW_HAND_PHYSX_CFG # isort:skip wp.init() @@ -437,7 +437,7 @@ def generate_articulation_cfg( actuators={"body": ImplicitActuatorCfg(joint_names_expr=[".*"], stiffness=stiffness, damping=damping)}, ) elif articulation_type == "panda": - articulation_cfg = FRANKA_PANDA_CFG + articulation_cfg = FRANKA_PANDA_FLAT_CFG elif articulation_type == "anymal": articulation_cfg = ANYMAL_C_CFG elif articulation_type == "shadow_hand": @@ -528,6 +528,32 @@ def generate_articulation( return articulation, translations +@pytest.mark.parametrize("device", ["cuda:0"]) +@pytest.mark.parametrize("gravity_enabled", [False]) +def test_franka_newton_mimic_constraint_tracks_passive_finger(sim, device, gravity_enabled): + """Drive only the Franka leader finger and preserve mimic tracking in every clone.""" + articulation, _ = generate_articulation(FRANKA_PANDA_FLAT_CFG, 2, device) + sim.reset() + + leader_id = articulation.find_joints("panda_finger_joint1")[0][0] + follower_id = articulation.find_joints("panda_finger_joint2")[0][0] + initial_leader_pos = articulation.data.joint_pos.torch[:, leader_id].clone() + leader_target = torch.full((articulation.num_instances, 1), 0.01, device=device) + articulation.actuators.target_command.set_position_index(value=leader_target, joint_ids=[leader_id]) + + for _ in range(120): + articulation.write_data_to_sim() + sim.step() + articulation.update(sim.cfg.dt) + assert torch.isfinite(articulation.data.joint_pos.torch).all() + assert torch.isfinite(articulation.data.joint_vel.torch).all() + + leader_pos = articulation.data.joint_pos.torch[:, leader_id] + follower_pos = articulation.data.joint_pos.torch[:, follower_id] + assert torch.all(torch.abs(leader_pos - initial_leader_pos) > 0.005) + torch.testing.assert_close(follower_pos, leader_pos, rtol=0.0, atol=5.0e-4) + + @pytest.mark.parametrize("device", ["cuda:0"]) def test_newton_native_explicit_actuator_submits_ovphysx_effort(device): """Run a Newton-native explicit actuator through the current OVPhysX state and effort binding.""" @@ -1745,7 +1771,7 @@ def test_out_of_range_default_joint_vel(sim, device): 1. The articulation fails to initialize when joint velocities are out of range 2. The error is properly handled """ - articulation_cfg = replace(FRANKA_PANDA_CFG, prim_path="/World/Robot") + articulation_cfg = replace(FRANKA_PANDA_FLAT_CFG, prim_path="/World/Robot") articulation_cfg.init_state.joint_vel = { "panda_joint1": 100.0, "panda_joint[2, 4]": -60.0, @@ -2555,7 +2581,7 @@ def test_com_orientation_write_invalidates_static_inertia_cache_with_body_orderi below forbids cross-device staging, so this test is CPU-only. """ sim._app_control_on_stop_handle = None - articulation_cfg = replace(FRANKA_PANDA_CFG, body_ordering=PANDA_ROOT_PRESERVING_REVERSED_BODY_NAMES) + articulation_cfg = replace(FRANKA_PANDA_FLAT_CFG, body_ordering=PANDA_ROOT_PRESERVING_REVERSED_BODY_NAMES) articulation, _ = generate_articulation(articulation_cfg, 1, device=device) sim.reset() diff --git a/source/isaaclab_ov/test/sensors/test_frame_transformer.py b/source/isaaclab_ov/test/sensors/test_frame_transformer.py index 4f520bd382b0..45aadfb9735b 100644 --- a/source/isaaclab_ov/test/sensors/test_frame_transformer.py +++ b/source/isaaclab_ov/test/sensors/test_frame_transformer.py @@ -50,6 +50,7 @@ from isaaclab.utils import configclass, replace # noqa: E402 from isaaclab_assets.robots.anymal import ANYMAL_C_CFG # noqa: E402 +from isaaclab_assets.robots.franka import FRANKA_PANDA_FLAT_CFG # noqa: E402 wp.init() @@ -668,3 +669,44 @@ class MultiRobotSceneCfg(InteractiveSceneCfg): assert any(torch.allclose(rf_pos, expected, atol=1e-5) for expected in expected_rf_positions), ( f"RF_SHANK position {rf_pos} doesn't match either configured body offset" ) + + +@pytest.mark.parametrize("device", ["cpu", "cuda:0"]) +def test_frame_transformer_nested_rigid_bodies(device): + """Test that a matched rigid body does not include nested rigid-body descendants.""" + with _ovphysx_sim_context(device=device) as sim: + sim._app_control_on_stop_handle = None + scene_cfg = _SceneCfg(num_envs=2, env_spacing=5.0, lazy_sensor_update=False) + scene_cfg.robot = replace(FRANKA_PANDA_FLAT_CFG, prim_path="{ENV_REGEX_NS}/Robot") + scene_cfg.frame_transformer = FrameTransformerCfg( + prim_path="{ENV_REGEX_NS}/Robot/(Geometry/)?panda_link0", + target_frames=[ + FrameTransformerCfg.FrameCfg( + name="hand", + prim_path="{ENV_REGEX_NS}/Robot/(Geometry/.*/)?panda_hand", + ), + FrameTransformerCfg.FrameCfg( + name="left_finger", + prim_path="{ENV_REGEX_NS}/Robot/(Geometry/.*/)?panda_leftfinger", + ), + FrameTransformerCfg.FrameCfg( + name="right_finger", + prim_path="{ENV_REGEX_NS}/Robot/(Geometry/.*/)?panda_rightfinger", + ), + ], + ) + scene = InteractiveScene(scene_cfg) + + sim.reset() + scene.update(sim.get_physics_dt()) + + robot = scene.articulations["robot"] + source_id = robot.find_bodies("panda_link0")[0][0] + target_ids = robot.find_bodies(["panda_hand", "panda_leftfinger", "panda_rightfinger"])[0] + frame_data = scene.sensors["frame_transformer"].data + + assert frame_data.target_frame_names == ["hand", "left_finger", "right_finger"] + torch.testing.assert_close(frame_data.source_pos_w.torch, robot.data.body_pos_w.torch[:, source_id]) + torch.testing.assert_close(frame_data.source_quat_w.torch, robot.data.body_quat_w.torch[:, source_id]) + torch.testing.assert_close(frame_data.target_pos_w.torch, robot.data.body_pos_w.torch[:, target_ids]) + torch.testing.assert_close(frame_data.target_quat_w.torch, robot.data.body_quat_w.torch[:, target_ids]) diff --git a/source/isaaclab_physx/changelog.d/maximiliank-frame-transformer-nested-bodies.rst b/source/isaaclab_physx/changelog.d/maximiliank-frame-transformer-nested-bodies.rst new file mode 100644 index 000000000000..02a80fb24471 --- /dev/null +++ b/source/isaaclab_physx/changelog.d/maximiliank-frame-transformer-nested-bodies.rst @@ -0,0 +1,5 @@ +Fixed +^^^^^ + +* Fixed frame-transformer path expressions that directly match a rigid body from also selecting + nested rigid-body descendants. diff --git a/source/isaaclab_physx/isaaclab_physx/sensors/frame_transformer/frame_transformer.py b/source/isaaclab_physx/isaaclab_physx/sensors/frame_transformer/frame_transformer.py index 9a9055e10c98..1f17ce8463b2 100644 --- a/source/isaaclab_physx/isaaclab_physx/sensors/frame_transformer/frame_transformer.py +++ b/source/isaaclab_physx/isaaclab_physx/sensors/frame_transformer/frame_transformer.py @@ -191,7 +191,13 @@ def _initialize_impl(self): def has_rigid_body_api(prim) -> bool: return bool(prim.HasAPI(UsdPhysics.RigidBodyAPI)) - matches = resolve_matching_prims_from_source(prim_path, has_rigid_body_api, raise_if_no_matches=False) + matches = resolve_matching_prims_from_source( + prim_path, + has_rigid_body_api, + expected_num_matches=1 if frame_type == "source" else None, + raise_if_no_matches=False, + prefer_direct_matches=True, + ) if not matches: raise ValueError( f"Failed to create frame transformer for frame '{frame}' with path '{prim_path}'." @@ -298,8 +304,8 @@ def extract_env_num_and_prim_path(item: str) -> tuple[int, str]: # -- target frames: use relative prim path for unique identification self._target_frame_body_names = [self._get_relative_body_path(prim_path) for prim_path in sorted_prim_paths] - # -- source frame: use relative prim path for unique identification - self._source_frame_body_name = self._get_relative_body_path(self.cfg.prim_path) + # -- source frame: retain the concrete body resolved from the configured expression + self._source_frame_body_name = tracked_body_names[0] source_frame_index = self._target_frame_body_names.index(self._source_frame_body_name) # Only remove source frame from tracked bodies if it is not also a target frame diff --git a/source/isaaclab_physx/test/assets/test_articulation.py b/source/isaaclab_physx/test/assets/test_articulation.py index 7078c528911e..81e6007b18e4 100644 --- a/source/isaaclab_physx/test/assets/test_articulation.py +++ b/source/isaaclab_physx/test/assets/test_articulation.py @@ -51,8 +51,8 @@ ## from isaaclab_assets import ( # isort:skip ANYMAL_C_CFG, - FRANKA_PANDA_CFG, - FRANKA_PANDA_HIGH_PD_CFG, + FRANKA_PANDA_FLAT_CFG, + FRANKA_PANDA_FLAT_HIGH_PD_CFG, ) from isaaclab_assets.robots.shadow_hand import SHADOW_HAND_PHYSX_CFG @@ -98,7 +98,7 @@ def generate_articulation_cfg( actuators={"body": ImplicitActuatorCfg(joint_names_expr=[".*"], stiffness=stiffness, damping=damping)}, ) elif articulation_type == "panda": - articulation_cfg = FRANKA_PANDA_CFG + articulation_cfg = FRANKA_PANDA_FLAT_CFG elif articulation_type == "anymal": articulation_cfg = ANYMAL_C_CFG elif articulation_type == "shadow_hand": @@ -217,7 +217,7 @@ def _setup_franka_at_home_pose(sim): Returns: Tuple of ``(robot, ee_frame_idx, ee_jacobi_idx, arm_joint_ids)``. """ - cfg = replace(clone(FRANKA_PANDA_HIGH_PD_CFG), prim_path="/World/Env_[^/]*/Robot") + cfg = replace(clone(FRANKA_PANDA_FLAT_HIGH_PD_CFG), prim_path="/World/Env_[^/]*/Robot") sim_utils.create_prim("/World/Env_0", "Xform", translation=(0.0, 0.0, 0.0)) robot = Articulation(cfg) sim.reset() @@ -309,7 +309,7 @@ def sim(request): def test_live_manual_root_preserving_ordering_reorders_backend_reads_and_writes(sim, device, gravity_enabled): """Smoke-test non-identity joint/body ordering through a live PhysX articulation.""" articulation_cfg = replace( - FRANKA_PANDA_CFG, + FRANKA_PANDA_FLAT_CFG, prim_path="/World/Robot", joint_ordering=tuple(reversed(PANDA_JOINT_NAMES)), body_ordering=PANDA_ROOT_PRESERVING_REVERSED_BODY_NAMES, @@ -433,13 +433,13 @@ def test_reversed_joint_dynamics_use_public_joint_basis(sim, device, gravity_ena @pytest.mark.parametrize("gravity_enabled", [False]) def test_live_floating_root_writers_match_identity_after_body_reordering(sim, device, gravity_enabled): """Keep floating-base root writes invariant when public body order moves the root.""" - floating_spawn = replace(FRANKA_PANDA_CFG.spawn, fix_root_link=False) + floating_spawn = replace(FRANKA_PANDA_FLAT_CFG.spawn, fix_root_link=False) identity = Articulation( - replace(FRANKA_PANDA_CFG, prim_path="/World/IdentityRobot", spawn=floating_spawn, body_ordering=None) + replace(FRANKA_PANDA_FLAT_CFG, prim_path="/World/IdentityRobot", spawn=floating_spawn, body_ordering=None) ) ordered = Articulation( replace( - FRANKA_PANDA_CFG, + FRANKA_PANDA_FLAT_CFG, prim_path="/World/OrderedRobot", spawn=floating_spawn, body_ordering=tuple(reversed(PANDA_BODY_NAMES)), @@ -511,7 +511,9 @@ def test_live_direct_view_mass_inertia_writes_become_visible(sim, device, gravit ``user_to_backend``. """ body_ordering_arg = None if body_ordering == "identity" else PANDA_ROOT_PRESERVING_REVERSED_BODY_NAMES - articulation = Articulation(replace(FRANKA_PANDA_CFG, prim_path="/World/Robot", body_ordering=body_ordering_arg)) + articulation = Articulation( + replace(FRANKA_PANDA_FLAT_CFG, prim_path="/World/Robot", body_ordering=body_ordering_arg) + ) sim.reset() assert articulation.is_initialized @@ -861,7 +863,7 @@ def test_out_of_range_default_joint_vel(sim, device): 1. The articulation fails to initialize when joint velocities are out of range 2. The error is properly handled """ - articulation_cfg = replace(FRANKA_PANDA_CFG, prim_path="/World/Robot") + articulation_cfg = replace(FRANKA_PANDA_FLAT_CFG, prim_path="/World/Robot") articulation_cfg.init_state.joint_vel = { "panda_joint1": 100.0, "panda_joint[2, 4]": -60.0, @@ -2141,7 +2143,7 @@ def test_get_gravity_compensation_forces_static_equilibrium(sim, num_articulatio # gravity-comp signal. Default Franka cfg has stiffness=80 / damping=4 # which would absorb gravity through PD bias and hide accessor bugs. cfg = replace(base_cfg, actuators={"all": ImplicitActuatorCfg(joint_names_expr=[".*"], stiffness=0.0, damping=0.0)}) - # FRANKA_PANDA_CFG has rigid_props.disable_gravity=False already, but be + # FRANKA_PANDA_FLAT_CFG has rigid_props.disable_gravity=False already, but be # defensive — gravity must be ON for τ_gc to have anything to cancel. cfg = replace(cfg, spawn=replace(cfg.spawn, rigid_props=replace(cfg.spawn.rigid_props, disable_gravity=False))) @@ -2157,7 +2159,7 @@ def test_get_gravity_compensation_forces_static_equilibrium(sim, num_articulatio articulation.write_joint_state_to_sim(default_q, default_qd) articulation.update(sim.cfg.dt) - # Default joint pose from FRANKA_PANDA_CFG bends the elbow + # Default joint pose from FRANKA_PANDA_FLAT_CFG bends the elbow # (joint2=-0.569, joint4=-2.81, joint6=3.04) so several links carry a # gravity load — τ_gc is non-trivial in this configuration. A natural- # hang pose (all zeros) would produce near-zero τ_gc and make this diff --git a/source/isaaclab_physx/test/sensors/test_frame_transformer.py b/source/isaaclab_physx/test/sensors/test_frame_transformer.py index 7d0ffb0d918a..8775fa47d35f 100644 --- a/source/isaaclab_physx/test/sensors/test_frame_transformer.py +++ b/source/isaaclab_physx/test/sensors/test_frame_transformer.py @@ -32,6 +32,7 @@ # Pre-defined configs ## from isaaclab_assets.robots.anymal import ANYMAL_C_CFG # isort:skip +from isaaclab_assets.robots.franka import FRANKA_PANDA_FLAT_CFG # isort:skip def quat_from_euler_rpy(roll, pitch, yaw, degrees=False): @@ -720,3 +721,41 @@ def test_frame_transformer_invalidation_drops_cached_launch_state(monkeypatch): assert sensor._frame_physx_view is None assert sensor._raw_transforms is None assert sensor._update_cmd is None + + +def test_frame_transformer_nested_rigid_bodies(sim): + """Test that a matched rigid body does not include nested rigid-body descendants.""" + scene_cfg = MySceneCfg(num_envs=2, env_spacing=5.0, lazy_sensor_update=False) + scene_cfg.robot = replace(FRANKA_PANDA_FLAT_CFG, prim_path="{ENV_REGEX_NS}/Robot") + scene_cfg.frame_transformer = FrameTransformerCfg( + prim_path="{ENV_REGEX_NS}/Robot/(Geometry/)?panda_link0", + target_frames=[ + FrameTransformerCfg.FrameCfg( + name="hand", + prim_path="{ENV_REGEX_NS}/Robot/(Geometry/.*/)?panda_hand", + ), + FrameTransformerCfg.FrameCfg( + name="left_finger", + prim_path="{ENV_REGEX_NS}/Robot/(Geometry/.*/)?panda_leftfinger", + ), + FrameTransformerCfg.FrameCfg( + name="right_finger", + prim_path="{ENV_REGEX_NS}/Robot/(Geometry/.*/)?panda_rightfinger", + ), + ], + ) + scene = InteractiveScene(scene_cfg) + + sim.reset() + scene.update(sim.get_physics_dt()) + + robot = scene.articulations["robot"] + source_id = robot.find_bodies("panda_link0")[0][0] + target_ids = robot.find_bodies(["panda_hand", "panda_leftfinger", "panda_rightfinger"])[0] + frame_data = scene.sensors["frame_transformer"].data + + assert frame_data.target_frame_names == ["hand", "left_finger", "right_finger"] + torch.testing.assert_close(frame_data.source_pos_w.torch, robot.data.body_pos_w.torch[:, source_id]) + torch.testing.assert_close(frame_data.source_quat_w.torch, robot.data.body_quat_w.torch[:, source_id]) + torch.testing.assert_close(frame_data.target_pos_w.torch, robot.data.body_pos_w.torch[:, target_ids]) + torch.testing.assert_close(frame_data.target_quat_w.torch, robot.data.body_quat_w.torch[:, target_ids])