diff --git a/docs/source/how-to/make_fixed_prim.rst b/docs/source/how-to/make_fixed_prim.rst index 7bc431888aaa..9d9872c030cf 100644 --- a/docs/source/how-to/make_fixed_prim.rst +++ b/docs/source/how-to/make_fixed_prim.rst @@ -159,6 +159,20 @@ and instancing layout. ``fix_root_link`` is consumed on this path regardless of └── Link0Xform (RigidBody and ArticulationRoot schema) └── FixedJoint (connecting the world frame and Link0Xform) + 5. **Articulation root schema on the fixed joint**: + + UsdPhysics also allows the articulation root properties on the fixed joint that connects the root link to the + world. The articulation is parsed as a fixed-base system rooted at that joint. Setting + :attr:`~isaaclab.sim.spawners.from_files.FileCfg.fix_root_link` to True enables the joint. Setting it to False + is not supported for such an articulation: it logs a warning and leaves the joint enabled. To obtain a floating + base, author the asset without the world joint, or convert it with ``fix_base=False``. + + .. code-block:: text + + ArticulationXform + └── Link0Xform (RigidBody schema) + └── FixedJoint (ArticulationRoot schema, connecting the world frame and Link0Xform) + For floating base articulations, the root prim usually has both the rigid body and the articulation root properties. However, directly connecting this prim to the world frame will cause the simulation to consider the fixed joint as a part of the maximal coordinate tree. This is different from PhysX diff --git a/source/isaaclab/changelog.d/fix-fixed-base-articulation-root.fixed.rst b/source/isaaclab/changelog.d/fix-fixed-base-articulation-root.fixed.rst new file mode 100644 index 000000000000..70f23789c03d --- /dev/null +++ b/source/isaaclab/changelog.d/fix-fixed-base-articulation-root.fixed.rst @@ -0,0 +1,7 @@ +* Fixed ``fix_root_link=True`` raising ``NotImplementedError`` for an articulation rooted at the fixed joint that + attaches it to the world when the joint's ``body0`` targets the asset prim, a fixed-base layout that UsdPhysics + allows. It now enables that joint instead of raising. A fixed joint between two bodies, between a body and a static + collider, or between two prims that are not rigid bodies still raises the error. + ``fix_root_link=False`` is not supported for an articulation rooted at its fixed world joint; it now logs a warning + and leaves the joint enabled. :func:`~isaaclab.sim.utils.find_global_fixed_joint_prim` returns that joint when + called on it. diff --git a/source/isaaclab/isaaclab/physics/physics_manager.py b/source/isaaclab/isaaclab/physics/physics_manager.py index 661ed78998c7..1c8d955d40ef 100644 --- a/source/isaaclab/isaaclab/physics/physics_manager.py +++ b/source/isaaclab/isaaclab/physics/physics_manager.py @@ -103,8 +103,9 @@ def _prepare_stage_creation(cls) -> None: def fix_articulation_root(cls, articulation_prim: Any, stage: Any = None) -> Any: """Ensure that an articulation root has one enabled world fixed joint. - The base implementation leaves the root in place. Backends whose parser requires a different - root topology may relocate it and return the resulting root prim. + An articulation rooted at a fixed joint that attaches it to the world, a layout that UsdPhysics allows, is + fixed by that joint. The base implementation leaves the root in place. Backends whose parser requires a + different root topology may relocate it and return the resulting root prim. Args: articulation_prim: The articulation-root prim to fix. diff --git a/source/isaaclab/isaaclab/sim/schemas/schemas.py b/source/isaaclab/isaaclab/sim/schemas/schemas.py index 50fa16ede3cf..bfa77f54ec6f 100644 --- a/source/isaaclab/isaaclab/sim/schemas/schemas.py +++ b/source/isaaclab/isaaclab/sim/schemas/schemas.py @@ -267,6 +267,16 @@ def apply_namespaced(cfg: schemas_cfg.SchemaFragment, prim_path: str, stage: Usd """ +def _warn_fix_root_link_false_unsupported(root_path: str) -> None: + """Warn that ``fix_root_link=False`` is not supported for an articulation rooted at its fixed world joint.""" + logger.warning( + "fix_root_link=False is not supported for articulation '%s', which is rooted at the fixed joint that attaches" + " it to the world. To make it floating, author the asset without that joint, or convert it with" + " fix_base=False.", + root_path, + ) + + def apply_articulation_root_properties( prim_path_expr: str, fragments: Iterable[schemas_cfg.ArticulationRootFragment], @@ -357,7 +367,9 @@ def apply_articulation_root_properties( root = sim.physics_manager.fix_articulation_root(root, stage) elif fix_root_link is False: joint = find_global_fixed_joint_prim(root.GetPath().pathString, stage=stage) - if joint is not None: + if joint is not None and joint.GetPrim() == root: + _warn_fix_root_link_false_unsupported(root.GetPath().pathString) + elif joint is not None: joint.GetJointEnabledAttr().Set(False) root_path = root.GetPath().pathString @@ -519,7 +531,12 @@ def modify_articulation_root_properties( if fix_root_link is not None: existing_fixed_joint_prim = find_global_fixed_joint_prim(prim_path, stage=stage) # enable/disable an existing world joint, otherwise create one - if existing_fixed_joint_prim is not None: + root_is_joint = ( + existing_fixed_joint_prim is not None and existing_fixed_joint_prim.GetPrim() == articulation_prim + ) + if root_is_joint and not fix_root_link: + _warn_fix_root_link_false_unsupported(prim_path) + elif existing_fixed_joint_prim is not None: logger.info( f"Found an existing fixed joint for the articulation: '{prim_path}'. Setting it to: {fix_root_link}." ) diff --git a/source/isaaclab/isaaclab/sim/utils/queries.py b/source/isaaclab/isaaclab/sim/utils/queries.py index 70c6e43d6eba..e1bf0889ec28 100644 --- a/source/isaaclab/isaaclab/sim/utils/queries.py +++ b/source/isaaclab/isaaclab/sim/utils/queries.py @@ -510,8 +510,10 @@ def find_global_fixed_joint_prim( between the two bodies. When a fixed joint has only one target body, it is considered to attach the body to the simulation world. - This function finds the fixed joint prim that has only one target under the specified prim path. If no such - fixed joint prim exists, it returns None. + If the prim itself is a fixed joint that attaches a rigid body to the world, as for an articulation rooted at + its fixed world joint, that joint is returned (see :func:`_is_world_fixed_joint`). Otherwise, this function + finds the joint prim that has only one target under the specified prim path. If no such joint prim exists, it + returns None. Args: prim_path: The prim path to search for the fixed joint prim. @@ -520,7 +522,7 @@ def find_global_fixed_joint_prim( stage: The stage where the prim exists. Defaults to None, in which case the current stage is used. Returns: - The fixed joint prim that has only one target. If no such fixed joint prim exists, it returns None. + The fixed joint prim that attaches the prim to the world. If no such joint prim exists, it returns None. Raises: ValueError: If the prim path is not global (i.e: does not start with '/'). @@ -539,6 +541,12 @@ def find_global_fixed_joint_prim( if not prim.IsValid(): raise ValueError(f"Prim at path '{prim_path}' is not valid.") + # an articulation rooted at its fixed world joint: the prim itself is that joint + if _is_world_fixed_joint(prim): + joint_prim = UsdPhysics.Joint(prim) + if not check_enabled_only or joint_prim.GetJointEnabledAttr().Get(): + return joint_prim + # note: some assets author fixed joints with the generic "Joint" schema, so every joint is checked # and a joint missing one of its two body targets is taken to attach the asset to the world. for child_prim in Usd.PrimRange(prim): @@ -552,6 +560,43 @@ def find_global_fixed_joint_prim( return None +def _is_world_fixed_joint(prim: Usd.Prim) -> bool: + """Check whether a prim is a fixed joint that attaches a rigid body to the simulation world. + + One body target of the joint must be a rigid body or a prim under one. The other side counts as the world when + its body target is empty or names no prim, or when neither the target nor an ancestor is a rigid body or a + collider. A collider without a rigid-body ancestor is a static body, not the world. + :func:`find_global_fixed_joint_prim` applies this rule to the prim it is called on; for the joints under that + prim it treats only an empty body target as the world. + + Args: + prim: The USD prim to check. + + Returns: + True if the prim is a fixed joint between a rigid body and the world. + """ + from pxr import UsdPhysics # noqa: PLC0415 + + # a plain UsdPhysics.Joint is a free D6 joint, so it is excluded on purpose, unlike in the subtree search + if not prim.IsA(UsdPhysics.FixedJoint): + return False + joint = UsdPhysics.FixedJoint(prim) + stage = prim.GetStage() + + def resolve(rel: Usd.Relationship) -> str: + """Resolve a body relationship of the joint to ``"body"``, ``"static"`` or ``"world"``.""" + targets = rel.GetTargets() + if not targets or not stage.GetPrimAtPath(targets[0]): + return "world" + if get_first_matching_ancestor_prim(targets[0], lambda p: p.HasAPI(UsdPhysics.RigidBodyAPI), stage=stage): + return "body" + if get_first_matching_ancestor_prim(targets[0], lambda p: p.HasAPI(UsdPhysics.CollisionAPI), stage=stage): + return "static" + return "world" + + return sorted((resolve(joint.GetBody0Rel()), resolve(joint.GetBody1Rel()))) == ["body", "world"] + + def has_deformable_body_api(prim: Usd.Prim) -> bool: """Check whether a deformable body API schema is applied on the prim. diff --git a/source/isaaclab/test/sim/test_schemas.py b/source/isaaclab/test/sim/test_schemas.py index 4aff5bf130c4..98a4b29d3541 100644 --- a/source/isaaclab/test/sim/test_schemas.py +++ b/source/isaaclab/test/sim/test_schemas.py @@ -412,6 +412,94 @@ def test_modify_articulation_root_fix_root_link_uses_given_stage(setup_simulatio assert joint.GetJointEnabledAttr().Get() is False +@pytest.mark.parametrize("legacy_writer", [False, True], ids=["fragments", "legacy"]) +def test_fix_root_link_enables_fixed_world_joint_root(setup_simulation, legacy_writer): + """``fix_root_link=True`` enables the fixed world joint that roots an articulation instead of adding one.""" + stage = sim_utils.get_current_stage() + sim_utils.create_prim("/World/Robot", prim_type="Xform") + UsdPhysics.RigidBodyAPI.Apply(sim_utils.create_prim("/World/Robot/base", prim_type="Xform")) + joint = UsdPhysics.FixedJoint.Define(stage, "/World/Robot/root_joint") + joint.CreateBody0Rel().SetTargets(["/World/Robot"]) + joint.CreateBody1Rel().SetTargets(["/World/Robot/base"]) + joint.CreateJointEnabledAttr(False) + UsdPhysics.ArticulationRootAPI.Apply(joint.GetPrim()) + + if legacy_writer: + with pytest.warns(DeprecationWarning, match="modify_articulation_root_properties"): + schemas.modify_articulation_root_properties( + "/World/Robot/root_joint", schemas.ArticulationRootBaseCfg(fix_root_link=True) + ) + else: + schemas.apply_articulation_root_properties("/World/Robot/root_joint", [], fix_root_link=True) + + assert joint.GetJointEnabledAttr().Get() is True + robot = stage.GetPrimAtPath("/World/Robot") + assert [prim.GetName() for prim in Usd.PrimRange(robot) if prim.IsA(UsdPhysics.Joint)] == ["root_joint"] + roots = [prim.GetName() for prim in Usd.PrimRange(robot) if prim.HasAPI(UsdPhysics.ArticulationRootAPI)] + assert roots == ["root_joint"] + + +@pytest.mark.parametrize( + "body0, body1", + [ + ("/World/Robot/base", "/World/Robot/link"), + ("/World/Robot/static_base", "/World/Robot/link"), + ("/World/Robot", "/World/Robot"), + ("/World/Robot", "/World/Robot/static_base"), + ], + ids=["rigid_base", "static_collider_base", "no_rigid_body", "world_static_collider"], +) +def test_fix_root_link_rejects_non_world_fixed_joint_root(setup_simulation, body0, body1): + """``fix_root_link=True`` still raises for a fixed joint root between two bodies, a body and a static collider, + or two prims that are not rigid bodies.""" + stage = sim_utils.get_current_stage() + sim_utils.create_prim("/World/Robot", prim_type="Xform") + UsdPhysics.RigidBodyAPI.Apply(sim_utils.create_prim("/World/Robot/base", prim_type="Xform")) + UsdPhysics.CollisionAPI.Apply(sim_utils.create_prim("/World/Robot/static_base", prim_type="Cube")) + UsdPhysics.RigidBodyAPI.Apply(sim_utils.create_prim("/World/Robot/link", prim_type="Xform")) + joint = UsdPhysics.FixedJoint.Define(stage, "/World/Robot/joint") + joint.CreateBody0Rel().SetTargets([body0]) + joint.CreateBody1Rel().SetTargets([body1]) + UsdPhysics.ArticulationRootAPI.Apply(joint.GetPrim()) + + with pytest.raises(NotImplementedError): + schemas.apply_articulation_root_properties("/World/Robot/joint", [], fix_root_link=True) + with pytest.warns(DeprecationWarning, match="modify_articulation_root_properties"): + with pytest.raises(NotImplementedError): + schemas.modify_articulation_root_properties( + "/World/Robot/joint", schemas.ArticulationRootBaseCfg(fix_root_link=True) + ) + + +@pytest.mark.parametrize( + "body0, legacy_writer", + [(["/World/Robot"], False), ([], True)], + ids=["asset_prim_body0-fragments", "empty_body0-legacy"], +) +def test_fix_root_link_false_warns_for_fixed_world_joint_root(setup_simulation, caplog, body0, legacy_writer): + """``fix_root_link=False`` warns that it is not supported for an articulation rooted at its fixed world joint + and leaves that joint enabled.""" + stage = sim_utils.get_current_stage() + sim_utils.create_prim("/World/Robot", prim_type="Xform") + UsdPhysics.RigidBodyAPI.Apply(sim_utils.create_prim("/World/Robot/base", prim_type="Xform")) + joint = UsdPhysics.FixedJoint.Define(stage, "/World/Robot/root_joint") + joint.CreateBody0Rel().SetTargets(body0) + joint.CreateBody1Rel().SetTargets(["/World/Robot/base"]) + UsdPhysics.ArticulationRootAPI.Apply(joint.GetPrim()) + + with caplog.at_level("WARNING"): + if legacy_writer: + with pytest.warns(DeprecationWarning, match="modify_articulation_root_properties"): + schemas.modify_articulation_root_properties( + "/World/Robot/root_joint", schemas.ArticulationRootBaseCfg(fix_root_link=False) + ) + else: + schemas.apply_articulation_root_properties("/World/Robot/root_joint", [], fix_root_link=False) + + assert "fix_root_link=False is not supported for articulation '/World/Robot/root_joint'" in caplog.text + assert joint.GetJointEnabledAttr().Get() is True + + @pytest.mark.isaacsim_ci def test_physx_articulation_root_writes_self_collisions(setup_simulation): """Setting ``enabled_self_collisions`` on ``PhysxArticulationRootPropertiesCfg`` must author diff --git a/source/isaaclab_newton/changelog.d/fix-fixed-base-articulation-root.skip b/source/isaaclab_newton/changelog.d/fix-fixed-base-articulation-root.skip new file mode 100644 index 000000000000..e69de29bb2d1 diff --git a/source/isaaclab_newton/test/assets/test_articulation.py b/source/isaaclab_newton/test/assets/test_articulation.py index 56a778ced34f..899769e9af3c 100644 --- a/source/isaaclab_newton/test/assets/test_articulation.py +++ b/source/isaaclab_newton/test/assets/test_articulation.py @@ -15,9 +15,9 @@ rest and restores the properties it changes. Scene gravity is off; a test that needs gravity applies it to every world for its own duration. -Configurations that fail initialization, nested-root IK, the rebind scenario, which swaps the live Newton -state, and the CUDA-graph check build their own small scenes. Only one simulation context can be alive, so -these tests come first and fail if selected after a composite-scene test. +Configurations that fail initialization, an articulation rooted at its fixed world joint, nested-root IK, the rebind +scenario, which swaps the live Newton state, and the CUDA-graph check build their own small scenes. Only one +simulation context can be alive, so these tests come first and fail if selected after a composite-scene test. The composite scene runs on the CPU without CUDA-graph capture. On CUDA, one own-scene test covers the ordered-state republish recorded into a captured graph. @@ -63,7 +63,7 @@ world_gravity, ) -from pxr import UsdPhysics +from pxr import Usd, UsdGeom, UsdPhysics import isaaclab.assets.articulation.ordering_resolvers as ordering_resolvers import isaaclab.sim as sim_utils @@ -585,6 +585,42 @@ def test_invalid_articulation_cfg_fails_initialization(device: str, invalid_fiel sim.reset() +def _author_fixed_joint_root_robot(usd_path: str) -> None: + """Author a two-link robot rooted at the fixed joint that attaches its base to the asset prim.""" + stage = Usd.Stage.CreateNew(usd_path) + stage.SetDefaultPrim(UsdGeom.Xform.Define(stage, "/Robot").GetPrim()) + for name in ("base", "link"): + body = UsdGeom.Xform.Define(stage, f"/Robot/{name}").GetPrim() + UsdPhysics.RigidBodyAPI.Apply(body) + UsdPhysics.MassAPI.Apply(body).CreateMassAttr(1.0) + # body0 targets the asset prim, neither a rigid body nor a collider, so the joint attaches the base to the world + root_joint = UsdPhysics.FixedJoint.Define(stage, "/Robot/root_joint") + root_joint.CreateBody0Rel().SetTargets(["/Robot"]) + root_joint.CreateBody1Rel().SetTargets(["/Robot/base"]) + UsdPhysics.ArticulationRootAPI.Apply(root_joint.GetPrim()) + joint = UsdPhysics.RevoluteJoint.Define(stage, "/Robot/joint") + joint.CreateBody0Rel().SetTargets(["/Robot/base"]) + joint.CreateBody1Rel().SetTargets(["/Robot/link"]) + stage.Save() + + +@pytest.mark.parametrize("device", test_devices(DeviceScope.CPU)) +def test_initialization_fixed_joint_root(device, tmp_path) -> None: + """``fix_root_link=True`` keeps an articulation rooted at its fixed world joint fixed-base.""" + usd_path = str(tmp_path / "fixed_joint_root.usda") + _author_fixed_joint_root_robot(usd_path) + articulation_cfg = ArticulationCfg( + prim_path="/World/Env_[^/]*/Robot", + spawn=sim_utils.UsdFileCfg(usd_path=usd_path, fix_root_link=True), + actuators={}, + ) + with build_simulation_context(sim_cfg=newton_sim_cfg(device)) as sim: + articulation = spawn_assets({"robot": articulation_cfg}, num_envs=1)["robot"] + sim.reset() + + assert articulation.is_fixed_base + + @pytest.mark.parametrize("device", test_devices(DeviceScope.CPU)) def test_hard_reset_recreates_ordered_state_and_actuators(device: str) -> None: """Hard resets rebuild ordered state and actuators once, preserving configured gains and live writes.""" diff --git a/source/isaaclab_physx/changelog.d/fix-fixed-base-articulation-root.skip b/source/isaaclab_physx/changelog.d/fix-fixed-base-articulation-root.skip new file mode 100644 index 000000000000..e69de29bb2d1 diff --git a/source/isaaclab_physx/test/assets/test_articulation.py b/source/isaaclab_physx/test/assets/test_articulation.py index 2e86635bdc3e..8af85cb89fc6 100644 --- a/source/isaaclab_physx/test/assets/test_articulation.py +++ b/source/isaaclab_physx/test/assets/test_articulation.py @@ -40,7 +40,7 @@ from isaaclab_physx.assets import Articulation from isaaclab_physx.assets.articulation.kernels import write_joint_state_data, write_joint_state_data_kernel -from pxr import Gf, PhysxSchema, UsdGeom, UsdPhysics +from pxr import Gf, PhysxSchema, Usd, UsdGeom, UsdPhysics import isaaclab.sim as sim_utils from isaaclab.actuators import ImplicitActuatorCfg @@ -281,6 +281,37 @@ def test_gravity_compensation_holds_static_equilibrium(sim, device) -> None: assert not torch.allclose(data.mass_matrix.torch, primed[1], atol=1e-4), f"{kwarg} write left a stale matrix" +def _author_fixed_joint_root_robot(usd_path: str) -> None: + """Author a two-link robot rooted at the fixed joint that attaches its base to the asset prim.""" + stage = Usd.Stage.CreateNew(usd_path) + stage.SetDefaultPrim(UsdGeom.Xform.Define(stage, "/Robot").GetPrim()) + for name in ("base", "link"): + body = UsdGeom.Xform.Define(stage, f"/Robot/{name}").GetPrim() + UsdPhysics.RigidBodyAPI.Apply(body) + UsdPhysics.MassAPI.Apply(body).CreateMassAttr(1.0) + # body0 targets the asset prim, neither a rigid body nor a collider, so the joint attaches the base to the world + root_joint = UsdPhysics.FixedJoint.Define(stage, "/Robot/root_joint") + root_joint.CreateBody0Rel().SetTargets(["/Robot"]) + root_joint.CreateBody1Rel().SetTargets(["/Robot/base"]) + UsdPhysics.ArticulationRootAPI.Apply(root_joint.GetPrim()) + joint = UsdPhysics.RevoluteJoint.Define(stage, "/Robot/joint") + joint.CreateBody0Rel().SetTargets(["/Robot/base"]) + joint.CreateBody1Rel().SetTargets(["/Robot/link"]) + stage.Save() + + +@pytest.mark.parametrize("device", test_devices(DeviceScope.CUDA)) +def test_initialization_fixed_joint_root(sim, device, tmp_path) -> None: + """``fix_root_link=True`` keeps an articulation rooted at its fixed world joint fixed-base.""" + usd_path = str(tmp_path / "fixed_joint_root.usda") + _author_fixed_joint_root_robot(usd_path) + spawn = sim_utils.UsdFileCfg(usd_path=usd_path, fix_root_link=True) + articulation = Articulation(ArticulationCfg(prim_path="/World/Robot", spawn=spawn, actuators={})) + sim.reset() + + assert articulation.is_fixed_base + + ## # Composite scene shared by the remaining tests. ##