diff --git a/.github/PULL_REQUEST_TEMPLATE.md b/.github/PULL_REQUEST_TEMPLATE.md index ee9fa4ebdc5e..c19d35fb7e79 100644 --- a/.github/PULL_REQUEST_TEMPLATE.md +++ b/.github/PULL_REQUEST_TEMPLATE.md @@ -47,7 +47,7 @@ To upload images to a PR -- simply drag and drop an image while in edit mode and - [ ] I have made corresponding changes to the documentation - [ ] My changes generate no new warnings - [ ] I have added tests that prove my fix is effective or that my feature works -- [ ] I have updated the changelog and the corresponding version in the extension's `config/extension.toml` file +- [ ] I have added a changelog fragment under `source//changelog.d/` for every touched package (do **not** edit `CHANGELOG.rst` or bump `extension.toml` — CI handles that) - [ ] I have added my name to the `CONTRIBUTORS.md` or my name already exists there deg / s - cfg["max_velocity"] = cfg["max_velocity"] * 180.0 / math.pi - if cfg["stiffness"] is not None: + if cfg_dict.get("max_joint_velocity") is not None: + # rad / s --> deg / s (PhysX angular convention is degrees) + cfg_dict["max_joint_velocity"] = cfg_dict["max_joint_velocity"] * 180.0 / math.pi + if cfg_dict["stiffness"] is not None: # N-m/rad --> N-m/deg - cfg["stiffness"] = cfg["stiffness"] * math.pi / 180.0 - if cfg["damping"] is not None: + cfg_dict["stiffness"] = cfg_dict["stiffness"] * math.pi / 180.0 + if cfg_dict["damping"] is not None: # N-m-s/rad --> N-m-s/deg - cfg["damping"] = cfg["damping"] * math.pi / 180.0 - - # set into PhysX API (prim attributes under physxJoint:*) - for attr_name in ["max_velocity"]: - value = cfg.pop(attr_name, None) - usd_attr_name = cfg_to_usd_map[attr_name] - safe_set_attribute_on_usd_prim( - prim, f"physxJoint:{to_camel_case(usd_attr_name, 'cC')}", value, camel_case=False - ) - # set into USD API - for attr_name, attr_value in cfg.items(): - attr_name = cfg_to_usd_map.get(attr_name, attr_name) - safe_set_attribute_on_usd_schema(usd_drive_api, attr_name, attr_value, camel_case=True) + cfg_dict["damping"] = cfg_dict["damping"] * math.pi / 180.0 + + # set into USD API (solver-common properties; UsdPhysics.DriveAPI fields). Pop only + # the solver-common fields here; the helper handles the PhysX-namespaced remainder. + for attr_name in ["drive_type", "max_force", "stiffness", "damping"]: + if attr_name not in cfg_dict: + continue + attr_value = cfg_dict.pop(attr_name) + usd_attr_name = "type" if attr_name == "drive_type" else attr_name + safe_set_attribute_on_usd_schema(usd_drive_api, usd_attr_name, attr_value, camel_case=True) + + # apply per-field exceptions (max_velocity -> physxJoint:maxJointVelocity) + any + # PhysX-subclass main-namespace writes + _apply_namespaced_schemas(prim, cfg, cfg_dict) return True @@ -730,7 +842,7 @@ def modify_joint_drive_properties( @apply_nested def modify_fixed_tendon_properties( - prim_path: str, cfg: schemas_cfg.FixedTendonPropertiesCfg, stage: Usd.Stage | None = None + prim_path: str, cfg: schemas_cfg.PhysxFixedTendonPropertiesCfg, stage: Usd.Stage | None = None ) -> bool: """Modify PhysX parameters for a fixed tendon attachment prim. @@ -794,7 +906,7 @@ def modify_fixed_tendon_properties( @apply_nested def modify_spatial_tendon_properties( - prim_path: str, cfg: schemas_cfg.SpatialTendonPropertiesCfg, stage: Usd.Stage | None = None + prim_path: str, cfg: schemas_cfg.PhysxSpatialTendonPropertiesCfg, stage: Usd.Stage | None = None ) -> bool: """Modify PhysX parameters for a spatial tendon attachment prim. @@ -858,75 +970,19 @@ def modify_spatial_tendon_properties( """ -def _get_physx_collision_namespace(schema_name: str) -> str: - """Convert PhysX schema name to attribute namespace used on the prim.""" - if not schema_name: - raise ValueError("PhysX schema name must be provided for mesh collision properties.") - schema_name = schema_name.removesuffix("API") - return schema_name[0].lower() + schema_name[1:] - - -def _get_usd_mesh_collision_api(api_name: str): - """Resolve the USD mesh collision API from a string name.""" - if not api_name: - raise ValueError("USD schema name must be provided for mesh collision properties.") - usd_api = getattr(UsdPhysics, api_name, None) - if usd_api is None: - raise ValueError(f"USD schema '{api_name}' not found in UsdPhysics.") - return usd_api - - -def extract_mesh_collision_api_and_attrs( - cfg: schemas_cfg.MeshCollisionPropertiesCfg, -) -> tuple[tuple[str, Any], dict[str, Any]]: - """Extract the mesh collision API type/value and custom attributes from the configuration. - - Args: - cfg: The configuration for the mesh collision properties. - - Returns: - A tuple of ((api_type, api_value), custom_attrs). api_type is "usd" or "physx"; - api_value is the USD API class (callable) or PhysX schema name string. - - Raises: - ValueError: When neither USD nor PhysX API can be determined to be used. - """ - custom_attrs = { - key: value - for key, value in cfg.to_dict().items() - if value is not None and key not in ["usd_api", "physx_api", "mesh_approximation_name"] - } - - use_usd_api = False - use_physx_api = False - - if len(custom_attrs) > 0 and type(cfg) in PHYSX_MESH_COLLISION_CFGS: - use_physx_api = True - elif len(custom_attrs) == 0: - if type(cfg) in USD_MESH_COLLISION_CFGS: - use_usd_api = True - else: - use_physx_api = True - elif len(custom_attrs) > 0 and type(cfg) in USD_MESH_COLLISION_CFGS: - raise ValueError("Args are specified but the USD Mesh API doesn't support them!") - - if use_usd_api and getattr(cfg, "usd_api", None): - return ("usd", cfg.usd_api), custom_attrs - if use_physx_api and getattr(cfg, "physx_api", None): - return ("physx", cfg.physx_api), custom_attrs - raise ValueError("Either USD or PhysX API should be used for modifying mesh collision attributes!") - - def define_mesh_collision_properties( - prim_path: str, cfg: schemas_cfg.MeshCollisionPropertiesCfg, stage: Usd.Stage | None = None + prim_path: str, cfg: schemas_cfg.MeshCollisionBaseCfg, stage: Usd.Stage | None = None ): """Apply the mesh collision schema on the input prim and set its properties. - See :func:`modify_collision_mesh_properties` for more details on how the properties are set. + + See :func:`modify_mesh_collision_properties` for more details on how the properties are set. + Args: - prim_path : The prim path where to apply the mesh collision schema. - cfg : The configuration for the mesh collision properties. - stage : The stage where to find the prim. Defaults to None, in which case the + prim_path: The prim path where to apply the mesh collision schema. + cfg: The configuration for the mesh collision properties. + stage: The stage where to find the prim. Defaults to None, in which case the current stage is used. + Raises: ValueError: When the prim path is not valid. """ @@ -939,36 +995,43 @@ def define_mesh_collision_properties( if not prim.IsValid(): raise ValueError(f"Prim path '{prim_path}' is not valid.") - (api_type, api_value), _ = extract_mesh_collision_api_and_attrs(cfg=cfg) - - if api_type == "usd": - usd_api_class = _get_usd_mesh_collision_api(api_value) - if not usd_api_class(prim): - usd_api_class.Apply(prim) - else: - if api_value not in prim.GetAppliedSchemas(): - prim.AddAppliedSchema(api_value) + # Always apply the standard ``UsdPhysics.MeshCollisionAPI`` so the approximation token is + # writable. The PhysX cooking schema (if any) is applied lazily by the writer below + # only when the user authored at least one PhysX-namespaced tuning field. + if not UsdPhysics.MeshCollisionAPI(prim): + UsdPhysics.MeshCollisionAPI.Apply(prim) modify_mesh_collision_properties(prim_path=prim_path, cfg=cfg, stage=stage) @apply_nested def modify_mesh_collision_properties( - prim_path: str, cfg: schemas_cfg.MeshCollisionPropertiesCfg, stage: Usd.Stage | None = None + prim_path: str, cfg: schemas_cfg.MeshCollisionBaseCfg, stage: Usd.Stage | None = None ) -> bool: """Set properties for the mesh collision of a prim. - These properties are based on either the `Phsyx the `UsdPhysics.MeshCollisionAPI` schema. + + Metadata-driven writer. The standard ``UsdPhysics.MeshCollisionAPI`` is applied + unconditionally (it is the carrier of the ``physics:approximation`` token). The + PhysX cooking schema declared by ``_usd_applied_schema`` (e.g. + ``PhysxConvexHullCollisionAPI``) is gated on the user authoring at least one + non-``None`` namespaced tuning field, mirroring the gating used by the other + consumption-gated writers (rigid body, joint drive, collision, articulation root). + .. note:: - This function is decorated with :func:`apply_nested` that sets the properties to all the prims - (that have the schema applied on them) under the input prim path. - .. UsdPhysics.MeshCollisionAPI: https://openusd.org/release/api/class_usd_physics_mesh_collision_a_p_i.html + This function is decorated with :func:`apply_nested` that sets the properties to + all the prims (that have the schema applied on them) under the input prim path. + + .. _UsdPhysics.MeshCollisionAPI: https://openusd.org/release/api/class_usd_physics_mesh_collision_a_p_i.html + Args: - prim_path : The prim path of the rigid body. This prim should be a Mesh prim. - cfg : The configuration for the mesh collision properties. - stage : The stage where to find the prim. Defaults to None, in which case the + prim_path: The prim path of the rigid body. This prim should be a Mesh prim. + cfg: The configuration for the mesh collision properties. + stage: The stage where to find the prim. Defaults to None, in which case the current stage is used. + Returns: True if the properties were successfully set, False otherwise. + Raises: ValueError: When the mesh approximation name is invalid. """ @@ -981,8 +1044,12 @@ def modify_mesh_collision_properties( # we need MeshCollisionAPI to set mesh collision approximation attribute if not UsdPhysics.MeshCollisionAPI(prim): UsdPhysics.MeshCollisionAPI.Apply(prim) - # convert mesh approximation string to token - approximation_name = cfg.mesh_approximation_name + + # convert to dict, filtering out class metadata (underscore-prefixed keys) + cfg_dict = {f.name: getattr(cfg, f.name) for f in dataclasses.fields(cfg)} + + # write the standard ``physics:approximation`` token via UsdPhysics.MeshCollisionAPI + approximation_name = cfg_dict.pop("mesh_approximation_name", "none") if approximation_name not in MESH_APPROXIMATION_TOKENS: raise ValueError( f"Invalid mesh approximation name: '{approximation_name}'. " @@ -993,23 +1060,14 @@ def modify_mesh_collision_properties( UsdPhysics.MeshCollisionAPI(prim), "Approximation", approximation_token, camel_case=False ) - (api_type, api_value), custom_attrs = extract_mesh_collision_api_and_attrs(cfg=cfg) - - if api_type == "usd": - usd_api_class = _get_usd_mesh_collision_api(api_value) - mesh_collision_api = usd_api_class(prim) - if not mesh_collision_api: - return False - for attr_name, value in custom_attrs.items(): - camel_case = attr_name != "Attribute" - safe_set_attribute_on_usd_schema(mesh_collision_api, attr_name, value, camel_case=camel_case) - else: - if api_value not in prim.GetAppliedSchemas(): - return False - attr_namespace = _get_physx_collision_namespace(api_value) - for attr_name, value in custom_attrs.items(): - attr_token = attr_name if attr_name == "Attribute" else to_camel_case(attr_name, "cC") - safe_set_attribute_on_usd_prim(prim, f"{attr_namespace}:{attr_token}", value, camel_case=False) + # The standard ``UsdPhysics.MeshCollisionAPI`` is already applied above. The base + # ``MeshCollisionBaseCfg`` declares ``_usd_applied_schema = "MeshCollisionAPI"`` so the + # helper would re-apply (idempotent) if any base-namespace write fired. PhysX cooking + # subclasses (ConvexHull / TriangleMesh / SDF / ...) override the schema and namespace + # to author their tuning fields under e.g. ``physxConvexHullCollision:*``; the helper + # gates ``Physx*CollisionAPI`` application on at least one non-None tuning field, so + # Newton-targeted prims stay free of PhysX cooking schemas they did not opt in to. + _apply_namespaced_schemas(prim, cfg, cfg_dict) # success return True diff --git a/source/isaaclab/isaaclab/sim/schemas/schemas_cfg.py b/source/isaaclab/isaaclab/sim/schemas/schemas_cfg.py index 69cbc8bd5304..e54d71ca9d76 100644 --- a/source/isaaclab/isaaclab/sim/schemas/schemas_cfg.py +++ b/source/isaaclab/isaaclab/sim/schemas/schemas_cfg.py @@ -5,14 +5,119 @@ from __future__ import annotations -from typing import Literal +import warnings +from typing import ClassVar, Literal from isaaclab.utils import configclass +# Names that moved out of this submodule into ``isaaclab_physx.sim.schemas.schemas_cfg``. +# Resolved lazily so callers using ``from isaaclab.sim.schemas.schemas_cfg import +# RigidBodyPropertiesCfg`` continue to work without importing ``isaaclab_physx`` at module +# load time. +_PHYSX_FORWARDS = frozenset( + { + "RigidBodyPropertiesCfg", + "JointDrivePropertiesCfg", + "PhysxRigidBodyPropertiesCfg", + "PhysxJointDrivePropertiesCfg", + "CollisionPropertiesCfg", + "PhysxCollisionPropertiesCfg", + "PhysXCollisionPropertiesCfg", + "PhysxDeformableCollisionPropertiesCfg", + "ArticulationRootPropertiesCfg", + "PhysxArticulationRootPropertiesCfg", + "MeshCollisionPropertiesCfg", + "ConvexHullPropertiesCfg", + "ConvexDecompositionPropertiesCfg", + "TriangleMeshPropertiesCfg", + "TriangleMeshSimplificationPropertiesCfg", + "SDFMeshPropertiesCfg", + "PhysxConvexHullPropertiesCfg", + "PhysxConvexDecompositionPropertiesCfg", + "PhysxTriangleMeshPropertiesCfg", + "PhysxTriangleMeshSimplificationPropertiesCfg", + "PhysxSDFMeshPropertiesCfg", + "FixedTendonPropertiesCfg", + "SpatialTendonPropertiesCfg", + "PhysxFixedTendonPropertiesCfg", + "PhysxSpatialTendonPropertiesCfg", + } +) + +_NEWTON_FORWARDS = frozenset( + { + "MujocoRigidBodyPropertiesCfg", + "MujocoJointDrivePropertiesCfg", + "NewtonRigidBodyPropertiesCfg", + "NewtonJointDrivePropertiesCfg", + "NewtonCollisionPropertiesCfg", + "NewtonMeshCollisionPropertiesCfg", + "NewtonMaterialPropertiesCfg", + "NewtonArticulationRootPropertiesCfg", + } +) + + +def __getattr__(name): + if name in _PHYSX_FORWARDS: + try: + from isaaclab_physx.sim.schemas import schemas_cfg as _physx_cfg + except ImportError as e: + raise ImportError( + f"'isaaclab.sim.schemas.schemas_cfg.{name}' has moved to" + " 'isaaclab_physx.sim.schemas.schemas_cfg'. Install the isaaclab_physx" + " extension or update your import. This forwarding shim is scheduled for" + " removal in 4.0." + ) from e + return getattr(_physx_cfg, name) + if name in _NEWTON_FORWARDS: + try: + from isaaclab_newton.sim.schemas import schemas_cfg as _newton_cfg + except ImportError as e: + raise ImportError( + f"'isaaclab.sim.schemas.schemas_cfg.{name}' has moved to" + " 'isaaclab_newton.sim.schemas.schemas_cfg'. Install the isaaclab_newton" + " extension or update your import. This forwarding shim is scheduled for" + " removal in 4.0." + ) from e + return getattr(_newton_cfg, name) + raise AttributeError(f"module 'isaaclab.sim.schemas.schemas_cfg' has no attribute {name!r}") + + +def _deprecate_field_alias(cfg, alias: str, canonical: str) -> None: + """Forward a deprecated cfg field to its canonical replacement. + + If ``alias`` is set on the cfg instance, emit a ``DeprecationWarning`` and copy the + value to ``canonical`` (when ``canonical`` is unset). The alias is then nulled so + downstream metadata-driven writers see only the canonical name. + """ + value = getattr(cfg, alias, None) + if value is None: + return + warnings.warn( + f"'{alias}' is deprecated; use '{canonical}' instead. The alias is scheduled for removal in 4.0.", + DeprecationWarning, + stacklevel=3, + ) + if getattr(cfg, canonical, None) is None: + setattr(cfg, canonical, value) + setattr(cfg, alias, None) + @configclass -class ArticulationRootPropertiesCfg: - """Properties to apply to the root of an articulation. +class ArticulationRootBaseCfg: + """Solver-common properties to apply to the root of an articulation. + + Carries :attr:`fix_root_link` (writer-side; materializes a + :class:`UsdPhysics.FixedJoint` between the world frame and the root link) and + :attr:`articulation_enabled` whose only USD path today is the PhysX-namespaced + ``physxArticulation:articulationEnabled`` attribute. The base class itself + declares no USD namespace; the writer consults :attr:`_usd_field_exceptions` + to route ``articulation_enabled`` to its non-base namespace and apply + ``PhysxArticulationAPI`` only when the user authored that one field. + For PhysX-only articulation-root properties (self-collisions, TGS solver + iterations, sleep / stabilization thresholds), use + :class:`~isaaclab_physx.sim.schemas.PhysxArticulationRootPropertiesCfg`. See :meth:`modify_articulation_root_properties` for more information. @@ -21,23 +126,36 @@ class ArticulationRootPropertiesCfg: the properties and leave the rest as-is. """ - articulation_enabled: bool | None = None - """Whether to enable or disable articulation.""" - - enabled_self_collisions: bool | None = None - """Whether to enable or disable self-collisions.""" + # -- Class metadata (not dataclass fields) -- + # No base-native namespace today: every field is either solver-common (typed + # UsdPhysics API) or routed through ``_usd_field_exceptions``. + _usd_namespace: ClassVar[str | None] = None + _usd_applied_schema: ClassVar[str | None] = None + # Per-field exceptions: applied_schema -> (namespace, [cfg_field, ...]). The USD + # attribute name is the auto snake -> camelCase of the cfg field name (project + # convention). When any listed field is non-None at write time, the writer applies + # the schema and writes the attribute under the exception namespace. + _usd_field_exceptions: ClassVar[dict] = { + "PhysxArticulationAPI": ("physxArticulation", ["articulation_enabled"]), + } - solver_position_iteration_count: int | None = None - """Solver position iteration counts for the body.""" + articulation_enabled: bool | None = None + """Whether to enable or disable the articulation. - solver_velocity_iteration_count: int | None = None - """Solver velocity iteration counts for the body.""" + PhysX honors this per-articulation at sim time via + ``physxArticulation:articulationEnabled``: setting False makes PhysX skip + the articulation in its solver passes. - sleep_threshold: float | None = None - """Mass-normalized kinetic energy threshold below which an actor may go to sleep.""" + On Newton, the field is read by the IsaacLab Newton wrapper at spawn time + (``isaaclab_newton/assets/rigid_object/rigid_object.py:1035``) as a guard + against accidentally spawning a ``RigidObject`` over a prim that still has + ``ArticulationRootAPI`` applied; setting False suppresses the guard error. + The Newton solver itself does not consult the flag at sim time. - stabilization_threshold: float | None = None - """The mass-normalized kinetic energy threshold below which an articulation may participate in stabilization.""" + Placed on the solver-common class because the user-facing intent is + universal and both PhysX (sim-time) and the IL Newton wrapper (spawn-time) + honor it. + """ fix_root_link: bool | None = None """Whether to fix the root link of the articulation. @@ -54,16 +172,38 @@ class ArticulationRootPropertiesCfg: @configclass -class RigidBodyPropertiesCfg: - """Properties to apply to a rigid body. +class RigidBodyBaseCfg: + """Solver-common properties to apply to a rigid body. + + Contains properties from the `UsdPhysics.RigidBodyAPI`_ that are common across all + simulation backends, plus :attr:`disable_gravity` whose USD attribute today is + PhysX-namespaced but whose semantics (per-body gravity exclusion) are universal: + PhysX honors it per-body; Newton's importer consumes it at the scene level + (partial honor, documented on the field). For PhysX-only rigid-body properties, + use :class:`PhysxRigidBodyPropertiesCfg`. See :meth:`modify_rigid_body_properties` for more information. .. note:: If the values are None, they are not modified. This is useful when you want to set only a subset of the properties and leave the rest as-is. + + .. _UsdPhysics.RigidBodyAPI: https://openusd.org/dev/api/class_usd_physics_rigid_body_a_p_i.html """ + # -- Class metadata (not dataclass fields) -- + # ``rigid_body_enabled`` and ``kinematic_enabled`` write to ``physics:*`` (UsdPhysics + # standard attributes). The helper's per-declaring-class routing keeps these under + # the base namespace even when the cfg is a PhysX subclass instance. The + # ``UsdPhysics.RigidBodyAPI`` schema is applied upstream by ``define_rigid_body_properties`` + # so ``_usd_applied_schema`` here stays None. ``disable_gravity`` is routed via + # ``_usd_field_exceptions`` to ``physxRigidBody:disableGravity``. + _usd_namespace: ClassVar[str | None] = "physics" + _usd_applied_schema: ClassVar[str | None] = None + _usd_field_exceptions: ClassVar[dict] = { + "PhysxRigidBodyAPI": ("physxRigidBody", ["disable_gravity"]), + } + rigid_body_enabled: bool | None = None """Whether to enable or disable the rigid body.""" @@ -77,85 +217,88 @@ class RigidBodyPropertiesCfg: """ disable_gravity: bool | None = None - """Disable gravity for the actor.""" - - linear_damping: float | None = None - """Linear damping for the body.""" - - angular_damping: float | None = None - """Angular damping for the body.""" + """Disable gravity for the body. - max_linear_velocity: float | None = None - """Maximum linear velocity for rigid bodies (in m/s).""" + PhysX honors this per-body via ``physxRigidBody:disableGravity``: setting True + excludes the body from world gravity integration. - max_angular_velocity: float | None = None - """Maximum angular velocity for rigid bodies (in deg/s).""" + Newton currently consumes the same USD attribute at the **scene level** -- + Newton's importer reads ``physxRigidBody:disableGravity`` on the scene prim + and uses it to drive the scene-wide ``builder.gravity`` flag (``import_usd.py:1212``). + Per-body intent is therefore partially honored on Newton: whichever rigid body + has the attribute authored ends up controlling scene-wide gravity, and other + bodies cannot be selectively excluded. - max_depenetration_velocity: float | None = None - """Maximum depenetration velocity permitted to be introduced by the solver (in m/s).""" - - max_contact_impulse: float | None = None - """The limit on the impulse that may be applied at a contact.""" - - enable_gyroscopic_forces: bool | None = None - """Enables computation of gyroscopic forces on the rigid body.""" - - retain_accelerations: bool | None = None - """Carries over forces/accelerations over sub-steps.""" - - solver_position_iteration_count: int | None = None - """Solver position iteration counts for the body.""" - - solver_velocity_iteration_count: int | None = None - """Solver position iteration counts for the body.""" - - sleep_threshold: float | None = None - """Mass-normalized kinetic energy threshold below which an actor may go to sleep.""" - - stabilization_threshold: float | None = None - """The mass-normalized kinetic energy threshold below which an actor may participate in stabilization.""" + The field is placed on the base because the user-facing intent (per-body + gravity exclusion for markers, sensors, kinematic targets) is universal physics + and PhysX honors it fully. Closing the Newton gap is a kernel-level fix + (introduce ``Model.body_disable_gravity`` boolean array consumed by the + integrator) that does not require a cfg-API change. + """ @configclass -class CollisionPropertiesCfg: - """Properties to apply to colliders in a rigid body. +class CollisionBaseCfg: + """Solver-common properties to apply to colliders. + + Contains :attr:`collision_enabled` from the `UsdPhysics.CollisionAPI`_ and the + :attr:`contact_offset` / :attr:`rest_offset` knobs whose USD attributes today are + PhysX-namespaced (``physxCollision:contactOffset``, ``physxCollision:restOffset``) + but whose semantics (collision-pair generation distance, rest separation gap) are + universal physics: PhysX consumes them natively, Newton's importer consumes them + via the PhysX bridge resolver and populates ``Model.shape_collision_radius`` / + ``Model.shape_collision_thickness`` from the ``gap`` and ``margin`` keys (see + ``import_usd.py:2104, 2111``). For PhysX-only collision properties (e.g. torsional + patch friction), use :class:`~isaaclab_physx.sim.schemas.PhysxCollisionPropertiesCfg`. See :meth:`modify_collision_properties` for more information. .. note:: If the values are None, they are not modified. This is useful when you want to set only a subset of the properties and leave the rest as-is. + + .. _UsdPhysics.CollisionAPI: https://openusd.org/dev/api/class_usd_physics_collision_a_p_i.html """ + # -- Class metadata (not dataclass fields) -- + # ``collision_enabled`` writes to ``physics:collisionEnabled`` (UsdPhysics standard). + # The helper's per-declaring-class routing keeps it under ``physics:*`` even when + # the cfg is a PhysX subclass instance. ``contact_offset`` / ``rest_offset`` are + # routed via ``_usd_field_exceptions`` to ``physxCollision:*``. + _usd_namespace: ClassVar[str | None] = "physics" + _usd_applied_schema: ClassVar[str | None] = None + _usd_field_exceptions: ClassVar[dict] = { + "PhysxCollisionAPI": ("physxCollision", ["contact_offset", "rest_offset"]), + } + collision_enabled: bool | None = None - """Whether to enable or disable collisions.""" + """Whether to enable or disable collisions. + + Writes ``physics:collisionEnabled`` via :class:`UsdPhysics.CollisionAPI`. + """ contact_offset: float | None = None - """Contact offset for the collision shape (in m). + """Contact offset for the collision shape [m]. The collision detector generates contact points as soon as two shapes get closer than the sum of their contact offsets. This quantity should be non-negative which means that contact generation can potentially start before the shapes actually penetrate. + + Writes ``physxCollision:contactOffset``. Newton's USD importer consumes the same + attribute via its PhysX-bridge resolver. """ rest_offset: float | None = None - """Rest offset for the collision shape (in m). + """Rest offset for the collision shape [m]. The rest offset quantifies how close a shape gets to others at rest, At rest, the distance between two vertically stacked objects is the sum of their rest offsets. If a pair of shapes have a positive rest offset, the shapes will be separated at rest by an air gap. - """ - - torsional_patch_radius: float | None = None - """Radius of the contact patch for applying torsional friction (in m). - It is used to approximate rotational friction introduced by the compression of contacting surfaces. - If the radius is zero, no torsional friction is applied. + Writes ``physxCollision:restOffset``. Newton's USD importer consumes the same + attribute via its PhysX-bridge resolver. """ - min_torsional_patch_radius: float | None = None - """Minimum radius of the contact patch for applying torsional friction (in m).""" - @configclass class MassPropertiesCfg: @@ -168,6 +311,13 @@ class MassPropertiesCfg: the properties and leave the rest as-is. """ + # -- Class metadata (not dataclass fields) -- + # ``mass`` / ``density`` write to ``physics:*`` (UsdPhysics standard attributes). + # The ``UsdPhysics.MassAPI`` schema is applied upstream by ``define_mass_properties``. + _usd_namespace: ClassVar[str | None] = "physics" + _usd_applied_schema: ClassVar[str | None] = None + _usd_field_exceptions: ClassVar[dict] = {} + mass: float | None = None """The mass of the rigid body (in kg). @@ -184,16 +334,43 @@ class MassPropertiesCfg: @configclass -class JointDrivePropertiesCfg: - """Properties to define the drive mechanism of a joint. +class JointDriveBaseCfg: + """Solver-common properties to define the drive mechanism of a joint. + + Contains properties from the `UsdPhysics.DriveAPI`_ that are common across all + simulation backends, plus :attr:`max_joint_velocity` whose USD attribute today is + PhysX-namespaced but whose semantics (per-DOF velocity limit) are universal: + Newton's importer consumes ``physxJoint:maxJointVelocity`` and populates + ``Model.joint_velocity_limit``; PhysX consumes it natively. For PhysX-only + drive properties, use :class:`PhysxJointDrivePropertiesCfg`. See :meth:`modify_joint_drive_properties` for more information. .. note:: If the values are None, they are not modified. This is useful when you want to set only a subset of the properties and leave the rest as-is. + + .. _UsdPhysics.DriveAPI: https://openusd.org/dev/api/class_usd_physics_drive_a_p_i.html """ + # -- Class metadata (not dataclass fields) -- + # No base-native namespace today: drive-type / max-effort / stiffness / damping are + # written via the typed ``UsdPhysics.DriveAPI``; ``max_joint_velocity`` is routed + # through ``_usd_field_exceptions`` to ``physxJoint:maxJointVelocity`` (the only + # USD path to ``Model.joint_velocity_limit`` today). + _usd_namespace: ClassVar[str | None] = None + _usd_applied_schema: ClassVar[str | None] = None + _usd_field_exceptions: ClassVar[dict] = { + "PhysxJointAPI": ("physxJoint", ["max_joint_velocity"]), + } + + def __post_init__(self): + # Deprecation aliases: project convention is that python ``snake_case`` cfg field + # names map identity-style to USD ``camelCase`` attrs. Legacy short names that + # diverged are forwarded here. + _deprecate_field_alias(self, "max_velocity", "max_joint_velocity") + _deprecate_field_alias(self, "max_effort", "max_force") + drive_type: Literal["force", "acceleration"] | None = None """Joint drive type to apply. @@ -201,16 +378,20 @@ class JointDrivePropertiesCfg: then the joint is driven by an acceleration (usually used for kinematic joints). """ - max_effort: float | None = None - """Maximum effort that can be applied to the joint (in kg-m^2/s^2).""" + max_force: float | None = None + """Maximum force/torque that can be applied to the joint [N for linear joints, N-m for angular joints]. - max_velocity: float | None = None - """Maximum velocity of the joint. + Writes ``drive::physics:maxForce`` via :class:`UsdPhysics.DriveAPI`. + """ - The unit depends on the joint model: + max_effort: float | None = None + """Deprecated alias for :attr:`max_force`. - * For linear joints, the unit is m/s. - * For angular joints, the unit is rad/s. + .. deprecated:: 4.6.25 + Use :attr:`max_force` instead. The cfg field is renamed so its + snake_case name maps identity-style to the USD camelCase attribute + (``maxForce`` on ``UsdPhysics.DriveAPI``). The alias is forwarded to + :attr:`max_force` in :meth:`__post_init__` and will be removed in 4.0. """ stiffness: float | None = None @@ -243,307 +424,122 @@ class JointDrivePropertiesCfg: overridden later by the actuator model. """ + max_joint_velocity: float | None = None + """Maximum velocity of the joint [m/s for linear joints, rad/s for angular joints]. -@configclass -class FixedTendonPropertiesCfg: - """Properties to define fixed tendons of an articulation. - - See :meth:`modify_fixed_tendon_properties` for more information. - - .. note:: - If the values are None, they are not modified. This is useful when you want to set only a subset of - the properties and leave the rest as-is. + Notes: + Today this writes ``physxJoint:maxJointVelocity`` (a PhysX add-on schema attribute). + Newton's USD importer consumes the same attribute via its PhysX-bridge resolver and + populates ``Model.joint_velocity_limit``; the PhysX engine consumes it natively. The + Kamino solver honors the limit at the simulation step. The XPBD, Featherstone, and + Semi-implicit Newton solvers import the value but do not consume it in their kernels; + the MuJoCo (MJC) solver explicitly drops it. When Newton ships ``newton:maxJointVelocity`` + as a registered applied API, the writer namespace will switch transparently and this + docstring caveat will be removed. """ - tendon_enabled: bool | None = None - """Whether to enable or disable the tendon.""" - - stiffness: float | None = None - """Spring stiffness term acting on the tendon's length.""" - - damping: float | None = None - """The damping term acting on both the tendon length and the tendon-length limits.""" - - limit_stiffness: float | None = None - """Limit stiffness term acting on the tendon's length limits.""" - - offset: float | None = None - """Length offset term for the tendon. + max_velocity: float | None = None + """Deprecated alias for :attr:`max_joint_velocity`. - It defines an amount to be added to the accumulated length computed for the tendon. This allows the application - to actuate the tendon by shortening or lengthening it. + .. deprecated:: 4.6.25 + Use :attr:`max_joint_velocity` instead. The cfg field is renamed so its + snake_case name maps identity-style to the USD camelCase attribute + (``physxJoint:maxJointVelocity``). The alias is forwarded to + :attr:`max_joint_velocity` in :meth:`__post_init__` and will be removed in 4.0. """ - rest_length: float | None = None - """Spring rest length of the tendon.""" - @configclass -class SpatialTendonPropertiesCfg: - """Properties to define spatial tendons of an articulation. +class MeshCollisionBaseCfg: + """Solver-common properties to apply to a mesh in regards to collision. - See :meth:`modify_spatial_tendon_properties` for more information. - - .. note:: - If the values are None, they are not modified. This is useful when you want to set only a subset of - the properties and leave the rest as-is. - """ - - tendon_enabled: bool | None = None - """Whether to enable or disable the tendon.""" - - stiffness: float | None = None - """Spring stiffness term acting on the tendon's length.""" - - damping: float | None = None - """The damping term acting on both the tendon length and the tendon-length limits.""" + Carries only the standard ``UsdPhysics:MeshCollisionAPI`` token + (:attr:`mesh_approximation_name` -> ``physics:approximation``). For PhysX-cooking + tunables (convex hull / decomposition / triangle mesh / SDF), use the + ``Physx*PropertiesCfg`` subclasses in :mod:`isaaclab_physx.sim.schemas`. - limit_stiffness: float | None = None - """Limit stiffness term acting on the tendon's length limits.""" - - offset: float | None = None - """Length offset term for the tendon. - - It defines an amount to be added to the accumulated length computed for the tendon. This allows the application - to actuate the tendon by shortening or lengthening it. - """ - - -@configclass -class MeshCollisionPropertiesCfg: - """Properties to apply to a mesh in regards to collision. - See :meth:`set_mesh_collision_properties` for more information. + See :meth:`modify_mesh_collision_properties` for more information. .. note:: - If the values are None, they are not modified. This is useful when you want to set only a subset of - the properties and leave the rest as-is. + If the values are None, they are not modified. This is useful when you want to + set only a subset of the properties and leave the rest as-is. """ - usd_api: str | None = None - """USD API name for mesh collision (e.g. 'MeshCollisionAPI').""" - - physx_api: str | None = None - """PhysX schema name for mesh collision (e.g. 'PhysxConvexDecompositionCollisionAPI').""" + # -- Class metadata (not dataclass fields) -- + # The standard ``UsdPhysics.MeshCollisionAPI`` is always applied by the writer when a + # mesh-collision cfg is supplied; ``_usd_applied_schema`` here records the standard + # API name so subclasses that author no PhysX namespace can rely on the writer's + # standard-vs-PhysX gating logic. PhysX-cooking subclasses override this. + _usd_applied_schema: ClassVar[str | None] = "MeshCollisionAPI" + # Base class authors no PhysX-namespaced fields, so no namespace is defined. + _usd_namespace: ClassVar[str | None] = None + _usd_attr_name_map: ClassVar[dict] = {} + _usd_field_exceptions: ClassVar[dict] = {} mesh_approximation_name: str = "none" """Name of mesh collision approximation method. Default: "none". - Refer to :const:`schemas.MESH_APPROXIMATION_TOKENS` for available options. - """ - -@configclass -class BoundingCubePropertiesCfg(MeshCollisionPropertiesCfg): - usd_api: str = "MeshCollisionAPI" - """Original USD Documentation: - https://docs.omniverse.nvidia.com/kit/docs/omni_usd_schema_physics/latest/class_usd_physics_mesh_collision_a_p_i.html - """ - - mesh_approximation_name: str = "boundingCube" - """Name of mesh collision approximation method. Default: "boundingCube". + Writes ``physics:approximation`` via :class:`UsdPhysics.MeshCollisionAPI`. Refer to :const:`schemas.MESH_APPROXIMATION_TOKENS` for available options. """ - -@configclass -class BoundingSpherePropertiesCfg(MeshCollisionPropertiesCfg): - usd_api: str = "MeshCollisionAPI" - """Original USD Documentation: - https://docs.omniverse.nvidia.com/kit/docs/omni_usd_schema_physics/latest/class_usd_physics_mesh_collision_a_p_i.html - """ - - mesh_approximation_name: str = "boundingSphere" - """Name of mesh collision approximation method. Default: "boundingSphere". - Refer to :const:`schemas.MESH_APPROXIMATION_TOKENS` for available options. - """ + def __getattr__(self, name: str): + """Deprecated read-only access to the legacy ``usd_api`` / ``physx_api`` instance attrs. + + Falls back here only when the attribute is not found on the dataclass instance. + Returns the legacy-mapped string value derived from the class-level + ``_usd_applied_schema`` metadata and emits a ``DeprecationWarning``. + """ + if name == "usd_api": + warnings.warn( + "'usd_api' attribute is deprecated and will be removed in 4.0. Use class-level" + " metadata via getattr(cfg, '_usd_applied_schema').", + DeprecationWarning, + stacklevel=2, + ) + schema = self.__dict__.get("_usd_applied_schema", None) + # Every PhysX cooking subclass legacy-mapped to ``"MeshCollisionAPI"``; the base + # class also wrote that token. Return ``None`` only when no schema is declared. + return "MeshCollisionAPI" if schema is not None else None + if name == "physx_api": + warnings.warn( + "'physx_api' attribute is deprecated and will be removed in 4.0. Use class-level" + " metadata via getattr(cfg, '_usd_applied_schema').", + DeprecationWarning, + stacklevel=2, + ) + schema = self.__dict__.get("_usd_applied_schema", None) + if schema and schema.startswith("Physx"): + return schema + return None + raise AttributeError(f"{type(self).__name__!r} object has no attribute {name!r}") @configclass -class ConvexDecompositionPropertiesCfg(MeshCollisionPropertiesCfg): - usd_api: str = "MeshCollisionAPI" - """Original USD Documentation: - https://docs.omniverse.nvidia.com/kit/docs/omni_usd_schema_physics/latest/class_usd_physics_mesh_collision_a_p_i.html - """ - - physx_api: str = "PhysxConvexDecompositionCollisionAPI" - """Original PhysX Documentation: - https://docs.omniverse.nvidia.com/kit/docs/omni_usd_schema_physics/latest/class_physx_schema_physx_convex_decomposition_collision_a_p_i.html - """ - - mesh_approximation_name: str = "convexDecomposition" - """Name of mesh collision approximation method. Default: "convexDecomposition". - Refer to :const:`schemas.MESH_APPROXIMATION_TOKENS` for available options. - """ - - hull_vertex_limit: int | None = None - """Convex hull vertex limit used for convex hull cooking. - - Defaults to 64. - """ - max_convex_hulls: int | None = None - """Maximum of convex hulls created during convex decomposition. - Default value is 32. - """ - min_thickness: float | None = None - """Convex hull min thickness. - - Range: [0, inf). Units are distance. Default value is 0.001. - """ - voxel_resolution: int | None = None - """Voxel resolution used for convex decomposition. - - Defaults to 500,000 voxels. - """ - error_percentage: float | None = None - """Convex decomposition error percentage parameter. - - Defaults to 10 percent. Units are percent. - """ - shrink_wrap: bool | None = None - """Attempts to adjust the convex hull points so that they are projected onto the surface of the original graphics - mesh. - - Defaults to False. - """ +class BoundingCubePropertiesCfg(MeshCollisionBaseCfg): + """Bounding-cube mesh collision approximation. USD-only; authors no PhysX schema. + Writes the ``boundingCube`` token to ``physics:approximation`` via + :class:`UsdPhysics.MeshCollisionAPI`. -@configclass -class ConvexHullPropertiesCfg(MeshCollisionPropertiesCfg): - usd_api: str = "MeshCollisionAPI" - """Original USD Documentation: + Original USD Documentation: https://docs.omniverse.nvidia.com/kit/docs/omni_usd_schema_physics/latest/class_usd_physics_mesh_collision_a_p_i.html """ - physx_api: str = "PhysxConvexHullCollisionAPI" - """Original PhysX Documentation: - https://docs.omniverse.nvidia.com/kit/docs/omni_usd_schema_physics/latest/class_physx_schema_physx_convex_hull_collision_a_p_i.html - """ - - mesh_approximation_name: str = "convexHull" - """Name of mesh collision approximation method. Default: "convexHull". - Refer to :const:`schemas.MESH_APPROXIMATION_TOKENS` for available options. - """ - - hull_vertex_limit: int | None = None - """Convex hull vertex limit used for convex hull cooking. - - Defaults to 64. - """ - min_thickness: float | None = None - """Convex hull min thickness. - - Range: [0, inf). Units are distance. Default value is 0.001. - """ + mesh_approximation_name: str = "boundingCube" + """Name of mesh collision approximation method. Default: "boundingCube".""" @configclass -class TriangleMeshPropertiesCfg(MeshCollisionPropertiesCfg): - physx_api: str = "PhysxTriangleMeshCollisionAPI" - """Triangle mesh is only supported by PhysX API. +class BoundingSpherePropertiesCfg(MeshCollisionBaseCfg): + """Bounding-sphere mesh collision approximation. USD-only; authors no PhysX schema. - Original PhysX Documentation: - https://docs.omniverse.nvidia.com/kit/docs/omni_usd_schema_physics/latest/class_physx_schema_physx_triangle_mesh_collision_a_p_i.html - """ + Writes the ``boundingSphere`` token to ``physics:approximation`` via + :class:`UsdPhysics.MeshCollisionAPI`. - mesh_approximation_name: str = "none" - """Name of mesh collision approximation method. Default: "none" (uses triangle mesh). - Refer to :const:`schemas.MESH_APPROXIMATION_TOKENS` for available options. - """ - - weld_tolerance: float | None = None - """Mesh weld tolerance, controls the distance at which vertices are welded. - - Default -inf will autocompute the welding tolerance based on the mesh size. Zero value will disable welding. - Range: [0, inf) Units: distance - """ - - -@configclass -class TriangleMeshSimplificationPropertiesCfg(MeshCollisionPropertiesCfg): - usd_api: str = "MeshCollisionAPI" - """Original USD Documentation: + Original USD Documentation: https://docs.omniverse.nvidia.com/kit/docs/omni_usd_schema_physics/latest/class_usd_physics_mesh_collision_a_p_i.html """ - physx_api: str = "PhysxTriangleMeshSimplificationCollisionAPI" - """Original PhysX Documentation: - https://docs.omniverse.nvidia.com/kit/docs/omni_usd_schema_physics/latest/class_physx_schema_physx_triangle_mesh_simplification_collision_a_p_i.html - """ - - mesh_approximation_name: str = "meshSimplification" - """Name of mesh collision approximation method. Default: "meshSimplification". - Refer to :const:`schemas.MESH_APPROXIMATION_TOKENS` for available options. - """ - - simplification_metric: float | None = None - """Mesh simplification accuracy. - - Defaults to 0.55. - """ - weld_tolerance: float | None = None - """Mesh weld tolerance, controls the distance at which vertices are welded. - - Default -inf will autocompute the welding tolerance based on the mesh size. Zero value will disable welding. - Range: [0, inf) Units: distance - """ - - -@configclass -class SDFMeshPropertiesCfg(MeshCollisionPropertiesCfg): - physx_api: str = "PhysxSDFMeshCollisionAPI" - """SDF mesh is only supported by PhysX API. - - Original PhysX documentation: - https://docs.omniverse.nvidia.com/kit/docs/omni_usd_schema_physics/latest/class_physx_schema_physx_s_d_f_mesh_collision_a_p_i.html - - More details and steps for optimizing SDF results can be found here: - https://nvidia-omniverse.github.io/PhysX/physx/5.2.1/docs/RigidBodyCollision.html#dynamic-triangle-meshes-with-sdfs - """ - - mesh_approximation_name: str = "sdf" - """Name of mesh collision approximation method. Default: "sdf". - Refer to :const:`schemas.MESH_APPROXIMATION_TOKENS` for available options. - """ - - sdf_margin: float | None = None - """Margin to increase the size of the SDF relative to the bounding box diagonal length of the mesh. - - - A sdf margin value of 0.01 means the sdf boundary will be enlarged in any direction by 1% of the mesh's bounding - box diagonal length. Representing the margin relative to the bounding box diagonal length ensures that it is scale - independent. Margins allow for precise distance queries in a region slightly outside of the mesh's bounding box. - - Default value is 0.01. - Range: [0, inf) Units: dimensionless - """ - sdf_narrow_band_thickness: float | None = None - """Size of the narrow band around the mesh surface where high resolution SDF samples are available. - - Outside of the narrow band, only low resolution samples are stored. Representing the narrow band thickness as a - fraction of the mesh's bounding box diagonal length ensures that it is scale independent. A value of 0.01 is - usually large enough. The smaller the narrow band thickness, the smaller the memory consumption of the sparse SDF. - - Default value is 0.01. - Range: [0, 1] Units: dimensionless - """ - sdf_resolution: int | None = None - """The spacing of the uniformly sampled SDF is equal to the largest AABB extent of the mesh, - divided by the resolution. - - Choose the lowest possible resolution that provides acceptable performance; very high resolution results in large - memory consumption, and slower cooking and simulation performance. - - Default value is 256. - Range: (1, inf) - """ - sdf_subgrid_resolution: int | None = None - """A positive subgrid resolution enables sparsity on signed-distance-fields (SDF) while a value of 0 leads to the - usage of a dense SDF. - - A value in the range of 4 to 8 is a reasonable compromise between block size and the overhead introduced by block - addressing. The smaller a block, the more memory is spent on the address table. The bigger a block, the less - precisely the sparse SDF can adapt to the mesh's surface. In most cases sparsity reduces the memory consumption of - a SDF significantly. - - Default value is 6. - Range: [0, inf) - """ + mesh_approximation_name: str = "boundingSphere" + """Name of mesh collision approximation method. Default: "boundingSphere".""" diff --git a/source/isaaclab/isaaclab/sim/simulation_cfg.py b/source/isaaclab/isaaclab/sim/simulation_cfg.py index 6c2e97bb7e82..eb0f9821592c 100644 --- a/source/isaaclab/isaaclab/sim/simulation_cfg.py +++ b/source/isaaclab/isaaclab/sim/simulation_cfg.py @@ -151,6 +151,63 @@ class RenderCfg: This is set by the variable: ``/rtx/domeLight/upperLowerStrategy``. """ + max_bounces: int | None = None + """Maximum number of ray bounces for path tracing (RT2). Default is 2. + + For global illumination (indirect diffuse), this should be at least 3. + + This is set by the variable: ``/rtx/rtpt/maxBounces``. + """ + + split_glass: bool | None = None + """Enables separate glass ray splitting for improved glass rendering (RT2). Default is False. + + Enabling this can reduce noise on glass materials at the cost of performance. + + This is set by the variable: ``/rtx/rtpt/splitGlass``. + """ + + split_clearcoat: bool | None = None + """Enables separate clearcoat ray splitting (RT2). Default is False. + + Enabling this can reduce noise on clearcoat materials at the cost of performance. + + This is set by the variable: ``/rtx/rtpt/splitClearcoat``. + """ + + split_rough_reflection: bool | None = None + """Enables separate rough reflection ray splitting (RT2). Default is False. + + Enabling this can reduce noise on rough reflective materials at the cost of performance. + + This is set by the variable: ``/rtx/rtpt/splitRoughReflection``. + """ + + ambient_light_intensity: float | None = None + """Scene ambient light intensity. Default is 1.0. + + This is set by the variable: ``/rtx/sceneDb/ambientLightIntensity``. + """ + + ambient_occlusion_denoiser_mode: Literal[0, 1] | None = None + """Ambient occlusion denoiser mode. Default is 1. + + Valid values are: + + * 0: Higher quality denoising + * 1: Performance-oriented denoising + + This is set by the variable: ``/rtx/ambientOcclusion/denoiserMode``. + """ + + view_tile_limit: int | None = None + """Maximum number of view tiles. Default is 1000000. + + This setting helps avoid silent trimming of tiles. + + This is set by the variable: ``/rtx/viewTile/limit``. + """ + carb_settings: dict[str, Any] | None = None """A general dictionary for users to supply all carb rendering settings with native names. diff --git a/source/isaaclab/isaaclab/sim/simulation_context.py b/source/isaaclab/isaaclab/sim/simulation_context.py index 89be3163359f..175961fcd383 100644 --- a/source/isaaclab/isaaclab/sim/simulation_context.py +++ b/source/isaaclab/isaaclab/sim/simulation_context.py @@ -22,12 +22,14 @@ import isaaclab.sim.utils.stage as stage_utils from isaaclab.app.settings_manager import SettingsManager from isaaclab.envs.utils.recording_hooks import run_recording_hooks_after_visualizers -from isaaclab.physics import BaseSceneDataProvider, PhysicsManager, SceneDataProvider +from isaaclab.markers.vis_marker_registry import VisMarkerRegistry +from isaaclab.physics import PhysicsEvent, PhysicsManager from isaaclab.physics.scene_data_requirements import ( SceneDataRequirement, resolve_scene_data_requirements, ) from isaaclab.renderers.render_context import RenderContext +from isaaclab.scene.scene_data_provider import SceneDataProvider from isaaclab.sim.utils import create_new_stage from isaaclab.utils.string import clear_resolve_matching_names_cache from isaaclab.utils.version import has_kit @@ -171,16 +173,14 @@ def __init__(self, cfg: SimulationCfg | None = None): self.physics_manager.initialize(self) self._apply_render_cfg_settings() - # Initialize visualizer state (provider/visualizers are created lazily during initialize_visualizers()). - self._scene_data_provider: BaseSceneDataProvider | None = None + # Initialize visualizer state (visualizers are created lazily during initialize_visualizers()). + self._scene_data_provider = SceneDataProvider(self.physics_manager.get_scene_data_backend()) self._visualizers: list[BaseVisualizer] = [] self._scene_data_requirements = SceneDataRequirement() - # Per-group clone plans published by InteractiveScene after cloning. Providers (e.g. - # the Newton visualizer model rebuilder on a PhysX backend) consume these to derive - # their own backend args. Empty dict until :meth:`InteractiveScene.clone_environments` - # runs. - self._clone_plans: dict[str, ClonePlan] = {} - self._visualizer_step_counter = 0 + # Clone plan published by InteractiveScene after cloning. Providers (e.g. the + # Newton visualizer model rebuilder on a PhysX backend) consume this to derive + # their own backend args. None until :meth:`InteractiveScene.clone_environments` runs. + self._clone_plan: ClonePlan | None = None # Default visualization dt used before/without visualizer initialization. physics_dt = getattr(self.cfg.physics, "dt", None) self._viz_dt = (physics_dt if physics_dt is not None else self.cfg.dt) * self.cfg.render_interval @@ -191,6 +191,7 @@ def __init__(self, cfg: SimulationCfg | None = None): self._xr_enabled = bool(self.get_setting("/isaaclab/xr/enabled")) # Note: has_rtx_sensors is NOT cached because it changes when Camera sensors are created self._pending_camera_view: tuple[tuple[float, float, float], tuple[float, float, float]] | None = None + self.vis_marker_registry = VisMarkerRegistry() # Simulation state self._is_playing = False @@ -206,6 +207,13 @@ def __init__(self, cfg: SimulationCfg | None = None): # Shared renderers for all Camera sensors (compatible renderer_cfg only). self._render_context = RenderContext() + # Run renderer post-physics setup. + self.physics_manager.register_callback( + lambda _payload: self._render_context.ensure_initialize(), + PhysicsEvent.PHYSICS_READY, + order=5, + ) + type(self)._instance = self # Mark as valid singleton only after successful init def _apply_render_cfg_settings(self) -> None: @@ -266,6 +274,17 @@ def _apply_nested(data: dict[str, Any], path: str = "") -> None: "enable_shadows": "/rtx/shadows/enabled", "enable_ambient_occlusion": "/rtx/ambientOcclusion/enabled", "dome_light_upper_lower_strategy": "/rtx/domeLight/upperLowerStrategy", + "ambient_light_intensity": "/rtx/sceneDb/ambientLightIntensity", + "ambient_occlusion_denoiser_mode": "/rtx/ambientOcclusion/denoiserMode", + "subpixel_mode": "/rtx/raytracing/subpixel/mode", + "enable_cached_raytracing": "/rtx/raytracing/cached/enabled", + "max_samples_per_launch": "/rtx/pathtracing/maxSamplesPerLaunch", + "view_tile_limit": "/rtx/viewTile/limit", + # RT2 path tracing settings + "max_bounces": "/rtx/rtpt/maxBounces", + "split_glass": "/rtx/rtpt/splitGlass", + "split_clearcoat": "/rtx/rtpt/splitClearcoat", + "split_rough_reflection": "/rtx/rtpt/splitRoughReflection", } for key, value in vars(render_cfg).items(): @@ -585,7 +604,6 @@ def initialize_visualizers(self) -> None: ] requirements = resolve_scene_data_requirements(visualizer_types=visualizer_types) self._scene_data_requirements = requirements - self.initialize_scene_data_provider() self._visualizers = [] for cfg in visualizer_cfgs: @@ -614,15 +632,7 @@ def initialize_visualizers(self) -> None: viz.set_camera_view(eye, target) self._pending_camera_view = None - if not self._visualizers and self._scene_data_provider is not None: - close_provider = getattr(self._scene_data_provider, "close", None) - if callable(close_provider): - close_provider() - self._scene_data_provider = None - - def initialize_scene_data_provider(self) -> BaseSceneDataProvider: - if self._scene_data_provider is None: - self._scene_data_provider = SceneDataProvider(self.stage, self) + def get_scene_data_provider(self) -> SceneDataProvider: return self._scene_data_provider def get_scene_data_requirements(self) -> SceneDataRequirement: @@ -633,18 +643,18 @@ def update_scene_data_requirements(self, requirements: SceneDataRequirement) -> """Update scene-data requirements.""" self._scene_data_requirements = requirements - def get_clone_plans(self) -> dict[str, ClonePlan]: - """Return per-group clone plans published by the scene, keyed by destination template. + def get_clone_plan(self) -> ClonePlan | None: + """Return the clone plan published by the scene. Set by :meth:`InteractiveScene.clone_environments` after replication. Consumed by scene data providers that build backend models (e.g. Newton visualizer model on a - PhysX backend) from the same plan the cloner used. Empty dict until the scene clones. + PhysX backend) from the same plan the cloner used. ``None`` until the scene clones. """ - return self._clone_plans + return self._clone_plan - def set_clone_plans(self, plans: dict[str, ClonePlan]) -> None: - """Set the cloner's per-group clone-plan map.""" - self._clone_plans = plans + def set_clone_plan(self, plan: ClonePlan | None) -> None: + """Set the cloner's clone plan.""" + self._clone_plan = plan @property def visualizers(self) -> list[BaseVisualizer]: @@ -751,7 +761,18 @@ def update_visualizers(self, dt: float, skip_app_pumping: bool = False) -> None: if not self._visualizers: return - self.update_scene_data_provider() + if self._should_forward_before_visualizer_update(): + self.physics_manager.forward() + + # Marker callbacks update VisualizationMarkers state; visualizer step() + # consumes that state later in this method. + if any(viz.supports_markers() for viz in self._visualizers): + self.vis_marker_registry.dispatch_callbacks() + + # Marker callbacks update VisualizationMarkers state; visualizer step() + # consumes that state later in this method. + if any(viz.supports_markers() for viz in self._visualizers): + self.vis_marker_registry.dispatch_callbacks() visualizers_to_remove = [] for viz in self._visualizers: @@ -788,14 +809,6 @@ def update_visualizers(self, dt: float, skip_app_pumping: bool = False) -> None: except Exception as exc: logger.error("Error closing visualizer: %s", exc) - def update_scene_data_provider(self, force_require_forward: bool = False): - if force_require_forward or self._should_forward_before_visualizer_update(): - self.physics_manager.forward() - self._visualizer_step_counter += 1 - if self._scene_data_provider is None: - return - self._scene_data_provider.update() - def _should_forward_before_visualizer_update(self) -> bool: """Return True if any visualizer requires pre-step forward kinematics.""" return any(viz.requires_forward_before_step() for viz in self._visualizers) @@ -851,11 +864,6 @@ def clear_instance(cls) -> None: for viz in cls._instance._visualizers: viz.close() cls._instance._visualizers.clear() - if cls._instance._scene_data_provider is not None: - close_provider = getattr(cls._instance._scene_data_provider, "close", None) - if callable(close_provider): - close_provider() - cls._instance._scene_data_provider = None # Tear down the stage. We skip clear_stage() (prim-by-prim deletion) since # close_stage() + app shutdown destroy the entire stage at once. diff --git a/source/isaaclab/isaaclab/sim/spawners/from_files/from_files.py b/source/isaaclab/isaaclab/sim/spawners/from_files/from_files.py index be7bd6e7074e..18ffcaf37c98 100644 --- a/source/isaaclab/isaaclab/sim/spawners/from_files/from_files.py +++ b/source/isaaclab/isaaclab/sim/spawners/from_files/from_files.py @@ -5,12 +5,14 @@ from __future__ import annotations -import fcntl import logging import os import tempfile +from contextlib import nullcontext from typing import TYPE_CHECKING +from filelock import FileLock + # deformables only supported on PhysX backend from isaaclab_physx.sim import schemas as schemas_physx from isaaclab_physx.sim.spawners.materials import SurfaceDeformableBodyMaterialCfg @@ -316,10 +318,10 @@ def _spawn_from_usd_file( raise FileNotFoundError(f"USD file not found at path: '{usd_path}'.") if _world_size > 1: - lock_path = os.path.join(tempfile.gettempdir(), "isaaclab_usd_spawn.lock") - lock_fd = open(lock_path, "w") # noqa: SIM115 - fcntl.flock(lock_fd, fcntl.LOCK_EX) - try: + lock = FileLock(os.path.join(tempfile.gettempdir(), "isaaclab_usd_spawn.lock")) + else: + lock = nullcontext() + with lock: if file_status == 2: usd_path = retrieve_file_path(usd_path, force_download=False) stage = get_current_stage() @@ -334,10 +336,6 @@ def _spawn_from_usd_file( ) else: logger.warning(f"A prim already exists at prim path: '{prim_path}'.") - finally: - if _world_size > 1: - fcntl.flock(lock_fd, fcntl.LOCK_UN) - lock_fd.close() # modify variants if hasattr(cfg, "variants") and cfg.variants is not None: @@ -365,6 +363,25 @@ def _spawn_from_usd_file( # note: these are only for setting low-level simulation properties. all others should be set or are # and overridden by the articulation/actuator properties. if cfg.joint_drive_props is not None: + # auto-enable body-level gravcomp if joint-level actuator gravcomp is requested + # without it — actuatorgravcomp has no effect since there are no forces to route. + # Only auto-populates when the user did not already set ``gravcomp`` themselves; + # an explicit ``MujocoRigidBodyPropertiesCfg(gravcomp=0.5)`` is preserved as-is. + from isaaclab_newton.sim.schemas.schemas_cfg import MujocoJointDrivePropertiesCfg, MujocoRigidBodyPropertiesCfg + + body_gravcomp_unset = ( + not isinstance(cfg.rigid_props, MujocoRigidBodyPropertiesCfg) or cfg.rigid_props.gravcomp is None + ) + if ( + isinstance(cfg.joint_drive_props, MujocoJointDrivePropertiesCfg) + and cfg.joint_drive_props.actuatorgravcomp + and body_gravcomp_unset + ): + logger.info( + "Joint-level actuator gravity compensation requires body-level gravcomp." + " Auto-setting MujocoRigidBodyPropertiesCfg(gravcomp=1.0)." + ) + schemas.modify_rigid_body_properties(prim_path, MujocoRigidBodyPropertiesCfg(gravcomp=1.0)) schemas.modify_joint_drive_properties(prim_path, cfg.joint_drive_props) # define deformable body properties, or modify if deformable body API is present (PhysX only) diff --git a/source/isaaclab/isaaclab/sim/spawners/from_files/from_files_cfg.py b/source/isaaclab/isaaclab/sim/spawners/from_files/from_files_cfg.py index 78aaf1b15c94..87fc48de4d30 100644 --- a/source/isaaclab/isaaclab/sim/spawners/from_files/from_files_cfg.py +++ b/source/isaaclab/isaaclab/sim/spawners/from_files/from_files_cfg.py @@ -46,7 +46,7 @@ class FileCfg(RigidObjectSpawnerCfg, DeformableObjectSpawnerCfg): spatial_tendons_props: schemas.SpatialTendonPropertiesCfg | None = None """Properties to apply to the spatial tendons (if any).""" - joint_drive_props: schemas.JointDrivePropertiesCfg | None = None + joint_drive_props: schemas.JointDriveBaseCfg | None = None """Properties to apply to a joint. .. note:: diff --git a/source/isaaclab/isaaclab/sim/spawners/materials/__init__.py b/source/isaaclab/isaaclab/sim/spawners/materials/__init__.py index b86e98599fc0..3158625219c5 100644 --- a/source/isaaclab/isaaclab/sim/spawners/materials/__init__.py +++ b/source/isaaclab/isaaclab/sim/spawners/materials/__init__.py @@ -54,4 +54,30 @@ from isaaclab.utils.module import lazy_export -lazy_export() +_stub_getattr, _stub_dir, __all__ = lazy_export() + +# Names that moved out of this module into ``isaaclab_physx.sim.spawners.materials``. +# Resolved lazily on first access so importing ``isaaclab.sim.spawners.materials`` does +# not require ``isaaclab_physx`` to be installed. +_PHYSX_FORWARDS = frozenset({ + "RigidBodyMaterialCfg", + "PhysxRigidBodyMaterialCfg", +}) + + +def __getattr__(name): + if name in _PHYSX_FORWARDS: + try: + from isaaclab_physx.sim.spawners.materials import physics_materials_cfg as _physx_cfg + except ImportError as e: + raise ImportError( + f"'isaaclab.sim.spawners.materials.{name}' has moved to" + " 'isaaclab_physx.sim.spawners.materials'. Install the isaaclab_physx extension" + " or update your import. This forwarding shim is scheduled for removal in 4.0." + ) from e + return getattr(_physx_cfg, name) + return _stub_getattr(name) + + +def __dir__(): + return sorted(set(_stub_dir()) | _PHYSX_FORWARDS) diff --git a/source/isaaclab/isaaclab/sim/spawners/materials/__init__.pyi b/source/isaaclab/isaaclab/sim/spawners/materials/__init__.pyi index 93142ddab389..0dd023c2998f 100644 --- a/source/isaaclab/isaaclab/sim/spawners/materials/__init__.pyi +++ b/source/isaaclab/isaaclab/sim/spawners/materials/__init__.pyi @@ -6,7 +6,7 @@ __all__ = [ "spawn_rigid_body_material", "PhysicsMaterialCfg", - "RigidBodyMaterialCfg", + "RigidBodyMaterialBaseCfg", "spawn_from_mdl_file", "spawn_preview_surface", "GlassMdlCfg", @@ -18,7 +18,7 @@ __all__ = [ from .physics_materials import spawn_rigid_body_material from .physics_materials_cfg import ( PhysicsMaterialCfg, - RigidBodyMaterialCfg, + RigidBodyMaterialBaseCfg, ) from .visual_materials import spawn_from_mdl_file, spawn_preview_surface from .visual_materials_cfg import GlassMdlCfg, MdlFileCfg, PreviewSurfaceCfg, VisualMaterialCfg diff --git a/source/isaaclab/isaaclab/sim/spawners/materials/physics_materials.py b/source/isaaclab/isaaclab/sim/spawners/materials/physics_materials.py index 240a0ff00746..b76077b38bed 100644 --- a/source/isaaclab/isaaclab/sim/spawners/materials/physics_materials.py +++ b/source/isaaclab/isaaclab/sim/spawners/materials/physics_materials.py @@ -5,25 +5,33 @@ from __future__ import annotations +import dataclasses from typing import TYPE_CHECKING from pxr import Usd, UsdPhysics, UsdShade -from isaaclab.sim.utils import clone, safe_set_attribute_on_usd_prim, safe_set_attribute_on_usd_schema +from isaaclab.sim.schemas.schemas import _apply_namespaced_schemas +from isaaclab.sim.utils import clone from isaaclab.sim.utils.stage import get_current_stage -from isaaclab.utils.string import to_camel_case if TYPE_CHECKING: from . import physics_materials_cfg @clone -def spawn_rigid_body_material(prim_path: str, cfg: physics_materials_cfg.RigidBodyMaterialCfg) -> Usd.Prim: +def spawn_rigid_body_material(prim_path: str, cfg: physics_materials_cfg.RigidBodyMaterialBaseCfg) -> Usd.Prim: """Create material with rigid-body physics properties. Rigid body materials are used to define the physical properties to meshes of a rigid body. These - include the friction, restitution, and their respective combination modes. For more information on - rigid body material, please refer to the `documentation on PxMaterial `_. + include the friction, restitution, and (PhysX-only) compliant-contact spring and combine-mode + tokens. For more information on rigid body material, please refer to the `documentation on + PxMaterial `_. + + The writer is metadata-driven: it always applies the standard ``UsdPhysics.MaterialAPI`` and + writes the friction/restitution fields, then reads ``_usd_applied_schema``, ``_usd_namespace``, + and ``_usd_attr_name_map`` from the cfg to author solver-specific attributes. The applied + schema (e.g. ``PhysxMaterialAPI``) is added only when at least one solver-specific field has a + non-``None`` value at the instance level. .. note:: This function is decorated with :func:`clone` that resolves prim path into list of paths @@ -39,7 +47,8 @@ def spawn_rigid_body_material(prim_path: str, cfg: physics_materials_cfg.RigidBo The spawned rigid body material prim. Raises: - ValueError: When a prim already exists at the specified prim path and is not a material. + ValueError: When a prim already exists at the specified prim path and is not a material. + ValueError: When the cfg defines solver-specific fields but does not define ``_usd_namespace``. """ # get stage handle stage = get_current_stage() @@ -53,24 +62,17 @@ def spawn_rigid_body_material(prim_path: str, cfg: physics_materials_cfg.RigidBo # check if prim is a material if not prim.IsA(UsdShade.Material): raise ValueError(f"A prim already exists at path: '{prim_path}' but is not a material.") - # retrieve the USD rigid-body api - usd_physics_material_api = UsdPhysics.MaterialAPI(prim) - if not usd_physics_material_api: - usd_physics_material_api = UsdPhysics.MaterialAPI.Apply(prim) - # ensure PhysX material API is applied - applied = prim.GetAppliedSchemas() - if "PhysxMaterialAPI" not in applied: - prim.AddAppliedSchema("PhysxMaterialAPI") - - # convert to dict - cfg = cfg.to_dict() - del cfg["func"] - # set into USD API - for attr_name in ["static_friction", "dynamic_friction", "restitution"]: - value = cfg.pop(attr_name, None) - safe_set_attribute_on_usd_schema(usd_physics_material_api, attr_name, value, camel_case=True) - # set into PhysX API (prim attributes: physxMaterial:*) - for attr_name, value in cfg.items(): - safe_set_attribute_on_usd_prim(prim, f"physxMaterial:{to_camel_case(attr_name, 'cC')}", value, camel_case=False) + + # apply the standard UsdPhysics MaterialAPI (always) + if not UsdPhysics.MaterialAPI(prim): + UsdPhysics.MaterialAPI.Apply(prim) + + # build cfg dict, dropping underscore-prefixed metadata keys and the spawner ``func`` field + cfg_dict = {f.name: getattr(cfg, f.name) for f in dataclasses.fields(cfg) if f.name != "func"} + + # All fields routed by the helper: base friction/restitution under ``physics:*``, + # PhysX-subclass fields (compliant-contact, combine modes) under ``physxMaterial:*``. + _apply_namespaced_schemas(prim, cfg, cfg_dict) + # return the prim return prim diff --git a/source/isaaclab/isaaclab/sim/spawners/materials/physics_materials_cfg.py b/source/isaaclab/isaaclab/sim/spawners/materials/physics_materials_cfg.py index dde9aec6d905..0c9a7be478e8 100644 --- a/source/isaaclab/isaaclab/sim/spawners/materials/physics_materials_cfg.py +++ b/source/isaaclab/isaaclab/sim/spawners/materials/physics_materials_cfg.py @@ -7,10 +7,31 @@ from collections.abc import Callable from dataclasses import MISSING -from typing import Literal +from typing import ClassVar from isaaclab.utils import configclass +# Names that moved out of this submodule into ``isaaclab_physx.sim.spawners.materials.physics_materials_cfg``. +# Resolved lazily so callers using ``from isaaclab.sim.spawners.materials.physics_materials_cfg +# import RigidBodyMaterialCfg`` continue to work without importing ``isaaclab_physx`` at module +# load time. +_PHYSX_FORWARDS = frozenset({"RigidBodyMaterialCfg", "PhysxRigidBodyMaterialCfg"}) + + +def __getattr__(name): + if name in _PHYSX_FORWARDS: + try: + from isaaclab_physx.sim.spawners.materials import physics_materials_cfg as _physx_mat_cfg + except ImportError as e: + raise ImportError( + f"'isaaclab.sim.spawners.materials.physics_materials_cfg.{name}' has moved to" + " 'isaaclab_physx.sim.spawners.materials.physics_materials_cfg'. Install the" + " isaaclab_physx extension or update your import. This forwarding shim is scheduled" + " for removal in 4.0." + ) from e + return getattr(_physx_mat_cfg, name) + raise AttributeError(f"module 'isaaclab.sim.spawners.materials.physics_materials_cfg' has no attribute {name!r}") + @configclass class PhysicsMaterialCfg: @@ -27,12 +48,26 @@ class PhysicsMaterialCfg: @configclass -class RigidBodyMaterialCfg(PhysicsMaterialCfg): - """Physics material parameters for rigid bodies. +class RigidBodyMaterialBaseCfg(PhysicsMaterialCfg): + """Solver-common physics-material parameters for rigid bodies. + + Contains the friction and restitution fields from the `UsdPhysics.MaterialAPI`_ that are common + across all simulation backends. For PhysX-only material properties (compliant-contact spring, + combine modes), use :class:`~isaaclab_physx.sim.spawners.materials.PhysxRigidBodyMaterialCfg`. See :meth:`spawn_rigid_body_material` for more information. + + .. _UsdPhysics.MaterialAPI: https://openusd.org/dev/api/class_usd_physics_material_a_p_i.html """ + # -- Class metadata (not dataclass fields) -- + # ``static_friction`` / ``dynamic_friction`` / ``restitution`` write to ``physics:*`` + # (UsdPhysics standard attributes). The helper's per-declaring-class routing keeps + # them under the base namespace even when the cfg is a PhysX subclass instance. + _usd_namespace: ClassVar[str | None] = "physics" + _usd_applied_schema: ClassVar[str | None] = None + _usd_field_exceptions: ClassVar[dict] = {} + func: Callable | str = "{DIR}.physics_materials:spawn_rigid_body_material" static_friction: float = 0.5 @@ -43,37 +78,3 @@ class RigidBodyMaterialCfg(PhysicsMaterialCfg): restitution: float = 0.0 """The restitution coefficient. Defaults to 0.0.""" - - friction_combine_mode: Literal["average", "min", "multiply", "max"] = "average" - """Determines the way friction will be combined during collisions. Defaults to `"average"`. - - .. attention:: - - When two physics materials with different combine modes collide, the combine mode with the higher - priority will be used. The priority order is provided `here - `__. - """ - - restitution_combine_mode: Literal["average", "min", "multiply", "max"] = "average" - """Determines the way restitution coefficient will be combined during collisions. Defaults to `"average"`. - - .. attention:: - - When two physics materials with different combine modes collide, the combine mode with the higher - priority will be used. The priority order is provided `here - `__. - """ - - compliant_contact_stiffness: float = 0.0 - """Spring stiffness for a compliant contact model using implicit springs. Defaults to 0.0. - - A higher stiffness results in behavior closer to a rigid contact. The compliant contact model is only enabled - if the stiffness is larger than 0. - """ - - compliant_contact_damping: float = 0.0 - """Damping coefficient for a compliant contact model using implicit springs. Defaults to 0.0. - - Irrelevant if compliant contacts are disabled when :obj:`compliant_contact_stiffness` is set to zero and - rigid contacts are active. - """ diff --git a/source/isaaclab/isaaclab/sim/spawners/spawner_cfg.py b/source/isaaclab/isaaclab/sim/spawners/spawner_cfg.py index 7a803ad0e0dd..0adf9215f81e 100644 --- a/source/isaaclab/isaaclab/sim/spawners/spawner_cfg.py +++ b/source/isaaclab/isaaclab/sim/spawners/spawner_cfg.py @@ -85,10 +85,10 @@ class RigidObjectSpawnerCfg(SpawnerCfg): mass_props: schemas.MassPropertiesCfg | None = None """Mass properties.""" - rigid_props: schemas.RigidBodyPropertiesCfg | None = None + rigid_props: schemas.RigidBodyBaseCfg | None = None """Rigid body properties. - For making a rigid object static, set the :attr:`schemas.RigidBodyPropertiesCfg.kinematic_enabled` + For making a rigid object static, set the :attr:`schemas.RigidBodyBaseCfg.kinematic_enabled` as True. This will make the object static and will not be affected by gravity or other forces. """ diff --git a/source/isaaclab/isaaclab/sim/spawners/wrappers/wrappers.py b/source/isaaclab/isaaclab/sim/spawners/wrappers/wrappers.py index 285dd0373063..f6f087cfa129 100644 --- a/source/isaaclab/isaaclab/sim/spawners/wrappers/wrappers.py +++ b/source/isaaclab/isaaclab/sim/spawners/wrappers/wrappers.py @@ -31,9 +31,6 @@ def spawn_multi_asset( Assets are created in the order they appear in ``cfg.assets_cfg`` using the base name in ``prim_path``, which must contain ``.*`` (for example, ``/World/Env_0/asset_.*`` spawns ``asset_0``, ``asset_1``, ...). - The prefix portion of ``prim_path`` may also include ``.*`` (for example, ``/World/env_.*/asset_.*``); - in this case, assets are spawned under the first match (``env_0``) and that structure is cloned to - other matching environments by the scene's cloner. Args: prim_path: The prim path to spawn the assets. @@ -46,21 +43,33 @@ def spawn_multi_asset( Returns: The created prim at the first prim path. """ - split_path = prim_path.split("/") - prefix_path, base_name = "/".join(split_path[:-1]), split_path[-1] - if ".*" not in base_name: - raise ValueError( - f" The base name '{base_name}' in the prim path '{prim_path}' must contain '.*' to indicate" - " the path each individual multiple-asset to be spawned." - ) + if cfg.spawn_paths is not None: + if len(cfg.spawn_paths) != len(cfg.assets_cfg): + raise ValueError( + f"Expected spawn_paths to match assets_cfg length, got {len(cfg.spawn_paths)} and" + f" {len(cfg.assets_cfg)}." + ) + asset_prim_paths = list(cfg.spawn_paths) + else: + split_path = prim_path.split("/") + prefix_path, base_name = "/".join(split_path[:-1]), split_path[-1] + if ".*" not in base_name: + raise ValueError( + f" The base name '{base_name}' in the prim path '{prim_path}' must contain '.*' to indicate" + " the path each individual multiple-asset to be spawned." + ) + asset_prim_paths = [f"{prefix_path}/{base_name.replace('.*', str(i))}" for i in range(len(cfg.assets_cfg))] + if cfg.random_choice: logger.warning( "`random_choice` parameter in `spawn_multi_asset` is deprecated, and nothing will happen. " "Use `isaaclab.scene.interactive_scene_cfg.InteractiveSceneCfg.random_heterogeneous_cloning` instead." ) - proto_prim_paths = list() - for index, asset_cfg in enumerate(cfg.assets_cfg): + spawned_prim_paths: list[str] = [] + for asset_prim_path, asset_cfg in zip(asset_prim_paths, cfg.assets_cfg): + if asset_prim_path is None: + continue # append semantic tags if specified if cfg.semantic_tags is not None: if asset_cfg.semantic_tags is None: @@ -74,19 +83,18 @@ def spawn_multi_asset( if hasattr(asset_cfg, attr_name) and attr_value is not None: setattr(asset_cfg, attr_name, attr_value) - proto_prim_path = f"{prefix_path}/{base_name.replace('.*', str(index))}" asset_cfg.func( - proto_prim_path, + asset_prim_path, asset_cfg, translation=translation, orientation=orientation, clone_in_fabric=clone_in_fabric, replicate_physics=replicate_physics, ) - # append to proto prim paths - proto_prim_paths.append(proto_prim_path) - - return sim_utils.find_first_matching_prim(proto_prim_paths[0]) + spawned_prim_paths.append(asset_prim_path) + if not spawned_prim_paths: + raise ValueError("No assets were spawned. At least one spawn path must be active.") + return sim_utils.find_first_matching_prim(spawned_prim_paths[0]) def spawn_multi_usd_file( @@ -126,13 +134,13 @@ def spawn_multi_usd_file( usd_template_cfg = UsdFileCfg() for attr_name, attr_value in cfg.__dict__.items(): # skip names we know are not present - if attr_name in ["func", "usd_path", "random_choice", "spawn_path"]: + if attr_name in ["func", "usd_path", "random_choice", "spawn_path", "spawn_paths"]: continue # set the attribute into the template setattr(usd_template_cfg, attr_name, attr_value) # create multi asset configuration of USD files - multi_asset_cfg = MultiAssetSpawnerCfg(assets_cfg=[]) + multi_asset_cfg = MultiAssetSpawnerCfg(assets_cfg=[], spawn_paths=cfg.spawn_paths) for usd_path in usd_paths: usd_cfg = usd_template_cfg.replace(usd_path=usd_path) multi_asset_cfg.assets_cfg.append(usd_cfg) diff --git a/source/isaaclab/isaaclab/sim/spawners/wrappers/wrappers_cfg.py b/source/isaaclab/isaaclab/sim/spawners/wrappers/wrappers_cfg.py index d9b7d9ed0c35..e335393f7d94 100644 --- a/source/isaaclab/isaaclab/sim/spawners/wrappers/wrappers_cfg.py +++ b/source/isaaclab/isaaclab/sim/spawners/wrappers/wrappers_cfg.py @@ -49,6 +49,14 @@ class MultiAssetSpawnerCfg(RigidObjectSpawnerCfg, DeformableObjectSpawnerCfg): assets_cfg: list[SpawnerCfg] = MISSING """List of asset configurations to spawn.""" + spawn_paths: list[str | None] | None = None + """Optional concrete spawn paths, one per asset configuration. + + When set, :func:`spawn_multi_asset` uses these paths instead of deriving + sibling paths from the input ``prim_path``. Entries set to ``None`` are + skipped. + """ + random_choice: bool = True """ This parameter is ignored. See :attr:`isaaclab.scene.interactive_scene_cfg.InteractiveSceneCfg.random_heterogeneous_cloning` for details. @@ -77,6 +85,14 @@ class MultiUsdFileCfg(UsdFileCfg): usd_path: str | list[str] = MISSING """Path or a list of paths to the USD files to spawn asset from.""" + spawn_paths: list[str | None] | None = None + """Optional concrete spawn paths, one per USD path. + + When set, :func:`spawn_multi_usd_file` uses these paths instead of deriving + sibling paths from the input ``prim_path``. Entries set to ``None`` are + skipped. + """ + random_choice: bool = True """Whether to randomly select an asset configuration. Default is True. diff --git a/source/isaaclab/isaaclab/sim/views/usd_frame_view.py b/source/isaaclab/isaaclab/sim/views/usd_frame_view.py index 7730c3dd735d..88392d54b2a0 100644 --- a/source/isaaclab/isaaclab/sim/views/usd_frame_view.py +++ b/source/isaaclab/isaaclab/sim/views/usd_frame_view.py @@ -72,8 +72,7 @@ def __init__( stage: USD stage to search for prims. Defaults to None, in which case the current active stage from the simulation context is used. **kwargs: Additional keyword arguments (ignored). Allows forward-compatible - construction when callers pass backend-specific options like - ``sync_usd_on_fabric_write``. + construction when callers pass backend-specific options. Raises: ValueError: If any matched prim is not Xformable or doesn't have standardized diff --git a/source/isaaclab/isaaclab/utils/backend_utils.py b/source/isaaclab/isaaclab/utils/backend_utils.py index 9f69a66aa04d..ddc627171ae1 100644 --- a/source/isaaclab/isaaclab/utils/backend_utils.py +++ b/source/isaaclab/isaaclab/utils/backend_utils.py @@ -3,12 +3,48 @@ # # SPDX-License-Identifier: BSD-3-Clause +from __future__ import annotations + import importlib import logging +from typing import TYPE_CHECKING + +if TYPE_CHECKING: + from isaaclab.renderers.renderer_cfg import RendererCfg logger = logging.getLogger(__name__) +def get_default_renderer_cfg() -> RendererCfg: + """Return the default :class:`~isaaclab.renderers.renderer_cfg.RendererCfg` for cameras. + + Lazily imports :mod:`isaaclab_physx.renderers` and returns a new + :class:`~isaaclab_physx.renderers.IsaacRtxRendererCfg` instance. + + Returns: + A new default Isaac RTX renderer configuration. + + Raises: + ImportError: If :mod:`isaaclab_physx.renderers` cannot be imported or does not + expose ``IsaacRtxRendererCfg``. + """ + try: + renderers_mod = importlib.import_module("isaaclab_physx.renderers") + except ImportError as e: + raise ImportError( + "The default camera renderer configuration requires the optional 'isaaclab_physx' " + "package (import 'isaaclab_physx.renderers'). Install isaaclab_physx or set " + "CameraCfg.renderer_cfg explicitly." + ) from e + try: + default_cls = renderers_mod.IsaacRtxRendererCfg + except AttributeError as e: + raise ImportError( + "Module 'isaaclab_physx.renderers' is available but does not define 'IsaacRtxRendererCfg'." + ) from e + return default_cls() + + class FactoryBase: """A generic factory class that dynamically loads backends.""" diff --git a/source/isaaclab/isaaclab/utils/configclass.py b/source/isaaclab/isaaclab/utils/configclass.py index 59605f2797cc..f44f1288e017 100644 --- a/source/isaaclab/isaaclab/utils/configclass.py +++ b/source/isaaclab/isaaclab/utils/configclass.py @@ -452,9 +452,13 @@ class State: for key in ann: # find matching field in class value = class_members.get(key, MISSING) - # check if key belongs to ClassVar - # in that case, we cannot use default_factory! - origin = getattr(ann[key], "__origin__", None) + # check if key belongs to ClassVar -- in that case, we cannot use default_factory! + # ``from __future__ import annotations`` turns annotations into strings, so we + # also detect the string form (``"ClassVar[...]"``) for files using PEP 563. + ann_value = ann[key] + if isinstance(ann_value, str) and ann_value.startswith(("ClassVar", "typing.ClassVar")): + continue + origin = getattr(ann_value, "__origin__", None) if origin is ClassVar: continue # check if f is MISSING diff --git a/source/isaaclab/isaaclab/utils/math.py b/source/isaaclab/isaaclab/utils/math.py index b01d7e7ab4fe..9b33252091aa 100644 --- a/source/isaaclab/isaaclab/utils/math.py +++ b/source/isaaclab/isaaclab/utils/math.py @@ -322,7 +322,9 @@ def quat_from_matrix(matrix: torch.Tensor) -> torch.Tensor: matrix: The rotation matrices. Shape is (..., 3, 3). Returns: - The quaternion in (x, y, z, w). Shape is (..., 4). + The quaternion in (x, y, z, w). Shape is (..., 4). Rows whose input is not a + valid rotation (e.g. singular, reflection, or scale-error matrices) are filled + with NaN, so callers can detect them via :func:`torch.isnan`. Reference: https://github.com/facebookresearch/pytorch3d/blob/main/pytorch3d/transforms/rotation_conversions.py#L102-L161 @@ -368,9 +370,13 @@ def quat_from_matrix(matrix: torch.Tensor) -> torch.Tensor: # if not for numerical problems, quat_candidates[i] should be same (up to a sign), # forall i; we pick the best-conditioned one (with the largest denominator) - return quat_candidates[torch.nn.functional.one_hot(q_abs.argmax(dim=-1), num_classes=4) > 0.5, :].reshape( + quat = quat_candidates[torch.nn.functional.one_hot(q_abs.argmax(dim=-1), num_classes=4) > 0.5, :].reshape( batch_dim + (4,) ) + # guard against non-rotation input: a valid rotation must yield a unit quaternion. + # Threshold is 2x the worst-case float32 accumulated error (~1e-5) through this function. + invalid = (quat.norm(p=2, dim=-1, keepdim=True) - 1.0).abs() > 2e-5 + return torch.where(invalid, torch.full_like(quat, float("nan")), quat) def _axis_angle_rotation(axis: Literal["X", "Y", "Z"], angle: torch.Tensor) -> torch.Tensor: @@ -1633,7 +1639,17 @@ def create_rotation_matrix_from_view( The vectors are broadcast against each other so they all have shape (N, 3). Returns: - R: (N, 3, 3) batched rotation matrices + ``(N, 3, 3)`` batched rotation matrices. Rows with an undefined forward + direction (``eyes == targets`` or non-finite input) are filled with NaN. + Callers detect per-row failure with ``torch.isnan(R).any(dim=(-2, -1))`` + and total failure with ``.all()``. + + Note: + When the look-at direction is parallel to ``up_axis`` the camera roll + is mathematically undefined; a deterministic frame is returned via an + alternate reference vector. Tracking a target continuously through the + singularity will produce a discontinuous rotation -- smooth tracking + requires interpolation at the caller (e.g., quaternion slerp). Reference: Based on PyTorch3D (https://github.com/facebookresearch/pytorch3d/blob/eaf0709d6af0025fe94d1ee7cec454bc3054826a/pytorch3d/renderer/cameras.py#L1635-L1685) @@ -1645,16 +1661,27 @@ def create_rotation_matrix_from_view( else: raise ValueError(f"Invalid up axis: {up_axis}. Valid options are 'Y' and 'Z'.") + forward = targets - eyes + # 1e-5 matches the torch.nn.functional.normalize eps below: smaller magnitudes produce a sub-unit z_axis + undefined_forward = (torch.linalg.norm(forward, dim=1, keepdim=True) < 1e-5) | ~torch.isfinite(forward).all( + dim=1, keepdim=True + ) + # get rotation matrix in opengl format (-Z forward, +Y up) - z_axis = -torch.nn.functional.normalize(targets - eyes, eps=1e-5) + z_axis = -torch.nn.functional.normalize(forward, eps=1e-5) x_axis = torch.nn.functional.normalize(torch.cross(up_axis_vec, z_axis, dim=1), eps=1e-5) y_axis = torch.nn.functional.normalize(torch.cross(z_axis, x_axis, dim=1), eps=1e-5) is_close = torch.isclose(x_axis, torch.tensor(0.0), atol=5e-3).all(dim=1, keepdim=True) if is_close.any(): - replacement = torch.nn.functional.normalize(torch.cross(y_axis, z_axis, dim=1), eps=1e-5) - x_axis = torch.where(is_close, replacement, x_axis) - R = torch.cat((x_axis[:, None, :], y_axis[:, None, :], z_axis[:, None, :]), dim=1) - return R.transpose(1, 2) + # alt-up substitution when up_axis_vec is parallel to z_axis; both x and y must be recomputed. + # World X is non-parallel to z whenever the symptom fires for the supported up_axis values. + alt_up = torch.tensor((1.0, 0.0, 0.0), device=device, dtype=torch.float32).repeat(eyes.shape[0], 1) + replacement_x = torch.nn.functional.normalize(torch.cross(alt_up, z_axis, dim=1), eps=1e-5) + replacement_y = torch.nn.functional.normalize(torch.cross(z_axis, replacement_x, dim=1), eps=1e-5) + x_axis = torch.where(is_close, replacement_x, x_axis) + y_axis = torch.where(is_close, replacement_y, y_axis) + R = torch.cat((x_axis[:, None, :], y_axis[:, None, :], z_axis[:, None, :]), dim=1).transpose(1, 2) + return torch.where(undefined_forward.unsqueeze(-1), torch.full_like(R, float("nan")), R) def make_pose(pos: torch.Tensor, rot: torch.Tensor) -> torch.Tensor: diff --git a/source/isaaclab/isaaclab/utils/warp/fabric.py b/source/isaaclab/isaaclab/utils/warp/fabric.py index a48f773f4991..6f9963f290a1 100644 --- a/source/isaaclab/isaaclab/utils/warp/fabric.py +++ b/source/isaaclab/isaaclab/utils/warp/fabric.py @@ -15,15 +15,28 @@ import warp as wp +__all__ = [ + "arange_k", + "compose_fabric_transformation_matrix_from_warp_arrays", + "compose_indexed_fabric_transforms", + "decompose_fabric_transformation_matrix_to_warp_arrays", + "decompose_indexed_fabric_transforms", + "set_view_to_fabric_array", + "update_indexed_local_matrix_from_world", + "update_indexed_world_matrix_from_local", +] + if TYPE_CHECKING: FabricArrayUInt32 = Any FabricArrayMat44d = Any + IndexedFabricArrayMat44d = Any ArrayUInt32 = Any ArrayUInt32_1d = Any ArrayFloat32_2d = Any else: FabricArrayUInt32 = wp.fabricarray(dtype=wp.uint32) FabricArrayMat44d = wp.fabricarray(dtype=wp.mat44d) + IndexedFabricArrayMat44d = wp.indexedfabricarray(dtype=wp.mat44d) ArrayUInt32 = wp.array(ndim=1, dtype=wp.uint32) ArrayUInt32_1d = wp.array(dtype=wp.uint32) ArrayFloat32_2d = wp.array(ndim=2, dtype=wp.float32) @@ -130,29 +143,20 @@ def compose_fabric_transformation_matrix_from_warp_arrays( position, rotation, scale = _decompose_transformation_matrix(wp.mat44f(fabric_matrices[fabric_index])) # update position (check if array has elements, not just if it exists) if array_positions.shape[0] > 0: - if broadcast_positions: - index = 0 - else: - index = i + index = wp.where(broadcast_positions, 0, i) position[0] = array_positions[index, 0] position[1] = array_positions[index, 1] position[2] = array_positions[index, 2] # update orientation (convert from wxyz to xyzw for Warp) if array_orientations.shape[0] > 0: - if broadcast_orientations: - index = 0 - else: - index = i + index = wp.where(broadcast_orientations, 0, i) rotation[0] = array_orientations[index, 0] # x rotation[1] = array_orientations[index, 1] # y rotation[2] = array_orientations[index, 2] # z rotation[3] = array_orientations[index, 3] # w # update scale if array_scales.shape[0] > 0: - if broadcast_scales: - index = 0 - else: - index = i + index = wp.where(broadcast_scales, 0, i) scale[0] = array_scales[index, 0] scale[1] = array_scales[index, 1] scale[2] = array_scales[index, 2] @@ -163,6 +167,167 @@ def compose_fabric_transformation_matrix_from_warp_arrays( ) +@wp.kernel(enable_backward=False) +def decompose_indexed_fabric_transforms( + fabric_matrices: IndexedFabricArrayMat44d, + array_positions: ArrayFloat32_2d, + array_orientations: ArrayFloat32_2d, + array_scales: ArrayFloat32_2d, + indices: ArrayUInt32, +): + """Decompose indexed Fabric transformation matrices into position, orientation, and scale. + + Like :func:`decompose_fabric_transformation_matrix_to_warp_arrays` but operates on a + :class:`wp.indexedfabricarray` that already encodes the view-to-fabric mapping, removing + the need for a separate ``mapping`` array. + + Args: + fabric_matrices: Indexed fabric array containing 4x4 transformation matrices. + array_positions: Output array for positions [m], shape (N, 3). + array_orientations: Output array for quaternions in xyzw format, shape (N, 4). + array_scales: Output array for scales, shape (N, 3). + indices: View indices to process (subset selection). + """ + output_index = wp.tid() + view_index = indices[output_index] + + position, rotation, scale = _decompose_transformation_matrix(wp.mat44f(fabric_matrices[view_index])) + + if array_positions.shape[0] > 0: + array_positions[output_index, 0] = position[0] + array_positions[output_index, 1] = position[1] + array_positions[output_index, 2] = position[2] + if array_orientations.shape[0] > 0: + array_orientations[output_index, 0] = rotation[0] + array_orientations[output_index, 1] = rotation[1] + array_orientations[output_index, 2] = rotation[2] + array_orientations[output_index, 3] = rotation[3] + if array_scales.shape[0] > 0: + array_scales[output_index, 0] = scale[0] + array_scales[output_index, 1] = scale[1] + array_scales[output_index, 2] = scale[2] + + +@wp.kernel(enable_backward=False) +def compose_indexed_fabric_transforms( + fabric_matrices: IndexedFabricArrayMat44d, + array_positions: ArrayFloat32_2d, + array_orientations: ArrayFloat32_2d, + array_scales: ArrayFloat32_2d, + broadcast_positions: bool, + broadcast_orientations: bool, + broadcast_scales: bool, + indices: ArrayUInt32, +): + """Compose indexed Fabric transformation matrices from position, orientation, and scale. + + Like :func:`compose_fabric_transformation_matrix_from_warp_arrays` but operates on a + :class:`wp.indexedfabricarray` that already encodes the view-to-fabric mapping, removing + the need for a separate ``mapping`` array. + + Args: + fabric_matrices: Indexed fabric array containing 4x4 transformation matrices to update. + array_positions: Input array for positions [m], shape (N, 3). + array_orientations: Input array for quaternions in xyzw format, shape (N, 4). + array_scales: Input array for scales, shape (N, 3). + broadcast_positions: If True, use first position for all prims. + broadcast_orientations: If True, use first orientation for all prims. + broadcast_scales: If True, use first scale for all prims. + indices: View indices to process (subset selection). + """ + i = wp.tid() + view_index = indices[i] + position, rotation, scale = _decompose_transformation_matrix(wp.mat44f(fabric_matrices[view_index])) + + if array_positions.shape[0] > 0: + index = wp.where(broadcast_positions, 0, i) + position[0] = array_positions[index, 0] + position[1] = array_positions[index, 1] + position[2] = array_positions[index, 2] + if array_orientations.shape[0] > 0: + index = wp.where(broadcast_orientations, 0, i) + rotation[0] = array_orientations[index, 0] + rotation[1] = array_orientations[index, 1] + rotation[2] = array_orientations[index, 2] + rotation[3] = array_orientations[index, 3] + if array_scales.shape[0] > 0: + index = wp.where(broadcast_scales, 0, i) + scale[0] = array_scales[index, 0] + scale[1] = array_scales[index, 1] + scale[2] = array_scales[index, 2] + + fabric_matrices[view_index] = wp.mat44d( # type: ignore[arg-type] + wp.transpose(wp.transform_compose(position, rotation, scale)) # type: ignore[arg-type] + ) + + +@wp.kernel(enable_backward=False) +def update_indexed_local_matrix_from_world( + child_world_matrices: IndexedFabricArrayMat44d, + parent_world_matrices: IndexedFabricArrayMat44d, + child_local_matrices: IndexedFabricArrayMat44d, + indices: ArrayUInt32, +): + """Recompute child localMatrix from (parent worldMatrix, child worldMatrix). + + Computes ``child_local = inv(parent_world) * child_world`` per prim and writes the + result back to the child's :data:`omni:fabric:localMatrix` so that subsequent + ``get_local_poses`` calls see consistent values after a world-pose write. + + All three indexed arrays are expected to be indexed by the same per-view indices + (i.e. ``view_to_child_fabric``, ``view_to_parent_fabric``, ``view_to_child_fabric``) + so the kernel only needs the view-side indices. + + Storage convention: Fabric matrices are stored as the transpose of the standard + column-major math convention. Math is ``local = inv(parent) * world``; under + the transpose identity ``(A * B)^T = B^T * A^T`` (and ``inv(A^T) = inv(A)^T``) + that is equivalent to storage-side ``local^T = world^T * inv(parent^T)``, so we + can compute it directly on the stored matrices without explicit transposes. + + Args: + child_world_matrices: Indexed fabric array of child world matrices (read). + parent_world_matrices: Indexed fabric array of parent world matrices (read). + child_local_matrices: Indexed fabric array of child local matrices (written). + indices: View indices to process. + """ + i = wp.tid() + view_index = indices[i] + child_world = wp.mat44f(child_world_matrices[view_index]) + parent_world = wp.mat44f(parent_world_matrices[view_index]) + child_local_matrices[view_index] = wp.mat44d(child_world * wp.inverse(parent_world)) # type: ignore[arg-type] + + +@wp.kernel(enable_backward=False) +def update_indexed_world_matrix_from_local( + child_local_matrices: IndexedFabricArrayMat44d, + parent_world_matrices: IndexedFabricArrayMat44d, + child_world_matrices: IndexedFabricArrayMat44d, + indices: ArrayUInt32, +): + """Recompute child worldMatrix from (parent worldMatrix, child localMatrix). + + Computes ``child_world = parent_world * child_local`` per prim and writes the + result back to the child's :data:`omni:fabric:worldMatrix`. Used after a + ``set_local_poses`` write so that subsequent ``get_world_poses`` calls see + consistent values. Mirror of :func:`update_indexed_local_matrix_from_world`. + + Args: + child_local_matrices: Indexed fabric array of child local matrices (read). + parent_world_matrices: Indexed fabric array of parent world matrices (read). + child_world_matrices: Indexed fabric array of child world matrices (written). + indices: View indices to process. + + Storage convention: same as :func:`update_indexed_local_matrix_from_world`. + Math is ``world = parent * local``; under the transpose identity that becomes + storage-side ``world^T = local^T * parent^T``, no explicit transposes needed. + """ + i = wp.tid() + view_index = indices[i] + child_local = wp.mat44f(child_local_matrices[view_index]) + parent_world = wp.mat44f(parent_world_matrices[view_index]) + child_world_matrices[view_index] = wp.mat44d(child_local * parent_world) # type: ignore[arg-type] + + @wp.func def _decompose_transformation_matrix(m: Any): # -> tuple[wp.vec3f, wp.quatf, wp.vec3f] """Decompose a 4x4 transformation matrix into position, orientation, and scale. diff --git a/source/isaaclab/isaaclab/visualizers/base_visualizer.py b/source/isaaclab/isaaclab/visualizers/base_visualizer.py index b0fc5a81088f..92f8c37ce947 100644 --- a/source/isaaclab/isaaclab/visualizers/base_visualizer.py +++ b/source/isaaclab/isaaclab/visualizers/base_visualizer.py @@ -14,7 +14,7 @@ from typing import TYPE_CHECKING, Any if TYPE_CHECKING: - from isaaclab.physics import SceneDataProvider + from isaaclab.scene.scene_data_provider import SceneDataProvider from .visualizer_cfg import VisualizerCfg @@ -147,9 +147,9 @@ def _compute_visualized_env_ids(self) -> list[int] | None: if self._scene_data_provider is None: return None cfg = self.cfg - num_envs = self._scene_data_provider.get_metadata().get("num_envs", 0) + num_envs = self._scene_data_provider.num_envs if num_envs <= 0: - logger.debug("[Visualizer] num_envs is 0 or missing from provider metadata; env selection disabled.") + logger.debug("[Visualizer] num_envs is 0 or missing from provider; env selection disabled.") return None # Explicit list wins; never combine with random cap-only mode. if cfg.visible_env_indices is not None: diff --git a/source/isaaclab/setup.py b/source/isaaclab/setup.py index 67c18c4c62d1..1f3be503574e 100644 --- a/source/isaaclab/setup.py +++ b/source/isaaclab/setup.py @@ -30,19 +30,18 @@ # procedural-generation "trimesh", "pyglet>=2.1.6,<3", - "mujoco==3.6.0", - "mujoco-warp==3.6.0", # image processing "transformers==4.57.6", "einops", # needed for transformers, doesn't always auto-install - "warp-lang==1.12.0", + "warp-lang==1.13.0", "matplotlib>=3.10.3", # minimum version for Python 3.12 support # make sure this is consistent with isaac sim version "pillow==12.1.1", # required by omni.replicator.core S3 backend "botocore", # livestream - "starlette==0.49.1", + # range chosen to coexist with isaacsim 6.0 (isaacsim-kernel pulls fastapi==0.117.1 -> starlette<0.49.0) + "starlette>=0.46.0,<0.50", "omniverseclient==2.71.1.7015", # testing "pytest", @@ -54,6 +53,8 @@ "flaky", "packaging", "psutil", + # cross-platform file locking (used to serialize USD spawn across distributed ranks) + "filelock", # Required by pydantic-core/imgui_bundle on Python 3.12 (Sentinel symbol). "typing_extensions>=4.14.0", "lazy_loader>=0.4", @@ -66,7 +67,7 @@ # required by isaaclab.isaaclab.controllers.pink_ik f"pin ; platform_system == 'Linux' and ({SUPPORTED_ARCHS_ARM})", f"pin-pink==3.1.0 ; platform_system == 'Linux' and ({SUPPORTED_ARCHS_ARM})", - f"daqp==0.7.2 ; platform_system == 'Linux' and ({SUPPORTED_ARCHS_ARM})", + f"daqp==0.8.5 ; platform_system == 'Linux' and ({SUPPORTED_ARCHS_ARM})", ] # Adds OpenUSD dependencies based on architecture for Kit less mode. INSTALL_REQUIRES += [ diff --git a/source/isaaclab/test/assets/test_articulation_iface.py b/source/isaaclab/test/assets/test_articulation_iface.py index 3e471026a9dc..8dcc2b0ebc43 100644 --- a/source/isaaclab/test/assets/test_articulation_iface.py +++ b/source/isaaclab/test/assets/test_articulation_iface.py @@ -20,9 +20,13 @@ from unittest.mock import MagicMock # When running kitless (e.g., ovphysx backend via run_ovphysx.sh), AppLauncher -# will try to boot Kit and hang. Skip it entirely when LD_PRELOAD is cleared -# (the signature of run_ovphysx.sh) or when EXP_PATH is not set. -_kitless = os.environ.get("LD_PRELOAD", "") == "" and "EXP_PATH" not in os.environ +# will try to boot Kit and hang. Skip it entirely: run_ovphysx.sh sets +# LD_PRELOAD to the ovphysx libcarb.so, which is the signature of a kitless +# ovphysx run. Also guard the case where neither LD_PRELOAD nor EXP_PATH is +# set (bare Python, no Kit at all). +_kitless = "ovphysx" in os.environ.get("LD_PRELOAD", "") or ( + os.environ.get("LD_PRELOAD", "") == "" and "EXP_PATH" not in os.environ +) if not _kitless: from isaaclab.app import AppLauncher @@ -30,6 +34,19 @@ simulation_app = AppLauncher(headless=True).app else: simulation_app = None + # Stub out the Kit/Omniverse modules that are not present under + # run_ovphysx.sh (pxr, carb, omni, omni.kit[.app] are real on PYTHONPATH). + # ``omni`` is a real namespace package, so missing submodules also need + # to be installed as attributes on it -- ``sys.modules`` alone is not + # enough because attribute access on the real ``omni`` won't fall + # through to ``sys.modules``. + import omni as _omni + + for _mod in ("physics", "physics.tensors", "physx", "timeline", "usd"): + _stub = MagicMock() + sys.modules[f"omni.{_mod}"] = _stub + # Bind the leaf attribute so that ``omni.`` resolves. + setattr(_omni, _mod.split(".", 1)[0], _stub) for _mod in ("isaacsim.core", "isaacsim.core.simulation_manager"): sys.modules.setdefault(_mod, MagicMock()) @@ -329,6 +346,14 @@ def create_newton_articulation( # Mock NewtonManager (aliased as SimulationManager in Newton modules) mock_model = MagicMock() mock_model.gravity = wp.array(np.array([[0.0, 0.0, -9.81]], dtype=np.float32), dtype=wp.vec3f, device=device) + # Sizes consumed by the task-space scratch buffers in NewtonArticulationData.__init__. + # Model-wide counts equal the per-articulation counts here because the mock contains a + # single homogeneous world. + mock_model.articulation_count = num_instances + mock_model.max_joints_per_articulation = num_bodies + mock_model.max_dofs_per_articulation = num_joints + mock_model.joint_dof_count = num_instances * num_joints + mock_model.body_count = num_instances * num_bodies mock_state = MagicMock() mock_control = MagicMock() diff --git a/source/isaaclab/test/assets/test_rigid_object_iface.py b/source/isaaclab/test/assets/test_rigid_object_iface.py index 178feeddb603..772130149ee3 100644 --- a/source/isaaclab/test/assets/test_rigid_object_iface.py +++ b/source/isaaclab/test/assets/test_rigid_object_iface.py @@ -13,16 +13,42 @@ The setup is a bit convoluted so that we can run these tests without requiring Isaac Sim or GPU simulation. """ -"""Launch Isaac Sim Simulator first.""" +"""Launch Isaac Sim Simulator first (when available).""" -from isaaclab.app import AppLauncher - -HEADLESS = True +import os +import sys +from unittest.mock import MagicMock -# launch omniverse app -simulation_app = AppLauncher(headless=True).app +# When running kitless (e.g., ovphysx backend via run_ovphysx.sh), AppLauncher +# will try to boot Kit and hang. Skip it entirely: run_ovphysx.sh sets +# LD_PRELOAD to the ovphysx libcarb.so, which is the signature of a kitless +# ovphysx run. Also guard the case where neither LD_PRELOAD nor EXP_PATH is +# set (bare Python, no Kit at all). +_kitless = "ovphysx" in os.environ.get("LD_PRELOAD", "") or ( + os.environ.get("LD_PRELOAD", "") == "" and "EXP_PATH" not in os.environ +) -from unittest.mock import MagicMock +if not _kitless: + from isaaclab.app import AppLauncher + + simulation_app = AppLauncher(headless=True).app +else: + simulation_app = None + # Stub out the Kit/Omniverse modules that are not present under + # run_ovphysx.sh (pxr, carb, omni, omni.kit[.app] are real on PYTHONPATH). + # ``omni`` is a real namespace package, so missing submodules also need + # to be installed as attributes on it -- ``sys.modules`` alone is not + # enough because attribute access on the real ``omni`` won't fall + # through to ``sys.modules``. + import omni as _omni + + for _mod in ("physics", "physics.tensors", "physx", "timeline", "usd"): + _stub = MagicMock() + sys.modules[f"omni.{_mod}"] = _stub + # Bind the leaf attribute so that ``omni.`` resolves. + setattr(_omni, _mod.split(".", 1)[0], _stub) + for _mod in ("isaacsim.core", "isaacsim.core.simulation_manager"): + sys.modules.setdefault(_mod, MagicMock()) import numpy as np import pytest @@ -66,6 +92,15 @@ except ImportError: pass +try: + from isaaclab_ovphysx.assets.rigid_object.rigid_object import RigidObject as OvPhysxRigidObject + from isaaclab_ovphysx.assets.rigid_object.rigid_object_data import RigidObjectData as OvPhysxRigidObjectData + from isaaclab_ovphysx.test.mock_interfaces.views import MockOvPhysxBindingSet + + BACKENDS.append("ovphysx") +except (ImportError, AttributeError): + pass + def create_physx_rigid_object( num_instances: int = 2, @@ -206,6 +241,62 @@ def create_newton_rigid_object( return rigid_object, mock_view +def create_ovphysx_rigid_object( + num_instances: int = 2, + device: str = "cuda:0", +): + """Create a test OvPhysX RigidObject instance with mocked tensor bindings.""" + body_names = ["base_link"] + + obj = object.__new__(OvPhysxRigidObject) + + obj.cfg = RigidObjectCfg(prim_path="/World/object") + + # Create mock binding set + mock_bindings = MockOvPhysxBindingSet( + num_instances=num_instances, + num_joints=0, + num_bodies=1, + body_names=body_names, + asset_kind="rigid_object", + ) + mock_bindings.set_random_data() + + object.__setattr__(obj, "_device", device) + object.__setattr__(obj, "_ovphysx", MagicMock()) + object.__setattr__(obj, "_bindings", mock_bindings.bindings) + object.__setattr__(obj, "_num_instances", num_instances) + object.__setattr__(obj, "_num_bodies", 1) + object.__setattr__(obj, "_body_names", body_names) + + # Create RigidObjectData + data = OvPhysxRigidObjectData(mock_bindings.bindings, device) + data.num_instances = num_instances + data.num_bodies = 1 + data._is_primed = True + object.__setattr__(obj, "_data", data) + + # Build the buffers RigidObject normally allocates in _initialize_impl + # (_ALL_INDICES, _ALL_*_MASK, pinned CPU staging buffers, wrench buf). + # _create_buffers also instantiates real WrenchComposers; those get + # replaced with mocks just below. + obj._create_buffers() + + # Replace the real wrench composers with mocks for iface coverage. + mock_inst_wrench = MockWrenchComposer(obj) + mock_perm_wrench = MockWrenchComposer(obj) + object.__setattr__(obj, "_instantaneous_wrench_composer", mock_inst_wrench) + object.__setattr__(obj, "_permanent_wrench_composer", mock_perm_wrench) + + # Prevent __del__ / _clear_callbacks from raising + object.__setattr__(obj, "_initialize_handle", None) + object.__setattr__(obj, "_invalidate_initialize_handle", None) + object.__setattr__(obj, "_prim_deletion_handle", None) + object.__setattr__(obj, "_debug_vis_handle", None) + + return obj, mock_bindings + + def create_mock_rigid_object( num_instances: int = 2, device: str = "cuda:0", @@ -226,6 +317,8 @@ def get_rigid_object( ): if backend == "physx": return create_physx_rigid_object(num_instances, device) + elif backend == "ovphysx": + return create_ovphysx_rigid_object(num_instances, device) elif backend == "newton": return create_newton_rigid_object(num_instances, device) elif backend.lower() == "mock": diff --git a/source/isaaclab/test/cli/test_install_commands.py b/source/isaaclab/test/cli/test_install_commands.py index a7c89ccd9d53..f773d9f026cf 100644 --- a/source/isaaclab/test/cli/test_install_commands.py +++ b/source/isaaclab/test/cli/test_install_commands.py @@ -17,6 +17,7 @@ import pytest +import isaaclab.cli.commands.install as install_cmd from isaaclab.cli.commands.install import ( _PREBUNDLE_REPOINT_PACKAGES, _ensure_cuda_torch, @@ -68,6 +69,167 @@ def _make_site_packages( return site_pkgs +# --------------------------------------------------------------------------- +# _install_isaaclab_submodules targeted dependency upgrades +# --------------------------------------------------------------------------- + + +class TestInstallSubmodulesTargetedDependencyUpgrades: + """Tests for extension.toml-driven dependency upgrades.""" + + def _make_extension(self, tmp_path, extension_toml: str) -> Path: + """Create a minimal installable extension fixture.""" + source_dir = tmp_path / "source" + extension_dir = source_dir / "isaaclab_teleop" + config_dir = extension_dir / "config" + config_dir.mkdir(parents=True) + (extension_dir / "setup.py").write_text("# test fixture\n", encoding="utf-8") + (config_dir / "extension.toml").write_text(extension_toml, encoding="utf-8") + return extension_dir + + def test_installs_editable_then_upgrades_declared_dependency_from_metadata(self, tmp_path): + """An opted-in dependency is upgraded using the requirement recorded in installed metadata.""" + extension_dir = self._make_extension( + tmp_path, + '[isaac_lab_settings]\npip_upgrade_dependencies = ["isaacteleop"]\n', + ) + + python_exe = str(tmp_path / "python") + pip_cmd = [python_exe, "-m", "pip"] + isaacteleop_req = 'isaacteleop[cloudxr,retargeters,ui] ~=1.2.0; platform_system == "Linux"' + + with ( + mock.patch("isaaclab.cli.commands.install.ISAACLAB_ROOT", tmp_path), + mock.patch("isaaclab.cli.commands.install.extract_python_exe", return_value=python_exe), + mock.patch("isaaclab.cli.commands.install.get_pip_command", return_value=pip_cmd), + mock.patch( + "isaaclab.cli.commands.install._get_installed_distribution_requirements", + return_value=[isaacteleop_req], + ), + mock.patch("isaaclab.cli.commands.install.run_command") as mock_run, + ): + install_cmd._install_isaaclab_submodules(["isaaclab_teleop"]) + + assert [call.args[0] for call in mock_run.call_args_list] == [ + pip_cmd + ["install", "--editable", str(extension_dir)], + pip_cmd + ["install", "--upgrade", isaacteleop_req], + ] + + def test_uv_install_uses_upgrade_package_for_declared_dependency(self, tmp_path): + """uv upgrades only the declared package rather than using a global upgrade.""" + extension_dir = self._make_extension( + tmp_path, + '[isaac_lab_settings]\npip_upgrade_dependencies = ["isaacteleop"]\n', + ) + + python_exe = str(tmp_path / "python") + pip_cmd = ["uv", "pip"] + isaacteleop_req = 'isaacteleop[cloudxr,retargeters,ui] ~=1.2.0; platform_system == "Linux"' + + with ( + mock.patch("isaaclab.cli.commands.install.ISAACLAB_ROOT", tmp_path), + mock.patch("isaaclab.cli.commands.install.extract_python_exe", return_value=python_exe), + mock.patch("isaaclab.cli.commands.install.get_pip_command", return_value=pip_cmd), + mock.patch( + "isaaclab.cli.commands.install._get_installed_distribution_requirements", + return_value=[isaacteleop_req], + ), + mock.patch("isaaclab.cli.commands.install.run_command") as mock_run, + ): + install_cmd._install_isaaclab_submodules(["isaaclab_teleop"]) + + assert [call.args[0] for call in mock_run.call_args_list] == [ + pip_cmd + ["install", "--editable", str(extension_dir)], + pip_cmd + ["install", "--upgrade-package", "isaacteleop", isaacteleop_req], + ] + + def test_upgrades_all_matching_metadata_requirements(self, tmp_path): + """Duplicate metadata entries are preserved instead of collapsing to one requirement.""" + python_exe = str(tmp_path / "python") + pip_cmd = [python_exe, "-m", "pip"] + linux_req = 'example-package>=1.0; platform_system == "Linux"' + windows_req = 'example_package>=2.0; platform_system == "Windows"' + + with ( + mock.patch( + "isaaclab.cli.commands.install._get_installed_distribution_requirements", + return_value=[linux_req, windows_req], + ), + mock.patch("isaaclab.cli.commands.install.run_command") as mock_run, + ): + install_cmd._upgrade_extension_pip_dependencies( + python_exe, + pip_cmd, + "isaaclab_teleop", + ["example-package"], + ) + + assert [call.args[0] for call in mock_run.call_args_list] == [ + pip_cmd + ["install", "--upgrade", linux_req], + pip_cmd + ["install", "--upgrade", windows_req], + ] + + def test_skips_duplicate_declared_dependency_names(self, tmp_path): + """Duplicate TOML dependency names do not trigger duplicate pip commands.""" + python_exe = str(tmp_path / "python") + pip_cmd = [python_exe, "-m", "pip"] + req = "isaacteleop~=1.2.0" + + with ( + mock.patch( + "isaaclab.cli.commands.install._get_installed_distribution_requirements", + return_value=[req], + ), + mock.patch("isaaclab.cli.commands.install.run_command") as mock_run, + ): + install_cmd._upgrade_extension_pip_dependencies( + python_exe, + pip_cmd, + "isaaclab_teleop", + ["isaacteleop", "IsaacTeleop"], + ) + + mock_run.assert_called_once_with(pip_cmd + ["install", "--upgrade", req]) + + def test_skips_when_toml_has_no_upgrade_dependencies(self, tmp_path): + """Extensions without pip upgrade opt-ins do not trigger metadata probes.""" + extension_dir = self._make_extension(tmp_path, "[isaac_lab_settings]\n") + + assert install_cmd._get_extension_pip_upgrade_dependencies(extension_dir) == [] + + def test_warns_and_skips_invalid_upgrade_dependency_names(self, tmp_path): + """Invalid TOML value types warn and disable targeted upgrades.""" + extension_dir = self._make_extension( + tmp_path, + '[isaac_lab_settings]\npip_upgrade_dependencies = "isaacteleop"\n', + ) + + with mock.patch("isaaclab.cli.commands.install.print_warning") as mock_warning: + assert install_cmd._get_extension_pip_upgrade_dependencies(extension_dir) == [] + + mock_warning.assert_called_once() + + def test_warns_when_declared_dependency_missing_from_metadata(self, tmp_path): + """A declared dependency name must exist in installed package metadata.""" + with ( + mock.patch( + "isaaclab.cli.commands.install._get_installed_distribution_requirements", + return_value=["dex-retargeting==0.5.0"], + ), + mock.patch("isaaclab.cli.commands.install.print_warning") as mock_warning, + mock.patch("isaaclab.cli.commands.install.run_command") as mock_run, + ): + install_cmd._upgrade_extension_pip_dependencies( + str(tmp_path / "python"), + [str(tmp_path / "python"), "-m", "pip"], + "isaaclab_teleop", + ["isaacteleop"], + ) + + mock_warning.assert_called_once() + mock_run.assert_not_called() + + # --------------------------------------------------------------------------- # _torch_first_on_sys_path_is_prebundle # --------------------------------------------------------------------------- diff --git a/source/isaaclab/test/controllers/test_differential_ik.py b/source/isaaclab/test/controllers/test_differential_ik.py index 47abc78e0de6..2ba7af0ec028 100644 --- a/source/isaaclab/test/controllers/test_differential_ik.py +++ b/source/isaaclab/test/controllers/test_differential_ik.py @@ -14,7 +14,6 @@ import pytest import torch -import warp as wp import isaaclab.sim as sim_utils from isaaclab import cloner @@ -199,7 +198,7 @@ def _run_ik_controller( # at reset, the jacobians are not updated to the latest state # so we MUST skip the first step # obtain quantities from simulation - jacobian = wp.to_torch(robot.root_view.get_jacobians())[:, ee_jacobi_idx, :, arm_joint_ids] + jacobian = robot.data.body_link_jacobian_w.torch[:, ee_jacobi_idx, :, arm_joint_ids] ee_pose_w = robot.data.body_pose_w.torch[:, ee_frame_idx] root_pose_w = robot.data.root_pose_w.torch base_rot = root_pose_w[:, 3:7] diff --git a/source/isaaclab/test/controllers/test_operational_space.py b/source/isaaclab/test/controllers/test_operational_space.py index 63c1633c5f7c..80f1f8952623 100644 --- a/source/isaaclab/test/controllers/test_operational_space.py +++ b/source/isaaclab/test/controllers/test_operational_space.py @@ -14,7 +14,6 @@ import pytest import torch -import warp as wp from flaky import flaky import isaaclab.envs.mdp as mdp @@ -1299,8 +1298,11 @@ class _FloatingBaseOscActionsCfg: controller_cfg=OperationalSpaceControllerCfg( target_types=["pose_abs"], impedance_mode="fixed", + # Both flags enabled so the action term fetches mass matrix AND + # gravity each step, exercising the floating-base +6 indexing on + # both quantities. inertial_dynamics_decoupling=True, - gravity_compensation=False, + gravity_compensation=True, motion_stiffness_task=500.0, motion_damping_ratio_task=1.0, ), @@ -1330,14 +1332,16 @@ def test_floating_base_osc_action_term_indexing(): """Regression test for #4999 / PR #5107: verify OperationalSpaceControllerAction uses correct indices for mass matrix and gravity on floating-base robots. - For floating-base robots, PhysX prepends 6 virtual DOFs to the generalized mass matrix and - gravity vectors. The action term's ``_compute_dynamic_quantities()`` must use - ``_jacobi_joint_idx`` (with +6 offset) instead of ``_joint_ids``. This test instantiates the - real action term via a ManagerBasedEnv, triggers ``_compute_dynamic_quantities()``, and verifies - the extracted mass matrix and gravity match a manual extraction using the correct PhysX indices. + The Jacobian / mass-matrix / gravity-comp DoF axis prepends ``num_base_dofs`` + floating-base columns (``6`` for floating-base, ``0`` for fixed-base). The action + term's ``_compute_dynamic_quantities()`` must use ``_jacobi_joint_idx`` (with the + ``+ num_base_dofs`` shift) instead of ``_joint_ids``. This test instantiates the + real action term via a ManagerBasedEnv, triggers ``_compute_dynamic_quantities()``, + and verifies the extracted mass matrix and gravity match a manual extraction using + the correct indices. - If someone reverts ``_jacobi_joint_idx`` back to ``_joint_ids`` in ``_compute_dynamic_quantities``, - this test will fail. + If someone reverts ``_jacobi_joint_idx`` back to ``_joint_ids`` in + ``_compute_dynamic_quantities``, this test will fail. """ env_cfg = _FloatingBaseOscEnvCfg() env_cfg.sim.device = "cuda:0" @@ -1365,8 +1369,8 @@ def test_floating_base_osc_action_term_indexing(): # --- 5. Manually extract using the CORRECT indices (what the fix does) --- jacobi_joint_idx = action_term._jacobi_joint_idx - full_mass_matrix = wp.to_torch(robot.root_view.get_generalized_mass_matrices()) - full_gravity = wp.to_torch(robot.root_view.get_gravity_compensation_forces()) + full_mass_matrix = robot.data.mass_matrix.torch + full_gravity = robot.data.gravity_compensation_forces.torch manual_mass = full_mass_matrix[:, jacobi_joint_idx, :][:, :, jacobi_joint_idx] manual_gravity = full_gravity[:, jacobi_joint_idx] @@ -1375,10 +1379,10 @@ def test_floating_base_osc_action_term_indexing(): torch.testing.assert_close(term_mass, manual_mass, atol=1e-5, rtol=0) torch.testing.assert_close(term_gravity, manual_gravity, atol=1e-5, rtol=0) - # --- 7. Verify the full PhysX tensor has +6 virtual DOFs --- - expected_physx_dofs = robot.num_joints + 6 - assert full_mass_matrix.shape[1] == expected_physx_dofs, ( - f"Mass matrix should have {expected_physx_dofs} DOFs, got {full_mass_matrix.shape[1]}" + # --- 7. Verify the data-layer tensor exposes the full DoF axis (J + num_base_dofs) --- + expected_dofs = robot.num_joints + robot.num_base_dofs + assert full_mass_matrix.shape[1] == expected_dofs, ( + f"Mass matrix should have {expected_dofs} DoFs, got {full_mass_matrix.shape[1]}" ) # --- 8. Verify correct indices differ from raw joint_ids (the old bug) --- @@ -1386,7 +1390,7 @@ def test_floating_base_osc_action_term_indexing(): original_joint_ids, _ = robot.find_joints(_G1_ARM_JOINT_NAMES) buggy_mass = full_mass_matrix[:, original_joint_ids, :][:, :, original_joint_ids] assert not torch.allclose(term_mass, buggy_mass, atol=1e-6), ( - "Action term mass matrix should NOT match extraction with raw joint_ids (no +6 offset)" + "Action term mass matrix should NOT match extraction with raw joint_ids (no num_base_dofs offset)" ) # --- 9. Verify physically reasonable values --- @@ -1498,8 +1502,9 @@ def _run_op_space_controller( # reset joint state to default default_joint_pos = robot.data.default_joint_pos.torch.clone() default_joint_vel = robot.data.default_joint_vel.torch.clone() - robot.write_joint_state_to_sim(default_joint_pos, default_joint_vel) - robot.set_joint_effort_target(zero_joint_efforts) # Set zero torques in the initial step + robot.write_joint_position_to_sim_index(position=default_joint_pos) + robot.write_joint_velocity_to_sim_index(velocity=default_joint_vel) + robot.set_joint_effort_target_index(target=zero_joint_efforts) # Set zero torques in the initial step robot.write_data_to_sim() robot.reset() # reset contact sensor @@ -1545,7 +1550,7 @@ def _run_op_space_controller( current_joint_vel=joint_vel, nullspace_joint_pos_target=joint_centers, ) - robot.set_joint_effort_target(joint_efforts, joint_ids=arm_joint_ids) + robot.set_joint_effort_target_index(target=joint_efforts, joint_ids=arm_joint_ids) robot.write_data_to_sim() # update marker positions @@ -1590,9 +1595,9 @@ def _update_states( """ # obtain dynamics related quantities from simulation ee_jacobi_idx = ee_frame_idx - 1 - jacobian_w = wp.to_torch(robot.root_view.get_jacobians())[:, ee_jacobi_idx, :, arm_joint_ids] - mass_matrix = wp.to_torch(robot.root_view.get_generalized_mass_matrices())[:, arm_joint_ids, :][:, :, arm_joint_ids] - gravity = wp.to_torch(robot.root_view.get_gravity_compensation_forces())[:, arm_joint_ids] + jacobian_w = robot.data.body_link_jacobian_w.torch[:, ee_jacobi_idx, :, arm_joint_ids] + mass_matrix = robot.data.mass_matrix.torch[:, arm_joint_ids, :][:, :, arm_joint_ids] + gravity = robot.data.gravity_compensation_forces.torch[:, arm_joint_ids] # Convert the Jacobian from world to root frame jacobian_b = jacobian_w.clone() root_rot_matrix = matrix_from_quat(quat_inv(robot.data.root_quat_w.torch)) diff --git a/source/isaaclab/test/envs/test_scale_randomization.py b/source/isaaclab/test/envs/test_scale_randomization.py index af5dc220e63f..ec4c6cd42a96 100644 --- a/source/isaaclab/test/envs/test_scale_randomization.py +++ b/source/isaaclab/test/envs/test_scale_randomization.py @@ -105,7 +105,7 @@ def apply_actions(self): vel_error = -self._asset.data.root_lin_vel_w.torch # set velocity targets self._vel_command[:, :3] = self.p_gain * pos_error + self.d_gain * vel_error - self._asset.write_root_velocity_to_sim(self._vel_command) + self._asset.write_root_velocity_to_sim_index(root_velocity=self._vel_command) @configclass diff --git a/source/isaaclab/test/scene/test_interactive_scene.py b/source/isaaclab/test/scene/test_interactive_scene.py index 31b577db634f..626e4d8e44df 100644 --- a/source/isaaclab/test/scene/test_interactive_scene.py +++ b/source/isaaclab/test/scene/test_interactive_scene.py @@ -20,7 +20,7 @@ import isaaclab.sim as sim_utils from isaaclab.actuators import ImplicitActuatorCfg -from isaaclab.assets import ArticulationCfg, RigidObjectCfg +from isaaclab.assets import ArticulationCfg, RigidObjectCfg, RigidObjectCollectionCfg from isaaclab.physics.scene_data_requirements import SceneDataRequirement from isaaclab.scene import InteractiveScene, InteractiveSceneCfg from isaaclab.sim import build_simulation_context @@ -84,10 +84,10 @@ def test_relative_flag(device, setup_scene): # test is relative == False prev_state = scene.get_state(is_relative=False) - scene["robot"].write_joint_state_to_sim( - position=torch.rand_like(scene["robot"].data.joint_pos.torch), - velocity=torch.rand_like(scene["robot"].data.joint_pos.torch), - ) + joint_pos = torch.rand_like(scene["robot"].data.joint_pos.torch) + joint_vel = torch.rand_like(scene["robot"].data.joint_pos.torch) + scene["robot"].write_joint_position_to_sim_index(position=joint_pos) + scene["robot"].write_joint_velocity_to_sim_index(velocity=joint_vel) next_state = scene.get_state(is_relative=False) assert_state_different(prev_state, next_state) scene.reset_to(prev_state, is_relative=False) @@ -95,10 +95,10 @@ def test_relative_flag(device, setup_scene): # test is relative == True prev_state = scene.get_state(is_relative=True) - scene["robot"].write_joint_state_to_sim( - position=torch.rand_like(scene["robot"].data.joint_pos.torch), - velocity=torch.rand_like(scene["robot"].data.joint_pos.torch), - ) + joint_pos = torch.rand_like(scene["robot"].data.joint_pos.torch) + joint_vel = torch.rand_like(scene["robot"].data.joint_pos.torch) + scene["robot"].write_joint_position_to_sim_index(position=joint_pos) + scene["robot"].write_joint_velocity_to_sim_index(velocity=joint_vel) next_state = scene.get_state(is_relative=True) assert_state_different(prev_state, next_state) scene.reset_to(prev_state, is_relative=True) @@ -114,30 +114,24 @@ def test_reset_to_env_ids_input_types(device, setup_scene): # test env_ids = None prev_state = scene.get_state() - scene["robot"].write_joint_state_to_sim( - position=torch.rand_like(scene["robot"].data.joint_pos.torch), - velocity=torch.rand_like(scene["robot"].data.joint_pos.torch), - ) + joint_pos = torch.rand_like(scene["robot"].data.joint_pos.torch) + joint_vel = torch.rand_like(scene["robot"].data.joint_pos.torch) + scene["robot"].write_joint_position_to_sim_index(position=joint_pos) + scene["robot"].write_joint_velocity_to_sim_index(velocity=joint_vel) scene.reset_to(prev_state, env_ids=None) assert_state_equal(prev_state, scene.get_state()) # test env_ids = torch tensor - scene["robot"].write_joint_state_to_sim( - position=torch.rand_like(scene["robot"].data.joint_pos.torch), - velocity=torch.rand_like(scene["robot"].data.joint_pos.torch), - ) + joint_pos = torch.rand_like(scene["robot"].data.joint_pos.torch) + joint_vel = torch.rand_like(scene["robot"].data.joint_pos.torch) + scene["robot"].write_joint_position_to_sim_index(position=joint_pos) + scene["robot"].write_joint_velocity_to_sim_index(velocity=joint_vel) scene.reset_to(prev_state, env_ids=torch.arange(scene.num_envs, device=scene.device, dtype=torch.int32)) assert_state_equal(prev_state, scene.get_state()) -def test_clone_environments_non_cfg_publishes_clone_plans(monkeypatch: pytest.MonkeyPatch): - """Non-cfg clone path must dispatch physics + USD replicate and publish a ``ClonePlan``. - - Replaces the old test that asserted a per-call visualizer clone callback was invoked. The - visualizer-fn callback was removed in favor of providers reading - :meth:`SimulationContext.get_clone_plans`; this test asserts the new contract: even - without prototype templates, the scene synthesizes a single trivial ClonePlan. - """ +def test_clone_environments_executes_env_root_plan_with_positions(monkeypatch: pytest.MonkeyPatch): + """Env-root plans replicate the whole environment and keep grid positions.""" from isaaclab.cloner import ClonePlan scene = object.__new__(InteractiveScene) @@ -146,24 +140,27 @@ def test_clone_environments_non_cfg_publishes_clone_plans(monkeypatch: pytest.Mo scene.physics_backend = "physx" scene._sensors = {} - set_plans_calls: list = [] - sim_state: dict = {"plans": {}} + set_plan_calls: list = [] + sim_state: dict = {"plan": None} - def _set_clone_plans(plans): - sim_state["plans"] = plans - set_plans_calls.append(plans) + def _set_clone_plan(plan): + sim_state["plan"] = plan + set_plan_calls.append(plan) scene.sim = SimpleNamespace( get_scene_data_requirements=lambda: SceneDataRequirement(), update_scene_data_requirements=lambda requirements: None, - set_clone_plans=_set_clone_plans, - get_clone_plans=lambda: sim_state["plans"], + set_clone_plan=_set_clone_plan, + get_clone_plan=lambda: sim_state["plan"], ) scene.env_fmt = "/World/envs/env_{}" scene._ALL_INDICES = torch.arange(3, dtype=torch.long) scene._default_env_origins = torch.zeros((3, 3), dtype=torch.float32) - scene._is_scene_setup_from_cfg = lambda: False - + scene._clone_plan = ClonePlan( + sources=(scene.env_fmt.format(0),), + destinations=(scene.env_fmt,), + clone_mask=torch.ones((1, scene.num_envs), dtype=torch.bool), + ) # Avoid binding this unit test to global SimulationContext singleton state. monkeypatch.setattr(InteractiveScene, "device", property(lambda self: "cpu")) @@ -198,24 +195,237 @@ def _usd_replicate(stage, *args, **kwargs): mapping = physics_calls[0][1][3] assert mapping.dtype == torch.bool assert mapping.shape == (1, scene.num_envs) - # Plans are published once per clone, regardless of physics/usd flag combinations. - assert len(set_plans_calls) == 1 - plans = set_plans_calls[-1] - assert set(plans.keys()) == {scene.env_fmt} - plan = plans[scene.env_fmt] + assert physics_calls[0][2]["positions"] is scene._default_env_origins + assert usd_calls[0][2]["positions"] is scene._default_env_origins + assert len(set_plan_calls) == 1 + plan = set_plan_calls[-1] assert isinstance(plan, ClonePlan) - assert plan.dest_template == scene.env_fmt - assert plan.prototype_paths == [scene.env_fmt.format(0)] + assert plan.sources == (scene.env_fmt.format(0),) + assert plan.destinations == (scene.env_fmt,) assert plan.clone_mask.shape == (1, scene.num_envs) - assert scene.clone_plans is plans + assert scene.clone_plan is plan physics_calls.clear() usd_calls.clear() - set_plans_calls.clear() + set_plan_calls.clear() scene.clone_environments(copy_from_source=True) assert len(physics_calls) == 0 assert len(usd_calls) == 1 - assert len(set_plans_calls) == 1 + assert len(set_plan_calls) == 1 + + +def test_clone_environments_skips_replication_without_plan(): + """Direct-path cfg scenes publish no plan and do not dispatch cloners.""" + scene = object.__new__(InteractiveScene) + scene._clone_plan = None + set_plan_calls = [] + scene.sim = SimpleNamespace(set_clone_plan=set_plan_calls.append) + + scene.clone_environments(copy_from_source=False) + + assert set_plan_calls == [None] + + +def test_clone_environments_executes_asset_level_plan_without_usd_positions(monkeypatch: pytest.MonkeyPatch): + """Asset-level plans preserve env-root transforms by skipping USD positions.""" + from isaaclab.cloner import ClonePlan + + scene = object.__new__(InteractiveScene) + scene.cfg = SimpleNamespace(replicate_physics=False, num_envs=2) + scene.stage = object() + scene.physics_backend = "physx" + scene._sensors = {} + scene.env_fmt = "/World/envs/env_{}" + scene._ALL_INDICES = torch.arange(2, dtype=torch.long) + scene._default_env_origins = torch.ones((2, 3), dtype=torch.float32) + scene._clone_plan = ClonePlan( + sources=("/World/envs/env_0/Object", "/World/envs/env_1/Object"), + destinations=("/World/envs/env_{}/Object", "/World/envs/env_{}/Object"), + clone_mask=torch.tensor([[True, False], [False, True]], dtype=torch.bool), + ) + + set_plan_calls: list = [] + scene.sim = SimpleNamespace(set_clone_plan=set_plan_calls.append) + monkeypatch.setattr(InteractiveScene, "device", property(lambda self: "cpu")) + + @contextlib.contextmanager + def _noop_fabric_notices(stage, *, restore=True): + yield + + monkeypatch.setattr("isaaclab.scene.interactive_scene.cloner.disabled_fabric_change_notifies", _noop_fabric_notices) + monkeypatch.setattr( + "isaaclab.scene.interactive_scene.cloner.usd_replicate", + lambda *args, **kwargs: usd_calls.append((args, kwargs)), + ) + + physics_calls = [] + usd_calls = [] + scene.cloner_cfg = SimpleNamespace( + device="cpu", + physics_clone_fn=lambda *args, **kwargs: physics_calls.append((args, kwargs)), + clone_usd=True, + ) + + scene.clone_environments(copy_from_source=False) + + assert len(physics_calls) == 1 + assert physics_calls[0][1]["positions"] is scene._default_env_origins + assert len(usd_calls) == 1 + assert usd_calls[0][1]["positions"] is None + assert set_plan_calls == [scene._clone_plan] + + +def test_build_clone_plan_from_cfg_plans_multi_and_single_spawners(monkeypatch: pytest.MonkeyPatch): + """Heterogeneous planning writes source paths for multi and single spawners.""" + from isaaclab.cloner import sequential + + scene = object.__new__(InteractiveScene) + scene.cfg = SimpleNamespace( + num_envs=4, + object=SimpleNamespace( + prim_path="{ENV_REGEX_NS}/Object", + spawn=sim_utils.MultiAssetSpawnerCfg( + assets_cfg=[ + sim_utils.ConeCfg(radius=0.1, height=0.2), + sim_utils.SphereCfg(radius=0.1), + ] + ), + ), + robot=SimpleNamespace( + prim_path="{ENV_REGEX_NS}/Robot", + spawn=sim_utils.CuboidCfg(size=(0.1, 0.1, 0.1)), + ), + ) + scene.env_fmt = "/World/envs/env_{}" + scene.cloner_cfg = SimpleNamespace(clone_strategy=sequential) + monkeypatch.setattr(InteractiveScene, "device", property(lambda self: "cpu")) + + plan = scene._build_clone_plan_from_cfg() + + assert plan is not None + assert plan.sources == ( + "/World/envs/env_0/Object", + "/World/envs/env_1/Object", + "/World/envs/env_0/Robot", + ) + assert plan.destinations == ( + "/World/envs/env_{}/Object", + "/World/envs/env_{}/Object", + "/World/envs/env_{}/Robot", + ) + assert scene.cfg.object.spawn.spawn_paths == ["/World/envs/env_0/Object", "/World/envs/env_1/Object"] + assert scene.cfg.robot.spawn.spawn_path == "/World/envs/env_0/Robot" + assert scene.cfg.object.prim_path == "{ENV_REGEX_NS}/Object" + assert scene.cfg.robot.prim_path == "{ENV_REGEX_NS}/Robot" + assert torch.equal(plan.clone_mask.to(torch.int).argmax(dim=0).cpu(), torch.tensor([0, 1, 0, 1])) + + +def test_build_clone_plan_from_cfg_defaults_to_env0_plan(monkeypatch: pytest.MonkeyPatch): + """Homogeneous cfg scenes use the default env_0-to-all ClonePlan.""" + from isaaclab.cloner import sequential + + scene = object.__new__(InteractiveScene) + scene.cfg = SimpleNamespace( + num_envs=3, + robot=SimpleNamespace( + prim_path="{ENV_REGEX_NS}/Robot", + spawn=sim_utils.CuboidCfg(size=(0.1, 0.1, 0.1)), + ), + ) + scene.env_fmt = "/World/envs/env_{}" + scene.cloner_cfg = SimpleNamespace(clone_strategy=sequential) + monkeypatch.setattr(InteractiveScene, "device", property(lambda self: "cpu")) + + plan = scene._build_clone_plan_from_cfg() + + assert plan is not None + assert plan.sources == ("/World/envs/env_0",) + assert plan.destinations == (scene.env_fmt,) + assert plan.clone_mask.shape == (1, scene.num_envs) + assert scene.cfg.robot.spawn.spawn_path == "/World/envs/env_0/Robot" + + +def test_build_clone_plan_from_cfg_returns_none_without_env_scoped_groups(monkeypatch: pytest.MonkeyPatch): + """Direct-path cfg scenes should not force env-root replication.""" + from isaaclab.cloner import sequential + + scene = object.__new__(InteractiveScene) + scene.cfg = SimpleNamespace( + num_envs=1, + robot=SimpleNamespace( + prim_path="/World/Robot", + spawn=sim_utils.CuboidCfg(size=(0.1, 0.1, 0.1)), + ), + ) + scene.env_fmt = "/World/envs/env_{}" + scene.cloner_cfg = SimpleNamespace(clone_strategy=sequential) + monkeypatch.setattr(InteractiveScene, "device", property(lambda self: "cpu")) + + assert scene._build_clone_plan_from_cfg() is None + assert scene.cfg.robot.spawn.spawn_path is None + + +def test_build_clone_plan_from_cfg_sets_collection_member_paths(monkeypatch: pytest.MonkeyPatch): + """Rigid object collection members are planned independently.""" + from isaaclab.cloner import sequential + + scene = object.__new__(InteractiveScene) + cube_cfg = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/Cube", + spawn=sim_utils.CuboidCfg(size=(0.1, 0.1, 0.1)), + ) + shape_cfg = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/Shape", + spawn=sim_utils.MultiAssetSpawnerCfg( + assets_cfg=[sim_utils.ConeCfg(radius=0.1, height=0.2), sim_utils.SphereCfg(radius=0.1)] + ), + ) + scene.cfg = SimpleNamespace( + num_envs=4, + objects=RigidObjectCollectionCfg(rigid_objects={"cube": cube_cfg, "shape": shape_cfg}), + ) + scene.env_fmt = "/World/envs/env_{}" + scene.cloner_cfg = SimpleNamespace(clone_strategy=sequential) + monkeypatch.setattr(InteractiveScene, "device", property(lambda self: "cpu")) + + plan = scene._build_clone_plan_from_cfg() + + assert plan is not None + planned_cube = scene.cfg.objects.rigid_objects["cube"] + planned_shape = scene.cfg.objects.rigid_objects["shape"] + assert planned_cube.spawn.spawn_path == "/World/envs/env_0/Cube" + assert planned_shape.spawn.spawn_paths == ["/World/envs/env_0/Shape", "/World/envs/env_1/Shape"] + assert "/World/envs/env_{}/Cube" in plan.destinations + assert "/World/envs/env_{}/Shape" in plan.destinations + + +def test_build_clone_plan_from_cfg_marks_unused_variants(monkeypatch: pytest.MonkeyPatch): + """Unused variants keep a mask row but do not get spawned.""" + from isaaclab.cloner import sequential + + scene = object.__new__(InteractiveScene) + scene.cfg = SimpleNamespace( + num_envs=2, + object=SimpleNamespace( + prim_path="{ENV_REGEX_NS}/Object", + spawn=sim_utils.MultiAssetSpawnerCfg( + assets_cfg=[ + sim_utils.ConeCfg(radius=0.1, height=0.2), + sim_utils.CuboidCfg(size=(0.1, 0.1, 0.1)), + sim_utils.SphereCfg(radius=0.1), + ] + ), + ), + ) + scene.env_fmt = "/World/envs/env_{}" + scene.cloner_cfg = SimpleNamespace(clone_strategy=sequential) + monkeypatch.setattr(InteractiveScene, "device", property(lambda self: "cpu")) + + plan = scene._build_clone_plan_from_cfg() + + assert plan is not None + assert scene.cfg.object.spawn.spawn_paths == ["/World/envs/env_0/Object", "/World/envs/env_1/Object", None] + assert plan.clone_mask[2].sum() == 0 def test_aggregate_scene_data_requirements_merges_visualizers_and_renderers(monkeypatch: pytest.MonkeyPatch): diff --git a/source/isaaclab/test/sensors/test_camera.py b/source/isaaclab/test/sensors/test_camera.py index daed8e95773d..99c93fec7323 100644 --- a/source/isaaclab/test/sensors/test_camera.py +++ b/source/isaaclab/test/sensors/test_camera.py @@ -1161,6 +1161,72 @@ def cleanup(self, render_data): Renderer._registry.pop(backend, None) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.isaacsim_ci +def test_camera_pose_update_reflected_in_render(setup_camera_device, device): + """Camera pose changes via FrameView should be visible in rendered depth. + + Moves the camera close then far, renders depth, and verifies that the mean + valid depth from the far position is significantly larger (>1.5×) than the + close position. This validates that Fabric-side pose writes (via + PrepareForReuse) and USD writes are correctly propagated to the RTX + renderer. + """ + sim, _unused_cam_cfg, dt = setup_camera_device + + cam_cfg = CameraCfg( + prim_path="/World/PoseTestCam", + height=128, + width=256, + update_period=0, + update_latest_camera_pose=True, + data_types=["distance_to_camera"], + spawn=sim_utils.PinholeCameraCfg( + focal_length=24.0, + focus_distance=400.0, + horizontal_aperture=20.955, + clipping_range=(0.1, 1.0e5), + ), + ) + camera = Camera(cam_cfg) + try: + sim.reset() + + target = torch.tensor([[0.0, 0.0, 0.0]], dtype=torch.float32, device=camera.device) + max_range = cam_cfg.spawn.clipping_range[1] + + # -- close position -- + eyes_close = torch.tensor([[2.0, 2.0, 2.0]], dtype=torch.float32, device=camera.device) + camera.set_world_poses_from_view(eyes_close, target) + sim.step() + camera.update(dt) + depth_close = camera.data.output["distance_to_camera"].clone() + + # -- far position -- + eyes_far = torch.tensor([[8.0, 8.0, 8.0]], dtype=torch.float32, device=camera.device) + camera.set_world_poses_from_view(eyes_far, target) + sim.step() + camera.update(dt) + depth_far = camera.data.output["distance_to_camera"].clone() + + # -- validate -- + valid_close = depth_close[depth_close < max_range] + valid_far = depth_far[depth_far < max_range] + + assert valid_close.numel() > 0, "No valid close-range depth pixels" + assert valid_far.numel() > 0, "No valid far-range depth pixels" + + mean_close = valid_close.mean().item() + mean_far = valid_far.mean().item() + + assert mean_far > mean_close * 1.5, ( + f"Far depth ({mean_far:.2f}) should be > 1.5× close depth ({mean_close:.2f}). " + "Camera pose change may not be reaching the renderer." + ) + finally: + del camera + + def _populate_scene(): """Add prims to the scene.""" # Ground-plane diff --git a/source/isaaclab/test/sensors/test_ray_caster_camera.py b/source/isaaclab/test/sensors/test_ray_caster_camera.py index a913d38dd833..cc10b092a806 100644 --- a/source/isaaclab/test/sensors/test_ray_caster_camera.py +++ b/source/isaaclab/test/sensors/test_ray_caster_camera.py @@ -898,11 +898,11 @@ def test_output_equal_to_usd_camera_when_intrinsics_set(setup_sim, focal_length_ # set camera position camera_warp.set_world_poses_from_view( - eyes=torch.tensor([[0.001, 0.0, 5.0]], device=camera_warp.device), + eyes=torch.tensor([[0.0, 0.0, 5.0]], device=camera_warp.device), targets=torch.tensor([[0.0, 0.0, 0.0]], device=camera_warp.device), ) camera_usd.set_world_poses_from_view( - eyes=torch.tensor([[0.001, 0.0, 5.0]], device=camera_usd.device), + eyes=torch.tensor([[0.0, 0.0, 5.0]], device=camera_usd.device), targets=torch.tensor([[0.0, 0.0, 0.0]], device=camera_usd.device), ) diff --git a/source/isaaclab/test/sim/test_cloner.py b/source/isaaclab/test/sim/test_cloner.py index 1f8af90387b5..1f526ac74584 100644 --- a/source/isaaclab/test/sim/test_cloner.py +++ b/source/isaaclab/test/sim/test_cloner.py @@ -20,7 +20,7 @@ from pxr import UsdGeom import isaaclab.sim as sim_utils -from isaaclab.cloner import ClonePlan, TemplateCloneCfg, clone_from_template, sequential, usd_replicate +from isaaclab.cloner import make_clone_plan, sequential, usd_replicate from isaaclab.sim import build_simulation_context pytestmark = pytest.mark.isaacsim_ci @@ -221,56 +221,20 @@ def test_clone_decorator_wildcard_patterns( ) -def test_clone_from_template_returns_clone_plan(sim): - """clone_from_template exposes per-group ClonePlan dicts with prototype-to-env masks. - - Builds two USD prototypes under one group, clones across four envs with the deterministic - sequential strategy, and asserts the returned dict has one entry keyed by the group's - destination template, with a ``[2, 4]`` boolean mask whose columns sum to one. - """ - num_clones = 4 - cfg = TemplateCloneCfg(device=sim.cfg.device, clone_strategy=sequential, clone_physics=False) - - sim_utils.create_prim(cfg.template_root, "Xform") - sim_utils.create_prim(f"{cfg.template_root}/Object", "Xform") - sim_utils.create_prim(f"{cfg.template_root}/Object/proto_asset_0", "Xform") - sim_utils.create_prim(f"{cfg.template_root}/Object/proto_asset_1", "Xform") - sim_utils.create_prim("/World/envs", "Xform") - for i in range(num_clones): - sim_utils.create_prim(f"/World/envs/env_{i}", "Xform", translation=(0, 0, 0)) +def test_make_clone_plan_returns_flat_source_rows(sim): + """make_clone_plan exposes the flat source-to-env mask used by scene cloning.""" + plan = make_clone_plan( + [["/World/envs/env_0/Object", "/World/envs/env_1/Object"]], + ["/World/envs/env_{}/Object"], + num_clones=4, + clone_strategy=sequential, + device=sim.cfg.device, + ) - stage = sim_utils.get_current_stage() - plans = clone_from_template(stage, num_clones=num_clones, template_clone_cfg=cfg) - - assert isinstance(plans, dict) - assert list(plans.keys()) == ["/World/envs/env_{}/Object"] - plan = plans["/World/envs/env_{}/Object"] - assert isinstance(plan, ClonePlan) - assert plan.dest_template == "/World/envs/env_{}/Object" - assert sorted(plan.prototype_paths) == [ - "/World/template/Object/proto_asset_0", - "/World/template/Object/proto_asset_1", - ] - assert plan.clone_mask.shape == (2, num_clones) + assert plan.sources == ("/World/envs/env_0/Object", "/World/envs/env_1/Object") + assert plan.destinations == ("/World/envs/env_{}/Object", "/World/envs/env_{}/Object") + assert plan.clone_mask.shape == (2, 4) assert plan.clone_mask.dtype == torch.bool - # Each env gets exactly one prototype (column-sum invariant) assert torch.all(plan.clone_mask.sum(dim=0) == 1) - # Sequential strategy assigns env i → prototype (i % num_protos) - actual_proto_idx = plan.clone_mask.to(torch.int).argmax(dim=0).cpu() - assert torch.equal(actual_proto_idx, torch.tensor([0, 1, 0, 1])) - - -def test_clone_from_template_returns_empty_dict_when_no_prototypes(sim): - """clone_from_template returns an empty dict when no prototypes match the identifier.""" - num_clones = 2 - cfg = TemplateCloneCfg(device=sim.cfg.device, clone_strategy=sequential, clone_physics=False) - - sim_utils.create_prim(cfg.template_root, "Xform") - sim_utils.create_prim("/World/envs", "Xform") - for i in range(num_clones): - sim_utils.create_prim(f"/World/envs/env_{i}", "Xform", translation=(0, 0, 0)) - - stage = sim_utils.get_current_stage() - plans = clone_from_template(stage, num_clones=num_clones, template_clone_cfg=cfg) - - assert plans == {} + actual_source_idx = plan.clone_mask.to(torch.int).argmax(dim=0).cpu() + assert torch.equal(actual_source_idx, torch.tensor([0, 1, 0, 1])) diff --git a/source/isaaclab/test/sim/test_newton_manager_visualization_state.py b/source/isaaclab/test/sim/test_newton_manager_visualization_state.py new file mode 100644 index 000000000000..de0515c57e07 --- /dev/null +++ b/source/isaaclab/test/sim/test_newton_manager_visualization_state.py @@ -0,0 +1,162 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Unit tests for ``NewtonManager.update_visualization_state`` and shadow-model build. + +When the active sim backend is PhysX and a Newton-native visualizer/renderer is in +use, :meth:`NewtonManager._ensure_visualization_model` must build the manager's +``_model`` / ``_state_0`` directly from the USD stage (via +:meth:`NewtonManager._build_visualization_model_from_stage`), and +:meth:`NewtonManager.update_visualization_state` must copy fresh transforms into +``_state_0.body_q`` via the new +:class:`~isaaclab.scene.scene_data_provider.SceneDataProvider`. +""" + +from __future__ import annotations + +from types import SimpleNamespace + + +def _reset_newton_manager_state(): + from isaaclab_newton.physics import NewtonManager + + NewtonManager._builder = None + NewtonManager._model = None + NewtonManager._state_0 = None + NewtonManager._num_envs = None + NewtonManager._physx_visualization_scene_data = None + NewtonManager._physx_visualization_mapping = None + + +def test_ensure_visualization_model_noop_when_backend_is_newton(monkeypatch): + """When sim backend is Newton, the manager keeps its own model/state untouched.""" + from isaaclab_newton.physics import NewtonManager + + _reset_newton_manager_state() + monkeypatch.setattr(NewtonManager, "_backend_is_newton", classmethod(lambda cls: True)) + NewtonManager._ensure_visualization_model() + assert NewtonManager._model is None + assert NewtonManager._state_0 is None + + +def test_ensure_visualization_model_builds_from_stage_when_backend_is_physx(monkeypatch): + """With a PhysX sim backend, the shadow Newton model is built directly from the stage.""" + from isaaclab_newton.physics import NewtonManager + from isaaclab_newton.physics import newton_manager as nm + + _reset_newton_manager_state() + monkeypatch.setattr(NewtonManager, "_backend_is_newton", classmethod(lambda cls: False)) + monkeypatch.setattr(nm, "get_current_stage", lambda *args, **kwargs: SimpleNamespace()) + monkeypatch.setattr(nm, "replace_newton_shape_colors", lambda model, *a, **kw: 0) + + finalize_calls: list[str] = [] + + class _FakeBuilder: + body_count = 3 + + def finalize(self, device): + finalize_calls.append(device) + return SimpleNamespace(state=lambda: SimpleNamespace(body_q=None)) + + monkeypatch.setattr( + NewtonManager, + "_build_visualization_model_from_stage", + classmethod(lambda cls, stage: _FakeBuilder()), + ) + monkeypatch.setattr(nm.PhysicsManager, "_device", "cpu", raising=False) + + NewtonManager._ensure_visualization_model() + + assert finalize_calls == ["cpu"] + assert NewtonManager._model is not None + assert NewtonManager._state_0 is not None + + +def test_ensure_visualization_model_empty_builder_logs_and_skips(monkeypatch, caplog): + """When the stage walk produces no bodies, model/state stay unset and an error is logged.""" + from isaaclab_newton.physics import NewtonManager + from isaaclab_newton.physics import newton_manager as nm + + _reset_newton_manager_state() + monkeypatch.setattr(NewtonManager, "_backend_is_newton", classmethod(lambda cls: False)) + monkeypatch.setattr(nm, "get_current_stage", lambda *args, **kwargs: SimpleNamespace()) + + class _EmptyBuilder: + body_count = 0 + + monkeypatch.setattr( + NewtonManager, + "_build_visualization_model_from_stage", + classmethod(lambda cls, stage: _EmptyBuilder()), + ) + + with caplog.at_level("ERROR"): + NewtonManager._ensure_visualization_model() + + assert NewtonManager._model is None + assert NewtonManager._state_0 is None + assert any("no Newton bodies" in r.message for r in caplog.records) + + +def test_ensure_visualization_model_populates_num_envs_when_backend_is_physx(monkeypatch): + """Shadow-model build must populate ``_num_envs`` so ``get_num_envs`` is correct under PhysX.""" + from isaaclab_newton.physics import NewtonManager + from isaaclab_newton.physics import newton_manager as nm + + _reset_newton_manager_state() + monkeypatch.setattr(NewtonManager, "_backend_is_newton", classmethod(lambda cls: False)) + monkeypatch.setattr(nm, "get_current_stage", lambda *args, **kwargs: SimpleNamespace()) + monkeypatch.setattr(nm, "replace_newton_shape_colors", lambda model, *a, **kw: 0) + + class _FakeBuilder: + body_count = 3 + + def finalize(self, device): + return SimpleNamespace(state=lambda: SimpleNamespace(body_q=None)) + + def _fake_build(cls, stage): + # Mirror the real shadow-build behaviour: writes the env count discovered on the stage. + NewtonManager._num_envs = 4 + return _FakeBuilder() + + monkeypatch.setattr(NewtonManager, "_build_visualization_model_from_stage", classmethod(_fake_build)) + monkeypatch.setattr(nm.PhysicsManager, "_device", "cpu", raising=False) + + NewtonManager._ensure_visualization_model() + + assert NewtonManager.get_num_envs() == 4 + assert NewtonManager._model.num_envs == 4 + + +def test_ensure_visualization_model_missing_stage_leaves_state_unset(monkeypatch, caplog): + """When no USD stage is available, model/state stay unset and an error is logged.""" + from isaaclab_newton.physics import NewtonManager + from isaaclab_newton.physics import newton_manager as nm + + _reset_newton_manager_state() + monkeypatch.setattr(NewtonManager, "_backend_is_newton", classmethod(lambda cls: False)) + monkeypatch.setattr(nm, "get_current_stage", lambda *args, **kwargs: None) + + with caplog.at_level("ERROR"): + NewtonManager._ensure_visualization_model() + + assert NewtonManager._model is None + assert NewtonManager._state_0 is None + assert any("No USD stage available" in r.message for r in caplog.records) + + +def test_update_visualization_state_noop_when_backend_is_newton(monkeypatch): + """When sim backend is Newton, update_visualization_state is a no-op.""" + from isaaclab_newton.physics import NewtonManager + + _reset_newton_manager_state() + monkeypatch.setattr(NewtonManager, "_backend_is_newton", classmethod(lambda cls: True)) + + # Pre-set sentinel values to ensure update doesn't touch them. + NewtonManager._model = "live-model" + NewtonManager._state_0 = "live-state" + NewtonManager.update_visualization_state() + assert NewtonManager._model == "live-model" + assert NewtonManager._state_0 == "live-state" diff --git a/source/isaaclab/test/sim/test_physx_scene_data_provider_visualizer_contract.py b/source/isaaclab/test/sim/test_physx_scene_data_provider_visualizer_contract.py deleted file mode 100644 index 979d66cc4a7e..000000000000 --- a/source/isaaclab/test/sim/test_physx_scene_data_provider_visualizer_contract.py +++ /dev/null @@ -1,221 +0,0 @@ -# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). -# All rights reserved. -# -# SPDX-License-Identifier: BSD-3-Clause - -"""Unit tests for PhysxSceneDataProvider visualizer-facing contracts.""" - -from __future__ import annotations - -import sys -from types import SimpleNamespace - -import pytest -import torch -from isaaclab_physx.scene_data_providers import PhysxSceneDataProvider - -from isaaclab.cloner import ClonePlan - -PROVIDER_MOD = "isaaclab_physx.scene_data_providers.physx_scene_data_provider" - - -def _silent_stage() -> SimpleNamespace: - """Stage stub whose ``GetPrimAtPath`` returns an invalid prim — env xforms read as zero.""" - return SimpleNamespace(GetPrimAtPath=lambda path: SimpleNamespace(IsValid=lambda: False)) - - -@pytest.fixture -def stub_provider(): - """Bare :class:`PhysxSceneDataProvider` with all buffer attrs initialized to defaults. - - Tests assign ``_simulation_context`` and ``_stage`` themselves; everything else is the - pre-build state the build path expects. - """ - p = object.__new__(PhysxSceneDataProvider) - p._device = "cpu" - p._xform_views = {} - p._view_body_index_map = {} - p._view_order_tensors = {} - p._pose_buf_num_bodies = 0 - p._positions_buf = None - p._orientations_buf = None - p._covered_buf = None - p._xform_mask_buf = None - return p - - -@pytest.fixture -def newton_stub(monkeypatch): - """Stub the ``isaaclab_newton`` newton-prebuild module and the side-effect helpers. - - Returned :class:`SimpleNamespace` exposes: - - * ``calls`` — list of kwargs from each prebuild invocation, - * ``model`` / ``state_obj`` — what prebuild returns; tests can override before invoking. - """ - state = SimpleNamespace( - calls=[], - model=SimpleNamespace(body_label=[], articulation_label=[]), - state_obj=object(), - ) - - def _prebuild(**kwargs): - state.calls.append(dict(kwargs)) - return state.model, state.state_obj - - monkeypatch.setitem( - sys.modules, "isaaclab_newton.cloner.newton_replicate", SimpleNamespace(newton_visualizer_prebuild=_prebuild) - ) - monkeypatch.setattr(f"{PROVIDER_MOD}.UsdGeom.GetStageUpAxis", lambda stage: "Z") - monkeypatch.setattr(f"{PROVIDER_MOD}.replace_newton_shape_colors", lambda m, s: None) - return state - - -def test_get_newton_model_returns_model_when_sync_enabled(stub_provider): - """Callers receive the full Newton model from :meth:`get_newton_model`.""" - stub_provider._needs_newton_sync = True - stub_provider._newton_model = "full-model" - assert stub_provider.get_newton_model() == "full-model" - - -def test_build_from_clone_plans_populates_provider_state(stub_provider, newton_stub): - """Building from per-group clone plans sets model, state, and rigid-body paths. - - Asserts the provider derives its own (sources, destinations, mask) from the plans - without consulting any auxiliary spec object: representative source paths are recovered - from ``dest_template.format()``, masks are concatenated - along the prototype axis, and per-env positions are read from stage xforms. - """ - newton_stub.model = SimpleNamespace( - body_label=["/World/envs/env_0/Object/A"], - articulation_label=["/World/envs/env_0/Robot"], - ) - plans = { - "/World/envs/env_{}/Object": ClonePlan( - dest_template="/World/envs/env_{}/Object", - prototype_paths=["/World/template/Object/proto_0", "/World/template/Object/proto_1"], - # proto 0 → env 0, 2 ; proto 1 → env 1, 3 - clone_mask=torch.tensor([[True, False, True, False], [False, True, False, True]], dtype=torch.bool), - ), - "/World/envs/env_{}/Robot": ClonePlan( - dest_template="/World/envs/env_{}/Robot", - prototype_paths=["/World/template/Robot/proto_0"], - clone_mask=torch.ones((1, 4), dtype=torch.bool), - ), - } - stub_provider._simulation_context = SimpleNamespace(get_clone_plans=lambda: plans) - stub_provider._stage = _silent_stage() - - stub_provider._build_newton_model_from_clone_plans() - - assert stub_provider._newton_model is newton_stub.model - assert stub_provider._newton_state is newton_stub.state_obj - assert stub_provider._rigid_body_paths == newton_stub.model.body_label - assert stub_provider._rigid_body_view_paths == newton_stub.model.body_label + newton_stub.model.articulation_label - assert stub_provider._num_envs_at_last_newton_build == 4 - assert stub_provider._last_newton_model_build_source == "built" - - kw = newton_stub.calls[-1] - # Source recovery picks the first-env user per prototype. - assert kw["sources"] == [ - "/World/envs/env_0/Object", - "/World/envs/env_1/Object", - "/World/envs/env_0/Robot", - ] - assert kw["destinations"] == ["/World/envs/env_{}/Object", "/World/envs/env_{}/Object", "/World/envs/env_{}/Robot"] - assert kw["mapping"].shape == (3, 4) - assert kw["positions"].shape == (4, 3) - - -def test_build_from_clone_plans_missing_sets_error_state(stub_provider): - """When no clone plans are published, model/state stay unset.""" - stub_provider._simulation_context = SimpleNamespace(get_clone_plans=lambda: {}) - stub_provider._stage = object() - - stub_provider._build_newton_model_from_clone_plans() - - assert stub_provider._last_newton_model_build_source == "missing" - assert stub_provider._newton_model is None - assert stub_provider._newton_state is None - - -def test_build_from_clone_plans_skips_unused_prototype_rows(stub_provider, newton_stub): - """A prototype row with no assigned env (all-False mask row) is dropped, not raised on. - - When ``num_prototypes > num_envs`` under a sequential strategy (or any strategy that - leaves some prototypes unused), ``clone_mask[row].nonzero()[0]`` would otherwise raise - ``IndexError``. The provider must filter unused rows out of sources/destinations/mask. - """ - # 3 prototypes, 2 envs, sequential: env 0 → proto 0, env 1 → proto 1, proto 2 unused. - plans = { - "/World/envs/env_{}/Object": ClonePlan( - dest_template="/World/envs/env_{}/Object", - prototype_paths=[ - "/World/template/Object/proto_0", - "/World/template/Object/proto_1", - "/World/template/Object/proto_2", - ], - clone_mask=torch.tensor([[True, False], [False, True], [False, False]], dtype=torch.bool), - ) - } - stub_provider._simulation_context = SimpleNamespace(get_clone_plans=lambda: plans) - stub_provider._stage = _silent_stage() - - stub_provider._build_newton_model_from_clone_plans() - - assert stub_provider._last_newton_model_build_source == "built" - kw = newton_stub.calls[-1] - # Unused proto_2 row dropped; only the two assigned prototypes survive. - assert kw["sources"] == ["/World/envs/env_0/Object", "/World/envs/env_1/Object"] - assert kw["mapping"].shape == (2, 2) - - -def test_build_from_clone_plans_uses_dest_template_for_env_lookup(stub_provider, newton_stub): - """Env-origin lookup uses the per-plan ``dest_template`` prefix, not a hardcoded path. - - A scene with a non-default env path (``/Stage/scenes/env_``) should still have its - xform translates read correctly. Replaces the prior hardcoded ``/World/envs/env_``. - """ - visited: list[str] = [] - - def _get_prim(path): - visited.append(path) - return SimpleNamespace(IsValid=lambda: False) - - plans = { - "/Stage/scenes/env_{}/Object": ClonePlan( - dest_template="/Stage/scenes/env_{}/Object", - prototype_paths=["/Stage/template/Object/proto_0"], - clone_mask=torch.ones((1, 3), dtype=torch.bool), - ) - } - stub_provider._simulation_context = SimpleNamespace(get_clone_plans=lambda: plans) - stub_provider._stage = SimpleNamespace(GetPrimAtPath=_get_prim) - - stub_provider._build_newton_model_from_clone_plans() - - assert {f"/Stage/scenes/env_{i}" for i in range(3)} <= set(visited) - assert not any(p.startswith("/World/envs/") for p in visited) - - -def test_clone_plan_is_hashable_with_unhashable_fields(): - """``ClonePlan`` must hash despite carrying a tensor and a list. - - With ``field(hash=False)`` on the unhashable members, hashing operates on - ``dest_template`` only — the natural identity (it is the dict key in - :meth:`SimulationContext.get_clone_plans`). - """ - plan_a = ClonePlan( - dest_template="/World/envs/env_{}/Object", - prototype_paths=["/World/template/Object/proto_0"], - clone_mask=torch.ones((1, 4), dtype=torch.bool), - ) - plan_b = ClonePlan( - dest_template="/World/envs/env_{}/Object", - prototype_paths=["/World/template/Object/proto_99"], - clone_mask=torch.zeros((1, 4), dtype=torch.bool), - ) - assert isinstance(hash(plan_a), int) - # Equality folds in only dest_template, so two plans with the same destination compare - # equal regardless of prototype/mask differences. - assert plan_a == plan_b diff --git a/source/isaaclab/test/sim/test_schemas.py b/source/isaaclab/test/sim/test_schemas.py index 1acda3b149c6..1fb03ede1433 100644 --- a/source/isaaclab/test/sim/test_schemas.py +++ b/source/isaaclab/test/sim/test_schemas.py @@ -13,14 +13,32 @@ """Rest everything follows.""" import math +import warnings import pytest +from isaaclab_physx.sim.schemas import ( + ArticulationRootPropertiesCfg as ArticulationRootDeprecatedAliasCfg, +) +from isaaclab_physx.sim.schemas import ( + CollisionPropertiesCfg as PhysxCollisionPropertiesCfgAlias, +) +from isaaclab_physx.sim.schemas import ( + PhysxArticulationRootPropertiesCfg, + PhysxCollisionPropertiesCfg, + PhysxJointDrivePropertiesCfg, + PhysxRigidBodyPropertiesCfg, +) +from isaaclab_physx.sim.schemas import ( + PhysXCollisionPropertiesCfg as PhysxDeformableCollisionAliasCfg, +) +from isaaclab_physx.sim.spawners.materials import PhysxRigidBodyMaterialCfg, RigidBodyMaterialCfg from pxr import UsdPhysics import isaaclab.sim as sim_utils import isaaclab.sim.schemas as schemas from isaaclab.sim import SimulationCfg, SimulationContext +from isaaclab.sim.spawners.materials import RigidBodyMaterialBaseCfg, spawn_rigid_body_material from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR from isaaclab.utils.string import to_camel_case @@ -44,7 +62,7 @@ def setup_simulation(): stabilization_threshold=5.0, fix_root_link=False, ) - rigid_cfg = schemas.RigidBodyPropertiesCfg( + rigid_cfg = PhysxRigidBodyPropertiesCfg( rigid_body_enabled=True, kinematic_enabled=False, disable_gravity=False, @@ -69,8 +87,8 @@ def setup_simulation(): torsional_patch_radius=1.0, ) mass_cfg = schemas.MassPropertiesCfg(mass=1.0, density=100.0) - joint_cfg = schemas.JointDrivePropertiesCfg( - drive_type="acceleration", max_effort=80.0, max_velocity=10.0, stiffness=10.0, damping=0.1 + joint_cfg = PhysxJointDrivePropertiesCfg( + drive_type="acceleration", max_force=80.0, max_joint_velocity=10.0, stiffness=10.0, damping=0.1 ) yield sim, arti_cfg, rigid_cfg, collision_cfg, mass_cfg, joint_cfg # Teardown @@ -86,12 +104,602 @@ def test_valid_properties_cfg(setup_simulation): This is to ensure that we check that all the properties of the schema are set. """ sim, arti_cfg, rigid_cfg, collision_cfg, mass_cfg, joint_cfg = setup_simulation + # deprecation aliases are nulled by __post_init__ after forwarding to the canonical + # field; exclude them from the all-non-None check. + deprecation_aliases = {"max_velocity", "max_effort"} for cfg in [arti_cfg, rigid_cfg, collision_cfg, mass_cfg, joint_cfg]: - # check nothing is none for k, v in cfg.__dict__.items(): + # skip class-metadata keys (``_usd_*``) and deprecation aliases nulled in __post_init__ + if k.startswith("_") or k in deprecation_aliases: + continue assert v is not None, f"{cfg.__class__.__name__}:{k} is None. Please make sure schemas are valid." +@pytest.mark.isaacsim_ci +def test_max_joint_velocity_on_base_cfg(setup_simulation): + """Setting ``max_joint_velocity`` on the base ``JointDriveBaseCfg`` must author + ``physxJoint:maxJointVelocity`` on the prim, identical to setting it on + the deprecated PhysX subclass. + + Regression test for the Path 2 placement rule: ``max_joint_velocity`` is the + only USD path to ``Model.joint_velocity_limit`` and lives on the base. + """ + sim, _, _, _, _, _ = setup_simulation + stage = sim_utils.get_current_stage() + + base_cfg = schemas.JointDriveBaseCfg( + drive_type="acceleration", + max_force=80.0, + max_joint_velocity=10.0, + stiffness=10.0, + damping=0.1, + ) + + # spawn a minimal articulation with a revolute joint, then write properties. + sim_utils.create_prim("/World/Articulation", prim_type="Xform") + sim_utils.create_prim("/World/Articulation/body0", prim_type="Cube") + sim_utils.create_prim("/World/Articulation/body1", prim_type="Cube") + UsdPhysics.RevoluteJoint.Define(stage, "/World/Articulation/joint_0") + + prim_path = "/World/Articulation/joint_0" + # use unwrapped function (no parent traversal) so this returns the inner bool + schemas.modify_joint_drive_properties.__wrapped__(prim_path, base_cfg) + + # Revolute drives convert rad/s -> deg/s; check the authored value. + attr = stage.GetPrimAtPath(prim_path).GetAttribute("physxJoint:maxJointVelocity") + assert attr.IsValid(), "physxJoint:maxJointVelocity was not authored on the prim" + expected_deg_per_sec = 10.0 * 180.0 / math.pi + assert attr.Get() == pytest.approx(expected_deg_per_sec, rel=1e-6) + + +@pytest.mark.isaacsim_ci +def test_max_velocity_deprecation_alias(setup_simulation): + """Legacy ``max_velocity`` kwarg must forward to ``max_joint_velocity`` and emit + a ``DeprecationWarning``. Behavior must match setting ``max_joint_velocity`` directly. + """ + sim, _, _, _, _, _ = setup_simulation + stage = sim_utils.get_current_stage() + + with pytest.warns(DeprecationWarning, match="max_velocity"): + base_cfg = schemas.JointDriveBaseCfg( + drive_type="acceleration", + max_force=80.0, + max_velocity=10.0, + stiffness=10.0, + damping=0.1, + ) + + assert base_cfg.max_joint_velocity == 10.0 + assert base_cfg.max_velocity is None + + sim_utils.create_prim("/World/Articulation_dep", prim_type="Xform") + sim_utils.create_prim("/World/Articulation_dep/body0", prim_type="Cube") + sim_utils.create_prim("/World/Articulation_dep/body1", prim_type="Cube") + UsdPhysics.RevoluteJoint.Define(stage, "/World/Articulation_dep/joint_0") + prim_path = "/World/Articulation_dep/joint_0" + schemas.modify_joint_drive_properties.__wrapped__(prim_path, base_cfg) + + attr = stage.GetPrimAtPath(prim_path).GetAttribute("physxJoint:maxJointVelocity") + assert attr.IsValid() + assert attr.Get() == pytest.approx(10.0 * 180.0 / math.pi, rel=1e-6) + + +@pytest.mark.isaacsim_ci +def test_max_effort_deprecation_alias(setup_simulation): + """Legacy ``max_effort`` kwarg must forward to ``max_force`` and emit + a ``DeprecationWarning``. Behavior must match setting ``max_force`` directly. + """ + sim, _, _, _, _, _ = setup_simulation + stage = sim_utils.get_current_stage() + + with pytest.warns(DeprecationWarning, match="max_effort"): + base_cfg = schemas.JointDriveBaseCfg( + drive_type="acceleration", + max_effort=42.0, + stiffness=10.0, + damping=0.1, + ) + + assert base_cfg.max_force == 42.0 + assert base_cfg.max_effort is None + + sim_utils.create_prim("/World/Articulation_eff", prim_type="Xform") + sim_utils.create_prim("/World/Articulation_eff/body0", prim_type="Cube") + sim_utils.create_prim("/World/Articulation_eff/body1", prim_type="Cube") + UsdPhysics.PrismaticJoint.Define(stage, "/World/Articulation_eff/joint_0") + prim_path = "/World/Articulation_eff/joint_0" + schemas.modify_joint_drive_properties.__wrapped__(prim_path, base_cfg) + + attr = stage.GetPrimAtPath(prim_path).GetAttribute("drive:linear:physics:maxForce") + assert attr.IsValid() + assert attr.Get() == pytest.approx(42.0, rel=1e-6) + + +@pytest.mark.isaacsim_ci +def test_joint_drive_base_no_physx_schema_when_max_joint_velocity_unset(setup_simulation): + """Regression: setting only UsdPhysics drive fields on JointDriveBaseCfg + must NOT cause PhysxJointAPI to be applied to the prim. Without this, + Newton-targeted users get PhysX schemas stamped on every joint.""" + sim, _, _, _, _, _ = setup_simulation + stage = sim_utils.get_current_stage() + + base_cfg = schemas.JointDriveBaseCfg( + drive_type="acceleration", + max_force=80.0, + stiffness=10.0, + damping=0.1, + # max_joint_velocity intentionally left None + ) + sim_utils.create_prim("/World/Articulation", prim_type="Xform") + sim_utils.create_prim("/World/Articulation/body0", prim_type="Cube") + sim_utils.create_prim("/World/Articulation/body1", prim_type="Cube") + UsdPhysics.RevoluteJoint.Define(stage, "/World/Articulation/joint_0") + + prim_path = "/World/Articulation/joint_0" + schemas.modify_joint_drive_properties.__wrapped__(prim_path, base_cfg) + + applied = stage.GetPrimAtPath(prim_path).GetAppliedSchemas() + assert "PhysxJointAPI" not in applied, ( + f"PhysxJointAPI should not be applied when max_velocity is None; got {list(applied)}" + ) + + +@pytest.mark.isaacsim_ci +def test_disable_gravity_on_base_cfg(setup_simulation): + """Setting disable_gravity on the base RigidBodyBaseCfg must author + physxRigidBody:disableGravity on the prim. PhysX honors per-body; + Newton currently honors at scene level (partial), documented in field + docstring. Regression test for the consumption-gated placement rule.""" + sim, _, _, _, _, _ = setup_simulation + stage = sim_utils.get_current_stage() + + base_cfg = schemas.RigidBodyBaseCfg( + rigid_body_enabled=True, + kinematic_enabled=False, + disable_gravity=True, + ) + sim_utils.create_prim("/World/cube_dg", prim_type="Cube", translation=(0.0, 0.0, 0.62)) + schemas.define_rigid_body_properties("/World/cube_dg", base_cfg) + + prim_path = "/World/cube_dg" + attr = stage.GetPrimAtPath(prim_path).GetAttribute("physxRigidBody:disableGravity") + assert attr.IsValid(), "physxRigidBody:disableGravity was not authored on the prim" + assert attr.Get() is True + applied = stage.GetPrimAtPath(prim_path).GetAppliedSchemas() + assert "PhysxRigidBodyAPI" in applied, ( + f"PhysxRigidBodyAPI must be applied when disable_gravity is set; got {list(applied)}" + ) + + +@pytest.mark.isaacsim_ci +def test_physx_rigid_body_no_physx_schema_when_all_physx_fields_none(setup_simulation): + """Regression: PhysxRigidBodyPropertiesCfg with all PhysX-specific fields + left as None must NOT cause PhysxRigidBodyAPI to be applied to the prim. + The user only authored UsdPhysics-standard fields; the PhysX schema + should not be stamped onto a Newton-targeted asset.""" + sim, _, _, _, _, _ = setup_simulation + stage = sim_utils.get_current_stage() + + cfg = PhysxRigidBodyPropertiesCfg( + rigid_body_enabled=True, + kinematic_enabled=False, + # every PhysX field intentionally left None + ) + sim_utils.create_prim("/World/cube_no_physx", prim_type="Cube", translation=(0.0, 0.0, 0.62)) + schemas.define_rigid_body_properties("/World/cube_no_physx", cfg) + + prim_path = "/World/cube_no_physx" + applied = stage.GetPrimAtPath(prim_path).GetAppliedSchemas() + assert "PhysxRigidBodyAPI" not in applied, ( + f"PhysxRigidBodyAPI should not be applied when no PhysX fields are set; got {list(applied)}" + ) + + +@pytest.mark.isaacsim_ci +def test_rigid_body_material_base_cfg(setup_simulation): + """Setting only UsdPhysics fields on RigidBodyMaterialBaseCfg must author the + three friction/restitution attrs and must NOT apply PhysxMaterialAPI.""" + sim, _, _, _, _, _ = setup_simulation + stage = sim_utils.get_current_stage() + + cfg = RigidBodyMaterialBaseCfg(static_friction=0.7, dynamic_friction=0.6, restitution=0.1) + prim_path = "/World/Looks/BaseMaterial" + spawn_rigid_body_material.__wrapped__(prim_path, cfg) + + prim = stage.GetPrimAtPath(prim_path) + assert prim.GetAttribute("physics:staticFriction").Get() == pytest.approx(0.7) + assert prim.GetAttribute("physics:dynamicFriction").Get() == pytest.approx(0.6) + assert prim.GetAttribute("physics:restitution").Get() == pytest.approx(0.1) + applied = prim.GetAppliedSchemas() + assert "PhysxMaterialAPI" not in applied, ( + f"PhysxMaterialAPI must not be applied for the base cfg; got {list(applied)}" + ) + + +@pytest.mark.isaacsim_ci +def test_physx_rigid_body_material_cfg(setup_simulation): + """Setting a PhysX-namespaced field on PhysxRigidBodyMaterialCfg must author the + namespaced attribute AND apply PhysxMaterialAPI.""" + sim, _, _, _, _, _ = setup_simulation + stage = sim_utils.get_current_stage() + + cfg = PhysxRigidBodyMaterialCfg(static_friction=0.7, compliant_contact_stiffness=100.0) + prim_path = "/World/Looks/PhysxMaterial" + spawn_rigid_body_material.__wrapped__(prim_path, cfg) + + prim = stage.GetPrimAtPath(prim_path) + assert prim.GetAttribute("physics:staticFriction").Get() == pytest.approx(0.7) + assert prim.GetAttribute("physxMaterial:compliantContactStiffness").Get() == pytest.approx(100.0) + applied = prim.GetAppliedSchemas() + assert "PhysxMaterialAPI" in applied, ( + f"PhysxMaterialAPI must be applied when a PhysX field is set; got {list(applied)}" + ) + + +@pytest.mark.isaacsim_ci +def test_rigid_body_material_deprecation_alias(setup_simulation): + """Instantiating the legacy ``RigidBodyMaterialCfg`` name emits exactly one + ``DeprecationWarning`` whose message references the 5.0 removal target.""" + with warnings.catch_warnings(record=True) as caught: + warnings.simplefilter("always") + RigidBodyMaterialCfg() + deprecations = [w for w in caught if issubclass(w.category, DeprecationWarning)] + assert len(deprecations) == 1, f"expected exactly one DeprecationWarning, got {len(deprecations)}" + assert "5.0" in str(deprecations[0].message) + + +@pytest.mark.isaacsim_ci +def test_collision_base_cfg_writes_physx_namespaced_attrs(setup_simulation): + """Setting ``contact_offset`` / ``rest_offset`` on the base ``CollisionBaseCfg`` must + author the ``physxCollision:*`` attributes AND apply ``PhysxCollisionAPI``. Newton's + importer consumes them via the PhysX bridge resolver.""" + sim, _, _, _, _, _ = setup_simulation + stage = sim_utils.get_current_stage() + + base_cfg = schemas.CollisionBaseCfg(collision_enabled=True, contact_offset=0.05, rest_offset=0.001) + sim_utils.create_prim("/World/cube_co", prim_type="Cube", translation=(0.0, 0.0, 0.62)) + schemas.define_collision_properties("/World/cube_co", base_cfg) + + prim = stage.GetPrimAtPath("/World/cube_co") + assert prim.GetAttribute("physxCollision:contactOffset").Get() == pytest.approx(0.05) + assert prim.GetAttribute("physxCollision:restOffset").Get() == pytest.approx(0.001) + applied = prim.GetAppliedSchemas() + assert "PhysxCollisionAPI" in applied, ( + f"PhysxCollisionAPI must be applied when contact_offset/rest_offset are set; got {list(applied)}" + ) + + +@pytest.mark.isaacsim_ci +def test_collision_base_cfg_no_physx_schema_when_only_usd_field_set(setup_simulation): + """Regression: setting only ``collision_enabled`` on ``CollisionBaseCfg`` must NOT + cause ``PhysxCollisionAPI`` to be applied. The user only authored a UsdPhysics-standard + field; the PhysX schema should not be stamped onto a Newton-targeted prim.""" + sim, _, _, _, _, _ = setup_simulation + stage = sim_utils.get_current_stage() + + base_cfg = schemas.CollisionBaseCfg(collision_enabled=True) + sim_utils.create_prim("/World/cube_co_only", prim_type="Cube", translation=(0.0, 0.0, 0.62)) + schemas.define_collision_properties("/World/cube_co_only", base_cfg) + + prim = stage.GetPrimAtPath("/World/cube_co_only") + assert prim.GetAttribute("physics:collisionEnabled").Get() is True + applied = prim.GetAppliedSchemas() + assert "PhysxCollisionAPI" not in applied, ( + f"PhysxCollisionAPI should not be applied when only collision_enabled is set; got {list(applied)}" + ) + + +@pytest.mark.isaacsim_ci +def test_physx_collision_cfg_writes_torsional_patch(setup_simulation): + """Setting ``torsional_patch_radius`` on ``PhysxCollisionPropertiesCfg`` must author + the ``physxCollision:torsionalPatchRadius`` attribute AND apply ``PhysxCollisionAPI``.""" + sim, _, _, _, _, _ = setup_simulation + stage = sim_utils.get_current_stage() + + cfg = PhysxCollisionPropertiesCfg(torsional_patch_radius=1.0) + sim_utils.create_prim("/World/cube_tpr", prim_type="Cube", translation=(0.0, 0.0, 0.62)) + schemas.define_collision_properties("/World/cube_tpr", cfg) + + prim = stage.GetPrimAtPath("/World/cube_tpr") + assert prim.GetAttribute("physxCollision:torsionalPatchRadius").Get() == pytest.approx(1.0) + applied = prim.GetAppliedSchemas() + assert "PhysxCollisionAPI" in applied + + +@pytest.mark.isaacsim_ci +def test_collision_deprecation_alias(setup_simulation): + """Instantiating the legacy ``CollisionPropertiesCfg`` name emits exactly one + ``DeprecationWarning`` whose message references the 5.0 removal target.""" + with warnings.catch_warnings(record=True) as caught: + warnings.simplefilter("always") + PhysxCollisionPropertiesCfgAlias() + deprecations = [w for w in caught if issubclass(w.category, DeprecationWarning)] + assert len(deprecations) == 1, f"expected exactly one DeprecationWarning, got {len(deprecations)}" + assert "5.0" in str(deprecations[0].message) + + +@pytest.mark.isaacsim_ci +def test_physx_capitalx_collision_deprecation_alias(setup_simulation): + """Instantiating the legacy ``PhysXCollisionPropertiesCfg`` (capital X, deformable) + name emits exactly one ``DeprecationWarning`` pointing to + ``PhysxDeformableCollisionPropertiesCfg``.""" + with warnings.catch_warnings(record=True) as caught: + warnings.simplefilter("always") + PhysxDeformableCollisionAliasCfg() + deprecations = [w for w in caught if issubclass(w.category, DeprecationWarning)] + assert len(deprecations) == 1, f"expected exactly one DeprecationWarning, got {len(deprecations)}" + assert "PhysxDeformableCollisionPropertiesCfg" in str(deprecations[0].message) + + +@pytest.mark.isaacsim_ci +def test_articulation_root_base_cfg_writes_articulation_enabled(setup_simulation): + """Setting ``articulation_enabled`` on the base ``ArticulationRootBaseCfg`` must author + ``physxArticulation:articulationEnabled`` AND apply ``PhysxArticulationAPI``. The + PhysX namespace is honored at sim time by PhysX and as a spawn-time guard by the IL + Newton wrapper.""" + sim, _, _, _, _, _ = setup_simulation + stage = sim_utils.get_current_stage() + + base_cfg = schemas.ArticulationRootBaseCfg(articulation_enabled=False) + sim_utils.create_prim("/World/arti_ae", prim_type="Xform") + schemas.define_articulation_root_properties("/World/arti_ae", base_cfg) + + prim = stage.GetPrimAtPath("/World/arti_ae") + assert prim.GetAttribute("physxArticulation:articulationEnabled").Get() is False + applied = prim.GetAppliedSchemas() + assert "PhysxArticulationAPI" in applied, ( + f"PhysxArticulationAPI must be applied when articulation_enabled is set; got {list(applied)}" + ) + + +@pytest.mark.isaacsim_ci +def test_articulation_root_base_no_physx_schema_when_only_fix_root_link_set(setup_simulation): + """Regression: setting only ``fix_root_link`` on ``ArticulationRootBaseCfg`` must NOT + cause ``PhysxArticulationAPI`` to be applied. ``fix_root_link`` is a writer-side flag + materializing ``UsdPhysics.FixedJoint``; it does not author any PhysX-namespaced + attribute. Newton-targeted prims that only set ``fix_root_link`` should not receive + ``PhysxArticulationAPI`` stamping.""" + sim, _, _, _, _, _ = setup_simulation + stage = sim_utils.get_current_stage() + + base_cfg = schemas.ArticulationRootBaseCfg(fix_root_link=False) + sim_utils.create_prim("/World/arti_frl", prim_type="Xform") + schemas.define_articulation_root_properties("/World/arti_frl", base_cfg) + + prim = stage.GetPrimAtPath("/World/arti_frl") + applied = prim.GetAppliedSchemas() + assert "PhysxArticulationAPI" not in applied, ( + f"PhysxArticulationAPI should not be applied when only fix_root_link is set; got {list(applied)}" + ) + + +@pytest.mark.isaacsim_ci +def test_physx_articulation_root_writes_self_collisions(setup_simulation): + """Setting ``enabled_self_collisions`` on ``PhysxArticulationRootPropertiesCfg`` must + author ``physxArticulation:enabledSelfCollisions`` AND apply ``PhysxArticulationAPI``.""" + sim, _, _, _, _, _ = setup_simulation + stage = sim_utils.get_current_stage() + + cfg = PhysxArticulationRootPropertiesCfg(enabled_self_collisions=True) + sim_utils.create_prim("/World/arti_sc", prim_type="Xform") + schemas.define_articulation_root_properties("/World/arti_sc", cfg) + + prim = stage.GetPrimAtPath("/World/arti_sc") + assert prim.GetAttribute("physxArticulation:enabledSelfCollisions").Get() is True + applied = prim.GetAppliedSchemas() + assert "PhysxArticulationAPI" in applied + + +@pytest.mark.isaacsim_ci +def test_articulation_root_deprecation_alias(setup_simulation): + """Instantiating the legacy ``ArticulationRootPropertiesCfg`` name emits exactly one + ``DeprecationWarning`` whose message references the 5.0 removal target.""" + with warnings.catch_warnings(record=True) as caught: + warnings.simplefilter("always") + ArticulationRootDeprecatedAliasCfg() + deprecations = [w for w in caught if issubclass(w.category, DeprecationWarning)] + assert len(deprecations) == 1, f"expected exactly one DeprecationWarning, got {len(deprecations)}" + assert "5.0" in str(deprecations[0].message) + + +@pytest.mark.isaacsim_ci +def test_mesh_collision_base_cfg_writes_approximation_token(setup_simulation): + """``MeshCollisionBaseCfg(mesh_approximation_name="boundingCube")`` authors + ``physics:approximation`` via ``UsdPhysics.MeshCollisionAPI``. No PhysX cooking schema is + applied because the base class declares no PhysX namespace.""" + from pxr import UsdGeom + + sim, _, _, _, _, _ = setup_simulation + stage = sim_utils.get_current_stage() + + UsdGeom.Mesh.Define(stage, "/World/mesh_base") + cfg = schemas.MeshCollisionBaseCfg(mesh_approximation_name="boundingCube") + schemas.define_mesh_collision_properties("/World/mesh_base", cfg) + + prim = stage.GetPrimAtPath("/World/mesh_base") + assert prim.GetAttribute("physics:approximation").Get() == "boundingCube" + applied = prim.GetAppliedSchemas() + # The standard UsdPhysics.MeshCollisionAPI is registered under + # ``PhysicsMeshCollisionAPI`` in the prim's applied-schema list. + assert any("MeshCollisionAPI" in s for s in applied), ( + f"a MeshCollisionAPI schema must be applied; got {list(applied)}" + ) + # no PhysX cooking schema applied for the base class + assert not any(s.startswith("Physx") and "Mesh" in s for s in applied), ( + f"no PhysX mesh schema should be applied for the base class; got {list(applied)}" + ) + + +@pytest.mark.isaacsim_ci +def test_physx_convex_hull_writes_tuning_attrs(setup_simulation): + """Setting tuning fields on ``PhysxConvexHullPropertiesCfg`` authors the + ``physxConvexHullCollision:*`` namespaced attributes AND applies + ``PhysxConvexHullCollisionAPI``.""" + from isaaclab_physx.sim.schemas import PhysxConvexHullPropertiesCfg + + from pxr import UsdGeom + + sim, _, _, _, _, _ = setup_simulation + stage = sim_utils.get_current_stage() + + UsdGeom.Mesh.Define(stage, "/World/mesh_ch") + cfg = PhysxConvexHullPropertiesCfg(hull_vertex_limit=64, min_thickness=0.001) + schemas.define_mesh_collision_properties("/World/mesh_ch", cfg) + + prim = stage.GetPrimAtPath("/World/mesh_ch") + assert prim.GetAttribute("physics:approximation").Get() == "convexHull" + assert prim.GetAttribute("physxConvexHullCollision:hullVertexLimit").Get() == 64 + assert prim.GetAttribute("physxConvexHullCollision:minThickness").Get() == pytest.approx(0.001) + applied = prim.GetAppliedSchemas() + assert "PhysxConvexHullCollisionAPI" in applied + + +@pytest.mark.isaacsim_ci +def test_physx_convex_hull_no_physx_schema_when_no_tuning_fields_set(setup_simulation): + """Regression: ``PhysxConvexHullPropertiesCfg()`` with all tuning fields None must NOT + apply ``PhysxConvexHullCollisionAPI``. The approximation token is still authored on the + standard ``UsdPhysics.MeshCollisionAPI``.""" + from isaaclab_physx.sim.schemas import PhysxConvexHullPropertiesCfg + + from pxr import UsdGeom + + sim, _, _, _, _, _ = setup_simulation + stage = sim_utils.get_current_stage() + + UsdGeom.Mesh.Define(stage, "/World/mesh_ch_default") + cfg = PhysxConvexHullPropertiesCfg() + schemas.define_mesh_collision_properties("/World/mesh_ch_default", cfg) + + prim = stage.GetPrimAtPath("/World/mesh_ch_default") + assert prim.GetAttribute("physics:approximation").Get() == "convexHull" + applied = prim.GetAppliedSchemas() + assert "PhysxConvexHullCollisionAPI" not in applied, ( + f"PhysxConvexHullCollisionAPI should not be applied without tuning fields; got {list(applied)}" + ) + + +@pytest.mark.isaacsim_ci +def test_bounding_cube_default_token(setup_simulation): + """``BoundingCubePropertiesCfg()`` defaults to the ``boundingCube`` token.""" + cfg = schemas.BoundingCubePropertiesCfg() + assert cfg.mesh_approximation_name == "boundingCube" + + +@pytest.mark.isaacsim_ci +@pytest.mark.parametrize( + "name", + [ + "MeshCollisionPropertiesCfg", + "ConvexHullPropertiesCfg", + "ConvexDecompositionPropertiesCfg", + "TriangleMeshPropertiesCfg", + "TriangleMeshSimplificationPropertiesCfg", + "SDFMeshPropertiesCfg", + ], +) +def test_mesh_collision_deprecation_aliases(setup_simulation, name): + """Each legacy mesh-collision class name emits exactly one DeprecationWarning on + instantiation and the warning message references the 5.0 removal target.""" + from isaaclab_physx.sim.schemas import schemas_cfg as physx_cfg + + cls = getattr(physx_cfg, name) + with warnings.catch_warnings(record=True) as caught: + warnings.simplefilter("always") + cls() + deprecations = [w for w in caught if issubclass(w.category, DeprecationWarning)] + assert len(deprecations) == 1, f"{name}: expected one DeprecationWarning, got {len(deprecations)}" + assert "5.0" in str(deprecations[0].message) + + +@pytest.mark.isaacsim_ci +def test_physx_fixed_tendon_relocation(setup_simulation): + """``PhysxFixedTendonPropertiesCfg`` is importable from + :mod:`isaaclab_physx.sim.schemas` and round-trips its fields.""" + from isaaclab_physx.sim.schemas import PhysxFixedTendonPropertiesCfg + + cfg = PhysxFixedTendonPropertiesCfg( + tendon_enabled=True, + stiffness=10.0, + damping=0.5, + limit_stiffness=1.0, + offset=0.1, + rest_length=0.2, + ) + assert cfg.tendon_enabled is True + assert cfg.stiffness == 10.0 + assert cfg.damping == 0.5 + assert cfg.limit_stiffness == 1.0 + assert cfg.offset == 0.1 + assert cfg.rest_length == 0.2 + + +@pytest.mark.isaacsim_ci +def test_fixed_tendon_deprecation_alias(setup_simulation): + """Instantiating the legacy ``FixedTendonPropertiesCfg`` (via the shim) emits exactly + one ``DeprecationWarning`` whose message references the 5.0 removal target.""" + cls = schemas.FixedTendonPropertiesCfg + with warnings.catch_warnings(record=True) as caught: + warnings.simplefilter("always") + cls() + deprecations = [w for w in caught if issubclass(w.category, DeprecationWarning)] + assert len(deprecations) == 1, f"expected one DeprecationWarning, got {len(deprecations)}" + assert "5.0" in str(deprecations[0].message) + + +@pytest.mark.isaacsim_ci +def test_physx_spatial_tendon_relocation(setup_simulation): + """``PhysxSpatialTendonPropertiesCfg`` is importable from + :mod:`isaaclab_physx.sim.schemas` and round-trips its fields.""" + from isaaclab_physx.sim.schemas import PhysxSpatialTendonPropertiesCfg + + cfg = PhysxSpatialTendonPropertiesCfg( + tendon_enabled=True, + stiffness=20.0, + damping=0.25, + limit_stiffness=2.0, + offset=0.05, + ) + assert cfg.tendon_enabled is True + assert cfg.stiffness == 20.0 + assert cfg.damping == 0.25 + assert cfg.limit_stiffness == 2.0 + assert cfg.offset == 0.05 + + +@pytest.mark.isaacsim_ci +def test_spatial_tendon_deprecation_alias(setup_simulation): + """Instantiating the legacy ``SpatialTendonPropertiesCfg`` (via the shim) emits exactly + one ``DeprecationWarning`` whose message references the 5.0 removal target.""" + cls = schemas.SpatialTendonPropertiesCfg + with warnings.catch_warnings(record=True) as caught: + warnings.simplefilter("always") + cls() + deprecations = [w for w in caught if issubclass(w.category, DeprecationWarning)] + assert len(deprecations) == 1, f"expected one DeprecationWarning, got {len(deprecations)}" + assert "5.0" in str(deprecations[0].message) + + +@pytest.mark.isaacsim_ci +def test_usd_api_physx_api_attrs_deprecated(setup_simulation): + """Reading ``cfg.usd_api`` and ``cfg.physx_api`` on the new mesh cfgs emits a + DeprecationWarning and returns the legacy-mapped string value.""" + from isaaclab_physx.sim.schemas import PhysxConvexHullPropertiesCfg + + cfg = PhysxConvexHullPropertiesCfg() + + with warnings.catch_warnings(record=True) as caught: + warnings.simplefilter("always") + usd_api_value = cfg.usd_api + assert any(issubclass(w.category, DeprecationWarning) for w in caught) + assert usd_api_value == "MeshCollisionAPI" + + with warnings.catch_warnings(record=True) as caught: + warnings.simplefilter("always") + physx_api_value = cfg.physx_api + assert any(issubclass(w.category, DeprecationWarning) for w in caught) + assert physx_api_value == "PhysxConvexHullCollisionAPI" + + @pytest.mark.isaacsim_ci def test_modify_properties_on_invalid_prim(setup_simulation): """Test modifying properties on a prim that does not exist.""" @@ -290,8 +898,8 @@ def _validate_articulation_properties_on_prim( root_prim = stage.GetPrimAtPath(prim_path) # check articulation properties are set correctly for attr_name, attr_value in arti_cfg.__dict__.items(): - # skip names we know are not present - if attr_name == "func": + # skip class metadata and names we know are not present + if attr_name.startswith("_") or attr_name == "func": continue # handle fixed root link if attr_name == "fix_root_link" and attr_value is not None: @@ -334,8 +942,12 @@ def _validate_rigid_body_properties_on_prim(prim_path: str, rigid_cfg, verbose: for link_prim in root_prim.GetChildren(): if UsdPhysics.RigidBodyAPI(link_prim): for attr_name, attr_value in rigid_cfg.__dict__.items(): - # skip names we know are not present - if attr_name in ["func", "rigid_body_enabled", "kinematic_enabled"]: + # skip class metadata and names we know are not present + if attr_name.startswith("_") or attr_name in [ + "func", + "rigid_body_enabled", + "kinematic_enabled", + ]: continue # convert attribute name in prim to cfg name prim_prop_name = f"physxRigidBody:{to_camel_case(attr_name, to='cC')}" @@ -363,8 +975,8 @@ def _validate_collision_properties_on_prim(prim_path: str, collision_cfg, verbos for mesh_prim in link_prim.GetChildren(): if UsdPhysics.CollisionAPI(mesh_prim): for attr_name, attr_value in collision_cfg.__dict__.items(): - # skip names we know are not present - if attr_name in ["func", "collision_enabled"]: + # skip names we know are not present and class-metadata keys + if attr_name.startswith("_") or attr_name in ["func", "collision_enabled"]: continue # convert attribute name in prim to cfg name prim_prop_name = f"physxCollision:{to_camel_case(attr_name, to='cC')}" @@ -391,8 +1003,8 @@ def _validate_mass_properties_on_prim(prim_path: str, mass_cfg, verbose: bool = for link_prim in root_prim.GetChildren(): if UsdPhysics.MassAPI(link_prim): for attr_name, attr_value in mass_cfg.__dict__.items(): - # skip names we know are not present - if attr_name in ["func"]: + # skip names we know are not present and class-metadata keys + if attr_name in ["func"] or attr_name.startswith("_"): continue # print(link_prim.GetProperties()) prim_prop_name = f"physics:{to_camel_case(attr_name, to='cC')}" @@ -423,8 +1035,8 @@ def _validate_joint_drive_properties_on_prim(prim_path: str, joint_cfg, verbose: assert joint_prim.HasAPI(UsdPhysics.DriveAPI) # iterate over the joint properties for attr_name, attr_value in joint_cfg.__dict__.items(): - # skip names we know are not present - if attr_name in ["func", "ensure_drives_exist"]: + # skip class metadata and names we know are not present on the USD prim + if attr_name.startswith("_") or attr_name in ["func", "ensure_drives_exist"]: continue # resolve the drive (linear or angular) drive_model = "linear" if joint_prim.IsA(UsdPhysics.PrismaticJoint) else "angular" @@ -437,10 +1049,8 @@ def _validate_joint_drive_properties_on_prim(prim_path: str, joint_cfg, verbose: continue # non-string attributes - if attr_name == "max_velocity": + if attr_name == "max_joint_velocity": prim_attr_name = "physxJoint:maxJointVelocity" - elif attr_name == "max_effort": - prim_attr_name = f"drive:{drive_model}:physics:maxForce" else: prim_attr_name = f"drive:{drive_model}:physics:{to_camel_case(attr_name, to='cC')}" @@ -450,7 +1060,7 @@ def _validate_joint_drive_properties_on_prim(prim_path: str, joint_cfg, verbose: # for angular drives, we expect user to set in radians # the values reported by USD are in degrees if drive_model == "angular": - if attr_name == "max_velocity": + if attr_name == "max_joint_velocity": # deg / s --> rad / s prim_attr_value = prim_attr_value * math.pi / 180.0 elif attr_name in ["stiffness", "damping"]: diff --git a/source/isaaclab/test/sim/test_schemas_shim.py b/source/isaaclab/test/sim/test_schemas_shim.py new file mode 100644 index 000000000000..11e6e5d1ba24 --- /dev/null +++ b/source/isaaclab/test/sim/test_schemas_shim.py @@ -0,0 +1,172 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Tests that the forwarding shims resolve the PhysX cfgs that were relocated to +:mod:`isaaclab_physx`. Covers both the schema cfgs (in :mod:`isaaclab.sim.schemas`) and the +material cfgs (in :mod:`isaaclab.sim.spawners.materials`). + +These tests do not require Isaac Sim — only Python import semantics. +""" + +import warnings + +import pytest +from isaaclab_physx.sim.schemas import schemas_cfg as physx_cfg +from isaaclab_physx.sim.spawners.materials import physics_materials_cfg as physx_mat_cfg + +import isaaclab.sim as sim_utils +import isaaclab.sim.schemas as schemas +import isaaclab.sim.schemas.schemas_cfg as schemas_cfg_submodule +import isaaclab.sim.spawners.materials as materials +import isaaclab.sim.spawners.materials.physics_materials_cfg as materials_cfg_submodule + +FORWARDED_NAMES = [ + "RigidBodyPropertiesCfg", + "JointDrivePropertiesCfg", + "PhysxRigidBodyPropertiesCfg", + "PhysxJointDrivePropertiesCfg", + "CollisionPropertiesCfg", + "PhysxCollisionPropertiesCfg", + "PhysxDeformableCollisionPropertiesCfg", + "PhysXCollisionPropertiesCfg", + "ArticulationRootPropertiesCfg", + "PhysxArticulationRootPropertiesCfg", + "MeshCollisionPropertiesCfg", + "ConvexHullPropertiesCfg", + "ConvexDecompositionPropertiesCfg", + "TriangleMeshPropertiesCfg", + "TriangleMeshSimplificationPropertiesCfg", + "SDFMeshPropertiesCfg", + "PhysxConvexHullPropertiesCfg", + "PhysxConvexDecompositionPropertiesCfg", + "PhysxTriangleMeshPropertiesCfg", + "PhysxTriangleMeshSimplificationPropertiesCfg", + "PhysxSDFMeshPropertiesCfg", + "FixedTendonPropertiesCfg", + "SpatialTendonPropertiesCfg", + "PhysxFixedTendonPropertiesCfg", + "PhysxSpatialTendonPropertiesCfg", +] + +DEPRECATED_FORWARDED_NAMES = [ + "RigidBodyPropertiesCfg", + "JointDrivePropertiesCfg", + "CollisionPropertiesCfg", + "PhysXCollisionPropertiesCfg", + "ArticulationRootPropertiesCfg", + "MeshCollisionPropertiesCfg", + "ConvexHullPropertiesCfg", + "ConvexDecompositionPropertiesCfg", + "TriangleMeshPropertiesCfg", + "TriangleMeshSimplificationPropertiesCfg", + "SDFMeshPropertiesCfg", + "FixedTendonPropertiesCfg", + "SpatialTendonPropertiesCfg", +] + +FORWARDED_MATERIAL_NAMES = [ + "RigidBodyMaterialCfg", + "PhysxRigidBodyMaterialCfg", +] + + +@pytest.mark.parametrize("name", FORWARDED_NAMES) +def test_schemas_shim_resolves_to_physx_class(name): + """``isaaclab.sim.schemas.`` resolves to the same class object as the one in + ``isaaclab_physx.sim.schemas.schemas_cfg``.""" + assert getattr(schemas, name) is getattr(physx_cfg, name) + + +@pytest.mark.parametrize("name", FORWARDED_NAMES) +def test_sim_namespace_shim_resolves_to_physx_class(name): + """``isaaclab.sim.`` (i.e. ``sim_utils.``) resolves to the same class object.""" + assert getattr(sim_utils, name) is getattr(physx_cfg, name) + + +@pytest.mark.parametrize("name", FORWARDED_NAMES) +def test_schemas_cfg_submodule_shim_resolves_to_physx_class(name): + """``from isaaclab.sim.schemas.schemas_cfg import `` (direct submodule import path) + resolves to the same class object as the relocated definition.""" + assert getattr(schemas_cfg_submodule, name) is getattr(physx_cfg, name) + + +@pytest.mark.parametrize("name", FORWARDED_MATERIAL_NAMES) +def test_materials_shim_resolves_to_physx_class(name): + """``isaaclab.sim.spawners.materials.`` resolves to the same class object as the + one in ``isaaclab_physx.sim.spawners.materials.physics_materials_cfg``.""" + assert getattr(materials, name) is getattr(physx_mat_cfg, name) + + +@pytest.mark.parametrize("name", FORWARDED_MATERIAL_NAMES) +def test_materials_cfg_submodule_shim_resolves_to_physx_class(name): + """``from isaaclab.sim.spawners.materials.physics_materials_cfg import `` (direct + submodule import path) resolves to the same class object as the relocated definition.""" + assert getattr(materials_cfg_submodule, name) is getattr(physx_mat_cfg, name) + + +@pytest.mark.parametrize("name", FORWARDED_MATERIAL_NAMES) +def test_sim_namespace_material_shim_resolves_to_physx_class(name): + """``isaaclab.sim.`` (i.e. ``sim_utils.``) resolves to the relocated material class.""" + assert getattr(sim_utils, name) is getattr(physx_mat_cfg, name) + + +def test_deprecated_alias_emits_deprecation_warning(): + """Instantiating ``RigidBodyPropertiesCfg`` via the shim still emits ``DeprecationWarning``.""" + with warnings.catch_warnings(record=True) as caught: + warnings.simplefilter("always") + schemas.RigidBodyPropertiesCfg() + assert any(issubclass(w.category, DeprecationWarning) for w in caught) + + +@pytest.mark.parametrize("name", DEPRECATED_FORWARDED_NAMES) +def test_deprecated_aliases_emit_deprecation_warning(name): + """Instantiating each deprecated forwarded alias via the shim emits exactly one + ``DeprecationWarning``.""" + cls = getattr(schemas, name) + with warnings.catch_warnings(record=True) as caught: + warnings.simplefilter("always") + cls() + deprecations = [w for w in caught if issubclass(w.category, DeprecationWarning)] + assert len(deprecations) == 1, f"{name}: expected one DeprecationWarning, got {len(deprecations)}" + + +def test_deprecated_material_alias_emits_deprecation_warning(): + """Instantiating ``RigidBodyMaterialCfg`` via the shim still emits ``DeprecationWarning``.""" + with warnings.catch_warnings(record=True) as caught: + warnings.simplefilter("always") + materials.RigidBodyMaterialCfg() + deprecations = [w for w in caught if issubclass(w.category, DeprecationWarning)] + assert len(deprecations) == 1 + assert "5.0" in str(deprecations[0].message) + + +def test_new_class_does_not_emit_deprecation_warning(): + """Instantiating ``PhysxRigidBodyPropertiesCfg`` directly does NOT emit ``DeprecationWarning``.""" + with warnings.catch_warnings(record=True) as caught: + warnings.simplefilter("always") + schemas.PhysxRigidBodyPropertiesCfg() + assert not any(issubclass(w.category, DeprecationWarning) for w in caught) + + +def test_new_material_class_does_not_emit_deprecation_warning(): + """Instantiating ``PhysxRigidBodyMaterialCfg`` directly does NOT emit ``DeprecationWarning``.""" + with warnings.catch_warnings(record=True) as caught: + warnings.simplefilter("always") + materials.PhysxRigidBodyMaterialCfg() + assert not any(issubclass(w.category, DeprecationWarning) for w in caught) + + +def test_dir_lists_forwarded_names(): + """``dir(isaaclab.sim.schemas)`` includes the forwarded names so IDE autocomplete works.""" + listing = dir(schemas) + for name in FORWARDED_NAMES: + assert name in listing + + +def test_dir_lists_forwarded_material_names(): + """``dir(isaaclab.sim.spawners.materials)`` includes the forwarded names.""" + listing = dir(materials) + for name in FORWARDED_MATERIAL_NAMES: + assert name in listing diff --git a/source/isaaclab/test/sim/test_simulation_context_visualizers.py b/source/isaaclab/test/sim/test_simulation_context_visualizers.py index 1f86f32872c9..cf136c37053a 100644 --- a/source/isaaclab/test/sim/test_simulation_context_visualizers.py +++ b/source/isaaclab/test/sim/test_simulation_context_visualizers.py @@ -7,14 +7,21 @@ from __future__ import annotations +import sys from typing import Any, cast +import isaaclab_visualizers.kit.kit_visualizer as kit_visualizer +import isaaclab_visualizers.newton.newton_visualization_markers as newton_markers import isaaclab_visualizers.rerun.rerun_visualizer as rerun_visualizer import isaaclab_visualizers.viser.viser_visualizer as viser_visualizer +import numpy as np import pytest +import torch +from isaaclab_visualizers.kit.kit_visualizer_cfg import KitVisualizerCfg from isaaclab_visualizers.rerun.rerun_visualizer_cfg import RerunVisualizerCfg from isaaclab_visualizers.viser.viser_visualizer_cfg import ViserVisualizerCfg +from isaaclab.markers.vis_marker_registry import VisMarkerRegistry from isaaclab.sim.simulation_context import SimulationContext @@ -27,11 +34,21 @@ def forward(self): class _FakeProvider: - def __init__(self): - self.update_calls = [] + """Fake new-style SceneDataProvider for tests; only provides what visualizers read.""" + + def __init__(self, num_envs: int = 0): + self._num_envs = num_envs + + @property + def num_envs(self) -> int: + return self._num_envs + + @property + def usd_stage(self): + return None - def update(self): - self.update_calls.append(True) + def get_camera_transforms(self): + return None class _FakeVisualizer: @@ -94,36 +111,39 @@ def requires_forward_before_step(self): def pumps_app_update(self): return self._pumps_app_update + def supports_markers(self): + return False + def _make_context(visualizers, provider=None): ctx = object.__new__(SimulationContext) ctx._visualizers = list(visualizers) ctx._scene_data_provider = provider ctx.physics_manager = _FakePhysicsManager() - ctx._visualizer_step_counter = 0 return ctx -def test_update_scene_data_provider_forwards_and_updates_provider(): +def test_update_visualizers_runs_forward_when_a_visualizer_requires_it(): provider = _FakeProvider() viz_a = _FakeVisualizer(env_ids=[0, 2], requires_forward=True) viz_b = _FakeVisualizer(env_ids=[2, 3]) - viz_c = _FakeVisualizer(env_ids=None) - ctx = _make_context([viz_a, viz_b, viz_c], provider=provider) + ctx = _make_context([viz_a, viz_b], provider=provider) - ctx.update_scene_data_provider() + ctx.update_visualizers(0.1) assert ctx.physics_manager.forward_calls == 1 - assert provider.update_calls == [True] - assert ctx._visualizer_step_counter == 1 + assert viz_a.step_calls == [0.1] + assert viz_b.step_calls == [0.1] -def test_update_scene_data_provider_force_forward_with_no_visualizers(): +def test_update_visualizers_skips_forward_when_no_visualizer_requires_it(): provider = _FakeProvider() - ctx = _make_context([], provider=provider) - ctx.update_scene_data_provider(force_require_forward=True) - assert ctx.physics_manager.forward_calls == 1 - assert provider.update_calls == [True] + viz = _FakeVisualizer(env_ids=[0]) + ctx = _make_context([viz], provider=provider) + + ctx.update_visualizers(0.1) + + assert ctx.physics_manager.forward_calls == 0 def test_update_visualizers_removes_closed_nonrunning_and_failed(caplog): @@ -168,20 +188,34 @@ def test_update_visualizers_handles_training_pause_loop(): assert viz.step_calls == [0.0, 0.2] -class _DummyViserSceneDataProvider: - def __init__(self): - self._metadata = {"num_envs": 4} - self.state_calls: list[list[int] | None] = [] +def test_vis_marker_registry_dispatch_allows_callback_mutation(): + registry = VisMarkerRegistry() + calls = [] - def get_metadata(self) -> dict: - return self._metadata + def _remove_other_callback(event): + calls.append(("remove_other", event)) + registry.remove_callback("other") - def get_newton_model(self): - return "dummy-model" + def _other_callback(event): + calls.append(("other", event)) - def get_newton_state(self): - self.state_calls.append(None) - return {"state_call": len(self.state_calls)} + registry.add_callback("remove_other", _remove_other_callback) + registry.add_callback("other", _other_callback) + + registry.dispatch_callbacks("tick") + + assert calls == [("remove_other", "tick"), ("other", "tick")] + assert "other" not in registry._callbacks + + +class _DummyViserSceneDataProvider: + @property + def num_envs(self) -> int: + return 4 + + @property + def usd_stage(self): + return None def get_camera_transforms(self): return {} @@ -204,29 +238,277 @@ def is_running(self) -> bool: return True -def test_viser_visualizer_initialize_and_step_uses_provider_state(monkeypatch: pytest.MonkeyPatch): +def test_viser_visualizer_initialize_and_step_uses_newton_manager_state(monkeypatch: pytest.MonkeyPatch): provider = _DummyViserSceneDataProvider() viewer = _DummyViserViewer() def _fake_create_viewer(self, record_to_viser: str | None, metadata: dict | None = None): assert record_to_viser is None - assert metadata == provider.get_metadata() + assert metadata == {"num_envs": provider.num_envs} self._viewer = viewer monkeypatch.setattr(viser_visualizer.ViserVisualizer, "_create_viewer", _fake_create_viewer) + state_calls: list[None] = [] + + class _FakeNewtonManager: + @staticmethod + def get_model(): + return "dummy-model" + + @staticmethod + def get_state(): + state_calls.append(None) + return {"state_call": len(state_calls)} + + @staticmethod + def get_num_envs() -> int: + return 1 + + import isaaclab_newton.physics as _np_mod + + monkeypatch.setattr(_np_mod, "NewtonManager", _FakeNewtonManager) + visualizer = viser_visualizer.ViserVisualizer(ViserVisualizerCfg()) visualizer.initialize(cast(Any, provider)) visualizer.step(0.25) assert visualizer.is_initialized - assert provider.state_calls == [None, None] + assert state_calls == [None, None] assert visualizer._sim_time == pytest.approx(0.25) assert viewer.calls[0][0] == "begin_frame" assert viewer.calls[0][1] == pytest.approx(0.25) - # log_state passes through get_newton_state() as-is; no env_ids (or other) keys are merged in. + # log_state passes NewtonManager.get_state() through as-is; no env_ids merged in. + assert viewer.calls[1] == ("log_state", {"state_call": 2}) + assert viewer.calls[2] == ("end_frame",) + + +def test_viser_visualizer_marker_render_failure_does_not_interrupt_state_updates( + monkeypatch: pytest.MonkeyPatch, caplog: pytest.LogCaptureFixture +): + provider = _DummyViserSceneDataProvider() + viewer = _DummyViserViewer() + marker_calls = [] + + def _fake_create_viewer(self, record_to_viser: str | None, metadata: dict | None = None): + self._viewer = viewer + + def _raise_marker_render(*args, **kwargs): + marker_calls.append((args, kwargs)) + raise RuntimeError("marker overlay failed") + + monkeypatch.setattr(viser_visualizer.ViserVisualizer, "_create_viewer", _fake_create_viewer) + monkeypatch.setattr(viser_visualizer, "render_newton_visualization_markers", _raise_marker_render) + + state_calls: list[None] = [] + + class _FakeNewtonManager: + @staticmethod + def get_model(): + return "dummy-model" + + @staticmethod + def get_state(): + state_calls.append(None) + return {"state_call": len(state_calls)} + + @staticmethod + def get_num_envs() -> int: + return 1 + + import isaaclab_newton.physics as _np_mod + + monkeypatch.setattr(_np_mod, "NewtonManager", _FakeNewtonManager) + + visualizer = viser_visualizer.ViserVisualizer(ViserVisualizerCfg()) + visualizer.initialize(cast(Any, provider)) + + with caplog.at_level("WARNING"): + visualizer.step(0.25) + + assert marker_calls + assert viewer.calls[0][0] == "begin_frame" assert viewer.calls[1] == ("log_state", {"state_call": 2}) assert viewer.calls[2] == ("end_frame",) + assert "Marker rendering failed; continuing body updates" in caplog.text + + +def test_newton_marker_mesh_registration_is_per_viewer(monkeypatch: pytest.MonkeyPatch): + marker = object.__new__(newton_markers.NewtonVisualizationMarkers) + marker._registered_meshes = set() + + class _FakeMesh: + vertices = np.zeros((1, 3), dtype=np.float32) + indices = np.zeros((3,), dtype=np.int32) + normals = np.zeros((0, 3), dtype=np.float32) + uvs = np.zeros((0, 2), dtype=np.float32) + + class _FakeViewer: + def __init__(self): + self.meshes = [] + + def log_mesh(self, name, vertices, indices, **kwargs): + self.meshes.append((name, vertices, indices, kwargs)) + + monkeypatch.setattr(newton_markers, "_create_mesh", lambda cfg: _FakeMesh()) + monkeypatch.setattr(newton_markers.wp, "array", lambda value, dtype=None: value) + + spec = newton_markers._NewtonMarkerSpec(renderer="mesh", mesh_type="box", mesh_params={"size": (1.0, 1.0, 1.0)}) + viewer_a = _FakeViewer() + viewer_b = _FakeViewer() + + marker._ensure_mesh_registered(viewer_a, "/Visuals/marker/meshes/arrow", spec) + marker._ensure_mesh_registered(viewer_a, "/Visuals/marker/meshes/arrow", spec) + marker._ensure_mesh_registered(viewer_b, "/Visuals/marker/meshes/arrow", spec) + + assert len(viewer_a.meshes) == 1 + assert len(viewer_b.meshes) == 1 + + +class _FakeNewtonMarkerMesh: + vertices = np.zeros((1, 3), dtype=np.float32) + indices = np.zeros((3,), dtype=np.int32) + normals = np.zeros((0, 3), dtype=np.float32) + uvs = np.zeros((0, 2), dtype=np.float32) + + +class _FakeNewtonMarkerViewer: + def __init__(self): + self.meshes = [] + self.instances = [] + self.lines = [] + + def log_mesh(self, name, vertices, indices, **kwargs): + self.meshes.append((name, vertices, indices, kwargs)) + + def log_instances(self, batch_name, mesh_name, xforms, scales, colors, materials, hidden=False): + self.instances.append( + { + "batch_name": batch_name, + "mesh_name": mesh_name, + "xforms": xforms, + "scales": scales, + "colors": colors, + "materials": materials, + "hidden": hidden, + } + ) + + def log_lines(self, batch_name, starts, ends, colors, width=None, hidden=False): + self.lines.append( + { + "batch_name": batch_name, + "starts": starts, + "ends": ends, + "colors": colors, + "width": width, + "hidden": hidden, + } + ) + + +def _make_newton_marker_for_render( + *, + marker_names: list[str], + translations: torch.Tensor, + marker_indices: torch.Tensor | None = None, + visible: bool = True, +): + marker = object.__new__(newton_markers.NewtonVisualizationMarkers) + marker_cfg_type = type("MarkerCfg", (), {"visual_material": None}) + marker.cfg = type("Cfg", (), {"markers": {name: marker_cfg_type() for name in marker_names}})() + marker.group_id = "/Visuals/marker::test" + marker.visible = visible + marker.translations = translations + marker.orientations = torch.tensor([[0.0, 0.0, 0.0, 1.0]], dtype=torch.float32).repeat(translations.shape[0], 1) + marker.scales = torch.ones((translations.shape[0], 3), dtype=torch.float32) + marker.marker_indices = marker_indices + marker.count = translations.shape[0] + marker._registered_meshes = set() + marker._warned_unsupported = set() + return marker + + +def _patch_newton_marker_render_deps(monkeypatch: pytest.MonkeyPatch) -> None: + specs = { + "arrow": newton_markers._NewtonMarkerSpec( + renderer="mesh", + mesh_type="box", + mesh_params={"size": (1.0, 1.0, 1.0)}, + color=(1.0, 1.0, 1.0), + texture=np.zeros((2, 2, 3), dtype=np.uint8), + ), + "sphere": newton_markers._NewtonMarkerSpec(renderer="mesh", mesh_type="sphere", mesh_params={"radius": 1.0}), + "frame": newton_markers._NewtonMarkerSpec(renderer="frame"), + } + + monkeypatch.setattr(newton_markers, "_create_mesh", lambda cfg: _FakeNewtonMarkerMesh()) + monkeypatch.setattr(newton_markers.wp, "array", lambda value, dtype=None: value) + monkeypatch.setattr(newton_markers, "_resolve_newton_marker_cfg", lambda name, marker_cfg, cfg: specs[name]) + + +def test_newton_marker_render_filters_visible_envs(monkeypatch: pytest.MonkeyPatch): + _patch_newton_marker_render_deps(monkeypatch) + translations = torch.arange(8, dtype=torch.float32).unsqueeze(1).repeat(1, 3) + marker = _make_newton_marker_for_render( + marker_names=["arrow"], + translations=translations, + marker_indices=torch.zeros(8, dtype=torch.int32), + ) + viewer = _FakeNewtonMarkerViewer() + + marker.render(viewer, visible_env_ids=[1, 3], num_envs=4) + + assert len(viewer.instances) == 1 + assert viewer.instances[0]["hidden"] is False + assert viewer.instances[0]["xforms"][:, 0].tolist() == [1.0, 3.0, 5.0, 7.0] + + +def test_newton_marker_render_routes_instances_by_prototype(monkeypatch: pytest.MonkeyPatch): + _patch_newton_marker_render_deps(monkeypatch) + translations = torch.arange(4, dtype=torch.float32).unsqueeze(1).repeat(1, 3) + marker = _make_newton_marker_for_render( + marker_names=["arrow", "sphere"], + translations=translations, + marker_indices=torch.tensor([0, 1, 0, 1], dtype=torch.int32), + ) + viewer = _FakeNewtonMarkerViewer() + + marker.render(viewer, visible_env_ids=None, num_envs=4) + + visible_instances = [call for call in viewer.instances if not call["hidden"]] + assert [call["batch_name"] for call in visible_instances] == [ + "/Visuals/marker::test/arrow", + "/Visuals/marker::test/sphere", + ] + assert [call["xforms"].shape[0] for call in visible_instances] == [2, 2] + assert visible_instances[0]["materials"][:, 3].tolist() == [1.0, 1.0] + assert visible_instances[1]["materials"][:, 3].tolist() == [0.0, 0.0] + + +def test_newton_marker_render_hides_unselected_prototypes(monkeypatch: pytest.MonkeyPatch): + _patch_newton_marker_render_deps(monkeypatch) + marker = _make_newton_marker_for_render( + marker_names=["arrow", "sphere", "frame"], + translations=torch.zeros((3, 3), dtype=torch.float32), + marker_indices=torch.zeros(3, dtype=torch.int32), + ) + viewer = _FakeNewtonMarkerViewer() + + marker.render(viewer, visible_env_ids=None, num_envs=3) + + hidden_instances = [call for call in viewer.instances if call["hidden"]] + assert [call["batch_name"] for call in hidden_instances] == ["/Visuals/marker::test/sphere"] + assert viewer.lines == [ + { + "batch_name": "/Visuals/marker::test/frame", + "starts": None, + "ends": None, + "colors": None, + "width": None, + "hidden": True, + } + ] @pytest.mark.parametrize( @@ -348,17 +630,33 @@ def close(self) -> None: captured["closed"] = True class _DummyRerunSceneDataProvider: - def get_metadata(self) -> dict: - return {"num_envs": 4} + @property + def num_envs(self) -> int: + return 4 + + @property + def usd_stage(self): + return None - def get_newton_model(self): + def get_camera_transforms(self): + return {} + + class _FakeNewtonManager: + @staticmethod + def get_model(): return "dummy-model" - def get_newton_state(self): + @staticmethod + def get_state(): return {"ok": True} - def get_camera_transforms(self): - return {} + @staticmethod + def get_num_envs() -> int: + return 1 + + import isaaclab_newton.physics as _np_mod + + monkeypatch.setattr(_np_mod, "NewtonManager", _FakeNewtonManager) monkeypatch.setattr(rerun_visualizer, "NewtonViewerRerun", _FakeNewtonViewerRerun) monkeypatch.setattr( @@ -385,6 +683,129 @@ def get_camera_transforms(self): assert captured["set_world_offsets"] == (0.0, 0.0, 0.0) +def test_rerun_visualizer_marker_failure_still_ends_frame(monkeypatch: pytest.MonkeyPatch): + class _FakeRerunViewer: + def __init__(self): + self.calls = [] + + def is_paused(self): + return False + + def begin_frame(self, sim_time): + self.calls.append(("begin_frame", sim_time)) + + def log_state(self, state): + self.calls.append(("log_state", state)) + + def end_frame(self): + self.calls.append(("end_frame",)) + + class _DummyRerunSceneDataProvider: + def get_metadata(self) -> dict: + return {"num_envs": 4} + + def get_newton_state(self): + return {"ok": True} + + def get_camera_transforms(self): + return {} + + def _raise_marker_render(*args, **kwargs): + raise RuntimeError("marker render failed") + + monkeypatch.setattr(rerun_visualizer, "render_newton_visualization_markers", _raise_marker_render) + + class _FakeNewtonManager: + @staticmethod + def get_model(): + return "dummy-model" + + @staticmethod + def get_state(): + return {"ok": True} + + @staticmethod + def get_num_envs() -> int: + return 4 + + import isaaclab_newton.physics as _np_mod + + monkeypatch.setattr(_np_mod, "NewtonManager", _FakeNewtonManager) + + visualizer = rerun_visualizer.RerunVisualizer(RerunVisualizerCfg()) + viewer = _FakeRerunViewer() + visualizer._is_initialized = True + visualizer._is_closed = False + visualizer._viewer = viewer + visualizer._scene_data_provider = _DummyRerunSceneDataProvider() + visualizer._resolved_visible_env_ids = None + + with pytest.raises(RuntimeError, match="marker render failed"): + visualizer.step(0.25) + + assert [call[0] for call in viewer.calls] == ["begin_frame", "log_state", "end_frame"] + + +def test_kit_visualizer_default_camera_source_does_not_require_camera_prim(monkeypatch: pytest.MonkeyPatch): + """Default ``--viz kit`` should work for envs without a camera prim.""" + + class _FakeViewportApi: + def __init__(self): + self.set_active_camera_calls = [] + + def get_active_camera(self): + return "/OmniverseKit_Persp" + + def set_active_camera(self, camera_path): + self.set_active_camera_calls.append(camera_path) + + class _FakeViewportWindow: + def __init__(self): + self.viewport_api = _FakeViewportApi() + + class _FakeStage: + def GetPrimAtPath(self, path): + raise AssertionError(f"default Kit visualizer should not look up camera prims: {path}") + + class _FakeProvider: + def get_usd_stage(self): + return _FakeStage() + + viewport_window = _FakeViewportWindow() + viewport_utility = type( + "ViewportUtility", + (), + { + "create_viewport_window": staticmethod(lambda **kwargs: viewport_window), + "get_active_viewport_window": staticmethod(lambda: viewport_window), + }, + ) + monkeypatch.setitem(sys.modules, "omni", type(sys)("omni")) + monkeypatch.setitem(sys.modules, "omni.kit", type(sys)("omni.kit")) + monkeypatch.setitem(sys.modules, "omni.kit.viewport", type(sys)("omni.kit.viewport")) + monkeypatch.setitem(sys.modules, "omni.kit.viewport.utility", viewport_utility) + monkeypatch.setitem(sys.modules, "omni.ui", type("OmniUi", (), {"DockPosition": object})()) + + applied_camera_poses = [] + monkeypatch.setattr( + kit_visualizer.KitVisualizer, + "_set_viewport_camera", + lambda self, eye, target: applied_camera_poses.append((tuple(eye), tuple(target))), + ) + + cfg = KitVisualizerCfg() + visualizer = kit_visualizer.KitVisualizer(cfg) + visualizer._scene_data_provider = _FakeProvider() + visualizer._runtime_headless = False + + visualizer._setup_viewport() + + assert cfg.cam_source == "cfg" + assert applied_camera_poses == [(cfg.eye, cfg.lookat)] + assert viewport_window.viewport_api.set_active_camera_calls == [] + assert visualizer._controlled_camera_path == "/OmniverseKit_Persp" + + def test_get_cli_visualizer_types_handles_non_string_setting_without_crashing(): ctx = object.__new__(SimulationContext) ctx.get_setting = lambda name: {"types": "newton,kit"} if name == "/isaaclab/visualizer/types" else None @@ -449,8 +870,7 @@ def _make_context_with_settings( ctx._visualizers = [] ctx._scene_data_provider = _FakeProvider() ctx._scene_data_requirements = None - ctx._clone_plans = {} - ctx._visualizer_step_counter = 0 + ctx._clone_plan = None ctx._viz_dt = 0.01 ctx.get_setting = lambda name: settings.get(name) return ctx diff --git a/source/isaaclab/test/sim/test_simulation_render_config.py b/source/isaaclab/test/sim/test_simulation_render_config.py index e5ad4274d184..7675ae896342 100644 --- a/source/isaaclab/test/sim/test_simulation_render_config.py +++ b/source/isaaclab/test/sim/test_simulation_render_config.py @@ -41,6 +41,17 @@ def test_render_cfg(): samples_per_pixel = 4 enable_shadows = True enable_ambient_occlusion = True + # RT2 settings + max_bounces = 4 + split_glass = True + split_clearcoat = True + split_rough_reflection = True + ambient_light_intensity = 0.5 + ambient_occlusion_denoiser_mode = 0 + subpixel_mode = 1 + enable_cached_raytracing = True + max_samples_per_launch = 500000 + view_tile_limit = 500000 render_cfg = RenderCfg( enable_translucency=enable_translucency, @@ -54,6 +65,17 @@ def test_render_cfg(): samples_per_pixel=samples_per_pixel, enable_shadows=enable_shadows, enable_ambient_occlusion=enable_ambient_occlusion, + # RT2 settings + max_bounces=max_bounces, + split_glass=split_glass, + split_clearcoat=split_clearcoat, + split_rough_reflection=split_rough_reflection, + ambient_light_intensity=ambient_light_intensity, + ambient_occlusion_denoiser_mode=ambient_occlusion_denoiser_mode, + subpixel_mode=subpixel_mode, + enable_cached_raytracing=enable_cached_raytracing, + max_samples_per_launch=max_samples_per_launch, + view_tile_limit=view_tile_limit, ) cfg = SimulationCfg(render=render_cfg) @@ -74,6 +96,16 @@ def test_render_cfg(): assert sim.cfg.render.samples_per_pixel == samples_per_pixel assert sim.cfg.render.enable_shadows == enable_shadows assert sim.cfg.render.enable_ambient_occlusion == enable_ambient_occlusion + assert sim.cfg.render.max_bounces == max_bounces + assert sim.cfg.render.split_glass == split_glass + assert sim.cfg.render.split_clearcoat == split_clearcoat + assert sim.cfg.render.split_rough_reflection == split_rough_reflection + assert sim.cfg.render.ambient_light_intensity == ambient_light_intensity + assert sim.cfg.render.ambient_occlusion_denoiser_mode == ambient_occlusion_denoiser_mode + assert sim.cfg.render.subpixel_mode == subpixel_mode + assert sim.cfg.render.enable_cached_raytracing == enable_cached_raytracing + assert sim.cfg.render.max_samples_per_launch == max_samples_per_launch + assert sim.cfg.render.view_tile_limit == view_tile_limit assert sim.get_setting("/rtx/translucency/enabled") == sim.cfg.render.enable_translucency assert sim.get_setting("/rtx/reflections/enabled") == sim.cfg.render.enable_reflections @@ -85,6 +117,16 @@ def test_render_cfg(): assert sim.get_setting("/rtx/directLighting/sampledLighting/samplesPerPixel") == sim.cfg.render.samples_per_pixel assert sim.get_setting("/rtx/shadows/enabled") == sim.cfg.render.enable_shadows assert sim.get_setting("/rtx/ambientOcclusion/enabled") == sim.cfg.render.enable_ambient_occlusion + assert sim.get_setting("/rtx/rtpt/maxBounces") == sim.cfg.render.max_bounces + assert sim.get_setting("/rtx/rtpt/splitGlass") == sim.cfg.render.split_glass + assert sim.get_setting("/rtx/rtpt/splitClearcoat") == sim.cfg.render.split_clearcoat + assert sim.get_setting("/rtx/rtpt/splitRoughReflection") == sim.cfg.render.split_rough_reflection + assert sim.get_setting("/rtx/sceneDb/ambientLightIntensity") == sim.cfg.render.ambient_light_intensity + assert sim.get_setting("/rtx/ambientOcclusion/denoiserMode") == sim.cfg.render.ambient_occlusion_denoiser_mode + assert sim.get_setting("/rtx/raytracing/subpixel/mode") == sim.cfg.render.subpixel_mode + assert sim.get_setting("/rtx/raytracing/cached/enabled") == sim.cfg.render.enable_cached_raytracing + assert sim.get_setting("/rtx/pathtracing/maxSamplesPerLaunch") == sim.cfg.render.max_samples_per_launch + assert sim.get_setting("/rtx/viewTile/limit") == sim.cfg.render.view_tile_limit assert sim.get_setting("/rtx/post/aa/op") == 4 # dlss = 3, dlaa=4 @@ -162,6 +204,17 @@ def test_render_cfg_defaults(): samples_per_pixel = 1 enable_shadows = False enable_ambient_occlusion = False + # RT2 defaults + max_bounces = 2 + split_glass = False + split_clearcoat = False + split_rough_reflection = False + ambient_light_intensity = 1.0 + ambient_occlusion_denoiser_mode = 1 + subpixel_mode = 0 + enable_cached_raytracing = False + max_samples_per_launch = 1000000 + view_tile_limit = 1000000 render_cfg = RenderCfg( enable_translucency=enable_translucency, @@ -175,6 +228,17 @@ def test_render_cfg_defaults(): samples_per_pixel=samples_per_pixel, enable_shadows=enable_shadows, enable_ambient_occlusion=enable_ambient_occlusion, + # RT2 settings + max_bounces=max_bounces, + split_glass=split_glass, + split_clearcoat=split_clearcoat, + split_rough_reflection=split_rough_reflection, + ambient_light_intensity=ambient_light_intensity, + ambient_occlusion_denoiser_mode=ambient_occlusion_denoiser_mode, + subpixel_mode=subpixel_mode, + enable_cached_raytracing=enable_cached_raytracing, + max_samples_per_launch=max_samples_per_launch, + view_tile_limit=view_tile_limit, ) cfg = SimulationCfg(render=render_cfg) @@ -192,6 +256,16 @@ def test_render_cfg_defaults(): assert sim.cfg.render.samples_per_pixel == samples_per_pixel assert sim.cfg.render.enable_shadows == enable_shadows assert sim.cfg.render.enable_ambient_occlusion == enable_ambient_occlusion + assert sim.cfg.render.max_bounces == max_bounces + assert sim.cfg.render.split_glass == split_glass + assert sim.cfg.render.split_clearcoat == split_clearcoat + assert sim.cfg.render.split_rough_reflection == split_rough_reflection + assert sim.cfg.render.ambient_light_intensity == ambient_light_intensity + assert sim.cfg.render.ambient_occlusion_denoiser_mode == ambient_occlusion_denoiser_mode + assert sim.cfg.render.subpixel_mode == subpixel_mode + assert sim.cfg.render.enable_cached_raytracing == enable_cached_raytracing + assert sim.cfg.render.max_samples_per_launch == max_samples_per_launch + assert sim.cfg.render.view_tile_limit == view_tile_limit assert sim.get_setting("/rtx/translucency/enabled") == sim.cfg.render.enable_translucency assert sim.get_setting("/rtx/reflections/enabled") == sim.cfg.render.enable_reflections @@ -203,4 +277,14 @@ def test_render_cfg_defaults(): assert sim.get_setting("/rtx/directLighting/sampledLighting/samplesPerPixel") == sim.cfg.render.samples_per_pixel assert sim.get_setting("/rtx/shadows/enabled") == sim.cfg.render.enable_shadows assert sim.get_setting("/rtx/ambientOcclusion/enabled") == sim.cfg.render.enable_ambient_occlusion + assert sim.get_setting("/rtx/rtpt/maxBounces") == sim.cfg.render.max_bounces + assert sim.get_setting("/rtx/rtpt/splitGlass") == sim.cfg.render.split_glass + assert sim.get_setting("/rtx/rtpt/splitClearcoat") == sim.cfg.render.split_clearcoat + assert sim.get_setting("/rtx/rtpt/splitRoughReflection") == sim.cfg.render.split_rough_reflection + assert sim.get_setting("/rtx/sceneDb/ambientLightIntensity") == sim.cfg.render.ambient_light_intensity + assert sim.get_setting("/rtx/ambientOcclusion/denoiserMode") == sim.cfg.render.ambient_occlusion_denoiser_mode + assert sim.get_setting("/rtx/raytracing/subpixel/mode") == sim.cfg.render.subpixel_mode + assert sim.get_setting("/rtx/raytracing/cached/enabled") == sim.cfg.render.enable_cached_raytracing + assert sim.get_setting("/rtx/pathtracing/maxSamplesPerLaunch") == sim.cfg.render.max_samples_per_launch + assert sim.get_setting("/rtx/viewTile/limit") == sim.cfg.render.view_tile_limit assert sim.get_setting("/rtx/post/aa/op") == 3 # dlss = 3, dlaa=4 diff --git a/source/isaaclab/test/sim/test_spawn_wrappers.py b/source/isaaclab/test/sim/test_spawn_wrappers.py index c053a6362f4d..69ad8b105723 100644 --- a/source/isaaclab/test/sim/test_spawn_wrappers.py +++ b/source/isaaclab/test/sim/test_spawn_wrappers.py @@ -162,6 +162,41 @@ def test_spawn_multiple_shapes_with_individual_settings(sim): assert prim.GetAttribute("physics:mass").Get() in mass_variations +def test_spawn_multiple_shapes_with_explicit_spawn_paths(sim): + """Multi-asset spawner accepts planned per-variant source paths.""" + sim_utils.create_prim("/World/planned", "Xform", translation=(0, 0, 0)) + + cfg = sim_utils.MultiAssetSpawnerCfg( + assets_cfg=[ + sim_utils.ConeCfg(radius=0.3, height=0.6), + sim_utils.CuboidCfg(size=(0.3, 0.3, 0.3)), + sim_utils.SphereCfg(radius=0.3), + ], + spawn_paths=["/World/planned/apple", None, "/World/planned/banana"], + mass_props=sim_utils.MassPropertiesCfg(mass=1.0), + collision_props=sim_utils.CollisionPropertiesCfg(), + ) + + prim = cfg.func("/World/ignored_without_regex", cfg) + + assert str(prim.GetPath()) == "/World/planned/apple" + assert sim.stage.GetPrimAtPath("/World/planned/apple").IsValid() + assert not sim.stage.GetPrimAtPath("/World/planned/ignored").IsValid() + assert sim.stage.GetPrimAtPath("/World/planned/banana").IsValid() + assert sim.stage.GetPrimAtPath("/World/planned/apple").GetAttribute("physics:mass").Get() == 1.0 + + +def test_spawn_multiple_shapes_spawn_paths_length_mismatch(sim): + """Explicit multi-asset paths must align one-to-one with variants.""" + cfg = sim_utils.MultiAssetSpawnerCfg( + assets_cfg=[sim_utils.ConeCfg(radius=0.3, height=0.6), sim_utils.SphereCfg(radius=0.3)], + spawn_paths=["/World/planned/apple"], + ) + + with pytest.raises(ValueError, match="spawn_paths"): + cfg.func("/World/ignored_without_regex", cfg) + + """ Tests - Multiple USDs. """ diff --git a/source/isaaclab/test/utils/test_math.py b/source/isaaclab/test/utils/test_math.py index e0d85b14eb25..000bf00eb859 100644 --- a/source/isaaclab/test/utils/test_math.py +++ b/source/isaaclab/test/utils/test_math.py @@ -554,12 +554,15 @@ def test_combine_frame_transform(device): @pytest.mark.parametrize("device", ["cpu", "cuda:0"]) -def test_interpolate_poses(device): +@pytest.mark.parametrize("seed", [0, 1, 2, 3, 4]) +def test_interpolate_poses(device, seed): """Test interpolate_poses function. This test checks the output from the :meth:`~isaaclab.utils.math_utils.interpolate_poses` function against the output from :func:`scipy.spatial.transform.Slerp` and :func:`np.linspace`. """ + torch.manual_seed(seed) + np.random.seed(seed) for _ in range(100): mat1 = math_utils.generate_random_transformation_matrix() mat2 = math_utils.generate_random_transformation_matrix() @@ -1323,3 +1326,117 @@ def test_euler_xyz_from_quat(): wrapped = expected % (2 * torch.pi) output = torch.stack(math_utils.euler_xyz_from_quat(quat, wrap_to_2pi=True), dim=-1) torch.testing.assert_close(output, wrapped) + + +@pytest.mark.parametrize("device", ("cpu", "cuda:0")) +def test_create_rotation_matrix_from_view_lookat_along_up_axis_z(device): + """Camera above target on +Z axis with Z-up should return a valid orthonormal frame.""" + eyes = torch.tensor([[0.0, 0.0, 5.0]], device=device) + targets = torch.tensor([[0.0, 0.0, 0.0]], device=device) + R = math_utils.create_rotation_matrix_from_view(eyes, targets, up_axis="Z", device=device) + assert R is not None + identity = torch.eye(3, device=device).expand(1, 3, 3) + torch.testing.assert_close(R @ R.transpose(-1, -2), identity, atol=1e-5, rtol=1e-5) + torch.testing.assert_close(torch.linalg.det(R), torch.ones(1, device=device), atol=1e-5, rtol=1e-5) + + +@pytest.mark.parametrize("device", ("cpu", "cuda:0")) +def test_create_rotation_matrix_from_view_lookat_along_up_axis_y(device): + """Camera at +Y looking at origin with Y-up should return a valid orthonormal frame.""" + eyes = torch.tensor([[0.0, 5.0, 0.0]], device=device) + targets = torch.tensor([[0.0, 0.0, 0.0]], device=device) + R = math_utils.create_rotation_matrix_from_view(eyes, targets, up_axis="Y", device=device) + assert R is not None + identity = torch.eye(3, device=device).expand(1, 3, 3) + torch.testing.assert_close(R @ R.transpose(-1, -2), identity, atol=1e-5, rtol=1e-5) + torch.testing.assert_close(torch.linalg.det(R), torch.ones(1, device=device), atol=1e-5, rtol=1e-5) + + +@pytest.mark.parametrize("device", ("cpu", "cuda:0")) +def test_create_rotation_matrix_from_view_lookat_along_negative_up_axis(device): + """Camera below target looking up (-Z alignment with Z-up) should return a valid orthonormal frame.""" + eyes = torch.tensor([[0.0, 0.0, -5.0]], device=device) + targets = torch.tensor([[0.0, 0.0, 0.0]], device=device) + R = math_utils.create_rotation_matrix_from_view(eyes, targets, up_axis="Z", device=device) + assert R is not None + identity = torch.eye(3, device=device).expand(1, 3, 3) + torch.testing.assert_close(R @ R.transpose(-1, -2), identity, atol=1e-5, rtol=1e-5) + torch.testing.assert_close(torch.linalg.det(R), torch.ones(1, device=device), atol=1e-5, rtol=1e-5) + + +@pytest.mark.parametrize("device", ("cpu", "cuda:0")) +def test_create_rotation_matrix_from_view_zero_forward_returns_nan(device): + """When eyes == targets the forward direction is undefined; all entries of the row are NaN.""" + eyes = torch.tensor([[1.0, 2.0, 3.0]], device=device) + targets = eyes.clone() + R = math_utils.create_rotation_matrix_from_view(eyes, targets, up_axis="Z", device=device) + assert torch.isnan(R).all() + + +@pytest.mark.parametrize("device", ("cpu", "cuda:0")) +def test_create_rotation_matrix_from_view_batched_partial_failure(device): + """Mixed batch with one degenerate row should produce NaN in that row and a valid rotation in the other.""" + eyes = torch.tensor([[1.0, 2.0, 3.0], [0.0, 0.0, 5.0]], device=device) + targets = torch.tensor([[1.0, 2.0, 3.0], [0.0, 0.0, 0.0]], device=device) + R = math_utils.create_rotation_matrix_from_view(eyes, targets, up_axis="Z", device=device) + assert torch.isnan(R[0]).any() + torch.testing.assert_close(R[1] @ R[1].T, torch.eye(3, device=device), atol=1e-5, rtol=1e-5) + torch.testing.assert_close(torch.linalg.det(R[1]), torch.tensor(1.0, device=device), atol=1e-5, rtol=1e-5) + + +@pytest.mark.parametrize("device", ("cpu", "cuda:0")) +def test_quat_from_matrix_unit_norm_on_valid_input(device): + """quat_from_matrix should produce unit quaternions for any valid rotation matrix.""" + n = 100 + q_rand = math_utils.random_orientation(num=n, device=device) + rot_mat = math_utils.matrix_from_quat(q_rand) + q_value = math_utils.quat_from_matrix(rot_mat) + norms = torch.linalg.norm(q_value, dim=-1) + torch.testing.assert_close(norms, torch.ones(n, device=device), atol=1e-5, rtol=1e-5) + + +@pytest.mark.parametrize("device", ("cpu", "cuda:0")) +def test_quat_from_matrix_singular_matrix_returns_nan(device): + """quat_from_matrix on a singular (non-rotation) matrix should signal NaN, not garbage.""" + singular = torch.tensor([[[0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 1.0]]], device=device) + q = math_utils.quat_from_matrix(singular) + assert torch.isnan(q).all() + + +@pytest.mark.parametrize("device", ("cpu", "cuda:0")) +def test_create_rotation_matrix_from_view_standard(device): + """Sanity: off-axis eye produces an orthonormal frame whose z-axis points from target back to eye.""" + eyes = torch.tensor([[3.0, 0.0, 4.0]], device=device) + targets = torch.tensor([[0.0, 0.0, 0.0]], device=device) + R = math_utils.create_rotation_matrix_from_view(eyes, targets, up_axis="Z", device=device) + identity = torch.eye(3, device=device).expand(1, 3, 3) + torch.testing.assert_close(R @ R.transpose(-1, -2), identity, atol=1e-5, rtol=1e-5) + torch.testing.assert_close(torch.linalg.det(R), torch.ones(1, device=device), atol=1e-5, rtol=1e-5) + # z_axis is back-of-camera in OpenGL convention: points from target to eye + expected_z = torch.tensor([[0.6, 0.0, 0.8]], device=device) + torch.testing.assert_close(R[:, :, 2], expected_z, atol=1e-5, rtol=1e-5) + + +@pytest.mark.parametrize("device", ("cpu", "cuda:0")) +def test_create_rotation_matrix_from_view_non_finite_returns_nan(device): + """Non-finite input (NaN or Inf in eyes/targets) should produce NaN rows.""" + eyes = torch.tensor([[float("nan"), 0.0, 0.0]], device=device) + targets = torch.tensor([[0.0, 0.0, 0.0]], device=device) + R = math_utils.create_rotation_matrix_from_view(eyes, targets, up_axis="Z", device=device) + assert torch.isnan(R).all() + + +@pytest.mark.parametrize("device", ("cpu", "cuda:0")) +def test_quat_from_matrix_reflection_returns_nan(device): + """A reflection matrix (det = -1) is not a proper rotation; the safeguard should signal NaN.""" + reflection = torch.diag(torch.tensor([1.0, 1.0, -1.0], device=device)).unsqueeze(0) + q = math_utils.quat_from_matrix(reflection) + assert torch.isnan(q).all() + + +@pytest.mark.parametrize("device", ("cpu", "cuda:0")) +def test_quat_from_matrix_non_orthonormal_returns_nan(device): + """A non-orthonormal matrix (1% scale error on one axis) is not a valid rotation; expect NaN.""" + R = torch.diag(torch.tensor([1.01, 1.0, 1.0], device=device)).unsqueeze(0) + q = math_utils.quat_from_matrix(R) + assert torch.isnan(q).all() diff --git a/source/isaaclab/test/utils/test_wrench_composer_integration.py b/source/isaaclab/test/utils/test_wrench_composer_integration.py index bb195e856030..acb1682bde02 100644 --- a/source/isaaclab/test/utils/test_wrench_composer_integration.py +++ b/source/isaaclab/test/utils/test_wrench_composer_integration.py @@ -79,7 +79,7 @@ def test_global_force_invariant_under_rotation(device): forces[..., 0] = FORCE_MAGNITUDE torques = torch.zeros(1, len(body_ids), 3, device=device) - cube_object.permanent_wrench_composer.set_forces_and_torques( + cube_object.permanent_wrench_composer.set_forces_and_torques_index( forces=forces, torques=torques, positions=com, @@ -96,9 +96,9 @@ def test_global_force_invariant_under_rotation(device): vel_after_phase1 = cube_object.data.root_lin_vel_w.torch[0].clone() # Rotate body 180deg about Z (quat wxyz = [0, 0, 0, 1]) while keeping velocity - root_pose = cube_object.data.root_state_w.torch[0, :7].clone().unsqueeze(0) + root_pose = cube_object.data.root_pose_w.torch[0].clone().unsqueeze(0) root_pose[0, 3:7] = torch.tensor([0.0, 0.0, 1.0, 0.0], device=device) # 180deg about Z (xyzw) - cube_object.write_root_pose_to_sim(root_pose) + cube_object.write_root_pose_to_sim_index(root_pose=root_pose) # Phase 2: run N_STEPS more for _ in range(N_STEPS): @@ -152,7 +152,7 @@ def test_local_force_follows_rotation(device): forces[..., 0] = FORCE_MAGNITUDE torques = torch.zeros(1, len(body_ids), 3, device=device) - cube_object.permanent_wrench_composer.set_forces_and_torques( + cube_object.permanent_wrench_composer.set_forces_and_torques_index( forces=forces, torques=torques, body_ids=body_ids, @@ -169,9 +169,9 @@ def test_local_force_follows_rotation(device): assert vel_after_phase1[0].item() > 1.0, "Object should be moving in +X" # Rotate body 180deg about Z while keeping velocity - root_pose = cube_object.data.root_state_w.torch[0, :7].clone().unsqueeze(0) + root_pose = cube_object.data.root_pose_w.torch[0].clone().unsqueeze(0) root_pose[0, 3:7] = torch.tensor([0.0, 0.0, 1.0, 0.0], device=device) # 180deg about Z (xyzw) - cube_object.write_root_pose_to_sim(root_pose) + cube_object.write_root_pose_to_sim_index(root_pose=root_pose) # Phase 2: run N_STEPS — local +X is now world -X, so force decelerates for _ in range(N_STEPS): @@ -217,7 +217,7 @@ def test_global_force_at_offset_generates_torque(device): positions = com_pos.clone() positions[..., 1] += 1.0 # +1m Y offset - cube_object.permanent_wrench_composer.set_forces_and_torques( + cube_object.permanent_wrench_composer.set_forces_and_torques_index( forces=forces, torques=torques, positions=positions, @@ -262,7 +262,7 @@ def test_global_torque_invariant_under_rotation(device): torques = torch.zeros(1, len(body_ids), 3, device=device) torques[..., 2] = TORQUE_MAGNITUDE - cube_object.permanent_wrench_composer.set_forces_and_torques( + cube_object.permanent_wrench_composer.set_forces_and_torques_index( forces=forces, torques=torques, body_ids=body_ids, @@ -279,10 +279,13 @@ def test_global_torque_invariant_under_rotation(device): # Rotate body 90deg about X and zero out velocities so phase 2 starts from rest # (avoids gyroscopic cross-coupling at high omega) - root_pose = cube_object.data.root_state_w.torch[0, :7].clone().unsqueeze(0) + root_pose = cube_object.data.root_pose_w.torch[0].clone().unsqueeze(0) root_pose[0, 3:7] = torch.tensor([0.7071, 0.0, 0.0, 0.7071], device=device) # 90deg about X (xyzw) - cube_object.write_root_pose_to_sim(root_pose) - cube_object.write_root_velocity_to_sim(torch.zeros(1, 6, device=device)) + cube_object.write_root_pose_to_sim_index(root_pose=root_pose) + + root_vel = cube_object.data.root_vel_w.torch.clone() + root_vel[0, :] = 0.0 + cube_object.write_root_velocity_to_sim_index(root_velocity=root_vel) # Phase 2: run N_STEPS from rest with different body orientation for _ in range(N_STEPS): @@ -324,13 +327,16 @@ def test_global_force_torque_after_translation(device): body_ids, _ = cube_object.find_bodies(".*") # Phase 1 setup: Move cube to (1, 0, 1) and apply force at (1, 0, 1) - root_state = cube_object.data.root_state_w.torch.clone() - root_state[0, 0] = 1.0 # x = 1 - root_state[0, 1] = 0.0 # y = 0 - root_state[0, 2] = 1.0 # z = 1 - root_state[0, 3:7] = torch.tensor([0.0, 0.0, 0.0, 1.0], device=device) # identity quat (xyzw) - root_state[0, 7:] = 0.0 # zero velocity - cube_object.write_root_state_to_sim(root_state) + root_pose = cube_object.data.root_pose_w.torch.clone() + root_pose[0, 0] = 1.0 # x = 1 + root_pose[0, 1] = 0.0 # y = 0 + root_pose[0, 2] = 1.0 # z = 1 + root_pose[0, 3:7] = torch.tensor([0.0, 0.0, 0.0, 1.0], device=device) # identity quat (xyzw) + cube_object.write_root_pose_to_sim_index(root_pose=root_pose) + + root_vel = cube_object.data.root_vel_w.torch.clone() + root_vel[0, :] = 0.0 # zero velocity + cube_object.write_root_velocity_to_sim_index(root_velocity=root_vel) # Step once to let the state settle sim.step() @@ -343,7 +349,7 @@ def test_global_force_torque_after_translation(device): forces[..., 1] = FORCE_MAGNITUDE # +Y force torques = torch.zeros(1, len(body_ids), 3, device=device) - cube_object.permanent_wrench_composer.set_forces_and_torques( + cube_object.permanent_wrench_composer.set_forces_and_torques_index( forces=forces, torques=torques, positions=com_pos, @@ -369,13 +375,16 @@ def test_global_force_torque_after_translation(device): ) # Phase 2: Teleport cube to origin, zero velocity, don't re-apply force - root_state2 = cube_object.data.root_state_w.torch.clone() - root_state2[0, 0] = 0.0 # x = 0 - root_state2[0, 1] = 0.0 - root_state2[0, 2] = 1.0 # z = 1 - root_state2[0, 3:7] = torch.tensor([0.0, 0.0, 0.0, 1.0], device=device) # identity (xyzw) - root_state2[0, 7:] = 0.0 # zero velocity - cube_object.write_root_state_to_sim(root_state2) + root_pose2 = cube_object.data.root_pose_w.torch.clone() + root_pose2[0, 0] = 0.0 # x = 0 + root_pose2[0, 1] = 0.0 + root_pose2[0, 2] = 1.0 # z = 1 + root_pose2[0, 3:7] = torch.tensor([0.0, 0.0, 0.0, 1.0], device=device) # identity (xyzw) + cube_object.write_root_pose_to_sim_index(root_pose=root_pose2) + + root_vel2 = cube_object.data.root_vel_w.torch.clone() + root_vel2[0, :] = 0.0 # zero velocity + cube_object.write_root_velocity_to_sim_index(root_velocity=root_vel2) # Step once to let state settle sim.step() @@ -422,13 +431,16 @@ def test_global_force_torque_reverses_on_opposite_side(device): body_ids, _ = cube_object.find_bodies(".*") # Move cube to (-1, 0, 1) - root_state = cube_object.data.root_state_w.torch.clone() - root_state[0, 0] = -1.0 - root_state[0, 1] = 0.0 - root_state[0, 2] = 1.0 - root_state[0, 3:7] = torch.tensor([0.0, 0.0, 0.0, 1.0], device=device) # identity (xyzw) - root_state[0, 7:] = 0.0 - cube_object.write_root_state_to_sim(root_state) + root_pose = cube_object.data.root_pose_w.torch.clone() + root_pose[0, 0] = -1.0 + root_pose[0, 1] = 0.0 + root_pose[0, 2] = 1.0 + root_pose[0, 3:7] = torch.tensor([0.0, 0.0, 0.0, 1.0], device=device) # identity (xyzw) + cube_object.write_root_pose_to_sim_index(root_pose=root_pose) + + root_vel = cube_object.data.root_vel_w.torch.clone() + root_vel[0, :] = 0.0 + cube_object.write_root_velocity_to_sim_index(root_velocity=root_vel) sim.step() cube_object.update(sim.cfg.dt) @@ -439,7 +451,7 @@ def test_global_force_torque_reverses_on_opposite_side(device): positions = torch.zeros(1, len(body_ids), 3, device=device) positions[..., 2] = 1.0 # P = (0, 0, 1) - cube_object.permanent_wrench_composer.set_forces_and_torques( + cube_object.permanent_wrench_composer.set_forces_and_torques_index( forces=forces, torques=torques, positions=positions, @@ -457,13 +469,16 @@ def test_global_force_torque_reverses_on_opposite_side(device): assert omega_z_phase1 > 0.1, f"Phase 1: expected positive omega_z, got {omega_z_phase1}" # Phase 2: Teleport cube to (+1, 0, 1), zero velocity - root_state2 = cube_object.data.root_state_w.torch.clone() - root_state2[0, 0] = 1.0 - root_state2[0, 1] = 0.0 - root_state2[0, 2] = 1.0 - root_state2[0, 3:7] = torch.tensor([0.0, 0.0, 0.0, 1.0], device=device) # identity (xyzw) - root_state2[0, 7:] = 0.0 - cube_object.write_root_state_to_sim(root_state2) + root_pose2 = cube_object.data.root_pose_w.torch.clone() + root_pose2[0, 0] = 1.0 + root_pose2[0, 1] = 0.0 + root_pose2[0, 2] = 1.0 + root_pose2[0, 3:7] = torch.tensor([0.0, 0.0, 0.0, 1.0], device=device) # identity (xyzw) + cube_object.write_root_pose_to_sim_index(root_pose=root_pose2) + + root_vel2 = cube_object.data.root_vel_w.torch.clone() + root_vel2[0, :] = 0.0 + cube_object.write_root_velocity_to_sim_index(root_velocity=root_vel2) sim.step() cube_object.update(sim.cfg.dt) @@ -493,13 +508,16 @@ def test_global_force_no_position_no_torque(device): body_ids, _ = cube_object.find_bodies(".*") # Move cube to (2, 0, 1) - root_state = cube_object.data.root_state_w.torch.clone() - root_state[0, 0] = 2.0 - root_state[0, 1] = 0.0 - root_state[0, 2] = 1.0 - root_state[0, 3:7] = torch.tensor([0.0, 0.0, 0.0, 1.0], device=device) # identity (xyzw) - root_state[0, 7:] = 0.0 - cube_object.write_root_state_to_sim(root_state) + root_pose = cube_object.data.root_pose_w.torch.clone() + root_pose[0, 0] = 2.0 + root_pose[0, 1] = 0.0 + root_pose[0, 2] = 1.0 + root_pose[0, 3:7] = torch.tensor([0.0, 0.0, 0.0, 1.0], device=device) # identity (xyzw) + cube_object.write_root_pose_to_sim_index(root_pose=root_pose) + + root_vel = cube_object.data.root_vel_w.torch.clone() + root_vel[0, :] = 0.0 + cube_object.write_root_velocity_to_sim_index(root_velocity=root_vel) sim.step() cube_object.update(sim.cfg.dt) @@ -508,7 +526,7 @@ def test_global_force_no_position_no_torque(device): forces[..., 1] = FORCE_MAGNITUDE torques = torch.zeros(1, len(body_ids), 3, device=device) - cube_object.permanent_wrench_composer.set_forces_and_torques( + cube_object.permanent_wrench_composer.set_forces_and_torques_index( forces=forces, torques=torques, body_ids=body_ids, @@ -549,19 +567,21 @@ def test_multi_cube_different_torques_from_same_force(device): body_ids, _ = cube_object.find_bodies(".*") # Position cubes: Cube 0 at (-1, 0, 1), Cube 1 at (+1, 0, 1) - root_state = cube_object.data.root_state_w.torch.clone() - root_state[0, 0] = -1.0 - root_state[0, 1] = 0.0 - root_state[0, 2] = 1.0 - root_state[0, 3:7] = torch.tensor([0.0, 0.0, 0.0, 1.0], device=device) # identity (xyzw) - root_state[0, 7:] = 0.0 - - root_state[1, 0] = 1.0 - root_state[1, 1] = 0.0 - root_state[1, 2] = 1.0 - root_state[1, 3:7] = torch.tensor([0.0, 0.0, 0.0, 1.0], device=device) # identity (xyzw) - root_state[1, 7:] = 0.0 - cube_object.write_root_state_to_sim(root_state) + root_pose = cube_object.data.root_pose_w.torch.clone() + root_pose[0, 0] = -1.0 + root_pose[0, 1] = 0.0 + root_pose[0, 2] = 1.0 + root_pose[0, 3:7] = torch.tensor([0.0, 0.0, 0.0, 1.0], device=device) # identity (xyzw) + + root_pose[1, 0] = 1.0 + root_pose[1, 1] = 0.0 + root_pose[1, 2] = 1.0 + root_pose[1, 3:7] = torch.tensor([0.0, 0.0, 0.0, 1.0], device=device) # identity (xyzw) + cube_object.write_root_pose_to_sim_index(root_pose=root_pose) + + root_vel = cube_object.data.root_vel_w.torch.clone() + root_vel[:, :] = 0.0 + cube_object.write_root_velocity_to_sim_index(root_velocity=root_vel) sim.step() cube_object.update(sim.cfg.dt) @@ -572,7 +592,7 @@ def test_multi_cube_different_torques_from_same_force(device): positions = torch.zeros(2, len(body_ids), 3, device=device) positions[..., 2] = 1.0 # P = (0, 0, 1) - cube_object.permanent_wrench_composer.set_forces_and_torques( + cube_object.permanent_wrench_composer.set_forces_and_torques_index( forces=forces, torques=torques, positions=positions, @@ -628,20 +648,22 @@ def test_global_force_torque_far_from_origin(device): body_ids, _ = cube_object.find_bodies(".*") # Position cubes: Cube 0 near origin, Cube 1 far from origin - root_state = cube_object.data.root_state_w.torch.clone() + root_pose = cube_object.data.root_pose_w.torch.clone() # Cube 0 at (0, 0, 1) - root_state[0, 0] = 0.0 - root_state[0, 1] = 0.0 - root_state[0, 2] = 1.0 - root_state[0, 3:7] = torch.tensor([0.0, 0.0, 0.0, 1.0], device=device) # identity (xyzw) - root_state[0, 7:] = 0.0 + root_pose[0, 0] = 0.0 + root_pose[0, 1] = 0.0 + root_pose[0, 2] = 1.0 + root_pose[0, 3:7] = torch.tensor([0.0, 0.0, 0.0, 1.0], device=device) # identity (xyzw) # Cube 1 at (2000, 0, 1) - root_state[1, 0] = 2000.0 - root_state[1, 1] = 0.0 - root_state[1, 2] = 1.0 - root_state[1, 3:7] = torch.tensor([0.0, 0.0, 0.0, 1.0], device=device) # identity (xyzw) - root_state[1, 7:] = 0.0 - cube_object.write_root_state_to_sim(root_state) + root_pose[1, 0] = 2000.0 + root_pose[1, 1] = 0.0 + root_pose[1, 2] = 1.0 + root_pose[1, 3:7] = torch.tensor([0.0, 0.0, 0.0, 1.0], device=device) # identity (xyzw) + cube_object.write_root_pose_to_sim_index(root_pose=root_pose) + + root_vel = cube_object.data.root_vel_w.torch.clone() + root_vel[:, :] = 0.0 + cube_object.write_root_velocity_to_sim_index(root_velocity=root_vel) sim.step() cube_object.update(sim.cfg.dt) @@ -655,7 +677,7 @@ def test_global_force_torque_far_from_origin(device): positions = com_pos.clone() positions[..., 0] += 1.0 # +1m X offset from CoM - cube_object.permanent_wrench_composer.set_forces_and_torques( + cube_object.permanent_wrench_composer.set_forces_and_torques_index( forces=forces, torques=torques, positions=positions, @@ -723,19 +745,21 @@ def test_global_force_no_position_no_rotation_large_offset(device): body_ids, _ = cube_object.find_bodies(".*") # Place cube at large X offset - root_state = cube_object.data.default_root_state.torch.clone() - root_state[0, 0] = 2000.0 # large X position - root_state[0, 1] = 0.0 - root_state[0, 2] = 1.0 - cube_object.write_root_pose_to_sim(root_state[:, :7]) - cube_object.write_root_velocity_to_sim(root_state[:, 7:]) + root_pose = cube_object.data.default_root_pose.torch.clone() + root_pose[0, 0] = 2000.0 # large X position + root_pose[0, 1] = 0.0 + root_pose[0, 2] = 1.0 + cube_object.write_root_pose_to_sim_index(root_pose=root_pose) + + root_vel = cube_object.data.default_root_vel.torch.clone() + cube_object.write_root_velocity_to_sim_index(root_velocity=root_vel) cube_object.reset() # Apply global force without positions (should go to CoM, no torque) forces = torch.zeros(cube_object.num_instances, len(body_ids), 3, device=device) forces[0, :, 1] = 10.0 # F_y = 10 N - cube_object.permanent_wrench_composer.set_forces_and_torques( + cube_object.permanent_wrench_composer.set_forces_and_torques_index( forces=forces, body_ids=body_ids, is_global=True, @@ -778,12 +802,14 @@ def test_global_force_at_com_position_no_rotation_large_offset(device): body_ids, _ = cube_object.find_bodies(".*") # Place cube at large X offset - root_state = cube_object.data.default_root_state.torch.clone() - root_state[0, 0] = 2000.0 - root_state[0, 1] = 0.0 - root_state[0, 2] = 1.0 - cube_object.write_root_pose_to_sim(root_state[:, :7]) - cube_object.write_root_velocity_to_sim(root_state[:, 7:]) + root_pose = cube_object.data.default_root_pose.torch.clone() + root_pose[0, 0] = 2000.0 + root_pose[0, 1] = 0.0 + root_pose[0, 2] = 1.0 + cube_object.write_root_pose_to_sim_index(root_pose=root_pose) + + root_vel = cube_object.data.default_root_vel.torch.clone() + cube_object.write_root_velocity_to_sim_index(root_velocity=root_vel) cube_object.reset() # Apply global force AT the cube's position (torque should cancel) @@ -794,7 +820,7 @@ def test_global_force_at_com_position_no_rotation_large_offset(device): positions[0, :, 0] = 2000.0 positions[0, :, 2] = 1.0 - cube_object.permanent_wrench_composer.set_forces_and_torques( + cube_object.permanent_wrench_composer.set_forces_and_torques_index( forces=forces, positions=positions, body_ids=body_ids, diff --git a/source/isaaclab/test/utils/test_wrench_composer_vs_physx.py b/source/isaaclab/test/utils/test_wrench_composer_vs_physx.py index 8d47b003369c..ca8f22be437c 100644 --- a/source/isaaclab/test/utils/test_wrench_composer_vs_physx.py +++ b/source/isaaclab/test/utils/test_wrench_composer_vs_physx.py @@ -110,7 +110,7 @@ def test_composer_vs_physx_local_force(device): forces[..., 0] = FORCE_MAGNITUDE torques = torch.zeros(1, len(body_ids), 3, device=device) - cube_composer.permanent_wrench_composer.set_forces_and_torques( + cube_composer.permanent_wrench_composer.set_forces_and_torques_index( forces=forces, torques=torques, body_ids=body_ids, @@ -175,7 +175,7 @@ def test_composer_vs_physx_global_force(device): forces[..., 0] = FORCE_MAGNITUDE torques = torch.zeros(1, len(body_ids), 3, device=device) - cube_composer.permanent_wrench_composer.set_forces_and_torques( + cube_composer.permanent_wrench_composer.set_forces_and_torques_index( forces=forces, torques=torques, body_ids=body_ids, @@ -249,7 +249,7 @@ def test_composer_vs_physx_local_force_at_position(device): positions = torch.zeros(1, len(body_ids), 3, device=device) positions[..., 1] = 0.5 # +0.5m Y offset in local frame - cube_composer.permanent_wrench_composer.set_forces_and_torques( + cube_composer.permanent_wrench_composer.set_forces_and_torques_index( forces=forces, torques=torques, positions=positions, @@ -322,7 +322,7 @@ def test_composer_vs_physx_global_force_at_position(device): pos_composer = cube_composer.data.body_com_pos_w.torch[:, body_ids, :3].clone() + offset pos_raw = cube_raw.data.body_com_pos_w.torch[:, body_ids, :3].clone() + offset - cube_composer.permanent_wrench_composer.set_forces_and_torques( + cube_composer.permanent_wrench_composer.set_forces_and_torques_index( forces=forces, torques=torques, positions=pos_composer, @@ -387,7 +387,7 @@ def test_composer_vs_physx_local_torque(device): torques = torch.zeros(1, len(body_ids), 3, device=device) torques[..., 2] = TORQUE_MAGNITUDE - cube_composer.permanent_wrench_composer.set_forces_and_torques( + cube_composer.permanent_wrench_composer.set_forces_and_torques_index( forces=forces, torques=torques, body_ids=body_ids, @@ -452,7 +452,7 @@ def test_composer_vs_physx_global_torque(device): torques = torch.zeros(1, len(body_ids), 3, device=device) torques[..., 2] = TORQUE_MAGNITUDE - cube_composer.permanent_wrench_composer.set_forces_and_torques( + cube_composer.permanent_wrench_composer.set_forces_and_torques_index( forces=forces, torques=torques, body_ids=body_ids, @@ -513,7 +513,7 @@ def test_composer_vs_physx_global_force_multi_env(device): forces[..., 0] = FORCE_MAGNITUDE torques = torch.zeros(NUM_CUBES_MULTI, len(body_ids), 3, device=device) - cube_composer.permanent_wrench_composer.set_forces_and_torques( + cube_composer.permanent_wrench_composer.set_forces_and_torques_index( forces=forces, torques=torques, body_ids=body_ids, @@ -605,7 +605,7 @@ def apply_global_force(): forces = torch.zeros(NUM_CUBES_MULTI, len(body_ids), 3, device=device) forces[..., 0] = FORCE_MAGNITUDE torques = torch.zeros(NUM_CUBES_MULTI, len(body_ids), 3, device=device) - cube_composer.permanent_wrench_composer.set_forces_and_torques( + cube_composer.permanent_wrench_composer.set_forces_and_torques_index( forces=forces, torques=torques, body_ids=body_ids, @@ -641,8 +641,18 @@ def apply_global_force(): reset_ids_torch = torch.tensor(reset_ids, dtype=torch.long, device=device) # Reset root state using captured world-frame initial state (includes env origins) - cube_composer.write_root_state_to_sim(initial_state_composer[reset_ids_torch], env_ids=reset_ids_torch) - cube_raw.write_root_state_to_sim(initial_state_raw[reset_ids_torch], env_ids=reset_ids_torch) + cube_composer.write_root_link_pose_to_sim_index( + root_pose=initial_state_composer[reset_ids_torch, :7], env_ids=reset_ids_torch + ) + cube_composer.write_root_com_velocity_to_sim_index( + root_velocity=initial_state_composer[reset_ids_torch, 7:], env_ids=reset_ids_torch + ) + cube_raw.write_root_link_pose_to_sim_index( + root_pose=initial_state_raw[reset_ids_torch, :7], env_ids=reset_ids_torch + ) + cube_raw.write_root_com_velocity_to_sim_index( + root_velocity=initial_state_raw[reset_ids_torch, 7:], env_ids=reset_ids_torch + ) cube_composer.reset(reset_ids) cube_raw.reset(reset_ids) @@ -717,7 +727,7 @@ def test_composer_vs_physx_payload_scenario(device): forces[..., 2] = -payload_force torques = torch.zeros(1, len(body_ids), 3, device=device) - cube_composer.permanent_wrench_composer.set_forces_and_torques( + cube_composer.permanent_wrench_composer.set_forces_and_torques_index( forces=forces, torques=torques, body_ids=body_ids, @@ -790,7 +800,7 @@ def test_composer_vs_physx_permanent_global_force_at_position_long_run(device): pos_composer = cube_composer.data.body_com_pos_w.torch[:, body_ids, :3].clone() + offset pos_raw = cube_raw.data.body_com_pos_w.torch[:, body_ids, :3].clone() + offset - cube_composer.permanent_wrench_composer.set_forces_and_torques( + cube_composer.permanent_wrench_composer.set_forces_and_torques_index( forces=forces, torques=torques, positions=pos_composer, diff --git a/source/isaaclab/test/visualizers/test_visualizer.py b/source/isaaclab/test/visualizers/test_visualizer.py index 44d3a89aea16..4b050431eb82 100644 --- a/source/isaaclab/test/visualizers/test_visualizer.py +++ b/source/isaaclab/test/visualizers/test_visualizer.py @@ -79,6 +79,10 @@ def __init__(self, num_envs: int = 0, transforms: dict | None = None): self._num_envs = num_envs self._transforms = transforms + @property + def num_envs(self) -> int: + return self._num_envs + def get_metadata(self) -> dict: return {"num_envs": self._num_envs} diff --git a/source/isaaclab_assets/config/extension.toml b/source/isaaclab_assets/config/extension.toml index 1bf36d627e3e..055e3a5ff2f2 100644 --- a/source/isaaclab_assets/config/extension.toml +++ b/source/isaaclab_assets/config/extension.toml @@ -1,6 +1,6 @@ [package] # Semantic Versioning is used: https://semver.org/ -version = "0.3.3" +version = "0.3.4" # Description title = "Isaac Lab Assets" diff --git a/source/isaaclab_assets/docs/CHANGELOG.rst b/source/isaaclab_assets/docs/CHANGELOG.rst index 1d676f70a27e..e9eda5822210 100644 --- a/source/isaaclab_assets/docs/CHANGELOG.rst +++ b/source/isaaclab_assets/docs/CHANGELOG.rst @@ -1,6 +1,16 @@ Changelog --------- +0.3.4 (2026-05-12) +~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added :class:`~isaaclab_assets.robots.unitree.G129_CFG_WITH_DEX3_BASE_FIX` robot configuration + for the Unitree G1 29-DOF with Dex3 hands. + + 0.3.3 (2026-04-29) ~~~~~~~~~~~~~~~~~~~ diff --git a/source/isaaclab_assets/isaaclab_assets/robots/arl_robot_1.py b/source/isaaclab_assets/isaaclab_assets/robots/arl_robot_1.py index 32e01adc93b4..4f81f8a0645c 100644 --- a/source/isaaclab_assets/isaaclab_assets/robots/arl_robot_1.py +++ b/source/isaaclab_assets/isaaclab_assets/robots/arl_robot_1.py @@ -7,7 +7,7 @@ The following configuration parameters are available: -* :obj:`ARL_ROBOT_1_CFG`: The ARL_Robot_1 with (TODO add motor propeller combination) +* :obj:`ARL_ROBOT_1_CFG`: The ARL_Robot_1 """ import isaaclab.sim as sim_utils diff --git a/source/isaaclab_assets/isaaclab_assets/robots/unitree.py b/source/isaaclab_assets/isaaclab_assets/robots/unitree.py index 7a02c6eff294..8e4f692ca6df 100644 --- a/source/isaaclab_assets/isaaclab_assets/robots/unitree.py +++ b/source/isaaclab_assets/isaaclab_assets/robots/unitree.py @@ -21,10 +21,12 @@ """ import isaaclab.sim as sim_utils -from isaaclab.actuators import ActuatorNetMLPCfg, DCMotorCfg, ImplicitActuatorCfg +from isaaclab.actuators import ActuatorNetMLPCfg, DCMotorCfg, IdealPDActuatorCfg, ImplicitActuatorCfg from isaaclab.assets.articulation import ArticulationCfg from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR, ISAACLAB_NUCLEUS_DIR +HEALTHCARE_S3 = "https://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/Healthcare/0.5.0/132c82d" + ## # Configuration - Actuators. ## @@ -609,3 +611,201 @@ damping=0.2, armature=0.001, ) + + +G129_CFG_WITH_DEX3_BASE_FIX = ArticulationCfg( + spawn=sim_utils.UsdFileCfg( + usd_path=f"{HEALTHCARE_S3}/Robots/UnitreeG1/g1_29dof_with_dex3_base_fix/g1_29dof_with_dex3_base_fix.usd", + activate_contact_sensors=False, + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=False, + retain_accelerations=False, + linear_damping=0.0, + angular_damping=0.0, + max_linear_velocity=1000.0, + max_angular_velocity=1000.0, + max_depenetration_velocity=1.0, + solver_position_iteration_count=4, + solver_velocity_iteration_count=0, + ), + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, solver_position_iteration_count=4, solver_velocity_iteration_count=0 + ), + ), + prim_path="/World/envs/env_.*/Robot", + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.0, 0.0, 0.75), + joint_pos={ + "left_hip_yaw_joint": 0.0, + "left_hip_roll_joint": 0.0, + "left_hip_pitch_joint": -0.05, + "left_knee_joint": 0.2, + "left_ankle_pitch_joint": -0.15, + "left_ankle_roll_joint": 0.0, + "right_hip_yaw_joint": 0.0, + "right_hip_roll_joint": 0.0, + "right_hip_pitch_joint": -0.05, + "right_knee_joint": 0.2, + "right_ankle_pitch_joint": -0.15, + "right_ankle_roll_joint": 0.0, + "waist_yaw_joint": 0.0, + "waist_roll_joint": 0.0, + "waist_pitch_joint": 0.0, + "left_shoulder_pitch_joint": 0.0, + "left_shoulder_roll_joint": 0.0, + "left_shoulder_yaw_joint": 0.0, + "left_elbow_joint": -0.3, + "left_wrist_roll_joint": 0.0, + "left_wrist_pitch_joint": 0.0, + "left_wrist_yaw_joint": 0.0, + "right_shoulder_pitch_joint": 0.0, + "right_shoulder_roll_joint": 0.0, + "right_shoulder_yaw_joint": 0.0, + "right_elbow_joint": -0.3, + "right_wrist_roll_joint": 0.0, + "right_wrist_pitch_joint": 0.0, + "right_wrist_yaw_joint": 0.0, + "left_hand_index_0_joint": 0.0, + "left_hand_middle_0_joint": 0.0, + "left_hand_thumb_0_joint": 0.0, + "left_hand_index_1_joint": 0.0, + "left_hand_middle_1_joint": 0.0, + "left_hand_thumb_1_joint": 0.0, + "left_hand_thumb_2_joint": 0.0, + "right_hand_index_0_joint": 0.0, + "right_hand_middle_0_joint": 0.0, + "right_hand_thumb_0_joint": 0.0, + "right_hand_index_1_joint": 0.0, + "right_hand_middle_1_joint": 0.0, + "right_hand_thumb_1_joint": 0.0, + "right_hand_thumb_2_joint": 0.0, + }, + joint_vel={".*": 0.0}, + ), + soft_joint_pos_limit_factor=0.9, + actuators={ + "legs": IdealPDActuatorCfg( + joint_names_expr=[ + ".*_hip_yaw_joint", + ".*_hip_roll_joint", + ".*_hip_pitch_joint", + ".*_knee_joint", + ], + effort_limit={ + ".*_hip_yaw_joint": 88.0, + ".*_hip_roll_joint": 88.0, + ".*_hip_pitch_joint": 88.0, + ".*_knee_joint": 139.0, + }, + velocity_limit={ + ".*_hip_yaw_joint": 32.0, + ".*_hip_roll_joint": 32.0, + ".*_hip_pitch_joint": 32.0, + ".*_knee_joint": 20.0, + }, + stiffness={ + ".*_hip_yaw_joint": 150.0, + ".*_hip_roll_joint": 150.0, + ".*_hip_pitch_joint": 150.0, + ".*_knee_joint": 300.0, + }, + damping={ + ".*_hip_yaw_joint": 2.0, + ".*_hip_roll_joint": 2.0, + ".*_hip_pitch_joint": 2.0, + ".*_knee_joint": 4.0, + }, + armature={ + ".*_hip_.*": 0.03, + ".*_knee_joint": 0.03, + }, + ), + "feet": IdealPDActuatorCfg( + joint_names_expr=[".*_ankle_pitch_joint", ".*_ankle_roll_joint"], + stiffness={ + ".*_ankle_pitch_joint": 40.0, + ".*_ankle_roll_joint": 40.0, + }, + damping={ + ".*_ankle_pitch_joint": 2, + ".*_ankle_roll_joint": 2, + }, + effort_limit={ + ".*_ankle_pitch_joint": 50.0, + ".*_ankle_roll_joint": 50.0, + }, + velocity_limit={ + ".*_ankle_pitch_joint": 37.0, + ".*_ankle_roll_joint": 37.0, + }, + armature=0.03, + friction=0.03, + ), + "waist": ImplicitActuatorCfg( + joint_names_expr=["waist_yaw_joint", "waist_roll_joint", "waist_pitch_joint"], + effort_limit=1000.0, + velocity_limit=0.0, + stiffness={"waist_yaw_joint": 10000.0, "waist_roll_joint": 10000.0, "waist_pitch_joint": 10000.0}, + damping={"waist_yaw_joint": 10000.0, "waist_roll_joint": 10000.0, "waist_pitch_joint": 10000.0}, + armature=None, + ), + "arms": IdealPDActuatorCfg( + joint_names_expr=[ + ".*_shoulder_pitch_joint", + ".*_shoulder_roll_joint", + ".*_shoulder_yaw_joint", + ".*_elbow_joint", + ".*_wrist_.*_joint", + ], + effort_limit={ + ".*_shoulder_pitch_joint": 25.0, + ".*_shoulder_roll_joint": 25.0, + ".*_shoulder_yaw_joint": 25.0, + ".*_elbow_joint": 25.0, + ".*_wrist_roll_joint": 25.0, + ".*_wrist_pitch_joint": 5.0, + ".*_wrist_yaw_joint": 5.0, + }, + velocity_limit={ + ".*_shoulder_pitch_joint": 37.0, + ".*_shoulder_roll_joint": 37.0, + ".*_shoulder_yaw_joint": 37.0, + ".*_elbow_joint": 37.0, + ".*_wrist_roll_joint": 37.0, + ".*_wrist_pitch_joint": 22.0, + ".*_wrist_yaw_joint": 22.0, + }, + stiffness={ + ".*_shoulder_pitch_joint": 100.0, + ".*_shoulder_roll_joint": 100.0, + ".*_shoulder_yaw_joint": 40.0, + ".*_elbow_joint": 40.0, + ".*_wrist_.*_joint": 20.0, + }, + damping={ + ".*_shoulder_pitch_joint": 15.0, + ".*_shoulder_roll_joint": 15.0, + ".*_shoulder_yaw_joint": 8.0, + ".*_elbow_joint": 8.0, + ".*_wrist_.*_joint": 4.0, + }, + armature={".*_shoulder_.*": 0.03, ".*_elbow_.*": 0.03, ".*_wrist_.*_joint": 0.03}, + friction=0.03, + ), + "hands": IdealPDActuatorCfg( + joint_names_expr=[ + ".*_hand_.*", + ], + effort_limit=5.0, + velocity_limit=10.0, + stiffness=8.0, + damping=1.5, + armature=0.03, + friction=0.5, + ), + }, +) +"""Configuration for the Unitree G1 29DOF robot with Dex3 hands and fixed base. + +This configuration is designed for high-precision manipulation tasks such as trocar assembly. +""" diff --git a/source/isaaclab_contrib/config/extension.toml b/source/isaaclab_contrib/config/extension.toml index 7fd2b5d139e5..bdeec969ff56 100644 --- a/source/isaaclab_contrib/config/extension.toml +++ b/source/isaaclab_contrib/config/extension.toml @@ -1,6 +1,6 @@ [package] # Semantic Versioning is used: https://semver.org/ -version = "0.3.0" +version = "0.3.2" # Description title = "Isaac Lab External Contributions" diff --git a/source/isaaclab_contrib/docs/CHANGELOG.rst b/source/isaaclab_contrib/docs/CHANGELOG.rst index 721c705a5981..24981d6deca6 100644 --- a/source/isaaclab_contrib/docs/CHANGELOG.rst +++ b/source/isaaclab_contrib/docs/CHANGELOG.rst @@ -1,6 +1,27 @@ Changelog --------- +0.3.2 (2026-05-12) +~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Removed ``_patched_reset`` monkey-patch in RLinf extension; use + ``num_rerenders_on_reset`` env config instead. + + +0.3.1 (2026-05-09) +~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Updated TacSL visuotactile sensor camera configuration and examples to use + :class:`~isaaclab.sensors.CameraCfg` and :class:`~isaaclab.sensors.Camera` + instead of deprecated tiled-camera aliases. + + 0.3.0 (2026-02-13) ~~~~~~~~~~~~~~~~~~ diff --git a/source/isaaclab_contrib/docs/README.md b/source/isaaclab_contrib/docs/README.md index 346b47e5522c..ea09129849f5 100644 --- a/source/isaaclab_contrib/docs/README.md +++ b/source/isaaclab_contrib/docs/README.md @@ -208,11 +208,11 @@ The `ThrustAction` term provides flexible preprocessing to support all modes thr ### Demo Script -A complete demonstration of quadcopter simulation is available: +A complete demonstration of multirotor simulation is available: ```bash -# Run quadcopter demo -./isaaclab.sh -p scripts/demos/quadcopter.py +# Run multirotor demo +./isaaclab.sh -p scripts/demos/arl_robot_1.py ``` ## TacSL Tactile Sensor (Detailed) @@ -478,7 +478,7 @@ The extension includes comprehensive unit tests for all contributed components: # Test multirotor components python -m pytest source/isaaclab_contrib/test/assets/test_multirotor.py python -m pytest source/isaaclab_contrib/test/actuators/test_thruster.py - +python -m pytest source/isaaclab_contrib/test/assets/test_drone_geometric_controllers.py # Run all contrib tests python -m pytest source/isaaclab_contrib/test/ ``` diff --git a/source/isaaclab_contrib/isaaclab_contrib/actuators/thruster.py b/source/isaaclab_contrib/isaaclab_contrib/actuators/thruster.py index 036a817fbfbd..da9053107b78 100644 --- a/source/isaaclab_contrib/isaaclab_contrib/actuators/thruster.py +++ b/source/isaaclab_contrib/isaaclab_contrib/actuators/thruster.py @@ -188,9 +188,9 @@ def motor_model_rate(self, error: torch.Tensor, mixing_factor: torch.Tensor): def rk4_integration(self, error: torch.Tensor, mixing_factor: torch.Tensor): k1 = self.motor_model_rate(error, mixing_factor) - k2 = self.motor_model_rate(error + 0.5 * self.cfg.dt * k1, mixing_factor) - k3 = self.motor_model_rate(error + 0.5 * self.cfg.dt * k2, mixing_factor) - k4 = self.motor_model_rate(error + self.cfg.dt * k3, mixing_factor) + k2 = self.motor_model_rate(error - 0.5 * self.cfg.dt * k1, mixing_factor) + k3 = self.motor_model_rate(error - 0.5 * self.cfg.dt * k2, mixing_factor) + k4 = self.motor_model_rate(error - self.cfg.dt * k3, mixing_factor) return (self.cfg.dt / 6.0) * (k1 + 2.0 * k2 + 2.0 * k3 + k4) def discrete_mixing_factor(self, time_constant: torch.Tensor): diff --git a/source/isaaclab_contrib/isaaclab_contrib/controllers/__init__.py b/source/isaaclab_contrib/isaaclab_contrib/controllers/__init__.py new file mode 100644 index 000000000000..15ad731d8b9d --- /dev/null +++ b/source/isaaclab_contrib/isaaclab_contrib/controllers/__init__.py @@ -0,0 +1,16 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-package for different controllers and motion-generators. + +Controllers or motion generators are responsible for closed-loop tracking of a given command. The +controller can be a simple PID controller or a more complex controller such as impedance control +or inverse kinematics control. The controller is responsible for generating the desired joint-level +commands to be sent to the robot. +""" + + +from isaaclab.utils.module import lazy_export +lazy_export() diff --git a/source/isaaclab_contrib/isaaclab_contrib/controllers/__init__.pyi b/source/isaaclab_contrib/isaaclab_contrib/controllers/__init__.pyi new file mode 100644 index 000000000000..648fa27731de --- /dev/null +++ b/source/isaaclab_contrib/isaaclab_contrib/controllers/__init__.pyi @@ -0,0 +1,32 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +__all__ = [ + "compute_desired_orientation", + "compute_body_torque", + "yaw_rate_to_body_angvel", + "LeeControllerBase", + "LeeControllerBaseCfg", + "LeeAttController", + "LeeAttControllerCfg", + "LeeAccController", + "LeeAccControllerCfg", + "LeePosController", + "LeePosControllerCfg", + "LeeVelController", + "LeeVelControllerCfg", +] + +from .lee_controller_utils import compute_body_torque, compute_desired_orientation, yaw_rate_to_body_angvel +from .lee_controller_base import LeeControllerBase +from .lee_controller_base_cfg import LeeControllerBaseCfg +from .lee_attitude_control import LeeAttController +from .lee_attitude_control_cfg import LeeAttControllerCfg +from .lee_acceleration_control import LeeAccController +from .lee_acceleration_control_cfg import LeeAccControllerCfg +from .lee_position_control import LeePosController +from .lee_position_control_cfg import LeePosControllerCfg +from .lee_velocity_control import LeeVelController +from .lee_velocity_control_cfg import LeeVelControllerCfg diff --git a/source/isaaclab_contrib/isaaclab_contrib/controllers/lee_acceleration_control.py b/source/isaaclab_contrib/isaaclab_contrib/controllers/lee_acceleration_control.py new file mode 100644 index 000000000000..908f05745a8e --- /dev/null +++ b/source/isaaclab_contrib/isaaclab_contrib/controllers/lee_acceleration_control.py @@ -0,0 +1,102 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +from typing import TYPE_CHECKING + +import torch + +import isaaclab.utils.math as math_utils + +from .lee_controller_base import LeeControllerBase +from .lee_controller_utils import compute_body_torque, compute_desired_orientation, yaw_rate_to_body_angvel + +if TYPE_CHECKING: + from isaaclab.assets import Multirotor + + from .lee_acceleration_control_cfg import LeeAccControllerCfg + + +class LeeAccController(LeeControllerBase): + """Lee acceleration controller for multirotor tracking acceleration setpoints. + + Computes a body-frame wrench command ``[Fx, Fy, Fz, Tx, Ty, Tz]`` from an acceleration setpoint + in the world frame. Gains may be randomized per environment if enabled in the configuration. + """ + + cfg: LeeAccControllerCfg + + def __init__(self, cfg: LeeAccControllerCfg, asset: Multirotor, num_envs: int, device: str): + """Initialize controller. + + Args: + cfg: Controller configuration. + asset: Multirotor asset to control. + num_envs: Number of environments. + device: Device to run computations on. + """ + super().__init__(cfg, asset, num_envs, device) + + # Gain ranges + self.K_rot_range = torch.tensor(self.cfg.K_rot_range, device=device).repeat(num_envs, 1, 1) + self.K_angvel_range = torch.tensor(self.cfg.K_angvel_range, device=device).repeat(num_envs, 1, 1) + + # Current gains + self.K_rot_current = self.K_rot_range.mean(dim=1) + self.K_angvel_current = self.K_angvel_range.mean(dim=1) + + def compute(self, command: torch.Tensor) -> torch.Tensor: + """Compute wrench command from acceleration setpoint. + + Args: + command: (num_envs, 4) acceleration command command [ax, ay, az, yaw_rate] in body frame. + + Returns: + (num_envs, 6) wrench command [fx, fy, fz, tx, ty, tz] in body frame. + """ + self.wrench_command_b.zero_() + + root_quat_w, root_ang_vel_b, _ = self._root_state_tensors() + + # Use command directly as acceleration setpoint + forces_w = (command[:, :3] - self.gravity) * self.mass.view(-1, 1) + + # Project forces to body z-axis for thrust command + body_z_w = math_utils.matrix_from_quat(root_quat_w)[:, :, 2] + self.wrench_command_b[:, 2] = torch.sum(forces_w * body_z_w, dim=1) + + # Get current yaw and compute desired orientation + roll, pitch, yaw = math_utils.euler_xyz_from_quat(root_quat_w) + desired_quat = compute_desired_orientation(forces_w, yaw, self.rotation_matrix_buffer) + + # Compute desired angular velocity in body frame from yaw rate command + desired_angvel_b = yaw_rate_to_body_angvel(command[:, 3], roll, pitch, self.device) + + # Compute torque command + self.wrench_command_b[:, 3:6] = compute_body_torque( + desired_quat, + desired_angvel_b, + root_quat_w, + root_ang_vel_b, + self.robot_inertia, + self.K_rot_current, + self.K_angvel_current, + self.cfg.max_yaw_rate, + ) + + return self.wrench_command_b + + def _randomize_params(self, env_ids: slice | torch.Tensor): + """Randomize controller gains for the given environments if enabled.""" + self.K_rot_current[env_ids] = math_utils.sample_uniform( + self.K_rot_range[env_ids, 0], self.K_rot_range[env_ids, 1], self.K_rot_range[env_ids, 0].shape, self.device + ) + self.K_angvel_current[env_ids] = math_utils.sample_uniform( + self.K_angvel_range[env_ids, 0], + self.K_angvel_range[env_ids, 1], + self.K_angvel_range[env_ids, 0].shape, + self.device, + ) diff --git a/source/isaaclab_contrib/isaaclab_contrib/controllers/lee_acceleration_control_cfg.py b/source/isaaclab_contrib/isaaclab_contrib/controllers/lee_acceleration_control_cfg.py new file mode 100644 index 000000000000..6a1f6c7db7e2 --- /dev/null +++ b/source/isaaclab_contrib/isaaclab_contrib/controllers/lee_acceleration_control_cfg.py @@ -0,0 +1,23 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + + +from isaaclab.utils import configclass + +from .lee_acceleration_control import LeeAccController +from .lee_controller_base_cfg import LeeControllerBaseCfg + + +@configclass +class LeeAccControllerCfg(LeeControllerBaseCfg): + """Configuration for a Lee-style geometric quadrotor acceleration controller. + + Unless otherwise noted, vectors are ordered as (x, y, z) in the simulation world/body frames. + The acceleration controller gains are sampled uniformly per environment between + their corresponding ``*_min`` and ``*_max`` bounds at reset. + """ + + class_type: type = LeeAccController + """The class type for the acceleration controller.""" diff --git a/source/isaaclab_contrib/isaaclab_contrib/controllers/lee_attitude_control.py b/source/isaaclab_contrib/isaaclab_contrib/controllers/lee_attitude_control.py new file mode 100644 index 000000000000..2ea176bde295 --- /dev/null +++ b/source/isaaclab_contrib/isaaclab_contrib/controllers/lee_attitude_control.py @@ -0,0 +1,98 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +from typing import TYPE_CHECKING + +import torch + +import isaaclab.utils.math as math_utils + +from .lee_controller_base import LeeControllerBase +from .lee_controller_utils import compute_body_torque, yaw_rate_to_body_angvel + +if TYPE_CHECKING: + from isaaclab.assets import Multirotor + + from .lee_attitude_control_cfg import LeeAttControllerCfg + + +class LeeAttController(LeeControllerBase): + """Lee attitude controller for multirotor tracking attitude setpoints. + + Computes a body-frame wrench command ``[Fx, Fy, Fz, Tx, Ty, Tz]`` from an attitude setpoint + in the world frame. Gains may be randomized per environment if enabled in the configuration. + """ + + cfg: LeeAttControllerCfg + + def __init__(self, cfg: LeeAttControllerCfg, asset: Multirotor, num_envs: int, device: str): + """Initialize controller. + + Args: + cfg: Controller configuration. + asset: Multirotor asset to control. + num_envs: Number of environments. + device: Device to run computations on. + """ + super().__init__(cfg, asset, num_envs, device) + + # Gain ranges + self.K_rot_range = torch.tensor(self.cfg.K_rot_range, device=device).repeat(num_envs, 1, 1) + self.K_angvel_range = torch.tensor(self.cfg.K_angvel_range, device=device).repeat(num_envs, 1, 1) + + # Current gains + self.K_rot_current = self.K_rot_range.mean(dim=1) + self.K_angvel_current = self.K_angvel_range.mean(dim=1) + + def compute(self, command: torch.Tensor) -> torch.Tensor: + """Compute wrench command from attitude setpoint. + + Args: + command: (num_envs, 4) attitude command command [thrust, roll, pitch, yaw_rate] in body frame. + + Returns: + (num_envs, 6) wrench command [fx, fy, fz, tx, ty, tz] in body frame. + """ + self.wrench_command_b.zero_() + + root_quat_w, root_ang_vel_b, _ = self._root_state_tensors() + + # Use command directly as attitude setpoint + self.wrench_command_b[:, 2] = (command[:, 0] + 1.0) * self.mass * torch.norm(self.gravity, dim=1) + + # Get current yaw and compute desired orientation + roll, pitch, yaw = math_utils.euler_xyz_from_quat(root_quat_w) + desired_quat = math_utils.quat_from_euler_xyz(command[:, 1], command[:, 2], yaw) + + # Compute desired angular velocity in body frame from yaw rate command + desired_angvel_b = yaw_rate_to_body_angvel(command[:, 3], roll, pitch, self.device) + + # Compute torque command + self.wrench_command_b[:, 3:6] = compute_body_torque( + desired_quat, + desired_angvel_b, + root_quat_w, + root_ang_vel_b, + self.robot_inertia, + self.K_rot_current, + self.K_angvel_current, + self.cfg.max_yaw_rate, + ) + + return self.wrench_command_b + + def _randomize_params(self, env_ids: slice | torch.Tensor): + """Randomize controller gains for the given environments if enabled.""" + self.K_rot_current[env_ids] = math_utils.sample_uniform( + self.K_rot_range[env_ids, 0], self.K_rot_range[env_ids, 1], self.K_rot_range[env_ids, 0].shape, self.device + ) + self.K_angvel_current[env_ids] = math_utils.sample_uniform( + self.K_angvel_range[env_ids, 0], + self.K_angvel_range[env_ids, 1], + self.K_angvel_range[env_ids, 0].shape, + self.device, + ) diff --git a/source/isaaclab_contrib/isaaclab_contrib/controllers/lee_attitude_control_cfg.py b/source/isaaclab_contrib/isaaclab_contrib/controllers/lee_attitude_control_cfg.py new file mode 100644 index 000000000000..bcf0f9f3ca13 --- /dev/null +++ b/source/isaaclab_contrib/isaaclab_contrib/controllers/lee_attitude_control_cfg.py @@ -0,0 +1,23 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + + +from isaaclab.utils import configclass + +from .lee_attitude_control import LeeAttController +from .lee_controller_base_cfg import LeeControllerBaseCfg + + +@configclass +class LeeAttControllerCfg(LeeControllerBaseCfg): + """Configuration for a Lee-style geometric quadrotor attitude controller. + + Unless otherwise noted, vectors are ordered as (x, y, z) in the simulation world/body frames. + The attitude controller gains are sampled uniformly per environment between + their corresponding ``*_min`` and ``*_max`` bounds at reset. + """ + + class_type: type = LeeAttController + """The class type for the attitude controller.""" diff --git a/source/isaaclab_contrib/isaaclab_contrib/controllers/lee_controller_base.py b/source/isaaclab_contrib/isaaclab_contrib/controllers/lee_controller_base.py new file mode 100644 index 000000000000..231c15710716 --- /dev/null +++ b/source/isaaclab_contrib/isaaclab_contrib/controllers/lee_controller_base.py @@ -0,0 +1,120 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Base class for Lee-style geometric controllers.""" + +from __future__ import annotations + +from typing import TYPE_CHECKING + +import torch +import warp as wp + +import isaaclab.sim as sim_utils +import isaaclab.utils.math as math_utils + +from isaaclab_contrib.utils.math import aggregate_inertia_about_robot_com + +if TYPE_CHECKING: + from isaaclab.assets import Multirotor + + from .lee_controller_base_cfg import LeeControllerBaseCfg + + +class LeeControllerBase: + """Base class for Lee-style geometric controllers.""" + + cfg: LeeControllerBaseCfg + device: str + robot: Multirotor + + def __init__(self, cfg: LeeControllerBaseCfg, asset: Multirotor, num_envs: int, device: str): + """Initialize controller buffers and pre-compute aggregate inertias. + + Args: + cfg: Controller configuration. + asset: Multirotor asset to control. + num_envs: Number of environments. + device: Device to run computations on. + """ + self.cfg = cfg + self.robot = asset + self.device = device + self.num_envs = num_envs + + root_quat_w = self._to_torch(self.robot.data.root_link_quat_w) + body_link_pos_w = self._to_torch(self.robot.data.body_link_pos_w) + root_pos_w = self._to_torch(self.robot.data.root_pos_w) + body_com_pos_b = self._to_torch(self.robot.data.body_com_pos_b) + body_com_quat_b = self._to_torch(self.robot.data.body_com_quat_b) + body_link_quat_w = self._to_torch(self.robot.data.body_link_quat_w) + + # Aggregate mass and inertia about the robot COM for all bodies + root_quat_exp = root_quat_w.unsqueeze(1).expand(num_envs, self.robot.num_bodies, 4) + body_link_pos_delta = body_link_pos_w - root_pos_w.unsqueeze(1) + + body_masses = self._to_torch(self.robot.root_view.get_masses()) + body_inv_mass_local = torch.where(body_masses > 0, 1.0 / body_masses, torch.zeros_like(body_masses)) + self.mass, self.robot_inertia, _ = aggregate_inertia_about_robot_com( + self._to_torch(self.robot.root_view.get_inertias()), + body_inv_mass_local, + body_com_pos_b, + body_com_quat_b, + math_utils.quat_apply_inverse(root_quat_exp, body_link_pos_delta), + math_utils.quat_mul(math_utils.quat_inv(root_quat_exp), body_link_quat_w), + ) + # Get gravity from simulation context + sim = sim_utils.SimulationContext.instance() + gravity_vec = sim.cfg.gravity + self.gravity = torch.tensor(gravity_vec, device=device, dtype=torch.float32).expand(num_envs, -1) + + # Buffers + self.wrench_command_b = torch.zeros((num_envs, 6), device=device) # [fx, fy, fz, tx, ty, tz] + self.rotation_matrix_buffer = torch.zeros((num_envs, 3, 3), device=device) + + def _to_torch(self, x): + """Convert warp array to torch tensor on controller device; no-op for torch tensors.""" + if torch.is_tensor(x): + return x.to(self.device) + return wp.to_torch(x).to(self.device) + + def _root_state_tensors(self) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor]: + """Fetch root state once per control step.""" + root_quat_w = self._to_torch(self.robot.data.root_quat_w) + root_ang_vel_b = self._to_torch(self.robot.data.root_ang_vel_b) + root_lin_vel_w = self._to_torch(self.robot.data.root_lin_vel_w) + return root_quat_w, root_ang_vel_b, root_lin_vel_w + + def reset(self): + """Reset controller state for all environments.""" + self.reset_idx(env_ids=None) + + def reset_idx(self, env_ids: torch.Tensor | None): + """Reset controller state (and optionally randomize gains) for selected environments. + + Args: + env_ids: Tensor of environment indices, or ``None`` for all. + """ + if env_ids is None: + env_ids = slice(None) + self._randomize_params(env_ids) + + def _randomize_params(self, env_ids: slice | torch.Tensor): + """Randomize controller gains for the given environments if enabled. + + Override in subclass to implement parameter randomization. + """ + pass + + def compute(self, command: torch.Tensor) -> torch.Tensor: + """Compute wrench command from input command. + + Args: + command: Input command (shape depends on controller type). + + Returns: + (num_envs, 6) wrench command [fx, fy, fz, tx, ty, tz] in body frame. + """ + raise NotImplementedError("Subclasses must implement compute()") diff --git a/source/isaaclab_contrib/isaaclab_contrib/controllers/lee_controller_base_cfg.py b/source/isaaclab_contrib/isaaclab_contrib/controllers/lee_controller_base_cfg.py new file mode 100644 index 000000000000..3a279f7ddb81 --- /dev/null +++ b/source/isaaclab_contrib/isaaclab_contrib/controllers/lee_controller_base_cfg.py @@ -0,0 +1,73 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import math +from dataclasses import MISSING + +from isaaclab.utils import configclass + + +@configclass +class LeeControllerBaseCfg: + """Base configuration for Lee-style geometric quadrotor controllers. + + Unless otherwise noted, vectors are ordered as (x, y, z) in the simulation world/body frames. + The controller gains are sampled uniformly per environment between + their corresponding ``*_min`` and ``*_max`` bounds at reset. + + Note: + To disable randomization, set the min and max values to be identical. + For example: K_rot_range = ((1.85, 1.85, 0.4), (1.85, 1.85, 0.4)) + """ + + K_rot_range: tuple[tuple[float, float, float], tuple[float, float, float]] = MISSING + """Orientation (rotation) error proportional gain range about body axes [unitless]. + + This is a tuple of two tuples containing the minimum and maximum gains for roll, pitch, and yaw. + Format: ((min_roll, min_pitch, min_yaw), (max_roll, max_pitch, max_yaw)) + + To disable randomization, set both tuples to the same values. + + Example (with randomization): + ((1.6, 1.6, 0.25), (1.85, 1.85, 0.4)) for ARL Robot 1 + + Example (without randomization): + ((1.85, 1.85, 0.4), (1.85, 1.85, 0.4)) for fixed gains + """ + + K_angvel_range: tuple[tuple[float, float, float], tuple[float, float, float]] = MISSING + """Body angular-velocity error proportional gain range [unitless]. + + This is a tuple of two tuples containing the minimum and maximum gains for roll, pitch, and yaw rates. + Format: ((min_roll_rate, min_pitch_rate, min_yaw_rate), (max_roll_rate, max_pitch_rate, max_yaw_rate)) + + To disable randomization, set both tuples to the same values. + + Example (with randomization): + ((0.4, 0.4, 0.075), (0.5, 0.5, 0.09)) for ARL Robot 1 + + Example (without randomization): + ((0.5, 0.5, 0.09), (0.5, 0.5, 0.09)) for fixed gains + """ + + max_inclination_angle_rad: float = math.pi / 3 + """Maximum allowed roll/pitch magnitude (inclination) in radians. + + This limits the maximum tilt angle of the quadrotor during control. + Typical range: 0.5 to 1.57 radians (30° to 90°) + + Example: + 1.0471975511965976 (60° in radians) for ARL Robot 1 + """ + + max_yaw_rate: float = MISSING + """Maximum allowed yaw rate command [rad/s]. + + This limits the maximum rotational velocity about the z-axis. + Typical range: 0.5 to 2.0 rad/s + + Example: + 1.0471975511965976 (60°/s in radians) for ARL Robot 1 + """ diff --git a/source/isaaclab_contrib/isaaclab_contrib/controllers/lee_controller_utils.py b/source/isaaclab_contrib/isaaclab_contrib/controllers/lee_controller_utils.py new file mode 100644 index 000000000000..6a89ec0bdde4 --- /dev/null +++ b/source/isaaclab_contrib/isaaclab_contrib/controllers/lee_controller_utils.py @@ -0,0 +1,127 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Shared utilities for Lee-style geometric controllers.""" + +import torch + +import isaaclab.utils.math as math_utils + + +def compute_desired_orientation( + forces_w: torch.Tensor, yaw_setpoint: torch.Tensor, rotation_matrix_buffer: torch.Tensor +) -> torch.Tensor: + """Compute desired orientation from force direction and yaw setpoint. + + Args: + forces_w: (num_envs, 3) desired force vector in world frame. + yaw_setpoint: (num_envs,) desired yaw angle [rad]. + rotation_matrix_buffer: (num_envs, 3, 3) pre-allocated buffer for rotation matrix. + + Returns: + (num_envs, 4) desired orientation quaternion (wxyz). + """ + # Desired z-axis (thrust direction) + b3_c = forces_w / (torch.norm(forces_w, dim=1, keepdim=True) + 1e-12) + + # Intermediate direction for yaw + temp_dir = torch.zeros_like(forces_w) + temp_dir[:, 0] = torch.cos(yaw_setpoint) + temp_dir[:, 1] = torch.sin(yaw_setpoint) + + # Desired y-axis (orthogonal to thrust and yaw direction) + b2_c = torch.cross(b3_c, temp_dir, dim=1) + b2_c = b2_c / (torch.norm(b2_c, dim=1, keepdim=True) + 1e-12) + + # Desired x-axis (complete right-handed frame) + b1_c = torch.cross(b2_c, b3_c, dim=1) + + # Build rotation matrix + rotation_matrix_buffer[:, :, 0] = b1_c + rotation_matrix_buffer[:, :, 1] = b2_c + rotation_matrix_buffer[:, :, 2] = b3_c + + # Convert to quaternion + return math_utils.quat_from_matrix(rotation_matrix_buffer) + + +def compute_body_torque( + setpoint_orientation: torch.Tensor, + setpoint_angvel_b: torch.Tensor, + current_quat: torch.Tensor, + current_angvel_b: torch.Tensor, + robot_inertia: torch.Tensor, + K_rot: torch.Tensor, + K_angvel: torch.Tensor, + max_yaw_rate: float, +) -> torch.Tensor: + """PD attitude control in body frame with feedforward Coriolis term. + + Args: + setpoint_orientation: (num_envs, 4) desired orientation quaternion (wxyz) in world frame. + setpoint_angvel_b: (num_envs, 3) desired angular velocity in body frame [rad/s]. + current_quat: (num_envs, 4) current orientation quaternion (wxyz). + current_angvel_b: (num_envs, 3) current angular velocity in body frame [rad/s]. + robot_inertia: (num_envs, 3, 3) robot inertia matrix. + K_rot: (num_envs, 3) rotation gain. + K_angvel: (num_envs, 3) angular velocity gain. + max_yaw_rate: Maximum yaw rate [rad/s]. + + Returns: + (num_envs, 3) body torque command [N·m]. + """ + # Clamp yaw rate + setpoint_angvel_b[:, 2] = torch.clamp(setpoint_angvel_b[:, 2], -max_yaw_rate, max_yaw_rate) + + # Compute orientation error (R^T @ R_d) + RT_Rd_quat = math_utils.quat_mul(math_utils.quat_inv(current_quat), setpoint_orientation) + R_err = math_utils.matrix_from_quat(RT_Rd_quat) + + # Extract rotation error vector from skew-symmetric part + skew_matrix = R_err.transpose(-1, -2) - R_err + rotation_error = 0.5 * torch.stack([-skew_matrix[:, 1, 2], skew_matrix[:, 0, 2], -skew_matrix[:, 0, 1]], dim=1) + + # Angular velocity error + angvel_error = current_angvel_b - setpoint_angvel_b + + # Coriolis feedforward term: ω × (I·ω) + inertia_angvel = torch.bmm(robot_inertia, current_angvel_b.unsqueeze(2)).squeeze(2) + coriolis_term = torch.cross(current_angvel_b, inertia_angvel, dim=1) + + # PD + feedforward + torque = -K_rot * rotation_error - K_angvel * angvel_error + coriolis_term + return torque + + +def yaw_rate_to_body_angvel( + yaw_rate: torch.Tensor, roll: torch.Tensor, pitch: torch.Tensor, device: torch.device +) -> torch.Tensor: + """Convert yaw rate command to body angular velocity. + + Transformation: ω_body = T(roll, pitch) @ [0, 0, yaw_rate]^T + where T is the euler-to-body rate transformation matrix. + + Args: + yaw_rate: (num_envs,) desired yaw rate [rad/s]. + roll: (num_envs,) current roll angle [rad]. + pitch: (num_envs,) current pitch angle [rad]. + device: Device to allocate tensors on. + + Returns: + (num_envs, 3) desired angular velocity in body frame [rad/s]. + """ + s_pitch = torch.sin(pitch) + c_pitch = torch.cos(pitch) + s_roll = torch.sin(roll) + c_roll = torch.cos(roll) + + # Only yaw rate is non-zero, so only the third column matters + # ω_body = [−sin(pitch), sin(roll)*cos(pitch), cos(roll)*cos(pitch)]^T * yaw_rate + angvel_b = torch.zeros((yaw_rate.shape[0], 3), device=device) + angvel_b[:, 0] = -s_pitch * yaw_rate + angvel_b[:, 1] = s_roll * c_pitch * yaw_rate + angvel_b[:, 2] = c_roll * c_pitch * yaw_rate + + return angvel_b diff --git a/source/isaaclab_contrib/isaaclab_contrib/controllers/lee_position_control.py b/source/isaaclab_contrib/isaaclab_contrib/controllers/lee_position_control.py new file mode 100644 index 000000000000..82fff709042a --- /dev/null +++ b/source/isaaclab_contrib/isaaclab_contrib/controllers/lee_position_control.py @@ -0,0 +1,134 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +from typing import TYPE_CHECKING + +import torch + +import isaaclab.utils.math as math_utils + +from .lee_controller_base import LeeControllerBase +from .lee_controller_utils import compute_body_torque, compute_desired_orientation + +if TYPE_CHECKING: + from isaaclab.assets import Multirotor + + from .lee_position_control_cfg import LeePosControllerCfg + + +class LeePosController(LeeControllerBase): + """Lee position controller for multirotor tracking position setpoints. + + Computes a body-frame wrench command ``[Fx, Fy, Fz, Tx, Ty, Tz]`` from a position setpoint + in the world frame. Gains may be randomized per environment if enabled in the configuration. + """ + + cfg: LeePosControllerCfg + + def __init__(self, cfg: LeePosControllerCfg, asset: Multirotor, num_envs: int, device: str): + """Initialize controller. + + Args: + cfg: Controller configuration. + asset: Multirotor asset to control. + num_envs: Number of environments. + device: Device to run computations on. + """ + super().__init__(cfg, asset, num_envs, device) + + # Gain ranges + self.K_pos_range = torch.tensor(self.cfg.K_pos_range, device=device).repeat(num_envs, 1, 1) + self.K_vel_range = torch.tensor(self.cfg.K_vel_range, device=device).repeat(num_envs, 1, 1) + self.K_rot_range = torch.tensor(self.cfg.K_rot_range, device=device).repeat(num_envs, 1, 1) + self.K_angvel_range = torch.tensor(self.cfg.K_angvel_range, device=device).repeat(num_envs, 1, 1) + + # Current gains + self.K_pos_current = self.K_pos_range.mean(dim=1) + self.K_vel_current = self.K_vel_range.mean(dim=1) + self.K_rot_current = self.K_rot_range.mean(dim=1) + self.K_angvel_current = self.K_angvel_range.mean(dim=1) + + def compute(self, command: torch.Tensor) -> torch.Tensor: + """Compute wrench command from position setpoint. + + Args: + command: (num_envs, 4) [x, y, z, yaw] in body frame. + + Returns: + (num_envs, 6) wrench command [fx, fy, fz, tx, ty, tz] in body frame. + """ + self.wrench_command_b.zero_() + + root_quat_w, root_ang_vel_b, root_lin_vel_w = self._root_state_tensors() + root_pos_w = self._to_torch(self.robot.data.root_pos_w) + + # Compute acceleration from position error + acc = self._compute_acceleration( + setpoint_position=command[:, :3], + root_pos_w=root_pos_w, + root_lin_vel_w=root_lin_vel_w, + ) + forces_w = (acc - self.gravity) * self.mass.view(-1, 1) + + # Project forces to body z-axis for thrust command + body_z_w = math_utils.matrix_from_quat(root_quat_w)[:, :, 2] + self.wrench_command_b[:, 2] = torch.sum(forces_w * body_z_w, dim=1) + + # Get current yaw and compute desired orientation + desired_quat = compute_desired_orientation(forces_w, command[:, 3], self.rotation_matrix_buffer) + + # Zero angular velocity setpoint (hover) + desired_angvel_b = torch.zeros((self.num_envs, 3), device=self.device) + + # Compute torque command + self.wrench_command_b[:, 3:6] = compute_body_torque( + desired_quat, + desired_angvel_b, + root_quat_w, + root_ang_vel_b, + self.robot_inertia, + self.K_rot_current, + self.K_angvel_current, + self.cfg.max_yaw_rate, + ) + + return self.wrench_command_b + + def _randomize_params(self, env_ids: slice | torch.Tensor): + """Randomize controller gains for the given environments if enabled.""" + self.K_pos_current[env_ids] = math_utils.sample_uniform( + self.K_pos_range[env_ids, 0], self.K_pos_range[env_ids, 1], self.K_pos_range[env_ids, 0].shape, self.device + ) + self.K_vel_current[env_ids] = math_utils.sample_uniform( + self.K_vel_range[env_ids, 0], self.K_vel_range[env_ids, 1], self.K_vel_range[env_ids, 0].shape, self.device + ) + self.K_rot_current[env_ids] = math_utils.sample_uniform( + self.K_rot_range[env_ids, 0], self.K_rot_range[env_ids, 1], self.K_rot_range[env_ids, 0].shape, self.device + ) + self.K_angvel_current[env_ids] = math_utils.sample_uniform( + self.K_angvel_range[env_ids, 0], + self.K_angvel_range[env_ids, 1], + self.K_angvel_range[env_ids, 0].shape, + self.device, + ) + + def _compute_acceleration( + self, setpoint_position: torch.Tensor, root_pos_w: torch.Tensor, root_lin_vel_w: torch.Tensor + ) -> torch.Tensor: + """Compute desired acceleration from position error. + + Args: + setpoint_position: (num_envs, 3) desired position in world frame. + + Returns: + (num_envs, 3) desired acceleration in body frame. + """ + position_error = setpoint_position - root_pos_w + # Compute velocity error for position controller + velocity_error = -root_lin_vel_w + + return self.K_vel_current * velocity_error + self.K_pos_current * position_error diff --git a/source/isaaclab_contrib/isaaclab_contrib/controllers/lee_position_control_cfg.py b/source/isaaclab_contrib/isaaclab_contrib/controllers/lee_position_control_cfg.py new file mode 100644 index 000000000000..e2df30f8aa70 --- /dev/null +++ b/source/isaaclab_contrib/isaaclab_contrib/controllers/lee_position_control_cfg.py @@ -0,0 +1,44 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from dataclasses import MISSING + +from isaaclab.utils import configclass + +from .lee_controller_base_cfg import LeeControllerBaseCfg +from .lee_position_control import LeePosController + + +@configclass +class LeePosControllerCfg(LeeControllerBaseCfg): + """Configuration for a Lee-style geometric quadrotor position controller. + + Unless otherwise noted, vectors are ordered as (x, y, z) in the simulation world/body frames. + The position controller gains are sampled uniformly per environment between + their corresponding ``*_min`` and ``*_max`` bounds at reset. + """ + + class_type: type = LeePosController + """The class type for the position controller.""" + + K_pos_range: tuple[tuple[float, float, float], tuple[float, float, float]] = MISSING + """Position error proportional gain range about body axes [unitless]. + + This is a tuple of two tuples containing the minimum and maximum gains for each axis (x, y, z). + Format: ((min_x, min_y, min_z), (max_x, max_y, max_z)) + + Example: + ((3.0, 3.0, 2.0), (4.0, 4.0, 2.5)) for ARL Robot 1 + """ + + K_vel_range: tuple[tuple[float, float, float], tuple[float, float, float]] = MISSING + """Velocity error proportional gain range about body axes [unitless]. + + This is a tuple of two tuples containing the minimum and maximum gains for each axis (x, y, z). + Format: ((min_x, min_y, min_z), (max_x, max_y, max_z)) + + Example: + ((2.5, 2.5, 1.5), (3.5, 3.5, 2.0)) for ARL Robot 1 + """ diff --git a/source/isaaclab_contrib/isaaclab_contrib/controllers/lee_velocity_control.py b/source/isaaclab_contrib/isaaclab_contrib/controllers/lee_velocity_control.py new file mode 100644 index 000000000000..14dc1ff970c3 --- /dev/null +++ b/source/isaaclab_contrib/isaaclab_contrib/controllers/lee_velocity_control.py @@ -0,0 +1,133 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +from typing import TYPE_CHECKING + +import torch + +import isaaclab.utils.math as math_utils + +from .lee_controller_base import LeeControllerBase +from .lee_controller_utils import compute_body_torque, compute_desired_orientation, yaw_rate_to_body_angvel + +if TYPE_CHECKING: + from isaaclab.assets import Multirotor + + from .lee_velocity_control_cfg import LeeVelControllerCfg + + +class LeeVelController(LeeControllerBase): + """Lee velocity controller for multirotor tracking velocity setpoints. + + Computes a body-frame wrench command ``[Fx, Fy, Fz, Tx, Ty, Tz]`` from a velocity setpoint: + [vx, vy, vz, yaw_rate]. Gains may be randomized per environment if enabled in the configuration. + """ + + cfg: LeeVelControllerCfg + + def __init__(self, cfg: LeeVelControllerCfg, asset: Multirotor, num_envs: int, device: str): + """Initialize controller. + + Args: + cfg: Controller configuration. + asset: Multirotor asset to control. + num_envs: Number of environments. + device: Device to run computations on. + """ + super().__init__(cfg, asset, num_envs, device) + + # Gain ranges + self.K_vel_range = torch.tensor(self.cfg.K_vel_range, device=device).repeat(num_envs, 1, 1) + self.K_rot_range = torch.tensor(self.cfg.K_rot_range, device=device).repeat(num_envs, 1, 1) + self.K_angvel_range = torch.tensor(self.cfg.K_angvel_range, device=device).repeat(num_envs, 1, 1) + + # Current gains + self.K_vel_current = self.K_vel_range.mean(dim=1) + self.K_rot_current = self.K_rot_range.mean(dim=1) + self.K_angvel_current = self.K_angvel_range.mean(dim=1) + + def compute(self, command: torch.Tensor) -> torch.Tensor: + """Compute wrench command from velocity setpoint. + + Args: + command: (num_envs, 4) velocity command [vx, vy, vz, yaw_rate] in body frame. + + Returns: + (num_envs, 6) wrench command [fx, fy, fz, tx, ty, tz] in body frame. + """ + self.wrench_command_b.zero_() + + root_quat_w, root_ang_vel_b, root_lin_vel_w = self._root_state_tensors() + + # Compute acceleration from velocity tracking + acc = self._compute_acceleration( + setpoint_velocity=command[:, :3], root_quat_w=root_quat_w, root_lin_vel_w=root_lin_vel_w + ) + + forces_w = (acc - self.gravity) * self.mass.view(-1, 1) + + # Project forces to body z-axis for thrust command + body_z_w = math_utils.matrix_from_quat(root_quat_w)[:, :, 2] + self.wrench_command_b[:, 2] = torch.sum(forces_w * body_z_w, dim=1) + + # Compute desired orientation from force direction and yaw setpoint + roll, pitch, yaw = math_utils.euler_xyz_from_quat(root_quat_w) + desired_quat = compute_desired_orientation(forces_w, yaw, self.rotation_matrix_buffer) + + # Compute desired angular velocity in body frame from yaw rate command + desired_angvel_b = yaw_rate_to_body_angvel(command[:, 3], roll, pitch, self.device) + + # Compute torque command + self.wrench_command_b[:, 3:6] = compute_body_torque( + desired_quat, + desired_angvel_b, + root_quat_w, + root_ang_vel_b, + self.robot_inertia, + self.K_rot_current, + self.K_angvel_current, + self.cfg.max_yaw_rate, + ) + + return self.wrench_command_b + + def _randomize_params(self, env_ids: slice | torch.Tensor): + """Randomize controller gains for the given environments if enabled.""" + self.K_vel_current[env_ids] = math_utils.sample_uniform( + self.K_vel_range[env_ids, 0], self.K_vel_range[env_ids, 1], self.K_vel_range[env_ids, 0].shape, self.device + ) + self.K_rot_current[env_ids] = math_utils.sample_uniform( + self.K_rot_range[env_ids, 0], self.K_rot_range[env_ids, 1], self.K_rot_range[env_ids, 0].shape, self.device + ) + self.K_angvel_current[env_ids] = math_utils.sample_uniform( + self.K_angvel_range[env_ids, 0], + self.K_angvel_range[env_ids, 1], + self.K_angvel_range[env_ids, 0].shape, + self.device, + ) + + def _compute_acceleration( + self, setpoint_velocity: torch.Tensor, root_quat_w: torch.Tensor, root_lin_vel_w: torch.Tensor + ) -> torch.Tensor: + """Compute desired acceleration from velocity tracking error. + + Args: + setpoint_velocity: (num_envs, 3) desired velocity in body frame. + + Returns: + (num_envs, 3) desired acceleration in body frame. + """ + # Get yaw-only orientation (vehicle frame) + _, _, yaw = math_utils.euler_xyz_from_quat(root_quat_w) + vehicle_quat = math_utils.quat_from_euler_xyz(torch.zeros_like(yaw), torch.zeros_like(yaw), yaw) + + # Transform setpoint from body to world frame + setpoint_velocity_w = math_utils.quat_apply(vehicle_quat, setpoint_velocity) + + # Compute velocity error and acceleration command + velocity_error = setpoint_velocity_w - root_lin_vel_w + return self.K_vel_current * velocity_error diff --git a/source/isaaclab_contrib/isaaclab_contrib/controllers/lee_velocity_control_cfg.py b/source/isaaclab_contrib/isaaclab_contrib/controllers/lee_velocity_control_cfg.py new file mode 100644 index 000000000000..13ef9814d268 --- /dev/null +++ b/source/isaaclab_contrib/isaaclab_contrib/controllers/lee_velocity_control_cfg.py @@ -0,0 +1,34 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from dataclasses import MISSING + +from isaaclab.utils import configclass + +from .lee_controller_base_cfg import LeeControllerBaseCfg +from .lee_velocity_control import LeeVelController + + +@configclass +class LeeVelControllerCfg(LeeControllerBaseCfg): + """Configuration for a Lee-style geometric quadrotor velocity controller. + + Unless otherwise noted, vectors are ordered as (x, y, z) in the simulation world/body frames. + The velocity controller gains are sampled uniformly per environment between + their corresponding ``*_min`` and ``*_max`` bounds at reset. + """ + + class_type: type = LeeVelController + """The class type for the velocity controller.""" + + K_vel_range: tuple[tuple[float, float, float], tuple[float, float, float]] = MISSING + """Velocity error proportional gain range about body axes [unitless]. + + This is a tuple of two tuples containing the minimum and maximum gains for each axis (x, y, z). + Format: ((min_x, min_y, min_z), (max_x, max_y, max_z)) + + Example: + ((2.5, 2.5, 1.5), (3.5, 3.5, 2.0)) for ARL Robot 1 + """ diff --git a/source/isaaclab_contrib/isaaclab_contrib/mdp/__init__.pyi b/source/isaaclab_contrib/isaaclab_contrib/mdp/__init__.pyi index 412db0ce4d8f..497bd981d0dd 100644 --- a/source/isaaclab_contrib/isaaclab_contrib/mdp/__init__.pyi +++ b/source/isaaclab_contrib/isaaclab_contrib/mdp/__init__.pyi @@ -5,7 +5,9 @@ __all__ = [ "ThrustAction", + "NavigationAction", "ThrustActionCfg", + "NavigationActionCfg", ] -from .actions import ThrustAction, ThrustActionCfg +from .actions import NavigationAction, NavigationActionCfg, ThrustAction, ThrustActionCfg diff --git a/source/isaaclab_contrib/isaaclab_contrib/mdp/actions/__init__.pyi b/source/isaaclab_contrib/isaaclab_contrib/mdp/actions/__init__.pyi index 8203016432d4..ede3ae69e2d0 100644 --- a/source/isaaclab_contrib/isaaclab_contrib/mdp/actions/__init__.pyi +++ b/source/isaaclab_contrib/isaaclab_contrib/mdp/actions/__init__.pyi @@ -5,8 +5,10 @@ __all__ = [ "ThrustAction", + "NavigationAction", "ThrustActionCfg", + "NavigationActionCfg", ] -from .thrust_actions import ThrustAction -from .thrust_actions_cfg import ThrustActionCfg +from .thrust_actions import NavigationAction, ThrustAction +from .thrust_actions_cfg import NavigationActionCfg, ThrustActionCfg diff --git a/source/isaaclab_contrib/isaaclab_contrib/mdp/actions/thrust_actions.py b/source/isaaclab_contrib/isaaclab_contrib/mdp/actions/thrust_actions.py index 5ed60f190c4f..d529692a8485 100644 --- a/source/isaaclab_contrib/isaaclab_contrib/mdp/actions/thrust_actions.py +++ b/source/isaaclab_contrib/isaaclab_contrib/mdp/actions/thrust_actions.py @@ -244,3 +244,171 @@ def apply_actions(self): """ # Set thrust targets using thruster IDs self._asset.set_thrust_target(self.processed_actions, thruster_ids=self._thruster_ids) + + +class NavigationAction(ThrustAction): + """Navigation action term that converts high-level navigation commands to thrust commands + using a geometric tracking controller. + + This action term extends `ThrustAction` by adding a controller layer that computes wrench + (force and torque) commands from navigation setpoints, then allocates those wrenches to + individual thruster commands using the multirotor's allocation matrix. + + The controller type is automatically determined based on the `controller_cfg` type: + - LeeVelControllerCfg: Velocity tracking controller + - LeePosControllerCfg: Position tracking controller + - LeeAccControllerCfg: Acceleration tracking controller + + The control pipeline: + 1. Process raw actions (scale, offset, clip) using parent `ThrustAction` + 2. Transform processed actions into setpoints constrained within camera FOV + 3. Compute 6-DOF wrench command using the selected Lee controller + 4. Solve thrust allocation: thrust_cmd = pinv(allocation_matrix) @ wrench_cmd + 5. Apply thrust commands to thrusters + + Attributes: + cfg: Configuration for the navigation action term, including controller config. + _lc: Lee controller instance (LeeVelController, LeePosController, or LeeAccController). + + Action Space: + The action dimension is always 3D: (forward_magnitude, pitch_angle, yaw_rate) + + Actions are clipped in range [-1, 1] and are transformed to controller commands: + - Forward position/velocity/acceleration: + [0, max_magnitude] via (action[0] + 1) * cos(pitch) * max_magnitude / 2 + - Lateral position/velocity/acceleration: + Always 0.0 (constrained to camera FOV) + - Vertical position/velocity/acceleration: + [0, max_magnitude] via (action[0] + 1) * sin(pitch) * max_magnitude / 2 + - Yaw command: [-max_yaw_command, max_yaw_command] via action[2] * max_yaw_command (yaw command is yawrate + [rad/s] for velocity and acceleration control and relative yaw change [rad] for position control) + + Where: + - pitch angle is computed as: action[1] * max_inclination_angle + + Parameters (from cfg): + max_magnitude: Maximum translational magnitude for position/velocity/acceleration commands. + max_yaw_command: Maximum yaw command in rad/s for velocity and acceleration + control and relative yaw change [rad] for position control. + max_inclination_angle: Maximum pitch angle in rad. + + Notes: + - The controller's internal states (e.g., integral terms) are reset when `reset()` is called. + - Lateral term is constrained to 0.0 to keep commands within camera FOV. + - The x and z components are derived from magnitude and inclination angle. + - Requires the multirotor asset to have a valid `allocation_matrix` attribute. + + Example: + ```python + cfg = NavigationActionCfg( + controller_cfg=LeeVelControllerCfg(...), + asset_name="robot", + max_magnitude=2.0, + max_yaw_command=1.047, + max_inclination_angle=0.785, # pi/4 + ) + nav_action = NavigationAction(cfg, env) + ``` + """ + + cfg: thrust_actions_cfg.NavigationActionCfg + """The configuration of the action term.""" + + def __init__(self, cfg: thrust_actions_cfg.NavigationActionCfg, env: ManagerBasedEnv) -> None: + # Initialize parent class (this handles all the thruster setup) + super().__init__(cfg, env) + + # Initialize controller using class_type from config + self._lc = self.cfg.controller_cfg.class_type( + cfg=self.cfg.controller_cfg, asset=self._asset, num_envs=self.num_envs, device=self.device + ) + + # Log warning if not using velocity controller + from isaaclab_contrib.controllers import LeeVelControllerCfg + + if not isinstance(self.cfg.controller_cfg, LeeVelControllerCfg): + logger.warning( + "Navigation task tuned for velocity control. " + "Consider using velocity controller for better performance or retune reward function." + ) + + # Cache allocation matrix and its pseudo-inverse (static for this asset/config) + self._allocation_matrix = self._asset.allocation_matrix + self._allocation_pinv = torch.linalg.pinv(self._allocation_matrix) + + # Add buffer to store velocity commands for observations) + self._commands = torch.zeros(self.num_envs, 4, device=self.device) + self._prev_commands = torch.zeros(self.num_envs, 4, device=self.device) + + @property + def action_dim(self) -> int: + return 3 + + @property + def prev_commands(self) -> torch.Tensor: + return self._prev_commands + + @property + def IO_descriptor(self) -> GenericActionIODescriptor: + """The IO descriptor of the action term.""" + # Get parent IO descriptor + descriptor = super().IO_descriptor + # Override action type for navigation + descriptor.action_type = "NavigationAction" + return descriptor + + def process_actions(self, actions: torch.Tensor): + """Process actions by applying scaling, offset, and clipping.""" + # Call parent to handle basic processing + super().process_actions(actions) + + self._has_actions_updated = False + + def apply_actions(self): + """Apply the processed actions as velocity commands.""" + # process the actions to be in the correct range + clamped_action = torch.clamp(self.processed_actions, min=-1.0, max=1.0) + processed_actions = torch.zeros(self.num_envs, 4, device=self.device) + + clamped_action[:, 0] += 1.0 # only allow positive thrust commands [0, 2] + processed_actions[:, 0] = ( + clamped_action[:, 0] + * torch.cos(self.cfg.max_inclination_angle * clamped_action[:, 1]) + * self.cfg.max_magnitude + / 2.0 + ) + processed_actions[:, 1] = 0.0 # set lateral thrust command to 0 + processed_actions[:, 2] = ( + clamped_action[:, 0] + * torch.sin(self.cfg.max_inclination_angle * clamped_action[:, 1]) + * self.cfg.max_magnitude + / 2.0 + ) + processed_actions[:, 3] = clamped_action[:, 2] * self.cfg.max_yaw_command + + # Store velocity commands for observations + if not self._has_actions_updated: + self._prev_commands[:] = self._commands + self._commands[:] = processed_actions + self._has_actions_updated = True + + # Compute wrench command using controller + wrench_command = self._lc.compute(processed_actions) + + # Convert wrench to thrust commands using allocation matrix + thrust_commands = wrench_command @ self._allocation_pinv.T + + # Apply thrust commands using thruster IDs + self._asset.set_thrust_target(thrust_commands, thruster_ids=self._thruster_ids) + + def reset(self, env_ids: Sequence[int] | None = None) -> None: + # Call parent reset + super().reset(env_ids) + # Reset controller internal states + self._lc.reset_idx(env_ids) + + if env_ids is None: + env_ids = slice(None) + + self._commands[env_ids] = 0.0 + self._prev_commands[env_ids] = 0.0 diff --git a/source/isaaclab_contrib/isaaclab_contrib/mdp/actions/thrust_actions_cfg.py b/source/isaaclab_contrib/isaaclab_contrib/mdp/actions/thrust_actions_cfg.py index 3a464b8fce84..d06242f80ab0 100644 --- a/source/isaaclab_contrib/isaaclab_contrib/mdp/actions/thrust_actions_cfg.py +++ b/source/isaaclab_contrib/isaaclab_contrib/mdp/actions/thrust_actions_cfg.py @@ -2,6 +2,7 @@ # All rights reserved. # # SPDX-License-Identifier: BSD-3-Clause +from __future__ import annotations from dataclasses import MISSING from typing import TYPE_CHECKING @@ -10,7 +11,9 @@ from isaaclab.utils import configclass if TYPE_CHECKING: - from .thrust_actions import ThrustAction + from isaaclab_contrib.controllers import LeeAccControllerCfg, LeePosControllerCfg, LeeVelControllerCfg + + from .thrust_actions import NavigationAction, ThrustAction @configclass @@ -72,7 +75,7 @@ class ThrustActionCfg(ActionTermCfg): - :class:`~isaaclab.managers.ActionTermCfg`: Base action term configuration """ - class_type: type["ThrustAction"] | str = "{DIR}.thrust_actions:ThrustAction" + class_type: type[ThrustAction] | str = "{DIR}.thrust_actions:ThrustAction" asset_name: str = MISSING """Name or regex expression of the asset that the action will be mapped to. @@ -168,3 +171,37 @@ class ThrustActionCfg(ActionTermCfg): If ``False``, the manually specified :attr:`offset` value is used. """ + + +@configclass +class NavigationActionCfg(ThrustActionCfg): + """Configuration for the navigation action term. + + This action term constrains the controller action to be within the field of view (FOV) + of the camera sensor. Specifically: + + - **y-component**: Always 0, as the camera FOV constraint restricts lateral movement + - **x and z components**: Derived from the action max_magnitude and max_inclination_angle, + ensuring the desired acceleration/velocity/position vector remains aligned with the camera's + viewing direction + + This constraint ensures that navigation commands respect the sensor's field of view + limitations, preventing commands that would be out of the camera's visual range. + + See :class:`NavigationAction` for more details. + """ + + class_type: type[NavigationAction] | str = "{DIR}.thrust_actions:NavigationAction" + + controller_cfg: LeeVelControllerCfg | LeePosControllerCfg | LeeAccControllerCfg = MISSING + """The configuration for the Lee velocity controller.""" + + max_magnitude: float = MISSING + """Maximum magnitude for position [m], velocity [m/s], or acceleration [m/s²] commands.""" + + max_yaw_command: float = MISSING + """Maximum yaw command. Yaw rate [rad/s] for velocity and acceleration lee geometric controller and relative + yaw change [rad] for position lee geometric controller.""" + + max_inclination_angle: float = MISSING + """Maximum inclination angle [rad] for position, velocity and acceleration lee geometric controller.""" diff --git a/source/isaaclab_contrib/isaaclab_contrib/rl/rlinf/extension.py b/source/isaaclab_contrib/isaaclab_contrib/rl/rlinf/extension.py index 89368c532109..1defd1a0d503 100644 --- a/source/isaaclab_contrib/isaaclab_contrib/rl/rlinf/extension.py +++ b/source/isaaclab_contrib/isaaclab_contrib/rl/rlinf/extension.py @@ -440,6 +440,19 @@ def __init__(self, cfg, num_envs: int, seed_offset: int, total_num_processes: in """ super().__init__(cfg, num_envs, seed_offset, total_num_processes, worker_info) + def _record_metrics(self, step_reward, terminations, infos): + """Override to use terminations (task completion) for success_once.""" + + episode_info = {} + self.returns += step_reward + self.success_once = self.success_once | terminations.bool() + episode_info["success_once"] = self.success_once.clone() + episode_info["return"] = self.returns.clone() + episode_info["episode_len"] = self.elapsed_steps.clone() + episode_info["reward"] = episode_info["return"] / episode_info["episode_len"] + infos["episode"] = episode_info + return infos + def _make_env_function(self) -> collections.abc.Callable: """Create the environment factory function. @@ -468,6 +481,7 @@ def make_env_isaaclab() -> tuple: isaac_env_cfg.scene.num_envs = self.cfg.init_params.num_envs env = gym.make(self.isaaclab_env_id, cfg=isaac_env_cfg, render_mode="rgb_array").unwrapped + return env, sim_app return make_env_isaaclab @@ -481,7 +495,6 @@ def _wrap_obs(self, obs: dict) -> dict: - ``"extra_view_images"``: ``(B, N, H, W, C)`` — stacked extra cameras. - ``"states"``: ``(B, D)`` — concatenated state vector. - ``"task_descriptions"``: ``list[str]`` — task descriptions. - Config is read from the YAML file via :func:`_get_isaaclab_cfg`. Args: diff --git a/source/isaaclab_contrib/isaaclab_contrib/sensors/tacsl_sensor/visuotactile_sensor.py b/source/isaaclab_contrib/isaaclab_contrib/sensors/tacsl_sensor/visuotactile_sensor.py index 720a85223aa6..12ff6cfd3e8d 100644 --- a/source/isaaclab_contrib/isaaclab_contrib/sensors/tacsl_sensor/visuotactile_sensor.py +++ b/source/isaaclab_contrib/isaaclab_contrib/sensors/tacsl_sensor/visuotactile_sensor.py @@ -66,7 +66,8 @@ class VisuoTactileSensor(SensorBase): The following requirements must be satisfied for proper sensor operation: **Camera Tactile Imaging** - If ``enable_camera_tactile=True``, a valid ``camera_cfg`` (CameraCfg) must be + If ``enable_camera_tactile=True``, a valid ``camera_cfg`` + (:class:`~isaaclab.sensors.CameraCfg`) must be provided with appropriate camera parameters. **Force Field Computation** diff --git a/source/isaaclab_contrib/isaaclab_contrib/utils/math.py b/source/isaaclab_contrib/isaaclab_contrib/utils/math.py new file mode 100644 index 000000000000..840cc62440b5 --- /dev/null +++ b/source/isaaclab_contrib/isaaclab_contrib/utils/math.py @@ -0,0 +1,93 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-module containing utilities for various math operations.""" + +# needed to import for allowing type-hinting: torch.Tensor | np.ndarray +from __future__ import annotations + +import logging + +import torch +import torch.nn.functional + +from isaaclab.utils.math import matrix_from_quat + +# import logger +logger = logging.getLogger(__name__) + + +def aggregate_inertia_about_robot_com( + body_inertias_local: torch.Tensor, + body_inv_mass_local: torch.Tensor, + body_com_pos_b: torch.Tensor, + body_com_quat_b: torch.Tensor, + body_pos_b: torch.Tensor, + body_quat_b: torch.Tensor, + eps=1e-12, +) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor]: + """ + Aggregate per-link inertias into a single inertia about the robot COM, + expressed in the base (root link) frame. + + Shapes: + num_envs=N, num_bodies=B + + Args: + body_inertias_local (N,B,9|3,3): Link inertias in the mass/COM frame. + body_inv_mass_local (N,B): Inverse link masses (<=0 treated as padding). + body_com_pos_b (N,B,3): Link COM position relative to the link frame + (massLocalPose translation); used as body_pos_b + R_link_base @ body_com_pos_b. + body_com_quat_b (N,B,4 xyzw): Mass→link rotation (massLocalPose rotation). + body_pos_b (N,B,3): Link origins in base frame. + body_quat_b (N,B,4 xyzw): Link→base orientation. + eps (float): Small value to guard division by zero. + + Returns: + total_mass (N,): Sum of link masses. + I_total (N,3,3): Inertia about robot COM in base frame (symmetrized). + com_robot_b (N,3): Robot COM in base frame. + + Method (base frame throughout): + 1) COM of each link: com_link_b = body_pos_b + R_link_base @ body_com_pos_b + 2) Robot COM: mass-weighted average of com_link_b + 3) Transform each link inertia via R: I_b = R I_local R^T + 4) Parallel-axis: I_pa = m (‖r‖² I - r rᵀ), r = com_link_b - com_robot_b + 5) Sum over links and symmetrize + """ + # Inertia in mass frame (local to COM) + num_envs, num_bodies, _ = body_inertias_local.shape + I_local = body_inertias_local.view(num_envs, num_bodies, 3, 3) + + # Masses + m = torch.where(body_inv_mass_local > 0, 1.0 / body_inv_mass_local, torch.zeros_like(body_inv_mass_local)) + m_sum = m.sum(dim=1, keepdim=True) + valid = (m > 0).float().unsqueeze(-1) + + # Link COM positions in base frame + R_link_base = matrix_from_quat(body_quat_b) + com_link_b = body_pos_b + (R_link_base @ body_com_pos_b[..., :, None]).squeeze(-1) + + # Robot COM base frame (mass-weighted) + com_robot_b = (m.unsqueeze(-1) * com_link_b).sum(dim=1) / (m_sum + eps) + + # Rotate inertia from mass frame to world: R = R_link_base * R_mass + R_mass = matrix_from_quat(body_com_quat_b) + R = R_link_base @ R_mass + I_world = R @ I_local @ R.transpose(-1, -2) + + # Parallel-axis to robot COM + r = com_link_b - com_robot_b[:, None, :] + rrT = r[..., :, None] @ r[..., None, :] + r2 = (r * r).sum(dim=-1, keepdim=True) + I3 = torch.eye(3, device=body_pos_b.device).reshape(1, 1, 3, 3).expand(num_envs, num_bodies, 3, 3) + I_pa = m[..., None, None] * (r2[..., None] * I3 - rrT) + + # Sum over links (ignore zero-mass pads) + I_total = ((I_world + I_pa) * valid[..., None]).sum(dim=1) + I_total = 0.5 * (I_total + I_total.transpose(-1, -2)) + total_mass = m.sum(dim=1) + + return total_mass, I_total, com_robot_b diff --git a/source/isaaclab_contrib/test/controllers/test_drone_geometric_controllers.py b/source/isaaclab_contrib/test/controllers/test_drone_geometric_controllers.py new file mode 100644 index 000000000000..c24c8e5dc870 --- /dev/null +++ b/source/isaaclab_contrib/test/controllers/test_drone_geometric_controllers.py @@ -0,0 +1,345 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Launch Isaac Sim Simulator first.""" + +from isaaclab.app import AppLauncher + +# launch omniverse app +simulation_app = AppLauncher(headless=True).app + +"""Rest everything follows.""" + +import types + +import pytest +import torch + +from isaaclab_contrib.controllers import ( + lee_acceleration_control as acc_mod, +) +from isaaclab_contrib.controllers import ( + lee_attitude_control as att_mod, +) +from isaaclab_contrib.controllers import lee_controller_base as base_mod +from isaaclab_contrib.controllers import ( + lee_position_control as pos_mod, +) +from isaaclab_contrib.controllers import ( + lee_velocity_control as vel_mod, +) +from isaaclab_contrib.controllers.lee_acceleration_control_cfg import LeeAccControllerCfg +from isaaclab_contrib.controllers.lee_attitude_control_cfg import LeeAttControllerCfg +from isaaclab_contrib.controllers.lee_position_control_cfg import LeePosControllerCfg +from isaaclab_contrib.controllers.lee_velocity_control_cfg import LeeVelControllerCfg + + +class _DummyRootView: + """Stub articulation view with ``get_masses`` and ``get_inertias`` for controller tests.""" + + def __init__(self, num_envs: int, num_bodies: int, device: torch.device): + inertia_flat = torch.eye(3, device=device).reshape(9) + self._inertias = inertia_flat.unsqueeze(0).unsqueeze(0).expand(num_envs, num_bodies, 9).clone() + self._masses = torch.ones((num_envs, num_bodies), device=device) + + def get_inertias(self) -> torch.Tensor: + return self._inertias + + def get_masses(self) -> torch.Tensor: + return self._masses + + +class _DummyRobot: + """Minimal multirotor stub exposing the attributes used by the controllers.""" + + def __init__(self, num_envs: int, num_bodies: int, device: torch.device): + self.num_bodies = num_bodies + quat_id = torch.tensor([0.0, 0.0, 0.0, 1.0], device=device) + self.data = types.SimpleNamespace( + root_link_quat_w=quat_id.repeat(num_envs, 1), + root_quat_w=quat_id.repeat(num_envs, 1), + root_pos_w=torch.zeros((num_envs, 3), device=device), + root_lin_vel_w=torch.zeros((num_envs, 3), device=device), + root_ang_vel_b=torch.zeros((num_envs, 3), device=device), + body_link_pos_w=torch.zeros((num_envs, num_bodies, 3), device=device), + body_link_quat_w=quat_id.repeat(num_envs, num_bodies, 1), + body_com_pos_b=torch.zeros((num_envs, num_bodies, 3), device=device), + body_com_quat_b=quat_id.repeat(num_envs, num_bodies, 1), + ) + self.root_view = _DummyRootView(num_envs, num_bodies, device) + + +class _DummySimCfg: + """Mock simulation config.""" + + def __init__(self): + self.gravity = (0.0, 0.0, -9.81) + + +class _DummySimContext: + """Mock simulation context.""" + + def __init__(self): + self.cfg = _DummySimCfg() + + +def _patch_aggregate(monkeypatch, _module, num_envs, device): + def _agg(*_args, **_kwargs): + return ( + torch.ones(num_envs, device=device), + torch.eye(3, device=device).repeat(num_envs, 1, 1), + torch.zeros((num_envs, 3, 3), device=device), + ) + + monkeypatch.setattr(base_mod, "aggregate_inertia_about_robot_com", _agg) + + +def _patch_sim_context(monkeypatch: pytest.MonkeyPatch, module) -> None: + """Monkeypatch SimulationContext.instance() to return a mock.""" + import isaaclab.sim as sim_utils + + def _mock_instance(): + return _DummySimContext() + + monkeypatch.setattr(sim_utils.SimulationContext, "instance", _mock_instance) + + +def _device_param(device_str: str) -> torch.device: + """Return the torch.device or skip when CUDA is unavailable.""" + if device_str == "cuda" and not torch.cuda.is_available(): + pytest.skip("CUDA not available on this system") + return torch.device(device_str) + + +def _create_vel_cfg() -> LeeVelControllerCfg: + """Create velocity controller config with required parameters.""" + cfg = LeeVelControllerCfg() + cfg.K_vel_range = ((2.7, 2.7, 1.3), (3.3, 3.3, 1.7)) + cfg.K_rot_range = ((1.6, 1.6, 0.25), (1.85, 1.85, 0.4)) + cfg.K_angvel_range = ((0.4, 0.4, 0.075), (0.5, 0.5, 0.09)) + cfg.max_inclination_angle_rad = 1.0471975511965976 + cfg.max_yaw_rate = 1.0471975511965976 + return cfg + + +def _create_pos_cfg() -> LeePosControllerCfg: + """Create position controller config with required parameters.""" + cfg = LeePosControllerCfg() + cfg.K_pos_range = ((3.0, 3.0, 2.0), (4.0, 4.0, 2.5)) + cfg.K_vel_range = ((2.5, 2.5, 1.5), (3.5, 3.5, 2.0)) + cfg.K_rot_range = ((1.6, 1.6, 0.25), (1.85, 1.85, 0.4)) + cfg.K_angvel_range = ((0.4, 0.4, 0.075), (0.5, 0.5, 0.09)) + cfg.max_inclination_angle_rad = 1.0471975511965976 + cfg.max_yaw_rate = 1.0471975511965976 + return cfg + + +def _create_acc_cfg() -> LeeAccControllerCfg: + """Create acceleration controller config with required parameters.""" + cfg = LeeAccControllerCfg() + cfg.K_rot_range = ((1.6, 1.6, 0.25), (1.85, 1.85, 0.4)) + cfg.K_angvel_range = ((0.4, 0.4, 0.075), (0.5, 0.5, 0.09)) + cfg.max_inclination_angle_rad = 1.0471975511965976 + cfg.max_yaw_rate = 1.0471975511965976 + return cfg + + +def _create_att_cfg() -> LeeAttControllerCfg: + """Create attitude controller config with required parameters.""" + cfg = LeeAttControllerCfg() + cfg.K_rot_range = ((1.6, 1.6, 0.25), (1.85, 1.85, 0.4)) + cfg.K_angvel_range = ((0.4, 0.4, 0.075), (0.5, 0.5, 0.09)) + cfg.max_yaw_rate = 1.0471975511965976 + return cfg + + +@pytest.mark.parametrize("device_str", ["cpu", "cuda"]) +@pytest.mark.parametrize("num_envs", [1, 2, 8]) +@pytest.mark.parametrize("num_bodies", [1, 4]) +@pytest.mark.parametrize( + "controller_cls,cfg_factory,mod_name", + [ + ("LeeVelController", _create_vel_cfg, vel_mod), + ("LeePosController", _create_pos_cfg, pos_mod), + ("LeeAccController", _create_acc_cfg, acc_mod), + ("LeeAttController", _create_att_cfg, att_mod), + ], +) +def test_lee_controllers_basic( + monkeypatch: pytest.MonkeyPatch, + device_str: str, + num_envs: int, + num_bodies: int, + controller_cls: str, + cfg_factory, + mod_name, +): + """Controllers return finite (N, 6) wrench on zero state and counter gravity on +Z. + + Tests various configurations of number of environments and bodies to catch edge cases. + """ + device = _device_param(device_str) + _patch_aggregate(monkeypatch, mod_name, num_envs, device) + _patch_sim_context(monkeypatch, mod_name) + robot = _DummyRobot(num_envs, num_bodies, device) + + cfg = cfg_factory() + controller = getattr(mod_name, controller_cls)(cfg, robot, num_envs=num_envs, device=str(device)) + + command = torch.zeros((num_envs, 4), device=device) + + wrench = controller.compute(command) + + assert wrench.shape == (num_envs, 6), f"Expected shape ({num_envs}, 6), got {wrench.shape}" + assert torch.isfinite(wrench).all(), "Wrench contains non-finite values" + assert torch.all(wrench[:, 2] > 0.0), "Body-z force should oppose gravity" + + +@pytest.mark.parametrize("device_str", ["cpu", "cuda"]) +@pytest.mark.parametrize("num_envs", [1, 2, 8]) +@pytest.mark.parametrize("num_bodies", [1, 4]) +def test_lee_vel_randomize_params_within_bounds( + monkeypatch: pytest.MonkeyPatch, device_str: str, num_envs: int, num_bodies: int +): + """Randomized gains stay within configured ranges for velocity controller. + + Tests edge cases with single and multiple environments and bodies. + """ + device = _device_param(device_str) + _patch_aggregate(monkeypatch, vel_mod, num_envs, device) + _patch_sim_context(monkeypatch, vel_mod) + robot = _DummyRobot(num_envs, num_bodies, device) + + cfg = _create_vel_cfg() + controller = vel_mod.LeeVelController(cfg, robot, num_envs=num_envs, device=str(device)) + + controller.reset_idx(env_ids=None) + + # Ensure tensors are on the correct device + K_vel_min = torch.tensor(cfg.K_vel_range[0], device=device, dtype=torch.float32) + K_vel_max = torch.tensor(cfg.K_vel_range[1], device=device, dtype=torch.float32) + + # Move controller gains to same device if needed + K_vel_current = controller.K_vel_current.to(device) + + assert K_vel_current.shape == (num_envs, 3), f"Expected shape ({num_envs}, 3), got {K_vel_current.shape}" + assert torch.all(K_vel_current >= K_vel_min), f"K_vel below minimum: {K_vel_current.min()} < {K_vel_min.min()}" + assert torch.all(K_vel_current <= K_vel_max), f"K_vel above maximum: {K_vel_current.max()} > {K_vel_max.max()}" + + +@pytest.mark.parametrize("device_str", ["cpu", "cuda"]) +@pytest.mark.parametrize("num_envs", [1, 2, 8]) +@pytest.mark.parametrize("num_bodies", [1, 4]) +def test_lee_pos_randomize_params_within_bounds( + monkeypatch: pytest.MonkeyPatch, device_str: str, num_envs: int, num_bodies: int +): + """Randomized gains stay within configured ranges for position controller. + + Tests edge cases with single and multiple environments and bodies. + """ + device = _device_param(device_str) + _patch_aggregate(monkeypatch, pos_mod, num_envs, device) + _patch_sim_context(monkeypatch, pos_mod) + robot = _DummyRobot(num_envs, num_bodies, device) + + cfg = _create_pos_cfg() + controller = pos_mod.LeePosController(cfg, robot, num_envs=num_envs, device=str(device)) + + controller.reset_idx(env_ids=None) + + # Check K_pos gains + K_pos_min = torch.tensor(cfg.K_pos_range[0], device=device, dtype=torch.float32) + K_pos_max = torch.tensor(cfg.K_pos_range[1], device=device, dtype=torch.float32) + K_pos_current = controller.K_pos_current.to(device) + + assert K_pos_current.shape == (num_envs, 3), f"Expected shape ({num_envs}, 3), got {K_pos_current.shape}" + assert torch.all(K_pos_current >= K_pos_min), f"K_pos below minimum: {K_pos_current.min()} < {K_pos_min.min()}" + assert torch.all(K_pos_current <= K_pos_max), f"K_pos above maximum: {K_pos_current.max()} > {K_pos_max.max()}" + + +@pytest.mark.parametrize("device_str", ["cpu", "cuda"]) +@pytest.mark.parametrize("num_envs", [1, 2, 8]) +@pytest.mark.parametrize("num_bodies", [1, 4]) +def test_lee_acc_randomize_params_within_bounds( + monkeypatch: pytest.MonkeyPatch, device_str: str, num_envs: int, num_bodies: int +): + """Randomized gains stay within configured ranges for acceleration controller. + + Tests edge cases with single and multiple environments and bodies. + """ + device = _device_param(device_str) + _patch_aggregate(monkeypatch, acc_mod, num_envs, device) + _patch_sim_context(monkeypatch, acc_mod) + robot = _DummyRobot(num_envs, num_bodies, device) + + cfg = _create_acc_cfg() + controller = acc_mod.LeeAccController(cfg, robot, num_envs=num_envs, device=str(device)) + + controller.reset_idx(env_ids=None) + + # Check K_rot gains + K_rot_min = torch.tensor(cfg.K_rot_range[0], device=device, dtype=torch.float32) + K_rot_max = torch.tensor(cfg.K_rot_range[1], device=device, dtype=torch.float32) + K_rot_current = controller.K_rot_current.to(device) + + assert K_rot_current.shape == (num_envs, 3), f"Expected shape ({num_envs}, 3), got {K_rot_current.shape}" + assert torch.all(K_rot_current >= K_rot_min), f"K_rot below minimum: {K_rot_current.min()} < {K_rot_min.min()}" + assert torch.all(K_rot_current <= K_rot_max), f"K_rot above maximum: {K_rot_current.max()} > {K_rot_max.max()}" + + # Check K_angvel gains + K_angvel_min = torch.tensor(cfg.K_angvel_range[0], device=device, dtype=torch.float32) + K_angvel_max = torch.tensor(cfg.K_angvel_range[1], device=device, dtype=torch.float32) + K_angvel_current = controller.K_angvel_current.to(device) + + assert K_angvel_current.shape == (num_envs, 3), f"Expected shape ({num_envs}, 3), got {K_angvel_current.shape}" + assert torch.all(K_angvel_current >= K_angvel_min), ( + f"K_angvel below minimum: {K_angvel_current.min()} < {K_angvel_min.min()}" + ) + assert torch.all(K_angvel_current <= K_angvel_max), ( + f"K_angvel above maximum: {K_angvel_current.max()} > {K_angvel_max.max()}" + ) + + +@pytest.mark.parametrize("device_str", ["cpu", "cuda"]) +@pytest.mark.parametrize("num_envs", [1, 2, 8]) +@pytest.mark.parametrize("num_bodies", [1, 4]) +def test_lee_att_randomize_params_within_bounds( + monkeypatch: pytest.MonkeyPatch, device_str: str, num_envs: int, num_bodies: int +): + """Randomized gains stay within configured ranges for attitude controller. + + Tests edge cases with single and multiple environments and bodies. + """ + device = _device_param(device_str) + _patch_aggregate(monkeypatch, att_mod, num_envs, device) + _patch_sim_context(monkeypatch, att_mod) + robot = _DummyRobot(num_envs, num_bodies, device) + + cfg = _create_att_cfg() + controller = att_mod.LeeAttController(cfg, robot, num_envs=num_envs, device=str(device)) + + controller.reset_idx(env_ids=None) + + # Check K_rot gains + K_rot_min = torch.tensor(cfg.K_rot_range[0], device=device, dtype=torch.float32) + K_rot_max = torch.tensor(cfg.K_rot_range[1], device=device, dtype=torch.float32) + K_rot_current = controller.K_rot_current.to(device) + + assert K_rot_current.shape == (num_envs, 3), f"Expected shape ({num_envs}, 3), got {K_rot_current.shape}" + assert torch.all(K_rot_current >= K_rot_min), f"K_rot below minimum: {K_rot_current.min()} < {K_rot_min.min()}" + assert torch.all(K_rot_current <= K_rot_max), f"K_rot above maximum: {K_rot_current.max()} > {K_rot_max.max()}" + + # Check K_angvel gains + K_angvel_min = torch.tensor(cfg.K_angvel_range[0], device=device, dtype=torch.float32) + K_angvel_max = torch.tensor(cfg.K_angvel_range[1], device=device, dtype=torch.float32) + K_angvel_current = controller.K_angvel_current.to(device) + + assert K_angvel_current.shape == (num_envs, 3), f"Expected shape ({num_envs}, 3), got {K_angvel_current.shape}" + assert torch.all(K_angvel_current >= K_angvel_min), ( + f"K_angvel below minimum: {K_angvel_current.min()} < {K_angvel_min.min()}" + ) + assert torch.all(K_angvel_current <= K_angvel_max), ( + f"K_angvel above maximum: {K_angvel_current.max()} > {K_angvel_max.max()}" + ) diff --git a/source/isaaclab_experimental/config/extension.toml b/source/isaaclab_experimental/config/extension.toml index 9bcfc0753383..6e5bee6fb01b 100644 --- a/source/isaaclab_experimental/config/extension.toml +++ b/source/isaaclab_experimental/config/extension.toml @@ -1,7 +1,7 @@ [package] # Note: Semantic Versioning is used: https://semver.org/ -version = "0.0.3" +version = "0.0.4" # Description title = "Experimental playground for upcoming IsaacLab features" diff --git a/source/isaaclab_experimental/docs/CHANGELOG.rst b/source/isaaclab_experimental/docs/CHANGELOG.rst index 5f19d1fb8c5a..2131ff672994 100644 --- a/source/isaaclab_experimental/docs/CHANGELOG.rst +++ b/source/isaaclab_experimental/docs/CHANGELOG.rst @@ -1,6 +1,21 @@ Changelog --------- +0.0.4 (2026-05-12) +~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Pre-create renderer backends in + :class:`~isaaclab_experimental.envs.ManagerBasedEnvWarp` and + :class:`~isaaclab_experimental.envs.DirectRLEnvWarp` by invoking + :meth:`~isaaclab.scene.InteractiveScene.initialize_renderers` after scene + construction so that renderer backend creation order is deterministic and + front-loaded before the first + :meth:`~isaaclab.sim.SimulationContext.reset`. + + 0.0.3 (2026-04-27) ~~~~~~~~~~~~~~~~~~ diff --git a/source/isaaclab_experimental/isaaclab_experimental/envs/direct_rl_env_warp.py b/source/isaaclab_experimental/isaaclab_experimental/envs/direct_rl_env_warp.py index 125f2e61a429..20c82e582690 100644 --- a/source/isaaclab_experimental/isaaclab_experimental/envs/direct_rl_env_warp.py +++ b/source/isaaclab_experimental/isaaclab_experimental/envs/direct_rl_env_warp.py @@ -164,6 +164,7 @@ def __init__(self, cfg: DirectRLEnvCfg, render_mode: str | None = None, **kwargs with use_stage(self.sim.stage): self.scene = InteractiveSceneWarp(self.cfg.scene) self._setup_scene() + self.scene.initialize_renderers() # attach_stage_to_usd_context() print("[INFO]: Scene manager: ", self.scene) diff --git a/source/isaaclab_experimental/isaaclab_experimental/envs/manager_based_env_warp.py b/source/isaaclab_experimental/isaaclab_experimental/envs/manager_based_env_warp.py index 8c558b925947..5ace5168a9c8 100644 --- a/source/isaaclab_experimental/isaaclab_experimental/envs/manager_based_env_warp.py +++ b/source/isaaclab_experimental/isaaclab_experimental/envs/manager_based_env_warp.py @@ -132,6 +132,7 @@ def __init__(self, cfg: ManagerBasedEnvCfg): with use_stage(self.sim.stage): self.scene = InteractiveScene(self.cfg.scene) # attach_stage_to_usd_context() + self.scene.initialize_renderers() print("[INFO]: Scene manager: ", self.scene) # Shared per-env Warp RNG state (accessible to all managers/terms via `env`). diff --git a/source/isaaclab_mimic/config/extension.toml b/source/isaaclab_mimic/config/extension.toml index f53461c3fc6f..1a4f579a323c 100644 --- a/source/isaaclab_mimic/config/extension.toml +++ b/source/isaaclab_mimic/config/extension.toml @@ -1,7 +1,7 @@ [package] # Semantic Versioning is used: https://semver.org/ -version = "1.2.5" +version = "1.2.7" # Description category = "isaaclab" diff --git a/source/isaaclab_mimic/docs/CHANGELOG.rst b/source/isaaclab_mimic/docs/CHANGELOG.rst index 782cf6f7d220..e1e50f99e72b 100644 --- a/source/isaaclab_mimic/docs/CHANGELOG.rst +++ b/source/isaaclab_mimic/docs/CHANGELOG.rst @@ -1,6 +1,36 @@ Changelog --------- +1.2.7 (2026-05-14) +~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Fixed :mod:`isaaclab_mimic.datagen` imports in packaged installs and avoided + importing task configuration modules until data generation config setup. + + +1.2.6 (2026-05-08) +~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added a temporary ``warp.torch`` compatibility shim at + :mod:`isaaclab_mimic` import time so that cuRobo (NVlabs/curobo) keeps + working with ``warp-lang>=1.13``, which dropped the ``warp.torch`` + submodule in favour of top-level ``warp.*`` (e.g. + ``wp.torch.device_from_torch`` → ``wp.device_from_torch``). cuRobo's + pinned commit and ``main`` still call ``wp.torch.*`` and raise + ``AttributeError: module 'warp' has no attribute 'torch'`` at + :meth:`MotionGenConfig.load_from_robot_config` time. The shim + reconstructs ``warp.torch`` as a thin forwarding module and is a + no-op once warp re-introduces the namespace or cuRobo migrates. + Remove this shim once the cuRobo pin in ``docker/Dockerfile.curobo`` + is bumped to a commit that uses the top-level ``wp.*`` API directly. + + 1.2.5 (2026-04-14) ~~~~~~~~~~~~~~~~~~~ diff --git a/source/isaaclab_mimic/isaaclab_mimic/__init__.py b/source/isaaclab_mimic/isaaclab_mimic/__init__.py index 17f1264a6b59..56b86cc6034d 100644 --- a/source/isaaclab_mimic/isaaclab_mimic/__init__.py +++ b/source/isaaclab_mimic/isaaclab_mimic/__init__.py @@ -5,4 +5,53 @@ """Package containing implementation of Isaac Lab Mimic data generation.""" +# --------------------------------------------------------------------------- +# Compatibility shim: re-expose ``warp.torch`` after warp-lang 1.13 dropped it +# +# Newton ``v1.2.0rc2`` requires ``warp-lang>=1.13``. Warp 1.13 collapsed the +# ``warp.torch`` submodule into the top-level ``warp`` namespace, so e.g. +# ``wp.torch.device_from_torch`` is now ``wp.device_from_torch``. cuRobo +# (NVlabs/curobo) still uses the old ``wp.torch.*`` form (verified at +# ``ebb71702f`` and on ``main`` as of 2026-05-07) and raises +# ``AttributeError: module 'warp' has no attribute 'torch'`` at +# ``MotionGenConfig.load_from_robot_config(...)`` time. +# +# This shim runs at ``isaaclab_mimic`` import — which Python evaluates before +# any submodule, including +# :mod:`isaaclab_mimic.motion_planners.curobo.curobo_planner` — so curobo +# sees a ``warp.torch`` namespace whose members forward to the relocated +# top-level ``warp.*`` callables. Idempotent: a no-op once warp ships +# ``wp.torch`` again or curobo migrates. +# +# TODO: remove this shim once the cuRobo pin in ``docker/Dockerfile.curobo`` +# bumps to a commit that uses ``wp.from_torch``/``wp.device_from_torch``/ +# etc. directly. Tracking upstream at https://github.com/NVlabs/curobo — +# follow up on the open issue / PR there to confirm the migration landed +# before deleting this block. +import sys as _sys +import types as _types + +import warp as _wp + +if not hasattr(_wp, "torch"): + _wp_torch_shim = _types.ModuleType("warp.torch") + for _name in ( + "from_torch", + "to_torch", + "device_from_torch", + "device_to_torch", + "dtype_from_torch", + "dtype_to_torch", + "stream_from_torch", + "stream_to_torch", + ): + if hasattr(_wp, _name): + setattr(_wp_torch_shim, _name, getattr(_wp, _name)) + _wp.torch = _wp_torch_shim + _sys.modules["warp.torch"] = _wp_torch_shim + del _wp_torch_shim, _name + +del _sys, _types, _wp + + __version__ = "1.0.0" diff --git a/source/isaaclab_mimic/isaaclab_mimic/datagen/generation.py b/source/isaaclab_mimic/isaaclab_mimic/datagen/generation.py index 1761cf9beaa7..72dc9c8d4c13 100644 --- a/source/isaaclab_mimic/isaaclab_mimic/datagen/generation.py +++ b/source/isaaclab_mimic/isaaclab_mimic/datagen/generation.py @@ -19,8 +19,6 @@ from isaaclab_mimic.datagen.data_generator import DataGenerator from isaaclab_mimic.datagen.datagen_info_pool import DataGenInfoPool -from isaaclab_tasks.utils.parse_cfg import parse_env_cfg - # global variable to keep track of the data generation statistics num_success = 0 num_failures = 0 @@ -180,6 +178,8 @@ def setup_env_config( Raises: NotImplementedError: If no success termination term found """ + from isaaclab_tasks.utils.parse_cfg import parse_env_cfg + env_cfg = parse_env_cfg(env_name, device=device, num_envs=num_envs) if generation_num_trials is not None: diff --git a/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/scene_utils.py b/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/scene_utils.py index a0eb00fb4a58..4ba068fc8f56 100644 --- a/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/scene_utils.py +++ b/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/scene_utils.py @@ -106,7 +106,9 @@ def _get_xform_view(self) -> FrameView: xform_prim = self.scene[self.entity_name] if xform_prim.count == 0: # The view was created before environment cloning; rebuild it now that prims exist. - xform_prim = FrameView(xform_prim._prim_path, device=xform_prim.device) + # FabricFrameView composes UsdFrameView; the template prim_path lives on the inner USD view. + inner = getattr(xform_prim, "_usd_view", xform_prim) + xform_prim = FrameView(inner._prim_path, device=xform_prim.device) self.scene.extras[self.entity_name] = xform_prim return xform_prim diff --git a/source/isaaclab_mimic/setup.py b/source/isaaclab_mimic/setup.py index e3d9e2dc6ac3..279a7a0a248d 100644 --- a/source/isaaclab_mimic/setup.py +++ b/source/isaaclab_mimic/setup.py @@ -10,7 +10,7 @@ import platform import toml -from setuptools import setup +from setuptools import find_namespace_packages, setup # Obtain the extension data from the extension.toml file EXTENSION_PATH = os.path.dirname(os.path.realpath(__file__)) @@ -45,7 +45,7 @@ # Installation operation setup( name="isaaclab_mimic", - packages=["isaaclab_mimic"], + packages=find_namespace_packages(include=["isaaclab_mimic", "isaaclab_mimic.*"]), author=EXTENSION_TOML_DATA["package"]["author"], maintainer=EXTENSION_TOML_DATA["package"]["maintainer"], url=EXTENSION_TOML_DATA["package"]["repository"], diff --git a/source/isaaclab_newton/changelog.d/clone-plan-visualizer-cleanup.minor.rst b/source/isaaclab_newton/changelog.d/clone-plan-visualizer-cleanup.minor.rst deleted file mode 100644 index 6fed4677a471..000000000000 --- a/source/isaaclab_newton/changelog.d/clone-plan-visualizer-cleanup.minor.rst +++ /dev/null @@ -1,9 +0,0 @@ -Removed -^^^^^^^ - -* **Breaking:** Removed - ``isaaclab_newton.cloner.newton_replicate.create_newton_visualizer_prebuild_clone_fn``. - Callers that need a Newton model for visualization should call - :func:`~isaaclab_newton.cloner.newton_replicate.newton_visualizer_prebuild` - directly with the ``(sources, destinations, env_ids, mask, positions)`` bundle - derived from :meth:`~isaaclab.sim.SimulationContext.get_clone_plans`. diff --git a/source/isaaclab_newton/changelog.d/mtrepte-expand_viz_markers.skip b/source/isaaclab_newton/changelog.d/mtrepte-expand_viz_markers.skip deleted file mode 100644 index a23b7c7322b3..000000000000 --- a/source/isaaclab_newton/changelog.d/mtrepte-expand_viz_markers.skip +++ /dev/null @@ -1 +0,0 @@ -Marker visualization changes are covered by the isaaclab fragment. diff --git a/source/isaaclab_newton/changelog.d/mym-newton-manager-abstraction.rst b/source/isaaclab_newton/changelog.d/mym-newton-manager-abstraction.rst deleted file mode 100644 index 1ad8a9265830..000000000000 --- a/source/isaaclab_newton/changelog.d/mym-newton-manager-abstraction.rst +++ /dev/null @@ -1,14 +0,0 @@ -Changed -^^^^^^^ - -* Changed :class:`~isaaclab_newton.physics.NewtonManager` to dispatch through - solver-specific manager subclasses while preserving the existing - ``NewtonCfg(solver_cfg=...)`` configuration pattern. - -Deprecated -^^^^^^^^^^ - -* Deprecated :attr:`~isaaclab_newton.physics.NewtonSolverCfg.solver_type` for - manager dispatch in favor of - :attr:`~isaaclab_newton.physics.NewtonSolverCfg.class_type`. Existing configs - remain valid, but new code should rely on ``class_type``. diff --git a/source/isaaclab_newton/changelog.d/pr-5458-merge-develop.rst b/source/isaaclab_newton/changelog.d/pr-5458-merge-develop.rst deleted file mode 100644 index 30eb959531c2..000000000000 --- a/source/isaaclab_newton/changelog.d/pr-5458-merge-develop.rst +++ /dev/null @@ -1,13 +0,0 @@ -Removed -^^^^^^^ - -* Removed the unimplemented ``ArticulationData.body_incoming_joint_wrench_b`` - accessor. Add :class:`~isaaclab.sensors.JointWrenchSensorCfg` to the scene - and read :attr:`~isaaclab.sensors.JointWrenchSensorData.force` and - :attr:`~isaaclab.sensors.JointWrenchSensorData.torque` instead. - -Fixed -^^^^^ - -* Fixed :class:`~isaaclab_newton.sensors.JointWrenchSensor` initialization for - USD assets whose articulation root is nested below the configured asset prim. diff --git a/source/isaaclab_newton/changelog.d/rschmitt_decouple_rednerer_camera.rst b/source/isaaclab_newton/changelog.d/rschmitt_decouple_rednerer_camera.rst deleted file mode 100644 index 1c3efcb5c33e..000000000000 --- a/source/isaaclab_newton/changelog.d/rschmitt_decouple_rednerer_camera.rst +++ /dev/null @@ -1,4 +0,0 @@ -Changed -^^^^^^^^ - -* Modified the newton renderer to use the new patterns from renderer/camera decoupling. diff --git a/source/isaaclab_newton/config/extension.toml b/source/isaaclab_newton/config/extension.toml index 0a8eed8000c2..ba4905bfd73e 100644 --- a/source/isaaclab_newton/config/extension.toml +++ b/source/isaaclab_newton/config/extension.toml @@ -1,7 +1,7 @@ [package] # Note: Semantic Versioning is used: https://semver.org/ -version = "0.5.26" +version = "0.9.1" # Description title = "Newton simulation interfaces for IsaacLab core package" diff --git a/source/isaaclab_newton/docs/CHANGELOG.rst b/source/isaaclab_newton/docs/CHANGELOG.rst index d9626a890476..01bcae178b7b 100644 --- a/source/isaaclab_newton/docs/CHANGELOG.rst +++ b/source/isaaclab_newton/docs/CHANGELOG.rst @@ -1,6 +1,274 @@ Changelog --------- +0.9.1 (2026-05-15) +~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Fixed the acceleration-arrow debug visualizer in + :class:`~isaaclab_newton.sensors.pva.Pva` drawing arrows in undefined directions for + bodies with effectively zero acceleration. Such bodies are now skipped from the + visualization. + + +0.9.0 (2026-05-14) +~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added Newton implementations of + :attr:`~isaaclab.assets.BaseArticulationData.body_link_jacobian_w`, + :attr:`~isaaclab.assets.BaseArticulationData.body_com_jacobian_w`, and + :attr:`~isaaclab.assets.BaseArticulationData.mass_matrix` on + :class:`~isaaclab_newton.assets.ArticulationData`. The properties wrap + ``ArticulationView.eval_jacobian`` and ``ArticulationView.eval_mass_matrix`` + with view-sized output buffers cached via the standard timestamped-buffer + pattern. Per-step behavior is allocation-free and safe under CUDA-graph + capture: source / scratch / output buffers are pre-allocated in + ``_create_buffers``, and + :func:`~isaaclab_newton.assets.articulation.kernels.gather_jacobian_rows` + and :func:`~isaaclab_newton.assets.articulation.kernels.gather_mass_matrix_rows` + Warp kernels gather just this view's rows from the model-sized buffers + Newton populates. The DoF axis preserves the leading 6 floating-base + columns Newton fills for floating-base articulations (matching the + cross-library industry convention and PhysX's layout). +* Added the + :func:`~isaaclab_newton.assets.articulation.kernels.shift_jacobian_com_to_origin` + Warp kernel applying the + ``v_origin = v_com - omega x (R · body_com_pos_b)`` shift to the + linear-velocity rows of the gathered, view-sized Jacobian, so the link- + origin form matches the cross-backend + :attr:`~isaaclab.assets.BaseArticulationData.body_link_jacobian_w` + contract. +* Added :meth:`~isaaclab_newton.physics.NewtonManager.get_state` and + :meth:`~isaaclab_newton.physics.NewtonManager.update_visualization_state` so + Newton-based renderers, visualizers, and video recorders can fetch a Newton + ``Model``/``State`` regardless of the active sim backend. When the sim + backend is PhysX the manager builds a shadow Newton model directly from the + USD stage (via + :meth:`~isaaclab_newton.physics.NewtonManager.instantiate_builder_from_stage`) + and refreshes ``state_0.body_q`` from rigid-body transforms supplied by the + :class:`~isaaclab.scene.scene_data_provider.SceneDataProvider` each render + frame. + +Changed +^^^^^^^ + +* :attr:`~isaaclab_newton.assets.ArticulationData.gravity_compensation_forces` + raises :class:`NotImplementedError` with a message pointing at the + upstream gap. Newton's ``ArticulationView`` does not expose an + inverse-dynamics primitive yet (upstream Newton issues + `#2497 `_, + `#2529 `_, + `#2625 `_). + OSC users on Newton must set ``gravity_compensation=False`` until + upstream lands the primitive. +* **Breaking:** :class:`~isaaclab_newton.renderers.NewtonWarpRenderer`, + :class:`~isaaclab_newton.video_recording.NewtonGlPerspectiveVideo`, and the + Newton/Rerun/Viser visualizers now read Newton ``Model``/``State`` from + :class:`~isaaclab_newton.physics.NewtonManager` instead of the removed + ``BaseSceneDataProvider.get_newton_model()`` / ``get_newton_state()``. +* Switched the Newton install to ``newton[sim]`` so that ``mujoco`` and + ``mujoco-warp`` are pulled in transitively via Newton's ``[sim]`` extra. + The explicit ``mujoco==3.8.0`` and ``mujoco-warp==3.8.0.1`` pins were + removed from :mod:`isaaclab_newton` — Newton is now the single source of + truth for those versions. + +Removed +^^^^^^^ + +* **Breaking:** Removed the ``isaaclab_newton.scene_data_providers`` package + (``NewtonSceneDataProvider``). Replace direct uses with + :meth:`~isaaclab_newton.physics.NewtonManager.get_model` / + :meth:`~isaaclab_newton.physics.NewtonManager.get_state` and the + Warp-native :class:`~isaaclab.scene.scene_data_provider.SceneDataProvider`. + +Fixed +^^^^^ + +* Fixed :class:`~isaaclab_newton.sensors.ContactSensor` metadata extraction + after the migration to Newton 1.1, where ``sensing_obj_type`` and + ``counterpart_type`` became scalar strings and ``counterpart_indices`` + became per-row. +* Fixed per-environment string identifiers (e.g. ``mujoco:tendon_label``) + keeping the source proto path after replication. + :func:`~isaaclab_newton.cloner.newton_replicate._rename_builder_labels` + now also walks string-typed custom-attribute columns whose frequency + declares a ``references="world"`` companion, rewriting their per-row + source-path prefix to the destination world root in the same pass that + handles built-in label arrays. Adds ``constraint_mimic`` and + ``equality_constraint`` to that built-in pass for completeness. The + prefix match uses a path-separator boundary so a source path that is a + string prefix of another (e.g. ``/Sources/protoA`` vs + ``/Sources/protoAB``) does not cross-contaminate during the rename. + + +0.8.1 (2026-05-13) +~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Changed Newton integration to use the packaged Newton 1.2.0 release candidate + and updated transform conversion calls for Warp 1.13 compatibility. + +Fixed +^^^^^ + +* Fixed a spurious ``[Error][carb] Client passed into the framework is nullptr.`` + log emitted from :meth:`~isaaclab_newton.physics._cubric.CubricBindings.initialize` + when the first ``tryAcquireInterfaceWithClient`` attempt returned null. The + helper used to retry with ``clientName=None``, which Carbonite has rejected as + invalid since 2018 — the retry only emitted a misleading error log. Removed + the null-client retry; the existing ``acquireInterfaceWithClient`` fallback + with the ``isaaclab.cubric`` client name still handles configurations where + the plugin needs to be loaded on demand. + + +0.8.0 (2026-05-12) +~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added :class:`~isaaclab_newton.sim.schemas.NewtonRigidBodyPropertiesCfg` and + :class:`~isaaclab_newton.sim.schemas.NewtonJointDrivePropertiesCfg` as Newton-targeted + bases for solver-specific subclasses. Currently empty (no Newton-native ``newton:*`` + rigid-body or joint-drive attributes today); reserved as the family root for any + future Newton-native fields. +* Added :class:`~isaaclab_newton.sim.schemas.MujocoRigidBodyPropertiesCfg` (subclasses + :class:`NewtonRigidBodyPropertiesCfg`) with :attr:`gravcomp` for body-level gravity + compensation (``mjc:gravcomp``). +* Added :class:`~isaaclab_newton.sim.schemas.MujocoJointDrivePropertiesCfg` (subclasses + :class:`NewtonJointDrivePropertiesCfg`) with :attr:`actuatorgravcomp` for joint-level + gravity compensation routing (``mjc:actuatorgravcomp`` via ``MjcJointAPI``). +* Added :class:`~isaaclab_newton.sim.schemas.NewtonCollisionPropertiesCfg` with + :attr:`contact_margin` and :attr:`contact_gap` (``newton:*`` via ``NewtonCollisionAPI``). +* Added :class:`~isaaclab_newton.sim.schemas.NewtonMeshCollisionPropertiesCfg` with + :attr:`max_hull_vertices` (``newton:maxHullVertices`` via ``NewtonMeshCollisionAPI``). +* Added :class:`~isaaclab_newton.sim.schemas.NewtonMaterialPropertiesCfg` with + :attr:`torsional_friction` and :attr:`rolling_friction` (``newton:*`` via ``NewtonMaterialAPI``). +* Added :class:`~isaaclab_newton.sim.schemas.NewtonArticulationRootPropertiesCfg` with + :attr:`self_collision_enabled` (``newton:selfCollisionEnabled`` via ``NewtonArticulationRootAPI``). + +Changed +^^^^^^^ + +* Split :class:`~isaaclab_newton.renderers.NewtonWarpRenderer` construction + into a pre-physics ``__init__`` (stores cfg and registers the Newton-Warp + scene-data requirement on + :class:`~isaaclab.sim.SimulationContext`) and a post-physics + :meth:`~isaaclab_newton.renderers.NewtonWarpRenderer.initialize` (reads + the built Newton model. + + +0.7.2 (2026-05-11) +~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Changed rigid object collection spawning to honor planned ``spawn_path`` + values while falling back to ``prim_path`` for direct construction. + + +0.7.1 (2026-05-09) +~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Fixed :class:`~isaaclab_newton.assets.Articulation` joint friction docs to identify Newton friction as a force or + torque value instead of a unitless coefficient. +* Fixed :class:`~isaaclab_newton.sensors.contact_sensor.ContactSensor` to use + current Newton contact sensor API names, removing deprecation warnings from + Newton contact sensor test runs. +* Fixed stale Newton forward-kinematics state after explicit pose writes so + downstream collision queries and :attr:`~isaaclab_newton.assets.RigidObjectData.body_link_pose_w` + reads use updated transforms. + + +0.7.0 (2026-05-08) +~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Bumped Newton pin to ``v1.2.0rc2``. Pulls in IsaacLab-relevant fixes from + `newton-physics/newton#2678 `_ + and `newton-physics/newton#2720 + `_ (``SolverKamino`` + reset under ``world_mask``), the upstream tendon-scoping fix from + `newton-physics/newton#2659 + `_ ("Scope USD + custom-frequency parsing"), and a VRAM-leak fix on example reset + (`newton-physics/newton#2710 + `_). +* Newton ``v1.2.0rc2`` requires ``warp-lang==1.13.0``, ``mujoco==3.8.0``, + and ``mujoco-warp==3.8.0.1``. ``warp-lang``/``mujoco``/``mujoco-warp`` + pins live in :mod:`isaaclab` and ``tools/wheel_builder/res/python_packages.toml``; + the Newton pin is mirrored across :mod:`isaaclab_newton`, + :mod:`isaaclab_visualizers` (3×), :mod:`isaaclab_physx` (``[newton]`` + extra), and the wheel-builder TOML. +* Updated ``wp.math.transform_to_matrix`` to ``wp.transform_to_matrix`` in + :mod:`~isaaclab_newton.physics.newton_manager` and + :mod:`~isaaclab_ov.renderers.ovrtx_renderer_kernels` to match the + ``warp-lang`` 1.13 API (the ``wp.math`` namespace was removed). +* Adapted :class:`~isaaclab_newton.renderers.NewtonWarpRenderer` to + Newton ``v1.2.0rc2``'s explicit shape-BVH lifecycle. + :meth:`~newton.sensors.SensorTiledCamera.update` no longer auto-builds + the BVH when a non-``None`` state is passed and the underlying + ``RenderContext.render`` now raises ``RuntimeError("build_bvh_shape() + must be called before rendering shapes.")`` if it was never built. The + renderer now calls ``newton.geometry.build_bvh_shape`` once after + sensor construction and ``newton.geometry.refit_bvh_shape`` each frame + before :meth:`~newton.sensors.SensorTiledCamera.update`, since env + body poses move every step. + + +0.6.0 (2026-05-08) +~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Modified the newton renderer to use the new patterns from renderer/camera decoupling. +* Changed :class:`~isaaclab_newton.physics.NewtonManager` to dispatch through + solver-specific manager subclasses while preserving the existing + ``NewtonCfg(solver_cfg=...)`` configuration pattern. + +Deprecated +^^^^^^^^^^ + +* Deprecated :attr:`~isaaclab_newton.physics.NewtonSolverCfg.solver_type` for + manager dispatch in favor of + :attr:`~isaaclab_newton.physics.NewtonSolverCfg.class_type`. Existing configs + remain valid, but new code should rely on ``class_type``. + +Removed +^^^^^^^ + +* **Breaking:** Removed + ``isaaclab_newton.cloner.newton_replicate.create_newton_visualizer_prebuild_clone_fn``. + Callers that need a Newton model for visualization should call + :func:`~isaaclab_newton.cloner.newton_replicate.newton_visualizer_prebuild` + directly with the ``(sources, destinations, env_ids, mask, positions)`` bundle + derived from :meth:`~isaaclab.sim.SimulationContext.get_clone_plans`. +* Removed the unimplemented ``ArticulationData.body_incoming_joint_wrench_b`` + accessor. Add :class:`~isaaclab.sensors.JointWrenchSensorCfg` to the scene + and read :attr:`~isaaclab.sensors.JointWrenchSensorData.force` and + :attr:`~isaaclab.sensors.JointWrenchSensorData.torque` instead. + +Fixed +^^^^^ + +* Fixed :class:`~isaaclab_newton.sensors.JointWrenchSensor` initialization for + USD assets whose articulation root is nested below the configured asset prim. + + 0.5.26 (2026-04-30) ~~~~~~~~~~~~~~~~~~~ diff --git a/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation.py b/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation.py index c3c6eca044f7..ff04b96c63ca 100644 --- a/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation.py +++ b/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation.py @@ -1964,7 +1964,19 @@ def write_joint_friction_coefficient_to_sim_index( joint_ids: Sequence[int] | torch.Tensor | wp.array | None = None, env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, ): - r"""Write joint friction coefficients over selected environment indices into the simulation. + r"""Write Newton joint friction force/torque values over selected environment indices into the simulation. + + This writes to Newton's ``Model.joint_friction`` field. Despite the ``coeff`` suffix in the Isaac Lab API + name, Newton treats this value as an absolute friction force/torque [N or N·m, depending on joint type], not + as a unitless coefficient. + + For example, the MJWarp solver copies this value into MuJoCo Warp's ``dof_frictionloss``. Setting + ``joint_friction_coeff`` to 0.2 configures a dry-friction loss limit of 0.2 N·m on a revolute joint DOF, + or 0.2 N on a prismatic joint DOF. + + .. note:: + Solver support is defined by the active Newton solver. Unsupported solvers may ignore + ``Model.joint_friction``. .. note:: This method expects partial data. @@ -1974,7 +1986,7 @@ def write_joint_friction_coefficient_to_sim_index( However, to allow graphed pipelines, the mask method must be used. Args: - joint_friction_coeff: Static friction coefficient :math:`\mu_s`. + joint_friction_coeff: Joint friction force/torque [N or N·m, depending on joint type]. Shape is (len(env_ids), len(joint_ids)). joint_ids: Joint indices. If None, then all joints are used. env_ids: Environment indices. If None, then all indices are used. @@ -2024,7 +2036,19 @@ def write_joint_friction_coefficient_to_sim_mask( joint_mask: wp.array | None = None, env_mask: wp.array | None = None, ): - r"""Write joint friction coefficients over selected environment mask into the simulation. + r"""Write Newton joint friction force/torque values over selected environment mask into the simulation. + + This writes to Newton's ``Model.joint_friction`` field. Despite the ``coeff`` suffix in the Isaac Lab API + name, Newton treats this value as an absolute friction force/torque [N or N·m, depending on joint type], not + as a unitless coefficient. + + For example, the MJWarp solver copies this value into MuJoCo Warp's ``dof_frictionloss``. Setting + ``joint_friction_coeff`` to 0.2 configures a dry-friction loss limit of 0.2 N·m on a revolute joint DOF, + or 0.2 N on a prismatic joint DOF. + + .. note:: + Solver support is defined by the active Newton solver. Unsupported solvers may ignore + ``Model.joint_friction``. .. note:: This method expects full data. @@ -2034,7 +2058,7 @@ def write_joint_friction_coefficient_to_sim_mask( However, to allow graphed pipelines, the mask method must be used. Args: - joint_friction_coeff: Static friction coefficient :math:`\mu_s`. + joint_friction_coeff: Joint friction force/torque [N or N·m, depending on joint type]. Shape is (num_instances, num_joints). joint_mask: Joint mask. If None, then all joints are used. Shape is (num_joints,). env_mask: Environment mask. If None, then all the instances are updated. Shape is (num_instances,). diff --git a/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation_data.py b/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation_data.py index a22ba73e1725..0a2a84392619 100644 --- a/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation_data.py +++ b/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation_data.py @@ -127,6 +127,19 @@ def update(self, dt: float) -> None: self.joint_acc self.body_com_acc_w + def _ensure_fk_fresh(self) -> None: + """Run forward kinematics if joint state has changed since the last FK update. + + Newton's ``state.body_q`` (per-body world transforms) is updated by ``eval_fk``, + invoked here through ``SimulationManager.forward()``. After a manual joint or root + write that bypassed the sim step (``write_*_to_sim_*``), ``_fk_timestamp`` is set + to ``-1.0`` to force a refresh on the next read of any property that depends on + body poses (``body_link_pose_w``, the Jacobian properties, ``mass_matrix``). + """ + if self._fk_timestamp < self._sim_timestamp: + SimulationManager.forward() + self._fk_timestamp = self._sim_timestamp + """ Names. """ @@ -342,7 +355,14 @@ def joint_armature(self) -> ProxyArray: @property def joint_friction_coeff(self) -> ProxyArray: - """Joint static friction coefficient provided to the simulation. + """Newton joint friction force/torque provided to the simulation. + + Despite the ``coeff`` suffix in the Isaac Lab API name, Newton stores this as an absolute joint friction + force/torque [N or N·m, depending on joint type]. + + For example, the MJWarp solver copies this value into MuJoCo Warp's ``dof_frictionloss``. Setting + ``joint_friction_coeff`` to 0.2 configures a dry-friction loss limit of 0.2 N·m on a revolute joint DOF, + or 0.2 N on a prismatic joint DOF. Shape is (num_instances, num_joints), dtype = wp.float32. In torch this resolves to (num_instances, num_joints). """ @@ -660,9 +680,7 @@ def body_link_pose_w(self) -> ProxyArray: This quantity is the pose of the articulation links' actor frame relative to the world. The orientation is provided in (x, y, z, w) format. """ - if self._fk_timestamp < self._sim_timestamp: - SimulationManager.forward() - self._fk_timestamp = self._sim_timestamp + self._ensure_fk_fresh() return self._body_link_pose_w_ta @property @@ -805,6 +823,130 @@ def body_com_pose_b(self) -> ProxyArray: self._body_com_pose_b.timestamp = self._sim_timestamp return self._body_com_pose_b_ta + """ + Dynamics quantities (task-space controllers). + """ + + @property + def body_com_jacobian_w(self) -> ProxyArray: + """See :attr:`isaaclab.assets.BaseArticulationData.body_com_jacobian_w`. + + Newton implementation: ``eval_jacobian`` (writes the model-wide buffer) then a + gather kernel extracts this view's rows. ``link_offset`` drops Newton's fixed- + root row for fixed-base; the DoF axis is preserved in full. + """ + # Newton's eval_jacobian reads ``state.body_q`` (link poses); refresh FK if stale. + # Matches the convention in ``body_link_pose_w`` — Python-guarded lazy refresh. + self._ensure_fk_fresh() + # eval_jacobian writes every articulation in the model; gather kernel extracts this + # view's rows. ``link_offset`` skips Newton's fixed-root row for fixed-base; the DoF + # axis is preserved in full (free-root joint's 6 columns up front for floating-base), + # matching the PhysX layout and the cross-library industry convention. + self._root_view.eval_jacobian( + SimulationManager.get_state_0(), + J=self._jacobian_buf_flat, + joint_S_s=self._joint_S_s_buf, + ) + wp.launch( + articulation_kernels.gather_jacobian_rows, + dim=self._body_com_jacobian_w_buf.shape, + inputs=[ + self._jacobian_buf, + self._jacobian_view_art_ids, + self._jacobian_link_offset, + ], + outputs=[self._body_com_jacobian_w_buf], + device=self.device, + ) + return self._body_com_jacobian_w_ta + + @property + def body_link_jacobian_w(self) -> ProxyArray: + """See :attr:`isaaclab.assets.BaseArticulationData.body_link_jacobian_w`. + + Newton implementation: applies the COM→origin shift kernel to + :attr:`body_com_jacobian_w` (Newton's ``eval_jacobian`` is COM-referenced). + """ + # ``body_link_pose_w`` accessor triggers ``SimulationManager.forward()`` if FK is + # stale (after a manual joint / root write that bypassed the sim step). Reading the + # property here — not ``_sim_bind_body_link_pose_w`` directly — keeps the shift + # kernel from using stale link rotations during reset / IK-warm-start paths. + link_pose_w = self.body_link_pose_w.warp + com_jac = self.body_com_jacobian_w + wp.launch( + articulation_kernels.shift_jacobian_com_to_origin, + dim=self._body_link_jacobian_w_buf.shape[:2] + (self._body_link_jacobian_w_buf.shape[3],), + inputs=[ + link_pose_w, + self._sim_bind_body_com_pos_b, + self._jacobian_link_offset, + com_jac.warp, + ], + outputs=[self._body_link_jacobian_w_buf], + device=self.device, + ) + return self._body_link_jacobian_w_ta + + @property + def mass_matrix(self) -> ProxyArray: + """See :attr:`isaaclab.assets.BaseArticulationData.mass_matrix`. + + Newton implementation: ``eval_mass_matrix`` (writes the model-wide buffer) then a + gather kernel extracts this view's rows. + """ + # eval_jacobian / eval_mass_matrix read ``state.body_q``; refresh FK if stale. + # Matches the convention in ``body_link_pose_w`` — Python-guarded lazy refresh. + self._ensure_fk_fresh() + # eval_mass_matrix treats ``J`` as an input (skips its own jacobian compute when + # provided), so we must populate the scratch first via eval_jacobian. Reusing + # ``_jacobian_buf_flat`` (same shape) avoids a second allocation. All scratch buffers + # are pre-allocated for CUDA-graph capture safety. + state = SimulationManager.get_state_0() + self._root_view.eval_jacobian( + state, + J=self._jacobian_buf_flat, + joint_S_s=self._joint_S_s_buf, + ) + self._root_view.eval_mass_matrix( + state, + H=self._mass_matrix_full_buf, + J=self._jacobian_buf_flat, + body_I_s=self._mass_matrix_body_I_s_buf, + joint_S_s=self._joint_S_s_buf, + ) + wp.launch( + articulation_kernels.gather_mass_matrix_rows, + dim=self._mass_matrix_buf.shape, + inputs=[ + self._mass_matrix_full_buf, + self._jacobian_view_art_ids, + ], + outputs=[self._mass_matrix_buf], + device=self.device, + ) + return self._mass_matrix_ta + + @property + def gravity_compensation_forces(self) -> ProxyArray: + """See :attr:`isaaclab.assets.BaseArticulationData.gravity_compensation_forces`. + + Newton implementation: raises :class:`NotImplementedError` — Newton's + ``ArticulationView`` exposes only ``eval_fk`` / ``eval_jacobian`` / + ``eval_mass_matrix``. Use PhysX, or set the controller's + ``gravity_compensation=False`` until upstream Newton adds the primitive. + Tracking upstream: `newton#2497 `_, + `newton#2529 `_, + `newton#2625 `_. + """ + raise NotImplementedError( + "Newton has no gravity-compensation primitive. Use PhysX, or set the controller's" + " ``gravity_compensation=False`` until upstream Newton adds an" + " ``eval_gravity_compensation`` API. Tracking upstream:" + " https://github.com/newton-physics/newton/issues/2497," + " https://github.com/newton-physics/newton/issues/2529," + " https://github.com/newton-physics/newton/issues/2625." + ) + """ Joint state properties. """ @@ -1294,18 +1436,21 @@ def _create_simulation_bindings(self) -> None: # -- root properties self._sim_bind_root_link_pose_w = self._root_view.get_root_transforms(SimulationManager.get_state_0())[:, 0] - self._sim_bind_root_com_vel_w = self._root_view.get_root_velocities(SimulationManager.get_state_0()) - if self._sim_bind_root_com_vel_w is not None: + # ``get_root_velocities`` returns ``None`` for fixed-base articulations; the + # ``wp.zeros`` fallback set by :meth:`_create_buffers` must survive subsequent + # resets, so only overwrite when the solver actually exposes the binding. + root_vel_w = self._root_view.get_root_velocities(SimulationManager.get_state_0()) + if root_vel_w is not None: if self._root_view.is_fixed_base: - self._sim_bind_root_com_vel_w = self._sim_bind_root_com_vel_w[:, 0, 0] + self._sim_bind_root_com_vel_w = root_vel_w[:, 0, 0] else: - self._sim_bind_root_com_vel_w = self._sim_bind_root_com_vel_w[:, 0] + self._sim_bind_root_com_vel_w = root_vel_w[:, 0] # -- body properties self._sim_bind_body_com_pos_b = self._root_view.get_attribute("body_com", SimulationManager.get_model())[:, 0] self._sim_bind_body_link_pose_w = self._root_view.get_link_transforms(SimulationManager.get_state_0())[:, 0] - self._sim_bind_body_com_vel_w = self._root_view.get_link_velocities(SimulationManager.get_state_0()) - if self._sim_bind_body_com_vel_w is not None: - self._sim_bind_body_com_vel_w = self._sim_bind_body_com_vel_w[:, 0] + body_com_vel_w = self._root_view.get_link_velocities(SimulationManager.get_state_0()) + if body_com_vel_w is not None: + self._sim_bind_body_com_vel_w = body_com_vel_w[:, 0] self._sim_bind_body_mass = self._root_view.get_attribute("body_mass", SimulationManager.get_model())[:, 0] # Newton stores body_inertia as (N, 1, B) mat33f — the [:, 0] removes the padding dim # giving (N, B) mat33f. Reinterpret as (N, B, 9) float32 via pointer aliasing. @@ -1509,6 +1654,8 @@ def _create_buffers(self) -> None: self._joint_acc = TimestampedBuffer( shape=(self._num_instances, self._num_joints), dtype=wp.float32, device=self.device ) + # -- dynamics quantities for task-space controllers + self._create_jacobian_buffers(SimulationManager.get_model()) # Empty memory pre-allocations self._root_link_lin_vel_b = None self._root_link_ang_vel_b = None @@ -1545,6 +1692,74 @@ def _create_buffers(self) -> None: # Pin all ProxyArray wrappers to current buffers. self._pin_proxy_arrays() + def _create_jacobian_buffers(self, model) -> None: + """Allocate the scratch + view-sized buffers used by task-space accessors. + + Newton's :meth:`eval_jacobian` / :meth:`eval_mass_matrix` write into model-sized + scratch buffers spanning every articulation in the model; the gather kernels in + :attr:`body_com_jacobian_w` / :attr:`mass_matrix` extract this view's rows. The + output buffers are sized using THIS articulation's body / DoF counts (not the + model-wide ``max_*``) so heterogeneous scenes do not leak zero-padded rows / cols + into the returned tensor. The DoF axis includes ``num_base_dofs`` floating-base + columns up front (0 for fixed-base, 6 for floating-base), matching the cross- + library industry convention (PhysX, Pinocchio, Drake, MuJoCo, RBDL, OCS2, iDynTree). + + Args: + model: Newton ``Model`` from :meth:`SimulationManager.get_model`. Read for + ``articulation_count``, ``max_joints_per_articulation``, + ``max_dofs_per_articulation``, ``joint_dof_count``, ``body_count``. + """ + max_links = model.max_joints_per_articulation + max_dofs = model.max_dofs_per_articulation + + # -- shared scratch (eval_jacobian outputs; consumed by ``body_com_jacobian_w`` + # and reused as ``eval_mass_matrix``'s ``J`` input to skip a re-compute) + self._jacobian_buf_flat = wp.zeros( + (model.articulation_count, max_links * 6, max_dofs), dtype=wp.float32, device=self.device + ) + # Motion subspace (Featherstone ``S``, spatial frame); produced by eval_jacobian, + # also consumed by eval_mass_matrix. + self._joint_S_s_buf = wp.zeros(model.joint_dof_count, dtype=wp.spatial_vector, device=self.device) + + # -- per-view gather config (shared by every gather/shift kernel below) + # Link-row offset: fixed-base skips Newton's row-0 fixed-root row; floating-base keeps it. + self._jacobian_link_offset = 1 if self._root_view.is_fixed_base else 0 + num_jacobi_bodies = self._num_bodies - self._jacobian_link_offset + # Free-root DoF columns Newton fills for floating-base (0 fixed-base, 6 floating-base); + # included in the DoF axis to match the cross-library industry convention. + num_base_dofs = 0 if self._root_view.is_fixed_base else 6 + # Flattened (num_worlds*num_per_view,) view-to-model index map for the gather kernels. + self._jacobian_view_art_ids = self._root_view.articulation_ids.reshape((-1,)) + + # -- ``body_com_jacobian_w``: 4-D reshape view of the shared scratch (kernel input + # to the gather) and the per-view output buffer (gather output) + self._jacobian_buf = self._jacobian_buf_flat.reshape((model.articulation_count, max_links, 6, max_dofs)) + self._body_com_jacobian_w_buf = wp.zeros( + (self._num_instances, num_jacobi_bodies, 6, self._num_joints + num_base_dofs), + dtype=wp.float32, + device=self.device, + ) + + # -- ``body_link_jacobian_w``: output of the COM→origin shift kernel applied to + # the COM-referenced Jacobian above; same shape, link-origin reference + self._body_link_jacobian_w_buf = wp.zeros( + (self._num_instances, num_jacobi_bodies, 6, self._num_joints + num_base_dofs), + dtype=wp.float32, + device=self.device, + ) + + # -- ``mass_matrix``: model-wide ``H`` scratch (eval_mass_matrix output), per-body + # spatial-inertia aux (Featherstone ``I``), and per-view output (gather output) + self._mass_matrix_full_buf = wp.zeros( + (model.articulation_count, max_dofs, max_dofs), dtype=wp.float32, device=self.device + ) + self._mass_matrix_body_I_s_buf = wp.zeros(model.body_count, dtype=wp.spatial_matrix, device=self.device) + self._mass_matrix_buf = wp.zeros( + (self._num_instances, self._num_joints + num_base_dofs, self._num_joints + num_base_dofs), + dtype=wp.float32, + device=self.device, + ) + def _pin_proxy_arrays(self) -> None: """Create or rebind all pinned ProxyArray wrappers. @@ -1618,6 +1833,9 @@ def _pin_proxy_arrays(self) -> None: self._projected_gravity_b_ta = ProxyArray(self._projected_gravity_b.data) self._heading_w_ta = ProxyArray(self._heading_w.data) self._joint_acc_ta = ProxyArray(self._joint_acc.data) + self._body_com_jacobian_w_ta = ProxyArray(self._body_com_jacobian_w_buf) + self._body_link_jacobian_w_ta = ProxyArray(self._body_link_jacobian_w_buf) + self._mass_matrix_ta = ProxyArray(self._mass_matrix_buf) # -- deprecated state properties (lazy); type annotations declared once here self._root_state_w_ta: ProxyArray | None = None diff --git a/source/isaaclab_newton/isaaclab_newton/assets/articulation/kernels.py b/source/isaaclab_newton/isaaclab_newton/assets/articulation/kernels.py index 5e66b867c09a..a928bd524761 100644 --- a/source/isaaclab_newton/isaaclab_newton/assets/articulation/kernels.py +++ b/source/isaaclab_newton/isaaclab_newton/assets/articulation/kernels.py @@ -618,3 +618,142 @@ def concat_joint_pos_limits_lower_and_upper( """ i, j = wp.tid() joint_pos_limits[i, j] = wp.vec2f(joint_pos_limits_lower[i, j], joint_pos_limits_upper[i, j]) + + +@wp.kernel +def gather_jacobian_rows( + src: wp.array4d(dtype=wp.float32), + art_ids: wp.array(dtype=wp.int32), + link_offset: wp.int32, + dst: wp.array4d(dtype=wp.float32), +): + """Copy per-view articulation jacobian rows from a model-sized buffer into a view-sized buffer. + + Newton's ``eval_jacobian`` writes every articulation in the model (across all + :class:`~newton.selection.ArticulationView` instances) into a single 4-D output + shaped ``(model.articulation_count, max_links, 6, max_dofs)``. An + ``ArticulationView`` owns only the subset indexed by ``articulation_ids``. This + kernel gathers those rows into a contiguous view-sized destination so the + caller-facing + :attr:`~isaaclab.assets.BaseArticulationData.body_com_jacobian_w` contract + ``(num_instances, num_jacobi_bodies, 6, num_joints + num_base_dofs)`` is + preserved. + + For fixed-base articulations Newton fills link row 0 with the fixed root joint + (zero motion subspace), so we skip it with ``link_offset = 1``. For floating- + base, ``link_offset = 0`` and the full DoF axis is preserved including the 6 + leading free-root joint columns, matching the cross-library industry + convention used by PhysX, Pinocchio, Drake, MuJoCo, RBDL, OCS2, and iDynTree. + + The gather is in-place on a pre-allocated ``dst`` buffer, so the kernel launch + is safe under CUDA graph capture. + + Args: + src: Input jacobian buffer reshaped to 4-D. Shape is + (model.articulation_count, max_links, 6, max_dofs). + art_ids: Model-level articulation indices owned by this view. Shape is + (num_instances,). + link_offset: Constant offset added to the destination link index when + reading from ``src``. ``1`` for fixed-base views, ``0`` for + floating-base. + dst: Output jacobian buffer for this view. Shape is + (num_instances, num_jacobi_bodies, 6, num_joints + num_base_dofs), + where ``num_jacobi_bodies = this asset's num_bodies - link_offset`` + (per-asset count, not the model-wide ``max_links``). + """ + i, link, s, d = wp.tid() + dst[i, link, s, d] = src[art_ids[i], link + link_offset, s, d] + + +@wp.kernel +def gather_mass_matrix_rows( + src: wp.array3d(dtype=wp.float32), + art_ids: wp.array(dtype=wp.int32), + dst: wp.array3d(dtype=wp.float32), +): + """Copy per-view articulation mass-matrix rows from a model-sized buffer into a view-sized buffer. + + 3-D analogue of :func:`gather_jacobian_rows` for the joint-space mass + matrix written by :func:`newton.sim.articulation.eval_mass_matrix`. The + DoF axis is preserved in full (including the leading 6 free-root rows/cols + for floating-base articulations), matching the cross-library industry + convention used by PhysX, Pinocchio, Drake, MuJoCo, RBDL, OCS2, and iDynTree. + + Args: + src: Input mass-matrix buffer. Shape is + (model.articulation_count, max_dofs, max_dofs). + art_ids: Model-level articulation indices owned by this view. Shape is + (num_instances,). + dst: Output mass-matrix buffer for this view. Shape is + (num_instances, num_joints + num_base_dofs, + num_joints + num_base_dofs). + """ + i, r, c = wp.tid() + dst[i, r, c] = src[art_ids[i], r, c] + + +@wp.kernel +def shift_jacobian_com_to_origin( + body_link_pose: wp.array2d(dtype=wp.transformf), + body_com_pos_b: wp.array2d(dtype=wp.vec3f), + link_offset: wp.int32, + src: wp.array4d(dtype=wp.float32), + dst: wp.array4d(dtype=wp.float32), +): + """Shift the linear-velocity rows of the Jacobian from COM to link origin. + + Newton's ``eval_jacobian`` returns ``J · q_dot = [v_com_world, omega_world]`` + per link — the linear rows are the velocity of the link's center of mass, + expressed in world frame. The + :attr:`~isaaclab.assets.BaseArticulationData.body_link_jacobian_w` contract + requires the linear rows to be the velocity at the link **origin** + (USD prim transform) so that ``J · q_dot[body_idx]`` matches + :attr:`~isaaclab.assets.BaseArticulationData.body_link_lin_vel_w` / + :attr:`~isaaclab.assets.BaseArticulationData.body_link_ang_vel_w`. + + The shift is the same one applied per-body by + :func:`get_link_vel_from_root_com_vel_func`, but layered onto every + Jacobian column: each column represents the spatial velocity contribution + of one DoF, and shifting a spatial velocity from COM to link origin uses + the same ``v_origin = v_com - omega x (R · body_com_pos_b)`` identity. + + Notes on layout: + * Jacobian rows ``[0:3]`` are linear velocity, ``[3:6]`` are angular. + * ``body_link_pose`` and ``body_com_pos_b`` are indexed by the + articulation's full body count, so ``link_offset`` must be applied + to map a row in the (already-gathered) ``src`` to its body index in + the asset data. ``link_offset = 1`` for fixed-base (Newton's row 0 + fixed-root row was dropped during the prior gather); + ``link_offset = 0`` for floating-base. + + Args: + body_link_pose: Per-body link pose in world frame. Shape is + (num_instances, num_bodies). + body_com_pos_b: Per-body center-of-mass offset expressed in the body's + link frame. Shape is (num_instances, num_bodies). + link_offset: Offset added to the jacobian-row body index to reach the + full body index. ``1`` for fixed-base, ``0`` for floating-base. + src: COM-referenced Jacobian (read-only). Shape is + (num_instances, num_jacobi_bodies, 6, num_joints + num_base_dofs). + dst: Output buffer for the link-origin Jacobian. Same shape as + ``src``. Linear rows ``[0:3]`` are written with the shifted + velocity; angular rows ``[3:6]`` are copied unchanged (angular + velocity is reference-point invariant). + """ + n, b, dof = wp.tid() + full_body_idx = b + link_offset + + R = wp.transform_get_rotation(body_link_pose[n, full_body_idx]) + c_world = wp.quat_rotate(R, body_com_pos_b[n, full_body_idx]) + + v_com = wp.vec3(src[n, b, 0, dof], src[n, b, 1, dof], src[n, b, 2, dof]) + omega = wp.vec3(src[n, b, 3, dof], src[n, b, 4, dof], src[n, b, 5, dof]) + + v_origin = v_com - wp.cross(omega, c_world) + + dst[n, b, 0, dof] = v_origin[0] + dst[n, b, 1, dof] = v_origin[1] + dst[n, b, 2, dof] = v_origin[2] + dst[n, b, 3, dof] = omega[0] + dst[n, b, 4, dof] = omega[1] + dst[n, b, 5, dof] = omega[2] diff --git a/source/isaaclab_newton/isaaclab_newton/assets/rigid_object/rigid_object.py b/source/isaaclab_newton/isaaclab_newton/assets/rigid_object/rigid_object.py index 7e31fa22b60f..b93c9075393d 100644 --- a/source/isaaclab_newton/isaaclab_newton/assets/rigid_object/rigid_object.py +++ b/source/isaaclab_newton/isaaclab_newton/assets/rigid_object/rigid_object.py @@ -334,6 +334,7 @@ def write_root_link_pose_to_sim_index( self.data._root_link_state_w.timestamp = -1.0 if self.data._root_state_w is not None: self.data._root_state_w.timestamp = -1.0 + self.data._fk_timestamp = -1.0 # Forces a kinematic update to get the latest body link poses. SimulationManager.invalidate_fk(env_ids=env_ids, articulation_ids=self._root_view.articulation_ids) def write_root_link_pose_to_sim_mask( @@ -382,6 +383,7 @@ def write_root_link_pose_to_sim_mask( self.data._root_link_state_w.timestamp = -1.0 if self.data._root_state_w is not None: self.data._root_state_w.timestamp = -1.0 + self.data._fk_timestamp = -1.0 # Forces a kinematic update to get the latest body link poses. SimulationManager.invalidate_fk(env_mask=env_mask, articulation_ids=self._root_view.articulation_ids) def write_root_com_pose_to_sim_index( @@ -437,6 +439,7 @@ def write_root_com_pose_to_sim_index( self.data._root_link_state_w.timestamp = -1.0 if self.data._root_state_w is not None: self.data._root_state_w.timestamp = -1.0 + self.data._fk_timestamp = -1.0 # Forces a kinematic update to get the latest body link poses. SimulationManager.invalidate_fk(env_ids=env_ids, articulation_ids=self._root_view.articulation_ids) def write_root_com_pose_to_sim_mask( @@ -489,6 +492,7 @@ def write_root_com_pose_to_sim_mask( self.data._root_link_state_w.timestamp = -1.0 if self.data._root_state_w is not None: self.data._root_state_w.timestamp = -1.0 + self.data._fk_timestamp = -1.0 # Forces a kinematic update to get the latest body link poses. SimulationManager.invalidate_fk(env_mask=env_mask, articulation_ids=self._root_view.articulation_ids) def write_root_com_velocity_to_sim_index( diff --git a/source/isaaclab_newton/isaaclab_newton/assets/rigid_object/rigid_object_data.py b/source/isaaclab_newton/isaaclab_newton/assets/rigid_object/rigid_object_data.py index 0e9ecc8a41d0..43e719d3a580 100644 --- a/source/isaaclab_newton/isaaclab_newton/assets/rigid_object/rigid_object_data.py +++ b/source/isaaclab_newton/isaaclab_newton/assets/rigid_object/rigid_object_data.py @@ -76,6 +76,7 @@ def __init__(self, root_view: ArticulationView, device: str): # Set initial time stamp self._sim_timestamp = 0.0 self._is_primed = False + self._fk_timestamp = 0.0 # Convert to direction vector gravity = wp.to_torch(SimulationManager.get_model().gravity)[0] @@ -121,6 +122,9 @@ def update(self, dt: float) -> None: """ # update the simulation timestamp self._sim_timestamp += dt + # FK is current after a sim step — keep fk_timestamp in sync unless it was explicitly invalidated + if self._fk_timestamp >= 0.0: + self._fk_timestamp = self._sim_timestamp # Trigger an update of the body com acceleration buffer at a higher frequency # since we do finite differencing. self.body_com_acc_w @@ -291,6 +295,9 @@ def body_link_pose_w(self) -> ProxyArray: This quantity is the pose of the actor frame of the rigid body relative to the world. The orientation is provided in (x, y, z, w) format. """ + if self._fk_timestamp < self._sim_timestamp: + SimulationManager.forward() + self._fk_timestamp = self._sim_timestamp return self._body_link_pose_w_ta @property diff --git a/source/isaaclab_newton/isaaclab_newton/assets/rigid_object_collection/rigid_object_collection.py b/source/isaaclab_newton/isaaclab_newton/assets/rigid_object_collection/rigid_object_collection.py index 8c499d75396c..b11415d48231 100644 --- a/source/isaaclab_newton/isaaclab_newton/assets/rigid_object_collection/rigid_object_collection.py +++ b/source/isaaclab_newton/isaaclab_newton/assets/rigid_object_collection/rigid_object_collection.py @@ -78,8 +78,9 @@ def __init__(self, cfg: RigidObjectCollectionCfg): for rigid_body_cfg in self.cfg.rigid_objects.values(): # spawn the asset if rigid_body_cfg.spawn is not None: + spawn_path = rigid_body_cfg.spawn.spawn_path or rigid_body_cfg.prim_path rigid_body_cfg.spawn.func( - rigid_body_cfg.prim_path, + spawn_path, rigid_body_cfg.spawn, translation=rigid_body_cfg.init_state.pos, orientation=rigid_body_cfg.init_state.rot, diff --git a/source/isaaclab_newton/isaaclab_newton/cloner/newton_replicate.py b/source/isaaclab_newton/isaaclab_newton/cloner/newton_replicate.py index 34cd35de4fa2..46d4f967d51f 100644 --- a/source/isaaclab_newton/isaaclab_newton/cloner/newton_replicate.py +++ b/source/isaaclab_newton/isaaclab_newton/cloner/newton_replicate.py @@ -5,6 +5,8 @@ from __future__ import annotations +from collections.abc import Sequence + import torch import warp as wp from newton import ModelBuilder, solvers @@ -17,7 +19,7 @@ def _build_newton_builder_from_mapping( stage: Usd.Stage, - sources: list[str], + sources: Sequence[str], env_ids: torch.Tensor, mapping: torch.Tensor, positions: torch.Tensor | None = None, @@ -53,7 +55,7 @@ def _build_newton_builder_from_mapping( builder = NewtonManager.create_builder(up_axis=up_axis) stage_info = builder.add_usd( stage, - ignore_paths=["/World/envs"] + sources, + ignore_paths=["/World/envs", *sources], schema_resolvers=schema_resolvers, ) @@ -117,10 +119,23 @@ def _build_newton_builder_from_mapping( def _rename_builder_labels( - builder: ModelBuilder, sources: list[str], destinations: list[str], env_ids: torch.Tensor, mapping: torch.Tensor + builder: ModelBuilder, + sources: Sequence[str], + destinations: Sequence[str], + env_ids: torch.Tensor, + mapping: torch.Tensor, ) -> None: """Rename builder labels/keys from source roots to destination roots. + Walks both built-in label arrays (``body``, ``joint``, ``shape``, + ``articulation``, ``constraint_mimic``, ``equality_constraint``) and any + string-typed custom-attribute column whose frequency declares a sibling + world column (``references="world"``). + The ``startswith(src_prefix)`` guard makes the rewrite a no-op for strings that + are not paths under the source, so non-path custom string columns are passed + through untouched and any future solver-registered string column is handled + automatically without changes here. + Args: builder: Newton model builder to update in-place. sources: Source prim root paths. @@ -130,27 +145,61 @@ def _rename_builder_labels( """ # per-source, per-world renaming (strict prefix swap), compact style preserved for i, src_path in enumerate(sources): - src_prefix_len = len(src_path.rstrip("/")) + # Boundary-terminated prefix prevents over-matching when one source path is a + # prefix of another (e.g. ``/Sources/protoA`` vs ``/Sources/protoAB``). + src_prefix = src_path.rstrip("/") + "/" + src_prefix_len = len(src_prefix) - 1 # slice index keeps the leading "/" in the suffix swap = lambda name, new_root: new_root + name[src_prefix_len:] # noqa: E731 world_cols = torch.nonzero(mapping[i], as_tuple=True)[0].tolist() # Map Newton world IDs (sequential) to destination paths using env_ids world_roots = {int(env_ids[c]): destinations[i].format(int(env_ids[c])) for c in world_cols} - for t in ("body", "joint", "shape", "articulation"): + def _rename_pair(values, worlds): + if len(values) != len(worlds): + raise ValueError(f"label/world column length mismatch: {len(values)} vs {len(worlds)}") + for k in range(len(values)): + world_id = int(worlds[k]) + if world_id in world_roots and isinstance(values[k], str) and values[k].startswith(src_prefix): + values[k] = swap(values[k], world_roots[world_id]) + + # Pass 1: built-in label arrays. Each has a paired ``*_world`` int column. + # Use ``is None`` (not ``or``) so an empty-but-defined ``*_label`` column + # is recognized — falling through to ``*_key`` would over-match a + # builder that legitimately exposes both attributes. + for t in ("body", "joint", "shape", "articulation", "constraint_mimic", "equality_constraint"): labels = getattr(builder, f"{t}_label", None) if labels is None: - labels = getattr(builder, f"{t}_key") - worlds_arr = getattr(builder, f"{t}_world") - for k, w in enumerate(worlds_arr): - world_id = int(w) - if world_id in world_roots and labels[k].startswith(src_path): - labels[k] = swap(labels[k], world_roots[world_id]) + labels = getattr(builder, f"{t}_key", None) + worlds_arr = getattr(builder, f"{t}_world", None) + if labels is None or worlds_arr is None: + continue + _rename_pair(labels, worlds_arr) + + # Pass 2: string-typed custom-attribute columns (e.g. ``mujoco:tendon_label``) + # paired with a world companion declared via ``references="world"``. Index + # world companions by frequency for O(1) lookup, then walk the str columns. + custom = builder.custom_attributes + world_by_freq: dict[str, ModelBuilder.CustomAttribute] = {} + for attr in custom.values(): + if getattr(attr, "references", None) == "world": + world_by_freq[attr.frequency] = attr + for attr in custom.values(): + if attr.dtype is not str: + continue + world_attr = world_by_freq.get(attr.frequency) + if world_attr is None: + continue + values = attr.values + worlds = world_attr.values + if not values or not worlds: + continue + _rename_pair(values, worlds) def newton_physics_replicate( stage: Usd.Stage, - sources: list[str], - destinations: list[str], + sources: Sequence[str], + destinations: Sequence[str], env_ids: torch.Tensor, mapping: torch.Tensor, positions: torch.Tensor | None = None, @@ -195,8 +244,8 @@ def newton_physics_replicate( def newton_visualizer_prebuild( stage: Usd.Stage, - sources: list[str], - destinations: list[str], + sources: Sequence[str], + destinations: Sequence[str], env_ids: torch.Tensor, mapping: torch.Tensor, positions: torch.Tensor | None = None, diff --git a/source/isaaclab_newton/isaaclab_newton/physics/_cubric.py b/source/isaaclab_newton/isaaclab_newton/physics/_cubric.py index cc549d4b82be..abe09cb03bdc 100644 --- a/source/isaaclab_newton/isaaclab_newton/physics/_cubric.py +++ b/source/isaaclab_newton/isaaclab_newton/physics/_cubric.py @@ -161,11 +161,8 @@ def initialize(self) -> bool: version=_Version(0, 1), ) - # Try several acquisition strategies — the required client name - # varies across Kit configurations. - ia_ptr = try_acquire_fn(b"carb.scripting-python.plugin", desc, None) - if not ia_ptr: - ia_ptr = try_acquire_fn(None, desc, None) + # Try tryAcquire first (non-loading); fall back to acquire (will load the plugin if registered). + ia_ptr = try_acquire_fn(b"isaaclab.cubric", desc, None) if not ia_ptr: acquire_addr = _read_u64(fw_ptr + 16) # acquireInterfaceWithClient if acquire_addr: diff --git a/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py b/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py index 2bb2f1f37397..7f112af4bacf 100644 --- a/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py +++ b/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py @@ -33,7 +33,7 @@ from newton.sensors import SensorIMU as NewtonSensorIMU from newton.solvers import SolverBase, SolverNotifyFlags -from isaaclab.physics import CallbackHandle, PhysicsEvent, PhysicsManager +from isaaclab.physics import CallbackHandle, PhysicsEvent, PhysicsManager, SceneDataBackend, SceneDataFormat from isaaclab.sim.utils.newton_model_utils import replace_newton_shape_colors from isaaclab.sim.utils.stage import get_current_stage from isaaclab.utils import checked_apply @@ -69,7 +69,7 @@ def _set_fabric_transforms( i = int(wp.tid()) idx = int(newton_indices[i]) transform = newton_body_q[idx] - fabric_transforms[i] = wp.transpose(wp.mat44d(wp.math.transform_to_matrix(transform))) + fabric_transforms[i] = wp.transpose(wp.mat44d(wp.transform_to_matrix(transform))) @wp.kernel(enable_backward=False) @@ -100,6 +100,44 @@ def _scatter_reset_masks_from_ids( fk_mask[articulation_ids[world, arti]] = True +class NewtonSceneDataBackend(SceneDataBackend): + """Scene data backend that reads rigid body transforms from Newton's simulation state. + + The backend reads ``body_q`` (an array of :class:`wp.transformf`) from + Newton's current state and exposes it as :class:`SceneDataFormat.Transform`. + Body paths come from the model's ``body_label`` attribute. + """ + + def __init__(self): + self._scene_data = SceneDataFormat.Transform() + + @property + def transforms(self) -> SceneDataFormat.Transform: + """Return the current Newton rigid body transforms as :class:`SceneDataFormat.Transform`.""" + self._scene_data.transforms = self.state.body_q + return self._scene_data + + @property + def transform_count(self) -> int: + """Return the number of rigid body transforms in the Newton sim.""" + return self.model.body_count + + @property + def transform_paths(self) -> list[str]: + """Return the prim paths for each rigid body transform.""" + if self.model.body_label is not None: + return list(self.model.body_label) + return [] + + @property + def model(self) -> Model: + return NewtonManager.get_model() + + @property + def state(self) -> Model: + return NewtonManager.get_state_0() + + class NewtonManager(PhysicsManager): """Abstract Newton physics manager for Isaac Lab. @@ -178,6 +216,15 @@ class NewtonManager(PhysicsManager): # Model changes (callbacks use unified system from PhysicsManager) _model_changes: set[int] = set() + # Scene data backend + _scene_data_backend: NewtonSceneDataBackend | None = None + + # Visualization-only state used when the sim backend is PhysX. Populated + # lazily in :meth:`_ensure_visualization_model` and updated each render + # frame in :meth:`update_visualization_state`. + _visualization_scene_data: SceneDataFormat.Transform | None = None + _visualization_mapping: wp.array | None = None + # Views list for assets to register their views _views: list = [] @@ -219,7 +266,9 @@ def initialize(cls, sim_context: SimulationContext) -> None: from isaaclab.app.settings_manager import get_settings_manager cameras_enabled = bool(get_settings_manager().get("/isaaclab/cameras_enabled", False)) - NewtonManager._clone_physics_only = "kit" not in requested and not cameras_enabled + cls._clone_physics_only = "kit" not in requested and not cameras_enabled + + cls._scene_data_backend = NewtonSceneDataBackend() @classmethod def reset(cls, soft: bool = False) -> None: @@ -414,6 +463,11 @@ def close(cls) -> None: cls.clear() super().close() + @classmethod + def get_scene_data_backend(cls) -> SceneDataBackend: + """Return the SceneDataBackend for the SceneDataProvider.""" + return cls._scene_data_backend + @classmethod def register_callback( cls, @@ -477,7 +531,10 @@ def clear(cls): NewtonManager._usdrt_stage = None NewtonManager._transforms_dirty = False NewtonManager._up_axis = "Z" + NewtonManager._visualization_scene_data = None + NewtonManager._visualization_mapping = None NewtonManager._model_changes = set() + NewtonManager._scene_data_backend = None NewtonManager._cl_pending_sites = {} NewtonManager._cl_site_index_map = {} NewtonManager._pending_extended_state_attributes = set() @@ -1246,14 +1303,288 @@ def get_solver_convergence_steps(cls) -> dict[str, float | int]: # State accessors (used extensively by articulation/rigid object data) @classmethod def get_model(cls) -> Model: - """Get the Newton model.""" + """Get the Newton model. + + When the active sim backend is Newton this returns the manager's own + authoritative model. When the active sim backend is PhysX a shadow + Newton model is built lazily (from the visualizer prebuilt artifact) so + renderers/visualizers that operate on Newton ``Model`` and ``State`` can + still drive a PhysX-simulated scene. + """ + cls._ensure_visualization_model() return cls._model @classmethod def get_state_0(cls) -> State: """Get the current state.""" + cls._ensure_visualization_model() return cls._state_0 + @classmethod + def get_state(cls) -> State: + """Get the current Newton state for visualization. + + Use this method from visualizers/renderers/video recorders that need a + backend-agnostic Newton ``State``. When the sim backend is PhysX this + refreshes the shadow ``_state_0.body_q`` from the live PhysX scene via + :meth:`update_visualization_state` before returning, so callers never + observe stale transforms. Under the Newton sim backend + :meth:`update_visualization_state` is a no-op and this is equivalent to + :meth:`get_state_0`. + """ + cls.update_visualization_state() + return cls.get_state_0() + + @classmethod + def get_num_envs(cls) -> int: + return cls._num_envs + + @classmethod + def _backend_is_newton(cls) -> bool: + """Return ``True`` when the active sim backend is Newton.""" + sim = PhysicsManager._sim + if sim is None: + return False + return isinstance(sim.get_scene_data_provider().backend, NewtonSceneDataBackend) + + @classmethod + def _ensure_visualization_model(cls) -> None: + """Build a shadow Newton model from the USD stage when the sim backend is PhysX. + + No-op when the sim backend is Newton (the manager's own ``_model`` / + ``_state_0`` are authoritative) or when a shadow model has already been + built. This is the entry point that makes :meth:`get_model` / + :meth:`get_state` work uniformly across both sim backends. + + The shadow model is built by walking the USD stage via + :meth:`_build_visualization_model_from_stage` and finalizing the resulting + :class:`~newton.ModelBuilder`. Per-frame body transforms are pushed into + ``_state_0.body_q`` by :meth:`update_visualization_state` using the new + :class:`~isaaclab.scene.scene_data_provider.SceneDataProvider`. + """ + + if cls._model is not None and cls._state_0 is not None: + return + + if cls._backend_is_newton(): + return + + stage = get_current_stage() + if stage is None: + logger.error( + "[NewtonManager] No USD stage available; cannot build a Newton " + "Model/State for visualization while the sim backend is PhysX." + ) + return + + try: + builder = cls._build_visualization_model_from_stage(stage) + except Exception: + logger.exception( + "[NewtonManager] Failed to build a Newton ModelBuilder from the USD stage " + "for visualization (sim backend is PhysX)." + ) + return + + if builder is None or builder.body_count == 0: + logger.error( + "[NewtonManager] USD stage walk produced no Newton bodies; the shadow " + "Newton model for visualization will be empty. Common causes: the cloned " + "envs are not yet on the stage, or PhysX schemas could not be parsed by " + "Newton's add_usd. Check that /World/envs/env_ prims exist when the " + "renderer is initialized." + ) + return + + device = PhysicsManager._device or "cpu" + try: + cls._model = builder.finalize(device=device) + cls._state_0 = cls._model.state() + cls._model.num_envs = cls._num_envs + replace_newton_shape_colors(cls._model) + + except Exception: + logger.exception( + "[NewtonManager] Failed to finalize the shadow Newton ModelBuilder for " + "visualization (sim backend is PhysX)." + ) + cls._model = None + cls._state_0 = None + + @classmethod + def _build_visualization_model_from_stage(cls, stage) -> ModelBuilder | None: + """Build a fresh Newton ``ModelBuilder`` from the USD stage for visualization. + + Walks IsaacLab's ``/World/envs/env_`` convention and adds each env as + its own Newton world. When the env subtree is identical across envs (the + common cloned-scene case) a single env_0 prototype is built once and + replicated via :meth:`ModelBuilder.add_builder`; otherwise each env is + ingested independently with :meth:`ModelBuilder.add_usd`. + + This routine is intentionally independent of + :meth:`instantiate_builder_from_stage` (which targets the live-sim path + and uses a different naming convention and writes into ``cls._builder`` + and ``cls._cl_site_index_map``). The visualization shadow path must not + pollute those live-sim slots. ``cls._num_envs`` is populated here too so + :meth:`get_num_envs` returns the env count when the sim backend is PhysX + (the live-sim path never runs in that configuration, so there is no slot + to collide with). + + Args: + stage: USD stage to inspect. + + Returns: + A populated :class:`~newton.ModelBuilder`, or ``None`` when no + ``/World/envs/env_`` prims exist on the stage. + """ + import re + + from pxr import UsdGeom + + up_axis_token = UsdGeom.GetStageUpAxis(stage) + up_axis = Axis.from_string(str(up_axis_token)) + schema_resolvers = [SchemaResolverNewton(), SchemaResolverPhysx()] + + env_pattern = re.compile(r"^env_(\d+)$") + env_paths: list[tuple[int, str]] = [] + envs_root = stage.GetPrimAtPath("/World/envs") + if envs_root and envs_root.IsValid(): + for child in envs_root.GetChildren(): + if match := env_pattern.match(child.GetName()): + env_paths.append((int(match.group(1)), child.GetPath().pathString)) + env_paths.sort(key=lambda x: x[0]) + + builder = ModelBuilder(up_axis=up_axis) + + if not env_paths: + # Fallback: ingest the whole stage as a single world. + builder.add_usd(stage, schema_resolvers=schema_resolvers) + NewtonManager._num_envs = 1 + return builder + + NewtonManager._num_envs = len(env_paths) + + # Ingest stage-level (non-env) geometry into the global world (``current_world == -1``) + # so visualization sees the ground plane, ceilings, fixed props, etc. The legacy + # cloner-based prebuild did this via ``add_usd(stage, ignore_paths=["/World/envs"], ...)`` + # before adding the per-env worlds; without this, renderers/visualizers driven off the + # shadow Newton model are missing every shape authored outside the env hierarchy. + builder.add_usd( + stage, + ignore_paths=[r"/World/envs($|/.*)"], + schema_resolvers=schema_resolvers, + ) + + # Build env_0 as a prototype, then replicate across envs. + proto_env_path = env_paths[0][1] + proto = ModelBuilder(up_axis=up_axis) + proto.add_usd( + stage, + root_path=proto_env_path, + schema_resolvers=schema_resolvers, + ) + + xform_cache = UsdGeom.XformCache() + + # ``add_builder`` copies the prototype's ``body_label`` (and sibling label arrays) + # verbatim into each replicated world, so all worlds end up with prim paths under + # the prototype env (e.g. ``/World/envs/env_0/...``). The visualization sync uses + # these labels to map PhysX transforms (which carry distinct per-env paths) into + # ``state.body_q``; without rewriting, ``paths.index()`` resolves every match to + # world 0 and worlds 1..N never receive fresh poses. Rewrite the newly-added + # labels after each ``add_builder`` so each world references its own env prim path. + label_attrs = ("body_label", "articulation_label", "joint_label", "shape_label") + label_starts = {attr: len(getattr(builder, attr)) for attr in label_attrs} + + # ``proto.add_usd`` ingests env_0's bodies at their absolute world positions + # (``UsdPhysics.LoadUsdPhysicsFromRange`` reports world-space transforms), so + # ``proto.body_q`` already encodes env_0's world transform. ``add_builder`` + # composes its ``xform`` onto every imported body, so passing each env's + # absolute world transform here would double the offset; the correct xform is + # the env's pose relative to the prototype (identity for env_0, env_X * env_0^-1 + # for the rest). Dynamic bodies are overwritten in ``update_visualization_state`` + # via the PhysX sync, but static bodies (e.g. the table) keep this initial pose + # and render at the wrong position when env_0 is not at the world origin. + proto_world_gf = xform_cache.GetLocalToWorldTransform(stage.GetPrimAtPath(proto_env_path)) + proto_translation = proto_world_gf.ExtractTranslation() + proto_rotation = proto_world_gf.ExtractRotationQuat() + proto_world_tf = wp.transform( + (proto_translation[0], proto_translation[1], proto_translation[2]), + ( + proto_rotation.GetImaginary()[0], + proto_rotation.GetImaginary()[1], + proto_rotation.GetImaginary()[2], + proto_rotation.GetReal(), + ), + ) + proto_world_tf_inv = wp.transform_inverse(proto_world_tf) + + for _, env_path in env_paths: + world_xform = xform_cache.GetLocalToWorldTransform(stage.GetPrimAtPath(env_path)) + translation = world_xform.ExtractTranslation() + rotation = world_xform.ExtractRotationQuat() + env_world_tf = wp.transform( + (translation[0], translation[1], translation[2]), + ( + rotation.GetImaginary()[0], + rotation.GetImaginary()[1], + rotation.GetImaginary()[2], + rotation.GetReal(), + ), + ) + relative_tf = wp.transform_multiply(env_world_tf, proto_world_tf_inv) + builder.begin_world() + builder.add_builder(proto, xform=relative_tf) + if env_path != proto_env_path: + for attr in label_attrs: + labels = getattr(builder, attr) + for i in range(label_starts[attr], len(labels)): + labels[i] = labels[i].replace(proto_env_path, env_path, 1) + for attr in label_attrs: + label_starts[attr] = len(getattr(builder, attr)) + builder.end_world() + + return builder + + @classmethod + def update_visualization_state(cls) -> None: + """Refresh visualization state for the active sim backend. + + Newton sim backend: no-op — ``_state_0`` is the live, authoritative state + already advanced by :meth:`step` / forward kinematics. + + PhysX sim backend: pull rigid-body transforms from the + :class:`~isaaclab.scene.scene_data_provider.SceneDataProvider` and write + them into the shadow ``_state_0.body_q`` so Newton-native consumers + (Newton renderer, Newton/Rerun/Viser visualizers, OVRTX renderer, Newton + GL video) see fresh poses. + + Invoked lazily from :meth:`get_state` so consumers do not need to + coordinate the sync explicitly. + """ + if cls._backend_is_newton(): + return + cls._ensure_visualization_model() + if cls._state_0 is None or cls._model is None or cls._state_0.body_q is None: + return + sim = PhysicsManager._sim + if sim is None: + return + + sdp = sim.get_scene_data_provider() + if cls._visualization_scene_data is None: + cls._visualization_scene_data = SceneDataFormat.Transform() + if cls._visualization_mapping is None: + body_paths = list(getattr(cls._model, "body_label", None) or []) + cls._visualization_mapping = sdp.create_mapping(body_paths) + + cls._visualization_scene_data.transforms = cls._state_0.body_q + sdp.get_transforms( + cls._visualization_scene_data, + mapping=cls._visualization_mapping, + allow_passthrough=False, + ) + @classmethod def get_state_1(cls) -> State: """Get the next state.""" @@ -1351,7 +1682,7 @@ def _normalize_for_labels(expr: str | list[str] | None, labels: list[str]) -> st sensing_obj_shapes=_normalize_for_labels(_to_fnmatch(shape_names_expr), shape_labels), counterpart_bodies=_normalize_for_labels(_to_fnmatch(contact_partners_body_expr), body_labels), counterpart_shapes=_normalize_for_labels(_to_fnmatch(contact_partners_shape_expr), shape_labels), - include_total=True, + measure_total=True, verbose=verbose, ) diff --git a/source/isaaclab_newton/isaaclab_newton/renderers/newton_warp_renderer.py b/source/isaaclab_newton/isaaclab_newton/renderers/newton_warp_renderer.py index a02d820f2951..fbccc3da4137 100644 --- a/source/isaaclab_newton/isaaclab_newton/renderers/newton_warp_renderer.py +++ b/source/isaaclab_newton/isaaclab_newton/renderers/newton_warp_renderer.py @@ -20,10 +20,10 @@ from isaaclab.sim import SimulationContext from isaaclab.utils.math import convert_camera_frame_orientation_convention +from ..physics.newton_manager import NewtonManager from .newton_warp_renderer_cfg import NewtonWarpRendererCfg if TYPE_CHECKING: - from isaaclab.physics import BaseSceneDataProvider from isaaclab.sensors.camera.camera_data import CameraData logger = logging.getLogger(__name__) @@ -139,12 +139,15 @@ class NewtonWarpRenderer(BaseRenderer): RenderData = RenderData def __init__(self, cfg: NewtonWarpRendererCfg): + """Pre-physics initialization.""" from isaaclab.physics.scene_data_requirements import ( aggregate_requirements, requirement_for_renderer_type, ) self.cfg = cfg + self.newton_sensor: newton.sensors.SensorTiledCamera | None = None + sim = SimulationContext.instance() current_req = sim.get_scene_data_requirements() renderer_req = requirement_for_renderer_type("newton_warp") @@ -152,28 +155,38 @@ def __init__(self, cfg: NewtonWarpRendererCfg): if merged != current_req: sim.update_scene_data_requirements(merged) - newton_model = self.get_scene_data_provider().get_newton_model() - if newton_model is None: + def initialize(self) -> None: + """Post-physics setup: read the built Newton model and construct the sensor.""" + self._newton_model: newton.Model = NewtonManager.get_model() + if self._newton_model is None: raise RuntimeError( - "NewtonWarpRenderer requires a Newton model but the scene data provider returned None. " + "NewtonWarpRenderer requires a Newton model but NewtonManager.get_model() returned None. " "This usually means the Newton model failed to build from the USD stage " "(e.g., unsupported PhysX schemas such as tendons). " "Check the log for earlier Newton model build errors." ) self.newton_sensor = newton.sensors.SensorTiledCamera( - newton_model, + self._newton_model, config=newton.sensors.SensorTiledCamera.RenderConfig( - enable_textures=cfg.enable_textures, - enable_shadows=cfg.enable_shadows, - enable_ambient_lighting=cfg.enable_ambient_lighting, - enable_backface_culling=cfg.enable_backface_culling, - max_distance=cfg.max_distance, + enable_textures=self.cfg.enable_textures, + enable_shadows=self.cfg.enable_shadows, + enable_ambient_lighting=self.cfg.enable_ambient_lighting, + enable_backface_culling=self.cfg.enable_backface_culling, + max_distance=self.cfg.max_distance, ), ) - if cfg.create_default_light: - self.newton_sensor.utils.create_default_light(enable_shadows=cfg.enable_shadows) + # Newton ``v1.2.0rc2`` made shape-BVH construction explicit; ``SensorTiledCamera.update`` + # no longer auto-builds when a non-``None`` state is passed, and the underlying + # ``RenderContext.render`` raises if ``build_bvh_shape`` was never called for the model. + # Build it once per model — idempotent across multiple sensors that share ``newton_model`` + # because subsequent calls overwrite the same model-level BVH attributes. + if self._newton_model.shape_count > 0 and self._newton_model.bvh_shapes is None: + newton.geometry.build_bvh_shape(self._newton_model, self._newton_model.state()) + + if self.cfg.create_default_light: + self.newton_sensor.utils.create_default_light(enable_shadows=self.cfg.enable_shadows) def supported_output_types(self) -> dict[RenderBufferKind, RenderBufferSpec]: """Publish the per-output layout this Newton Warp backend writes. @@ -209,7 +222,9 @@ def set_outputs(self, render_data: RenderData, output_data: dict[str, torch.Tens def update_transforms(self): """Sync Newton scene state before rendering. See :meth:`~isaaclab.renderers.base_renderer.BaseRenderer.update_transforms`.""" - SimulationContext.instance().update_scene_data_provider(True) + sim = SimulationContext.instance() + sim.physics_manager.forward() + NewtonManager.update_visualization_state() def update_camera( self, render_data: RenderData, positions: torch.Tensor, orientations: torch.Tensor, intrinsics: torch.Tensor @@ -220,8 +235,16 @@ def update_camera( def render(self, render_data: RenderData): """Render and write to output buffers. See :meth:`~isaaclab.renderers.base_renderer.BaseRenderer.render`.""" + + newton_state: newton.State = NewtonManager.get_state() + + # Refit the shape BVH against the current state since env body poses move every frame. + # ``build_bvh_shape`` ran once in ``__init__``; ``refit_bvh_shape`` reuses that topology. + if self.newton_sensor.model.shape_count > 0: + newton.geometry.refit_bvh_shape(self.newton_sensor.model, newton_state) + self.newton_sensor.update( - self.get_scene_data_provider().get_newton_state(), + newton_state, render_data.camera_transforms, render_data.camera_rays, color_image=render_data.outputs.color_image, @@ -248,7 +271,5 @@ def read_output(self, render_data: RenderData, camera_data: CameraData) -> None: def cleanup(self, render_data: RenderData | None): """Release resources. No-op for Newton Warp. See :meth:`~isaaclab.renderers.base_renderer.BaseRenderer.cleanup`.""" - pass - - def get_scene_data_provider(self) -> BaseSceneDataProvider: - return SimulationContext.instance().initialize_scene_data_provider() + if render_data: + render_data.sensor = None diff --git a/source/isaaclab_newton/isaaclab_newton/scene_data_providers/newton_scene_data_provider.py b/source/isaaclab_newton/isaaclab_newton/scene_data_providers/newton_scene_data_provider.py deleted file mode 100644 index ba19f4e7c63a..000000000000 --- a/source/isaaclab_newton/isaaclab_newton/scene_data_providers/newton_scene_data_provider.py +++ /dev/null @@ -1,306 +0,0 @@ -# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). -# All rights reserved. -# -# SPDX-License-Identifier: BSD-3-Clause - -"""Scene data provider for Newton physics backend.""" - -from __future__ import annotations - -import logging -import re -from collections import deque -from typing import Any - -from pxr import UsdGeom - -from isaaclab.physics.base_scene_data_provider import BaseSceneDataProvider - -logger = logging.getLogger(__name__) - -_ENV_ID_RE = re.compile(r"/World/envs/env_(\d+)") - - -class NewtonSceneDataProvider(BaseSceneDataProvider): - """Scene data provider for Newton physics backend. - - Provides access to Newton model, state, and USD stage for visualizers and renderers. - Unlike PhysxSceneDataProvider which must build its own Newton model from USD and sync - PhysX transforms into it, this provider delegates directly to NewtonManager since the - Newton backend already owns the authoritative model and state. - """ - - def __init__(self, stage, simulation_context) -> None: - """Initialize the Newton scene data provider. - - Args: - stage: USD stage handle. - simulation_context: Active simulation context. - """ - self._simulation_context = simulation_context - self._stage = stage - self._metadata = {"physics_backend": "newton"} - self._num_envs: int | None = None - self._warned_once: set[str] = set() - - # Determine if usd stage sync is required for selected renderers and visualizers - requirements = self._simulation_context.get_scene_data_requirements() - self._needs_usd_sync = bool(requirements.requires_usd_stage) - - def _warn_once(self, key: str, message: str, *args) -> None: - """Emit a warning once per unique key. - - Args: - key: Unique warning key. - message: Warning message format string. - *args: Optional formatting arguments. - """ - if key in self._warned_once: - return - self._warned_once.add(key) - logger.warning(message, *args) - - # ---- Environment discovery --------------------------------------------------------------- - - def get_num_envs(self) -> int: - """Return discovered environment count. - - Returns: - Number of environments discovered from stage prim paths. - """ - if self._num_envs is not None and self._num_envs > 0: - return self._num_envs - discovered = self._determine_num_envs_in_scene() - if discovered > 0: - self._num_envs = discovered - return discovered - return 0 - - def _determine_num_envs_in_scene(self) -> int: - """Infer environment count from ``/World/envs/env_`` prim names. - - Returns: - Number of environments inferred from the stage. - """ - if self._stage is None: - return 0 - max_env_id = -1 - env_name_re = re.compile(r"^env_(\d+)$") - envs_root = self._stage.GetPrimAtPath("/World/envs") - if envs_root.IsValid(): - for child in envs_root.GetChildren(): - match = env_name_re.match(child.GetName()) - if match: - max_env_id = max(max_env_id, int(match.group(1))) - return max_env_id + 1 if max_env_id >= 0 else 0 - - # ---- Core provider API ------------------------------------------------------------------- - - def update(self) -> None: - """Sync Newton body transforms to USD Fabric when a Kit viewport is active. - - Called at render cadence by :meth:`~isaaclab.sim.SimulationContext.update_scene_data_provider`, - after forward kinematics have been evaluated. Only calls - :meth:`~isaaclab_newton.physics.NewtonManager.sync_transforms_to_usd` when a Kit - (or other USD-based) visualizer is in use. When both sim and rendering backend - are Newton (or Rerun), the sync is skipped to avoid unnecessary slowdown. - """ - if not self._needs_usd_sync: - return - try: - from isaaclab_newton.physics import NewtonManager - - NewtonManager.sync_transforms_to_usd() - except Exception: - pass - - def get_newton_model(self) -> Any | None: - """Return Newton model from ``NewtonManager``. - - Returns: - Newton model object, or ``None`` when unavailable. - """ - from isaaclab_newton.physics import NewtonManager - - return NewtonManager.get_model() - - def get_newton_state(self) -> Any | None: - """Return Newton state from NewtonManager. - - Returns: - The current Newton state (state_0) from NewtonManager. - """ - from isaaclab_newton.physics import NewtonManager - - return NewtonManager.get_state_0() - - def get_model(self) -> Any | None: - """Alias for :meth:`get_newton_model` for visualizer compatibility. - - Returns: - Newton model object, or ``None`` when unavailable. - """ - return self.get_newton_model() - - def get_state(self) -> Any | None: - """Alias for :meth:`get_newton_state` for visualizer compatibility.""" - return self.get_newton_state() - - def get_usd_stage(self) -> Any | None: - """Return the USD stage handle. - - Returns: - USD stage object, or ``None`` when unavailable. - """ - if self._stage is not None: - return self._stage - return getattr(self._simulation_context, "stage", None) - - def get_metadata(self) -> dict[str, Any]: - """Return provider metadata. - - Returns: - Metadata dictionary with backend and synchronization information. - """ - out = dict(self._metadata) - out["num_envs"] = self.get_num_envs() - out["needs_usd_sync"] = self._needs_usd_sync - return out - - def get_transforms(self) -> dict[str, Any] | None: - """Return body transforms from Newton state. - - Reads body_q from the authoritative Newton state and splits it into - positions (vec3) and orientations (quaternion xyzw). - - Returns: - Dictionary containing positions and orientations, or ``None`` when unavailable. - """ - try: - import warp as wp - - from isaaclab_newton.physics import NewtonManager - - state = NewtonManager.get_state_0() - if state is None or state.body_q is None: - return None - - body_q_t = wp.to_torch(state.body_q) - positions = body_q_t[:, :3] - orientations = body_q_t[:, 3:7] - return {"positions": positions, "orientations": orientations} - except Exception as exc: - self._warn_once( - "get-transforms-failed", - "[NewtonSceneDataProvider] get_transforms() failed: %s", - exc, - ) - return None - - def get_velocities(self) -> dict[str, Any] | None: - """Return body velocities from Newton state. - - Returns: - Dictionary containing linear and angular velocities, or ``None`` when unavailable. - """ - try: - import warp as wp - - from isaaclab_newton.physics import NewtonManager - - state = NewtonManager.get_state_0() - if state is None: - return None - - body_qd = getattr(state, "body_qd", None) - if body_qd is None: - return None - - body_qd_t = wp.to_torch(body_qd) - linear = body_qd_t[:, :3] - angular = body_qd_t[:, 3:6] - return {"linear": linear, "angular": angular, "source": "newton"} - except Exception as exc: - self._warn_once( - "get-velocities-failed", - "[NewtonSceneDataProvider] get_velocities() failed: %s", - exc, - ) - return None - - def get_contacts(self) -> dict[str, Any] | None: - """Return contact data for Newton provider. - - Returns: - ``None`` because contacts are not currently implemented in this provider. - """ - return None - - def get_camera_transforms(self) -> dict[str, Any] | None: - """Return per-camera, per-environment transforms. - - Returns: - Dictionary containing camera order, positions, orientations, and environment count, - or ``None`` when unavailable. - """ - if self._stage is None: - return None - - import isaaclab.sim as isaaclab_sim - - env_pattern = re.compile(r"(?P/World/envs/env_)(?P\d+)(?P/.*)") - shared_paths: list[str] = [] - instances: dict[str, list[tuple[int, str]]] = {} - num_envs = -1 - - stage_prims = deque([self._stage.GetPseudoRoot()]) - while stage_prims: - prim = stage_prims.popleft() - prim_path = prim.GetPath().pathString - - world_id = 0 - template_path = prim_path - if match := env_pattern.match(prim_path): - world_id = int(match.group("id")) - template_path = match.group("root") + "%d" + match.group("path") - if world_id > num_envs: - num_envs = world_id - - imageable = UsdGeom.Imageable(prim) - if imageable and imageable.ComputeVisibility() == UsdGeom.Tokens.invisible: - continue - - if prim.IsA(UsdGeom.Camera): - if template_path not in instances: - instances[template_path] = [] - instances[template_path].append((world_id, prim_path)) - if template_path not in shared_paths: - shared_paths.append(template_path) - - if hasattr(UsdGeom, "TraverseInstanceProxies"): - child_prims = prim.GetFilteredChildren(UsdGeom.TraverseInstanceProxies()) - else: - child_prims = prim.GetChildren() - if child_prims: - stage_prims.extend(child_prims) - - num_envs += 1 - positions: list[list[list[float] | None]] = [] - orientations: list[list[list[float] | None]] = [] - - for template_path in shared_paths: - per_world_pos: list[list[float] | None] = [None] * num_envs - per_world_ori: list[list[float] | None] = [None] * num_envs - for world_id, prim_path in instances.get(template_path, []): - if world_id < 0 or world_id >= num_envs: - continue - prim = self._stage.GetPrimAtPath(prim_path) - if not prim.IsValid(): - continue - pos, ori = isaaclab_sim.resolve_prim_pose(prim) - per_world_pos[world_id] = [float(pos[0]), float(pos[1]), float(pos[2])] - per_world_ori[world_id] = [float(ori[0]), float(ori[1]), float(ori[2]), float(ori[3])] - positions.append(per_world_pos) - orientations.append(per_world_ori) - - return {"order": shared_paths, "positions": positions, "orientations": orientations, "num_envs": num_envs} diff --git a/source/isaaclab_newton/isaaclab_newton/sensors/contact_sensor/contact_sensor.py b/source/isaaclab_newton/isaaclab_newton/sensors/contact_sensor/contact_sensor.py index 02850e9cc903..65d15de98750 100644 --- a/source/isaaclab_newton/isaaclab_newton/sensors/contact_sensor/contact_sensor.py +++ b/source/isaaclab_newton/isaaclab_newton/sensors/contact_sensor/contact_sensor.py @@ -322,17 +322,17 @@ def _initialize_impl(self): raise RuntimeError(self._init_error) from err def _create_buffers(self): - # Get Newton sensor shape: (n_sensors * n_envs, n_counterparts) - newton_shape = self.contact_view.shape + # Get Newton sensor count from total force: (n_sensors * n_envs) + total_sensor_count = self.contact_view.total_force.shape[0] # resolve the true count of sensors - self._num_sensors = newton_shape[0] // self._num_envs + self._num_sensors = total_sensor_count // self._num_envs # Check that number of sensors is an integer - if newton_shape[0] % self._num_envs != 0: + if total_sensor_count % self._num_envs != 0: raise RuntimeError( "Number of sensors is not an integer multiple of the number of environments. Received:" - f" {newton_shape[0]} sensors across {self._num_envs} environments." + f" {total_sensor_count} sensors across {self._num_envs} environments." ) if self._num_sensors == 0: raise RuntimeError( @@ -346,29 +346,47 @@ def _create_buffers(self): body_labels = self._get_model_labels("body") shape_labels = self._get_model_labels("shape") - def get_name(idx, kind): - kind_name = getattr(kind, "name", None) - kind_value = getattr(kind, "value", kind) - if kind_name == "BODY" or kind_value == 2: - return body_labels[idx].split("/")[-1] - if kind_name == "SHAPE" or kind_value == 1: - return shape_labels[idx].split("/")[-1] - return "MATCH_ANY" - - flat_sensing = [obj for world_objs in self.contact_view.sensing_objs for obj in world_objs] - self._sensor_names = [get_name(idx, kind) for idx, kind in flat_sensing] + s_kind = self.contact_view.sensing_obj_type + if s_kind == "body": + s_labels = body_labels + elif s_kind == "shape": + s_labels = shape_labels + else: + raise RuntimeError(f"Unexpected Newton sensing_obj_type {s_kind!r}; expected 'body' or 'shape'.") + self._sensor_names = [s_labels[i].split("/")[-1] for i in self.contact_view.sensing_obj_idx] # Assumes the environments are processed in order. self._sensor_names = self._sensor_names[: self._num_sensors] - flat_counterparts = [obj for world_objs in self.contact_view.counterparts for obj in world_objs] - self._filter_object_names = [get_name(idx, kind) for idx, kind in flat_counterparts] - - # Number of filter objects (counterparts minus the total column) - self._num_filter_objects = max(newton_shape[1] - 1, 0) - # Store reshaped Newton net_force view for copying data - # Newton net_force shape: (n_sensors * n_envs, n_counterparts) - # Reshaped to: (n_envs, n_sensors, n_counterparts) - self._newton_forces_view = self.contact_view.net_force.reshape((self._num_envs, self._num_sensors, -1)) + c_kind = self.contact_view.counterpart_type + c_idx_per_sensor = self.contact_view.counterpart_indices + if c_kind is None: + if self._generate_force_matrix: + raise RuntimeError("Filter expressions were configured but Newton reports no counterpart type.") + self._filter_object_names = [] + else: + if c_kind == "body": + c_labels = body_labels + elif c_kind == "shape": + c_labels = shape_labels + else: + raise RuntimeError(f"Unexpected Newton counterpart_type {c_kind!r}; expected 'body' or 'shape'.") + # Envs are homogeneous: every sensor row sees the same counterpart list. Take row 0. + row0 = c_idx_per_sensor[0] if c_idx_per_sensor else [] + self._filter_object_names = [c_labels[i].split("/")[-1] for i in row0] + if self._generate_force_matrix and not self._filter_object_names: + logger.warning("Filter expressions matched zero counterpart objects; force matrix will be empty.") + + force_matrix = self.contact_view.force_matrix + force_matrix_shape = force_matrix.shape if force_matrix is not None else (total_sensor_count, 0) + # Number of filter objects. + self._num_filter_objects = force_matrix_shape[1] if len(force_matrix_shape) > 1 else 0 + if self._num_filter_objects > 0 and force_matrix is None: + raise RuntimeError("Filter counterparts present but Newton force_matrix is None.") + + # Store flat Newton force views for copying data. These may be non-contiguous + # views, so the copy kernel indexes them without reshaping. + self._newton_total_force_view = self.contact_view.total_force + self._newton_force_matrix_view = force_matrix if self._num_filter_objects > 0 else None # prepare data buffers logger.info( @@ -413,7 +431,9 @@ def _update_buffers_impl(self, env_mask: wp.array): dim=(self._num_envs, self._num_sensors, max(self._num_filter_objects, 1)), inputs=[ env_mask, - self._newton_forces_view, + self._num_sensors, + self._newton_total_force_view, + self._newton_force_matrix_view, ], outputs=[ self._data._net_forces_w, diff --git a/source/isaaclab_newton/isaaclab_newton/sensors/contact_sensor/contact_sensor_kernels.py b/source/isaaclab_newton/isaaclab_newton/sensors/contact_sensor/contact_sensor_kernels.py index 324f0106682f..81fef0bcab63 100644 --- a/source/isaaclab_newton/isaaclab_newton/sensors/contact_sensor/contact_sensor_kernels.py +++ b/source/isaaclab_newton/isaaclab_newton/sensors/contact_sensor/contact_sensor_kernels.py @@ -13,7 +13,9 @@ def copy_from_newton_kernel( # in env_mask: wp.array(dtype=wp.bool), - newton_forces: wp.array3d(dtype=wp.vec3f), # (n_envs, n_sensors, n_counterparts) + num_sensors: int, + newton_total_force: wp.array(dtype=wp.vec3f), # (n_envs * n_sensors) + newton_force_matrix: wp.array2d(dtype=wp.vec3f), # (n_envs * n_sensors, n_filter_objects) or None # outputs net_force_total: wp.array2d(dtype=wp.vec3f), # (n_envs, n_sensors) force_matrix: wp.array3d(dtype=wp.vec3f), # (n_envs, n_sensors, n_filter_objects) or None @@ -30,13 +32,14 @@ def copy_from_newton_kernel( return # Copy total force (column 0) - only thread with f_idx == 0 does this + src_idx = env * num_sensors + sensor if f_idx == 0: - net_force_total[env, sensor] = newton_forces[env, sensor, 0] + net_force_total[env, sensor] = newton_total_force[src_idx] - # Copy per-filter-object forces (columns 1+) + # Copy per-filter-object forces. # Guard with `if force_matrix:` to handle None case (no filter objects) if force_matrix: - force_matrix[env, sensor, f_idx] = newton_forces[env, sensor, f_idx + 1] + force_matrix[env, sensor, f_idx] = newton_force_matrix[src_idx, f_idx] @wp.kernel diff --git a/source/isaaclab_newton/isaaclab_newton/sensors/pva/pva.py b/source/isaaclab_newton/isaaclab_newton/sensors/pva/pva.py index c000e17c437a..437140accdf6 100644 --- a/source/isaaclab_newton/isaaclab_newton/sensors/pva/pva.py +++ b/source/isaaclab_newton/isaaclab_newton/sensors/pva/pva.py @@ -214,21 +214,28 @@ def _debug_vis_callback(self, event): # arrow scale default_scale = self.acceleration_visualizer.cfg.markers["arrow"].scale arrow_scale = torch.tensor(default_scale, device=self.device).repeat(self._data.lin_acc_b.torch.shape[0], 1) - # arrow direction from acceleration + # arrow direction from acceleration; filter out bodies with effectively zero accel (no defined direction) up_axis = UsdGeom.GetStageUpAxis(self.stage) pos_w_torch = self._data.pos_w.torch - quat_w_torch = self._data.quat_w.torch - lin_acc_b_torch = self._data.lin_acc_b.torch - quat_opengl = math_utils.quat_from_matrix( - math_utils.create_rotation_matrix_from_view( - pos_w_torch, - pos_w_torch + math_utils.quat_apply(quat_w_torch, lin_acc_b_torch), - up_axis=up_axis, - device=self._device, - ) + accel_w = math_utils.quat_apply(self._data.quat_w.torch, self._data.lin_acc_b.torch) + valid_indices = (torch.linalg.norm(accel_w, dim=-1) > 1e-5).nonzero(as_tuple=True)[0] + if valid_indices.numel() == 0: + return + pos_filtered = pos_w_torch.index_select(0, valid_indices) + accel_filtered = accel_w.index_select(0, valid_indices) + rotation_matrix = math_utils.create_rotation_matrix_from_view( + pos_filtered, + pos_filtered + accel_filtered, + up_axis=up_axis, + device=self._device, ) + quat_opengl = math_utils.quat_from_matrix(rotation_matrix) quat_w = math_utils.convert_camera_frame_orientation_convention(quat_opengl, "opengl", "world") - self.acceleration_visualizer.visualize(base_pos_w, quat_w, arrow_scale) + self.acceleration_visualizer.visualize( + base_pos_w.index_select(0, valid_indices), + quat_w, + arrow_scale.index_select(0, valid_indices), + ) def _invalidate_initialize_callback(self, event): """Clears references for re-initialization and re-registers with NewtonManager.""" diff --git a/source/isaaclab_newton/isaaclab_newton/sim/schemas/__init__.py b/source/isaaclab_newton/isaaclab_newton/sim/schemas/__init__.py new file mode 100644 index 000000000000..80f943ad46ee --- /dev/null +++ b/source/isaaclab_newton/isaaclab_newton/sim/schemas/__init__.py @@ -0,0 +1,17 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Newton and MuJoCo simulation schema configuration classes.""" + +from .schemas_cfg import ( + MujocoJointDrivePropertiesCfg, + MujocoRigidBodyPropertiesCfg, + NewtonArticulationRootPropertiesCfg, + NewtonCollisionPropertiesCfg, + NewtonJointDrivePropertiesCfg, + NewtonMaterialPropertiesCfg, + NewtonMeshCollisionPropertiesCfg, + NewtonRigidBodyPropertiesCfg, +) diff --git a/source/isaaclab_newton/isaaclab_newton/sim/schemas/__init__.pyi b/source/isaaclab_newton/isaaclab_newton/sim/schemas/__init__.pyi new file mode 100644 index 000000000000..6dae7b8cf2b7 --- /dev/null +++ b/source/isaaclab_newton/isaaclab_newton/sim/schemas/__init__.pyi @@ -0,0 +1,15 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from .schemas_cfg import ( + MujocoJointDrivePropertiesCfg, + MujocoRigidBodyPropertiesCfg, + NewtonArticulationRootPropertiesCfg, + NewtonCollisionPropertiesCfg, + NewtonJointDrivePropertiesCfg, + NewtonMaterialPropertiesCfg, + NewtonMeshCollisionPropertiesCfg, + NewtonRigidBodyPropertiesCfg, +) diff --git a/source/isaaclab_newton/isaaclab_newton/sim/schemas/schemas_cfg.py b/source/isaaclab_newton/isaaclab_newton/sim/schemas/schemas_cfg.py new file mode 100644 index 000000000000..f0065daa867f --- /dev/null +++ b/source/isaaclab_newton/isaaclab_newton/sim/schemas/schemas_cfg.py @@ -0,0 +1,230 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +from typing import ClassVar + +from isaaclab.sim.schemas.schemas_cfg import ( + ArticulationRootBaseCfg, + CollisionBaseCfg, + JointDriveBaseCfg, + MeshCollisionBaseCfg, + RigidBodyBaseCfg, +) +from isaaclab.sim.spawners.materials.physics_materials_cfg import RigidBodyMaterialBaseCfg +from isaaclab.utils import configclass + + +@configclass +class NewtonRigidBodyPropertiesCfg(RigidBodyBaseCfg): + """Newton-targeted rigid body properties. + + Base class for cfgs that author rigid-body attributes consumed by any of + Newton's solver options (MuJoCo, XPBD, Featherstone, Semi-implicit, Kamino). + Newton has no native ``newton:*`` rigid-body attributes today, so this class + is currently empty — solver-specific subclasses (e.g., + :class:`MujocoRigidBodyPropertiesCfg`) carry the actual fields. + + The ``newton:`` namespace is reserved here so future Newton-native + rigid-body fields can be added without an API change. + + See :meth:`~isaaclab.sim.schemas.modify_rigid_body_properties` for more information. + """ + + _usd_namespace: ClassVar[str | None] = "newton" + _usd_applied_schema: ClassVar[str | None] = None + _usd_field_exceptions: ClassVar[dict] = {} + + +@configclass +class MujocoRigidBodyPropertiesCfg(NewtonRigidBodyPropertiesCfg): + """MuJoCo-solver-specific rigid body properties. + + Extends :class:`NewtonRigidBodyPropertiesCfg` with body-level gravity + compensation, consumed only when running Newton's MuJoCo solver. + + See :meth:`~isaaclab.sim.schemas.modify_rigid_body_properties` for more information. + + .. note:: + If the values are None, they are not modified. + """ + + _usd_namespace: ClassVar[str | None] = "mjc" + _usd_applied_schema: ClassVar[str | None] = None + _usd_field_exceptions: ClassVar[dict] = {} + + gravcomp: float | None = None + """Gravity compensation scale for the body [dimensionless]. + + ``0.0`` = no compensation; ``1.0`` = full compensation. + Written to ``mjc:gravcomp`` on the rigid-body prim. + Body-level gravcomp must be set for joint-level actuatorgravcomp to have any effect. + """ + + +@configclass +class NewtonJointDrivePropertiesCfg(JointDriveBaseCfg): + """Newton-targeted joint drive properties. + + Base class for cfgs that author joint-drive attributes consumed by any of + Newton's solver options. Newton has no native ``newton:*`` joint-drive + attributes today, so this class is currently empty — solver-specific + subclasses (e.g., :class:`MujocoJointDrivePropertiesCfg`) carry the actual + fields. + + The ``newton:`` namespace is reserved here so future Newton-native + joint-drive fields can be added without an API change. + + See :meth:`~isaaclab.sim.schemas.modify_joint_drive_properties` for more information. + """ + + _usd_namespace: ClassVar[str | None] = "newton" + _usd_applied_schema: ClassVar[str | None] = None + _usd_field_exceptions: ClassVar[dict] = {} + + +@configclass +class MujocoJointDrivePropertiesCfg(NewtonJointDrivePropertiesCfg): + """MuJoCo-solver-specific joint drive properties. + + Extends :class:`NewtonJointDrivePropertiesCfg` with joint-level gravity + compensation routing, consumed only when running Newton's MuJoCo solver. + + See :meth:`~isaaclab.sim.schemas.modify_joint_drive_properties` for more information. + + .. note:: + If the values are None, they are not modified. + """ + + _usd_namespace: ClassVar[str | None] = "mjc" + _usd_applied_schema: ClassVar[str | None] = "MjcJointAPI" + _usd_field_exceptions: ClassVar[dict] = {} + + actuatorgravcomp: bool | None = None + """Route gravity compensation forces through the actuator channel. + + When ``True``, compensation forces go to ``qfrc_actuator`` (subject to force limits). + Requires body-level :attr:`MujocoRigidBodyPropertiesCfg.gravcomp`. + Written to ``mjc:actuatorgravcomp`` via ``MjcJointAPI``. + """ + + +@configclass +class NewtonCollisionPropertiesCfg(CollisionBaseCfg): + """Newton-specific collision properties. + + Extends :class:`~isaaclab.sim.schemas.CollisionBaseCfg` with Newton-native + contact geometry attributes. + + See :meth:`~isaaclab.sim.schemas.modify_collision_properties` for more information. + + .. note:: + If the values are None, they are not modified. + """ + + _usd_namespace: ClassVar[str | None] = "newton" + _usd_applied_schema: ClassVar[str | None] = "NewtonCollisionAPI" + _usd_field_exceptions: ClassVar[dict] = {} + + contact_margin: float | None = None + """Outward inflation of the collision surface [m]. + + Extends the effective collision surface outward. Sum of both bodies' margins is + used for collision detection. Essential for thin shells and cloth. + Written to ``newton:contactMargin`` via ``NewtonCollisionAPI``. + Range: [0, inf). + """ + + contact_gap: float | None = None + """Additional contact detection gap [m]. + + AABBs are expanded by this value; contacts detected earlier to avoid tunneling. + Written to ``newton:contactGap`` via ``NewtonCollisionAPI``. + Set to ``-inf`` to use Newton's builder default. Range: [0, inf). + """ + + +@configclass +class NewtonMeshCollisionPropertiesCfg(NewtonCollisionPropertiesCfg, MeshCollisionBaseCfg): + """Newton-specific mesh collision properties. + + Extends :class:`NewtonCollisionPropertiesCfg` with convex-hull vertex limit. + + See :meth:`~isaaclab.sim.schemas.modify_mesh_collision_properties` for more information. + + .. note:: + If the values are None, they are not modified. + """ + + _usd_namespace: ClassVar[str | None] = "newton" + _usd_applied_schema: ClassVar[str | None] = "NewtonMeshCollisionAPI" + _usd_field_exceptions: ClassVar[dict] = {} + + max_hull_vertices: int | None = None + """Maximum vertices in the convex hull approximation [dimensionless]. + + Only relevant when ``physics:approximation = "convexHull"``. + Written to ``newton:maxHullVertices`` via ``NewtonMeshCollisionAPI``. + Set to ``-1`` to use as many vertices as needed for a perfect hull. + """ + + +@configclass +class NewtonMaterialPropertiesCfg(RigidBodyMaterialBaseCfg): + """Newton-specific rigid body material properties. + + Extends :class:`~isaaclab.sim.spawners.materials.RigidBodyMaterialBaseCfg` + with Newton-native friction attributes. + + See :meth:`~isaaclab.sim.spawners.materials.spawn_rigid_body_material` for more information. + + .. note:: + If the values are None, they are not modified. + """ + + _usd_namespace: ClassVar[str | None] = "newton" + _usd_applied_schema: ClassVar[str | None] = "NewtonMaterialAPI" + _usd_field_exceptions: ClassVar[dict] = {} + + torsional_friction: float | None = None + """Torsional friction coefficient (resistance to spinning at a contact point) [dimensionless]. + + Written to ``newton:torsionalFriction`` via ``NewtonMaterialAPI``. + Range: [0, inf). + """ + + rolling_friction: float | None = None + """Rolling friction coefficient (resistance to rolling motion) [dimensionless]. + + Written to ``newton:rollingFriction`` via ``NewtonMaterialAPI``. + Range: [0, inf). + """ + + +@configclass +class NewtonArticulationRootPropertiesCfg(ArticulationRootBaseCfg): + """Newton-specific articulation root properties. + + Extends :class:`~isaaclab.sim.schemas.ArticulationRootBaseCfg` with + Newton-native self-collision control. + + See :meth:`~isaaclab.sim.schemas.modify_articulation_root_properties` for more information. + + .. note:: + If the values are None, they are not modified. + """ + + _usd_namespace: ClassVar[str | None] = "newton" + _usd_applied_schema: ClassVar[str | None] = "NewtonArticulationRootAPI" + _usd_field_exceptions: ClassVar[dict] = {} + + self_collision_enabled: bool | None = None + """Whether self-collisions between bodies in this articulation are enabled. + + Written to ``newton:selfCollisionEnabled`` via ``NewtonArticulationRootAPI``. + Newton's resolver checks this native attribute first before falling back to + ``physxArticulation:enabledSelfCollisions``. + """ diff --git a/source/isaaclab_newton/isaaclab_newton/video_recording/newton_gl_perspective_video.py b/source/isaaclab_newton/isaaclab_newton/video_recording/newton_gl_perspective_video.py index 9c440a657981..1f557300529b 100644 --- a/source/isaaclab_newton/isaaclab_newton/video_recording/newton_gl_perspective_video.py +++ b/source/isaaclab_newton/isaaclab_newton/video_recording/newton_gl_perspective_video.py @@ -31,13 +31,12 @@ def _ensure_viewer(self) -> None: if self._init_attempted: return self._init_attempted = True - from isaaclab.sim import SimulationContext + from isaaclab_newton.physics import NewtonManager - sdp = SimulationContext.instance().initialize_scene_data_provider() - model = sdp.get_newton_model() + model = NewtonManager.get_model() if model is None: raise RuntimeError( - "Newton GL perspective video requires a Newton model on the scene data provider. " + "Newton GL perspective video requires a Newton model from NewtonManager. " "Do not use --video for this setup." ) @@ -105,9 +104,10 @@ def render_rgb_array(self) -> np.ndarray: self._ensure_viewer() from isaaclab.sim import SimulationContext + from isaaclab_newton.physics import NewtonManager + sim = SimulationContext.instance() - sdp = sim.initialize_scene_data_provider() - state = sdp.get_newton_state() + state = NewtonManager.get_state() dt = sim.get_physics_dt() viewer = self._viewer diff --git a/source/isaaclab_newton/setup.py b/source/isaaclab_newton/setup.py index 2e0b87f17543..4c4a43633b9f 100644 --- a/source/isaaclab_newton/setup.py +++ b/source/isaaclab_newton/setup.py @@ -38,10 +38,8 @@ def run(self): EXTRAS_REQUIRE = { "all": [ "prettytable==3.3.0", - "mujoco==3.6.0", - "mujoco-warp==3.6.0", "PyOpenGL-accelerate==3.1.10", - "newton @ git+https://github.com/newton-physics/newton.git@a27277ed49d6f307b8a1e4c394be7e1d14965a62", + "newton[sim] @ git+https://github.com/newton-physics/newton.git@v1.2.0rc2", ], } @@ -69,7 +67,6 @@ def run(self): "isaaclab_newton.cloner", "isaaclab_newton.physics", "isaaclab_newton.renderers", - "isaaclab_newton.scene_data_providers", "isaaclab_newton.sensors", "isaaclab_newton.sensors.contact_sensor", "isaaclab_newton.sensors.frame_transformer", diff --git a/source/isaaclab_newton/test/assets/test_articulation.py b/source/isaaclab_newton/test/assets/test_articulation.py index a5eacf95045e..5202da63418b 100644 --- a/source/isaaclab_newton/test/assets/test_articulation.py +++ b/source/isaaclab_newton/test/assets/test_articulation.py @@ -18,6 +18,7 @@ """Rest everything follows.""" import sys +from copy import deepcopy import pytest import torch @@ -32,16 +33,23 @@ import isaaclab.utils.string as string_utils from isaaclab.actuators import ActuatorBase, IdealPDActuatorCfg, ImplicitActuatorCfg from isaaclab.assets import ArticulationCfg +from isaaclab.controllers import ( + DifferentialIKController, + DifferentialIKControllerCfg, + OperationalSpaceController, + OperationalSpaceControllerCfg, +) from isaaclab.envs.mdp.terminations import joint_effort_out_of_limit from isaaclab.managers import SceneEntityCfg from isaaclab.sim import SimulationCfg, build_simulation_context from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR +from isaaclab.utils.math import compute_pose_error, matrix_from_quat, quat_inv, subtract_frame_transforms from isaaclab.utils.version import get_isaac_sim_version, has_kit ## # Pre-defined configs ## -from isaaclab_assets import ANYMAL_C_CFG, FRANKA_PANDA_CFG # isort:skip +from isaaclab_assets import ANYMAL_C_CFG, FRANKA_PANDA_CFG, FRANKA_PANDA_HIGH_PD_CFG # isort:skip # , SHADOW_HAND_CFG # isort:skip SIM_CFGs = { @@ -196,7 +204,7 @@ def generate_articulation_cfg( # we set 80.0 default for max force because default in USD is 10e10 which makes testing annoying. spawn=sim_utils.UsdFileCfg( usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/IsaacSim/SimpleArticulation/revolute_articulation.usd", - joint_drive_props=sim_utils.JointDrivePropertiesCfg(max_effort=80.0, max_velocity=5.0), + joint_drive_props=sim_utils.JointDrivePropertiesCfg(max_force=80.0, max_joint_velocity=5.0), ), actuators={ "joint": ImplicitActuatorCfg( @@ -220,7 +228,7 @@ def generate_articulation_cfg( articulation_cfg = ArticulationCfg( spawn=sim_utils.UsdFileCfg( usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/IsaacSim/SimpleArticulation/revolute_articulation.usd", - joint_drive_props=sim_utils.JointDrivePropertiesCfg(max_effort=80.0, max_velocity=5.0), + joint_drive_props=sim_utils.JointDrivePropertiesCfg(max_force=80.0, max_joint_velocity=5.0), ), actuators={ "joint": IdealPDActuatorCfg( @@ -353,6 +361,98 @@ def generate_articulation( return articulation, translations +# --------------------------------------------------------------------------- +# Franka task-space tracking helpers (shared between IK and OSC tests). +# --------------------------------------------------------------------------- + + +def _setup_franka_at_home_pose(sim, *, zero_actuator_pd: bool = False): + """Build a Franka articulation at its configured home pose. + + Constructs :data:`FRANKA_PANDA_HIGH_PD_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 + robot would otherwise sit at the URDF-neutral pose where the + Franka wrist is near-singular). + + 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 + so OSC's joint-effort output is not opposed by the + implicit-PD's residual ``kp·(target − q)``. + + Returns: + Tuple of ``(robot, ee_frame_idx, ee_jacobi_idx, arm_joint_ids)``. + """ + cfg = FRANKA_PANDA_HIGH_PD_CFG.copy().replace(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 + sim_utils.create_prim("/World/Env_0", "Xform", translation=(0.0, 0.0, 0.0)) + robot = Articulation(cfg) + sim.reset() + assert robot.is_initialized + + ee_frame_idx = robot.find_bodies("panda_hand")[0][0] + ee_jacobi_idx = ee_frame_idx - 1 + arm_joint_ids = robot.find_joints(["panda_joint.*"])[0] + + robot.write_joint_position_to_sim_index(position=robot.data.default_joint_pos.torch[:, :].clone()) + robot.write_joint_velocity_to_sim_index(velocity=robot.data.default_joint_vel.torch[:, :].clone()) + return robot, ee_frame_idx, ee_jacobi_idx, arm_joint_ids + + +def _compute_ee_pose_root(robot, ee_frame_idx): + """Return ``(ee_pos_b, ee_quat_b, root_pose_w)`` in the root frame.""" + ee_pose_w = robot.data.body_pose_w.torch[:, ee_frame_idx] + root_pose_w = robot.data.root_pose_w.torch + ee_pos_b, ee_quat_b = subtract_frame_transforms( + root_pose_w[:, 0:3], root_pose_w[:, 3:7], ee_pose_w[:, 0:3], ee_pose_w[:, 3:7] + ) + return ee_pos_b, ee_quat_b, root_pose_w + + +def _compute_jacobian_root_frame(robot, ee_jacobi_idx, arm_joint_ids): + """Return the EE Jacobian sliced to ``arm_joint_ids`` and rotated to the root frame.""" + jacobian = robot.data.body_link_jacobian_w.torch[:, ee_jacobi_idx, :, arm_joint_ids] + base_rot_matrix = matrix_from_quat(quat_inv(robot.data.root_pose_w.torch[:, 3:7])) + jacobian[:, :3, :] = torch.bmm(base_rot_matrix, jacobian[:, :3, :]) + jacobian[:, 3:, :] = torch.bmm(base_rot_matrix, jacobian[:, 3:, :]) + return jacobian + + +def _compute_ee_vel_root(jacobian_b, joint_vel): + """Return the EE 6D velocity in the root frame as ``J · q_dot``. + + Required to make OSC's ``kd * ee_vel_b`` damping term meaningful. + Passing zero EE velocity (the convenient hack) leaves the impedance + undamped and the EE oscillates around the target. We use ``J · q_dot`` + rather than reading ``data.body_vel_w`` because Newton's lazy + velocity buffers can return stale/zero values until forced + materialization, while ``joint_vel`` and ``J`` are already pulled + by the loop. ``J`` correctness is pinned independently by + ``test_get_jacobians_link_origin_contract``. + """ + return torch.bmm(jacobian_b, joint_vel.unsqueeze(-1)).squeeze(-1) + + +def _build_relative_pose_target(robot, ee_frame_idx, delta_xyz, device): + """Build a target pose = (current EE pose) + ``delta_xyz``, preserving orientation.""" + initial_ee_pos_b, initial_ee_quat_b, _ = _compute_ee_pose_root(robot, ee_frame_idx) + target_pos_b = initial_ee_pos_b + torch.tensor([list(delta_xyz)], device=device, dtype=initial_ee_pos_b.dtype) + return torch.cat([target_pos_b, initial_ee_quat_b], dim=-1) + + +def _summarize_history(history, tail: int = 200): + """Return ``(min, mean)`` over the last ``tail`` samples.""" + tail_slice = history[-tail:] + return min(tail_slice), sum(tail_slice) / len(tail_slice) + + @pytest.fixture def sim(request): """Create simulation context with the specified device.""" @@ -366,8 +466,13 @@ def sim(request): else: add_ground_plane = False # default to no ground plane articulation_type = request.getfixturevalue("articulation_type") - sim_cfg = SIM_CFGs[articulation_type] + sim_cfg = deepcopy(SIM_CFGs[articulation_type]) sim_cfg.device = device + # ``gravity_enabled`` is silently ignored by ``build_simulation_context`` + # when an explicit ``sim_cfg`` is also passed; apply it here so the + # fixture honors what its parameter advertises. + if not gravity_enabled: + sim_cfg.gravity = (0.0, 0.0, 0.0) with build_simulation_context( device=device, auto_add_lighting=True, @@ -379,7 +484,6 @@ def sim(request): yield sim -@pytest.mark.isaacsim_ci @pytest.mark.parametrize("num_articulations", [1, 2]) @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.parametrize("add_ground_plane", [True]) @@ -429,7 +533,6 @@ def test_initialization_floating_base_non_root(sim, num_articulations, device, a articulation.update(sim.cfg.dt) -@pytest.mark.isaacsim_ci @pytest.mark.parametrize("num_articulations", [1, 2]) @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.parametrize("add_ground_plane", [True]) @@ -480,7 +583,6 @@ def test_initialization_floating_base(sim, num_articulations, device, add_ground articulation.update(sim.cfg.dt) -@pytest.mark.isaacsim_ci @pytest.mark.parametrize("num_articulations", [1, 2]) @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.parametrize("articulation_type", ["panda"]) @@ -538,7 +640,6 @@ def test_initialization_fixed_base(sim, num_articulations, device, articulation_ torch.testing.assert_close(articulation.data.root_com_vel_w.torch, default_root_vel) -@pytest.mark.isaacsim_ci @pytest.mark.parametrize("num_articulations", [1, 2]) @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.parametrize("add_ground_plane", [True]) @@ -597,7 +698,6 @@ def test_initialization_fixed_base_single_joint(sim, num_articulations, device, torch.testing.assert_close(articulation.data.root_com_vel_w.torch, default_root_vel) -@pytest.mark.isaacsim_ci @pytest.mark.parametrize("num_articulations", [1, 2]) @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.parametrize("articulation_type", ["shadow_hand"]) @@ -647,7 +747,6 @@ def test_initialization_hand_with_tendons(sim, num_articulations, device, articu articulation.update(sim.cfg.dt) -@pytest.mark.isaacsim_ci @pytest.mark.parametrize("num_articulations", [1, 2]) @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.parametrize("add_ground_plane", [True]) @@ -702,7 +801,6 @@ def test_initialization_floating_base_made_fixed_base( torch.testing.assert_close(articulation.data.root_com_vel_w.torch, default_root_vel) -@pytest.mark.isaacsim_ci @pytest.mark.parametrize("num_articulations", [1, 2]) @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.parametrize("add_ground_plane", [True]) @@ -722,7 +820,7 @@ def test_initialization_fixed_base_made_floating_base( sim: The simulation fixture num_articulations: Number of articulations to test """ - articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) + articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type).copy() # Unfix root link by making it non-kinematic articulation_cfg.spawn.articulation_props.fix_root_link = False articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=sim.device) @@ -749,7 +847,6 @@ def test_initialization_fixed_base_made_floating_base( articulation.update(sim.cfg.dt) -@pytest.mark.isaacsim_ci @pytest.mark.parametrize("num_articulations", [1, 2]) @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.parametrize("add_ground_plane", [True]) @@ -782,7 +879,6 @@ def test_out_of_range_default_joint_pos(sim, num_articulations, device, add_grou sim.reset() -@pytest.mark.isaacsim_ci @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.parametrize("articulation_type", ["panda"]) def test_out_of_range_default_joint_vel(sim, device, articulation_type): @@ -807,7 +903,6 @@ def test_out_of_range_default_joint_vel(sim, device, articulation_type): sim.reset() -@pytest.mark.isaacsim_ci @pytest.mark.parametrize("num_articulations", [1, 2]) @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.parametrize("add_ground_plane", [True]) @@ -918,7 +1013,6 @@ def __init__(self, art): assert torch.all(out) -@pytest.mark.isaacsim_ci @pytest.mark.parametrize("num_articulations", [1, 2]) @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.parametrize("articulation_type", ["anymal"]) @@ -983,8 +1077,8 @@ def test_external_force_buffer(sim, num_articulations, device, articulation_type # check if the articulation's force and torque buffers are correctly updated for i in range(num_articulations): - assert articulation.permanent_wrench_composer.composed_force.torch[i, 0, 0].item() == force - assert articulation.permanent_wrench_composer.composed_torque.torch[i, 0, 0].item() == force + assert articulation.permanent_wrench_composer.out_force_b.torch[i, 0, 0].item() == force + assert articulation.permanent_wrench_composer.out_torque_b.torch[i, 0, 0].item() == force # Check if the instantaneous wrench is correctly added to the permanent wrench articulation.instantaneous_wrench_composer.add_forces_and_torques_index( @@ -1004,7 +1098,6 @@ def test_external_force_buffer(sim, num_articulations, device, articulation_type articulation.update(sim.cfg.dt) -@pytest.mark.isaacsim_ci @pytest.mark.parametrize("num_articulations", [1, 2]) @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.parametrize("articulation_type", ["anymal"]) @@ -1063,7 +1156,6 @@ def test_external_force_on_single_body(sim, num_articulations, device, articulat assert articulation.data.root_pos_w.torch[i, 2].item() < 0.2 -@pytest.mark.isaacsim_ci @pytest.mark.parametrize("num_articulations", [1, 2]) @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.parametrize("articulation_type", ["anymal"]) @@ -1159,7 +1251,6 @@ def test_external_force_on_single_body_at_position(sim, num_articulations, devic assert articulation.data.root_pos_w.torch[i, 2].item() < 0.2 -@pytest.mark.isaacsim_ci @pytest.mark.parametrize("num_articulations", [1, 2]) @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.parametrize("articulation_type", ["anymal"]) @@ -1220,7 +1311,6 @@ def test_external_force_on_multiple_bodies(sim, num_articulations, device, artic assert articulation.data.root_ang_vel_w.torch[i, 2].item() > 0.1 -@pytest.mark.isaacsim_ci @pytest.mark.parametrize("num_articulations", [1, 2]) @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.parametrize("articulation_type", ["anymal"]) @@ -1315,7 +1405,6 @@ def test_external_force_on_multiple_bodies_at_position(sim, num_articulations, d assert torch.abs(articulation.data.root_ang_vel_w.torch[i, 2]).item() > 0.1 -@pytest.mark.isaacsim_ci @pytest.mark.parametrize("num_articulations", [1, 2]) @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.parametrize("articulation_type", ["humanoid"]) @@ -1378,7 +1467,6 @@ def test_loading_gains_from_usd(sim, num_articulations, device, articulation_typ torch.testing.assert_close(articulation.actuators["body"].damping, expected_damping) -@pytest.mark.isaacsim_ci @pytest.mark.parametrize("num_articulations", [1, 2]) @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.parametrize("add_ground_plane", [True]) @@ -1414,7 +1502,6 @@ def test_setting_gains_from_cfg(sim, num_articulations, device, add_ground_plane torch.testing.assert_close(articulation.actuators["body"].damping, expected_damping) -@pytest.mark.isaacsim_ci @pytest.mark.parametrize("num_articulations", [1, 2]) @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.parametrize("articulation_type", ["humanoid"]) @@ -1448,7 +1535,6 @@ def test_setting_gains_from_cfg_dict(sim, num_articulations, device, articulatio torch.testing.assert_close(articulation.actuators["body"].damping, expected_damping) -@pytest.mark.isaacsim_ci @pytest.mark.parametrize("num_articulations", [1, 2]) @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.parametrize("vel_limit_sim", [1e5, None]) @@ -1510,7 +1596,7 @@ def test_setting_velocity_limit_implicit( # Case 3: velocity limit sim is not set but velocity limit is set # For backwards compatibility, we do not set velocity limit to simulation # Thus, both default to USD default value. - limit = articulation_cfg.spawn.joint_drive_props.max_velocity + limit = articulation_cfg.spawn.joint_drive_props.max_joint_velocity else: # Case 4: only velocity limit sim is set # In this case, the velocity limit is set to the USD value @@ -1521,7 +1607,6 @@ def test_setting_velocity_limit_implicit( torch.testing.assert_close(newton_vel_limit, expected_velocity_limit) -@pytest.mark.isaacsim_ci @pytest.mark.parametrize("num_articulations", [1, 2]) @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.parametrize("vel_limit_sim", [1e5, None]) @@ -1572,13 +1657,12 @@ def test_setting_velocity_limit_explicit(sim, num_articulations, device, vel_lim if vel_limit_sim is not None: limit = vel_limit_sim else: - limit = articulation_cfg.spawn.joint_drive_props.max_velocity + limit = articulation_cfg.spawn.joint_drive_props.max_joint_velocity # check physx is set to expected value expected_vel_limit = torch.full_like(newton_vel_limit, limit) torch.testing.assert_close(newton_vel_limit, expected_vel_limit) -@pytest.mark.isaacsim_ci @pytest.mark.parametrize("num_articulations", [1, 2]) @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.parametrize("effort_limit_sim", [1e5, None]) @@ -1625,7 +1709,7 @@ def test_setting_effort_limit_implicit( # decide the limit based on what is set if effort_limit_sim is None and effort_limit is None: - limit = articulation_cfg.spawn.joint_drive_props.max_effort + limit = articulation_cfg.spawn.joint_drive_props.max_force elif effort_limit_sim is not None and effort_limit is None: limit = effort_limit_sim elif effort_limit_sim is None and effort_limit is not None: @@ -1636,7 +1720,6 @@ def test_setting_effort_limit_implicit( torch.testing.assert_close(newton_effort_limit, expected_effort_limit) -@pytest.mark.isaacsim_ci @pytest.mark.parametrize("num_articulations", [1, 2]) @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.parametrize("effort_limit_sim", [1e5, None]) @@ -1703,7 +1786,6 @@ def test_setting_effort_limit_explicit( torch.testing.assert_close(newton_effort_limit, expected_effort_limit) -@pytest.mark.isaacsim_ci @pytest.mark.parametrize("num_articulations", [1, 2]) @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.parametrize("articulation_type", ["humanoid"]) @@ -1724,10 +1806,10 @@ def test_reset(sim, num_articulations, device, articulation_type): # Reset should zero external forces and torques assert not articulation._instantaneous_wrench_composer.active assert not articulation._permanent_wrench_composer.active - assert torch.count_nonzero(articulation._instantaneous_wrench_composer.composed_force.torch) == 0 - assert torch.count_nonzero(articulation._instantaneous_wrench_composer.composed_torque.torch) == 0 - assert torch.count_nonzero(articulation._permanent_wrench_composer.composed_force.torch) == 0 - assert torch.count_nonzero(articulation._permanent_wrench_composer.composed_torque.torch) == 0 + assert torch.count_nonzero(articulation._instantaneous_wrench_composer.out_force_b.torch) == 0 + assert torch.count_nonzero(articulation._instantaneous_wrench_composer.out_torque_b.torch) == 0 + assert torch.count_nonzero(articulation._permanent_wrench_composer.out_force_b.torch) == 0 + assert torch.count_nonzero(articulation._permanent_wrench_composer.out_torque_b.torch) == 0 if num_articulations > 1: num_bodies = articulation.num_bodies @@ -1742,13 +1824,12 @@ def test_reset(sim, num_articulations, device, articulation_type): articulation.reset(env_ids=torch.tensor([0], device=device)) assert articulation._instantaneous_wrench_composer.active assert articulation._permanent_wrench_composer.active - assert torch.count_nonzero(articulation._instantaneous_wrench_composer.composed_force.torch) == num_bodies * 3 - assert torch.count_nonzero(articulation._instantaneous_wrench_composer.composed_torque.torch) == num_bodies * 3 - assert torch.count_nonzero(articulation._permanent_wrench_composer.composed_force.torch) == num_bodies * 3 - assert torch.count_nonzero(articulation._permanent_wrench_composer.composed_torque.torch) == num_bodies * 3 + assert torch.count_nonzero(articulation._instantaneous_wrench_composer.out_force_b.torch) == num_bodies * 3 + assert torch.count_nonzero(articulation._instantaneous_wrench_composer.out_torque_b.torch) == num_bodies * 3 + assert torch.count_nonzero(articulation._permanent_wrench_composer.out_force_b.torch) == num_bodies * 3 + assert torch.count_nonzero(articulation._permanent_wrench_composer.out_torque_b.torch) == num_bodies * 3 -@pytest.mark.isaacsim_ci @pytest.mark.parametrize("num_articulations", [1, 2]) @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.parametrize("add_ground_plane", [True]) @@ -1789,7 +1870,6 @@ def test_apply_joint_command(sim, num_articulations, device, add_ground_plane, a assert not torch.allclose(articulation.data.joint_pos.torch, joint_pos) -@pytest.mark.isaacsim_ci @pytest.mark.parametrize("num_articulations", [1, 2]) @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.parametrize("with_offset", [True, False]) @@ -1915,7 +1995,6 @@ def test_body_root_state(sim, num_articulations, device, with_offset, articulati torch.testing.assert_close(body_com_vel_w, body_link_vel_w) -@pytest.mark.isaacsim_ci @pytest.mark.parametrize("num_articulations", [1, 2]) @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.parametrize("with_offset", [True, False]) @@ -2006,7 +2085,6 @@ def test_write_root_state( torch.testing.assert_close(rand_state[..., 7:], articulation.data.root_link_vel_w.torch) -@pytest.mark.isaacsim_ci @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.parametrize("articulation_type", ["humanoid"]) def test_setting_articulation_root_prim_path(sim, device, articulation_type): @@ -2026,7 +2104,6 @@ def test_setting_articulation_root_prim_path(sim, device, articulation_type): assert articulation._is_initialized -@pytest.mark.isaacsim_ci @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.parametrize("articulation_type", ["humanoid"]) def test_setting_invalid_articulation_root_prim_path(sim, device, articulation_type): @@ -2045,7 +2122,6 @@ def test_setting_invalid_articulation_root_prim_path(sim, device, articulation_t sim.reset() -@pytest.mark.isaacsim_ci @pytest.mark.parametrize("num_articulations", [1, 2]) @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.parametrize("gravity_enabled", [False]) @@ -2357,7 +2433,6 @@ def _patched_simulate(cls): ) -@pytest.mark.isaacsim_ci @pytest.mark.parametrize("add_ground_plane", [True]) @pytest.mark.parametrize("num_articulations", [1, 2]) @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @@ -2472,5 +2547,647 @@ def test_randomize_rigid_body_collider_offsets(sim, num_articulations, device, a torch.testing.assert_close(updated_gap, new_gap) +@pytest.mark.parametrize("num_articulations", [1]) +@pytest.mark.parametrize("device", ["cuda:0"]) +@pytest.mark.parametrize("articulation_type", ["panda"]) +@pytest.mark.isaacsim_ci +@pytest.mark.xfail( + strict=True, + raises=NotImplementedError, + reason=( + "Newton's ArticulationView exposes eval_fk / eval_jacobian /" + " eval_mass_matrix only — no inverse-dynamics primitive yet." + " Upstream Newton is actively working on this through the inverse-" + " dynamics feature request (https://github.com/newton-physics/newton/issues/2497)" + " and its sub-task for Coriolis + gravity compensation" + " (https://github.com/newton-physics/newton/issues/2529). A known" + " correctness bug for floating-base + non-identity root pose is" + " tracked separately at" + " https://github.com/newton-physics/newton/issues/2625, and" + " informs why we deliberately do NOT roll our own J^T·m·g shim in" + " this PR — Newton's eventual primitive is going through RNEA via" + " MuJoCo Warp and may differ at corner cases we wouldn't catch." + " Once the wrapper at" + " isaaclab_newton.assets.ArticulationData.gravity_compensation_forces" + " switches from a NotImplementedError stub to a real implementation" + " (likely calling the new Newton primitive), this XFAIL will turn" + " into XPASS and fail under strict=True. The maintainer should" + " then: (1) drop this xfail or invert it into a positive value" + " assertion against PhysX (the cross-backend accuracy diff), and" + " (2) remove the OSC config-time guidance about setting" + " gravity_compensation=False on Newton." + ), +) +def test_get_gravity_compensation_forces_not_implemented_on_newton(sim, num_articulations, device, articulation_type): + """Pin the known Newton gravity-compensation gap. + + See the ``xfail`` marker for full rationale. The body simply invokes the + wrapper and lets the strict-xfail marker handle the expected failure. + """ + articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) + articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) + sim.reset() + assert articulation.is_initialized + + _ = articulation.data.gravity_compensation_forces + + +## +# Shape-contract regression tests for the new BaseArticulation accessors. +# These pin the public shape contract so future regressions (e.g., reverting +# to model-wide max sizing or to the wrong fixed-base row offset) fail fast. +## + + +@pytest.mark.parametrize("num_articulations", [1, 4]) +@pytest.mark.parametrize("device", ["cuda:0"]) +@pytest.mark.parametrize("articulation_type", ["panda"]) +@pytest.mark.isaacsim_ci +def test_get_jacobians_shape_fixed_base(sim, num_articulations, device, articulation_type): + """Fixed-base ``body_link_jacobian_w`` must drop the fixed-root row. + + Contract: shape ``(N, num_bodies - 1, 6, num_joints)``. Catches + regressions of (a) the link_offset fix that drops Newton's row 0 for + fixed-base, and (b) the per-articulation output sizing — using + model-wide ``max_links`` here would over-allocate in heterogeneous + scenes. + """ + articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) + articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) + sim.reset() + assert articulation.is_initialized + assert articulation.is_fixed_base, "panda fixture must be fixed-base for this test" + + J = articulation.data.body_link_jacobian_w.torch + + expected_shape = (num_articulations, articulation.num_bodies - 1, 6, articulation.num_joints) + assert J.shape == torch.Size(expected_shape), f"expected {expected_shape}, got {tuple(J.shape)}" + assert J.dtype == torch.float32 + + +@pytest.mark.parametrize("num_articulations", [1, 4]) +@pytest.mark.parametrize("device", ["cuda:0"]) +@pytest.mark.parametrize("articulation_type", ["panda"]) +@pytest.mark.isaacsim_ci +def test_get_mass_matrix_shape_and_nonsingular_fixed_base(sim, num_articulations, device, articulation_type): + """Fixed-base ``mass_matrix`` shape + non-singularity. + + Contract: shape ``(N, num_joints, num_joints)`` and the matrix must be + non-singular. The non-singularity check catches the heterogeneous + padding bug — if the wrapper accidentally returns ``model.max_dofs`` + sized output, the padded zero rows/cols make the matrix rank-deficient. + """ + articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) + articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) + sim.reset() + assert articulation.is_initialized + + sim.step() + articulation.update(sim.cfg.dt) + + M = articulation.data.mass_matrix.torch + + expected_shape = (num_articulations, articulation.num_joints, articulation.num_joints) + assert M.shape == torch.Size(expected_shape), f"expected {expected_shape}, got {tuple(M.shape)}" + assert M.dtype == torch.float32 + + # Each diagonal entry is a joint's effective inertia and must be strictly + # positive for any physical articulation. Padded zero rows/cols (the + # heterogeneous bug) would surface as zero diagonal entries — much more + # sensitive than checking the determinant, which can be small purely from + # numerical conditioning of a well-formed 9x9 mass matrix (Franka det + # is ~1e-13 in practice). + diag = M.diagonal(dim1=-2, dim2=-1) + assert (diag > 1e-6).all(), f"mass matrix has non-positive diagonal entries: min={diag.min()}" + + +@pytest.mark.parametrize("num_articulations", [1, 4]) +@pytest.mark.parametrize("device", ["cuda:0"]) +@pytest.mark.parametrize("add_ground_plane", [True]) +@pytest.mark.parametrize("articulation_type", ["anymal"]) +@pytest.mark.isaacsim_ci +def test_get_jacobians_shape_floating_base(sim, num_articulations, device, add_ground_plane, articulation_type): + """Floating-base ``body_link_jacobian_w`` keeps every body row and prepends 6 base-DoF columns. + + Contract for floating-base: shape + ``(N, num_bodies, 6, num_joints + num_base_dofs)`` — no fixed-root row + to drop, and the leading 6 DoF columns are the floating-base spatial- + velocity columns Newton's ``eval_jacobian`` writes for the free root + joint. Matches the cross-library industry convention (Pinocchio, Drake, + MuJoCo, RBDL, OCS2, iDynTree). + """ + articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) + articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) + sim.reset() + assert articulation.is_initialized + assert not articulation.is_fixed_base, "anymal fixture must be floating-base for this test" + + J = articulation.data.body_link_jacobian_w.torch + + expected_shape = ( + num_articulations, + articulation.num_bodies, + 6, + articulation.num_joints + articulation.num_base_dofs, + ) + assert J.shape == torch.Size(expected_shape), f"expected {expected_shape}, got {tuple(J.shape)}" + assert J.dtype == torch.float32 + + +@pytest.mark.parametrize("num_articulations", [1, 4]) +@pytest.mark.parametrize("device", ["cuda:0"]) +@pytest.mark.parametrize("add_ground_plane", [True]) +@pytest.mark.parametrize("articulation_type", ["anymal"]) +@pytest.mark.isaacsim_ci +def test_get_mass_matrix_shape_floating_base(sim, num_articulations, device, add_ground_plane, articulation_type): + """Floating-base ``mass_matrix`` shape ``(N, num_joints + 6, num_joints + 6)``. + + Includes the 6 floating-base rows/cols on the DoF axis, matching the + cross-library industry convention. + """ + articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) + articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) + sim.reset() + assert articulation.is_initialized + + sim.step() + articulation.update(sim.cfg.dt) + + M = articulation.data.mass_matrix.torch + + expected_dofs = articulation.num_joints + articulation.num_base_dofs + expected_shape = (num_articulations, expected_dofs, expected_dofs) + assert M.shape == torch.Size(expected_shape), f"expected {expected_shape}, got {tuple(M.shape)}" + assert M.dtype == torch.float32 + + +@pytest.mark.parametrize("device", ["cuda:0"]) +@pytest.mark.parametrize("add_ground_plane", [True]) +@pytest.mark.parametrize("articulation_type", ["anymal"]) +@pytest.mark.isaacsim_ci +def test_heterogeneous_scene_per_view_shapes(sim, device, add_ground_plane, articulation_type): + """Mixed-articulation scene: each view returns ITS OWN asset's shape. + + Direct regression test for the Codex round-2 finding. With Franka + (9 DoFs) and Anymal-C (18 DoFs) co-resident in the model, + ``model.max_dofs_per_articulation == 18`` and + ``model.max_joints_per_articulation == anymal.num_bodies``. The Franka + view's ``body_link_jacobian_w`` / ``mass_matrix`` outputs must use + Franka's per-asset counts, NOT the model-wide maxima — otherwise + Franka's mass matrix would carry zero-padded rows/cols and be + singular. + + Uses the ``anymal`` ``SIM_CFGs`` entry (more capable solver settings) + for the host sim; the ``articulation_type`` parametrize is only there + so the ``sim`` fixture picks a config — the test itself constructs + both Anymal and Franka articulations directly. + """ + # ``num_per_type=1`` keeps the actuator-default replication path off — + # Newton's USD default loader hits a (1, num_joints) vs (num_envs, + # num_joints) shape mismatch with multi-instance multi-type scenes; one + # of each is the minimum heterogeneous setup that still exercises the + # per-articulation shape gate without that pre-existing quirk. + num_per_type = 1 + + franka_cfg = FRANKA_PANDA_CFG.replace(prim_path="/World/Env_franka_.*/Robot") + anymal_cfg = ANYMAL_C_CFG.replace(prim_path="/World/Env_anymal_.*/Robot") + + for i in range(num_per_type): + sim_utils.create_prim(f"/World/Env_franka_{i}", "Xform", translation=(2.5 * i, 0.0, 0.0)) + sim_utils.create_prim(f"/World/Env_anymal_{i}", "Xform", translation=(2.5 * i, 5.0, 0.0)) + + franka = Articulation(franka_cfg) + anymal = Articulation(anymal_cfg) + sim.reset() + assert franka.is_initialized and anymal.is_initialized + assert franka.is_fixed_base and not anymal.is_fixed_base + + # Sanity: the model-wide maxima are larger than at least one view's + # per-asset count, so a regression to model-wide sizing would manifest + # as wrong shapes here. Assert that precondition explicitly so the test + # fails clearly if the fixture stops being heterogeneous. + model = SimulationManager.get_model() + assert model.max_dofs_per_articulation > min(franka.num_joints, anymal.num_joints), ( + "scene is no longer heterogeneous; this test relies on model.max_dofs > one view's num_joints" + ) + + franka_J = franka.data.body_link_jacobian_w.torch + anymal_J = anymal.data.body_link_jacobian_w.torch + + # Each view's output uses its OWN per-asset count, not the model-wide max. + # Floating-base assets prepend ``num_base_dofs`` floating-base columns; fixed-base + # assets have ``num_base_dofs == 0``. + franka_dofs = franka.num_joints + franka.num_base_dofs + anymal_dofs = anymal.num_joints + anymal.num_base_dofs + assert franka_J.shape == torch.Size((num_per_type, franka.num_bodies - 1, 6, franka_dofs)), ( + f"Franka jacobian leaked model-wide shape: got {tuple(franka_J.shape)}" + ) + assert anymal_J.shape == torch.Size((num_per_type, anymal.num_bodies, 6, anymal_dofs)), ( + f"Anymal jacobian leaked model-wide shape: got {tuple(anymal_J.shape)}" + ) + + sim.step() + franka.update(sim.cfg.dt) + anymal.update(sim.cfg.dt) + + franka_M = franka.data.mass_matrix.torch + anymal_M = anymal.data.mass_matrix.torch + + assert franka_M.shape == torch.Size((num_per_type, franka_dofs, franka_dofs)) + assert anymal_M.shape == torch.Size((num_per_type, anymal_dofs, anymal_dofs)) + + # Each view's mass matrix must have positive diagonals — padded zero + # rows/cols (the round-2 bug) would surface as zero diagonals on the + # smaller-DoF view. Using a per-diagonal check here instead of det() + # because det of a real Franka mass matrix is naturally ~1e-13. + assert (franka_M.diagonal(dim1=-2, dim2=-1) > 1e-6).all(), ( + "Franka mass matrix has non-positive diagonal under heterogeneous scene" + ) + assert (anymal_M.diagonal(dim1=-2, dim2=-1) > 1e-6).all(), ( + "Anymal mass matrix has non-positive diagonal under heterogeneous scene" + ) + + +@pytest.mark.parametrize("num_articulations", [4]) +@pytest.mark.parametrize("device", ["cuda:0"]) +@pytest.mark.parametrize("articulation_type", ["panda", "anymal"]) +@pytest.mark.parametrize("gravity_enabled", [False]) +@pytest.mark.isaacsim_ci +def test_get_jacobians_link_origin_contract(sim, num_articulations, device, articulation_type, gravity_enabled): + """``J · q_dot`` must encode the link-origin twist (after the COM->origin shift). + + The IsaacLab task-space controllers (IK / OSC / RMPFlow) silently + rely on :attr:`~isaaclab.assets.BaseArticulationData.body_link_jacobian_w` + returning a Jacobian whose linear rows reference each link's origin + (the body's USD prim transform), not its COM. Newton's ``eval_jacobian`` + natively produces COM-referenced rows; the wrapper applies a per-column + shift ``v_origin = v_com - omega x (R · body_com_pos_b)`` to honor the + contract. This test asserts the identity by computing both sides + independently: + + * Predicted by ``J · q_dot``: takes the (already-shifted) Jacobian + and the same ``q_dot`` Newton has post-step. Linear rows should + equal v_origin. + * Ground truth from ``state.body_qd``: read Newton's per-body spatial + twist directly via ``ArticulationView.get_link_velocities`` (which + returns ``(v_com_world, omega_world)``), then apply the same shift + in python and compare. + + Reading the velocity from the ArticulationView state rather than + ``data.body_com_lin_vel_w`` bypasses the IsaacLab lazy-buffer chain, + which is irrelevant to the contract being tested. + """ + articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) + articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) + sim.reset() + assert articulation.is_initialized + + # Reproducible non-trivial q_dot — large enough to drive omega well above + # the floor where COM offset effects would round into noise. + torch.manual_seed(0) + qdot = torch.randn(num_articulations, articulation.num_joints, device=device) * 0.5 + articulation.write_joint_velocity_to_sim_index(velocity=qdot) + sim.step() + articulation.update(sim.cfg.dt) + + # body_link_jacobian_w prepends ``num_base_dofs`` floating-base columns; slice past + # them so the joint axis aligns with joint_vel (actuated-only). + J = articulation.data.body_link_jacobian_w.torch[..., articulation.num_base_dofs :] + qdot_view = articulation.data.joint_vel.torch + v_pred = torch.einsum("nbij,nj->nbi", J, qdot_view) # (N, B_jac, 6) + v_pred_lin = v_pred[..., 0:3] + v_pred_ang = v_pred[..., 3:6] + + # Ground truth from Newton state. ``get_link_velocities`` returns shape + # (num_instances, 1, num_bodies, 6) — per-articulation grouping with + # one articulation per instance — so we squeeze the inner dim. + state = SimulationManager.get_state_0() + body_qd_view = wp.to_torch(articulation.root_view.get_link_velocities(state)).squeeze(1) + body_v_com = body_qd_view[..., :3] + body_omega = body_qd_view[..., 3:] + + # World-frame COM-to-origin offset, derived from already-computed + # data layer outputs (avoids quaternion-convention pitfalls). + body_com_pos_w = articulation.data.body_com_pos_w.torch # (N, num_bodies, 3) + body_link_pos_w = articulation.data.body_link_pos_w.torch # (N, num_bodies, 3) + c_world = body_com_pos_w - body_link_pos_w + + if articulation.is_fixed_base: + body_v_com = body_v_com[:, 1:] + body_omega = body_omega[:, 1:] + c_world = c_world[:, 1:] + + # Expected v_origin = v_com - omega x c_world. + v_origin_expected = body_v_com - torch.cross(body_omega, c_world, dim=-1) + + # Tolerance: 5 mm absolute. The COM-offset bug produces a ~3 cm bias + # on the panda hand under the 0.5-rad/s injected qdot, well above + # this floor; numerical noise from kernel ordering stays under 1 mm. + torch.testing.assert_close(v_pred_ang, body_omega, atol=5e-3, rtol=1e-2) + torch.testing.assert_close(v_pred_lin, v_origin_expected, atol=5e-3, rtol=1e-2) + + +@pytest.mark.parametrize("num_articulations", [4]) +@pytest.mark.parametrize("device", ["cuda:0"]) +@pytest.mark.parametrize("articulation_type", ["panda", "anymal"]) +@pytest.mark.parametrize("gravity_enabled", [False]) +@pytest.mark.isaacsim_ci +def test_get_mass_matrix_symmetry_pd(sim, num_articulations, device, articulation_type, gravity_enabled): + """The joint-space mass matrix ``M(q)`` must be square, symmetric, and positive-definite. + + This pins three structural properties of + :attr:`~isaaclab.assets.BaseArticulationData.mass_matrix`: + + * **Square**: shape ``(N, num_joints + num_base_dofs, num_joints + num_base_dofs)``. + A transposed gather or a non-square scratch buffer would be caught + here before downstream OSC inversion silently propagates garbage. + * **Symmetric**: ``M == M.T`` to numerical precision. The joint- + space inertia tensor is symmetric by construction; an asymmetric + result indicates a wrong-axis gather, half-populated buffer, or + Cholesky-input bug. + * **Positive-definite**: ``torch.linalg.cholesky(M)`` succeeds. OSC + computes ``M_b = (J · M^-1 · J^T)^-1`` which requires PD on every + step. A non-PD M would fail downstream as ``LinAlgError``; this + test catches it earlier and pinpoints the source. + + Parameterized on both fixed-base (panda) and floating-base (anymal). + Both backends include the floating-base DoF rows/cols on the front of + the DoF axis for floating-base assets. + """ + articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) + articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) + sim.reset() + assert articulation.is_initialized + + sim.step() + articulation.update(sim.cfg.dt) + + M = articulation.data.mass_matrix.torch # (N, J, J) + assert M.dim() == 3, f"expected 3-D mass matrix, got shape {tuple(M.shape)}" + assert M.shape[0] == num_articulations + assert M.shape[1] == M.shape[2], f"mass matrix is not square: {tuple(M.shape)}" + + # Symmetric to numerical precision. + asym = (M - M.transpose(-1, -2)).abs().max().item() + assert asym < 1e-4, f"|M - M^T|_max = {asym:.3e} — mass matrix is not symmetric" + + # Positive-definite via Cholesky. Adds a tiny diagonal jitter to + # tolerate the floor of float32 PD eigenvalues without masking real + # non-PD bugs (the jitter is well below realistic inertia scales). + eye = torch.eye(M.shape[-1], device=M.device, dtype=M.dtype).expand_as(M) + torch.linalg.cholesky(M + 1e-6 * eye) + + +@pytest.mark.parametrize("num_articulations", [1]) +@pytest.mark.parametrize("device", ["cuda:0"]) +@pytest.mark.parametrize("articulation_type", ["panda", "anymal"]) +@pytest.mark.parametrize("gravity_enabled", [False]) +@pytest.mark.isaacsim_ci +def test_jacobian_refreshes_after_manual_joint_write( + sim, num_articulations, device, articulation_type, gravity_enabled +): + """After ``write_joint_position_to_sim_index`` (no sim step), the Jacobian read + must reflect the new joint state — not the previous one. + + Catches: + - Missing FK trigger in :attr:`body_com_jacobian_w` (eval_jacobian uses stale + ``state.body_q``). + - Missing FK trigger in :attr:`body_link_jacobian_w` shift kernel. + + The contract: ``J`` read directly after a manual write must equal ``J`` read + after ``sim.step + update`` — the latter is the ground-truth fresh-FK reference. + """ + articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) + articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) + sim.reset() + sim.step() + articulation.update(sim.cfg.dt) + + # Read J / M at the baseline joint state. + q_baseline = articulation.data.joint_pos.torch.clone() + J_link_0 = articulation.data.body_link_jacobian_w.torch.clone() + J_com_0 = articulation.data.body_com_jacobian_w.torch.clone() + + # Manually write a different joint state — large delta to make Jacobian change visible. + # No sim.step / update — FK becomes stale (write_joint_position_to_sim sets _fk_timestamp = -1). + q_target = q_baseline + 0.5 + env_ids = wp.array([0], dtype=wp.int32, device=device) + articulation.write_joint_position_to_sim_index(position=q_target, env_ids=env_ids) + + # If the FK trigger works: forward() runs, body_q is refreshed to match q_target, + # eval_jacobian / shift kernel see fresh body poses, J reflects q_target → differs from J at baseline. + # If the trigger is missing: body_q stays at baseline, J unchanged from J_link_0 / J_com_0. + J_link_1 = articulation.data.body_link_jacobian_w.torch.clone() + J_com_1 = articulation.data.body_com_jacobian_w.torch.clone() + + assert not torch.allclose(J_link_0, J_link_1, atol=1e-3), ( + "body_link_jacobian_w did not change after manual joint write — " + "FK trigger likely missing (eval_jacobian / shift kernel reading stale state.body_q)." + ) + assert not torch.allclose(J_com_0, J_com_1, atol=1e-3), ( + "body_com_jacobian_w did not change after manual joint write — FK trigger likely missing before eval_jacobian." + ) + + +@pytest.mark.parametrize("num_articulations", [1]) +@pytest.mark.parametrize("device", ["cuda:0"]) +@pytest.mark.parametrize("articulation_type", ["panda", "anymal"]) +@pytest.mark.parametrize("gravity_enabled", [False]) +@pytest.mark.isaacsim_ci +def test_mass_matrix_refreshes_after_manual_joint_write( + sim, num_articulations, device, articulation_type, gravity_enabled +): + """After ``write_joint_position_to_sim_index`` (no sim step), the mass matrix read + must reflect the new joint state. + + The mass matrix depends on ``q`` (joint positions) through the body-spatial-inertia + transformation in eval_mass_matrix's ``compute_body_spatial_inertia`` step, which + reads ``state.body_q``. Same FK-staleness pattern as the Jacobian. + """ + articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) + articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) + sim.reset() + sim.step() + articulation.update(sim.cfg.dt) + + M_0 = articulation.data.mass_matrix.torch.clone() + q_target = articulation.data.joint_pos.torch.clone() + 0.5 + env_ids = wp.array([0], dtype=wp.int32, device=device) + articulation.write_joint_position_to_sim_index(position=q_target, env_ids=env_ids) + M_1 = articulation.data.mass_matrix.torch.clone() + + assert not torch.allclose(M_0, M_1, atol=1e-3), ( + "mass_matrix did not change after manual joint write — " + "FK trigger likely missing before eval_mass_matrix (compute_body_spatial_inertia " + "reads stale state.body_q)." + ) + + +@pytest.mark.parametrize("device", ["cuda:0"]) +@pytest.mark.parametrize("articulation_type", ["panda"]) +@pytest.mark.parametrize("gravity_enabled", [False]) +@pytest.mark.isaacsim_ci +def test_franka_ik_tracking_accuracy(sim, device, articulation_type, gravity_enabled): + """Newton-side IK convergence sentinel. + + Runs a full IK tracking loop end-to-end through the new + ``robot.data.body_link_jacobian_w`` accessor and records the steady-state EE + pose error. With the robot teleported to its configured init_state + home pose and scene gravity off, Newton's IK converges to + machine-precision tracking (sub-mm). A bridge regression + (wrong-reference-frame Jacobian, missing COM->origin shift, DoF + mis-ordering) would push the steady-state error well above the + threshold below. + + The pose teleport is deliberate: the standalone test path does not + invoke a manager-based env reset (which is what normally pushes + :attr:`~isaaclab.assets.ArticulationData.default_joint_pos` to sim). + Without it, the robot starts at the URDF-neutral pose where the + Franka wrist axes nearly align (rank-deficient Jacobian) and DLS + plateaus at multi-cm error -- a kinematic-singularity artifact, not + a bridge or Newton issue. + + See ``test_get_jacobians_link_origin_contract`` (above) for the + sharper unit-level pin on the Jacobian's reference-point contract. + """ + robot, ee_frame_idx, ee_jacobi_idx, arm_joint_ids = _setup_franka_at_home_pose(sim) + + sim.step() + robot.update(sim.cfg.dt) + target_pose_b = _build_relative_pose_target(robot, ee_frame_idx, (0.05, 0.0, 0.0), device) + + ik = DifferentialIKController( + DifferentialIKControllerCfg(command_type="pose", use_relative_mode=False, ik_method="dls"), + num_envs=1, + device=device, + ) + ik.set_command(target_pose_b) + + pos_history: list[float] = [] + rot_history: list[float] = [] + for _ in range(800): + jacobian = _compute_jacobian_root_frame(robot, ee_jacobi_idx, arm_joint_ids) + ee_pos_b, ee_quat_b, _ = _compute_ee_pose_root(robot, ee_frame_idx) + joint_pos = robot.data.joint_pos.torch[:, arm_joint_ids] + + joint_pos_des = ik.compute(ee_pos_b, ee_quat_b, jacobian, joint_pos) + + robot.set_joint_position_target(joint_pos_des, joint_ids=arm_joint_ids) + robot.write_data_to_sim() + sim.step() + robot.update(sim.cfg.dt) + + pos_error, rot_error = compute_pose_error(ee_pos_b, ee_quat_b, target_pose_b[:, 0:3], target_pose_b[:, 3:7]) + pos_history.append(pos_error.norm(dim=-1).max().item()) + rot_history.append(rot_error.norm(dim=-1).max().item()) + + pos_min, pos_mean = _summarize_history(pos_history) + rot_min, rot_mean = _summarize_history(rot_history) + + # Print metrics every run for stress-test capture. + print(f"IK_METRIC pos_min={pos_min:.5f} pos_mean={pos_mean:.5f} rot_min={rot_min:.5f} rot_mean={rot_mean:.5f}") + + # Regression sentinel: assert on tail mean rather than min. Tail + # min is the bottom of any oscillation envelope and can be tiny + # while the actual tracking error is much larger. With the + # configured home pose and scene gravity off, Newton converges to + # machine precision (sub-mm). The 5 mm bound absorbs any CUDA- + # kernel-ordering noise while remaining well below the "totally + # broken" regime: a bridge regression (wrong-frame Jacobian, + # missing COM->origin shift, DoF mis-ordering) would push the + # steady-state error well past this bound. + assert pos_mean < 5e-3, f"IK pos_mean {pos_mean:.5f} > 5 mm — bridge regression?" + assert rot_mean < 5e-2, f"IK rot_mean {rot_mean:.5f} > 0.05 rad — bridge regression?" + + +@pytest.mark.parametrize("device", ["cuda:0"]) +@pytest.mark.parametrize("articulation_type", ["panda"]) +@pytest.mark.parametrize("gravity_enabled", [False]) +@pytest.mark.isaacsim_ci +def test_franka_osc_tracking_accuracy(sim, device, articulation_type, gravity_enabled): + """Newton-side OSC pose tracking sentinel. + + Mirror of the existing PhysX-side OSC tests in + :mod:`isaaclab.test.controllers.test_operational_space`, scoped to + Franka pose-abs tracking on Newton. Like the IK sentinel above, this + test exercises the full controller-bridge pipeline + (:attr:`~isaaclab.assets.BaseArticulationData.body_link_jacobian_w` + + :attr:`~isaaclab.assets.BaseArticulationData.mass_matrix`) end-to-end + and asserts a loose regression bound rather than a tight correctness + oracle. + + Newton lacks a gravity-comp primitive (see ``xfail`` test below; + upstream Newton issues #2497, #2529, #2625), so OSC runs with + ``gravity_compensation=False`` and the test isolates from gravity by + disabling scene gravity. ``inertial_dynamics_decoupling=True`` + exercises ``mass_matrix`` and the Newton COM-referenced J → + M_b → J product. The actuator PD is zeroed at cfg time so OSC's + joint-effort output is not opposed by ``kp·(target − q)``. + """ + robot, ee_frame_idx, ee_jacobi_idx, arm_joint_ids = _setup_franka_at_home_pose(sim, zero_actuator_pd=True) + + osc = OperationalSpaceController( + OperationalSpaceControllerCfg( + target_types=["pose_abs"], + impedance_mode="fixed", + inertial_dynamics_decoupling=True, + partial_inertial_dynamics_decoupling=False, + gravity_compensation=False, + motion_stiffness_task=500.0, + motion_damping_ratio_task=1.0, + ), + num_envs=1, + device=device, + ) + + sim.step() + robot.update(sim.cfg.dt) + target_pose_b = _build_relative_pose_target(robot, ee_frame_idx, (0.05, 0.0, 0.0), device) + + pos_history: list[float] = [] + rot_history: list[float] = [] + for _ in range(800): + jacobian_b = _compute_jacobian_root_frame(robot, ee_jacobi_idx, arm_joint_ids) + mass_matrix = robot.data.mass_matrix.torch[:, arm_joint_ids, :][:, :, arm_joint_ids] + ee_pos_b, ee_quat_b, _ = _compute_ee_pose_root(robot, ee_frame_idx) + ee_pose_b = torch.cat([ee_pos_b, ee_quat_b], dim=-1) + joint_vel = robot.data.joint_vel.torch[:, arm_joint_ids] + ee_vel_b = _compute_ee_vel_root(jacobian_b, joint_vel) + + osc.set_command(target_pose_b, current_ee_pose_b=ee_pose_b) + joint_efforts = osc.compute( + jacobian_b=jacobian_b, + current_ee_pose_b=ee_pose_b, + current_ee_vel_b=ee_vel_b, + mass_matrix=mass_matrix, + gravity=None, + ) + + robot.set_joint_effort_target(joint_efforts, joint_ids=arm_joint_ids) + robot.write_data_to_sim() + sim.step() + robot.update(sim.cfg.dt) + + pos_error, rot_error = compute_pose_error(ee_pos_b, ee_quat_b, target_pose_b[:, 0:3], target_pose_b[:, 3:7]) + pos_history.append(pos_error.norm(dim=-1).max().item()) + rot_history.append(rot_error.norm(dim=-1).max().item()) + + pos_min, pos_mean = _summarize_history(pos_history) + rot_min, rot_mean = _summarize_history(rot_history) + + print(f"OSC_METRIC pos_min={pos_min:.5f} pos_mean={pos_mean:.5f} rot_min={rot_min:.5f} rot_mean={rot_mean:.5f}") + + # Regression sentinel: assert on tail mean rather than min. With + # ``current_ee_vel_b = J · q_dot`` providing OSC's damping term and + # the actuator PD zeroed, the impedance settles to machine + # precision -- same ballpark as the IK test. The 5 mm bound is a + # bridge regression sentinel: a wrong J, wrong mass matrix, or + # DoF mis-ordering pushes the steady-state error well past it + # because OSC consumes both ``body_link_jacobian_w`` and + # ``mass_matrix`` per step. + assert pos_mean < 5e-3, f"OSC pos_mean {pos_mean:.5f} > 5 mm — bridge regression?" + assert rot_mean < 5e-2, f"OSC rot_mean {rot_mean:.5f} > 0.05 rad — bridge regression?" + + if __name__ == "__main__": pytest.main([__file__, "-v", "--maxfail=1"]) diff --git a/source/isaaclab_newton/test/assets/test_rigid_object.py b/source/isaaclab_newton/test/assets/test_rigid_object.py index 3b8e3aa9bf85..ba2c47e24f9b 100644 --- a/source/isaaclab_newton/test/assets/test_rigid_object.py +++ b/source/isaaclab_newton/test/assets/test_rigid_object.py @@ -276,8 +276,8 @@ def test_external_force_buffer(device): # check if the cube's force and torque buffers are correctly updated for i in range(cube_object.num_instances): - assert cube_object._permanent_wrench_composer.composed_force.torch[i, 0, 0].item() == force - assert cube_object._permanent_wrench_composer.composed_torque.torch[i, 0, 0].item() == force + assert cube_object._permanent_wrench_composer.out_force_b.torch[i, 0, 0].item() == force + assert cube_object._permanent_wrench_composer.out_torque_b.torch[i, 0, 0].item() == force # Check if the instantaneous wrench is correctly added to the permanent wrench cube_object.permanent_wrench_composer.add_forces_and_torques_index( @@ -565,10 +565,10 @@ def test_reset_rigid_object(num_cubes, device): # Reset should zero external forces and torques assert not cube_object._instantaneous_wrench_composer.active assert not cube_object._permanent_wrench_composer.active - assert torch.count_nonzero(cube_object._instantaneous_wrench_composer.composed_force.torch) == 0 - assert torch.count_nonzero(cube_object._instantaneous_wrench_composer.composed_torque.torch) == 0 - assert torch.count_nonzero(cube_object._permanent_wrench_composer.composed_force.torch) == 0 - assert torch.count_nonzero(cube_object._permanent_wrench_composer.composed_torque.torch) == 0 + assert torch.count_nonzero(cube_object._instantaneous_wrench_composer.out_force_b.torch) == 0 + assert torch.count_nonzero(cube_object._instantaneous_wrench_composer.out_torque_b.torch) == 0 + assert torch.count_nonzero(cube_object._permanent_wrench_composer.out_force_b.torch) == 0 + assert torch.count_nonzero(cube_object._permanent_wrench_composer.out_torque_b.torch) == 0 @pytest.mark.isaacsim_ci @@ -1259,3 +1259,77 @@ def test_warmup_attach_stage_not_called_for_cpu(): f"This indicates the CPU MBP broadphase double-initialization regression is present: " f"attach_stage() + force_load_physics_from_usd() must not be combined for CPU." ) + + +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.parametrize("writer", ["link_index", "link_mask", "com_index", "com_mask"]) +@pytest.mark.isaacsim_ci +def test_body_link_pose_w_fresh_after_root_pose_write(device, writer): + """Regression: ``body_link_pose_w`` must reflect a freshly written root pose without an intervening sim step. + + After ``write_root_{link,com}_pose_to_sim_{index,mask}``, the cached ``_sim_bind_body_link_pose_w`` + (Newton ``body_q``) is stale until forward kinematics is re-evaluated. The getter must call + :meth:`SimulationManager.forward` so the returned tensor matches the written pose. Without the fix, + the getter returns the pre-write value. The write must also dirty the simulator-side + ``_fk_reset_mask`` so collision queries (which read ``body_q`` directly, not via the property) + re-run FK before the next step. + """ + + def _fk_reset_mask_dirty() -> bool: + assert SimulationManager._fk_reset_mask is not None + return bool(wp.to_torch(SimulationManager._fk_reset_mask).any().item()) + + num_cubes = 2 + with _newton_sim_context(device, gravity_enabled=False, auto_add_lighting=True) as sim: + sim._app_control_on_stop_handle = None + cube_object, _ = generate_cubes_scene(num_cubes=num_cubes, height=0.5, device=device) + + sim.reset() + assert cube_object.is_initialized + + # Step once so that _sim_timestamp > 0 and caches are primed. + sim.step() + cube_object.update(sim.cfg.dt) + + # Prime the body_link_pose_w cache with the current pose. + pre_write_pose = wp.to_torch(cube_object.data.body_link_pose_w).clone().view(num_cubes, 7) + + # Clear the dirty flag so we can observe that the write sets it. + SimulationManager.forward() + assert not _fk_reset_mask_dirty() + + # Build a target pose clearly distinct from the current one in both translation and orientation. + # Quaternion in (x, y, z, w) for 90° about z: [0, 0, sin(pi/4), cos(pi/4)] = [0, 0, sqrt(0.5), sqrt(0.5)]. + target_pose = wp.to_torch(cube_object.data.root_link_pose_w).clone() + target_pose[..., 0] += 10.0 + target_pose[..., 1] += 5.0 + target_pose[..., 2] += 2.0 + sqrt_half = 0.7071067811865476 + target_pose[..., 3] = 0.0 + target_pose[..., 4] = 0.0 + target_pose[..., 5] = sqrt_half + target_pose[..., 6] = sqrt_half + + if writer == "link_index": + cube_object.write_root_link_pose_to_sim_index(root_pose=target_pose) + elif writer == "link_mask": + cube_object.write_root_link_pose_to_sim_mask(root_pose=target_pose) + elif writer == "com_index": + cube_object.write_root_com_pose_to_sim_index(root_pose=target_pose) + elif writer == "com_mask": + cube_object.write_root_com_pose_to_sim_mask(root_pose=target_pose) + + # The simulator-side dirty flag must be set before any property read clears it via forward(). + assert _fk_reset_mask_dirty(), "pose write must call SimulationManager.invalidate_fk()" + + # Read without stepping: getter must trigger forward kinematics and return the fresh pose. + body_link = wp.to_torch(cube_object.data.body_link_pose_w).view(num_cubes, 7) + # Defeat alias accidents: the property must not still return the pre-write value. + assert not torch.allclose(body_link[..., :3], pre_write_pose[..., :3], rtol=1e-4, atol=1e-4), ( + "body_link_pose_w returned the pre-write cached pose; forward() was not invoked" + ) + # Translation must match the write. + torch.testing.assert_close(body_link[..., :3], target_pose[..., :3], rtol=1e-4, atol=1e-4) + # Orientation: compare via |q1 · q2| ≈ 1 to account for the q ≡ -q double cover. + quat_dot = torch.abs((body_link[..., 3:7] * target_pose[..., 3:7]).sum(dim=-1)) + torch.testing.assert_close(quat_dot, torch.ones_like(quat_dot), rtol=1e-4, atol=1e-4) diff --git a/source/isaaclab_newton/test/assets/test_rigid_object_collection.py b/source/isaaclab_newton/test/assets/test_rigid_object_collection.py index 0873da8ecf22..5ee470469548 100644 --- a/source/isaaclab_newton/test/assets/test_rigid_object_collection.py +++ b/source/isaaclab_newton/test/assets/test_rigid_object_collection.py @@ -241,8 +241,8 @@ def test_external_force_buffer(device): # check if the object collection's force and torque buffers are correctly updated for i in range(num_envs): - assert object_collection._permanent_wrench_composer.composed_force.torch[i, 0, 0].item() == force - assert object_collection._permanent_wrench_composer.composed_torque.torch[i, 0, 0].item() == force + assert object_collection._permanent_wrench_composer.out_force_b.torch[i, 0, 0].item() == force + assert object_collection._permanent_wrench_composer.out_torque_b.torch[i, 0, 0].item() == force object_collection.instantaneous_wrench_composer.add_forces_and_torques_index( body_ids=object_ids, @@ -502,10 +502,10 @@ def test_reset_object_collection(num_envs, num_cubes, device): # Reset should zero external forces and torques assert not object_collection._instantaneous_wrench_composer.active assert not object_collection._permanent_wrench_composer.active - assert torch.count_nonzero(object_collection._instantaneous_wrench_composer.composed_force.torch) == 0 - assert torch.count_nonzero(object_collection._instantaneous_wrench_composer.composed_torque.torch) == 0 - assert torch.count_nonzero(object_collection._permanent_wrench_composer.composed_force.torch) == 0 - assert torch.count_nonzero(object_collection._permanent_wrench_composer.composed_torque.torch) == 0 + assert torch.count_nonzero(object_collection._instantaneous_wrench_composer.out_force_b.torch) == 0 + assert torch.count_nonzero(object_collection._instantaneous_wrench_composer.out_torque_b.torch) == 0 + assert torch.count_nonzero(object_collection._permanent_wrench_composer.out_force_b.torch) == 0 + assert torch.count_nonzero(object_collection._permanent_wrench_composer.out_torque_b.torch) == 0 @pytest.mark.parametrize("num_envs", [1, 3]) @@ -892,3 +892,66 @@ def test_write_object_state_functions_data_consistency(num_envs, num_cubes, devi torch.testing.assert_close(body_com_vel_w, com_vel_w) torch.testing.assert_close(body_link_pose_w, link_pose_w) torch.testing.assert_close(body_com_vel_w[..., 3:], link_vel_w[..., 3:]) + + +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.parametrize("writer", ["link_index", "link_mask", "com_index", "com_mask"]) +@pytest.mark.isaacsim_ci +def test_body_pose_write_marks_fk_reset_mask(device, writer): + """Regression: ``write_body_{link,com}_pose_to_sim_{index,mask}`` must mark FK dirty. + + For a collection, ``_sim_bind_body_link_pose_w`` is bound directly to the simulator's root-transforms + buffer, so the property read is not what becomes stale — the simulator's internal ``body_q`` used by + collision detection is. The write methods must therefore call :meth:`SimulationManager.invalidate_fk` + so downstream consumers re-run forward kinematics before the next step. Without the fix, + ``_fk_reset_mask`` remains unset after an explicit pose write. The buffer-aliasing invariant is + also pinned: a refactor that decouples ``_sim_bind_body_link_pose_w`` from the write target would + silently make the property stale, so we check the post-write pose matches the written value. + """ + + def _fk_reset_mask_dirty() -> bool: + assert SimulationManager._fk_reset_mask is not None + return bool(wp.to_torch(SimulationManager._fk_reset_mask).any().item()) + + num_envs = 2 + num_cubes = 2 + with _newton_sim_context(device, gravity_enabled=False, auto_add_lighting=True) as sim: + sim._app_control_on_stop_handle = None + cube_object, _ = generate_cubes_scene(num_envs=num_envs, num_cubes=num_cubes, height=0.5, device=device) + + sim.reset() + assert cube_object.is_initialized + + sim.step() + cube_object.update(sim.cfg.dt) + + # Clear the dirty flag so we can observe that the write sets it. + SimulationManager.forward() + assert not _fk_reset_mask_dirty() + + pre_write_pose = wp.to_torch(cube_object.data.body_link_pose_w).clone() + + target_pose = wp.to_torch(cube_object.data.body_link_pose_w).clone() + target_pose[..., 0] += 10.0 + target_pose[..., 1] += 5.0 + target_pose[..., 2] += 2.0 + + if writer == "link_index": + cube_object.write_body_link_pose_to_sim_index(body_poses=target_pose) + elif writer == "link_mask": + cube_object.write_body_link_pose_to_sim_mask(body_poses=target_pose) + elif writer == "com_index": + cube_object.write_body_com_pose_to_sim_index(body_poses=target_pose) + elif writer == "com_mask": + cube_object.write_body_com_pose_to_sim_mask(body_poses=target_pose) + + assert _fk_reset_mask_dirty(), "pose write must call SimulationManager.invalidate_fk()" + + # body_link_pose_w must reflect the write immediately — its underlying buffer is the write + # target. A regression that moves this property to a separate cached buffer (mirroring the + # single-object case) would silently break this invariant. + body_link = wp.to_torch(cube_object.data.body_link_pose_w) + assert not torch.allclose(body_link[..., :3], pre_write_pose[..., :3], rtol=1e-4, atol=1e-4), ( + "body_link_pose_w still aliases the pre-write pose; the underlying buffer was not written" + ) + torch.testing.assert_close(body_link[..., :3], target_pose[..., :3], rtol=1e-4, atol=1e-4) diff --git a/source/isaaclab_newton/test/cloner/test_rename_builder_labels.py b/source/isaaclab_newton/test/cloner/test_rename_builder_labels.py new file mode 100644 index 000000000000..2fe930f8b520 --- /dev/null +++ b/source/isaaclab_newton/test/cloner/test_rename_builder_labels.py @@ -0,0 +1,281 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Unit tests for ``_rename_builder_labels``. + +Covers both passes of the rewrite: + + * Pass 1 — built-in label arrays (``body``, ``joint``, ``shape``, + ``articulation``, ``constraint_mimic``, ``equality_constraint``). + * Pass 2 — any string-typed custom-attribute column whose frequency declares a + sibling ``references="world"`` companion (e.g. ``mujoco:tendon_label``). + +The contract under test: every label whose row maps to a world in ``env_ids`` +and whose value starts with the source root is rewritten to the destination +template's per-env path; everything else is left alone. +""" + +import unittest + +import newton +import torch +from isaaclab_newton.cloner.newton_replicate import _rename_builder_labels +from newton.solvers import SolverMuJoCo + +_TENDON_FREQ = "mujoco:tendon" +_SRC = "/Sources/protoA" +_DST = "/World/envs/env_{}" + + +# ─── helpers ───────────────────────────────────────────────────────────────── + + +def _inject_builtins(builder: newton.ModelBuilder, types: tuple[str, ...], src_path: str, worlds: list[int]) -> None: + """Append ``len(worlds)`` synthetic entries to each built-in ``*_label``/``*_world`` pair.""" + for t in types: + labels = getattr(builder, f"{t}_label") + worlds_arr = getattr(builder, f"{t}_world") + for w in worlds: + labels.append(f"{src_path}/{t}_{w}") + worlds_arr.append(w) + + +def _inject_tendon_strings(builder: newton.ModelBuilder, src_path: str, worlds: list[int]) -> None: + """Append synthetic ``mujoco:tendon_label`` + ``mujoco:tendon_world`` rows.""" + label_attr = builder.custom_attributes["mujoco:tendon_label"] + world_attr = builder.custom_attributes["mujoco:tendon_world"] + if label_attr.values is None: + label_attr.values = [] + if world_attr.values is None: + world_attr.values = [] + for w in worlds: + label_attr.values.append(f"{src_path}/Tendon_{w}") + world_attr.values.append(w) + builder._custom_frequency_counts[_TENDON_FREQ] = builder._custom_frequency_counts.get(_TENDON_FREQ, 0) + len(worlds) + + +def _make_builder_with_entries(worlds: list[int]) -> newton.ModelBuilder: + """Builder pre-populated with one row per world for every label class under test.""" + b = newton.ModelBuilder() + SolverMuJoCo.register_custom_attributes(b) + _inject_builtins( + b, ("body", "joint", "shape", "articulation", "constraint_mimic", "equality_constraint"), _SRC, worlds + ) + _inject_tendon_strings(b, _SRC, worlds) + return b + + +# ─── tests ─────────────────────────────────────────────────────────────────── + + +class TestRenameBuilderLabels(unittest.TestCase): + """Both passes rewrite to the same per-env destination pattern.""" + + def setUp(self): + self.worlds = [0, 1, 2] + self.env_ids = torch.tensor(self.worlds, dtype=torch.int32) + self.mapping = torch.ones(1, len(self.worlds), dtype=torch.bool) + + def _rename(self, builder): + _rename_builder_labels(builder, [_SRC], [_DST], self.env_ids, self.mapping) + + # Pass 1 --------------------------------------------------------------- + + def test_builtin_labels_rewritten_per_world(self): + b = _make_builder_with_entries(self.worlds) + self._rename(b) + for t in ("body", "joint", "shape", "articulation", "constraint_mimic", "equality_constraint"): + labels = getattr(b, f"{t}_label") + worlds_arr = getattr(b, f"{t}_world") + for k, w in enumerate(worlds_arr): + self.assertEqual( + labels[k], + f"{_DST.format(int(w))}/{t}_{int(w)}", + msg=f"{t}_label[{k}] not rewritten correctly", + ) + + # Pass 2 --------------------------------------------------------------- + + def test_tendon_label_string_custom_attr_rewritten(self): + b = _make_builder_with_entries(self.worlds) + self._rename(b) + labels = b.custom_attributes["mujoco:tendon_label"].values + worlds_arr = b.custom_attributes["mujoco:tendon_world"].values + for k, w in enumerate(worlds_arr): + self.assertEqual(labels[k], f"{_DST.format(int(w))}/Tendon_{int(w)}") + + # Cross-pass consistency ---------------------------------------------- + + def test_all_renamed_labels_share_the_per_env_root(self): + """Every label written by either pass must live under ``/World/envs/env_/``.""" + b = _make_builder_with_entries(self.worlds) + self._rename(b) + per_world = {int(w): _DST.format(int(w)) + "/" for w in self.env_ids.tolist()} + for t in ("body", "joint", "shape", "articulation", "constraint_mimic", "equality_constraint"): + for label, w in zip(getattr(b, f"{t}_label"), getattr(b, f"{t}_world")): + self.assertTrue(label.startswith(per_world[int(w)]), msg=f"{t}: {label!r}") + tendon_labels = b.custom_attributes["mujoco:tendon_label"].values + tendon_worlds = b.custom_attributes["mujoco:tendon_world"].values + for label, w in zip(tendon_labels, tendon_worlds): + self.assertTrue(label.startswith(per_world[int(w)]), msg=f"tendon: {label!r}") + + # Guards --------------------------------------------------------------- + + def test_non_path_string_left_untouched(self): + """Strings that don't start with ``src_path`` must pass through unchanged.""" + b = _make_builder_with_entries(self.worlds) + # Inject one tendon row whose label is an opaque identifier, not a path. + b.custom_attributes["mujoco:tendon_label"].values.append("named_tendon") + b.custom_attributes["mujoco:tendon_world"].values.append(self.worlds[0]) + self._rename(b) + self.assertEqual(b.custom_attributes["mujoco:tendon_label"].values[-1], "named_tendon") + + def test_world_outside_env_ids_left_untouched(self): + """A row whose world is not in ``env_ids`` must keep its original label.""" + b = _make_builder_with_entries(self.worlds) + # Inject one extra row tagged with a world id not present in env_ids. + b.body_label.append(f"{_SRC}/body_99") + b.body_world.append(99) + self._rename(b) + self.assertEqual(b.body_label[-1], f"{_SRC}/body_99") + + def test_sparse_env_ids(self): + """Non-contiguous ``env_ids`` (e.g. [10, 20, 30]) must rewrite using the right per-env root.""" + worlds = [10, 20, 30] + b = newton.ModelBuilder() + SolverMuJoCo.register_custom_attributes(b) + _inject_builtins(b, ("body",), _SRC, worlds) + env_ids = torch.tensor(worlds, dtype=torch.int32) + mapping = torch.ones(1, len(worlds), dtype=torch.bool) + _rename_builder_labels(b, [_SRC], [_DST], env_ids, mapping) + for k, w in enumerate(b.body_world): + self.assertEqual(b.body_label[k], f"/World/envs/env_{int(w)}/body_{int(w)}") + + +class TestRenamePass2Generality(unittest.TestCase): + """Pass 2 must generalize across coexisting frequencies and multiple string columns.""" + + def setUp(self): + self.worlds = [0, 1] + self.env_ids = torch.tensor(self.worlds, dtype=torch.int32) + self.mapping = torch.ones(1, len(self.worlds), dtype=torch.bool) + + def _register_synthetic_freq(self, builder, freq_name, world_attr_name, str_attr_names): + """Register a ``syn:`` frequency with one world int column and N string columns.""" + freq = f"syn:{freq_name}" + builder.add_custom_frequency(newton.ModelBuilder.CustomFrequency(name=freq_name, namespace="syn")) + builder.add_custom_attribute( + newton.ModelBuilder.CustomAttribute( + name=world_attr_name, + frequency=freq, + dtype=int, + default=0, + namespace="syn", + references="world", + ) + ) + for n in str_attr_names: + builder.add_custom_attribute( + newton.ModelBuilder.CustomAttribute( + name=n, + frequency=freq, + dtype=str, + default="", + namespace="syn", + ) + ) + + def _populate(self, builder, freq, world_attr_name, str_attr_names, worlds): + wa = builder.custom_attributes[f"syn:{world_attr_name}"] + if wa.values is None: + wa.values = [] + for w in worlds: + wa.values.append(w) + for n in str_attr_names: + sa = builder.custom_attributes[f"syn:{n}"] + if sa.values is None: + sa.values = [] + for w in worlds: + sa.values.append(f"{_SRC}/{n}_{w}") + builder._custom_frequency_counts[freq] = builder._custom_frequency_counts.get(freq, 0) + len(worlds) + + def test_two_coexisting_custom_frequencies(self): + """Each registered ``references='world'`` companion must drive its own frequency's str columns.""" + b = newton.ModelBuilder() + self._register_synthetic_freq(b, "freqA", "freqA_world", ["freqA_label"]) + self._register_synthetic_freq(b, "freqB", "freqB_world", ["freqB_label"]) + self._populate(b, "syn:freqA", "freqA_world", ["freqA_label"], self.worlds) + self._populate(b, "syn:freqB", "freqB_world", ["freqB_label"], self.worlds) + _rename_builder_labels(b, [_SRC], [_DST], self.env_ids, self.mapping) + for n in ("freqA_label", "freqB_label"): + wa = b.custom_attributes[f"syn:{n.split('_')[0]}_world"].values + sa = b.custom_attributes[f"syn:{n}"].values + for k, w in enumerate(wa): + self.assertEqual(sa[k], f"/World/envs/env_{int(w)}/{n}_{int(w)}") + + def test_multiple_string_columns_at_one_frequency(self): + """Two str columns sharing one frequency must both be rewritten using the shared world companion.""" + b = newton.ModelBuilder() + self._register_synthetic_freq(b, "freqA", "freqA_world", ["freqA_label", "freqA_alt"]) + self._populate(b, "syn:freqA", "freqA_world", ["freqA_label", "freqA_alt"], self.worlds) + _rename_builder_labels(b, [_SRC], [_DST], self.env_ids, self.mapping) + wa = b.custom_attributes["syn:freqA_world"].values + for n in ("freqA_label", "freqA_alt"): + sa = b.custom_attributes[f"syn:{n}"].values + for k, w in enumerate(wa): + self.assertEqual(sa[k], f"/World/envs/env_{int(w)}/{n}_{int(w)}") + + def test_empty_values_pass_through(self): + """A registered-but-empty string column must not crash the rename pass.""" + b = newton.ModelBuilder() + self._register_synthetic_freq(b, "freqA", "freqA_world", ["freqA_label"]) + # values stay None (registered, never populated) + _rename_builder_labels(b, [_SRC], [_DST], self.env_ids, self.mapping) + # Fully populate after the no-op rename: ensures the early-return guard didn't corrupt state. + self._populate(b, "syn:freqA", "freqA_world", ["freqA_label"], self.worlds) + self.assertEqual(len(b.custom_attributes["syn:freqA_label"].values), len(self.worlds)) + + +class TestRenameMultiSource(unittest.TestCase): + """Multi-source handling must not cross-contaminate when source paths share a string prefix.""" + + def test_prefix_overlap_does_not_cross_contaminate(self): + """Sources whose paths share a string prefix and that both feed the same envs must not cross-rename. + + Common IL pattern: a robot proto and an object proto both feed every env. If the two source + paths share a string prefix (``/Sources/protoA`` and ``/Sources/protoAB``), iter 0 + (``src=protoA``) sees the protoAB rows for the same world ids it owns and would over-match + them under a non-boundary ``startswith``. The world-id guard alone does not catch this case + because both sources contribute to the same set of worlds. + """ + sources = ["/Sources/protoA", "/Sources/protoAB"] + # 2 envs, both fed by both sources. + env_ids = torch.tensor([0, 1], dtype=torch.int32) + mapping = torch.tensor([[1, 1], [1, 1]], dtype=torch.bool) + b = newton.ModelBuilder() + SolverMuJoCo.register_custom_attributes(b) + # One body row from each source per env: 4 rows total, world ids interleaved. + b.body_label.extend( + [ + f"{sources[0]}/body", # row 0: protoA, world 0 + f"{sources[1]}/body", # row 1: protoAB, world 0 + f"{sources[0]}/body", # row 2: protoA, world 1 + f"{sources[1]}/body", # row 3: protoAB, world 1 + ] + ) + b.body_world.extend([0, 0, 1, 1]) + _rename_builder_labels(b, sources, ["/World/envs/env_{}", "/World/envs/env_{}"], env_ids, mapping) + # Each row must end up under its own per-env root with the suffix preserved verbatim. + # Without the "/" boundary on ``startswith``, iter 0 (src=protoA) would match rows 1 and 3 + # because ``/Sources/protoAB/body``.startswith(``/Sources/protoA``) is True, rewriting them + # to ``/World/envs/env_/B/body`` (wrong suffix). + self.assertEqual(b.body_label[0], "/World/envs/env_0/body") + self.assertEqual(b.body_label[1], "/World/envs/env_0/body") + self.assertEqual(b.body_label[2], "/World/envs/env_1/body") + self.assertEqual(b.body_label[3], "/World/envs/env_1/body") + + +if __name__ == "__main__": + unittest.main() diff --git a/source/isaaclab_newton/test/sensors/test_contact_sensor.py b/source/isaaclab_newton/test/sensors/test_contact_sensor.py index 066803ee884b..3aaa6e14b39c 100644 --- a/source/isaaclab_newton/test/sensors/test_contact_sensor.py +++ b/source/isaaclab_newton/test/sensors/test_contact_sensor.py @@ -26,6 +26,7 @@ import pytest import torch +from isaaclab_newton.sensors.contact_sensor import ContactSensorCfg as NewtonContactSensorCfg from physics.physics_test_utils import ( COLLISION_PIPELINES, STABLE_SHAPES, @@ -780,6 +781,112 @@ def test_finger_contact_sensor_isolation(device: str, use_mujoco_contacts: bool, ) +# =================================================================== +# Sensor metadata +# =================================================================== + + +def _make_two_box_scene_cfg(num_envs: int) -> ContactSensorTestSceneCfg: + """Scene with two distinct Cuboid bodies (BoxA, BoxB) per env.""" + rigid_props = sim_utils.RigidBodyPropertiesCfg(disable_gravity=True, linear_damping=0.0, angular_damping=0.0) + scene_cfg = ContactSensorTestSceneCfg(num_envs=num_envs, env_spacing=5.0, lazy_sensor_update=False) + scene_cfg.object_a = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/BoxA", + spawn=sim_utils.CuboidCfg( + size=(0.3, 0.3, 0.3), + rigid_props=rigid_props, + collision_props=sim_utils.CollisionPropertiesCfg(collision_enabled=True), + mass_props=sim_utils.MassPropertiesCfg(mass=1.0), + activate_contact_sensors=True, + ), + init_state=RigidObjectCfg.InitialStateCfg(pos=(-0.5, 0.0, 1.0)), + ) + scene_cfg.object_b = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/BoxB", + spawn=sim_utils.CuboidCfg( + size=(0.3, 0.3, 0.3), + rigid_props=rigid_props, + collision_props=sim_utils.CollisionPropertiesCfg(collision_enabled=True), + mass_props=sim_utils.MassPropertiesCfg(mass=1.0), + activate_contact_sensors=True, + ), + init_state=RigidObjectCfg.InitialStateCfg(pos=(0.5, 0.0, 1.0)), + ) + return scene_cfg + + +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +def test_sensor_metadata(device: str): + """Verify sensor_names and filter_object_names match the underlying sensing and + counterpart configuration across body-mode, body-mode-with-filter, and shape-mode. + """ + num_envs = 4 + sim_cfg = make_sim_cfg(use_mujoco_contacts=False, device=device, gravity=(0.0, 0.0, -9.81)) + + # (1) Body-mode, no filter: pattern matches two distinct body names per env. + with build_simulation_context(sim_cfg=sim_cfg, auto_add_lighting=True, add_ground_plane=True) as sim: + sim._app_control_on_stop_handle = None + scene_cfg = _make_two_box_scene_cfg(num_envs) + scene_cfg.contact_sensor_a = ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/Box.*", + update_period=0.0, + history_length=1, + ) + scene = InteractiveScene(scene_cfg) + sim.reset() + scene.reset() + + sensor: ContactSensor = scene["contact_sensor_a"] + assert sensor.num_sensors == 2, f"expected 2 sensors per env, got {sensor.num_sensors}" + assert sensor.sensor_names == ["BoxA", "BoxB"], f"unexpected sensor_names: {sensor.sensor_names}" + assert sensor.filter_object_names == [], ( + f"expected empty filter_object_names with no filter, got {sensor.filter_object_names}" + ) + + # (2) Body-mode, with filter: one body matches the sensor pattern, one matches the filter pattern. + with build_simulation_context(sim_cfg=sim_cfg, auto_add_lighting=True, add_ground_plane=True) as sim: + sim._app_control_on_stop_handle = None + scene_cfg = _make_two_box_scene_cfg(num_envs) + scene_cfg.contact_sensor_a = ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/BoxA", + filter_prim_paths_expr=["{ENV_REGEX_NS}/BoxB"], + update_period=0.0, + history_length=1, + ) + scene = InteractiveScene(scene_cfg) + sim.reset() + scene.reset() + + sensor: ContactSensor = scene["contact_sensor_a"] + assert sensor.num_sensors == 1, f"expected 1 sensor per env, got {sensor.num_sensors}" + assert sensor.sensor_names == ["BoxA"], f"unexpected sensor_names: {sensor.sensor_names}" + assert sensor.num_filter_objects == 1, f"expected 1 filter object per sensor, got {sensor.num_filter_objects}" + assert sensor.filter_object_names == ["BoxB"], f"unexpected filter_object_names: {sensor.filter_object_names}" + + # (3) Shape-mode, no filter: pattern matches shapes (not bodies). + # `sensor_shape_prim_expr` is a Newton-only extension, so this block uses the + # backend-specific NewtonContactSensorCfg subclass. + with build_simulation_context(sim_cfg=sim_cfg, auto_add_lighting=True, add_ground_plane=True) as sim: + sim._app_control_on_stop_handle = None + scene_cfg = _make_two_box_scene_cfg(num_envs) + scene_cfg.contact_sensor_a = NewtonContactSensorCfg( + prim_path="{ENV_REGEX_NS}/Box.*", + sensor_shape_prim_expr=["{ENV_REGEX_NS}/Box.*"], + update_period=0.0, + history_length=1, + ) + scene = InteractiveScene(scene_cfg) + sim.reset() + scene.reset() + + sensor: ContactSensor = scene["contact_sensor_a"] + assert sensor.num_sensors == 2, f"expected 2 shape sensors per env, got {sensor.num_sensors}" + assert sensor.sensor_names == ["mesh", "mesh"], f"unexpected shape sensor_names: {sensor.sensor_names}" + assert sensor.filter_object_names == [], ( + f"expected empty filter_object_names with no filter, got {sensor.filter_object_names}" + ) + + # =================================================================== # Utility # =================================================================== diff --git a/source/isaaclab_newton/test/sensors/test_frame_transformer.py b/source/isaaclab_newton/test/sensors/test_frame_transformer.py index 513ba9dda918..236fb53f25d9 100644 --- a/source/isaaclab_newton/test/sensors/test_frame_transformer.py +++ b/source/isaaclab_newton/test/sensors/test_frame_transformer.py @@ -168,21 +168,28 @@ def test_frame_transformer_feet_wrt_base(sim): # # reset if count % 25 == 0: # reset root state - root_state = scene.articulations["robot"].data.default_root_state.torch.clone() + root_state = torch.cat( + ( + scene.articulations["robot"].data.default_root_pose.torch, + scene.articulations["robot"].data.default_root_vel.torch, + ), + dim=-1, + ).clone() root_state[:, :3] += scene.env_origins joint_pos = scene.articulations["robot"].data.default_joint_pos.torch joint_vel = scene.articulations["robot"].data.default_joint_vel.torch # -- set root state # -- robot - scene.articulations["robot"].write_root_pose_to_sim(root_state[:, :7]) - scene.articulations["robot"].write_root_velocity_to_sim(root_state[:, 7:]) - scene.articulations["robot"].write_joint_state_to_sim(joint_pos, joint_vel) + scene.articulations["robot"].write_root_pose_to_sim_index(root_pose=root_state[:, :7]) + scene.articulations["robot"].write_root_velocity_to_sim_index(root_velocity=root_state[:, 7:]) + scene.articulations["robot"].write_joint_position_to_sim_index(position=joint_pos) + scene.articulations["robot"].write_joint_velocity_to_sim_index(velocity=joint_vel) # reset buffers scene.reset() # set joint targets robot_actions = default_actions + 0.5 * torch.randn_like(default_actions) - scene.articulations["robot"].set_joint_position_target(robot_actions) + scene.articulations["robot"].set_joint_position_target_index(target=robot_actions) # write data to sim scene.write_data_to_sim() # perform step @@ -266,21 +273,28 @@ def test_frame_transformer_feet_wrt_thigh(sim): # # reset if count % 25 == 0: # reset root state - root_state = scene.articulations["robot"].data.default_root_state.torch.clone() + root_state = torch.cat( + ( + scene.articulations["robot"].data.default_root_pose.torch, + scene.articulations["robot"].data.default_root_vel.torch, + ), + dim=-1, + ).clone() root_state[:, :3] += scene.env_origins joint_pos = scene.articulations["robot"].data.default_joint_pos.torch joint_vel = scene.articulations["robot"].data.default_joint_vel.torch # -- set root state # -- robot - scene.articulations["robot"].write_root_pose_to_sim(root_state[:, :7]) - scene.articulations["robot"].write_root_velocity_to_sim(root_state[:, 7:]) - scene.articulations["robot"].write_joint_state_to_sim(joint_pos, joint_vel) + scene.articulations["robot"].write_root_pose_to_sim_index(root_pose=root_state[:, :7]) + scene.articulations["robot"].write_root_velocity_to_sim_index(root_velocity=root_state[:, 7:]) + scene.articulations["robot"].write_joint_position_to_sim_index(position=joint_pos) + scene.articulations["robot"].write_joint_velocity_to_sim_index(velocity=joint_vel) # reset buffers scene.reset() # set joint targets robot_actions = default_actions + 0.5 * torch.randn_like(default_actions) - scene.articulations["robot"].set_joint_position_target(robot_actions) + scene.articulations["robot"].set_joint_position_target_index(target=robot_actions) # write data to sim scene.write_data_to_sim() # perform step @@ -344,21 +358,28 @@ def test_frame_transformer_robot_body_to_external_cube(sim): # # reset if count % 25 == 0: # reset root state - root_state = scene.articulations["robot"].data.default_root_state.torch.clone() + root_state = torch.cat( + ( + scene.articulations["robot"].data.default_root_pose.torch, + scene.articulations["robot"].data.default_root_vel.torch, + ), + dim=-1, + ).clone() root_state[:, :3] += scene.env_origins joint_pos = scene.articulations["robot"].data.default_joint_pos.torch joint_vel = scene.articulations["robot"].data.default_joint_vel.torch # -- set root state # -- robot - scene.articulations["robot"].write_root_pose_to_sim(root_state[:, :7]) - scene.articulations["robot"].write_root_velocity_to_sim(root_state[:, 7:]) - scene.articulations["robot"].write_joint_state_to_sim(joint_pos, joint_vel) + scene.articulations["robot"].write_root_pose_to_sim_index(root_pose=root_state[:, :7]) + scene.articulations["robot"].write_root_velocity_to_sim_index(root_velocity=root_state[:, 7:]) + scene.articulations["robot"].write_joint_position_to_sim_index(position=joint_pos) + scene.articulations["robot"].write_joint_velocity_to_sim_index(velocity=joint_vel) # reset buffers scene.reset() # set joint targets robot_actions = default_actions + 0.5 * torch.randn_like(default_actions) - scene.articulations["robot"].set_joint_position_target(robot_actions) + scene.articulations["robot"].set_joint_position_target_index(target=robot_actions) # write data to sim scene.write_data_to_sim() # perform step @@ -440,12 +461,18 @@ def test_frame_transformer_offset_frames(sim): # # reset if count % 25 == 0: # reset root state - root_state = scene["cube"].data.default_root_state.torch.clone() + root_state = torch.cat( + ( + scene["cube"].data.default_root_pose.torch, + scene["cube"].data.default_root_vel.torch, + ), + dim=-1, + ).clone() root_state[:, :3] += scene.env_origins # -- set root state # -- cube - scene["cube"].write_root_pose_to_sim(root_state[:, :7]) - scene["cube"].write_root_velocity_to_sim(root_state[:, 7:]) + scene["cube"].write_root_pose_to_sim_index(root_pose=root_state[:, :7]) + scene["cube"].write_root_velocity_to_sim_index(root_velocity=root_state[:, 7:]) # reset buffers scene.reset() @@ -532,21 +559,28 @@ def test_frame_transformer_all_bodies(sim): # # reset if count % 25 == 0: # reset root state - root_state = scene.articulations["robot"].data.default_root_state.torch.clone() + root_state = torch.cat( + ( + scene.articulations["robot"].data.default_root_pose.torch, + scene.articulations["robot"].data.default_root_vel.torch, + ), + dim=-1, + ).clone() root_state[:, :3] += scene.env_origins joint_pos = scene.articulations["robot"].data.default_joint_pos.torch joint_vel = scene.articulations["robot"].data.default_joint_vel.torch # -- set root state # -- robot - scene.articulations["robot"].write_root_pose_to_sim(root_state[:, :7]) - scene.articulations["robot"].write_root_velocity_to_sim(root_state[:, 7:]) - scene.articulations["robot"].write_joint_state_to_sim(joint_pos, joint_vel) + scene.articulations["robot"].write_root_pose_to_sim_index(root_pose=root_state[:, :7]) + scene.articulations["robot"].write_root_velocity_to_sim_index(root_velocity=root_state[:, 7:]) + scene.articulations["robot"].write_joint_position_to_sim_index(position=joint_pos) + scene.articulations["robot"].write_joint_velocity_to_sim_index(velocity=joint_vel) # reset buffers scene.reset() # set joint targets robot_actions = default_actions + 0.5 * torch.randn_like(default_actions) - scene.articulations["robot"].set_joint_position_target(robot_actions) + scene.articulations["robot"].set_joint_position_target_index(target=robot_actions) # write data to sim scene.write_data_to_sim() # perform step @@ -727,22 +761,38 @@ class MultiRobotSceneCfg(InteractiveSceneCfg): # Reset periodically if count % 10 == 0: # Reset robot - root_state = scene.articulations["robot"].data.default_root_state.torch.clone() + root_state = torch.cat( + ( + scene.articulations["robot"].data.default_root_pose.torch, + scene.articulations["robot"].data.default_root_vel.torch, + ), + dim=-1, + ).clone() root_state[:, :3] += scene.env_origins - scene.articulations["robot"].write_root_pose_to_sim(root_state[:, :7]) - scene.articulations["robot"].write_root_velocity_to_sim(root_state[:, 7:]) - scene.articulations["robot"].write_joint_state_to_sim( - scene.articulations["robot"].data.default_joint_pos.torch, - scene.articulations["robot"].data.default_joint_vel.torch, + scene.articulations["robot"].write_root_pose_to_sim_index(root_pose=root_state[:, :7]) + scene.articulations["robot"].write_root_velocity_to_sim_index(root_velocity=root_state[:, 7:]) + scene.articulations["robot"].write_joint_position_to_sim_index( + position=scene.articulations["robot"].data.default_joint_pos.torch + ) + scene.articulations["robot"].write_joint_velocity_to_sim_index( + velocity=scene.articulations["robot"].data.default_joint_vel.torch ) # Reset robot_1 - root_state_1 = scene.articulations["robot_1"].data.default_root_state.torch.clone() + root_state_1 = torch.cat( + ( + scene.articulations["robot_1"].data.default_root_pose.torch, + scene.articulations["robot_1"].data.default_root_vel.torch, + ), + dim=-1, + ).clone() root_state_1[:, :3] += scene.env_origins - scene.articulations["robot_1"].write_root_pose_to_sim(root_state_1[:, :7]) - scene.articulations["robot_1"].write_root_velocity_to_sim(root_state_1[:, 7:]) - scene.articulations["robot_1"].write_joint_state_to_sim( - scene.articulations["robot_1"].data.default_joint_pos.torch, - scene.articulations["robot_1"].data.default_joint_vel.torch, + scene.articulations["robot_1"].write_root_pose_to_sim_index(root_pose=root_state_1[:, :7]) + scene.articulations["robot_1"].write_root_velocity_to_sim_index(root_velocity=root_state_1[:, 7:]) + scene.articulations["robot_1"].write_joint_position_to_sim_index( + position=scene.articulations["robot_1"].data.default_joint_pos.torch + ) + scene.articulations["robot_1"].write_joint_velocity_to_sim_index( + velocity=scene.articulations["robot_1"].data.default_joint_vel.torch ) scene.reset() diff --git a/source/isaaclab_newton/test/sim/test_newton_schemas.py b/source/isaaclab_newton/test/sim/test_newton_schemas.py new file mode 100644 index 000000000000..67ee6265d82c --- /dev/null +++ b/source/isaaclab_newton/test/sim/test_newton_schemas.py @@ -0,0 +1,269 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Tests for Newton and MuJoCo schema cfg classes in isaaclab_newton.""" + +from isaaclab.app import AppLauncher + +# launch omniverse app +simulation_app = AppLauncher(headless=True).app + +"""Rest everything follows.""" + +import pytest +from isaaclab_newton.sim.schemas import ( + MujocoJointDrivePropertiesCfg, + MujocoRigidBodyPropertiesCfg, + NewtonArticulationRootPropertiesCfg, + NewtonCollisionPropertiesCfg, + NewtonJointDrivePropertiesCfg, + NewtonMaterialPropertiesCfg, + NewtonMeshCollisionPropertiesCfg, + NewtonRigidBodyPropertiesCfg, +) + +from pxr import UsdPhysics + +import isaaclab.sim as sim_utils +import isaaclab.sim.schemas as schemas +from isaaclab.sim import SimulationCfg, SimulationContext +from isaaclab.sim.spawners.materials import spawn_rigid_body_material + + +@pytest.fixture +def setup_sim(): + """Fixture to set up and tear down the simulation context.""" + sim_utils.create_new_stage() + sim = SimulationContext(SimulationCfg(dt=0.1)) + yield sim + sim._disable_app_control_on_stop_handle = True + sim.stop() + sim.clear_instance() + + +# --------------------------------------------------------------------------- +# MuJoCo rigid body gravity compensation +# --------------------------------------------------------------------------- + + +@pytest.mark.isaacsim_ci +def test_mujoco_gravcomp_written(setup_sim): + """gravcomp=0.5 must write mjc:gravcomp=0.5 on the prim.""" + stage = sim_utils.get_current_stage() + sim_utils.create_prim("/World/body_gc", prim_type="Cube", translation=(0.0, 0.0, 0.5)) + schemas.define_rigid_body_properties("/World/body_gc", MujocoRigidBodyPropertiesCfg(gravcomp=0.5)) + attr = stage.GetPrimAtPath("/World/body_gc").GetAttribute("mjc:gravcomp") + assert attr.IsValid(), "mjc:gravcomp was not authored" + assert attr.Get() == pytest.approx(0.5) + + +@pytest.mark.isaacsim_ci +def test_mujoco_gravcomp_not_written_when_none(setup_sim): + """gravcomp=None must not write mjc:gravcomp.""" + stage = sim_utils.get_current_stage() + sim_utils.create_prim("/World/body_gc2", prim_type="Cube", translation=(1.0, 0.0, 0.5)) + schemas.define_rigid_body_properties("/World/body_gc2", MujocoRigidBodyPropertiesCfg()) + attr = stage.GetPrimAtPath("/World/body_gc2").GetAttribute("mjc:gravcomp") + assert not attr.IsValid(), "mjc:gravcomp should not be authored when gravcomp=None" + + +# --------------------------------------------------------------------------- +# MuJoCo joint actuator gravity comp +# --------------------------------------------------------------------------- + + +@pytest.mark.isaacsim_ci +def test_mujoco_actuatorgravcomp_written(setup_sim): + """actuatorgravcomp=True must write mjc:actuatorgravcomp=True on the joint prim.""" + stage = sim_utils.get_current_stage() + sim_utils.create_prim("/World/art_gc", prim_type="Xform") + sim_utils.create_prim("/World/art_gc/body0", prim_type="Cube") + sim_utils.create_prim("/World/art_gc/body1", prim_type="Cube") + UsdPhysics.RevoluteJoint.Define(stage, "/World/art_gc/joint0") + schemas.modify_joint_drive_properties("/World/art_gc", MujocoJointDrivePropertiesCfg(actuatorgravcomp=True)) + attr = stage.GetPrimAtPath("/World/art_gc/joint0").GetAttribute("mjc:actuatorgravcomp") + assert attr.IsValid(), "mjc:actuatorgravcomp was not authored" + assert attr.Get() is True + + +@pytest.mark.isaacsim_ci +def test_mujoco_actuatorgravcomp_not_written_when_none(setup_sim): + """actuatorgravcomp=None must not write mjc:actuatorgravcomp.""" + stage = sim_utils.get_current_stage() + sim_utils.create_prim("/World/art_gc2", prim_type="Xform") + sim_utils.create_prim("/World/art_gc2/body0", prim_type="Cube") + sim_utils.create_prim("/World/art_gc2/body1", prim_type="Cube") + UsdPhysics.RevoluteJoint.Define(stage, "/World/art_gc2/joint0") + schemas.modify_joint_drive_properties("/World/art_gc2", MujocoJointDrivePropertiesCfg()) + attr = stage.GetPrimAtPath("/World/art_gc2/joint0").GetAttribute("mjc:actuatorgravcomp") + assert not attr.IsValid(), "mjc:actuatorgravcomp should not be authored when None" + + +# --------------------------------------------------------------------------- +# Newton collision +# --------------------------------------------------------------------------- + + +@pytest.mark.isaacsim_ci +def test_newton_collision_contact_margin_written(setup_sim): + """contact_margin=0.01 must write newton:contactMargin and apply NewtonCollisionAPI.""" + stage = sim_utils.get_current_stage() + sim_utils.create_prim("/World/col_newton", prim_type="Cube", translation=(2.0, 0.0, 0.5)) + schemas.define_collision_properties("/World/col_newton", NewtonCollisionPropertiesCfg(contact_margin=0.01)) + prim = stage.GetPrimAtPath("/World/col_newton") + assert prim.GetAttribute("newton:contactMargin").Get() == pytest.approx(0.01) + assert "NewtonCollisionAPI" in prim.GetAppliedSchemas() + + +@pytest.mark.isaacsim_ci +def test_newton_collision_no_schema_when_none(setup_sim): + """NewtonCollisionPropertiesCfg() with all None must NOT apply NewtonCollisionAPI.""" + stage = sim_utils.get_current_stage() + sim_utils.create_prim("/World/col_newton2", prim_type="Cube", translation=(3.0, 0.0, 0.5)) + schemas.define_collision_properties("/World/col_newton2", NewtonCollisionPropertiesCfg()) + applied = stage.GetPrimAtPath("/World/col_newton2").GetAppliedSchemas() + assert "NewtonCollisionAPI" not in applied + + +# --------------------------------------------------------------------------- +# Newton material +# --------------------------------------------------------------------------- + + +@pytest.mark.isaacsim_ci +def test_newton_material_properties_written(setup_sim): + """torsional_friction and rolling_friction must be written and NewtonMaterialAPI applied.""" + mat_cfg = NewtonMaterialPropertiesCfg(torsional_friction=0.3, rolling_friction=0.001) + prim = spawn_rigid_body_material("/World/newton_mat", mat_cfg) + assert prim.GetAttribute("newton:torsionalFriction").Get() == pytest.approx(0.3) + assert prim.GetAttribute("newton:rollingFriction").Get() == pytest.approx(0.001) + assert "NewtonMaterialAPI" in prim.GetAppliedSchemas() + + +@pytest.mark.isaacsim_ci +def test_newton_material_no_schema_when_none(setup_sim): + """NewtonMaterialPropertiesCfg() with all Newton fields None must NOT apply NewtonMaterialAPI.""" + mat_cfg = NewtonMaterialPropertiesCfg() + prim = spawn_rigid_body_material("/World/newton_mat2", mat_cfg) + assert "NewtonMaterialAPI" not in prim.GetAppliedSchemas() + + +# --------------------------------------------------------------------------- +# Newton articulation root +# --------------------------------------------------------------------------- + + +@pytest.mark.isaacsim_ci +def test_newton_articulation_self_collision_written(setup_sim): + """self_collision_enabled=True must write newton:selfCollisionEnabled and apply the API.""" + stage = sim_utils.get_current_stage() + sim_utils.create_prim("/World/nart", prim_type="Xform") + sim_utils.create_prim("/World/nart/body0", prim_type="Cube") + UsdPhysics.ArticulationRootAPI.Apply(stage.GetPrimAtPath("/World/nart")) + schemas.modify_articulation_root_properties( + "/World/nart", + NewtonArticulationRootPropertiesCfg(self_collision_enabled=True), + ) + prim = stage.GetPrimAtPath("/World/nart") + assert prim.GetAttribute("newton:selfCollisionEnabled").Get() is True + assert "NewtonArticulationRootAPI" in prim.GetAppliedSchemas() + + +@pytest.mark.isaacsim_ci +def test_newton_articulation_no_schema_when_none(setup_sim): + """NewtonArticulationRootPropertiesCfg() with None must NOT apply NewtonArticulationRootAPI.""" + stage = sim_utils.get_current_stage() + sim_utils.create_prim("/World/nart2", prim_type="Xform") + sim_utils.create_prim("/World/nart2/body0", prim_type="Cube") + UsdPhysics.ArticulationRootAPI.Apply(stage.GetPrimAtPath("/World/nart2")) + schemas.modify_articulation_root_properties( + "/World/nart2", + NewtonArticulationRootPropertiesCfg(), + ) + applied = stage.GetPrimAtPath("/World/nart2").GetAppliedSchemas() + assert "NewtonArticulationRootAPI" not in applied + + +# --------------------------------------------------------------------------- +# Newton mesh collision (max_hull_vertices, NewtonMeshCollisionAPI) +# --------------------------------------------------------------------------- + + +@pytest.mark.isaacsim_ci +def test_newton_mesh_collision_max_hull_vertices_written(setup_sim): + """max_hull_vertices=64 must write newton:maxHullVertices and apply NewtonMeshCollisionAPI.""" + stage = sim_utils.get_current_stage() + sim_utils.create_prim("/World/mesh_col", prim_type="Cube", translation=(4.0, 0.0, 0.5)) + schemas.define_mesh_collision_properties( + "/World/mesh_col", + NewtonMeshCollisionPropertiesCfg(mesh_approximation_name="convexHull", max_hull_vertices=64), + ) + prim = stage.GetPrimAtPath("/World/mesh_col") + assert prim.GetAttribute("newton:maxHullVertices").Get() == 64 + assert "NewtonMeshCollisionAPI" in prim.GetAppliedSchemas() + + +@pytest.mark.isaacsim_ci +def test_newton_mesh_collision_no_schema_when_none(setup_sim): + """NewtonMeshCollisionPropertiesCfg() with max_hull_vertices=None must NOT apply NewtonMeshCollisionAPI.""" + stage = sim_utils.get_current_stage() + sim_utils.create_prim("/World/mesh_col2", prim_type="Cube", translation=(5.0, 0.0, 0.5)) + schemas.define_mesh_collision_properties( + "/World/mesh_col2", + NewtonMeshCollisionPropertiesCfg(mesh_approximation_name="convexHull"), + ) + applied = stage.GetPrimAtPath("/World/mesh_col2").GetAppliedSchemas() + assert "NewtonMeshCollisionAPI" not in applied + + +# --------------------------------------------------------------------------- +# Class hierarchy contract: Mujoco IS-A Newton +# --------------------------------------------------------------------------- + + +def test_mujoco_isinstance_newton(): + """MujocoXxxCfg instances must be isinstance of their Newton parent. + + The auto-enable spawner logic and any future polymorphic dispatch on + ``isinstance(cfg, NewtonRigidBodyPropertiesCfg)`` depends on this contract. + """ + mjc_rigid = MujocoRigidBodyPropertiesCfg(gravcomp=0.5) + assert isinstance(mjc_rigid, NewtonRigidBodyPropertiesCfg) + + mjc_joint = MujocoJointDrivePropertiesCfg(actuatorgravcomp=True) + assert isinstance(mjc_joint, NewtonJointDrivePropertiesCfg) + + +# --------------------------------------------------------------------------- +# Multi-namespace mixed write — verify per-declaring-class MRO routing keeps +# fields owned by different classes in different namespaces on the same prim. +# --------------------------------------------------------------------------- + + +@pytest.mark.isaacsim_ci +def test_newton_mesh_collision_mixed_namespace_write(setup_sim): + """A NewtonMeshCollisionPropertiesCfg with both contact_margin (declared on + NewtonCollisionPropertiesCfg) and max_hull_vertices (declared on + NewtonMeshCollisionPropertiesCfg) must write each under its declaring class's + namespace and apply both schemas. + """ + stage = sim_utils.get_current_stage() + sim_utils.create_prim("/World/mesh_mixed", prim_type="Cube", translation=(6.0, 0.0, 0.5)) + schemas.define_mesh_collision_properties( + "/World/mesh_mixed", + NewtonMeshCollisionPropertiesCfg( + mesh_approximation_name="convexHull", + max_hull_vertices=32, + contact_margin=0.005, + ), + ) + prim = stage.GetPrimAtPath("/World/mesh_mixed") + # Both attributes share the newton namespace but are gated on different applied + # schemas (NewtonCollisionAPI for contact_margin, NewtonMeshCollisionAPI for + # max_hull_vertices); per-declaring-class routing applies the right schema for each. + assert prim.GetAttribute("newton:contactMargin").Get() == pytest.approx(0.005) + assert prim.GetAttribute("newton:maxHullVertices").Get() == 32 + applied = prim.GetAppliedSchemas() + assert "NewtonMeshCollisionAPI" in applied diff --git a/source/isaaclab_ov/changelog.d/pbarejko-open-usd.rst b/source/isaaclab_ov/changelog.d/pbarejko-open-usd.rst deleted file mode 100644 index 455768ad5a5c..000000000000 --- a/source/isaaclab_ov/changelog.d/pbarejko-open-usd.rst +++ /dev/null @@ -1,7 +0,0 @@ -Fixed -^^^^^ - -* Fixed ``AttributeError: 'Renderer' object has no attribute 'add_usd'`` in - :class:`~isaaclab_ov.renderers.OVRTXRenderer` when using ``ovrtx`` 0.3.0 or - newer. The renderer now calls :meth:`ovrtx.Renderer.open_usd` on 0.3.0+ and - falls back to ``Renderer.add_usd`` on older versions. diff --git a/source/isaaclab_ov/changelog.d/rschmitt_decouple_renderer_camera.rst b/source/isaaclab_ov/changelog.d/rschmitt_decouple_renderer_camera.rst deleted file mode 100644 index 1de2259dc2c3..000000000000 --- a/source/isaaclab_ov/changelog.d/rschmitt_decouple_renderer_camera.rst +++ /dev/null @@ -1,4 +0,0 @@ -Changed -^^^^^^^^ - -* Modified the OVRTX renderer to use the new patterns from renderer/camera decoupling. diff --git a/source/isaaclab_ov/config/extension.toml b/source/isaaclab_ov/config/extension.toml index ba8f5046c4eb..540638e401ca 100644 --- a/source/isaaclab_ov/config/extension.toml +++ b/source/isaaclab_ov/config/extension.toml @@ -1,5 +1,5 @@ [package] -version = "0.1.3" +version = "0.1.9" title = "Omniverse renderers for IsaacLab" description = "Extension providing Omniverse renderers (OVRTX, ovphysx, etc.) for tiled camera rendering." readme = "docs/README.md" diff --git a/source/isaaclab_ov/docs/CHANGELOG.rst b/source/isaaclab_ov/docs/CHANGELOG.rst index e8afeda30bda..a4846a27bee8 100644 --- a/source/isaaclab_ov/docs/CHANGELOG.rst +++ b/source/isaaclab_ov/docs/CHANGELOG.rst @@ -1,6 +1,112 @@ Changelog --------- +0.1.9 (2026-05-14) +~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* **Breaking:** :class:`~isaaclab_ov.renderers.OVRTXRenderer` now reads the + Newton ``Model`` and ``State`` it binds OVRTX attributes against from + :meth:`~isaaclab_newton.physics.NewtonManager.get_model` / + :meth:`~isaaclab_newton.physics.NewtonManager.get_state` instead of the + removed ``BaseSceneDataProvider.get_newton_model()`` / + ``get_newton_state()``. + + +0.1.8 (2026-05-13) +~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Fixed Newton transform synchronization for Warp 1.13 compatibility in the + RTX renderer. + + +0.1.7 (2026-05-12) +~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Construct the underlying OVRTX ``Renderer`` in + :class:`~isaaclab_ov.renderers.OVRTXRenderer` ``__init__`` instead of + during :meth:`~isaaclab_ov.renderers.OVRTXRenderer.prepare_stage`. This + pairs with the new pre-physics ``__init__`` / + post-physics :meth:`~isaaclab.renderers.base_renderer.BaseRenderer.initialize` + lifecycle: when invoked eagerly via + :meth:`~isaaclab.scene.InteractiveScene.initialize_renderers`, the OVRTX + ``Renderer`` is created before + :meth:`~isaaclab.sim.SimulationContext.reset` (and therefore before + ovphysx initialises), which OVRTX 0.3 requires. +* Replaced an ``assert`` on the OVRTX ``Renderer`` construction with an + explicit :class:`RuntimeError` so the failure is reported even when + Python is run with ``-O``. +* Renamed the internal ``OVRTXRenderer.initialize(spec)`` helper to + ``_initialize_from_spec(spec)`` to avoid shadowing the new + no-arg :meth:`~isaaclab.renderers.base_renderer.BaseRenderer.initialize` + lifecycle hook. + + +0.1.6 (2026-05-09) +~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Set ``keep_system_alive=True`` on the internal OVRTX ``RendererConfig`` in + :class:`~isaaclab_ov.renderers.ovrtx_renderer.OVRTXRenderer` so the renderer + system is not torn down prematurely during pytest sessions. +* Initialize Warp runtime for OvRTX renderer. + + +0.1.5 (2026-05-08) +~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Bumped Newton pin to ``v1.2.0rc2``. Pulls in IsaacLab-relevant fixes from + `newton-physics/newton#2678 `_ + and `newton-physics/newton#2720 + `_ (``SolverKamino`` + reset under ``world_mask``), the upstream tendon-scoping fix from + `newton-physics/newton#2659 + `_ ("Scope USD + custom-frequency parsing"), and a VRAM-leak fix on example reset + (`newton-physics/newton#2710 + `_). +* Newton ``v1.2.0rc2`` requires ``warp-lang==1.13.0``, ``mujoco==3.8.0``, + and ``mujoco-warp==3.8.0.1``. ``warp-lang``/``mujoco``/``mujoco-warp`` + pins live in :mod:`isaaclab` and ``tools/wheel_builder/res/python_packages.toml``; + the Newton pin is mirrored across :mod:`isaaclab_newton`, + :mod:`isaaclab_visualizers` (3×), :mod:`isaaclab_physx` (``[newton]`` + extra), and the wheel-builder TOML. +* Updated ``wp.math.transform_to_matrix`` to ``wp.transform_to_matrix`` in + :mod:`~isaaclab_newton.physics.newton_manager` and + :mod:`~isaaclab_ov.renderers.ovrtx_renderer_kernels` to match the + ``warp-lang`` 1.13 API (the ``wp.math`` namespace was removed). + + +0.1.4 (2026-05-08) +~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Modified the OVRTX renderer to use the new patterns from renderer/camera decoupling. + +Fixed +^^^^^ + +* Fixed ``AttributeError: 'Renderer' object has no attribute 'add_usd'`` in + :class:`~isaaclab_ov.renderers.OVRTXRenderer` when using ``ovrtx`` 0.3.0 or + newer. The renderer now calls :meth:`ovrtx.Renderer.open_usd` on 0.3.0+ and + falls back to ``Renderer.add_usd`` on older versions. + + 0.1.3 (2026-04-30) ~~~~~~~~~~~~~~~~~~ diff --git a/source/isaaclab_ov/isaaclab_ov/renderers/ovrtx_renderer.py b/source/isaaclab_ov/isaaclab_ov/renderers/ovrtx_renderer.py index 5d1782373d87..170ef1d44c66 100644 --- a/source/isaaclab_ov/isaaclab_ov/renderers/ovrtx_renderer.py +++ b/source/isaaclab_ov/isaaclab_ov/renderers/ovrtx_renderer.py @@ -30,6 +30,8 @@ import torch import warp as wp +import isaaclab.utils.warp # noqa: F401 # initializes Warp runtime + # The ovrtx C library links to its own version of the USD libraries. Having # the pxr Python package available can cause the C library to load an # incompatible version of libusd, potentially leading to undefined behavior. @@ -157,6 +159,21 @@ def __init__(self, cfg: OVRTXRendererCfg): self._camera_rel_path: str | None = None self._output_semantic_color_buffer: wp.array | None = None + logger.info("Creating OVRTX renderer...") + OVRTX_CONFIG = RendererConfig( + log_file_path=self.cfg.log_file_path, + log_level=self.cfg.log_level, + read_gpu_transforms=_IS_OVRTX_0_3_0_OR_NEWER, + keep_system_alive=True, + ) + self._renderer = Renderer(OVRTX_CONFIG) + if not self._renderer: + raise RuntimeError( + "Failed to create OVRTX Renderer; the underlying ovrtx.Renderer constructor returned a falsy" + " value. Check that ovrtx is installed correctly and its native dependencies are available." + ) + logger.info("OVRTX renderer created successfully") + def prepare_stage(self, stage: Any, num_envs: int) -> None: """Export the USD stage for OVRTX before create_render_data. @@ -176,7 +193,7 @@ def prepare_stage(self, stage: Any, num_envs: int) -> None: self._exported_usd_path = export_path logger.info("Exported to %s", export_path) - def initialize(self, spec: CameraRenderSpec): + def _initialize_from_spec(self, spec: CameraRenderSpec): """Initialize the OVRTX renderer with internal environment cloning. Args: @@ -196,16 +213,6 @@ def initialize(self, spec: CameraRenderSpec): usd_scene_path = self._exported_usd_path use_cloning = self.cfg.use_cloning - logger.info("Creating OVRTX renderer...") - OVRTX_CONFIG = RendererConfig( - log_file_path=self.cfg.log_file_path, - log_level=self.cfg.log_level, - read_gpu_transforms=_IS_OVRTX_0_3_0_OR_NEWER, - ) - self._renderer = Renderer(OVRTX_CONFIG) - assert self._renderer, "Renderer should be valid after creation" - logger.info("OVRTX renderer created successfully") - if usd_scene_path is not None: logger.info("Injecting camera definitions...") @@ -307,10 +314,9 @@ def _update_scene_partitions_after_clone(self, usd_file_path: str, num_envs: int def _setup_object_bindings(self): """Setup OVRTX bindings for scene objects to sync with Newton physics.""" try: - from isaaclab.sim import SimulationContext + from isaaclab_newton.physics import NewtonManager - provider = SimulationContext.instance().initialize_scene_data_provider() - newton_model = provider.get_newton_model() + newton_model = NewtonManager.get_model() if newton_model is None: logger.info("Newton model not available, skipping object bindings") return @@ -364,7 +370,7 @@ def create_render_data(self, spec: CameraRenderSpec) -> OVRTXRenderData: matching the interface of Isaac RTX and Newton Warp which need no separate initialize(). """ if not self._initialized_scene: - self.initialize(spec) + self._initialize_from_spec(spec) return OVRTXRenderData(spec, DEVICE) # Map torch dtypes to their warp counterparts for zero-copy wrapping. @@ -412,10 +418,9 @@ def update_transforms(self) -> None: return try: - from isaaclab.sim import SimulationContext + from isaaclab_newton.physics import NewtonManager - provider = SimulationContext.instance().initialize_scene_data_provider() - newton_state = provider.get_newton_state() + newton_state = NewtonManager.get_state() if newton_state is None: return body_q = getattr(newton_state, "body_q", None) diff --git a/source/isaaclab_ov/isaaclab_ov/renderers/ovrtx_renderer_kernels.py b/source/isaaclab_ov/isaaclab_ov/renderers/ovrtx_renderer_kernels.py index c287f1257632..0c1626916414 100644 --- a/source/isaaclab_ov/isaaclab_ov/renderers/ovrtx_renderer_kernels.py +++ b/source/isaaclab_ov/isaaclab_ov/renderers/ovrtx_renderer_kernels.py @@ -327,4 +327,4 @@ def sync_newton_transforms_kernel( i = wp.tid() body_idx = newton_body_indices[i] transform = newton_body_q[body_idx] - ovrtx_transforms[i] = wp.transpose(wp.mat44d(wp.math.transform_to_matrix(transform))) + ovrtx_transforms[i] = wp.transpose(wp.mat44d(wp.transform_to_matrix(transform))) diff --git a/source/isaaclab_ovphysx/changelog.d/pr-5458-merge-develop.rst b/source/isaaclab_ovphysx/changelog.d/pr-5458-merge-develop.rst deleted file mode 100644 index 546cb8acc1c7..000000000000 --- a/source/isaaclab_ovphysx/changelog.d/pr-5458-merge-develop.rst +++ /dev/null @@ -1,7 +0,0 @@ -Removed -^^^^^^^ - -* Removed ``ArticulationData.body_incoming_joint_wrench_b`` to match the - shared articulation data API. Code that needs incoming joint reaction - wrenches should use a backend joint-wrench sensor instead of the articulation - data object. diff --git a/source/isaaclab_ovphysx/config/extension.toml b/source/isaaclab_ovphysx/config/extension.toml index 8648b7fa9587..f280545b5e2e 100644 --- a/source/isaaclab_ovphysx/config/extension.toml +++ b/source/isaaclab_ovphysx/config/extension.toml @@ -1,7 +1,7 @@ [package] # Note: Semantic Versioning is used: https://semver.org/ -version = "0.1.2" +version = "1.0.0" # Description title = "OvPhysX simulation interfaces for IsaacLab core package" diff --git a/source/isaaclab_ovphysx/docs/CHANGELOG.rst b/source/isaaclab_ovphysx/docs/CHANGELOG.rst index b2eb969d7845..0858c693523f 100644 --- a/source/isaaclab_ovphysx/docs/CHANGELOG.rst +++ b/source/isaaclab_ovphysx/docs/CHANGELOG.rst @@ -1,6 +1,93 @@ Changelog --------- +1.0.0 (2026-05-14) +~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added :class:`~isaaclab_ovphysx.assets.RigidObject` and + :class:`~isaaclab_ovphysx.assets.RigidObjectData` for single-actor rigid-body + simulation against the OVPhysX backend, satisfying the + :class:`~isaaclab.assets.BaseRigidObject` and + :class:`~isaaclab.assets.BaseRigidObjectData` contracts. Public surface + matches the PhysX/Newton conventions: ``write_root_*_to_sim_index`` / + ``write_root_*_to_sim_mask`` writers (link- and com-frame variants), + ``set_masses_*``, ``set_coms_*``, ``set_inertias_*`` setters, and the + external-wrench composers exposed via + :meth:`~isaaclab_ovphysx.assets.RigidObject.set_external_force_and_torque`. +* Added the ``RIGID_BODY_*`` :class:`TensorType` aliases in + :mod:`isaaclab_ovphysx.tensor_types` (``POSE``, ``VELOCITY``, ``WRENCH``, + ``MASS``, ``COM_POSE``, ``INERTIA``; plus ``ACCELERATION``, ``INV_MASS``, + ``INV_INERTIA`` declared for forward compatibility once the wheel ships + them). +* Added :class:`~isaaclab_ovphysx.assets.kernels` as a shared Warp-kernel + module (frame conversions, state concatenation, finite-difference + acceleration, index- and mask-style scatter writers) consumed by both the + rigid-object and articulation assets. +* Added USD prim-scan validation in + :meth:`~isaaclab_ovphysx.assets.RigidObject._initialize_impl`: a clear + ``RuntimeError`` is raised when ``cfg.prim_path`` resolves to no + ``UsdPhysics.RigidBodyAPI`` prim, multiple rigid-body prims, or a prim with + an enabled ``UsdPhysics.ArticulationRootAPI``. + +Changed +^^^^^^^ + +* Changed :meth:`~isaaclab_ovphysx.physics.OvPhysxManager._release_physx` to + perform a soft reset (``physx.reset()``) and keep the cached + :class:`ovphysx.PhysX` reference alive across + :class:`~isaaclab.sim.SimulationContext` lifetimes, instead of dropping the + reference and triggering the wheel's dual-Carbonite static-destructor race. + :meth:`~isaaclab_ovphysx.physics.OvPhysxManager._warmup_and_load` now reuses + the cached instance on subsequent calls. +* Changed :meth:`~isaaclab_ovphysx.physics.OvPhysxManager._warmup_and_load` to + raise a clear ``RuntimeError`` when a later + :class:`~isaaclab.sim.SimulationContext` requests a different device than + the one the process is locked to, surfacing the wheel's process-global + device-mode lock as a Python error before + :exc:`ovphysx.types.PhysXDeviceError` would fire. +* Changed :meth:`~isaaclab_ovphysx.physics.OvPhysxManager._configure_physx_scene_prim` + to apply the ``UsdPhysics.PhysxSceneAPI`` schema and + ``enableSceneQuerySupport`` on both CPU and GPU; GPU-only attributes + (``enableGPUDynamics``, ``broadphaseType``, the ``gpu*`` capacity attributes + from :class:`~isaaclab_ovphysx.physics.OvPhysxCfg`) remain gated on + ``device == "gpu"``. +* Inherits the base + :attr:`~isaaclab.assets.BaseArticulationData.body_link_jacobian_w`, + :attr:`~isaaclab.assets.BaseArticulationData.body_com_jacobian_w`, + :attr:`~isaaclab.assets.BaseArticulationData.mass_matrix`, and + :attr:`~isaaclab.assets.BaseArticulationData.gravity_compensation_forces` + :class:`NotImplementedError` defaults — ovphysx's OmniGraph-based view + does not expose articulation Jacobians, mass matrices, or gravity + compensation. Use the PhysX or Newton backends for task-space + controllers. + + +0.1.4 (2026-05-09) +~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Fixed OvPhysX articulation tensor reads and writes for ``ovphysx`` 0.4 + compatibility. +* Restored DirectGPU startup settings for OvPhysX GPU simulations. + + +0.1.3 (2026-05-08) +~~~~~~~~~~~~~~~~~~ + +Removed +^^^^^^^ + +* Removed ``ArticulationData.body_incoming_joint_wrench_b`` to match the + shared articulation data API. Code that needs incoming joint reaction + wrenches should use a backend joint-wrench sensor instead of the articulation + data object. + + 0.1.2 (2026-04-23) ~~~~~~~~~~~~~~~~~~ diff --git a/source/isaaclab_ovphysx/isaaclab_ovphysx/assets/__init__.pyi b/source/isaaclab_ovphysx/isaaclab_ovphysx/assets/__init__.pyi index 516e15c5ef6a..52d1a435596a 100644 --- a/source/isaaclab_ovphysx/isaaclab_ovphysx/assets/__init__.pyi +++ b/source/isaaclab_ovphysx/isaaclab_ovphysx/assets/__init__.pyi @@ -6,6 +6,9 @@ __all__ = [ "Articulation", "ArticulationData", + "RigidObject", + "RigidObjectData", ] from .articulation import Articulation, ArticulationData +from .rigid_object import RigidObject, RigidObjectData diff --git a/source/isaaclab_ovphysx/isaaclab_ovphysx/assets/articulation/articulation.py b/source/isaaclab_ovphysx/isaaclab_ovphysx/assets/articulation/articulation.py index f3919d56da73..351467cb6164 100644 --- a/source/isaaclab_ovphysx/isaaclab_ovphysx/assets/articulation/articulation.py +++ b/source/isaaclab_ovphysx/isaaclab_ovphysx/assets/articulation/articulation.py @@ -22,10 +22,11 @@ from isaaclab.utils.wrench_composer import WrenchComposer from isaaclab_ovphysx import tensor_types as TT +from isaaclab_ovphysx.assets.kernels import _body_wrench_to_world, _scatter_rows_partial from isaaclab_ovphysx.physics import OvPhysxManager from .articulation_data import ArticulationData -from .kernels import _body_wrench_to_world, _scatter_rows_partial, update_soft_joint_pos_limits +from .kernels import update_soft_joint_pos_limits if TYPE_CHECKING: from isaaclab.actuators import ActuatorBase @@ -1759,7 +1760,7 @@ def _initialize_impl(self) -> None: # (keyed on object identity) handles the fast path automatically. self._effort_binding = self._get_binding(TT.DOF_ACTUATION_FORCE) if self._effort_binding is not None: - torque = self._data.applied_torque + torque = self._data._applied_torque shape = self._effort_binding.shape self._effort_write_view = wp.array( ptr=torque.ptr, @@ -1780,10 +1781,10 @@ def _make_write_view(tt, buf): return b, v self._pos_target_binding, self._pos_target_write_view = _make_write_view( - TT.DOF_POSITION_TARGET, self._data.joint_pos_target + TT.DOF_POSITION_TARGET, self._data._joint_pos_target ) self._vel_target_binding, self._vel_target_write_view = _make_write_view( - TT.DOF_VELOCITY_TARGET, self._data.joint_vel_target + TT.DOF_VELOCITY_TARGET, self._data._joint_vel_target ) # Let the articulation data know that it is fully instantiated and ready to use. diff --git a/source/isaaclab_ovphysx/isaaclab_ovphysx/assets/articulation/articulation_data.py b/source/isaaclab_ovphysx/isaaclab_ovphysx/assets/articulation/articulation_data.py index 10c7e4b7ecd6..7c59c946dfae 100644 --- a/source/isaaclab_ovphysx/isaaclab_ovphysx/assets/articulation/articulation_data.py +++ b/source/isaaclab_ovphysx/isaaclab_ovphysx/assets/articulation/articulation_data.py @@ -18,18 +18,17 @@ from isaaclab.utils.warp import ProxyArray from isaaclab_ovphysx import tensor_types as TT - -from .kernels import ( - _compose_body_com_poses, +from isaaclab_ovphysx.assets.kernels import ( _compose_root_com_pose, _compute_heading, _copy_first_body, - _fd_joint_acc, _projected_gravity, _world_vel_to_body_ang, _world_vel_to_body_lin, ) +from .kernels import _compose_body_com_poses, _fd_joint_acc + class ArticulationData(BaseArticulationData): """Data container for an articulation backed by ovphysx tensor bindings. @@ -1684,10 +1683,24 @@ def _read_binding_into_flat(self, tensor_type: int, wp_array: wp.array) -> None: Reads directly into the target array -- no scratch buffer, no extra copy. """ + self._read_binding_into_view(tensor_type, wp_array) + + def _read_binding_into_view(self, tensor_type: int, view: wp.array) -> None: + """Read an ovphysx binding into a float32 warp view.""" binding = self._get_binding(tensor_type) if binding is None: return - binding.read(wp_array) + + from isaaclab_ovphysx.tensor_types import _CPU_ONLY_TYPES + + if tensor_type in _CPU_ONLY_TYPES and str(view.device) != "cpu": + scratch = self._get_read_scratch(tensor_type) + if scratch is None: + return + binding.read(scratch) + wp.copy(view, scratch) + else: + binding.read(view) def _read_binding_into_buf(self, tensor_type: int, buf: TimestampedBuffer) -> None: """Read from an ovphysx binding into a TimestampedBuffer, skipping if fresh.""" @@ -1696,7 +1709,7 @@ def _read_binding_into_buf(self, tensor_type: int, buf: TimestampedBuffer) -> No view = self._get_read_view(tensor_type, buf.data) if view is None: return - self._get_binding(tensor_type).read(view) + self._read_binding_into_view(tensor_type, view) buf.timestamp = self._sim_timestamp def _read_transform_binding(self, tensor_type: int, buf: TimestampedBuffer) -> None: @@ -1706,7 +1719,7 @@ def _read_transform_binding(self, tensor_type: int, buf: TimestampedBuffer) -> N view = self._get_read_view(tensor_type, buf.data, 7) if view is None: return - self._get_binding(tensor_type).read(view) + self._read_binding_into_view(tensor_type, view) buf.timestamp = self._sim_timestamp def _read_spatial_vector_binding(self, tensor_type: int, buf: TimestampedBuffer) -> None: @@ -1716,7 +1729,7 @@ def _read_spatial_vector_binding(self, tensor_type: int, buf: TimestampedBuffer) view = self._get_read_view(tensor_type, buf.data, 6) if view is None: return - self._get_binding(tensor_type).read(view) + self._read_binding_into_view(tensor_type, view) buf.timestamp = self._sim_timestamp """ diff --git a/source/isaaclab_ovphysx/isaaclab_ovphysx/assets/articulation/kernels.py b/source/isaaclab_ovphysx/isaaclab_ovphysx/assets/articulation/kernels.py index b93c4e6d4b41..cc9faf15753a 100644 --- a/source/isaaclab_ovphysx/isaaclab_ovphysx/assets/articulation/kernels.py +++ b/source/isaaclab_ovphysx/isaaclab_ovphysx/assets/articulation/kernels.py @@ -8,41 +8,6 @@ import warp as wp -@wp.kernel -def _body_wrench_to_world( - force_b: wp.array(dtype=wp.vec3f, ndim=2), - torque_b: wp.array(dtype=wp.vec3f, ndim=2), - poses: wp.array(dtype=wp.transformf, ndim=2), - wrench_out: wp.array(dtype=wp.float32, ndim=3), -): - """Rotate body-frame force/torque to world frame and pack into [N, L, 9].""" - i, j = wp.tid() - q = wp.transform_get_rotation(poses[i, j]) - f_w = wp.quat_rotate(q, force_b[i, j]) - t_w = wp.quat_rotate(q, torque_b[i, j]) - wrench_out[i, j, 0] = f_w[0] - wrench_out[i, j, 1] = f_w[1] - wrench_out[i, j, 2] = f_w[2] - wrench_out[i, j, 3] = t_w[0] - wrench_out[i, j, 4] = t_w[1] - wrench_out[i, j, 5] = t_w[2] - p_w = wp.transform_get_translation(poses[i, j]) - wrench_out[i, j, 6] = p_w[0] - wrench_out[i, j, 7] = p_w[1] - wrench_out[i, j, 8] = p_w[2] - - -@wp.kernel -def _scatter_rows_partial( - dst: wp.array2d(dtype=wp.float32), - src: wp.array2d(dtype=wp.float32), - ids: wp.array(dtype=wp.int32), -): - """dst[ids[i], j] = src[i, j] -- scatter partial [K,C] into full [N,C] on GPU.""" - i, j = wp.tid() - dst[ids[i], j] = src[i, j] - - @wp.func def compute_soft_joint_pos_limits_func( joint_pos_limits: wp.vec2f, @@ -93,38 +58,6 @@ def _fd_joint_acc( prev_vel[i, j] = cur_vel[i, j] -@wp.kernel -def _copy_first_body( - body_vel: wp.array(dtype=wp.spatial_vectorf, ndim=2), - root_vel: wp.array(dtype=wp.spatial_vectorf), -): - """Copy the first body's velocity to the root velocity buffer. - - Args: - body_vel: Body velocities. Shape is (num_envs, num_bodies). - root_vel: Output root velocities. Shape is (num_envs,). - """ - i = wp.tid() - root_vel[i] = body_vel[i, 0] - - -@wp.kernel -def _compose_root_com_pose( - link_pose: wp.array(dtype=wp.transformf), - com_pose_b: wp.array(dtype=wp.transformf, ndim=2), - com_pose_w: wp.array(dtype=wp.transformf), -): - """Compose root link pose with body-frame CoM offset to get world-frame root CoM pose. - - Args: - link_pose: Root link poses in world frame. Shape is (num_envs,). - com_pose_b: Body-frame CoM offsets. Shape is (num_envs, num_bodies). - com_pose_w: Output world-frame root CoM poses. Shape is (num_envs,). - """ - i = wp.tid() - com_pose_w[i] = wp.transform_multiply(link_pose[i], com_pose_b[i, 0]) - - @wp.kernel def _compose_body_com_poses( link_pose: wp.array(dtype=wp.transformf, ndim=2), @@ -140,78 +73,3 @@ def _compose_body_com_poses( """ i, j = wp.tid() com_pose_w[i, j] = wp.transform_multiply(link_pose[i, j], com_pose_b[i, j]) - - -@wp.kernel -def _projected_gravity( - gravity_vec_w: wp.array(dtype=wp.vec3f), - root_pose: wp.array(dtype=wp.transformf), - out: wp.array(dtype=wp.vec3f), -): - """Project world-frame gravity direction into the root body frame. - - Args: - gravity_vec_w: Gravity unit vector per instance in world frame. Shape is (num_envs,). - root_pose: Root link poses in world frame. Shape is (num_envs,). - out: Output projected gravity in body frame. Shape is (num_envs,). - """ - i = wp.tid() - q = wp.transform_get_rotation(root_pose[i]) - out[i] = wp.quat_rotate_inv(q, gravity_vec_w[i]) - - -@wp.kernel -def _compute_heading( - forward_vec_b: wp.array(dtype=wp.vec3f), - root_pose: wp.array(dtype=wp.transformf), - out: wp.array(dtype=wp.float32), -): - """Compute yaw heading angle from the forward direction rotated into the world frame. - - Args: - forward_vec_b: Forward direction in body frame per instance. Shape is (num_envs,). - root_pose: Root link poses in world frame. Shape is (num_envs,). - out: Output heading angles [rad]. Shape is (num_envs,). - """ - i = wp.tid() - q = wp.transform_get_rotation(root_pose[i]) - forward = wp.quat_rotate(q, forward_vec_b[i]) - out[i] = wp.atan2(forward[1], forward[0]) - - -@wp.kernel -def _world_vel_to_body_lin( - root_pose: wp.array(dtype=wp.transformf), - vel_w: wp.array(dtype=wp.spatial_vectorf), - out: wp.array(dtype=wp.vec3f), -): - """Rotate world-frame linear velocity into the root body frame. - - Args: - root_pose: Root link poses in world frame. Shape is (num_envs,). - vel_w: Spatial velocities in world frame. Shape is (num_envs,). - out: Output linear velocity in body frame. Shape is (num_envs,). - """ - i = wp.tid() - q = wp.transform_get_rotation(root_pose[i]) - lin = wp.spatial_top(vel_w[i]) - out[i] = wp.quat_rotate_inv(q, lin) - - -@wp.kernel -def _world_vel_to_body_ang( - root_pose: wp.array(dtype=wp.transformf), - vel_w: wp.array(dtype=wp.spatial_vectorf), - out: wp.array(dtype=wp.vec3f), -): - """Rotate world-frame angular velocity into the root body frame. - - Args: - root_pose: Root link poses in world frame. Shape is (num_envs,). - vel_w: Spatial velocities in world frame. Shape is (num_envs,). - out: Output angular velocity in body frame. Shape is (num_envs,). - """ - i = wp.tid() - q = wp.transform_get_rotation(root_pose[i]) - ang = wp.spatial_bottom(vel_w[i]) - out[i] = wp.quat_rotate_inv(q, ang) diff --git a/source/isaaclab_ovphysx/isaaclab_ovphysx/assets/kernels.py b/source/isaaclab_ovphysx/isaaclab_ovphysx/assets/kernels.py new file mode 100644 index 000000000000..cf49c8362636 --- /dev/null +++ b/source/isaaclab_ovphysx/isaaclab_ovphysx/assets/kernels.py @@ -0,0 +1,1108 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import warp as wp + +vec13f = wp.types.vector(length=13, dtype=wp.float32) + +""" +Shared @wp.func helpers. +""" + + +@wp.func +def get_link_vel_from_root_com_vel_func( + com_vel: wp.spatial_vectorf, + link_pose: wp.transformf, + body_com_pose: wp.transformf, +): + """Compute link velocity from center-of-mass velocity. + + Transforms a COM spatial velocity into a link-frame velocity by projecting + the angular velocity contribution from the COM offset relative to the link frame. + + Args: + com_vel: COM spatial velocity (angular, linear). + link_pose: Link pose in world frame. + body_com_pose: COM pose in body (link) frame. + + Returns: + Link spatial velocity (angular, linear). + """ + projected_vel = wp.cross( + wp.spatial_bottom(com_vel), + wp.quat_rotate(wp.transform_get_rotation(link_pose), -wp.transform_get_translation(body_com_pose)), + ) + return wp.spatial_vector(wp.spatial_top(com_vel) + projected_vel, wp.spatial_bottom(com_vel)) + + +@wp.func +def get_com_pose_from_link_pose_func( + link_pose: wp.transformf, + body_com_pose: wp.transformf, +): + """Compute COM pose in world frame from link pose and body-frame COM offset. + + Args: + link_pose: Link pose in world frame. + body_com_pose: COM pose in body (link) frame. + + Returns: + COM pose in world frame. + """ + return link_pose * body_com_pose + + +@wp.func +def concat_pose_and_vel_to_state_func( + pose: wp.transformf, + vel: wp.spatial_vectorf, +) -> vec13f: + """Concatenate a pose and velocity into a 13-element state vector. + + The state vector layout is [pos(3), quat(4), ang_vel(3), lin_vel(3)]. + + Args: + pose: Pose as a transform (position + quaternion). + vel: Spatial velocity (angular, linear). + + Returns: + 13-element state vector. + """ + return vec13f( + pose[0], pose[1], pose[2], pose[3], pose[4], pose[5], pose[6], vel[0], vel[1], vel[2], vel[3], vel[4], vel[5] + ) + + +@wp.func +def compute_heading_w_func( + forward_vec: wp.vec3f, + quat: wp.quatf, +): + """Compute heading angle (yaw) in world frame from a forward vector and orientation. + + Rotates the forward vector by the quaternion and computes atan2(y, x). + + Args: + forward_vec: Forward direction vector in body frame. + quat: Orientation quaternion. + + Returns: + Heading angle in radians. + """ + forward_w = wp.quat_rotate(quat, forward_vec) + return wp.atan2(forward_w[1], forward_w[0]) + + +@wp.func +def set_state_transforms_func( + state: vec13f, + transform: wp.transformf, +) -> vec13f: + """Set the pose portion (first 7 elements) of a 13-element state vector. + + Overwrites elements [0..6] (position + quaternion) with the given transform, + leaving the velocity portion [7..12] unchanged. + + Args: + state: 13-element state vector to modify. + transform: New pose (position + quaternion). + + Returns: + Updated 13-element state vector. + """ + state[0] = transform[0] + state[1] = transform[1] + state[2] = transform[2] + state[3] = transform[3] + state[4] = transform[4] + state[5] = transform[5] + state[6] = transform[6] + return state + + +@wp.func +def set_state_velocities_func( + state: vec13f, + velocity: wp.spatial_vectorf, +) -> vec13f: + """Set the velocity portion (last 6 elements) of a 13-element state vector. + + Overwrites elements [7..12] (angular + linear velocity) with the given spatial velocity, + leaving the pose portion [0..6] unchanged. + + Args: + state: 13-element state vector to modify. + velocity: New spatial velocity (angular, linear). + + Returns: + Updated 13-element state vector. + """ + state[7] = velocity[0] + state[8] = velocity[1] + state[9] = velocity[2] + state[10] = velocity[3] + state[11] = velocity[4] + state[12] = velocity[5] + return state + + +@wp.func +def get_link_velocity_in_com_frame_func( + link_velocity_w: wp.spatial_vectorf, + link_pose_w: wp.transformf, + body_com_pose_b: wp.transformf, +): + """Compute COM velocity from link velocity by accounting for the COM offset. + + Transforms a link-frame spatial velocity into a COM-frame velocity by adding + the cross-product contribution of the COM offset rotated into the world frame. + + Args: + link_velocity_w: Link spatial velocity in world frame (angular, linear). + link_pose_w: Link pose in world frame. + body_com_pose_b: COM pose in body (link) frame. + + Returns: + COM spatial velocity in world frame (angular, linear). + """ + return wp.spatial_vector( + wp.spatial_top(link_velocity_w) + + wp.cross( + wp.spatial_bottom(link_velocity_w), + wp.quat_rotate(wp.transform_get_rotation(link_pose_w), wp.transform_get_translation(body_com_pose_b)), + ), + wp.spatial_bottom(link_velocity_w), + ) + + +@wp.func +def get_com_pose_in_link_frame_func( + com_pose_w: wp.transformf, + com_pose_b: wp.transformf, +): + """Compute link pose in world frame from COM pose by inverting the body-frame COM offset. + + This is the inverse of ``get_com_pose_from_link_pose_func``. Given the COM pose in + world frame and the COM offset in body frame, it recovers the link pose in world frame. + + Args: + com_pose_w: COM pose in world frame. + com_pose_b: COM pose in body (link) frame. + + Returns: + Link pose in world frame. + """ + T2 = wp.transform( + wp.quat_rotate( + wp.quat_inverse(wp.transform_get_rotation(com_pose_b)), -wp.transform_get_translation(com_pose_b) + ), + wp.quat_inverse(wp.transform_get_rotation(com_pose_b)), + ) + link_pose_w = com_pose_w * T2 + return link_pose_w + + +""" +Root-level @wp.kernel (1D — used by RigidObject + Articulation). +""" + + +@wp.kernel +def get_root_link_vel_from_root_com_vel( + com_vel: wp.array(dtype=wp.spatial_vectorf), + link_pose: wp.array(dtype=wp.transformf), + body_com_pose_b: wp.array2d(dtype=wp.transformf), + link_vel: wp.array(dtype=wp.spatial_vectorf), +): + """Compute root link velocity from root center-of-mass velocity. + + This kernel transforms the root COM velocity into link-frame velocity by projecting + the angular velocity contribution from the COM offset. + + Args: + com_vel: Input array of root COM spatial velocities. Shape is (num_envs,). + link_pose: Input array of root link poses in world frame. Shape is (num_envs,). + body_com_pose_b: Input array of body COM poses in body frame. Shape is (num_envs, num_bodies). + Only the first body (index 0) is used for the root. + link_vel: Output array where root link velocities are written. Shape is (num_envs,). + """ + i = wp.tid() + link_vel[i] = get_link_vel_from_root_com_vel_func(com_vel[i], link_pose[i], body_com_pose_b[i, 0]) + + +@wp.kernel +def get_root_com_pose_from_root_link_pose( + link_pose: wp.array(dtype=wp.transformf), + body_com_pose_b: wp.array2d(dtype=wp.transformf), + com_pose_w: wp.array(dtype=wp.transformf), +): + """Compute root COM pose from root link pose. + + This kernel transforms the root link pose to the root COM pose using the body COM offset. + + Args: + link_pose: Input array of root link poses in world frame. Shape is (num_envs,). + body_com_pose_b: Input array of body COM poses in body frame. Shape is (num_envs, num_bodies). + Only the first body (index 0) is used for the root. + com_pose_w: Output array where root COM poses are written. Shape is (num_envs,). + """ + i = wp.tid() + com_pose_w[i] = get_com_pose_from_link_pose_func(link_pose[i], body_com_pose_b[i, 0]) + + +@wp.kernel +def concat_root_pose_and_vel_to_state( + pose: wp.array(dtype=wp.transformf), + vel: wp.array(dtype=wp.spatial_vectorf), + state: wp.array(dtype=vec13f), +): + """Concatenate root pose and velocity into a 13-element state vector. + + This kernel combines a 7-element pose (pos + quat) and a 6-element velocity + (angular + linear) into a single 13-element state vector. + + Args: + pose: Input array of root poses in world frame. Shape is (num_envs,). + vel: Input array of root spatial velocities. Shape is (num_envs,). + state: Output array where concatenated state vectors are written. Shape is (num_envs,). + """ + i = wp.tid() + state[i] = concat_pose_and_vel_to_state_func(pose[i], vel[i]) + + +@wp.kernel +def split_state_to_root_pose_and_vel( + state: wp.array2d(dtype=wp.float32), + pose: wp.array(dtype=wp.transformf), + vel: wp.array(dtype=wp.spatial_vectorf), +): + """Split a 13-element state vector into root pose and velocity. + + This kernel extracts a 7-element pose (pos + quat) and a 6-element velocity + (angular + linear) from a 13-element state vector. + + Args: + state: Input array of root states. Shape is (num_envs, 13). + pose: Output array where root poses are written. Shape is (num_envs,). + vel: Output array where root spatial velocities are written. Shape is (num_envs,). + """ + i = wp.tid() + # Extract pose: [pos(3), quat(4)] = state[0:7] + pose[i] = wp.transform( + wp.vec3f(state[i, 0], state[i, 1], state[i, 2]), wp.quatf(state[i, 3], state[i, 4], state[i, 5], state[i, 6]) + ) + # Extract velocity: [ang_vel(3), lin_vel(3)] = state[7:13] + vel[i] = wp.spatial_vector( + wp.vec3f(state[i, 7], state[i, 8], state[i, 9]), # angular velocity + wp.vec3f(state[i, 10], state[i, 11], state[i, 12]), # linear velocity + ) + + +""" +Body-level @wp.kernel (2D — used by Articulation + RigidObjectCollection). +""" + + +@wp.kernel +def get_body_link_vel_from_body_com_vel( + body_com_vel: wp.array2d(dtype=wp.spatial_vectorf), + body_link_pose: wp.array2d(dtype=wp.transformf), + body_com_pose: wp.array2d(dtype=wp.transformf), + body_link_vel: wp.array2d(dtype=wp.spatial_vectorf), +): + """Compute body link velocities from body COM velocities for all bodies. + + This kernel transforms COM velocities into link-frame velocities by projecting + the angular velocity contribution from the COM offset, for each body in each environment. + + Args: + body_com_vel: Input array of body COM spatial velocities. Shape is (num_envs, num_bodies). + body_link_pose: Input array of body link poses in world frame. Shape is (num_envs, num_bodies). + body_com_pose: Input array of body COM poses in body frame. Shape is (num_envs, num_bodies). + body_link_vel: Output array where body link velocities are written. Shape is (num_envs, num_bodies). + """ + i, j = wp.tid() + body_link_vel[i, j] = get_link_vel_from_root_com_vel_func( + body_com_vel[i, j], body_link_pose[i, j], body_com_pose[i, j] + ) + + +@wp.kernel +def get_body_com_pose_from_body_link_pose( + body_link_pose: wp.array2d(dtype=wp.transformf), + body_com_pose_b: wp.array2d(dtype=wp.transformf), + body_com_pose_w: wp.array2d(dtype=wp.transformf), +): + """Compute body COM poses from body link poses for all bodies. + + This kernel transforms link poses to COM poses using the body COM offset in the body frame. + + Args: + body_link_pose: Input array of body link poses in world frame. Shape is (num_envs, num_bodies). + body_com_pose_b: Input array of body COM poses in body frame. Shape is (num_envs, num_bodies). + body_com_pose_w: Output array where body COM poses in world frame are written. + Shape is (num_envs, num_bodies). + """ + i, j = wp.tid() + body_com_pose_w[i, j] = get_com_pose_from_link_pose_func(body_link_pose[i, j], body_com_pose_b[i, j]) + + +@wp.kernel +def concat_body_pose_and_vel_to_state( + pose: wp.array2d(dtype=wp.transformf), + vel: wp.array2d(dtype=wp.spatial_vectorf), + state: wp.array2d(dtype=vec13f), +): + """Concatenate body pose and velocity into 13-element state vectors for all bodies. + + This kernel combines a 7-element pose (pos + quat) and a 6-element velocity + (angular + linear) into a single 13-element state vector, for each body in each environment. + + Args: + pose: Input array of body poses in world frame. Shape is (num_envs, num_bodies). + vel: Input array of body spatial velocities. Shape is (num_envs, num_bodies). + state: Output array where concatenated state vectors are written. + Shape is (num_envs, num_bodies). + """ + i, j = wp.tid() + state[i, j] = concat_pose_and_vel_to_state_func(pose[i, j], vel[i, j]) + + +""" +Derived property kernels. +""" + + +@wp.kernel +def quat_apply_inverse_1D_kernel( + gravity: wp.array(dtype=wp.vec3f), + quat: wp.array(dtype=wp.quatf), + projected_gravity: wp.array(dtype=wp.vec3f), +): + """Apply inverse quaternion rotation to gravity vectors (1D). + + This kernel rotates gravity vectors into the local frame of each environment + using the inverse of the provided quaternion. + + Args: + gravity: Input array of gravity vectors in world frame. Shape is (num_envs,). + quat: Input array of quaternions representing orientations. Shape is (num_envs,). + projected_gravity: Output array where projected gravity vectors are written. + Shape is (num_envs,). + """ + i = wp.tid() + projected_gravity[i] = wp.quat_rotate_inv(quat[i], gravity[i]) + + +@wp.kernel +def root_heading_w( + forward_vec: wp.array(dtype=wp.vec3f), + quat: wp.array(dtype=wp.quatf), + heading_w: wp.array(dtype=wp.float32), +): + """Compute root heading angle in the world frame. + + This kernel computes the heading angle (yaw) by rotating the forward vector + by the root quaternion and computing atan2 of the resulting x and y components. + + Args: + forward_vec: Input array of forward direction vectors. Shape is (num_envs,). + quat: Input array of root quaternions. Shape is (num_envs,). + heading_w: Output array where heading angles (radians) are written. Shape is (num_envs,). + """ + i = wp.tid() + heading_w[i] = compute_heading_w_func(forward_vec[i], quat[i]) + + +@wp.kernel +def quat_apply_inverse_2D_kernel( + vec: wp.array2d(dtype=wp.vec3f), + quat: wp.array2d(dtype=wp.quatf), + result: wp.array2d(dtype=wp.vec3f), +): + """Apply inverse quaternion rotation to vectors (2D). + + This kernel rotates vectors into the local frame of each body in each environment + using the inverse of the provided quaternion. + + Args: + vec: Input array of vectors in world frame. Shape is (num_envs, num_bodies). + quat: Input array of quaternions representing orientations. Shape is (num_envs, num_bodies). + result: Output array where rotated vectors are written. Shape is (num_envs, num_bodies). + """ + i, j = wp.tid() + result[i, j] = wp.quat_rotate_inv(quat[i, j], vec[i, j]) + + +@wp.kernel +def body_heading_w( + forward_vec: wp.array2d(dtype=wp.vec3f), + quat: wp.array2d(dtype=wp.quatf), + heading_w: wp.array2d(dtype=wp.float32), +): + """Compute body heading angles in the world frame for all bodies. + + This kernel computes heading angles (yaw) by rotating forward vectors + by body quaternions and computing atan2 of the resulting x and y components. + + Args: + forward_vec: Input array of forward direction vectors. Shape is (num_envs, num_bodies). + quat: Input array of body quaternions. Shape is (num_envs, num_bodies). + heading_w: Output array where heading angles (radians) are written. + Shape is (num_envs, num_bodies). + """ + i, j = wp.tid() + heading_w[i, j] = compute_heading_w_func(forward_vec[i, j], quat[i, j]) + + +""" +Root-level write kernels (1D — used by RigidObject + Articulation). +""" + + +@wp.kernel +def set_root_link_pose_to_sim_index( + data: wp.array(dtype=wp.transformf), + env_ids: wp.array(dtype=wp.int32), + root_link_pose_w: wp.array(dtype=wp.transformf), +): + """Write root link pose data to simulation buffers. + + This kernel scatters root link poses from the partial input array into the cached + world-frame buffer at the specified environment indices. + + Args: + data: Input array of root link poses. Shape is (num_selected_envs,). + env_ids: Input array of environment indices to write to. Shape is (num_selected_envs,). + root_link_pose_w: Output array where root link poses are written. Shape is (num_envs,). + """ + i = wp.tid() + root_link_pose_w[env_ids[i]] = data[i] + + +@wp.kernel +def set_root_com_pose_to_sim_index( + data: wp.array(dtype=wp.transformf), + body_com_pose_b: wp.array2d(dtype=wp.transformf), + env_ids: wp.array(dtype=wp.int32), + root_com_pose_w: wp.array(dtype=wp.transformf), + root_link_pose_w: wp.array(dtype=wp.transformf), +): + """Write root COM pose data to simulation buffers. + + This kernel scatters root COM poses from the partial input array into the cached + world-frame buffer at the specified environment indices and derives the + corresponding link pose via the body-frame COM offset. + + Args: + data: Input array of root COM poses. Shape is (num_selected_envs,). + body_com_pose_b: Input array of body COM poses in body frame. Shape is + (num_envs, num_bodies). Only the first body (index 0) is used for the root. + env_ids: Input array of environment indices to write to. Shape is (num_selected_envs,). + root_com_pose_w: Output array where root COM poses are written. Shape is (num_envs,). + root_link_pose_w: Output array where root link poses (derived from COM) are written. + Shape is (num_envs,). + """ + i = wp.tid() + root_com_pose_w[env_ids[i]] = data[i] + # Get the com pose in the link frame + root_link_pose_w[env_ids[i]] = get_com_pose_in_link_frame_func( + root_com_pose_w[env_ids[i]], body_com_pose_b[env_ids[i], 0] + ) + + +@wp.kernel +def set_root_com_velocity_to_sim_index( + data: wp.array(dtype=wp.spatial_vectorf), + env_ids: wp.array(dtype=wp.int32), + num_bodies: wp.int32, + root_com_velocity_w: wp.array(dtype=wp.spatial_vectorf), + body_acc_w: wp.array2d(dtype=wp.spatial_vectorf), +): + """Write root COM velocity data to simulation buffers. + + This kernel scatters root COM velocities from the partial input array into the cached + world-frame buffer at the specified environment indices and zeros the body acceleration + buffer to prevent reporting stale values. + + Args: + data: Input array of root COM spatial velocities. Shape is (num_selected_envs,). + env_ids: Input array of environment indices to write to. Shape is (num_selected_envs,). + num_bodies: Input scalar number of bodies per environment. + root_com_velocity_w: Output array where root COM velocities are written. Shape is (num_envs,). + body_acc_w: Output array where body accelerations are zeroed. Shape is + (num_envs, num_bodies). + """ + i = wp.tid() + root_com_velocity_w[env_ids[i]] = data[i] + # Make the acceleration zero to prevent reporting old values + for j in range(num_bodies): + body_acc_w[env_ids[i], j] = wp.spatial_vectorf(0.0, 0.0, 0.0, 0.0, 0.0, 0.0) + + +@wp.kernel +def set_root_link_velocity_to_sim_index( + data: wp.array(dtype=wp.spatial_vectorf), + body_com_pose_b: wp.array2d(dtype=wp.transformf), + link_pose_w: wp.array(dtype=wp.transformf), + env_ids: wp.array(dtype=wp.int32), + num_bodies: wp.int32, + root_link_velocity_w: wp.array(dtype=wp.spatial_vectorf), + root_com_velocity_w: wp.array(dtype=wp.spatial_vectorf), + body_acc_w: wp.array2d(dtype=wp.spatial_vectorf), +): + """Write root link velocity data to simulation buffers. + + This kernel scatters root link velocities from the partial input array into the cached + world-frame buffer at the specified environment indices, derives the corresponding + COM velocity via the lever-arm transform, and zeros the body acceleration buffer. + + Args: + data: Input array of root link spatial velocities. Shape is (num_selected_envs,). + body_com_pose_b: Input array of body COM poses in body frame. Shape is + (num_envs, num_bodies). Only the first body (index 0) is used for the root. + link_pose_w: Input array of root link poses in world frame. Shape is (num_envs,). + env_ids: Input array of environment indices to write to. Shape is (num_selected_envs,). + num_bodies: Input scalar number of bodies per environment. + root_link_velocity_w: Output array where root link velocities are written. + Shape is (num_envs,). + root_com_velocity_w: Output array where root COM velocities (derived from link) + are written. Shape is (num_envs,). + body_acc_w: Output array where body accelerations are zeroed. + Shape is (num_envs, num_bodies). + """ + i = wp.tid() + root_link_velocity_w[env_ids[i]] = data[i] + # Get the link velocity in the com frame + root_com_velocity_w[env_ids[i]] = get_link_velocity_in_com_frame_func( + root_link_velocity_w[env_ids[i]], link_pose_w[env_ids[i]], body_com_pose_b[env_ids[i], 0] + ) + # Make the acceleration zero to prevent reporting old values + for j in range(num_bodies): + body_acc_w[env_ids[i], j] = wp.spatial_vectorf(0.0, 0.0, 0.0, 0.0, 0.0, 0.0) + + +""" +Body-level write kernels (2D — used by RigidObjectCollection). +""" + + +@wp.kernel +def set_body_link_pose_to_sim( + data: wp.array2d(dtype=wp.transformf), + env_ids: wp.array(dtype=wp.int32), + body_ids: wp.array(dtype=wp.int32), + from_mask: bool, + body_link_pose_w: wp.array2d(dtype=wp.transformf), + body_link_state_w: wp.array2d(dtype=vec13f), + body_state_w: wp.array2d(dtype=vec13f), +): + """Write body link pose data to simulation buffers. + + This kernel writes body link poses from the input array to the output buffers + and optionally updates the corresponding state vectors, for each body in each environment. + + Args: + data: Input array of body link poses. Shape is (num_envs, num_bodies) or + (num_selected_envs, num_selected_bodies) depending on from_mask. + env_ids: Input array of environment indices to write to. Shape is (num_selected_envs,). + body_ids: Input array of body indices to write to. Shape is (num_selected_bodies,). + from_mask: Input flag indicating whether to use masked indexing. + body_link_pose_w: Output array where body link poses are written. + Shape is (num_envs, num_bodies). + body_link_state_w: Output array where body link states are updated (pose portion). + Shape is (num_envs, num_bodies). Can be None if not needed. + body_state_w: Output array where body states are updated (pose portion). + Shape is (num_envs, num_bodies). Can be None if not needed. + """ + i, j = wp.tid() + if from_mask: + body_link_pose_w[env_ids[i], body_ids[j]] = data[env_ids[i], body_ids[j]] + if body_link_state_w: + body_link_state_w[env_ids[i], body_ids[j]] = set_state_transforms_func( + body_link_state_w[env_ids[i], body_ids[j]], data[env_ids[i], body_ids[j]] + ) + if body_state_w: + body_state_w[env_ids[i], body_ids[j]] = set_state_transforms_func( + body_state_w[env_ids[i], body_ids[j]], data[env_ids[i], body_ids[j]] + ) + else: + body_link_pose_w[env_ids[i], body_ids[j]] = data[i, j] + if body_link_state_w: + body_link_state_w[env_ids[i], body_ids[j]] = set_state_transforms_func( + body_link_state_w[env_ids[i], body_ids[j]], data[i, j] + ) + if body_state_w: + body_state_w[env_ids[i], body_ids[j]] = set_state_transforms_func( + body_state_w[env_ids[i], body_ids[j]], data[i, j] + ) + + +@wp.kernel +def set_body_com_pose_to_sim( + data: wp.array2d(dtype=wp.transformf), + body_com_pose_b: wp.array2d(dtype=wp.transformf), + env_ids: wp.array(dtype=wp.int32), + body_ids: wp.array(dtype=wp.int32), + from_mask: bool, + body_com_pose_w: wp.array2d(dtype=wp.transformf), + body_link_pose_w: wp.array2d(dtype=wp.transformf), + body_com_state_w: wp.array2d(dtype=vec13f), + body_link_state_w: wp.array2d(dtype=vec13f), + body_state_w: wp.array2d(dtype=vec13f), +): + """Write body COM pose data to simulation buffers. + + This kernel writes body COM poses from the input array to the output buffers, + computes the corresponding link poses from the COM poses, and optionally updates + the corresponding state vectors, for each body in each environment. + + Args: + data: Input array of body COM poses. Shape is (num_envs, num_bodies) or + (num_selected_envs, num_selected_bodies) depending on from_mask. + body_com_pose_b: Input array of body COM poses in body frame. Shape is + (num_envs, num_bodies). + env_ids: Input array of environment indices to write to. Shape is (num_selected_envs,). + body_ids: Input array of body indices to write to. Shape is (num_selected_bodies,). + from_mask: Input flag indicating whether to use masked indexing. + body_com_pose_w: Output array where body COM poses are written. + Shape is (num_envs, num_bodies). + body_link_pose_w: Output array where body link poses (derived from COM) are written. + Shape is (num_envs, num_bodies). + body_com_state_w: Output array where body COM states are updated (pose portion). + Shape is (num_envs, num_bodies). Can be None if not needed. + body_link_state_w: Output array where body link states are updated (pose portion). + Shape is (num_envs, num_bodies). Can be None if not needed. + body_state_w: Output array where body states are updated (pose portion). + Shape is (num_envs, num_bodies). Can be None if not needed. + """ + i, j = wp.tid() + if from_mask: + body_com_pose_w[env_ids[i], body_ids[j]] = data[env_ids[i], body_ids[j]] + if body_com_state_w: + body_com_state_w[env_ids[i], body_ids[j]] = set_state_transforms_func( + body_com_state_w[env_ids[i], body_ids[j]], data[env_ids[i], body_ids[j]] + ) + else: + body_com_pose_w[env_ids[i], body_ids[j]] = data[i, j] + if body_com_state_w: + body_com_state_w[env_ids[i], body_ids[j]] = set_state_transforms_func( + body_com_state_w[env_ids[i], body_ids[j]], data[i, j] + ) + # Get the link pose from com pose + body_link_pose_w[env_ids[i], body_ids[j]] = get_com_pose_in_link_frame_func( + body_com_pose_w[env_ids[i], body_ids[j]], body_com_pose_b[env_ids[i], body_ids[j]] + ) + if body_link_state_w: + body_link_state_w[env_ids[i], body_ids[j]] = set_state_transforms_func( + body_link_state_w[env_ids[i], body_ids[j]], body_link_pose_w[env_ids[i], body_ids[j]] + ) + if body_state_w: + body_state_w[env_ids[i], body_ids[j]] = set_state_transforms_func( + body_state_w[env_ids[i], body_ids[j]], body_link_pose_w[env_ids[i], body_ids[j]] + ) + + +@wp.kernel +def set_body_com_velocity_to_sim( + data: wp.array2d(dtype=wp.spatial_vectorf), + env_ids: wp.array(dtype=wp.int32), + body_ids: wp.array(dtype=wp.int32), + from_mask: bool, + body_com_velocity_w: wp.array2d(dtype=wp.spatial_vectorf), + body_acc_w: wp.array2d(dtype=wp.spatial_vectorf), + body_state_w: wp.array2d(dtype=vec13f), + body_com_state_w: wp.array2d(dtype=vec13f), +): + """Write body COM velocity data to simulation buffers. + + This kernel writes body COM velocities from the input array to the output buffers, + optionally updates the corresponding state vectors, and zeros out the body + acceleration buffer, for each body in each environment. + + Args: + data: Input array of body COM spatial velocities. Shape is (num_envs, num_bodies) or + (num_selected_envs, num_selected_bodies) depending on from_mask. + env_ids: Input array of environment indices to write to. Shape is (num_selected_envs,). + body_ids: Input array of body indices to write to. Shape is (num_selected_bodies,). + from_mask: Input flag indicating whether to use masked indexing. + body_com_velocity_w: Output array where body COM velocities are written. + Shape is (num_envs, num_bodies). + body_acc_w: Output array where body accelerations are zeroed. + Shape is (num_envs, num_bodies). + body_state_w: Output array where body states are updated (velocity portion). + Shape is (num_envs, num_bodies). Can be None if not needed. + body_com_state_w: Output array where body COM states are updated (velocity portion). + Shape is (num_envs, num_bodies). Can be None if not needed. + """ + i, j = wp.tid() + if from_mask: + body_com_velocity_w[env_ids[i], body_ids[j]] = data[env_ids[i], body_ids[j]] + if body_state_w: + body_state_w[env_ids[i], body_ids[j]] = set_state_velocities_func( + body_state_w[env_ids[i], body_ids[j]], data[env_ids[i], body_ids[j]] + ) + if body_com_state_w: + body_com_state_w[env_ids[i], body_ids[j]] = set_state_velocities_func( + body_com_state_w[env_ids[i], body_ids[j]], data[env_ids[i], body_ids[j]] + ) + else: + body_com_velocity_w[env_ids[i], body_ids[j]] = data[i, j] + if body_state_w: + body_state_w[env_ids[i], body_ids[j]] = set_state_velocities_func( + body_state_w[env_ids[i], body_ids[j]], data[i, j] + ) + if body_com_state_w: + body_com_state_w[env_ids[i], body_ids[j]] = set_state_velocities_func( + body_com_state_w[env_ids[i], body_ids[j]], data[i, j] + ) + # Make the acceleration zero to prevent reporting old values + body_acc_w[env_ids[i], body_ids[j]] = wp.spatial_vectorf(0.0, 0.0, 0.0, 0.0, 0.0, 0.0) + + +@wp.kernel +def set_body_link_velocity_to_sim( + data: wp.array2d(dtype=wp.spatial_vectorf), + body_com_pose_b: wp.array2d(dtype=wp.transformf), + body_link_pose_w: wp.array2d(dtype=wp.transformf), + env_ids: wp.array(dtype=wp.int32), + body_ids: wp.array(dtype=wp.int32), + from_mask: bool, + body_link_velocity_w: wp.array2d(dtype=wp.spatial_vectorf), + body_com_velocity_w: wp.array2d(dtype=wp.spatial_vectorf), + body_acc_w: wp.array2d(dtype=wp.spatial_vectorf), + body_link_state_w: wp.array2d(dtype=vec13f), + body_state_w: wp.array2d(dtype=vec13f), + body_com_state_w: wp.array2d(dtype=vec13f), +): + """Write body link velocity data to simulation buffers. + + This kernel writes body link velocities from the input array to the output buffers, + computes the corresponding COM velocities from the link velocities, optionally updates + the corresponding state vectors, and zeros out the body acceleration buffer. + + Args: + data: Input array of body link spatial velocities. Shape is (num_envs, num_bodies) + or (num_selected_envs, num_selected_bodies) depending on from_mask. + body_com_pose_b: Input array of body COM poses in body frame. Shape is + (num_envs, num_bodies). + body_link_pose_w: Input array of body link poses in world frame. Shape is + (num_envs, num_bodies). + env_ids: Input array of environment indices to write to. Shape is (num_selected_envs,). + body_ids: Input array of body indices to write to. Shape is (num_selected_bodies,). + from_mask: Input flag indicating whether to use masked indexing. + body_link_velocity_w: Output array where body link velocities are written. + Shape is (num_envs, num_bodies). + body_com_velocity_w: Output array where body COM velocities (derived from link) + are written. Shape is (num_envs, num_bodies). + body_acc_w: Output array where body accelerations are zeroed. + Shape is (num_envs, num_bodies). + body_link_state_w: Output array where body link states are updated (velocity portion). + Shape is (num_envs, num_bodies). Can be None if not needed. + body_state_w: Output array where body states are updated (velocity portion). + Shape is (num_envs, num_bodies). Can be None if not needed. + body_com_state_w: Output array where body COM states are updated (velocity portion). + Shape is (num_envs, num_bodies). Can be None if not needed. + """ + i, j = wp.tid() + if from_mask: + body_link_velocity_w[env_ids[i], body_ids[j]] = data[env_ids[i], body_ids[j]] + if body_link_state_w: + body_link_state_w[env_ids[i], body_ids[j]] = set_state_velocities_func( + body_link_state_w[env_ids[i], body_ids[j]], data[env_ids[i], body_ids[j]] + ) + else: + body_link_velocity_w[env_ids[i], body_ids[j]] = data[i, j] + if body_link_state_w: + body_link_state_w[env_ids[i], body_ids[j]] = set_state_velocities_func( + body_link_state_w[env_ids[i], body_ids[j]], data[i, j] + ) + # Get the link velocity in the com frame + body_com_velocity_w[env_ids[i], body_ids[j]] = get_link_velocity_in_com_frame_func( + body_link_velocity_w[env_ids[i], body_ids[j]], + body_link_pose_w[env_ids[i], body_ids[j]], + body_com_pose_b[env_ids[i], body_ids[j]], + ) + if body_com_state_w: + body_com_state_w[env_ids[i], body_ids[j]] = set_state_velocities_func( + body_com_state_w[env_ids[i], body_ids[j]], body_com_velocity_w[env_ids[i], body_ids[j]] + ) + if body_state_w: + body_state_w[env_ids[i], body_ids[j]] = set_state_velocities_func( + body_state_w[env_ids[i], body_ids[j]], body_com_velocity_w[env_ids[i], body_ids[j]] + ) + # Make the acceleration zero to prevent reporting old values + body_acc_w[env_ids[i], body_ids[j]] = wp.spatial_vectorf(0.0, 0.0, 0.0, 0.0, 0.0, 0.0) + + +""" +Generic buffer-writing kernels (used by Articulation + RigidObject + RigidObjectCollection). +""" + + +@wp.kernel +def write_2d_data_to_buffer_with_indices( + in_data: wp.array2d(dtype=wp.float32), + env_ids: wp.array(dtype=wp.int32), + joint_ids: wp.array(dtype=wp.int32), + out_data: wp.array2d(dtype=wp.float32), +): + """Write 2D float data to a buffer at specified indices. + + This kernel copies float data from a partial input array to an output buffer at the + specified environment and joint/body indices. + + Args: + in_data: Input array containing float data. Shape is (num_selected_envs, num_selected_joints). + env_ids: Input array of environment indices to write to. Shape is (num_selected_envs,). + joint_ids: Input array of joint/body indices to write to. Shape is (num_selected_joints,). + out_data: Output array where data is written. Shape is (num_envs, num_joints). + """ + i, j = wp.tid() + out_data[env_ids[i], joint_ids[j]] = in_data[i, j] + + +@wp.kernel +def write_body_inertia_to_buffer_index( + in_data: wp.array3d(dtype=wp.float32), + env_ids: wp.array(dtype=wp.int32), + body_ids: wp.array(dtype=wp.int32), + out_data: wp.array3d(dtype=wp.float32), +): + """Write body inertia data to a buffer at specified indices. + + This kernel copies 3x3 inertia tensor data (stored as 9 floats) from a partial input + array to an output buffer at the specified environment and body indices. + + Args: + in_data: Input array containing inertia data. Shape is (num_selected_envs, num_selected_bodies, 9). + env_ids: Input array of environment indices to write to. Shape is (num_selected_envs,). + body_ids: Input array of body indices to write to. Shape is (num_selected_bodies,). + out_data: Output array where inertia data is written. Shape is (num_envs, num_bodies, 9). + """ + i, j = wp.tid() + for k in range(9): + out_data[env_ids[i], body_ids[j], k] = in_data[i, j, k] + + +@wp.kernel +def write_body_com_pose_to_buffer_index( + in_data: wp.array2d(dtype=wp.transformf), + env_ids: wp.array(dtype=wp.int32), + body_ids: wp.array(dtype=wp.int32), + out_data: wp.array2d(dtype=wp.transformf), +): + """Write body COM pose data to a buffer at specified indices. + + This kernel copies body COM pose data from a partial input array to an output buffer + at the specified environment and body indices. + + Args: + in_data: Input array containing body COM poses. Shape is (num_selected_envs, num_selected_bodies). + env_ids: Input array of environment indices to write to. Shape is (num_selected_envs,). + body_ids: Input array of body indices to write to. Shape is (num_selected_bodies,). + out_data: Output array where body COM poses are written. Shape is (num_envs, num_bodies). + """ + i, j = wp.tid() + out_data[env_ids[i], body_ids[j]] = in_data[i, j] + + +@wp.kernel +def derive_body_acceleration_from_body_com_velocities( + body_com_vel: wp.array2d(dtype=wp.spatial_vectorf), + dt: wp.float32, + prev_body_com_vel: wp.array2d(dtype=wp.spatial_vectorf), + body_acc: wp.array2d(dtype=wp.spatial_vectorf), +): + """Derive body acceleration from body COM velocities. + + This kernel derives body acceleration from body COM velocities using finite differencing. + + Args: + body_com_vel: Input array of body COM velocities. Shape is (num_envs, num_bodies). + dt: Input time step (scalar) used for finite differencing. + prev_body_com_vel: Input/output array of previous body COM velocities. Shape is (num_envs, num_bodies). + body_acc: Output array where body accelerations are written. Shape is (num_envs, num_bodies). + """ + i, j = wp.tid() + # Compute the acceleration + body_acc[i, j] = (body_com_vel[i, j] - prev_body_com_vel[i, j]) / dt + # Update the previous body COM velocity + prev_body_com_vel[i, j] = body_com_vel[i, j] + + +@wp.kernel +def _body_wrench_to_world( + force_b: wp.array(dtype=wp.vec3f, ndim=2), + torque_b: wp.array(dtype=wp.vec3f, ndim=2), + poses: wp.array(dtype=wp.transformf, ndim=2), + wrench_out: wp.array(dtype=wp.float32, ndim=3), +): + """Rotate body-frame force/torque to world frame and pack into a flat output array. + + Output layout per ``(i, j)`` slice (9 floats total): + + * ``[0:3]`` -- world-frame force ``[N]`` + * ``[3:6]`` -- world-frame torque ``[N*m]`` + * ``[6:9]`` -- world-frame link position ``[m]`` + + Args: + force_b: Body-frame applied forces ``[N]``. Shape is ``(N, L)``. + torque_b: Body-frame applied torques ``[N*m]``. Shape is ``(N, L)``. + poses: Link poses in world frame. Shape is ``(N, L)``. + wrench_out: Output packed wrench array. Shape is ``(N, L, 9)``. + """ + i, j = wp.tid() + q = wp.transform_get_rotation(poses[i, j]) + f_w = wp.quat_rotate(q, force_b[i, j]) + t_w = wp.quat_rotate(q, torque_b[i, j]) + wrench_out[i, j, 0] = f_w[0] + wrench_out[i, j, 1] = f_w[1] + wrench_out[i, j, 2] = f_w[2] + wrench_out[i, j, 3] = t_w[0] + wrench_out[i, j, 4] = t_w[1] + wrench_out[i, j, 5] = t_w[2] + p_w = wp.transform_get_translation(poses[i, j]) + wrench_out[i, j, 6] = p_w[0] + wrench_out[i, j, 7] = p_w[1] + wrench_out[i, j, 8] = p_w[2] + + +@wp.kernel +def _scatter_rows_partial( + dst: wp.array2d(dtype=wp.float32), + src: wp.array2d(dtype=wp.float32), + ids: wp.array(dtype=wp.int32), +): + """Scatter a partial row-indexed source array into a larger destination array. + + For each thread ``(i, j)`` writes ``dst[ids[i], j] = src[i, j]``. + + Args: + dst: Destination array of shape ``(N, C)`` to scatter values into. + src: Source array of shape ``(K, C)`` containing the values to scatter. + ids: Row indices into ``dst`` for each row of ``src``. Shape is ``(K,)``. + """ + i, j = wp.tid() + dst[ids[i], j] = src[i, j] + + +""" +Native-mask scatter kernels (mirrors Newton; the OVPhysX wheel's ``binding.write`` natively +supports a boolean mask via the ``mask=`` argument, so the ``*_mask`` setters update the cache +in-place and pass the mask straight through to the wheel without a ``torch.nonzero`` round-trip). +""" + + +@wp.kernel +def set_root_link_pose_to_sim_mask( + data: wp.array(dtype=wp.transformf), + env_mask: wp.array(dtype=wp.bool), + root_link_pose_w: wp.array(dtype=wp.transformf), +): + """Mask-scatter root link poses into the cache; rows where ``env_mask[i]`` is False are untouched.""" + i = wp.tid() + if env_mask[i]: + root_link_pose_w[i] = data[i] + + +@wp.kernel +def set_root_com_pose_to_sim_mask( + data: wp.array(dtype=wp.transformf), + body_com_pose_b: wp.array2d(dtype=wp.transformf), + env_mask: wp.array(dtype=wp.bool), + root_com_pose_w: wp.array(dtype=wp.transformf), + root_link_pose_w: wp.array(dtype=wp.transformf), +): + """Mask-scatter root COM poses into the cache and derive the corresponding link poses.""" + i = wp.tid() + if env_mask[i]: + root_com_pose_w[i] = data[i] + # link_pose = com_pose * inverse(com_pose_b) + root_link_pose_w[i] = wp.transform_multiply(root_com_pose_w[i], wp.transform_inverse(body_com_pose_b[i, 0])) + + +@wp.kernel +def set_root_com_velocity_to_sim_mask( + data: wp.array(dtype=wp.spatial_vectorf), + env_mask: wp.array(dtype=wp.bool), + num_bodies: wp.int32, + root_com_velocity_w: wp.array(dtype=wp.spatial_vectorf), + body_acc_w: wp.array2d(dtype=wp.spatial_vectorf), +): + """Mask-scatter root COM velocities into the cache and zero the dependent body acceleration.""" + i = wp.tid() + if env_mask[i]: + root_com_velocity_w[i] = data[i] + for j in range(num_bodies): + body_acc_w[i, j] = wp.spatial_vectorf(0.0, 0.0, 0.0, 0.0, 0.0, 0.0) + + +@wp.kernel +def set_root_link_velocity_to_sim_mask( + data: wp.array(dtype=wp.spatial_vectorf), + body_com_pose_b: wp.array2d(dtype=wp.transformf), + link_pose_w: wp.array(dtype=wp.transformf), + env_mask: wp.array(dtype=wp.bool), + num_bodies: wp.int32, + root_link_velocity_w: wp.array(dtype=wp.spatial_vectorf), + root_com_velocity_w: wp.array(dtype=wp.spatial_vectorf), + body_acc_w: wp.array2d(dtype=wp.spatial_vectorf), +): + """Mask-scatter root link velocities into the cache and derive the corresponding COM velocities + via the lever-arm transform: ``com_lin = link_lin + omega x rot(link_rot, com_offset)``. + """ + i = wp.tid() + if env_mask[i]: + root_link_velocity_w[i] = data[i] + ang = wp.spatial_bottom(data[i]) + lever = wp.quat_rotate( + wp.transform_get_rotation(link_pose_w[i]), wp.transform_get_translation(body_com_pose_b[i, 0]) + ) + com_lin = wp.spatial_top(data[i]) + wp.cross(ang, lever) + root_com_velocity_w[i] = wp.spatial_vector(com_lin, ang) + for j in range(num_bodies): + body_acc_w[i, j] = wp.spatial_vectorf(0.0, 0.0, 0.0, 0.0, 0.0, 0.0) + + +@wp.kernel +def write_2d_data_to_buffer_with_mask( + in_data: wp.array2d(dtype=wp.float32), + env_mask: wp.array(dtype=wp.bool), + body_mask: wp.array(dtype=wp.bool), + out_data: wp.array2d(dtype=wp.float32), +): + """Mask-scatter 2D float data into the cache where both ``env_mask[i]`` and ``body_mask[j]`` are True.""" + i, j = wp.tid() + if env_mask[i] and body_mask[j]: + out_data[i, j] = in_data[i, j] + + +@wp.kernel +def write_body_inertia_to_buffer_mask( + in_data: wp.array3d(dtype=wp.float32), + env_mask: wp.array(dtype=wp.bool), + body_mask: wp.array(dtype=wp.bool), + out_data: wp.array3d(dtype=wp.float32), +): + """Mask-scatter body inertia (3x3 = 9 floats per body) into the cache.""" + i, j = wp.tid() + if env_mask[i] and body_mask[j]: + for k in range(9): + out_data[i, j, k] = in_data[i, j, k] + + +@wp.kernel +def write_body_com_pose_to_buffer_mask( + in_data: wp.array2d(dtype=wp.transformf), + env_mask: wp.array(dtype=wp.bool), + body_mask: wp.array(dtype=wp.bool), + out_data: wp.array2d(dtype=wp.transformf), +): + """Mask-scatter body COM poses (transformf) into the cache.""" + i, j = wp.tid() + if env_mask[i] and body_mask[j]: + out_data[i, j] = in_data[i, j] diff --git a/source/isaaclab_physx/isaaclab_physx/scene_data_providers/__init__.py b/source/isaaclab_ovphysx/isaaclab_ovphysx/assets/rigid_object/__init__.py similarity index 81% rename from source/isaaclab_physx/isaaclab_physx/scene_data_providers/__init__.py rename to source/isaaclab_ovphysx/isaaclab_ovphysx/assets/rigid_object/__init__.py index 0a3fe4d79fb4..441515cd83a9 100644 --- a/source/isaaclab_physx/isaaclab_physx/scene_data_providers/__init__.py +++ b/source/isaaclab_ovphysx/isaaclab_ovphysx/assets/rigid_object/__init__.py @@ -3,7 +3,7 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""PhysX scene data provider backends.""" +"""Sub-module for ovphysx-backed rigid object assets.""" from isaaclab.utils.module import lazy_export diff --git a/source/isaaclab_ovphysx/isaaclab_ovphysx/assets/rigid_object/__init__.pyi b/source/isaaclab_ovphysx/isaaclab_ovphysx/assets/rigid_object/__init__.pyi new file mode 100644 index 000000000000..1c96a5aa4550 --- /dev/null +++ b/source/isaaclab_ovphysx/isaaclab_ovphysx/assets/rigid_object/__init__.pyi @@ -0,0 +1,12 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +__all__ = [ + "RigidObject", + "RigidObjectData", +] + +from .rigid_object import RigidObject +from .rigid_object_data import RigidObjectData diff --git a/source/isaaclab_ovphysx/isaaclab_ovphysx/assets/rigid_object/rigid_object.py b/source/isaaclab_ovphysx/isaaclab_ovphysx/assets/rigid_object/rigid_object.py new file mode 100644 index 000000000000..015c8f102f44 --- /dev/null +++ b/source/isaaclab_ovphysx/isaaclab_ovphysx/assets/rigid_object/rigid_object.py @@ -0,0 +1,1173 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""OVPhysX-backed RigidObject implementation.""" + +from __future__ import annotations + +import re +import warnings +from collections.abc import Sequence +from typing import Any + +import numpy as np +import torch +import warp as wp + +from pxr import UsdPhysics + +import isaaclab.sim as sim_utils +from isaaclab.assets.rigid_object.base_rigid_object import BaseRigidObject +from isaaclab.assets.rigid_object.rigid_object_cfg import RigidObjectCfg +from isaaclab.utils.string import resolve_matching_names +from isaaclab.utils.wrench_composer import WrenchComposer + +from isaaclab_ovphysx import tensor_types as TT +from isaaclab_ovphysx.assets import kernels as shared_kernels +from isaaclab_ovphysx.assets.kernels import _body_wrench_to_world +from isaaclab_ovphysx.physics import OvPhysxManager + +from .rigid_object_data import RigidObjectData + + +class RigidObject(BaseRigidObject): + """A rigid object asset class. + + Rigid objects are assets comprising of rigid bodies. They can be used to represent dynamic objects + such as boxes, spheres, etc. A rigid body is described by its pose, velocity and mass distribution. + + For an asset to be considered a rigid object, the root prim of the asset must have the `USD RigidBodyAPI`_ + applied to it. This API is used to define the simulation properties of the rigid body. On playing the + simulation, the physics engine will automatically register the rigid body and create a corresponding + rigid body handle. State is read and written through ovphysx ``TensorBinding`` objects acquired from + the :class:`~isaaclab_ovphysx.physics.OvPhysxManager`. Only free (non-articulated) rigid bodies are + supported; prims under an ``ArticulationRootAPI`` should use + :class:`~isaaclab_ovphysx.assets.articulation.Articulation` instead. + + .. _`USD RigidBodyAPI`: https://openusd.org/dev/api/class_usd_physics_rigid_body_a_p_i.html + """ + + cfg: RigidObjectCfg + """Configuration instance for the rigid object.""" + + __backend_name__: str = "ovphysx" + """The name of the backend for the rigid object.""" + + def __init__(self, cfg: RigidObjectCfg): + """Initialize the rigid object. + + Args: + cfg: A configuration instance. + """ + super().__init__(cfg) + # Bindings are created lazily (on first access) to avoid allocating + # handles for tensor types the user never queries. + self._bindings: dict[int, Any] = {} + + """ + Properties + """ + + @property + def data(self) -> RigidObjectData: + return self._data + + @property + def num_instances(self) -> int: + return self._num_instances + + @property + def num_bodies(self) -> int: + """Number of bodies in the asset. + + This is always 1 since each object is a single rigid body. + """ + return self._num_bodies + + @property + def body_names(self) -> list[str]: + """Ordered names of bodies in the rigid object.""" + return self._body_names + + @property + def root_view(self) -> dict[int, Any]: + """Root view for the asset. + + OVPhysX exposes per-tensor-type bindings rather than a single opaque view object + as used by the PhysX and Newton backends. Callers that need low-level binding + access should call :meth:`_get_binding` rather than iterating this dict directly. + For high-level state access (instance counts, prim paths, transforms), use the + :attr:`num_instances`, :attr:`body_names`, and + :attr:`~RigidObjectData.root_link_pose_w` accessors instead. + + .. note:: + Use this view with caution. It requires handling of tensors in a specific way. + """ + return self._bindings + + @property + def instantaneous_wrench_composer(self) -> WrenchComposer | None: + """Instantaneous wrench composer. + + Returns a :class:`~isaaclab.utils.wrench_composer.WrenchComposer` instance. Wrenches added or set to this wrench + composer are only valid for the current simulation step. At the end of the simulation step, the wrenches set + to this object are discarded. This is useful to apply forces that change all the time, things like drag forces + for instance. + """ + return self._instantaneous_wrench_composer + + @property + def permanent_wrench_composer(self) -> WrenchComposer | None: + """Permanent wrench composer. + + Returns a :class:`~isaaclab.utils.wrench_composer.WrenchComposer` instance. Wrenches added or set to this wrench + composer are persistent and are applied to the simulation at every step. This is useful to apply forces that + are constant over a period of time, things like the thrust of a motor for instance. + """ + return self._permanent_wrench_composer + + """ + Operations. + """ + + def reset( + self, env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, env_mask: wp.array | None = None + ) -> None: + """Reset the rigid object. + + Args: + env_ids: Environment indices. If None, then all indices are used. + env_mask: Environment mask. If None, then all the instances are updated. Shape is (num_instances,). + """ + # resolve all indices + if (env_ids is None) or (env_ids == slice(None)): + env_ids = slice(None) + # reset external wrench + self._instantaneous_wrench_composer.reset(env_ids, env_mask) + self._permanent_wrench_composer.reset(env_ids, env_mask) + + def write_data_to_sim(self) -> None: + """Write external wrench to the simulation. + + .. note:: + We write external wrench to the simulation here since this function is called before the simulation step. + This ensures that the external wrench is applied at every simulation step. + """ + inst = self._instantaneous_wrench_composer + perm = self._permanent_wrench_composer + if not inst.active and not perm.active: + return + if inst.active: + if perm.active: + inst.add_raw_buffers_from(perm) + force_b = inst.out_force_b.warp + torque_b = inst.out_torque_b.warp + else: + force_b = perm.out_force_b.warp + torque_b = perm.out_torque_b.warp + + poses = self._data.body_link_pose_w.warp # (N, 1) wp.transformf + wp.launch( + _body_wrench_to_world, + dim=(self._num_instances, 1), + inputs=[force_b, torque_b, poses], + outputs=[self._wrench_buf], + device=self._device, + ) + binding = self._get_binding(TT.RIGID_BODY_WRENCH) + binding.write(self._wrench_buf_flat) + inst.reset() + + def update(self, dt: float) -> None: + """Updates the simulation data. + + Args: + dt: The time step size in seconds. + """ + self._data.update(dt) + + """ + Operations - Finders. + """ + + def find_bodies(self, name_keys: str | Sequence[str], preserve_order: bool = False) -> tuple[list[int], list[str]]: + """Find bodies in the rigid body based on the name keys. + + Please check the :meth:`isaaclab.utils.string.resolve_matching_names` function for more + information on the name matching. + + Args: + name_keys: A regular expression or a list of regular expressions to match the body names. + preserve_order: Whether to preserve the order of the name keys in the output. Defaults to False. + + Returns: + A tuple of lists containing the body indices and names. + """ + return resolve_matching_names(name_keys, self._body_names, preserve_order) + + """ + Operations - Write to simulation. + """ + + def write_root_pose_to_sim_index( + self, + *, + root_pose: torch.Tensor | wp.array, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + ) -> None: + """Set the root pose over selected environment indices into the simulation. + + The root pose comprises of the cartesian position and quaternion orientation in (x, y, z, w). + + .. note:: + This method expects partial data. + + .. tip:: + Both the index and mask methods have dedicated optimized implementations. Performance is similar for both. + However, to allow graphed pipelines, the mask method must be used. + + Args: + root_pose: Root poses in simulation frame. Shape is (len(env_ids), 7) + or (len(env_ids),) with dtype wp.transformf. + env_ids: Environment indices. If None, then all indices are used. + """ + self.write_root_link_pose_to_sim_index(root_pose=root_pose, env_ids=env_ids) + + def write_root_pose_to_sim_mask( + self, + *, + root_pose: torch.Tensor | wp.array, + env_mask: wp.array | None = None, + ) -> None: + """Set the root pose over selected environment mask into the simulation. + + .. note:: + This method expects full data. + + .. tip:: + Both the index and mask methods have dedicated optimized implementations. Performance is similar for both. + However, to allow graphed pipelines, the mask method must be used. + + Args: + root_pose: Root poses in simulation frame. Shape is (num_instances, 7) + or (num_instances,) with dtype wp.transformf. + env_mask: Environment mask. If None, then all the instances are updated. Shape is (num_instances,). + """ + self.write_root_link_pose_to_sim_mask(root_pose=root_pose, env_mask=env_mask) + + def write_root_velocity_to_sim_index( + self, + *, + root_velocity: torch.Tensor | wp.array, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + ) -> None: + """Set the root center of mass velocity over selected environment indices into the simulation. + + The velocity comprises linear velocity (x, y, z) and angular velocity (x, y, z) in that order. + + .. note:: + This sets the velocity of the root's center of mass rather than the root's frame. + + .. note:: + This method expects partial data. + + .. tip:: + Both the index and mask methods have dedicated optimized implementations. Performance is similar for both. + However, to allow graphed pipelines, the mask method must be used. + + Args: + root_velocity: Root center of mass velocities in simulation world frame. Shape is (len(env_ids), 6) + or (len(env_ids),) with dtype wp.spatial_vectorf. + env_ids: Environment indices. If None, then all indices are used. + """ + self.write_root_com_velocity_to_sim_index(root_velocity=root_velocity, env_ids=env_ids) + + def write_root_velocity_to_sim_mask( + self, + *, + root_velocity: torch.Tensor | wp.array, + env_mask: wp.array | None = None, + ) -> None: + """Set the root center of mass velocity over selected environment mask into the simulation. + + .. note:: + This method expects full data. + + .. tip:: + Both the index and mask methods have dedicated optimized implementations. Performance is similar for both. + However, to allow graphed pipelines, the mask method must be used. + + Args: + root_velocity: Root center of mass velocities in simulation world frame. Shape is (num_instances, 6) + or (num_instances,) with dtype wp.spatial_vectorf. + env_mask: Environment mask. If None, then all the instances are updated. Shape is (num_instances,). + """ + self.write_root_com_velocity_to_sim_mask(root_velocity=root_velocity, env_mask=env_mask) + + def write_root_link_pose_to_sim_index( + self, + *, + root_pose: torch.Tensor | wp.array, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + ) -> None: + """Set the root link pose over selected environment indices into the simulation. + + The root pose comprises of the cartesian position and quaternion orientation in (x, y, z, w). + + .. note:: + This method expects partial data. + + .. tip:: + Both the index and mask methods have dedicated optimized implementations. Performance is similar for both. + However, to allow graphed pipelines, the mask method must be used. + + Args: + root_pose: Root link poses in simulation frame. Shape is (len(env_ids), 7) + or (len(env_ids),) with dtype wp.transformf. + env_ids: Environment indices. If None, then all indices are used. + """ + env_ids = self._resolve_env_ids(env_ids) + self.assert_shape_and_dtype(root_pose, (env_ids.shape[0],), wp.transformf, "root_pose") + wp.launch( + shared_kernels.set_root_link_pose_to_sim_index, + dim=env_ids.shape[0], + inputs=[root_pose, env_ids], + outputs=[self.data.root_link_pose_w], + device=self._device, + ) + # Invalidate dependent root_com_pose timestamp so the next read recomposes it. + self.data._root_com_pose_w.timestamp = -1.0 + # Push cache to the wheel via an indexed write. + binding = self._get_binding(TT.RIGID_BODY_POSE) + binding.write(self.data._root_link_pose_w.data.view(wp.float32), indices=env_ids) + + def write_root_link_pose_to_sim_mask( + self, + *, + root_pose: torch.Tensor | wp.array, + env_mask: wp.array | None = None, + ) -> None: + """Set the root link pose over selected environment mask into the simulation. + + The root pose comprises of the cartesian position and quaternion orientation in (x, y, z, w). + + .. note:: + This method expects full data. + + .. tip:: + Both the index and mask methods have dedicated optimized implementations. Performance is similar for both. + However, to allow graphed pipelines, the mask method must be used. + + Args: + root_pose: Root poses in simulation frame. Shape is (num_instances, 7) + or (num_instances,) with dtype wp.transformf. + env_mask: Environment mask. If None, then all the instances are updated. Shape is (num_instances,). + """ + env_mask_wp = self._resolve_env_mask(env_mask) + self.assert_shape_and_dtype(root_pose, (self._num_instances,), wp.transformf, "root_pose") + wp.launch( + shared_kernels.set_root_link_pose_to_sim_mask, + dim=self._num_instances, + inputs=[root_pose, env_mask_wp], + outputs=[self.data.root_link_pose_w], + device=self._device, + ) + self.data._root_com_pose_w.timestamp = -1.0 + binding = self._get_binding(TT.RIGID_BODY_POSE) + binding.write(self.data._root_link_pose_w.data.view(wp.float32), mask=env_mask_wp) + + def write_root_com_pose_to_sim_index( + self, + *, + root_pose: torch.Tensor | wp.array, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + ) -> None: + """Set the root center of mass pose over selected environment indices into the simulation. + + The root pose comprises of the cartesian position and quaternion orientation in (x, y, z, w). + The orientation is the orientation of the principal axes of inertia. + + .. note:: + This method expects partial data. + + .. tip:: + Both the index and mask methods have dedicated optimized implementations. Performance is similar for both. + However, to allow graphed pipelines, the mask method must be used. + + Args: + root_pose: Root center of mass poses in simulation frame. Shape is (len(env_ids), 7) + or (len(env_ids),) with dtype wp.transformf. + env_ids: Environment indices. If None, then all indices are used. + """ + env_ids = self._resolve_env_ids(env_ids) + self.assert_shape_and_dtype(root_pose, (env_ids.shape[0],), wp.transformf, "root_pose") + wp.launch( + shared_kernels.set_root_com_pose_to_sim_index, + dim=env_ids.shape[0], + inputs=[root_pose, self.data.body_com_pose_b, env_ids], + outputs=[self.data.root_com_pose_w, self.data.root_link_pose_w], + device=self._device, + ) + binding = self._get_binding(TT.RIGID_BODY_POSE) + binding.write(self.data._root_link_pose_w.data.view(wp.float32), indices=env_ids) + + def write_root_com_pose_to_sim_mask( + self, + *, + root_pose: torch.Tensor | wp.array, + env_mask: wp.array | None = None, + ) -> None: + """Set the root center of mass pose over selected environment mask into the simulation. + + The root pose comprises of the cartesian position and quaternion orientation in (x, y, z, w). + The orientation is the orientation of the principal axes of inertia. + + .. note:: + This method expects full data. + + .. tip:: + Both the index and mask methods have dedicated optimized implementations. Performance is similar for both. + However, to allow graphed pipelines, the mask method must be used. + + Args: + root_pose: Root center of mass poses in simulation frame. Shape is (num_instances, 7) + or (num_instances,) with dtype wp.transformf. + env_mask: Environment mask. If None, then all the instances are updated. Shape is (num_instances,). + """ + env_mask_wp = self._resolve_env_mask(env_mask) + self.assert_shape_and_dtype(root_pose, (self._num_instances,), wp.transformf, "root_pose") + wp.launch( + shared_kernels.set_root_com_pose_to_sim_mask, + dim=self._num_instances, + inputs=[root_pose, self.data.body_com_pose_b, env_mask_wp], + outputs=[self.data.root_com_pose_w, self.data.root_link_pose_w], + device=self._device, + ) + binding = self._get_binding(TT.RIGID_BODY_POSE) + binding.write(self.data._root_link_pose_w.data.view(wp.float32), mask=env_mask_wp) + + def write_root_com_velocity_to_sim_index( + self, + *, + root_velocity: torch.Tensor | wp.array, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + ) -> None: + """Set the root center of mass velocity over selected environment indices into the simulation. + + The velocity comprises linear velocity (x, y, z) and angular velocity (x, y, z) in that order. + + .. note:: + This sets the velocity of the root's center of mass rather than the root's frame. + + .. note:: + This method expects partial data. + + .. tip:: + Both the index and mask methods have dedicated optimized implementations. Performance is similar for both. + However, to allow graphed pipelines, the mask method must be used. + + Args: + root_velocity: Root center of mass velocities in simulation world frame. Shape is (len(env_ids), 6) + or (len(env_ids),) with dtype wp.spatial_vectorf. + env_ids: Environment indices. If None, then all indices are used. + """ + env_ids = self._resolve_env_ids(env_ids) + self.assert_shape_and_dtype(root_velocity, (env_ids.shape[0],), wp.spatial_vectorf, "root_velocity") + wp.launch( + shared_kernels.set_root_com_velocity_to_sim_index, + dim=env_ids.shape[0], + inputs=[root_velocity, env_ids, 1], + outputs=[self.data.root_com_vel_w, self.data.body_com_acc_w], + device=self._device, + ) + # Invalidate dependent root_link_vel timestamp. + self.data._root_link_vel_w.timestamp = -1.0 + binding = self._get_binding(TT.RIGID_BODY_VELOCITY) + binding.write(self.data._root_com_vel_w.data.view(wp.float32), indices=env_ids) + + def write_root_com_velocity_to_sim_mask( + self, + *, + root_velocity: torch.Tensor | wp.array, + env_mask: wp.array | None = None, + ) -> None: + """Set the root center of mass velocity over selected environment mask into the simulation. + + The velocity comprises linear velocity (x, y, z) and angular velocity (x, y, z) in that order. + + .. note:: + This sets the velocity of the root's center of mass rather than the root's frame. + + .. note:: + This method expects full data. + + .. tip:: + Both the index and mask methods have dedicated optimized implementations. Performance is similar for both. + However, to allow graphed pipelines, the mask method must be used. + + Args: + root_velocity: Root center of mass velocities in simulation world frame. Shape is (num_instances, 6) + or (num_instances,) with dtype wp.spatial_vectorf. + env_mask: Environment mask. If None, then all the instances are updated. Shape is (num_instances,). + """ + env_mask_wp = self._resolve_env_mask(env_mask) + self.assert_shape_and_dtype(root_velocity, (self._num_instances,), wp.spatial_vectorf, "root_velocity") + wp.launch( + shared_kernels.set_root_com_velocity_to_sim_mask, + dim=self._num_instances, + inputs=[root_velocity, env_mask_wp, 1], + outputs=[self.data.root_com_vel_w, self.data.body_com_acc_w], + device=self._device, + ) + self.data._root_link_vel_w.timestamp = -1.0 + binding = self._get_binding(TT.RIGID_BODY_VELOCITY) + binding.write(self.data._root_com_vel_w.data.view(wp.float32), mask=env_mask_wp) + + def write_root_link_velocity_to_sim_index( + self, + *, + root_velocity: torch.Tensor | wp.array, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + ) -> None: + """Set the root link velocity over selected environment indices into the simulation. + + The velocity comprises linear velocity (x, y, z) and angular velocity (x, y, z) in that order. + + .. note:: + This sets the velocity of the root's frame rather than the root's center of mass. + + .. note:: + This method expects partial data. + + .. tip:: + Both the index and mask methods have dedicated optimized implementations. Performance is similar for both. + However, to allow graphed pipelines, the mask method must be used. + + Args: + root_velocity: Root frame velocities in simulation world frame. Shape is (len(env_ids), 6) + or (len(env_ids),) with dtype wp.spatial_vectorf. + env_ids: Environment indices. If None, then all indices are used. + """ + env_ids = self._resolve_env_ids(env_ids) + self.assert_shape_and_dtype(root_velocity, (env_ids.shape[0],), wp.spatial_vectorf, "root_velocity") + wp.launch( + shared_kernels.set_root_link_velocity_to_sim_index, + dim=env_ids.shape[0], + inputs=[ + root_velocity, + self.data.body_com_pose_b, + self.data.root_link_pose_w, + env_ids, + 1, # num_bodies is always 1 for RigidObject + ], + outputs=[self.data.root_link_vel_w, self.data.root_com_vel_w, self.data.body_com_acc_w], + device=self._device, + ) + binding = self._get_binding(TT.RIGID_BODY_VELOCITY) + binding.write(self.data._root_com_vel_w.data.view(wp.float32), indices=env_ids) + + def write_root_link_velocity_to_sim_mask( + self, + *, + root_velocity: torch.Tensor | wp.array, + env_mask: wp.array | None = None, + ) -> None: + """Set the root link velocity over selected environment mask into the simulation. + + The velocity comprises linear velocity (x, y, z) and angular velocity (x, y, z) in that order. + + .. note:: + This sets the velocity of the root's frame rather than the root's center of mass. + + .. note:: + This method expects full data. + + .. tip:: + Both the index and mask methods have dedicated optimized implementations. Performance is similar for both. + However, to allow graphed pipelines, the mask method must be used. + + Args: + root_velocity: Root frame velocities in simulation world frame. Shape is (num_instances, 6) + or (num_instances,) with dtype wp.spatial_vectorf. + env_mask: Environment mask. If None, then all the instances are updated. Shape is (num_instances,). + """ + env_mask_wp = self._resolve_env_mask(env_mask) + self.assert_shape_and_dtype(root_velocity, (self._num_instances,), wp.spatial_vectorf, "root_velocity") + wp.launch( + shared_kernels.set_root_link_velocity_to_sim_mask, + dim=self._num_instances, + inputs=[root_velocity, self.data.body_com_pose_b, self.data.root_link_pose_w, env_mask_wp, 1], + outputs=[self.data.root_link_vel_w, self.data.root_com_vel_w, self.data.body_com_acc_w], + device=self._device, + ) + binding = self._get_binding(TT.RIGID_BODY_VELOCITY) + binding.write(self.data._root_com_vel_w.data.view(wp.float32), mask=env_mask_wp) + + """ + Operations - Setters. + """ + + def set_masses_index( + self, + *, + masses: torch.Tensor | wp.array, + body_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + ) -> None: + """Set masses of all bodies using indices. + + .. note:: + This method expects partial data. + + .. tip:: + Both the index and mask methods have dedicated optimized implementations. Performance is similar for both. + However, to allow graphed pipelines, the mask method must be used. + + Args: + masses: Masses of all bodies. Shape is (len(env_ids), len(body_ids)). + body_ids: The body indices to set the masses for. Defaults to None (all bodies). + env_ids: The environment indices to set the masses for. Defaults to None (all environments). + """ + env_ids = self._resolve_env_ids(env_ids) + body_ids = self._resolve_body_ids(body_ids) + self.assert_shape_and_dtype(masses, (env_ids.shape[0], body_ids.shape[0]), wp.float32, "masses") + # Scatter user data into the cached _body_mass at (env_ids, body_ids). + wp.launch( + shared_kernels.write_2d_data_to_buffer_with_indices, + dim=(env_ids.shape[0], body_ids.shape[0]), + inputs=[masses, env_ids, body_ids], + outputs=[self.data._body_mass], + device=self._device, + ) + # Push cache to the wheel via pinned-CPU staging (RIGID_BODY_MASS is CPU-only). + cpu_env_ids = self._get_cpu_env_ids(env_ids) + wp.copy(self._cpu_body_mass, self.data._body_mass) + binding = self._get_binding(TT.RIGID_BODY_MASS) + binding.write(self._cpu_body_mass.flatten(), indices=cpu_env_ids) + + def set_masses_mask( + self, + *, + masses: torch.Tensor | wp.array, + body_mask: wp.array | None = None, + env_mask: wp.array | None = None, + ) -> None: + """Set masses of all bodies using masks. + + .. note:: + This method expects full data. + + .. tip:: + Both the index and mask methods have dedicated optimized implementations. Performance is similar for both. + However, to allow graphed pipelines, the mask method must be used. + + Args: + masses: Masses of all bodies. Shape is (num_instances, num_bodies). + body_mask: Body mask. If None, then all bodies are used. + env_mask: Environment mask. If None, then all the instances are updated. Shape is (num_instances,). + """ + env_mask_wp = self._resolve_env_mask(env_mask) + body_mask_wp = self._resolve_body_mask(body_mask) + self.assert_shape_and_dtype(masses, (self._num_instances, self._num_bodies), wp.float32, "masses") + wp.launch( + shared_kernels.write_2d_data_to_buffer_with_mask, + dim=(self._num_instances, self._num_bodies), + inputs=[masses, env_mask_wp, body_mask_wp], + outputs=[self.data._body_mass], + device=self._device, + ) + wp.copy(self._cpu_body_mass, self.data._body_mass) + binding = self._get_binding(TT.RIGID_BODY_MASS) + binding.write(self._cpu_body_mass.flatten(), mask=self._get_cpu_env_mask(env_mask_wp)) + + def set_coms_index( + self, + *, + coms: torch.Tensor | wp.array, + body_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + ) -> None: + """Set center of mass pose of all bodies using indices. + + .. note:: + This method expects partial data. + + .. tip:: + Both the index and mask methods have dedicated optimized implementations. Performance is similar for both. + However, to allow graphed pipelines, the mask method must be used. + + Args: + coms: Center of mass pose of all bodies. Shape is (len(env_ids), len(body_ids), 7). + body_ids: The body indices to set the center of mass pose for. Defaults to None (all bodies). + env_ids: The environment indices to set the center of mass pose for. Defaults to None + (all environments). + """ + env_ids = self._resolve_env_ids(env_ids) + body_ids = self._resolve_body_ids(body_ids) + self.assert_shape_and_dtype(coms, (env_ids.shape[0], body_ids.shape[0]), wp.transformf, "coms") + wp.launch( + shared_kernels.write_body_com_pose_to_buffer_index, + dim=(env_ids.shape[0], body_ids.shape[0]), + inputs=[coms, env_ids, body_ids], + outputs=[self.data._body_com_pose_b.data], + device=self._device, + ) + # Invalidate dependent root_com_pose timestamp -- it's derived from body_com_pose_b. + self.data._root_com_pose_w.timestamp = -1.0 + # Push cache to the wheel via pinned-CPU staging (RIGID_BODY_COM_POSE is CPU-only). + cpu_env_ids = self._get_cpu_env_ids(env_ids) + wp.copy(self._cpu_body_coms, self.data._body_com_pose_b.data) + binding = self._get_binding(TT.RIGID_BODY_COM_POSE) + # Wheel binding shape is (N, 7); squeeze singleton body dim with a flat float32 view. + binding.write(self._cpu_body_coms.reshape((self._num_instances, 7)), indices=cpu_env_ids) + + def set_coms_mask( + self, + *, + coms: torch.Tensor | wp.array, + body_mask: wp.array | None = None, + env_mask: wp.array | None = None, + ) -> None: + """Set center of mass pose of all bodies using masks. + + .. note:: + This method expects full data. + + .. tip:: + Both the index and mask methods have dedicated optimized implementations. Performance is similar for both. + However, to allow graphed pipelines, the mask method must be used. + + Args: + coms: Center of mass pose of all bodies. Shape is (num_instances, num_bodies, 7). + body_mask: Body mask. If None, then all bodies are used. + env_mask: Environment mask. If None, then all the instances are updated. Shape is (num_instances,). + """ + env_mask_wp = self._resolve_env_mask(env_mask) + body_mask_wp = self._resolve_body_mask(body_mask) + self.assert_shape_and_dtype(coms, (self._num_instances, self._num_bodies), wp.transformf, "coms") + wp.launch( + shared_kernels.write_body_com_pose_to_buffer_mask, + dim=(self._num_instances, self._num_bodies), + inputs=[coms, env_mask_wp, body_mask_wp], + outputs=[self.data._body_com_pose_b.data], + device=self._device, + ) + self.data._root_com_pose_w.timestamp = -1.0 + wp.copy(self._cpu_body_coms, self.data._body_com_pose_b.data) + binding = self._get_binding(TT.RIGID_BODY_COM_POSE) + binding.write(self._cpu_body_coms.reshape((self._num_instances, 7)), mask=self._get_cpu_env_mask(env_mask_wp)) + + def set_inertias_index( + self, + *, + inertias: torch.Tensor | wp.array, + body_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + ) -> None: + """Set inertias of all bodies using indices. + + .. note:: + This method expects partial data. + + .. tip:: + Both the index and mask methods have dedicated optimized implementations. Performance is similar for both. + However, to allow graphed pipelines, the mask method must be used. + + Args: + inertias: Inertias of all bodies. Shape is (len(env_ids), len(body_ids), 9). + body_ids: The body indices to set the inertias for. Defaults to None (all bodies). + env_ids: The environment indices to set the inertias for. Defaults to None (all environments). + """ + env_ids = self._resolve_env_ids(env_ids) + body_ids = self._resolve_body_ids(body_ids) + self.assert_shape_and_dtype(inertias, (env_ids.shape[0], body_ids.shape[0], 9), wp.float32, "inertias") + wp.launch( + shared_kernels.write_body_inertia_to_buffer_index, + dim=(env_ids.shape[0], body_ids.shape[0]), + inputs=[inertias, env_ids, body_ids], + outputs=[self.data._body_inertia], + device=self._device, + ) + # Push cache to the wheel via pinned-CPU staging (RIGID_BODY_INERTIA is CPU-only). + cpu_env_ids = self._get_cpu_env_ids(env_ids) + wp.copy(self._cpu_body_inertia, self.data._body_inertia) + binding = self._get_binding(TT.RIGID_BODY_INERTIA) + # Wheel binding shape is (N, 9); flatten the singleton body dim. + binding.write(self._cpu_body_inertia.reshape((self._num_instances, 9)), indices=cpu_env_ids) + + def set_inertias_mask( + self, + *, + inertias: torch.Tensor | wp.array, + body_mask: wp.array | None = None, + env_mask: wp.array | None = None, + ) -> None: + """Set inertias of all bodies using masks. + + .. note:: + This method expects full data. + + .. tip:: + Both the index and mask methods have dedicated optimized implementations. Performance is similar for both. + However, to allow graphed pipelines, the mask method must be used. + + Args: + inertias: Inertias of all bodies. Shape is (num_instances, num_bodies, 9). + body_mask: Body mask. If None, then all bodies are used. + env_mask: Environment mask. If None, then all the instances are updated. Shape is (num_instances,). + """ + env_mask_wp = self._resolve_env_mask(env_mask) + body_mask_wp = self._resolve_body_mask(body_mask) + self.assert_shape_and_dtype(inertias, (self._num_instances, self._num_bodies, 9), wp.float32, "inertias") + wp.launch( + shared_kernels.write_body_inertia_to_buffer_mask, + dim=(self._num_instances, self._num_bodies), + inputs=[inertias, env_mask_wp, body_mask_wp], + outputs=[self.data._body_inertia], + device=self._device, + ) + wp.copy(self._cpu_body_inertia, self.data._body_inertia) + binding = self._get_binding(TT.RIGID_BODY_INERTIA) + binding.write( + self._cpu_body_inertia.reshape((self._num_instances, 9)), mask=self._get_cpu_env_mask(env_mask_wp) + ) + + """ + Internal helper. + """ + + def _initialize_impl(self) -> None: + # acquire ovphysx instance + physx_instance = OvPhysxManager.get_physx_instance() + if physx_instance is None: + raise RuntimeError("OvPhysxManager has not been initialized yet.") + self._ovphysx = physx_instance + # Derive the device from PhysicsManager (which mirrors SimulationContext.cfg.device). + # The ovphysx PhysX object does not expose a .device property; reading it would + # raise AttributeError (masked by hasattr) and fall back to "cuda:0" even when the + # simulation is running on CPU, causing a device mismatch in binding.read(). + self._device = OvPhysxManager.get_device() + # Convert IsaacLab prim-path notation to the glob patterns ovphysx expects. + # IsaacLab uses two conventions: + # /World/envs/env_.*/object -- regex dot-star for "any env index" + # /World/envs/{ENV_REGEX_NS}/object -- explicit placeholder + # ovphysx ``create_tensor_binding`` uses fnmatch-style globs, so both map to ``*``. + pattern = re.sub(r"\{ENV_REGEX_NS\}", "*", self.cfg.prim_path) + pattern = re.sub(r"\.\*", "*", pattern) + self._binding_pattern = pattern + + # Validate the prim tree before creating tensor bindings -- the wheel silently + # produces a 0-prim binding when the pattern matches nothing, which surfaces as an + # obscure ``TypeError`` deep in property accessors. + # obtain the first prim in the regex expression (all others are assumed to be a copy of this) + template_prim = sim_utils.find_first_matching_prim(self.cfg.prim_path) + if template_prim is None: + raise RuntimeError(f"Failed to find prim for expression: '{self.cfg.prim_path}'.") + template_prim_path = template_prim.GetPath().pathString + + # find rigid root prims + root_prims = sim_utils.get_all_matching_child_prims( + template_prim_path, + predicate=lambda prim: prim.HasAPI(UsdPhysics.RigidBodyAPI), + traverse_instance_prims=False, + ) + if len(root_prims) == 0: + raise RuntimeError( + f"Failed to find a rigid body when resolving '{self.cfg.prim_path}'." + " Please ensure that the prim has 'USD RigidBodyAPI' applied." + ) + if len(root_prims) > 1: + raise RuntimeError( + f"Failed to find a single rigid body when resolving '{self.cfg.prim_path}'." + f" Found multiple '{root_prims}' under '{template_prim_path}'." + " Please ensure that there is only one rigid body in the prim path tree." + ) + articulation_prims = sim_utils.get_all_matching_child_prims( + template_prim_path, + predicate=lambda prim: prim.HasAPI(UsdPhysics.ArticulationRootAPI), + traverse_instance_prims=False, + ) + if len(articulation_prims) != 0: + if articulation_prims[0].GetAttribute("physxArticulation:articulationEnabled").Get(): + raise RuntimeError( + f"Found an articulation root when resolving '{self.cfg.prim_path}' for rigid" + f" objects. These are located at: '{articulation_prims}' under" + f" '{template_prim_path}'. Please disable the articulation root in the USD" + " or from code by setting the parameter" + " 'ArticulationRootPropertiesCfg.articulation_enabled' to False in the spawn" + " configuration." + ) + + # Eagerly create every binding the data container reads at init, so failures + # surface here with a helpful message rather than as a raw wheel exception + # (or a KeyError) at first writer call. + for tt in ( + TT.RIGID_BODY_POSE, + TT.RIGID_BODY_VELOCITY, + TT.RIGID_BODY_WRENCH, + TT.RIGID_BODY_MASS, + TT.RIGID_BODY_COM_POSE, + TT.RIGID_BODY_INERTIA, + ): + try: + self._get_binding(tt) + except Exception as e: + raise RuntimeError( + f"OVPhysX could not create rigid-body binding {tt!r}. " + f"Check that prim_path={self._binding_pattern!r} matches " + f"at least one UsdPhysics.RigidBodyAPI prim and that the " + f"ovphysx wheel exposes the RIGID_BODY_* TensorType. " + f"Note: pattern resolution may currently include articulation " + f"links; an explicit selection policy is on the wheel-side roadmap." + ) from e + + # read counts and body names from the root-pose binding + root_pose = self._bindings[TT.RIGID_BODY_POSE] + self._num_instances = root_pose.count + self._num_bodies = 1 + try: + body_names_value = root_pose.body_names + # body_names may be an empty list for non-articulation bindings; fall back to + # the documented single-body default in that case. + self._body_names = list(body_names_value) if body_names_value else ["base_link"] + except (AttributeError, TypeError): + # ovphysx TensorBinding raises TypeError (not AttributeError) when body_names + # is queried on a non-articulation tensor type such as RIGID_BODY_POSE: + # "Articulation metadata … is not available for tensor type 'RIGID_BODY_POSE'." + # For a single-body rigid object the default ["base_link"] is always correct. + self._body_names = ["base_link"] + + # container for data access + self._data = RigidObjectData(self._bindings, self._device, check_shapes=self._check_shapes) + + # create buffers + self._create_buffers() + # process configuration + self._process_cfg() + # update the rigid body data + self.update(0.0) + # Let the rigid object data know that it is fully instantiated and ready to use. + self._data.is_primed = True + + def _create_buffers(self) -> None: + """Create buffers for storing data.""" + N = self._num_instances + B = 1 # rigid object always has a single body + device = self._device + + # constants + self._ALL_INDICES = wp.array(np.arange(N, dtype=np.int32), device=device) + self._ALL_BODY_INDICES = wp.array(np.arange(B, dtype=np.int32), device=device) + # All-true masks for default mask paths. These let ``binding.write(..., mask=...)`` + # cover all instances when no env_mask is supplied, without converting back to indices. + self._ALL_TRUE_ENV_MASK = wp.array(np.ones(N, dtype=bool), dtype=wp.bool, device=device) + self._ALL_TRUE_BODY_MASK = wp.array(np.ones(B, dtype=bool), dtype=wp.bool, device=device) + + # external wrench composer + # The kernel writes into the (N, 1, 9) view; the binding consumes the (N, 9) view -- + # both alias the same allocation, so we cache the flat reshape once. + self._wrench_buf = wp.zeros((N, 1, 9), dtype=wp.float32, device=device) + self._wrench_buf_flat = wp.array( + ptr=self._wrench_buf.ptr, + shape=(N, 9), + dtype=wp.float32, + device=device, + copy=False, + ) + self._instantaneous_wrench_composer = WrenchComposer(self) + self._permanent_wrench_composer = WrenchComposer(self) + + # set information about rigid body into data + self._data.body_names = self._body_names + + # Pre-allocated pinned CPU buffers for OVPhysX TensorBinding writes. The wheel + # requires CPU arrays for "model" property updates (mass / coms / inertia); pinned + # host memory enables DMA fast path and avoids per-call ``wp.clone`` allocation. + self._cpu_env_ids_all = wp.zeros(N, dtype=wp.int32, device="cpu", pinned=True) + wp.copy(self._cpu_env_ids_all, self._ALL_INDICES) + self._cpu_body_mass = wp.zeros((N, B), dtype=wp.float32, device="cpu", pinned=True) + self._cpu_body_coms = wp.zeros((N, B, 7), dtype=wp.float32, device="cpu", pinned=True) + self._cpu_body_inertia = wp.zeros((N, B, 9), dtype=wp.float32, device="cpu", pinned=True) + # Pinned-host mask staging for CPU-only binding writes (mass/coms/inertia). + self._cpu_env_mask = wp.zeros(N, dtype=wp.bool, device="cpu", pinned=True) + + def _process_cfg(self) -> None: + """Post processing of configuration parameters.""" + # default state + # -- root state + # note: we cast to tuple to avoid torch/numpy type mismatch. + default_root_pose = tuple(self.cfg.init_state.pos) + tuple(self.cfg.init_state.rot) + default_root_vel = tuple(self.cfg.init_state.lin_vel) + tuple(self.cfg.init_state.ang_vel) + default_root_pose = np.tile(np.array(default_root_pose, dtype=np.float32), (self._num_instances, 1)) + default_root_vel = np.tile(np.array(default_root_vel, dtype=np.float32), (self._num_instances, 1)) + self._data.default_root_pose = wp.array(default_root_pose, dtype=wp.transformf, device=self._device) + self._data.default_root_vel = wp.array(default_root_vel, dtype=wp.spatial_vectorf, device=self._device) + + def _resolve_env_ids(self, env_ids: Sequence[int] | torch.Tensor | wp.array | None) -> wp.array: + """Resolve environment indices to a warp array. + + Args: + env_ids: Environment indices. If None, then all indices are used. + + Returns: + A warp array of environment indices on ``self._device``. + """ + if env_ids is None or env_ids == slice(None): + return self._ALL_INDICES + if isinstance(env_ids, list): + return wp.array(env_ids, dtype=wp.int32, device=self._device) + if isinstance(env_ids, torch.Tensor): + return wp.from_torch(env_ids.to(torch.int32), dtype=wp.int32) + if isinstance(env_ids, wp.array) and str(env_ids.device) != self._device: + env_ids = wp.clone(env_ids, device=self._device) + return env_ids + + def _resolve_body_ids(self, body_ids: Sequence[int] | torch.Tensor | wp.array | None) -> wp.array: + """Resolve body indices to a warp array. + + Args: + body_ids: Body indices. If None, then all indices are used. + + Returns: + A warp array of body indices on ``self._device``. + """ + if body_ids is None or body_ids == slice(None): + return self._ALL_BODY_INDICES + if isinstance(body_ids, list): + return wp.array(body_ids, dtype=wp.int32, device=self._device) + return body_ids + + def _resolve_env_mask(self, env_mask: wp.array | None) -> wp.array: + """Resolve an environment mask to a ``wp.bool`` array. + + Args: + env_mask: Environment mask. If None, then the pre-allocated all-true mask is used. + + Returns: + A warp ``wp.bool`` array on ``self._device``. + """ + if env_mask is None: + return self._ALL_TRUE_ENV_MASK + if isinstance(env_mask, torch.Tensor): + return wp.from_torch(env_mask.to(torch.bool), dtype=wp.bool) + if isinstance(env_mask, wp.array) and str(env_mask.device) != self._device: + env_mask = wp.clone(env_mask, device=self._device) + return env_mask + + def _resolve_body_mask(self, body_mask: wp.array | None) -> wp.array: + """Resolve a body mask to a ``wp.bool`` array. + + Args: + body_mask: Body mask. If None, then the pre-allocated all-true mask is used. + + Returns: + A warp ``wp.bool`` array on ``self._device``. + """ + if body_mask is None: + return self._ALL_TRUE_BODY_MASK + if isinstance(body_mask, torch.Tensor): + return wp.from_torch(body_mask.to(torch.bool), dtype=wp.bool) + if isinstance(body_mask, wp.array) and str(body_mask.device) != self._device: + body_mask = wp.clone(body_mask, device=self._device) + return body_mask + + def _get_cpu_env_mask(self, env_mask: wp.array) -> wp.array: + """Return a pinned-host CPU copy of *env_mask* for a CPU-only binding write. + + The wheel's ``binding.write(mask=...)`` requires the mask on the binding's + device, which is CPU for mass / coms / inertia. Reuses the pre-allocated + ``_cpu_env_mask`` pinned buffer. + """ + wp.copy(self._cpu_env_mask, env_mask) + return self._cpu_env_mask + + def _get_cpu_env_ids(self, env_ids: wp.array | torch.Tensor) -> wp.array: + """Return CPU int32 indices, using the pre-allocated pinned ``_cpu_env_ids_all`` + fast path when *env_ids* matches ``_ALL_INDICES``. + """ + if isinstance(env_ids, torch.Tensor): + env_ids = wp.from_torch(env_ids, dtype=wp.int32) + if env_ids.ptr == self._ALL_INDICES.ptr: + return self._cpu_env_ids_all + return wp.clone(env_ids, device="cpu") + + def _get_binding(self, tensor_type: int): + """Return a cached TensorBinding, creating it on first access. + + Bindings are lightweight handles (a pointer + shape metadata into PhysX's + shared GPU buffer). Creating one does NOT allocate new GPU memory -- the + underlying simulation buffers are allocated once by PhysX regardless of how + many bindings point into them. Still, we defer creation so that tensor types + the user never queries are never looked up. + + Args: + tensor_type: The TensorType constant identifying which simulation buffer + to bind (e.g. :attr:`~isaaclab_ovphysx.tensor_types.RIGID_BODY_POSE`). + + Returns: + The cached TensorBinding for ``tensor_type``. + """ + binding = self._bindings.get(tensor_type) + if binding is not None: + return binding + binding = self._ovphysx.create_tensor_binding(pattern=self._binding_pattern, tensor_type=tensor_type) + self._bindings[tensor_type] = binding + return binding + + """ + Internal simulation callbacks. + """ + + def _invalidate_initialize_callback(self, event) -> None: + """Invalidates the scene elements.""" + super()._invalidate_initialize_callback(event) + + def write_root_state_to_sim( + self, + root_state: torch.Tensor | wp.array, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + ) -> None: + """Deprecated, same as :meth:`write_root_link_pose_to_sim_index` and + :meth:`write_root_com_velocity_to_sim_index`.""" + warnings.warn( + "The function 'write_root_state_to_sim' will be deprecated in a future release. Please" + " use 'write_root_link_pose_to_sim_index' and 'write_root_com_velocity_to_sim_index' instead.", + DeprecationWarning, + stacklevel=2, + ) + self.write_root_link_pose_to_sim_index(root_pose=root_state[:, :7], env_ids=env_ids) + self.write_root_com_velocity_to_sim_index(root_velocity=root_state[:, 7:], env_ids=env_ids) + + def write_root_com_state_to_sim( + self, + root_state: torch.Tensor | wp.array, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + ) -> None: + """Deprecated, same as :meth:`write_root_com_pose_to_sim_index` and + :meth:`write_root_com_velocity_to_sim_index`.""" + warnings.warn( + "The function 'write_root_com_state_to_sim' will be deprecated in a future release. Please" + " use 'write_root_com_pose_to_sim_index' and 'write_root_com_velocity_to_sim_index' instead.", + DeprecationWarning, + stacklevel=2, + ) + self.write_root_com_pose_to_sim_index(root_pose=root_state[:, :7], env_ids=env_ids) + self.write_root_com_velocity_to_sim_index(root_velocity=root_state[:, 7:], env_ids=env_ids) + + def write_root_link_state_to_sim( + self, + root_state: torch.Tensor | wp.array, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + ) -> None: + """Deprecated, same as :meth:`write_root_link_pose_to_sim_index` and + :meth:`write_root_link_velocity_to_sim_index`.""" + warnings.warn( + "The function 'write_root_link_state_to_sim' will be deprecated in a future release. Please" + " use 'write_root_link_pose_to_sim_index' and 'write_root_link_velocity_to_sim_index' instead.", + DeprecationWarning, + stacklevel=2, + ) + self.write_root_link_pose_to_sim_index(root_pose=root_state[:, :7], env_ids=env_ids) + self.write_root_link_velocity_to_sim_index(root_velocity=root_state[:, 7:], env_ids=env_ids) diff --git a/source/isaaclab_ovphysx/isaaclab_ovphysx/assets/rigid_object/rigid_object_data.py b/source/isaaclab_ovphysx/isaaclab_ovphysx/assets/rigid_object/rigid_object_data.py new file mode 100644 index 000000000000..05e5c45a0ebb --- /dev/null +++ b/source/isaaclab_ovphysx/isaaclab_ovphysx/assets/rigid_object/rigid_object_data.py @@ -0,0 +1,1198 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""OVPhysX-backed RigidObjectData implementation.""" + +from __future__ import annotations + +import math +import warnings + +import torch +import warp as wp + +from isaaclab.assets.rigid_object.base_rigid_object_data import BaseRigidObjectData +from isaaclab.utils.buffers import TimestampedBufferWarp as TimestampedBuffer +from isaaclab.utils.math import normalize +from isaaclab.utils.warp import ProxyArray + +from isaaclab_ovphysx import tensor_types as TT +from isaaclab_ovphysx.assets import kernels as shared_kernels +from isaaclab_ovphysx.physics import OvPhysxManager as SimulationManager + + +class RigidObjectData(BaseRigidObjectData): + """Data container for a rigid object. + + This class contains the data for a rigid object in the simulation. The data includes the state of + the root rigid body and the state of all the bodies in the object. The data is stored in the simulation + world frame unless otherwise specified. + + For a rigid body, there are two frames of reference that are used: + + - Actor frame: The frame of reference of the rigid body prim. This typically corresponds to the Xform prim + with the rigid body schema. + - Center of mass frame: The frame of reference of the center of mass of the rigid body. + + Depending on the settings of the simulation, the actor frame and the center of mass frame may be the same. + This needs to be taken into account when interpreting the data. + + The data is lazily updated, meaning that the data is only updated when it is accessed. This is useful + when the data is expensive to compute or retrieve. The data is updated when the timestamp of the buffer + is older than the current simulation timestamp. The timestamp is updated whenever the data is updated. + + .. note:: + **Pull-to-refresh model.** Properties pull fresh data from the PhysX tensor API on first + access per timestamp and cache the result. This differs from Newton, where buffers are + refreshed automatically by the simulation. + + .. note:: + **ProxyArray pointer stability.** Each :class:`ProxyArray` wrapper is created once and + reused because the PhysX tensor API returns views into stable, pre-allocated GPU buffers + whose device pointer does not change across simulation steps. + """ + + __backend_name__: str = "ovphysx" + """The name of the backend for the rigid object data.""" + + def __init__( + self, + bindings: dict, + device: str, + check_shapes: bool = True, + ): + """Initializes the rigid object data. + + Args: + bindings: The OVPhysX tensor bindings dict keyed by tensor-type constant. + ``num_instances`` is read from ``bindings[RIGID_BODY_POSE].count`` and + ``num_bodies`` is fixed at 1; ``body_names`` is set by + :meth:`~isaaclab_ovphysx.assets.RigidObject._initialize_impl`. + device: The device used for processing. + check_shapes: Whether to enforce internal shape/dtype invariants on + lazy reads. Defaults to ``True``; production callers thread this + from :attr:`~isaaclab.assets.AssetBaseCfg.disable_shape_checks`. + """ + super().__init__(bindings, device) + # Set the tensor bindings (OVPhysX exposes per-tensor-type bindings rather than a single view). + self._bindings = bindings + self._check_shapes = check_shapes + # Set initial time stamp + self._sim_timestamp = 0.0 + self._is_primed = False + root_pose = self._bindings[TT.RIGID_BODY_POSE] + self._num_instances = root_pose.count + self._num_bodies = 1 + + if SimulationManager._sim is not None and hasattr(SimulationManager._sim, "cfg"): + gravity = SimulationManager._sim.cfg.gravity + else: + gravity = (0.0, 0.0, -9.81) + + gravity_dir = torch.tensor((gravity[0], gravity[1], gravity[2]), device=self.device) + # When gravity is disabled (cfg.gravity == (0, 0, 0)), normalize() would NaN. + if torch.linalg.norm(gravity_dir) > 0.0: + gravity_dir = normalize(gravity_dir.unsqueeze(0)).squeeze(0) + gravity_dir = gravity_dir.repeat(self._num_instances, 1) + forward_vec = torch.tensor((1.0, 0.0, 0.0), device=self.device).repeat(self._num_instances, 1) + + # Initialize constants + self.GRAVITY_VEC_W = ProxyArray(wp.from_torch(gravity_dir, dtype=wp.vec3f)) + self.FORWARD_VEC_B = ProxyArray(wp.from_torch(forward_vec, dtype=wp.vec3f)) + + self._create_buffers() + + @property + def is_primed(self) -> bool: + """Whether the rigid object data is fully instantiated and ready to use.""" + return self._is_primed + + @is_primed.setter + def is_primed(self, value: bool) -> None: + """Set whether the rigid object data is fully instantiated and ready to use. + + .. note:: + Once this quantity is set to True, it cannot be changed. + + Args: + value: The primed state. + + Raises: + ValueError: If the rigid object data is already primed. + """ + if self._is_primed: + raise ValueError("The rigid object data is already primed.") + self._is_primed = value + + def update(self, dt: float) -> None: + """Updates the data for the rigid object. + + Args: + dt: The time step for the update [s]. This must be a positive value. + """ + # update the simulation timestamp + self._sim_timestamp += dt + # Trigger an update of the body com acceleration buffer at a higher frequency + # since we do finite differencing. + self.body_com_acc_w + + """ + Names. + """ + + body_names: list[str] = None + """Body names in the order parsed by the simulation view.""" + + """ + Defaults. + """ + + @property + def default_root_pose(self) -> ProxyArray: + """Default root pose ``[pos, quat]`` in simulation world frame [m, -]. + Shape is (num_instances,), dtype = wp.transformf. + In torch this resolves to (num_instances, 7). + + Populated from :attr:`RigidObjectCfg.init_state` during initialisation. + """ + if self._default_root_pose_ta is None: + self._default_root_pose_ta = ProxyArray(self._default_root_pose) + return self._default_root_pose_ta + + @default_root_pose.setter + def default_root_pose(self, value: wp.array) -> None: + """Set the default root pose. + + Args: + value: The default root pose. Shape is (num_instances, 7). + + Raises: + ValueError: If the rigid object data is already primed. + """ + if self._is_primed: + raise ValueError("The rigid object data is already primed.") + self._default_root_pose.assign(value) + + @property + def default_root_vel(self) -> ProxyArray: + """Default root velocity ``[lin_vel, ang_vel]`` in simulation world frame [m/s, rad/s]. + Shape is (num_instances,), dtype = wp.spatial_vectorf. + In torch this resolves to (num_instances, 6). + + Populated from :attr:`RigidObjectCfg.init_state` during initialisation. + """ + if self._default_root_vel_ta is None: + self._default_root_vel_ta = ProxyArray(self._default_root_vel) + return self._default_root_vel_ta + + @default_root_vel.setter + def default_root_vel(self, value: wp.array) -> None: + """Set the default root velocity. + + Args: + value: The default root velocity. Shape is (num_instances, 6). + + Raises: + ValueError: If the rigid object data is already primed. + """ + if self._is_primed: + raise ValueError("The rigid object data is already primed.") + self._default_root_vel.assign(value) + + """ + Root state properties. + """ + + @property + def root_link_pose_w(self) -> ProxyArray: + """Root link pose ``[pos, quat]`` in simulation world frame [m, -]. + + Shape is (num_instances,), dtype = wp.transformf. In torch this resolves to (num_instances, 7). + This quantity is the pose of the actor frame of the root rigid body relative to the world. + The orientation is provided in (x, y, z, w) format. + """ + if self._root_link_pose_w.timestamp < self._sim_timestamp: + # read data from simulation + self._read_binding_into(TT.RIGID_BODY_POSE, self._root_link_pose_w.data) + self._root_link_pose_w.timestamp = self._sim_timestamp + if self._root_link_pose_w_ta is None: + self._root_link_pose_w_ta = ProxyArray(self._root_link_pose_w.data) + return self._root_link_pose_w_ta + + @property + def root_link_vel_w(self) -> ProxyArray: + """Root link velocity ``[lin_vel, ang_vel]`` in simulation world frame [m/s, rad/s]. + + Shape is (num_instances,), dtype = wp.spatial_vectorf. In torch this resolves to (num_instances, 6). + This quantity contains the linear and angular velocities of the actor frame of the root + rigid body relative to the world. + """ + if self._root_link_vel_w.timestamp < self._sim_timestamp: + wp.launch( + shared_kernels.get_root_link_vel_from_root_com_vel, + dim=self._num_instances, + inputs=[ + self.root_com_vel_w, + self.root_link_pose_w, + self.body_com_pose_b, + ], + outputs=[self._root_link_vel_w.data], + device=self.device, + ) + self._root_link_vel_w.timestamp = self._sim_timestamp + if self._root_link_vel_w_ta is None: + self._root_link_vel_w_ta = ProxyArray(self._root_link_vel_w.data) + return self._root_link_vel_w_ta + + @property + def root_com_pose_w(self) -> ProxyArray: + """Root center of mass pose ``[pos, quat]`` in simulation world frame [m, -]. + + Shape is (num_instances,), dtype = wp.transformf. In torch this resolves to (num_instances, 7). + This quantity is the pose of the center of mass frame of the root rigid body relative to the world. + The orientation is provided in (x, y, z, w) format. + """ + if self._root_com_pose_w.timestamp < self._sim_timestamp: + # apply local transform to center of mass frame + wp.launch( + shared_kernels.get_root_com_pose_from_root_link_pose, + dim=self._num_instances, + inputs=[ + self.root_link_pose_w, + self.body_com_pose_b, + ], + outputs=[ + self._root_com_pose_w.data, + ], + device=self.device, + ) + self._root_com_pose_w.timestamp = self._sim_timestamp + + if self._root_com_pose_w_ta is None: + self._root_com_pose_w_ta = ProxyArray(self._root_com_pose_w.data) + return self._root_com_pose_w_ta + + @property + def root_com_vel_w(self) -> ProxyArray: + """Root center of mass velocity ``[lin_vel, ang_vel]`` in simulation world frame [m/s, rad/s]. + + Shape is (num_instances,), dtype = wp.spatial_vectorf. In torch this resolves to (num_instances, 6). + This quantity contains the linear and angular velocities of the root rigid body's center of mass frame + relative to the world. + """ + if self._root_com_vel_w.timestamp < self._sim_timestamp: + self._read_binding_into(TT.RIGID_BODY_VELOCITY, self._root_com_vel_w.data) + self._root_com_vel_w.timestamp = self._sim_timestamp + if self._root_com_vel_w_ta is None: + self._root_com_vel_w_ta = ProxyArray(self._root_com_vel_w.data) + return self._root_com_vel_w_ta + + """ + Body state properties. + """ + + @property + def body_mass(self) -> ProxyArray: + """Mass of all bodies [kg]. + + Shape is (num_instances, 1), dtype = wp.float32. + In torch this resolves to (num_instances, 1). + """ + if self._body_mass_ta is None: + self._body_mass_ta = ProxyArray(self._body_mass) + return self._body_mass_ta + + @property + def body_inertia(self) -> ProxyArray: + """Inertia tensor of all bodies, expressed at the center of mass [kg·m²]. + + Shape is (num_instances, 1, 9), dtype = wp.float32. The 9 components are the row-major + flatten of the 3×3 inertia matrix ``(Ixx, Ixy, Ixz, Iyx, Iyy, Iyz, Izx, Izy, Izz)``. + In torch this resolves to (num_instances, 1, 9). + """ + if self._body_inertia_ta is None: + self._body_inertia_ta = ProxyArray(self._body_inertia) + return self._body_inertia_ta + + @property + def body_link_pose_w(self) -> ProxyArray: + """Body link pose ``[pos, quat]`` in simulation world frame [m, -]. + + Shape is (num_instances, 1), dtype = wp.transformf. In torch this resolves to (num_instances, 1, 7). + This quantity is the pose of the actor frame of the rigid body relative to the world. + The orientation is provided in (x, y, z, w) format. + """ + parent = self.root_link_pose_w + if self._body_link_pose_w_ta is None: + self._body_link_pose_w_ta = ProxyArray(parent.warp.reshape((self._num_instances, 1))) + return self._body_link_pose_w_ta + + @property + def body_link_vel_w(self) -> ProxyArray: + """Body link velocity ``[lin_vel, ang_vel]`` in simulation world frame [m/s, rad/s]. + + Shape is (num_instances, 1), dtype = wp.spatial_vectorf. In torch this resolves to (num_instances, 1, 6). + This quantity contains the linear and angular velocities of the body's link (actor) frame + relative to the world. + """ + parent = self.root_link_vel_w + if self._body_link_vel_w_ta is None: + self._body_link_vel_w_ta = ProxyArray(parent.warp.reshape((self._num_instances, 1))) + return self._body_link_vel_w_ta + + @property + def body_com_pose_w(self) -> ProxyArray: + """Body center of mass pose ``[pos, quat]`` in simulation world frame. + + Shape is (num_instances, 1), dtype = wp.transformf. In torch this resolves to (num_instances, 1, 7). + This quantity is the pose of the center of mass frame of the rigid body relative to the world. + The orientation is provided in (x, y, z, w) format. + """ + parent = self.root_com_pose_w + if self._body_com_pose_w_ta is None: + self._body_com_pose_w_ta = ProxyArray(parent.warp.reshape((self._num_instances, 1))) + return self._body_com_pose_w_ta + + @property + def body_com_vel_w(self) -> ProxyArray: + """Body center of mass velocity ``[lin_vel, ang_vel]`` in simulation world frame [m/s, rad/s]. + + Shape is (num_instances, 1), dtype = wp.spatial_vectorf. In torch this resolves to (num_instances, 1, 6). + This quantity contains the linear and angular velocities of the body's center of mass frame + relative to the world. + """ + parent = self.root_com_vel_w + if self._body_com_vel_w_ta is None: + self._body_com_vel_w_ta = ProxyArray(parent.warp.reshape((self._num_instances, 1))) + return self._body_com_vel_w_ta + + @property + def body_com_acc_w(self) -> ProxyArray: + """Acceleration of all bodies ``[lin_acc, ang_acc]`` in the simulation world frame [m/s², rad/s²]. + + Shape is (num_instances, 1), dtype = wp.spatial_vectorf. In torch this resolves to (num_instances, 1, 6). + This quantity is the acceleration of the rigid bodies' center of mass frame relative to the world. + """ + if self._body_com_acc_w.timestamp < self._sim_timestamp: + if self._previous_body_com_vel is None: + self._previous_body_com_vel = wp.clone(self.body_com_vel_w.warp) + wp.launch( + shared_kernels.derive_body_acceleration_from_body_com_velocities, + dim=(self._num_instances, 1), + device=self.device, + inputs=[ + self.body_com_vel_w.warp, + SimulationManager.get_physics_dt(), + self._previous_body_com_vel, + ], + outputs=[ + self._body_com_acc_w.data, + ], + ) + self._body_com_acc_w.timestamp = self._sim_timestamp + if self._body_com_acc_w_ta is None: + self._body_com_acc_w_ta = ProxyArray(self._body_com_acc_w.data) + return self._body_com_acc_w_ta + + @property + def body_com_pose_b(self) -> ProxyArray: + """Center of mass pose ``[pos, quat]`` of all bodies in their respective body's link frames. + + Shape is (num_instances, 1), dtype = wp.transformf. In torch this resolves to (num_instances, 1, 7). + This quantity is the pose of the center of mass frame of the rigid body relative to the body's link frame. + The orientation is provided in (x, y, z, w) format. + """ + if self._body_com_pose_b.timestamp < self._sim_timestamp: + # read data from simulation + self._read_binding_into(TT.RIGID_BODY_COM_POSE, self._body_com_pose_b.data) + self._body_com_pose_b.timestamp = self._sim_timestamp + + if self._body_com_pose_b_ta is None: + self._body_com_pose_b_ta = ProxyArray(self._body_com_pose_b.data) + return self._body_com_pose_b_ta + + """ + Derived Properties. + """ + + @property + def projected_gravity_b(self) -> ProxyArray: + """Projection of the gravity direction on base frame. + + Shape is (num_instances,), dtype = wp.vec3f. In torch this resolves to (num_instances, 3). + """ + if self._projected_gravity_b.timestamp < self._sim_timestamp: + wp.launch( + shared_kernels.quat_apply_inverse_1D_kernel, + dim=self._num_instances, + inputs=[self.GRAVITY_VEC_W, self.root_link_quat_w], + outputs=[self._projected_gravity_b.data], + device=self.device, + ) + self._projected_gravity_b.timestamp = self._sim_timestamp + if self._projected_gravity_b_ta is None: + self._projected_gravity_b_ta = ProxyArray(self._projected_gravity_b.data) + return self._projected_gravity_b_ta + + @property + def heading_w(self) -> ProxyArray: + """Yaw heading of the base frame (in radians). + + Shape is (num_instances,), dtype = wp.float32. In torch this resolves to (num_instances,). + + .. note:: + This quantity is computed by assuming that the forward-direction of the base + frame is along x-direction, i.e. :math:`(1, 0, 0)`. + """ + if self._heading_w.timestamp < self._sim_timestamp: + wp.launch( + shared_kernels.root_heading_w, + dim=self._num_instances, + inputs=[self.FORWARD_VEC_B, self.root_link_quat_w], + outputs=[self._heading_w.data], + device=self.device, + ) + self._heading_w.timestamp = self._sim_timestamp + if self._heading_w_ta is None: + self._heading_w_ta = ProxyArray(self._heading_w.data) + return self._heading_w_ta + + @property + def root_link_lin_vel_b(self) -> ProxyArray: + """Root link linear velocity in base frame [m/s]. + + Shape is (num_instances,), dtype = wp.vec3f. In torch this resolves to (num_instances, 3). + This quantity is the linear velocity of the root link frame relative to the world, + expressed in the root link's actor frame. + """ + if self._root_link_lin_vel_b.timestamp < self._sim_timestamp: + wp.launch( + shared_kernels.quat_apply_inverse_1D_kernel, + dim=self._num_instances, + inputs=[self.root_link_lin_vel_w, self.root_link_quat_w], + outputs=[self._root_link_lin_vel_b.data], + device=self.device, + ) + self._root_link_lin_vel_b.timestamp = self._sim_timestamp + if self._root_link_lin_vel_b_ta is None: + self._root_link_lin_vel_b_ta = ProxyArray(self._root_link_lin_vel_b.data) + return self._root_link_lin_vel_b_ta + + @property + def root_link_ang_vel_b(self) -> ProxyArray: + """Root link angular velocity in base frame [rad/s]. + + Shape is (num_instances,), dtype = wp.vec3f. In torch this resolves to (num_instances, 3). + This quantity is the angular velocity of the root link frame relative to the world, + expressed in the root link's actor frame. + """ + if self._root_link_ang_vel_b.timestamp < self._sim_timestamp: + wp.launch( + shared_kernels.quat_apply_inverse_1D_kernel, + dim=self._num_instances, + inputs=[self.root_link_ang_vel_w, self.root_link_quat_w], + outputs=[self._root_link_ang_vel_b.data], + device=self.device, + ) + self._root_link_ang_vel_b.timestamp = self._sim_timestamp + if self._root_link_ang_vel_b_ta is None: + self._root_link_ang_vel_b_ta = ProxyArray(self._root_link_ang_vel_b.data) + return self._root_link_ang_vel_b_ta + + @property + def root_com_lin_vel_b(self) -> ProxyArray: + """Root center of mass linear velocity in base frame [m/s]. + + Shape is (num_instances,), dtype = wp.vec3f. In torch this resolves to (num_instances, 3). + This quantity is the linear velocity of the root center of mass frame relative to the world, + expressed in the root link's actor frame. + """ + if self._root_com_lin_vel_b.timestamp < self._sim_timestamp: + wp.launch( + shared_kernels.quat_apply_inverse_1D_kernel, + dim=self._num_instances, + inputs=[self.root_com_lin_vel_w, self.root_link_quat_w], + outputs=[self._root_com_lin_vel_b.data], + device=self.device, + ) + self._root_com_lin_vel_b.timestamp = self._sim_timestamp + if self._root_com_lin_vel_b_ta is None: + self._root_com_lin_vel_b_ta = ProxyArray(self._root_com_lin_vel_b.data) + return self._root_com_lin_vel_b_ta + + @property + def root_com_ang_vel_b(self) -> ProxyArray: + """Root center of mass angular velocity in base frame [rad/s]. + + Shape is (num_instances,), dtype = wp.vec3f. In torch this resolves to (num_instances, 3). + This quantity is the angular velocity of the root center of mass frame relative to the world, + expressed in the root link's actor frame. + """ + if self._root_com_ang_vel_b.timestamp < self._sim_timestamp: + wp.launch( + shared_kernels.quat_apply_inverse_1D_kernel, + dim=self._num_instances, + inputs=[self.root_com_ang_vel_w, self.root_link_quat_w], + outputs=[self._root_com_ang_vel_b.data], + device=self.device, + ) + self._root_com_ang_vel_b.timestamp = self._sim_timestamp + if self._root_com_ang_vel_b_ta is None: + self._root_com_ang_vel_b_ta = ProxyArray(self._root_com_ang_vel_b.data) + return self._root_com_ang_vel_b_ta + + """ + Sliced properties. + """ + + @property + def root_link_pos_w(self) -> ProxyArray: + """Root link position in simulation world frame. + + Shape is (num_instances,), dtype = wp.vec3f. In torch this resolves to (num_instances, 3). + This quantity is the position of the actor frame of the root rigid body relative to the world. + """ + parent = self.root_link_pose_w + if self._root_link_pos_w_ta is None: + self._root_link_pos_w_ta = ProxyArray(self._get_pos_from_transform(parent.warp)) + return self._root_link_pos_w_ta + + @property + def root_link_quat_w(self) -> ProxyArray: + """Root link orientation (x, y, z, w) in simulation world frame. + + Shape is (num_instances,), dtype = wp.quatf. In torch this resolves to (num_instances, 4). + This quantity is the orientation of the actor frame of the root rigid body. + """ + parent = self.root_link_pose_w + if self._root_link_quat_w_ta is None: + self._root_link_quat_w_ta = ProxyArray(self._get_quat_from_transform(parent.warp)) + return self._root_link_quat_w_ta + + @property + def root_link_lin_vel_w(self) -> ProxyArray: + """Root linear velocity in simulation world frame. + + Shape is (num_instances,), dtype = wp.vec3f. In torch this resolves to (num_instances, 3). + This quantity is the linear velocity of the root rigid body's actor frame relative to the world. + """ + parent = self.root_link_vel_w + if self._root_link_lin_vel_w_ta is None: + self._root_link_lin_vel_w_ta = ProxyArray(self._get_lin_vel_from_spatial_vector(parent.warp)) + return self._root_link_lin_vel_w_ta + + @property + def root_link_ang_vel_w(self) -> ProxyArray: + """Root link angular velocity in simulation world frame. + + Shape is (num_instances,), dtype = wp.vec3f. In torch this resolves to (num_instances, 3). + This quantity is the angular velocity of the actor frame of the root rigid body relative to the world. + """ + parent = self.root_link_vel_w + if self._root_link_ang_vel_w_ta is None: + self._root_link_ang_vel_w_ta = ProxyArray(self._get_ang_vel_from_spatial_vector(parent.warp)) + return self._root_link_ang_vel_w_ta + + @property + def root_com_pos_w(self) -> ProxyArray: + """Root center of mass position in simulation world frame. + + Shape is (num_instances,), dtype = wp.vec3f. In torch this resolves to (num_instances, 3). + This quantity is the position of the center of mass frame of the root rigid body relative to the world. + """ + parent = self.root_com_pose_w + if self._root_com_pos_w_ta is None: + self._root_com_pos_w_ta = ProxyArray(self._get_pos_from_transform(parent.warp)) + return self._root_com_pos_w_ta + + @property + def root_com_quat_w(self) -> ProxyArray: + """Root center of mass orientation (x, y, z, w) in simulation world frame. + + Shape is (num_instances,), dtype = wp.quatf. In torch this resolves to (num_instances, 4). + This quantity is the orientation of the principal axes of inertia of the root rigid body relative to the world. + """ + parent = self.root_com_pose_w + if self._root_com_quat_w_ta is None: + self._root_com_quat_w_ta = ProxyArray(self._get_quat_from_transform(parent.warp)) + return self._root_com_quat_w_ta + + @property + def root_com_lin_vel_w(self) -> ProxyArray: + """Root center of mass linear velocity in simulation world frame. + + Shape is (num_instances,), dtype = wp.vec3f. In torch this resolves to (num_instances, 3). + This quantity is the linear velocity of the root rigid body's center of mass frame relative to the world. + """ + parent = self.root_com_vel_w + if self._root_com_lin_vel_w_ta is None: + self._root_com_lin_vel_w_ta = ProxyArray(self._get_lin_vel_from_spatial_vector(parent.warp)) + return self._root_com_lin_vel_w_ta + + @property + def root_com_ang_vel_w(self) -> ProxyArray: + """Root center of mass angular velocity in simulation world frame. + + Shape is (num_instances,), dtype = wp.vec3f. In torch this resolves to (num_instances, 3). + This quantity is the angular velocity of the root rigid body's center of mass frame relative to the world. + """ + parent = self.root_com_vel_w + if self._root_com_ang_vel_w_ta is None: + self._root_com_ang_vel_w_ta = ProxyArray(self._get_ang_vel_from_spatial_vector(parent.warp)) + return self._root_com_ang_vel_w_ta + + @property + def body_link_pos_w(self) -> ProxyArray: + """Positions of all bodies in simulation world frame. + + Shape is (num_instances, 1), dtype = wp.vec3f. In torch this resolves to (num_instances, 1, 3). + This quantity is the position of the rigid bodies' actor frame relative to the world. + """ + parent = self.body_link_pose_w + if self._body_link_pos_w_ta is None: + self._body_link_pos_w_ta = ProxyArray(self._get_pos_from_transform(parent.warp)) + return self._body_link_pos_w_ta + + @property + def body_link_quat_w(self) -> ProxyArray: + """Orientation (x, y, z, w) of all bodies in simulation world frame. + + Shape is (num_instances, 1), dtype = wp.quatf. In torch this resolves to (num_instances, 1, 4). + This quantity is the orientation of the rigid bodies' actor frame relative to the world. + """ + parent = self.body_link_pose_w + if self._body_link_quat_w_ta is None: + self._body_link_quat_w_ta = ProxyArray(self._get_quat_from_transform(parent.warp)) + return self._body_link_quat_w_ta + + @property + def body_link_lin_vel_w(self) -> ProxyArray: + """Linear velocity of all bodies in simulation world frame. + + Shape is (num_instances, 1), dtype = wp.vec3f. In torch this resolves to (num_instances, 1, 3). + This quantity is the linear velocity of the rigid bodies' actor frame relative to the world. + """ + parent = self.body_link_vel_w + if self._body_link_lin_vel_w_ta is None: + self._body_link_lin_vel_w_ta = ProxyArray(self._get_lin_vel_from_spatial_vector(parent.warp)) + return self._body_link_lin_vel_w_ta + + @property + def body_link_ang_vel_w(self) -> ProxyArray: + """Angular velocity of all bodies in simulation world frame. + + Shape is (num_instances, 1), dtype = wp.vec3f. In torch this resolves to (num_instances, 1, 3). + This quantity is the angular velocity of the rigid bodies' actor frame relative to the world. + """ + parent = self.body_link_vel_w + if self._body_link_ang_vel_w_ta is None: + self._body_link_ang_vel_w_ta = ProxyArray(self._get_ang_vel_from_spatial_vector(parent.warp)) + return self._body_link_ang_vel_w_ta + + @property + def body_com_pos_w(self) -> ProxyArray: + """Positions of all bodies' center of mass in simulation world frame. + + Shape is (num_instances, 1), dtype = wp.vec3f. In torch this resolves to (num_instances, 1, 3). + This quantity is the position of the rigid bodies' center of mass frame. + """ + parent = self.body_com_pose_w + if self._body_com_pos_w_ta is None: + self._body_com_pos_w_ta = ProxyArray(self._get_pos_from_transform(parent.warp)) + return self._body_com_pos_w_ta + + @property + def body_com_quat_w(self) -> ProxyArray: + """Orientation (x, y, z, w) of the principal axes of inertia of all bodies in simulation world frame. + + Shape is (num_instances, 1), dtype = wp.quatf. In torch this resolves to (num_instances, 1, 4). + This quantity is the orientation of the principal axes of inertia of the rigid bodies. + """ + parent = self.body_com_pose_w + if self._body_com_quat_w_ta is None: + self._body_com_quat_w_ta = ProxyArray(self._get_quat_from_transform(parent.warp)) + return self._body_com_quat_w_ta + + @property + def body_com_lin_vel_w(self) -> ProxyArray: + """Linear velocity of all bodies in simulation world frame. + + Shape is (num_instances, 1), dtype = wp.vec3f. In torch this resolves to (num_instances, 1, 3). + This quantity is the linear velocity of the rigid bodies' center of mass frame. + """ + parent = self.body_com_vel_w + if self._body_com_lin_vel_w_ta is None: + self._body_com_lin_vel_w_ta = ProxyArray(self._get_lin_vel_from_spatial_vector(parent.warp)) + return self._body_com_lin_vel_w_ta + + @property + def body_com_ang_vel_w(self) -> ProxyArray: + """Angular velocity of all bodies in simulation world frame. + + Shape is (num_instances, 1), dtype = wp.vec3f. In torch this resolves to (num_instances, 1, 3). + This quantity is the angular velocity of the rigid bodies' center of mass frame. + """ + parent = self.body_com_vel_w + if self._body_com_ang_vel_w_ta is None: + self._body_com_ang_vel_w_ta = ProxyArray(self._get_ang_vel_from_spatial_vector(parent.warp)) + return self._body_com_ang_vel_w_ta + + @property + def body_com_lin_acc_w(self) -> ProxyArray: + """Linear acceleration of all bodies in simulation world frame. + + Shape is (num_instances, 1), dtype = wp.vec3f. In torch this resolves to (num_instances, 1, 3). + This quantity is the linear acceleration of the rigid bodies' center of mass frame. + """ + parent = self.body_com_acc_w + if self._body_com_lin_acc_w_ta is None: + self._body_com_lin_acc_w_ta = ProxyArray(self._get_lin_vel_from_spatial_vector(parent.warp)) + return self._body_com_lin_acc_w_ta + + @property + def body_com_ang_acc_w(self) -> ProxyArray: + """Angular acceleration of all bodies in simulation world frame. + + Shape is (num_instances, 1), dtype = wp.vec3f. In torch this resolves to (num_instances, 1, 3). + This quantity is the angular acceleration of the rigid bodies' center of mass frame. + """ + parent = self.body_com_acc_w + if self._body_com_ang_acc_w_ta is None: + self._body_com_ang_acc_w_ta = ProxyArray(self._get_ang_vel_from_spatial_vector(parent.warp)) + return self._body_com_ang_acc_w_ta + + @property + def body_com_pos_b(self) -> ProxyArray: + """Center of mass position of all of the bodies in their respective link frames. + + Shape is (num_instances, 1), dtype = wp.vec3f. In torch this resolves to (num_instances, 1, 3). + This quantity is the center of mass location relative to its body's link frame. + """ + parent = self.body_com_pose_b + if self._body_com_pos_b_ta is None: + self._body_com_pos_b_ta = ProxyArray(self._get_pos_from_transform(parent.warp)) + return self._body_com_pos_b_ta + + @property + def body_com_quat_b(self) -> ProxyArray: + """Orientation (x, y, z, w) of the principal axes of inertia of all of the bodies in their + respective link frames. + + Shape is (num_instances, 1), dtype = wp.quatf. In torch this resolves to (num_instances, 1, 4). + This quantity is the orientation of the principal axes of inertia relative to its body's link frame. + """ + parent = self.body_com_pose_b + if self._body_com_quat_b_ta is None: + self._body_com_quat_b_ta = ProxyArray(self._get_quat_from_transform(parent.warp)) + return self._body_com_quat_b_ta + + def _create_buffers(self) -> None: + super()._create_buffers() + # Initialize the lazy buffers. + # -- link frame w.r.t. world frame + self._root_link_pose_w = TimestampedBuffer((self._num_instances), self.device, wp.transformf) + self._root_link_vel_w = TimestampedBuffer((self._num_instances), self.device, wp.spatial_vectorf) + # -- com frame w.r.t. link frame + self._body_com_pose_b = TimestampedBuffer((self._num_instances, 1), self.device, wp.transformf) + # -- com frame w.r.t. world frame + self._root_com_pose_w = TimestampedBuffer((self._num_instances), self.device, wp.transformf) + self._root_com_vel_w = TimestampedBuffer((self._num_instances), self.device, wp.spatial_vectorf) + self._body_com_acc_w = TimestampedBuffer((self._num_instances, 1), self.device, wp.spatial_vectorf) + # -- combined state (these are cached as they concatenate) + self._root_state_w = TimestampedBuffer((self._num_instances), self.device, shared_kernels.vec13f) + self._root_link_state_w = TimestampedBuffer((self._num_instances), self.device, shared_kernels.vec13f) + self._root_com_state_w = TimestampedBuffer((self._num_instances), self.device, shared_kernels.vec13f) + # -- derived properties (these are cached to avoid repeated memory allocations) + self._projected_gravity_b = TimestampedBuffer((self._num_instances), self.device, wp.vec3f) + self._heading_w = TimestampedBuffer((self._num_instances), self.device, wp.float32) + self._root_link_lin_vel_b = TimestampedBuffer((self._num_instances), self.device, wp.vec3f) + self._root_link_ang_vel_b = TimestampedBuffer((self._num_instances), self.device, wp.vec3f) + self._root_com_lin_vel_b = TimestampedBuffer((self._num_instances), self.device, wp.vec3f) + self._root_com_ang_vel_b = TimestampedBuffer((self._num_instances), self.device, wp.vec3f) + + # -- Default state + self._default_root_pose = wp.zeros((self._num_instances), dtype=wp.transformf, device=self.device) + self._default_root_vel = wp.zeros((self._num_instances), dtype=wp.spatial_vectorf, device=self.device) + self._default_root_state = None + + # -- Previous body com velocity + self._previous_body_com_vel = None + + # -- Pinned-host staging buffers for CPU-only bindings on a non-CPU sim + # (lazily allocated, keyed by tensor type). + self._cpu_staging_buffers: dict[int, wp.array] = {} + + # -- Body properties (semi-static; read once from CPU-only bindings). + # The wheel exposes ``RIGID_BODY_MASS`` as ``(N,)`` and ``RIGID_BODY_INERTIA`` as ``(N, 9)``; + # the ``BaseRigidObjectData`` contract is ``(N, 1)`` and ``(N, 1, 9)`` respectively, so we + # read into a flat buffer and reshape (zero-copy) after the read. + mass_binding = self._bindings[TT.RIGID_BODY_MASS] + inertia_binding = self._bindings[TT.RIGID_BODY_INERTIA] + self._body_mass = wp.zeros(mass_binding.shape, dtype=wp.float32, device=self.device) + self._body_inertia = wp.zeros(inertia_binding.shape, dtype=wp.float32, device=self.device) + self._read_binding_into(TT.RIGID_BODY_MASS, self._body_mass) + self._read_binding_into(TT.RIGID_BODY_INERTIA, self._body_inertia) + self._body_mass = self._body_mass.reshape((self._num_instances, 1)) + self._body_inertia = self._body_inertia.reshape((self._num_instances, 1, 9)) + + # Initialize ProxyArray wrappers + self._pin_proxy_arrays() + + def _pin_proxy_arrays(self) -> None: + """Create pinned ProxyArray wrappers for all data buffers. + + This is called once from :meth:`_create_buffers` during initialization. + PhysX tensor API buffers have stable GPU pointers across simulation steps, + so no rebinding is needed (unlike Newton). + """ + # -- Pinned ProxyArray cache (one per read property, lazily created on first access) + # Defaults + self._default_root_pose_ta: ProxyArray | None = None + self._default_root_vel_ta: ProxyArray | None = None + # Root state (timestamped) + self._root_link_pose_w_ta: ProxyArray | None = None + self._root_link_vel_w_ta: ProxyArray | None = None + self._root_com_pose_w_ta: ProxyArray | None = None + self._root_com_vel_w_ta: ProxyArray | None = None + # Body properties + self._body_mass_ta: ProxyArray | None = None + self._body_inertia_ta: ProxyArray | None = None + # Body state (reshaped from root) + self._body_link_pose_w_ta: ProxyArray | None = None + self._body_link_vel_w_ta: ProxyArray | None = None + self._body_com_pose_w_ta: ProxyArray | None = None + self._body_com_vel_w_ta: ProxyArray | None = None + self._body_com_acc_w_ta: ProxyArray | None = None + self._body_com_pose_b_ta: ProxyArray | None = None + # Derived properties (timestamped) + self._projected_gravity_b_ta: ProxyArray | None = None + self._heading_w_ta: ProxyArray | None = None + self._root_link_lin_vel_b_ta: ProxyArray | None = None + self._root_link_ang_vel_b_ta: ProxyArray | None = None + self._root_com_lin_vel_b_ta: ProxyArray | None = None + self._root_com_ang_vel_b_ta: ProxyArray | None = None + # Sliced properties (root link) + self._root_link_pos_w_ta: ProxyArray | None = None + self._root_link_quat_w_ta: ProxyArray | None = None + self._root_link_lin_vel_w_ta: ProxyArray | None = None + self._root_link_ang_vel_w_ta: ProxyArray | None = None + # Sliced properties (root com) + self._root_com_pos_w_ta: ProxyArray | None = None + self._root_com_quat_w_ta: ProxyArray | None = None + self._root_com_lin_vel_w_ta: ProxyArray | None = None + self._root_com_ang_vel_w_ta: ProxyArray | None = None + # Sliced properties (body link) + self._body_link_pos_w_ta: ProxyArray | None = None + self._body_link_quat_w_ta: ProxyArray | None = None + self._body_link_lin_vel_w_ta: ProxyArray | None = None + self._body_link_ang_vel_w_ta: ProxyArray | None = None + # Sliced properties (body com) + self._body_com_pos_w_ta: ProxyArray | None = None + self._body_com_quat_w_ta: ProxyArray | None = None + self._body_com_lin_vel_w_ta: ProxyArray | None = None + self._body_com_ang_vel_w_ta: ProxyArray | None = None + self._body_com_lin_acc_w_ta: ProxyArray | None = None + self._body_com_ang_acc_w_ta: ProxyArray | None = None + # Sliced properties (body com in body frame) + self._body_com_pos_b_ta: ProxyArray | None = None + self._body_com_quat_b_ta: ProxyArray | None = None + # Deprecated state-concat properties + self._default_root_state_ta: ProxyArray | None = None + self._root_state_w_ta: ProxyArray | None = None + self._root_link_state_w_ta: ProxyArray | None = None + self._root_com_state_w_ta: ProxyArray | None = None + self._body_state_w_ta: ProxyArray | None = None + self._body_link_state_w_ta: ProxyArray | None = None + self._body_com_state_w_ta: ProxyArray | None = None + + """ + Internal helpers. + """ + + def _get_binding(self, tensor_type: int): + """Return the binding for the given tensor type, or None.""" + return self._bindings.get(tensor_type) + + def _read_binding_into(self, tensor_type: int, dst: wp.array) -> None: + """Read the OVPhysX TensorBinding for *tensor_type* into *dst*. + + Adapter that replaces PhysX's view-getter pattern: the wheel exposes + ``binding.read(target)`` rather than a getter returning a wp.array, so + we read into a flat float32 view of *dst*. CPU-only bindings on a + non-CPU sim go through a lazily-allocated pinned-host wp.array to + satisfy the wheel's device match. + """ + binding = self._bindings[tensor_type] + if self._check_shapes: + dst_bytes = dst.size * wp.types.type_size_in_bytes(dst.dtype) + binding_bytes = 4 * math.prod(binding.shape) + assert dst_bytes >= binding_bytes, ( + f"_read_binding_into: dst buffer too small for binding {tensor_type!r} " + f"({dst_bytes} B < {binding_bytes} B). Caller allocated dst with " + f"shape={tuple(dst.shape)}, dtype={dst.dtype}; binding shape={tuple(binding.shape)}." + ) + # Build a flat float32 view of dst matching the binding's shape. + if dst.dtype == wp.float32: + view = dst + else: + view = wp.array( + ptr=dst.ptr, + shape=binding.shape, + dtype=wp.float32, + device=str(dst.device), + copy=False, + ) + if tensor_type in TT._CPU_ONLY_TYPES and str(view.device) != "cpu": + staging = self._cpu_staging_buffers.get(tensor_type) + if staging is None: + staging = wp.zeros(binding.shape, dtype=wp.float32, device="cpu", pinned=True) + self._cpu_staging_buffers[tensor_type] = staging + binding.read(staging) + wp.copy(view, staging) + else: + binding.read(view) + + def _get_pos_from_transform(self, transform: wp.array) -> wp.array: + """Generates a position array from a transform array.""" + return wp.array( + ptr=transform.ptr, + shape=transform.shape, + dtype=wp.vec3f, + strides=transform.strides, + device=self.device, + ) + + def _get_quat_from_transform(self, transform: wp.array) -> wp.array: + """Generates a quaternion array from a transform array.""" + return wp.array( + ptr=transform.ptr + 3 * 4, + shape=transform.shape, + dtype=wp.quatf, + strides=transform.strides, + device=self.device, + ) + + def _get_lin_vel_from_spatial_vector(self, sv: wp.array) -> wp.array: + """Generates a linear velocity array from a spatial vector array.""" + return wp.array( + ptr=sv.ptr, + shape=sv.shape, + dtype=wp.vec3f, + strides=sv.strides, + device=self.device, + ) + + def _get_ang_vel_from_spatial_vector(self, sv: wp.array) -> wp.array: + """Generates an angular velocity array from a spatial vector array.""" + return wp.array( + ptr=sv.ptr + 3 * 4, + shape=sv.shape, + dtype=wp.vec3f, + strides=sv.strides, + device=self.device, + ) + + """ + Deprecated properties. + """ + + @property + def default_root_state(self) -> ProxyArray: + """Default root state ``[pos, quat, lin_vel, ang_vel]`` in local environment frame. + + The position and quaternion are of the rigid body's actor frame. Meanwhile, the linear and angular velocities + are of the center of mass frame. Shape is (num_instances, 13). + """ + warnings.warn( + "Reading the root state directly is deprecated since IsaacLab 3.0 and will be removed in a future version. " + "Please use the default_root_pose and default_root_vel properties instead.", + DeprecationWarning, + stacklevel=2, + ) + if self._default_root_state is None: + self._default_root_state = wp.zeros((self._num_instances), dtype=shared_kernels.vec13f, device=self.device) + wp.launch( + shared_kernels.concat_root_pose_and_vel_to_state, + dim=self._num_instances, + inputs=[ + self._default_root_pose, + self._default_root_vel, + ], + outputs=[ + self._default_root_state, + ], + device=self.device, + ) + if self._default_root_state_ta is None: + self._default_root_state_ta = ProxyArray(self._default_root_state) + return self._default_root_state_ta + + @property + def root_state_w(self) -> ProxyArray: + """Deprecated, same as :attr:`root_link_pose_w` and :attr:`root_com_vel_w`.""" + warnings.warn( + "The `root_state_w` property will be deprecated in IsaacLab 4.0. Please use `root_link_pose_w` and " + "`root_com_vel_w` instead.", + DeprecationWarning, + stacklevel=2, + ) + if self._root_state_w.timestamp < self._sim_timestamp: + wp.launch( + shared_kernels.concat_root_pose_and_vel_to_state, + dim=self._num_instances, + inputs=[ + self.root_link_pose_w, + self.root_com_vel_w, + ], + outputs=[ + self._root_state_w.data, + ], + device=self.device, + ) + self._root_state_w.timestamp = self._sim_timestamp + + if self._root_state_w_ta is None: + self._root_state_w_ta = ProxyArray(self._root_state_w.data) + return self._root_state_w_ta + + @property + def root_link_state_w(self) -> ProxyArray: + """Deprecated, same as :attr:`root_link_pose_w` and :attr:`root_link_vel_w`.""" + warnings.warn( + "The `root_link_state_w` property will be deprecated in IsaacLab 4.0. Please use `root_link_pose_w` and " + "`root_link_vel_w` instead.", + DeprecationWarning, + stacklevel=2, + ) + if self._root_link_state_w.timestamp < self._sim_timestamp: + wp.launch( + shared_kernels.concat_root_pose_and_vel_to_state, + dim=self._num_instances, + inputs=[ + self.root_link_pose_w, + self.root_link_vel_w, + ], + outputs=[ + self._root_link_state_w.data, + ], + device=self.device, + ) + self._root_link_state_w.timestamp = self._sim_timestamp + + if self._root_link_state_w_ta is None: + self._root_link_state_w_ta = ProxyArray(self._root_link_state_w.data) + return self._root_link_state_w_ta + + @property + def root_com_state_w(self) -> ProxyArray: + """Deprecated, same as :attr:`root_com_pose_w` and :attr:`root_com_vel_w`.""" + warnings.warn( + "The `root_com_state_w` property will be deprecated in IsaacLab 4.0. Please use `root_com_pose_w` and " + "`root_com_vel_w` instead.", + DeprecationWarning, + stacklevel=2, + ) + if self._root_com_state_w.timestamp < self._sim_timestamp: + wp.launch( + shared_kernels.concat_root_pose_and_vel_to_state, + dim=self._num_instances, + inputs=[ + self.root_com_pose_w, + self.root_com_vel_w, + ], + outputs=[ + self._root_com_state_w.data, + ], + device=self.device, + ) + self._root_com_state_w.timestamp = self._sim_timestamp + + if self._root_com_state_w_ta is None: + self._root_com_state_w_ta = ProxyArray(self._root_com_state_w.data) + return self._root_com_state_w_ta + + @property + def body_state_w(self) -> ProxyArray: + """Deprecated, same as :attr:`body_link_pose_w` and :attr:`body_com_vel_w`.""" + warnings.warn( + "The `body_state_w` property will be deprecated in IsaacLab 4.0. Please use `body_link_pose_w` and " + "`body_com_vel_w` instead.", + DeprecationWarning, + stacklevel=2, + ) + # Access internal buffer directly to avoid cascading deprecation warnings from root_state_w + if self._root_state_w.timestamp < self._sim_timestamp: + wp.launch( + shared_kernels.concat_root_pose_and_vel_to_state, + dim=self._num_instances, + inputs=[ + self.root_link_pose_w, + self.root_com_vel_w, + ], + outputs=[ + self._root_state_w.data, + ], + device=self.device, + ) + self._root_state_w.timestamp = self._sim_timestamp + if self._body_state_w_ta is None: + self._body_state_w_ta = ProxyArray(self._root_state_w.data.reshape((self._num_instances, 1))) + return self._body_state_w_ta + + @property + def body_link_state_w(self) -> ProxyArray: + """Deprecated, same as :attr:`body_link_pose_w` and :attr:`body_link_vel_w`.""" + warnings.warn( + "The `body_link_state_w` property will be deprecated in IsaacLab 4.0. Please use `body_link_pose_w` and " + "`body_link_vel_w` instead.", + DeprecationWarning, + stacklevel=2, + ) + # Access internal buffer directly to avoid cascading deprecation warnings from root_link_state_w + if self._root_link_state_w.timestamp < self._sim_timestamp: + wp.launch( + shared_kernels.concat_root_pose_and_vel_to_state, + dim=self._num_instances, + inputs=[ + self.root_link_pose_w, + self.root_link_vel_w, + ], + outputs=[ + self._root_link_state_w.data, + ], + device=self.device, + ) + self._root_link_state_w.timestamp = self._sim_timestamp + if self._body_link_state_w_ta is None: + self._body_link_state_w_ta = ProxyArray(self._root_link_state_w.data.reshape((self._num_instances, 1))) + return self._body_link_state_w_ta + + @property + def body_com_state_w(self) -> ProxyArray: + """Deprecated, same as :attr:`body_com_pose_w` and :attr:`body_com_vel_w`.""" + warnings.warn( + "The `body_com_state_w` property will be deprecated in IsaacLab 4.0. Please use `body_com_pose_w` and " + "`body_com_vel_w` instead.", + DeprecationWarning, + stacklevel=2, + ) + # Access internal buffer directly to avoid cascading deprecation warnings from root_com_state_w + if self._root_com_state_w.timestamp < self._sim_timestamp: + wp.launch( + shared_kernels.concat_root_pose_and_vel_to_state, + dim=self._num_instances, + inputs=[ + self.root_com_pose_w, + self.root_com_vel_w, + ], + outputs=[ + self._root_com_state_w.data, + ], + device=self.device, + ) + self._root_com_state_w.timestamp = self._sim_timestamp + if self._body_com_state_w_ta is None: + self._body_com_state_w_ta = ProxyArray(self._root_com_state_w.data.reshape((self._num_instances, 1))) + return self._body_com_state_w_ta diff --git a/source/isaaclab_ovphysx/isaaclab_ovphysx/cloner/ovphysx_replicate.py b/source/isaaclab_ovphysx/isaaclab_ovphysx/cloner/ovphysx_replicate.py index 7c46a6060b88..d89a45280a50 100644 --- a/source/isaaclab_ovphysx/isaaclab_ovphysx/cloner/ovphysx_replicate.py +++ b/source/isaaclab_ovphysx/isaaclab_ovphysx/cloner/ovphysx_replicate.py @@ -5,9 +5,9 @@ """OvPhysX replication hook for IsaacLab's cloning pipeline. -Called by :func:`isaaclab.cloner.clone_from_template` in place of the PhysX -or Newton replicators. Unlike those replicators, ovphysx.PhysX does not exist -yet at this point in the scene setup — it is created lazily on the first +Called from the scene cloning path in place of immediate PhysX or Newton +replication. Unlike those replicators, ovphysx.PhysX does not exist yet at +this point in the scene setup — it is created lazily on the first :meth:`~isaaclab_ovphysx.physics.OvPhysxManager.reset` call. This function records a *pending clone* on :class:`OvPhysxManager`. When @@ -20,6 +20,8 @@ from __future__ import annotations +from collections.abc import Sequence + import torch from pxr import Usd @@ -27,8 +29,8 @@ def ovphysx_replicate( stage: Usd.Stage, - sources: list[str], - destinations: list[str], + sources: Sequence[str], + destinations: Sequence[str], env_ids: torch.Tensor, mapping: torch.Tensor, positions: torch.Tensor | None = None, diff --git a/source/isaaclab_ovphysx/isaaclab_ovphysx/physics/ovphysx_manager.py b/source/isaaclab_ovphysx/isaaclab_ovphysx/physics/ovphysx_manager.py index 9063078e45b3..9be415fed577 100644 --- a/source/isaaclab_ovphysx/isaaclab_ovphysx/physics/ovphysx_manager.py +++ b/source/isaaclab_ovphysx/isaaclab_ovphysx/physics/ovphysx_manager.py @@ -13,6 +13,7 @@ from __future__ import annotations import atexit +import inspect import logging import os import tempfile @@ -46,6 +47,12 @@ class OvPhysxManager(PhysicsManager): _stage_path: ClassVar[str | None] = None _warmup_done: ClassVar[bool] = False _tmp_dir: ClassVar[tempfile.TemporaryDirectory | None] = None + # Device the process is locked to once :meth:`_warmup_and_load` constructs the + # ``ovphysx.PhysX`` instance for the first time. ``ovphysx<=0.3.7`` enforces + # a process-global device-mode lock at the C++ layer (see HACK note on + # :meth:`_release_physx`); we mirror it here so a clear Python error is raised + # if a later :class:`~isaaclab.sim.SimulationContext` requests a different device. + _locked_device: ClassVar[str | None] = None # Pending (source, targets, parent_positions) triples registered by # ovphysx_replicate() before the PhysX instance exists. Replayed via # physx.clone() in _warmup_and_load(). @@ -53,6 +60,11 @@ class OvPhysxManager(PhysicsManager): _pending_clones: ClassVar[list[tuple[str, list[str], list[tuple[float, float, float]]]]] = [] _atexit_registered: ClassVar[bool] = False + @classmethod + def get_dt(cls) -> float: + """Get the physics timestep. Alias for get_physics_dt().""" + return cls.get_physics_dt() + @classmethod def register_clone( cls, source: str, targets: list[str], parent_positions: list[tuple[float, float, float]] | None = None @@ -79,13 +91,20 @@ def register_clone( def initialize(cls, sim_context: SimulationContext) -> None: """Initialize the physics manager with simulation context. - This stores the config and device but does not create the ovphysx - instance yet -- the USD stage may not be fully populated at this point. - The actual creation happens lazily in :meth:`reset`. + This stores the config and device but does not load the USD stage yet -- + the stage may not be fully populated at this point. The actual load + happens lazily in :meth:`reset`. + + ``cls._physx`` is intentionally not cleared here: the ovphysx C++ instance + is process-global (see HACK on :meth:`_release_physx`). When a previous + :class:`SimulationContext` has already constructed it, we reuse it rather + than dropping the only Python reference (which would trigger the + destructor race) or re-constructing (which would hit the wheel's + device-mode lock). ``cls._locked_device`` carries the device the cached + instance is bound to. """ super().initialize(sim_context) cls._warmup_done = False - cls._physx = None cls._usd_handle = None cls._stage_path = None cls._pending_clones = [] @@ -138,15 +157,27 @@ def close(cls) -> None: @classmethod def _release_physx(cls) -> None: - """Release the ovphysx instance if it exists. Safe to call multiple times. - - With ovphysx<=0.3.7 and Kit's pxr in the same process, physx.release() - deadlocks due to dual-Carbonite static destructor races. Skip the - native release and let os._exit() (registered via atexit) terminate the - process; GPU resources are reclaimed by the driver. + """Soft-reset the ovphysx runtime stage; keep the C++ instance alive. + + Calls ``physx.reset()`` to clear the loaded scene, but does **not** drop + the Python reference. The cached :class:`ovphysx.PhysX` is reused by the + next :class:`~isaaclab.sim.SimulationContext` via the reuse path in + :meth:`_warmup_and_load`. Safe to call multiple times. + + HACK(ovphysx<=0.3.7): the wheel's bundled libcarb.so and Kit's libcarb.so + coexist in the same process whenever ``import pxr`` runs (Kit USD plugins + on ``LD_LIBRARY_PATH`` pull in Kit's Carbonite). Both register C++ static + destructors that race at process exit -- and crucially, also race when + ``ovphysx.PhysX``'s Python destructor fires mid-process via refcount drop. + So we must never let the only Python reference go to zero while the + process is alive. ``os._exit(0)`` (registered via ``atexit`` in + :meth:`_warmup_and_load`) sidesteps the static-destructor phase entirely + at process exit. Remove this workaround once the wheel ships a + namespace-isolated Carbonite (different soname / hidden visibility). """ if cls._physx is not None: - cls._physx = None + op = cls._physx.reset() + cls._physx.wait_op(op) @classmethod def get_physx_instance(cls) -> Any: @@ -159,7 +190,22 @@ def get_physx_instance(cls) -> Any: @classmethod def _warmup_and_load(cls) -> None: - """Export the USD stage, create the ovphysx instance, and load the scene.""" + """Export the USD stage and load it into the ovphysx runtime. + + On the first call per process, constructs the :class:`ovphysx.PhysX` + instance, registers the ``atexit`` handler, and locks the process to + the resolved device. On subsequent calls, reuses the cached instance + (see HACK on :meth:`_release_physx`) -- exporting the new USD, + re-attaching it via ``add_usd``, replaying pending clones, and (on GPU) + re-running ``warmup_gpu`` so the new stage's bodies are resident. + + Raises: + RuntimeError: if ``SimulationContext`` is not set, or if a device + different from the process-locked one is requested. The wheel + enforces a process-global device-mode lock at the C++ layer; + we surface it here as a clear Python error before the wheel + would raise :exc:`ovphysx.types.PhysXDeviceError`. + """ sim = PhysicsManager._sim if sim is None: raise RuntimeError("OvPhysxManager: SimulationContext is not set.") @@ -173,9 +219,16 @@ def _warmup_and_load(cls) -> None: gpu_index = 0 ovphysx_device = "cpu" + if cls._locked_device is not None and ovphysx_device != cls._locked_device: + raise RuntimeError( + f"OvPhysxManager is locked to device {cls._locked_device!r} for the lifetime of this process; " + f"cannot switch to {ovphysx_device!r}. ovphysx<=0.3.7 binds device mode at the C++ layer on the " + "first ovphysx.PhysX(...) construction and it cannot be changed without restarting the process." + ) + scene_prim = sim.stage.GetPrimAtPath(sim.cfg.physics_prim_path) - if scene_prim.IsValid() and ovphysx_device == "gpu": - cls._configure_physx_scene_prim(scene_prim, PhysicsManager._cfg) + if scene_prim.IsValid(): + cls._configure_physx_scene_prim(scene_prim, PhysicsManager._cfg, ovphysx_device) # Export the current USD stage to a temporary file so ovphysx can load it. cls._tmp_dir = tempfile.TemporaryDirectory(prefix="isaaclab_ovphysx_") @@ -184,6 +237,66 @@ def _warmup_and_load(cls) -> None: cls._stage_path = stage_file logger.info("OvPhysxManager: exported USD stage to %s", stage_file) + if cls._physx is None: + cls._construct_physx(ovphysx_device, gpu_index) + cls._locked_device = ovphysx_device + else: + # Reuse path: the cached PhysX may still hold the prior stage (the + # wheel allows only one loaded USD at a time). ``physx.reset()`` is + # idempotent on an already-cleared stage and required when this is + # a second :meth:`_warmup_and_load` within the same SimulationContext + # (e.g. when a caller manually clears ``_warmup_done`` to force a + # re-warmup). + op = cls._physx.reset() + cls._physx.wait_op(op) + + usd_handle, op_idx = cls._physx.add_usd(stage_file) + cls._physx.wait_op(op_idx) + cls._usd_handle = usd_handle + logger.info("OvPhysxManager: loaded USD into ovphysx (device=%s)", ovphysx_device) + + # Replay pending physics clones registered by ovphysx_replicate(). + # The USD stage contains only env_0's physics; env_1..N are empty + # Xform containers. physx.clone() creates the remaining environments + # in the physics runtime without modifying the USD file. + if cls._pending_clones: + # ovphysx_replicate() only registers pending clones when clone_usd=False, + # meaning the USD contains only env_0 physics and physx.clone() is required + # to populate env_1..N in the physics runtime. Execute unconditionally — + # no USD content heuristic is needed. + for source, targets, parent_positions in cls._pending_clones: + logger.info( + "OvPhysxManager: cloning %s -> %d targets (%s ... %s)", + source, + len(targets), + targets[0], + targets[-1], + ) + if parent_positions: + transforms = [(x, y, z, 0.0, 0.0, 0.0, 1.0) for x, y, z in parent_positions] + else: + transforms = None + op_idx = cls._physx.clone(source, targets, transforms) + cls._physx.wait_op(op_idx) + cls._pending_clones = [] + + # GPU bodies must be re-warmed after every add_usd: the cached PhysX + # instance carries its old buffer layout from the previous stage. + if ovphysx_device == "gpu": + cls._physx.warmup_gpu() + + cls.dispatch_event(PhysicsEvent.MODEL_INIT, payload={}) + cls._warmup_done = True + + @classmethod + def _construct_physx(cls, ovphysx_device: str, gpu_index: int) -> None: + """Bootstrap the ``ovphysx`` wheel and create the :class:`ovphysx.PhysX` instance. + + Runs once per process. Configures worker threads, registers the + process-exit ``os._exit(0)`` handler, and stores the result on + ``cls._physx``. See HACK on :meth:`_release_physx` for why the + instance must outlive every individual :class:`SimulationContext`. + """ # HACK (temporary): hide pxr from sys.modules during ovphysx bootstrap. # IsaacSim's pxr reports version 0.25.5 (pip convention) while ovphysx # expects 25.11 (OpenUSD release convention). Hiding pxr causes @@ -202,7 +315,23 @@ def _warmup_and_load(cls) -> None: import ovphysx - cls._physx = ovphysx.PhysX(device=ovphysx_device, gpu_index=gpu_index) + physx_kwargs = {"device": ovphysx_device} + physx_signature = inspect.signature(ovphysx.PhysX) + physx_parameters = physx_signature.parameters + if "active_cuda_gpus" in physx_parameters: + if ovphysx_device == "gpu": + # ovphysx 0.4 accepts a comma-separated CUDA ordinal string; IsaacLab selects one GPU. + physx_kwargs["active_cuda_gpus"] = str(gpu_index) + physx_kwargs["config"] = ovphysx.PhysXConfig( + carbonite_overrides={ + "/physics/suppressReadback": True, + "/physics/suppressFabricUpdate": True, + } + ) + elif "gpu_index" in physx_parameters: + physx_kwargs["gpu_index"] = gpu_index + + cls._physx = ovphysx.PhysX(**physx_kwargs) # Without worker threads the stepper runs simulate()+fetchResults() # synchronously, blocking the calling thread for the full GPU step time. @@ -253,52 +382,25 @@ def _atexit_release_and_exit(): atexit.register(_atexit_release_and_exit) cls._atexit_registered = True - usd_handle, op_idx = cls._physx.add_usd(stage_file) - cls._physx.wait_op(op_idx) - cls._usd_handle = usd_handle - logger.info("OvPhysxManager: loaded USD into ovphysx (device=%s)", ovphysx_device) - - # Replay pending physics clones registered by ovphysx_replicate(). - # The USD stage contains only env_0's physics; env_1..N are empty - # Xform containers. physx.clone() creates the remaining environments - # in the physics runtime without modifying the USD file. - if cls._pending_clones: - # ovphysx_replicate() only registers pending clones when clone_usd=False, - # meaning the USD contains only env_0 physics and physx.clone() is required - # to populate env_1..N in the physics runtime. Execute unconditionally — - # no USD content heuristic is needed. - for source, targets, parent_positions in cls._pending_clones: - logger.info( - "OvPhysxManager: cloning %s -> %d targets (%s ... %s)", - source, - len(targets), - targets[0], - targets[-1], - ) - if parent_positions: - transforms = [(x, y, z, 0.0, 0.0, 0.0, 1.0) for x, y, z in parent_positions] - else: - transforms = None - op_idx = cls._physx.clone(source, targets, transforms) - cls._physx.wait_op(op_idx) - cls._pending_clones = [] - - if ovphysx_device == "gpu": - cls._physx.warmup_gpu() - - cls.dispatch_event(PhysicsEvent.MODEL_INIT, payload={}) - cls._warmup_done = True - @staticmethod - def _configure_physx_scene_prim(scene_prim, cfg) -> None: - """Apply PhysxSceneAPI schema and GPU dynamics attributes to a scene prim. + def _configure_physx_scene_prim(scene_prim, cfg, device: str) -> None: + """Apply PhysxSceneAPI schema and device-specific scene attributes to the + scene prim. The PhysxSchema USD plugin may not be loaded in standalone ovphysx mode, so we write the apiSchemas list entry and scene attributes directly via raw Sdf metadata manipulation instead of using the high-level USD API. - Without these attributes PhysX defaults to CPU broadphase even when - ovphysx is created with device="gpu". + The schema and scene-query-support attribute are applied regardless of + device. The GPU-specific dynamics/broadphase/capacity attributes are + applied only when ``device == "gpu"`` — without them PhysX defaults to + CPU broadphase even when ovphysx is created with ``device="gpu"``. + + Args: + scene_prim: The /World/PhysicsScene prim to configure. + cfg: The :class:`OvPhysxCfg` carrying GPU buffer-capacity values. + Only consulted when ``device == "gpu"``. + device: Resolved physics device — one of ``"cpu"`` or ``"gpu"``. """ from pxr import Sdf @@ -309,22 +411,24 @@ def _configure_physx_scene_prim(scene_prim, cfg) -> None: items.append("PhysxSceneAPI") schemas.prependedItems = items scene_prim.SetMetadata("apiSchemas", schemas) - scene_prim.CreateAttribute("physxScene:enableGPUDynamics", Sdf.ValueTypeNames.Bool).Set(True) - scene_prim.CreateAttribute("physxScene:broadphaseType", Sdf.ValueTypeNames.String).Set("GPU") - - if cfg is not None: - for attr, val in [ - ("gpuMaxRigidContactCount", cfg.gpu_max_rigid_contact_count), - ("gpuMaxRigidPatchCount", cfg.gpu_max_rigid_patch_count), - ("gpuFoundLostPairsCapacity", cfg.gpu_found_lost_pairs_capacity), - ("gpuFoundLostAggregatePairsCapacity", cfg.gpu_found_lost_aggregate_pairs_capacity), - ("gpuTotalAggregatePairsCapacity", cfg.gpu_total_aggregate_pairs_capacity), - ("gpuCollisionStackSize", cfg.gpu_collision_stack_size), - ]: - scene_prim.CreateAttribute(f"physxScene:{attr}", Sdf.ValueTypeNames.UInt).Set(val) # Propagate scene query support from SimulationCfg so omni.physx creates # the scene with the correct query mode. OvPhysxCfg does not carry this field. sim_cfg = PhysicsManager._sim.cfg if PhysicsManager._sim is not None else None enable_sq = getattr(sim_cfg, "enable_scene_query_support", False) scene_prim.CreateAttribute("physxScene:enableSceneQuerySupport", Sdf.ValueTypeNames.Bool).Set(enable_sq) + + if device == "gpu": + scene_prim.CreateAttribute("physxScene:enableGPUDynamics", Sdf.ValueTypeNames.Bool).Set(True) + scene_prim.CreateAttribute("physxScene:broadphaseType", Sdf.ValueTypeNames.String).Set("GPU") + + if cfg is not None: + for attr, val in [ + ("gpuMaxRigidContactCount", cfg.gpu_max_rigid_contact_count), + ("gpuMaxRigidPatchCount", cfg.gpu_max_rigid_patch_count), + ("gpuFoundLostPairsCapacity", cfg.gpu_found_lost_pairs_capacity), + ("gpuFoundLostAggregatePairsCapacity", cfg.gpu_found_lost_aggregate_pairs_capacity), + ("gpuTotalAggregatePairsCapacity", cfg.gpu_total_aggregate_pairs_capacity), + ("gpuCollisionStackSize", cfg.gpu_collision_stack_size), + ]: + scene_prim.CreateAttribute(f"physxScene:{attr}", Sdf.ValueTypeNames.UInt).Set(val) diff --git a/source/isaaclab_ovphysx/isaaclab_ovphysx/tensor_types.py b/source/isaaclab_ovphysx/isaaclab_ovphysx/tensor_types.py index 44a5cadeeb0a..41afe07cf09c 100644 --- a/source/isaaclab_ovphysx/isaaclab_ovphysx/tensor_types.py +++ b/source/isaaclab_ovphysx/isaaclab_ovphysx/tensor_types.py @@ -191,6 +191,65 @@ Shape is ``[N, L, 9]``, dtype ``float32``. """ +""" +Rigid-body TensorTypes + +Shapes assume N = number of rigid actor instances matched by the binding +pattern. Components and units are stated per alias below. +""" + +RIGID_BODY_POSE = _TT.RIGID_BODY_POSE +"""Rigid actor root transform — read/write, GPU. Shape ``(N, 7)``, +components ``(px, py, pz, qx, qy, qz, qw)`` [m, dimensionless].""" + +RIGID_BODY_VELOCITY = _TT.RIGID_BODY_VELOCITY +"""Rigid actor root spatial velocity — read/write, GPU. Shape ``(N, 6)``, +components ``(vx, vy, vz, wx, wy, wz)`` [m/s, rad/s].""" + +RIGID_BODY_WRENCH = _TT.RIGID_BODY_WRENCH +"""External wrench applied at a world-frame point — write-only, GPU. +Shape ``(N, 9)``, components ``(fx, fy, fz, tx, ty, tz, px, py, pz)`` +[N, N·m, m]. Cleared after each sim step (instantaneous semantics).""" + +RIGID_BODY_MASS = _TT.RIGID_BODY_MASS +"""Rigid actor mass — read/write, CPU. Shape ``(N,)`` [kg].""" + +RIGID_BODY_COM_POSE = _TT.RIGID_BODY_COM_POSE +"""Center-of-mass pose in actor-link frame — read/write, CPU. Shape +``(N, 7)``, components ``(px, py, pz, qx, qy, qz, qw)`` [m, dimensionless].""" + +RIGID_BODY_INERTIA = _TT.RIGID_BODY_INERTIA +"""Rigid actor inertia tensor in COM frame — read/write, CPU. Shape +``(N, 9)``, row-major flatten of the 3×3 inertia matrix +``(Ixx, Ixy, Ixz, Iyx, Iyy, Iyz, Izx, Izy, Izz)`` [kg·m²].""" + +# These three aliases are pending an upcoming ovphysx wheel update. +# When the wheel ships them, the corresponding ``hasattr`` checks below +# in IsaacLab consumers will start returning True and the bindings will +# become usable; until then, ``isaaclab_ovphysx.tensor_types`` simply +# does not expose the alias. +try: + RIGID_BODY_ACCELERATION = _TT.RIGID_BODY_ACCELERATION + """Rigid actor spatial acceleration — read-only, GPU. Shape ``(N, 6)``, + components ``(ax, ay, az, αx, αy, αz)`` [m/s², rad/s²].""" +except AttributeError: + pass + +try: + RIGID_BODY_INV_MASS = _TT.RIGID_BODY_INV_MASS + """Rigid actor inverse mass — read-only, CPU. Shape ``(N,)`` [1/kg]. + Zero indicates an immovable actor.""" +except AttributeError: + pass + +try: + RIGID_BODY_INV_INERTIA = _TT.RIGID_BODY_INV_INERTIA + """Rigid actor inverse inertia tensor in COM frame — read-only, CPU. + Shape ``(N, 9)``, row-major flatten of the 3×3 matrix [1/(kg·m²)]. + Zero rows indicate locked rotational DOFs.""" +except AttributeError: + pass + """ Dynamics tensors (GPU) """ @@ -306,29 +365,36 @@ # fmt: on # DOF/body property tensor types are CPU-resident even in GPU simulations. # Write helpers check this set to route data through CPU, not self._device. -_CPU_ONLY_TYPES: frozenset[TensorType] = frozenset( - { - DOF_STIFFNESS, - DOF_DAMPING, - DOF_LIMIT, - DOF_MAX_VELOCITY, - DOF_MAX_FORCE, - DOF_ARMATURE, - DOF_FRICTION_PROPERTIES, - BODY_MASS, - BODY_COM_POSE, - BODY_INERTIA, - BODY_INV_MASS, - BODY_INV_INERTIA, - FIXED_TENDON_STIFFNESS, - FIXED_TENDON_DAMPING, - FIXED_TENDON_LIMIT_STIFFNESS, - FIXED_TENDON_LIMIT, - FIXED_TENDON_REST_LENGTH, - FIXED_TENDON_OFFSET, - SPATIAL_TENDON_STIFFNESS, - SPATIAL_TENDON_DAMPING, - SPATIAL_TENDON_LIMIT_STIFFNESS, - SPATIAL_TENDON_OFFSET, - } +_CPU_ONLY_TYPES_CANDIDATES: tuple = ( + DOF_STIFFNESS, + DOF_DAMPING, + DOF_LIMIT, + DOF_MAX_VELOCITY, + DOF_MAX_FORCE, + DOF_ARMATURE, + DOF_FRICTION_PROPERTIES, + BODY_MASS, + BODY_COM_POSE, + BODY_INERTIA, + BODY_INV_MASS, + BODY_INV_INERTIA, + FIXED_TENDON_STIFFNESS, + FIXED_TENDON_DAMPING, + FIXED_TENDON_LIMIT_STIFFNESS, + FIXED_TENDON_LIMIT, + FIXED_TENDON_REST_LENGTH, + FIXED_TENDON_OFFSET, + SPATIAL_TENDON_STIFFNESS, + SPATIAL_TENDON_DAMPING, + SPATIAL_TENDON_LIMIT_STIFFNESS, + SPATIAL_TENDON_OFFSET, + # Rigid-body CPU-only entries (always available) + RIGID_BODY_MASS, + RIGID_BODY_COM_POSE, + RIGID_BODY_INERTIA, +) +# Optional rigid-body CPU entries: only included when the wheel exposes them. +_RIGID_BODY_OPTIONAL_CPU: tuple = tuple( + globals()[name] for name in ("RIGID_BODY_INV_MASS", "RIGID_BODY_INV_INERTIA") if name in globals() ) +_CPU_ONLY_TYPES: frozenset[TensorType] = frozenset(_CPU_ONLY_TYPES_CANDIDATES + _RIGID_BODY_OPTIONAL_CPU) diff --git a/source/isaaclab_ovphysx/isaaclab_ovphysx/test/mock_interfaces/views/mock_ovphysx_bindings.py b/source/isaaclab_ovphysx/isaaclab_ovphysx/test/mock_interfaces/views/mock_ovphysx_bindings.py index 29472ce74fd0..51e96d9bb427 100644 --- a/source/isaaclab_ovphysx/isaaclab_ovphysx/test/mock_interfaces/views/mock_ovphysx_bindings.py +++ b/source/isaaclab_ovphysx/isaaclab_ovphysx/test/mock_interfaces/views/mock_ovphysx_bindings.py @@ -7,6 +7,8 @@ from __future__ import annotations +from typing import Literal + import numpy as np from isaaclab_ovphysx import tensor_types as TT @@ -161,6 +163,10 @@ class MockOvPhysxBindingSet: for a given articulation configuration. Mirrors the tensor types that ``Articulation._initialize_impl`` creates. + + With ``asset_kind='rigid_object'`` it produces the smaller set + consumed by ``RigidObject._initialize_impl``: ``RIGID_BODY_*`` only, + ``num_joints`` must be 0, ``num_bodies`` must be 1, no tendons. """ def __init__( @@ -173,7 +179,54 @@ def __init__( body_names: list[str] | None = None, num_fixed_tendons: int = 0, num_spatial_tendons: int = 0, + *, + asset_kind: Literal["articulation", "rigid_object"] = "articulation", ): + if asset_kind == "rigid_object": + if num_joints != 0 or num_bodies != 1 or num_fixed_tendons != 0 or num_spatial_tendons != 0: + raise ValueError( + "asset_kind='rigid_object' requires num_joints=0, num_bodies=1, " + "num_fixed_tendons=0, num_spatial_tendons=0; got " + f"num_joints={num_joints}, num_bodies={num_bodies}, " + f"num_fixed_tendons={num_fixed_tendons}, " + f"num_spatial_tendons={num_spatial_tendons}" + ) + N = num_instances + if body_names is None: + body_names = ["base_link"] + common = dict( + count=N, + dof_count=0, + body_count=1, + joint_count=0, + is_fixed_base=is_fixed_base, + dof_names=[], + body_names=body_names, + joint_names=[], + fixed_tendon_count=0, + spatial_tendon_count=0, + ) + self.bindings: dict[int, MockTensorBinding] = { + TT.RIGID_BODY_POSE: MockTensorBinding(TT.RIGID_BODY_POSE, (N, 7), **common), + TT.RIGID_BODY_VELOCITY: MockTensorBinding(TT.RIGID_BODY_VELOCITY, (N, 6), **common), + TT.RIGID_BODY_WRENCH: MockTensorBinding(TT.RIGID_BODY_WRENCH, (N, 9), write_only=True, **common), + TT.RIGID_BODY_MASS: MockTensorBinding(TT.RIGID_BODY_MASS, (N,), **common), + TT.RIGID_BODY_COM_POSE: MockTensorBinding(TT.RIGID_BODY_COM_POSE, (N, 7), **common), + TT.RIGID_BODY_INERTIA: MockTensorBinding(TT.RIGID_BODY_INERTIA, (N, 9), **common), + } + # Optional bindings: only present when the wheel exposes the alias. + if hasattr(TT, "RIGID_BODY_ACCELERATION"): + self.bindings[TT.RIGID_BODY_ACCELERATION] = MockTensorBinding( + TT.RIGID_BODY_ACCELERATION, (N, 6), **common + ) + if hasattr(TT, "RIGID_BODY_INV_MASS"): + self.bindings[TT.RIGID_BODY_INV_MASS] = MockTensorBinding(TT.RIGID_BODY_INV_MASS, (N,), **common) + if hasattr(TT, "RIGID_BODY_INV_INERTIA"): + self.bindings[TT.RIGID_BODY_INV_INERTIA] = MockTensorBinding( + TT.RIGID_BODY_INV_INERTIA, (N, 9), **common + ) + return + N = num_instances D = num_joints L = num_bodies @@ -259,17 +312,25 @@ def set_random_data(self) -> None: for b in self.bindings.values(): if not b._write_only: b.set_random_data() - lim = self.bindings[TT.DOF_LIMIT] - lim._data[..., 0] = -3.14 - lim._data[..., 1] = 3.14 - for tt in (TT.ROOT_POSE, TT.LINK_POSE, TT.BODY_COM_POSE): + if TT.DOF_LIMIT in self.bindings: + lim = self.bindings[TT.DOF_LIMIT] + lim._data[..., 0] = -3.14 + lim._data[..., 1] = 3.14 + pose_keys = [ + k + for k in (TT.ROOT_POSE, TT.LINK_POSE, TT.BODY_COM_POSE, TT.RIGID_BODY_POSE, TT.RIGID_BODY_COM_POSE) + if k in self.bindings + ] + for tt in pose_keys: b = self.bindings[tt] b._data[..., 3:6] = 0.0 b._data[..., 6] = 1.0 - self.bindings[TT.BODY_MASS]._data = np.abs(self.bindings[TT.BODY_MASS]._data) + 0.1 - self.bindings[TT.DOF_MAX_VELOCITY]._data = np.abs(self.bindings[TT.DOF_MAX_VELOCITY]._data) + 1.0 - self.bindings[TT.DOF_MAX_FORCE]._data = np.abs(self.bindings[TT.DOF_MAX_FORCE]._data) + 1.0 - # Set sensible defaults for fixed tendon limits + for mass_key in (TT.BODY_MASS, TT.RIGID_BODY_MASS): + if mass_key in self.bindings: + self.bindings[mass_key]._data = np.abs(self.bindings[mass_key]._data) + 0.1 + for max_key in (TT.DOF_MAX_VELOCITY, TT.DOF_MAX_FORCE): + if max_key in self.bindings: + self.bindings[max_key]._data = np.abs(self.bindings[max_key]._data) + 1.0 if TT.FIXED_TENDON_LIMIT in self.bindings: tlim = self.bindings[TT.FIXED_TENDON_LIMIT] tlim._data[..., 0] = -1.0 diff --git a/source/isaaclab_ovphysx/test/assets/test_articulation_data.py b/source/isaaclab_ovphysx/test/assets/test_articulation_data.py index 16bb99a4d6c4..390e5defa0f2 100644 --- a/source/isaaclab_ovphysx/test/assets/test_articulation_data.py +++ b/source/isaaclab_ovphysx/test/assets/test_articulation_data.py @@ -40,3 +40,19 @@ def test_joint_acc_uses_inverse_dt(self): atol=1e-6, err_msg="Joint acceleration should be computed as delta_velocity / dt.", ) + + def test_cpu_only_binding_read_stages_to_gpu_view(self): + """CPU-only bindings should be staged before copying into GPU-backed data buffers.""" + if not wp.is_cuda_available(): + pytest.skip("CUDA is required to test CPU-to-GPU staging.") + + mock_bindings = MockOvPhysxBindingSet(num_instances=1, num_joints=2, num_bodies=1) + data = ArticulationData(mock_bindings.bindings, device="cuda") + data._create_buffers() + + expected = np.array([[[1.0, 2.0, 3.0, 0.0, 0.0, 0.0, 1.0]]], dtype=np.float32) + mock_bindings.bindings[TT.BODY_COM_POSE]._data[...] = expected + + data._read_transform_binding(TT.BODY_COM_POSE, data._body_com_pose_b) + + np.testing.assert_allclose(data._body_com_pose_b.data.numpy(), expected, atol=1e-6) diff --git a/source/isaaclab_ovphysx/test/assets/test_rigid_object.py b/source/isaaclab_ovphysx/test/assets/test_rigid_object.py new file mode 100644 index 000000000000..407cf4b41e22 --- /dev/null +++ b/source/isaaclab_ovphysx/test/assets/test_rigid_object.py @@ -0,0 +1,1134 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# ignore private usage of variables warning +# pyright: reportPrivateUsage=none + + +"""Real-backend tests for the OVPhysX RigidObject. + +Run via ``./scripts/run_ovphysx.sh -m pytest`` (kitless, no ``AppLauncher``). + +``ovphysx<=0.3.7`` binds device mode (CPU vs GPU) at the C++ layer on the +first ``ovphysx.PhysX(device=...)`` construction and cannot swap it without a +process restart. Full coverage therefore requires two separate pytest +invocations -- once with ``-k 'cpu'`` and once with ``-k 'cuda:0'``. The +``_ovphysx_skip_other_device`` autouse fixture below preempts the manager's +:exc:`RuntimeError` by ``pytest.skip``-ing on the unlocked device so +single-device runs finish cleanly. +""" + +from __future__ import annotations + +import logging +import sys +from typing import Literal +from unittest.mock import MagicMock + +import pytest +import torch +import warp as wp +from flaky import flaky + +# The CI isaaclab_ov* pattern unintentionally collects isaaclab_ovphysx tests, +# but the ovphysx wheel is not installed in that environment. Skip gracefully +# so the isaaclab_ov CI pipeline is not blocked by an unrelated dependency. +pytest.importorskip("ovphysx.types", reason="ovphysx wheel not installed") + +from isaaclab_ovphysx.assets import RigidObject # noqa: E402 +from isaaclab_ovphysx.physics import OvPhysxCfg, OvPhysxManager # noqa: E402 + +import isaaclab.sim as sim_utils # noqa: E402 +from isaaclab.assets import RigidObjectCfg # noqa: E402 +from isaaclab.sim import SimulationCfg, build_simulation_context # noqa: E402 +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR, ISAACLAB_NUCLEUS_DIR # noqa: E402 +from isaaclab.utils.math import ( # noqa: E402 + combine_frame_transforms, + default_orientation, + quat_apply_inverse, + quat_inv, + quat_mul, + quat_rotate, + random_orientation, +) + +wp.init() + +_logger = logging.getLogger(__name__) + + +_LOCKED_DEVICE: list[str | None] = [None] +"""Device the session pins to on the first parametrized test that runs.""" + + +@pytest.fixture(autouse=True) +def _ovphysx_skip_other_device(request): + """Skip parametrized tests on the device the session is not pinned to. + + See the module docstring for the wheel's process-global device-mode lock. + """ + callspec = getattr(request.node, "callspec", None) + device = callspec.params.get("device") if callspec is not None else None + if device is None: + # Test does not parametrize on device (e.g. test_warmup_attach_stage_not_called_for_cpu). + return + locked = _LOCKED_DEVICE[0] + if locked is None: + _LOCKED_DEVICE[0] = device + return + if device != locked: + pytest.skip( + f"ovphysx process-global device lock is held by '{locked}'; cannot run '{device}' " + "tests in the same session. Run pytest twice (once per device) for full coverage." + ) + + +def _ovphysx_sim_context(device: str, **kwargs): + """Wrapper around :func:`build_simulation_context` that injects OVPhysX cfg. + + PhysX tests pass ``device=device`` directly and let + :func:`build_simulation_context` build a default :class:`SimulationCfg`. + OVPhysX needs ``physics=OvPhysxCfg()`` set on the cfg so the manager + dispatches to OVPhysX rather than PhysX, so we build the cfg here and + pass it through. ``gravity_enabled`` is consumed locally (it is ignored + by ``build_simulation_context`` once a ``sim_cfg`` is provided). + ``add_ground_plane``, ``auto_add_lighting``, and other kwargs continue + to flow through ``build_simulation_context`` as before. + """ + dt = kwargs.pop("dt", 1.0 / 60.0) + gravity_enabled = kwargs.pop("gravity_enabled", True) + gravity = (0.0, 0.0, -9.81) if gravity_enabled else (0.0, 0.0, 0.0) + sim_cfg = SimulationCfg(physics=OvPhysxCfg(), device=device, dt=dt, gravity=gravity) + return build_simulation_context(device=device, sim_cfg=sim_cfg, **kwargs) + + +def generate_cubes_scene( + num_cubes: int = 1, + height=1.0, + api: Literal["none", "rigid_body", "articulation_root"] = "rigid_body", + kinematic_enabled: bool = False, + device: str = "cuda:0", +) -> tuple[RigidObject, torch.Tensor]: + """Generate a scene with the provided number of cubes. + + Args: + num_cubes: Number of cubes to generate. + height: Height of the cubes. + api: The type of API that the cubes should have. + kinematic_enabled: Whether the cubes are kinematic. + device: Device to use for the simulation. + + Returns: + A tuple containing the rigid object representing the cubes and the origins of the cubes. + + """ + origins = torch.tensor([(i * 1.0, 0, height) for i in range(num_cubes)]).to(device) + # Create Top-level Xforms, one for each cube + for i, origin in enumerate(origins): + sim_utils.create_prim(f"/World/Table_{i}", "Xform", translation=origin) + + # Resolve spawn configuration + if api == "none": + # since no rigid body properties defined, this is just a static collider + spawn_cfg = sim_utils.CuboidCfg( + size=(0.1, 0.1, 0.1), + collision_props=sim_utils.CollisionPropertiesCfg(), + ) + elif api == "rigid_body": + spawn_cfg = sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Blocks/DexCube/dex_cube_instanceable.usd", + rigid_props=sim_utils.RigidBodyPropertiesCfg(kinematic_enabled=kinematic_enabled), + ) + elif api == "articulation_root": + spawn_cfg = sim_utils.UsdFileCfg( + usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Tests/RigidObject/Cube/dex_cube_instanceable_with_articulation_root.usd", + rigid_props=sim_utils.RigidBodyPropertiesCfg(kinematic_enabled=kinematic_enabled), + ) + else: + raise ValueError(f"Unknown api: {api}") + + # Create rigid object. OVPhysX matches prim paths via fnmatch globs (not regex), + # so use ``Table_*`` rather than the PhysX ``Table_.*`` form. + cube_object_cfg = RigidObjectCfg( + prim_path="/World/Table_*/Object", + spawn=spawn_cfg, + init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, 0.0, height)), + ) + cube_object = RigidObject(cfg=cube_object_cfg) + + return cube_object, origins + + +# --------------------------------------------------------------------------- +# Material-property gap (xfail reason shared by 5 tests below) +# --------------------------------------------------------------------------- + +_MATERIAL_GAP_REASON = ( + "Requires RIGID_BODY_MATERIAL TensorType (or a view-helper) on the ovphysx " + "wheel side. RigidObject.root_view is a per-tensor-type bindings dict on " + "OVPhysX, so root_view.get_material_properties() / set_material_properties() " + "are not available. See " + "docs/superpowers/specs/2026-04-28-ovphysx-wheel-gaps-for-marco.md." +) + + +@pytest.mark.parametrize("num_cubes", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.isaacsim_ci +def test_initialization(num_cubes, device): + """Test initialization for prim with rigid body API at the provided prim path.""" + with _ovphysx_sim_context(device=device, auto_add_lighting=True) as sim: + # Generate cubes scene + cube_object, _ = generate_cubes_scene(num_cubes=num_cubes, device=device) + + # Check that the framework doesn't hold excessive strong references. + assert sys.getrefcount(cube_object) < 10 + + # Play sim + sim.reset() + + # Check if object is initialized + assert cube_object.is_initialized + assert len(cube_object.body_names) == 1 + + # Check buffers that exists and have correct shapes + assert cube_object.data.root_pos_w.torch.shape == (num_cubes, 3) + assert cube_object.data.root_quat_w.torch.shape == (num_cubes, 4) + assert cube_object.data.body_mass.torch.shape == (num_cubes, 1) + assert cube_object.data.body_inertia.torch.shape == (num_cubes, 1, 9) + + # Simulate physics + for _ in range(2): + # perform rendering + sim.step() + # update object + cube_object.update(sim.cfg.dt) + + +@pytest.mark.parametrize("num_cubes", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.isaacsim_ci +def test_initialization_with_kinematic_enabled(num_cubes, device): + """Test that initialization for prim with kinematic flag enabled.""" + with _ovphysx_sim_context(device=device, auto_add_lighting=True) as sim: + # Generate cubes scene + cube_object, origins = generate_cubes_scene(num_cubes=num_cubes, kinematic_enabled=True, device=device) + + # Check that the framework doesn't hold excessive strong references. + assert sys.getrefcount(cube_object) < 10 + + # Play sim + sim.reset() + + # Check if object is initialized + assert cube_object.is_initialized + assert len(cube_object.body_names) == 1 + + # Check buffers that exists and have correct shapes + assert cube_object.data.root_pos_w.torch.shape == (num_cubes, 3) + assert cube_object.data.root_quat_w.torch.shape == (num_cubes, 4) + + # Simulate physics + for _ in range(2): + # perform rendering + sim.step() + # update object + cube_object.update(sim.cfg.dt) + # check that the object is kinematic + default_root_pose = cube_object.data.default_root_pose.torch.clone() + default_root_vel = cube_object.data.default_root_vel.torch.clone() + default_root_pose[:, :3] += origins + torch.testing.assert_close(cube_object.data.root_link_pose_w.torch, default_root_pose) + torch.testing.assert_close(cube_object.data.root_com_vel_w.torch, default_root_vel) + + +@pytest.mark.parametrize("num_cubes", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.isaacsim_ci +def test_initialization_with_no_rigid_body(num_cubes, device): + """Test that initialization fails when no rigid body is found at the provided prim path.""" + with _ovphysx_sim_context(device=device, auto_add_lighting=True) as sim: + # Generate cubes scene + cube_object, _ = generate_cubes_scene(num_cubes=num_cubes, api="none", device=device) + + # Check that the framework doesn't hold excessive strong references. + assert sys.getrefcount(cube_object) < 10 + + # Play sim + with pytest.raises(RuntimeError): + sim.reset() + + +@pytest.mark.parametrize("num_cubes", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.isaacsim_ci +def test_initialization_with_articulation_root(num_cubes, device): + """Test that initialization fails when an articulation root is found at the provided prim path.""" + with _ovphysx_sim_context(device=device, auto_add_lighting=True) as sim: + # Generate cubes scene + cube_object, _ = generate_cubes_scene(num_cubes=num_cubes, api="articulation_root", device=device) + + # Check that the framework doesn't hold excessive strong references. + assert sys.getrefcount(cube_object) < 10 + + # Play sim + with pytest.raises(RuntimeError): + sim.reset() + + +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.isaacsim_ci +def test_external_force_buffer(device): + """Test if external force buffer correctly updates in the force value is zero case. + + In this test, we apply a non-zero force, then a zero force, then finally a non-zero force + to an object. We check if the force buffer is properly updated at each step. + """ + + # Generate cubes scene + with _ovphysx_sim_context(device=device, add_ground_plane=True, auto_add_lighting=True) as sim: + cube_object, origins = generate_cubes_scene(num_cubes=1, device=device) + + # play the simulator + sim.reset() + + # find bodies to apply the force + body_ids, body_names = cube_object.find_bodies(".*") + + # reset object + cube_object.reset() + + # perform simulation + for step in range(5): + # initiate force tensor + external_wrench_b = torch.zeros(cube_object.num_instances, len(body_ids), 6, device=sim.device) + + if step == 0 or step == 3: + # set a non-zero force + force = 1 + else: + # set a zero force + force = 0 + + # set force value + external_wrench_b[:, :, 0] = force + external_wrench_b[:, :, 3] = force + + # apply force + cube_object.permanent_wrench_composer.set_forces_and_torques_index( + forces=external_wrench_b[..., :3], + torques=external_wrench_b[..., 3:], + body_ids=body_ids, + ) + + # check if the cube's force and torque buffers are correctly updated + for i in range(cube_object.num_instances): + assert cube_object._permanent_wrench_composer.composed_force.torch[i, 0, 0].item() == force + assert cube_object._permanent_wrench_composer.composed_torque.torch[i, 0, 0].item() == force + + # Check if the instantaneous wrench is correctly added to the permanent wrench + cube_object.permanent_wrench_composer.add_forces_and_torques_index( + forces=external_wrench_b[..., :3], + torques=external_wrench_b[..., 3:], + body_ids=body_ids, + ) + + # apply action to the object + cube_object.write_data_to_sim() + + # perform step + sim.step() + + # update buffers + cube_object.update(sim.cfg.dt) + + +@pytest.mark.parametrize("num_cubes", [2, 4]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.isaacsim_ci +def test_external_force_on_single_body(num_cubes, device): + """Test application of external force on the base of the object. + + In this test, we apply a force equal to the weight of an object on the base of + one of the objects. We check that the object does not move. For the other object, + we do not apply any force and check that it falls down. + + We validate that this works when we apply the force in the global frame and in the local frame. + """ + # Generate cubes scene + with _ovphysx_sim_context(device=device, add_ground_plane=True, auto_add_lighting=True) as sim: + cube_object, origins = generate_cubes_scene(num_cubes=num_cubes, device=device) + + # Play the simulator + sim.reset() + + # Find bodies to apply the force + body_ids, body_names = cube_object.find_bodies(".*") + + # Sample a force equal to the weight of the object. PhysX reads the mass + # from ``root_view.get_masses()``; OVPhysX exposes the same value via + # ``cube_object.data.body_mass`` (shape ``(N, 1)``). + external_wrench_b = torch.zeros(cube_object.num_instances, len(body_ids), 6, device=sim.device) + # Every 2nd cube should have a force applied to it + external_wrench_b[0::2, :, 2] = 9.81 * cube_object.data.body_mass.torch[0] + + # Now we are ready! + for i in range(5): + # reset root state + root_pose = cube_object.data.default_root_pose.torch.clone() + root_vel = cube_object.data.default_root_vel.torch.clone() + + # need to shift the position of the cubes otherwise they will be on top of each other + root_pose[:, :3] = origins + cube_object.write_root_pose_to_sim_index(root_pose=root_pose) + cube_object.write_root_velocity_to_sim_index(root_velocity=root_vel) + + # reset object + cube_object.reset() + + is_global = False + if i % 2 == 0: + is_global = True + positions = cube_object.data.body_com_pos_w.torch[:, body_ids, :3] + else: + positions = None + + # apply force + cube_object.permanent_wrench_composer.set_forces_and_torques_index( + forces=external_wrench_b[..., :3], + torques=external_wrench_b[..., 3:], + positions=positions, + body_ids=body_ids, + is_global=is_global, + ) + # perform simulation + for _ in range(5): + # apply action to the object + cube_object.write_data_to_sim() + + # perform step + sim.step() + + # update buffers + cube_object.update(sim.cfg.dt) + + # First object should still be at the same Z position (1.0) + torch.testing.assert_close( + cube_object.data.root_pos_w.torch[0::2, 2], torch.ones(num_cubes // 2, device=sim.device) + ) + # Second object should have fallen, so it's Z height should be less than initial height of 1.0 + assert torch.all(cube_object.data.root_pos_w.torch[1::2, 2] < 1.0) + + +@pytest.mark.parametrize("num_cubes", [2, 4]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +def test_external_force_on_single_body_at_position(num_cubes, device): + """Test application of external force on the base of the object at a specific position. + + In this test, we apply a force equal to the weight of an object on the base of + one of the objects at 1m in the Y direction, we check that the object rotates around it's X axis. + For the other object, we do not apply any force and check that it falls down. + + We validate that this works when we apply the force in the global frame and in the local frame. + """ + # Generate cubes scene + with _ovphysx_sim_context(device=device, add_ground_plane=True, auto_add_lighting=True) as sim: + cube_object, origins = generate_cubes_scene(num_cubes=num_cubes, device=device) + + # Play the simulator + sim.reset() + + # Find bodies to apply the force + body_ids, body_names = cube_object.find_bodies(".*") + + # Sample a force equal to the weight of the object + external_wrench_b = torch.zeros(cube_object.num_instances, len(body_ids), 6, device=sim.device) + external_wrench_positions_b = torch.zeros(cube_object.num_instances, len(body_ids), 3, device=sim.device) + # Every 2nd cube should have a force applied to it + external_wrench_b[0::2, :, 2] = 500.0 + external_wrench_positions_b[0::2, :, 1] = 1.0 + + # Desired force and torque + desired_force = torch.zeros(cube_object.num_instances, len(body_ids), 3, device=sim.device) + desired_force[0::2, :, 2] = 1000.0 + desired_torque = torch.zeros(cube_object.num_instances, len(body_ids), 3, device=sim.device) + desired_torque[0::2, :, 0] = 1000.0 + # Now we are ready! + for i in range(5): + # reset root state + root_pose = cube_object.data.default_root_pose.torch.clone() + root_vel = cube_object.data.default_root_vel.torch.clone() + + # need to shift the position of the cubes otherwise they will be on top of each other + root_pose[:, :3] = origins + cube_object.write_root_pose_to_sim_index(root_pose=root_pose) + cube_object.write_root_velocity_to_sim_index(root_velocity=root_vel) + + # reset object + cube_object.reset() + + is_global = False + if i % 2 == 0: + is_global = True + body_com_pos_w = cube_object.data.body_com_pos_w.torch[:, body_ids, :3] + external_wrench_positions_b[..., 0] = 0.0 + external_wrench_positions_b[..., 1] = 1.0 + external_wrench_positions_b[..., 2] = 0.0 + external_wrench_positions_b += body_com_pos_w + else: + external_wrench_positions_b[..., 0] = 0.0 + external_wrench_positions_b[..., 1] = 1.0 + external_wrench_positions_b[..., 2] = 0.0 + + # apply force + cube_object.permanent_wrench_composer.set_forces_and_torques_index( + forces=external_wrench_b[..., :3], + torques=external_wrench_b[..., 3:], + positions=external_wrench_positions_b, + body_ids=body_ids, + is_global=is_global, + ) + cube_object.permanent_wrench_composer.add_forces_and_torques_index( + forces=external_wrench_b[..., :3], + torques=external_wrench_b[..., 3:], + positions=external_wrench_positions_b, + body_ids=body_ids, + is_global=is_global, + ) + torch.testing.assert_close( + cube_object._permanent_wrench_composer.composed_force.torch[:, 0, :], + desired_force[:, 0, :], + rtol=1e-6, + atol=1e-7, + ) + torch.testing.assert_close( + cube_object._permanent_wrench_composer.composed_torque.torch[:, 0, :], + desired_torque[:, 0, :], + rtol=1e-6, + atol=1e-7, + ) + # perform simulation + for _ in range(5): + # apply action to the object + cube_object.write_data_to_sim() + + # perform step + sim.step() + + # update buffers + cube_object.update(sim.cfg.dt) + + # The first object should be rotating around it's X axis + assert torch.all(torch.abs(cube_object.data.root_ang_vel_b.torch[0::2, 0]) > 0.1) + # Second object should have fallen, so it's Z height should be less than initial height of 1.0 + assert torch.all(cube_object.data.root_pos_w.torch[1::2, 2] < 1.0) + + +@pytest.mark.parametrize("num_cubes", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.isaacsim_ci +def test_set_rigid_object_state(num_cubes, device): + """Test setting the state of the rigid object. + + In this test, we set the state of the rigid object to a random state and check + that the object is in that state after simulation. We set gravity to zero as + we don't want any external forces acting on the object to ensure state remains static. + """ + # Turn off gravity for this test as we don't want any external forces acting on the object + # to ensure state remains static + with _ovphysx_sim_context(device=device, gravity_enabled=False, auto_add_lighting=True) as sim: + # Generate cubes scene + cube_object, _ = generate_cubes_scene(num_cubes=num_cubes, device=device) + + # Play the simulator + sim.reset() + + state_types = ["root_pos_w", "root_quat_w", "root_lin_vel_w", "root_ang_vel_w"] + + # Set each state type individually as they are dependent on each other + for state_type_to_randomize in state_types: + state_dict = { + "root_pos_w": torch.zeros_like(cube_object.data.root_pos_w.torch, device=sim.device), + "root_quat_w": default_orientation(num=num_cubes, device=sim.device), + "root_lin_vel_w": torch.zeros_like(cube_object.data.root_lin_vel_w.torch, device=sim.device), + "root_ang_vel_w": torch.zeros_like(cube_object.data.root_ang_vel_w.torch, device=sim.device), + } + + # Now we are ready! + for _ in range(5): + # reset object + cube_object.reset() + + # Set random state + if state_type_to_randomize == "root_quat_w": + state_dict[state_type_to_randomize] = random_orientation(num=num_cubes, device=sim.device) + else: + state_dict[state_type_to_randomize] = torch.randn(num_cubes, 3, device=sim.device) + + # perform simulation + for _ in range(5): + root_pose = torch.cat( + [state_dict["root_pos_w"], state_dict["root_quat_w"]], + dim=-1, + ) + root_vel = torch.cat( + [state_dict["root_lin_vel_w"], state_dict["root_ang_vel_w"]], + dim=-1, + ) + # reset root state + cube_object.write_root_pose_to_sim_index(root_pose=root_pose) + cube_object.write_root_velocity_to_sim_index(root_velocity=root_vel) + + sim.step() + + # assert that set root quantities are equal to the ones set in the state_dict + for key, expected_value in state_dict.items(): + value = getattr(cube_object.data, key).torch + torch.testing.assert_close(value, expected_value, rtol=1e-3, atol=1e-3) + + cube_object.update(sim.cfg.dt) + + +@pytest.mark.parametrize("num_cubes", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.isaacsim_ci +def test_reset_rigid_object(num_cubes, device): + """Test resetting the state of the rigid object.""" + with _ovphysx_sim_context(device=device, gravity_enabled=True, auto_add_lighting=True) as sim: + # Generate cubes scene + cube_object, _ = generate_cubes_scene(num_cubes=num_cubes, device=device) + + # Play the simulator + sim.reset() + + for i in range(5): + # perform rendering + sim.step() + + # update object + cube_object.update(sim.cfg.dt) + + # Move the object to a random position + root_pose = cube_object.data.default_root_pose.torch.clone() + root_pose[:, :3] = torch.randn(num_cubes, 3, device=sim.device) + + # Random orientation + root_pose[:, 3:7] = random_orientation(num=num_cubes, device=sim.device) + cube_object.write_root_pose_to_sim_index(root_pose=root_pose) + root_vel = cube_object.data.default_root_vel.torch.clone() + cube_object.write_root_velocity_to_sim_index(root_velocity=root_vel) + + if i % 2 == 0: + # reset object + cube_object.reset() + + # Reset should zero external forces and torques + assert not cube_object._instantaneous_wrench_composer.active + assert not cube_object._permanent_wrench_composer.active + assert torch.count_nonzero(cube_object._instantaneous_wrench_composer.composed_force.torch) == 0 + assert torch.count_nonzero(cube_object._instantaneous_wrench_composer.composed_torque.torch) == 0 + assert torch.count_nonzero(cube_object._permanent_wrench_composer.composed_force.torch) == 0 + assert torch.count_nonzero(cube_object._permanent_wrench_composer.composed_torque.torch) == 0 + + +@pytest.mark.xfail(reason=_MATERIAL_GAP_REASON, strict=False) +@pytest.mark.parametrize("num_cubes", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.isaacsim_ci +def test_rigid_body_set_material_properties(num_cubes, device): + """Test getting and setting material properties of rigid object.""" + raise NotImplementedError(_MATERIAL_GAP_REASON) + + +@pytest.mark.xfail(reason=_MATERIAL_GAP_REASON, strict=False) +@pytest.mark.parametrize("num_cubes", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.isaacsim_ci +def test_set_material_properties_via_view(num_cubes, device): + """Test setting material properties via the PhysX view-level API.""" + raise NotImplementedError(_MATERIAL_GAP_REASON) + + +@pytest.mark.xfail(reason=_MATERIAL_GAP_REASON, strict=False) +@pytest.mark.parametrize("num_cubes", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.isaacsim_ci +def test_rigid_body_no_friction(num_cubes, device): + """Test that a rigid object with no friction will maintain it's velocity when sliding across a plane.""" + raise NotImplementedError(_MATERIAL_GAP_REASON) + + +@pytest.mark.xfail(reason=_MATERIAL_GAP_REASON, strict=False) +@pytest.mark.parametrize("num_cubes", [1, 2]) +@pytest.mark.parametrize("device", ["cuda", "cpu"]) +@pytest.mark.isaacsim_ci +def test_rigid_body_with_static_friction(num_cubes, device): + """Test that static friction applied to rigid object works as expected. + + This test works by applying a force to the object and checking if the object moves or not based on the + mu (coefficient of static friction) value set for the object. We set the static friction to be non-zero and + apply a force to the object. When the force applied is below mu, the object should not move. When the force + applied is above mu, the object should move. + """ + raise NotImplementedError(_MATERIAL_GAP_REASON) + + +@pytest.mark.xfail(reason=_MATERIAL_GAP_REASON, strict=False) +@pytest.mark.parametrize("num_cubes", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.isaacsim_ci +def test_rigid_body_with_restitution(num_cubes, device): + """Test that restitution when applied to rigid object works as expected. + + This test works by dropping a block from a height and checking if the block bounces or not based on the + restitution value set for the object. We set the restitution to be non-zero and drop the block from a height. + When the restitution is 0, the block should not bounce. When the restitution is between 0 and 1, the block + should bounce with less energy. + """ + raise NotImplementedError(_MATERIAL_GAP_REASON) + + +@pytest.mark.parametrize("num_cubes", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.isaacsim_ci +def test_rigid_body_set_mass(num_cubes, device): + """Test getting and setting mass of rigid object.""" + with _ovphysx_sim_context( + device=device, gravity_enabled=False, add_ground_plane=True, auto_add_lighting=True + ) as sim: + # Create a scene with random cubes + cube_object, _ = generate_cubes_scene(num_cubes=num_cubes, height=1.0, device=device) + + # Play sim + sim.reset() + + # Get masses before increasing + original_masses = cube_object.data.body_mass.torch.clone() + + assert original_masses.shape == (num_cubes, 1) + + # Randomize mass of the object + masses = original_masses + torch.FloatTensor(num_cubes, 1).uniform_(4, 8).to(sim.device) + + indices = torch.tensor(range(num_cubes), dtype=torch.int32) + + # Set the new masses via the OVPhysX writer (matches PhysX/Newton). + cube_object.set_masses_index( + masses=wp.from_torch(masses.contiguous(), dtype=wp.float32), + env_ids=wp.from_torch(indices, dtype=wp.int32), + ) + + torch.testing.assert_close(cube_object.data.body_mass.torch, masses) + + # Simulate physics + # perform rendering + sim.step() + # update object + cube_object.update(sim.cfg.dt) + + masses_to_check = cube_object.data.body_mass.torch + + # Check if mass is set correctly + torch.testing.assert_close(masses, masses_to_check) + + +@pytest.mark.parametrize("num_cubes", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.parametrize("gravity_enabled", [True, False]) +@pytest.mark.isaacsim_ci +def test_gravity_vec_w(num_cubes, device, gravity_enabled): + """Test that gravity vector direction is set correctly for the rigid object.""" + with _ovphysx_sim_context(device=device, gravity_enabled=gravity_enabled) as sim: + # Create a scene with random cubes + cube_object, _ = generate_cubes_scene(num_cubes=num_cubes, device=device) + + # Obtain gravity direction + if gravity_enabled: + gravity_dir = (0.0, 0.0, -1.0) + else: + gravity_dir = (0.0, 0.0, 0.0) + + # Play sim + sim.reset() + + # Check that gravity is set correctly + assert cube_object.data.GRAVITY_VEC_W.torch[0, 0] == gravity_dir[0] + assert cube_object.data.GRAVITY_VEC_W.torch[0, 1] == gravity_dir[1] + assert cube_object.data.GRAVITY_VEC_W.torch[0, 2] == gravity_dir[2] + + # Simulate physics + for _ in range(2): + # perform rendering + sim.step() + # update object + cube_object.update(sim.cfg.dt) + + # Expected gravity value is the acceleration of the body + gravity = torch.zeros(num_cubes, 1, 6, device=device) + if gravity_enabled: + gravity[:, :, 2] = -9.81 + # Check the body accelerations are correct + torch.testing.assert_close(cube_object.data.body_acc_w.torch, gravity) + + +@pytest.mark.parametrize("num_cubes", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.parametrize("with_offset", [True, False]) +@pytest.mark.isaacsim_ci +@flaky(max_runs=3, min_passes=1) +def test_body_root_state_properties(num_cubes, device, with_offset): + """Test the root_com_state_w, root_link_state_w, body_com_state_w, and body_link_state_w properties.""" + with _ovphysx_sim_context(device=device, gravity_enabled=False, auto_add_lighting=True) as sim: + # Create a scene with random cubes + cube_object, env_pos = generate_cubes_scene(num_cubes=num_cubes, height=0.0, device=device) + env_idx = torch.tensor([x for x in range(num_cubes)], dtype=torch.int32) + + # Play sim + sim.reset() + + # Check if cube_object is initialized + assert cube_object.is_initialized + + # change center of mass offset from link frame + if with_offset: + offset = torch.tensor([0.1, 0.0, 0.0], device=device).repeat(num_cubes, 1) + else: + offset = torch.tensor([0.0, 0.0, 0.0], device=device).repeat(num_cubes, 1) + + # Read current COMs, mutate the translation, write back via the OVPhysX + # ``set_coms_index`` setter (PhysX uses ``root_view.set_coms`` for the same + # operation; OVPhysX wraps the wheel ``RIGID_BODY_COM_POSE`` write in + # :meth:`set_coms_index`, which follows the PhysX ``wp.transformf`` contract). + com = cube_object.data.body_com_pose_b.torch.clone() # shape (N, 1, 7) + com[..., :3] = offset.to(com.device).unsqueeze(1) + cube_object.set_coms_index( + coms=wp.from_torch(com.contiguous(), dtype=wp.transformf), + env_ids=wp.from_torch(env_idx, dtype=wp.int32), + ) + + # check ceter of mass has been set + torch.testing.assert_close(cube_object.data.body_com_pose_b.torch, com) + + # random z spin velocity + spin_twist = torch.zeros(6, device=device) + spin_twist[5] = torch.randn(1, device=device) + + # Simulate physics + for _ in range(100): + # spin the object around Z axis (com) + cube_object.write_root_velocity_to_sim_index(root_velocity=spin_twist.repeat(num_cubes, 1)) + # perform rendering + sim.step() + # update object + cube_object.update(sim.cfg.dt) + + # get state properties + root_link_pose_w = cube_object.data.root_link_pose_w.torch + root_link_vel_w = cube_object.data.root_link_vel_w.torch + root_com_pose_w = cube_object.data.root_com_pose_w.torch + root_com_vel_w = cube_object.data.root_com_vel_w.torch + body_link_pose_w = cube_object.data.body_link_pose_w.torch + body_link_vel_w = cube_object.data.body_link_vel_w.torch + body_com_pose_w = cube_object.data.body_com_pose_w.torch + body_com_vel_w = cube_object.data.body_com_vel_w.torch + + # if offset is [0,0,0] all root_state_%_w will match and all body_%_w will match + if not with_offset: + torch.testing.assert_close(root_link_pose_w, root_com_pose_w) + torch.testing.assert_close(root_com_vel_w, root_link_vel_w) + torch.testing.assert_close(root_link_pose_w, root_link_pose_w) + torch.testing.assert_close(root_com_vel_w, root_link_vel_w) + torch.testing.assert_close(body_link_pose_w, body_com_pose_w) + torch.testing.assert_close(body_com_vel_w, body_link_vel_w) + torch.testing.assert_close(body_link_pose_w, body_link_pose_w) + torch.testing.assert_close(body_com_vel_w, body_link_vel_w) + else: + # cubes are spinning around center of mass + # position will not match + # center of mass position will be constant (i.e. spinning around com) + torch.testing.assert_close(env_pos + offset, root_com_pose_w[..., :3]) + torch.testing.assert_close(env_pos + offset, body_com_pose_w[..., :3].squeeze(-2)) + # link position will be moving but should stay constant away from center of mass + root_link_state_pos_rel_com = quat_apply_inverse( + root_link_pose_w[..., 3:], + root_link_pose_w[..., :3] - root_com_pose_w[..., :3], + ) + torch.testing.assert_close(-offset, root_link_state_pos_rel_com) + body_link_state_pos_rel_com = quat_apply_inverse( + body_link_pose_w[..., 3:], + body_link_pose_w[..., :3] - body_com_pose_w[..., :3], + ) + torch.testing.assert_close(-offset, body_link_state_pos_rel_com.squeeze(-2)) + + # orientation of com will be a constant rotation from link orientation + com_quat_b = cube_object.data.body_com_quat_b.torch + com_quat_w = quat_mul(body_link_pose_w[..., 3:], com_quat_b) + torch.testing.assert_close(com_quat_w, body_com_pose_w[..., 3:]) + torch.testing.assert_close(com_quat_w.squeeze(-2), root_com_pose_w[..., 3:]) + + # orientation of link will match root state will always match + torch.testing.assert_close(root_link_pose_w[..., 3:], root_link_pose_w[..., 3:]) + torch.testing.assert_close(body_link_pose_w[..., 3:], body_link_pose_w[..., 3:]) + + # lin_vel will not match + # center of mass vel will be constant (i.e. spinning around com) + torch.testing.assert_close(torch.zeros_like(root_com_vel_w[..., :3]), root_com_vel_w[..., :3]) + torch.testing.assert_close(torch.zeros_like(body_com_vel_w[..., :3]), body_com_vel_w[..., :3]) + # link frame will be moving, and should be equal to input angular velocity cross offset + lin_vel_rel_root_gt = quat_apply_inverse(root_link_pose_w[..., 3:], root_link_vel_w[..., :3]) + lin_vel_rel_body_gt = quat_apply_inverse(body_link_pose_w[..., 3:], body_link_vel_w[..., :3]) + lin_vel_rel_gt = torch.linalg.cross(spin_twist.repeat(num_cubes, 1)[..., 3:], -offset) + torch.testing.assert_close(lin_vel_rel_gt, lin_vel_rel_root_gt, atol=1e-4, rtol=1e-4) + torch.testing.assert_close(lin_vel_rel_gt, lin_vel_rel_body_gt.squeeze(-2), atol=1e-4, rtol=1e-4) + + # ang_vel will always match + torch.testing.assert_close(root_com_vel_w[..., 3:], root_com_vel_w[..., 3:]) + torch.testing.assert_close(root_com_vel_w[..., 3:], root_link_vel_w[..., 3:]) + torch.testing.assert_close(body_com_vel_w[..., 3:], body_com_vel_w[..., 3:]) + torch.testing.assert_close(body_com_vel_w[..., 3:], body_link_vel_w[..., 3:]) + + +@pytest.mark.parametrize("num_cubes", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.parametrize("with_offset", [True, False]) +@pytest.mark.parametrize("state_location", ["com", "link"]) +@pytest.mark.isaacsim_ci +def test_write_root_state(num_cubes, device, with_offset, state_location): + """Test the setters for root_state using both the link frame and center of mass as reference frame.""" + with _ovphysx_sim_context(device=device, gravity_enabled=False, auto_add_lighting=True) as sim: + # Create a scene with random cubes + cube_object, env_pos = generate_cubes_scene(num_cubes=num_cubes, height=0.0, device=device) + env_idx = torch.tensor([x for x in range(num_cubes)], dtype=torch.int32) + + # Play sim + sim.reset() + + # Check if cube_object is initialized + assert cube_object.is_initialized + + # change center of mass offset from link frame + if with_offset: + offset = torch.tensor([0.1, 0.0, 0.0], device=device).repeat(num_cubes, 1) + else: + offset = torch.tensor([0.0, 0.0, 0.0], device=device).repeat(num_cubes, 1) + + com = cube_object.data.body_com_pose_b.torch.clone() # shape (N, 1, 7) + com[..., :3] = offset.to(com.device).unsqueeze(1) + cube_object.set_coms_index( + coms=wp.from_torch(com.contiguous(), dtype=wp.transformf), + env_ids=wp.from_torch(env_idx, dtype=wp.int32), + ) + + # check center of mass has been set + torch.testing.assert_close(cube_object.data.body_com_pose_b.torch, com) + + rand_state = torch.zeros(num_cubes, 13, device=device) + rand_state[..., :7] = cube_object.data.default_root_pose.torch + rand_state[..., :3] += env_pos + # make quaternion a unit vector + rand_state[..., 3:7] = torch.nn.functional.normalize(rand_state[..., 3:7], dim=-1) + + env_idx = env_idx.to(device) + for i in range(10): + # perform step + sim.step() + # update buffers + cube_object.update(sim.cfg.dt) + + if state_location == "com": + if i % 2 == 0: + cube_object.write_root_com_pose_to_sim_index(root_pose=rand_state[..., :7]) + cube_object.write_root_com_velocity_to_sim_index(root_velocity=rand_state[..., 7:]) + else: + cube_object.write_root_com_pose_to_sim_index(root_pose=rand_state[..., :7], env_ids=env_idx) + cube_object.write_root_com_velocity_to_sim_index(root_velocity=rand_state[..., 7:], env_ids=env_idx) + elif state_location == "link": + if i % 2 == 0: + cube_object.write_root_link_pose_to_sim_index(root_pose=rand_state[..., :7]) + cube_object.write_root_link_velocity_to_sim_index(root_velocity=rand_state[..., 7:]) + else: + cube_object.write_root_link_pose_to_sim_index(root_pose=rand_state[..., :7], env_ids=env_idx) + cube_object.write_root_link_velocity_to_sim_index( + root_velocity=rand_state[..., 7:], env_ids=env_idx + ) + + if state_location == "com": + torch.testing.assert_close(rand_state[..., :7], cube_object.data.root_com_pose_w.torch) + torch.testing.assert_close(rand_state[..., 7:], cube_object.data.root_com_vel_w.torch) + elif state_location == "link": + torch.testing.assert_close(rand_state[..., :7], cube_object.data.root_link_pose_w.torch) + torch.testing.assert_close(rand_state[..., 7:], cube_object.data.root_link_vel_w.torch) + + +@pytest.mark.parametrize("num_cubes", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.parametrize("with_offset", [True]) +@pytest.mark.parametrize("state_location", ["com", "link", "root"]) +@pytest.mark.isaacsim_ci +def test_write_state_functions_data_consistency(num_cubes, device, with_offset, state_location): + """Test the setters for root_state using both the link frame and center of mass as reference frame.""" + with _ovphysx_sim_context(device=device, gravity_enabled=False, auto_add_lighting=True) as sim: + # Create a scene with random cubes + cube_object, env_pos = generate_cubes_scene(num_cubes=num_cubes, height=0.0, device=device) + env_idx = torch.tensor([x for x in range(num_cubes)], dtype=torch.int32) + + # Play sim + sim.reset() + + # Check if cube_object is initialized + assert cube_object.is_initialized + + # change center of mass offset from link frame + if with_offset: + offset = torch.tensor([0.1, 0.0, 0.0], device=device).repeat(num_cubes, 1) + else: + offset = torch.tensor([0.0, 0.0, 0.0], device=device).repeat(num_cubes, 1) + + com = cube_object.data.body_com_pose_b.torch.clone() # shape (N, 1, 7) + com[..., :3] = offset.to(com.device).unsqueeze(1) + cube_object.set_coms_index( + coms=wp.from_torch(com.contiguous(), dtype=wp.transformf), + env_ids=wp.from_torch(env_idx, dtype=wp.int32), + ) + + # check ceter of mass has been set + torch.testing.assert_close(cube_object.data.body_com_pose_b.torch, com) + + rand_state = torch.rand(num_cubes, 13, device=device) + rand_state[..., :3] += env_pos + # make quaternion a unit vector + rand_state[..., 3:7] = torch.nn.functional.normalize(rand_state[..., 3:7], dim=-1) + + env_idx = env_idx.to(device) + + # perform step + sim.step() + # update buffers + cube_object.update(sim.cfg.dt) + + if state_location == "com": + cube_object.write_root_com_pose_to_sim_index(root_pose=rand_state[..., :7]) + cube_object.write_root_com_velocity_to_sim_index(root_velocity=rand_state[..., 7:]) + elif state_location == "link": + cube_object.write_root_link_pose_to_sim_index(root_pose=rand_state[..., :7]) + cube_object.write_root_link_velocity_to_sim_index(root_velocity=rand_state[..., 7:]) + elif state_location == "root": + cube_object.write_root_pose_to_sim_index(root_pose=rand_state[..., :7]) + cube_object.write_root_velocity_to_sim_index(root_velocity=rand_state[..., 7:]) + + if state_location == "com": + root_com_pose_w = cube_object.data.root_com_pose_w.torch + root_com_vel_w = cube_object.data.root_com_vel_w.torch + body_com_pose_b = cube_object.data.body_com_pose_b.torch + expected_root_link_pos, expected_root_link_quat = combine_frame_transforms( + root_com_pose_w[:, :3], + root_com_pose_w[:, 3:], + quat_rotate(quat_inv(body_com_pose_b[:, 0, 3:7]), -body_com_pose_b[:, 0, :3]), + quat_inv(body_com_pose_b[:, 0, 3:7]), + ) + expected_root_link_pose = torch.cat((expected_root_link_pos, expected_root_link_quat), dim=1) + root_link_pose_w = cube_object.data.root_link_pose_w.torch + root_link_vel_w = cube_object.data.root_link_vel_w.torch + # test both root_pose and root_link successfully updated when root_com updates + torch.testing.assert_close(expected_root_link_pose, root_link_pose_w) + # skip lin_vel because it differs from link frame, this should be fine because we are only checking + # if velocity update is triggered, which can be determined by comparing angular velocity + torch.testing.assert_close(root_com_vel_w[:, 3:], root_link_vel_w[:, 3:]) + torch.testing.assert_close(expected_root_link_pose, root_link_pose_w) + torch.testing.assert_close(root_com_vel_w[:, 3:], cube_object.data.root_com_vel_w.torch[:, 3:]) + elif state_location == "link": + root_link_pose_w = cube_object.data.root_link_pose_w.torch + root_link_vel_w = cube_object.data.root_link_vel_w.torch + body_com_pose_b = cube_object.data.body_com_pose_b.torch + expected_com_pos, expected_com_quat = combine_frame_transforms( + root_link_pose_w[:, :3], + root_link_pose_w[:, 3:], + body_com_pose_b[:, 0, :3], + body_com_pose_b[:, 0, 3:7], + ) + expected_com_pose = torch.cat((expected_com_pos, expected_com_quat), dim=1) + root_com_pose_w = cube_object.data.root_com_pose_w.torch + root_com_vel_w = cube_object.data.root_com_vel_w.torch + # test both root_pose and root_com successfully updated when root_link updates + torch.testing.assert_close(expected_com_pose, root_com_pose_w) + # skip lin_vel because it differs from link frame, this should be fine because we are only checking + # if velocity update is triggered, which can be determined by comparing angular velocity + torch.testing.assert_close(root_link_vel_w[:, 3:], root_com_vel_w[:, 3:]) + torch.testing.assert_close(root_link_pose_w, cube_object.data.root_link_pose_w.torch) + torch.testing.assert_close(root_link_vel_w[:, 3:], cube_object.data.root_com_vel_w.torch[:, 3:]) + elif state_location == "root": + root_link_pose_w = cube_object.data.root_link_pose_w.torch + root_com_vel_w = cube_object.data.root_com_vel_w.torch + body_com_pose_b = cube_object.data.body_com_pose_b.torch + expected_com_pos, expected_com_quat = combine_frame_transforms( + root_link_pose_w[:, :3], + root_link_pose_w[:, 3:], + body_com_pose_b[:, 0, :3], + body_com_pose_b[:, 0, 3:7], + ) + expected_com_pose = torch.cat((expected_com_pos, expected_com_quat), dim=1) + root_com_pose_w = cube_object.data.root_com_pose_w.torch + root_link_vel_w = cube_object.data.root_link_vel_w.torch + # test both root_com and root_link successfully updated when root_pose updates + torch.testing.assert_close(expected_com_pose, root_com_pose_w) + torch.testing.assert_close(root_com_vel_w, cube_object.data.root_com_vel_w.torch) + torch.testing.assert_close(root_link_pose_w, cube_object.data.root_link_pose_w.torch) + torch.testing.assert_close(root_com_vel_w[:, 3:], root_link_vel_w[:, 3:]) + + +@pytest.mark.isaacsim_ci +def test_warmup_attach_stage_not_called_for_cpu(): + """Regression test: ``physx.warmup_gpu()`` must not be called for CPU. + + OVPhysX-equivalent of PhysX's ``test_warmup_attach_stage_not_called_for_cpu``: + PhysX guards :meth:`attach_stage` with ``if is_gpu:`` so the CPU MBP + broadphase is not double-initialised. The OVPhysX manager has the same + structural guard around :meth:`OvPhysxManager._physx.warmup_gpu`: it is + only invoked when ``ovphysx_device == "gpu"``. + + We monkey-patch ``OvPhysxManager._physx`` with a :class:`MagicMock` + wrapping the live PhysX object so that ``warmup_gpu`` becomes a spy while + other calls continue to forward, then assert ``warmup_gpu.call_count == 0`` + after a CPU-mode :meth:`sim.reset`. + + The test always runs CPU regardless of session parametrization, so it is + skipped when the session-locked device is anything other than CPU. The + skip is enforced inline (rather than in the autouse fixture) so the rest + of the suite can still pin to GPU when invoked together. + """ + if _LOCKED_DEVICE[0] not in (None, "cpu"): + pytest.skip( + f"ovphysx process-global device lock is held by '{_LOCKED_DEVICE[0]}'; cannot run " + "CPU-only regression test in the same session." + ) + _LOCKED_DEVICE[0] = "cpu" + + with _ovphysx_sim_context(device="cpu", add_ground_plane=True, dt=0.01, auto_add_lighting=True) as sim: + # Allocate a single rigid body so the manager has something to load. + generate_cubes_scene(num_cubes=1, height=1.0, device="cpu") + + # First reset constructs (or reuses) the real ovphysx.PhysX so we have + # a live instance to wrap. The PhysX object is a C++ binding, so we + # cannot patch attributes directly — replace the class-level reference + # with a MagicMock(wraps=...) that forwards every call. + sim.reset() + original_physx = OvPhysxManager._physx + assert original_physx is not None, "PhysX should be constructed after sim.reset()" + spy = MagicMock(wraps=original_physx) + OvPhysxManager._physx = spy + # Force _warmup_and_load to run again on the next reset so the spy + # observes the warmup_gpu (or non-call) decision; close() resets + # _warmup_done back to False but we just called sim.reset() above. + OvPhysxManager._warmup_done = False + try: + sim.reset() + finally: + OvPhysxManager._physx = original_physx + + assert spy.warmup_gpu.call_count == 0, ( + f"warmup_gpu() was called {spy.warmup_gpu.call_count} time(s) during CPU warmup. " + "OvPhysxManager._warmup_and_load() must guard warmup_gpu() with " + "ovphysx_device == 'gpu' so the CPU pipeline is not mis-initialised." + ) diff --git a/source/isaaclab_ovphysx/test/assets/test_rigid_object_helpers.py b/source/isaaclab_ovphysx/test/assets/test_rigid_object_helpers.py new file mode 100644 index 000000000000..e57d8d2651d7 --- /dev/null +++ b/source/isaaclab_ovphysx/test/assets/test_rigid_object_helpers.py @@ -0,0 +1,45 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""OVPhysX-only unit tests for rigid-object helpers. + +These tests cover OVPhysX-specific scaffolding (mock binding-set shape +contracts for ``asset_kind="rigid_object"``) that has no PhysX equivalent +and therefore does not appear in the PhysX-mirrored ``test_rigid_object.py``. +""" + +from __future__ import annotations + +import pytest +import warp as wp + +# The CI isaaclab_ov* pattern unintentionally collects isaaclab_ovphysx tests, +# but the ovphysx wheel is not installed in that environment. Skip gracefully +# so the isaaclab_ov CI pipeline is not blocked by an unrelated dependency. +pytest.importorskip("ovphysx.types", reason="ovphysx wheel not installed") + +from isaaclab_ovphysx import tensor_types as TT # noqa: E402 +from isaaclab_ovphysx.test.mock_interfaces.views import MockOvPhysxBindingSet # noqa: E402 + +wp.init() + + +def test_mock_binding_set_rigid_object_shapes(): + pytest.importorskip("isaaclab_ovphysx.tensor_types").RIGID_BODY_POSE # gates on wheel + + bindings = MockOvPhysxBindingSet( + num_instances=4, + num_joints=0, + num_bodies=1, + asset_kind="rigid_object", + ) + assert bindings.bindings[TT.RIGID_BODY_POSE].shape == (4, 7) + assert bindings.bindings[TT.RIGID_BODY_VELOCITY].shape == (4, 6) + assert bindings.bindings[TT.RIGID_BODY_WRENCH].shape == (4, 9) + assert bindings.bindings[TT.RIGID_BODY_MASS].shape == (4,) + assert bindings.bindings[TT.RIGID_BODY_INERTIA].shape == (4, 9) + # Articulation-only bindings must be absent + assert TT.DOF_POSITION not in bindings.bindings + assert TT.LINK_WRENCH not in bindings.bindings diff --git a/source/isaaclab_physx/changelog.d/fix-fabric-local-matrix.rst b/source/isaaclab_physx/changelog.d/fix-fabric-local-matrix.rst new file mode 100644 index 000000000000..43f2eaae0ee7 --- /dev/null +++ b/source/isaaclab_physx/changelog.d/fix-fabric-local-matrix.rst @@ -0,0 +1,24 @@ +Fixed +^^^^^ + +* Fixed :meth:`~isaaclab_physx.sim.views.FabricFrameView.get_local_poses` + returning stale USD values after Fabric world-pose writes. Local poses + are now read directly from Fabric's ``omni:fabric:localMatrix`` via + :class:`wp.indexedfabricarray`, and are kept consistent with worldMatrix + through Warp kernels that propagate either direction on writes. + +Changed +^^^^^^^ + +* Reworked :class:`~isaaclab_physx.sim.views.FabricFrameView` to use three + persistent ``PrimSelection`` instances (one per access mode), path-based + view → fabric index mapping (no custom prim attributes), and Warp kernels + that operate on :class:`wp.indexedfabricarray` so the kernels just index + ``ifa[view_index]`` instead of taking a separate mapping array. +* :meth:`~isaaclab_physx.sim.views.FabricFrameView.set_local_poses` now + writes ``omni:fabric:localMatrix`` directly through Fabric. The next + ``get_world_poses`` runs a Warp kernel that recomputes + ``child_world = parent_world * child_local``. Symmetrically, + ``set_world_poses`` runs a kernel that recomputes + ``child_local = inv(parent_world) * child_world`` so subsequent + ``get_local_poses`` calls return consistent values. diff --git a/source/isaaclab_physx/changelog.d/mtrepte-expand_viz_markers.skip b/source/isaaclab_physx/changelog.d/mtrepte-expand_viz_markers.skip deleted file mode 100644 index 4f6915f6b47b..000000000000 --- a/source/isaaclab_physx/changelog.d/mtrepte-expand_viz_markers.skip +++ /dev/null @@ -1,2 +0,0 @@ -Marker visualization changes are covered by the isaaclab fragment. -Marker visualization changes are covered by the isaaclab fragment. diff --git a/source/isaaclab_physx/changelog.d/pr-5458-merge-develop.rst b/source/isaaclab_physx/changelog.d/pr-5458-merge-develop.rst deleted file mode 100644 index 1fe6a600bb99..000000000000 --- a/source/isaaclab_physx/changelog.d/pr-5458-merge-develop.rst +++ /dev/null @@ -1,16 +0,0 @@ -Added -^^^^^ - -* Added :class:`~isaaclab_physx.sensors.JointWrenchSensor` for reading PhysX - incoming joint reaction wrenches as split force [N] and torque [N·m] buffers. - The sensor accepts asset prim paths whose articulation root is nested below - the configured prim and converts PhysX's native body-frame wrench to the - shared child-side joint-frame convention. - -Removed -^^^^^^^ - -* Removed ``ArticulationData.body_incoming_joint_wrench_b``. Add - :class:`~isaaclab.sensors.JointWrenchSensorCfg` to the scene and read - :attr:`~isaaclab.sensors.JointWrenchSensorData.force` and - :attr:`~isaaclab.sensors.JointWrenchSensorData.torque` instead. diff --git a/source/isaaclab_physx/changelog.d/rschmitt_decouple_renderer_camera.rst b/source/isaaclab_physx/changelog.d/rschmitt_decouple_renderer_camera.rst deleted file mode 100644 index eada0bbb809c..000000000000 --- a/source/isaaclab_physx/changelog.d/rschmitt_decouple_renderer_camera.rst +++ /dev/null @@ -1,4 +0,0 @@ -Changed -^^^^^^^^ - -* Modified the isaac rtx renderer to use the new patterns from renderer/camera decoupling. diff --git a/source/isaaclab_physx/config/extension.toml b/source/isaaclab_physx/config/extension.toml index 5c63b0e6322f..7b3903e18087 100644 --- a/source/isaaclab_physx/config/extension.toml +++ b/source/isaaclab_physx/config/extension.toml @@ -1,7 +1,7 @@ [package] # Note: Semantic Versioning is used: https://semver.org/ -version = "0.5.29" +version = "0.7.1" # Description title = "PhysX simulation interfaces for IsaacLab core package" diff --git a/source/isaaclab_physx/docs/CHANGELOG.rst b/source/isaaclab_physx/docs/CHANGELOG.rst index 14425eb74869..c344fb19b0ea 100644 --- a/source/isaaclab_physx/docs/CHANGELOG.rst +++ b/source/isaaclab_physx/docs/CHANGELOG.rst @@ -1,6 +1,236 @@ Changelog --------- +0.7.1 (2026-05-15) +~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Fixed the acceleration-arrow debug visualizer in + :class:`~isaaclab_physx.sensors.pva.Pva` drawing arrows in undefined directions for + bodies with effectively zero acceleration. Such bodies are now skipped from the + visualization. + + +0.7.0 (2026-05-14) +~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added PhysX implementations of + :attr:`~isaaclab.assets.BaseArticulationData.body_link_jacobian_w`, + :attr:`~isaaclab.assets.BaseArticulationData.body_com_jacobian_w`, + :attr:`~isaaclab.assets.BaseArticulationData.mass_matrix`, and + :attr:`~isaaclab.assets.BaseArticulationData.gravity_compensation_forces` + on :class:`~isaaclab_physx.assets.ArticulationData`. The COM + variant is a passthrough to ``physx.ArticulationView.get_jacobians``; + the link-origin variant applies a new + :func:`~isaaclab_physx.assets.articulation.kernels.shift_jacobian_com_to_origin` + Warp kernel to convert the COM-referenced linear-velocity rows to + link-origin references using each body's pose and COM offset. All + four properties preserve the full DoF axis, including the 6 leading + floating-base columns/rows PhysX's raw tensor view prepends on + floating-base assets — matching the cross-library industry convention + (Pinocchio, Drake, MuJoCo, RBDL, OCS2, iDynTree) and Newton's + ``ArticulationView`` layout. +* Added :meth:`~isaaclab_physx.physics.PhysxManager.pre_render` so the + PhysX backend can drive + :meth:`~isaaclab_newton.physics.NewtonManager.update_visualization_state` + once per render frame when the active visualizer/renderer set requires a + Newton model. + +Changed +^^^^^^^ + +* Switched the Newton install spec to ``newton[sim]`` in the ``newton`` + extra so the MuJoCo solver dependencies are pulled in transitively. + Required because pip resolves a git-URL requirement once for the URL; + a bare ``newton @ git+...`` here would shadow the ``[sim]`` extra + requested elsewhere. + +Removed +^^^^^^^ + +* **Breaking:** Removed the ``isaaclab_physx.scene_data_providers`` package + (``PhysxSceneDataProvider``). The Warp-native + :class:`~isaaclab.scene.scene_data_provider.SceneDataProvider` now exposes + PhysX rigid-body transforms via + :class:`~isaaclab_physx.physics.PhysxSceneDataBackend`, and the + PhysX→Newton state sync used by Newton visualizers/renderers moved to + :meth:`~isaaclab_newton.physics.NewtonManager.update_visualization_state`. + +Fixed +^^^^^ + +* Fixed a latent correctness bug in IK / OSC controllers on the PhysX + backend, where the previously-exposed Jacobian was COM-referenced but + the controllers used :attr:`~isaaclab_physx.assets.ArticulationData.body_link_pose_w` + as the EE pose setpoint. The frame mismatch caused tracking error on + bodies whose COM offset is non-trivial. The new + :attr:`~isaaclab.assets.BaseArticulationData.body_link_jacobian_w` + applies the COM→origin shift so the Jacobian and pose share a + reference point. + + +0.6.4 (2026-05-13) +~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Changed the Newton extra to depend on the packaged Newton 1.2.0 release + candidate instead of a Git commit. + + +0.6.3 (2026-05-11) +~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Changed rigid object collection spawning to honor planned ``spawn_path`` + values while falling back to ``prim_path`` for direct construction. + + +0.6.2 (2026-05-09) +~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Fixed :class:`~isaaclab_physx.assets.Articulation` joint friction docs to distinguish legacy coefficients from + PhysX 5 static and dynamic friction efforts. +* Fixed PhysX backend tests to use current contact sensor and asset API names, + removing deprecation warnings from scoped test runs. + + +0.6.1 (2026-05-08) +~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Bumped the optional ``[newton]`` extra to ``v1.2.0rc2`` so the Newton + scene representation built by + :class:`~isaaclab_physx.scene_data_providers.PhysxSceneDataProvider` + for the OV/Rerun/Viser visualizers stays in sync with the version + pinned in :mod:`isaaclab_newton` and :mod:`isaaclab_visualizers`. + + +0.6.0 (2026-05-08) +~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added :class:`~isaaclab_physx.sensors.JointWrenchSensor` for reading PhysX + incoming joint reaction wrenches as split force [N] and torque [N·m] buffers. + The sensor accepts asset prim paths whose articulation root is nested below + the configured prim and converts PhysX's native body-frame wrench to the + shared child-side joint-frame convention. +* Added :class:`PhysxRigidBodyMaterialCfg`, a subclass of + :class:`~isaaclab.sim.spawners.materials.RigidBodyMaterialBaseCfg` carrying the + ``PhysxMaterialAPI`` schema fields (``compliant_contact_stiffness``, + ``compliant_contact_damping``, ``friction_combine_mode``, ``restitution_combine_mode``). + Use this when authoring PhysX-specific material knobs; use the base class when only the + UsdPhysics-standard friction/restitution fields are needed. +* Added :class:`PhysxCollisionPropertiesCfg`, a subclass of + :class:`~isaaclab.sim.schemas.CollisionBaseCfg` carrying the PhysX-specific + ``torsional_patch_radius`` / ``min_torsional_patch_radius`` friction approximations. + These fields have no Newton equivalent. +* Added :class:`PhysxDeformableCollisionPropertiesCfg`, renaming the previous + ``PhysXCollisionPropertiesCfg`` (capital X) for clarity. Used internally by + :class:`DeformableBodyPropertiesCfg`. +* Added :class:`PhysxArticulationRootPropertiesCfg`, a subclass of + :class:`~isaaclab.sim.schemas.ArticulationRootBaseCfg` carrying the PhysX-specific + ``enabled_self_collisions``, ``solver_position_iteration_count``, + ``solver_velocity_iteration_count``, ``sleep_threshold``, ``stabilization_threshold``. +* Added :class:`PhysxConvexHullPropertiesCfg`, :class:`PhysxConvexDecompositionPropertiesCfg`, + :class:`PhysxTriangleMeshPropertiesCfg`, + :class:`PhysxTriangleMeshSimplificationPropertiesCfg`, and + :class:`PhysxSDFMeshPropertiesCfg` -- the PhysX-cooking-specific mesh collision + subclasses. Each declares its own PhysxSchema cooking API via class-level + ``_usd_applied_schema`` metadata and inherits ``mesh_approximation_name`` from + :class:`~isaaclab.sim.schemas.MeshCollisionBaseCfg`. +* Added :class:`PhysxFixedTendonPropertiesCfg` and :class:`PhysxSpatialTendonPropertiesCfg`, + the relocated PhysX-only tendon cfg classes. Same fields as the legacy core-side classes; + no field-level split. + +Changed +^^^^^^^ + +* Modified the isaac rtx renderer to use the new patterns from renderer/camera decoupling. +* **Breaking:** Removed the ``sync_usd_on_fabric_write`` keyword argument from + :class:`~isaaclab_physx.sim.views.FabricFrameView`. Fabric writes + (``set_world_poses``, ``set_scales``) now notify the renderer via + ``PrepareForReuse()`` on the underlying ``PrimSelection`` instead of writing + back to USD, which is ~200x faster and avoids the stale USD shadow state the + old path produced. Callers passing ``sync_usd_on_fabric_write=True`` should + remove the argument; if they relied on USD reflecting Fabric writes, they + should now read Fabric poses directly via the view's getters or refresh USD + explicitly. +* Removed the ``max_velocity`` field and USD metadata + (``_usd_applied_schema``, ``_usd_namespace``, ``_usd_attr_name_map``) from + :class:`PhysxJointDrivePropertiesCfg`. The field moved to + :class:`~isaaclab.sim.schemas.JointDriveBaseCfg`; ``PhysxJointDrivePropertiesCfg`` + inherits it. Existing instantiations continue to work unchanged. +* Removed the ``disable_gravity`` field from :class:`PhysxRigidBodyPropertiesCfg`. + The field moved to :class:`~isaaclab.sim.schemas.RigidBodyBaseCfg`; + ``PhysxRigidBodyPropertiesCfg`` inherits it. Existing instantiations continue + to work unchanged. + +Deprecated +^^^^^^^^^^ + +* Deprecated :class:`RigidBodyMaterialCfg` in favor of + :class:`PhysxRigidBodyMaterialCfg` (PhysX-specific) or + :class:`~isaaclab.sim.spawners.materials.RigidBodyMaterialBaseCfg` (solver-common). + The legacy name remains as a concrete subclass of :class:`PhysxRigidBodyMaterialCfg` + that emits ``DeprecationWarning`` on instantiation. Scheduled for removal in 5.0. +* Deprecated :class:`CollisionPropertiesCfg` in favor of + :class:`PhysxCollisionPropertiesCfg` (PhysX-specific) or + :class:`~isaaclab.sim.schemas.CollisionBaseCfg` (solver-common). The legacy name remains + as a concrete subclass of :class:`PhysxCollisionPropertiesCfg` that emits + ``DeprecationWarning`` on instantiation. Scheduled for removal in 5.0. +* Deprecated :class:`PhysXCollisionPropertiesCfg` (capital X, deformable-body) in favor of + :class:`PhysxDeformableCollisionPropertiesCfg`. The capital-X name is preserved as a + deprecation alias (concrete subclass) and is scheduled for removal in 5.0. +* Deprecated :class:`ArticulationRootPropertiesCfg` in favor of + :class:`PhysxArticulationRootPropertiesCfg` (PhysX-specific) or + :class:`~isaaclab.sim.schemas.ArticulationRootBaseCfg` (solver-common). The legacy name + remains as a concrete subclass of :class:`PhysxArticulationRootPropertiesCfg` that emits + ``DeprecationWarning`` on instantiation. Scheduled for removal in 5.0. +* Deprecated :class:`MeshCollisionPropertiesCfg`, :class:`ConvexHullPropertiesCfg`, + :class:`ConvexDecompositionPropertiesCfg`, :class:`TriangleMeshPropertiesCfg`, + :class:`TriangleMeshSimplificationPropertiesCfg`, and :class:`SDFMeshPropertiesCfg` in + favor of :class:`~isaaclab.sim.schemas.MeshCollisionBaseCfg` or the new ``Physx*`` + subclasses. Legacy names remain as concrete subclasses that emit ``DeprecationWarning`` + on instantiation. Scheduled for removal in 5.0. +* Deprecated :class:`FixedTendonPropertiesCfg` in favor of + :class:`PhysxFixedTendonPropertiesCfg`. Legacy name remains as a concrete subclass that + emits ``DeprecationWarning`` on instantiation. Scheduled for removal in 5.0. +* Deprecated :class:`SpatialTendonPropertiesCfg` in favor of + :class:`PhysxSpatialTendonPropertiesCfg`. Legacy name remains as a concrete subclass + that emits ``DeprecationWarning`` on instantiation. Scheduled for removal in 5.0. + +Removed +^^^^^^^ + +* Removed ``ArticulationData.body_incoming_joint_wrench_b``. Add + :class:`~isaaclab.sensors.JointWrenchSensorCfg` to the scene and read + :attr:`~isaaclab.sensors.JointWrenchSensorData.force` and + :attr:`~isaaclab.sensors.JointWrenchSensorData.torque` instead. + +Fixed +^^^^^ + +* Fixed :class:`~isaaclab_physx.assets.SurfaceGripper` initialization on + non-CPU simulation backends to raise before loading the surface gripper + extension, avoiding hangs during startup. + + 0.5.29 (2026-04-30) ~~~~~~~~~~~~~~~~~~~ diff --git a/source/isaaclab_physx/isaaclab_physx/assets/articulation/articulation.py b/source/isaaclab_physx/isaaclab_physx/assets/articulation/articulation.py index 913914e29f30..ea2c85e4c3bd 100644 --- a/source/isaaclab_physx/isaaclab_physx/assets/articulation/articulation.py +++ b/source/isaaclab_physx/isaaclab_physx/assets/articulation/articulation.py @@ -464,6 +464,10 @@ def write_root_link_pose_to_sim_index( self.data._body_state_w.timestamp = -1.0 self.data._body_link_state_w.timestamp = -1.0 self.data._body_com_state_w.timestamp = -1.0 + # Task-space accessors: body-frame Jacobian + gravity comp depend on root orientation; + # mass_matrix does not (configuration-space). + self.data._body_com_jacobian_w.timestamp = -1.0 + self.data._gravity_compensation_forces.timestamp = -1.0 # set into simulation self.root_view.set_root_transforms(self._get_root_link_pose_w_f32(), indices=env_ids) @@ -553,6 +557,10 @@ def write_root_com_pose_to_sim_index( self.data._body_state_w.timestamp = -1.0 self.data._body_link_state_w.timestamp = -1.0 self.data._body_com_state_w.timestamp = -1.0 + # Task-space accessors: body-frame Jacobian + gravity comp depend on root orientation; + # mass_matrix does not (configuration-space). + self.data._body_com_jacobian_w.timestamp = -1.0 + self.data._gravity_compensation_forces.timestamp = -1.0 # set into simulation self.root_view.set_root_transforms(self._get_root_link_pose_w_f32(), indices=env_ids) @@ -874,6 +882,10 @@ def write_joint_state_to_sim_index( self.data._body_state_w.timestamp = -1.0 self.data._body_link_state_w.timestamp = -1.0 self.data._body_com_state_w.timestamp = -1.0 + # Task-space accessors all depend on joint state. + self.data._body_com_jacobian_w.timestamp = -1.0 + self.data._mass_matrix.timestamp = -1.0 + self.data._gravity_compensation_forces.timestamp = -1.0 # set into simulation self.root_view.set_dof_positions(self.data._joint_pos.data, indices=env_ids) self.root_view.set_dof_velocities(self.data._joint_vel.data, indices=env_ids) @@ -963,6 +975,10 @@ def write_joint_position_to_sim_index( self.data._body_state_w.timestamp = -1.0 self.data._body_link_state_w.timestamp = -1.0 self.data._body_com_state_w.timestamp = -1.0 + # Task-space accessors all depend on joint state. + self.data._body_com_jacobian_w.timestamp = -1.0 + self.data._mass_matrix.timestamp = -1.0 + self.data._gravity_compensation_forces.timestamp = -1.0 # set into simulation self.root_view.set_dof_positions(self.data._joint_pos.data, indices=env_ids) @@ -1684,16 +1700,23 @@ def write_joint_friction_coefficient_to_sim_index( ): r"""Write joint friction coefficients over selected environment indices into the simulation. - For Isaac Sim versions below 5.0, only the static friction coefficient is set. - This limits the resisting force or torque up to a maximum proportional to the transmitted - spatial force: :math:`\|F_{resist}\| \leq \mu_s \, \|F_{spatial}\|`. + For Isaac Sim versions below 5.0, only the legacy unitless joint friction coefficient is set. + This limits the resisting force or torque up to a maximum proportional to the transmitted spatial force: + :math:`\|F_{resist}\| \leq \mu_s \, \|F_{spatial}\|`. - For Isaac Sim versions 5.0 and above, the static, dynamic, and viscous friction coefficients - are set. The model combines Coulomb (static & dynamic) friction with a viscous term: + For Isaac Sim versions 5.0 and above, the PhysX joint friction parameter model is used. It combines + Coulomb (static and dynamic) friction with a viscous term: - - Static friction :math:`\mu_s` defines the maximum effort that prevents motion at rest. - - Dynamic friction :math:`\mu_d` applies once motion begins and remains constant during motion. - - Viscous friction :math:`c_v` is a velocity-proportional resistive term. + - Static friction effort defines the maximum effort that prevents motion at rest [N or N·m, depending on + joint type]. + - Dynamic friction effort applies once motion begins and remains constant during motion [N or N·m, + depending on joint type]. + - Viscous friction coefficient is a velocity-proportional resistive term [N·s/m or N·m·s/rad, depending + on joint type]. + + .. warning:: + For Isaac Sim versions 5.0 and above, the static friction effort must be greater than or equal to the + dynamic friction effort. .. note:: This method expects partial data or full data. @@ -1703,11 +1726,12 @@ def write_joint_friction_coefficient_to_sim_index( is only supporting indexing, hence masks need to be converted to indices. Args: - joint_friction_coeff: Static friction coefficient :math:`\mu_s`. - Shape is (len(env_ids), len(joint_ids)) or (num_instances, num_joints). - joint_dynamic_friction_coeff: Dynamic (Coulomb) friction coefficient :math:`\mu_d`. + joint_friction_coeff: Legacy unitless coefficient for Isaac Sim versions below 5.0, or static friction + effort [N or N·m, depending on joint type] for Isaac Sim versions 5.0 and above. Shape is + (len(env_ids), len(joint_ids)) or (num_instances, num_joints). + joint_dynamic_friction_coeff: Dynamic friction effort [N or N·m, depending on joint type]. Same shape as above. If None, the dynamic coefficient is not updated. - joint_viscous_friction_coeff: Viscous friction coefficient :math:`c_v`. + joint_viscous_friction_coeff: Viscous friction coefficient [N·s/m or N·m·s/rad, depending on joint type]. Same shape as above. If None, the viscous coefficient is not updated. joint_ids: Joint indices. If None, then all joints are used. env_ids: Environment indices. If None, then all indices are used. @@ -1793,16 +1817,23 @@ def write_joint_friction_coefficient_to_sim_mask( ): r"""Write joint friction coefficients over selected environment mask into the simulation. - For Isaac Sim versions below 5.0, only the static friction coefficient is set. - This limits the resisting force or torque up to a maximum proportional to the transmitted - spatial force: :math:`\|F_{resist}\| \leq \mu_s \, \|F_{spatial}\|`. + For Isaac Sim versions below 5.0, only the legacy unitless joint friction coefficient is set. + This limits the resisting force or torque up to a maximum proportional to the transmitted spatial force: + :math:`\|F_{resist}\| \leq \mu_s \, \|F_{spatial}\|`. + + For Isaac Sim versions 5.0 and above, the PhysX joint friction parameter model is used. It combines + Coulomb (static and dynamic) friction with a viscous term: - For Isaac Sim versions 5.0 and above, the static, dynamic, and viscous friction coefficients - are set. The model combines Coulomb (static & dynamic) friction with a viscous term: + - Static friction effort defines the maximum effort that prevents motion at rest [N or N·m, depending on + joint type]. + - Dynamic friction effort applies once motion begins and remains constant during motion [N or N·m, + depending on joint type]. + - Viscous friction coefficient is a velocity-proportional resistive term [N·s/m or N·m·s/rad, depending + on joint type]. - - Static friction :math:`\mu_s` defines the maximum effort that prevents motion at rest. - - Dynamic friction :math:`\mu_d` applies once motion begins and remains constant during motion. - - Viscous friction :math:`c_v` is a velocity-proportional resistive term. + .. warning:: + For Isaac Sim versions 5.0 and above, the static friction effort must be greater than or equal to the + dynamic friction effort. .. note:: This method expects full data. @@ -1812,11 +1843,12 @@ def write_joint_friction_coefficient_to_sim_mask( is only supporting indexing, hence masks need to be converted to indices. Args: - joint_friction_coeff: Static friction coefficient :math:`\mu_s`. - Shape is (num_instances, num_joints). - joint_dynamic_friction_coeff: Dynamic (Coulomb) friction coefficient :math:`\mu_d`. + joint_friction_coeff: Legacy unitless coefficient for Isaac Sim versions below 5.0, or static friction + effort [N or N·m, depending on joint type] for Isaac Sim versions 5.0 and above. Shape is + (num_instances, num_joints). + joint_dynamic_friction_coeff: Dynamic friction effort [N or N·m, depending on joint type]. Same shape as above. If None, the dynamic coefficient is not updated. - joint_viscous_friction_coeff: Viscous friction coefficient :math:`c_v`. + joint_viscous_friction_coeff: Viscous friction coefficient [N·s/m or N·m·s/rad, depending on joint type]. Same shape as above. If None, the viscous coefficient is not updated. joint_mask: Joint mask. If None, then all joints are used. env_mask: Environment mask. If None, then all the instances are updated. Shape is (num_instances,). @@ -1842,7 +1874,9 @@ def write_joint_dynamic_friction_coefficient_to_sim_index( env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, full_data: bool = False, ) -> None: - """Write joint dynamic friction coefficient over selected environment indices into the simulation. + """Write joint dynamic friction effort over selected environment indices into the simulation. + + The dynamic friction effort is [N or N·m, depending on joint type]. .. note:: This method expects partial data or full data. @@ -1852,8 +1886,8 @@ def write_joint_dynamic_friction_coefficient_to_sim_index( is only supporting indexing, hence masks need to be converted to indices. Args: - joint_dynamic_friction_coeff: Joint dynamic friction coefficient. Shape is (len(env_ids), len(joint_ids)) - or (num_instances, num_joints) if full_data. + joint_dynamic_friction_coeff: Dynamic friction effort [N or N·m, depending on joint type]. Shape is + (len(env_ids), len(joint_ids)) or (num_instances, num_joints) if full_data. joint_ids: Joint indices. If None, then all joints are used. env_ids: Environment indices. If None, then all indices are used. full_data: Whether to expect full data. Defaults to False. @@ -1907,7 +1941,9 @@ def write_joint_dynamic_friction_coefficient_to_sim_mask( joint_mask: wp.array | None = None, env_mask: wp.array | None = None, ) -> None: - """Write joint dynamic friction coefficient over selected environment mask into the simulation. + """Write joint dynamic friction effort over selected environment mask into the simulation. + + The dynamic friction effort is [N or N·m, depending on joint type]. .. note:: This method expects full data. @@ -1917,7 +1953,8 @@ def write_joint_dynamic_friction_coefficient_to_sim_mask( is only supporting indexing, hence masks need to be converted to indices. Args: - joint_dynamic_friction_coeff: Joint dynamic friction coefficient. Shape is (num_instances, num_joints). + joint_dynamic_friction_coeff: Dynamic friction effort [N or N·m, depending on joint type]. Shape is + (num_instances, num_joints). joint_mask: Joint mask. If None, then all joints are used. env_mask: Environment mask. If None, then all the instances are updated. Shape is (num_instances,). """ @@ -1942,6 +1979,8 @@ def write_joint_viscous_friction_coefficient_to_sim_index( ) -> None: """Write joint viscous friction coefficient over selected environment indices into the simulation. + The coefficient is [N·s/m or N·m·s/rad, depending on joint type]. + .. note:: This method expects partial data or full data. @@ -1950,8 +1989,8 @@ def write_joint_viscous_friction_coefficient_to_sim_index( is only supporting indexing, hence masks need to be converted to indices. Args: - joint_viscous_friction_coeff: Joint viscous friction coefficient. Shape is (len(env_ids), len(joint_ids)) - or (num_instances, num_joints) if full_data. + joint_viscous_friction_coeff: Viscous friction coefficient [N·s/m or N·m·s/rad, depending on joint type]. + Shape is (len(env_ids), len(joint_ids)) or (num_instances, num_joints) if full_data. joint_ids: Joint indices. If None, then all joints are used. env_ids: Environment indices. If None, then all indices are used. full_data: Whether to expect full data. Defaults to False. @@ -2010,6 +2049,8 @@ def write_joint_viscous_friction_coefficient_to_sim_mask( ) -> None: """Write joint viscous friction coefficient over selected environment mask into the simulation. + The coefficient is [N·s/m or N·m·s/rad, depending on joint type]. + .. note:: This method expects full data. @@ -2018,7 +2059,8 @@ def write_joint_viscous_friction_coefficient_to_sim_mask( is only supporting indexing, hence masks need to be converted to indices. Args: - joint_viscous_friction_coeff: Joint viscous friction coefficient. Shape is (num_instances, num_joints). + joint_viscous_friction_coeff: Viscous friction coefficient [N·s/m or N·m·s/rad, depending on joint type]. + Shape is (num_instances, num_joints). joint_mask: Joint mask. If None, then all joints are used. env_mask: Environment mask. If None, then all the instances are updated. Shape is (num_instances,). """ diff --git a/source/isaaclab_physx/isaaclab_physx/assets/articulation/articulation_data.py b/source/isaaclab_physx/isaaclab_physx/assets/articulation/articulation_data.py index 8c2056d9cbfa..6f1ab28547c9 100644 --- a/source/isaaclab_physx/isaaclab_physx/assets/articulation/articulation_data.py +++ b/source/isaaclab_physx/isaaclab_physx/assets/articulation/articulation_data.py @@ -369,7 +369,10 @@ def joint_armature(self) -> ProxyArray: @property def joint_friction_coeff(self) -> ProxyArray: - """Joint static friction coefficient provided to the simulation. + """PhysX joint static friction value provided to the simulation. + + For Isaac Sim 5.0 and later, this is the static friction effort [N or N·m, depending on joint type]. + For earlier Isaac Sim versions, this is the legacy unitless joint friction coefficient. Shape is (num_instances, num_joints), dtype = wp.float32. In torch this resolves to (num_instances, num_joints). """ @@ -379,7 +382,9 @@ def joint_friction_coeff(self) -> ProxyArray: @property def joint_dynamic_friction_coeff(self) -> ProxyArray: - """Joint dynamic friction coefficient provided to the simulation. + """PhysX joint dynamic friction effort provided to the simulation. + + The effort is [N or N·m, depending on joint type]. Shape is (num_instances, num_joints), dtype = wp.float32. In torch this resolves to (num_instances, num_joints). """ @@ -391,6 +396,8 @@ def joint_dynamic_friction_coeff(self) -> ProxyArray: def joint_viscous_friction_coeff(self) -> ProxyArray: """Joint viscous friction coefficient provided to the simulation. + The coefficient is [N·s/m or N·m·s/rad, depending on joint type]. + Shape is (num_instances, num_joints), dtype = wp.float32. In torch this resolves to (num_instances, num_joints). """ if self._joint_viscous_friction_coeff_ta is None: @@ -850,6 +857,77 @@ def body_com_pose_b(self) -> ProxyArray: self._body_com_pose_b_ta = ProxyArray(self._body_com_pose_b.data) return self._body_com_pose_b_ta + """ + Dynamics quantities (task-space controllers). + """ + + @property + def body_com_jacobian_w(self) -> ProxyArray: + """See :attr:`isaaclab.assets.BaseArticulationData.body_com_jacobian_w`. + + PhysX implementation: passthrough of ``_root_view.get_jacobians()``, which is + natively Center-Of-Mass-referenced. Refresh is gated by ``_sim_timestamp`` and + invalidated by ``write_*_to_sim_index``; the ``ProxyArray`` wrapper is lazy-init + once and reused thereafter. + """ + if self._body_com_jacobian_w.timestamp < self._sim_timestamp: + self._body_com_jacobian_w.data = self._root_view.get_jacobians() + self._body_com_jacobian_w.timestamp = self._sim_timestamp + if self._body_com_jacobian_w_ta is None: + self._body_com_jacobian_w_ta = ProxyArray(self._body_com_jacobian_w.data) + return self._body_com_jacobian_w_ta + + @property + def body_link_jacobian_w(self) -> ProxyArray: + """See :attr:`isaaclab.assets.BaseArticulationData.body_link_jacobian_w`. + + PhysX implementation: applies the COM→origin shift kernel to + :attr:`body_com_jacobian_w` (PhysX's engine output is COM-referenced). + """ + wp.launch( + articulation_kernels.shift_jacobian_com_to_origin, + dim=self._body_link_jacobian_w_buf.shape[:2] + (self._body_link_jacobian_w_buf.shape[3],), + inputs=[ + self.body_link_pose_w.warp, + self.body_com_pos_b.warp, + self._jacobian_link_offset, + self.body_com_jacobian_w.warp, + ], + outputs=[self._body_link_jacobian_w_buf], + device=self.device, + ) + return self._body_link_jacobian_w_ta + + @property + def mass_matrix(self) -> ProxyArray: + """See :attr:`isaaclab.assets.BaseArticulationData.mass_matrix`. + + PhysX implementation: passthrough of ``_root_view.get_generalized_mass_matrices()``. + Refresh is gated by ``_sim_timestamp`` and invalidated by ``write_*_to_sim_index``; + the ``ProxyArray`` wrapper is lazy-init once and reused thereafter. + """ + if self._mass_matrix.timestamp < self._sim_timestamp: + self._mass_matrix.data = self._root_view.get_generalized_mass_matrices() + self._mass_matrix.timestamp = self._sim_timestamp + if self._mass_matrix_ta is None: + self._mass_matrix_ta = ProxyArray(self._mass_matrix.data) + return self._mass_matrix_ta + + @property + def gravity_compensation_forces(self) -> ProxyArray: + """See :attr:`isaaclab.assets.BaseArticulationData.gravity_compensation_forces`. + + PhysX implementation: passthrough of ``_root_view.get_gravity_compensation_forces()``. + Refresh is gated by ``_sim_timestamp`` and invalidated by ``write_*_to_sim_index``; + the ``ProxyArray`` wrapper is lazy-init once and reused thereafter. + """ + if self._gravity_compensation_forces.timestamp < self._sim_timestamp: + self._gravity_compensation_forces.data = self._root_view.get_gravity_compensation_forces() + self._gravity_compensation_forces.timestamp = self._sim_timestamp + if self._gravity_compensation_forces_ta is None: + self._gravity_compensation_forces_ta = ProxyArray(self._gravity_compensation_forces.data) + return self._gravity_compensation_forces_ta + """ Joint state properties. """ @@ -1363,6 +1441,46 @@ def _create_buffers(self) -> None: self._root_com_lin_vel_b = TimestampedBuffer((self._num_instances), self.device, wp.vec3f) self._root_com_ang_vel_b = TimestampedBuffer((self._num_instances), self.device, wp.vec3f) + # -- dynamics quantities for task-space controllers + # PhysX's Jacobian rows include the root body for floating-base and exclude only the + # fixed root for fixed-base (``_jacobian_link_offset`` handles the body axis). PhysX's + # raw Jacobian / mass matrix / gravity-comp prepend 6 base-DoF columns on floating- + # base (the engine's natural form), matching the industry-standard convention used by + # Pinocchio, Drake, MuJoCo, RBDL, OCS2, and iDynTree. We pass through the full DoF + # axis: shape ``(N, num_jacobi_bodies, 6, num_joints + num_base_dofs)``. Newton wraps + # ``eval_jacobian`` to match the same column layout. ``body_com_jacobian_w`` / + # ``mass_matrix`` / ``gravity_compensation_forces`` pass through the engine buffer on + # every read; we only own a buffer for the link-origin Jacobian (output of the shift + # kernel). + is_fixed_base = self._root_view.shared_metatype.fixed_base + self._jacobian_link_offset = 1 if is_fixed_base else 0 + num_jacobi_bodies = self._num_bodies - self._jacobian_link_offset + num_base_dofs = 0 if is_fixed_base else 6 + self._body_link_jacobian_w_buf = wp.zeros( + (self._num_instances, num_jacobi_bodies, 6, self._num_joints + num_base_dofs), + dtype=wp.float32, + device=self.device, + ) + # ``TimestampedBuffer``s for the three engine-passthrough properties. The placeholder + # ``wp.zeros`` allocation is replaced on first read by the engine view returned from + # ``_root_view.get_*()``; timestamps are advanced on each refresh and invalidated by + # write-paths. + self._body_com_jacobian_w = TimestampedBuffer( + (self._num_instances, num_jacobi_bodies, 6, self._num_joints + num_base_dofs), + self.device, + wp.float32, + ) + self._mass_matrix = TimestampedBuffer( + (self._num_instances, self._num_joints + num_base_dofs, self._num_joints + num_base_dofs), + self.device, + wp.float32, + ) + self._gravity_compensation_forces = TimestampedBuffer( + (self._num_instances, self._num_joints + num_base_dofs), + self.device, + wp.float32, + ) + # Default root pose and velocity self._default_root_pose = wp.zeros((self._num_instances), dtype=wp.transformf, device=self.device) self._default_root_vel = wp.zeros((self._num_instances), dtype=wp.spatial_vectorf, device=self.device) @@ -1525,6 +1643,16 @@ def _pin_proxy_arrays(self) -> None: self._body_com_vel_w_ta: ProxyArray | None = None self._body_com_acc_w_ta: ProxyArray | None = None self._body_com_pose_b_ta: ProxyArray | None = None + # Dynamics quantities (task-space controllers). ``_body_link_jacobian_w`` wraps our + # own pre-allocated buffer (pointer-stable, eager wrap). The three engine-passthrough + # wrappers are lazy-init inside their property bodies on first read, matching the + # ``TimestampedBuffer`` + ``ProxyArray`` cache pattern used by ``body_link_pose_w``, + # ``joint_pos``, and the rest of this file. Refresh is gated by ``_sim_timestamp`` and + # invalidated by ``write_*_to_sim_index`` setting ``timestamp = -1.0``. + self._body_link_jacobian_w_ta = ProxyArray(self._body_link_jacobian_w_buf) + self._body_com_jacobian_w_ta: ProxyArray | None = None + self._mass_matrix_ta: ProxyArray | None = None + self._gravity_compensation_forces_ta: ProxyArray | None = None # Body properties self._body_mass_ta: ProxyArray | None = None self._body_inertia_ta: ProxyArray | None = None diff --git a/source/isaaclab_physx/isaaclab_physx/assets/articulation/kernels.py b/source/isaaclab_physx/isaaclab_physx/assets/articulation/kernels.py index 5686c864dd94..0c2e385af173 100644 --- a/source/isaaclab_physx/isaaclab_physx/assets/articulation/kernels.py +++ b/source/isaaclab_physx/isaaclab_physx/assets/articulation/kernels.py @@ -486,3 +486,65 @@ def extract_friction_properties( out_friction[i, j] = friction_props[i, j, 0] out_dynamic_friction[i, j] = friction_props[i, j, 1] out_viscous_friction[i, j] = friction_props[i, j, 2] + + +@wp.kernel +def shift_jacobian_com_to_origin( + body_link_pose: wp.array2d(dtype=wp.transformf), + body_com_pos_b: wp.array2d(dtype=wp.vec3f), + link_offset: wp.int32, + src: wp.array4d(dtype=wp.float32), + dst: wp.array4d(dtype=wp.float32), +): + """Shift the linear-velocity rows of the Jacobian from COM to link origin. + + PhysX's ``ArticulationView.get_jacobians()`` returns ``J · q_dot = [v_com_world, omega_world]`` + per link — the linear rows are the velocity at the link's center of mass, expressed in + world frame. The :attr:`~isaaclab.assets.BaseArticulationData.body_link_jacobian_w` contract + requires the linear rows to be the velocity at the link **origin** (USD prim transform) so + that ``J · q_dot[body_idx]`` matches + :attr:`~isaaclab.assets.BaseArticulationData.body_link_lin_vel_w` / + :attr:`~isaaclab.assets.BaseArticulationData.body_link_ang_vel_w`. + + The shift identity is the same one applied per-body by + :func:`~isaaclab_physx.assets.kernels.get_link_vel_from_root_com_vel_func`, but layered onto + every Jacobian column: each column represents the spatial velocity contribution of one DoF, + and shifting a spatial velocity from COM to link origin uses ``v_origin = v_com - omega x + (R · body_com_pos_b)``. + + Notes on layout: + * Jacobian rows ``[0:3]`` are linear velocity, ``[3:6]`` are angular. + * ``body_link_pose`` and ``body_com_pos_b`` are indexed by the articulation's full body + count. PhysX's Jacobian rows are also indexed by the full body count for floating-base + and exclude only the root for fixed-base, so ``link_offset = 1`` for fixed-base and + ``link_offset = 0`` for floating-base, matching Newton's convention. + + Args: + body_link_pose: Per-body link pose in world frame. Shape is (num_instances, num_bodies). + body_com_pos_b: Per-body center-of-mass offset expressed in the body's link frame. Shape + is (num_instances, num_bodies). + link_offset: Offset added to the jacobian-row body index to reach the full body index. + ``1`` for fixed-base, ``0`` for floating-base. + src: COM-referenced Jacobian (read-only). Shape is (num_instances, num_jacobi_bodies, 6, + num_joints + num_base_dofs). + dst: Output buffer for the link-origin Jacobian. Same shape as ``src``. Linear rows + ``[0:3]`` are written with the shifted velocity; angular rows ``[3:6]`` are copied + unchanged (angular velocity is reference-point invariant). + """ + n, b, dof = wp.tid() + full_body_idx = b + link_offset + + R = wp.transform_get_rotation(body_link_pose[n, full_body_idx]) + c_world = wp.quat_rotate(R, body_com_pos_b[n, full_body_idx]) + + v_com = wp.vec3(src[n, b, 0, dof], src[n, b, 1, dof], src[n, b, 2, dof]) + omega = wp.vec3(src[n, b, 3, dof], src[n, b, 4, dof], src[n, b, 5, dof]) + + v_origin = v_com - wp.cross(omega, c_world) + + dst[n, b, 0, dof] = v_origin[0] + dst[n, b, 1, dof] = v_origin[1] + dst[n, b, 2, dof] = v_origin[2] + dst[n, b, 3, dof] = omega[0] + dst[n, b, 4, dof] = omega[1] + dst[n, b, 5, dof] = omega[2] diff --git a/source/isaaclab_physx/isaaclab_physx/assets/rigid_object_collection/rigid_object_collection.py b/source/isaaclab_physx/isaaclab_physx/assets/rigid_object_collection/rigid_object_collection.py index 6d07ddbf1bc1..2031ded53b2f 100644 --- a/source/isaaclab_physx/isaaclab_physx/assets/rigid_object_collection/rigid_object_collection.py +++ b/source/isaaclab_physx/isaaclab_physx/assets/rigid_object_collection/rigid_object_collection.py @@ -81,8 +81,9 @@ def __init__(self, cfg: RigidObjectCollectionCfg): for rigid_body_cfg in self.cfg.rigid_objects.values(): # spawn the asset if rigid_body_cfg.spawn is not None: + spawn_path = rigid_body_cfg.spawn.spawn_path or rigid_body_cfg.prim_path rigid_body_cfg.spawn.func( - rigid_body_cfg.prim_path, + spawn_path, rigid_body_cfg.spawn, translation=rigid_body_cfg.init_state.pos, orientation=rigid_body_cfg.init_state.rot, diff --git a/source/isaaclab_physx/isaaclab_physx/assets/surface_gripper/surface_gripper.py b/source/isaaclab_physx/isaaclab_physx/assets/surface_gripper/surface_gripper.py index 590289feb659..6662582dac7c 100644 --- a/source/isaaclab_physx/isaaclab_physx/assets/surface_gripper/surface_gripper.py +++ b/source/isaaclab_physx/isaaclab_physx/assets/surface_gripper/surface_gripper.py @@ -443,9 +443,6 @@ def _initialize_impl(self) -> None: Use `--device cpu` to run the simulation on CPU. """ - enable_extension("isaacsim.robot.surface_gripper") - from isaacsim.robot.surface_gripper import GripperView - # Check that we are using the CPU backend. if self._device != "cpu": raise Exception( @@ -453,6 +450,9 @@ def _initialize_impl(self) -> None: " `--device cpu` to run the simulation on CPU." ) + enable_extension("isaacsim.robot.surface_gripper") + from isaacsim.robot.surface_gripper import GripperView + # obtain the first prim in the regex expression (all others are assumed to be a copy of this) template_prim = sim_utils.find_first_matching_prim(self._cfg.prim_path) if template_prim is None: diff --git a/source/isaaclab_physx/isaaclab_physx/cloner/physx_replicate.py b/source/isaaclab_physx/isaaclab_physx/cloner/physx_replicate.py index d90d413bffa2..dcc5cc6d9677 100644 --- a/source/isaaclab_physx/isaaclab_physx/cloner/physx_replicate.py +++ b/source/isaaclab_physx/isaaclab_physx/cloner/physx_replicate.py @@ -5,6 +5,8 @@ from __future__ import annotations +from collections.abc import Sequence + import torch from omni.physx import get_physx_replicator_interface @@ -13,8 +15,8 @@ def physx_replicate( stage: Usd.Stage, - sources: list[str], # e.g. ["/World/Template/A", "/World/Template/B"] - destinations: list[str], # e.g. ["/World/envs/env_{}/Robot", "/World/envs/env_{}/Object"] + sources: Sequence[str], # e.g. ["/World/Template/A", "/World/Template/B"] + destinations: Sequence[str], # e.g. ["/World/envs/env_{}/Robot", "/World/envs/env_{}/Object"] env_ids: torch.Tensor, # env_ids mapping: torch.Tensor, # (num_sources, num_envs) bool; True -> place sources[i] into world=j positions: torch.Tensor | None = None, diff --git a/source/isaaclab_physx/isaaclab_physx/physics/physx_manager.py b/source/isaaclab_physx/isaaclab_physx/physics/physx_manager.py index 4fb55064f072..bb23bc130ad5 100644 --- a/source/isaaclab_physx/isaaclab_physx/physics/physx_manager.py +++ b/source/isaaclab_physx/isaaclab_physx/physics/physx_manager.py @@ -21,6 +21,7 @@ from typing import TYPE_CHECKING, Any, ClassVar import torch +import warp as wp import carb import omni.kit.app @@ -28,10 +29,10 @@ import omni.physx import omni.timeline import omni.usd -from pxr import Sdf, UsdUtils +from pxr import Sdf, Usd, UsdPhysics, UsdUtils import isaaclab.sim as sim_utils -from isaaclab.physics import CallbackHandle, PhysicsEvent, PhysicsManager +from isaaclab.physics import CallbackHandle, PhysicsEvent, PhysicsManager, SceneDataBackend, SceneDataFormat from isaaclab.utils.string import to_camel_case if TYPE_CHECKING: @@ -153,6 +154,71 @@ def _update_usda_start_time(self, file_path: str) -> None: f.write(content) +class PhysxSceneDataBackend(SceneDataBackend): + def __init__(self): + self._simulation_view: omni.physics.tensors.SimulationView | None = None + self._rigid_body_view: omni.physics.tensors.RigidBodyView | None = None + self._scene_data = SceneDataFormat.Transform() + + @property + def simulation_view(self) -> omni.physics.tensors.SimulationView | None: + return self._simulation_view + + @simulation_view.setter + def simulation_view(self, simulation_view: omni.physics.tensors.SimulationView | None): + self._simulation_view = simulation_view + self._rigid_body_view = None + + def get_rigid_body_view(self) -> omni.physics.tensors.RigidBodyView | None: + """Lazily create a rigid body view covering all rigid bodies in the scene. + + Discovers rigid body prims by traversing the USD stage and converts + per-environment paths (``/World/envs/env_N/...``) into wildcard + patterns so a single PhysX view covers every environment instance. + """ + if self._rigid_body_view is not None: + return self._rigid_body_view + + if self._simulation_view is None: + return None + + stage: Usd.Stage = omni.usd.get_context().get_stage() + if stage is None: + return None + + patterns: set[str] = set() + for prim in stage.Traverse(): + if prim.HasAPI(UsdPhysics.RigidBodyAPI): + patterns.add(re.sub(r"/World/envs/env_\d+", "/World/envs/env_*", prim.GetPath().pathString)) + + if not patterns: + return None + + self._rigid_body_view = self._simulation_view.create_rigid_body_view(list(patterns)) + return self._rigid_body_view + + @property + def transforms(self) -> SceneDataFormat.Transform: + """Return the current PhysX rigid body transforms as :class:`SceneDataFormat.Transform`.""" + if view := self.get_rigid_body_view(): + self._scene_data.transforms = view.get_transforms().view(wp.transformf) + return self._scene_data + + @property + def transform_count(self) -> int: + """Return the number of rigid body transforms in the PhysX sim.""" + if view := self.get_rigid_body_view(): + return view.count + return 0 + + @property + def transform_paths(self) -> list[str]: + """Return the prim paths for each rigid body transform.""" + if view := self.get_rigid_body_view(): + return list(view.prim_paths) + return [] + + class PhysxManager(PhysicsManager): """Manages PhysX physics simulation lifecycle. @@ -165,6 +231,7 @@ class PhysxManager(PhysicsManager): _event_bus: ClassVar[carb.eventdispatcher.IEventDispatcher] = carb.eventdispatcher.get_eventdispatcher() _physx: ClassVar[omni.physx.IPhysx] = omni.physx.get_physx_interface() _physx_sim: ClassVar[omni.physx.IPhysxSimulation] = omni.physx.get_physx_simulation_interface() + _scene_data_backend: ClassVar[PhysxSceneDataBackend | None] = None _view: ClassVar[omni.physics.tensors.SimulationView | None] = None _view_warp: ClassVar[omni.physics.tensors.SimulationView | None] = None @@ -210,6 +277,7 @@ def initialize(cls, sim_context: SimulationContext) -> None: cls._configure_physics() cls._load_fabric() cls._anim_recorder = AnimationRecorder(sim_context) + cls._scene_data_backend = PhysxSceneDataBackend() # force update cycle to apply dt sim = PhysicsManager._sim @@ -248,6 +316,11 @@ def forward(cls) -> None: cls._view.update_articulations_kinematic() cls._update_fabric(0.0, 0.0) + @classmethod + def get_scene_data_backend(cls) -> SceneDataBackend: + """Return the SceneDataBackend for the SceneDataProvider.""" + return cls._scene_data_backend + @classmethod def step(cls) -> None: """Step the physics simulation.""" @@ -689,6 +762,7 @@ def _warmup_and_create_views(cls) -> None: # Final update after view creation cls._physx.update_simulation(cls.get_physics_dt(), 0.0) cls._view_created = True + cls._scene_data_backend.simulation_view = cls._view cls._event_bus.dispatch_event(IsaacEvents.SIMULATION_VIEW_CREATED.value, payload={}) cls.dispatch_event(PhysicsEvent.PHYSICS_READY, payload={}) diff --git a/source/isaaclab_physx/isaaclab_physx/scene_data_providers/physx_scene_data_provider.py b/source/isaaclab_physx/isaaclab_physx/scene_data_providers/physx_scene_data_provider.py deleted file mode 100644 index 9a88660da498..000000000000 --- a/source/isaaclab_physx/isaaclab_physx/scene_data_providers/physx_scene_data_provider.py +++ /dev/null @@ -1,727 +0,0 @@ -# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). -# All rights reserved. -# -# SPDX-License-Identifier: BSD-3-Clause - -"""PhysX scene data provider for Omni/PhysX backend.""" - -from __future__ import annotations - -import logging -import re -import time -from collections import deque -from typing import Any - -import torch -import warp as wp - -from pxr import UsdGeom, UsdPhysics - -from isaaclab.physics.base_scene_data_provider import BaseSceneDataProvider -from isaaclab.sim.utils.newton_model_utils import replace_newton_shape_colors - -logger = logging.getLogger(__name__) - - -@wp.kernel(enable_backward=False) -def _set_body_q_kernel( - positions: wp.array(dtype=wp.vec3), - orientations: wp.array(dtype=wp.quatf), - body_q: wp.array(dtype=wp.transformf), -): - """Write pose arrays into Newton ``body_q`` in one-to-one index order.""" - i = wp.tid() - body_q[i] = wp.transformf(positions[i], orientations[i]) - - -class PhysxSceneDataProvider(BaseSceneDataProvider): - """Scene data provider for Omni PhysX backend. - - Supports: - - body poses via PhysX tensor views, with FrameView fallback - - camera poses & intrinsics - - USD stage handles - - Newton model/state (built locally from the scene's per-group :class:`ClonePlan` map - when required) - """ - - # ---- Environment discovery / metadata ------------------------------------------------- - - def get_num_envs(self) -> int: - """Return env count from stage discovery, cached once available.""" - if self._num_envs is not None and self._num_envs > 0: - return self._num_envs - discovered_num_envs = self._determine_num_envs_in_scene() - if discovered_num_envs > 0: - self._num_envs = discovered_num_envs - return discovered_num_envs - return 0 - - def _determine_num_envs_in_scene(self) -> int: - """Infer env count from /World/envs/env_ prims.""" - if self._stage is None: - return 0 - - max_env_id = -1 - env_name_re = re.compile(r"^env_(\d+)$") - - envs_root = self._stage.GetPrimAtPath("/World/envs") - if envs_root.IsValid(): - for child in envs_root.GetChildren(): - match = env_name_re.match(child.GetName()) - if match: - max_env_id = max(max_env_id, int(match.group(1))) - return max_env_id + 1 if max_env_id >= 0 else 0 - - def __init__(self, stage, simulation_context) -> None: - """Initialize the PhysX scene data provider. - - Args: - stage: USD stage handle. - simulation_context: Active simulation context. - """ - from isaaclab_physx.physics import PhysxManager as SimulationManager - - self._simulation_context = simulation_context - self._stage = stage - self._physics_sim_view = SimulationManager.get_physics_sim_view() - self._rigid_body_view = None - self._xform_views: dict[str, Any] = {} - self._xform_view_failures: set[str] = set() - self._view_body_index_map: dict[str, list[int]] = {} - self._warned_once: set[str] = set() - - # Single source of truth: discovered from stage and cached once available. - self._num_envs: int | None = None - - # Determine if newton model sync is required for selected renderers and visualizers - requirements = self._simulation_context.get_scene_data_requirements() - self._needs_newton_sync = bool(requirements.requires_newton_model) - - # Fixed metadata for visualizers. get_metadata() returns this plus num_envs so visualizers - # can .get("num_envs", 0), .get("physics_backend", ...) etc. without the provider exposing many methods. - self._metadata = {"physics_backend": "omni"} - if self._stage is None: - raise RuntimeError( - "[PhysxSceneDataProvider] USD stage is None and not available from simulation_context. " - "Ensure the simulation context has a valid stage when using OV/Newton/Rerun/Viser visualizers." - ) - self._num_envs_at_last_newton_build: int | None = None # for _refresh_newton_model_if_needed - - self._device = getattr(self._simulation_context, "device", "cuda:0") - self._newton_model = None - self._newton_state = None - self._rigid_body_paths: list[str] = [] - # Paths used to create PhysX views. May include articulation roots for coverage. - self._rigid_body_view_paths: list[str] = [] - - # Reused pose buffers (MR perf): avoid per-call allocations in _read_poses_from_best_source. - self._pose_buf_num_bodies = 0 - self._positions_buf = None - self._orientations_buf = None - self._covered_buf = None - self._xform_mask_buf = None - # View index order as device tensors for vectorized scatter in _apply_view_poses. - self._view_order_tensors: dict[str, Any] = {} - # Last load outcome (tests / debug): "built" | "missing" | "error". - self._last_newton_model_build_source: str | None = None - self._last_newton_model_build_elapsed_ms: float | None = None - - if self._needs_newton_sync: - self._build_newton_model_from_clone_plans() - self._setup_rigid_body_view() - - # ---- Newton model + PhysX view setup -------------------------------------------------- - - def _wildcard_env_paths(self, paths: list[str]) -> list[str]: - """Convert /World/envs/env_0 paths to a wildcard pattern when possible.""" - wildcard_paths = [ - path.replace("/World/envs/env_0", "/World/envs/env_*") for path in paths if "/World/envs/env_0" in path - ] - return list(dict.fromkeys(wildcard_paths)) if wildcard_paths else paths - - def _refresh_newton_model_if_needed(self) -> None: - """Reload Newton model/state and PhysX views when the discovered env count changes.""" - num_envs = self.get_num_envs() - if num_envs <= 0: - return - - needs_rebuild = self._newton_model is None or self._newton_state is None - needs_rebuild = needs_rebuild or (self._num_envs_at_last_newton_build != num_envs) - if needs_rebuild: - self._build_newton_model_from_clone_plans() - self._setup_rigid_body_view() - - def _build_newton_model_from_clone_plans(self) -> None: - """Build Newton model and state from the scene's per-group :class:`ClonePlan` map. - - Reads plans :meth:`InteractiveScene.clone_environments` publishes on - :class:`SimulationContext`, derives the flat ``(sources, destinations, mask)`` shape - :func:`isaaclab_newton.cloner.newton_visualizer_prebuild` expects, and caches the - resulting model/state. Per-prototype source paths recover as - ``dest_template.format()``; per-env positions are - read off ``xformOp:translate`` on the env-level prims derived from the same template. - Pre-condition violations raise :class:`RuntimeError` (logged as ``"missing"``); - ``isaaclab_newton`` being absent (optional dep) maps to ``"missing"`` via the - import's own exception types; unexpected failures fall through to ``"error"``. - """ - start_t = time.perf_counter() - source = "missing" - try: - plans = self._simulation_context.get_clone_plans() - if not plans: - raise RuntimeError("No clone plans on simulation context.") - from isaaclab_newton.cloner.newton_replicate import newton_visualizer_prebuild - - # Flatten per-group plans into one (sources, destinations, mask) bundle. Source - # paths recover via ``dest_template.format()``; - # all-False rows are dropped (possible when ``num_prototypes > num_envs``). - plan_list = list(plans.values()) - num_envs = plan_list[0].clone_mask.size(1) - if any(p.clone_mask.size(1) != num_envs for p in plan_list): - raise RuntimeError(f"Clone plans disagree on num_envs: {[p.clone_mask.size(1) for p in plan_list]}") - sources, destinations, mask_rows = [], [], [] - for p in plan_list: - for i in range(p.clone_mask.size(0)): - nz = p.clone_mask[i].nonzero(as_tuple=False) - if nz.numel() == 0: - continue - sources.append(p.dest_template.format(int(nz[0].item()))) - destinations.append(p.dest_template) - mask_rows.append(p.clone_mask[i : i + 1]) - if not sources: - raise RuntimeError("All clone-plan prototype rows are empty.") - mask = torch.cat(mask_rows, dim=0) - - # Env-level path template = dest_template up to the first ``{}``. Per-env world - # positions: xformOp:translate read off each env prim; missing prims fall through. - env_path_template = plan_list[0].dest_template.split("{}")[0] + "{}" - positions = torch.zeros((num_envs, 3), dtype=torch.float32, device=self._device) - for i in range(num_envs): - prim = self._stage.GetPrimAtPath(env_path_template.format(i)) - if prim.IsValid() and (v := prim.GetAttribute("xformOp:translate").Get()) is not None: - positions[i] = torch.tensor([v[0], v[1], v[2]], device=self._device) - - model, state = newton_visualizer_prebuild( - stage=self._stage, - sources=sources, - destinations=destinations, - env_ids=torch.arange(num_envs, dtype=torch.long, device=mask.device), - mapping=mask, - positions=positions, - device=self._device, - up_axis=UsdGeom.GetStageUpAxis(self._stage), - ) - if model is None or state is None: - raise RuntimeError("newton_visualizer_prebuild returned None.") - - self._newton_model, self._newton_state = model, state - replace_newton_shape_colors(self._newton_model, self._stage) - # Newton renamed ``*_key`` → ``*_label`` mid-development; fall back so we work either way. - # ``dict.fromkeys`` preserves order while deduping — articulation roots can overlap rigid bodies. - label_or_key = lambda kind: list(getattr(model, f"{kind}_label", None) or getattr(model, f"{kind}_key", [])) # noqa: E731 - self._rigid_body_paths = label_or_key("body") - self._rigid_body_view_paths = list(dict.fromkeys(self._rigid_body_paths + label_or_key("articulation"))) - # Reset cached views/buffers; rebuilt lazily by ``_setup_rigid_body_view``. - self._xform_views.clear() - self._view_order_tensors.clear() - self._view_body_index_map = {} - self._pose_buf_num_bodies = 0 - self._positions_buf = self._orientations_buf = self._covered_buf = self._xform_mask_buf = None - self._num_envs_at_last_newton_build = num_envs - source = "built" - except (ImportError, ModuleNotFoundError) as exc: - logger.warning("[PhysxSceneDataProvider] isaaclab_newton not available: %s", exc) - self._clear_newton_model_state() - except RuntimeError as exc: - logger.error("[PhysxSceneDataProvider] %s", exc) - self._clear_newton_model_state() - except Exception as exc: - source = "error" - logger.error("[PhysxSceneDataProvider] Failed to build Newton model from clone plans: %s", exc) - self._clear_newton_model_state() - finally: - self._last_newton_model_build_elapsed_ms = (time.perf_counter() - start_t) * 1000.0 - self._last_newton_model_build_source = source - logger.debug( - "[PhysxSceneDataProvider] Newton model build source=%s elapsed_ms=%.2f", - source, - self._last_newton_model_build_elapsed_ms, - ) - - def _clear_newton_model_state(self) -> None: - """Clear cached Newton model, state, and rigid-body path lists.""" - self._newton_model = None - self._newton_state = None - self._rigid_body_paths = [] - self._rigid_body_view_paths = [] - self._num_envs_at_last_newton_build = None - - def _setup_rigid_body_view(self) -> None: - """Create PhysX RigidBodyView from Newton's body paths. - - Uses body paths extracted from Newton model to create PhysX tensor API view - for reading rigid body transforms. - """ - if self._physics_sim_view is None: - return - paths = self._rigid_body_view_paths or self._rigid_body_paths - if not paths: - return - # Defensive: only pass true rigid-body prims into PhysX RigidBodyView. - # Some prebuilt artifacts carry articulation root paths for coverage, but - # those roots are not guaranteed to be rigid-body prims and can trip native - # view creation paths on some tasks. - rigid_paths: list[str] = [] - dropped_non_rigid = 0 - for path in paths: - prim = self._stage.GetPrimAtPath(path) if self._stage is not None else None - if prim and prim.IsValid() and prim.HasAPI(UsdPhysics.RigidBodyAPI): - rigid_paths.append(path) - else: - dropped_non_rigid += 1 - if not rigid_paths: - self._warn_once( - "rigid-view-no-rigid-paths", - "[PhysxSceneDataProvider] No rigid-body prim paths available for RigidBodyView creation.", - level=logging.WARNING, - ) - return - try: - paths_to_use = self._wildcard_env_paths(rigid_paths) - self._rigid_body_view = self._physics_sim_view.create_rigid_body_view(paths_to_use) - self._cache_view_index_map(self._rigid_body_view, "rigid_body_view") - except Exception as exc: - logger.warning(f"[PhysxSceneDataProvider] Failed to create RigidBodyView: {exc}") - self._rigid_body_view = None - - # ---- Pose/velocity read pipeline ------------------------------------------------------ - - def _warn_once(self, key: str, message: str, *args, level=logging.WARNING) -> None: - """Log a warning only once for a given key.""" - if key in self._warned_once: - return - self._warned_once.add(key) - logger.log(level, message, *args) - - def _get_view_world_poses(self, view: Any): - """Read world poses from a PhysX view.""" - if view is None: - return None, None - - result = view.get_transforms() - if isinstance(result, tuple) and len(result) == 2: - return result - if hasattr(result, "shape"): - return result[:, :3], result[:, 3:7] - - import warp as wp - - result_t = wp.to_torch(result) - return result_t[:, :3], result_t[:, 3:7] - - def _cache_view_index_map(self, view, key: str) -> None: - """Map PhysX view indices to Newton body_key ordering.""" - prim_paths = getattr(view, "prim_paths", None) - if not prim_paths or not self._rigid_body_paths: - return - - # Build map: (env_id, relative_path) -> view_index to align view order. - view_map: dict[tuple[int | None, str], int] = {} - for view_idx, path in enumerate(prim_paths): - env_id, rel = self._split_env_relative_path(path) - view_map[(env_id, rel)] = view_idx - - # Build reordering: newton_body_index -> view_index so we can scatter - # PhysX view outputs into Newton body ordering. - order: list[int | None] = [None] * len(self._rigid_body_paths) - for body_idx, path in enumerate(self._rigid_body_paths): - env_id, rel = self._split_env_relative_path(path) - view_idx = view_map.get((env_id, rel)) - if view_idx is None: - view_idx = view_map.get((None, rel)) # Try without env_id - order[body_idx] = view_idx - - if all(idx is not None for idx in order): - self._view_body_index_map[key] = order # type: ignore[arg-type] - # Cache as device tensor for vectorized scatter in _apply_view_poses. - import torch - - self._view_order_tensors[key] = torch.tensor(order, dtype=torch.long, device=self._device) - - def _split_env_relative_path(self, path: str) -> tuple[int | None, str]: - """Extract (env_id, relative_path) from a prim path.""" - match = re.search(r"/World/envs/env_(\d+)(/.*)", path) - return (int(match.group(1)), match.group(2)) if match else (None, path) - - def _get_view_velocities(self, view): - """Read linear/angular velocities from a PhysX view.""" - if view is None: - return None, None - - try: - # Canonical API for PhysX tensor views. - result = view.get_velocities() - if isinstance(result, tuple) and len(result) == 2: - return result - if hasattr(result, "shape") and result.shape[-1] == 6: - return result[..., :3], result[..., 3:6] - except (AttributeError, RuntimeError, TypeError) as exc: - logger.debug("[PhysxSceneDataProvider] get_velocities() unavailable/failed for %s: %s", type(view), exc) - return None, None - - def _apply_view_poses(self, view: Any, view_key: str, positions: Any, orientations: Any, covered: Any) -> int: - """Fill poses from a PhysX view for bodies not yet covered.""" - import torch - import warp as wp - - if view is None: - return 0 - - pos, quat = self._get_view_world_poses(view) - if pos is None or quat is None: - return 0 - - order = self._view_body_index_map.get(view_key) - if not order: - return 0 - - # Normalize returned arrays to torch tensors across backends (torch/warp/other). - if not isinstance(pos, torch.Tensor): - try: - pos = wp.to_torch(pos) - except Exception: - pos = torch.as_tensor(pos) - if not isinstance(quat, torch.Tensor): - try: - quat = wp.to_torch(quat) - except Exception: - quat = torch.as_tensor(quat) - - pos = pos.to(device=self._device, dtype=torch.float32) - quat = quat.to(device=self._device, dtype=torch.float32) - - # Vectorized scatter when we have a cached order tensor (view fully covers bodies). - order_t = self._view_order_tensors.get(view_key) - if order_t is not None: - uncovered_mask = ~covered - if uncovered_mask.any(): - newton_indices = uncovered_mask.nonzero(as_tuple=True)[0] - view_indices = order_t[newton_indices] - positions[newton_indices] = pos[view_indices] - orientations[newton_indices] = quat[view_indices] - covered[newton_indices] = True - return newton_indices.numel() - return 0 - - # Per-index path when the view does not fully cover bodies or the order cache is missing. - count = 0 - for newton_idx, view_idx in enumerate(order): - if view_idx is not None and not covered[newton_idx]: - positions[newton_idx] = pos[view_idx] - orientations[newton_idx] = quat[view_idx] - covered[newton_idx] = True - count += 1 - - return count - - def _apply_xform_poses(self, positions: Any, orientations: Any, covered: Any, xform_mask: Any) -> int: - """Fill remaining body poses using ``XformPrimView`` for prims not covered by the rigid-body view.""" - import torch - - from isaaclab.sim.views import FrameView - - uncovered = torch.where(~covered)[0].cpu().tolist() - if not uncovered: - return 0 - - # Query each uncovered prim path directly from USD. - count = 0 - for idx in uncovered: - path = self._rigid_body_paths[idx] - try: - if path not in self._xform_views: - self._xform_views[path] = FrameView( - path, device=self._device, stage=self._stage, validate_xform_ops=False - ) - - pos_w, quat_w = self._xform_views[path].get_world_poses() - if pos_w is not None and quat_w is not None: - positions[idx] = pos_w.torch.to(device=self._device, dtype=torch.float32).squeeze() - orientations[idx] = quat_w.torch.to(device=self._device, dtype=torch.float32).squeeze() - covered[idx] = True - xform_mask[idx] = True - count += 1 - except Exception: - self._xform_view_failures.add(path) - continue - - if len(self._xform_view_failures) > 0: - self._warn_once( - "xform-fallback-failures", - "[PhysxSceneDataProvider] XformPrimView reads failed for %d body paths.", - len(self._xform_view_failures), - level=logging.DEBUG, - ) - return count - - def _convert_xform_quats(self, orientations: Any, xform_mask: Any) -> Any: - """Return quaternions in xyzw convention. - - PhysX views, FrameView, and resolve_prim_pose() in Isaac Lab all use xyzw. - Keeping this helper as a no-op preserves a single conversion point if conventions - ever diverge again. - """ - return orientations - - def _read_poses_from_best_source(self) -> tuple[Any, Any, str, Any] | None: - """Merge pose data from rigid-body and xform views.""" - if self._newton_state is None or not self._rigid_body_paths: - return None - - import torch - - num_bodies = len(self._rigid_body_paths) - if num_bodies != self._newton_state.body_q.shape[0]: - self._warn_once( - "body-count-mismatch", - "[PhysxSceneDataProvider] Body count mismatch: body_key=%d, state=%d", - num_bodies, - int(self._newton_state.body_q.shape[0]), - ) - return None - - # Reuse buffers when size unchanged to avoid per-call allocations (MR perf). - if num_bodies != self._pose_buf_num_bodies or self._positions_buf is None: - self._pose_buf_num_bodies = num_bodies - self._positions_buf = torch.zeros((num_bodies, 3), dtype=torch.float32, device=self._device) - self._orientations_buf = torch.zeros((num_bodies, 4), dtype=torch.float32, device=self._device) - self._covered_buf = torch.zeros(num_bodies, dtype=torch.bool, device=self._device) - self._xform_mask_buf = torch.zeros(num_bodies, dtype=torch.bool, device=self._device) - else: - self._covered_buf.zero_() - self._xform_mask_buf.zero_() - - positions = self._positions_buf - orientations = self._orientations_buf - covered = self._covered_buf - xform_mask = self._xform_mask_buf - - rigid_count = self._apply_view_poses(self._rigid_body_view, "rigid_body_view", positions, orientations, covered) - xform_count = self._apply_xform_poses(positions, orientations, covered, xform_mask) - if rigid_count == 0: - self._warn_once( - "rigid-source-unused", - ( - "[PhysxSceneDataProvider] RigidBodyView returned no transforms; " - "filled from XformPrimView where needed." - ), - level=logging.DEBUG, - ) - - if not covered.all(): - self._warn_once( - "pose-read-incomplete", - "[PhysxSceneDataProvider] Failed to read %d/%d body poses.", - int((~covered).sum().item()), - num_bodies, - ) - return None - - active = sum([rigid_count > 0, xform_count > 0]) - source = ( - "merged" if active > 1 else ("rigid_body_view" if rigid_count else "xform_view" if xform_count else "none") - ) - return positions, orientations, source, xform_mask - - def _get_set_body_q_kernel(self): - """Return module-level Warp kernel for writing transforms to Newton state.""" - return _set_body_q_kernel - - # ---- Newton state sync ---------------------------------------------------------------- - - def update(self) -> None: - """Sync PhysX transforms into the full Newton state (one kernel launch).""" - if not self._needs_newton_sync or self._newton_state is None: - return - - try: - # Re-check env count in case stage population completed after provider construction. - self._refresh_newton_model_if_needed() - - result = self._read_poses_from_best_source() - if result is None: - return - - positions, orientations, _, xform_mask = result - orientations_xyzw = self._convert_xform_quats(orientations.reshape(-1, 4), xform_mask) - - positions_wp = wp.from_torch(positions.reshape(-1, 3), dtype=wp.vec3) - orientations_wp = wp.from_torch(orientations_xyzw, dtype=wp.quatf) - - set_body_q = self._get_set_body_q_kernel() - if set_body_q is None or positions_wp.shape[0] != self._newton_state.body_q.shape[0]: - return - wp.launch( - set_body_q, - dim=positions_wp.shape[0], - inputs=[positions_wp, orientations_wp, self._newton_state.body_q], - device=self._device, - ) - except Exception as exc: - self._warn_once( - "newton-sync-update-failed", - "[PhysxSceneDataProvider] Failed to sync transforms to Newton state: %s", - exc, - ) - - def get_newton_model(self) -> Any | None: - """Return Newton model when sync is enabled. - - Returns: - Newton model object, or ``None`` when unavailable. - """ - return self._newton_model if self._needs_newton_sync else None - - def get_newton_state(self) -> Any | None: - """Return full Newton state when sync is enabled.""" - if not self._needs_newton_sync or self._newton_state is None: - return None - return self._newton_state - - # ---- Public provider API --------------------------------------------------------------- - - def get_usd_stage(self) -> Any: - """Return USD stage handle. - - Returns: - USD stage object. - """ - if self._stage is not None: - return self._stage - return getattr(self._simulation_context, "stage", None) - - def get_camera_transforms(self) -> dict[str, Any] | None: - """Return per-camera, per-environment transforms. - - Returns: - Dictionary containing camera order, positions, orientations, and environment count, - or ``None`` when unavailable. - """ - if self._stage is None: - return None - - import isaaclab.sim as isaaclab_sim - - env_pattern = re.compile(r"(?P/World/envs/env_)(?P\d+)(?P/.*)") - shared_paths: list[str] = [] - instances: dict[str, list[tuple[int, str]]] = {} - num_envs = -1 - - # Breadth-first walk so we discover camera prims across the full stage. - stage_prims = deque([self._stage.GetPseudoRoot()]) - while stage_prims: - prim = stage_prims.popleft() - prim_path = prim.GetPath().pathString - - world_id = 0 - template_path = prim_path - if match := env_pattern.match(prim_path): - # Normalize per-env path to a shared template key (env_%d/...) so - # visualizers can query one camera path for all env instances. - world_id = int(match.group("id")) - template_path = match.group("root") + "%d" + match.group("path") - if world_id > num_envs: - num_envs = world_id - - imageable = UsdGeom.Imageable(prim) - if imageable and imageable.ComputeVisibility() == UsdGeom.Tokens.invisible: - continue - - if prim.IsA(UsdGeom.Camera): - if template_path not in instances: - instances[template_path] = [] - instances[template_path].append((world_id, prim_path)) - if template_path not in shared_paths: - shared_paths.append(template_path) - - if hasattr(UsdGeom, "TraverseInstanceProxies"): - child_prims = prim.GetFilteredChildren(UsdGeom.TraverseInstanceProxies()) - else: - child_prims = prim.GetChildren() - if child_prims: - stage_prims.extend(child_prims) - - num_envs += 1 - positions: list[list[list[float] | None]] = [] - orientations: list[list[list[float] | None]] = [] - - for template_path in shared_paths: - per_world_pos: list[list[float] | None] = [None] * num_envs - per_world_ori: list[list[float] | None] = [None] * num_envs - for world_id, prim_path in instances.get(template_path, []): - if world_id < 0 or world_id >= num_envs: - continue - prim = self._stage.GetPrimAtPath(prim_path) - if not prim.IsValid(): - continue - pos, ori = isaaclab_sim.resolve_prim_pose(prim) - per_world_pos[world_id] = [float(pos[0]), float(pos[1]), float(pos[2])] - per_world_ori[world_id] = [float(ori[0]), float(ori[1]), float(ori[2]), float(ori[3])] - positions.append(per_world_pos) - orientations.append(per_world_ori) - - return {"order": shared_paths, "positions": positions, "orientations": orientations, "num_envs": num_envs} - - def get_metadata(self) -> dict[str, Any]: - """Return provider metadata for visualizers and renderers. - - Returns: - Metadata dictionary with backend and environment count. - """ - out = dict(self._metadata) - out["num_envs"] = self.get_num_envs() - return out - - def get_transforms(self) -> dict[str, Any] | None: - """Return merged body transforms from available PhysX views. - - Returns: - Dictionary with positions/orientations, or ``None`` when unavailable. - """ - try: - result = self._read_poses_from_best_source() - if result is None: - return None - - positions, orientations, _, xform_mask = result - orientations_xyzw = self._convert_xform_quats(orientations, xform_mask) - return {"positions": positions, "orientations": orientations_xyzw} - except Exception as exc: - self._warn_once( - "get-transforms-failed", - "[PhysxSceneDataProvider] get_transforms() failed: %s", - exc, - ) - return None - - def get_velocities(self) -> dict[str, Any] | None: - """Return linear/angular velocities from available PhysX views. - - Returns: - Dictionary with linear/angular velocities, or ``None`` when unavailable. - """ - for source, view in (("rigid_body_view", self._rigid_body_view),): - linear, angular = self._get_view_velocities(view) - if linear is not None and angular is not None: - return {"linear": linear, "angular": angular, "source": source} - return None - - def get_contacts(self) -> dict[str, Any] | None: - """Return contact data for PhysX provider. - - Returns: - ``None`` because contacts are not currently implemented in this provider. - """ - return None diff --git a/source/isaaclab_physx/isaaclab_physx/sensors/pva/pva.py b/source/isaaclab_physx/isaaclab_physx/sensors/pva/pva.py index 32f4549e416d..6f2ad9a70118 100644 --- a/source/isaaclab_physx/isaaclab_physx/sensors/pva/pva.py +++ b/source/isaaclab_physx/isaaclab_physx/sensors/pva/pva.py @@ -284,18 +284,25 @@ def _debug_vis_callback(self, event): arrow_scale = torch.tensor(default_scale, device=self.device).repeat(self._data.lin_acc_b.torch.shape[0], 1) # get up axis of current stage up_axis = UsdGeom.GetStageUpAxis(self.stage) - # arrow-direction + # arrow-direction; filter out bodies with effectively zero accel (no defined direction) pos_w_torch = self._data.pos_w.torch - quat_w_torch = self._data.quat_w.torch - lin_acc_b_torch = self._data.lin_acc_b.torch - quat_opengl = math_utils.quat_from_matrix( - math_utils.create_rotation_matrix_from_view( - pos_w_torch, - pos_w_torch + math_utils.quat_apply(quat_w_torch, lin_acc_b_torch), - up_axis=up_axis, - device=self._device, - ) + accel_w = math_utils.quat_apply(self._data.quat_w.torch, self._data.lin_acc_b.torch) + valid_indices = (torch.linalg.norm(accel_w, dim=-1) > 1e-5).nonzero(as_tuple=True)[0] + if valid_indices.numel() == 0: + return + pos_filtered = pos_w_torch.index_select(0, valid_indices) + accel_filtered = accel_w.index_select(0, valid_indices) + rotation_matrix = math_utils.create_rotation_matrix_from_view( + pos_filtered, + pos_filtered + accel_filtered, + up_axis=up_axis, + device=self._device, ) + quat_opengl = math_utils.quat_from_matrix(rotation_matrix) quat_w = math_utils.convert_camera_frame_orientation_convention(quat_opengl, "opengl", "world") # display markers - self.acceleration_visualizer.visualize(base_pos_w, quat_w, arrow_scale) + self.acceleration_visualizer.visualize( + base_pos_w.index_select(0, valid_indices), + quat_w, + arrow_scale.index_select(0, valid_indices), + ) diff --git a/source/isaaclab_physx/isaaclab_physx/sim/__init__.pyi b/source/isaaclab_physx/isaaclab_physx/sim/__init__.pyi index abc8d0087afd..9a522d96ff8f 100644 --- a/source/isaaclab_physx/isaaclab_physx/sim/__init__.pyi +++ b/source/isaaclab_physx/isaaclab_physx/sim/__init__.pyi @@ -7,6 +7,10 @@ __all__ = [ "define_deformable_body_properties", "modify_deformable_body_properties", "DeformableBodyPropertiesCfg", + "JointDrivePropertiesCfg", + "PhysxJointDrivePropertiesCfg", + "PhysxRigidBodyPropertiesCfg", + "RigidBodyPropertiesCfg", "DeformableObjectSpawnerCfg", "spawn_deformable_body_material", "DeformableBodyMaterialCfg", @@ -17,7 +21,11 @@ __all__ = [ from .schemas import ( define_deformable_body_properties, modify_deformable_body_properties, - DeformableBodyPropertiesCfg + DeformableBodyPropertiesCfg, + JointDrivePropertiesCfg, + PhysxJointDrivePropertiesCfg, + PhysxRigidBodyPropertiesCfg, + RigidBodyPropertiesCfg, ) from .spawners import ( DeformableObjectSpawnerCfg, diff --git a/source/isaaclab_physx/isaaclab_physx/sim/schemas/__init__.pyi b/source/isaaclab_physx/isaaclab_physx/sim/schemas/__init__.pyi index bb0e51a19b4b..6d2a05bf803c 100644 --- a/source/isaaclab_physx/isaaclab_physx/sim/schemas/__init__.pyi +++ b/source/isaaclab_physx/isaaclab_physx/sim/schemas/__init__.pyi @@ -6,11 +6,63 @@ __all__ = [ "define_deformable_body_properties", "modify_deformable_body_properties", + "ArticulationRootPropertiesCfg", + "CollisionPropertiesCfg", + "ConvexDecompositionPropertiesCfg", + "ConvexHullPropertiesCfg", "DeformableBodyPropertiesCfg", + "FixedTendonPropertiesCfg", + "JointDrivePropertiesCfg", + "MeshCollisionPropertiesCfg", + "PhysxArticulationRootPropertiesCfg", + "PhysxCollisionPropertiesCfg", + "PhysXCollisionPropertiesCfg", + "PhysxConvexDecompositionPropertiesCfg", + "PhysxConvexHullPropertiesCfg", + "PhysxDeformableCollisionPropertiesCfg", + "PhysxFixedTendonPropertiesCfg", + "PhysxJointDrivePropertiesCfg", + "PhysxRigidBodyPropertiesCfg", + "PhysxSDFMeshPropertiesCfg", + "PhysxSpatialTendonPropertiesCfg", + "PhysxTriangleMeshPropertiesCfg", + "PhysxTriangleMeshSimplificationPropertiesCfg", + "RigidBodyPropertiesCfg", + "SDFMeshPropertiesCfg", + "SpatialTendonPropertiesCfg", + "TriangleMeshPropertiesCfg", + "TriangleMeshSimplificationPropertiesCfg", ] from .schemas import ( define_deformable_body_properties, modify_deformable_body_properties, ) -from .schemas_cfg import DeformableBodyPropertiesCfg +from .schemas_cfg import ( + ArticulationRootPropertiesCfg, + CollisionPropertiesCfg, + ConvexDecompositionPropertiesCfg, + ConvexHullPropertiesCfg, + DeformableBodyPropertiesCfg, + FixedTendonPropertiesCfg, + JointDrivePropertiesCfg, + MeshCollisionPropertiesCfg, + PhysxArticulationRootPropertiesCfg, + PhysxCollisionPropertiesCfg, + PhysXCollisionPropertiesCfg, + PhysxConvexDecompositionPropertiesCfg, + PhysxConvexHullPropertiesCfg, + PhysxDeformableCollisionPropertiesCfg, + PhysxFixedTendonPropertiesCfg, + PhysxJointDrivePropertiesCfg, + PhysxRigidBodyPropertiesCfg, + PhysxSDFMeshPropertiesCfg, + PhysxSpatialTendonPropertiesCfg, + PhysxTriangleMeshPropertiesCfg, + PhysxTriangleMeshSimplificationPropertiesCfg, + RigidBodyPropertiesCfg, + SDFMeshPropertiesCfg, + SpatialTendonPropertiesCfg, + TriangleMeshPropertiesCfg, + TriangleMeshSimplificationPropertiesCfg, +) diff --git a/source/isaaclab_physx/isaaclab_physx/sim/schemas/schemas_cfg.py b/source/isaaclab_physx/isaaclab_physx/sim/schemas/schemas_cfg.py index 6579fee6356a..e6bdea28d24e 100644 --- a/source/isaaclab_physx/isaaclab_physx/sim/schemas/schemas_cfg.py +++ b/source/isaaclab_physx/isaaclab_physx/sim/schemas/schemas_cfg.py @@ -6,7 +6,16 @@ from __future__ import annotations import dataclasses - +import warnings +from typing import ClassVar + +from isaaclab.sim.schemas.schemas_cfg import ( + ArticulationRootBaseCfg, + CollisionBaseCfg, + JointDriveBaseCfg, + MeshCollisionBaseCfg, + RigidBodyBaseCfg, +) from isaaclab.utils import configclass @@ -110,12 +119,19 @@ class PhysXDeformableBodyPropertiesCfg: @configclass -class PhysXCollisionPropertiesCfg: +class PhysxDeformableCollisionPropertiesCfg: """PhysX-specific collision properties for a deformable body. These properties are set with the prefix ``physxCollision:``. See the PhysX documentation for more information on the available properties. + + .. note:: + This class is distinct from + :class:`~isaaclab_physx.sim.schemas.PhysxCollisionPropertiesCfg` (lowercase x), + which is the rigid-body collision cfg layered on + :class:`~isaaclab.sim.schemas.CollisionBaseCfg`. This class is used internally + as a base of :class:`DeformableBodyPropertiesCfg`. """ contact_offset: float | None = None @@ -135,9 +151,30 @@ class PhysXCollisionPropertiesCfg: """ +@configclass +class PhysXCollisionPropertiesCfg(PhysxDeformableCollisionPropertiesCfg): + """Deprecated: use :class:`PhysxDeformableCollisionPropertiesCfg`. + + .. deprecated:: 4.6.23 + ``PhysXCollisionPropertiesCfg`` (capital X) was renamed to + :class:`PhysxDeformableCollisionPropertiesCfg` to clear the namespace for the + new rigid-body :class:`PhysxCollisionPropertiesCfg` (lowercase x). The capital-X + name is preserved as a deprecation alias and is scheduled for removal in 5.0. + """ + + def __post_init__(self): + warnings.warn( + "'PhysXCollisionPropertiesCfg' (capital X) is deprecated and will be removed in 5.0." + " Use 'isaaclab_physx.sim.schemas.PhysxDeformableCollisionPropertiesCfg' instead.", + DeprecationWarning, + stacklevel=2, + ) + super().__post_init__() + + @configclass class DeformableBodyPropertiesCfg( - OmniPhysicsPropertiesCfg, PhysXDeformableBodyPropertiesCfg, PhysXCollisionPropertiesCfg + OmniPhysicsPropertiesCfg, PhysXDeformableBodyPropertiesCfg, PhysxDeformableCollisionPropertiesCfg ): """Properties to apply to a deformable body. @@ -158,6 +195,716 @@ class DeformableBodyPropertiesCfg( _property_prefix: dict[str, list[str]] = { "omniphysics": [field.name for field in dataclasses.fields(OmniPhysicsPropertiesCfg)], "physxDeformableBody": [field.name for field in dataclasses.fields(PhysXDeformableBodyPropertiesCfg)], - "physxCollision": [field.name for field in dataclasses.fields(PhysXCollisionPropertiesCfg)], + "physxCollision": [field.name for field in dataclasses.fields(PhysxDeformableCollisionPropertiesCfg)], } """Mapping between the property prefixes and the properties that fall under each prefix.""" + + +@configclass +class PhysxRigidBodyPropertiesCfg(RigidBodyBaseCfg): + """PhysX-specific rigid body properties. + + Extends :class:`~isaaclab.sim.schemas.RigidBodyBaseCfg` with properties from the `PhysxRigidBodyAPI`_ schema. + + See :meth:`~isaaclab.sim.schemas.modify_rigid_body_properties` for more information. + + .. note:: + If the values are None, they are not modified. This is useful when you want to set only a subset of + the properties and leave the rest as-is. + + .. _PhysxRigidBodyAPI: https://docs.omniverse.nvidia.com/kit/docs/omni_usd_schema_physics/104.2/class_physx_schema_physx_rigid_body_a_p_i.html + """ + + # PhysX-specific fields below all live under the ``PhysxRigidBodyAPI`` schema's + # ``physxRigidBody:*`` namespace. The ``disable_gravity`` field on the base remains + # routed via ``_usd_field_exceptions`` (inherited). + _usd_applied_schema: ClassVar[str | None] = "PhysxRigidBodyAPI" + _usd_namespace: ClassVar[str | None] = "physxRigidBody" + + linear_damping: float | None = None + """Linear damping for the body.""" + + angular_damping: float | None = None + """Angular damping for the body.""" + + max_linear_velocity: float | None = None + """Maximum linear velocity for rigid bodies (in m/s).""" + + max_angular_velocity: float | None = None + """Maximum angular velocity for rigid bodies (in deg/s).""" + + max_depenetration_velocity: float | None = None + """Maximum depenetration velocity permitted to be introduced by the solver (in m/s).""" + + max_contact_impulse: float | None = None + """The limit on the impulse that may be applied at a contact.""" + + enable_gyroscopic_forces: bool | None = None + """Enables computation of gyroscopic forces on the rigid body.""" + + retain_accelerations: bool | None = None + """Carries over forces/accelerations over sub-steps.""" + + solver_position_iteration_count: int | None = None + """Solver position iteration counts for the body.""" + + solver_velocity_iteration_count: int | None = None + """Solver velocity iteration counts for the body.""" + + sleep_threshold: float | None = None + """Mass-normalized kinetic energy threshold below which an actor may go to sleep.""" + + stabilization_threshold: float | None = None + """The mass-normalized kinetic energy threshold below which an actor may participate in stabilization.""" + + +@configclass +class RigidBodyPropertiesCfg(PhysxRigidBodyPropertiesCfg): + """Deprecated: use :class:`PhysxRigidBodyPropertiesCfg` or :class:`~isaaclab.sim.schemas.RigidBodyBaseCfg`. + + .. deprecated:: 4.6.22 + ``RigidBodyPropertiesCfg`` has been split into + :class:`~isaaclab.sim.schemas.RigidBodyBaseCfg` (solver-common) and + :class:`PhysxRigidBodyPropertiesCfg` (PhysX-specific) and relocated to + :mod:`isaaclab_physx.sim.schemas`. This alias preserves backwards compatibility and is + scheduled for removal in 5.0. + """ + + def __post_init__(self): + warnings.warn( + "'RigidBodyPropertiesCfg' is deprecated and will be removed in 5.0. Use" + " 'isaaclab_physx.sim.schemas.PhysxRigidBodyPropertiesCfg' for PhysX properties, or" + " 'isaaclab.sim.schemas.RigidBodyBaseCfg' for solver-common properties only.", + DeprecationWarning, + stacklevel=2, + ) + super().__post_init__() + + +@configclass +class PhysxJointDrivePropertiesCfg(JointDriveBaseCfg): + """PhysX-specific joint drive properties. + + Currently empty after the consumption-gated split moved :attr:`max_joint_velocity` + to :class:`~isaaclab.sim.schemas.JointDriveBaseCfg`. This class is retained + as the deprecation-alias target for the legacy :class:`JointDrivePropertiesCfg` + name and as the home for any future PhysX-only joint-drive fields (e.g. + PhysX-specific drive force-limit modes). + + Inherits all fields and USD metadata from + :class:`~isaaclab.sim.schemas.JointDriveBaseCfg`. + + See :meth:`~isaaclab.sim.schemas.modify_joint_drive_properties` for more information. + + .. _PhysxJointAPI: https://docs.omniverse.nvidia.com/kit/docs/omni_usd_schema_physics/104.2/class_physx_schema_physx_joint_a_p_i.html + """ + + # ``max_joint_velocity`` on the base remains routed via ``_usd_field_exceptions`` + # (inherited). Future PhysX-only joint-drive fields would be written under this + # namespace. + _usd_applied_schema: ClassVar[str | None] = "PhysxJointAPI" + _usd_namespace: ClassVar[str | None] = "physxJoint" + + +@configclass +class JointDrivePropertiesCfg(PhysxJointDrivePropertiesCfg): + """Deprecated: use :class:`PhysxJointDrivePropertiesCfg` or :class:`~isaaclab.sim.schemas.JointDriveBaseCfg`. + + .. deprecated:: 4.6.22 + ``JointDrivePropertiesCfg`` has been split into + :class:`~isaaclab.sim.schemas.JointDriveBaseCfg` (solver-common) and + :class:`PhysxJointDrivePropertiesCfg` (PhysX-specific) and relocated to + :mod:`isaaclab_physx.sim.schemas`. This alias preserves backwards compatibility and is + scheduled for removal in 5.0. + """ + + def __post_init__(self): + warnings.warn( + "'JointDrivePropertiesCfg' is deprecated and will be removed in 5.0. Use" + " 'isaaclab_physx.sim.schemas.PhysxJointDrivePropertiesCfg' for PhysX properties, or" + " 'isaaclab.sim.schemas.JointDriveBaseCfg' for solver-common properties only.", + DeprecationWarning, + stacklevel=2, + ) + super().__post_init__() + + +@configclass +class PhysxCollisionPropertiesCfg(CollisionBaseCfg): + """PhysX-specific rigid-body collision properties. + + Extends :class:`~isaaclab.sim.schemas.CollisionBaseCfg` with the PhysX-only torsional + patch friction approximations (:attr:`torsional_patch_radius`, + :attr:`min_torsional_patch_radius`). These fields have no Newton equivalent and are + consumed only by the PhysX solver. + + See :meth:`~isaaclab.sim.schemas.modify_collision_properties` for more information. + + .. note:: + If the values are None, they are not modified. This is useful when you want to set only a subset of + the properties and leave the rest as-is. + + .. _PhysxCollisionAPI: https://docs.omniverse.nvidia.com/kit/docs/omni_usd_schema_physics/104.2/class_physx_schema_physx_collision_a_p_i.html + """ + + # PhysX torsional-friction fields below live under the ``PhysxCollisionAPI`` schema's + # ``physxCollision:*`` namespace. Base ``contact_offset`` / ``rest_offset`` remain + # routed via ``_usd_field_exceptions`` (inherited). + _usd_applied_schema: ClassVar[str | None] = "PhysxCollisionAPI" + _usd_namespace: ClassVar[str | None] = "physxCollision" + + torsional_patch_radius: float | None = None + """Radius of the contact patch for applying torsional friction [m]. + + It is used to approximate rotational friction introduced by the compression of contacting surfaces. + If the radius is zero, no torsional friction is applied. + """ + + min_torsional_patch_radius: float | None = None + """Minimum radius of the contact patch for applying torsional friction [m].""" + + +@configclass +class PhysxArticulationRootPropertiesCfg(ArticulationRootBaseCfg): + """PhysX-specific articulation-root properties. + + Extends :class:`~isaaclab.sim.schemas.ArticulationRootBaseCfg` with the + `PhysxArticulationAPI`_ schema fields that are PhysX-only or dual-namespace + (Rule 2 — the conceptual quantity also has a ``newton:*`` attribute, and a + future ``NewtonArticulationRootPropertiesCfg`` would carry it on the Newton + side). Use this class when authoring PhysX-specific articulation knobs; + use :class:`~isaaclab.sim.schemas.ArticulationRootBaseCfg` when only the + solver-common ``fix_root_link`` / ``articulation_enabled`` fields are needed. + + See :meth:`~isaaclab.sim.schemas.modify_articulation_root_properties` for more information. + + .. note:: + If the values are None, they are not modified. This is useful when you want to set only a subset of + the properties and leave the rest as-is. + + .. _PhysxArticulationAPI: https://docs.omniverse.nvidia.com/kit/docs/omni_usd_schema_physics/104.2/class_physx_schema_physx_articulation_a_p_i.html + """ + + # PhysX articulation-root fields below live under the ``PhysxArticulationAPI`` schema's + # ``physxArticulation:*`` namespace. Base ``articulation_enabled`` remains routed via + # ``_usd_field_exceptions`` (inherited). + _usd_applied_schema: ClassVar[str | None] = "PhysxArticulationAPI" + _usd_namespace: ClassVar[str | None] = "physxArticulation" + + enabled_self_collisions: bool | None = None + """Whether self-collisions between bodies in the same articulation are enabled. + + The conceptual quantity exists in two USD namespaces simultaneously: + + * ``physxArticulation:enabledSelfCollisions`` (PhysX, ``PhysxArticulationAPI``) + * ``newton:selfCollisionEnabled`` (Newton-native, on a future ``NewtonArticulationRootAPI``) + + Newton's resolver checks the native ``newton:*`` attribute first and falls back + to the PhysX namespace. Both backends honor the field end-to-end. + + Because the conceptual quantity has a dedicated USD attribute in each backend's + namespace, this field is placed on the **PhysX subclass** (one cfg per namespace). + A future ``NewtonArticulationRootPropertiesCfg`` will carry the same field over the + ``newton:*`` namespace. + """ + + solver_position_iteration_count: int | None = None + """Solver position iteration counts for the body.""" + + solver_velocity_iteration_count: int | None = None + """Solver velocity iteration counts for the body.""" + + sleep_threshold: float | None = None + """Mass-normalized kinetic energy threshold below which an actor may go to sleep.""" + + stabilization_threshold: float | None = None + """The mass-normalized kinetic energy threshold below which an articulation may participate in stabilization.""" + + +@configclass +class ArticulationRootPropertiesCfg(PhysxArticulationRootPropertiesCfg): + """Deprecated: use :class:`PhysxArticulationRootPropertiesCfg` or the solver-common base class. + + Use :class:`PhysxArticulationRootPropertiesCfg` for PhysX-specific properties or + :class:`~isaaclab.sim.schemas.ArticulationRootBaseCfg` for solver-common properties only. + + .. deprecated:: 4.6.24 + ``ArticulationRootPropertiesCfg`` has been split into + :class:`~isaaclab.sim.schemas.ArticulationRootBaseCfg` (solver-common + ``fix_root_link`` and the PhysX-namespaced but IL-Newton-consumed + ``articulation_enabled``) and + :class:`PhysxArticulationRootPropertiesCfg` (PhysX-specific + self-collisions, TGS solver iter / sleep / stabilization thresholds) + and relocated to :mod:`isaaclab_physx.sim.schemas`. This alias preserves + backwards compatibility and is scheduled for removal in 5.0. + """ + + def __post_init__(self): + warnings.warn( + "'ArticulationRootPropertiesCfg' is deprecated and will be removed in 5.0. Use" + " 'isaaclab_physx.sim.schemas.PhysxArticulationRootPropertiesCfg' for PhysX properties, or" + " 'isaaclab.sim.schemas.ArticulationRootBaseCfg' for solver-common properties only.", + DeprecationWarning, + stacklevel=2, + ) + super().__post_init__() + + +@configclass +class CollisionPropertiesCfg(PhysxCollisionPropertiesCfg): + """Deprecated: use :class:`PhysxCollisionPropertiesCfg` or :class:`~isaaclab.sim.schemas.CollisionBaseCfg`. + + .. deprecated:: 4.6.23 + ``CollisionPropertiesCfg`` has been split into + :class:`~isaaclab.sim.schemas.CollisionBaseCfg` (solver-common) and + :class:`PhysxCollisionPropertiesCfg` (PhysX-specific) and relocated to + :mod:`isaaclab_physx.sim.schemas`. This alias preserves backwards compatibility and is + scheduled for removal in 5.0. + """ + + def __post_init__(self): + warnings.warn( + "'CollisionPropertiesCfg' is deprecated and will be removed in 5.0. Use" + " 'isaaclab_physx.sim.schemas.PhysxCollisionPropertiesCfg' for PhysX properties, or" + " 'isaaclab.sim.schemas.CollisionBaseCfg' for solver-common properties only.", + DeprecationWarning, + stacklevel=2, + ) + super().__post_init__() + + +@configclass +class PhysxConvexHullPropertiesCfg(MeshCollisionBaseCfg): + """PhysX convex-hull cooking properties for a mesh collider. + + Extends :class:`~isaaclab.sim.schemas.MeshCollisionBaseCfg` with the + ``PhysxConvexHullCollisionAPI`` schema's tuning fields. The ``convexHull`` token is + written to ``physics:approximation``; the cooking schema is applied only when at + least one tuning field is set (consistent with the other consumption-gated writers). + + See :meth:`~isaaclab.sim.schemas.modify_mesh_collision_properties` for more information. + + Original PhysX Documentation: + https://docs.omniverse.nvidia.com/kit/docs/omni_usd_schema_physics/latest/class_physx_schema_physx_convex_hull_collision_a_p_i.html + """ + + _usd_applied_schema: ClassVar[str | None] = "PhysxConvexHullCollisionAPI" + _usd_namespace: ClassVar[str | None] = "physxConvexHullCollision" + + mesh_approximation_name: str = "convexHull" + """Name of mesh collision approximation method. Default: "convexHull".""" + + hull_vertex_limit: int | None = None + """Convex hull vertex limit used for convex hull cooking. + + Defaults to 64. + """ + min_thickness: float | None = None + """Convex hull min thickness. + + Range: [0, inf). Units are distance. Default value is 0.001. + """ + + +@configclass +class PhysxConvexDecompositionPropertiesCfg(MeshCollisionBaseCfg): + """PhysX convex-decomposition cooking properties for a mesh collider. + + See :meth:`~isaaclab.sim.schemas.modify_mesh_collision_properties` for more information. + + Original PhysX Documentation: + https://docs.omniverse.nvidia.com/kit/docs/omni_usd_schema_physics/latest/class_physx_schema_physx_convex_decomposition_collision_a_p_i.html + """ + + _usd_applied_schema: ClassVar[str | None] = "PhysxConvexDecompositionCollisionAPI" + _usd_namespace: ClassVar[str | None] = "physxConvexDecompositionCollision" + + mesh_approximation_name: str = "convexDecomposition" + """Name of mesh collision approximation method. Default: "convexDecomposition".""" + + hull_vertex_limit: int | None = None + """Convex hull vertex limit used for convex hull cooking. + + Defaults to 64. + """ + max_convex_hulls: int | None = None + """Maximum of convex hulls created during convex decomposition. + Default value is 32. + """ + min_thickness: float | None = None + """Convex hull min thickness. + + Range: [0, inf). Units are distance. Default value is 0.001. + """ + voxel_resolution: int | None = None + """Voxel resolution used for convex decomposition. + + Defaults to 500,000 voxels. + """ + error_percentage: float | None = None + """Convex decomposition error percentage parameter. + + Defaults to 10 percent. Units are percent. + """ + shrink_wrap: bool | None = None + """Attempts to adjust the convex hull points so that they are projected onto the surface of the original graphics + mesh. + + Defaults to False. + """ + + +@configclass +class PhysxTriangleMeshPropertiesCfg(MeshCollisionBaseCfg): + """PhysX triangle-mesh cooking properties for a mesh collider. + + Triangle-mesh colliders are PhysX-only. + + See :meth:`~isaaclab.sim.schemas.modify_mesh_collision_properties` for more information. + + Original PhysX Documentation: + https://docs.omniverse.nvidia.com/kit/docs/omni_usd_schema_physics/latest/class_physx_schema_physx_triangle_mesh_collision_a_p_i.html + """ + + _usd_applied_schema: ClassVar[str | None] = "PhysxTriangleMeshCollisionAPI" + _usd_namespace: ClassVar[str | None] = "physxTriangleMeshCollision" + + mesh_approximation_name: str = "none" + """Name of mesh collision approximation method. Default: "none" (uses triangle mesh).""" + + weld_tolerance: float | None = None + """Mesh weld tolerance, controls the distance at which vertices are welded. + + Default -inf will autocompute the welding tolerance based on the mesh size. Zero value will disable welding. + Range: [0, inf) Units: distance + """ + + +@configclass +class PhysxTriangleMeshSimplificationPropertiesCfg(MeshCollisionBaseCfg): + """PhysX triangle-mesh-simplification cooking properties for a mesh collider. + + See :meth:`~isaaclab.sim.schemas.modify_mesh_collision_properties` for more information. + + Original PhysX Documentation: + https://docs.omniverse.nvidia.com/kit/docs/omni_usd_schema_physics/latest/class_physx_schema_physx_triangle_mesh_simplification_collision_a_p_i.html + """ + + _usd_applied_schema: ClassVar[str | None] = "PhysxTriangleMeshSimplificationCollisionAPI" + _usd_namespace: ClassVar[str | None] = "physxTriangleMeshSimplificationCollision" + + mesh_approximation_name: str = "meshSimplification" + """Name of mesh collision approximation method. Default: "meshSimplification".""" + + simplification_metric: float | None = None + """Mesh simplification accuracy. + + Defaults to 0.55. + """ + weld_tolerance: float | None = None + """Mesh weld tolerance, controls the distance at which vertices are welded. + + Default -inf will autocompute the welding tolerance based on the mesh size. Zero value will disable welding. + Range: [0, inf) Units: distance + """ + + +@configclass +class PhysxSDFMeshPropertiesCfg(MeshCollisionBaseCfg): + """PhysX SDF-mesh cooking properties for a mesh collider. + + SDF-mesh colliders are PhysX-only. + + See :meth:`~isaaclab.sim.schemas.modify_mesh_collision_properties` for more information. + + Original PhysX documentation: + https://docs.omniverse.nvidia.com/kit/docs/omni_usd_schema_physics/latest/class_physx_schema_physx_s_d_f_mesh_collision_a_p_i.html + + More details and steps for optimizing SDF results can be found here: + https://nvidia-omniverse.github.io/PhysX/physx/5.2.1/docs/RigidBodyCollision.html#dynamic-triangle-meshes-with-sdfs + """ + + _usd_applied_schema: ClassVar[str | None] = "PhysxSDFMeshCollisionAPI" + _usd_namespace: ClassVar[str | None] = "physxSDFMeshCollision" + + mesh_approximation_name: str = "sdf" + """Name of mesh collision approximation method. Default: "sdf".""" + + sdf_margin: float | None = None + """Margin to increase the size of the SDF relative to the bounding box diagonal length of the mesh. + + A sdf margin value of 0.01 means the sdf boundary will be enlarged in any direction by 1% of the mesh's bounding + box diagonal length. Representing the margin relative to the bounding box diagonal length ensures that it is scale + independent. Margins allow for precise distance queries in a region slightly outside of the mesh's bounding box. + + Default value is 0.01. + Range: [0, inf) Units: dimensionless + """ + sdf_narrow_band_thickness: float | None = None + """Size of the narrow band around the mesh surface where high resolution SDF samples are available. + + Outside of the narrow band, only low resolution samples are stored. Representing the narrow band thickness as a + fraction of the mesh's bounding box diagonal length ensures that it is scale independent. A value of 0.01 is + usually large enough. The smaller the narrow band thickness, the smaller the memory consumption of the sparse SDF. + + Default value is 0.01. + Range: [0, 1] Units: dimensionless + """ + sdf_resolution: int | None = None + """The spacing of the uniformly sampled SDF is equal to the largest AABB extent of the mesh, + divided by the resolution. + + Choose the lowest possible resolution that provides acceptable performance; very high resolution results in large + memory consumption, and slower cooking and simulation performance. + + Default value is 256. + Range: (1, inf) + """ + sdf_subgrid_resolution: int | None = None + """A positive subgrid resolution enables sparsity on signed-distance-fields (SDF) while a value of 0 leads to the + usage of a dense SDF. + + A value in the range of 4 to 8 is a reasonable compromise between block size and the overhead introduced by block + addressing. The smaller a block, the more memory is spent on the address table. The bigger a block, the less + precisely the sparse SDF can adapt to the mesh's surface. In most cases sparsity reduces the memory consumption of + a SDF significantly. + + Default value is 6. + Range: [0, inf) + """ + + +@configclass +class MeshCollisionPropertiesCfg(MeshCollisionBaseCfg): + """Deprecated: use :class:`~isaaclab.sim.schemas.MeshCollisionBaseCfg`. + + .. deprecated:: 4.6.25 + ``MeshCollisionPropertiesCfg`` was the flat (non-leaf) base of the legacy + mesh-collision cfg family. It has been renamed to + :class:`~isaaclab.sim.schemas.MeshCollisionBaseCfg` to match the rest of the + consumption-gated split. This alias preserves backwards compatibility and is + scheduled for removal in 5.0. + """ + + def __post_init__(self): + warnings.warn( + "'MeshCollisionPropertiesCfg' is deprecated and will be removed in 5.0. Use" + " 'isaaclab.sim.schemas.MeshCollisionBaseCfg' instead.", + DeprecationWarning, + stacklevel=2, + ) + super().__post_init__() + + +@configclass +class ConvexHullPropertiesCfg(PhysxConvexHullPropertiesCfg): + """Deprecated: use :class:`PhysxConvexHullPropertiesCfg`. + + .. deprecated:: 4.6.25 + Renamed and relocated. This alias preserves backwards compatibility and is + scheduled for removal in 5.0. + """ + + def __post_init__(self): + warnings.warn( + "'ConvexHullPropertiesCfg' is deprecated and will be removed in 5.0. Use" + " 'isaaclab_physx.sim.schemas.PhysxConvexHullPropertiesCfg' instead.", + DeprecationWarning, + stacklevel=2, + ) + super().__post_init__() + + +@configclass +class ConvexDecompositionPropertiesCfg(PhysxConvexDecompositionPropertiesCfg): + """Deprecated: use :class:`PhysxConvexDecompositionPropertiesCfg`. + + .. deprecated:: 4.6.25 + Renamed and relocated. This alias preserves backwards compatibility and is + scheduled for removal in 5.0. + """ + + def __post_init__(self): + warnings.warn( + "'ConvexDecompositionPropertiesCfg' is deprecated and will be removed in 5.0. Use" + " 'isaaclab_physx.sim.schemas.PhysxConvexDecompositionPropertiesCfg' instead.", + DeprecationWarning, + stacklevel=2, + ) + super().__post_init__() + + +@configclass +class TriangleMeshPropertiesCfg(PhysxTriangleMeshPropertiesCfg): + """Deprecated: use :class:`PhysxTriangleMeshPropertiesCfg`. + + .. deprecated:: 4.6.25 + Renamed and relocated. This alias preserves backwards compatibility and is + scheduled for removal in 5.0. + """ + + def __post_init__(self): + warnings.warn( + "'TriangleMeshPropertiesCfg' is deprecated and will be removed in 5.0. Use" + " 'isaaclab_physx.sim.schemas.PhysxTriangleMeshPropertiesCfg' instead.", + DeprecationWarning, + stacklevel=2, + ) + super().__post_init__() + + +@configclass +class TriangleMeshSimplificationPropertiesCfg(PhysxTriangleMeshSimplificationPropertiesCfg): + """Deprecated: use :class:`PhysxTriangleMeshSimplificationPropertiesCfg`. + + .. deprecated:: 4.6.25 + Renamed and relocated. This alias preserves backwards compatibility and is + scheduled for removal in 5.0. + """ + + def __post_init__(self): + warnings.warn( + "'TriangleMeshSimplificationPropertiesCfg' is deprecated and will be removed in 5.0. Use" + " 'isaaclab_physx.sim.schemas.PhysxTriangleMeshSimplificationPropertiesCfg' instead.", + DeprecationWarning, + stacklevel=2, + ) + super().__post_init__() + + +@configclass +class SDFMeshPropertiesCfg(PhysxSDFMeshPropertiesCfg): + """Deprecated: use :class:`PhysxSDFMeshPropertiesCfg`. + + .. deprecated:: 4.6.25 + Renamed and relocated. This alias preserves backwards compatibility and is + scheduled for removal in 5.0. + """ + + def __post_init__(self): + warnings.warn( + "'SDFMeshPropertiesCfg' is deprecated and will be removed in 5.0. Use" + " 'isaaclab_physx.sim.schemas.PhysxSDFMeshPropertiesCfg' instead.", + DeprecationWarning, + stacklevel=2, + ) + super().__post_init__() + + +@configclass +class PhysxFixedTendonPropertiesCfg: + """PhysX fixed-tendon properties for an articulation. + + Tendons are a PhysX-only feature -- Newton has no tendon system -- so this class + is a pure data carrier that is consumed by the PhysX-specific writer + :func:`~isaaclab.sim.schemas.modify_fixed_tendon_properties`. The writer authors + the multi-instance ``PhysxTendonAxisRootAPI`` schema; this cfg class declares no + metadata-driven writer plumbing of its own. + + See :func:`~isaaclab.sim.schemas.modify_fixed_tendon_properties` for more information. + + .. note:: + If the values are None, they are not modified. This is useful when you want to set only a subset of + the properties and leave the rest as-is. + """ + + tendon_enabled: bool | None = None + """Whether to enable or disable the tendon.""" + + stiffness: float | None = None + """Spring stiffness term acting on the tendon's length.""" + + damping: float | None = None + """The damping term acting on both the tendon length and the tendon-length limits.""" + + limit_stiffness: float | None = None + """Limit stiffness term acting on the tendon's length limits.""" + + offset: float | None = None + """Length offset term for the tendon. + + It defines an amount to be added to the accumulated length computed for the tendon. This allows the application + to actuate the tendon by shortening or lengthening it. + """ + + rest_length: float | None = None + """Spring rest length of the tendon.""" + + +@configclass +class FixedTendonPropertiesCfg(PhysxFixedTendonPropertiesCfg): + """Deprecated: use :class:`PhysxFixedTendonPropertiesCfg`. + + .. deprecated:: 4.6.x + ``FixedTendonPropertiesCfg`` was relocated to + :mod:`isaaclab_physx.sim.schemas` and renamed to + :class:`PhysxFixedTendonPropertiesCfg`. The legacy name remains as a + deprecation alias and is scheduled for removal in 5.0. + """ + + def __post_init__(self): + warnings.warn( + "'FixedTendonPropertiesCfg' is deprecated and will be removed in 5.0. Use" + " 'isaaclab_physx.sim.schemas.PhysxFixedTendonPropertiesCfg' instead.", + DeprecationWarning, + stacklevel=2, + ) + super().__post_init__() + + +@configclass +class PhysxSpatialTendonPropertiesCfg: + """PhysX spatial-tendon properties for an articulation. + + Tendons are a PhysX-only feature -- Newton has no tendon system -- so this class + is a pure data carrier that is consumed by the PhysX-specific writer + :func:`~isaaclab.sim.schemas.modify_spatial_tendon_properties`. The writer authors + the multi-instance ``PhysxTendonAttachmentRootAPI`` / ``PhysxTendonAttachmentLeafAPI`` + schemas; this cfg class declares no metadata-driven writer plumbing of its own. + + See :func:`~isaaclab.sim.schemas.modify_spatial_tendon_properties` for more information. + + .. note:: + If the values are None, they are not modified. This is useful when you want to set only a subset of + the properties and leave the rest as-is. + """ + + tendon_enabled: bool | None = None + """Whether to enable or disable the tendon.""" + + stiffness: float | None = None + """Spring stiffness term acting on the tendon's length.""" + + damping: float | None = None + """The damping term acting on both the tendon length and the tendon-length limits.""" + + limit_stiffness: float | None = None + """Limit stiffness term acting on the tendon's length limits.""" + + offset: float | None = None + """Length offset term for the tendon. + + It defines an amount to be added to the accumulated length computed for the tendon. This allows the application + to actuate the tendon by shortening or lengthening it. + """ + + +@configclass +class SpatialTendonPropertiesCfg(PhysxSpatialTendonPropertiesCfg): + """Deprecated: use :class:`PhysxSpatialTendonPropertiesCfg`. + + .. deprecated:: 4.6.x + ``SpatialTendonPropertiesCfg`` was relocated to + :mod:`isaaclab_physx.sim.schemas` and renamed to + :class:`PhysxSpatialTendonPropertiesCfg`. The legacy name remains as a + deprecation alias and is scheduled for removal in 5.0. + """ + + def __post_init__(self): + warnings.warn( + "'SpatialTendonPropertiesCfg' is deprecated and will be removed in 5.0. Use" + " 'isaaclab_physx.sim.schemas.PhysxSpatialTendonPropertiesCfg' instead.", + DeprecationWarning, + stacklevel=2, + ) + super().__post_init__() diff --git a/source/isaaclab_physx/isaaclab_physx/sim/spawners/materials/__init__.pyi b/source/isaaclab_physx/isaaclab_physx/sim/spawners/materials/__init__.pyi index 6e8a48b2f118..1a3e833d61d9 100644 --- a/source/isaaclab_physx/isaaclab_physx/sim/spawners/materials/__init__.pyi +++ b/source/isaaclab_physx/isaaclab_physx/sim/spawners/materials/__init__.pyi @@ -6,11 +6,15 @@ __all__ = [ "spawn_deformable_body_material", "DeformableBodyMaterialCfg", + "PhysxRigidBodyMaterialCfg", + "RigidBodyMaterialCfg", "SurfaceDeformableBodyMaterialCfg", ] from .physics_materials import spawn_deformable_body_material from .physics_materials_cfg import ( DeformableBodyMaterialCfg, + PhysxRigidBodyMaterialCfg, + RigidBodyMaterialCfg, SurfaceDeformableBodyMaterialCfg, ) diff --git a/source/isaaclab_physx/isaaclab_physx/sim/spawners/materials/physics_materials_cfg.py b/source/isaaclab_physx/isaaclab_physx/sim/spawners/materials/physics_materials_cfg.py index 35b066fa8736..2c8121a5cbee 100644 --- a/source/isaaclab_physx/isaaclab_physx/sim/spawners/materials/physics_materials_cfg.py +++ b/source/isaaclab_physx/isaaclab_physx/sim/spawners/materials/physics_materials_cfg.py @@ -6,9 +6,12 @@ from __future__ import annotations import dataclasses +import warnings from collections.abc import Callable +from typing import ClassVar, Literal from isaaclab.sim.spawners.materials import PhysicsMaterialCfg +from isaaclab.sim.spawners.materials.physics_materials_cfg import RigidBodyMaterialBaseCfg from isaaclab.utils import configclass @@ -120,3 +123,83 @@ class SurfaceDeformableBodyMaterialCfg(DeformableBodyMaterialCfg, OmniPhysicsSur "physxDeformableBody": [field.name for field in dataclasses.fields(PhysXDeformableMaterialCfg)], } """Extend DeformableBodyMaterialCfg properties under each prefix.""" + + +@configclass +class PhysxRigidBodyMaterialCfg(RigidBodyMaterialBaseCfg): + """PhysX-specific physics-material parameters for rigid bodies. + + Extends :class:`~isaaclab.sim.spawners.materials.RigidBodyMaterialBaseCfg` with the + `PhysxMaterialAPI`_ schema fields: compliant-contact spring (stiffness/damping) and the + friction/restitution combine-mode tokens. None of these fields have a Newton consumer + today; they are PhysX-engine-only knobs. + + See :meth:`~isaaclab.sim.spawners.materials.spawn_rigid_body_material` for more information. + + .. _PhysxMaterialAPI: https://docs.omniverse.nvidia.com/kit/docs/omni_usd_schema_physics/104.2/class_physx_schema_physx_material_a_p_i.html + """ + + # -- Class metadata (not dataclass fields) -- + # USD applied schema written when at least one PhysX-namespaced field is set. + _usd_applied_schema: ClassVar[str | None] = "PhysxMaterialAPI" + # Prim attribute namespace for PhysX-specific fields. + _usd_namespace: ClassVar[str | None] = "physxMaterial" + + compliant_contact_stiffness: float | None = None + """Spring stiffness for a compliant contact model using implicit springs. + + A higher stiffness results in behavior closer to a rigid contact. The compliant contact model + is only enabled if the stiffness is larger than 0. PhysX-only; not consumed by Newton. + """ + + compliant_contact_damping: float | None = None + """Damping coefficient for a compliant contact model using implicit springs. + + Irrelevant if compliant contacts are disabled when :attr:`compliant_contact_stiffness` is set + to zero and rigid contacts are active. PhysX-only; not consumed by Newton. + """ + + friction_combine_mode: Literal["average", "min", "multiply", "max"] | None = None + """Determines the way friction will be combined during collisions. + + .. attention:: + + When two physics materials with different combine modes collide, the combine mode with + the higher priority will be used. The priority order is provided `here + `__. + """ + + restitution_combine_mode: Literal["average", "min", "multiply", "max"] | None = None + """Determines the way restitution coefficient will be combined during collisions. + + .. attention:: + + When two physics materials with different combine modes collide, the combine mode with + the higher priority will be used. The priority order is provided `here + `__. + """ + + +@configclass +class RigidBodyMaterialCfg(PhysxRigidBodyMaterialCfg): + """Deprecated: use :class:`PhysxRigidBodyMaterialCfg` or + :class:`~isaaclab.sim.spawners.materials.RigidBodyMaterialBaseCfg`. + + .. deprecated:: 4.6.22 + ``RigidBodyMaterialCfg`` has been split into + :class:`~isaaclab.sim.spawners.materials.RigidBodyMaterialBaseCfg` (solver-common) and + :class:`PhysxRigidBodyMaterialCfg` (PhysX-specific) and relocated to + :mod:`isaaclab_physx.sim.spawners.materials`. This alias preserves backwards compatibility + and is scheduled for removal in 5.0. + """ + + def __post_init__(self): + warnings.warn( + "'RigidBodyMaterialCfg' is deprecated and will be removed in 5.0. Use" + " 'isaaclab_physx.sim.spawners.materials.PhysxRigidBodyMaterialCfg' for PhysX" + " properties, or 'isaaclab.sim.spawners.materials.RigidBodyMaterialBaseCfg' for" + " solver-common properties only.", + DeprecationWarning, + stacklevel=2, + ) + super().__post_init__() diff --git a/source/isaaclab_physx/isaaclab_physx/sim/views/fabric_frame_view.py b/source/isaaclab_physx/isaaclab_physx/sim/views/fabric_frame_view.py index de65e8501793..74e669ab5c8b 100644 --- a/source/isaaclab_physx/isaaclab_physx/sim/views/fabric_frame_view.py +++ b/source/isaaclab_physx/isaaclab_physx/sim/views/fabric_frame_view.py @@ -8,11 +8,12 @@ from __future__ import annotations import logging +from typing import ClassVar import torch import warp as wp -from pxr import Usd +from pxr import Gf, Usd, UsdGeom import isaaclab.sim as sim_utils from isaaclab.app.settings_manager import SettingsManager @@ -23,6 +24,12 @@ logger = logging.getLogger(__name__) +# TODO: extend this to ``cuda:N`` once we wire up multi-GPU support for the view. +# Recent Kit / USDRT releases do support multi-GPU ``SelectPrims``, but the +# rest of the FabricFrameView wiring (selections, indexed arrays, etc.) still +# assumes a single device — to be tackled in a follow-up. +_fabric_supported_devices = ("cpu", "cuda", "cuda:0") + def _to_float32_2d(a: wp.array | torch.Tensor) -> wp.array | torch.Tensor: """Ensure array is compatible with Fabric kernels (2-D float32). @@ -43,55 +50,145 @@ def _to_float32_2d(a: wp.array | torch.Tensor) -> wp.array | torch.Tensor: class FabricFrameView(BaseFrameView): """FrameView with Fabric GPU acceleration for the PhysX backend. - Uses composition: holds a :class:`UsdFrameView` internally for USD - fallback and non-accelerated operations (local poses, visibility, scales - when Fabric is disabled). - - When Fabric is enabled, world-pose and scale operations use GPU-accelerated - Warp kernels operating on ``omni:fabric:worldMatrix``. All other operations - delegate to the internal USD view. - - Pose getters return :class:`~isaaclab.utils.warp.ProxyArray`. Setters accept ``wp.array``. + World-pose, local-pose, and scale operations run on the GPU via Warp + kernels that read and write ``omni:fabric:worldMatrix`` and + ``omni:fabric:localMatrix`` directly. Typical speedup vs. the + :class:`~isaaclab.sim.views.UsdFrameView` baseline at 1024 prims is + 150-260× per call (see ``scripts/benchmarks/benchmark_view_comparison.py``). + + When Fabric is unavailable — ``/physics/fabricEnabled`` is false or the + device is unsupported — the view transparently falls back to + :class:`~isaaclab.sim.views.UsdFrameView` for all pose and scale + operations. The ``count``, ``prims``, ``prim_paths`` properties and the + ``get_visibility`` / ``set_visibility`` methods always delegate to + :class:`~isaaclab.sim.views.UsdFrameView`; Fabric has no equivalent fast + path for those. + + Behavior: + + * **No write-back to USD.** Fabric writes update only + ``omni:fabric:worldMatrix`` / ``omni:fabric:localMatrix``; the prim's + USD ``xformOp:*`` attributes are unchanged. Downstream consumers that + read the prim's USD attributes after a Fabric write will see stale + values until the next USD-side sync. + * **World ↔ local consistency.** After ``set_world_poses`` (or + ``set_scales``) the local matrix is updated so that subsequent + ``get_local_poses`` is consistent; after ``set_local_poses`` the world + matrix is recomputed on the next world read. Both directions stay in + sync without round-tripping through USD. + * **Topology-adaptive.** Fabric topology changes are detected on each + access; the view rebuilds its internal mapping automatically and no + manual refresh is required. Steady-state overhead is negligible. + + Pose getters return :class:`~isaaclab.utils.warp.ProxyArray`; setters + accept :class:`wp.array`. """ + _WORLD_MATRIX_NAME = "omni:fabric:worldMatrix" + _LOCAL_MATRIX_NAME = "omni:fabric:localMatrix" + + # Stage-level shared state. Multiple FabricFrameView instances on the same stage + # share one ``IFabricHierarchy`` handle, keyed by ``(stage_id, fabric_id_int)`` + # so that a recycled ``stage_id`` paired with a fresh Fabric attachment never + # reuses a stale handle. The Fabric id is stored as a plain ``int`` (extracted + # from ``FabricId.id``) because the ``FabricId`` wrapper itself is freshly + # allocated on every ``GetFabricId()`` call and has no value equality, which + # would defeat the cache. ``ClassVar`` plus the ``_static_`` prefix make it + # explicit (and enforceable by type checkers) that this is class-level — never + # shadow it with ``self. = ...``. The cache is not protected by a lock; + # Isaac Lab's simulation loop is single-threaded, and adding locking would + # negate the per-stage hit-cache cost it exists to avoid. Call + # :meth:`clear_static_caches` on stage teardown if you tear down many stages + # in one process. + _static_hierarchy_cache: ClassVar[dict[tuple[int, int], object]] = {} + + @classmethod + def clear_static_caches(cls) -> None: + """Drop all cached ``IFabricHierarchy`` handles. + + Call this after tearing down a USD stage if the process will continue + opening new stages. Cached handles are otherwise retained for the + lifetime of the process, which can leak memory in long test suites and, + in theory, allow a recycled ``stage_id`` paired with a new Fabric + attachment to read a stale handle (though the ``(stage_id, fabric_id)`` + cache key already guards against the latter). + """ + cls._static_hierarchy_cache.clear() + def __init__( self, prim_path: str, device: str = "cpu", validate_xform_ops: bool = True, - sync_usd_on_fabric_write: bool = False, stage: Usd.Stage | None = None, + **kwargs, ): + """Initialize the view. + + Args: + prim_path: USD prim-path pattern to match. + device: Device for Warp arrays (``"cpu"`` or ``"cuda:0"``). + validate_xform_ops: Whether to validate prim xform-ops. + stage: USD stage; defaults to the current sim context's stage. + **kwargs: Additional keyword arguments (ignored). Matches the signature of + :class:`~isaaclab.sim.views.UsdFrameView` so that the top-level + :class:`~isaaclab.sim.views.FrameView` factory can forward backend-agnostic + kwargs without each backend having to know about every option. + """ self._usd_view = UsdFrameView(prim_path, device=device, validate_xform_ops=validate_xform_ops, stage=stage) self._device = device - self._sync_usd_on_fabric_write = sync_usd_on_fabric_write settings = SettingsManager.instance() self._use_fabric = bool(settings.get("/physics/fabricEnabled", False)) - if self._use_fabric and self._device == "cpu": - logger.warning( - "Fabric mode with Warp fabric-array operations is not supported on CPU devices. " - "Falling back to standard USD operations on the CPU. This may impact performance." - ) - self._use_fabric = False - - if self._use_fabric and self._device not in ("cuda", "cuda:0"): + if self._use_fabric and self._device not in _fabric_supported_devices: logger.warning( f"Fabric mode is not supported on device '{self._device}'. " - "USDRT SelectPrims and Warp fabric arrays only support cuda:0. " + "USDRT SelectPrims and Warp fabric arrays are currently " + f"only supported on {', '.join(_fabric_supported_devices)}. " "Falling back to standard USD operations. This may impact performance." ) self._use_fabric = False + # Fabric state — all populated lazily in :meth:`_initialize_fabric`. self._fabric_initialized = False - self._fabric_usd_sync_done = False - self._fabric_selection = None - self._fabric_to_view: wp.array | None = None - self._view_to_fabric: wp.array | None = None - self._default_view_indices: wp.array | None = None + self._stage_id: int | None = None + self._stage = None self._fabric_hierarchy = None - self._view_index_attr = f"isaaclab:view_index:{abs(hash(self))}" + # Set by ``set_local_poses``; cleared by ``_sync_world_from_local_if_dirty``. + # Per-view (not per-stage) so concurrent views on the same stage don't clear + # each other's flag. + self._world_dirty: bool = False + + # Selections. + self._trans_sel_ro = None + self._world_sel_rw = None + self._local_sel_rw = None + + # Index arrays (view-side indices and view→fabric mappings). Each selection's + # ``GetPaths()`` ordering is independent, so the view→fabric mapping is cached + # per selection rather than shared — sharing would silently corrupt indexed + # arrays whose selection didn't fire ``PrepareForReuse`` on the same frame. + self._view_indices: wp.array | None = None + self._trans_ro_fabric_indices: wp.array | None = None + self._world_rw_fabric_indices: wp.array | None = None + self._local_rw_fabric_indices: wp.array | None = None + self._parent_fabric_indices: wp.array | None = None + + # Indexed fabric arrays. + self._world_ifa_ro = None + self._local_ifa_ro = None + self._world_ifa_rw = None + self._local_ifa_rw = None + self._parent_world_ifa_ro = None + + # Sentinel passed to ``compose_indexed_fabric_transforms`` / + # ``decompose_indexed_fabric_transforms`` for slots the caller does not want + # written or read. The kernels gate every per-row access on + # ``shape[0] > 0``, so a ``(0, 0)`` array is enough — the inner dim is never + # indexed. One shared instance covers all "unused" slots regardless of + # whether they would have held positions, quaternions, or scales. + self._fabric_empty_2d_array_sentinel: wp.array | None = None # ------------------------------------------------------------------ # Delegated properties @@ -136,39 +233,33 @@ def set_world_poses(self, positions=None, orientations=None, indices=None): if not self._fabric_initialized: self._initialize_fabric() - indices_wp = self._resolve_indices_wp(indices) - count = indices_wp.shape[0] + # If a prior set_local_poses left worldMatrix stale, propagate local → world first. + self._sync_world_from_local_if_dirty() - dummy = wp.zeros((0, 3), dtype=wp.float32, device=self._device) - positions_wp = _to_float32_2d(positions) if positions is not None else dummy - orientations_wp = ( - _to_float32_2d(orientations) - if orientations is not None - else wp.zeros((0, 4), dtype=wp.float32, device=self._device) - ) + indices_wp = self._resolve_indices_wp(indices) + positions_wp = self._to_float32_2d_or_empty(positions) + orientations_wp = self._to_float32_2d_or_empty(orientations) wp.launch( - kernel=fabric_utils.compose_fabric_transformation_matrix_from_warp_arrays, - dim=count, + kernel=fabric_utils.compose_indexed_fabric_transforms, + dim=indices_wp.shape[0], inputs=[ - self._fabric_world_matrices, + self._get_world_rw_array(), positions_wp, orientations_wp, - dummy, + self._fabric_empty_2d_array_sentinel, False, False, False, indices_wp, - self._view_to_fabric, ], - device=self._fabric_device, + device=self._device, ) wp.synchronize() - self._fabric_hierarchy.update_world_xforms() - self._fabric_usd_sync_done = True - if self._sync_usd_on_fabric_write: - self._usd_view.set_world_poses(positions, orientations, indices) + # World was just written — recompute child localMatrix from parent worldMatrix + # so the next get_local_poses returns consistent values. + self._sync_local_from_world(indices_wp) def get_world_poses(self, indices: wp.array | None = None) -> tuple[ProxyArray, ProxyArray]: if not self._use_fabric: @@ -176,8 +267,9 @@ def get_world_poses(self, indices: wp.array | None = None) -> tuple[ProxyArray, if not self._fabric_initialized: self._initialize_fabric() - if not self._fabric_usd_sync_done: - self._sync_fabric_from_usd_once() + + # If a prior set_local_poses left worldMatrix stale, propagate local → world first. + self._sync_world_from_local_if_dirty() indices_wp = self._resolve_indices_wp(indices) count = indices_wp.shape[0] @@ -191,17 +283,16 @@ def get_world_poses(self, indices: wp.array | None = None) -> tuple[ProxyArray, orientations_wp = wp.zeros((count, 4), dtype=wp.float32, device=self._device) wp.launch( - kernel=fabric_utils.decompose_fabric_transformation_matrix_to_warp_arrays, + kernel=fabric_utils.decompose_indexed_fabric_transforms, dim=count, inputs=[ - self._fabric_world_matrices, + self._get_world_ro_array(), positions_wp, orientations_wp, - self._fabric_dummy_buffer, + self._fabric_empty_2d_array_sentinel, indices_wp, - self._view_to_fabric, ], - device=self._fabric_device, + device=self._device, ) if use_cached: @@ -210,17 +301,79 @@ def get_world_poses(self, indices: wp.array | None = None) -> tuple[ProxyArray, return ProxyArray(positions_wp), ProxyArray(orientations_wp) # ------------------------------------------------------------------ - # Local poses — USD fallback (Fabric only accelerates world poses) + # Local poses # ------------------------------------------------------------------ def set_local_poses(self, translations=None, orientations=None, indices=None): - self._usd_view.set_local_poses(translations, orientations, indices) + if not self._use_fabric: + self._usd_view.set_local_poses(translations, orientations, indices) + return + + if not self._fabric_initialized: + self._initialize_fabric() + + indices_wp = self._resolve_indices_wp(indices) + translations_wp = self._to_float32_2d_or_empty(translations) + orientations_wp = self._to_float32_2d_or_empty(orientations) + + wp.launch( + kernel=fabric_utils.compose_indexed_fabric_transforms, + dim=indices_wp.shape[0], + inputs=[ + self._get_local_rw_array(), + translations_wp, + orientations_wp, + self._fabric_empty_2d_array_sentinel, + False, + False, + False, + indices_wp, + ], + device=self._device, + ) + wp.synchronize() + + # Mark this view's worlds stale so the next world read recomputes them. + self._world_dirty = True def get_local_poses(self, indices: wp.array | None = None) -> tuple[ProxyArray, ProxyArray]: - return self._usd_view.get_local_poses(indices) + if not self._use_fabric: + return self._usd_view.get_local_poses(indices) + + if not self._fabric_initialized: + self._initialize_fabric() + + indices_wp = self._resolve_indices_wp(indices) + count = indices_wp.shape[0] + + use_cached = indices is None or indices == slice(None) + if use_cached: + translations_wp = self._fabric_local_translations_buf + orientations_wp = self._fabric_local_orientations_buf + else: + translations_wp = wp.zeros((count, 3), dtype=wp.float32, device=self._device) + orientations_wp = wp.zeros((count, 4), dtype=wp.float32, device=self._device) + + wp.launch( + kernel=fabric_utils.decompose_indexed_fabric_transforms, + dim=count, + inputs=[ + self._get_local_ro_array(), + translations_wp, + orientations_wp, + self._fabric_empty_2d_array_sentinel, + indices_wp, + ], + device=self._device, + ) + + if use_cached: + wp.synchronize() + return self._fabric_local_translations_ta, self._fabric_local_orientations_ta + return ProxyArray(translations_wp), ProxyArray(orientations_wp) # ------------------------------------------------------------------ - # Scales — Fabric-accelerated or USD fallback + # Scales # ------------------------------------------------------------------ def set_scales(self, scales, indices=None): @@ -231,35 +384,32 @@ def set_scales(self, scales, indices=None): if not self._fabric_initialized: self._initialize_fabric() - indices_wp = self._resolve_indices_wp(indices) - count = indices_wp.shape[0] + # Sync world matrices first if local writes are pending. + self._sync_world_from_local_if_dirty() - dummy3 = wp.zeros((0, 3), dtype=wp.float32, device=self._device) - dummy4 = wp.zeros((0, 4), dtype=wp.float32, device=self._device) - scales_wp = _to_float32_2d(scales) + indices_wp = self._resolve_indices_wp(indices) + scales_wp = self._to_float32_2d_or_empty(scales) wp.launch( - kernel=fabric_utils.compose_fabric_transformation_matrix_from_warp_arrays, - dim=count, + kernel=fabric_utils.compose_indexed_fabric_transforms, + dim=indices_wp.shape[0], inputs=[ - self._fabric_world_matrices, - dummy3, - dummy4, + self._get_world_rw_array(), + self._fabric_empty_2d_array_sentinel, + self._fabric_empty_2d_array_sentinel, scales_wp, False, False, False, indices_wp, - self._view_to_fabric, ], - device=self._fabric_device, + device=self._device, ) wp.synchronize() - self._fabric_hierarchy.update_world_xforms() - self._fabric_usd_sync_done = True - if self._sync_usd_on_fabric_write: - self._usd_view.set_scales(scales, indices) + # World was just written — recompute child localMatrix from parent worldMatrix + # so the next get_local_poses returns the new scale rather than the stale one. + self._sync_local_from_world(indices_wp) def get_scales(self, indices=None): if not self._use_fabric: @@ -267,8 +417,9 @@ def get_scales(self, indices=None): if not self._fabric_initialized: self._initialize_fabric() - if not self._fabric_usd_sync_done: - self._sync_fabric_from_usd_once() + + # Sync world matrices first if local writes are pending. + self._sync_world_from_local_if_dirty() indices_wp = self._resolve_indices_wp(indices) count = indices_wp.shape[0] @@ -280,17 +431,16 @@ def get_scales(self, indices=None): scales_wp = wp.zeros((count, 3), dtype=wp.float32, device=self._device) wp.launch( - kernel=fabric_utils.decompose_fabric_transformation_matrix_to_warp_arrays, + kernel=fabric_utils.decompose_indexed_fabric_transforms, dim=count, inputs=[ - self._fabric_world_matrices, - self._fabric_dummy_buffer, - self._fabric_dummy_buffer, + self._get_world_ro_array(), + self._fabric_empty_2d_array_sentinel, + self._fabric_empty_2d_array_sentinel, scales_wp, indices_wp, - self._view_to_fabric, ], - device=self._fabric_device, + device=self._device, ) if use_cached: @@ -298,113 +448,406 @@ def get_scales(self, indices=None): return scales_wp # ------------------------------------------------------------------ - # Internal — Fabric initialization + # Internal — sync helpers # ------------------------------------------------------------------ - def _initialize_fabric(self) -> None: - """Initialize Fabric batch infrastructure for GPU-accelerated pose queries.""" - import usdrt # noqa: PLC0415 - from usdrt import Rt # noqa: PLC0415 - - stage_id = sim_utils.get_current_stage_id() - fabric_stage = usdrt.Usd.Stage.Attach(stage_id) + def _to_float32_2d_or_empty(self, data): + return self._fabric_empty_2d_array_sentinel if data is None else _to_float32_2d(data) - for i in range(self.count): - rt_prim = fabric_stage.GetPrimAtPath(self.prim_paths[i]) - rt_xformable = Rt.Xformable(rt_prim) + def _sync_world_from_local_if_dirty(self) -> None: + """If a prior local write left world matrices stale, recompute them on the fly. - has_attr = ( - rt_xformable.HasFabricHierarchyWorldMatrixAttr() - if hasattr(rt_xformable, "HasFabricHierarchyWorldMatrixAttr") - else False - ) - if not has_attr: - rt_xformable.CreateFabricHierarchyWorldMatrixAttr() + We deliberately do NOT call ``IFabricHierarchy.update_world_xforms()`` — + in practice that re-reads USD's authored xformOps and overwrites the Fabric + local+world matrices we just authored. Instead we fire a Warp kernel that + does ``child_world = parent_world * child_local`` per child, leaving the + Fabric-side localMatrix untouched. + """ + if not self._world_dirty: + return + # Refresh trans_sel_ro once, then read _local_ifa_ro and _parent_world_ifa_ro + # directly to avoid calling PrepareForReuse twice on the same selection. + if self._trans_sel_ro.PrepareForReuse() or self._parent_world_ifa_ro is None: + self._rebuild_trans_ro_arrays() + wp.launch( + kernel=fabric_utils.update_indexed_world_matrix_from_local, + dim=self.count, + inputs=[ + self._local_ifa_ro, + self._parent_world_ifa_ro, + self._get_world_rw_array(), + self._view_indices, + ], + device=self._device, + ) + wp.synchronize() + self._world_dirty = False + + def _sync_local_from_world(self, indices_wp: wp.array) -> None: + """Recompute child ``localMatrix`` from (parent worldMatrix, child worldMatrix). + + Called after ``set_world_poses`` so that subsequent ``get_local_poses`` returns + values consistent with the just-written world poses. Fabric Hierarchy does + not provide a built-in world → local sync, so we do it via a Warp kernel + using the parent indexed fabric array. + """ + # Refresh trans_sel_ro once; _world_ifa_ro and _parent_world_ifa_ro share it. + if self._trans_sel_ro.PrepareForReuse() or self._parent_world_ifa_ro is None: + self._rebuild_trans_ro_arrays() + wp.launch( + kernel=fabric_utils.update_indexed_local_matrix_from_world, + dim=indices_wp.shape[0], + inputs=[ + self._world_ifa_ro, + self._parent_world_ifa_ro, + self._get_local_rw_array(), + indices_wp, + ], + device=self._device, + ) + wp.synchronize() - rt_xformable.SetWorldXformFromUsd() + # ------------------------------------------------------------------ + # Internal — selection accessors with on-demand index rebuild + # ------------------------------------------------------------------ - rt_prim.CreateAttribute(self._view_index_attr, usdrt.Sdf.ValueTypeNames.UInt, custom=True) - rt_prim.GetAttribute(self._view_index_attr).Set(i) + def _get_world_ro_array(self): + if self._trans_sel_ro.PrepareForReuse(): + self._rebuild_trans_ro_arrays() + return self._world_ifa_ro + + def _get_local_ro_array(self): + if self._trans_sel_ro.PrepareForReuse(): + self._rebuild_trans_ro_arrays() + return self._local_ifa_ro + + def _get_world_rw_array(self): + if self._world_sel_rw.PrepareForReuse(): + self._world_rw_fabric_indices = self._compute_fabric_indices(self._world_sel_rw) + self._world_ifa_rw = self._build_indexed_array( + self._world_sel_rw, self._WORLD_MATRIX_NAME, self._world_rw_fabric_indices + ) + return self._world_ifa_rw - self._fabric_hierarchy = usdrt.hierarchy.IFabricHierarchy().get_fabric_hierarchy( - fabric_stage.GetFabricId(), fabric_stage.GetStageIdAsStageId() + def _get_local_rw_array(self): + if self._local_sel_rw.PrepareForReuse(): + self._local_rw_fabric_indices = self._compute_fabric_indices(self._local_sel_rw) + self._local_ifa_rw = self._build_indexed_array( + self._local_sel_rw, self._LOCAL_MATRIX_NAME, self._local_rw_fabric_indices + ) + return self._local_ifa_rw + + def _get_parent_world_ro_array(self): + # Built and refreshed alongside the trans_ro selection (parents share that selection). + if self._parent_world_ifa_ro is None or self._trans_sel_ro.PrepareForReuse(): + self._rebuild_trans_ro_arrays() + return self._parent_world_ifa_ro + + def _rebuild_trans_ro_arrays(self) -> None: + """Rebuild the trans_ro indices and the three indexed arrays that depend on them. + + ``_world_ifa_ro``, ``_local_ifa_ro`` and ``_parent_world_ifa_ro`` are all + keyed off the ``trans_sel_ro`` path ordering, so they are refreshed together. + """ + self._trans_ro_fabric_indices = self._compute_fabric_indices(self._trans_sel_ro) + self._world_ifa_ro = self._build_indexed_array( + self._trans_sel_ro, self._WORLD_MATRIX_NAME, self._trans_ro_fabric_indices ) - self._fabric_hierarchy.update_world_xforms() - - self._default_view_indices = wp.zeros((self.count,), dtype=wp.uint32, device=self._device) - wp.launch( - kernel=fabric_utils.arange_k, dim=self.count, inputs=[self._default_view_indices], device=self._device + self._local_ifa_ro = self._build_indexed_array( + self._trans_sel_ro, self._LOCAL_MATRIX_NAME, self._trans_ro_fabric_indices ) - wp.synchronize() + self._parent_world_ifa_ro = self._build_parent_indexed_array(self._trans_sel_ro) - fabric_device = self._device - if self._device == "cuda": - logger.warning("Fabric device is not specified, defaulting to 'cuda:0'.") - fabric_device = "cuda:0" - elif self._device.startswith("cuda:"): - if self._device != "cuda:0": - logger.debug( - f"SelectPrims only supports cuda:0. Using cuda:0 for SelectPrims " - f"even though simulation device is {self._device}." + # ------------------------------------------------------------------ + # Internal — index computation + # ------------------------------------------------------------------ + + def _compute_fabric_indices(self, selection) -> wp.array: + fabric_paths = selection.GetPaths() + path_to_fabric_idx: dict[str, int] = {str(p): i for i, p in enumerate(fabric_paths)} + indices: list[int] = [] + for prim_path in self.prim_paths: + fabric_idx = path_to_fabric_idx.get(prim_path) + if fabric_idx is None: + raise RuntimeError( + f"Prim '{prim_path}' not found in Fabric selection. Ensure the hierarchy has been populated." + ) + indices.append(fabric_idx) + return wp.array(indices, dtype=wp.int32, device=self._device) + + def _compute_parent_fabric_indices(self, selection) -> wp.array: + """For each child in this view, look up the parent prim's fabric index.""" + fabric_paths = selection.GetPaths() + path_to_fabric_idx: dict[str, int] = {str(p): i for i, p in enumerate(fabric_paths)} + indices: list[int] = [] + for prim_path in self.prim_paths: + parent_path = prim_path.rsplit("/", 1)[0] + if parent_path == "": + raise RuntimeError( + f"Child prim '{prim_path}' is at stage root and has no parent prim. " + "FabricFrameView requires every prim to have a non-pseudoroot parent " + "with Fabric world+local matrices." + ) + fabric_idx = path_to_fabric_idx.get(parent_path) + if fabric_idx is None: + raise RuntimeError( + f"Parent prim '{parent_path}' (for child '{prim_path}') not found in Fabric selection. " + "Ensure parents have Fabric world+local matrices populated." ) - fabric_device = "cuda:0" + indices.append(fabric_idx) + return wp.array(indices, dtype=wp.int32, device=self._device) - self._fabric_selection = fabric_stage.SelectPrims( - require_attrs=[ - (usdrt.Sdf.ValueTypeNames.UInt, self._view_index_attr, usdrt.Usd.Access.Read), - (usdrt.Sdf.ValueTypeNames.Matrix4d, "omni:fabric:worldMatrix", usdrt.Usd.Access.ReadWrite), - ], - device=fabric_device, - ) + def _build_indexed_array(self, selection, attribute_name: str, fabric_indices: wp.array) -> wp.indexedfabricarray: + fa = wp.fabricarray(selection, attribute_name) + return wp.indexedfabricarray(fa=fa, indices=fabric_indices) - self._view_to_fabric = wp.zeros((self.count,), dtype=wp.uint32, device=fabric_device) - self._fabric_to_view = wp.fabricarray(self._fabric_selection, self._view_index_attr) + def _build_parent_indexed_array(self, selection) -> wp.indexedfabricarray: + self._parent_fabric_indices = self._compute_parent_fabric_indices(selection) + fa = wp.fabricarray(selection, self._WORLD_MATRIX_NAME) + return wp.indexedfabricarray(fa=fa, indices=self._parent_fabric_indices) - wp.launch( - kernel=fabric_utils.set_view_to_fabric_array, - dim=self._fabric_to_view.shape[0], - inputs=[self._fabric_to_view, self._view_to_fabric], - device=fabric_device, + def _resolve_indices_wp(self, indices: wp.array | None) -> wp.array: + """Resolve view indices as a Warp uint32 array.""" + if indices is None or indices == slice(None): + if self._view_indices is None: + raise RuntimeError("Fabric view indices are not initialized.") + return self._view_indices + if indices.dtype != wp.uint32: + return wp.array(indices.numpy().astype("uint32"), dtype=wp.uint32, device=self._device) + return indices + + # ------------------------------------------------------------------ + # Internal — Fabric initialization + # ------------------------------------------------------------------ + + def _initialize_fabric(self) -> None: + """One-time Fabric setup: hierarchy handle, attribute population, selections, indexed arrays.""" + import usdrt # noqa: PLC0415 + from usdrt import Rt # noqa: PLC0415 + + self._stage_id = sim_utils.get_current_stage_id() + self._stage = usdrt.Usd.Stage.Attach(self._stage_id) + self._stage.SynchronizeToFabric() + + # Reuse (or create) a hierarchy handle for this stage. The cache key is + # ``(stage_id, fabric_id_int)`` so a recycled ``stage_id`` paired with a fresh + # Fabric attachment never returns a stale handle. Note: ``GetFabricId()`` + # returns a fresh ``FabricId`` *wrapper* on every call, and the wrapper has + # no value equality (two wrappers for the same underlying Fabric compare + # unequal and hash differently), so we key on its stable ``.id`` int. + # Enable change-tracking BEFORE we author any local matrices, so the per-prim + # ``SetLocalXformFromUsd`` calls below mark themselves dirty. + fabric_id = self._stage.GetFabricId() + self._fabric_id = fabric_id.id + cache_key = (self._stage_id, fabric_id.id) + if cache_key not in FabricFrameView._static_hierarchy_cache: + hierarchy = usdrt.hierarchy.IFabricHierarchy().get_fabric_hierarchy( + fabric_id, self._stage.GetStageIdAsStageId() + ) + hierarchy.track_local_xform_changes(True) + hierarchy.track_world_xform_changes(True) + FabricFrameView._static_hierarchy_cache[cache_key] = hierarchy + self._fabric_hierarchy = FabricFrameView._static_hierarchy_cache[cache_key] + + # Ensure each child prim AND its parent have BOTH Fabric world and local matrix + # attributes. Our ``trans_ro`` selection requires both, so prims missing either + # would silently be excluded. ``Create*Attr`` calls are idempotent. + # + # ``SetWorldXformFromUsd`` writes Fabric's worldMatrix from USD's accumulated + # local-to-world transform (so it picks up the parent chain). + # ``SetLocalXformFromUsd`` writes Fabric's localMatrix from USD's authored + # xformOps on this prim only. Calling both gives Fabric a consistent + # (worldMatrix, localMatrix) pair for each prim before we touch the hierarchy. + seen_paths: set[str] = set() + for child_path in self.prim_paths: + for path in (child_path, child_path.rsplit("/", 1)[0]): + if path in seen_paths: + continue + seen_paths.add(path) + rt_prim = self._stage.GetPrimAtPath(path) + if not rt_prim.IsValid(): + continue + rt_xformable = Rt.Xformable(rt_prim) + rt_xformable.CreateFabricHierarchyWorldMatrixAttr() + rt_xformable.CreateFabricHierarchyLocalMatrixAttr() + rt_xformable.SetLocalXformFromUsd() + rt_xformable.SetWorldXformFromUsd() + + # Three persistent selections — read both, write world, write local. + matrix = usdrt.Sdf.ValueTypeNames.Matrix4d + ro = usdrt.Usd.Access.Read + rw = usdrt.Usd.Access.ReadWrite + wm_ro = (matrix, self._WORLD_MATRIX_NAME, ro) + lm_ro = (matrix, self._LOCAL_MATRIX_NAME, ro) + wm_rw = (matrix, self._WORLD_MATRIX_NAME, rw) + lm_rw = (matrix, self._LOCAL_MATRIX_NAME, rw) + self._trans_sel_ro = self._stage.SelectPrims(require_attrs=[wm_ro, lm_ro], device=self._device, want_paths=True) + self._world_sel_rw = self._stage.SelectPrims(require_attrs=[wm_rw, lm_ro], device=self._device, want_paths=True) + self._local_sel_rw = self._stage.SelectPrims(require_attrs=[wm_ro, lm_rw], device=self._device, want_paths=True) + + # Build the view-side indices array (just [0..count-1]) and a per-selection + # view→fabric mapping (selections do not guarantee a shared path ordering). + self._view_indices = wp.array(list(range(self.count)), dtype=wp.uint32, device=self._device) + self._trans_ro_fabric_indices = self._compute_fabric_indices(self._trans_sel_ro) + self._world_rw_fabric_indices = self._compute_fabric_indices(self._world_sel_rw) + self._local_rw_fabric_indices = self._compute_fabric_indices(self._local_sel_rw) + + # Indexed fabric arrays per (selection × attribute). + self._world_ifa_ro = self._build_indexed_array( + self._trans_sel_ro, self._WORLD_MATRIX_NAME, self._trans_ro_fabric_indices ) - wp.synchronize() + self._local_ifa_ro = self._build_indexed_array( + self._trans_sel_ro, self._LOCAL_MATRIX_NAME, self._trans_ro_fabric_indices + ) + self._world_ifa_rw = self._build_indexed_array( + self._world_sel_rw, self._WORLD_MATRIX_NAME, self._world_rw_fabric_indices + ) + self._local_ifa_rw = self._build_indexed_array( + self._local_sel_rw, self._LOCAL_MATRIX_NAME, self._local_rw_fabric_indices + ) + self._parent_world_ifa_ro = self._build_parent_indexed_array(self._trans_sel_ro) + # Pre-allocated reusable output buffers (world + local + scales). self._fabric_positions_buf = wp.zeros((self.count, 3), dtype=wp.float32, device=self._device) self._fabric_orientations_buf = wp.zeros((self.count, 4), dtype=wp.float32, device=self._device) + self._fabric_scales_buf = wp.zeros((self.count, 3), dtype=wp.float32, device=self._device) + self._fabric_local_translations_buf = wp.zeros((self.count, 3), dtype=wp.float32, device=self._device) + self._fabric_local_orientations_buf = wp.zeros((self.count, 4), dtype=wp.float32, device=self._device) + self._fabric_empty_2d_array_sentinel = wp.zeros((0, 0), dtype=wp.float32, device=self._device) + self._fabric_positions_ta = ProxyArray(self._fabric_positions_buf) self._fabric_orientations_ta = ProxyArray(self._fabric_orientations_buf) - self._fabric_scales_buf = wp.zeros((self.count, 3), dtype=wp.float32, device=self._device) - self._fabric_dummy_buffer = wp.zeros((0, 3), dtype=wp.float32, device=self._device) - self._fabric_world_matrices = wp.fabricarray(self._fabric_selection, "omni:fabric:worldMatrix") - self._fabric_stage = fabric_stage - self._fabric_device = fabric_device + self._fabric_local_translations_ta = ProxyArray(self._fabric_local_translations_buf) + self._fabric_local_orientations_ta = ProxyArray(self._fabric_local_orientations_buf) self._fabric_initialized = True - self._fabric_usd_sync_done = False - def _sync_fabric_from_usd_once(self) -> None: - """Sync Fabric world matrices from USD once, on the first read.""" - if not self._fabric_initialized: - self._initialize_fabric() - - positions_usd_ta, orientations_usd_ta = self._usd_view.get_world_poses() - positions_usd = positions_usd_ta.warp - orientations_usd = orientations_usd_ta.warp - scales_usd = self._usd_view.get_scales() - - prev_sync = self._sync_usd_on_fabric_write - self._sync_usd_on_fabric_write = False - self.set_world_poses(positions_usd, orientations_usd) - self.set_scales(scales_usd) - self._sync_usd_on_fabric_write = prev_sync + # Seed Fabric matrices from USD authoritatively. ``SetWorldXformFromUsd`` / + # ``SetLocalXformFromUsd`` are no-ops on freshly authored stages that haven't + # been rendered yet; we instead read through the USD view (children) and + # ``UsdGeom.XformCache`` (parents) and write via the same compose kernel that + # ``set_world_poses`` uses. + self._sync_fabric_from_usd_initial() + + def _sync_fabric_from_usd_initial(self) -> None: + """Populate Fabric world+local matrices for children and parents from USD. + + Performed once during ``_initialize_fabric``. Without this step Fabric's + matrices are identity for stages that haven't been rendered yet, and our + getters (which read from Fabric) would return wrong values. + """ + # --- Children --- + pos_ta, ori_ta = self._usd_view.get_world_poses() + scales_obj = self._usd_view.get_scales() + scales_wp = ( + scales_obj.warp + if hasattr(scales_obj, "warp") + else scales_obj + if isinstance(scales_obj, wp.array) + else self._fabric_empty_2d_array_sentinel + ) + local_pos_ta, local_ori_ta = self._usd_view.get_local_poses() + # Compose into child worldMatrix. + wp.launch( + kernel=fabric_utils.compose_indexed_fabric_transforms, + dim=self.count, + inputs=[ + self._world_ifa_rw, + _to_float32_2d(pos_ta.warp), + _to_float32_2d(ori_ta.warp), + _to_float32_2d(scales_wp), + False, + False, + False, + self._view_indices, + ], + device=self._device, + ) + # Compose into child localMatrix. Pass the locally-authored scale so + # that a subsequent ``_sync_world_from_local_if_dirty`` produces the + # right world-space scale (``world = parent_world * local`` carries + # ``local``'s scale through the multiply). + wp.launch( + kernel=fabric_utils.compose_indexed_fabric_transforms, + dim=self.count, + inputs=[ + self._local_ifa_rw, + _to_float32_2d(local_pos_ta.warp), + _to_float32_2d(local_ori_ta.warp), + _to_float32_2d(scales_wp), + False, + False, + False, + self._view_indices, + ], + device=self._device, + ) - self._fabric_usd_sync_done = True + # --- Parents (one entry per unique parent path) --- + unique_parent_paths = list(dict.fromkeys(p.rsplit("/", 1)[0] for p in self.prim_paths)) + if unique_parent_paths: + usd_stage = sim_utils.get_current_stage() + xform_cache = UsdGeom.XformCache(Usd.TimeCode.Default()) + world_pos_rows: list[list[float]] = [] + world_ori_rows: list[list[float]] = [] + world_scale_rows: list[list[float]] = [] + decomposer = Gf.Transform() + for path in unique_parent_paths: + prim = usd_stage.GetPrimAtPath(path) + tf = xform_cache.GetLocalToWorldTransform(prim) + # Extract scale before ``Orthonormalize`` strips it from the rows. + decomposer.SetMatrix(tf) + s = decomposer.GetScale() + tf.Orthonormalize() + t = tf.ExtractTranslation() + q = tf.ExtractRotationQuat() + img, real = q.GetImaginary(), q.GetReal() + world_pos_rows.append([float(t[0]), float(t[1]), float(t[2])]) + world_ori_rows.append([float(img[0]), float(img[1]), float(img[2]), float(real)]) + world_scale_rows.append([float(s[0]), float(s[1]), float(s[2])]) + parent_view_indices = wp.array(list(range(len(unique_parent_paths))), dtype=wp.uint32, device=self._device) + parent_pos_wp = wp.array(world_pos_rows, dtype=wp.float32, device=self._device) + parent_ori_wp = wp.array(world_ori_rows, dtype=wp.float32, device=self._device) + parent_scale_wp = wp.array(world_scale_rows, dtype=wp.float32, device=self._device) + # Compose worldMatrix for parents (use a one-shot indexed array against + # ``world_sel_rw`` keyed on the unique parent paths). + parent_world_rw = wp.indexedfabricarray( + fa=wp.fabricarray(self._world_sel_rw, self._WORLD_MATRIX_NAME), + indices=self._compute_fabric_indices_for(self._world_sel_rw, unique_parent_paths), + ) + wp.launch( + kernel=fabric_utils.compose_indexed_fabric_transforms, + dim=len(unique_parent_paths), + inputs=[ + parent_world_rw, + parent_pos_wp, + parent_ori_wp, + parent_scale_wp, + False, + False, + False, + parent_view_indices, + ], + device=self._device, + ) + wp.synchronize() - def _resolve_indices_wp(self, indices: wp.array | None) -> wp.array: - """Resolve view indices as a Warp uint32 array.""" - if indices is None or indices == slice(None): - if self._default_view_indices is None: - raise RuntimeError("Fabric indices are not initialized.") - return self._default_view_indices - if indices.dtype != wp.uint32: - return wp.array(indices.numpy().astype("uint32"), dtype=wp.uint32, device=self._device) - return indices + # The child worldMatrix above was composed with the child's *local* scale, + # which is wrong whenever a parent has a non-unit world scale. Mark the + # view dirty so the next world read fires ``_sync_world_from_local_if_dirty`` + # and recomputes ``child_world = parent_world * child_local`` — that + # multiply produces the correct world-space scale because the parent and + # local matrices now both carry the right scale (seeded above). + self._world_dirty = True + + def _compute_fabric_indices_for(self, selection, paths: list[str]) -> wp.array: + """Path-dict lookup helper used to build one-shot indexed arrays for a custom path set.""" + fabric_paths = selection.GetPaths() + path_to_idx = {str(p): i for i, p in enumerate(fabric_paths)} + indices: list[int] = [] + for path in paths: + idx = path_to_idx.get(path) + if idx is None: + raise RuntimeError(f"Path '{path}' not found in Fabric selection.") + indices.append(idx) + return wp.array(indices, dtype=wp.int32, device=self._device) diff --git a/source/isaaclab_physx/setup.py b/source/isaaclab_physx/setup.py index 1e917e938c2b..09fc76bdac69 100644 --- a/source/isaaclab_physx/setup.py +++ b/source/isaaclab_physx/setup.py @@ -20,7 +20,7 @@ EXTRAS_REQUIRE = { "newton": [ - "newton @ git+https://github.com/newton-physics/newton.git@2684d75bfa4bb8b058a93b81c458a74b7701c997", + "newton[sim] @ git+https://github.com/newton-physics/newton.git@v1.2.0rc2", ], } @@ -50,7 +50,6 @@ "isaaclab_physx.cloner", "isaaclab_physx.physics", "isaaclab_physx.renderers", - "isaaclab_physx.scene_data_providers", "isaaclab_physx.sensors", "isaaclab_physx.sensors.contact_sensor", "isaaclab_physx.sensors.frame_transformer", diff --git a/source/isaaclab_physx/test/assets/test_articulation.py b/source/isaaclab_physx/test/assets/test_articulation.py index 3687dae5961d..af36a365cb66 100644 --- a/source/isaaclab_physx/test/assets/test_articulation.py +++ b/source/isaaclab_physx/test/assets/test_articulation.py @@ -29,16 +29,23 @@ import isaaclab.utils.string as string_utils from isaaclab.actuators import ActuatorBase, IdealPDActuatorCfg, ImplicitActuatorCfg from isaaclab.assets import ArticulationCfg +from isaaclab.controllers import ( + DifferentialIKController, + DifferentialIKControllerCfg, + OperationalSpaceController, + OperationalSpaceControllerCfg, +) from isaaclab.envs.mdp.terminations import joint_effort_out_of_limit from isaaclab.managers import SceneEntityCfg from isaaclab.sim import build_simulation_context from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR +from isaaclab.utils.math import compute_pose_error, matrix_from_quat, quat_inv, subtract_frame_transforms from isaaclab.utils.version import get_isaac_sim_version, has_kit ## # Pre-defined configs ## -from isaaclab_assets import ANYMAL_C_CFG, FRANKA_PANDA_CFG, SHADOW_HAND_CFG # isort:skip +from isaaclab_assets import ANYMAL_C_CFG, FRANKA_PANDA_CFG, FRANKA_PANDA_HIGH_PD_CFG, SHADOW_HAND_CFG # isort:skip def generate_articulation_cfg( @@ -92,7 +99,7 @@ def generate_articulation_cfg( # we set 80.0 default for max force because default in USD is 10e10 which makes testing annoying. spawn=sim_utils.UsdFileCfg( usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/IsaacSim/SimpleArticulation/revolute_articulation.usd", - joint_drive_props=sim_utils.JointDrivePropertiesCfg(max_effort=80.0, max_velocity=5.0), + joint_drive_props=sim_utils.JointDrivePropertiesCfg(max_force=80.0, max_joint_velocity=5.0), ), actuators={ "joint": ImplicitActuatorCfg( @@ -116,7 +123,7 @@ def generate_articulation_cfg( articulation_cfg = ArticulationCfg( spawn=sim_utils.UsdFileCfg( usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/IsaacSim/SimpleArticulation/revolute_articulation.usd", - joint_drive_props=sim_utils.JointDrivePropertiesCfg(max_effort=80.0, max_velocity=5.0), + joint_drive_props=sim_utils.JointDrivePropertiesCfg(max_force=80.0, max_joint_velocity=5.0), ), actuators={ "joint": IdealPDActuatorCfg( @@ -182,6 +189,107 @@ def generate_articulation( return articulation, translations +# --------------------------------------------------------------------------- +# Franka task-space tracking helpers (shared between IK and OSC tests). +# Mirrors the helpers in ``isaaclab_newton/test/assets/test_articulation.py``. +# --------------------------------------------------------------------------- + + +def _setup_franka_at_home_pose(sim, *, zero_actuator_pd: bool = False, enable_rigid_body_gravity: bool = False): + """Build a Franka articulation at its configured home pose. + + See the Newton-side mirror for full docs. Standalone tests skip the + env reset path that normally pushes ``default_joint_pos`` to sim, + so we teleport explicitly to avoid the URDF-neutral + near-singular pose where the Franka wrist axes nearly align. + + Args: + sim: The simulation context to use. + zero_actuator_pd: If True, sets the panda_shoulder/panda_forearm + actuator stiffness and damping to zero. + enable_rigid_body_gravity: If True, override + ``FRANKA_PANDA_HIGH_PD_CFG.spawn.rigid_props.disable_gravity`` + (which defaults to True) so gravity actually loads the arm. Required + for any test that wants to exercise gravity-related dynamics + (e.g. gravity-compensation accuracy tests). + + Returns: + Tuple of ``(robot, ee_frame_idx, ee_jacobi_idx, arm_joint_ids)``. + """ + cfg = FRANKA_PANDA_HIGH_PD_CFG.copy().replace(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 + if enable_rigid_body_gravity: + cfg = cfg.replace( + spawn=cfg.spawn.replace( + rigid_props=cfg.spawn.rigid_props.replace(disable_gravity=False), + ), + ) + sim_utils.create_prim("/World/Env_0", "Xform", translation=(0.0, 0.0, 0.0)) + robot = Articulation(cfg) + sim.reset() + assert robot.is_initialized + + ee_frame_idx = robot.find_bodies("panda_hand")[0][0] + ee_jacobi_idx = ee_frame_idx - 1 + arm_joint_ids = robot.find_joints(["panda_joint.*"])[0] + + robot.write_joint_state_to_sim( + position=robot.data.default_joint_pos.torch[:, :].clone(), + velocity=robot.data.default_joint_vel.torch[:, :].clone(), + ) + return robot, ee_frame_idx, ee_jacobi_idx, arm_joint_ids + + +def _compute_ee_pose_root(robot, ee_frame_idx): + """Return ``(ee_pos_b, ee_quat_b, root_pose_w)`` in the root frame.""" + ee_pose_w = robot.data.body_pose_w.torch[:, ee_frame_idx] + root_pose_w = robot.data.root_pose_w.torch + ee_pos_b, ee_quat_b = subtract_frame_transforms( + root_pose_w[:, 0:3], root_pose_w[:, 3:7], ee_pose_w[:, 0:3], ee_pose_w[:, 3:7] + ) + return ee_pos_b, ee_quat_b, root_pose_w + + +def _compute_jacobian_root_frame(robot, ee_jacobi_idx, arm_joint_ids): + """Return the EE Jacobian sliced to ``arm_joint_ids`` and rotated to the root frame.""" + jacobian = robot.data.body_link_jacobian_w.torch[:, ee_jacobi_idx, :, :][:, :, arm_joint_ids] + base_rot_matrix = matrix_from_quat(quat_inv(robot.data.root_pose_w.torch[:, 3:7])) + jacobian[:, :3, :] = torch.bmm(base_rot_matrix, jacobian[:, :3, :]) + jacobian[:, 3:, :] = torch.bmm(base_rot_matrix, jacobian[:, 3:, :]) + return jacobian + + +def _compute_ee_vel_root(jacobian_b, joint_vel): + """Return the EE 6D velocity in the root frame as ``J · q_dot``. + + Required to make OSC's ``kd * ee_vel_b`` damping term meaningful. + Passing zero EE velocity (the convenient hack) leaves the impedance + undamped and the EE oscillates around the target. ``J · q_dot`` + avoids relying on ``data.body_vel_w`` (Newton's lazy velocity + buffers can return stale/zero values until forced materialization), + keeping the helper backend-symmetric. ``J`` correctness is pinned + independently by ``test_get_jacobians_link_origin_contract``. + """ + return torch.bmm(jacobian_b, joint_vel.unsqueeze(-1)).squeeze(-1) + + +def _build_relative_pose_target(robot, ee_frame_idx, delta_xyz, device): + """Build a target pose = (current EE pose) + ``delta_xyz``, preserving orientation.""" + initial_ee_pos_b, initial_ee_quat_b, _ = _compute_ee_pose_root(robot, ee_frame_idx) + target_pos_b = initial_ee_pos_b + torch.tensor([list(delta_xyz)], device=device, dtype=initial_ee_pos_b.dtype) + return torch.cat([target_pos_b, initial_ee_quat_b], dim=-1) + + +def _summarize_history(history, tail: int = 200): + """Return ``(min, mean)`` over the last ``tail`` samples.""" + tail_slice = history[-tail:] + return min(tail_slice), sum(tail_slice) / len(tail_slice) + + @pytest.fixture def sim(request): """Create simulation context with the specified device.""" @@ -204,7 +312,6 @@ def sim(request): @pytest.mark.parametrize("num_articulations", [1, 2]) @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.parametrize("add_ground_plane", [True]) -@pytest.mark.isaacsim_ci def test_initialization_floating_base_non_root(sim, num_articulations, device, add_ground_plane): """Test initialization for a floating-base with articulation root on a rigid body. @@ -261,7 +368,6 @@ def test_initialization_floating_base_non_root(sim, num_articulations, device, a @pytest.mark.parametrize("num_articulations", [1, 2]) @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.parametrize("add_ground_plane", [True]) -@pytest.mark.isaacsim_ci def test_initialization_floating_base(sim, num_articulations, device, add_ground_plane): """Test initialization for a floating-base with articulation root on provided prim path. @@ -318,7 +424,6 @@ def test_initialization_floating_base(sim, num_articulations, device, add_ground @pytest.mark.parametrize("num_articulations", [1, 2]) @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) -@pytest.mark.isaacsim_ci def test_initialization_fixed_base(sim, num_articulations, device): """Test initialization for fixed base. @@ -384,7 +489,6 @@ def test_initialization_fixed_base(sim, num_articulations, device): @pytest.mark.parametrize("num_articulations", [1, 2]) @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.parametrize("add_ground_plane", [True]) -@pytest.mark.isaacsim_ci def test_initialization_fixed_base_single_joint(sim, num_articulations, device, add_ground_plane): """Test initialization for fixed base articulation with a single joint. @@ -449,7 +553,6 @@ def test_initialization_fixed_base_single_joint(sim, num_articulations, device, @pytest.mark.parametrize("num_articulations", [1, 2]) @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) -@pytest.mark.isaacsim_ci def test_initialization_hand_with_tendons(sim, num_articulations, device): """Test initialization for fixed base articulated hand with tendons. @@ -504,7 +607,6 @@ def test_initialization_hand_with_tendons(sim, num_articulations, device): @pytest.mark.parametrize("num_articulations", [1, 2]) @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.parametrize("add_ground_plane", [True]) -@pytest.mark.isaacsim_ci def test_initialization_floating_base_made_fixed_base(sim, num_articulations, device, add_ground_plane): """Test initialization for a floating-base articulation made fixed-base using schema properties. @@ -565,7 +667,6 @@ def test_initialization_floating_base_made_fixed_base(sim, num_articulations, de @pytest.mark.parametrize("num_articulations", [1, 2]) @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.parametrize("add_ground_plane", [True]) -@pytest.mark.isaacsim_ci def test_initialization_fixed_base_made_floating_base(sim, num_articulations, device, add_ground_plane): """Test initialization for fixed base made floating-base using schema properties. @@ -579,7 +680,7 @@ def test_initialization_fixed_base_made_floating_base(sim, num_articulations, de sim: The simulation fixture num_articulations: Number of articulations to test """ - articulation_cfg = generate_articulation_cfg(articulation_type="panda") + articulation_cfg = generate_articulation_cfg(articulation_type="panda").copy() # Unfix root link by making it non-kinematic articulation_cfg.spawn.articulation_props.fix_root_link = False articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=sim.device) @@ -618,7 +719,6 @@ def test_initialization_fixed_base_made_floating_base(sim, num_articulations, de @pytest.mark.parametrize("num_articulations", [1, 2]) @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.parametrize("add_ground_plane", [True]) -@pytest.mark.isaacsim_ci def test_out_of_range_default_joint_pos(sim, num_articulations, device, add_ground_plane): """Test that the default joint position from configuration is out of range. @@ -648,7 +748,6 @@ def test_out_of_range_default_joint_pos(sim, num_articulations, device, add_grou @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) -@pytest.mark.isaacsim_ci def test_out_of_range_default_joint_vel(sim, device): """Test that the default joint velocity from configuration is out of range. @@ -674,7 +773,6 @@ def test_out_of_range_default_joint_vel(sim, device): @pytest.mark.parametrize("num_articulations", [1, 2]) @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.parametrize("add_ground_plane", [True]) -@pytest.mark.isaacsim_ci def test_joint_pos_limits(sim, num_articulations, device, add_ground_plane): """Test write_joint_limits_to_sim API and when default pos falls outside of the new limits. @@ -782,7 +880,6 @@ def __init__(self, art): @pytest.mark.parametrize("num_articulations", [1, 2]) @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) -@pytest.mark.isaacsim_ci def test_external_force_buffer(sim, num_articulations, device): """Test if external force buffer correctly updates in the force value is zero case. @@ -844,8 +941,8 @@ def test_external_force_buffer(sim, num_articulations, device): # check if the articulation's force and torque buffers are correctly updated for i in range(num_articulations): - assert articulation.permanent_wrench_composer.composed_force.torch[i, 0, 0].item() == force - assert articulation.permanent_wrench_composer.composed_torque.torch[i, 0, 0].item() == force + assert articulation.permanent_wrench_composer.out_force_b.torch[i, 0, 0].item() == force + assert articulation.permanent_wrench_composer.out_torque_b.torch[i, 0, 0].item() == force # Check if the instantaneous wrench is correctly added to the permanent wrench articulation.instantaneous_wrench_composer.add_forces_and_torques_index( @@ -867,7 +964,6 @@ def test_external_force_buffer(sim, num_articulations, device): @pytest.mark.parametrize("num_articulations", [1, 2]) @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) -@pytest.mark.isaacsim_ci def test_external_force_on_single_body(sim, num_articulations, device): """Test application of external force on the base of the articulation. @@ -925,7 +1021,6 @@ def test_external_force_on_single_body(sim, num_articulations, device): @pytest.mark.parametrize("num_articulations", [1, 2]) @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) -@pytest.mark.isaacsim_ci def test_external_force_on_single_body_at_position(sim, num_articulations, device): """Test application of external force on the base of the articulation at a given position. @@ -1020,7 +1115,6 @@ def test_external_force_on_single_body_at_position(sim, num_articulations, devic @pytest.mark.parametrize("num_articulations", [1, 2]) @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) -@pytest.mark.isaacsim_ci def test_external_force_on_multiple_bodies(sim, num_articulations, device): """Test application of external force on the legs of the articulation. @@ -1080,7 +1174,6 @@ def test_external_force_on_multiple_bodies(sim, num_articulations, device): @pytest.mark.parametrize("num_articulations", [1, 2]) @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) -@pytest.mark.isaacsim_ci def test_external_force_on_multiple_bodies_at_position(sim, num_articulations, device): """Test application of external force on the legs of the articulation at a given position. @@ -1174,7 +1267,6 @@ def test_external_force_on_multiple_bodies_at_position(sim, num_articulations, d @pytest.mark.parametrize("num_articulations", [1, 2]) @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) -@pytest.mark.isaacsim_ci def test_loading_gains_from_usd(sim, num_articulations, device): """Test that gains are loaded from USD file if actuator model has them as None. @@ -1237,7 +1329,6 @@ def test_loading_gains_from_usd(sim, num_articulations, device): @pytest.mark.parametrize("num_articulations", [1, 2]) @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.parametrize("add_ground_plane", [True]) -@pytest.mark.isaacsim_ci def test_setting_gains_from_cfg(sim, num_articulations, device, add_ground_plane): """Test that gains are loaded from the configuration correctly. @@ -1271,7 +1362,6 @@ def test_setting_gains_from_cfg(sim, num_articulations, device, add_ground_plane @pytest.mark.parametrize("num_articulations", [1, 2]) @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) -@pytest.mark.isaacsim_ci def test_setting_gains_from_cfg_dict(sim, num_articulations, device): """Test that gains are loaded from the configuration dictionary correctly. @@ -1307,7 +1397,6 @@ def test_setting_gains_from_cfg_dict(sim, num_articulations, device): @pytest.mark.parametrize("vel_limit_sim", [1e5, None]) @pytest.mark.parametrize("vel_limit", [1e2, None]) @pytest.mark.parametrize("add_ground_plane", [False]) -@pytest.mark.isaacsim_ci def test_setting_velocity_limit_implicit(sim, num_articulations, device, vel_limit_sim, vel_limit, add_ground_plane): """Test setting of velocity limit for implicit actuators. @@ -1359,7 +1448,7 @@ def test_setting_velocity_limit_implicit(sim, num_articulations, device, vel_lim # Case 3: velocity limit sim is not set but velocity limit is set # For backwards compatibility, we do not set velocity limit to simulation # Thus, both default to USD default value. - limit = articulation_cfg.spawn.joint_drive_props.max_velocity + limit = articulation_cfg.spawn.joint_drive_props.max_joint_velocity else: # Case 4: only velocity limit sim is set # In this case, the velocity limit is set to the USD value @@ -1374,7 +1463,6 @@ def test_setting_velocity_limit_implicit(sim, num_articulations, device, vel_lim @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.parametrize("vel_limit_sim", [1e5, None]) @pytest.mark.parametrize("vel_limit", [1e2, None]) -@pytest.mark.isaacsim_ci def test_setting_velocity_limit_explicit(sim, num_articulations, device, vel_limit_sim, vel_limit): """Test setting of velocity limit for explicit actuators.""" articulation_cfg = generate_articulation_cfg( @@ -1418,7 +1506,7 @@ def test_setting_velocity_limit_explicit(sim, num_articulations, device, vel_lim if vel_limit_sim is not None: limit = vel_limit_sim else: - limit = articulation_cfg.spawn.joint_drive_props.max_velocity + limit = articulation_cfg.spawn.joint_drive_props.max_joint_velocity # check physx is set to expected value expected_vel_limit = torch.full_like(physx_vel_limit, limit) torch.testing.assert_close(physx_vel_limit, expected_vel_limit) @@ -1428,7 +1516,6 @@ def test_setting_velocity_limit_explicit(sim, num_articulations, device, vel_lim @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.parametrize("effort_limit_sim", [1e5, None]) @pytest.mark.parametrize("effort_limit", [1e2, 80.0, None]) -@pytest.mark.isaacsim_ci def test_setting_effort_limit_implicit(sim, num_articulations, device, effort_limit_sim, effort_limit): """Test setting of effort limit for implicit actuators. @@ -1466,7 +1553,7 @@ def test_setting_effort_limit_implicit(sim, num_articulations, device, effort_li # decide the limit based on what is set if effort_limit_sim is None and effort_limit is None: - limit = articulation_cfg.spawn.joint_drive_props.max_effort + limit = articulation_cfg.spawn.joint_drive_props.max_force elif effort_limit_sim is not None and effort_limit is None: limit = effort_limit_sim elif effort_limit_sim is None and effort_limit is not None: @@ -1481,7 +1568,6 @@ def test_setting_effort_limit_implicit(sim, num_articulations, device, effort_li @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.parametrize("effort_limit_sim", [1e5, None]) @pytest.mark.parametrize("effort_limit", [80.0, 1e2, None]) -@pytest.mark.isaacsim_ci def test_setting_effort_limit_explicit(sim, num_articulations, device, effort_limit_sim, effort_limit): """Test setting of effort limit for explicit actuators. @@ -1541,7 +1627,6 @@ def test_setting_effort_limit_explicit(sim, num_articulations, device, effort_li @pytest.mark.parametrize("num_articulations", [1, 2]) @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) -@pytest.mark.isaacsim_ci def test_reset(sim, num_articulations, device): """Test that reset method works properly.""" articulation_cfg = generate_articulation_cfg(articulation_type="humanoid") @@ -1559,10 +1644,10 @@ def test_reset(sim, num_articulations, device): # Reset should zero external forces and torques assert not articulation._instantaneous_wrench_composer.active assert not articulation._permanent_wrench_composer.active - assert torch.count_nonzero(articulation._instantaneous_wrench_composer.composed_force.torch) == 0 - assert torch.count_nonzero(articulation._instantaneous_wrench_composer.composed_torque.torch) == 0 - assert torch.count_nonzero(articulation._permanent_wrench_composer.composed_force.torch) == 0 - assert torch.count_nonzero(articulation._permanent_wrench_composer.composed_torque.torch) == 0 + assert torch.count_nonzero(articulation._instantaneous_wrench_composer.out_force_b.torch) == 0 + assert torch.count_nonzero(articulation._instantaneous_wrench_composer.out_torque_b.torch) == 0 + assert torch.count_nonzero(articulation._permanent_wrench_composer.out_force_b.torch) == 0 + assert torch.count_nonzero(articulation._permanent_wrench_composer.out_torque_b.torch) == 0 if num_articulations > 1: num_bodies = articulation.num_bodies @@ -1577,16 +1662,15 @@ def test_reset(sim, num_articulations, device): articulation.reset(env_ids=torch.tensor([0], device=device)) assert articulation._instantaneous_wrench_composer.active assert articulation._permanent_wrench_composer.active - assert torch.count_nonzero(articulation._instantaneous_wrench_composer.composed_force.torch) == num_bodies * 3 - assert torch.count_nonzero(articulation._instantaneous_wrench_composer.composed_torque.torch) == num_bodies * 3 - assert torch.count_nonzero(articulation._permanent_wrench_composer.composed_force.torch) == num_bodies * 3 - assert torch.count_nonzero(articulation._permanent_wrench_composer.composed_torque.torch) == num_bodies * 3 + assert torch.count_nonzero(articulation._instantaneous_wrench_composer.out_force_b.torch) == num_bodies * 3 + assert torch.count_nonzero(articulation._instantaneous_wrench_composer.out_torque_b.torch) == num_bodies * 3 + assert torch.count_nonzero(articulation._permanent_wrench_composer.out_force_b.torch) == num_bodies * 3 + assert torch.count_nonzero(articulation._permanent_wrench_composer.out_torque_b.torch) == num_bodies * 3 @pytest.mark.parametrize("num_articulations", [1, 2]) @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.parametrize("add_ground_plane", [True]) -@pytest.mark.isaacsim_ci def test_apply_joint_command(sim, num_articulations, device, add_ground_plane): """Test applying of joint position target functions correctly for a robotic arm.""" articulation_cfg = generate_articulation_cfg(articulation_type="panda") @@ -1626,7 +1710,6 @@ def test_apply_joint_command(sim, num_articulations, device, add_ground_plane): @pytest.mark.parametrize("num_articulations", [1, 2]) @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.parametrize("with_offset", [True, False]) -@pytest.mark.isaacsim_ci def test_body_root_state(sim, num_articulations, device, with_offset): """Test for reading the `body_state_w` property. @@ -1752,7 +1835,6 @@ def test_body_root_state(sim, num_articulations, device, with_offset): @pytest.mark.parametrize("with_offset", [True, False]) @pytest.mark.parametrize("state_location", ["com", "link"]) @pytest.mark.parametrize("gravity_enabled", [False]) -@pytest.mark.isaacsim_ci def test_write_root_state(sim, num_articulations, device, with_offset, state_location, gravity_enabled): """Test the setters for root_state using both the link frame and center of mass as reference frame. @@ -1831,7 +1913,6 @@ def test_write_root_state(sim, num_articulations, device, with_offset, state_loc @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) -@pytest.mark.isaacsim_ci def test_setting_articulation_root_prim_path(sim, device): """Test that the articulation root prim path can be set explicitly.""" sim._app_control_on_stop_handle = None @@ -1850,7 +1931,6 @@ def test_setting_articulation_root_prim_path(sim, device): @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) -@pytest.mark.isaacsim_ci def test_setting_invalid_articulation_root_prim_path(sim, device): """Test that the articulation root prim path can be set explicitly.""" sim._app_control_on_stop_handle = None @@ -1870,7 +1950,6 @@ def test_setting_invalid_articulation_root_prim_path(sim, device): @pytest.mark.parametrize("num_articulations", [1, 2]) @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.parametrize("gravity_enabled", [False]) -@pytest.mark.isaacsim_ci def test_write_joint_state_data_consistency(sim, num_articulations, device, gravity_enabled): """Test the setters for root_state using both the link frame and center of mass as reference frame. @@ -2121,7 +2200,6 @@ def test_write_joint_frictions_to_sim(sim, num_articulations, device, add_ground @pytest.mark.parametrize("num_articulations", [1, 2]) @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.parametrize("articulation_type", ["panda"]) -@pytest.mark.isaacsim_ci def test_set_material_properties(sim, num_articulations, device, add_ground_plane, articulation_type): """Test getting and setting material properties (friction/restitution) of articulation shapes.""" articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) @@ -2157,5 +2235,635 @@ def test_set_material_properties(sim, num_articulations, device, add_ground_plan torch.testing.assert_close(materials_check, materials) +## +# Shape-contract regression tests for the new BaseArticulation accessors. +# Mirror the Newton-side tests so both backends can be diffed against the +# same documented contract. These are PhysX's reference shapes — when the +# Newton-side tests pass with the same expected_shape formulas, the +# cross-backend contract holds. +## + + +@pytest.mark.parametrize("num_articulations", [1, 4]) +@pytest.mark.parametrize("device", ["cuda:0"]) +@pytest.mark.parametrize("articulation_type", ["panda"]) +@pytest.mark.isaacsim_ci +def test_get_jacobians_shape_fixed_base(sim, num_articulations, device, articulation_type): + """PhysX reference: fixed-base ``body_link_jacobian_w`` is ``(N, num_bodies-1, 6, num_joints)``.""" + articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) + articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) + sim.reset() + assert articulation.is_initialized + assert articulation.is_fixed_base + + J = articulation.data.body_link_jacobian_w.torch + expected = (num_articulations, articulation.num_bodies - 1, 6, articulation.num_joints) + assert J.shape == torch.Size(expected), f"expected {expected}, got {tuple(J.shape)}" + + +@pytest.mark.parametrize("num_articulations", [1, 4]) +@pytest.mark.parametrize("device", ["cuda:0"]) +@pytest.mark.parametrize("articulation_type", ["panda"]) +@pytest.mark.isaacsim_ci +def test_get_mass_matrix_shape_and_nonsingular_fixed_base(sim, num_articulations, device, articulation_type): + """PhysX reference: fixed-base ``mass_matrix`` shape + non-singular.""" + articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) + articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) + sim.reset() + assert articulation.is_initialized + + sim.step() + articulation.update(sim.cfg.dt) + + M = articulation.data.mass_matrix.torch + expected = (num_articulations, articulation.num_joints, articulation.num_joints) + assert M.shape == torch.Size(expected), f"expected {expected}, got {tuple(M.shape)}" + + # Each diagonal entry is the joint's effective inertia and must be positive + # for any physical articulation. Padded zero rows/cols (the bug) would show + # up here as zero diagonal entries — much more sensitive than checking the + # determinant, which can be small for a well-conditioned 9x9 just from + # numerical cancellation. + diag = M.diagonal(dim1=-2, dim2=-1) + assert (diag > 1e-6).all(), f"mass matrix has non-positive diagonal entries: min={diag.min()}" + + +@pytest.mark.parametrize("num_articulations", [1, 4]) +@pytest.mark.parametrize("device", ["cuda:0"]) +@pytest.mark.parametrize("add_ground_plane", [True]) +@pytest.mark.parametrize("articulation_type", ["anymal"]) +@pytest.mark.isaacsim_ci +def test_get_jacobians_shape_floating_base(sim, num_articulations, device, add_ground_plane, articulation_type): + """PhysX reference: floating-base ``body_link_jacobian_w``. + + Floating-base articulations include the 6 floating-base spatial-velocity columns + at the front of the DoF axis, so the shape is + ``(N, num_bodies, 6, num_joints + num_base_dofs)`` — matching Newton and the + cross-library industry convention (Pinocchio, Drake, MuJoCo, RBDL, OCS2, + iDynTree). + """ + articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) + articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) + sim.reset() + assert articulation.is_initialized + assert not articulation.is_fixed_base + + J = articulation.data.body_link_jacobian_w.torch + expected = (num_articulations, articulation.num_bodies, 6, articulation.num_joints + articulation.num_base_dofs) + assert J.shape == torch.Size(expected), f"expected {expected}, got {tuple(J.shape)}" + + +@pytest.mark.parametrize("num_articulations", [4]) +@pytest.mark.parametrize("device", ["cuda:0"]) +@pytest.mark.parametrize("articulation_type", ["panda", "anymal"]) +@pytest.mark.parametrize("gravity_enabled", [False]) +@pytest.mark.isaacsim_ci +def test_get_jacobians_link_origin_contract(sim, num_articulations, device, articulation_type, gravity_enabled): + """PhysX reference: ``J · q_dot`` matches ``[body_link_lin_vel_w; body_link_ang_vel_w]``. + + The cross-backend contract on + :attr:`~isaaclab.assets.BaseArticulationData.body_link_jacobian_w` says + the Jacobian's linear rows reference each body's link origin. PhysX's + raw ``_root_view.get_jacobians()`` returns COM-referenced linear rows; + the IsaacLab wrapper applies the COM→origin shift kernel so the contract + holds. This test pins the identity from the PhysX side and parametrizes + on Anymal so the (non-trivial) shift surfaces if it ever regresses. + + Scene gravity is disabled (``gravity_enabled=False``) so the only source + of a J · q_dot ↔ body_*_w mismatch is the reference-point contract (or a + regression). The tolerance ``5e-2`` is loose enough to absorb the small + PhysX state-propagation lag between the Jacobian and the velocity + buffers (~2% on max angular speed) but well below the + COM-vs-link-origin bug magnitude (panda hand COM offset ≈ 3 cm × ω at + typical motion ≈ several rad/s gives a 0.1+ m/s linear-row residual, + 2× the tolerance). + """ + articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) + articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) + sim.reset() + assert articulation.is_initialized + + torch.manual_seed(0) + qdot = torch.randn(num_articulations, articulation.num_joints, device=device) * 0.5 + articulation.write_joint_velocity_to_sim(velocity=qdot) + sim.step() + articulation.update(sim.cfg.dt) + + # body_link_jacobian_w prepends ``num_base_dofs`` floating-base columns; slice past + # them so the joint axis aligns with joint_vel (actuated-only). + J = articulation.data.body_link_jacobian_w.torch[..., articulation.num_base_dofs :] + qdot_view = articulation.data.joint_vel.torch + v_pred = torch.einsum("nbij,nj->nbi", J, qdot_view) + + body_lin_w = articulation.data.body_link_lin_vel_w.torch + body_ang_w = articulation.data.body_link_ang_vel_w.torch + if articulation.is_fixed_base: + body_lin_w = body_lin_w[:, 1:] + body_ang_w = body_ang_w[:, 1:] + + torch.testing.assert_close(v_pred[..., 3:6], body_ang_w, atol=1.5e-1, rtol=5e-2) + torch.testing.assert_close(v_pred[..., 0:3], body_lin_w, atol=1.5e-1, rtol=5e-2) + + +@pytest.mark.parametrize("num_articulations", [4]) +@pytest.mark.parametrize("device", ["cuda:0"]) +@pytest.mark.parametrize("articulation_type", ["panda", "anymal"]) +@pytest.mark.parametrize("gravity_enabled", [False]) +@pytest.mark.isaacsim_ci +def test_get_mass_matrix_symmetry_pd(sim, num_articulations, device, articulation_type, gravity_enabled): + """The joint-space mass matrix ``M(q)`` must be square, symmetric, and positive-definite. + + Mirrors the Newton-side test in + ``source/isaaclab_newton/test/assets/test_articulation.py``. Pins + three structural properties of :attr:`~isaaclab.assets.BaseArticulationData.mass_matrix` + that every backend must satisfy. Both backends include the 6 floating-base + rows/cols on floating-base assets (matching the cross-library industry + convention); this test cares about square + symmetric + PD across both + fixed- and floating-base, not the absolute column count. + """ + articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) + articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) + sim.reset() + assert articulation.is_initialized + + sim.step() + articulation.update(sim.cfg.dt) + + M = articulation.data.mass_matrix.torch # (N, J, J) + assert M.dim() == 3, f"expected 3-D mass matrix, got shape {tuple(M.shape)}" + assert M.shape[0] == num_articulations + assert M.shape[1] == M.shape[2], f"mass matrix is not square: {tuple(M.shape)}" + + asym = (M - M.transpose(-1, -2)).abs().max().item() + assert asym < 1e-4, f"|M - M^T|_max = {asym:.3e} — mass matrix is not symmetric" + + eye = torch.eye(M.shape[-1], device=M.device, dtype=M.dtype).expand_as(M) + torch.linalg.cholesky(M + 1e-6 * eye) + + +@pytest.mark.parametrize("num_articulations", [1]) +@pytest.mark.parametrize("device", ["cuda:0"]) +@pytest.mark.parametrize("articulation_type", ["panda", "anymal"]) +@pytest.mark.parametrize("gravity_enabled", [False]) +@pytest.mark.isaacsim_ci +def test_jacobian_refreshes_after_manual_joint_write( + sim, num_articulations, device, articulation_type, gravity_enabled +): + """After ``write_joint_position_to_sim_index`` (no sim step), the Jacobian read + must reflect the new joint state — not the previous one. + + PhysX-side counterpart to the Newton test of the same name. PhysX's + :attr:`body_link_jacobian_w` triggers FK indirectly through + :attr:`body_link_pose_w` (used by the shift kernel); :attr:`body_com_jacobian_w` is + a passthrough to ``_root_view.get_jacobians()``. This test confirms that PhysX's + tensor view returns up-to-date Jacobians after a manual joint write — i.e., that + PhysX internally refreshes FK on ``get_jacobians`` (or that our property does). + Failure means we need to add ``update_articulations_kinematic()`` before the + passthrough. + """ + articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) + articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) + sim.reset() + sim.step() + articulation.update(sim.cfg.dt) + + # Read J at the baseline joint state. + J_link_0 = articulation.data.body_link_jacobian_w.torch.clone() + J_com_0 = articulation.data.body_com_jacobian_w.torch.clone() + + # Manually write a different joint state — large delta to make the change visible. + # No sim.step / update — FK becomes stale. + q_target = articulation.data.joint_pos.torch.clone() + 0.5 + env_ids = wp.array([0], dtype=wp.int32, device=device) + articulation.write_joint_position_to_sim_index(position=q_target, env_ids=env_ids) + + # Read J again. With the FK trigger, J reflects q_target and differs from J at baseline. + # Without the trigger, body_q stays at baseline, J unchanged. + J_link_1 = articulation.data.body_link_jacobian_w.torch.clone() + J_com_1 = articulation.data.body_com_jacobian_w.torch.clone() + + assert not torch.allclose(J_link_0, J_link_1, atol=1e-3), ( + "body_link_jacobian_w did not change after manual joint write — " + "FK trigger likely missing (eval_jacobian / shift kernel reading stale state.body_q)." + ) + assert not torch.allclose(J_com_0, J_com_1, atol=1e-3), ( + "body_com_jacobian_w did not change after manual joint write — " + "PhysX get_jacobians may not auto-refresh FK; consider adding update_articulations_kinematic()." + ) + + +@pytest.mark.parametrize("num_articulations", [1]) +@pytest.mark.parametrize("device", ["cuda:0"]) +@pytest.mark.parametrize("articulation_type", ["panda", "anymal"]) +@pytest.mark.parametrize("gravity_enabled", [False]) +@pytest.mark.isaacsim_ci +def test_mass_matrix_refreshes_after_manual_joint_write( + sim, num_articulations, device, articulation_type, gravity_enabled +): + """After ``write_joint_position_to_sim_index`` (no sim step), the mass matrix read + must reflect the new joint state. + + PhysX-side counterpart. :attr:`mass_matrix` is a passthrough to + ``_root_view.get_generalized_mass_matrices()``. Failure means PhysX's tensor view + does not auto-refresh FK on this getter, and we need to add + ``update_articulations_kinematic()`` before the passthrough. + """ + articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) + articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) + sim.reset() + sim.step() + articulation.update(sim.cfg.dt) + + M_0 = articulation.data.mass_matrix.torch.clone() + q_target = articulation.data.joint_pos.torch.clone() + 0.5 + env_ids = wp.array([0], dtype=wp.int32, device=device) + articulation.write_joint_position_to_sim_index(position=q_target, env_ids=env_ids) + M_1 = articulation.data.mass_matrix.torch.clone() + + assert not torch.allclose(M_0, M_1, atol=1e-3), ( + "mass_matrix did not change after manual joint write — " + "PhysX get_generalized_mass_matrices may not auto-refresh FK." + ) + + +@pytest.mark.parametrize("num_articulations", [1]) +@pytest.mark.parametrize("device", ["cuda:0"]) +@pytest.mark.parametrize("articulation_type", ["panda"]) +@pytest.mark.isaacsim_ci +def test_get_gravity_compensation_forces_static_equilibrium(sim, num_articulations, device, articulation_type): + """PhysX accuracy: ``τ_gc`` must hold the manipulator in static equilibrium. + + The contract is the EOM identity ``M(q) q̈ + C(q,q̇) q̇ + g(q) = τ_input``. + Setting ``τ_input = g(q)`` at ``q̇ = 0`` gives ``q̈ = 0`` — the arm should + not move. This pins + :attr:`~isaaclab.assets.BaseArticulationData.gravity_compensation_forces` + in isolation: sign errors, frame errors, and DoF-ordering errors all + surface as joint drift, while a controller-level test would have those + bugs averaged out by PD damping. + + Newton-side equivalent is deliberately omitted in this PR (see the + ``xfail`` test pinning the upstream gap). Newton's inverse-dynamics + primitive is in flight at upstream issues #2497 / #2529 and has a known + floating-base bug (#2625) that we'd have to test around. Ship a Newton + accuracy variant of this test alongside the Newton implementation when + upstream lands. + """ + base_cfg = generate_articulation_cfg(articulation_type=articulation_type) + # Replace default Franka actuators with a passthrough implicit actuator + # (stiffness = 0, damping = 0). With both gains zero the effort target + # we set IS the joint torque applied — no PD spring-damper masks the + # gravity-comp signal. Default Franka cfg has stiffness=80 / damping=4 + # which would absorb gravity through PD bias and hide accessor bugs. + cfg = base_cfg.replace( + actuators={ + "all": ImplicitActuatorCfg( + joint_names_expr=[".*"], + stiffness=0.0, + damping=0.0, + ), + }, + ) + # FRANKA_PANDA_CFG has rigid_props.disable_gravity=False already, but be + # defensive — gravity must be ON for τ_gc to have anything to cancel. + cfg = cfg.replace( + spawn=cfg.spawn.replace( + rigid_props=cfg.spawn.rigid_props.replace(disable_gravity=False), + ), + ) + + articulation, _ = generate_articulation(cfg, num_articulations, device=device) + sim.reset() + assert articulation.is_initialized + + # Force a clean static state: default joint positions, zero velocities. + # ``sim.reset`` may leave residual ``q_dot`` from solver settling under + # gravity, so we pin it explicitly here. + default_q = articulation.data.default_joint_pos.torch.clone() + default_qd = torch.zeros_like(default_q) + 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 + # (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 + # test uninformative. + init_q = articulation.data.joint_pos.torch.clone() + + # Step 100 times applying only τ_gc as joint efforts. + for _ in range(100): + # ``gravity_compensation_forces`` shape is ``(N, num_joints + num_base_dofs)`` + # — leading ``num_base_dofs`` floating-base entries (0 on fixed-base) followed + # by the actuated-joint entries. Slice past the floating-base entries so the + # remaining tensor aligns with ``set_joint_effort_target`` (actuated only). + tau_gc = articulation.data.gravity_compensation_forces.torch[:, articulation.num_base_dofs :] + articulation.set_joint_effort_target(tau_gc) + articulation.write_data_to_sim() + sim.step() + articulation.update(sim.cfg.dt) + + final_q = articulation.data.joint_pos.torch + drift = (final_q - init_q).abs().max() + # Tight bound: 5e-3 rad ≈ 0.3°. Numerical integration over 100 steps will + # accumulate some floor (sub-millirad on Franka), but a sign or frame bug + # in τ_gc produces drift of at least a degree per step on bent-elbow + # poses. This bound separates "correct" from "broken" cleanly. + assert drift < 5e-3, ( + f"max joint drift {drift:.5f} rad after 100 gravity-comp-only steps —" + " τ_gc did not hold static equilibrium. Check sign, DoF ordering, and" + " whether gravity_compensation_forces returns g(q) (positive) or" + " its negation." + ) + + +@pytest.mark.parametrize("device", ["cuda:0"]) +@pytest.mark.parametrize("articulation_type", ["panda"]) +@pytest.mark.parametrize("gravity_enabled", [False]) +@pytest.mark.isaacsim_ci +def test_franka_ik_tracking_accuracy(sim, device, articulation_type, gravity_enabled): + """PhysX-side IK convergence sentinel — backend parity with the Newton test. + + Mirrors :func:`isaaclab_newton.test.assets.test_articulation.test_franka_ik_tracking_accuracy` + so both backends are pinned by the same IK trajectory. With the + robot teleported to its configured init_state home pose and scene + gravity off, PhysX's IK converges to ~mm precision on this 5 cm + Cartesian step. A bridge regression (wrong J shape, wrong DoF + ordering) would push the steady-state error well past the + threshold. + """ + robot, ee_frame_idx, ee_jacobi_idx, arm_joint_ids = _setup_franka_at_home_pose(sim) + + sim.step() + robot.update(sim.cfg.dt) + target_pose_b = _build_relative_pose_target(robot, ee_frame_idx, (0.05, 0.0, 0.0), device) + + ik = DifferentialIKController( + DifferentialIKControllerCfg(command_type="pose", use_relative_mode=False, ik_method="dls"), + num_envs=1, + device=device, + ) + ik.set_command(target_pose_b) + + pos_history: list[float] = [] + rot_history: list[float] = [] + for _ in range(800): + jacobian = _compute_jacobian_root_frame(robot, ee_jacobi_idx, arm_joint_ids) + ee_pos_b, ee_quat_b, _ = _compute_ee_pose_root(robot, ee_frame_idx) + joint_pos = robot.data.joint_pos.torch[:, arm_joint_ids] + + joint_pos_des = ik.compute(ee_pos_b, ee_quat_b, jacobian, joint_pos) + + robot.set_joint_position_target(joint_pos_des, joint_ids=arm_joint_ids) + robot.write_data_to_sim() + sim.step() + robot.update(sim.cfg.dt) + + pos_error, rot_error = compute_pose_error(ee_pos_b, ee_quat_b, target_pose_b[:, 0:3], target_pose_b[:, 3:7]) + pos_history.append(pos_error.norm(dim=-1).max().item()) + rot_history.append(rot_error.norm(dim=-1).max().item()) + + pos_min, pos_mean = _summarize_history(pos_history) + rot_min, rot_mean = _summarize_history(rot_history) + + print(f"IK_METRIC pos_min={pos_min:.5f} pos_mean={pos_mean:.5f} rot_min={rot_min:.5f} rot_mean={rot_mean:.5f}") + + # Assert on tail mean (not min) so an oscillating envelope can't + # squeeze through. Threshold matched to the Newton-side test + # (5 mm / 0.05 rad). + assert pos_mean < 5e-3, f"IK pos_mean {pos_mean:.5f} > 5 mm — bridge regression?" + assert rot_mean < 5e-2, f"IK rot_mean {rot_mean:.5f} > 0.05 rad — bridge regression?" + + +@pytest.mark.parametrize("device", ["cuda:0"]) +@pytest.mark.parametrize("articulation_type", ["panda"]) +@pytest.mark.parametrize("gravity_enabled", [False]) +@pytest.mark.isaacsim_ci +def test_franka_osc_tracking_accuracy(sim, device, articulation_type, gravity_enabled): + """PhysX-side OSC pose tracking sentinel — backend parity with Newton. + + Mirrors :func:`isaaclab_newton.test.assets.test_articulation.test_franka_osc_tracking_accuracy`. + Zero out the actuator's PD gains so OSC's joint-effort output is + not opposed by the implicit-PD term, matching the Newton test setup. + """ + robot, ee_frame_idx, ee_jacobi_idx, arm_joint_ids = _setup_franka_at_home_pose(sim, zero_actuator_pd=True) + + osc = OperationalSpaceController( + OperationalSpaceControllerCfg( + target_types=["pose_abs"], + impedance_mode="fixed", + inertial_dynamics_decoupling=True, + partial_inertial_dynamics_decoupling=False, + gravity_compensation=False, + motion_stiffness_task=500.0, + motion_damping_ratio_task=1.0, + ), + num_envs=1, + device=device, + ) + + sim.step() + robot.update(sim.cfg.dt) + target_pose_b = _build_relative_pose_target(robot, ee_frame_idx, (0.05, 0.0, 0.0), device) + + pos_history: list[float] = [] + rot_history: list[float] = [] + for _ in range(800): + jacobian_b = _compute_jacobian_root_frame(robot, ee_jacobi_idx, arm_joint_ids) + mass_matrix = robot.data.mass_matrix.torch[:, arm_joint_ids, :][:, :, arm_joint_ids] + ee_pos_b, ee_quat_b, _ = _compute_ee_pose_root(robot, ee_frame_idx) + ee_pose_b = torch.cat([ee_pos_b, ee_quat_b], dim=-1) + joint_vel = robot.data.joint_vel.torch[:, arm_joint_ids] + ee_vel_b = _compute_ee_vel_root(jacobian_b, joint_vel) + + osc.set_command(target_pose_b, current_ee_pose_b=ee_pose_b) + joint_efforts = osc.compute( + jacobian_b=jacobian_b, + current_ee_pose_b=ee_pose_b, + current_ee_vel_b=ee_vel_b, + mass_matrix=mass_matrix, + gravity=None, + ) + + robot.set_joint_effort_target(joint_efforts, joint_ids=arm_joint_ids) + robot.write_data_to_sim() + sim.step() + robot.update(sim.cfg.dt) + + pos_error, rot_error = compute_pose_error(ee_pos_b, ee_quat_b, target_pose_b[:, 0:3], target_pose_b[:, 3:7]) + pos_history.append(pos_error.norm(dim=-1).max().item()) + rot_history.append(rot_error.norm(dim=-1).max().item()) + + pos_min, pos_mean = _summarize_history(pos_history) + rot_min, rot_mean = _summarize_history(rot_history) + + print(f"OSC_METRIC pos_min={pos_min:.5f} pos_mean={pos_mean:.5f} rot_min={rot_min:.5f} rot_mean={rot_mean:.5f}") + + # Assert on tail mean. Threshold matched to the Newton-side test + # (5 mm / 0.05 rad). Both backends converge to machine precision + # with proper ee-velocity feedback (``J · q_dot``). + assert pos_mean < 5e-3, f"OSC pos_mean {pos_mean:.5f} > 5 mm — bridge regression?" + assert rot_mean < 5e-2, f"OSC rot_mean {rot_mean:.5f} > 0.05 rad — bridge regression?" + + +def _run_osc_stay_still_under_gravity( + sim, + device: str, + *, + gravity_compensation_enabled: bool, + num_steps: int = 100, +): + """Run OSC with a stay-still target on Franka under gravity, return EE drift summary. + + Shared helper for the gravity-comp tests. Setup mirrors + :func:`test_franka_osc_tracking_accuracy` (zero actuator PD so OSC's joint-effort + output is not opposed by an implicit-PD spring), but with scene gravity ON and the + target = the EE pose captured after the first sim step (which already includes a + fraction-of-a-mm of gravity-induced motion; that's the baseline drift starts from). + + Args: + gravity_compensation_enabled: If ``True``, the OSC controller cfg has + ``gravity_compensation=True`` and ``osc.compute(gravity=g(q))`` receives + the data-layer ``gravity_compensation_forces`` slice. If ``False``, + ``gravity_compensation=False`` and ``gravity=None``. + + Returns: + Tuple ``((pos_min, pos_mean), (rot_min, rot_mean))`` over the last 20% of + steps (per :func:`_summarize_history`), where ``pos`` is in meters and + ``rot`` in radians. + """ + # Enable rigid-body gravity so the arm actually feels weight. + # ``FRANKA_PANDA_HIGH_PD_CFG`` defaults ``disable_gravity=True`` for IK/OSC tests. + robot, ee_frame_idx, ee_jacobi_idx, arm_joint_ids = _setup_franka_at_home_pose( + sim, zero_actuator_pd=True, enable_rigid_body_gravity=True + ) + + osc = OperationalSpaceController( + OperationalSpaceControllerCfg( + target_types=["pose_abs"], + impedance_mode="fixed", + inertial_dynamics_decoupling=True, + partial_inertial_dynamics_decoupling=False, + gravity_compensation=gravity_compensation_enabled, + motion_stiffness_task=500.0, + motion_damping_ratio_task=1.0, + ), + num_envs=1, + device=device, + ) + + sim.step() + robot.update(sim.cfg.dt) + + # Stay-still target = current EE pose in root frame, captured right after the + # first step. The OSC loop must hold this pose under gravity. + initial_ee_pos_b, initial_ee_quat_b, _ = _compute_ee_pose_root(robot, ee_frame_idx) + target_pose_b = torch.cat([initial_ee_pos_b, initial_ee_quat_b], dim=-1) + + pos_history: list[float] = [] + rot_history: list[float] = [] + for _ in range(num_steps): + jacobian_b = _compute_jacobian_root_frame(robot, ee_jacobi_idx, arm_joint_ids) + mass_matrix = robot.data.mass_matrix.torch[:, arm_joint_ids, :][:, :, arm_joint_ids] + ee_pos_b, ee_quat_b, _ = _compute_ee_pose_root(robot, ee_frame_idx) + ee_pose_b = torch.cat([ee_pos_b, ee_quat_b], dim=-1) + joint_vel = robot.data.joint_vel.torch[:, arm_joint_ids] + ee_vel_b = _compute_ee_vel_root(jacobian_b, joint_vel) + + # ``gravity_compensation_forces`` shape is ``(N, num_joints + num_base_dofs)``; + # slice past the leading floating-base columns (0 for fixed-base Franka, so a + # no-op here, but the pattern matches the action-term convention). + gravity = ( + robot.data.gravity_compensation_forces.torch[:, [j + robot.num_base_dofs for j in arm_joint_ids]] + if gravity_compensation_enabled + else None + ) + + osc.set_command(target_pose_b, current_ee_pose_b=ee_pose_b) + joint_efforts = osc.compute( + jacobian_b=jacobian_b, + current_ee_pose_b=ee_pose_b, + current_ee_vel_b=ee_vel_b, + mass_matrix=mass_matrix, + gravity=gravity, + ) + robot.set_joint_effort_target(joint_efforts, joint_ids=arm_joint_ids) + robot.write_data_to_sim() + sim.step() + robot.update(sim.cfg.dt) + + pos_error, rot_error = compute_pose_error(ee_pos_b, ee_quat_b, target_pose_b[:, 0:3], target_pose_b[:, 3:7]) + pos_history.append(pos_error.norm(dim=-1).max().item()) + rot_history.append(rot_error.norm(dim=-1).max().item()) + + return _summarize_history(pos_history), _summarize_history(rot_history) + + +@pytest.mark.parametrize("device", ["cuda:0"]) +@pytest.mark.parametrize("articulation_type", ["panda"]) +@pytest.mark.parametrize("gravity_enabled", [True]) +@pytest.mark.isaacsim_ci +def test_franka_osc_gravity_compensation_holds_under_gravity(sim, device, articulation_type, gravity_enabled): + """OSC with ``gravity_compensation=True`` must hold the EE pose under gravity. + + With scene gravity ON and zero actuator PD (so OSC torques are not opposed by an + implicit-PD spring), passing + :attr:`~isaaclab.assets.BaseArticulationData.gravity_compensation_forces` through + ``osc.compute(gravity=...)`` should keep the arm at the initial pose. + + Pins three things that the existing direct-primitive + :func:`test_get_gravity_compensation_forces_static_equilibrium` does not: + 1. OSC's ``_jacobi_joint_idx`` indexing — the ``+ num_base_dofs`` shift. + 2. OSC's :meth:`OperationalSpaceController.compute` correctly adds ``g(q)`` to + its torque output. + 3. The data-property ``gravity_compensation_forces`` is reachable from the OSC + pipeline (catches gating regressions in + :meth:`OperationalSpaceControllerAction._compute_dynamic_quantities`). + + Companion test :func:`test_franka_osc_no_gravity_compensation_sags_under_gravity` + runs the same setup with ``gravity_compensation=False`` and reports the + uncompensated drift magnitude — a sanity check that gravity is loading the arm. + """ + (pos_min, pos_mean), (rot_min, rot_mean) = _run_osc_stay_still_under_gravity( + sim, device, gravity_compensation_enabled=True + ) + print(f"OSC_GC_ON pos_min={pos_min:.5f} pos_mean={pos_mean:.5f} rot_min={rot_min:.5f} rot_mean={rot_mean:.5f}") + + assert pos_mean < 5e-3, f"OSC + gravity_compensation pos_mean {pos_mean:.5f} > 5 mm — regression?" + assert rot_mean < 5e-2, f"OSC + gravity_compensation rot_mean {rot_mean:.5f} > 0.05 rad — regression?" + + +@pytest.mark.parametrize("device", ["cuda:0"]) +@pytest.mark.parametrize("articulation_type", ["panda"]) +@pytest.mark.parametrize("gravity_enabled", [True]) +@pytest.mark.isaacsim_ci +def test_franka_osc_no_gravity_compensation_sags_under_gravity(sim, device, articulation_type, gravity_enabled): + """OSC without ``gravity_compensation`` under gravity: sanity check that the arm sags. + + Companion to :func:`test_franka_osc_gravity_compensation_holds_under_gravity`. + Same setup, but ``gravity_compensation=False`` and ``osc.compute(gravity=None)``. + With zero actuator PD, OSC's task-space impedance is the only restoring force — + the steady-state solution is whatever pose error the impedance produces enough + joint torque to balance ``g(q)``. + + Asserts the drift is **non-trivially larger** than the with-comp threshold (5 mm). + Without this check, a regression that broke ``gravity_compensation_forces`` by + returning zeros (or a no-op `g(q)`) would pass the with-comp test silently. The + bound here proves gravity is actually loading the arm and the with-comp pass is + meaningful. + """ + (pos_min, pos_mean), (rot_min, rot_mean) = _run_osc_stay_still_under_gravity( + sim, device, gravity_compensation_enabled=False + ) + print(f"OSC_GC_OFF pos_min={pos_min:.5f} pos_mean={pos_mean:.5f} rot_min={rot_min:.5f} rot_mean={rot_mean:.5f}") + + # Sanity: with gravity on and no comp, OSC's task-space spring vs gravity-load + # equilibrium produces a non-zero pose error. If this asserts fails, the test + # setup itself is broken (e.g., gravity is not on, or the home pose has no + # gravity load), which would invalidate the with-comp test as well. + assert pos_mean > 5e-3, ( + f"OSC + no gravity_compensation pos_mean {pos_mean:.5f} ≤ 5 mm — gravity not loading the arm?" + ) + + if __name__ == "__main__": pytest.main([__file__, "-v", "--maxfail=1"]) diff --git a/source/isaaclab_physx/test/assets/test_rigid_object.py b/source/isaaclab_physx/test/assets/test_rigid_object.py index 6a2211787e19..b7f422c4f2f0 100644 --- a/source/isaaclab_physx/test/assets/test_rigid_object.py +++ b/source/isaaclab_physx/test/assets/test_rigid_object.py @@ -253,8 +253,8 @@ def test_external_force_buffer(device): # check if the cube's force and torque buffers are correctly updated for i in range(cube_object.num_instances): - assert cube_object._permanent_wrench_composer.composed_force.torch[i, 0, 0].item() == force - assert cube_object._permanent_wrench_composer.composed_torque.torch[i, 0, 0].item() == force + assert cube_object._permanent_wrench_composer.out_force_b.torch[i, 0, 0].item() == force + assert cube_object._permanent_wrench_composer.out_torque_b.torch[i, 0, 0].item() == force # Check if the instantaneous wrench is correctly added to the permanent wrench cube_object.permanent_wrench_composer.add_forces_and_torques_index( @@ -426,13 +426,13 @@ def test_external_force_on_single_body_at_position(num_cubes, device): is_global=is_global, ) torch.testing.assert_close( - cube_object._permanent_wrench_composer.composed_force.torch[:, 0, :], + cube_object._permanent_wrench_composer.out_force_b.torch[:, 0, :], desired_force[:, 0, :], rtol=1e-6, atol=1e-7, ) torch.testing.assert_close( - cube_object._permanent_wrench_composer.composed_torque.torch[:, 0, :], + cube_object._permanent_wrench_composer.out_torque_b.torch[:, 0, :], desired_torque[:, 0, :], rtol=1e-6, atol=1e-7, @@ -557,10 +557,10 @@ def test_reset_rigid_object(num_cubes, device): # Reset should zero external forces and torques assert not cube_object._instantaneous_wrench_composer.active assert not cube_object._permanent_wrench_composer.active - assert torch.count_nonzero(cube_object._instantaneous_wrench_composer.composed_force.torch) == 0 - assert torch.count_nonzero(cube_object._instantaneous_wrench_composer.composed_torque.torch) == 0 - assert torch.count_nonzero(cube_object._permanent_wrench_composer.composed_force.torch) == 0 - assert torch.count_nonzero(cube_object._permanent_wrench_composer.composed_torque.torch) == 0 + assert torch.count_nonzero(cube_object._instantaneous_wrench_composer.out_force_b.torch) == 0 + assert torch.count_nonzero(cube_object._instantaneous_wrench_composer.out_torque_b.torch) == 0 + assert torch.count_nonzero(cube_object._permanent_wrench_composer.out_force_b.torch) == 0 + assert torch.count_nonzero(cube_object._permanent_wrench_composer.out_torque_b.torch) == 0 @pytest.mark.parametrize("num_cubes", [1, 2]) diff --git a/source/isaaclab_physx/test/assets/test_rigid_object_collection.py b/source/isaaclab_physx/test/assets/test_rigid_object_collection.py index 8401cc395a0c..f42f6dfcb085 100644 --- a/source/isaaclab_physx/test/assets/test_rigid_object_collection.py +++ b/source/isaaclab_physx/test/assets/test_rigid_object_collection.py @@ -259,8 +259,8 @@ def test_external_force_buffer(sim, device): # check if the object collection's force and torque buffers are correctly updated for i in range(num_envs): - assert object_collection._permanent_wrench_composer.composed_force.torch[i, 0, 0].item() == force - assert object_collection._permanent_wrench_composer.composed_torque.torch[i, 0, 0].item() == force + assert object_collection._permanent_wrench_composer.out_force_b.torch[i, 0, 0].item() == force + assert object_collection._permanent_wrench_composer.out_torque_b.torch[i, 0, 0].item() == force object_collection.instantaneous_wrench_composer.add_forces_and_torques_index( body_ids=object_ids, @@ -681,10 +681,10 @@ def test_reset_object_collection(sim, num_envs, num_cubes, device): # Reset should zero external forces and torques assert not object_collection._instantaneous_wrench_composer.active assert not object_collection._permanent_wrench_composer.active - assert torch.count_nonzero(object_collection._instantaneous_wrench_composer.composed_force.torch) == 0 - assert torch.count_nonzero(object_collection._instantaneous_wrench_composer.composed_torque.torch) == 0 - assert torch.count_nonzero(object_collection._permanent_wrench_composer.composed_force.torch) == 0 - assert torch.count_nonzero(object_collection._permanent_wrench_composer.composed_torque.torch) == 0 + assert torch.count_nonzero(object_collection._instantaneous_wrench_composer.out_force_b.torch) == 0 + assert torch.count_nonzero(object_collection._instantaneous_wrench_composer.out_torque_b.torch) == 0 + assert torch.count_nonzero(object_collection._permanent_wrench_composer.out_force_b.torch) == 0 + assert torch.count_nonzero(object_collection._permanent_wrench_composer.out_torque_b.torch) == 0 @pytest.mark.parametrize("num_envs", [1, 3]) diff --git a/source/isaaclab_physx/test/assets/test_surface_gripper.py b/source/isaaclab_physx/test/assets/test_surface_gripper.py index e85a4a8415cc..c075821bb985 100644 --- a/source/isaaclab_physx/test/assets/test_surface_gripper.py +++ b/source/isaaclab_physx/test/assets/test_surface_gripper.py @@ -9,6 +9,8 @@ """Launch Isaac Sim Simulator first.""" +import os + from isaaclab.app import AppLauncher # launch omniverse app @@ -35,6 +37,10 @@ # from isaacsim.robot.surface_gripper import GripperView +_RUNNING_CI = ( + os.environ.get("CI") == "true" or os.environ.get("GITHUB_ACTIONS") == "true" or os.environ.get("GITLAB_CI") +) + def generate_surface_gripper_cfgs( kinematic_enabled: bool = False, @@ -158,6 +164,10 @@ def sim(request): @pytest.mark.parametrize("device", ["cpu"]) @pytest.mark.parametrize("add_ground_plane", [True]) @pytest.mark.isaacsim_ci +@pytest.mark.skipif( + _RUNNING_CI, + reason="Isaac Sim SurfaceGripperView initialization can deadlock in CI; keep CUDA fail-fast coverage only.", +) def test_initialization(sim, num_articulations, device, add_ground_plane) -> None: """Test initialization for articulation with a surface gripper. diff --git a/source/isaaclab_physx/test/sensors/test_contact_sensor.py b/source/isaaclab_physx/test/sensors/test_contact_sensor.py index 00772e6cb0d3..685d9204b3ef 100644 --- a/source/isaaclab_physx/test/sensors/test_contact_sensor.py +++ b/source/isaaclab_physx/test/sensors/test_contact_sensor.py @@ -480,7 +480,7 @@ def test_friction_reporting(setup_simulation, grav_dir): sim.reset() scene["contact_sensor"].reset() - scene["shape"].write_root_pose_to_sim( + scene["shape"].write_root_pose_to_sim_index( root_pose=torch.tensor([0, 0.0, CUBE_CFG.spawn.size[2] / 2.0, 1, 0, 0, 0], device=device).unsqueeze(0) ) @@ -703,7 +703,7 @@ def _test_sensor_contact( duration = durations[idx] while current_test_time < duration: # set object states to contact the ground plane - shape.write_root_pose_to_sim(root_pose=torch.tensor(test_pose, device=shape.device).unsqueeze(0)) + shape.write_root_pose_to_sim_index(root_pose=torch.tensor(test_pose, device=shape.device).unsqueeze(0)) # perform simulation step _perform_sim_step(sim, scene, sim_dt) # increment contact time @@ -735,7 +735,7 @@ def _test_sensor_contact( _test_friction_forces(shape, sensor, mode) # switch the contact mode for 1 dt step before the next contact test begins. - shape.write_root_pose_to_sim(root_pose=torch.tensor(reset_pose, device=shape.device).unsqueeze(0)) + shape.write_root_pose_to_sim_index(root_pose=torch.tensor(reset_pose, device=shape.device).unsqueeze(0)) # perform simulation step _perform_sim_step(sim, scene, sim_dt) # set the last air time to 2 sim_dt steps, because last_air_time and last_contact_time @@ -750,9 +750,9 @@ def _test_friction_forces(shape: RigidObject, sensor: ContactSensor, mode: Conta return # check shape of the friction_forces_w tensor (wp.to_torch expands vec3f -> float32 trailing dim) - num_bodies = sensor.num_bodies + num_sensors = sensor.num_sensors friction_torch = sensor._data.friction_forces_w.torch - assert friction_torch.shape == (sensor.num_instances // num_bodies, num_bodies, 1, 3) + assert friction_torch.shape == (sensor.num_instances // num_sensors, num_sensors, 1, 3) # compare friction forces if mode == ContactTestMode.IN_CONTACT: assert torch.any(torch.abs(friction_torch) > 1e-5).item() @@ -762,14 +762,14 @@ def _test_friction_forces(shape: RigidObject, sensor: ContactSensor, mode: Conta friction_forces_t = wp.to_torch(friction_forces) buffer_count_t = wp.to_torch(buffer_count).to(torch.int32) buffer_start_t = wp.to_torch(buffer_start_indices).to(torch.int32) - for i in range(sensor.num_instances * num_bodies): + for i in range(sensor.num_instances * num_sensors): for j in range(sensor.contact_view.filter_count): start_index_ij = buffer_start_t[i, j] count_ij = buffer_count_t[i, j] force = torch.sum(friction_forces_t[start_index_ij : (start_index_ij + count_ij), :], dim=0) - env_idx = i // num_bodies - body_idx = i % num_bodies - assert torch.allclose(force, friction_torch[env_idx, body_idx, j, :], atol=1e-5) + env_idx = i // num_sensors + sensor_idx = i % num_sensors + assert torch.allclose(force, friction_torch[env_idx, sensor_idx, j, :], atol=1e-5) elif mode == ContactTestMode.NON_CONTACT: assert torch.all(friction_torch == 0.0).item() @@ -788,9 +788,9 @@ def _test_contact_position(shape: RigidObject, sensor: ContactSensor, mode: Cont return # check shape of the contact_pos_w tensor (wp.to_torch expands vec3f -> float32 trailing dim) - num_bodies = sensor.num_bodies + num_sensors = sensor.num_sensors contact_pos_torch = sensor._data.contact_pos_w.torch - assert contact_pos_torch.shape == (sensor.num_instances // num_bodies, num_bodies, 1, 3) + assert contact_pos_torch.shape == (sensor.num_instances // num_sensors, num_sensors, 1, 3) # check contact positions if mode == ContactTestMode.IN_CONTACT: pos_w_torch = sensor._data.pos_w.torch diff --git a/source/isaaclab_physx/test/sensors/test_frame_transformer.py b/source/isaaclab_physx/test/sensors/test_frame_transformer.py index 631e9ba118dd..28559f9b6da5 100644 --- a/source/isaaclab_physx/test/sensors/test_frame_transformer.py +++ b/source/isaaclab_physx/test/sensors/test_frame_transformer.py @@ -153,21 +153,28 @@ def test_frame_transformer_feet_wrt_base(sim): # # reset if count % 25 == 0: # reset root state - root_state = scene.articulations["robot"].data.default_root_state.torch.clone() + root_state = torch.cat( + ( + scene.articulations["robot"].data.default_root_pose.torch, + scene.articulations["robot"].data.default_root_vel.torch, + ), + dim=-1, + ).clone() root_state[:, :3] += scene.env_origins joint_pos = scene.articulations["robot"].data.default_joint_pos.torch joint_vel = scene.articulations["robot"].data.default_joint_vel.torch # -- set root state # -- robot - scene.articulations["robot"].write_root_pose_to_sim(root_state[:, :7]) - scene.articulations["robot"].write_root_velocity_to_sim(root_state[:, 7:]) - scene.articulations["robot"].write_joint_state_to_sim(joint_pos, joint_vel) + scene.articulations["robot"].write_root_pose_to_sim_index(root_pose=root_state[:, :7]) + scene.articulations["robot"].write_root_velocity_to_sim_index(root_velocity=root_state[:, 7:]) + scene.articulations["robot"].write_joint_position_to_sim_index(position=joint_pos) + scene.articulations["robot"].write_joint_velocity_to_sim_index(velocity=joint_vel) # reset buffers scene.reset() # set joint targets robot_actions = default_actions + 0.5 * torch.randn_like(default_actions) - scene.articulations["robot"].set_joint_position_target(robot_actions) + scene.articulations["robot"].set_joint_position_target_index(target=robot_actions) # write data to sim scene.write_data_to_sim() # perform step @@ -251,21 +258,28 @@ def test_frame_transformer_feet_wrt_thigh(sim): # # reset if count % 25 == 0: # reset root state - root_state = scene.articulations["robot"].data.default_root_state.torch.clone() + root_state = torch.cat( + ( + scene.articulations["robot"].data.default_root_pose.torch, + scene.articulations["robot"].data.default_root_vel.torch, + ), + dim=-1, + ).clone() root_state[:, :3] += scene.env_origins joint_pos = scene.articulations["robot"].data.default_joint_pos.torch joint_vel = scene.articulations["robot"].data.default_joint_vel.torch # -- set root state # -- robot - scene.articulations["robot"].write_root_pose_to_sim(root_state[:, :7]) - scene.articulations["robot"].write_root_velocity_to_sim(root_state[:, 7:]) - scene.articulations["robot"].write_joint_state_to_sim(joint_pos, joint_vel) + scene.articulations["robot"].write_root_pose_to_sim_index(root_pose=root_state[:, :7]) + scene.articulations["robot"].write_root_velocity_to_sim_index(root_velocity=root_state[:, 7:]) + scene.articulations["robot"].write_joint_position_to_sim_index(position=joint_pos) + scene.articulations["robot"].write_joint_velocity_to_sim_index(velocity=joint_vel) # reset buffers scene.reset() # set joint targets robot_actions = default_actions + 0.5 * torch.randn_like(default_actions) - scene.articulations["robot"].set_joint_position_target(robot_actions) + scene.articulations["robot"].set_joint_position_target_index(target=robot_actions) # write data to sim scene.write_data_to_sim() # perform step @@ -329,21 +343,28 @@ def test_frame_transformer_robot_body_to_external_cube(sim): # # reset if count % 25 == 0: # reset root state - root_state = scene.articulations["robot"].data.default_root_state.torch.clone() + root_state = torch.cat( + ( + scene.articulations["robot"].data.default_root_pose.torch, + scene.articulations["robot"].data.default_root_vel.torch, + ), + dim=-1, + ).clone() root_state[:, :3] += scene.env_origins joint_pos = scene.articulations["robot"].data.default_joint_pos.torch joint_vel = scene.articulations["robot"].data.default_joint_vel.torch # -- set root state # -- robot - scene.articulations["robot"].write_root_pose_to_sim(root_state[:, :7]) - scene.articulations["robot"].write_root_velocity_to_sim(root_state[:, 7:]) - scene.articulations["robot"].write_joint_state_to_sim(joint_pos, joint_vel) + scene.articulations["robot"].write_root_pose_to_sim_index(root_pose=root_state[:, :7]) + scene.articulations["robot"].write_root_velocity_to_sim_index(root_velocity=root_state[:, 7:]) + scene.articulations["robot"].write_joint_position_to_sim_index(position=joint_pos) + scene.articulations["robot"].write_joint_velocity_to_sim_index(velocity=joint_vel) # reset buffers scene.reset() # set joint targets robot_actions = default_actions + 0.5 * torch.randn_like(default_actions) - scene.articulations["robot"].set_joint_position_target(robot_actions) + scene.articulations["robot"].set_joint_position_target_index(target=robot_actions) # write data to sim scene.write_data_to_sim() # perform step @@ -425,12 +446,18 @@ def test_frame_transformer_offset_frames(sim): # # reset if count % 25 == 0: # reset root state - root_state = scene["cube"].data.default_root_state.torch.clone() + root_state = torch.cat( + ( + scene["cube"].data.default_root_pose.torch, + scene["cube"].data.default_root_vel.torch, + ), + dim=-1, + ).clone() root_state[:, :3] += scene.env_origins # -- set root state # -- cube - scene["cube"].write_root_pose_to_sim(root_state[:, :7]) - scene["cube"].write_root_velocity_to_sim(root_state[:, 7:]) + scene["cube"].write_root_pose_to_sim_index(root_pose=root_state[:, :7]) + scene["cube"].write_root_velocity_to_sim_index(root_velocity=root_state[:, 7:]) # reset buffers scene.reset() @@ -513,21 +540,28 @@ def test_frame_transformer_all_bodies(sim): # # reset if count % 25 == 0: # reset root state - root_state = scene.articulations["robot"].data.default_root_state.torch.clone() + root_state = torch.cat( + ( + scene.articulations["robot"].data.default_root_pose.torch, + scene.articulations["robot"].data.default_root_vel.torch, + ), + dim=-1, + ).clone() root_state[:, :3] += scene.env_origins joint_pos = scene.articulations["robot"].data.default_joint_pos.torch joint_vel = scene.articulations["robot"].data.default_joint_vel.torch # -- set root state # -- robot - scene.articulations["robot"].write_root_pose_to_sim(root_state[:, :7]) - scene.articulations["robot"].write_root_velocity_to_sim(root_state[:, 7:]) - scene.articulations["robot"].write_joint_state_to_sim(joint_pos, joint_vel) + scene.articulations["robot"].write_root_pose_to_sim_index(root_pose=root_state[:, :7]) + scene.articulations["robot"].write_root_velocity_to_sim_index(root_velocity=root_state[:, 7:]) + scene.articulations["robot"].write_joint_position_to_sim_index(position=joint_pos) + scene.articulations["robot"].write_joint_velocity_to_sim_index(velocity=joint_vel) # reset buffers scene.reset() # set joint targets robot_actions = default_actions + 0.5 * torch.randn_like(default_actions) - scene.articulations["robot"].set_joint_position_target(robot_actions) + scene.articulations["robot"].set_joint_position_target_index(target=robot_actions) # write data to sim scene.write_data_to_sim() # perform step @@ -708,22 +742,38 @@ class MultiRobotSceneCfg(InteractiveSceneCfg): # Reset periodically if count % 10 == 0: # Reset robot - root_state = scene.articulations["robot"].data.default_root_state.torch.clone() + root_state = torch.cat( + ( + scene.articulations["robot"].data.default_root_pose.torch, + scene.articulations["robot"].data.default_root_vel.torch, + ), + dim=-1, + ).clone() root_state[:, :3] += scene.env_origins - scene.articulations["robot"].write_root_pose_to_sim(root_state[:, :7]) - scene.articulations["robot"].write_root_velocity_to_sim(root_state[:, 7:]) - scene.articulations["robot"].write_joint_state_to_sim( - scene.articulations["robot"].data.default_joint_pos.torch, - scene.articulations["robot"].data.default_joint_vel.torch, + scene.articulations["robot"].write_root_pose_to_sim_index(root_pose=root_state[:, :7]) + scene.articulations["robot"].write_root_velocity_to_sim_index(root_velocity=root_state[:, 7:]) + scene.articulations["robot"].write_joint_position_to_sim_index( + position=scene.articulations["robot"].data.default_joint_pos.torch + ) + scene.articulations["robot"].write_joint_velocity_to_sim_index( + velocity=scene.articulations["robot"].data.default_joint_vel.torch ) # Reset robot_1 - root_state_1 = scene.articulations["robot_1"].data.default_root_state.torch.clone() + root_state_1 = torch.cat( + ( + scene.articulations["robot_1"].data.default_root_pose.torch, + scene.articulations["robot_1"].data.default_root_vel.torch, + ), + dim=-1, + ).clone() root_state_1[:, :3] += scene.env_origins - scene.articulations["robot_1"].write_root_pose_to_sim(root_state_1[:, :7]) - scene.articulations["robot_1"].write_root_velocity_to_sim(root_state_1[:, 7:]) - scene.articulations["robot_1"].write_joint_state_to_sim( - scene.articulations["robot_1"].data.default_joint_pos.torch, - scene.articulations["robot_1"].data.default_joint_vel.torch, + scene.articulations["robot_1"].write_root_pose_to_sim_index(root_pose=root_state_1[:, :7]) + scene.articulations["robot_1"].write_root_velocity_to_sim_index(root_velocity=root_state_1[:, 7:]) + scene.articulations["robot_1"].write_joint_position_to_sim_index( + position=scene.articulations["robot_1"].data.default_joint_pos.torch + ) + scene.articulations["robot_1"].write_joint_velocity_to_sim_index( + velocity=scene.articulations["robot_1"].data.default_joint_vel.torch ) scene.reset() diff --git a/source/isaaclab_physx/test/sim/test_cloner.py b/source/isaaclab_physx/test/sim/test_cloner.py index 4bfba07d99e8..f90c740f17ed 100644 --- a/source/isaaclab_physx/test/sim/test_cloner.py +++ b/source/isaaclab_physx/test/sim/test_cloner.py @@ -21,10 +21,9 @@ import isaaclab.sim as sim_utils from isaaclab.cloner import ( - TemplateCloneCfg, _fabric_notices, - clone_from_template, disabled_fabric_change_notifies, + make_clone_plan, sequential, usd_replicate, ) @@ -242,19 +241,9 @@ def test_physx_replicate_heterogeneous_isolated_sources(sim, device): assert "/World/envs" in attach_excluded -def test_clone_from_template(sim): - """Clone prototypes via TemplateCloneCfg and clone_from_template and exercise both USD and PhysX. - - Steps: - - Create /World/template and /World/envs/env_0..env_31 - - Spawn three prototypes under /World/template/Object/proto_asset_.* - - Clone using TemplateCloneCfg with random_heterogeneous_cloning=False (modulo mapping) - - Verify modulo placement exists; then call sim.reset(), and create PhysX view - """ +def test_direct_clone_plan_multi_asset(sim): + """Clone representative env sources directly and exercise both USD and PhysX.""" num_clones = 32 - clone_cfg = TemplateCloneCfg(device=sim.cfg.device, clone_strategy=sequential) - sim_utils.create_prim(clone_cfg.template_root, "Xform") - sim_utils.create_prim(f"{clone_cfg.template_root}/Object", "Xform") sim_utils.create_prim("/World/envs", "Xform") for i in range(num_clones): sim_utils.create_prim(f"/World/envs/env_{i}", "Xform", translation=(0, 0, 0)) @@ -282,11 +271,22 @@ def test_clone_from_template(sim): mass_props=sim_utils.MassPropertiesCfg(mass=1.0), collision_props=sim_utils.CollisionPropertiesCfg(), ) - prim = cfg.func(f"{clone_cfg.template_root}/Object/{clone_cfg.template_prototype_identifier}_.*", cfg) + plan = make_clone_plan( + [[f"/World/envs/env_{i}/Object" for i in range(len(cfg.assets_cfg))]], + ["/World/envs/env_{}/Object"], + num_clones, + sequential, + sim.cfg.device, + ) + spawn_paths: list[str | None] = list(plan.sources) + cfg.spawn_paths = spawn_paths + prim = cfg.func("/World/unused", cfg) assert prim.IsValid() stage = sim_utils.get_current_stage() - clone_from_template(stage, num_clones=num_clones, template_clone_cfg=clone_cfg) + env_ids = torch.arange(num_clones, dtype=torch.long, device=sim.cfg.device) + physx_replicate(stage, plan.sources, plan.destinations, env_ids, plan.clone_mask, device=sim.cfg.device) + usd_replicate(stage, plan.sources, plan.destinations, env_ids, plan.clone_mask) primitive_prims = sim_utils.get_all_matching_child_prims( "/World/envs", predicate=lambda prim: prim.GetTypeName() in ["Cone", "Cube", "Sphere"] @@ -302,27 +302,38 @@ def test_clone_from_template(sim): assert primitive_prim.GetTypeName() == "Sphere" sim.reset() - object_view_regex = f"{clone_cfg.clone_regex}/Object".replace(".*", "*") physics_sim_view = sim.physics_manager.get_physics_sim_view() - physx_view = physics_sim_view.create_rigid_body_view(object_view_regex) + physx_view = physics_sim_view.create_rigid_body_view("/World/envs/env_*/Object") assert physx_view is not None def _run_colocation_collision_filter(sim, asset_cfg, expected_types, assert_count=False): """Shared harness for colocated collision filter checks across devices.""" num_clones = 32 - clone_cfg = TemplateCloneCfg(device=sim.cfg.device, clone_strategy=sequential) - sim_utils.create_prim(clone_cfg.template_root, "Xform") - sim_utils.create_prim(f"{clone_cfg.template_root}/Object", "Xform") sim_utils.create_prim("/World/envs", "Xform") for i in range(num_clones): sim_utils.create_prim(f"/World/envs/env_{i}", "Xform", translation=(0, 0, 0)) - prim = asset_cfg.func(f"{clone_cfg.template_root}/Object/{clone_cfg.template_prototype_identifier}_.*", asset_cfg) + num_variants = len(asset_cfg.assets_cfg) if isinstance(asset_cfg, sim_utils.MultiAssetSpawnerCfg) else 1 + plan = make_clone_plan( + [[f"/World/envs/env_{i}/Object" for i in range(num_variants)]], + ["/World/envs/env_{}/Object"], + num_clones, + sequential, + sim.cfg.device, + ) + if isinstance(asset_cfg, sim_utils.MultiAssetSpawnerCfg): + spawn_paths: list[str | None] = list(plan.sources) + asset_cfg.spawn_paths = spawn_paths + prim = asset_cfg.func("/World/unused", asset_cfg) + else: + prim = asset_cfg.func(plan.sources[0], asset_cfg) assert prim.IsValid() stage = sim_utils.get_current_stage() - clone_from_template(stage, num_clones=num_clones, template_clone_cfg=clone_cfg) + env_ids = torch.arange(num_clones, dtype=torch.long, device=sim.cfg.device) + physx_replicate(stage, plan.sources, plan.destinations, env_ids, plan.clone_mask, device=sim.cfg.device) + usd_replicate(stage, plan.sources, plan.destinations, env_ids, plan.clone_mask) primitive_prims = sim_utils.get_all_matching_child_prims( "/World/envs", predicate=lambda prim: prim.GetTypeName() in expected_types @@ -335,9 +346,8 @@ def _run_colocation_collision_filter(sim, asset_cfg, expected_types, assert_coun assert primitive_prim.GetTypeName() == expected_types[i % len(expected_types)] sim.reset() - object_view_regex = f"{clone_cfg.clone_regex}/Object".replace(".*", "*") physics_sim_view = sim.physics_manager.get_physics_sim_view() - physx_view = physics_sim_view.create_rigid_body_view(object_view_regex) + physx_view = physics_sim_view.create_rigid_body_view("/World/envs/env_*/Object") for _ in range(100): sim.step() transforms = wp.to_torch(physx_view.get_transforms()) diff --git a/source/isaaclab_physx/test/sim/test_views_xform_prim_fabric.py b/source/isaaclab_physx/test/sim/test_views_xform_prim_fabric.py index 4376c0e0b8ea..059446f8a5b7 100644 --- a/source/isaaclab_physx/test/sim/test_views_xform_prim_fabric.py +++ b/source/isaaclab_physx/test/sim/test_views_xform_prim_fabric.py @@ -21,6 +21,7 @@ import pytest # noqa: E402 import torch # noqa: E402 +import warp as wp # noqa: E402 from frame_view_contract_utils import * # noqa: F401, F403, E402 from frame_view_contract_utils import CHILD_OFFSET, ViewBundle # noqa: E402 from isaaclab_physx.sim.views import FabricFrameView as FrameView # noqa: E402 @@ -40,13 +41,14 @@ def test_setup_teardown(): yield sim_utils.clear_stage() sim_utils.SimulationContext.clear_instance() + # Each test creates a fresh stage; drop cached IFabricHierarchy handles so + # the next test does not reuse a handle attached to the disposed stage. + FrameView.clear_static_caches() def _skip_if_unavailable(device: str): if device.startswith("cuda") and not torch.cuda.is_available(): pytest.skip("CUDA not available") - if device == "cpu": - pytest.skip("Warp fabricarray operations on CPU have known issues") # ------------------------------------------------------------------ @@ -95,7 +97,7 @@ def factory(num_envs: int, device: str) -> ViewBundle: sim_utils.create_prim(f"/World/Parent_{i}/Child", "Camera", translation=CHILD_OFFSET, stage=stage) sim_utils.SimulationContext(sim_utils.SimulationCfg(dt=0.01, device=device, use_fabric=True)) - view = FrameView("/World/Parent_.*/Child", device=device, sync_usd_on_fabric_write=True) + view = FrameView("/World/Parent_.*/Child", device=device) return ViewBundle( view=view, get_parent_pos=_get_parent_positions, @@ -104,3 +106,390 @@ def factory(num_envs: int, device: str) -> ViewBundle: ) return factory + + +# ------------------------------------------------------------------ +# Override: ensure the shared contract test runs without xfail now that +# get_local_poses computes local from Fabric world matrices. +# ------------------------------------------------------------------ +# (No override needed — the shared test_set_world_updates_local from +# frame_view_contract_utils is imported via wildcard and will run as-is.) + + +# ------------------------------------------------------------------ +# Fabric-specific tests (not in shared contract) +# ------------------------------------------------------------------ + + +@wp.kernel +def _fill_position(out: wp.array(dtype=wp.float32, ndim=2), x: float, y: float, z: float): + i = wp.tid() + out[i, 0] = wp.float32(x) + out[i, 1] = wp.float32(y) + out[i, 2] = wp.float32(z) + + +@pytest.mark.parametrize("device", ["cpu", "cuda:0"]) +def test_fabric_set_world_does_not_write_back_to_usd(device, view_factory): + """Verify that set_world_poses in Fabric mode does NOT sync back to USD. + + This confirms the removal of sync_usd_on_fabric_write. After calling + set_world_poses, the USD prim's xformOps should still contain the + original (stale) values. + """ + bundle = view_factory(1, device) + view = bundle.view + + # Capture the original USD world position BEFORE any Fabric write + stage = sim_utils.get_current_stage() + prim = stage.GetPrimAtPath(view.prim_paths[0]) + xform_cache = UsdGeom.XformCache() + usd_tf_before = xform_cache.GetLocalToWorldTransform(prim) + usd_t_before = usd_tf_before.ExtractTranslation() + orig_usd_pos = torch.tensor([float(usd_t_before[0]), float(usd_t_before[1]), float(usd_t_before[2])]) + + # Write to Fabric — move to (99, 99, 99) + new_pos = wp.zeros((1, 3), dtype=wp.float32, device=device) + wp.launch(kernel=_fill_position, dim=1, inputs=[new_pos, 99.0, 99.0, 99.0], device=device) + view.set_world_poses(positions=new_pos) + + # Verify Fabric has the new position + fab_pos, _ = view.get_world_poses() + pos_torch = wp.to_torch(fab_pos) + assert torch.allclose(pos_torch, torch.tensor([[99.0, 99.0, 99.0]], device=device), atol=0.1), ( + f"Fabric should have new position, got {pos_torch}" + ) + + # Verify USD still has the ORIGINAL position (no writeback). Equality, not + # approximate — USD should literally not have moved, so any drift would + # indicate a residual writeback path. + xform_cache_after = UsdGeom.XformCache() + usd_tf_after = xform_cache_after.GetLocalToWorldTransform(prim) + usd_t_after = usd_tf_after.ExtractTranslation() + usd_pos_after = torch.tensor([float(usd_t_after[0]), float(usd_t_after[1]), float(usd_t_after[2])]) + assert torch.allclose(usd_pos_after, orig_usd_pos, atol=0.0), ( + f"USD should still have original position {orig_usd_pos}, but got {usd_pos_after}. " + f"sync_usd_on_fabric_write may not have been fully removed." + ) + + +@pytest.mark.parametrize("device", ["cpu", "cuda:0"]) +def test_fabric_rebuild_after_topology_change(device, view_factory): + """A simulated topology change rebuilds the indexed fabric arrays and leaves + the view in a state where subsequent writes/reads still produce correct data. + + Real ``PrimSelection.PrepareForReuse`` reports topology change only when Fabric + reallocates internally, which is hard to provoke from a unit test. Instead we + invoke :meth:`FabricFrameView._compute_fabric_indices` and rebuild the indexed + arrays manually, mimicking what ``_get_*_array`` would do on a real topology + event, then verify a roundtrip still works. + """ + bundle = view_factory(2, device) + view = bundle.view + + # First write — initializes Fabric. + initial = wp.zeros((2, 3), dtype=wp.float32, device=device) + wp.launch(kernel=_fill_position, dim=2, inputs=[initial, 1.0, 2.0, 3.0], device=device) + view.set_world_poses(positions=initial) + + # Simulate topology change: recompute per-selection fabric indices and rebuild + # every indexed array, mirroring the lazy paths in the ``_get_*_array`` accessors. + view._rebuild_trans_ro_arrays() + view._world_rw_fabric_indices = view._compute_fabric_indices(view._world_sel_rw) + view._world_ifa_rw = view._build_indexed_array( + view._world_sel_rw, view._WORLD_MATRIX_NAME, view._world_rw_fabric_indices + ) + view._local_rw_fabric_indices = view._compute_fabric_indices(view._local_sel_rw) + view._local_ifa_rw = view._build_indexed_array( + view._local_sel_rw, view._LOCAL_MATRIX_NAME, view._local_rw_fabric_indices + ) + + # Trigger another write through the rebuilt arrays. + new = wp.zeros((2, 3), dtype=wp.float32, device=device) + wp.launch(kernel=_fill_position, dim=2, inputs=[new, 4.0, 5.0, 6.0], device=device) + view.set_world_poses(positions=new) + + ret_pos, _ = view.get_world_poses() + pos_torch = wp.to_torch(ret_pos) + expected = torch.tensor([[4.0, 5.0, 6.0], [4.0, 5.0, 6.0]], device=device) + # 1e-5 ≈ 20 ULP at magnitudes ~4-6; absorbs float32 SRT compose/decompose drift. + assert torch.allclose(pos_torch, expected, atol=1e-5), f"Read after rebuild failed on {device}: {pos_torch}" + + +@pytest.mark.parametrize("device", ["cuda:0"]) +def test_prepare_for_reuse_detects_topology_change(device, view_factory): + """Each persistent ``PrimSelection`` exposes ``PrepareForReuse`` and returns a + bool. When the underlying Fabric topology is unchanged it returns False. + """ + bundle = view_factory(1, device) + view = bundle.view + view.get_world_poses() # trigger Fabric init + + assert view._trans_sel_ro is not None, "trans_sel_ro selection not initialized" + for selection in (view._trans_sel_ro, view._world_sel_rw, view._local_sel_rw): + result = selection.PrepareForReuse() + assert isinstance(result, bool), f"PrepareForReuse should return bool, got {type(result)}" + assert not result, "PrepareForReuse should return False when no topology change" + + +@pytest.mark.parametrize("device", ["cuda:0"]) +def test_set_local_via_fabric_path(device, view_factory): + """Exercise the Fabric-native set_local_poses path. + + Ensures set_local_poses computes child_world = parent_world * local + entirely within Fabric (not falling back to USD) by first triggering + the Fabric sync via get_world_poses. + """ + bundle = view_factory(num_envs=1, device=device) + view = bundle.view + + # Trigger lazy `_initialize_fabric()` so subsequent calls take the Fabric path. + view.get_world_poses() + + # Now set_local_poses should take the Fabric path + new_local_pos = wp.zeros((1, 3), dtype=wp.float32, device=device) + wp.launch(kernel=_fill_position, dim=1, inputs=[new_local_pos, 1.0, 2.0, 3.0], device=device) + ori = torch.tensor([[0.0, 0.0, 0.0, 1.0]], dtype=torch.float32, device=device) + new_local_ori = wp.from_torch(ori) + + view.set_local_poses(translations=new_local_pos, orientations=new_local_ori) + + # Verify: world = parent(0,0,1) + local(1,2,3) = (1,2,4) + world_pos, _ = view.get_world_poses() + expected = torch.tensor([[1.0, 2.0, 4.0]], dtype=torch.float32, device=device) + torch.testing.assert_close(world_pos.torch, expected, atol=1e-4, rtol=0) + + # Verify get_local_poses returns the local offset + local_pos, _ = view.get_local_poses() + expected_local = torch.tensor([[1.0, 2.0, 3.0]], dtype=torch.float32, device=device) + torch.testing.assert_close(local_pos.torch, expected_local, atol=1e-4, rtol=0) + + +@pytest.mark.parametrize("device", ["cuda:0"]) +def test_get_scales_fabric_path(device, view_factory): + """Exercise the Fabric-native get_scales path.""" + bundle = view_factory(num_envs=1, device=device) + view = bundle.view + + # Trigger lazy `_initialize_fabric()` so the get_scales call below uses Fabric. + view.get_world_poses() + + scales = view.get_scales() + scales_t = wp.to_torch(scales) + # Default scale should be (1, 1, 1) + expected = torch.tensor([[1.0, 1.0, 1.0]], dtype=torch.float32, device=device) + torch.testing.assert_close(scales_t, expected, atol=1e-4, rtol=0) + + +# ------------------------------------------------------------------ +# Transpose-convention verification: world ↔ local kernels rely on the +# identity ``(A·B)ᵀ = Bᵀ·Aᵀ`` to drop explicit transposes when operating +# on Fabric's column-transposed matrix storage. The translation-only +# parents used by the standard fixture cannot distinguish the right +# convention from the wrong one — the rotation block is identity and +# equals its own transpose. These tests use a parent rotated 90° around +# Z so that an incorrect storage convention would produce a clearly +# wrong child pose. +# ------------------------------------------------------------------ + + +# Parent at (0, 0, 1) rotated +90° around Z (so the parent X axis points +# along world +Y). Quaternion components in (x, y, z, w) order. +_ROTATED_PARENT_POS = (0.0, 0.0, 1.0) +_ROTATED_PARENT_QUAT_XYZW = (0.0, 0.0, 0.70710678, 0.70710678) + + +def _build_rotated_parent_view(device: str) -> "FrameView": + """Build a 1-env FabricFrameView whose parent is rotated 90° around Z.""" + stage = sim_utils.get_current_stage() + sim_utils.create_prim( + "/World/Parent_0", + "Xform", + translation=_ROTATED_PARENT_POS, + orientation=_ROTATED_PARENT_QUAT_XYZW, + stage=stage, + ) + sim_utils.create_prim("/World/Parent_0/Child", "Camera", translation=(0.0, 0.0, 0.0), stage=stage) + sim_utils.SimulationContext(sim_utils.SimulationCfg(dt=0.01, device=device, use_fabric=True)) + view = FrameView("/World/Parent_.*/Child", device=device) + view.get_world_poses() # force Fabric init and USD→Fabric seed + return view + + +@pytest.mark.parametrize("device", ["cpu", "cuda:0"]) +def test_set_local_then_get_world_with_rotated_parent(device): + """Verify ``update_indexed_world_matrix_from_local`` under non-identity parent rotation. + + With parent rotated +90° around Z, a child local translation of (1, 0, 0) + must produce world translation (0, 1, 1) — parent_pos + R · local. If the + transpose convention in the kernel were wrong, the rotation would flip + direction and the world position would land at (0, -1, 1) instead. + """ + _skip_if_unavailable(device) + view = _build_rotated_parent_view(device) + + new_local = wp.zeros((1, 3), dtype=wp.float32, device=device) + wp.launch(kernel=_fill_position, dim=1, inputs=[new_local, 1.0, 0.0, 0.0], device=device) + identity_quat = wp.from_torch(torch.tensor([[0.0, 0.0, 0.0, 1.0]], dtype=torch.float32, device=device)) + view.set_local_poses(translations=new_local, orientations=identity_quat) + + world_pos, _ = view.get_world_poses() + expected = torch.tensor([[0.0, 1.0, 1.0]], dtype=torch.float32, device=device) + torch.testing.assert_close(world_pos.torch, expected, atol=1e-5, rtol=0) + + +@pytest.mark.parametrize("device", ["cpu", "cuda:0"]) +def test_set_world_then_get_local_with_rotated_parent(device): + """Verify ``update_indexed_local_matrix_from_world`` under non-identity parent rotation. + + With parent rotated +90° around Z and at (0, 0, 1), writing child world + translation (5, 0, 2) must yield child local translation Rᵀ · (5, 0, 1) = + (0, -5, 1). A wrong transpose convention would invert the rotation in the + wrong direction and produce (0, 5, 1) instead. + """ + _skip_if_unavailable(device) + view = _build_rotated_parent_view(device) + + new_world = wp.zeros((1, 3), dtype=wp.float32, device=device) + wp.launch(kernel=_fill_position, dim=1, inputs=[new_world, 5.0, 0.0, 2.0], device=device) + view.set_world_poses(positions=new_world) + + local_pos, _ = view.get_local_poses() + expected = torch.tensor([[0.0, -5.0, 1.0]], dtype=torch.float32, device=device) + torch.testing.assert_close(local_pos.torch, expected, atol=1e-5, rtol=0) + + +@pytest.mark.parametrize("device", ["cpu", "cuda:0"]) +def test_initial_seed_with_scaled_parent(device): + """Verify the initial USD→Fabric seed handles non-unit scales correctly. + + Sets up a parent with world scale (2, 1, 1) and a child with local scale + (3, 1, 1) at local translation (1, 0, 0). Expected world-space values for + the child: + + * world scale = parent_scale * child_local_scale = (6, 1, 1) + * world position = parent_pos + parent_scale * child_local_pos + = (0, 0, 1) + (2 * 1, 0, 0) = (2, 0, 1) + + If the parent's worldMatrix is seeded with a hardcoded unit scale, + ``get_scales`` returns (3, 1, 1) instead of (6, 1, 1) and ``get_world_poses`` + returns (1, 0, 1) instead of (2, 0, 1). If the child's localMatrix is + seeded without scale, after ``_sync_world_from_local_if_dirty`` the world + scale collapses to (2, 1, 1). This test catches both regressions. + """ + _skip_if_unavailable(device) + stage = sim_utils.get_current_stage() + sim_utils.create_prim("/World/Parent_0", "Xform", translation=(0.0, 0.0, 1.0), scale=(2.0, 1.0, 1.0), stage=stage) + sim_utils.create_prim( + "/World/Parent_0/Child", + "Camera", + translation=(1.0, 0.0, 0.0), + scale=(3.0, 1.0, 1.0), + stage=stage, + ) + sim_utils.SimulationContext(sim_utils.SimulationCfg(dt=0.01, device=device, use_fabric=True)) + view = FrameView("/World/Parent_.*/Child", device=device) + + world_pos, _ = view.get_world_poses() + torch.testing.assert_close( + world_pos.torch, + torch.tensor([[2.0, 0.0, 1.0]], dtype=torch.float32, device=device), + atol=1e-5, + rtol=0, + ) + + scales = wp.to_torch(view.get_scales()) + torch.testing.assert_close( + scales, + torch.tensor([[6.0, 1.0, 1.0]], dtype=torch.float32, device=device), + atol=1e-5, + rtol=0, + ) + + +# ------------------------------------------------------------------ +# Multi-view per stage: per-view dirty-flag isolation +# ------------------------------------------------------------------ + + +@pytest.mark.parametrize("device", ["cpu", "cuda:0"]) +def test_multi_view_per_view_dirty_isolation(device): + """Two ``FabricFrameView`` instances on the same stage must not clear each other's + pending local→world sync. + + Background: an earlier implementation stored the world-dirty flag at the class + level keyed by ``stage_id``. With two views on the same stage, view B reading + worlds would clear the flag set by view A's ``set_local_poses``, leaving A's + world matrices silently stale because A's per-view sync kernel never fired. + + This test sets up two views over disjoint child prims (under different parent + sub-trees of the same stage), interleaves their writes and reads, and verifies: + + * view A's ``set_local_poses`` only dirties view A + * view B's ``get_world_poses`` does not clear view A's flag + * after both views' world reads, each one's worlds reflect its own latest local + * neither view's reads/writes corrupt the other view's poses + """ + _skip_if_unavailable(device) + stage = sim_utils.get_current_stage() + + # Two disjoint sub-trees under the same stage. Use different parent names so + # the regex patterns for the two views don't accidentally overlap. + sim_utils.create_prim("/World/EnvA_0", "Xform", translation=(0.0, 0.0, 1.0), stage=stage) + sim_utils.create_prim("/World/EnvA_0/ChildA", "Camera", translation=(0.1, 0.0, 0.0), stage=stage) + sim_utils.create_prim("/World/EnvB_0", "Xform", translation=(0.0, 0.0, 2.0), stage=stage) + sim_utils.create_prim("/World/EnvB_0/ChildB", "Camera", translation=(0.2, 0.0, 0.0), stage=stage) + + sim_utils.SimulationContext(sim_utils.SimulationCfg(dt=0.01, device=device, use_fabric=True)) + view_a = FrameView("/World/EnvA_.*/ChildA", device=device) + view_b = FrameView("/World/EnvB_.*/ChildB", device=device) + + # Initial reads — triggers Fabric init + the seed-time ``_world_dirty = True`` + # path on both views, then clears it. + expected_a0 = torch.tensor([[0.1, 0.0, 1.0]], dtype=torch.float32, device=device) + expected_b0 = torch.tensor([[0.2, 0.0, 2.0]], dtype=torch.float32, device=device) + torch.testing.assert_close(view_a.get_world_poses()[0].torch, expected_a0, atol=1e-5, rtol=0) + torch.testing.assert_close(view_b.get_world_poses()[0].torch, expected_b0, atol=1e-5, rtol=0) + assert view_a._world_dirty is False + assert view_b._world_dirty is False + # Both views must reuse the same cached IFabricHierarchy (one stage = one handle). + assert view_a._fabric_hierarchy is view_b._fabric_hierarchy + assert len(FrameView._static_hierarchy_cache) == 1 + + # Write a new local pose on view A only. + new_local_a = wp.zeros((1, 3), dtype=wp.float32, device=device) + wp.launch(kernel=_fill_position, dim=1, inputs=[new_local_a, 1.0, 0.0, 0.0], device=device) + identity_quat = wp.from_torch(torch.tensor([[0.0, 0.0, 0.0, 1.0]], dtype=torch.float32, device=device)) + view_a.set_local_poses(translations=new_local_a, orientations=identity_quat) + + # Only view A should be dirty. Critical: a per-stage flag would have dirtied + # both views (or neither) at this point. + assert view_a._world_dirty is True, "set_local_poses should mark its own view dirty" + assert view_b._world_dirty is False, "set_local_poses on view A must not dirty view B" + + # Read worlds from view B FIRST. With a per-stage flag, B's + # ``_sync_world_from_local_if_dirty`` would fire and clear the flag, leaving A + # stale. With the per-view flag, B's read is a no-op sync-wise. + torch.testing.assert_close(view_b.get_world_poses()[0].torch, expected_b0, atol=1e-5, rtol=0) + assert view_b._world_dirty is False + assert view_a._world_dirty is True, "view B's world read must not clear view A's dirty flag" + + # Now read view A's worlds — sync fires, world reflects the new local. + expected_a1 = torch.tensor([[1.0, 0.0, 1.0]], dtype=torch.float32, device=device) + torch.testing.assert_close(view_a.get_world_poses()[0].torch, expected_a1, atol=1e-5, rtol=0) + assert view_a._world_dirty is False + + # Symmetric pass: write on B, ensure A is undisturbed. + new_local_b = wp.zeros((1, 3), dtype=wp.float32, device=device) + wp.launch(kernel=_fill_position, dim=1, inputs=[new_local_b, 3.0, 0.0, 0.0], device=device) + view_b.set_local_poses(translations=new_local_b, orientations=identity_quat) + assert view_a._world_dirty is False + assert view_b._world_dirty is True + + # A's worlds must still read back the post-set-local value from above; no + # cross-view stomp on the world matrix. + torch.testing.assert_close(view_a.get_world_poses()[0].torch, expected_a1, atol=1e-5, rtol=0) + expected_b1 = torch.tensor([[3.0, 0.0, 2.0]], dtype=torch.float32, device=device) + torch.testing.assert_close(view_b.get_world_poses()[0].torch, expected_b1, atol=1e-5, rtol=0) + assert view_a._world_dirty is False + assert view_b._world_dirty is False diff --git a/source/isaaclab_rl/changelog.d/leapp_export_integration.rst b/source/isaaclab_rl/changelog.d/leapp_export_integration.rst deleted file mode 100644 index 8a9a65b18d7f..000000000000 --- a/source/isaaclab_rl/changelog.d/leapp_export_integration.rst +++ /dev/null @@ -1,5 +0,0 @@ -Added -^^^^^ - -* Added RSL-RL LEAPP export scripts and integration tests for exporting trained - policies with semantic input, output, and state annotations. diff --git a/source/isaaclab_rl/config/extension.toml b/source/isaaclab_rl/config/extension.toml index 6b5ae668f03e..df9fe2b03612 100644 --- a/source/isaaclab_rl/config/extension.toml +++ b/source/isaaclab_rl/config/extension.toml @@ -1,7 +1,7 @@ [package] # Note: Semantic Versioning is used: https://semver.org/ -version = "0.5.1" +version = "0.5.2" # Description title = "Isaac Lab RL" diff --git a/source/isaaclab_rl/docs/CHANGELOG.rst b/source/isaaclab_rl/docs/CHANGELOG.rst index 0c4c4323ced7..ad62d198ad0b 100644 --- a/source/isaaclab_rl/docs/CHANGELOG.rst +++ b/source/isaaclab_rl/docs/CHANGELOG.rst @@ -1,6 +1,16 @@ Changelog --------- +0.5.2 (2026-05-08) +~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added RSL-RL LEAPP export scripts and integration tests for exporting trained + policies with semantic input, output, and state annotations. + + 0.5.1 (2026-04-21) ~~~~~~~~~~~~~~~~~~ diff --git a/source/isaaclab_tasks/changelog.d/antoiner-rename-newton-presets.rst b/source/isaaclab_tasks/changelog.d/antoiner-rename-newton-presets.rst deleted file mode 100644 index ae311dd1071b..000000000000 --- a/source/isaaclab_tasks/changelog.d/antoiner-rename-newton-presets.rst +++ /dev/null @@ -1,25 +0,0 @@ -Changed -^^^^^^^ - -* **Breaking:** Renamed the Newton-backend solver presets to a ``newton_`` - prefix so they group together in autocomplete and read distinctly from the - Newton backend label, package, and visualizer. The change is shimmed by - deprecation aliases (see ``Deprecated`` below), but workflows that iterate - ``__dataclass_fields__`` directly or treat :exc:`FutureWarning` as an error - will need updates. Migration: rename the field in any - :class:`~isaaclab_tasks.utils.hydra.PresetCfg` subclass and update CLI - invocations (``presets=...`` and ``env.=...``): - - - ``newton`` -> ``newton_mjwarp`` - - ``kamino`` -> ``newton_kamino`` - -Deprecated -^^^^^^^^^^ - -* Deprecated the legacy ``newton`` and ``kamino`` preset names. They still - resolve to ``newton_mjwarp`` and ``newton_kamino`` respectively but emit a - :exc:`FutureWarning` and will be removed in a future release. Update CLI - overrides (``presets=newton`` -> ``presets=newton_mjwarp``; - ``presets=kamino`` -> ``presets=newton_kamino``) and any - :class:`~isaaclab_tasks.utils.hydra.PresetCfg` field declarations - (``newton: NewtonCfg = ...`` -> ``newton_mjwarp: NewtonCfg = ...``). diff --git a/source/isaaclab_tasks/changelog.d/g1-rough-terrain-wip.rst b/source/isaaclab_tasks/changelog.d/g1-rough-terrain-wip.rst deleted file mode 100644 index 9efbf82b8bf6..000000000000 --- a/source/isaaclab_tasks/changelog.d/g1-rough-terrain-wip.rst +++ /dev/null @@ -1,11 +0,0 @@ -Added -^^^^^ - -* Added Newton rough terrain support for the G1 biped locomotion velocity - env. The only engine-specific change is a ~1.7x ``max_iterations`` preset on - :class:`~isaaclab_tasks.manager_based.locomotion.velocity.config.g1.agents.rsl_rl_ppo_cfg.G1RoughPPORunnerCfg` - (Newton = 5000, PhysX = 3000). PhysX saturates near iter 3000 on both - reward (≈ +18) and episode length (≈ 980) and does not meaningfully - improve further; Newton reaches the same (reward, ep_len) quality at - iter 5000. The iteration budget is bumped rather than tuning physics - or reward terms. diff --git a/source/isaaclab_tasks/changelog.d/leapp_export_integration.rst b/source/isaaclab_tasks/changelog.d/leapp_export_integration.rst deleted file mode 100644 index 94a128b6416f..000000000000 --- a/source/isaaclab_tasks/changelog.d/leapp_export_integration.rst +++ /dev/null @@ -1,5 +0,0 @@ -Added -^^^^^ - -* Added LEAPP-compatible policy deployment tutorials and tracing-compatible task - observation helpers for exported policy workflows. diff --git a/source/isaaclab_tasks/changelog.d/mtrepte-expand_viz_markers.skip b/source/isaaclab_tasks/changelog.d/mtrepte-expand_viz_markers.skip deleted file mode 100644 index a23b7c7322b3..000000000000 --- a/source/isaaclab_tasks/changelog.d/mtrepte-expand_viz_markers.skip +++ /dev/null @@ -1 +0,0 @@ -Marker visualization changes are covered by the isaaclab fragment. diff --git a/source/isaaclab_tasks/changelog.d/pr-5458-merge-develop.rst b/source/isaaclab_tasks/changelog.d/pr-5458-merge-develop.rst deleted file mode 100644 index cd4dc7e674d4..000000000000 --- a/source/isaaclab_tasks/changelog.d/pr-5458-merge-develop.rst +++ /dev/null @@ -1,10 +0,0 @@ -Changed -^^^^^^^ - -* Updated classic Ant/Humanoid manager-based environments and direct in-hand - manipulation environments to read body incoming wrenches from - :class:`~isaaclab.sensors.JointWrenchSensor` instead of - ``ArticulationData.body_incoming_joint_wrench_b``. Add a - :class:`~isaaclab.sensors.JointWrenchSensorCfg` to the scene and pass its - :class:`~isaaclab.managers.SceneEntityCfg` as ``sensor_cfg``. The classic - Ant/Humanoid Newton presets now use the same wrench observations as PhysX. diff --git a/source/isaaclab_tasks/changelog.d/rendering-test-flakiness.skip b/source/isaaclab_tasks/changelog.d/rendering-test-flakiness.skip deleted file mode 100644 index e69de29bb2d1..000000000000 diff --git a/source/isaaclab_tasks/changelog.d/rwiltz-restore-legacy-teleop.rst b/source/isaaclab_tasks/changelog.d/rwiltz-restore-legacy-teleop.rst deleted file mode 100644 index 59e71ddc3984..000000000000 --- a/source/isaaclab_tasks/changelog.d/rwiltz-restore-legacy-teleop.rst +++ /dev/null @@ -1,8 +0,0 @@ -Added -^^^^^ - -* Added legacy ``teleop_devices`` configuration (``OpenXRDeviceCfg``, - ``ManusViveCfg``, ``GR1T2RetargeterCfg``) to - :class:`~isaaclab_tasks.manager_based.manipulation.pick_place.pickplace_gr1t2_env_cfg.PickPlaceGR1T2EnvCfg` - alongside the existing ``isaac_teleop`` pipeline, enabling CI validation - via ``--teleop_device=handtracking``. diff --git a/source/isaaclab_tasks/config/extension.toml b/source/isaaclab_tasks/config/extension.toml index 273ae57d2cb9..f8894daa24b6 100644 --- a/source/isaaclab_tasks/config/extension.toml +++ b/source/isaaclab_tasks/config/extension.toml @@ -1,7 +1,7 @@ [package] # Note: Semantic Versioning is used: https://semver.org/ -version = "1.5.34" +version = "1.6.0" # Description title = "Isaac Lab Environments" diff --git a/source/isaaclab_tasks/docs/CHANGELOG.rst b/source/isaaclab_tasks/docs/CHANGELOG.rst index 2044f807afc5..5f61ba12b099 100644 --- a/source/isaaclab_tasks/docs/CHANGELOG.rst +++ b/source/isaaclab_tasks/docs/CHANGELOG.rst @@ -1,6 +1,167 @@ Changelog --------- +1.6.0 (2026-05-14) +~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added Newton backend support for the multi-agent + ``Isaac-Shadow-Hand-Over-Direct-v0`` (MAPPO/IPPO) env. Mirrors the + single-agent Shadow Hand Newton port: per-hand + :class:`~isaaclab.actuators.ImplicitActuatorCfg`, + ``shadow_hand_instanceable_newton.usd``, per-backend + :class:`~isaaclab_tasks.utils.PresetCfg` wrappers for sim physics, the + hand-over object (``RigidObjectCfg`` on both backends, dropping + PhysX-only knobs on Newton), and the two robot configs. Selectable via + ``--preset newton`` / Hydra preset resolution; PhysX behavior unchanged. + Migration details (Newton-side actuator gain overrides for ``fingers`` + and ``distal_passive``, and the ``ccd_iterations`` bump for multi-finger + contacts) live in + ``source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand_over/shadow_hand_over_env_cfg.py``. + +Changed +^^^^^^^ + +* Removed the ``self.sim.physics = PhysxCfg(...)`` overrides from + ``Isaac-Reach-Franka-{IK-Abs,IK-Rel,OSC}-v0`` env configs so they + inherit the parent ``ReachPhysicsCfg`` preset. Selecting + ``presets=newton`` now picks ``NewtonCfg``; the previous + ``bounce_threshold_velocity=0.2`` PhysX behavior is preserved as + the default in ``ReachPhysicsCfg``. Direct-workflow callers in + ``automate``, ``factory``, and the deploy MDP events module were + migrated to the new + :class:`~isaaclab.assets.BaseArticulationData` properties + (:attr:`body_link_jacobian_w`, :attr:`mass_matrix`). +* Changed RSL-RL task agent configs to use ``actor`` and ``critic`` model + configs with distribution configs instead of deprecated ``policy`` configs. + +Fixed +^^^^^ + +* Fixed ``Isaac-Navigation-3DObstacles-ARL-Robot-1-v0`` config load + raising ``TypeError: only 0-dimensional arrays can be converted to + Python scalars`` under NumPy 2.0+. The wall-color sampling now + requests a scalar from :func:`numpy.random.randint` instead of a + shape-``(1,)`` array. +* Fixed ``make current-docs`` failing to import + :mod:`isaaclab_mimic.datagen` because the ``assemble_trocar`` robot + config evaluated ``np.pi`` at module scope, which raised + ``TypeError`` under Sphinx's mocked ``numpy``. Switched the constant + factors to :data:`math.pi`. + + +1.5.38 (2026-05-13) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added Newton MJWarp physics preset support and mesh-based heterogeneous + object spawning for Dexsuite manipulation environments. + + +1.5.37 (2026-05-12) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added ``Isaac-Assemble-Trocar-G129-Dex3-v0`` and + ``Isaac-Assemble-Trocar-G129-Dex3-Eval-v0`` manipulation tasks: a Unitree G1 + 29-DOF humanoid with Dex3 hands assembles a trocar from a tray, trained via + RL post-training of a VLA model using RLinf. + + +1.5.36 (2026-05-09) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added explicit GR1T2 and Unitree G1 pick-place robot link pose and velocity + MDP helpers as replacements for packed robot link state observations. +* Added the ``ovphysx`` physics preset to the cartpole camera presets task. + +Changed +^^^^^^^ + +* Changed Dexsuite orientation tracking rewards to read root link orientation + directly instead of slicing packed root state tensors. +* Updated task camera configs and environments to use + :class:`~isaaclab.sensors.CameraCfg` and :class:`~isaaclab.sensors.Camera` + instead of deprecated tiled-camera aliases. +* Updated task state and write call sites to use explicit state properties and + indexed simulation write APIs. + +Deprecated +^^^^^^^^^^ + +* Deprecated + :func:`~isaaclab_tasks.manager_based.manipulation.pick_place.mdp.observations.get_all_robot_link_state` + in favor of + :func:`~isaaclab_tasks.manager_based.manipulation.pick_place.mdp.observations.get_all_robot_link_pose` + and + :func:`~isaaclab_tasks.manager_based.manipulation.pick_place.mdp.observations.get_all_robot_link_velocity`. + + +1.5.35 (2026-05-08) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added LEAPP-compatible policy deployment tutorials and tracing-compatible task + observation helpers for exported policy workflows. +* Added Newton rough terrain support for the G1 biped locomotion velocity + env. The only engine-specific change is a ~1.7x ``max_iterations`` preset on + :class:`~isaaclab_tasks.manager_based.locomotion.velocity.config.g1.agents.rsl_rl_ppo_cfg.G1RoughPPORunnerCfg` + (Newton = 5000, PhysX = 3000). PhysX saturates near iter 3000 on both + reward (≈ +18) and episode length (≈ 980) and does not meaningfully + improve further; Newton reaches the same (reward, ep_len) quality at + iter 5000. The iteration budget is bumped rather than tuning physics + or reward terms. +* Added legacy ``teleop_devices`` configuration (``OpenXRDeviceCfg``, + ``ManusViveCfg``, ``GR1T2RetargeterCfg``) to + :class:`~isaaclab_tasks.manager_based.manipulation.pick_place.pickplace_gr1t2_env_cfg.PickPlaceGR1T2EnvCfg` + alongside the existing ``isaac_teleop`` pipeline, enabling CI validation + via ``--teleop_device=handtracking``. + +Changed +^^^^^^^ + +* Updated classic Ant/Humanoid manager-based environments and direct in-hand + manipulation environments to read body incoming wrenches from + :class:`~isaaclab.sensors.JointWrenchSensor` instead of + ``ArticulationData.body_incoming_joint_wrench_b``. Add a + :class:`~isaaclab.sensors.JointWrenchSensorCfg` to the scene and pass its + :class:`~isaaclab.managers.SceneEntityCfg` as ``sensor_cfg``. The classic + Ant/Humanoid Newton presets now use the same wrench observations as PhysX. +* **Breaking:** Renamed the Newton-backend solver presets to a ``newton_`` + prefix so they group together in autocomplete and read distinctly from the + Newton backend label, package, and visualizer. The change is shimmed by + deprecation aliases (see ``Deprecated`` below), but workflows that iterate + ``__dataclass_fields__`` directly or treat :exc:`FutureWarning` as an error + will need updates. Migration: rename the field in any + :class:`~isaaclab_tasks.utils.hydra.PresetCfg` subclass and update CLI + invocations (``presets=...`` and ``env.=...``): + + - ``newton`` -> ``newton_mjwarp`` + - ``kamino`` -> ``newton_kamino`` + +Deprecated +^^^^^^^^^^ + +* Deprecated the legacy ``newton`` and ``kamino`` preset names. They still + resolve to ``newton_mjwarp`` and ``newton_kamino`` respectively but emit a + :exc:`FutureWarning` and will be removed in a future release. Update CLI + overrides (``presets=newton`` -> ``presets=newton_mjwarp``; + ``presets=kamino`` -> ``presets=newton_kamino``) and any + :class:`~isaaclab_tasks.utils.hydra.PresetCfg` field declarations + (``newton: NewtonCfg = ...`` -> ``newton_mjwarp: NewtonCfg = ...``). + + 1.5.34 (2026-04-30) ~~~~~~~~~~~~~~~~~~~ Added diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/allegro_hand/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/direct/allegro_hand/agents/rsl_rl_ppo_cfg.py index 871250fd0b17..a80cf1ee33a2 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/allegro_hand/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/allegro_hand/agents/rsl_rl_ppo_cfg.py @@ -5,7 +5,7 @@ from isaaclab.utils import configclass -from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg +from isaaclab_rl.rsl_rl import RslRlMLPModelCfg, RslRlOnPolicyRunnerCfg, RslRlPpoAlgorithmCfg @configclass @@ -14,13 +14,16 @@ class AllegroHandPPORunnerCfg(RslRlOnPolicyRunnerCfg): max_iterations = 10000 save_interval = 250 experiment_name = "allegro_hand" - policy = RslRlPpoActorCriticCfg( - init_noise_std=1.0, - actor_obs_normalization=True, - critic_obs_normalization=True, - actor_hidden_dims=[1024, 512, 256, 128], - critic_hidden_dims=[1024, 512, 256, 128], + actor = RslRlMLPModelCfg( + hidden_dims=[1024, 512, 256, 128], activation="elu", + obs_normalization=True, + distribution_cfg=RslRlMLPModelCfg.GaussianDistributionCfg(init_std=1.0), + ) + critic = RslRlMLPModelCfg( + hidden_dims=[1024, 512, 256, 128], + activation="elu", + obs_normalization=True, ) algorithm = RslRlPpoAlgorithmCfg( value_loss_coef=1.0, diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/ant/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/direct/ant/agents/rsl_rl_ppo_cfg.py index 00eefc843e20..c2e7f15852ff 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/ant/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/ant/agents/rsl_rl_ppo_cfg.py @@ -5,7 +5,7 @@ from isaaclab.utils import configclass -from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg +from isaaclab_rl.rsl_rl import RslRlMLPModelCfg, RslRlOnPolicyRunnerCfg, RslRlPpoAlgorithmCfg @configclass @@ -14,13 +14,16 @@ class AntPPORunnerCfg(RslRlOnPolicyRunnerCfg): max_iterations = 1000 save_interval = 50 experiment_name = "ant_direct" - policy = RslRlPpoActorCriticCfg( - init_noise_std=1.0, - actor_obs_normalization=False, - critic_obs_normalization=False, - actor_hidden_dims=[400, 200, 100], - critic_hidden_dims=[400, 200, 100], + actor = RslRlMLPModelCfg( + hidden_dims=[400, 200, 100], activation="elu", + obs_normalization=False, + distribution_cfg=RslRlMLPModelCfg.GaussianDistributionCfg(init_std=1.0), + ) + critic = RslRlMLPModelCfg( + hidden_dims=[400, 200, 100], + activation="elu", + obs_normalization=False, ) algorithm = RslRlPpoAlgorithmCfg( value_loss_coef=1.0, diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/anymal_c/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/direct/anymal_c/agents/rsl_rl_ppo_cfg.py index 117ad6e75bed..55b34655b639 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/anymal_c/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/anymal_c/agents/rsl_rl_ppo_cfg.py @@ -5,7 +5,7 @@ from isaaclab.utils import configclass -from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg +from isaaclab_rl.rsl_rl import RslRlMLPModelCfg, RslRlOnPolicyRunnerCfg, RslRlPpoAlgorithmCfg @configclass @@ -14,13 +14,16 @@ class AnymalCFlatPPORunnerCfg(RslRlOnPolicyRunnerCfg): max_iterations = 500 save_interval = 50 experiment_name = "anymal_c_flat_direct" - policy = RslRlPpoActorCriticCfg( - init_noise_std=1.0, - actor_obs_normalization=False, - critic_obs_normalization=False, - actor_hidden_dims=[128, 128, 128], - critic_hidden_dims=[128, 128, 128], + actor = RslRlMLPModelCfg( + hidden_dims=[128, 128, 128], activation="elu", + obs_normalization=False, + distribution_cfg=RslRlMLPModelCfg.GaussianDistributionCfg(init_std=1.0), + ) + critic = RslRlMLPModelCfg( + hidden_dims=[128, 128, 128], + activation="elu", + obs_normalization=False, ) algorithm = RslRlPpoAlgorithmCfg( value_loss_coef=1.0, @@ -44,13 +47,16 @@ class AnymalCRoughPPORunnerCfg(RslRlOnPolicyRunnerCfg): max_iterations = 1500 save_interval = 50 experiment_name = "anymal_c_rough_direct" - policy = RslRlPpoActorCriticCfg( - init_noise_std=1.0, - actor_obs_normalization=False, - critic_obs_normalization=False, - actor_hidden_dims=[512, 256, 128], - critic_hidden_dims=[512, 256, 128], + actor = RslRlMLPModelCfg( + hidden_dims=[512, 256, 128], + activation="elu", + obs_normalization=False, + distribution_cfg=RslRlMLPModelCfg.GaussianDistributionCfg(init_std=1.0), + ) + critic = RslRlMLPModelCfg( + hidden_dims=[512, 256, 128], activation="elu", + obs_normalization=False, ) algorithm = RslRlPpoAlgorithmCfg( value_loss_coef=1.0, diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/automate/assembly_env.py b/source/isaaclab_tasks/isaaclab_tasks/direct/automate/assembly_env.py index 438f0f80603b..e3037d67712d 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/automate/assembly_env.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/automate/assembly_env.py @@ -288,12 +288,12 @@ def _compute_intermediate_values(self, dt): self.fingertip_midpoint_linvel = self._robot.data.body_lin_vel_w.torch[:, self.fingertip_body_idx] self.fingertip_midpoint_angvel = self._robot.data.body_ang_vel_w.torch[:, self.fingertip_body_idx] - jacobians = wp.to_torch(self._robot.root_view.get_jacobians()) + jacobians = self._robot.data.body_link_jacobian_w.torch self.left_finger_jacobian = jacobians[:, self.left_finger_body_idx - 1, 0:6, 0:7] self.right_finger_jacobian = jacobians[:, self.right_finger_body_idx - 1, 0:6, 0:7] self.fingertip_midpoint_jacobian = (self.left_finger_jacobian + self.right_finger_jacobian) * 0.5 - self.arm_mass_matrix = wp.to_torch(self._robot.root_view.get_generalized_mass_matrices())[:, 0:7, 0:7] + self.arm_mass_matrix = self._robot.data.mass_matrix.torch[:, 0:7, 0:7] self.joint_pos = self._robot.data.joint_pos.torch.clone() self.joint_vel = self._robot.data.joint_vel.torch.clone() diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/automate/disassembly_env.py b/source/isaaclab_tasks/isaaclab_tasks/direct/automate/disassembly_env.py index c79441f223ae..1982786f65a7 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/automate/disassembly_env.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/automate/disassembly_env.py @@ -215,12 +215,12 @@ def _compute_intermediate_values(self, dt): self.fingertip_midpoint_linvel = self._robot.data.body_lin_vel_w.torch[:, self.fingertip_body_idx] self.fingertip_midpoint_angvel = self._robot.data.body_ang_vel_w.torch[:, self.fingertip_body_idx] - jacobians = wp.to_torch(self._robot.root_view.get_jacobians()) + jacobians = self._robot.data.body_link_jacobian_w.torch self.left_finger_jacobian = jacobians[:, self.left_finger_body_idx - 1, 0:6, 0:7] self.right_finger_jacobian = jacobians[:, self.right_finger_body_idx - 1, 0:6, 0:7] self.fingertip_midpoint_jacobian = (self.left_finger_jacobian + self.right_finger_jacobian) * 0.5 - self.arm_mass_matrix = wp.to_torch(self._robot.root_view.get_generalized_mass_matrices())[:, 0:7, 0:7] + self.arm_mass_matrix = self._robot.data.mass_matrix.torch[:, 0:7, 0:7] self.joint_pos = self._robot.data.joint_pos.torch.clone() self.joint_vel = self._robot.data.joint_vel.torch.clone() diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole/agents/rsl_rl_ppo_cfg.py index 097b7b43a672..7d308b9f5c45 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole/agents/rsl_rl_ppo_cfg.py @@ -5,7 +5,7 @@ from isaaclab.utils import configclass -from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg +from isaaclab_rl.rsl_rl import RslRlMLPModelCfg, RslRlOnPolicyRunnerCfg, RslRlPpoAlgorithmCfg @configclass @@ -14,13 +14,16 @@ class CartpolePPORunnerCfg(RslRlOnPolicyRunnerCfg): max_iterations = 150 save_interval = 50 experiment_name = "cartpole_direct" - policy = RslRlPpoActorCriticCfg( - init_noise_std=1.0, - actor_obs_normalization=False, - critic_obs_normalization=False, - actor_hidden_dims=[32, 32], - critic_hidden_dims=[32, 32], + actor = RslRlMLPModelCfg( + hidden_dims=[32, 32], activation="elu", + obs_normalization=False, + distribution_cfg=RslRlMLPModelCfg.GaussianDistributionCfg(init_std=1.0), + ) + critic = RslRlMLPModelCfg( + hidden_dims=[32, 32], + activation="elu", + obs_normalization=False, ) algorithm = RslRlPpoAlgorithmCfg( value_loss_coef=1.0, diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole/cartpole_camera_presets_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole/cartpole_camera_presets_env_cfg.py index 4c27674ff7c6..00d11f233ff6 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole/cartpole_camera_presets_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole/cartpole_camera_presets_env_cfg.py @@ -6,6 +6,7 @@ from __future__ import annotations from isaaclab_newton.physics import NewtonCfg +from isaaclab_ovphysx.physics import OvPhysxCfg from isaaclab_physx.physics import PhysxCfg import isaaclab.sim as sim_utils @@ -27,6 +28,7 @@ class PhysicsCfg(PresetCfg): default = PhysxCfg() physx = PhysxCfg() newton_mjwarp = NewtonCfg() + ovphysx = OvPhysxCfg() @configclass diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_env.py b/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_env.py index ecc1ef33a038..c38fe071b161 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_env.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/factory/factory_env.py @@ -5,7 +5,6 @@ import numpy as np import torch -import warp as wp import carb @@ -131,12 +130,12 @@ def _compute_intermediate_values(self, dt): self.fingertip_midpoint_linvel = self._robot.data.body_lin_vel_w.torch[:, self.fingertip_body_idx] self.fingertip_midpoint_angvel = self._robot.data.body_ang_vel_w.torch[:, self.fingertip_body_idx] - jacobians = wp.to_torch(self._robot.root_view.get_jacobians()) + jacobians = self._robot.data.body_link_jacobian_w.torch self.left_finger_jacobian = jacobians[:, self.left_finger_body_idx - 1, 0:6, 0:7] self.right_finger_jacobian = jacobians[:, self.right_finger_body_idx - 1, 0:6, 0:7] self.fingertip_midpoint_jacobian = (self.left_finger_jacobian + self.right_finger_jacobian) * 0.5 - self.arm_mass_matrix = wp.to_torch(self._robot.root_view.get_generalized_mass_matrices())[:, 0:7, 0:7] + self.arm_mass_matrix = self._robot.data.mass_matrix.torch[:, 0:7, 0:7] self.joint_pos = self._robot.data.joint_pos.torch.clone() self.joint_vel = self._robot.data.joint_vel.torch.clone() diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/franka_cabinet/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/direct/franka_cabinet/agents/rsl_rl_ppo_cfg.py index a2304fb2c4b7..c467360e61a5 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/franka_cabinet/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/franka_cabinet/agents/rsl_rl_ppo_cfg.py @@ -5,7 +5,7 @@ from isaaclab.utils import configclass -from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg +from isaaclab_rl.rsl_rl import RslRlMLPModelCfg, RslRlOnPolicyRunnerCfg, RslRlPpoAlgorithmCfg @configclass @@ -14,13 +14,16 @@ class FrankaCabinetPPORunnerCfg(RslRlOnPolicyRunnerCfg): max_iterations = 1500 save_interval = 50 experiment_name = "franka_cabinet_direct" - policy = RslRlPpoActorCriticCfg( - init_noise_std=1.0, - actor_obs_normalization=True, - critic_obs_normalization=True, - actor_hidden_dims=[256, 128, 64], - critic_hidden_dims=[256, 128, 64], + actor = RslRlMLPModelCfg( + hidden_dims=[256, 128, 64], activation="elu", + obs_normalization=True, + distribution_cfg=RslRlMLPModelCfg.GaussianDistributionCfg(init_std=1.0), + ) + critic = RslRlMLPModelCfg( + hidden_dims=[256, 128, 64], + activation="elu", + obs_normalization=True, ) algorithm = RslRlPpoAlgorithmCfg( value_loss_coef=1.0, diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid/agents/rsl_rl_ppo_cfg.py index 778d73f09119..07ea4e8590f1 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/humanoid/agents/rsl_rl_ppo_cfg.py @@ -5,7 +5,7 @@ from isaaclab.utils import configclass -from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg +from isaaclab_rl.rsl_rl import RslRlMLPModelCfg, RslRlOnPolicyRunnerCfg, RslRlPpoAlgorithmCfg @configclass @@ -14,13 +14,16 @@ class HumanoidPPORunnerCfg(RslRlOnPolicyRunnerCfg): max_iterations = 1000 save_interval = 50 experiment_name = "humanoid_direct" - policy = RslRlPpoActorCriticCfg( - init_noise_std=1.0, - actor_obs_normalization=True, - critic_obs_normalization=True, - actor_hidden_dims=[400, 200, 100], - critic_hidden_dims=[400, 200, 100], + actor = RslRlMLPModelCfg( + hidden_dims=[400, 200, 100], activation="elu", + obs_normalization=True, + distribution_cfg=RslRlMLPModelCfg.GaussianDistributionCfg(init_std=1.0), + ) + critic = RslRlMLPModelCfg( + hidden_dims=[400, 200, 100], + activation="elu", + obs_normalization=True, ) algorithm = RslRlPpoAlgorithmCfg( value_loss_coef=1.0, diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/inhand_manipulation/inhand_manipulation_env.py b/source/isaaclab_tasks/isaaclab_tasks/direct/inhand_manipulation/inhand_manipulation_env.py index 5969d8c9c4be..bd178f3745ea 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/inhand_manipulation/inhand_manipulation_env.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/inhand_manipulation/inhand_manipulation_env.py @@ -85,20 +85,12 @@ def __init__(self, cfg: AllegroHandEnvCfg | ShadowHandEnvCfg, render_mode: str | self.y_unit_tensor = torch.tensor([0, 1, 0], dtype=torch.float, device=self.device).repeat((self.num_envs, 1)) self.z_unit_tensor = torch.tensor([0, 0, 1], dtype=torch.float, device=self.device).repeat((self.num_envs, 1)) - # bind backend-optimal write methods (Newton prefers mask-based, PhysX prefers indexed) - use_mask = "newton" in self.sim.physics_manager.__name__.lower() - if use_mask: - self._set_joint_pos_target = self.hand.set_joint_position_target - self._write_obj_root_pose = self.object.write_root_pose_to_sim - self._write_obj_root_vel = self.object.write_root_velocity_to_sim - self._write_hand_joint_pos = self.hand.write_joint_position_to_sim - self._write_hand_joint_vel = self.hand.write_joint_velocity_to_sim - else: - self._set_joint_pos_target = self.hand.set_joint_position_target_index - self._write_obj_root_pose = self.object.write_root_pose_to_sim_index - self._write_obj_root_vel = self.object.write_root_velocity_to_sim_index - self._write_hand_joint_pos = self.hand.write_joint_position_to_sim_index - self._write_hand_joint_vel = self.hand.write_joint_velocity_to_sim_index + # bind write methods + self._set_joint_pos_target = self.hand.set_joint_position_target_index + self._write_obj_root_pose = self.object.write_root_pose_to_sim_index + self._write_obj_root_vel = self.object.write_root_velocity_to_sim_index + self._write_hand_joint_pos = self.hand.write_joint_position_to_sim_index + self._write_hand_joint_vel = self.hand.write_joint_velocity_to_sim_index def _setup_scene(self): # add hand, in-hand object, and goal object diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/quadcopter/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/direct/quadcopter/agents/rsl_rl_ppo_cfg.py index 607d9f0fb0ea..254072606f13 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/quadcopter/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/quadcopter/agents/rsl_rl_ppo_cfg.py @@ -5,7 +5,7 @@ from isaaclab.utils import configclass -from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg +from isaaclab_rl.rsl_rl import RslRlMLPModelCfg, RslRlOnPolicyRunnerCfg, RslRlPpoAlgorithmCfg @configclass @@ -14,13 +14,16 @@ class QuadcopterPPORunnerCfg(RslRlOnPolicyRunnerCfg): max_iterations = 200 save_interval = 50 experiment_name = "quadcopter_direct" - policy = RslRlPpoActorCriticCfg( - init_noise_std=1.0, - actor_obs_normalization=False, - critic_obs_normalization=False, - actor_hidden_dims=[64, 64], - critic_hidden_dims=[64, 64], + actor = RslRlMLPModelCfg( + hidden_dims=[64, 64], activation="elu", + obs_normalization=False, + distribution_cfg=RslRlMLPModelCfg.GaussianDistributionCfg(init_std=1.0), + ) + critic = RslRlMLPModelCfg( + hidden_dims=[64, 64], + activation="elu", + obs_normalization=False, ) algorithm = RslRlPpoAlgorithmCfg( value_loss_coef=1.0, diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand/agents/rsl_rl_ppo_cfg.py index 6ab4c9e56f5a..fa4a96cb32e7 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand/agents/rsl_rl_ppo_cfg.py @@ -5,7 +5,7 @@ from isaaclab.utils import configclass -from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg +from isaaclab_rl.rsl_rl import RslRlMLPModelCfg, RslRlOnPolicyRunnerCfg, RslRlPpoAlgorithmCfg @configclass @@ -14,13 +14,16 @@ class ShadowHandPPORunnerCfg(RslRlOnPolicyRunnerCfg): max_iterations = 10000 save_interval = 250 experiment_name = "shadow_hand" - policy = RslRlPpoActorCriticCfg( - init_noise_std=1.0, - actor_obs_normalization=True, - critic_obs_normalization=True, - actor_hidden_dims=[512, 512, 256, 128], - critic_hidden_dims=[512, 512, 256, 128], + actor = RslRlMLPModelCfg( + hidden_dims=[512, 512, 256, 128], activation="elu", + obs_normalization=True, + distribution_cfg=RslRlMLPModelCfg.GaussianDistributionCfg(init_std=1.0), + ) + critic = RslRlMLPModelCfg( + hidden_dims=[512, 512, 256, 128], + activation="elu", + obs_normalization=True, ) algorithm = RslRlPpoAlgorithmCfg( value_loss_coef=1.0, @@ -44,13 +47,16 @@ class ShadowHandAsymFFPPORunnerCfg(RslRlOnPolicyRunnerCfg): max_iterations = 10000 save_interval = 250 experiment_name = "shadow_hand_openai_ff" - policy = RslRlPpoActorCriticCfg( - init_noise_std=1.0, - actor_obs_normalization=True, - critic_obs_normalization=True, - actor_hidden_dims=[400, 400, 200, 100], - critic_hidden_dims=[512, 512, 256, 128], + actor = RslRlMLPModelCfg( + hidden_dims=[400, 400, 200, 100], + activation="elu", + obs_normalization=True, + distribution_cfg=RslRlMLPModelCfg.GaussianDistributionCfg(init_std=1.0), + ) + critic = RslRlMLPModelCfg( + hidden_dims=[512, 512, 256, 128], activation="elu", + obs_normalization=True, ) algorithm = RslRlPpoAlgorithmCfg( value_loss_coef=1.0, @@ -74,13 +80,16 @@ class ShadowHandVisionFFPPORunnerCfg(RslRlOnPolicyRunnerCfg): max_iterations = 50000 save_interval = 250 experiment_name = "shadow_hand_vision" - policy = RslRlPpoActorCriticCfg( - init_noise_std=1.0, - actor_obs_normalization=True, - critic_obs_normalization=True, - actor_hidden_dims=[1024, 512, 512, 256, 128], - critic_hidden_dims=[1024, 512, 512, 256, 128], + actor = RslRlMLPModelCfg( + hidden_dims=[1024, 512, 512, 256, 128], + activation="elu", + obs_normalization=True, + distribution_cfg=RslRlMLPModelCfg.GaussianDistributionCfg(init_std=1.0), + ) + critic = RslRlMLPModelCfg( + hidden_dims=[1024, 512, 512, 256, 128], activation="elu", + obs_normalization=True, ) algorithm = RslRlPpoAlgorithmCfg( value_loss_coef=1.0, diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand_over/shadow_hand_over_env.py b/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand_over/shadow_hand_over_env.py index a692253cf2fb..66b9012d036b 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand_over/shadow_hand_over_env.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand_over/shadow_hand_over_env.py @@ -10,7 +10,6 @@ import numpy as np import torch -import warp as wp import isaaclab.sim as sim_utils from isaaclab.assets import Articulation, RigidObject @@ -64,7 +63,7 @@ def __init__(self, cfg: ShadowHandOverEnvCfg, render_mode: str | None = None, ** self.num_fingertips = len(self.finger_bodies) # joint limits - joint_pos_limits = wp.to_torch(self.right_hand.root_view.get_dof_limits()).to(self.device) + joint_pos_limits = self.right_hand.data.joint_limits.torch.to(self.device) self.hand_dof_lower_limits = joint_pos_limits[..., 0] self.hand_dof_upper_limits = joint_pos_limits[..., 1] diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand_over/shadow_hand_over_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand_over/shadow_hand_over_env_cfg.py index acf37a54c4b0..e2d8bdbd8d6d 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand_over/shadow_hand_over_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand_over/shadow_hand_over_env_cfg.py @@ -3,10 +3,12 @@ # # SPDX-License-Identifier: BSD-3-Clause +from isaaclab_newton.physics import MJWarpSolverCfg, NewtonCfg from isaaclab_physx.physics import PhysxCfg import isaaclab.envs.mdp as mdp import isaaclab.sim as sim_utils +from isaaclab.actuators import ImplicitActuatorCfg from isaaclab.assets import ArticulationCfg, RigidObjectCfg from isaaclab.envs import DirectMARLEnvCfg from isaaclab.managers import EventTermCfg as EventTerm @@ -17,12 +19,21 @@ from isaaclab.sim.spawners.materials.physics_materials_cfg import RigidBodyMaterialCfg from isaaclab.utils import configclass +from isaaclab_tasks.direct.shadow_hand.shadow_hand_env_cfg import ShadowHandRobotCfg +from isaaclab_tasks.utils import PresetCfg, preset + from isaaclab_assets.robots.shadow_hand import SHADOW_HAND_CFG @configclass class EventCfg: - """Configuration for randomization.""" + """Configuration for randomization (PhysX path). + + Note: this config is currently not wired into ``ShadowHandOverEnvCfg.events`` - + it is kept as a reference for future event-randomization work. The event + terms here use PhysX-only APIs (rigid-body materials, fixed tendons), so + they would need a Newton variant before being enabled in the env. + """ # -- robot robot_physics_material = EventTerm( @@ -113,6 +124,146 @@ class EventCfg: ) +# Reuse the single-agent Shadow Hand Newton port (USD path, ``rot`` reapplication +# workaround, effort limits, joint regex). The multi-agent variant only diverges +# in actuator gains (stiffness/damping bumped for the catch task) and adds a +# ``distal_passive`` override for the J0 USD-baked values. +_SHADOW_HAND_NEWTON_CFG = ShadowHandRobotCfg().newton_mjwarp + + +def _shadow_hand_cfg( + prim_path: str, + init_pos: tuple[float, float, float], + init_rot: tuple[float, float, float, float], +) -> PresetCfg: + """Per-hand Shadow Hand preset (PhysX and Newton MJWarp variants). + + Both variants are placed at *prim_path* with the same init pose; per-hand + differences (right vs left) come from the caller's *prim_path* / *init_pos* / + *init_rot* — the gain tuning is identical on both hands. + + The Newton variant layers two :class:`~isaaclab.actuators.ImplicitActuatorCfg` + overrides on top of the single-agent Newton port: + + * ``fingers`` actuator: ``stiffness=20.0`` / ``damping=2.0`` (vs PhysX's + ``5.0`` / ``0.5`` on wrists and ``1.0`` / ``0.1`` on fingers). PhysX layers + ``fixed_tendons_props(limit_stiffness=30, damping=0.1)`` and runs + ``solver_position_iteration_count=8`` per substep — both amplify the + effective torque per unit nominal gain. Newton's MJWarp implicit-PD path + has neither, so a larger nominal gain is needed for comparable joint + authority. ``20.0`` / ``2.0`` is the smallest tested setting at which + MAPPO learns the catch (mean reward at iter 200 / 2048 envs goes from + ~27 at PhysX-mirrored gains to ~777). + * ``distal_passive`` on the four ``robot0_(FF|MF|RF|LF)J0`` joints with + ``stiffness=10.0`` / ``damping=0.1``. The Newton USD bakes + ``stiffness=286 / damping=57`` on these joints from the MJCF→USD + translation, which fights the ``MjcTendon`` coupling and bounces the + ball. ``stiffness=10`` (~1/3 of PhysX's ``limit_stiffness=30``) keeps + the joints near-passive while the tendon constraint dominates. + """ + physx_cfg = SHADOW_HAND_CFG.replace(prim_path=prim_path).replace( + init_state=ArticulationCfg.InitialStateCfg(pos=init_pos, rot=init_rot, joint_pos={".*": 0.0}) + ) + newton_cfg = _SHADOW_HAND_NEWTON_CFG.replace( + prim_path=prim_path, + init_state=_SHADOW_HAND_NEWTON_CFG.init_state.replace(pos=init_pos, rot=init_rot), + actuators={ + "fingers": _SHADOW_HAND_NEWTON_CFG.actuators["fingers"].replace(stiffness=20.0, damping=2.0), + "distal_passive": ImplicitActuatorCfg( + joint_names_expr=["robot0_(FF|MF|RF|LF)J0"], + stiffness=10.0, + damping=0.1, + friction=1e-2, + armature=2e-3, + ), + }, + ) + return preset(default=physx_cfg, physx=physx_cfg, newton_mjwarp=newton_cfg) + + +@configclass +class ObjectCfg(PresetCfg): + """Hand-over object preset. + + Both backends spawn the same procedural sphere as a free rigid body: + Newton's :class:`~isaaclab_newton.assets.RigidObject` resolves the + asset via the ``UsdPhysics.RigidBodyAPI`` that + :class:`~isaaclab.sim.RigidBodyPropertiesCfg` applies. The Newton + variant drops PhysX-only knobs (per-shape solver iterations, sleep + thresholds, max depenetration velocity, custom physics material). + """ + + physx = RigidObjectCfg( + prim_path="/World/envs/env_.*/object", + spawn=sim_utils.SphereCfg( + radius=0.0335, + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.8, 1.0, 0.0)), + physics_material=sim_utils.RigidBodyMaterialCfg(static_friction=0.7), + rigid_props=sim_utils.RigidBodyPropertiesCfg( + kinematic_enabled=False, + disable_gravity=False, + enable_gyroscopic_forces=True, + solver_position_iteration_count=8, + solver_velocity_iteration_count=0, + sleep_threshold=0.005, + stabilization_threshold=0.0025, + max_depenetration_velocity=1000.0, + ), + collision_props=sim_utils.CollisionPropertiesCfg(), + mass_props=sim_utils.MassPropertiesCfg(density=500.0), + ), + init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, -0.39, 0.54), rot=(0.0, 0.0, 0.0, 1.0)), + ) + newton_mjwarp = RigidObjectCfg( + prim_path="/World/envs/env_.*/object", + spawn=sim_utils.SphereCfg( + radius=0.0335, + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.8, 1.0, 0.0)), + rigid_props=sim_utils.RigidBodyPropertiesCfg( + kinematic_enabled=False, + disable_gravity=False, + enable_gyroscopic_forces=True, + ), + collision_props=sim_utils.CollisionPropertiesCfg(), + mass_props=sim_utils.MassPropertiesCfg(density=500.0), + ), + init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, -0.39, 0.54), rot=(0.0, 0.0, 0.0, 1.0)), + ) + default = physx + + +@configclass +class PhysicsCfg(PresetCfg): + """Physics-backend preset (PhysX vs Newton/MJWarp). + + Newton settings mirror the single-agent ShadowHand Newton port: elliptic + cone, ``impratio=10`` (favors normal contacts over friction), 100 solver + iterations, 2 substeps. Empirically converges on the single-agent ShadowHand + tasks; tuning may be needed for handover-specific contact dynamics. + """ + + physx = PhysxCfg( + bounce_threshold_velocity=0.2, + gpu_max_rigid_contact_count=2**23, + gpu_max_rigid_patch_count=2**23, + ) + newton_mjwarp = NewtonCfg( + solver_cfg=MJWarpSolverCfg( + solver="newton", + integrator="implicitfast", + njmax=200, + nconmax=70, + impratio=10.0, + cone="elliptic", + update_data_interval=2, + ccd_iterations=50, # bumped from default 35 for multi-finger contact geometry + ), + num_substeps=2, + debug_mode=False, + ) + default = physx + + @configclass class ShadowHandOverEnvCfg(DirectMARLEnvCfg): # env @@ -131,24 +282,18 @@ class ShadowHandOverEnvCfg(DirectMARLEnvCfg): static_friction=1.0, dynamic_friction=1.0, ), - physics=PhysxCfg( - bounce_threshold_velocity=0.2, - ), + physics=PhysicsCfg(), ) # robot - right_robot_cfg: ArticulationCfg = SHADOW_HAND_CFG.replace(prim_path="/World/envs/env_.*/RightRobot").replace( - init_state=ArticulationCfg.InitialStateCfg( - pos=(0.0, 0.0, 0.5), - rot=(0.0, 0.0, 0.0, 1.0), - joint_pos={".*": 0.0}, - ) - ) - left_robot_cfg: ArticulationCfg = SHADOW_HAND_CFG.replace(prim_path="/World/envs/env_.*/LeftRobot").replace( - init_state=ArticulationCfg.InitialStateCfg( - pos=(0.0, -1.0, 0.5), - rot=(0.0, 0.0, 1.0, 0.0), - joint_pos={".*": 0.0}, - ) + right_robot_cfg: PresetCfg = _shadow_hand_cfg( + prim_path="/World/envs/env_.*/RightRobot", + init_pos=(0.0, 0.0, 0.5), + init_rot=(0.0, 0.0, 0.0, 1.0), + ) + left_robot_cfg: PresetCfg = _shadow_hand_cfg( + prim_path="/World/envs/env_.*/LeftRobot", + init_pos=(0.0, -1.0, 0.5), + init_rot=(0.0, 0.0, 1.0, 0.0), ) actuated_joint_names = [ "robot0_WRJ1", @@ -181,27 +326,7 @@ class ShadowHandOverEnvCfg(DirectMARLEnvCfg): ] # in-hand object - object_cfg: RigidObjectCfg = RigidObjectCfg( - prim_path="/World/envs/env_.*/object", - spawn=sim_utils.SphereCfg( - radius=0.0335, - visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.8, 1.0, 0.0)), - physics_material=sim_utils.RigidBodyMaterialCfg(static_friction=0.7), - rigid_props=sim_utils.RigidBodyPropertiesCfg( - kinematic_enabled=False, - disable_gravity=False, - enable_gyroscopic_forces=True, - solver_position_iteration_count=8, - solver_velocity_iteration_count=0, - sleep_threshold=0.005, - stabilization_threshold=0.0025, - max_depenetration_velocity=1000.0, - ), - collision_props=sim_utils.CollisionPropertiesCfg(), - mass_props=sim_utils.MassPropertiesCfg(density=500.0), - ), - init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, -0.39, 0.54), rot=(0.0, 0.0, 0.0, 1.0)), - ) + object_cfg: ObjectCfg = ObjectCfg() # goal object goal_object_cfg: VisualizationMarkersCfg = VisualizationMarkersCfg( prim_path="/Visuals/goal_marker", diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/ant/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/ant/agents/rsl_rl_ppo_cfg.py index 986461733663..56cb2c4fd3a0 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/ant/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/ant/agents/rsl_rl_ppo_cfg.py @@ -5,7 +5,7 @@ from isaaclab.utils import configclass -from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg +from isaaclab_rl.rsl_rl import RslRlMLPModelCfg, RslRlOnPolicyRunnerCfg, RslRlPpoAlgorithmCfg @configclass @@ -14,13 +14,16 @@ class AntPPORunnerCfg(RslRlOnPolicyRunnerCfg): max_iterations = 1000 save_interval = 50 experiment_name = "ant" - policy = RslRlPpoActorCriticCfg( - init_noise_std=1.0, - actor_obs_normalization=False, - critic_obs_normalization=False, - actor_hidden_dims=[400, 200, 100], - critic_hidden_dims=[400, 200, 100], + actor = RslRlMLPModelCfg( + hidden_dims=[400, 200, 100], activation="elu", + obs_normalization=False, + distribution_cfg=RslRlMLPModelCfg.GaussianDistributionCfg(init_std=1.0), + ) + critic = RslRlMLPModelCfg( + hidden_dims=[400, 200, 100], + activation="elu", + obs_normalization=False, ) algorithm = RslRlPpoAlgorithmCfg( value_loss_coef=1.0, diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/cartpole/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/cartpole/agents/rsl_rl_ppo_cfg.py index 2a266a098df2..c53312ee7dd4 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/cartpole/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/cartpole/agents/rsl_rl_ppo_cfg.py @@ -5,7 +5,7 @@ from isaaclab.utils import configclass -from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg, RslRlSymmetryCfg +from isaaclab_rl.rsl_rl import RslRlMLPModelCfg, RslRlOnPolicyRunnerCfg, RslRlPpoAlgorithmCfg, RslRlSymmetryCfg import isaaclab_tasks.manager_based.classic.cartpole.mdp.symmetry as symmetry @@ -16,13 +16,16 @@ class CartpolePPORunnerCfg(RslRlOnPolicyRunnerCfg): max_iterations = 150 save_interval = 50 experiment_name = "cartpole" - policy = RslRlPpoActorCriticCfg( - init_noise_std=1.0, - actor_obs_normalization=False, - critic_obs_normalization=False, - actor_hidden_dims=[32, 32], - critic_hidden_dims=[32, 32], + actor = RslRlMLPModelCfg( + hidden_dims=[32, 32], activation="elu", + obs_normalization=False, + distribution_cfg=RslRlMLPModelCfg.GaussianDistributionCfg(init_std=1.0), + ) + critic = RslRlMLPModelCfg( + hidden_dims=[32, 32], + activation="elu", + obs_normalization=False, ) algorithm = RslRlPpoAlgorithmCfg( value_loss_coef=1.0, diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/humanoid/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/humanoid/agents/rsl_rl_ppo_cfg.py index f2c7f48e4558..076ad94480eb 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/humanoid/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/classic/humanoid/agents/rsl_rl_ppo_cfg.py @@ -15,7 +15,7 @@ from isaaclab.utils import configclass -from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg +from isaaclab_rl.rsl_rl import RslRlMLPModelCfg, RslRlOnPolicyRunnerCfg, RslRlPpoAlgorithmCfg @configclass @@ -24,13 +24,16 @@ class HumanoidPPORunnerCfg(RslRlOnPolicyRunnerCfg): max_iterations = 1000 save_interval = 100 experiment_name = "humanoid" - policy = RslRlPpoActorCriticCfg( - init_noise_std=1.0, - actor_obs_normalization=True, - critic_obs_normalization=True, - actor_hidden_dims=[400, 200, 100], - critic_hidden_dims=[400, 200, 100], + actor = RslRlMLPModelCfg( + hidden_dims=[400, 200, 100], activation="elu", + obs_normalization=True, + distribution_cfg=RslRlMLPModelCfg.GaussianDistributionCfg(init_std=1.0), + ) + critic = RslRlMLPModelCfg( + hidden_dims=[400, 200, 100], + activation="elu", + obs_normalization=True, ) algorithm = RslRlPpoAlgorithmCfg( value_loss_coef=2.0, diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/mdp/__init__.pyi b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/mdp/__init__.pyi index d54142b0609d..3085421b05d1 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/mdp/__init__.pyi +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/mdp/__init__.pyi @@ -3,19 +3,5 @@ # # SPDX-License-Identifier: BSD-3-Clause -__all__ = [ - "DroneUniformPoseCommand", - "DroneUniformPoseCommandCfg", - "base_roll_pitch", - "generated_drone_commands", - "ang_vel_xyz_exp", - "distance_to_goal_exp", - "lin_vel_xyz_exp", - "yaw_aligned", -] - -from .commands import DroneUniformPoseCommand, DroneUniformPoseCommandCfg -from .observations import base_roll_pitch, generated_drone_commands -from .rewards import ang_vel_xyz_exp, distance_to_goal_exp, lin_vel_xyz_exp, yaw_aligned from isaaclab.envs.mdp import * from isaaclab_contrib.mdp import * diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/mdp/commands/drone_pose_command.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/mdp/commands/drone_pose_command.py index a9072367c83b..5765177a4659 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/mdp/commands/drone_pose_command.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/mdp/commands/drone_pose_command.py @@ -42,7 +42,7 @@ def _update_metrics(self): ) # compute the error pos_error, rot_error = compute_pose_error( - # Sub-terrain shift for correct position error calculation @grzemal + # Sub-terrain shift for correct position error calculation self.pose_command_b[:, :3] + self._env.scene.env_origins, self.pose_command_w[:, 3:], self.robot.data.body_pos_w.torch[:, self.body_idx], @@ -58,7 +58,7 @@ def _debug_vis_callback(self, event): return # update the markers # -- goal pose - # Sub-terrain shift for visualization purposes @grzemal + # Sub-terrain shift for visualization purposes self.goal_pose_visualizer.visualize( self.pose_command_b[:, :3] + self._env.scene.env_origins, self.pose_command_b[:, 3:] ) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/mdp/curriculums.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/mdp/curriculums.py new file mode 100644 index 000000000000..3fc33a9328b5 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/mdp/curriculums.py @@ -0,0 +1,148 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Common curriculum classes for the drone navigation environment. + +The curriculum classes can be passed to the :class:`isaaclab.managers.CurriculumTermCfg` object to enable +the curriculum introduced by the class. +""" + +from __future__ import annotations + +from collections.abc import Sequence +from typing import TYPE_CHECKING + +import torch +import warp as wp + +from isaaclab.managers import ManagerTermBase, SceneEntityCfg +from isaaclab.managers.manager_term_cfg import CurriculumTermCfg + +if TYPE_CHECKING: + from isaaclab.assets import Articulation + from isaaclab.envs import ManagerBasedRLEnv + + +class ObstacleDensityCurriculum(ManagerTermBase): + """Curriculum that adjusts obstacle density based on performance. + + The difficulty state is stored internally in the class instance, avoiding + the need to store state on the environment object. + + The curriculum tracks per-environment difficulty levels used to control + the number of obstacles spawned in each environment. Difficulty progresses + based on agent performance (successful goal reaching vs. collisions). + + Attributes: + cfg: The configuration of the curriculum term. + _min_difficulty: Minimum difficulty level for obstacle density. + _max_difficulty: Maximum difficulty level for obstacle density. + _difficulty_levels: Tensor of shape (num_envs,) tracking difficulty per environment. + _asset_cfg: Scene entity configuration for the robot. + _command_name: Name of the command to track. + """ + + cfg: CurriculumTermCfg + """The configuration of the curriculum term.""" + + def __init__(self, cfg: CurriculumTermCfg, env: ManagerBasedRLEnv): + """Initialize the curriculum term. + + Args: + cfg: Configuration for the curriculum term. + env: The manager-based RL environment instance. + """ + super().__init__(cfg, env) + + # Extract parameters from config + self._min_difficulty = cfg.params["min_difficulty"] + self._max_difficulty = cfg.params["max_difficulty"] + self._asset_cfg = cfg.params.get("asset_cfg", SceneEntityCfg("robot")) + self._command_name = cfg.params.get("command_name", "target_pose") + + # Initialize difficulty levels for all environments + self._difficulty_levels = torch.ones(env.num_envs, device=env.device) * self._min_difficulty + + def __call__( + self, + env: ManagerBasedRLEnv, + env_ids: Sequence[int], + asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), + command_name: str = "target_pose", + min_difficulty: int | None = None, + max_difficulty: int | None = None, + ) -> float: + """Update obstacle density curriculum based on performance. + + Args: + env: The manager-based RL environment instance. + env_ids: Environment indices to update. + asset_cfg: Scene entity configuration for the robot. Defaults to SceneEntityCfg("robot"). + command_name: Name of the command to track. Defaults to "target_pose". + max_difficulty: Maximum difficulty level. Defaults to 10. + min_difficulty: Minimum difficulty level. Defaults to 2. + + Returns: + Mean difficulty level across all environments (for logging). + """ + # Extract robot and command + asset: Articulation = env.scene[asset_cfg.name] + command = env.command_manager.get_command(command_name) + + target_position_w = command[:, :3].clone() + current_position = wp.to_torch(asset.data.root_pos_w) - env.scene.env_origins + position_error = torch.norm(target_position_w[env_ids] - current_position[env_ids], dim=1) + + # Decide difficulty changes + crashed = env.termination_manager.terminated[env_ids] + move_up = position_error < 1.5 # Success + move_down = crashed & ~move_up + + # Update difficulty levels + self._difficulty_levels[env_ids] += move_up.long() - move_down.long() + self._difficulty_levels[env_ids] = torch.clamp( + self._difficulty_levels[env_ids], min=self._min_difficulty, max=self._max_difficulty - 1 + ) + + return self._difficulty_levels.float().mean().item() + + @property + def difficulty_levels(self) -> torch.Tensor: + """Get the current difficulty levels for all environments. + + Returns: + Tensor of shape (num_envs,) with difficulty levels. + """ + return self._difficulty_levels + + @property + def min_difficulty(self) -> int: + """Get the minimum difficulty level.""" + return self._min_difficulty + + @property + def max_difficulty(self) -> int: + """Get the maximum difficulty level.""" + return self._max_difficulty + + +def get_obstacle_curriculum_term(env: ManagerBasedRLEnv) -> ObstacleDensityCurriculum | None: + """Get the ObstacleDensityCurriculum instance from the curriculum manager. + + This helper function searches the curriculum manager for an active + ObstacleDensityCurriculum term and returns it if found. This allows + other MDP components (rewards, events) to access the curriculum state. + + Args: + env: The manager-based RL environment instance. + + Returns: + The ObstacleDensityCurriculum instance if found, None otherwise. + """ + curriculum_manager = env.curriculum_manager + for term_cfg in curriculum_manager._term_cfgs: + if isinstance(term_cfg.func, ObstacleDensityCurriculum): + return term_cfg.func + return None diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/mdp/events.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/mdp/events.py new file mode 100644 index 000000000000..992521fdd47e --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/mdp/events.py @@ -0,0 +1,189 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Event functions specific to the drone ARL environments.""" + +from __future__ import annotations + +from typing import TYPE_CHECKING + +import torch + +import isaaclab.utils.math as math_utils +from isaaclab.managers import SceneEntityCfg + +from .curriculums import get_obstacle_curriculum_term + +if TYPE_CHECKING: + from isaaclab.assets import RigidObjectCollection + from isaaclab.envs import ManagerBasedRLEnv + + +def reset_obstacles_with_individual_ranges( + env: ManagerBasedRLEnv, + env_ids: torch.Tensor, + asset_cfg: SceneEntityCfg, + obstacle_configs: dict, + wall_configs: dict, + env_size: tuple[float, float, float], + use_curriculum: bool = True, + min_num_obstacles: int = 1, + max_num_obstacles: int = 10, + ground_offset: float = 0.1, +) -> None: + """Reset obstacle and wall positions for specified environments without collision checking. + + This function repositions all walls and a curriculum-determined subset of obstacles + within the specified environment bounds. + + Walls are positioned at fixed locations based on their configuration ratios. Obstacles + are randomly placed within their designated zones, with the number of active obstacles + determined by the curriculum difficulty level. Inactive obstacles are moved far below + the scene (-1000m in Z) to effectively remove them from the environment. + + The curriculum scaling works as: + num_obstacles = min + (difficulty / max_difficulty) * (max - min) + + Args: + env: The manager-based RL environment instance. + env_ids: Tensor of environment indices to reset. + asset_cfg: Scene entity configuration identifying the obstacle collection. + obstacle_configs: Dictionary mapping obstacle type names to their BoxCfg + configurations, specifying size and placement ranges. + wall_configs: Dictionary mapping wall names to their BoxCfg configurations. + env_size: Tuple of (length, width, height) defining the environment bounds in meters. + use_curriculum: If True, number of obstacles scales with curriculum difficulty. + If False, spawns max_num_obstacles in every environment. Defaults to True. + min_num_obstacles: Minimum number of obstacles to spawn per environment. + Defaults to 1. + max_num_obstacles: Maximum number of obstacles to spawn per environment. + Defaults to 10. + ground_offset: Z-axis offset to prevent obstacles from spawning at z=0. + Defaults to 0.1 meters. + + Note: + This function expects the environment to have `_obstacle_difficulty_levels` and + `_max_obstacle_difficulty` attributes when `use_curriculum=True`. These are + typically set by :func:`obstacle_density_curriculum`. + """ + obstacles: RigidObjectCollection = env.scene[asset_cfg.name] + + num_objects = obstacles.num_objects + num_envs = len(env_ids) + object_names = obstacles.object_names + + # Get difficulty levels per environment + if use_curriculum: + curriculum_term = get_obstacle_curriculum_term(env) + if curriculum_term is not None: + # Get difficulty levels for the specific environments being reset + difficulty_levels = curriculum_term.difficulty_levels[env_ids] + max_difficulty = curriculum_term.max_difficulty + else: + # Fallback: use max obstacles if curriculum not found + difficulty_levels = torch.ones(num_envs, device=env.device) * max_num_obstacles + max_difficulty = max_num_obstacles + else: + difficulty_levels = torch.ones(num_envs, device=env.device) * max_num_obstacles + max_difficulty = max_num_obstacles + + # Calculate active obstacles per env based on difficulty + obstacles_per_env = ( + min_num_obstacles + (difficulty_levels / max_difficulty) * (max_num_obstacles - min_num_obstacles) + ).long() + + # Prepare tensors + all_poses = torch.zeros(num_envs, num_objects, 7, device=env.device) + all_velocities = torch.zeros(num_envs, num_objects, 6, device=env.device) + + wall_names = list(wall_configs.keys()) + obstacle_types = list(obstacle_configs.values()) + env_size_t = torch.tensor(env_size, device=env.device) + + # place walls + for wall_name, wall_cfg in wall_configs.items(): + if wall_name in object_names: + wall_idx = object_names.index(wall_name) + + min_ratio = torch.tensor(wall_cfg.center_ratio_min, device=env.device) + max_ratio = torch.tensor(wall_cfg.center_ratio_max, device=env.device) + + if torch.allclose(min_ratio, max_ratio): + center_ratios = min_ratio.unsqueeze(0).repeat(num_envs, 1) + else: + ratios = torch.rand(num_envs, 3, device=env.device) + center_ratios = ratios * (max_ratio - min_ratio) + min_ratio + + positions = (center_ratios - 0.5) * env_size_t + positions[:, 2] += ground_offset + positions += env.scene.env_origins[env_ids] + + all_poses[:, wall_idx, 0:3] = positions + all_poses[:, wall_idx, 3:7] = torch.tensor([1.0, 0.0, 0.0, 0.0], device=env.device).repeat(num_envs, 1) + + # Get obstacle indices + obstacle_indices = [idx for idx, name in enumerate(object_names) if name not in wall_names] + + if len(obstacle_indices) == 0: + obstacles.write_object_pose_to_sim(all_poses, env_ids=env_ids) + obstacles.write_object_velocity_to_sim(all_velocities, env_ids=env_ids) + return + + # Determine which obstacles are active per env + active_masks = torch.zeros(num_envs, len(obstacle_indices), dtype=torch.bool, device=env.device) + for env_idx in range(num_envs): + num_active = obstacles_per_env[env_idx].item() + perm = torch.randperm(len(obstacle_indices), device=env.device)[:num_active] + active_masks[env_idx, perm] = True + + # place obstacles + for obj_list_idx in range(len(obstacle_indices)): + obj_idx = obstacle_indices[obj_list_idx] + + # Which envs need this obstacle? + envs_need_obstacle = active_masks[:, obj_list_idx] + + if not envs_need_obstacle.any(): + # Move all to -1000 + all_poses[:, obj_idx, 0:3] = env.scene.env_origins[env_ids] + torch.tensor( + [0.0, 0.0, -1000.0], device=env.device + ) + all_poses[:, obj_idx, 3:7] = torch.tensor([1.0, 0.0, 0.0, 0.0], device=env.device) + continue + + # Get obstacle config + config_idx = obj_list_idx % len(obstacle_types) + obs_cfg = obstacle_types[config_idx] + + min_ratio = torch.tensor(obs_cfg.center_ratio_min, device=env.device) + max_ratio = torch.tensor(obs_cfg.center_ratio_max, device=env.device) + + # sample object positions + num_active_envs = envs_need_obstacle.sum().item() + ratios = torch.rand(num_active_envs, 3, device=env.device) + positions = (ratios * (max_ratio - min_ratio) + min_ratio - 0.5) * env_size_t + positions[:, 2] += ground_offset + + # Add env origins + active_env_indices = torch.where(envs_need_obstacle)[0] + positions += env.scene.env_origins[env_ids[active_env_indices]] + + # Generate quaternions + quats = math_utils.random_orientation(num_envs, device=env.device) + + # Write poses + all_poses[envs_need_obstacle, obj_idx, 0:3] = positions + all_poses[envs_need_obstacle, obj_idx, 3:7] = quats[envs_need_obstacle] + + # Move inactive obstacles far away + inactive = ~envs_need_obstacle + all_poses[inactive, obj_idx, 0:3] = env.scene.env_origins[env_ids[inactive]] + torch.tensor( + [0.0, 0.0, -1000.0], device=env.device + ) + all_poses[inactive, obj_idx, 3:7] = torch.tensor([1.0, 0.0, 0.0, 0.0], device=env.device) + + # Write to sim + obstacles.write_object_pose_to_sim(all_poses, env_ids=env_ids) + obstacles.write_object_velocity_to_sim(all_velocities, env_ids=env_ids) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/mdp/observations.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/mdp/observations.py index c8b8048bd68b..e084eabcd013 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/mdp/observations.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/mdp/observations.py @@ -11,20 +11,27 @@ from __future__ import annotations +import os from typing import TYPE_CHECKING import torch import isaaclab.utils.math as math_utils -from isaaclab.managers import SceneEntityCfg +from isaaclab.managers import ManagerTermBase, SceneEntityCfg if TYPE_CHECKING: from isaaclab.assets import Articulation from isaaclab.envs import ManagerBasedEnv, ManagerBasedRLEnv + from isaaclab.managers import ObservationTermCfg + from isaaclab.sensors.camera.camera import Camera + from isaaclab.sensors.camera.tiled_camera import TiledCamera + from isaaclab.sensors.ray_caster.multi_mesh_ray_caster_camera import MultiMeshRayCasterCamera + from isaaclab.sensors.ray_caster.ray_caster_camera import RayCasterCamera from isaaclab_contrib.assets import Multirotor from isaaclab.envs.utils.io_descriptors import generic_io_descriptor, record_shape +from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR, retrieve_file_path """ State. @@ -57,6 +64,178 @@ def base_roll_pitch(env: ManagerBasedEnv, asset_cfg: SceneEntityCfg = SceneEntit return torch.cat((roll.unsqueeze(-1), pitch.unsqueeze(-1)), dim=-1) +""" +Sensors +""" + + +class ImageLatentObservation(ManagerTermBase): + """Callable observation term that returns VAE latents from camera images. + + This observation term extracts images from a configured camera sensor, normalizes them + based on the data type, and passes them through a pre-trained VAE model to obtain + latent representations. The VAE model is loaded once and cached on the class to avoid + repeated disk loads across all instances. + + The term is designed to work with the Isaac Lab observation manager and integrates + seamlessly with other observation terms in multi-modal observation spaces. + + Attributes: + camera_sensor: The camera sensor to extract images from (TiledCamera, Camera, + RayCasterCamera, or MultiMeshRayCasterCamera). + data_type: Type of data to extract from the sensor (e.g., "distance_to_image_plane"). + convert_perspective_to_orthogonal: Whether to convert perspective depth to orthogonal. + normalize: Whether to normalize images before passing to VAE. + + Example: + To use this in an environment configuration: + + .. code-block:: python + + depth_latent = ObsTerm( + func=mdp.ImageLatentObservation, + params={ + "sensor_cfg": SceneEntityCfg("depth_camera"), + "data_type": "distance_to_image_plane", + "normalize": True, + }, + ) + """ + + _model: torch.jit.ScriptModule | None = None + + def __init__(self, cfg: ObservationTermCfg, env: ManagerBasedRLEnv): + """Initialize the image latent observation term. + + Extracts configuration from cfg.params and caches a reference to the camera sensor + for efficient repeated access during observation collection. + + Args: + cfg: Configuration object containing the observation term configuration, + including params dict with: + - sensor_cfg (SceneEntityCfg): Scene entity config for the camera sensor. + - data_type (str): Data type to extract from the sensor. + - convert_perspective_to_orthogonal (bool, optional): Whether to convert + perspective to orthogonal depth. Defaults to False. + - normalize (bool, optional): Whether to normalize images. Defaults to True. + env: The manager-based RL environment instance. + + Raises: + KeyError: If required params ("sensor_cfg", "data_type") are missing. + RuntimeError: If the specified camera sensor is not found in the scene. + """ + super().__init__(cfg, env) + self.camera_sensor: TiledCamera | Camera | RayCasterCamera | MultiMeshRayCasterCamera = env.scene.sensors[ + cfg.params["sensor_cfg"].name + ] + self.data_type: str = cfg.params["data_type"] + self.convert_perspective_to_orthogonal = bool(cfg.params.get("convert_perspective_to_orthogonal", False)) + self.normalize = bool(cfg.params.get("normalize", True)) + + @classmethod + def _get_model(cls, device): + """Load or retrieve the cached VAE model. + + The model is loaded from disk only once per process and cached on the class. + Subsequent calls return the cached instance, avoiding repeated I/O and model + initialization overhead. + + Args: + device: PyTorch device to load the model onto (e.g., "cpu", "cuda:0"). + + Returns: + Loaded VAE model as a TorchScript ScriptModule, set to evaluation mode. + + Raises: + FileNotFoundError: If the VAE model file cannot be found at the expected path. + RuntimeError: If the model cannot be loaded (e.g., corrupted file). + """ + if cls._model is None: + model_path = os.path.join(ISAACLAB_NUCLEUS_DIR, "Contrib/Drone/vae_model.pt") + download_dir = os.path.join(".pretrained_checkpoints", "drone_arl", "vae_model.pt") + resume_path = retrieve_file_path(model_path, download_dir) + cls._model = torch.jit.load(resume_path, map_location=device) + cls._model.eval() + return cls._model + + def __call__(self, env: ManagerBasedEnv, sensor_cfg: SceneEntityCfg, data_type: str) -> torch.Tensor: + """Compute VAE latents for the current camera frame. + + Extracts images from the camera sensor, applies normalization if configured, + and passes them through the VAE model to obtain latent representations. + + Args: + env: The manager-based environment providing scene and device information. + sensor_cfg: Scene entity config for the camera sensor (unused, already set in __init__). + data_type: Data type to extract from the sensor (unused, already set in __init__). + convert_perspective_to_orthogonal: Whether to convert perspective to orthogonal depth + (unused, already set in __init__). + normalize: Whether to normalize images (unused, already set in __init__). + + Returns: + torch.Tensor: Latent representations from the VAE model. + Shape is determined by the VAE architecture (typically (num_envs, latent_dim)). + + Raises: + ValueError: If data_type is "distance_to_image_plane" but normalize is False, + or if an unsupported data_type is encountered with normalize=True. + RuntimeError: If the VAE model inference fails or tensors have incompatible shapes. + + Notes: + - Images are converted to float16 before passing to the VAE for efficiency. + - Infinity values in depth images are clamped to 10.0 during normalization. + - Very small depth values (< 0.02) are set to -1.0 to indicate invalid regions. + - The parameters (sensor_cfg, data_type, etc.) are ignored here as they are + already stored during initialization. They are included in the signature only + to satisfy the observation manager's parameter validation. + """ + images = self.camera_sensor.data.output[self.data_type].clone() + + if (self.data_type == "distance_to_camera") and self.convert_perspective_to_orthogonal: + images = math_utils.orthogonalize_perspective_depth(images, self.camera_sensor.data.intrinsic_matrices) + + if self.normalize: + if self.data_type == "distance_to_image_plane": + images[images == float("inf")] = 10.0 + images[images == -float("inf")] = 10.0 + images[images > 10.0] = 10.0 + images = images / 10.0 + images[images < 0.02] = -1.0 + else: + raise ValueError(f"Image data type: {self.data_type} not supported") + + vae_model = self._get_model(env.device) + with torch.no_grad(): + latents = vae_model(images.squeeze(-1).half()) + + return latents + + +""" +Actions. +""" + + +@generic_io_descriptor(dtype=torch.float32, observation_type="Action", on_inspect=[record_shape]) +def last_action_navigation(env: ManagerBasedEnv, action_name: str = "velocity_commands") -> torch.Tensor: + """The last processed position/velocity/acceleration commands from the navigation action term. + + This function accesses the position/velocity/acceleration commands (vx, vy, vz, yaw_rate) that + were computed by the NavigationAction term. This avoids duplicating the + action processing logic. + + Args: + env: Manager-based environment providing the action manager. + action_name: Name of the navigation action term. Defaults to "velocity_commands". + + Returns: + torch.Tensor: Shape (num_envs, 4) containing position/velocity/acceleration commands. + """ + action_term = env.action_manager.get_term(action_name) + # Access the velocity_commands property from NavigationAction + return action_term.prev_commands + + """ Commands. """ diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/mdp/rewards.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/mdp/rewards.py index ce635cc544d8..a691df9e2d1e 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/mdp/rewards.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/mdp/rewards.py @@ -8,10 +8,13 @@ from typing import TYPE_CHECKING import torch +import warp as wp import isaaclab.utils.math as math_utils from isaaclab.managers import SceneEntityCfg +from .curriculums import get_obstacle_curriculum_term + if TYPE_CHECKING: from isaaclab.assets import RigidObject from isaaclab.envs import ManagerBasedRLEnv @@ -49,14 +52,65 @@ def distance_to_goal_exp( asset: RigidObject = env.scene[asset_cfg.name] command = env.command_manager.get_command(command_name) - target_position_w = command[:, :3].clone() current_position = asset.data.root_pos_w.torch - env.scene.env_origins # compute the error - position_error_square = torch.sum(torch.square(target_position_w - current_position), dim=1) + position_error_square = torch.sum(torch.square(command[:, :3] - current_position), dim=1) return torch.exp(-position_error_square / std**2) +def distance_to_goal_exp_curriculum( + env: ManagerBasedRLEnv, + asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), + std: float = 1.0, + command_name: str = "target_pose", +) -> torch.Tensor: + """Reward the distance to a goal position using an exponential kernel with curriculum-based scaling. + + This reward extends the basic exponential distance reward by applying a scaling factor + that increases with the obstacle difficulty level. As the curriculum progresses and + obstacle density increases, the reward weight grows to compensate for the added difficulty. + + The scaling weight is computed as: 1.0 + (difficulty_level / max_difficulty), meaning + the reward can scale from 1.0x (at minimum difficulty) to 2.0x (at maximum difficulty). + + Args: + env: The manager-based RL environment instance. + asset_cfg: SceneEntityCfg identifying the asset (defaults to "robot"). + std: Standard deviation used in the exponential kernel; larger values + produce a gentler falloff. Defaults to 1.0. + command_name: Name of the command to read the target pose from the + environment's command manager. The function expects the command + tensor to contain positions in its first three columns. + + Returns: + A 1-D tensor of shape (num_envs,) containing the per-environment weighted + reward values. Values are in [0, weight], where weight varies based on the + current curriculum difficulty level. + + Note: + If no curriculum is active (i.e., ObstacleDensityCurriculum is not found), + the function behaves identically to :func:`distance_to_goal_exp` with weight=1.0. + """ + # extract the used quantities (to enable type-hinting) + asset: RigidObject = env.scene[asset_cfg.name] + command = env.command_manager.get_command(command_name) + + current_position = wp.to_torch(asset.data.root_pos_w) - env.scene.env_origins + + # compute the error + position_error_square = torch.sum(torch.square(command[:, :3] - current_position), dim=1) + + # Get curriculum term and compute weight + curriculum_term = get_obstacle_curriculum_term(env) + if curriculum_term is not None: + weight = 1.0 + curriculum_term.difficulty_levels.float() / float(curriculum_term.max_difficulty) + else: + weight = 1.0 + + return weight * torch.exp(-position_error_square / std**2) + + def ang_vel_xyz_exp( env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), std: float = 1.0 ) -> torch.Tensor: @@ -86,6 +140,60 @@ def ang_vel_xyz_exp( return torch.exp(-ang_vel_squared / std**2) +def velocity_to_goal_reward_curriculum( + env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), command_name: str = "target_pose" +) -> torch.Tensor: + """Reward velocity alignment toward the goal with curriculum-based scaling. + + This reward encourages the agent to move in the direction of the goal by computing + the dot product between the asset's velocity vector and the normalized direction + vector to the goal. A curriculum-based scaling factor is applied that increases + with obstacle difficulty. + + The reward is positive when moving toward the goal, negative when moving away, + and zero when moving perpendicular to the goal direction. The magnitude scales + linearly with speed in the goal direction. + + The scaling weight is computed as: 1.0 + (difficulty_level / max_difficulty), + allowing the reward to scale from 1.0x to 2.0x as difficulty increases. + + Args: + env: The manager-based RL environment instance. + asset_cfg: SceneEntityCfg identifying the asset (defaults to "robot"). + command_name: Name of the command to read the target pose from the + environment's command manager. The function expects the command + tensor to contain positions in its first three columns. + + Returns: + A 1-D tensor of shape (num_envs,) containing the per-environment weighted + reward values. Values can be positive (moving toward goal), negative + (moving away), or zero (perpendicular motion), scaled by the curriculum weight. + + Note: + If no curriculum is active (i.e., ObstacleDensityCurriculum is not found), + the function uses weight=1.0 without curriculum scaling. + """ + # extract the used quantities (to enable type-hinting) + asset: RigidObject = env.scene[asset_cfg.name] + # get the center of the environment + command = env.command_manager.get_command(command_name) + + current_position = wp.to_torch(asset.data.root_pos_w) - env.scene.env_origins + direction_to_goal = command[:, :3] - current_position + direction_to_goal = direction_to_goal / (torch.norm(direction_to_goal, dim=1, keepdim=True) + 1e-8) + # compute the reward as the dot product between the velocity and the direction to the goal + velocity_towards_goal = torch.sum(wp.to_torch(asset.data.root_lin_vel_w) * direction_to_goal, dim=1) + + # Get curriculum term and compute weight + curriculum_term = get_obstacle_curriculum_term(env) + if curriculum_term is not None: + weight = 1.0 + curriculum_term.difficulty_levels.float() / float(curriculum_term.max_difficulty) + else: + weight = 1.0 + + return weight * velocity_towards_goal + + def lin_vel_xyz_exp( env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), std: float = 1.0 ) -> torch.Tensor: diff --git a/source/isaaclab_physx/isaaclab_physx/scene_data_providers/__init__.pyi b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/navigation/__init__.py similarity index 64% rename from source/isaaclab_physx/isaaclab_physx/scene_data_providers/__init__.pyi rename to source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/navigation/__init__.py index 32c6f9c07335..a2a3a87e6232 100644 --- a/source/isaaclab_physx/isaaclab_physx/scene_data_providers/__init__.pyi +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/navigation/__init__.py @@ -3,8 +3,4 @@ # # SPDX-License-Identifier: BSD-3-Clause -__all__ = [ - "PhysxSceneDataProvider", -] - -from .physx_scene_data_provider import PhysxSceneDataProvider +"""Drone NTNU navigation environments.""" diff --git a/source/isaaclab_newton/isaaclab_newton/scene_data_providers/__init__.pyi b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/navigation/config/__init__.py similarity index 63% rename from source/isaaclab_newton/isaaclab_newton/scene_data_providers/__init__.pyi rename to source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/navigation/config/__init__.py index 3cb204031738..61d66a54093c 100644 --- a/source/isaaclab_newton/isaaclab_newton/scene_data_providers/__init__.pyi +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/navigation/config/__init__.py @@ -3,8 +3,4 @@ # # SPDX-License-Identifier: BSD-3-Clause -__all__ = [ - "NewtonSceneDataProvider", -] - -from .newton_scene_data_provider import NewtonSceneDataProvider +"""Configurations for drone NTNU navigation environments.""" diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/navigation/config/arl_robot_1/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/navigation/config/arl_robot_1/__init__.py new file mode 100644 index 000000000000..7e7984260403 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/navigation/config/arl_robot_1/__init__.py @@ -0,0 +1,36 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import gymnasium as gym + +from . import agents + +## +# Register Gym environments. +## + +gym.register( + id="Isaac-Navigation-3DObstacles-ARL-Robot-1-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.floating_obstacles_env_cfg:FloatingObstacleEnvCfg", + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_rough_ppo_cfg.yaml", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:NavigationEnvPPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_rough_ppo_cfg.yaml", + }, +) + +gym.register( + id="Isaac-Navigation-3DObstacles-ARL-Robot-1-Play-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.floating_obstacles_env_cfg:FloatingObstacleEnvCfg_PLAY", + "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_rough_ppo_cfg.yaml", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:NavigationEnvPPORunnerCfg", + "skrl_cfg_entry_point": f"{agents.__name__}:skrl_rough_ppo_cfg.yaml", + }, +) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/navigation/config/arl_robot_1/agents/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/navigation/config/arl_robot_1/agents/__init__.py new file mode 100644 index 000000000000..460a30569089 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/navigation/config/arl_robot_1/agents/__init__.py @@ -0,0 +1,4 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/navigation/config/arl_robot_1/agents/rl_games_rough_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/navigation/config/arl_robot_1/agents/rl_games_rough_ppo_cfg.yaml new file mode 100644 index 000000000000..078d89875f77 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/navigation/config/arl_robot_1/agents/rl_games_rough_ppo_cfg.yaml @@ -0,0 +1,87 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +params: + seed: 42 + + # environment wrapper clipping + env: + clip_actions: 1.0 + algo: + name: a2c_continuous + + model: + name: continuous_a2c_logstd + + network: + name: actor_critic + separate: False + space: + continuous: + mu_activation: None + sigma_activation: None + mu_init: + name: default + sigma_init: + name: const_initializer + val: 0 + fixed_sigma: True + mlp: + units: [256,128,64] + d2rl: False + activation: elu + initializer: + name: default + scale: 2 + rnn: + name: gru + units: 64 + layers: 1 + # before_mlp: False + # layer_norm: True + config: + name: arl_robot_1_navigation + env_name: rlgpu + device: 'cuda:0' + device_name: 'cuda:0' + env_config: + num_envs: 8192 + + reward_shaper: + # min_val: -1 + scale_value: 0.1 + + normalize_advantage: True + gamma: 0.98 + tau: 0.95 + ppo: True + learning_rate: 1e-4 + lr_schedule: adaptive + kl_threshold: 0.016 + save_best_after: 10 + score_to_win: 100000 + grad_norm: 1.0 + entropy_coef: 0 + truncate_grads: True + e_clip: 0.2 + clip_value: False + num_actors: 1024 + horizon_length: 32 + minibatch_size: 2048 + mini_epochs: 4 + critic_coef: 2 + normalize_input: True + bounds_loss_coef: 0.0001 + max_epochs: 1500 + normalize_value: True + use_diagnostics: True + value_bootstrap: True + #weight_decay: 0.0001 + use_smooth_clamp: False + + player: + render: True + deterministic: True + games_num: 100000 diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/navigation/config/arl_robot_1/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/navigation/config/arl_robot_1/agents/rsl_rl_ppo_cfg.py new file mode 100644 index 000000000000..aeddab56e5f8 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/navigation/config/arl_robot_1/agents/rsl_rl_ppo_cfg.py @@ -0,0 +1,37 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.utils import configclass + +from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg + + +@configclass +class NavigationEnvPPORunnerCfg(RslRlOnPolicyRunnerCfg): + num_steps_per_env = 24 + max_iterations = 1500 + save_interval = 50 + experiment_name = "arl_robot_1_navigation" + empirical_normalization = False + policy = RslRlPpoActorCriticCfg( + init_noise_std=0.5, + actor_hidden_dims=[256, 128, 64], + critic_hidden_dims=[256, 128, 64], + activation="elu", + ) + algorithm = RslRlPpoAlgorithmCfg( + value_loss_coef=1.0, + use_clipped_value_loss=True, + clip_param=0.2, + entropy_coef=0.001, + num_learning_epochs=4, + num_mini_batches=4, + learning_rate=4.0e-4, + schedule="adaptive", + gamma=0.98, + lam=0.95, + desired_kl=0.01, + max_grad_norm=1.0, + ) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/navigation/config/arl_robot_1/agents/skrl_rough_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/navigation/config/arl_robot_1/agents/skrl_rough_ppo_cfg.yaml new file mode 100644 index 000000000000..c1465d64bef5 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/navigation/config/arl_robot_1/agents/skrl_rough_ppo_cfg.yaml @@ -0,0 +1,95 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +seed: 42 + + +# Models are instantiated using skrl's model instantiator utility +# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html +models: + separate: False + policy: + class: GaussianMixin + clip_actions: False + clip_log_std: True + min_log_std: -20.0 + max_log_std: 2.0 + initial_log_std: 0.0 + network: + - name: mlp + input: STATES + layers: [256, 128, 64] + activations: elu + - name: gru + input: mlp + type: GRU + layers: [64] + num_layers: 1 + output: ACTIONS + value: + class: DeterministicMixin + clip_actions: False + network: + - name: mlp + input: STATES + layers: [256, 128, 64] + activations: elu + - name: gru + input: mlp + type: GRU + layers: [64] + num_layers: 1 + output: ONE + + +# Rollout memory +# https://skrl.readthedocs.io/en/latest/api/memories/random.html +memory: + class: RandomMemory + memory_size: -1 # automatically determined (same as agent:rollouts) + + +# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) +# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html +agent: + class: PPO + rollouts: 24 + learning_epochs: 5 + mini_batches: 4 + discount_factor: 0.99 + lambda: 0.95 + learning_rate: 1.0e-03 + learning_rate_scheduler: KLAdaptiveLR + learning_rate_scheduler_kwargs: + kl_threshold: 0.01 + state_preprocessor: null + state_preprocessor_kwargs: null + value_preprocessor: RunningStandardScaler + value_preprocessor_kwargs: null + random_timesteps: 0 + learning_starts: 0 + grad_norm_clip: 1.0 + ratio_clip: 0.2 + value_clip: 0.2 + clip_predicted_values: True + entropy_loss_scale: 0.005 + value_loss_scale: 1.0 + kl_threshold: 0.0 + rewards_shaper_scale: 0.6 + time_limit_bootstrap: False + # logging and checkpoint + experiment: + directory: "arl_robot_1_navigation" + experiment_name: "" + write_interval: auto + checkpoint_interval: auto + + +# Sequential trainer +# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html +trainer: + class: SequentialTrainer + timesteps: 36000 + environment_info: log diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/navigation/config/arl_robot_1/floating_obstacles_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/navigation/config/arl_robot_1/floating_obstacles_env_cfg.py new file mode 100644 index 000000000000..1990750eefc0 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/navigation/config/arl_robot_1/floating_obstacles_env_cfg.py @@ -0,0 +1,41 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +## +# Pre-defined configs +## +from isaaclab.utils import configclass + +from isaaclab_tasks.manager_based.drone_arl.navigation.config.arl_robot_1.navigation_env_cfg import ( + NavigationVelocityFloatingObstacleEnvCfg, +) + +from isaaclab_assets.robots.arl_robot_1 import ARL_ROBOT_1_CFG + + +@configclass +class FloatingObstacleEnvCfg(NavigationVelocityFloatingObstacleEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + # switch robot to arl_robot_1 + self.scene.robot = ARL_ROBOT_1_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + self.scene.robot.actuators["thrusters"].dt = self.sim.dt + + +@configclass +class FloatingObstacleEnvCfg_PLAY(FloatingObstacleEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + self.curriculum.obstacle_levels.params["max_difficulty"] = 40 + self.curriculum.obstacle_levels.params["min_difficulty"] = 39 + + # disable randomization for play + self.observations.policy.enable_corruption = False + # remove random pushing event + self.events.base_external_force_torque = None + self.events.push_robot = None diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/navigation/config/arl_robot_1/navigation_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/navigation/config/arl_robot_1/navigation_env_cfg.py new file mode 100644 index 000000000000..6b4039e254a0 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/navigation/config/arl_robot_1/navigation_env_cfg.py @@ -0,0 +1,344 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import logging +import math +from dataclasses import MISSING + +from isaaclab_physx.physics import PhysxCfg + +import isaaclab.sim as sim_utils +from isaaclab.assets import AssetBaseCfg +from isaaclab.envs import ManagerBasedRLEnvCfg +from isaaclab.managers import CurriculumTermCfg as CurrTerm +from isaaclab.managers import EventTermCfg as EventTerm +from isaaclab.managers import ObservationGroupCfg as ObsGroup +from isaaclab.managers import ObservationTermCfg as ObsTerm +from isaaclab.managers import RewardTermCfg as RewTerm +from isaaclab.managers import SceneEntityCfg +from isaaclab.managers import TerminationTermCfg as DoneTerm +from isaaclab.scene import InteractiveSceneCfg +from isaaclab.sensors import ContactSensorCfg +from isaaclab.sensors.ray_caster.multi_mesh_ray_caster_camera_cfg import MultiMeshRayCasterCameraCfg +from isaaclab.sensors.ray_caster.patterns import PinholeCameraPatternCfg +from isaaclab.utils import configclass +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR +from isaaclab.utils.noise import UniformNoiseCfg as Unoise + +from isaaclab_contrib.assets import MultirotorCfg +from isaaclab_contrib.controllers import LeeVelControllerCfg + +import isaaclab_tasks.manager_based.drone_arl.mdp as mdp +from isaaclab_tasks.manager_based.drone_arl.mdp.commands import DroneUniformPoseCommandCfg +from isaaclab_tasks.manager_based.drone_arl.mdp.curriculums import ObstacleDensityCurriculum +from isaaclab_tasks.manager_based.drone_arl.mdp.events import reset_obstacles_with_individual_ranges +from isaaclab_tasks.manager_based.drone_arl.mdp.observations import ( + ImageLatentObservation, + base_roll_pitch, + generated_drone_commands, + last_action_navigation, +) +from isaaclab_tasks.manager_based.drone_arl.mdp.rewards import ( + distance_to_goal_exp_curriculum, + velocity_to_goal_reward_curriculum, +) + +logging.getLogger("isaaclab.sensors.ray_caster.multi_mesh_ray_caster").setLevel(logging.WARNING) + +## +# Pre-defined configs +## +from .scenes.obstacle_scenes.obstacle_scene import ( + OBSTACLE_SCENE_CFG, + generate_obstacle_collection, +) + + +## +# Scene definition +## +@configclass +class ArlNavigationSceneCfg(InteractiveSceneCfg): + """Scene configuration for drone navigation with obstacles.""" + + # obstacles + object_collection = generate_obstacle_collection(OBSTACLE_SCENE_CFG) + + # robots + robot: MultirotorCfg = MISSING + + # sensors + depth_camera = MultiMeshRayCasterCameraCfg( + prim_path="{ENV_REGEX_NS}/Robot/base_link", + mesh_prim_paths=[ + MultiMeshRayCasterCameraCfg.RaycastTargetCfg( + prim_expr=f"{{ENV_REGEX_NS}}/obstacle_{wall_name}", is_shared=False, track_mesh_transforms=True + ) + for wall_name, _ in OBSTACLE_SCENE_CFG.wall_cfgs.items() + ] + + [ + MultiMeshRayCasterCameraCfg.RaycastTargetCfg( + prim_expr=f"{{ENV_REGEX_NS}}/obstacle_{i}", is_shared=False, track_mesh_transforms=True + ) + for i in range(OBSTACLE_SCENE_CFG.max_num_obstacles) + ], + offset=MultiMeshRayCasterCameraCfg.OffsetCfg( + pos=(0.15, 0.0, 0.04), rot=(1.0, 0.0, 0.0, 0.0), convention="world" + ), + update_period=0.1, + pattern_cfg=PinholeCameraPatternCfg( + width=480, height=270, focal_length=0.193, horizontal_aperture=0.36, vertical_aperture=0.21 + ), + data_types=["distance_to_image_plane"], + max_distance=10.0, + depth_clipping_behavior="max", + ) + + contact_forces = ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/Robot/.*", + update_period=0.0, + history_length=10, + debug_vis=False, + ) + # lights + sky_light = AssetBaseCfg( + prim_path="/World/skyLight", + spawn=sim_utils.DomeLightCfg( + intensity=750.0, + texture_file=f"{ISAAC_NUCLEUS_DIR}/Materials/Textures/Skies/PolyHaven/kloofendal_43d_clear_puresky_4k.hdr", + ), + ) + + +## +# MDP settings +## + + +@configclass +class CommandsCfg: + """Command specifications for the MDP.""" + + target_pose = DroneUniformPoseCommandCfg( + asset_name="robot", + body_name="base_link", + resampling_time_range=(10.0, 10.0), + debug_vis=True, + ranges=DroneUniformPoseCommandCfg.Ranges( + pos_x=(4.0, 5.0), + pos_y=(-3.0, 3.0), + pos_z=(1.0, 5.0), + roll=(-0.0, 0.0), + pitch=(-0.0, 0.0), + yaw=(-0.0, 0.0), + ), + ) + + +@configclass +class ActionsCfg: + """Action specifications for the MDP.""" + + velocity_commands = mdp.NavigationActionCfg( + asset_name="robot", + scale=1.0, + offset=0.0, + preserve_order=False, + use_default_offset=False, + controller_cfg=LeeVelControllerCfg( + K_vel_range=((2.5, 2.5, 1.5), (3.5, 3.5, 2.0)), + K_rot_range=((1.6, 1.6, 0.25), (1.85, 1.85, 0.4)), + K_angvel_range=((0.4, 0.4, 0.075), (0.5, 0.5, 0.09)), + max_inclination_angle_rad=1.0471975511965976, + max_yaw_rate=1.0471975511965976, + ), + max_magnitude=2.0, + max_yaw_command=3.14 / 3.0, + max_inclination_angle=3.14 / 4.0, + ) + + +@configclass +class ObservationsCfg: + """Observation specifications for the MDP.""" + + @configclass + class PolicyCfg(ObsGroup): + """Observations for policy group.""" + + # observation terms (order preserved) + base_link_position = ObsTerm( + func=generated_drone_commands, + params={"command_name": "target_pose", "asset_cfg": SceneEntityCfg("robot")}, + noise=Unoise(n_min=-0.1, n_max=0.1), + ) + base_roll_pitch = ObsTerm(func=base_roll_pitch, noise=Unoise(n_min=-0.1, n_max=0.1)) + base_lin_vel = ObsTerm(func=mdp.base_lin_vel, noise=Unoise(n_min=-0.1, n_max=0.1)) + base_ang_vel = ObsTerm(func=mdp.base_ang_vel, noise=Unoise(n_min=-0.1, n_max=0.1)) + last_action = ObsTerm( + func=last_action_navigation, + params={"action_name": "velocity_commands"}, + ) + depth_latent = ObsTerm( + func=ImageLatentObservation, + params={"sensor_cfg": SceneEntityCfg("depth_camera"), "data_type": "distance_to_image_plane"}, + ) + + def __post_init__(self): + self.enable_corruption = False + self.concatenate_terms = True + + # observation groups + policy: PolicyCfg = PolicyCfg() + + +@configclass +class EventCfg: + """Configuration for events.""" + + # reset + + reset_base = EventTerm( + func=mdp.reset_root_state_uniform, + mode="reset", + params={ + "pose_range": { + "x": (-5.0, -4.5), + "y": (-3.0, 3.0), + "z": (1.0, 5.0), + "yaw": (-math.pi / 6.0, math.pi / 6.0), + }, + "velocity_range": { + "x": (-0.2, 0.2), + "y": (-0.2, 0.2), + "z": (-0.2, 0.2), + "roll": (-0.2, 0.2), + "pitch": (-0.2, 0.2), + "yaw": (-0.2, 0.2), + }, + }, + ) + + reset_obstacles = EventTerm( + func=reset_obstacles_with_individual_ranges, + mode="reset", + params={ + "asset_cfg": SceneEntityCfg("object_collection"), + "obstacle_configs": OBSTACLE_SCENE_CFG.obstacle_cfgs, + "wall_configs": OBSTACLE_SCENE_CFG.wall_cfgs, + "env_size": OBSTACLE_SCENE_CFG.env_size, + "use_curriculum": True, + "min_num_obstacles": OBSTACLE_SCENE_CFG.min_num_obstacles, + "max_num_obstacles": OBSTACLE_SCENE_CFG.max_num_obstacles, + "ground_offset": OBSTACLE_SCENE_CFG.ground_offset, + }, + ) + + +@configclass +class RewardsCfg: + """Reward terms for the MDP.""" + + goal_dist_exp1 = RewTerm( + func=distance_to_goal_exp_curriculum, + weight=2.0, + params={ + "asset_cfg": SceneEntityCfg("robot"), + "std": 7.0, + "command_name": "target_pose", + }, + ) + goal_dist_exp2 = RewTerm( + func=distance_to_goal_exp_curriculum, + weight=4.0, + params={ + "asset_cfg": SceneEntityCfg("robot"), + "std": 0.5, + "command_name": "target_pose", + }, + ) + velocity_reward = RewTerm( + func=velocity_to_goal_reward_curriculum, + weight=0.5, + params={ + "asset_cfg": SceneEntityCfg("robot"), + "command_name": "target_pose", + }, + ) + action_rate_l2 = RewTerm(func=mdp.action_rate_l2, weight=-0.05) + action_magnitude_l2 = RewTerm(func=mdp.action_l2, weight=-0.05) + + termination_penalty = RewTerm( + func=mdp.is_terminated, + weight=-100.0, + ) + + +@configclass +class TerminationsCfg: + """Termination terms for the MDP.""" + + time_out = DoneTerm(func=mdp.time_out, time_out=True) + collision = DoneTerm( + func=mdp.illegal_contact, + params={"sensor_cfg": SceneEntityCfg("contact_forces", body_names=".*"), "threshold": 1.0}, + time_out=False, + ) + + +@configclass +class CurriculumCfg: + """Curriculum terms for the MDP.""" + + obstacle_levels = CurrTerm( + func=ObstacleDensityCurriculum, + params={ + "asset_cfg": SceneEntityCfg("robot"), + "max_difficulty": 10, + "min_difficulty": 0, + }, + ) + + +## +# Environment configuration +## + + +@configclass +class NavigationVelocityFloatingObstacleEnvCfg(ManagerBasedRLEnvCfg): + """Configuration for the locomotion velocity-tracking environment.""" + + # Scene settings + scene: ArlNavigationSceneCfg = ArlNavigationSceneCfg(num_envs=4096, env_spacing=20.5) + # Basic settings + observations: ObservationsCfg = ObservationsCfg() + actions: ActionsCfg = ActionsCfg() + commands: CommandsCfg = CommandsCfg() + # MDP settings + rewards: RewardsCfg = RewardsCfg() + terminations: TerminationsCfg = TerminationsCfg() + events: EventCfg = EventCfg() + curriculum: CurriculumCfg = CurriculumCfg() + + def __post_init__(self): + """Post initialization.""" + # general settings + self.decimation = 10 + self.episode_length_s = 10.0 + # simulation settings + self.sim.dt = 0.01 + self.sim.render_interval = self.decimation + self.sim.physics_material = sim_utils.RigidBodyMaterialCfg( + friction_combine_mode="multiply", + restitution_combine_mode="multiply", + static_friction=1.0, + dynamic_friction=1.0, + ) + self.sim.physics = PhysxCfg(gpu_max_rigid_patch_count=2**21) + # update sensor update periods + # we tick all the sensors based on the smallest update period (physics update period) + if self.scene.contact_forces is not None: + self.scene.contact_forces.update_period = self.sim.dt diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/navigation/config/arl_robot_1/scenes/obstacle_scenes/obstacle_scene.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/navigation/config/arl_robot_1/scenes/obstacle_scenes/obstacle_scene.py new file mode 100644 index 000000000000..be3fe2e1fc53 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/navigation/config/arl_robot_1/scenes/obstacle_scenes/obstacle_scene.py @@ -0,0 +1,114 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import numpy as np + +import isaaclab.sim as sim_utils +from isaaclab.assets import RigidObjectCfg, RigidObjectCollectionCfg + +from .obstacle_scene_cfg import ObstaclesSceneCfg + +"""Obstacle scene generation and reset functionality for drone navigation environments. + +This module provides utilities for generating dynamic 3D obstacle courses with walls and +floating obstacles. The obstacle configurations support curriculum learning where difficulty +can be progressively increased by adjusting the number of active obstacles. +""" + +OBSTACLE_SCENE_CFG = ObstaclesSceneCfg( + env_size=(12.0, 8.0, 6.0), + min_num_obstacles=20, + max_num_obstacles=40, + ground_offset=3.0, +) + + +def generate_obstacle_collection(cfg: ObstaclesSceneCfg) -> RigidObjectCollectionCfg: + """Generate a rigid object collection configuration for walls and obstacles. + + Creates a complete scene with boundary walls and a variety of floating obstacles + (panels, cubes, rods, etc.) based on the provided configuration. Each obstacle is + assigned random colors and configured with appropriate physics properties. + + Wall objects are configured with very high mass (10^7 kg) and high damping to remain + stationary during collisions. Obstacle objects have moderate mass (100 kg) to move in the right position if reset + in collision. + + Args: + cfg: Configuration object specifying obstacle types, sizes, quantities, and + positioning constraints. + + Returns: + A RigidObjectCollectionCfg containing all wall and obstacle configurations, + ready to be added to a scene. + + Note: + All obstacles are initially placed at origin [0, 0, 0]. Actual positions are + set during environment reset via :func:`reset_obstacles_with_individual_ranges`. + """ + max_num_obstacles = cfg.max_num_obstacles + + rigid_objects = {} + + for wall_name, wall_cfg in cfg.wall_cfgs.items(): + # Walls get their specific size and default center + default_center = [0.0, 0.0, 0.0] # Will be set properly at reset + color = float(np.random.randint(0, 256, dtype=np.uint8)) / 255.0 + + rigid_objects[wall_name] = RigidObjectCfg( + prim_path=f"{{ENV_REGEX_NS}}/obstacle_{wall_name}", + spawn=sim_utils.CuboidCfg( + size=wall_cfg.size, + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.5, 0.5, color), metallic=0.2), + rigid_props=sim_utils.RigidBodyPropertiesCfg( + solver_position_iteration_count=4, + solver_velocity_iteration_count=0, + disable_gravity=True, + kinematic_enabled=False, + linear_damping=9999.0, + angular_damping=9999.0, + max_linear_velocity=0.0, + max_angular_velocity=0.0, + ), + # mass of walls needs to be way larger than weight of obstacles to make them not move during reset + mass_props=sim_utils.MassPropertiesCfg(mass=10000000.0), + collision_props=sim_utils.CollisionPropertiesCfg(), + ), + init_state=RigidObjectCfg.InitialStateCfg(pos=tuple(default_center)), + collision_group=0, + ) + + obstacle_types = list(cfg.obstacle_cfgs.values()) + for i in range(max_num_obstacles): + obj_name = f"obstacle_{i}" + obs_cfg = obstacle_types[i % len(obstacle_types)] + + default_center = [0.0, 0.0, 0.0] + color = np.random.randint(0, 256, size=3, dtype=np.uint8) + color_normalized = tuple(float(c) / 255.0 for c in color) + + rigid_objects[obj_name] = RigidObjectCfg( + prim_path=f"{{ENV_REGEX_NS}}/{obj_name}", + spawn=sim_utils.CuboidCfg( + size=obs_cfg.size, + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=color_normalized, metallic=0.2), + rigid_props=sim_utils.RigidBodyPropertiesCfg( + solver_position_iteration_count=4, + solver_velocity_iteration_count=0, + disable_gravity=True, + kinematic_enabled=False, + linear_damping=1.0, + angular_damping=1.0, + max_linear_velocity=0.0, + max_angular_velocity=0.0, + ), + mass_props=sim_utils.MassPropertiesCfg(mass=100.0), + collision_props=sim_utils.CollisionPropertiesCfg(), + ), + init_state=RigidObjectCfg.InitialStateCfg(pos=tuple(default_center)), + collision_group=0, + ) + + return RigidObjectCollectionCfg(rigid_objects=rigid_objects) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/navigation/config/arl_robot_1/scenes/obstacle_scenes/obstacle_scene_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/navigation/config/arl_robot_1/scenes/obstacle_scenes/obstacle_scene_cfg.py new file mode 100644 index 000000000000..c42610dc10b7 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/navigation/config/arl_robot_1/scenes/obstacle_scenes/obstacle_scene_cfg.py @@ -0,0 +1,88 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from dataclasses import MISSING + +from isaaclab.utils import configclass + + +@configclass +class ObstaclesSceneCfg: + """Configuration for a terrain with floating obstacles.""" + + min_num_obstacles: int = 1 + max_num_obstacles: int = 40 + ground_offset: float = 3.0 + + env_size: tuple[float, float, float] = MISSING + + @configclass + class BoxCfg: + """Configuration for a box-shaped obstacle or wall. + + Defines the size and placement constraints for rectangular obstacles within + the environment. The center position is specified as ratios of the environment + size, allowing for flexible scaling. + + Attributes: + size: Tuple of (length, width, height) in meters. + center_ratio_min: Minimum position as ratio of env_size (0.0 to 1.0) for + each axis. Used for random placement bounds. + center_ratio_max: Maximum position as ratio of env_size (0.0 to 1.0) for + each axis. For fixed positions, set equal to center_ratio_min. + """ + + size: tuple[float, float, float] = MISSING + center_ratio_min: tuple[float, float, float] = MISSING + center_ratio_max: tuple[float, float, float] = MISSING + + # Obstacle configurations + panel_obs_cfg = BoxCfg( + size=(0.1, 1.2, 3.0), center_ratio_min=(0.3, 0.05, 0.05), center_ratio_max=(0.85, 0.95, 0.95) + ) + + small_wall_obs_cfg = BoxCfg( + size=(0.1, 0.5, 0.5), center_ratio_min=(0.3, 0.05, 0.05), center_ratio_max=(0.85, 0.9, 0.9) + ) + + big_wall_obs_cfg = BoxCfg( + size=(0.1, 1.0, 1.0), center_ratio_min=(0.3, 0.05, 0.05), center_ratio_max=(0.85, 0.9, 0.9) + ) + + small_cube_obs_cfg = BoxCfg( + size=(0.4, 0.4, 0.4), center_ratio_min=(0.3, 0.05, 0.05), center_ratio_max=(0.85, 0.9, 0.9) + ) + + rod_obs_cfg = BoxCfg(size=(0.1, 0.1, 2.0), center_ratio_min=(0.3, 0.05, 0.05), center_ratio_max=(0.85, 0.9, 0.9)) + + # Wall configurations + left_wall_cfg = BoxCfg(size=(12.0, 0.2, 6.0), center_ratio_min=(0.5, 1.0, 0.5), center_ratio_max=(0.5, 1.0, 0.5)) + + right_wall_cfg = BoxCfg(size=(12.0, 0.2, 6.0), center_ratio_min=(0.5, 0.0, 0.5), center_ratio_max=(0.5, 0.0, 0.5)) + + back_wall_cfg = BoxCfg(size=(0.2, 8.0, 6.0), center_ratio_min=(0.0, 0.5, 0.5), center_ratio_max=(0.0, 0.5, 0.5)) + + front_wall_cfg = BoxCfg(size=(0.2, 8.0, 6.0), center_ratio_min=(1.0, 0.5, 0.5), center_ratio_max=(1.0, 0.5, 0.5)) + + top_wall_cfg = BoxCfg(size=(12.0, 8.0, 0.2), center_ratio_min=(0.5, 0.5, 1.0), center_ratio_max=(0.5, 0.5, 1.0)) + + bottom_wall_cfg = BoxCfg(size=(12.0, 8.0, 0.2), center_ratio_min=(0.5, 0.5, 0.0), center_ratio_max=(0.5, 0.5, 0.0)) + + wall_cfgs = { + "left_wall": left_wall_cfg, + "right_wall": right_wall_cfg, + "back_wall": back_wall_cfg, + "front_wall": front_wall_cfg, + "bottom_wall": bottom_wall_cfg, + "top_wall": top_wall_cfg, + } + + obstacle_cfgs = { + "panel": panel_obs_cfg, + "small_wall": small_wall_obs_cfg, + "big_wall": big_wall_obs_cfg, + "small_cube": small_cube_obs_cfg, + "rod": rod_obs_cfg, + } diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/track_position_state_based/config/arl_robot_1/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/track_position_state_based/config/arl_robot_1/agents/rsl_rl_ppo_cfg.py index b53c53dbdd0c..9a9b0de5bb38 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/track_position_state_based/config/arl_robot_1/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/track_position_state_based/config/arl_robot_1/agents/rsl_rl_ppo_cfg.py @@ -5,7 +5,7 @@ from isaaclab.utils import configclass -from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg +from isaaclab_rl.rsl_rl import RslRlMLPModelCfg, RslRlOnPolicyRunnerCfg, RslRlPpoAlgorithmCfg @configclass @@ -14,12 +14,16 @@ class TrackPositionNoObstaclesEnvPPORunnerCfg(RslRlOnPolicyRunnerCfg): max_iterations = 1500 save_interval = 50 experiment_name = "arl_robot_1_track_position_state_based" - empirical_normalization = False - policy = RslRlPpoActorCriticCfg( - init_noise_std=0.5, - actor_hidden_dims=[256, 128, 64], - critic_hidden_dims=[256, 128, 64], + actor = RslRlMLPModelCfg( + hidden_dims=[256, 128, 64], activation="elu", + obs_normalization=False, + distribution_cfg=RslRlMLPModelCfg.GaussianDistributionCfg(init_std=0.5), + ) + critic = RslRlMLPModelCfg( + hidden_dims=[256, 128, 64], + activation="elu", + obs_normalization=False, ) algorithm = RslRlPpoAlgorithmCfg( value_loss_coef=1.0, diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/track_position_state_based/config/arl_robot_1/track_position_state_based_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/track_position_state_based/config/arl_robot_1/track_position_state_based_env_cfg.py index a78e4a151162..de61f13b8a9a 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/track_position_state_based/config/arl_robot_1/track_position_state_based_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/track_position_state_based/config/arl_robot_1/track_position_state_based_env_cfg.py @@ -25,13 +25,20 @@ from isaaclab_contrib.assets import MultirotorCfg import isaaclab_tasks.manager_based.drone_arl.mdp as mdp +from isaaclab_tasks.manager_based.drone_arl.mdp.commands import DroneUniformPoseCommandCfg +from isaaclab_tasks.manager_based.drone_arl.mdp.rewards import ( + ang_vel_xyz_exp, + distance_to_goal_exp, + lin_vel_xyz_exp, + yaw_aligned, +) ## # Scene definition ## @configclass -class MySceneCfg(InteractiveSceneCfg): +class ArlTrackPositionStateBasedSceneCfg(InteractiveSceneCfg): """Configuration for the terrain scene with a flying robot.""" # robots @@ -56,12 +63,12 @@ class MySceneCfg(InteractiveSceneCfg): class CommandsCfg: """Command specifications for the MDP.""" - target_pose = mdp.DroneUniformPoseCommandCfg( + target_pose = DroneUniformPoseCommandCfg( asset_name="robot", body_name="base_link", resampling_time_range=(10.0, 10.0), debug_vis=True, - ranges=mdp.DroneUniformPoseCommandCfg.Ranges( + ranges=DroneUniformPoseCommandCfg.Ranges( pos_x=(-0.0, 0.0), pos_y=(-0.0, 0.0), pos_z=(-0.0, 0.0), @@ -149,7 +156,7 @@ class RewardsCfg: """Reward terms for the MDP.""" distance_to_goal_exp = RewTerm( - func=mdp.distance_to_goal_exp, + func=distance_to_goal_exp, weight=25.0, params={ "asset_cfg": SceneEntityCfg("robot"), @@ -163,17 +170,17 @@ class RewardsCfg: params={"asset_cfg": SceneEntityCfg("robot")}, ) yaw_aligned = RewTerm( - func=mdp.yaw_aligned, + func=yaw_aligned, weight=2.0, params={"asset_cfg": SceneEntityCfg("robot"), "std": 1.0}, ) lin_vel_xyz_exp = RewTerm( - func=mdp.lin_vel_xyz_exp, + func=lin_vel_xyz_exp, weight=2.5, params={"asset_cfg": SceneEntityCfg("robot"), "std": 2.0}, ) ang_vel_xyz_exp = RewTerm( - func=mdp.ang_vel_xyz_exp, + func=ang_vel_xyz_exp, weight=10.0, params={"asset_cfg": SceneEntityCfg("robot"), "std": 10.0}, ) @@ -204,7 +211,7 @@ class TrackPositionNoObstaclesEnvCfg(ManagerBasedRLEnvCfg): """Configuration for the state-based drone pose-control environment.""" # Scene settings - scene: MySceneCfg = MySceneCfg(num_envs=4096, env_spacing=2.5) + scene: ArlTrackPositionStateBasedSceneCfg = ArlTrackPositionStateBasedSceneCfg(num_envs=4096, env_spacing=2.5) # Basic settings observations: ObservationsCfg = ObservationsCfg() actions: ActionsCfg = ActionsCfg() diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/tracking/config/digit/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/tracking/config/digit/agents/rsl_rl_ppo_cfg.py index c98c2030a2ca..2118f550cc12 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/tracking/config/digit/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/tracking/config/digit/agents/rsl_rl_ppo_cfg.py @@ -5,7 +5,7 @@ from isaaclab.utils import configclass -from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg +from isaaclab_rl.rsl_rl import RslRlMLPModelCfg, RslRlOnPolicyRunnerCfg, RslRlPpoAlgorithmCfg @configclass @@ -14,13 +14,16 @@ class DigitLocoManipPPORunnerCfg(RslRlOnPolicyRunnerCfg): max_iterations = 2000 save_interval = 50 experiment_name = "digit_loco_manip" - policy = RslRlPpoActorCriticCfg( - init_noise_std=1.0, - actor_obs_normalization=False, - critic_obs_normalization=False, - actor_hidden_dims=[256, 128, 128], - critic_hidden_dims=[256, 128, 128], + actor = RslRlMLPModelCfg( + hidden_dims=[256, 128, 128], activation="elu", + obs_normalization=False, + distribution_cfg=RslRlMLPModelCfg.GaussianDistributionCfg(init_std=1.0), + ) + critic = RslRlMLPModelCfg( + hidden_dims=[256, 128, 128], + activation="elu", + obs_normalization=False, ) algorithm = RslRlPpoAlgorithmCfg( value_loss_coef=1.0, diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/a1/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/a1/agents/rsl_rl_ppo_cfg.py index 972ebf937367..334aa1768ed6 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/a1/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/a1/agents/rsl_rl_ppo_cfg.py @@ -5,7 +5,7 @@ from isaaclab.utils import configclass -from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg +from isaaclab_rl.rsl_rl import RslRlMLPModelCfg, RslRlOnPolicyRunnerCfg, RslRlPpoAlgorithmCfg @configclass @@ -14,13 +14,16 @@ class UnitreeA1RoughPPORunnerCfg(RslRlOnPolicyRunnerCfg): max_iterations = 1500 save_interval = 50 experiment_name = "unitree_a1_rough" - policy = RslRlPpoActorCriticCfg( - init_noise_std=1.0, - actor_obs_normalization=False, - critic_obs_normalization=False, - actor_hidden_dims=[512, 256, 128], - critic_hidden_dims=[512, 256, 128], + actor = RslRlMLPModelCfg( + hidden_dims=[512, 256, 128], activation="elu", + obs_normalization=False, + distribution_cfg=RslRlMLPModelCfg.GaussianDistributionCfg(init_std=1.0), + ) + critic = RslRlMLPModelCfg( + hidden_dims=[512, 256, 128], + activation="elu", + obs_normalization=False, ) algorithm = RslRlPpoAlgorithmCfg( value_loss_coef=1.0, @@ -45,5 +48,5 @@ def __post_init__(self): self.max_iterations = 300 self.experiment_name = "unitree_a1_flat" - self.policy.actor_hidden_dims = [128, 128, 128] - self.policy.critic_hidden_dims = [128, 128, 128] + self.actor.hidden_dims = [128, 128, 128] + self.critic.hidden_dims = [128, 128, 128] diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_b/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_b/agents/rsl_rl_ppo_cfg.py index f6d0c585dd15..49c227aecdde 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_b/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_b/agents/rsl_rl_ppo_cfg.py @@ -5,7 +5,7 @@ from isaaclab.utils import configclass -from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg, RslRlSymmetryCfg +from isaaclab_rl.rsl_rl import RslRlMLPModelCfg, RslRlOnPolicyRunnerCfg, RslRlPpoAlgorithmCfg, RslRlSymmetryCfg from isaaclab_tasks.manager_based.locomotion.velocity.mdp.symmetry import anymal @@ -16,13 +16,16 @@ class AnymalBRoughPPORunnerCfg(RslRlOnPolicyRunnerCfg): max_iterations = 1500 save_interval = 50 experiment_name = "anymal_b_rough" - policy = RslRlPpoActorCriticCfg( - init_noise_std=1.0, - actor_obs_normalization=False, - critic_obs_normalization=False, - actor_hidden_dims=[512, 256, 128], - critic_hidden_dims=[512, 256, 128], + actor = RslRlMLPModelCfg( + hidden_dims=[512, 256, 128], activation="elu", + obs_normalization=False, + distribution_cfg=RslRlMLPModelCfg.GaussianDistributionCfg(init_std=1.0), + ) + critic = RslRlMLPModelCfg( + hidden_dims=[512, 256, 128], + activation="elu", + obs_normalization=False, ) algorithm = RslRlPpoAlgorithmCfg( value_loss_coef=1.0, @@ -47,8 +50,8 @@ def __post_init__(self): self.max_iterations = 300 self.experiment_name = "anymal_b_flat" - self.policy.actor_hidden_dims = [128, 128, 128] - self.policy.critic_hidden_dims = [128, 128, 128] + self.actor.hidden_dims = [128, 128, 128] + self.critic.hidden_dims = [128, 128, 128] @configclass diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_c/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_c/agents/rsl_rl_ppo_cfg.py index 45f434fe7f0d..06072c7297fe 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_c/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/anymal_c/agents/rsl_rl_ppo_cfg.py @@ -5,7 +5,7 @@ from isaaclab.utils import configclass -from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg, RslRlSymmetryCfg +from isaaclab_rl.rsl_rl import RslRlMLPModelCfg, RslRlOnPolicyRunnerCfg, RslRlPpoAlgorithmCfg, RslRlSymmetryCfg from isaaclab_tasks.manager_based.locomotion.velocity.mdp.symmetry import anymal @@ -16,13 +16,16 @@ class AnymalCRoughPPORunnerCfg(RslRlOnPolicyRunnerCfg): max_iterations = 1500 save_interval = 50 experiment_name = "anymal_c_rough" - policy = RslRlPpoActorCriticCfg( - init_noise_std=1.0, - actor_obs_normalization=False, - critic_obs_normalization=False, - actor_hidden_dims=[512, 256, 128], - critic_hidden_dims=[512, 256, 128], + actor = RslRlMLPModelCfg( + hidden_dims=[512, 256, 128], activation="elu", + obs_normalization=False, + distribution_cfg=RslRlMLPModelCfg.GaussianDistributionCfg(init_std=1.0), + ) + critic = RslRlMLPModelCfg( + hidden_dims=[512, 256, 128], + activation="elu", + obs_normalization=False, ) algorithm = RslRlPpoAlgorithmCfg( value_loss_coef=1.0, @@ -47,8 +50,8 @@ def __post_init__(self): self.max_iterations = 300 self.experiment_name = "anymal_c_flat" - self.policy.actor_hidden_dims = [128, 128, 128] - self.policy.critic_hidden_dims = [128, 128, 128] + self.actor.hidden_dims = [128, 128, 128] + self.critic.hidden_dims = [128, 128, 128] @configclass diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/cassie/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/cassie/agents/rsl_rl_ppo_cfg.py index 93cce1bb9294..f7cabdc37474 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/cassie/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/cassie/agents/rsl_rl_ppo_cfg.py @@ -5,7 +5,7 @@ from isaaclab.utils import configclass -from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg +from isaaclab_rl.rsl_rl import RslRlMLPModelCfg, RslRlOnPolicyRunnerCfg, RslRlPpoAlgorithmCfg @configclass @@ -14,13 +14,16 @@ class CassieRoughPPORunnerCfg(RslRlOnPolicyRunnerCfg): max_iterations = 1500 save_interval = 50 experiment_name = "cassie_rough" - policy = RslRlPpoActorCriticCfg( - init_noise_std=1.0, - actor_obs_normalization=False, - critic_obs_normalization=False, - actor_hidden_dims=[512, 256, 128], - critic_hidden_dims=[512, 256, 128], + actor = RslRlMLPModelCfg( + hidden_dims=[512, 256, 128], activation="elu", + obs_normalization=False, + distribution_cfg=RslRlMLPModelCfg.GaussianDistributionCfg(init_std=1.0), + ) + critic = RslRlMLPModelCfg( + hidden_dims=[512, 256, 128], + activation="elu", + obs_normalization=False, ) algorithm = RslRlPpoAlgorithmCfg( value_loss_coef=1.0, @@ -45,5 +48,5 @@ def __post_init__(self): self.max_iterations = 1000 self.experiment_name = "cassie_flat" - self.policy.actor_hidden_dims = [128, 128, 128] - self.policy.critic_hidden_dims = [128, 128, 128] + self.actor.hidden_dims = [128, 128, 128] + self.critic.hidden_dims = [128, 128, 128] diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/digit/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/digit/agents/rsl_rl_ppo_cfg.py index 72eb4a2aa3ff..217c7482f19a 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/digit/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/digit/agents/rsl_rl_ppo_cfg.py @@ -5,7 +5,7 @@ from isaaclab.utils import configclass -from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg +from isaaclab_rl.rsl_rl import RslRlMLPModelCfg, RslRlOnPolicyRunnerCfg, RslRlPpoAlgorithmCfg @configclass @@ -14,13 +14,16 @@ class DigitRoughPPORunnerCfg(RslRlOnPolicyRunnerCfg): max_iterations = 3000 save_interval = 50 experiment_name = "digit_rough" - policy = RslRlPpoActorCriticCfg( - init_noise_std=1.0, - actor_obs_normalization=False, - critic_obs_normalization=False, - actor_hidden_dims=[512, 256, 128], - critic_hidden_dims=[512, 256, 128], + actor = RslRlMLPModelCfg( + hidden_dims=[512, 256, 128], activation="elu", + obs_normalization=False, + distribution_cfg=RslRlMLPModelCfg.GaussianDistributionCfg(init_std=1.0), + ) + critic = RslRlMLPModelCfg( + hidden_dims=[512, 256, 128], + activation="elu", + obs_normalization=False, ) algorithm = RslRlPpoAlgorithmCfg( value_loss_coef=1.0, @@ -46,5 +49,5 @@ def __post_init__(self): self.max_iterations = 2000 self.experiment_name = "digit_flat" - self.policy.actor_hidden_dims = [128, 128, 128] - self.policy.critic_hidden_dims = [128, 128, 128] + self.actor.hidden_dims = [128, 128, 128] + self.critic.hidden_dims = [128, 128, 128] diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/g1/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/g1/agents/rsl_rl_ppo_cfg.py index 7b61c184d353..d4b55dadc60f 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/g1/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/g1/agents/rsl_rl_ppo_cfg.py @@ -5,7 +5,7 @@ from isaaclab.utils import configclass -from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg +from isaaclab_rl.rsl_rl import RslRlMLPModelCfg, RslRlOnPolicyRunnerCfg, RslRlPpoAlgorithmCfg from isaaclab_tasks.utils import preset @@ -22,13 +22,16 @@ class G1RoughPPORunnerCfg(RslRlOnPolicyRunnerCfg): max_iterations = preset(default=3000, newton=5000) save_interval = 50 experiment_name = "g1_rough" - policy = RslRlPpoActorCriticCfg( - init_noise_std=1.0, - actor_obs_normalization=False, - critic_obs_normalization=False, - actor_hidden_dims=[512, 256, 128], - critic_hidden_dims=[512, 256, 128], + actor = RslRlMLPModelCfg( + hidden_dims=[512, 256, 128], activation="elu", + obs_normalization=False, + distribution_cfg=RslRlMLPModelCfg.GaussianDistributionCfg(init_std=1.0), + ) + critic = RslRlMLPModelCfg( + hidden_dims=[512, 256, 128], + activation="elu", + obs_normalization=False, ) algorithm = RslRlPpoAlgorithmCfg( value_loss_coef=1.0, @@ -53,5 +56,5 @@ def __post_init__(self): self.max_iterations = 1500 self.experiment_name = "g1_flat" - self.policy.actor_hidden_dims = [256, 128, 128] - self.policy.critic_hidden_dims = [256, 128, 128] + self.actor.hidden_dims = [256, 128, 128] + self.critic.hidden_dims = [256, 128, 128] diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go1/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go1/agents/rsl_rl_ppo_cfg.py index 9baa2b371ea3..de8ca9189ebe 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go1/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go1/agents/rsl_rl_ppo_cfg.py @@ -5,7 +5,7 @@ from isaaclab.utils import configclass -from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg +from isaaclab_rl.rsl_rl import RslRlMLPModelCfg, RslRlOnPolicyRunnerCfg, RslRlPpoAlgorithmCfg @configclass @@ -14,13 +14,16 @@ class UnitreeGo1RoughPPORunnerCfg(RslRlOnPolicyRunnerCfg): max_iterations = 1500 save_interval = 50 experiment_name = "unitree_go1_rough" - policy = RslRlPpoActorCriticCfg( - init_noise_std=1.0, - actor_obs_normalization=False, - critic_obs_normalization=False, - actor_hidden_dims=[512, 256, 128], - critic_hidden_dims=[512, 256, 128], + actor = RslRlMLPModelCfg( + hidden_dims=[512, 256, 128], activation="elu", + obs_normalization=False, + distribution_cfg=RslRlMLPModelCfg.GaussianDistributionCfg(init_std=1.0), + ) + critic = RslRlMLPModelCfg( + hidden_dims=[512, 256, 128], + activation="elu", + obs_normalization=False, ) algorithm = RslRlPpoAlgorithmCfg( value_loss_coef=1.0, @@ -45,5 +48,5 @@ def __post_init__(self): self.max_iterations = 300 self.experiment_name = "unitree_go1_flat" - self.policy.actor_hidden_dims = [128, 128, 128] - self.policy.critic_hidden_dims = [128, 128, 128] + self.actor.hidden_dims = [128, 128, 128] + self.critic.hidden_dims = [128, 128, 128] diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go2/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go2/agents/rsl_rl_ppo_cfg.py index 9777785f7e30..e7b0e67b5c2b 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go2/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go2/agents/rsl_rl_ppo_cfg.py @@ -5,7 +5,7 @@ from isaaclab.utils import configclass -from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg +from isaaclab_rl.rsl_rl import RslRlMLPModelCfg, RslRlOnPolicyRunnerCfg, RslRlPpoAlgorithmCfg @configclass @@ -14,13 +14,16 @@ class UnitreeGo2RoughPPORunnerCfg(RslRlOnPolicyRunnerCfg): max_iterations = 1500 save_interval = 50 experiment_name = "unitree_go2_rough" - policy = RslRlPpoActorCriticCfg( - init_noise_std=1.0, - actor_obs_normalization=False, - critic_obs_normalization=False, - actor_hidden_dims=[512, 256, 128], - critic_hidden_dims=[512, 256, 128], + actor = RslRlMLPModelCfg( + hidden_dims=[512, 256, 128], activation="elu", + obs_normalization=False, + distribution_cfg=RslRlMLPModelCfg.GaussianDistributionCfg(init_std=1.0), + ) + critic = RslRlMLPModelCfg( + hidden_dims=[512, 256, 128], + activation="elu", + obs_normalization=False, ) algorithm = RslRlPpoAlgorithmCfg( value_loss_coef=1.0, @@ -45,5 +48,5 @@ def __post_init__(self): self.max_iterations = 300 self.experiment_name = "unitree_go2_flat" - self.policy.actor_hidden_dims = [128, 128, 128] - self.policy.critic_hidden_dims = [128, 128, 128] + self.actor.hidden_dims = [128, 128, 128] + self.critic.hidden_dims = [128, 128, 128] diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/h1/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/h1/agents/rsl_rl_ppo_cfg.py index 102359770864..fe21c8d1da17 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/h1/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/h1/agents/rsl_rl_ppo_cfg.py @@ -5,7 +5,7 @@ from isaaclab.utils import configclass -from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg +from isaaclab_rl.rsl_rl import RslRlMLPModelCfg, RslRlOnPolicyRunnerCfg, RslRlPpoAlgorithmCfg @configclass @@ -14,13 +14,16 @@ class H1RoughPPORunnerCfg(RslRlOnPolicyRunnerCfg): max_iterations = 3000 save_interval = 50 experiment_name = "h1_rough" - policy = RslRlPpoActorCriticCfg( - init_noise_std=1.0, - actor_obs_normalization=False, - critic_obs_normalization=False, - actor_hidden_dims=[512, 256, 128], - critic_hidden_dims=[512, 256, 128], + actor = RslRlMLPModelCfg( + hidden_dims=[512, 256, 128], activation="elu", + obs_normalization=False, + distribution_cfg=RslRlMLPModelCfg.GaussianDistributionCfg(init_std=1.0), + ) + critic = RslRlMLPModelCfg( + hidden_dims=[512, 256, 128], + activation="elu", + obs_normalization=False, ) algorithm = RslRlPpoAlgorithmCfg( value_loss_coef=1.0, @@ -45,5 +48,5 @@ def __post_init__(self): self.max_iterations = 1000 self.experiment_name = "h1_flat" - self.policy.actor_hidden_dims = [128, 128, 128] - self.policy.critic_hidden_dims = [128, 128, 128] + self.actor.hidden_dims = [128, 128, 128] + self.critic.hidden_dims = [128, 128, 128] diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/spot/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/spot/agents/rsl_rl_ppo_cfg.py index 3985f6b3b491..0c7e6f30dac6 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/spot/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/spot/agents/rsl_rl_ppo_cfg.py @@ -5,7 +5,7 @@ from isaaclab.utils import configclass -from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg +from isaaclab_rl.rsl_rl import RslRlMLPModelCfg, RslRlOnPolicyRunnerCfg, RslRlPpoAlgorithmCfg @configclass @@ -15,13 +15,16 @@ class SpotFlatPPORunnerCfg(RslRlOnPolicyRunnerCfg): save_interval = 50 experiment_name = "spot_flat" store_code_state = False - policy = RslRlPpoActorCriticCfg( - init_noise_std=1.0, - actor_obs_normalization=False, - critic_obs_normalization=False, - actor_hidden_dims=[512, 256, 128], - critic_hidden_dims=[512, 256, 128], + actor = RslRlMLPModelCfg( + hidden_dims=[512, 256, 128], activation="elu", + obs_normalization=False, + distribution_cfg=RslRlMLPModelCfg.GaussianDistributionCfg(init_std=1.0), + ) + critic = RslRlMLPModelCfg( + hidden_dims=[512, 256, 128], + activation="elu", + obs_normalization=False, ) algorithm = RslRlPpoAlgorithmCfg( value_loss_coef=0.5, diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/assemble_trocar/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/assemble_trocar/__init__.py new file mode 100644 index 000000000000..624d02269813 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/assemble_trocar/__init__.py @@ -0,0 +1,6 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configurations for the assemble trocar environments.""" diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/assemble_trocar/config/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/assemble_trocar/config/__init__.py new file mode 100644 index 000000000000..cd8c26c840a5 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/assemble_trocar/config/__init__.py @@ -0,0 +1,9 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from .camera_config import CameraBaseCfg, CameraPresets +from .robot_config import G1_29DOF_BODY_JOINT_INDICES, G1_DEX3_JOINT_INDICES, G1RobotPresets + +__all__ = ["G1_29DOF_BODY_JOINT_INDICES", "G1_DEX3_JOINT_INDICES", "G1RobotPresets", "CameraBaseCfg", "CameraPresets"] diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/assemble_trocar/config/camera_config.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/assemble_trocar/config/camera_config.py new file mode 100644 index 000000000000..405948726034 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/assemble_trocar/config/camera_config.py @@ -0,0 +1,131 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +public camera configuration +include the basic configuration for different types of cameras, support scene-specific parameter customization +""" + +from collections.abc import Sequence + +import isaaclab.sim as sim_utils +from isaaclab.sensors import CameraCfg +from isaaclab.utils import configclass + + +@configclass +class CameraBaseCfg: + """camera base configuration class + + provide the default configuration for different types of cameras, support scene-specific parameter customization + """ + + @classmethod + def get_camera_config( + cls, + prim_path: str = "/World/envs/env_.*/Robot/d435_link/front_cam", + update_period: float = 0.02, + height: int = 480, + width: int = 640, + focal_length: float = 7.6, + focus_distance: float = 400.0, + horizontal_aperture: float = 20.0, + clipping_range: tuple[float, float] = (0.1, 1.0e5), + pos_offset: tuple[float, float, float] = (0.0, 0.0, 0.0), + rot_offset: tuple[float, float, float, float] = (0.5, -0.5, 0.5, -0.5), + data_types: Sequence[str] | None = None, + ) -> CameraCfg: + """Get a pinhole camera configuration. + + Args: + prim_path: the path of the camera in the scene + update_period: update period (seconds) + height: image height (pixels) + width: image width (pixels) + focal_length: focal length + focus_distance: focus distance + horizontal_aperture: horizontal aperture + clipping_range: clipping range (near clipping plane, far clipping plane) + pos_offset: position offset (x, y, z) + rot_offset: rotation offset quaternion + data_types: data type list + + Returns: + CameraCfg: camera configuration + """ + if data_types is None: + data_types = ("rgb",) + + return CameraCfg( + prim_path=prim_path, + update_period=update_period, + height=height, + width=width, + data_types=list(data_types), + spawn=sim_utils.PinholeCameraCfg( + focal_length=focal_length, + focus_distance=focus_distance, + horizontal_aperture=horizontal_aperture, + clipping_range=clipping_range, + ), + offset=CameraCfg.OffsetCfg(pos=pos_offset, rot=rot_offset, convention="ros"), + ) + + +@configclass +class CameraPresets: + """camera preset configuration collection + + include the common camera configuration preset for different scenes + """ + + @classmethod + def g1_front_camera(cls, **overrides) -> CameraCfg: + params = { + "height": 224, + "width": 224, + "focal_length": 10.5, + "horizontal_aperture": 14.25, # Match original vertical FOV after crop + } + params.update(overrides) + return CameraBaseCfg.get_camera_config(**params) + + @classmethod + def left_dex3_wrist_camera(cls, **overrides) -> CameraCfg: + """left wrist camera configuration""" + params = { + "prim_path": "/World/envs/env_.*/Robot/left_hand_camera_base_link/left_wrist_camera", + "height": 224, + "width": 224, + "update_period": 0.02, + "data_types": ["rgb"], + "focal_length": 12.0, + "focus_distance": 400.0, + "horizontal_aperture": 14.25, # Match original vertical FOV after crop + "clipping_range": (0.1, 1.0e5), + "pos_offset": (-0.04012, -0.07441, 0.15711), + "rot_offset": (0.00539, 0.86024, 0.0424, 0.50809), + } + params.update(overrides) + return CameraBaseCfg.get_camera_config(**params) + + @classmethod + def right_dex3_wrist_camera(cls, **overrides) -> CameraCfg: + """right wrist camera configuration""" + params = { + "prim_path": "/World/envs/env_.*/Robot/right_hand_camera_base_link/right_wrist_camera", + "height": 224, + "width": 224, + "update_period": 0.02, + "data_types": ["rgb"], + "focal_length": 12.0, + "focus_distance": 400.0, + "horizontal_aperture": 14.25, # Match original vertical FOV after crop + "clipping_range": (0.1, 1.0e5), + "pos_offset": (-0.04012, 0.07441, 0.15711), + "rot_offset": (0.00539, 0.86024, 0.0424, 0.50809), + } + params.update(overrides) + return CameraBaseCfg.get_camera_config(**params) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/assemble_trocar/config/gr00t_config.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/assemble_trocar/config/gr00t_config.py new file mode 100644 index 000000000000..540b0edbc3a0 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/assemble_trocar/config/gr00t_config.py @@ -0,0 +1,144 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""GR00T data configuration for IsaacLab tasks. + +This module defines customizable GR00T data configurations for different +embodiments. Users can create their own data config classes by subclassing +BaseDataConfig or copying/modifying the examples here. + +Example usage in run.sh: + export RLINF_DATA_CONFIG="policy.gr00t_config" + export RLINF_DATA_CONFIG_CLASS="policy.gr00t_config:IsaacLabDataConfig" +""" + +from gr00t.data.dataset import ModalityConfig +from gr00t.data.transform.base import ComposedModalityTransform +from gr00t.data.transform.concat import ConcatTransform +from gr00t.data.transform.state_action import StateActionSinCosTransform, StateActionToTensor, StateActionTransform +from gr00t.data.transform.video import VideoColorJitter, VideoToNumpy, VideoToTensor +from gr00t.experiment.data_config import DATA_CONFIG_MAP, BaseDataConfig +from gr00t.model.transforms import GR00TTransform + + +class IsaacLabDataConfig(BaseDataConfig): + """Generic GR00T data config for IsaacLab tasks with G1 + Dex3.""" + + # Video modality keys (from gr00t_mapping.video in RLINF_OBS_MAP_JSON) + video_keys = [ + "video.left_wrist_view", + "video.right_wrist_view", + "video.room_view", + ] + + # State modality keys (from gr00t_mapping.state in RLINF_OBS_MAP_JSON) + state_keys = [ + "state.left_arm", + "state.right_arm", + "state.left_hand", + "state.right_hand", + ] + + # Action modality keys (output from GR00T model) + action_keys = [ + "action.left_arm", + "action.right_arm", + "action.left_hand", + "action.right_hand", + ] + + # Language annotation key + language_keys = ["annotation.human.task_description"] + + # Observation and action indices + observation_indices = [0] + action_indices = list(range(16)) + + def modality_config(self) -> dict[str, ModalityConfig]: + """Define modality configurations for video, state, action, and language.""" + video_modality = ModalityConfig( + delta_indices=self.observation_indices, + modality_keys=self.video_keys, + ) + + state_modality = ModalityConfig( + delta_indices=self.observation_indices, + modality_keys=self.state_keys, + ) + + action_modality = ModalityConfig( + delta_indices=self.action_indices, + modality_keys=self.action_keys, + ) + + language_modality = ModalityConfig( + delta_indices=self.observation_indices, + modality_keys=self.language_keys, + ) + + return { + "video": video_modality, + "state": state_modality, + "action": action_modality, + "language": language_modality, + } + + def transform(self): + """Define the transform pipeline for processing observations and actions.""" + transforms = [ + # Video transforms + VideoToTensor(apply_to=self.video_keys), + # Disabled: camera already outputs 224×224 via TiledCameraCfg. + # To avoid VideoToTensor size-check errors, either: + # 1. Disable input size validation in VideoToTensor, OR + # 2. Set modality meta height/width to 224 to match actual input. + # Re-enable VideoCrop/VideoResize if camera resolution changes. + # VideoCrop(apply_to=self.video_keys, scale=0.95), + # VideoResize( + # apply_to=self.video_keys, + # height=224, + # width=224, + # interpolation="linear", + # ), + VideoColorJitter( + apply_to=self.video_keys, + brightness=0.3, + contrast=0.4, + saturation=0.5, + hue=0.08, + ), + VideoToNumpy(apply_to=self.video_keys), + # State transforms + StateActionToTensor(apply_to=self.state_keys), + StateActionSinCosTransform(apply_to=self.state_keys), + # Action transforms + StateActionToTensor(apply_to=self.action_keys), + StateActionTransform( + apply_to=self.action_keys, + normalization_modes={key: "min_max" for key in self.action_keys}, + ), + # Concat transforms + ConcatTransform( + video_concat_order=self.video_keys, + state_concat_order=self.state_keys, + action_concat_order=self.action_keys, + ), + # Model-specific transform + GR00TTransform( + state_horizon=len(self.observation_indices), + action_horizon=len(self.action_indices), + max_state_dim=64, + max_action_dim=32, + ), + ] + return ComposedModalityTransform(transforms=transforms) + + +# -------------------------------------------------------------------------- +# Register data configs into GR00T's DATA_CONFIG_MAP +# -------------------------------------------------------------------------- + +# This allows load_data_config("policy.gr00t_config:IsaacLabDataConfig") to work +DATA_CONFIG_MAP["isaaclab_g1_dex3"] = IsaacLabDataConfig() diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/assemble_trocar/config/isaaclab_ppo_gr00t_assemble_trocar.yaml b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/assemble_trocar/config/isaaclab_ppo_gr00t_assemble_trocar.yaml new file mode 100644 index 000000000000..b130a12a8a53 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/assemble_trocar/config/isaaclab_ppo_gr00t_assemble_trocar.yaml @@ -0,0 +1,298 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +defaults: + - override hydra/job_logging: stdout + +hydra: + run: + dir: . + output_subdir: null + +cluster: + num_nodes: 1 + component_placement: + actor,env,rollout: all + +runner: + task_type: embodied + logger: + log_path: "../results" + project_name: rlinf + experiment_name: "test_gr00t" + logger_backends: ["tensorboard"] # wandb, swanlab + + max_epochs: 1000 + max_steps: -1 + + only_eval: False + eval_policy_path: null # Optional: .pt file or None, if None, will use the checkpoint in rollout.model.model_path + val_check_interval: -1 + save_interval: 2 + seq_length: 4096 + max_prompt_length: 30 + + resume_dir: null + +algorithm: + normalize_advantages: True + kl_penalty: kl # how to estimate kl divergence: kl or kl_penalty + group_size: 1 + reward_coef: 1.0 + rollout_epoch: 2 + eval_rollout_epoch: 1 # set eval_rollout_epoch > 0 when enable runner.only_eval or runner.val_check_interval > 0 + + reward_type: chunk_level + logprob_type: chunk_level + entropy_type: chunk_level + + update_epoch: 4 + adv_type: gae + loss_type: actor_critic + loss_agg_func: "token-mean" + kl_beta: 0.0 + entropy_bonus: 0 + clip_ratio_high: 0.2 + clip_ratio_low: 0.2 + clip_ratio_c: 3.0 + value_clip: 0.2 + huber_delta: 10.0 + + gamma: 0.99 + gae_lambda: 0.95 + + filter_rewards: False + rewards_lower_bound: 0.1 + rewards_upper_bound: 0.9 + # params for generation + sampling_params: + do_sample: True + temperature_train: 1.0 + temperature_eval: 0.6 + top_k: 50 + top_p: 1.0 + repetition_penalty: 1.0 + add_BOS: False + + # length argument for autoregressive sampling + # max length means max amount of tokens to generate + length_params: + max_new_token: null + max_length: 1024 + min_length: 1 + +# --------------------------------------------------------------------------- +# Environment +# --------------------------------------------------------------------------- +env: + group_name: "EnvGroup" + channel: + name: "env_buffer_list" + queue_name: "obs_buffer" + queue_size: 0 + enable_offload: False + + train: + env_type: isaaclab + total_num_envs: 4 + auto_reset: False + ignore_terminations: False + use_rel_reward: True + seed: 0 + group_size: 1 + reward_coef: 1.0 + use_fixed_reset_state_ids: True + max_steps_per_rollout_epoch: 256 + max_episode_steps: 256 + video_cfg: + save_video: False + info_on_video: True + video_base_dir: ${runner.logger.log_path}/video/train + init_params: + id: "Isaac-Assemble-Trocar-G129-Dex3-v0" + num_envs: null + max_episode_steps: ${env.train.max_episode_steps} + task_description: "assemble trocar from tray" + + # ======================================================================== + # IsaacLab -> RLinf -> GR00T observation/action mapping configuration + # This section defines how IsaacLab observations are converted to GR00T format + # ======================================================================== + isaaclab: &isaaclab_config # YAML anchor for reuse in eval + # Task description for language conditioning + task_description: "assemble trocar from tray" + + # --- IsaacLab -> RLinf observation mapping --- + # main_images: single camera key for main view + main_images: "front_camera" + # extra_view_images: list of camera keys to stack as (B, N, H, W, C) + extra_view_images: + - "left_wrist_camera" + - "right_wrist_camera" + # states: list of state specs with optional slicing + # Each entry can be a string (use full tensor) or dict with "key" and "slice" + states: + - key: "robot_joint_state" + slice: [15, 29] # G129 shoulder joints + - key: "robot_dex3_joint_state" + # slice: null # Use full tensor + + # --- RLinf -> GR00T format conversion --- + gr00t_mapping: + video: + main_images: "video.room_view" + extra_view_images: + - "video.left_wrist_view" + - "video.right_wrist_view" + state: + # Slice concatenated states into GR00T state keys + # Total states: 14 (shoulder) + 14 (dex3) = 28 dims + - gr00t_key: "state.left_arm" + slice: [0, 7] + - gr00t_key: "state.right_arm" + slice: [7, 14] + - gr00t_key: "state.left_hand" + slice: [14, 21] + - gr00t_key: "state.right_hand" + slice: [21, 28] + + # --- GR00T -> IsaacLab action conversion --- + action_mapping: + prefix_pad: 15 # Pad zeros at front for G129 body joints (not controlled) + suffix_pad: 0 + + # --- GR00T model configuration (single source of truth) --- + # actor.model.embodiment_tag and obs_converter_type reference these values via ${} + obs_converter_type: "dex3" + embodiment_tag: "new_embodiment" + embodiment_tag_id: 31 + data_config_class: "gr00t_config:IsaacLabDataConfig" + + eval: + env_type: isaaclab + total_num_envs: 4 + auto_reset: True + ignore_terminations: True + use_rel_reward: True + seed: 0 + group_size: 1 + reward_coef: 1.0 + use_fixed_reset_state_ids: True + max_steps_per_rollout_epoch: 256 + max_episode_steps: 256 + video_cfg: + save_video: True + info_on_video: True + video_base_dir: ${runner.logger.log_path}/video/eval + init_params: + id: "Isaac-Assemble-Trocar-G129-Dex3-Eval-v0" + num_envs: null + max_episode_steps: ${env.eval.max_episode_steps} + task_description: "install trocar from box" + # Reuse IsaacLab config from train section via YAML anchor + isaaclab: *isaaclab_config + +# --------------------------------------------------------------------------- +# Rollout +# --------------------------------------------------------------------------- +rollout: + group_name: "RolloutGroup" + channel: + name: ${env.channel.name} + queue_name: "action_buffer" + queue_size: 0 + mode: "colocate" + backend: "huggingface" + enable_offload: True + pipeline_stage_num: 1 + + model: + model_path: "/mnt/ckpt/g1_install_trocar_sim_box_v3_60_train_bs32_1_gpus_cos_30k_tune_visual/" + precision: ${actor.model.precision} + obs_converter_type: ${env.train.isaaclab.obs_converter_type} + embodiment_tag: ${env.train.isaaclab.embodiment_tag} + +# --------------------------------------------------------------------------- +# Actor +# --------------------------------------------------------------------------- +actor: + group_name: "ActorGroup" + channel: + name: ${env.channel.name} + queue_name: "replay_buffer" + queue_size: 0 + training_backend: "fsdp" + micro_batch_size: 2 + global_batch_size: 4 + seed: 1234 + enable_offload: False + + model: + model_type: "gr00t" + model_path: "/mnt/ckpt/g1_install_trocar_sim_box_v3_60_train_bs32_1_gpus_cos_30k_tune_visual/" + precision: "bf16" + trust_remote_code: True + is_lora: false + action_dim: 28 + num_action_chunks: 1 + denoising_steps: 4 + policy_setup: "widowx_bridge" + obs_converter_type: ${env.train.isaaclab.obs_converter_type} + embodiment_tag: ${env.train.isaaclab.embodiment_tag} + add_value_head: True + rl_head_config: + joint_logprob: False + noise_method: "flow_sde" + ignore_last: False + safe_get_logprob: False + noise_anneal: False + noise_params: [0.7, 0.3, 400] + noise_level: 0.3 + add_value_head: ${actor.model.add_value_head} + chunk_critic_input: False + detach_critic_input: True + disable_dropout: True + use_vlm_value: False + value_vlm_mode: "mean_token" + padding_value: 850 + + optim: + lr: 5e-6 + value_lr: 1e-4 + adam_beta1: 0.9 + adam_beta2: 0.95 + adam_eps: 1.0e-08 + clip_grad: 1.0 + weight_decay: 0.01 + critic_warmup_steps: 0 + + fsdp_config: + strategy: "fsdp" + sharding_strategy: "full_shard" + gradient_checkpointing: False + cpu_offload: False + offload_pin_memory: False + reshard_after_forward: True + enable_gradient_accumulation: True + forward_prefetch: False + limit_all_gathers: False + backward_prefetch: null + use_orig_params: False + use_liger_kernel: False + fsdp_size: -1 + mixed_precision: + param_dtype: ${actor.model.precision} + reduce_dtype: ${actor.model.precision} + buffer_dtype: ${actor.model.precision} + amp: + enabled: False + precision: "bf16" + use_grad_scaler: False + +reward: + use_reward_model: False + +critic: + use_critic_model: False diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/assemble_trocar/config/robot_config.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/assemble_trocar/config/robot_config.py new file mode 100644 index 000000000000..ed70792ca6a0 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/assemble_trocar/config/robot_config.py @@ -0,0 +1,147 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Robot configuration for the `install_trocar` task. + +This file is intentionally **minimal**: +- Supported robot: **Unitree G1 (29 DOF body)** +- Supported hands: **Dex3** + +The only public entry point expected by the task is +`G1RobotPresets.g1_29dof_dex3_base_fix(...)`. +""" + +import math + +from isaaclab.assets import ArticulationCfg +from isaaclab.utils import configclass + +from isaaclab_assets.robots.unitree import G129_CFG_WITH_DEX3_BASE_FIX + +# Joint indices in the full robot joint vector for observation extraction. +# Body joints: 29 DOF (legs, waist, arms, wrists) +G1_29DOF_BODY_JOINT_INDICES: list[int] = [ + 0, + 3, + 6, + 9, + 13, + 17, + 1, + 4, + 7, + 10, + 14, + 18, + 2, + 5, + 8, + 11, + 15, + 19, + 21, + 23, + 25, + 27, + 12, + 16, + 20, + 22, + 24, + 26, + 28, +] + +# Dex3 hand joints: 14 DOF (left + right) +G1_DEX3_JOINT_INDICES: list[int] = [31, 37, 41, 30, 36, 29, 35, 34, 40, 42, 33, 39, 32, 38] + +# Default joint positions for the supported setup (G1 29DOF + Dex3). +DEFAULT_JOINT_POS: dict[str, float] = { + # legs + "left_hip_pitch_joint": 0.0, + "left_hip_roll_joint": 0.0, + "left_hip_yaw_joint": 0.0, + "left_knee_joint": 0.0, + "left_ankle_pitch_joint": 0.0, + "left_ankle_roll_joint": 0.0, + "right_hip_pitch_joint": 0.0, + "right_hip_roll_joint": 0.0, + "right_hip_yaw_joint": 0.0, + "right_knee_joint": 0.0, + "right_ankle_pitch_joint": 0.0, + "right_ankle_roll_joint": 0.0, + # waist + "waist_yaw_joint": 0.0, + "waist_roll_joint": 0.0, + "waist_pitch_joint": 0.0, + # arms + "left_shoulder_pitch_joint": -0.754599, + "left_shoulder_roll_joint": 0.550010, + "left_shoulder_yaw_joint": -0.399298, + "left_elbow_joint": 0.278886, + "left_wrist_roll_joint": 0.320559, + "left_wrist_pitch_joint": -0.203525, + "left_wrist_yaw_joint": -0.387435, + "right_shoulder_pitch_joint": -0.340858, + "right_shoulder_roll_joint": -0.186152, + "right_shoulder_yaw_joint": 0.015023, + "right_elbow_joint": -0.777159, + "right_wrist_roll_joint": 0.019805, + "right_wrist_pitch_joint": 1.182285, + "right_wrist_yaw_joint": -0.022848, + # dex3 hands (left) + "left_hand_index_0_joint": -60.0 * math.pi / 180.0, + "left_hand_middle_0_joint": -60.0 * math.pi / 180.0, + "left_hand_thumb_0_joint": 0.0, + "left_hand_index_1_joint": -40.0 * math.pi / 180.0, + "left_hand_middle_1_joint": -40.0 * math.pi / 180.0, + "left_hand_thumb_1_joint": 0.0, + "left_hand_thumb_2_joint": 0.0, + # dexterous hand joint - right hand + "right_hand_index_0_joint": 60.0 * math.pi / 180.0, + "right_hand_middle_0_joint": 60.0 * math.pi / 180.0, + "right_hand_thumb_0_joint": 0.0, + "right_hand_index_1_joint": 40.0 * math.pi / 180.0, + "right_hand_middle_1_joint": 40.0 * math.pi / 180.0, + "right_hand_thumb_1_joint": 0.0, + "right_hand_thumb_2_joint": 0.0, +} + + +def make_g1_29dof_dex3_cfg( + *, + prim_path: str = "/World/envs/env_.*/Robot", + init_pos: tuple[float, float, float] = (-0.15, 0.0, 0.744), + init_rot: tuple[float, float, float, float] = (0, 0, 0.7071, 0.7071), + custom_joint_pos: dict[str, float] | None = None, + base_config: ArticulationCfg = G129_CFG_WITH_DEX3_BASE_FIX, +) -> ArticulationCfg: + """Create the only supported robot articulation cfg for this task.""" + joint_pos = DEFAULT_JOINT_POS.copy() + if custom_joint_pos: + joint_pos.update(custom_joint_pos) + return base_config.replace( + prim_path=prim_path, + init_state=ArticulationCfg.InitialStateCfg( + pos=init_pos, + rot=init_rot, + joint_pos=joint_pos, + joint_vel={".*": 0.0}, + ), + ) + + +@configclass +class G1RobotPresets: + """G1 robot preset configuration collection""" + + @classmethod + def g1_29dof_dex3_base_fix( + cls, + init_pos: tuple[float, float, float] = (-0.15, 0.0, 0.76), + init_rot: tuple[float, float, float, float] = (0, 0, 0.7071, 0.7071), + ) -> ArticulationCfg: + """pick-place task configuration - dex3 hand""" + return make_g1_29dof_dex3_cfg(init_pos=init_pos, init_rot=init_rot) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/assemble_trocar/g129_dex3_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/assemble_trocar/g129_dex3_env_cfg.py new file mode 100644 index 000000000000..50e58134f14c --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/assemble_trocar/g129_dex3_env_cfg.py @@ -0,0 +1,444 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab_physx.physics import PhysxCfg + +import isaaclab.envs.mdp as base_mdp +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg, RigidObjectCfg +from isaaclab.envs import ManagerBasedRLEnvCfg, ViewerCfg +from isaaclab.managers import EventTermCfg, SceneEntityCfg +from isaaclab.managers import ObservationGroupCfg as ObsGroup +from isaaclab.managers import ObservationTermCfg as ObsTerm +from isaaclab.managers import RewardTermCfg as RewTerm +from isaaclab.managers import TerminationTermCfg as DoneTerm +from isaaclab.scene import InteractiveSceneCfg +from isaaclab.sim.spawners.from_files.from_files_cfg import UsdFileCfg +from isaaclab.utils import configclass + +from isaaclab_tasks.manager_based.manipulation.assemble_trocar import mdp + +from isaaclab_tasks.manager_based.manipulation.assemble_trocar.config import ( # isort: skip + CameraPresets, + G1RobotPresets, +) + +joint_names = [ + "left_hip_pitch_joint", + "right_hip_pitch_joint", + "left_hip_roll_joint", + "right_hip_roll_joint", + "left_hip_yaw_joint", + "right_hip_yaw_joint", + "left_knee_joint", + "right_knee_joint", + "left_ankle_pitch_joint", + "right_ankle_pitch_joint", + "left_ankle_roll_joint", + "right_ankle_roll_joint", + "waist_yaw_joint", + "waist_roll_joint", + "waist_pitch_joint", + "left_shoulder_pitch_joint", + "left_shoulder_roll_joint", + "left_shoulder_yaw_joint", + "left_elbow_joint", + "left_wrist_roll_joint", + "left_wrist_pitch_joint", + "left_wrist_yaw_joint", + "right_shoulder_pitch_joint", + "right_shoulder_roll_joint", + "right_shoulder_yaw_joint", + "right_elbow_joint", + "right_wrist_roll_joint", + "right_wrist_pitch_joint", + "right_wrist_yaw_joint", + "left_hand_thumb_0_joint", + "left_hand_thumb_1_joint", + "left_hand_thumb_2_joint", + "left_hand_middle_0_joint", + "left_hand_middle_1_joint", + "left_hand_index_0_joint", + "left_hand_index_1_joint", + "right_hand_thumb_0_joint", + "right_hand_thumb_1_joint", + "right_hand_thumb_2_joint", + "right_hand_middle_0_joint", + "right_hand_middle_1_joint", + "right_hand_index_0_joint", + "right_hand_index_1_joint", +] +offset_dict = { + "left_elbow_joint": -0.3, + "right_elbow_joint": -0.3, +} + +HEALTHCARE_S3 = "https://omniverse-content-production.s3-us-west-2.amazonaws.com/Assets/Isaac/Healthcare/0.5.0/132c82d" +USD_ROOT = f"{HEALTHCARE_S3}/Props/LightWheel" + + +@configclass +class AssembleTrocarSceneCfg(InteractiveSceneCfg): + """Scene configuration for the assemble_trocar task (robot + objects + lights).""" + + # humanoid robot configuration + robot: ArticulationCfg = G1RobotPresets.g1_29dof_dex3_base_fix( + init_pos=(-1.84919, 1.94, 0.81168), init_rot=(0.0, 0.0, 0.0, 1.0) + ) + # add camera configuration + front_camera = CameraPresets.g1_front_camera() + left_wrist_camera = CameraPresets.left_dex3_wrist_camera() + right_wrist_camera = CameraPresets.right_dex3_wrist_camera() + + scene = AssetBaseCfg( + prim_path="/World/envs/env_.*/Scene", + spawn=UsdFileCfg( + usd_path=f"{USD_ROOT}/scene03.usd", + ), + ) + + trocar_1 = RigidObjectCfg( + prim_path="/World/envs/env_.*/trocar_1", + spawn=UsdFileCfg( + usd_path=f"{USD_ROOT}/Assets/Trocar002/Trocar002-xform-wo.usd", + collision_props=sim_utils.CollisionPropertiesCfg( + collision_enabled=True, + contact_offset=0.001, + rest_offset=-0.001, + ), + ), + init_state=RigidObjectCfg.InitialStateCfg( + pos=[-1.60202, 1.91362, 0.87183], + rot=[-0.0, 0.70711, 0.70711, 0.0], + ), + ) + + trocar_2 = RigidObjectCfg( + prim_path="/World/envs/env_.*/trocar_2", + spawn=UsdFileCfg( + usd_path=( + f"{USD_ROOT}/Assets/" + "DisposableLaparoscopicPunctureDevice001/" + "DisposableLaparoscopicPunctureDevice005-xform.usd" + ), + rigid_props=sim_utils.RigidBodyPropertiesCfg( + rigid_body_enabled=True, + disable_gravity=False, + ), + ), + init_state=RigidObjectCfg.InitialStateCfg( + rot=[-0.71475, -0.000243, 0.05853, 0.69692], pos=[-1.50635, 1.90997, 0.8631] + ), + ) + tray = ArticulationCfg( + prim_path="/World/envs/env_.*/surgical_tray", + spawn=UsdFileCfg( + usd_path=f"{USD_ROOT}/Assets/SurgicalTray001/SurgicalTray001.usd", + ), + init_state=ArticulationCfg.InitialStateCfg(pos=[-1.54919, 2.03365, 0.84554], rot=[0.0, 0.0, -0.70711, 0.70711]), + actuators={}, # Empty dict for passive articulation (no motors) + ) + + # Lights + light = AssetBaseCfg( + prim_path="/World/light", + spawn=sim_utils.DomeLightCfg( + color=(0.75, 0.75, 0.75), + intensity=1000.0, + ), + ) + + +## +# MDP settings +## +@configclass +class ActionsCfg: + """defines the action configuration related to robot control, using direct joint angle control""" + + joint_pos = mdp.JointPositionActionCfg( + asset_name="robot", + joint_names=joint_names, + scale=1.0, + use_default_offset=False, + offset=offset_dict, + preserve_order=True, + ) + + +@configclass +class ObservationsCfg: + """defines all available observation information""" + + @configclass + class PolicyCfg(ObsGroup): + """policy group observation configuration class + defines all state observation values for policy decision + inherit from ObsGroup base class + """ + + # robot joint state observation + robot_joint_state = ObsTerm(func=mdp.get_robot_body_joint_states) + # dex3 hand joint state observation + robot_dex3_joint_state = ObsTerm(func=mdp.get_robot_dex3_joint_states) + + def __post_init__(self): + """post initialization function + set the basic attributes of the observation group + """ + self.enable_corruption = False # disable observation value corruption + self.concatenate_terms = False # disable observation item connection + + @configclass + class CameraImagesCfg(ObsGroup): + """Observations from the robot's cameras.""" + + front_camera = ObsTerm( + func=base_mdp.image, + params={"sensor_cfg": SceneEntityCfg("front_camera"), "data_type": "rgb", "normalize": False}, + ) + left_wrist_camera = ObsTerm( + func=base_mdp.image, + params={"sensor_cfg": SceneEntityCfg("left_wrist_camera"), "data_type": "rgb", "normalize": False}, + ) + right_wrist_camera = ObsTerm( + func=base_mdp.image, + params={"sensor_cfg": SceneEntityCfg("right_wrist_camera"), "data_type": "rgb", "normalize": False}, + ) + + def __post_init__(self): + self.concatenate_terms = False + + # observation groups + # create policy observation group instance + policy: PolicyCfg = PolicyCfg() + camera_images: CameraImagesCfg = CameraImagesCfg() + + +@configclass +class TerminationsCfg: + """Termination conditions for the environment.""" + + # Time out termination + time_out = DoneTerm(func=mdp.time_out, time_out=True) + + # Task success termination (all stages completed) + task_success = DoneTerm( + func=mdp.task_success_termination, + time_out=False, # This is a success termination, not a failure + params={ + "print_log": False, + "success_stage": 4, + }, + ) + object_drop = DoneTerm( + func=mdp.object_drop_termination, + time_out=True, # Treat as timeout/failure + params={ + "drop_height_threshold": 0.5, # Objects below this Z height are considered dropped + "asset_cfg1": SceneEntityCfg("trocar_1"), + "asset_cfg2": SceneEntityCfg("trocar_2"), + }, + ) + + +@configclass +class RewardsCfg: + """Reward configuration for sparse reward mode. + + Each stage gives 1.0 reward on completion -> Total reward for full task = 4.0 + This ensures clear reward signal for each stage transition. + + ``update_stage`` runs first (weight=0) to advance the task stage before any + reward term reads it, removing implicit ordering dependencies. + """ + + # Stage machine — weight=0, runs before all reward terms to update task stage + update_stage = RewTerm( + func=mdp.update_task_stage, + weight=0.0, + params={ + "asset_cfg1": SceneEntityCfg("trocar_1"), + "asset_cfg2": SceneEntityCfg("trocar_2"), + "table_height": 0.85483, + "lift_threshold": 0.15, + "tip_align_threshold": 0.015, + "insertion_dist_threshold": 0.05, + "insertion_angle_threshold": 0.15, + "placement_x_min": -1.8, + "placement_x_max": -1.4, + "placement_y_min": 1.5, + "placement_y_max": 1.8, + "print_log": False, + }, + ) + + # Stage 0: Lift trocars + lift_trocars = RewTerm( + func=mdp.lift_trocars_reward, + weight=1.0, + params={ + "table_height": 0.85483, + "lift_threshold": 0.15, + "asset_cfg1": SceneEntityCfg("trocar_1"), + "asset_cfg2": SceneEntityCfg("trocar_2"), + "use_sparse_reward": True, + "print_log": False, + }, + ) + + # Stage 1: Tip alignment (find hole) + tip_alignment = RewTerm( + func=mdp.trocar_tip_alignment_reward, + weight=1.0, # Give 1.0 reward when stage 1->2 completes + params={ + "tip_dist_std": 0.02, # Std for tip distance reward shaping + "asset_cfg1": SceneEntityCfg("trocar_1"), + "asset_cfg2": SceneEntityCfg("trocar_2"), + "use_sparse_reward": True, + "print_log": False, + }, + ) + + # Stage 2: Insertion (push in) + insert_trocars = RewTerm( + func=mdp.trocar_insertion_reward, + weight=1.0, # Give 1.0 reward when stage 2->3 completes + params={ + "angle_std": 0.2, # Std for angle alignment reward + "angle_threshold": 0.10, # ~5.7 degrees tolerance for parallelism + "center_dist_std": 0.05, # Std for center distance reward + "asset_cfg1": SceneEntityCfg("trocar_1"), + "asset_cfg2": SceneEntityCfg("trocar_2"), + "use_sparse_reward": True, + "print_log": False, + }, + ) + + # Stage 3: Placement (place in tray) + placement_trocars = RewTerm( + func=mdp.trocar_placement_reward, + weight=1.0, # Give 1.0 reward when stage 3->4 completes + params={ + "x_min": -1.8, + "x_max": -1.4, + "y_min": 1.5, + "y_max": 1.8, + "asset_cfg1": SceneEntityCfg("trocar_1"), + "asset_cfg2": SceneEntityCfg("trocar_2"), + "use_sparse_reward": True, + "print_log": False, + }, + ) + + +@configclass +class EventCfg: + """Event configuration for scene reset.""" + + # Reset scene when episode terminates (timeout or success) + reset_scene = EventTermCfg(func=base_mdp.reset_scene_to_default, mode="reset") + + # Reset task stage tracker when environment resets + reset_task_stage = EventTermCfg(func=mdp.reset_task_stage, mode="reset") + + # Random rotation for tray and trocars + reset_tray_random_rotation = EventTermCfg( + func=mdp.reset_tray_with_random_rotation, + mode="reset", + params={ + "tray_cfg": SceneEntityCfg("tray"), + "trocar_1_cfg": SceneEntityCfg("trocar_1"), + "trocar_2_cfg": SceneEntityCfg("trocar_2"), + "rotation_range": [0, 10], + }, + ) + + +@configclass +class G1AssembleTrocarEnvCfg(ManagerBasedRLEnvCfg): + """Unitree G1 robot assemble trocar environment configuration class + inherits from ManagerBasedRLEnvCfg, defines all configuration parameters for the entire environment + """ + + # scene settings + scene: AssembleTrocarSceneCfg = AssembleTrocarSceneCfg( + num_envs=1, + env_spacing=6.0, + replicate_physics=True, + ) + # viewer settings + viewer: ViewerCfg = ViewerCfg( + eye=(-0.5, 2.4, 1.6), + lookat=(-5.4, 0.2, -1.2), + cam_prim_path="/OmniverseKit_Persp", + ) + # basic settings + observations: ObservationsCfg = ObservationsCfg() + actions: ActionsCfg = ActionsCfg() + # MDP settings + terminations: TerminationsCfg = TerminationsCfg() + events: EventCfg = EventCfg() + commands = None + rewards: RewardsCfg = RewardsCfg() + curriculum = None + + num_rerenders_on_reset: int = 1 + + def __post_init__(self): + """Post initialization.""" + # general settings + self.decimation = 4 + self.episode_length_s = 20.0 + # simulation settings + self.sim.dt = 1 / 200 + self.sim.render_interval = self.decimation + self.sim.physics = PhysxCfg(bounce_threshold_velocity=0.01) + self.sim.render.enable_translucency = True + self.sim.render.carb_settings = { + "rtx.raytracing.fractionalCutoutOpacity": True, + } + self.sim.render.rendering_mode = "quality" + self.sim.render.antialiasing_mode = "DLAA" + + +@configclass +class EventCfgFixTrayRotation(EventCfg): + """Event configuration with a deterministic-but-different yaw per env index. + + This is useful for eval with many parallel envs: + - env 0..N-1 get different yaw angles, + - for a fixed global seed, the set of N angles is reproducible across runs/resets. + + Notes: + - Determinism is tied to torch's global seed (set by env reset seed in IsaacLab). + - Angle unit is degrees. + """ + + reset_tray_random_rotation = EventTermCfg( + func=mdp.reset_tray_with_random_rotation, + mode="reset", + params={ + "tray_cfg": SceneEntityCfg("tray"), + "trocar_1_cfg": SceneEntityCfg("trocar_1"), + "trocar_2_cfg": SceneEntityCfg("trocar_2"), + "rotation_range": [0, 10], + "deterministic_per_env": True, + # Use torch.initial_seed() by default to follow the env reset seed. + "deterministic_seed": None, + }, + ) + + +@configclass +class G1AssembleTrocarEvalEnvCfg(G1AssembleTrocarEnvCfg): + """Eval-friendly env cfg. + + This is currently an alias of `G1AssembleTrocarEnvCfg`, but registered under a + separate Gym id for compatibility with RLinf configs. + """ + + # Override events to enforce deterministic per-env tray yaw on every reset. + events: EventCfgFixTrayRotation = EventCfgFixTrayRotation() diff --git a/source/isaaclab_newton/isaaclab_newton/scene_data_providers/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/assemble_trocar/mdp/__init__.py similarity index 83% rename from source/isaaclab_newton/isaaclab_newton/scene_data_providers/__init__.py rename to source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/assemble_trocar/mdp/__init__.py index cf0f30853ead..d428ed46f7b4 100644 --- a/source/isaaclab_newton/isaaclab_newton/scene_data_providers/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/assemble_trocar/mdp/__init__.py @@ -3,7 +3,7 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""Newton scene data provider backends.""" +"""MDP utilities for the assemble_trocar task.""" from isaaclab.utils.module import lazy_export diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/assemble_trocar/mdp/events.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/assemble_trocar/mdp/events.py new file mode 100644 index 000000000000..92214471ac09 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/assemble_trocar/mdp/events.py @@ -0,0 +1,253 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Custom event functions for pick place surgical environment.""" + +from __future__ import annotations + +import logging +import math +from typing import TYPE_CHECKING + +import torch + +from isaaclab.managers import SceneEntityCfg +from isaaclab.utils.math import quat_apply, quat_mul + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + +logger = logging.getLogger(__name__) + +__all__ = [ + "reset_tray_with_random_rotation", + "reset_robot_to_default_joint_positions", + "reset_task_stage", +] + + +def reset_task_stage( + env: ManagerBasedRLEnv, + env_ids: torch.Tensor, + print_log: bool = False, +) -> None: + """Reset task stage to 0 for specified environments. + + This should be called during environment reset events. + Also resets all locked reward caches to maintain continuity. + + Args: + env: The environment instance + env_ids: Indices of environments to reset + print_log: If True, log debug information. + """ + from .rewards import get_assemble_trocar_state + + s = get_assemble_trocar_state(env) + s.task_stage[env_ids] = 0 + + # Reset dense-reward locked caches + s.lift_reward_locked[env_ids] = 0 + s.tip_reward_locked[env_ids] = 0 + s.insertion_reward_locked[env_ids] = 0 + s.placement_reward_locked[env_ids] = 0 + + # Reset sparse-reward previous-stage trackers + s.prev_stage_lift[env_ids] = 0 + s.prev_stage_tip[env_ids] = 0 + s.prev_stage_insert[env_ids] = 0 + s.prev_stage_place[env_ids] = 0 + + # Reset debug throttle + s.last_debug_print_step = -1 + + if print_log: + logger.debug("Reset task stage for %d environment(s)", len(env_ids)) + + +def reset_tray_with_random_rotation( + env: ManagerBasedRLEnv, + env_ids: torch.Tensor, + tray_cfg: SceneEntityCfg, + trocar_1_cfg: SceneEntityCfg, + trocar_2_cfg: SceneEntityCfg, + rotation_range: tuple[float, float] | float = (-5.0, 5.0), # (min, max) degrees or ±value + deterministic_per_env: bool = False, + deterministic_seed: int | None = None, +): + """Reset tray with random rotation while keeping relative positions of trocars. + + This function: + 1. Applies a random yaw rotation within rotation_range to the tray + 2. Rotates trocar_1 and trocar_2 around the tray center to maintain relative positions + 3. Uses separate pose/velocity writes to ensure instant teleportation (no interpolation) + + Args: + env: The environment instance. + env_ids: The environment indices to reset. + tray_cfg: Scene entity config for the tray. + trocar_1_cfg: Scene entity config for trocar_1. + trocar_2_cfg: Scene entity config for trocar_2. + rotation_range: Rotation angle range in degrees. Can be: + - tuple (min, max): Random rotation between min and max degrees + - float value: Random rotation between -value and +value degrees + Examples: (0, 10), (-5, 15), 5.0 (equivalent to (-5, 5)) + """ + if len(env_ids) == 0: + return + + # Parse rotation_range parameter + if isinstance(rotation_range, (tuple, list)): + # User provided (min, max) range + min_angle_deg, max_angle_deg = rotation_range[0], rotation_range[1] + else: + # User provided single value (symmetric range ±value) + min_angle_deg, max_angle_deg = -rotation_range, rotation_range + + # Get assets + tray = env.scene[tray_cfg.name] + trocar_1 = env.scene[trocar_1_cfg.name] + trocar_2 = env.scene[trocar_2_cfg.name] + + # Get default poses and velocities (local coordinates relative to env origin) + tray_default_pose = tray.data.default_root_pose.torch[env_ids].clone() + trocar_1_default_pose = trocar_1.data.default_root_pose.torch[env_ids].clone() + trocar_2_default_pose = trocar_2.data.default_root_pose.torch[env_ids].clone() + + env_origins = env.scene.env_origins[env_ids] # (num_envs, 3) + + # Convert local coordinate to world coordinate + tray_default_pose[:, :3] += env_origins + trocar_1_default_pose[:, :3] += env_origins + trocar_2_default_pose[:, :3] += env_origins + + # Tray center position (pivot point for rotation) - now in world coordinates + tray_center = tray_default_pose[:, :3] # (num_envs, 3) + + # Generate yaw angles (in radians) + # Convert degrees to radians + min_angle_rad = min_angle_deg * math.pi / 180.0 + max_angle_rad = max_angle_deg * math.pi / 180.0 + + # Generate angles uniformly distributed in [min_angle, max_angle] + if deterministic_per_env: + # Derive a stable "random" number per env id, so each env gets a distinct yaw, + # but it is repeatable across resets/runs given the same seed + env_id. + # + # If deterministic_seed is not provided, we tie it to torch's global seed. + # IsaacLab typically seeds torch during env reset with the provided seed. + if deterministic_seed is None: + deterministic_seed = int(torch.initial_seed()) + u = _deterministic_uniform_0_1_from_ids(env, env_ids, deterministic_seed) # (num_envs,) + else: + u = torch.rand(len(env_ids), device=env.device) + random_yaw = u * (max_angle_rad - min_angle_rad) + min_angle_rad # (num_envs,) + + # Create rotation quaternion for yaw (rotation around Z-axis) + # XYZW: quat = [x, y, z, w] = [0, 0, sin(θ/2), cos(θ/2)] + half_angle = random_yaw / 2.0 + delta_quat = torch.zeros(len(env_ids), 4, device=env.device) + delta_quat[:, 2] = torch.sin(half_angle) # z + delta_quat[:, 3] = torch.cos(half_angle) # w + + # Apply rotation to tray quaternion + tray_new_quat = quat_mul(delta_quat, tray_default_pose[:, 3:7]) + + # Update tray pose + tray_new_pose = tray_default_pose.clone() + tray_new_pose[:, 3:7] = tray_new_quat + + # Rotate trocar positions around tray center + trocar_1_relative_pos = trocar_1_default_pose[:, :3] - tray_center + trocar_2_relative_pos = trocar_2_default_pose[:, :3] - tray_center + + # Rotate relative positions using the delta quaternion + trocar_1_new_relative_pos = quat_apply(delta_quat, trocar_1_relative_pos) + trocar_2_new_relative_pos = quat_apply(delta_quat, trocar_2_relative_pos) + + # New absolute poses + trocar_1_new_pose = trocar_1_default_pose.clone() + trocar_2_new_pose = trocar_2_default_pose.clone() + + trocar_1_new_pose[:, :3] = tray_center + trocar_1_new_relative_pos + trocar_2_new_pose[:, :3] = tray_center + trocar_2_new_relative_pos + + # Also rotate trocar orientations + trocar_1_new_pose[:, 3:7] = quat_mul(delta_quat, trocar_1_default_pose[:, 3:7]) + trocar_2_new_pose[:, 3:7] = quat_mul(delta_quat, trocar_2_default_pose[:, 3:7]) + + zero_velocity = torch.zeros(len(env_ids), 6, device=env.device) # [lin_vel(3), ang_vel(3)] + + tray.write_root_pose_to_sim_index(root_pose=tray_new_pose, env_ids=env_ids) + trocar_1.write_root_pose_to_sim_index(root_pose=trocar_1_new_pose, env_ids=env_ids) + trocar_2.write_root_pose_to_sim_index(root_pose=trocar_2_new_pose, env_ids=env_ids) + + tray.write_root_velocity_to_sim_index(root_velocity=zero_velocity, env_ids=env_ids) + trocar_1.write_root_velocity_to_sim_index(root_velocity=zero_velocity, env_ids=env_ids) + trocar_2.write_root_velocity_to_sim_index(root_velocity=zero_velocity, env_ids=env_ids) + + +def _deterministic_uniform_0_1_from_ids( + env: ManagerBasedRLEnv, + ids: torch.Tensor, + seed: int, +) -> torch.Tensor: + """Deterministically map env ids -> floats in [0, 1) via a seeded lookup table. + + We generate a length-(env.num_envs) random table with a local torch.Generator + seeded by `seed`, then return table[ids]. This is deterministic and avoids + uint64 bitwise ops (which may not be supported on CPU). + """ + device = env.device + num_envs = int(env.num_envs) + seed = int(seed) + + cache = getattr(env, "_deterministic_u_table_cache", None) + cache_key = (seed, num_envs, str(device)) + if cache is None or cache.get("key") != cache_key: + gen = torch.Generator(device=device) + gen.manual_seed(seed & 0xFFFFFFFFFFFFFFFF) + u_table = torch.rand((num_envs,), generator=gen, device=device, dtype=torch.float32) + cache = {"key": cache_key, "u_table": u_table} + setattr(env, "_deterministic_u_table_cache", cache) + + return cache["u_table"][ids] + + +def reset_robot_to_default_joint_positions( + env: ManagerBasedRLEnv, + env_ids: torch.Tensor, + robot_cfg: SceneEntityCfg, +): + """Reset robot joint positions directly to default values. + + This function directly writes joint positions and velocities to the simulation, + bypassing the PD controller. This prevents the "drive to target" behavior + that causes arms to swing from 0 position to the target position. + + Args: + env: The environment instance. + env_ids: The environment indices to reset. + robot_cfg: Scene entity config for the robot. + """ + if len(env_ids) == 0: + return + + # Get robot asset + robot = env.scene[robot_cfg.name] + + # Get default joint positions and velocities + default_joint_pos = robot.data.default_joint_pos.torch[env_ids].clone() + default_joint_vel = robot.data.default_joint_vel.torch[env_ids].clone() + + # Directly write joint state to simulation (bypasses PD controller) + robot.write_joint_position_to_sim_index(position=default_joint_pos, env_ids=env_ids) + robot.write_joint_velocity_to_sim_index(velocity=default_joint_vel, env_ids=env_ids) + + # Also reset root pose and velocity + default_root_pose = robot.data.default_root_pose.torch[env_ids].clone() + default_root_vel = robot.data.default_root_vel.torch[env_ids].clone() + robot.write_root_pose_to_sim_index(root_pose=default_root_pose, env_ids=env_ids) + robot.write_root_velocity_to_sim_index(root_velocity=default_root_vel, env_ids=env_ids) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/assemble_trocar/mdp/observations.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/assemble_trocar/mdp/observations.py new file mode 100644 index 000000000000..06c037ba3d68 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/assemble_trocar/mdp/observations.py @@ -0,0 +1,119 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +G1 29DOF (body) + Dex3 joint state helpers for the assemble_trocar task. + +Notes: +- DDS has been removed (simulation-only observations). +- These functions are designed to be used as Isaac Lab observation terms. +""" + +from __future__ import annotations + +from typing import TYPE_CHECKING + +import torch + +from isaaclab_tasks.manager_based.manipulation.assemble_trocar.config import ( + G1_29DOF_BODY_JOINT_INDICES, + G1_DEX3_JOINT_INDICES, +) + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + + +# Observation cache: index tensors + preallocated output buffers (body joints) +_body_obs_cache = { + "device": None, + "batch": None, + "idx_t": None, + "idx_batch": None, + "pos_buf": None, + "vel_buf": None, + "torque_buf": None, + "combined_buf": None, +} + + +def get_robot_body_joint_states(env: ManagerBasedRLEnv) -> torch.Tensor: + """Return body joint states as a single tensor: [pos(29) | vel(29) | torque(29)].""" + robot_data = env.scene["robot"].data + joint_pos = robot_data.joint_pos.torch + joint_vel = robot_data.joint_vel.torch + joint_torque = robot_data.applied_torque.torch + device = joint_pos.device + batch = joint_pos.shape[0] + + global _body_obs_cache + if _body_obs_cache["device"] != device or _body_obs_cache["idx_t"] is None: + _body_obs_cache["idx_t"] = torch.tensor(G1_29DOF_BODY_JOINT_INDICES, dtype=torch.long, device=device) + _body_obs_cache["device"] = device + _body_obs_cache["batch"] = None + + idx_t = _body_obs_cache["idx_t"] + n = idx_t.numel() + + if _body_obs_cache["batch"] != batch or _body_obs_cache["idx_batch"] is None: + _body_obs_cache["idx_batch"] = idx_t.unsqueeze(0).expand(batch, n) + _body_obs_cache["pos_buf"] = torch.empty(batch, n, device=device, dtype=joint_pos.dtype) + _body_obs_cache["vel_buf"] = torch.empty(batch, n, device=device, dtype=joint_pos.dtype) + _body_obs_cache["torque_buf"] = torch.empty(batch, n, device=device, dtype=joint_pos.dtype) + _body_obs_cache["combined_buf"] = torch.empty(batch, n * 3, device=device, dtype=joint_pos.dtype) + _body_obs_cache["batch"] = batch + + idx_batch = _body_obs_cache["idx_batch"] + pos_buf = _body_obs_cache["pos_buf"] + vel_buf = _body_obs_cache["vel_buf"] + torque_buf = _body_obs_cache["torque_buf"] + combined_buf = _body_obs_cache["combined_buf"] + + torch.gather(joint_pos, 1, idx_batch, out=pos_buf) + torch.gather(joint_vel, 1, idx_batch, out=vel_buf) + torch.gather(joint_torque, 1, idx_batch, out=torque_buf) + + combined_buf[:, 0:n].copy_(pos_buf) + combined_buf[:, n : 2 * n].copy_(vel_buf) + combined_buf[:, 2 * n : 3 * n].copy_(torque_buf) + return combined_buf + + +# Observation cache: index tensors + preallocated output buffers (Dex3 hand joints) +_dex3_obs_cache = { + "device": None, + "batch": None, + "idx_t": None, + "idx_batch": None, + "pos_buf": None, +} + + +def get_robot_dex3_joint_states(env: ManagerBasedRLEnv) -> torch.Tensor: + """Return Dex3 joint positions [batch, 14].""" + joint_pos = env.scene["robot"].data.joint_pos.torch + device = joint_pos.device + batch = joint_pos.shape[0] + + global _dex3_obs_cache + if _dex3_obs_cache["device"] != device or _dex3_obs_cache["idx_t"] is None: + _dex3_obs_cache["idx_t"] = torch.tensor(G1_DEX3_JOINT_INDICES, dtype=torch.long, device=device) + _dex3_obs_cache["device"] = device + _dex3_obs_cache["batch"] = None + + idx_t = _dex3_obs_cache["idx_t"] + n = idx_t.numel() + + if _dex3_obs_cache["batch"] != batch or _dex3_obs_cache["idx_batch"] is None: + _dex3_obs_cache["idx_batch"] = idx_t.unsqueeze(0).expand(batch, n) + _dex3_obs_cache["pos_buf"] = torch.empty(batch, n, device=device, dtype=joint_pos.dtype) + _dex3_obs_cache["batch"] = batch + + idx_batch = _dex3_obs_cache["idx_batch"] + pos_buf = _dex3_obs_cache["pos_buf"] + + torch.gather(joint_pos, 1, idx_batch, out=pos_buf) + + return pos_buf diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/assemble_trocar/mdp/rewards.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/assemble_trocar/mdp/rewards.py new file mode 100644 index 000000000000..504d9caba67d --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/assemble_trocar/mdp/rewards.py @@ -0,0 +1,634 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import logging +from dataclasses import dataclass, field +from typing import TYPE_CHECKING + +import torch + +from isaaclab.assets import RigidObject +from isaaclab.managers import SceneEntityCfg +from isaaclab.utils.math import quat_apply + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + +logger = logging.getLogger(__name__) + +__all__ = [ + "AssembleTrocarState", + "update_task_stage", + "lift_trocars_reward", + "trocar_tip_alignment_reward", + "trocar_insertion_reward", + "trocar_placement_reward", +] + + +@dataclass +class AssembleTrocarState: + """Namespaced task state for the assemble-trocar environment. + + Holds per-env stage tracking, reward caches, and debug bookkeeping. + Attached to the env as ``env.assemble_trocar_state`` and initialised + lazily on first access via :func:`get_assemble_trocar_state`. + + Stage semantics: + 0 - Initial (need to lift) + 1 - Lifted (need to find hole / tip alignment) + 2 - Hole found (need to insert / push in) + 3 - Inserted (need to place) + 4 - Placed (task complete) + """ + + task_stage: torch.Tensor = field(default_factory=lambda: torch.empty(0)) + # Sparse-reward previous-stage trackers (one per reward term) + prev_stage_lift: torch.Tensor = field(default_factory=lambda: torch.empty(0)) + prev_stage_tip: torch.Tensor = field(default_factory=lambda: torch.empty(0)) + prev_stage_insert: torch.Tensor = field(default_factory=lambda: torch.empty(0)) + prev_stage_place: torch.Tensor = field(default_factory=lambda: torch.empty(0)) + # Dense-reward locked caches + lift_reward_locked: torch.Tensor = field(default_factory=lambda: torch.empty(0)) + tip_reward_locked: torch.Tensor = field(default_factory=lambda: torch.empty(0)) + insertion_reward_locked: torch.Tensor = field(default_factory=lambda: torch.empty(0)) + placement_reward_locked: torch.Tensor = field(default_factory=lambda: torch.empty(0)) + # Cached tip offsets (populated on first call to get_trocar_tip_position) + tip_offset_trocar_1: torch.Tensor | None = None + tip_offset_trocar_2: torch.Tensor | None = None + # Debug throttle + last_debug_print_step: int = -1 + + +def get_assemble_trocar_state(env: ManagerBasedRLEnv) -> AssembleTrocarState: + """Get or lazily initialise the :class:`AssembleTrocarState` on *env*.""" + if not hasattr(env, "assemble_trocar_state"): + s = AssembleTrocarState( + task_stage=torch.zeros(env.num_envs, dtype=torch.long, device=env.device), + prev_stage_lift=torch.zeros(env.num_envs, dtype=torch.long, device=env.device), + prev_stage_tip=torch.zeros(env.num_envs, dtype=torch.long, device=env.device), + prev_stage_insert=torch.zeros(env.num_envs, dtype=torch.long, device=env.device), + prev_stage_place=torch.zeros(env.num_envs, dtype=torch.long, device=env.device), + lift_reward_locked=torch.zeros(env.num_envs, device=env.device), + tip_reward_locked=torch.zeros(env.num_envs, device=env.device), + insertion_reward_locked=torch.zeros(env.num_envs, device=env.device), + placement_reward_locked=torch.zeros(env.num_envs, device=env.device), + ) + env.assemble_trocar_state = s + return env.assemble_trocar_state + + +def get_task_stage(env: ManagerBasedRLEnv) -> torch.Tensor: + """Return the current per-env task stage tensor.""" + return get_assemble_trocar_state(env).task_stage + + +def should_print_debug(env: ManagerBasedRLEnv, print_interval: int = 50, print_log: bool = True) -> bool: + """Check if debug info should be logged based on episode step counter.""" + if not print_log: + return False + if not hasattr(env, "episode_length_buf"): + return False + + current_step = env.episode_length_buf[0].item() + if current_step == 0 or current_step % print_interval != 0: + return False + + state = get_assemble_trocar_state(env) + if state.last_debug_print_step == current_step: + return False + + state.last_debug_print_step = current_step + return True + + +def update_task_stage( + env: ManagerBasedRLEnv, + asset_cfg1: SceneEntityCfg, + asset_cfg2: SceneEntityCfg, + table_height: float = 0.85483, + lift_threshold: float = 0.05, + tip_align_threshold: float = 0.015, + insertion_dist_threshold: float = 0.03, + insertion_angle_threshold: float = 0.15, + placement_x_min: float = -1.8, + placement_x_max: float = -1.4, + placement_y_min: float = 1.5, + placement_y_max: float = 1.8, + placement_z_min: float = 0.9, + print_log: bool = False, +) -> torch.Tensor: + """Update task stage based on current state. + + This function checks conditions and advances stages automatically. + Once a stage is completed, it never goes back. + Returns a zero-valued tensor (num_envs,) so it can be used as a + weight=0 reward term to run before the actual reward terms. + """ + state = get_assemble_trocar_state(env) + stage = state.task_stage + + obj1: RigidObject = env.scene[asset_cfg1.name] + obj2: RigidObject = env.scene[asset_cfg2.name] + + pos1 = obj1.data.root_pos_w.torch + pos2 = obj2.data.root_pos_w.torch + quat1 = obj1.data.root_quat_w.torch + quat2 = obj2.data.root_quat_w.torch + # Store old stage to detect changes (BEFORE any stage transitions) + old_stage = stage.clone() + + # Stage 0 -> 1: Check if lifted + target_z = table_height + lift_threshold + is_lifted_1 = pos1[:, 2] > target_z + is_lifted_2 = pos2[:, 2] > target_z + both_lifted = is_lifted_1 & is_lifted_2 + stage = torch.where((stage == 0) & both_lifted, torch.ones_like(stage), stage) + + # Stage 1 -> 2: Check if tips are aligned (hole found) + # Get tip positions + tip_pos1 = get_trocar_tip_position(env, asset_cfg1) + tip_pos2 = get_trocar_tip_position(env, asset_cfg2) + tip_dist = torch.norm(tip_pos1 - tip_pos2, dim=-1) + + # Tip alignment success + tip_aligned = tip_dist < tip_align_threshold + stage = torch.where((stage == 1) & tip_aligned, torch.full_like(stage, 2), stage) + + # Stage 2 -> 3: Check if inserted (parallel + center close) + # Get center distance + center_dist = torch.norm(pos1 - pos2, dim=-1) + + # Check alignment + target_axis1 = torch.tensor([0.0, 0.0, -1.0], device=env.device).repeat(env.num_envs, 1) + target_axis2 = torch.tensor([0.0, 0.0, -1.0], device=env.device).repeat(env.num_envs, 1) + axis1 = quat_apply(quat1, target_axis1) + axis2 = quat_apply(quat2, target_axis2) + dot_prod = torch.sum(axis1 * axis2, dim=-1) + abs_dot = torch.clamp(torch.abs(dot_prod), max=1.0) + angle = torch.acos(abs_dot) + + # Insertion success: parallel + center close + is_parallel = angle < insertion_angle_threshold + center_close = center_dist < insertion_dist_threshold + is_inserted = is_parallel & center_close + + stage = torch.where((stage == 2) & is_inserted, torch.full_like(stage, 3), stage) + + # Stage 3 -> 4: Check if placed in target zone + # Get environment origins to handle multi-env spatial offsets + env_origins = env.scene.env_origins # shape: (num_envs, 3) + + # Adjust target zone relative to each environment's origin + curr_x_min = env_origins[:, 0] + min(placement_x_min, placement_x_max) # (num_envs,) + curr_x_max = env_origins[:, 0] + max(placement_x_min, placement_x_max) + curr_y_min = env_origins[:, 1] + min(placement_y_min, placement_y_max) + curr_y_max = env_origins[:, 1] + max(placement_y_min, placement_y_max) + + in_zone_1 = ( + (pos1[:, 0] >= curr_x_min) + & (pos1[:, 0] <= curr_x_max) + & (pos1[:, 1] >= curr_y_min) + & (pos1[:, 1] <= curr_y_max) + & (pos1[:, 2] < placement_z_min) + ) + in_zone_2 = ( + (pos2[:, 0] >= curr_x_min) + & (pos2[:, 0] <= curr_x_max) + & (pos2[:, 1] >= curr_y_min) + & (pos2[:, 1] <= curr_y_max) + & (pos2[:, 2] < placement_z_min) + ) + both_in_zone = in_zone_1 & in_zone_2 + stage = torch.where((stage == 3) & both_in_zone, torch.full_like(stage, 4), stage) + + # Print stage transitions (AFTER all stage transitions - always print when stage changes) + if print_log and (stage != old_stage).any(): + for env_id in range(env.num_envs): + if stage[env_id] != old_stage[env_id]: + logger.debug("Env %d: Stage %d → %d", env_id, old_stage[env_id].item(), stage[env_id].item()) + + state.task_stage = stage + return torch.zeros(env.num_envs, device=env.device) + + +def lift_trocars_reward( + env: ManagerBasedRLEnv, + table_height: float = 0.85483, + lift_threshold: float = 0.05, + asset_cfg1: SceneEntityCfg = SceneEntityCfg("trocar_1"), + asset_cfg2: SceneEntityCfg = SceneEntityCfg("trocar_2"), + use_sparse_reward: bool = True, + print_log: bool = False, +) -> torch.Tensor: + """Reward for lifting both trocars above the table. + + Only active in Stage 0. Once completed, this reward is locked at the achieved value. + + Args: + use_sparse_reward: If True, only give reward (1.0) when stage transitions from 0->1. + If False, give continuous reward based on current state. + print_log: If True, log debug information. + """ + s = get_assemble_trocar_state(env) + stage = s.task_stage + + obj1: RigidObject = env.scene[asset_cfg1.name] + obj2: RigidObject = env.scene[asset_cfg2.name] + + pos1 = obj1.data.root_pos_w.torch + pos2 = obj2.data.root_pos_w.torch + target_z = table_height + lift_threshold + + is_lifted_1 = pos1[:, 2] > target_z + is_lifted_2 = pos2[:, 2] > target_z + both_lifted = is_lifted_1 & is_lifted_2 + + if use_sparse_reward: + stage_just_completed = (s.prev_stage_lift == 0) & (stage >= 1) + reward = torch.where( + stage_just_completed, + torch.ones(env.num_envs, device=env.device) / env.step_dt, + torch.zeros(env.num_envs, device=env.device), + ) + s.prev_stage_lift = stage.clone() + else: + current_reward = both_lifted.float() + s.lift_reward_locked = torch.where( + (stage >= 1) & (s.lift_reward_locked == 0), + current_reward, + s.lift_reward_locked, + ) + reward = torch.where(stage == 0, current_reward, s.lift_reward_locked) + + if should_print_debug(env, print_log=print_log): + mode_str = "Sparse" if use_sparse_reward else "Dense" + logger.debug( + " Stage: %d | Lift (%s): %.2f | z1: %.3f | z2: %.3f", + stage[0].item(), + mode_str, + reward[0].item(), + pos1[0, 2], + pos2[0, 2], + ) + + return reward + + +def get_trocar_tip_position( + env: ManagerBasedRLEnv, + asset_cfg: SceneEntityCfg = SceneEntityCfg("trocar_1"), +) -> torch.Tensor: + """Get trocar tip position (White_pos or Red_pos) in world coordinates. + + Calculates tip world position using trocar root's dynamic position and rotation, + plus the tip's relative offset. + + Args: + env: Environment instance + asset_cfg: Trocar asset configuration (trocar_1 or trocar_2) + + Returns: + torch.Tensor: Shape (num_envs, 3) - Position in world coordinates + """ + from pxr import Gf, Usd, UsdGeom + + import isaaclab.utils.math as math_utils + + # Cache the tip offset to avoid recalculating every step. + # The local offset from root to tip is a static geometric property of the USD + # asset and is identical across all replicated envs. We read it once from env_0's + # USD prim, then apply it per-env at runtime using each env's dynamic root pose. + s = get_assemble_trocar_state(env) + cache_attr = f"tip_offset_{asset_cfg.name}" + tip_offset_local = getattr(s, cache_attr, None) + + if tip_offset_local is None: + usd_stage = env.scene.stage + + if asset_cfg.name == "trocar_1": + tip_path = "/World/envs/env_0/trocar_1/Trocar002/White_pos" + root_path = "/World/envs/env_0/trocar_1" + elif asset_cfg.name == "trocar_2": + tip_path = "/World/envs/env_0/trocar_2/DisposableLaparoscopicPunctureDevice001/Red_pos" + root_path = "/World/envs/env_0/trocar_2" + else: + raise ValueError(f"Invalid asset configuration: {asset_cfg.name}") + + tip_prim = usd_stage.GetPrimAtPath(tip_path) + root_prim = usd_stage.GetPrimAtPath(root_path) + + if not tip_prim.IsValid(): + logger.warning("Tip prim not found at %s, using zero offset", tip_path) + tip_offset_local = torch.zeros(3, dtype=torch.float32, device=env.device) + else: + tip_xform = UsdGeom.Xformable(tip_prim) + root_xform = UsdGeom.Xformable(root_prim) + + tip_world_transform = tip_xform.ComputeLocalToWorldTransform(Usd.TimeCode.Default()) + root_world_transform = root_xform.ComputeLocalToWorldTransform(Usd.TimeCode.Default()) + + tip_world_pos = tip_world_transform.ExtractTranslation() + root_world_pos = root_world_transform.ExtractTranslation() + + root_rotation_mat = root_world_transform.ExtractRotationMatrix() + root_rotation_quat = root_rotation_mat.ExtractRotation().GetQuat() + + tip_offset_world = Gf.Vec3d( + tip_world_pos[0] - root_world_pos[0], + tip_world_pos[1] - root_world_pos[1], + tip_world_pos[2] - root_world_pos[2], + ) + + root_quat_inv = root_rotation_quat.GetInverse() + tip_offset_local_gf = root_quat_inv.Transform(tip_offset_world) + + tip_offset_local = torch.tensor( + [tip_offset_local_gf[0], tip_offset_local_gf[1], tip_offset_local_gf[2]], + dtype=torch.float32, + device=env.device, + ) + + logger.debug("Cached tip offset for %s: %s", asset_cfg.name, tip_offset_local) + + setattr(s, cache_attr, tip_offset_local) + + obj: RigidObject = env.scene[asset_cfg.name] + root_pos_w = obj.data.root_pos_w.torch # Shape: (num_envs, 3) + root_quat_w = obj.data.root_quat_w.torch # Shape: (num_envs, 4) XYZW + + tip_offset_local_batch = tip_offset_local.unsqueeze(0).repeat(env.num_envs, 1) + + tip_offset_world = math_utils.quat_apply(root_quat_w, tip_offset_local_batch) + tip_pos_world = root_pos_w + tip_offset_world + + return tip_pos_world # Shape: (num_envs, 3) + + +def trocar_tip_alignment_reward( + env: ManagerBasedRLEnv, + tip_dist_std: float = 0.02, # Std for tip distance reward + asset_cfg1: SceneEntityCfg = SceneEntityCfg("trocar_1"), + asset_cfg2: SceneEntityCfg = SceneEntityCfg("trocar_2"), + use_sparse_reward: bool = True, + print_log: bool = False, +) -> torch.Tensor: + """Reward for aligning trocar tips (Stage 1: Finding the hole). + + Reward based on tip distance - encourages bringing tips close together. + + Only active in Stage 1. Once completed (stage >= 2), this reward is locked at the achieved value. + + Args: + env: Environment instance + tip_dist_std: Standard deviation for tip distance reward shaping + asset_cfg1: Configuration for trocar 1 + asset_cfg2: Configuration for trocar 2 + use_sparse_reward: If True, only give reward (1.0) when stage >= 2. + If False, give continuous reward based on tip distance. + print_log: If True, print debug information. + + Returns: + torch.Tensor: Reward tensor (num_envs,) + """ + s = get_assemble_trocar_state(env) + stage = s.task_stage + + tip_pos1 = get_trocar_tip_position(env, asset_cfg1) + tip_pos2 = get_trocar_tip_position(env, asset_cfg2) + tip_dist = torch.norm(tip_pos1 - tip_pos2, dim=-1) + + if use_sparse_reward: + stage_just_completed = (s.prev_stage_tip == 1) & (stage >= 2) + reward = torch.where( + stage_just_completed, + torch.ones(env.num_envs, device=env.device) / env.step_dt, + torch.zeros(env.num_envs, device=env.device), + ) + s.prev_stage_tip = stage.clone() + else: + tip_reward = torch.exp(-torch.square(tip_dist) / (2 * tip_dist_std**2)) + s.tip_reward_locked = torch.where( + (stage >= 2) & (s.tip_reward_locked == 0), + tip_reward, + s.tip_reward_locked, + ) + reward = torch.where( + stage < 1, + torch.zeros(env.num_envs, device=env.device), + torch.where(stage == 1, tip_reward, s.tip_reward_locked), + ) + + # Debug info + if should_print_debug(env, print_log=print_log) and stage[0].item() == 1: + mode_str = "Sparse" if use_sparse_reward else "Dense" + logger.debug( + " Stage 1 (Find Hole, %s): tip_pos_1=(%.3f, %.3f, %.3f)" + " | tip_pos_2=(%.3f, %.3f, %.3f) | tip_d=%.4f | reward=%.3f", + mode_str, + tip_pos1[0, 0], + tip_pos1[0, 1], + tip_pos1[0, 2], + tip_pos2[0, 0], + tip_pos2[0, 1], + tip_pos2[0, 2], + tip_dist[0].item(), + reward[0].item(), + ) + + return reward + + +def trocar_insertion_reward( + env: ManagerBasedRLEnv, + angle_std: float = 0.2, # Std for angle alignment reward + angle_threshold: float = 0.15, # Tolerance for parallelism (radians) + center_dist_std: float = 0.05, # Std for center distance reward + asset_cfg1: SceneEntityCfg = SceneEntityCfg("trocar_1"), + asset_cfg2: SceneEntityCfg = SceneEntityCfg("trocar_2"), + use_sparse_reward: bool = True, + print_log: bool = False, +) -> torch.Tensor: + """Reward for inserting trocar_2 into trocar_1 (Stage 2: Pushing in). + + Reward based on: + 1. Orientation alignment (parallelism) + 2. Center distance (pushing in) + + Only active in Stage 2. Once completed (stage >= 3), this reward is locked at the achieved value. + + Args: + env: Environment instance + angle_std: Standard deviation for angle reward shaping + angle_threshold: Angle threshold for parallelism (radians) + center_dist_std: Standard deviation for center distance reward shaping + asset_cfg1: Configuration for trocar 1 + asset_cfg2: Configuration for trocar 2 + use_sparse_reward: If True, only give reward (1.0) when stage >= 3. + If False (default), give continuous reward based on alignment and distance. + print_log: If True, print debug information. + Returns: + torch.Tensor: Reward tensor (num_envs,) + """ + s = get_assemble_trocar_state(env) + stage = s.task_stage + + obj1: RigidObject = env.scene[asset_cfg1.name] + obj2: RigidObject = env.scene[asset_cfg2.name] + + pos1 = obj1.data.root_pos_w.torch + quat1 = obj1.data.root_quat_w.torch + pos2 = obj2.data.root_pos_w.torch + quat2 = obj2.data.root_quat_w.torch + center_dist = torch.norm(pos1 - pos2, dim=-1) + + target_axis1 = torch.tensor([0.0, 0.0, -1.0], device=env.device).repeat(env.num_envs, 1) + target_axis2 = torch.tensor([0.0, 0.0, -1.0], device=env.device).repeat(env.num_envs, 1) + + axis1 = quat_apply(quat1, target_axis1) + axis2 = quat_apply(quat2, target_axis2) + + dot_prod = torch.sum(axis1 * axis2, dim=-1) + abs_dot = torch.clamp(torch.abs(dot_prod), max=1.0) + angle = torch.acos(abs_dot) + is_parallel = angle < angle_threshold + + if use_sparse_reward: + stage_just_completed = (s.prev_stage_insert == 2) & (stage >= 3) + reward = torch.where( + stage_just_completed, + torch.ones(env.num_envs, device=env.device) / env.step_dt, + torch.zeros(env.num_envs, device=env.device), + ) + s.prev_stage_insert = stage.clone() + else: + excess_angle = torch.clamp(angle - angle_threshold, min=0.0) + align_reward = torch.exp(-torch.square(excess_angle) / (2 * angle_std**2)) + center_reward = torch.exp(-torch.square(center_dist) / (2 * center_dist_std**2)) + center_reward = torch.where(is_parallel, center_reward, torch.zeros_like(center_reward)) + insertion_reward = align_reward * center_reward + + s.insertion_reward_locked = torch.where( + (stage >= 3) & (s.insertion_reward_locked == 0), + insertion_reward, + s.insertion_reward_locked, + ) + reward = torch.where( + stage < 2, + torch.zeros(env.num_envs, device=env.device), + torch.where(stage == 2, insertion_reward, s.insertion_reward_locked), + ) + + # Debug info + if should_print_debug(env, print_log=print_log) and stage[0].item() == 2: + mode_str = "Sparse" if use_sparse_reward else "Dense" + logger.debug( + " Stage 2 (Push In, %s): angle=%.3f | center_d=%.4f | is_parallel=%s | reward=%.3f", + mode_str, + angle[0].item(), + center_dist[0].item(), + is_parallel[0].item(), + reward[0].item(), + ) + + return reward + + +def trocar_placement_reward( + env: ManagerBasedRLEnv, + x_min: float = -1.8, + x_max: float = -1.4, + y_min: float = 1.5, + y_max: float = 1.8, + z_min: float = 0.9, + asset_cfg1: SceneEntityCfg = SceneEntityCfg("trocar_1"), + asset_cfg2: SceneEntityCfg = SceneEntityCfg("trocar_2"), + use_sparse_reward: bool = True, + print_log: bool = False, +) -> torch.Tensor: + """Reward for placing both trocars in the target tray region (Stage 3). + + Only active in Stage 3. Once completed (stage >= 4), this reward is locked at the achieved value. + + Args: + env: Environment instance + x_min, x_max: X bounds of target zone (relative to env origin) + y_min, y_max: Y bounds of target zone (relative to env origin) + z_min: Z threshold (below this is considered placed) + asset_cfg1: Configuration for trocar 1 + asset_cfg2: Configuration for trocar 2 + use_sparse_reward: If True, only give reward (1.0) when stage >= 4. + If False (default), give continuous reward based on placement status. + print_log: If True, print debug information. + + Returns: + torch.Tensor: Reward tensor (num_envs,) + """ + s = get_assemble_trocar_state(env) + stage = s.task_stage + + obj1: RigidObject = env.scene[asset_cfg1.name] + obj2: RigidObject = env.scene[asset_cfg2.name] + + pos1 = obj1.data.root_pos_w.torch + pos2 = obj2.data.root_pos_w.torch + env_origins = env.scene.env_origins + + curr_x_min = env_origins[:, 0] + min(x_min, x_max) + curr_x_max = env_origins[:, 0] + max(x_min, x_max) + curr_y_min = env_origins[:, 1] + min(y_min, y_max) + curr_y_max = env_origins[:, 1] + max(y_min, y_max) + + in_zone_1 = ( + (pos1[:, 0] >= curr_x_min) + & (pos1[:, 0] <= curr_x_max) + & (pos1[:, 1] >= curr_y_min) + & (pos1[:, 1] <= curr_y_max) + & (pos1[:, 2] < z_min) + ) + in_zone_2 = ( + (pos2[:, 0] >= curr_x_min) + & (pos2[:, 0] <= curr_x_max) + & (pos2[:, 1] >= curr_y_min) + & (pos2[:, 1] <= curr_y_max) + & (pos2[:, 2] < z_min) + ) + both_in_zone = in_zone_1 & in_zone_2 + + if use_sparse_reward: + stage_just_completed = (s.prev_stage_place == 3) & (stage >= 4) + reward = torch.where( + stage_just_completed, + torch.ones(env.num_envs, device=env.device) / env.step_dt, + torch.zeros(env.num_envs, device=env.device), + ) + s.prev_stage_place = stage.clone() + else: + placement_reward = both_in_zone.float() + s.placement_reward_locked = torch.where( + (stage >= 4) & (s.placement_reward_locked == 0), + placement_reward, + s.placement_reward_locked, + ) + reward = torch.where( + stage < 3, + torch.zeros(env.num_envs, device=env.device), + torch.where(stage == 3, placement_reward, s.placement_reward_locked), + ) + + # Debug info + if should_print_debug(env, print_log=print_log) and stage[0].item() == 3: + mode_str = "Sparse" if use_sparse_reward else "Dense" + logger.debug( + " Stage 3 (Placement, %s): in_zone=%s | z1=%.3f | z2=%.3f", + mode_str, + both_in_zone[0].item(), + pos1[0, 2], + pos2[0, 2], + ) + + return reward diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/assemble_trocar/mdp/terminations.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/assemble_trocar/mdp/terminations.py new file mode 100644 index 000000000000..12b70ae473bd --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/assemble_trocar/mdp/terminations.py @@ -0,0 +1,80 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import logging +from typing import TYPE_CHECKING + +import torch + +from isaaclab.assets import RigidObject +from isaaclab.managers import SceneEntityCfg + +from .rewards import get_task_stage + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + +logger = logging.getLogger(__name__) + + +def object_drop_termination( + env: ManagerBasedRLEnv, + drop_height_threshold: float = 0.5, + asset_cfg1: SceneEntityCfg = SceneEntityCfg("trocar_1"), + asset_cfg2: SceneEntityCfg = SceneEntityCfg("trocar_2"), + print_log: bool = False, +) -> torch.Tensor: + """Termination function that triggers when objects drop below threshold. + + This can be used as an alternative to auto-reset, marking the episode as terminated + so the training framework handles the reset. + + Args: + env: The environment instance + drop_height_threshold: Height below which objects are considered dropped + asset_cfg1: Configuration for first trocar + asset_cfg2: Configuration for second trocar + print_log: If True, print debug information. + Returns: + Boolean tensor indicating which environments should terminate due to drops + """ + # Get rigid objects + obj1: RigidObject = env.scene[asset_cfg1.name] + obj2: RigidObject = env.scene[asset_cfg2.name] + + # Get positions + pos1 = obj1.data.root_pos_w.torch + pos2 = obj2.data.root_pos_w.torch + # Check if either object has dropped + dropped_1 = pos1[:, 2] < drop_height_threshold + dropped_2 = pos2[:, 2] < drop_height_threshold + + dropped = dropped_1 | dropped_2 + + if print_log and dropped.any(): + logger.debug("Drop termination triggered for %d environment(s)", dropped.sum().item()) + + return dropped + + +def task_success_termination( + env: ManagerBasedRLEnv, + success_stage: int = 4, + print_log: bool = False, +) -> torch.Tensor: + """Termination condition: task is complete when stage reaches 4. + + Returns: + torch.Tensor: Boolean tensor indicating which environments should terminate (num_envs,) + """ + stage = get_task_stage(env) + task_complete = stage >= success_stage + + if print_log and task_complete.any(): + logger.info("Task completed in %d environment(s)!", task_complete.sum().item()) + + return task_complete diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/agents/rsl_rl_ppo_cfg.py index 0ccb4787cdd6..aad72a2d8a69 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/franka/agents/rsl_rl_ppo_cfg.py @@ -5,7 +5,7 @@ from isaaclab.utils import configclass -from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg +from isaaclab_rl.rsl_rl import RslRlMLPModelCfg, RslRlOnPolicyRunnerCfg, RslRlPpoAlgorithmCfg @configclass @@ -14,13 +14,16 @@ class CabinetPPORunnerCfg(RslRlOnPolicyRunnerCfg): max_iterations = 400 save_interval = 50 experiment_name = "franka_open_drawer" - policy = RslRlPpoActorCriticCfg( - init_noise_std=1.0, - actor_obs_normalization=False, - critic_obs_normalization=False, - actor_hidden_dims=[256, 128, 64], - critic_hidden_dims=[256, 128, 64], + actor = RslRlMLPModelCfg( + hidden_dims=[256, 128, 64], activation="elu", + obs_normalization=False, + distribution_cfg=RslRlMLPModelCfg.GaussianDistributionCfg(init_std=1.0), + ) + critic = RslRlMLPModelCfg( + hidden_dims=[256, 128, 64], + activation="elu", + obs_normalization=False, ) algorithm = RslRlPpoAlgorithmCfg( value_loss_coef=1.0, diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/openarm/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/openarm/agents/rsl_rl_ppo_cfg.py index 67f3498c361d..8dfd24624513 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/openarm/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/cabinet/config/openarm/agents/rsl_rl_ppo_cfg.py @@ -5,7 +5,7 @@ from isaaclab.utils import configclass -from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg +from isaaclab_rl.rsl_rl import RslRlMLPModelCfg, RslRlOnPolicyRunnerCfg, RslRlPpoAlgorithmCfg @configclass @@ -14,12 +14,16 @@ class OpenArmCabinetPPORunnerCfg(RslRlOnPolicyRunnerCfg): max_iterations = 600 save_interval = 50 experiment_name = "openarm_open_drawer" - empirical_normalization = False - policy = RslRlPpoActorCriticCfg( - init_noise_std=1.0, - actor_hidden_dims=[256, 128, 64], - critic_hidden_dims=[256, 128, 64], + actor = RslRlMLPModelCfg( + hidden_dims=[256, 128, 64], activation="elu", + obs_normalization=False, + distribution_cfg=RslRlMLPModelCfg.GaussianDistributionCfg(init_std=1.0), + ) + critic = RslRlMLPModelCfg( + hidden_dims=[256, 128, 64], + activation="elu", + obs_normalization=False, ) algorithm = RslRlPpoAlgorithmCfg( value_loss_coef=1.0, diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/gear_assembly/config/rizon_4s/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/gear_assembly/config/rizon_4s/agents/rsl_rl_ppo_cfg.py index 06f64e731afc..20e68ba87fdf 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/gear_assembly/config/rizon_4s/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/gear_assembly/config/rizon_4s/agents/rsl_rl_ppo_cfg.py @@ -5,7 +5,7 @@ from isaaclab.utils import configclass -from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticRecurrentCfg, RslRlPpoAlgorithmCfg +from isaaclab_rl.rsl_rl import RslRlMLPModelCfg, RslRlOnPolicyRunnerCfg, RslRlPpoAlgorithmCfg, RslRlRNNModelCfg @configclass @@ -17,18 +17,22 @@ class Rizon4sGearAssemblyRNNPPORunnerCfg(RslRlOnPolicyRunnerCfg): clip_actions = 1.0 resume = False obs_groups = { - "policy": ["policy"], + "actor": ["policy"], "critic": ["critic"], } - policy = RslRlPpoActorCriticRecurrentCfg( - state_dependent_std=True, - init_noise_std=1.0, - actor_obs_normalization=True, - critic_obs_normalization=True, - actor_hidden_dims=[256, 128, 64], - critic_hidden_dims=[256, 128, 64], - noise_std_type="log", + actor = RslRlRNNModelCfg( + hidden_dims=[256, 128, 64], activation="elu", + obs_normalization=True, + distribution_cfg=RslRlMLPModelCfg.HeteroscedasticGaussianDistributionCfg(init_std=1.0, std_type="log"), + rnn_type="lstm", + rnn_hidden_dim=256, + rnn_num_layers=2, + ) + critic = RslRlRNNModelCfg( + hidden_dims=[256, 128, 64], + activation="elu", + obs_normalization=True, rnn_type="lstm", rnn_hidden_dim=256, rnn_num_layers=2, diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/gear_assembly/config/ur_10e/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/gear_assembly/config/ur_10e/agents/rsl_rl_ppo_cfg.py index ac1ecba8463d..d49d02f5990f 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/gear_assembly/config/ur_10e/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/gear_assembly/config/ur_10e/agents/rsl_rl_ppo_cfg.py @@ -5,7 +5,7 @@ from isaaclab.utils import configclass -from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticRecurrentCfg, RslRlPpoAlgorithmCfg +from isaaclab_rl.rsl_rl import RslRlMLPModelCfg, RslRlOnPolicyRunnerCfg, RslRlPpoAlgorithmCfg, RslRlRNNModelCfg @configclass @@ -17,18 +17,22 @@ class UR10GearAssemblyRNNPPORunnerCfg(RslRlOnPolicyRunnerCfg): clip_actions = 1.0 resume = False obs_groups = { - "policy": ["policy"], + "actor": ["policy"], "critic": ["critic"], } - policy = RslRlPpoActorCriticRecurrentCfg( - state_dependent_std=True, - init_noise_std=1.0, - actor_obs_normalization=True, - critic_obs_normalization=True, - actor_hidden_dims=[256, 128, 64], - critic_hidden_dims=[256, 128, 64], - noise_std_type="log", + actor = RslRlRNNModelCfg( + hidden_dims=[256, 128, 64], activation="elu", + obs_normalization=True, + distribution_cfg=RslRlMLPModelCfg.HeteroscedasticGaussianDistributionCfg(init_std=1.0, std_type="log"), + rnn_type="lstm", + rnn_hidden_dim=256, + rnn_num_layers=2, + ) + critic = RslRlRNNModelCfg( + hidden_dims=[256, 128, 64], + activation="elu", + obs_normalization=True, rnn_type="lstm", rnn_hidden_dim=256, rnn_num_layers=2, diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/mdp/events.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/mdp/events.py index b651a002966e..0156d486d1f0 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/mdp/events.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/mdp/events.py @@ -11,7 +11,6 @@ from typing import TYPE_CHECKING import torch -import warp as wp import isaaclab.utils.math as math_utils from isaaclab.managers import EventTermCfg, ManagerTermBase, SceneEntityCfg @@ -327,9 +326,11 @@ def __call__( if torch.all(pos_error_norm < pos_threshold) and torch.all(rot_error_norm < rot_threshold): break - # Solve IK using jacobian - jacobians = wp.to_torch(self.robot_asset.root_view.get_jacobians()).clone() - jacobian = jacobians[env_ids, self.jacobi_body_idx, :, :] + # Solve IK using jacobian. ``body_link_jacobian_w`` prepends ``num_base_dofs`` + # floating-base columns on the DoF axis (0 for fixed-base, 6 for floating-base); + # slice past them so the column axis aligns with the actuated-joint state. + jacobians = self.robot_asset.data.body_link_jacobian_w.torch.clone() + jacobian = jacobians[env_ids, self.jacobi_body_idx, :, self.robot_asset.num_base_dofs :] delta_dof_pos = fc._get_delta_dof_pos( delta_pose=delta_hand_pose, diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/reach/config/rizon_4s/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/reach/config/rizon_4s/agents/rsl_rl_ppo_cfg.py index cb37cb700181..85f2b38e3336 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/reach/config/rizon_4s/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/reach/config/rizon_4s/agents/rsl_rl_ppo_cfg.py @@ -5,7 +5,7 @@ from isaaclab.utils import configclass -from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg +from isaaclab_rl.rsl_rl import RslRlMLPModelCfg, RslRlOnPolicyRunnerCfg, RslRlPpoAlgorithmCfg @configclass @@ -14,13 +14,17 @@ class Rizon4sReachPPORunnerCfg(RslRlOnPolicyRunnerCfg): max_iterations = 1500 save_interval = 50 experiment_name = "reach_rizon4s" - empirical_normalization = True - obs_groups = {"policy": ["policy"], "critic": ["policy"]} - policy = RslRlPpoActorCriticCfg( - init_noise_std=1.0, - actor_hidden_dims=[256, 128, 64], - critic_hidden_dims=[256, 128, 64], + obs_groups = {"actor": ["policy"], "critic": ["policy"]} + actor = RslRlMLPModelCfg( + hidden_dims=[256, 128, 64], activation="elu", + obs_normalization=True, + distribution_cfg=RslRlMLPModelCfg.GaussianDistributionCfg(init_std=1.0), + ) + critic = RslRlMLPModelCfg( + hidden_dims=[256, 128, 64], + activation="elu", + obs_normalization=True, ) algorithm = RslRlPpoAlgorithmCfg( value_loss_coef=1.0, diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/reach/config/ur_10e/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/reach/config/ur_10e/agents/rsl_rl_ppo_cfg.py index 02af65eec9a0..2e5a4e8dfca9 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/reach/config/ur_10e/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/deploy/reach/config/ur_10e/agents/rsl_rl_ppo_cfg.py @@ -5,7 +5,7 @@ from isaaclab.utils import configclass -from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg +from isaaclab_rl.rsl_rl import RslRlMLPModelCfg, RslRlOnPolicyRunnerCfg, RslRlPpoAlgorithmCfg @configclass @@ -14,13 +14,17 @@ class URReachPPORunnerCfg(RslRlOnPolicyRunnerCfg): max_iterations = 1500 save_interval = 50 experiment_name = "reach_ur10e" - empirical_normalization = True - obs_groups = {"policy": ["policy"], "critic": ["policy"]} - policy = RslRlPpoActorCriticCfg( - init_noise_std=1.0, - actor_hidden_dims=[256, 128, 64], - critic_hidden_dims=[256, 128, 64], + obs_groups = {"actor": ["policy"], "critic": ["policy"]} + actor = RslRlMLPModelCfg( + hidden_dims=[256, 128, 64], activation="elu", + obs_normalization=True, + distribution_cfg=RslRlMLPModelCfg.GaussianDistributionCfg(init_std=1.0), + ) + critic = RslRlMLPModelCfg( + hidden_dims=[256, 128, 64], + activation="elu", + obs_normalization=True, ) algorithm = RslRlPpoAlgorithmCfg( value_loss_coef=1.0, diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/config/kuka_allegro/dexsuite_kuka_allegro_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/config/kuka_allegro/dexsuite_kuka_allegro_env_cfg.py index 4492d197f763..6513e8d7daa3 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/config/kuka_allegro/dexsuite_kuka_allegro_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/config/kuka_allegro/dexsuite_kuka_allegro_env_cfg.py @@ -3,9 +3,6 @@ # # SPDX-License-Identifier: BSD-3-Clause -from isaaclab_newton.physics import MJWarpSolverCfg, NewtonCfg -from isaaclab_physx.physics import PhysxCfg - from isaaclab.assets import ArticulationCfg from isaaclab.managers import RewardTermCfg as RewTerm from isaaclab.managers import SceneEntityCfg @@ -31,34 +28,6 @@ FINGER_SENSORS = [f"{name}_object_s" for name in FINGERTIP_LIST if name != "thumb_link_3"] -@configclass -class KukaAllegroPhysicsCfg(PresetCfg): - default = PhysxCfg( - bounce_threshold_velocity=0.01, - gpu_max_rigid_patch_count=4 * 5 * 2**15, - gpu_found_lost_pairs_capacity=2**26, - ) - newton_mjwarp = NewtonCfg( - solver_cfg=MJWarpSolverCfg( - solver="newton", - integrator="implicitfast", - njmax=300, - nconmax=70, - impratio=10.0, - cone="elliptic", - update_data_interval=2, - iterations=100, - ls_iterations=15, - ls_parallel=False, - use_mujoco_contacts=True, - ccd_iterations=5000, - ), - num_substeps=2, - debug_mode=False, - ) - physx = default - - @configclass class KukaAllegroSceneCfg(PresetCfg): @configclass @@ -132,28 +101,15 @@ class KukaAllegroObservationCfg(PresetCfg): default = state -@configclass -class KukaAllegroEventCfg(PresetCfg): - @configclass - class KukaAllegroPhysxEventCfg(dexsuite.StartupEventCfg, dexsuite.EventCfg): - pass - - default = KukaAllegroPhysxEventCfg() - newton_mjwarp = dexsuite.EventCfg() - physx = default - - @configclass class KukaAllegroMixinCfg: scene: KukaAllegroSceneCfg = KukaAllegroSceneCfg() rewards: KukaAllegroReorientRewardCfg = KukaAllegroReorientRewardCfg() observations: KukaAllegroObservationCfg = KukaAllegroObservationCfg() - events: KukaAllegroEventCfg = KukaAllegroEventCfg() actions: KukaAllegroRelJointPosActionCfg = KukaAllegroRelJointPosActionCfg() def __post_init__(self): super().__post_init__() - self.sim.physics = KukaAllegroPhysicsCfg() @configclass diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/dexsuite_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/dexsuite_env_cfg.py index 449043e3977b..9f12f5b3b0b9 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/dexsuite_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/dexsuite_env_cfg.py @@ -5,6 +5,7 @@ from dataclasses import MISSING +from isaaclab_newton.physics import MJWarpSolverCfg, NewtonCfg, NewtonCollisionPipelineCfg, NewtonShapeCfg from isaaclab_physx.physics import PhysxCfg import isaaclab.sim as sim_utils @@ -18,7 +19,7 @@ from isaaclab.managers import TerminationTermCfg as DoneTerm from isaaclab.markers import VisualizationMarkersCfg from isaaclab.scene import InteractiveSceneCfg -from isaaclab.sim import CapsuleCfg, ConeCfg, CuboidCfg, RigidBodyMaterialCfg, SphereCfg +from isaaclab.sim import MeshCapsuleCfg, MeshConeCfg, MeshCuboidCfg, MeshSphereCfg, RigidBodyMaterialCfg from isaaclab.utils import configclass from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR from isaaclab.utils.noise import UniformNoiseCfg as Unoise @@ -37,26 +38,32 @@ ) +OBJECT_PHYSICS = { + "physics_material": RigidBodyMaterialCfg(static_friction=0.5), + "collision_props": sim_utils.CollisionPropertiesCfg(contact_offset=0.002), +} + + @configclass class ObjectCfg(PresetCfg): shapes = sim_utils.MultiAssetSpawnerCfg( assets_cfg=[ - CuboidCfg(size=(0.05, 0.1, 0.1), physics_material=RigidBodyMaterialCfg(static_friction=0.5)), - CuboidCfg(size=(0.05, 0.05, 0.1), physics_material=RigidBodyMaterialCfg(static_friction=0.5)), - CuboidCfg(size=(0.025, 0.1, 0.1), physics_material=RigidBodyMaterialCfg(static_friction=0.5)), - CuboidCfg(size=(0.025, 0.05, 0.1), physics_material=RigidBodyMaterialCfg(static_friction=0.5)), - CuboidCfg(size=(0.025, 0.025, 0.1), physics_material=RigidBodyMaterialCfg(static_friction=0.5)), - CuboidCfg(size=(0.01, 0.1, 0.1), physics_material=RigidBodyMaterialCfg(static_friction=0.5)), - SphereCfg(radius=0.05, physics_material=RigidBodyMaterialCfg(static_friction=0.5)), - SphereCfg(radius=0.025, physics_material=RigidBodyMaterialCfg(static_friction=0.5)), - CapsuleCfg(radius=0.04, height=0.025, physics_material=RigidBodyMaterialCfg(static_friction=0.5)), - CapsuleCfg(radius=0.04, height=0.01, physics_material=RigidBodyMaterialCfg(static_friction=0.5)), - CapsuleCfg(radius=0.04, height=0.1, physics_material=RigidBodyMaterialCfg(static_friction=0.5)), - CapsuleCfg(radius=0.025, height=0.1, physics_material=RigidBodyMaterialCfg(static_friction=0.5)), - CapsuleCfg(radius=0.025, height=0.2, physics_material=RigidBodyMaterialCfg(static_friction=0.5)), - CapsuleCfg(radius=0.01, height=0.2, physics_material=RigidBodyMaterialCfg(static_friction=0.5)), - ConeCfg(radius=0.05, height=0.1, physics_material=RigidBodyMaterialCfg(static_friction=0.5)), - ConeCfg(radius=0.025, height=0.1, physics_material=RigidBodyMaterialCfg(static_friction=0.5)), + MeshCuboidCfg(size=(0.05, 0.1, 0.1), **OBJECT_PHYSICS), + MeshCuboidCfg(size=(0.05, 0.05, 0.1), **OBJECT_PHYSICS), + MeshCuboidCfg(size=(0.025, 0.1, 0.1), **OBJECT_PHYSICS), + MeshCuboidCfg(size=(0.025, 0.05, 0.1), **OBJECT_PHYSICS), + MeshCuboidCfg(size=(0.025, 0.025, 0.1), **OBJECT_PHYSICS), + MeshCuboidCfg(size=(0.01, 0.1, 0.1), **OBJECT_PHYSICS), + MeshSphereCfg(radius=0.05, **OBJECT_PHYSICS), + MeshSphereCfg(radius=0.025, **OBJECT_PHYSICS), + MeshCapsuleCfg(radius=0.04, height=0.025, **OBJECT_PHYSICS), + MeshCapsuleCfg(radius=0.04, height=0.01, **OBJECT_PHYSICS), + MeshCapsuleCfg(radius=0.04, height=0.1, **OBJECT_PHYSICS), + MeshCapsuleCfg(radius=0.025, height=0.1, **OBJECT_PHYSICS), + MeshCapsuleCfg(radius=0.025, height=0.2, **OBJECT_PHYSICS), + MeshCapsuleCfg(radius=0.01, height=0.2, **OBJECT_PHYSICS), + MeshConeCfg(radius=0.05, height=0.1, **OBJECT_PHYSICS), + MeshConeCfg(radius=0.025, height=0.1, **OBJECT_PHYSICS), ], rigid_props=sim_utils.RigidBodyPropertiesCfg( solver_position_iteration_count=16, @@ -77,7 +84,6 @@ class ObjectCfg(PresetCfg): collision_props=sim_utils.CollisionPropertiesCfg(), mass_props=sim_utils.MassPropertiesCfg(mass=0.2), ) - newton_mjwarp = cube # newton does not support multi-asset spawning yet default = shapes @@ -218,8 +224,8 @@ def __post_init__(self): @configclass -class StartupEventCfg: - """Startup-mode domain randomization (PhysX only — Newton does not support startup events).""" +class EventCfg: + """Reset-mode events (shared by all physics backends).""" robot_physics_material = EventTerm( func=mdp.randomize_rigid_body_material, @@ -245,6 +251,17 @@ class StartupEventCfg: }, ) + object_physics_inertia = EventTerm( + func=mdp.randomize_rigid_body_inertia, + mode="startup", + params={ + "asset_cfg": SceneEntityCfg("object"), + "inertia_distribution_params": [0.01, 0.01], + "operation": "add", + "diagonal_only": True, + }, + ) + joint_stiffness_and_damping = EventTerm( func=mdp.randomize_actuator_gains, mode="startup", @@ -276,11 +293,6 @@ class StartupEventCfg: }, ) - -@configclass -class EventCfg: - """Reset-mode events (shared by all physics backends).""" - # Gravity scheduling is a deliberate curriculum trick — starting with no # gravity (easy) and gradually introducing full gravity (hard) makes learning # smoother and removes the need for a separate "Lift" reward. @@ -409,6 +421,36 @@ class TerminationsCfg: abnormal_robot = DoneTerm(func=mdp.abnormal_robot_state) +@configclass +class PhysicsCfg(PresetCfg): + default = PhysxCfg( + bounce_threshold_velocity=0.01, + gpu_max_rigid_patch_count=4 * 5 * 2**15, + gpu_found_lost_pairs_capacity=2**26, + ) + newton_mjwarp = NewtonCfg( + solver_cfg=MJWarpSolverCfg( + solver="newton", + integrator="implicitfast", + njmax=300, + nconmax=200, + impratio=10.0, + cone="elliptic", + update_data_interval=2, + iterations=100, + ls_iterations=15, + ls_parallel=False, + use_mujoco_contacts=False, + ccd_iterations=35, + ), + collision_cfg=NewtonCollisionPipelineCfg(), + default_shape_cfg=NewtonShapeCfg(), + num_substeps=2, + debug_mode=False, + ) + physx = default + + @configclass class DexsuiteReorientEnvCfg(ManagerBasedEnvCfg): """Dexsuite reorientation task definition, also the base definition for derivative Lift task and evaluation task""" @@ -423,19 +465,11 @@ class DexsuiteReorientEnvCfg(ManagerBasedEnvCfg): # MDP settings rewards: RewardsCfg = RewardsCfg() terminations: TerminationsCfg = TerminationsCfg() - events: EventCfg = MISSING # type: ignore + events: EventCfg = EventCfg() curriculum: CurriculumCfg | None = CurriculumCfg() def validate_config(self): """Check for invalid preset combinations after resolution.""" - is_newton = not isinstance(self.sim.physics, PhysxCfg) - is_multi_asset = isinstance(self.scene.object.spawn, sim_utils.MultiAssetSpawnerCfg) - - if is_newton and is_multi_asset: - raise ValueError( - "Newton physics does not support multi-asset spawning." - " Use a single-geometry object preset (e.g. presets=cube) instead of 'shapes'." - ) warp_supported = {"rgb", "depth", "distance_to_image_plane"} for cam_attr in ("base_camera", "wrist_camera"): @@ -466,11 +500,7 @@ def __post_init__(self): # simulation settings self.sim.dt = 1 / 120 self.sim.render_interval = self.decimation - self.sim.physics = PhysxCfg( - bounce_threshold_velocity=0.01, - gpu_max_rigid_patch_count=4 * 5 * 2**15, - gpu_found_lost_pairs_capacity=2**26, - ) + self.sim.physics = PhysicsCfg() class DexsuiteLiftEnvCfg(DexsuiteReorientEnvCfg): diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/mdp/rewards.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/mdp/rewards.py index 8a5a013ea846..9e8b084afbae 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/mdp/rewards.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/mdp/rewards.py @@ -219,8 +219,7 @@ def orientation_command_error_tanh( obj: RigidObject = env.scene[align_asset_cfg.name] command = env.command_manager.get_command(command_name) des_quat_b = command[:, 3:7] - root_state = asset.data.root_state_w.torch - des_quat_w = math_utils.quat_mul(root_state[:, 3:7], des_quat_b) + des_quat_w = math_utils.quat_mul(asset.data.root_link_quat_w.torch, des_quat_b) quat_distance = math_utils.quat_error_magnitude(obj.data.root_quat_w.torch, des_quat_w) return (1 - torch.tanh(quat_distance / std)) * contacts(env, contact_threshold, thumb_name, finger_names).float() diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/config/allegro_hand/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/config/allegro_hand/agents/rsl_rl_ppo_cfg.py index b1d3d4be1756..ac773b10af3a 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/config/allegro_hand/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/inhand/config/allegro_hand/agents/rsl_rl_ppo_cfg.py @@ -5,7 +5,7 @@ from isaaclab.utils import configclass -from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg +from isaaclab_rl.rsl_rl import RslRlMLPModelCfg, RslRlOnPolicyRunnerCfg, RslRlPpoAlgorithmCfg @configclass @@ -14,13 +14,16 @@ class AllegroCubePPORunnerCfg(RslRlOnPolicyRunnerCfg): max_iterations = 5000 save_interval = 50 experiment_name = "allegro_cube" - policy = RslRlPpoActorCriticCfg( - init_noise_std=1.0, - actor_obs_normalization=True, - critic_obs_normalization=True, - actor_hidden_dims=[512, 256, 128], - critic_hidden_dims=[512, 256, 128], + actor = RslRlMLPModelCfg( + hidden_dims=[512, 256, 128], activation="elu", + obs_normalization=True, + distribution_cfg=RslRlMLPModelCfg.GaussianDistributionCfg(init_std=1.0), + ) + critic = RslRlMLPModelCfg( + hidden_dims=[512, 256, 128], + activation="elu", + obs_normalization=True, ) algorithm = RslRlPpoAlgorithmCfg( value_loss_coef=1.0, diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/franka/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/franka/agents/rsl_rl_ppo_cfg.py index 7a94614d8c97..c5010a9577df 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/franka/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/franka/agents/rsl_rl_ppo_cfg.py @@ -5,7 +5,7 @@ from isaaclab.utils import configclass -from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg +from isaaclab_rl.rsl_rl import RslRlMLPModelCfg, RslRlOnPolicyRunnerCfg, RslRlPpoAlgorithmCfg @configclass @@ -14,13 +14,16 @@ class LiftCubePPORunnerCfg(RslRlOnPolicyRunnerCfg): max_iterations = 1500 save_interval = 50 experiment_name = "franka_lift" - policy = RslRlPpoActorCriticCfg( - init_noise_std=1.0, - actor_obs_normalization=False, - critic_obs_normalization=False, - actor_hidden_dims=[256, 128, 64], - critic_hidden_dims=[256, 128, 64], + actor = RslRlMLPModelCfg( + hidden_dims=[256, 128, 64], activation="elu", + obs_normalization=False, + distribution_cfg=RslRlMLPModelCfg.GaussianDistributionCfg(init_std=1.0), + ) + critic = RslRlMLPModelCfg( + hidden_dims=[256, 128, 64], + activation="elu", + obs_normalization=False, ) algorithm = RslRlPpoAlgorithmCfg( value_loss_coef=1.0, diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/openarm/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/openarm/agents/rsl_rl_ppo_cfg.py index f079552f1f4a..fb962b9cd98f 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/openarm/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift/config/openarm/agents/rsl_rl_ppo_cfg.py @@ -6,7 +6,7 @@ from isaaclab.utils import configclass -from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg +from isaaclab_rl.rsl_rl import RslRlMLPModelCfg, RslRlOnPolicyRunnerCfg, RslRlPpoAlgorithmCfg @configclass @@ -15,12 +15,16 @@ class OpenArmLiftCubePPORunnerCfg(RslRlOnPolicyRunnerCfg): max_iterations = 2000 save_interval = 50 experiment_name = "openarm_lift" - empirical_normalization = False - policy = RslRlPpoActorCriticCfg( - init_noise_std=1.0, - actor_hidden_dims=[256, 128, 64], - critic_hidden_dims=[256, 128, 64], + actor = RslRlMLPModelCfg( + hidden_dims=[256, 128, 64], activation="elu", + obs_normalization=False, + distribution_cfg=RslRlMLPModelCfg.GaussianDistributionCfg(init_std=1.0), + ) + critic = RslRlMLPModelCfg( + hidden_dims=[256, 128, 64], + activation="elu", + obs_normalization=False, ) algorithm = RslRlPpoAlgorithmCfg( value_loss_coef=1.0, diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/mdp/__init__.pyi b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/mdp/__init__.pyi index 1421a52bc546..de6c777b49c1 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/mdp/__init__.pyi +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/mdp/__init__.pyi @@ -4,7 +4,9 @@ # SPDX-License-Identifier: BSD-3-Clause __all__ = [ + "get_all_robot_link_pose", "get_all_robot_link_state", + "get_all_robot_link_velocity", "get_eef_pos", "get_eef_quat", "get_robot_joint_state", @@ -16,7 +18,9 @@ __all__ = [ ] from .observations import ( + get_all_robot_link_pose, get_all_robot_link_state, + get_all_robot_link_velocity, get_eef_pos, get_eef_quat, get_robot_joint_state, diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/mdp/observations.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/mdp/observations.py index c3e030b33ec0..cedc6507d78c 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/mdp/observations.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/mdp/observations.py @@ -5,6 +5,7 @@ from __future__ import annotations +import warnings from typing import TYPE_CHECKING import torch @@ -77,10 +78,34 @@ def get_robot_joint_state( return robot_joint_states +def get_all_robot_link_pose(env: ManagerBasedRLEnv) -> torch.Tensor: + """Robot link poses in the world frame. + + Returns: + Link poses with shape ``[num_envs, num_bodies, 7]`` and layout + ``[x, y, z, qx, qy, qz, qw]`` where position is in ``[m]``. + """ + return env.scene["robot"].data.body_link_pose_w.torch + + +def get_all_robot_link_velocity(env: ManagerBasedRLEnv) -> torch.Tensor: + """Robot link velocities in the world frame. + + Returns: + Link velocities with shape ``[num_envs, num_bodies, 6]`` and layout + ``[linear_velocity(3), angular_velocity(3)]`` in ``[m/s, rad/s]``. + """ + return env.scene["robot"].data.body_link_vel_w.torch + + def get_all_robot_link_state( env: ManagerBasedRLEnv, ) -> torch.Tensor: - body_pos_w = env.scene["robot"].data.body_link_state_w.torch[:, :, :] - all_robot_link_pos = body_pos_w - - return all_robot_link_pos + # TODO: Remove this compatibility helper in IsaacLab 4.0. + warnings.warn( + "`get_all_robot_link_state` is deprecated and will be removed in IsaacLab 4.0. " + "Use `get_all_robot_link_pose` and `get_all_robot_link_velocity` instead.", + DeprecationWarning, + stacklevel=2, + ) + return torch.cat((get_all_robot_link_pose(env), get_all_robot_link_velocity(env)), dim=-1) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/franka/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/franka/agents/rsl_rl_ppo_cfg.py index ede70559fd56..f523c5cc2207 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/franka/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/franka/agents/rsl_rl_ppo_cfg.py @@ -5,7 +5,7 @@ from isaaclab.utils import configclass -from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg +from isaaclab_rl.rsl_rl import RslRlMLPModelCfg, RslRlOnPolicyRunnerCfg, RslRlPpoAlgorithmCfg @configclass @@ -15,13 +15,16 @@ class FrankaReachPPORunnerCfg(RslRlOnPolicyRunnerCfg): save_interval = 50 experiment_name = "franka_reach" run_name = "" - policy = RslRlPpoActorCriticCfg( - init_noise_std=1.0, - actor_obs_normalization=False, - critic_obs_normalization=False, - actor_hidden_dims=[64, 64], - critic_hidden_dims=[64, 64], + actor = RslRlMLPModelCfg( + hidden_dims=[64, 64], activation="elu", + obs_normalization=False, + distribution_cfg=RslRlMLPModelCfg.GaussianDistributionCfg(init_std=1.0), + ) + critic = RslRlMLPModelCfg( + hidden_dims=[64, 64], + activation="elu", + obs_normalization=False, ) algorithm = RslRlPpoAlgorithmCfg( value_loss_coef=1.0, diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/franka/ik_abs_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/franka/ik_abs_env_cfg.py index e8e955718559..b090e568965e 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/franka/ik_abs_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/franka/ik_abs_env_cfg.py @@ -3,8 +3,6 @@ # # SPDX-License-Identifier: BSD-3-Clause -from isaaclab_physx.physics import PhysxCfg - from isaaclab.controllers.differential_ik_cfg import DifferentialIKControllerCfg from isaaclab.envs.mdp.actions.actions_cfg import DifferentialInverseKinematicsActionCfg from isaaclab.utils import configclass @@ -36,9 +34,6 @@ def __post_init__(self): body_offset=DifferentialInverseKinematicsActionCfg.OffsetCfg(pos=[0.0, 0.0, 0.107]), ) - # IK control is not supported with Newton physics; use PhysX only. - self.sim.physics = PhysxCfg(bounce_threshold_velocity=0.2) - @configclass class FrankaReachEnvCfg_PLAY(FrankaReachEnvCfg): diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/franka/ik_rel_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/franka/ik_rel_env_cfg.py index 488e92493289..024a42270d85 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/franka/ik_rel_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/franka/ik_rel_env_cfg.py @@ -3,8 +3,6 @@ # # SPDX-License-Identifier: BSD-3-Clause -from isaaclab_physx.physics import PhysxCfg - from isaaclab.controllers.differential_ik_cfg import DifferentialIKControllerCfg from isaaclab.envs.mdp.actions.actions_cfg import DifferentialInverseKinematicsActionCfg from isaaclab.utils import configclass @@ -37,9 +35,6 @@ def __post_init__(self): body_offset=DifferentialInverseKinematicsActionCfg.OffsetCfg(pos=[0.0, 0.0, 0.107]), ) - # IK control is not supported with Newton physics; use PhysX only. - self.sim.physics = PhysxCfg(bounce_threshold_velocity=0.2) - @configclass class FrankaReachEnvCfg_PLAY(FrankaReachEnvCfg): diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/franka/osc_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/franka/osc_env_cfg.py index cca92aa019bb..e612439fda70 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/franka/osc_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/franka/osc_env_cfg.py @@ -3,8 +3,6 @@ # # SPDX-License-Identifier: BSD-3-Clause -from isaaclab_physx.physics import PhysxCfg - from isaaclab.controllers.operational_space_cfg import OperationalSpaceControllerCfg from isaaclab.envs.mdp.actions.actions_cfg import OperationalSpaceControllerActionCfg from isaaclab.utils import configclass @@ -62,9 +60,6 @@ def __post_init__(self): self.observations.policy.joint_pos = None self.observations.policy.joint_vel = None - # OSC control is not supported with Newton physics; use PhysX only. - self.sim.physics = PhysxCfg(bounce_threshold_velocity=0.2) - @configclass class FrankaReachEnvCfg_PLAY(FrankaReachEnvCfg): diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/openarm/bimanual/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/openarm/bimanual/agents/rsl_rl_ppo_cfg.py index d1dd736a2ed7..0afc5496d059 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/openarm/bimanual/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/openarm/bimanual/agents/rsl_rl_ppo_cfg.py @@ -5,7 +5,7 @@ from isaaclab.utils import configclass -from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg +from isaaclab_rl.rsl_rl import RslRlMLPModelCfg, RslRlOnPolicyRunnerCfg, RslRlPpoAlgorithmCfg @configclass @@ -16,12 +16,16 @@ class OpenArmReachPPORunnerCfg(RslRlOnPolicyRunnerCfg): experiment_name = "openarm_bi_reach" run_name = "" resume = False - empirical_normalization = False - policy = RslRlPpoActorCriticCfg( - init_noise_std=1.0, - actor_hidden_dims=[64, 64], - critic_hidden_dims=[64, 64], + actor = RslRlMLPModelCfg( + hidden_dims=[64, 64], activation="elu", + obs_normalization=False, + distribution_cfg=RslRlMLPModelCfg.GaussianDistributionCfg(init_std=1.0), + ) + critic = RslRlMLPModelCfg( + hidden_dims=[64, 64], + activation="elu", + obs_normalization=False, ) algorithm = RslRlPpoAlgorithmCfg( value_loss_coef=1.0, diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/openarm/unimanual/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/openarm/unimanual/agents/rsl_rl_ppo_cfg.py index 4d43c3574195..9bfdf6530499 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/openarm/unimanual/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/openarm/unimanual/agents/rsl_rl_ppo_cfg.py @@ -6,7 +6,7 @@ from isaaclab.utils import configclass -from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg +from isaaclab_rl.rsl_rl import RslRlMLPModelCfg, RslRlOnPolicyRunnerCfg, RslRlPpoAlgorithmCfg @configclass @@ -17,12 +17,16 @@ class OpenArmReachPPORunnerCfg(RslRlOnPolicyRunnerCfg): experiment_name = "openarm_reach" run_name = "" resume = False - empirical_normalization = True - policy = RslRlPpoActorCriticCfg( - init_noise_std=1.0, - actor_hidden_dims=[64, 64], - critic_hidden_dims=[64, 64], + actor = RslRlMLPModelCfg( + hidden_dims=[64, 64], activation="elu", + obs_normalization=True, + distribution_cfg=RslRlMLPModelCfg.GaussianDistributionCfg(init_std=1.0), + ) + critic = RslRlMLPModelCfg( + hidden_dims=[64, 64], + activation="elu", + obs_normalization=True, ) algorithm = RslRlPpoAlgorithmCfg( value_loss_coef=1.0, diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/ur_10/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/ur_10/agents/rsl_rl_ppo_cfg.py index c445786c44c7..ef6ae64a45f5 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/ur_10/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/reach/config/ur_10/agents/rsl_rl_ppo_cfg.py @@ -5,7 +5,7 @@ from isaaclab.utils import configclass -from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg +from isaaclab_rl.rsl_rl import RslRlMLPModelCfg, RslRlOnPolicyRunnerCfg, RslRlPpoAlgorithmCfg @configclass @@ -15,13 +15,16 @@ class UR10ReachPPORunnerCfg(RslRlOnPolicyRunnerCfg): save_interval = 50 experiment_name = "reach_ur10" run_name = "" - policy = RslRlPpoActorCriticCfg( - init_noise_std=1.0, - actor_obs_normalization=False, - critic_obs_normalization=False, - actor_hidden_dims=[64, 64], - critic_hidden_dims=[64, 64], + actor = RslRlMLPModelCfg( + hidden_dims=[64, 64], activation="elu", + obs_normalization=False, + distribution_cfg=RslRlMLPModelCfg.GaussianDistributionCfg(init_std=1.0), + ) + critic = RslRlMLPModelCfg( + hidden_dims=[64, 64], + activation="elu", + obs_normalization=False, ) algorithm = RslRlPpoAlgorithmCfg( value_loss_coef=1.0, diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/navigation/config/anymal_c/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/navigation/config/anymal_c/agents/rsl_rl_ppo_cfg.py index 93ec98732f8c..c02c9b0eb0d0 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/navigation/config/anymal_c/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/navigation/config/anymal_c/agents/rsl_rl_ppo_cfg.py @@ -5,7 +5,7 @@ from isaaclab.utils import configclass -from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg +from isaaclab_rl.rsl_rl import RslRlMLPModelCfg, RslRlOnPolicyRunnerCfg, RslRlPpoAlgorithmCfg @configclass @@ -14,13 +14,16 @@ class NavigationEnvPPORunnerCfg(RslRlOnPolicyRunnerCfg): max_iterations = 1500 save_interval = 50 experiment_name = "anymal_c_navigation" - policy = RslRlPpoActorCriticCfg( - init_noise_std=0.5, - actor_obs_normalization=False, - critic_obs_normalization=False, - actor_hidden_dims=[128, 128], - critic_hidden_dims=[128, 128], + actor = RslRlMLPModelCfg( + hidden_dims=[128, 128], activation="elu", + obs_normalization=False, + distribution_cfg=RslRlMLPModelCfg.GaussianDistributionCfg(init_std=0.5), + ) + critic = RslRlMLPModelCfg( + hidden_dims=[128, 128], + activation="elu", + obs_normalization=False, ) algorithm = RslRlPpoAlgorithmCfg( value_loss_coef=1.0, diff --git a/source/isaaclab_tasks/test/golden_images/cartpole/newton-ovrtx_renderer-rgb.png b/source/isaaclab_tasks/test/golden_images/cartpole/newton-ovrtx_renderer-rgb.png index e47c06e2ca7c..f35e82ae6582 100644 --- a/source/isaaclab_tasks/test/golden_images/cartpole/newton-ovrtx_renderer-rgb.png +++ b/source/isaaclab_tasks/test/golden_images/cartpole/newton-ovrtx_renderer-rgb.png @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:4029eb71d2361c9fa12d255415bb9edcf1caaeb9d230ca2e6c4e67596c037dd1 -size 2580 +oid sha256:3d0e2d1f537f42cb34ed7a0616802193e3ae0bcef43fbc0dc0b015d8af8aa5c8 +size 2685 diff --git a/source/isaaclab_tasks/test/golden_images/cartpole/newton-ovrtx_renderer-rgba.png b/source/isaaclab_tasks/test/golden_images/cartpole/newton-ovrtx_renderer-rgba.png index 791497af827c..5a53a6f517a6 100644 --- a/source/isaaclab_tasks/test/golden_images/cartpole/newton-ovrtx_renderer-rgba.png +++ b/source/isaaclab_tasks/test/golden_images/cartpole/newton-ovrtx_renderer-rgba.png @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:4e1fed2c618875f9f9b4520c52308b0831cf637835e7a62e7a84e96f914c1e83 -size 2882 +oid sha256:d985f4de8667d57b0ba2f44b8181541463c888becb0f38c8716c65c343658dfb +size 2999 diff --git a/source/isaaclab_tasks/test/golden_images/cartpole/newton-ovrtx_renderer-simple_shading_constant_diffuse.png b/source/isaaclab_tasks/test/golden_images/cartpole/newton-ovrtx_renderer-simple_shading_constant_diffuse.png index 87104cb87161..583746565afb 100644 --- a/source/isaaclab_tasks/test/golden_images/cartpole/newton-ovrtx_renderer-simple_shading_constant_diffuse.png +++ b/source/isaaclab_tasks/test/golden_images/cartpole/newton-ovrtx_renderer-simple_shading_constant_diffuse.png @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:47b5b15d79d0b61d00c0538caa0012172753a481ad6efb45df2888402be2f407 +oid sha256:11db8a198a6ccae0a7cdbce0e996eb74eab1a13dca65bf0e590752a95389a3dd size 391 diff --git a/source/isaaclab_tasks/test/golden_images/cartpole/newton-ovrtx_renderer-simple_shading_diffuse_mdl.png b/source/isaaclab_tasks/test/golden_images/cartpole/newton-ovrtx_renderer-simple_shading_diffuse_mdl.png index 7d05e4a7adbd..b33b4e8fd830 100644 --- a/source/isaaclab_tasks/test/golden_images/cartpole/newton-ovrtx_renderer-simple_shading_diffuse_mdl.png +++ b/source/isaaclab_tasks/test/golden_images/cartpole/newton-ovrtx_renderer-simple_shading_diffuse_mdl.png @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:f2ba382c0804ea49b55fc5216c9f1e28c34d5cae95b33d9982fd55763df178cc -size 435 +oid sha256:bfce56fc89bb014ecc876c4302344085fd2ad1cd6685d2295141256c375b1fc3 +size 436 diff --git a/source/isaaclab_tasks/test/golden_images/cartpole/newton-ovrtx_renderer-simple_shading_full_mdl.png b/source/isaaclab_tasks/test/golden_images/cartpole/newton-ovrtx_renderer-simple_shading_full_mdl.png index 6b4f8389da06..27d490c4b23f 100644 --- a/source/isaaclab_tasks/test/golden_images/cartpole/newton-ovrtx_renderer-simple_shading_full_mdl.png +++ b/source/isaaclab_tasks/test/golden_images/cartpole/newton-ovrtx_renderer-simple_shading_full_mdl.png @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:d7af4ef2afca01d0bf4f9c069c5c3778fa07050bcd8091486541ab11f14e8227 -size 742 +oid sha256:f2b846bae771345dc6b5bea0c6148d3942a55cb012b079589ed7104a8357c9e2 +size 776 diff --git a/source/isaaclab_tasks/test/golden_images/dexsuite_kuka/newton-ovrtx_renderer-albedo.png b/source/isaaclab_tasks/test/golden_images/dexsuite_kuka/newton-ovrtx_renderer-albedo.png index 5199099a7587..8e51c396efae 100644 --- a/source/isaaclab_tasks/test/golden_images/dexsuite_kuka/newton-ovrtx_renderer-albedo.png +++ b/source/isaaclab_tasks/test/golden_images/dexsuite_kuka/newton-ovrtx_renderer-albedo.png @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:7cf76622f5f5cc7e7889fe6032ba4cda22516248fa7bb5e957f181c92c86b42b -size 3054 +oid sha256:04c9b2668a6e544403f00850a1d12f2cc5d661c4b2038a786607880ebc768cb9 +size 2768 diff --git a/source/isaaclab_tasks/test/golden_images/dexsuite_kuka/newton-ovrtx_renderer-rgb.png b/source/isaaclab_tasks/test/golden_images/dexsuite_kuka/newton-ovrtx_renderer-rgb.png index 544e2ffd450b..0395c5f3d11c 100644 --- a/source/isaaclab_tasks/test/golden_images/dexsuite_kuka/newton-ovrtx_renderer-rgb.png +++ b/source/isaaclab_tasks/test/golden_images/dexsuite_kuka/newton-ovrtx_renderer-rgb.png @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:3445142682c88dc5ce11c5d749c7754bf2d54bf0f1aab6420513a733bf3cf645 -size 14919 +oid sha256:4add42d2a43cad3e1bbc17d9f3e190fb6f480c4ace605de42dfdbccf5e02680d +size 14894 diff --git a/source/isaaclab_tasks/test/golden_images/dexsuite_kuka/newton-ovrtx_renderer-rgba.png b/source/isaaclab_tasks/test/golden_images/dexsuite_kuka/newton-ovrtx_renderer-rgba.png index c3b229d34871..ca7d07531895 100644 --- a/source/isaaclab_tasks/test/golden_images/dexsuite_kuka/newton-ovrtx_renderer-rgba.png +++ b/source/isaaclab_tasks/test/golden_images/dexsuite_kuka/newton-ovrtx_renderer-rgba.png @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:179a5acba0a763fcc2317cd784f8c347ef48f0d81f8028a1a64996ade67f8706 -size 17836 +oid sha256:71375f209fb2cd8c2e0b7ea463f45ab662bb21b44aec8dcb74994e5969b6aecc +size 17747 diff --git a/source/isaaclab_tasks/test/golden_images/dexsuite_kuka/newton-ovrtx_renderer-simple_shading_constant_diffuse.png b/source/isaaclab_tasks/test/golden_images/dexsuite_kuka/newton-ovrtx_renderer-simple_shading_constant_diffuse.png index 46ce5933fb8b..7fefafde048e 100644 --- a/source/isaaclab_tasks/test/golden_images/dexsuite_kuka/newton-ovrtx_renderer-simple_shading_constant_diffuse.png +++ b/source/isaaclab_tasks/test/golden_images/dexsuite_kuka/newton-ovrtx_renderer-simple_shading_constant_diffuse.png @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:8e1d94f0c6ae2e40a1b0ff9cf27a0f2f9b756ebcebd9ddf27b0da31c89e3a57f -size 1485 +oid sha256:dde0d7363cc8550dfa985178d26bf72b6a7f84157ab8503aa36889e652f7e061 +size 1509 diff --git a/source/isaaclab_tasks/test/golden_images/dexsuite_kuka/newton-ovrtx_renderer-simple_shading_diffuse_mdl.png b/source/isaaclab_tasks/test/golden_images/dexsuite_kuka/newton-ovrtx_renderer-simple_shading_diffuse_mdl.png index 2e2f6cb257a7..b5d197da550d 100644 --- a/source/isaaclab_tasks/test/golden_images/dexsuite_kuka/newton-ovrtx_renderer-simple_shading_diffuse_mdl.png +++ b/source/isaaclab_tasks/test/golden_images/dexsuite_kuka/newton-ovrtx_renderer-simple_shading_diffuse_mdl.png @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:83ca3d8f55f971d473409c73e77582175906670f3962b56723cccd28d062a868 -size 3513 +oid sha256:7608f7f5846d6c78f9c0cf9d19b1eaaed0f79715de26243b8ae20f24ae063617 +size 1465 diff --git a/source/isaaclab_tasks/test/golden_images/dexsuite_kuka/newton-ovrtx_renderer-simple_shading_full_mdl.png b/source/isaaclab_tasks/test/golden_images/dexsuite_kuka/newton-ovrtx_renderer-simple_shading_full_mdl.png index 16b6b73ec7f4..90a2440d093b 100644 --- a/source/isaaclab_tasks/test/golden_images/dexsuite_kuka/newton-ovrtx_renderer-simple_shading_full_mdl.png +++ b/source/isaaclab_tasks/test/golden_images/dexsuite_kuka/newton-ovrtx_renderer-simple_shading_full_mdl.png @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:3afc4f214e51a1ccc1b0341448f64b82773bcc4bba3d913ad4f3052dcf497032 -size 4513 +oid sha256:1563003686040d979ddfe51acdf803f8a46bb268ca569a3fcd757f6e02befcbf +size 3810 diff --git a/source/isaaclab_tasks/test/golden_images/shadow_hand/newton-ovrtx_renderer-albedo.png b/source/isaaclab_tasks/test/golden_images/shadow_hand/newton-ovrtx_renderer-albedo.png index b0a81304d1b4..0a6d3e09769e 100644 --- a/source/isaaclab_tasks/test/golden_images/shadow_hand/newton-ovrtx_renderer-albedo.png +++ b/source/isaaclab_tasks/test/golden_images/shadow_hand/newton-ovrtx_renderer-albedo.png @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:d6ea478eb0b63ac6e9b19c66fabc9944a9cdd1d3131aa0d480ba645151765f41 -size 2150 +oid sha256:4ab0a216128aef68cfc0ebdb94c90f35c74df7283d46e331632c4f5dcc3cb586 +size 1900 diff --git a/source/isaaclab_tasks/test/golden_images/shadow_hand/newton-ovrtx_renderer-rgb.png b/source/isaaclab_tasks/test/golden_images/shadow_hand/newton-ovrtx_renderer-rgb.png index d6e87b16a116..e8d16133a54b 100644 --- a/source/isaaclab_tasks/test/golden_images/shadow_hand/newton-ovrtx_renderer-rgb.png +++ b/source/isaaclab_tasks/test/golden_images/shadow_hand/newton-ovrtx_renderer-rgb.png @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:87b927816b29714d92113b2fcab01569c60372f017119b37ced9a12f72b01cd7 -size 19717 +oid sha256:b827b0e3fc8f009db74351a8535b4c3b2fa0be6274cbd192144dc39d2b40126f +size 20205 diff --git a/source/isaaclab_tasks/test/golden_images/shadow_hand/newton-ovrtx_renderer-rgba.png b/source/isaaclab_tasks/test/golden_images/shadow_hand/newton-ovrtx_renderer-rgba.png index ddffaebf0722..97cf4a8487f5 100644 --- a/source/isaaclab_tasks/test/golden_images/shadow_hand/newton-ovrtx_renderer-rgba.png +++ b/source/isaaclab_tasks/test/golden_images/shadow_hand/newton-ovrtx_renderer-rgba.png @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:7fb6696c895cb07a86897002e434be6c8c67a9d50f15615c2fd16f5038eee209 -size 21761 +oid sha256:901061fc36ba049999e52d742a952340ffed23105e4861f6354d4eb63523d42c +size 22380 diff --git a/source/isaaclab_tasks/test/golden_images/shadow_hand/newton-ovrtx_renderer-simple_shading_constant_diffuse.png b/source/isaaclab_tasks/test/golden_images/shadow_hand/newton-ovrtx_renderer-simple_shading_constant_diffuse.png index a25340e96b0f..39a18ee2dc1e 100644 --- a/source/isaaclab_tasks/test/golden_images/shadow_hand/newton-ovrtx_renderer-simple_shading_constant_diffuse.png +++ b/source/isaaclab_tasks/test/golden_images/shadow_hand/newton-ovrtx_renderer-simple_shading_constant_diffuse.png @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:3122340f40be0b24e9d7f2d262bc7285607536c9cf82151750d2691f05d8950d -size 6840 +oid sha256:993f13cd9fe6970af68e98f72a66d221c4bd1325256f3dd7a6ea602dbcffbceb +size 7097 diff --git a/source/isaaclab_tasks/test/golden_images/shadow_hand/newton-ovrtx_renderer-simple_shading_diffuse_mdl.png b/source/isaaclab_tasks/test/golden_images/shadow_hand/newton-ovrtx_renderer-simple_shading_diffuse_mdl.png index 572a1759a30c..5b972abb61c5 100644 --- a/source/isaaclab_tasks/test/golden_images/shadow_hand/newton-ovrtx_renderer-simple_shading_diffuse_mdl.png +++ b/source/isaaclab_tasks/test/golden_images/shadow_hand/newton-ovrtx_renderer-simple_shading_diffuse_mdl.png @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:3f0ececab1b4b385c54352d02c9b07d6572f8a4f4069eea1afe2089343a164d6 -size 7429 +oid sha256:b2421be77a60117829b0043448599569e16f8c1f3dcf8ecab5c50125c6838a18 +size 7468 diff --git a/source/isaaclab_tasks/test/golden_images/shadow_hand/newton-ovrtx_renderer-simple_shading_full_mdl.png b/source/isaaclab_tasks/test/golden_images/shadow_hand/newton-ovrtx_renderer-simple_shading_full_mdl.png index 687917f13e3b..a5a024b8a9dc 100644 --- a/source/isaaclab_tasks/test/golden_images/shadow_hand/newton-ovrtx_renderer-simple_shading_full_mdl.png +++ b/source/isaaclab_tasks/test/golden_images/shadow_hand/newton-ovrtx_renderer-simple_shading_full_mdl.png @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:88ca19937bbf54a87c40f476c395f678a39d443df3899eb3c74b4e3e854866fc -size 9192 +oid sha256:7448f0ffd54581488a9fd99a24a0f8304bc1c623122db10f07233094bb2264ff +size 9241 diff --git a/source/isaaclab_tasks/test/rendering_test_utils.py b/source/isaaclab_tasks/test/rendering_test_utils.py index 788064900f85..1c80f668e749 100644 --- a/source/isaaclab_tasks/test/rendering_test_utils.py +++ b/source/isaaclab_tasks/test/rendering_test_utils.py @@ -66,12 +66,9 @@ # Parametrization: (physics_backend, renderer, data_type) # --------------------------------------------------------------------------- -# OVRTX kitless paths can segfault on GitHub Actions runners; keep warp/Kit paths in CI. -_SKIP_ON_GITHUB_ACTIONS = os.environ.get("GITHUB_ACTIONS") == "true" -_SKIP_ON_GITHUB_ACTIONS_MARK = pytest.mark.skipif( - _SKIP_ON_GITHUB_ACTIONS, - reason="Skipped on GitHub Actions until the test can run on GitHub Actions.", -) +# Low-resolution camera outputs from RTX renderers are not deterministic enough to pass golden image testing +# on every CI run. (NVBUG#6152566) +_FLAKY_MARK = pytest.mark.flaky(max_runs=3, min_passes=1) PHYSICS_RENDERER_AOV_COMBINATIONS = [ # physx + isaacsim_rtx_renderer @@ -80,42 +77,49 @@ "isaacsim_rtx_renderer", "rgb", id="physx-isaacsim_rtx-rgb", + marks=_FLAKY_MARK, ), pytest.param( "physx", "isaacsim_rtx_renderer", "albedo", id="physx-isaacsim_rtx-albedo", + marks=_FLAKY_MARK, ), pytest.param( "physx", "isaacsim_rtx_renderer", "depth", id="physx-isaacsim_rtx-depth", + marks=_FLAKY_MARK, ), pytest.param( "physx", "isaacsim_rtx_renderer", "simple_shading_constant_diffuse", id="physx-isaacsim_rtx-simple_shading_constant_diffuse", + marks=_FLAKY_MARK, ), pytest.param( "physx", "isaacsim_rtx_renderer", "simple_shading_diffuse_mdl", id="physx-isaacsim_rtx-simple_shading_diffuse_mdl", + marks=_FLAKY_MARK, ), pytest.param( "physx", "isaacsim_rtx_renderer", "simple_shading_full_mdl", id="physx-isaacsim_rtx-simple_shading_full_mdl", + marks=_FLAKY_MARK, ), pytest.param( "physx", "isaacsim_rtx_renderer", "semantic_segmentation", id="physx-isaacsim_rtx-semantic_segmentation", + marks=_FLAKY_MARK, ), # newton + isaacsim_rtx_renderer pytest.param( @@ -123,42 +127,49 @@ "isaacsim_rtx_renderer", "rgb", id="newton-isaacsim_rtx-rgb", + marks=_FLAKY_MARK, ), pytest.param( "newton", "isaacsim_rtx_renderer", "albedo", id="newton-isaacsim_rtx-albedo", + marks=_FLAKY_MARK, ), pytest.param( "newton", "isaacsim_rtx_renderer", "depth", id="newton-isaacsim_rtx-depth", + marks=_FLAKY_MARK, ), pytest.param( "newton", "isaacsim_rtx_renderer", "simple_shading_constant_diffuse", id="newton-isaacsim_rtx-simple_shading_constant_diffuse", + marks=_FLAKY_MARK, ), pytest.param( "newton", "isaacsim_rtx_renderer", "simple_shading_diffuse_mdl", id="newton-isaacsim_rtx-simple_shading_diffuse_mdl", + marks=_FLAKY_MARK, ), pytest.param( "newton", "isaacsim_rtx_renderer", "simple_shading_full_mdl", id="newton-isaacsim_rtx-simple_shading_full_mdl", + marks=_FLAKY_MARK, ), pytest.param( "newton", "isaacsim_rtx_renderer", "semantic_segmentation", id="newton-isaacsim_rtx-semantic_segmentation", + marks=_FLAKY_MARK, ), # physx + newton_renderer (warp) pytest.param( @@ -182,49 +193,49 @@ "ovrtx_renderer", "rgb", id="newton-ovrtx-rgb", - marks=_SKIP_ON_GITHUB_ACTIONS_MARK, + marks=_FLAKY_MARK, ), pytest.param( "newton", "ovrtx_renderer", "albedo", id="newton-ovrtx-albedo", - marks=_SKIP_ON_GITHUB_ACTIONS_MARK, + marks=_FLAKY_MARK, ), pytest.param( "newton", "ovrtx_renderer", "depth", id="newton-ovrtx-depth", - marks=_SKIP_ON_GITHUB_ACTIONS_MARK, + marks=_FLAKY_MARK, ), pytest.param( "newton", "ovrtx_renderer", "simple_shading_constant_diffuse", id="newton-ovrtx-simple_shading_constant_diffuse", - marks=_SKIP_ON_GITHUB_ACTIONS_MARK, + marks=_FLAKY_MARK, ), pytest.param( "newton", "ovrtx_renderer", "simple_shading_diffuse_mdl", id="newton-ovrtx-simple_shading_diffuse_mdl", - marks=_SKIP_ON_GITHUB_ACTIONS_MARK, + marks=_FLAKY_MARK, ), pytest.param( "newton", "ovrtx_renderer", "simple_shading_full_mdl", id="newton-ovrtx-simple_shading_full_mdl", - marks=_SKIP_ON_GITHUB_ACTIONS_MARK, + marks=_FLAKY_MARK, ), pytest.param( "newton", "ovrtx_renderer", "semantic_segmentation", id="newton-ovrtx-semantic_segmentation", - marks=_SKIP_ON_GITHUB_ACTIONS_MARK, + marks=_FLAKY_MARK, ), # newton + newton_renderer (warp) pytest.param( diff --git a/source/isaaclab_tasks/test/test_rendering_dexsuite_kuka.py b/source/isaaclab_tasks/test/test_rendering_dexsuite_kuka.py index 623c1e08c233..0400c3386e63 100644 --- a/source/isaaclab_tasks/test/test_rendering_dexsuite_kuka.py +++ b/source/isaaclab_tasks/test/test_rendering_dexsuite_kuka.py @@ -31,7 +31,6 @@ _attach_comparison_properties_fixture = make_attach_comparison_properties_fixture(_COMPARISON_SCORES) -@pytest.mark.flaky(max_runs=3, min_passes=1) @pytest.mark.parametrize("physics_backend,renderer,data_type", PHYSICS_RENDERER_AOV_COMBINATIONS) def test_rendering_dexsuite_kuka(physics_backend, renderer, data_type): """Test dexsuite kuka allegro lift environment rendering correctness.""" diff --git a/source/isaaclab_tasks/test/test_rendering_dexsuite_kuka_kitless.py b/source/isaaclab_tasks/test/test_rendering_dexsuite_kuka_kitless.py index f76d43364ab5..15afbee806b1 100644 --- a/source/isaaclab_tasks/test/test_rendering_dexsuite_kuka_kitless.py +++ b/source/isaaclab_tasks/test/test_rendering_dexsuite_kuka_kitless.py @@ -27,7 +27,6 @@ _require_ovrtx_install_fixture = make_require_ovrtx_install_fixture() -@pytest.mark.flaky(max_runs=3, min_passes=1) @pytest.mark.parametrize("physics_backend,renderer,data_type", KITLESS_PHYSICS_RENDERER_AOV_COMBINATIONS) def test_rendering_dexsuite_kuka_kitless(physics_backend, renderer, data_type): """Camera output must match golden images (Dexsuite Kuka-Allegro Lift, single camera).""" diff --git a/source/isaaclab_teleop/changelog.d/rwiltz-restore-legacy-teleop.rst b/source/isaaclab_teleop/changelog.d/rwiltz-restore-legacy-teleop.rst deleted file mode 100644 index 4c534f674c8a..000000000000 --- a/source/isaaclab_teleop/changelog.d/rwiltz-restore-legacy-teleop.rst +++ /dev/null @@ -1,11 +0,0 @@ -Changed -^^^^^^^ - -* Changed ``--teleop_device`` default to ``None`` in ``teleop_se3_agent.py`` - and ``record_demos.py``. When omitted, the IsaacTeleop pipeline is used if - the env configures ``isaac_teleop``; otherwise keyboard is used as fallback. - When explicitly provided, the scripts use the legacy ``teleop_devices`` path - and error out if no matching entry exists. -* Removed automatic ``--xr`` detection from ``--teleop_device`` containing - ``"handtracking"``. Users who need XR with the legacy path should pass - ``--xr`` explicitly. diff --git a/source/isaaclab_teleop/config/extension.toml b/source/isaaclab_teleop/config/extension.toml index 947103618b9a..876f53ffd43c 100644 --- a/source/isaaclab_teleop/config/extension.toml +++ b/source/isaaclab_teleop/config/extension.toml @@ -1,6 +1,6 @@ [package] # Semantic Versioning is used: https://semver.org/ -version = "0.3.9" +version = "0.3.11" # Description title = "Isaac Lab Teleop" @@ -13,6 +13,10 @@ keywords = ["kit", "robotics", "teleoperation", "xr", "isaaclab"] [dependencies] "isaaclab" = {} +[isaac_lab_settings] +# Names only. Version ranges, extras, and platform markers come from setup.py metadata. +pip_upgrade_dependencies = ["isaacteleop"] + [core] reloadable = false diff --git a/source/isaaclab_teleop/docs/CHANGELOG.rst b/source/isaaclab_teleop/docs/CHANGELOG.rst index 3856e6ec1346..295bc656a903 100644 --- a/source/isaaclab_teleop/docs/CHANGELOG.rst +++ b/source/isaaclab_teleop/docs/CHANGELOG.rst @@ -1,6 +1,50 @@ Changelog --------- +0.3.11 (2026-05-12) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added :attr:`~isaaclab_teleop.IsaacTeleopCfg.retargeting_execution` for + configuring IsaacTeleop retargeting execution mode from Isaac Lab. + +Changed +^^^^^^^ + +* Changed :class:`~isaaclab_teleop.IsaacTeleopCfg` to enable IsaacTeleop + deadline-paced pipelined retargeting by default. This returns the latest + completed retargeting output while the current frame is submitted, using + ``DeadlinePacingConfig(safety_margin_s=0.025)`` to sample close to the next + simulation consumption point and stagger IsaacTeleop's Python work behind + Isaac Lab's step Python. Set + ``retargeting_execution=RetargetingExecutionConfig(mode="sync")`` to restore + exact current-frame retargeting. + +Fixed +^^^^^ + +* Fixed installation to upgrade to the latest compatible ``isaacteleop`` + package when installing ``isaaclab_teleop``. + + +0.3.10 (2026-05-08) +~~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Changed ``--teleop_device`` default to ``None`` in ``teleop_se3_agent.py`` + and ``record_demos.py``. When omitted, the IsaacTeleop pipeline is used if + the env configures ``isaac_teleop``; otherwise keyboard is used as fallback. + When explicitly provided, the scripts use the legacy ``teleop_devices`` path + and error out if no matching entry exists. +* Removed automatic ``--xr`` detection from ``--teleop_device`` containing + ``"handtracking"``. Users who need XR with the legacy path should pass + ``--xr`` explicitly. + + 0.3.9 (2026-04-29) ~~~~~~~~~~~~~~~~~~ diff --git a/source/isaaclab_teleop/docs/README.md b/source/isaaclab_teleop/docs/README.md index bd6a3c7f6c7b..1f412e0f0238 100644 --- a/source/isaaclab_teleop/docs/README.md +++ b/source/isaaclab_teleop/docs/README.md @@ -120,9 +120,14 @@ rendering without blocking. | `retargeters_to_tune` | `Callable[[], list[BaseRetargeter]] \| None` | `None` | Retargeters to expose in the tuning UI | | `plugins` | `list[PluginConfig]` | `[]` | IsaacTeleop plugin configurations | | `sim_device` | `str` | `"cuda:0"` | Torch device for output action tensors | +| `retargeting_execution` | `RetargetingExecutionConfig` | `mode="pipelined", pacing=DeadlinePacingConfig(safety_margin_s=0.025)` | IsaacTeleop retargeting execution settings | | `teleoperation_active_default` | `bool` | `False` | Whether teleoperation is active on session start | | `app_name` | `str` | `"IsaacLabTeleop"` | Application name for the IsaacTeleop session | +The 25 ms `DeadlinePacingConfig` safety margin staggers IsaacTeleop's Python work behind Isaac Lab's +step Python, giving native work such as rendering time to overlap instead of having both Python stacks +contend for the GIL at the start of the step. + ### `XrCfg` | Field | Type | Default | Description | diff --git a/source/isaaclab_teleop/isaaclab_teleop/isaac_teleop_cfg.py b/source/isaaclab_teleop/isaaclab_teleop/isaac_teleop_cfg.py index f94a63d57589..a15d09bebb01 100644 --- a/source/isaaclab_teleop/isaaclab_teleop/isaac_teleop_cfg.py +++ b/source/isaaclab_teleop/isaaclab_teleop/isaac_teleop_cfg.py @@ -12,6 +12,8 @@ from pathlib import Path from typing import TYPE_CHECKING +from isaacteleop.teleop_session_manager import DeadlinePacingConfig, RetargetingExecutionConfig + from isaaclab.utils import configclass from .control_events import TELEOP_CONTROL_CHANNEL_UUID @@ -95,6 +97,19 @@ def build_pipeline(): sim_device: str = "cuda:0" """Torch device string for placing output action tensors.""" + retargeting_execution: RetargetingExecutionConfig = field( + default_factory=lambda: RetargetingExecutionConfig( + mode="pipelined", + pacing=DeadlinePacingConfig(safety_margin_s=0.025), + ) + ) + """IsaacTeleop retargeting execution settings. + + Isaac Lab opts into IsaacTeleop's pipelined execution by default. Set this + to ``RetargetingExecutionConfig(mode="sync")`` for exact current-frame + retargeting while debugging or comparing behavior. + """ + teleoperation_active_default: bool = False """Whether teleoperation should be active by default when the session starts. diff --git a/source/isaaclab_teleop/isaaclab_teleop/session_lifecycle.py b/source/isaaclab_teleop/isaaclab_teleop/session_lifecycle.py index fa5f36f658e0..9c3a7813cd81 100644 --- a/source/isaaclab_teleop/isaaclab_teleop/session_lifecycle.py +++ b/source/isaaclab_teleop/isaaclab_teleop/session_lifecycle.py @@ -502,6 +502,7 @@ def _try_start_session(self) -> bool: teleop_control_pipeline=self._teleop_control_pipeline, plugins=self._cfg.plugins, oxr_handles=oxr_handles, + retargeting_execution=self._cfg.retargeting_execution, ) # Create and enter the TeleopSession diff --git a/source/isaaclab_teleop/test/test_cloudxr_lifecycle.py b/source/isaaclab_teleop/test/test_cloudxr_lifecycle.py index 43131f70cfc3..457a2a26d967 100644 --- a/source/isaaclab_teleop/test/test_cloudxr_lifecycle.py +++ b/source/isaaclab_teleop/test/test_cloudxr_lifecycle.py @@ -20,6 +20,7 @@ import os import sys +from dataclasses import dataclass from pathlib import Path from types import ModuleType from unittest.mock import MagicMock, patch @@ -70,8 +71,38 @@ def _install_stubs(): """Insert MagicMock modules for all heavy dependencies.""" for name in _MODULES_TO_STUB: if name not in sys.modules: - _stubs_installed[name] = MagicMock() - sys.modules[name] = _stubs_installed[name] + sys.modules[name] = _stubs_installed.setdefault(name, MagicMock()) + if "." in name: + parent_name, child_name = name.rsplit(".", 1) + setattr(sys.modules[parent_name], child_name, sys.modules[name]) + + @dataclass + class DeadlinePacingConfig: + safety_margin_s: float = 0.025 + + @dataclass + class RetargetingExecutionConfig: + mode: str = "sync" + pacing: DeadlinePacingConfig | None = None + + tsm = sys.modules["isaacteleop.teleop_session_manager"] + tsm.DeadlinePacingConfig = DeadlinePacingConfig # type: ignore[attr-defined] + tsm.RetargetingExecutionConfig = RetargetingExecutionConfig # type: ignore[attr-defined] + + +def _restore_stubs(): + """Remove stubs installed for this test module from ``sys.modules``.""" + for name in reversed(_MODULES_TO_STUB): + stub = _stubs_installed.get(name) + if stub is None: + continue + if "." in name: + parent_name, child_name = name.rsplit(".", 1) + parent = sys.modules.get(parent_name) + if parent is not None and getattr(parent, child_name, None) is stub: + delattr(parent, child_name) + if sys.modules.get(name) is stub: + del sys.modules[name] _install_stubs() @@ -83,11 +114,21 @@ def _install_stubs(): ) from isaaclab_teleop.session_lifecycle import TeleopSessionLifecycle # noqa: E402 +_restore_stubs() + # --------------------------------------------------------------------------- # Helpers # --------------------------------------------------------------------------- +@pytest.fixture(autouse=True) +def _stub_heavy_dependencies(): + """Keep CloudXR tests isolated from modules collected later in the suite.""" + _install_stubs() + yield + _restore_stubs() + + def _make_cfg() -> IsaacTeleopCfg: """Build a minimal IsaacTeleopCfg with a dummy pipeline_builder.""" return IsaacTeleopCfg( @@ -139,6 +180,42 @@ def test_profiles_are_in_same_directory(self): assert Path(CLOUDXR_AVP_ENV).parent == Path(CLOUDXR_JS_ENV).parent +# ============================================================================ +# IsaacTeleop execution config +# ============================================================================ + + +class TestRetargetingExecutionConfig: + """Tests for Isaac Lab's IsaacTeleop retargeting execution defaults.""" + + def test_session_config_receives_deadline_paced_pipelined_retargeting(self): + """The default retargeting execution config is passed into TeleopSession.""" + cfg = _make_cfg() + + assert cfg.retargeting_execution.mode == "pipelined" + assert cfg.retargeting_execution.pacing.safety_margin_s == 0.025 + + sentinel_execution = cfg.retargeting_execution + + lifecycle = TeleopSessionLifecycle(cfg) + lifecycle._pipeline = MagicMock() + lifecycle._teleop_control_pipeline = None + + session_config_cls = MagicMock(return_value=MagicMock()) + session_cls = MagicMock() + fake_tsm_module = sys.modules["isaacteleop.teleop_session_manager"] + + with ( + patch.object(fake_tsm_module, "TeleopSessionConfig", session_config_cls), + patch.object(fake_tsm_module, "TeleopSession", session_cls), + patch.object(lifecycle, "_ensure_xr_ar_profile_enabled"), + patch.object(lifecycle, "_acquire_kit_oxr_handles", return_value=object()), + ): + assert lifecycle.try_start_session() is True + + assert session_config_cls.call_args.kwargs["retargeting_execution"] is sentinel_execution + + # ============================================================================ # _ensure_cloudxr_runtime # ============================================================================ diff --git a/source/isaaclab_physx/changelog.d/clone-plan-visualizer-cleanup.skip b/source/isaaclab_visualizers/changelog.d/jichuanh-disable-viewergl-flaky.skip similarity index 100% rename from source/isaaclab_physx/changelog.d/clone-plan-visualizer-cleanup.skip rename to source/isaaclab_visualizers/changelog.d/jichuanh-disable-viewergl-flaky.skip diff --git a/source/isaaclab_visualizers/changelog.d/jichuanh-drop-mujoco-deps.rst b/source/isaaclab_visualizers/changelog.d/jichuanh-drop-mujoco-deps.rst new file mode 100644 index 000000000000..043188cbee35 --- /dev/null +++ b/source/isaaclab_visualizers/changelog.d/jichuanh-drop-mujoco-deps.rst @@ -0,0 +1,8 @@ +Changed +^^^^^^^ + +* Switched the Newton install spec to ``newton[sim]`` in the ``newton``, + ``rerun``, and ``viser`` extras so the MuJoCo solver dependencies are + pulled in transitively. Required because pip resolves a git-URL + requirement once for the URL; a bare ``newton @ git+...`` here would + shadow the ``[sim]`` extra requested elsewhere. diff --git a/source/isaaclab_visualizers/changelog.d/jichuanh-fix-newton-cam-pos-type.rst b/source/isaaclab_visualizers/changelog.d/jichuanh-fix-newton-cam-pos-type.rst new file mode 100644 index 000000000000..6c6525acadf5 --- /dev/null +++ b/source/isaaclab_visualizers/changelog.d/jichuanh-fix-newton-cam-pos-type.rst @@ -0,0 +1,15 @@ +Fixed +^^^^^ + +* Fixed ``test_visualizer_cartpole_integration::test_cartpole_newton_visualizer_viewergl_rgb_motion`` + returning a fully-black ``ViewerGL.get_frame`` buffer on the Newton 1.2.0rc2 + + warp 1.13 cohort. ``NewtonVisualizer._apply_camera_pose`` was assigning + ``self._viewer.camera.pos = wp.vec3(*cam_pos)``, but Newton's + ``Camera.translate()`` adds a ``pyglet.math.Vec3`` delta with ``+=``. + warp 1.13's strict ``__add__`` rejects ``wp.vec3 + pyglet.math.Vec3`` + with ``TypeError``; the exception was silenced by the visualizer's + ``try/except``, which prevented ``renderer.render()`` from ever running + -- so the framebuffer stayed empty and read back as all zeros. The fix + assigns ``pyglet.math.Vec3`` instead, matching what Newton uses internally. +* Re-enabled ``test_cartpole_newton_visualizer_viewergl_rgb_motion`` after the + workaround skip in https://github.com/isaac-sim/IsaacLab/pull/5538. diff --git a/source/isaaclab_visualizers/isaaclab_visualizers/kit/kit_visualization_markers.py b/source/isaaclab_visualizers/isaaclab_visualizers/kit/kit_visualization_markers.py new file mode 100644 index 000000000000..c9f55b413e03 --- /dev/null +++ b/source/isaaclab_visualizers/isaaclab_visualizers/kit/kit_visualization_markers.py @@ -0,0 +1,221 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Kit/USD implementation for :class:`VisualizationMarkers`. + +This backend represents markers as :class:`UsdGeom.PointInstancer` prims in the +USD stage. Marker prototypes are created as child prims of the point instancer +and are instanced efficiently through prototype indices. + +.. _UsdGeom.PointInstancer: https://graphics.pixar.com/usd/dev/api/class_usd_geom_point_instancer.html +""" + +from __future__ import annotations + +import logging + +import torch + +import isaaclab.sim as sim_utils +from isaaclab.markers.visualization_markers_cfg import VisualizationMarkersCfg +from isaaclab.utils.version import has_kit + +logger = logging.getLogger(__name__) + + +class KitVisualizationMarkers: + """USD PointInstancer backend for visualization markers. + + This class wraps around the `UsdGeom.PointInstancer`_ for efficient + handling of objects in the USD stage by instancing the created marker + prototype prims. + + A marker prototype prim is a reusable template prim used for defining + variations of objects in the scene. For example, a sphere prim can be used + as a marker prototype prim to create multiple sphere prims at different + locations. The marker prim path is resolved using the marker name from the + :attr:`VisualizationMarkersCfg.markers` dictionary. + + .. _UsdGeom.PointInstancer: https://graphics.pixar.com/usd/dev/api/class_usd_geom_point_instancer.html + """ + + def __init__(self, cfg: VisualizationMarkersCfg, visible: bool = True): + """Initialize the USD point instancer and register marker prototypes. + + When this backend is initialized, the :class:`UsdGeom.PointInstancer` + is created in the stage and the marker prims are registered into it. + + .. note:: + If a prim already exists at the requested path, the next free path + is used for the :class:`UsdGeom.PointInstancer` prim. + """ + self.cfg = cfg + self.stage = sim_utils.get_current_stage() + # Resolve the next free prim path before creating the point instancer. + self.prim_path = sim_utils.get_next_free_prim_path(cfg.prim_path) + self._is_visible = visible + self._count = len(cfg.markers) + + from pxr import Gf, UsdGeom # noqa: PLC0415 + + self._instancer_manager = UsdGeom.PointInstancer.Define(self.stage, self.prim_path) + self._add_markers_prototypes(self.cfg.markers) + # Note: We need to do this the first time to initialize the instancer. + # Otherwise, the instancer is not fully "created" and USD instance + # queries such as GetInstanceIndices() can fail. + self._instancer_manager.GetProtoIndicesAttr().Set(list(range(len(self.cfg.markers)))) + self._instancer_manager.GetPositionsAttr().Set([Gf.Vec3f(0.0)] * len(self.cfg.markers)) + self.set_visibility(visible) + + @property + def count(self) -> int: + return self._count + + def set_visibility(self, visible: bool) -> None: + """Set USD PointInstancer visibility. + + The method does this through the USD API. + """ + from pxr import UsdGeom # noqa: PLC0415 + + self._is_visible = visible + imageable = UsdGeom.Imageable(self._instancer_manager) + if visible: + imageable.MakeVisible() + else: + imageable.MakeInvisible() + + def is_visible(self) -> bool: + """Return USD PointInstancer visibility.""" + from pxr import UsdGeom # noqa: PLC0415 + + return self._instancer_manager.GetVisibilityAttr().Get() != UsdGeom.Tokens.invisible + + def visualize( + self, + translations: torch.Tensor | None, + orientations: torch.Tensor | None, + scales: torch.Tensor | None, + marker_indices: torch.Tensor | None, + ) -> None: + """Write marker transforms to USD PointInstancer attributes. + + Args: + translations: Translations w.r.t. parent prim frame. Shape is + (M, 3). + orientations: Quaternion orientations (x, y, z, w) w.r.t. parent + prim frame. Shape is (M, 4). + scales: Scale applied before any rotation is applied. Shape is + (M, 3). + marker_indices: Decides which marker prototype to visualize. Shape + is (M). + """ + from pxr import Vt # noqa: PLC0415 + + num_markers = 0 + if translations is not None: + translations_np = translations.detach().cpu().numpy() + # Apply translations. + self._instancer_manager.GetPositionsAttr().Set(Vt.Vec3fArray.FromNumpy(translations_np)) + num_markers = translations_np.shape[0] + if orientations is not None: + orientations_np = orientations.detach().cpu().numpy() + # Apply orientations. USD expects quaternion data in xyzw format. + self._instancer_manager.GetOrientationsAttr().Set(Vt.QuathArray.FromNumpy(orientations_np)) + num_markers = orientations_np.shape[0] + if scales is not None: + scales_np = scales.detach().cpu().numpy() + # Apply scales. + self._instancer_manager.GetScalesAttr().Set(Vt.Vec3fArray.FromNumpy(scales_np)) + num_markers = scales_np.shape[0] + if marker_indices is not None or num_markers != self._count: + if marker_indices is not None: + marker_indices_np = marker_indices.detach().cpu().numpy() + # Apply prototype indices. + self._instancer_manager.GetProtoIndicesAttr().Set(Vt.IntArray.FromNumpy(marker_indices_np)) + num_markers = marker_indices_np.shape[0] + elif num_markers != 0: + # Set all markers to the first prototype when the marker count + # changes and explicit marker indices are not provided. + self._instancer_manager.GetProtoIndicesAttr().Set([0] * num_markers) + if num_markers != 0: + self._count = num_markers + + def _add_markers_prototypes(self, markers_cfg: dict[str, sim_utils.SpawnerCfg]) -> None: + """Add marker prototypes to the scene and register them with the point instancer.""" + # Add markers based on config. + for name, cfg in markers_cfg.items(): + # Resolve prim path from the marker name. + marker_prim_path = f"{self.prim_path}/{name}" + # Create a child prim for the marker. + marker_prim = cfg.func(prim_path=marker_prim_path, cfg=cfg) + # Make the asset uninstanceable in case it is already instanced. + # Point instancer defines its own prototypes, so already-instanced + # assets cannot be used directly. + self._process_prototype_prim(marker_prim) + # Add child reference to point instancer. + self._instancer_manager.GetPrototypesRel().AddTarget(marker_prim_path) + + # Check that all prototypes were loaded. + prototypes = self._instancer_manager.GetPrototypesRel().GetTargets() + if len(prototypes) != len(markers_cfg): + raise RuntimeError( + f"Failed to load all the prototypes. Expected: {len(markers_cfg)}. Received: {len(prototypes)}." + ) + + def _process_prototype_prim(self, prim) -> None: + """Process a prim and its descendants to make them suitable for prototypes. + + Point instancer defines its own prototypes, so if an asset is already + instanced, this does not work. This function checks if the prim and its + descendants are instanced. If so, it makes the respective prim + uninstanceable by disabling instancing on the prim. + + Additionally, it makes the prim invisible to secondary rays. This is + useful when marker prims should not appear in camera images. + + Args: + prim: The prim to process. + """ + from pxr import Sdf, UsdGeom, UsdPhysics # noqa: PLC0415 + + if not prim.IsValid(): + raise ValueError(f"Prim at path '{prim.GetPrimAtPath()}' is not valid.") + + # Iterate over all prims under the marker prim path. + all_prims = [prim] + while len(all_prims) > 0: + child_prim = all_prims.pop(0) + # Remove physics from marker prototypes because they are only for + # visualization. + if child_prim.HasAPI(UsdPhysics.ArticulationRootAPI): + child_prim.RemoveAPI(UsdPhysics.ArticulationRootAPI) + child_prim.RemoveAppliedSchema("PhysxArticulationAPI") + if child_prim.HasAPI(UsdPhysics.RigidBodyAPI): + child_prim.RemoveAPI(UsdPhysics.RigidBodyAPI) + child_prim.RemoveAppliedSchema("PhysxRigidBodyAPI") + if child_prim.IsA(UsdPhysics.Joint): + child_prim.GetAttribute("physics:jointEnabled").Set(False) + # Point instancer defines its own instancing, so nested instances + # must be made uninstanceable. + if child_prim.IsInstance(): + child_prim.SetInstanceable(False) + # Make renderable prims invisible to secondary rays such as depth + # images. + if child_prim.IsA(UsdGeom.Gprim): + sim_utils.change_prim_property( + prop_path=f"{child_prim.GetPrimPath().pathString}.primvars:invisibleToSecondaryRays", + value=True, + stage=prim.GetStage(), + type_to_create_if_not_exist=Sdf.ValueTypeNames.Bool, + ) + all_prims += child_prim.GetChildren() + + # Remove any remaining physics on the markers because they are only for + # visualization. + if has_kit(): + import omni.physx.scripts.utils as physx_utils # noqa: PLC0415 + + physx_utils.removeRigidBodySubtree(prim) diff --git a/source/isaaclab_visualizers/isaaclab_visualizers/kit/kit_visualizer.py b/source/isaaclab_visualizers/isaaclab_visualizers/kit/kit_visualizer.py index ced2935897ae..4766fbe7f68f 100644 --- a/source/isaaclab_visualizers/isaaclab_visualizers/kit/kit_visualizer.py +++ b/source/isaaclab_visualizers/isaaclab_visualizers/kit/kit_visualizer.py @@ -23,7 +23,7 @@ logger = logging.getLogger(__name__) if TYPE_CHECKING: - from isaaclab.physics import BaseSceneDataProvider + from isaaclab.scene.scene_data_provider import SceneDataProvider _DEFAULT_VIEWPORT_NAME = "Visualizer Viewport" @@ -55,7 +55,7 @@ def __init__(self, cfg: KitVisualizerCfg): # ---- Lifecycle ------------------------------------------------------------------------ - def initialize(self, scene_data_provider: BaseSceneDataProvider) -> None: + def initialize(self, scene_data_provider: SceneDataProvider) -> None: """Initialize viewport resources and bind scene data provider. Args: @@ -68,26 +68,23 @@ def initialize(self, scene_data_provider: BaseSceneDataProvider) -> None: if scene_data_provider is None: raise RuntimeError("[KitVisualizer] Requires a scene_data_provider.") self._scene_data_provider = scene_data_provider - usd_stage = scene_data_provider.get_usd_stage() + usd_stage = scene_data_provider.usd_stage if usd_stage is None: raise RuntimeError("[KitVisualizer] USD stage not available from scene_data_provider.") - metadata = scene_data_provider.get_metadata() + num_envs = scene_data_provider.num_envs self._ensure_simulation_app() self._setup_viewport() self._env_ids = self._compute_visualized_env_ids() - num_envs_meta = int(metadata.get("num_envs", 0)) - self._resolved_visible_env_ids = resolve_visible_env_indices( - self._env_ids, self.cfg.max_visible_envs, num_envs_meta - ) + self._resolved_visible_env_ids = resolve_visible_env_indices(self._env_ids, self.cfg.max_visible_envs, num_envs) if self._resolved_visible_env_ids is not None: logger.warning( "[KitVisualizer] Partial visualization in Kit uses visibility only; unselected env prims are hidden." ) - self._apply_env_visibility(usd_stage, metadata, self._resolved_visible_env_ids) + self._apply_env_visibility(usd_stage, num_envs, self._resolved_visible_env_ids) num_visualized_envs = ( - len(self._resolved_visible_env_ids) if self._resolved_visible_env_ids is not None else num_envs_meta + len(self._resolved_visible_env_ids) if self._resolved_visible_env_ids is not None else num_envs ) self._log_initialization_table( logger=logger, @@ -172,7 +169,7 @@ def is_training_paused(self) -> bool: def supports_markers(self) -> bool: """Kit viewport supports marker visualization through Omni UI rendering.""" - return True + return bool(self.cfg.enable_markers) def supports_live_plots(self) -> bool: """Kit backend can host live plot widgets via viewport UI panels.""" @@ -369,7 +366,7 @@ def _set_active_camera_path(self, camera_path: str) -> bool: """ if self._viewport_api is None: return False - usd_stage = self._scene_data_provider.get_usd_stage() if self._scene_data_provider else None + usd_stage = self._scene_data_provider.usd_stage if self._scene_data_provider else None if usd_stage is None: return False camera_prim = usd_stage.GetPrimAtPath(camera_path) @@ -378,9 +375,8 @@ def _set_active_camera_path(self, camera_path: str) -> bool: self._viewport_api.set_active_camera(camera_path) return True - def _apply_env_visibility(self, usd_stage, metadata: dict, visible_env_ids: list[int]) -> None: + def _apply_env_visibility(self, usd_stage, num_envs: int, visible_env_ids: list[int]) -> None: """Hide environments not listed in ``visible_env_ids`` (cosmetic partial visualization).""" - num_envs = int(metadata.get("num_envs", 0)) if num_envs <= 0: return visible = set(visible_env_ids) @@ -406,10 +402,10 @@ def _refresh_partial_viz_point_instancers_if_needed(self) -> None: """Re-apply ``invisibleIds`` for env-scaled `/Visuals` instancers (handles lazy marker creation).""" if self._resolved_visible_env_ids is None or self._scene_data_provider is None: return - usd_stage = self._scene_data_provider.get_usd_stage() + usd_stage = self._scene_data_provider.usd_stage if usd_stage is None: return - num_envs = int(self._scene_data_provider.get_metadata().get("num_envs", 0)) + num_envs = self._scene_data_provider.num_envs if num_envs <= 0: return self._apply_visual_point_instancer_visibility(usd_stage, num_envs, set(self._resolved_visible_env_ids)) @@ -457,7 +453,7 @@ def _point_instancer_instance_count(pi: UsdGeom.PointInstancer) -> int | None: def _restore_env_visibility(self) -> None: """Restore environment visibilities and PointInstancer ``invisibleIds`` from partial viz.""" - usd_stage = self._scene_data_provider.get_usd_stage() if self._scene_data_provider else None + usd_stage = self._scene_data_provider.usd_stage if self._scene_data_provider else None if usd_stage is None: return for env_path, prev in self._hidden_env_visibilities.items(): diff --git a/source/isaaclab_visualizers/isaaclab_visualizers/newton/newton_visualization_markers.py b/source/isaaclab_visualizers/isaaclab_visualizers/newton/newton_visualization_markers.py new file mode 100644 index 000000000000..9d222ed71a61 --- /dev/null +++ b/source/isaaclab_visualizers/isaaclab_visualizers/newton/newton_visualization_markers.py @@ -0,0 +1,531 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Newton-family implementation for :class:`VisualizationMarkers`.""" + +from __future__ import annotations + +import logging +from dataclasses import dataclass +from typing import Any, Literal + +import numpy as np +import torch +import warp as wp +from newton import Axis, Mesh + +import isaaclab.sim as sim_utils +from isaaclab.markers.visualization_markers_cfg import VisualizationMarkersCfg +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR +from isaaclab.utils.math import quat_apply + +logger = logging.getLogger(__name__) + +_OMNIPBR_DEFAULTS = { + "diffuse_color_constant": (0.2, 0.2, 0.2), + "diffuse_tint": (1.0, 1.0, 1.0), +} +_UNBOUND_DEFAULT_FALLBACK_GRAY = (0.18, 0.18, 0.18) +_DEX_CUBE_TEXTURE_URL = f"{ISAAC_NUCLEUS_DIR}/Props/Blocks/DexCube/Materials/dex_cube_mod.png" + + +@dataclass(frozen=True) +class _NewtonMarkerSpec: + renderer: Literal["mesh", "frame", "none"] + mesh_type: Literal["arrow", "box", "textured_box", "sphere", "cylinder", "capsule", "cone"] | None = None + mesh_params: dict[str, float | tuple[float, float, float]] | None = None + scale: tuple[float, float, float] | None = None + color: tuple[float, float, float] | None = None + texture: Any | None = None + + +@dataclass(frozen=True) +class _MeshData: + vertices: np.ndarray + indices: np.ndarray + normals: np.ndarray + uvs: np.ndarray + + +def render_newton_visualization_markers(viewer, visible_env_ids: list[int] | None, num_envs: int) -> None: + """Render all active Newton visualization marker groups into a Newton-family viewer.""" + sim = sim_utils.SimulationContext.instance() + if sim is None: + return + + for marker in sim.vis_marker_registry.get_groups().values(): + if isinstance(marker, NewtonVisualizationMarkers): + marker.render(viewer, visible_env_ids=visible_env_ids, num_envs=num_envs) + + +class NewtonVisualizationMarkers: + """Newton-family backend for visualization markers.""" + + def __init__(self, cfg: VisualizationMarkersCfg, visible: bool = True): + self.cfg = cfg + self.group_id = f"{cfg.prim_path}::{id(self)}" + self.visible = visible + self.translations: torch.Tensor | None = None + self.orientations: torch.Tensor | None = None + self.scales: torch.Tensor | None = None + self.marker_indices: torch.Tensor | None = None + self.count = len(cfg.markers) + self._registered_meshes: set[tuple[int, str]] = set() + self._warned_unsupported: set[str] = set() + + sim = sim_utils.SimulationContext.instance() + if sim is not None: + sim.vis_marker_registry.set_group(self.group_id, self) + + def close(self) -> None: + """Remove marker backend from the simulation marker registry.""" + sim = sim_utils.SimulationContext.instance() + if sim is not None: + sim.vis_marker_registry.remove_group(self.group_id) + + def infer_device(self) -> torch.device: + """Infer the device from current marker state.""" + for value in (self.translations, self.orientations, self.scales, self.marker_indices): + if value is not None: + return value.device + return torch.device("cpu") + + def set_visibility(self, visible: bool) -> None: + """Set marker visibility.""" + self.visible = visible + + def is_visible(self) -> bool: + """Return whether this marker group is visible.""" + return self.visible + + def visualize( + self, + translations: torch.Tensor | None, + orientations: torch.Tensor | None, + scales: torch.Tensor | None, + marker_indices: torch.Tensor | None, + ) -> None: + """Update marker state consumed by Newton-family visualizers.""" + if translations is not None: + self.translations = translations.detach() + self.count = translations.shape[0] + if orientations is not None: + self.orientations = orientations.detach() + self.count = orientations.shape[0] + if scales is not None: + self.scales = scales.detach() + self.count = scales.shape[0] + if marker_indices is not None: + self.marker_indices = marker_indices.detach().to(dtype=torch.int32) + self.count = marker_indices.shape[0] + elif self.count != 0: + self.marker_indices = torch.zeros(self.count, dtype=torch.int32, device=self.infer_device()) + + def render(self, viewer, visible_env_ids: list[int] | None, num_envs: int) -> None: + """Render marker state to a Newton viewer.""" + state = _filter_marker_state(self, visible_env_ids=visible_env_ids, num_envs=num_envs) + if state["count"] == 0: + for name, marker_cfg in self.cfg.markers.items(): + self._hide_batch(viewer, name, _resolve_newton_marker_cfg(name, marker_cfg, self.cfg)) + return + + translations = state["translations"] + if translations is None: + return + orientations = state["orientations"] + if orientations is None: + orientations = torch.tensor([[0.0, 0.0, 0.0, 1.0]], device=translations.device).repeat(state["count"], 1) + scales = state["scales"] + if scales is None: + scales = torch.ones((state["count"], 3), dtype=torch.float32, device=translations.device) + marker_indices = state["marker_indices"] + if marker_indices is None: + marker_indices = torch.zeros(state["count"], dtype=torch.int64, device=translations.device) + + for proto_index, (name, marker_cfg) in enumerate(self.cfg.markers.items()): + newton_cfg = _resolve_newton_marker_cfg(name, marker_cfg, self.cfg) + batch_name = f"{self.group_id}/{name}" + selected = marker_indices == proto_index + if not state["visible"] or int(selected.sum().item()) == 0: + self._hide_batch(viewer, name, newton_cfg) + continue + + if newton_cfg.renderer == "none": + unsupported_key = f"{self.group_id}:{name}" + if unsupported_key not in self._warned_unsupported: + logger.warning( + "[NewtonVisualizationMarkers] Unsupported marker prototype '%s' in group '%s'; skipping.", + name, + self.group_id, + ) + self._warned_unsupported.add(unsupported_key) + continue + + selected_translations = translations[selected] + selected_orientations = orientations[selected] + default_scale = newton_cfg.scale or _extract_scale_hint(marker_cfg) + selected_scales = scales[selected] * torch.tensor( + default_scale, dtype=torch.float32, device=scales.device + ).unsqueeze(0) + + if newton_cfg.renderer == "mesh": + mesh_name = f"{self.group_id}/meshes/{name}" + self._ensure_mesh_registered(viewer, mesh_name, newton_cfg) + color = newton_cfg.color or _extract_color(marker_cfg) + colors = torch.tensor(color, dtype=torch.float32, device=scales.device).repeat( + selected_scales.shape[0], 1 + ) + materials = torch.zeros((selected_scales.shape[0], 4), dtype=torch.float32, device=scales.device) + if newton_cfg.texture is not None: + # ViewerGL gates texture sampling with material.w. Rerun and + # Viser ignore this flag but consume the mesh texture. + materials[:, 3] = 1.0 + xforms = torch.cat((selected_translations, selected_orientations), dim=1).detach().cpu().numpy() + viewer.log_instances( + batch_name, + mesh_name, + wp.array(xforms.astype(np.float32), dtype=wp.transform), + wp.array(selected_scales.detach().cpu().numpy().astype(np.float32), dtype=wp.vec3), + wp.array(colors.detach().cpu().numpy().astype(np.float32), dtype=wp.vec3), + wp.array(materials.detach().cpu().numpy().astype(np.float32), dtype=wp.vec4), + hidden=False, + ) + elif newton_cfg.renderer == "frame": + starts, ends, colors = _build_frame_lines(selected_translations, selected_orientations, selected_scales) + width = max(float(selected_scales.mean().item()) * 0.05, 0.0025) + viewer.log_lines( + batch_name, + wp.array(starts.detach().cpu().numpy().astype(np.float32), dtype=wp.vec3), + wp.array(ends.detach().cpu().numpy().astype(np.float32), dtype=wp.vec3), + wp.array(colors.detach().cpu().numpy().astype(np.float32), dtype=wp.vec3), + width=width, + hidden=False, + ) + + def _hide_batch(self, viewer, name: str, newton_cfg: _NewtonMarkerSpec) -> None: + batch_name = f"{self.group_id}/{name}" + if newton_cfg.renderer == "mesh" and newton_cfg.mesh_type is not None: + mesh_name = f"{self.group_id}/meshes/{name}" + self._ensure_mesh_registered(viewer, mesh_name, newton_cfg) + viewer.log_instances(batch_name, mesh_name, None, None, None, None, hidden=True) + elif newton_cfg.renderer == "frame": + viewer.log_lines(batch_name, None, None, None, hidden=True) + + def _ensure_mesh_registered(self, viewer, mesh_name: str, newton_cfg: _NewtonMarkerSpec) -> None: + # The marker backend is shared by all Newton-family visualizers. Mesh + # registration is viewer-local, so the same marker mesh must be logged + # once per viewer (for example, once for Rerun and once for Viser). + registered_key = (id(viewer), mesh_name) + if registered_key in self._registered_meshes or newton_cfg.mesh_type is None: + return + mesh = _create_mesh(newton_cfg) + viewer.log_mesh( + mesh_name, + wp.array(mesh.vertices.astype(np.float32), dtype=wp.vec3), + wp.array(mesh.indices.astype(np.int32), dtype=wp.int32), + normals=wp.array(mesh.normals.astype(np.float32), dtype=wp.vec3) if mesh.normals.size else None, + uvs=wp.array(mesh.uvs.astype(np.float32), dtype=wp.vec2) if mesh.uvs.size else None, + texture=newton_cfg.texture, + hidden=True, + ) + self._registered_meshes.add(registered_key) + + +def _resolve_newton_marker_cfg(name: str, marker_cfg: object, cfg: VisualizationMarkersCfg) -> _NewtonMarkerSpec: + del name, cfg + return _infer_newton_marker_cfg(marker_cfg) + + +def _infer_newton_marker_cfg(marker_cfg: object) -> _NewtonMarkerSpec: + cfg_type = type(marker_cfg).__name__ + + if cfg_type == "SphereCfg": + return _NewtonMarkerSpec(renderer="mesh", mesh_type="sphere", mesh_params={"radius": float(marker_cfg.radius)}) + if cfg_type == "CuboidCfg": + return _NewtonMarkerSpec( + renderer="mesh", mesh_type="box", mesh_params={"size": tuple(float(v) for v in marker_cfg.size)} + ) + if cfg_type == "CylinderCfg": + return _NewtonMarkerSpec( + renderer="mesh", + mesh_type="cylinder", + mesh_params={"radius": float(marker_cfg.radius), "height": float(marker_cfg.height)}, + ) + if cfg_type == "CapsuleCfg": + return _NewtonMarkerSpec( + renderer="mesh", + mesh_type="capsule", + mesh_params={"radius": float(marker_cfg.radius), "height": float(marker_cfg.height)}, + ) + if cfg_type == "ConeCfg": + return _NewtonMarkerSpec( + renderer="mesh", + mesh_type="cone", + mesh_params={"radius": float(marker_cfg.radius), "height": float(marker_cfg.height)}, + ) + + if cfg_type == "UsdFileCfg": + usd_path = str(marker_cfg.usd_path).lower() + default_scale = _extract_scale_hint(marker_cfg) + if usd_path.endswith("arrow_x.usd"): + return _NewtonMarkerSpec( + renderer="mesh", + mesh_type="arrow", + mesh_params={"base_radius": 0.08, "base_height": 0.7, "cap_radius": 0.16, "cap_height": 0.3}, + scale=(default_scale[0], default_scale[1] * 2.5, default_scale[2] * 2.5), + ) + if usd_path.endswith("frame_prim.usd"): + return _NewtonMarkerSpec(renderer="frame", scale=default_scale) + if "dexcube" in usd_path or "dex_cube" in usd_path: + # TODO: Remove this specialized DexCube mesh code when general + # UsdFileCfg-to-Newton mesh conversion is supported. + # DexCube USDs are roughly 6 cm wide. Keep scale separate so task + # configs such as scale=(1.2, 1.2, 1.2) still apply naturally. + return _NewtonMarkerSpec( + renderer="mesh", + mesh_type="textured_box", + mesh_params={"size": (0.06, 0.06, 0.06)}, + color=(1.0, 1.0, 1.0), + texture=_DEX_CUBE_TEXTURE_URL, + ) + + # TODO: Add generic UsdFileCfg -> Newton mesh extraction for mesh-backed USD marker assets. + # For now, only common marker USDs are mapped to lightweight Newton-native fallbacks. + + return _NewtonMarkerSpec(renderer="none") + + +def _create_mesh(newton_cfg: _NewtonMarkerSpec): + mesh_params = newton_cfg.mesh_params or {} + if newton_cfg.mesh_type == "arrow": + return Mesh.create_arrow( + float(mesh_params["base_radius"]), + float(mesh_params["base_height"]), + cap_radius=float(mesh_params["cap_radius"]), + cap_height=float(mesh_params["cap_height"]), + up_axis=Axis.X, + ) + if newton_cfg.mesh_type == "box": + size = mesh_params["size"] + return Mesh.create_box(float(size[0]) * 0.5, float(size[1]) * 0.5, float(size[2]) * 0.5) + if newton_cfg.mesh_type == "textured_box": + return _create_textured_box_mesh(mesh_params["size"]) + if newton_cfg.mesh_type == "sphere": + return Mesh.create_sphere(radius=float(mesh_params["radius"])) + if newton_cfg.mesh_type == "cylinder": + return Mesh.create_cylinder( + float(mesh_params["radius"]), + float(mesh_params["height"]) * 0.5, + up_axis=Axis.Z, + ) + if newton_cfg.mesh_type == "capsule": + return Mesh.create_capsule( + float(mesh_params["radius"]), + float(mesh_params["height"]) * 0.5, + up_axis=Axis.Z, + ) + if newton_cfg.mesh_type == "cone": + return Mesh.create_cone( + float(mesh_params["radius"]), + float(mesh_params["height"]) * 0.5, + up_axis=Axis.Z, + ) + raise ValueError(f"Unsupported Newton mesh type: {newton_cfg.mesh_type}") + + +def _create_textured_box_mesh(size: tuple[float, float, float]) -> _MeshData: + # TODO: Remove this specialized DexCube mesh code when general + # UsdFileCfg-to-Newton mesh conversion is supported. + half = np.asarray(size, dtype=np.float32) * 0.5 + usd_vertices = np.asarray( + [ + (-1.0, -1.0, 1.0), + (-1.0, 1.0, 1.0), + (-1.0, 1.0, -1.0), + (-1.0, -1.0, -1.0), + (-1.0, -1.0, -1.0), + (-1.0, 1.0, -1.0), + (1.0, 1.0, -1.0), + (1.0, -1.0, -1.0), + (1.0, -1.0, -1.0), + (1.0, 1.0, -1.0), + (1.0, 1.0, 1.0), + (1.0, -1.0, 1.0), + (1.0, -1.0, 1.0), + (1.0, 1.0, 1.0), + (-1.0, 1.0, 1.0), + (-1.0, -1.0, 1.0), + (-1.0, -1.0, -1.0), + (1.0, -1.0, -1.0), + (1.0, -1.0, 1.0), + (-1.0, -1.0, 1.0), + (1.0, 1.0, -1.0), + (-1.0, 1.0, -1.0), + (-1.0, 1.0, 1.0), + (1.0, 1.0, 1.0), + ], + dtype=np.float32, + ) + uvs = np.asarray( + [ + (1.0, 0.333333), + (1.0, 0.666667), + (0.5, 0.666667), + (0.5, 0.333333), + (0.5, 0.666667), + (0.5, 1.0), + (0.0, 1.0), + (0.0, 0.666667), + (0.5, 0.333333), + (0.5, 0.666667), + (0.0, 0.666667), + (0.0, 0.333333), + (1.0, 0.0), + (1.0, 0.333333), + (0.5, 0.333333), + (0.5, 0.0), + (0.5, 0.0), + (0.5, 0.333333), + (0.0, 0.333333), + (0.0, 0.0), + (1.0, 0.666667), + (1.0, 1.0), + (0.5, 1.0), + (0.5, 0.666667), + ], + dtype=np.float32, + ) + indices: list[int] = [] + for base in range(0, 24, 4): + indices.extend([base, base + 1, base + 2, base, base + 2, base + 3]) + return _MeshData( + vertices=usd_vertices * half, + indices=np.asarray(indices, dtype=np.int32), + normals=np.zeros((0, 3), dtype=np.float32), + uvs=uvs, + ) + + +def _filter_marker_state( + marker: NewtonVisualizationMarkers, + visible_env_ids: list[int] | None, + num_envs: int, +) -> dict[str, Any]: + if visible_env_ids is None or marker.count == 0 or num_envs <= 0 or marker.count % num_envs != 0: + return { + "visible": marker.visible, + "translations": marker.translations, + "orientations": marker.orientations, + "scales": marker.scales, + "marker_indices": marker.marker_indices, + "count": marker.count, + } + + keep: list[int] = [] + repeat_count = marker.count // num_envs + for block_idx in range(repeat_count): + base = block_idx * num_envs + for env_id in visible_env_ids: + idx = base + env_id + if idx < marker.count: + keep.append(idx) + + if len(keep) == marker.count: + return { + "visible": marker.visible, + "translations": marker.translations, + "orientations": marker.orientations, + "scales": marker.scales, + "marker_indices": marker.marker_indices, + "count": marker.count, + } + + index = torch.tensor(keep, dtype=torch.long, device=marker.infer_device()) + return { + "visible": marker.visible, + "translations": marker.translations.index_select(0, index) if marker.translations is not None else None, + "orientations": marker.orientations.index_select(0, index) if marker.orientations is not None else None, + "scales": marker.scales.index_select(0, index) if marker.scales is not None else None, + "marker_indices": marker.marker_indices.index_select(0, index) if marker.marker_indices is not None else None, + "count": len(keep), + } + + +def _extract_scale_hint(marker_cfg: object) -> tuple[float, float, float]: + scale = marker_cfg.scale if type(marker_cfg).__name__ == "UsdFileCfg" else None + if scale is None: + return (1.0, 1.0, 1.0) + return tuple(float(v) for v in scale) + + +def _extract_color(marker_cfg: object) -> tuple[float, float, float]: + material_cfg = marker_cfg.visual_material + if material_cfg is None: + return _UNBOUND_DEFAULT_FALLBACK_GRAY + + if color := _extract_omnipbr_like_color(material_cfg): + return color + + material_type = type(material_cfg).__name__ + if material_type == "PreviewSurfaceCfg": + return _extract_rgb(material_cfg.diffuse_color) or _UNBOUND_DEFAULT_FALLBACK_GRAY + if material_type == "GlassMdlCfg": + return _extract_rgb(material_cfg.glass_color) or _UNBOUND_DEFAULT_FALLBACK_GRAY + + return _UNBOUND_DEFAULT_FALLBACK_GRAY + + +def _extract_omnipbr_like_color(material_cfg: object) -> tuple[float, float, float] | None: + material_type = type(material_cfg).__name__ + if material_type == "MdlFileCfg": + if not str(material_cfg.mdl_path).lower().endswith("omnipbr.mdl"): + return None + brightness = material_cfg.albedo_brightness + if brightness is not None: + diffuse_constant = (float(brightness), float(brightness), float(brightness)) + else: + diffuse_constant = _OMNIPBR_DEFAULTS["diffuse_color_constant"] + diffuse_tint = _OMNIPBR_DEFAULTS["diffuse_tint"] + else: + return None + + return ( + diffuse_constant[0] * diffuse_tint[0], + diffuse_constant[1] * diffuse_tint[1], + diffuse_constant[2] * diffuse_tint[2], + ) + + +def _extract_rgb(value: Any) -> tuple[float, float, float] | None: + if value is None: + return None + try: + rgb = tuple(float(v) for v in value) + except TypeError: + return None + if len(rgb) < 3: + return None + return (rgb[0], rgb[1], rgb[2]) + + +def _build_frame_lines( + translations: torch.Tensor, + orientations: torch.Tensor, + scales: torch.Tensor, +) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor]: + unit_axes = ( + torch.eye(3, dtype=torch.float32, device=translations.device).unsqueeze(0).repeat(translations.shape[0], 1, 1) + ) + scaled_axes = unit_axes * scales.unsqueeze(1) + repeated_quats = orientations.unsqueeze(1).repeat(1, 3, 1).reshape(-1, 4) + rotated_axes = quat_apply(repeated_quats, scaled_axes.reshape(-1, 3)).reshape(-1, 3, 3) + starts = translations.unsqueeze(1).repeat(1, 3, 1).reshape(-1, 3) + ends = (translations.unsqueeze(1) + rotated_axes).reshape(-1, 3) + colors = torch.tensor( + [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.35, 1.0]], + dtype=torch.float32, + device=translations.device, + ).repeat(translations.shape[0], 1) + return starts, ends, colors diff --git a/source/isaaclab_visualizers/isaaclab_visualizers/newton/newton_visualizer.py b/source/isaaclab_visualizers/isaaclab_visualizers/newton/newton_visualizer.py index 8c8bb0bed9d8..31c17a7b16d6 100644 --- a/source/isaaclab_visualizers/isaaclab_visualizers/newton/newton_visualizer.py +++ b/source/isaaclab_visualizers/isaaclab_visualizers/newton/newton_visualizer.py @@ -13,6 +13,7 @@ import numpy as np import warp as wp from newton.viewer import ViewerGL +from pyglet.math import Vec3 as PygletVec3 from isaaclab.visualizers.base_visualizer import BaseVisualizer @@ -23,7 +24,7 @@ logger = logging.getLogger(__name__) if TYPE_CHECKING: - from isaaclab.physics import BaseSceneDataProvider + from isaaclab.scene.scene_data_provider import SceneDataProvider class NewtonViewerGL(ViewerGL): @@ -268,13 +269,16 @@ def __init__(self, cfg: NewtonVisualizerCfg): self._scene_data_provider = None self._last_camera_pose: tuple[tuple[float, float, float], tuple[float, float, float]] | None = None self._headless_no_viewer = False + self._resolved_visible_env_ids: list[int] | None = None - def initialize(self, scene_data_provider: BaseSceneDataProvider) -> None: + def initialize(self, scene_data_provider: SceneDataProvider) -> None: """Initialize viewer resources and bind scene data provider. Args: scene_data_provider: Scene data provider used to fetch model/state data. """ + from isaaclab_newton.physics import NewtonManager + if self._is_initialized: logger.debug("[NewtonVisualizer] initialize() called while already initialized.") return @@ -282,11 +286,11 @@ def initialize(self, scene_data_provider: BaseSceneDataProvider) -> None: raise RuntimeError("Newton visualizer requires a scene_data_provider.") self._scene_data_provider = scene_data_provider - metadata = scene_data_provider.get_metadata() - num_envs = int(metadata.get("num_envs", 0)) + num_envs = scene_data_provider.num_envs + metadata = {"num_envs": num_envs} self._env_ids = self._compute_visualized_env_ids() - self._model = scene_data_provider.get_newton_model() - self._state = scene_data_provider.get_newton_state() + self._model = NewtonManager.get_model() + self._state = NewtonManager.get_state() # Use pyglet's EGL headless backend when requested. Must run before the first # ``pyglet.window`` import so ``Window`` resolves to :class:`~pyglet.window.headless.HeadlessWindow`. @@ -335,8 +339,10 @@ def initialize(self, scene_data_provider: BaseSceneDataProvider) -> None: self._viewer.renderer.sky_lower = self._viewer._coerce_color3(self.cfg.sky_lower_color) self._viewer.renderer._light_color = self._viewer._coerce_color3(self.cfg.light_color) - _resolved = resolve_visible_env_indices(self._env_ids, self.cfg.max_visible_envs, num_envs) - num_visualized_envs = len(_resolved) if _resolved is not None else num_envs + self._resolved_visible_env_ids = resolve_visible_env_indices(self._env_ids, self.cfg.max_visible_envs, num_envs) + num_visualized_envs = ( + len(self._resolved_visible_env_ids) if self._resolved_visible_env_ids is not None else num_envs + ) self._log_initialization_table( logger=logger, title="NewtonVisualizer Configuration", @@ -365,23 +371,16 @@ def step(self, dt: float) -> None: self._sim_time += dt self._step_counter += 1 + from isaaclab_newton.physics import NewtonManager + if self._viewer is None: - if self._scene_data_provider is not None: - self._state = self._scene_data_provider.get_newton_state() + self._state = NewtonManager.get_state() return if self.cfg.cam_source == "prim_path": self._update_camera_from_usd_path() - self._state = self._scene_data_provider.get_newton_state() - - contacts = None - if self._viewer.show_contacts: - contacts_data = self._scene_data_provider.get_contacts() - if isinstance(contacts_data, dict): - contacts = contacts_data.get("contacts", contacts_data) - else: - contacts = contacts_data + self._state = NewtonManager.get_state() update_frequency = self._viewer._update_frequency if self._viewer else self._update_frequency if self._step_counter % update_frequency != 0: @@ -396,11 +395,6 @@ def step(self, dt: float) -> None: self._viewer.end_frame() return self._viewer.log_state(self._state) - if contacts is not None and hasattr(self._viewer, "log_contacts"): - try: - self._viewer.log_contacts(contacts, self._state) - except RuntimeError as exc: - logger.debug(f"[NewtonVisualizer] Failed to log contacts: {exc}") self._viewer.end_frame() else: self._viewer._update() @@ -454,7 +448,8 @@ def _apply_camera_pose(self, pose: tuple[tuple[float, float, float], tuple[float if self._viewer is None: return cam_pos, cam_target = pose - self._viewer.camera.pos = wp.vec3(*cam_pos) + # Match Newton's Camera native pos type: PyVec3, not wp.vec3. + self._viewer.camera.pos = PygletVec3(*cam_pos) cam_pos_np = np.array(cam_pos, dtype=np.float32) cam_target_np = np.array(cam_target, dtype=np.float32) direction = cam_target_np - cam_pos_np @@ -475,8 +470,8 @@ def _update_camera_from_usd_path(self) -> None: self._apply_camera_pose(pose) def supports_markers(self) -> bool: - """Newton OpenGL viewer does not implement Isaac Lab marker primitives.""" - return False + """Newton OpenGL viewer supports Isaac Lab markers through viewer-side meshes and lines.""" + return bool(self.cfg.enable_markers) def supports_live_plots(self) -> bool: """Newton OpenGL viewer does not provide live-plot panels.""" diff --git a/source/isaaclab_visualizers/isaaclab_visualizers/rerun/rerun_visualizer.py b/source/isaaclab_visualizers/isaaclab_visualizers/rerun/rerun_visualizer.py index 5390802df69d..7e6f9a00331a 100644 --- a/source/isaaclab_visualizers/isaaclab_visualizers/rerun/rerun_visualizer.py +++ b/source/isaaclab_visualizers/isaaclab_visualizers/rerun/rerun_visualizer.py @@ -20,12 +20,13 @@ from isaaclab.visualizers.base_visualizer import BaseVisualizer +from isaaclab_visualizers.newton.newton_visualization_markers import render_newton_visualization_markers from isaaclab_visualizers.newton_adapter import apply_viewer_visible_worlds, resolve_visible_env_indices from .rerun_visualizer_cfg import RerunVisualizerCfg if TYPE_CHECKING: - from isaaclab.physics import BaseSceneDataProvider + from isaaclab.scene.scene_data_provider import SceneDataProvider logger = logging.getLogger(__name__) @@ -133,24 +134,26 @@ def __init__(self, cfg: RerunVisualizerCfg): self._state = None self._scene_data_provider = None self._last_camera_pose: tuple[tuple[float, float, float], tuple[float, float, float]] | None = None + self._resolved_visible_env_ids: list[int] | None = None - def initialize(self, scene_data_provider: BaseSceneDataProvider) -> None: + def initialize(self, scene_data_provider: SceneDataProvider) -> None: """Initialize rerun viewer and bind scene data provider. Args: scene_data_provider: Scene data provider used to fetch model/state data. """ + from isaaclab_newton.physics import NewtonManager + if self._is_initialized: return if scene_data_provider is None: raise RuntimeError("Rerun visualizer requires a scene_data_provider.") self._scene_data_provider = scene_data_provider - metadata = scene_data_provider.get_metadata() - num_envs = int(metadata.get("num_envs", 0)) + num_envs = scene_data_provider.num_envs self._env_ids = self._compute_visualized_env_ids() - self._model = scene_data_provider.get_newton_model() - self._state = scene_data_provider.get_newton_state() + self._model = NewtonManager.get_model() + self._state = NewtonManager.get_state() grpc_port = int(self.cfg.grpc_port) web_port = int(self.cfg.web_port) @@ -196,8 +199,10 @@ def initialize(self, scene_data_provider: BaseSceneDataProvider) -> None: self._viewer.scaling = 1.0 self._viewer._paused = False - _resolved = resolve_visible_env_indices(self._env_ids, self.cfg.max_visible_envs, num_envs) - num_visualized_envs = len(_resolved) if _resolved is not None else num_envs + self._resolved_visible_env_ids = resolve_visible_env_indices(self._env_ids, self.cfg.max_visible_envs, num_envs) + num_visualized_envs = ( + len(self._resolved_visible_env_ids) if self._resolved_visible_env_ids is not None else num_envs + ) self._log_initialization_table( logger=logger, title="RerunVisualizer Configuration", @@ -225,6 +230,8 @@ def step(self, dt: float) -> None: Args: dt: Simulation time-step in seconds. """ + from isaaclab_newton.physics import NewtonManager + if not self._is_initialized or self._is_closed or self._viewer is None: return @@ -234,17 +241,23 @@ def step(self, dt: float) -> None: if self.cfg.cam_source == "prim_path": self._update_camera_from_usd_path() - self._state = self._scene_data_provider.get_newton_state() + self._state = NewtonManager.get_state() + num_envs = NewtonManager.get_num_envs() if not self._viewer.is_paused(): self._viewer.begin_frame(self._sim_time) - if self._state is not None: - body_q = getattr(self._state, "body_q", None) - if hasattr(body_q, "shape") and body_q.shape[0] == 0: - self._viewer.end_frame() - return - self._viewer.log_state(self._state) - self._viewer.end_frame() + try: + if self._state is not None: + body_q = getattr(self._state, "body_q", None) + if hasattr(body_q, "shape") and body_q.shape[0] == 0: + return + self._viewer.log_state(self._state) + if self.cfg.enable_markers: + render_newton_visualization_markers( + self._viewer, self._resolved_visible_env_ids, num_envs=num_envs + ) + finally: + self._viewer.end_frame() def close(self) -> None: """Close viewer/session resources.""" @@ -323,8 +336,8 @@ def _update_camera_from_usd_path(self) -> None: self._apply_camera_pose(pose) def supports_markers(self) -> bool: - """Rerun backend currently does not expose Isaac Lab marker primitives.""" - return False + """Rerun backend supports Isaac Lab markers through Newton viewer primitives.""" + return bool(self.cfg.enable_markers) def supports_live_plots(self) -> bool: """Rerun backend currently does not expose Isaac Lab live-plot widgets.""" diff --git a/source/isaaclab_visualizers/isaaclab_visualizers/viser/viser_visualizer.py b/source/isaaclab_visualizers/isaaclab_visualizers/viser/viser_visualizer.py index a629ab8b2fed..b3569b04dbf3 100644 --- a/source/isaaclab_visualizers/isaaclab_visualizers/viser/viser_visualizer.py +++ b/source/isaaclab_visualizers/isaaclab_visualizers/viser/viser_visualizer.py @@ -19,6 +19,7 @@ from isaaclab.visualizers.base_visualizer import BaseVisualizer +from isaaclab_visualizers.newton.newton_visualization_markers import render_newton_visualization_markers from isaaclab_visualizers.newton_adapter import apply_viewer_visible_worlds, resolve_visible_env_indices from .viser_visualizer_cfg import ViserVisualizerCfg @@ -26,7 +27,7 @@ logger = logging.getLogger(__name__) if TYPE_CHECKING: - from isaaclab.physics import BaseSceneDataProvider + from isaaclab.scene.scene_data_provider import SceneDataProvider def _disable_viser_runtime_client_rebuild_if_bundled() -> None: @@ -129,13 +130,17 @@ def __init__(self, cfg: ViserVisualizerCfg): self._active_record_path: str | None = None self._last_camera_pose: tuple[tuple[float, float, float], tuple[float, float, float]] | None = None self._pending_camera_pose: tuple[tuple[float, float, float], tuple[float, float, float]] | None = None + self._resolved_visible_env_ids: list[int] | None = None + self._warned_marker_render_failure = False - def initialize(self, scene_data_provider: BaseSceneDataProvider) -> None: + def initialize(self, scene_data_provider: SceneDataProvider) -> None: """Initialize viewer resources and bind scene data provider. Args: scene_data_provider: Scene data provider used to fetch model/state data. """ + from isaaclab_newton.physics import NewtonManager + if self._is_initialized: logger.debug("[ViserVisualizer] initialize() called while already initialized.") return @@ -143,16 +148,18 @@ def initialize(self, scene_data_provider: BaseSceneDataProvider) -> None: raise RuntimeError("Viser visualizer requires a scene_data_provider.") self._scene_data_provider = scene_data_provider - metadata = scene_data_provider.get_metadata() + num_envs = scene_data_provider.num_envs + metadata = {"num_envs": num_envs} self._env_ids = self._compute_visualized_env_ids() - self._model = scene_data_provider.get_newton_model() - self._state = scene_data_provider.get_newton_state() + self._model = NewtonManager.get_model() + self._state = NewtonManager.get_state() self._active_record_path = self.cfg.record_to_viser self._create_viewer(record_to_viser=self.cfg.record_to_viser, metadata=metadata) - num_envs_meta = int(metadata.get("num_envs", 0)) - _resolved = resolve_visible_env_indices(self._env_ids, self.cfg.max_visible_envs, num_envs_meta) - num_visualized_envs = len(_resolved) if _resolved is not None else num_envs_meta + self._resolved_visible_env_ids = resolve_visible_env_indices(self._env_ids, self.cfg.max_visible_envs, num_envs) + num_visualized_envs = ( + len(self._resolved_visible_env_ids) if self._resolved_visible_env_ids is not None else num_envs + ) viewer_url = _viser_web_viewer_url(self.cfg.port) self._log_initialization_table( logger=logger, @@ -175,6 +182,8 @@ def step(self, dt: float) -> None: Args: dt: Simulation time-step in seconds. """ + from isaaclab_newton.physics import NewtonManager + if not self._is_initialized or self._viewer is None or self._scene_data_provider is None: return @@ -182,11 +191,28 @@ def step(self, dt: float) -> None: self._update_camera_from_usd_path() self._apply_pending_camera_pose() - self._state = self._scene_data_provider.get_newton_state() + self._state = NewtonManager.get_state() + num_envs = NewtonManager.get_num_envs() + self._sim_time += dt self._viewer.begin_frame(self._sim_time) - self._viewer.log_state(self._state) - self._viewer.end_frame() + try: + self._viewer.log_state(self._state) + if self.cfg.enable_markers: + self._render_markers(num_envs) + finally: + self._viewer.end_frame() + + def _render_markers(self, num_envs: int) -> None: + """Render marker overlays without letting them interrupt Viser body updates.""" + try: + render_newton_visualization_markers(self._viewer, self._resolved_visible_env_ids, num_envs=num_envs) + except Exception as exc: + if not self._warned_marker_render_failure: + logger.warning("[ViserVisualizer] Marker rendering failed; continuing body updates: %s", exc) + self._warned_marker_render_failure = True + else: + logger.debug("[ViserVisualizer] Marker rendering failed: %s", exc) def close(self) -> None: """Close viewer resources and finalize optional recording.""" @@ -223,8 +249,8 @@ def is_training_paused(self) -> bool: return False def supports_markers(self) -> bool: - """Viser backend currently does not expose Isaac Lab marker primitives.""" - return False + """Viser backend supports Isaac Lab markers through Newton viewer primitives.""" + return bool(self.cfg.enable_markers) def supports_live_plots(self) -> bool: """Viser backend currently does not expose Isaac Lab live-plot widgets.""" diff --git a/source/isaaclab_visualizers/setup.py b/source/isaaclab_visualizers/setup.py index fc120619787b..008fe15c8d6c 100644 --- a/source/isaaclab_visualizers/setup.py +++ b/source/isaaclab_visualizers/setup.py @@ -13,20 +13,27 @@ "numpy", ] +# Every Newton declaration in the repo must use the SAME extra spec (`newton[sim]`). +# Pip resolves a git-URL requirement once per URL: if any package declares bare +# `newton @ git+...` while another declares `newton[sim] @ git+...`, the first +# resolution wins and silently drops the `[sim]` extra. That breaks `isaaclab_newton` +# at import time because `mujoco` / `mujoco-warp` go missing. So even the rerun/viser +# extras — which don't use the MuJoCo solver directly — must pin `newton[sim]` to +# stay consistent with `isaaclab_newton`. EXTRAS_REQUIRE = { "kit": [], "newton": [ "warp-lang", - "newton @ git+https://github.com/newton-physics/newton.git@a27277ed49d6f307b8a1e4c394be7e1d14965a62", + "newton[sim] @ git+https://github.com/newton-physics/newton.git@v1.2.0rc2", "PyOpenGL-accelerate", "imgui-bundle>=1.92.5", ], "rerun": [ - "newton @ git+https://github.com/newton-physics/newton.git@a27277ed49d6f307b8a1e4c394be7e1d14965a62", + "newton[sim] @ git+https://github.com/newton-physics/newton.git@v1.2.0rc2", "rerun-sdk>=0.29.0", ], "viser": [ - "newton @ git+https://github.com/newton-physics/newton.git@a27277ed49d6f307b8a1e4c394be7e1d14965a62", + "newton[sim] @ git+https://github.com/newton-physics/newton.git@v1.2.0rc2", "viser>=1.0.16", ], } diff --git a/tools/changelog/cli.py b/tools/changelog/cli.py index 16ad551b4ca3..d316a416f89f 100644 --- a/tools/changelog/cli.py +++ b/tools/changelog/cli.py @@ -299,6 +299,28 @@ def validate(self) -> str | None: f"section(s) {', '.join(repr(s) for s in empty)} have no bullet entries — " "use ``* `` to start each entry, or remove the heading" ) + # Every line inside a section body must be a bullet (``* ``), a + # continuation (leading whitespace), or blank. A column-0 non-blank + # line that isn't a bullet terminates the list under RST rules and + # then sits as a paragraph adjacent to the next ``* `` — which the + # compile step splices into ``CHANGELOG.rst`` under the same + # ``^^^`` subheading and Sphinx then rejects with + # ``Unexpected indentation``. Catch it here before merge. + for section, lines in sections.items(): + for offset, line in enumerate(lines): + if not line.strip(): + continue + if line[0].isspace() or line.lstrip().startswith("*"): + continue + snippet = line.strip()[:80] + return ( + f"section {section!r} contains an orphan paragraph " + f"(non-bullet line {offset + 1}: {snippet!r}). Every line under " + "a section heading must start with ``* `` (new bullet) or whitespace " + "(continuation of the previous bullet). A flush-left paragraph here " + "splits the bullet list and Sphinx fails the doc build with " + "``Unexpected indentation``." + ) return None diff --git a/tools/changelog/test/invalid_content/3004.rst b/tools/changelog/test/invalid_content/3004.rst new file mode 100644 index 000000000000..57aa8df9bb87 --- /dev/null +++ b/tools/changelog/test/invalid_content/3004.rst @@ -0,0 +1,9 @@ +Added +^^^^^ + +* Added ``foo()`` to support feature X. + +This is a free-form paragraph that lives at column 0 inside the Added +section, neither a bullet nor a continuation of the previous bullet. +The bot would splice this verbatim into ``CHANGELOG.rst`` and Sphinx +then rejects the build with ``Unexpected indentation [docutils]``. diff --git a/tools/changelog/test/test_validate.py b/tools/changelog/test/test_validate.py index e02e1b95e990..fd1af7a31dfe 100644 --- a/tools/changelog/test/test_validate.py +++ b/tools/changelog/test/test_validate.py @@ -70,6 +70,14 @@ def test_validate_rejects_section_without_bullets_from_fixture(): assert err is not None and "bullet" in err.lower() +def test_validate_rejects_orphan_paragraph_from_fixture(): + """A flush-left paragraph between bullets / after the last bullet must be + rejected — the compile step would splice it verbatim into ``CHANGELOG.rst`` + and Sphinx then fails the doc build with ``Unexpected indentation``.""" + err = cli.Fragment(FIXTURES / "invalid_content" / "3004.rst").validate() + assert err is not None and "orphan" in err.lower() + + # --------------------------------------------------------------------------- # check_fragments — gate orchestration: immutability, slug uniqueness, and # the "PR must add at least one fragment per touched package" rule diff --git a/tools/conftest.py b/tools/conftest.py index bf92d62f6c46..5b844442c0a8 100644 --- a/tools/conftest.py +++ b/tools/conftest.py @@ -33,22 +33,22 @@ def pytest_ignore_collect(collection_path, config): on-disk cache is populated. """ -STARTUP_DEADLINE = 45 +STARTUP_DEADLINE = 120 """Seconds to wait for AppLauncher init or pytest collection before declaring a startup hang. AppLauncher prints ``[ISAACLAB] AppLauncher initialization complete`` to ``sys.__stderr__`` (never suppressed) when Kit finishes initializing, and pytest prints ``collected N items`` to stdout after collection. If neither appears -within this deadline the process is treated as hung. 45 s is above any -legitimate Kit startup (typically 30--60 s) while still catching real hangs -without wasting the full hard timeout. +within this deadline the process is treated as hung. Kit startup can exceed +60 s on cold CI workers, so this catches real startup hangs without killing +legitimate slow launches. """ STARTUP_HANG_RETRIES = 2 """Number of times to retry a test that hangs during startup before giving up.""" -TIMEOUT_RETRIES = 2 +TIMEOUT_RETRIES = 0 """Number of times to retry a test that reaches its hard timeout before giving up.""" SHUTDOWN_GRACE_PERIOD = 30 diff --git a/tools/template/templates/agents/rsl_rl_ppo_cfg b/tools/template/templates/agents/rsl_rl_ppo_cfg index 85970dfc2ce4..a29f0ae96833 100644 --- a/tools/template/templates/agents/rsl_rl_ppo_cfg +++ b/tools/template/templates/agents/rsl_rl_ppo_cfg @@ -5,7 +5,7 @@ from isaaclab.utils import configclass -from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg +from isaaclab_rl.rsl_rl import RslRlMLPModelCfg, RslRlOnPolicyRunnerCfg, RslRlPpoAlgorithmCfg @configclass @@ -14,13 +14,16 @@ class PPORunnerCfg(RslRlOnPolicyRunnerCfg): max_iterations = 150 save_interval = 50 experiment_name = "cartpole_direct" - policy = RslRlPpoActorCriticCfg( - init_noise_std=1.0, - actor_obs_normalization=False, - critic_obs_normalization=False, - actor_hidden_dims=[32, 32], - critic_hidden_dims=[32, 32], + actor = RslRlMLPModelCfg( + hidden_dims=[32, 32], activation="elu", + obs_normalization=False, + distribution_cfg=RslRlMLPModelCfg.GaussianDistributionCfg(init_std=1.0), + ) + critic = RslRlMLPModelCfg( + hidden_dims=[32, 32], + activation="elu", + obs_normalization=False, ) algorithm = RslRlPpoAlgorithmCfg( value_loss_coef=1.0, diff --git a/tools/template/templates/extension/config/extension.toml b/tools/template/templates/extension/config/extension.toml index dbe4b064fbc4..c23cf2de1287 100644 --- a/tools/template/templates/extension/config/extension.toml +++ b/tools/template/templates/extension/config/extension.toml @@ -33,3 +33,8 @@ name = "{{ name }}" # with rosdeps to be installed. If none, # leave it commented out. # ros_ws = "path/from/extension_root/to/ros_ws" +# TODO: Uncomment and list install_requires dependency names that should be upgraded +# after this extension is installed with ./isaaclab.sh --install. +# List package names only; version ranges, extras, and platform markers +# come from this extension's setup.py metadata. +# pip_upgrade_dependencies = ["example_package"] diff --git a/tools/test_settings.py b/tools/test_settings.py index 66832541e5cc..d06c282497c9 100644 --- a/tools/test_settings.py +++ b/tools/test_settings.py @@ -17,7 +17,7 @@ PER_TEST_TIMEOUTS = { - "test_articulation.py": 1500, + "test_articulation.py": 3000, "test_stage_in_memory.py": 1000, "test_imu.py": 1000, "test_environments.py": 10000, # This test runs through all the environments for 100 steps each @@ -62,6 +62,12 @@ "test_shadow_hand_vision_presets.py": 5000, "test_environments_newton.py": 5000, "test_surface_gripper.py": 3000, + # For some reason kitless rendering tests take much longer on CI than local machines. + # After we pin OVRTX to 0.3 we need to test whether it is still reproducible. + "test_rendering_cartpole_kitless.py": 2000, + "test_rendering_dexsuite_kuka_kitless.py": 2000, + "test_rendering_shadow_hand_kitless.py": 2000, + "test_contact_sensor.py": 2000, } """A dictionary of tests and their timeouts in seconds. diff --git a/tools/wheel_builder/res/python_packages.toml b/tools/wheel_builder/res/python_packages.toml index 1676fdc8f905..369c9ef899e1 100644 --- a/tools/wheel_builder/res/python_packages.toml +++ b/tools/wheel_builder/res/python_packages.toml @@ -22,13 +22,14 @@ pyproject.dependencies.all = [ # image processing "transformers==4.57.6", "einops", # needed for transformers, doesn't always auto-install - "warp-lang==1.12.0", + "warp-lang==1.13.0", "matplotlib>=3.10.3", # make sure this is consistent with isaac sim version "pillow==12.1.1", "botocore", # livestream - "starlette==0.49.1", # TODO: update starlette once Isaac Lab be released with Isaac Sim 6.0.0 + # range chosen to coexist with isaacsim 6.0 (isaacsim-kernel pulls fastapi==0.117.1 -> starlette<0.49.0) + "starlette>=0.46.0,<0.50", "omniverseclient==2.71.1.7015", # testing "pytest", @@ -40,7 +41,10 @@ pyproject.dependencies.all = [ # visualizers "imgui-bundle==1.92.4", "rerun-sdk>=0.29.0", - "viser>=1.0.16", + # viser is intentionally not a base dep: viser>=1.0.16 pulls websockets>=13.1, + # but isaacsim-kernel==6.0.0.0 pins websockets==12.0. Users who want the viser + # visualizer install isaaclab[viser] explicitly (see the optional-dependencies + # block below). "typing_extensions==4.12.2", "lazy_loader>=0.4", "pin ; platform_system == 'Linux'", @@ -82,10 +86,8 @@ pyproject.optional-dependencies.all = [ # https://github.com/isaac-sim/IsaacLab/blob/main/source/isaaclab_newton/setup.py # ================================================================================ { "newton" = [ - "warp-lang==1.12.0", - "mujoco==3.6.0", - "mujoco-warp==3.6.0", - "newton @ git+https://github.com/newton-physics/newton.git@a27277ed49d6f307b8a1e4c394be7e1d14965a62", + "warp-lang==1.13.0", + "newton[sim] @ git+https://github.com/newton-physics/newton.git@v1.2.0rc2", "PyOpenGL-accelerate==3.1.10" ] }, # ================================================================================ @@ -98,6 +100,12 @@ pyproject.optional-dependencies.all = [ # { "rl_games" = ["rl-games==1.6.1"] }, # TODO: re-enable when rl-games Python package supports Python 3.11 { "rsl-rl" = ["rsl-rl-lib==3.1.2", "onnxscript>=0.5"] }, { "rsl_rl" = ["rsl-rl-lib==3.1.2", "onnxscript>=0.5"] }, + # ================================================================================ + # https://github.com/isaac-sim/IsaacLab/blob/main/source/isaaclab_visualizers/setup.py + # ================================================================================ + # Viser visualizer (opt-in: viser pulls websockets>=13.1 which collides with + # isaacsim-kernel==6.0.0.0's websockets==12.0; do not include in [all]). + { "viser" = ["viser>=1.0.16"] }, # RL libraries (all) { "all" = [ "stable-baselines3>=2.6",