diff --git a/.github/PULL_REQUEST_TEMPLATE.md b/.github/PULL_REQUEST_TEMPLATE.md index ee9fa4ebdc5..c19d35fb7e7 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 69cbc8bd530..e54d71ca9d7 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_context.py b/source/isaaclab/isaaclab/sim/simulation_context.py index 89be3163359..607221bd487 100644 --- a/source/isaaclab/isaaclab/sim/simulation_context.py +++ b/source/isaaclab/isaaclab/sim/simulation_context.py @@ -22,7 +22,8 @@ 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 BaseSceneDataProvider, PhysicsEvent, PhysicsManager, SceneDataProvider from isaaclab.physics.scene_data_requirements import ( SceneDataRequirement, resolve_scene_data_requirements, @@ -175,11 +176,10 @@ def __init__(self, cfg: SimulationCfg | None = None): self._scene_data_provider: BaseSceneDataProvider | None = None 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] = {} + # 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 self._visualizer_step_counter = 0 # Default visualization dt used before/without visualizer initialization. physics_dt = getattr(self.cfg.physics, "dt", None) @@ -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: @@ -633,18 +641,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]: @@ -753,6 +761,11 @@ def update_visualizers(self, dt: float, skip_app_pumping: bool = False) -> None: self.update_scene_data_provider() + # 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: try: 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 be7bd6e7074..18ffcaf37c9 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 78aaf1b15c9..87fc48de4d3 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 b86e98599fc..3158625219c 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 93142ddab38..0dd023c2998 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 240a0ff0074..b76077b38be 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 dde9aec6d90..0c9a7be478e 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 7a803ad0e0d..0adf9215f81 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 285dd037306..f6f087cfa12 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 d9b7d9ed0c3..e335393f7d9 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 7730c3dd735..88392d54b2a 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 9f69a66aa04..ddc627171ae 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 59605f2797c..f44f1288e01 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/warp/fabric.py b/source/isaaclab/isaaclab/utils/warp/fabric.py index a48f773f499..0754c917a9a 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) @@ -163,6 +176,176 @@ 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: + if broadcast_positions: + index = 0 + else: + index = 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: + if broadcast_orientations: + index = 0 + else: + index = 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: + if broadcast_scales: + index = 0 + else: + index = 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/setup.py b/source/isaaclab/setup.py index 67c18c4c62d..cfced25a24a 100644 --- a/source/isaaclab/setup.py +++ b/source/isaaclab/setup.py @@ -30,19 +30,20 @@ # procedural-generation "trimesh", "pyglet>=2.1.6,<3", - "mujoco==3.6.0", - "mujoco-warp==3.6.0", + "mujoco==3.8.0", + "mujoco-warp==3.8.0.1", # 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 +55,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 +69,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/cli/test_install_commands.py b/source/isaaclab/test/cli/test_install_commands.py index a7c89ccd9d5..f773d9f026c 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_operational_space.py b/source/isaaclab/test/controllers/test_operational_space.py index 63c1633c5f7..badf51543e1 100644 --- a/source/isaaclab/test/controllers/test_operational_space.py +++ b/source/isaaclab/test/controllers/test_operational_space.py @@ -1498,8 +1498,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 +1546,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 diff --git a/source/isaaclab/test/envs/test_scale_randomization.py b/source/isaaclab/test/envs/test_scale_randomization.py index af5dc220e63..ec4c6cd42a9 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 31b577db634..626e4d8e44d 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 daed8e95773..99c93fec732 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_multi_mesh_ray_caster_camera.py b/source/isaaclab/test/sensors/test_multi_mesh_ray_caster_camera.py index 8657c938c69..7e7efe16d09 100644 --- a/source/isaaclab/test/sensors/test_multi_mesh_ray_caster_camera.py +++ b/source/isaaclab/test/sensors/test_multi_mesh_ray_caster_camera.py @@ -752,11 +752,11 @@ def test_output_equal_to_usd_camera_when_intrinsics_set(setup_simulation): # set camera position camera_warp.set_world_poses_from_view( - eyes=torch.tensor([[0.0, 0.0, 5.0]], device=camera_warp.device), + eyes=torch.tensor([[0.1, 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.0, 0.0, 5.0]], device=camera_usd.device), + eyes=torch.tensor([[0.1, 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/sensors/test_ray_caster_camera.py b/source/isaaclab/test/sensors/test_ray_caster_camera.py index a913d38dd83..75273493693 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.1, 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.1, 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 1f8af90387b..1f526ac7458 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_physx_scene_data_provider_visualizer_contract.py b/source/isaaclab/test/sim/test_physx_scene_data_provider_visualizer_contract.py index 979d66cc4a7..d8e640c394f 100644 --- 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 @@ -78,35 +78,33 @@ def test_get_newton_model_returns_model_when_sync_enabled(stub_provider): 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. +def test_build_from_clone_plan_populates_provider_state(stub_provider, newton_stub): + """Building from a flat clone plan 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. + Asserts the provider consumes the single source-of-truth ``(sources, + destinations, mask)`` contract directly and reads per-env positions 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), + plan = ClonePlan( + sources=( + "/World/envs/env_0/Object", + "/World/envs/env_1/Object", + "/World/envs/env_0/Robot", ), - "/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), + destinations=("/World/envs/env_{}/Object", "/World/envs/env_{}/Object", "/World/envs/env_{}/Robot"), + # object 0 -> env 0, 2 ; object 1 -> env 1, 3 ; robot -> all envs + clone_mask=torch.tensor( + [[True, False, True, False], [False, True, False, True], [True, True, True, True]], dtype=torch.bool ), - } - stub_provider._simulation_context = SimpleNamespace(get_clone_plans=lambda: plans) + ) + stub_provider._simulation_context = SimpleNamespace(get_clone_plan=lambda: plan) stub_provider._stage = _silent_stage() - stub_provider._build_newton_model_from_clone_plans() + stub_provider._build_newton_model_from_clone_plan() assert stub_provider._newton_model is newton_stub.model assert stub_provider._newton_state is newton_stub.state_obj @@ -116,7 +114,6 @@ def test_build_from_clone_plans_populates_provider_state(stub_provider, newton_s 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", @@ -127,41 +124,35 @@ def test_build_from_clone_plans_populates_provider_state(stub_provider, newton_s 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: {}) +def test_build_from_clone_plan_missing_sets_error_state(stub_provider): + """When no clone plan is published, model/state stay unset.""" + stub_provider._simulation_context = SimpleNamespace(get_clone_plan=lambda: None) stub_provider._stage = object() - stub_provider._build_newton_model_from_clone_plans() + stub_provider._build_newton_model_from_clone_plan() 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. +def test_build_from_clone_plan_skips_unused_source_rows(stub_provider, newton_stub): + """A source 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. + leaves some prototypes unused), 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) + plan = ClonePlan( + sources=("/World/envs/env_0/Object", "/World/envs/env_1/Object", "/World/envs/env_0/Object"), + destinations=("/World/envs/env_{}/Object",) * 3, + clone_mask=torch.tensor([[True, False], [False, True], [False, False]], dtype=torch.bool), + ) + stub_provider._simulation_context = SimpleNamespace(get_clone_plan=lambda: plan) stub_provider._stage = _silent_stage() - stub_provider._build_newton_model_from_clone_plans() + stub_provider._build_newton_model_from_clone_plan() assert stub_provider._last_newton_model_build_source == "built" kw = newton_stub.calls[-1] @@ -170,8 +161,8 @@ def test_build_from_clone_plans_skips_unused_prototype_rows(stub_provider, newto 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. +def test_build_from_clone_plan_uses_destination_template_for_env_lookup(stub_provider, newton_stub): + """Env-origin lookup uses the plan's destination 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_``. @@ -182,40 +173,27 @@ 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) + plan = ClonePlan( + sources=("/Stage/scenes/env_0/Object",), + destinations=("/Stage/scenes/env_{}/Object",), + clone_mask=torch.ones((1, 3), dtype=torch.bool), + ) + stub_provider._simulation_context = SimpleNamespace(get_clone_plan=lambda: plan) stub_provider._stage = SimpleNamespace(GetPrimAtPath=_get_prim) - stub_provider._build_newton_model_from_clone_plans() + stub_provider._build_newton_model_from_clone_plan() 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"], +def test_clone_plan_carries_flat_replication_contract(): + """``ClonePlan`` contains only sources, destinations, and the clone mask.""" + plan = ClonePlan( + sources=("/World/envs/env_0/Object",), + destinations=("/World/envs/env_{}/Object",), 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 + assert plan.sources == ("/World/envs/env_0/Object",) + assert plan.destinations == ("/World/envs/env_{}/Object",) + assert plan.clone_mask.shape == (1, 4) diff --git a/source/isaaclab/test/sim/test_schemas.py b/source/isaaclab/test/sim/test_schemas.py index 1acda3b149c..1fb03ede143 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 00000000000..11e6e5d1ba2 --- /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 1f86f32872c..1c40e21cb54 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 @@ -94,6 +101,9 @@ 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) @@ -168,6 +178,26 @@ def test_update_visualizers_handles_training_pause_loop(): assert viz.step_calls == [0.0, 0.2] +def test_vis_marker_registry_dispatch_allows_callback_mutation(): + registry = VisMarkerRegistry() + calls = [] + + def _remove_other_callback(event): + calls.append(("remove_other", event)) + registry.remove_callback("other") + + def _other_callback(event): + calls.append(("other", event)) + + 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: def __init__(self): self._metadata = {"num_envs": 4} @@ -229,6 +259,214 @@ def _fake_create_viewer(self, record_to_viser: str | None, metadata: dict | None 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) + + 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( ("cfg_max_visible_envs", "expected_visible"), [ @@ -385,6 +623,112 @@ 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) + + 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,7 +793,7 @@ def _make_context_with_settings( ctx._visualizers = [] ctx._scene_data_provider = _FakeProvider() ctx._scene_data_requirements = None - ctx._clone_plans = {} + ctx._clone_plan = None ctx._visualizer_step_counter = 0 ctx._viz_dt = 0.01 ctx.get_setting = lambda name: settings.get(name) diff --git a/source/isaaclab/test/sim/test_spawn_wrappers.py b/source/isaaclab/test/sim/test_spawn_wrappers.py index c053a6362f4..69ad8b10572 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 e0d85b14eb2..6bff0b31e26 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() diff --git a/source/isaaclab/test/utils/test_wrench_composer_integration.py b/source/isaaclab/test/utils/test_wrench_composer_integration.py index bb195e85603..acb1682bde0 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 8d47b003369..ca8f22be437 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_assets/config/extension.toml b/source/isaaclab_assets/config/extension.toml index 1bf36d627e3..055e3a5ff2f 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 1d676f70a27..e9eda582221 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 32e01adc93b..4f81f8a0645 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 7a02c6eff29..8e4f692ca6d 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 7fd2b5d139e..bdeec969ff5 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 721c705a598..24981d6deca 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 346b47e5522..ea09129849f 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 036a817fbfb..da9053107b7 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 00000000000..15ad731d8b9 --- /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 00000000000..648fa27731d --- /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 00000000000..908f05745a8 --- /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 00000000000..6a1f6c7db7e --- /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 00000000000..2ea176bde29 --- /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 00000000000..bcf0f9f3ca1 --- /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 00000000000..231c1571071 --- /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 00000000000..3a279f7ddb8 --- /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 00000000000..6a89ec0bdde --- /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 00000000000..82fff709042 --- /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 00000000000..e2df30f8aa7 --- /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 00000000000..14dc1ff970c --- /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 00000000000..13ef9814d26 --- /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 412db0ce4d8..497bd981d0d 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 8203016432d..ede3ae69e2d 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 5ed60f190c4..d529692a848 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 3a464b8fce8..d06242f80ab 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 89368c53210..1defd1a0d50 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 720a85223aa..12ff6cfd3e8 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 00000000000..840cc62440b --- /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 00000000000..c24c8e5dc87 --- /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 9bcfc075338..6e5bee6fb01 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 5f19d1fb8c5..2131ff67299 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 125f2e61a42..20c82e58269 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 8c558b92594..5ace5168a9c 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 f53461c3fc6..6646522f5f1 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.6" # Description category = "isaaclab" diff --git a/source/isaaclab_mimic/docs/CHANGELOG.rst b/source/isaaclab_mimic/docs/CHANGELOG.rst index 782cf6f7d22..08da411a579 100644 --- a/source/isaaclab_mimic/docs/CHANGELOG.rst +++ b/source/isaaclab_mimic/docs/CHANGELOG.rst @@ -1,6 +1,26 @@ Changelog --------- +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 17f1264a6b5..56b86cc6034 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/locomanipulation_sdg/scene_utils.py b/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/scene_utils.py index a0eb00fb4a5..4ba068fc8f5 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_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 6fed4677a47..00000000000 --- 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 a23b7c7322b..00000000000 --- 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 1ad8a926583..00000000000 --- 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 30eb959531c..00000000000 --- 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 1c3efcb5c33..00000000000 --- 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 0a8eed8000c..989e0b498db 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.8.0" # 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 d9626a89047..d841aa46d79 100644 --- a/source/isaaclab_newton/docs/CHANGELOG.rst +++ b/source/isaaclab_newton/docs/CHANGELOG.rst @@ -1,6 +1,147 @@ Changelog --------- +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 c3c6eca044f..ff04b96c63c 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 a22ba73e172..2da95b49b21 100644 --- a/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation_data.py +++ b/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation_data.py @@ -342,7 +342,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). """ 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 7e31fa22b60..b93c9075393 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 0e9ecc8a41d..43e719d3a58 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 8c499d75396..b11415d4823 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 34cd35de4fa..544756858d5 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,7 +119,11 @@ 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. @@ -149,8 +155,8 @@ def _rename_builder_labels( 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 +201,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/newton_manager.py b/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py index 2bb2f1f3739..97535fb8a32 100644 --- a/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py +++ b/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py @@ -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) @@ -1351,7 +1351,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 a02d820f295..0ba3558c350 100644 --- a/source/isaaclab_newton/isaaclab_newton/renderers/newton_warp_renderer.py +++ b/source/isaaclab_newton/isaaclab_newton/renderers/newton_warp_renderer.py @@ -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,6 +155,8 @@ def __init__(self, cfg: NewtonWarpRendererCfg): if merged != current_req: sim.update_scene_data_requirements(merged) + def initialize(self) -> None: + """Post-physics setup: read the built Newton model and construct the sensor.""" newton_model = self.get_scene_data_provider().get_newton_model() if newton_model is None: raise RuntimeError( @@ -164,16 +169,24 @@ def __init__(self, cfg: NewtonWarpRendererCfg): self.newton_sensor = newton.sensors.SensorTiledCamera( 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 newton_model.shape_count > 0 and newton_model.bvh_shapes is None: + newton.geometry.build_bvh_shape(newton_model, 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. @@ -220,8 +233,13 @@ 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 = self.get_scene_data_provider().get_newton_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, 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 02850e9cc90..43e878fecbf 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( @@ -350,25 +350,49 @@ 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] + return body_labels[int(idx)].split("/")[-1] if kind_name == "SHAPE" or kind_value == 1: - return shape_labels[idx].split("/")[-1] + return shape_labels[int(idx)].split("/")[-1] return "MATCH_ANY" - flat_sensing = [obj for world_objs in self.contact_view.sensing_objs for obj in world_objs] + def flatten_metadata(values): + if isinstance(values, wp.array): + values = values.numpy() + flat_values = np.asarray(values, dtype=object).reshape(-1).tolist() + if flat_values and isinstance(flat_values[0], list | tuple | np.ndarray): + return [ + value + for nested_values in flat_values + for value in np.asarray(nested_values, dtype=object).reshape(-1).tolist() + ] + return flat_values + + flat_sensing = list( + zip( + flatten_metadata(self.contact_view.sensing_obj_idx), + flatten_metadata(self.contact_view.sensing_obj_type), + ) + ) self._sensor_names = [get_name(idx, kind) for idx, kind in flat_sensing] # 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] + flat_counterparts = list( + zip( + flatten_metadata(self.contact_view.counterpart_indices), + flatten_metadata(self.contact_view.counterpart_type), + ) + ) 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) + 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 - # 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)) + # 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 +437,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 324f0106682..81fef0bcab6 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/sim/schemas/__init__.py b/source/isaaclab_newton/isaaclab_newton/sim/schemas/__init__.py new file mode 100644 index 00000000000..80f943ad46e --- /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 00000000000..6dae7b8cf2b --- /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 00000000000..f0065daa867 --- /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/setup.py b/source/isaaclab_newton/setup.py index 2e0b87f1754..4621e77f879 100644 --- a/source/isaaclab_newton/setup.py +++ b/source/isaaclab_newton/setup.py @@ -38,10 +38,10 @@ def run(self): EXTRAS_REQUIRE = { "all": [ "prettytable==3.3.0", - "mujoco==3.6.0", - "mujoco-warp==3.6.0", + "mujoco==3.8.0", + "mujoco-warp==3.8.0.1", "PyOpenGL-accelerate==3.1.10", - "newton @ git+https://github.com/newton-physics/newton.git@a27277ed49d6f307b8a1e4c394be7e1d14965a62", + "newton @ git+https://github.com/newton-physics/newton.git@v1.2.0rc2", ], } diff --git a/source/isaaclab_newton/test/assets/test_articulation.py b/source/isaaclab_newton/test/assets/test_articulation.py index a5eacf95045..cd0f2dcb03b 100644 --- a/source/isaaclab_newton/test/assets/test_articulation.py +++ b/source/isaaclab_newton/test/assets/test_articulation.py @@ -196,7 +196,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 +220,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( @@ -379,7 +379,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 +428,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 +478,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 +535,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 +593,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 +642,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 +696,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]) @@ -749,7 +742,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 +774,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 +798,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 +908,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 +972,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 +993,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 +1051,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 +1146,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 +1206,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 +1300,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 +1362,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 +1397,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 +1430,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 +1491,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 +1502,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 +1552,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 +1604,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 +1615,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 +1681,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 +1701,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 +1719,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 +1765,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 +1890,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 +1980,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 +1999,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 +2017,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 +2328,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"]) diff --git a/source/isaaclab_newton/test/assets/test_rigid_object.py b/source/isaaclab_newton/test/assets/test_rigid_object.py index 3b8e3aa9bf8..ba2c47e24f9 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 0873da8ecf2..5ee47046954 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/sensors/test_frame_transformer.py b/source/isaaclab_newton/test/sensors/test_frame_transformer.py index 513ba9dda91..236fb53f25d 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 00000000000..67ee6265d82 --- /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 455768ad5a5..00000000000 --- 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 1de2259dc2c..00000000000 --- 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 ba8f5046c4e..c16250b0175 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.7" 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 e8afeda30bd..f0962359544 100644 --- a/source/isaaclab_ov/docs/CHANGELOG.rst +++ b/source/isaaclab_ov/docs/CHANGELOG.rst @@ -1,6 +1,88 @@ Changelog --------- +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 5d1782373d8..99ad0554048 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...") @@ -364,7 +371,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. 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 c287f125763..0c162691641 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 546cb8acc1c..00000000000 --- 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 8648b7fa958..1ad422a1df3 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 = "0.1.4" # 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 b2eb969d784..cea22cdc70c 100644 --- a/source/isaaclab_ovphysx/docs/CHANGELOG.rst +++ b/source/isaaclab_ovphysx/docs/CHANGELOG.rst @@ -1,6 +1,29 @@ Changelog --------- +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/articulation/articulation.py b/source/isaaclab_ovphysx/isaaclab_ovphysx/assets/articulation/articulation.py index f3919d56da7..bea4345ca5b 100644 --- a/source/isaaclab_ovphysx/isaaclab_ovphysx/assets/articulation/articulation.py +++ b/source/isaaclab_ovphysx/isaaclab_ovphysx/assets/articulation/articulation.py @@ -1759,7 +1759,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 +1780,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 10c7e4b7ecd..e5be6c05328 100644 --- a/source/isaaclab_ovphysx/isaaclab_ovphysx/assets/articulation/articulation_data.py +++ b/source/isaaclab_ovphysx/isaaclab_ovphysx/assets/articulation/articulation_data.py @@ -1684,10 +1684,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 +1710,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 +1720,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 +1730,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/cloner/ovphysx_replicate.py b/source/isaaclab_ovphysx/isaaclab_ovphysx/cloner/ovphysx_replicate.py index 7c46a6060b8..d89a45280a5 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 9063078e45b..6caad37ab7b 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 @@ -202,7 +203,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. diff --git a/source/isaaclab_ovphysx/test/assets/test_articulation_data.py b/source/isaaclab_ovphysx/test/assets/test_articulation_data.py index 16bb99a4d6c..390e5defa0f 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_physx/changelog.d/fix-fabric-local-matrix.rst b/source/isaaclab_physx/changelog.d/fix-fabric-local-matrix.rst new file mode 100644 index 00000000000..43f2eaae0ee --- /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 4f6915f6b47..00000000000 --- 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 1fe6a600bb9..00000000000 --- 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 eada0bbb809..00000000000 --- 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 5c63b0e6322..4e00f31716d 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.6.3" # 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 14425eb7486..95e059b045b 100644 --- a/source/isaaclab_physx/docs/CHANGELOG.rst +++ b/source/isaaclab_physx/docs/CHANGELOG.rst @@ -1,6 +1,153 @@ Changelog --------- +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 913914e29f3..6258b5c5b8e 100644 --- a/source/isaaclab_physx/isaaclab_physx/assets/articulation/articulation.py +++ b/source/isaaclab_physx/isaaclab_physx/assets/articulation/articulation.py @@ -1684,16 +1684,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 +1710,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 +1801,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 +1827,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 +1858,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 +1870,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 +1925,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 +1937,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 +1963,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 +1973,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 +2033,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 +2043,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 8c2056d9cbf..3d7e6e9cb48 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: 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 6d07ddbf1bc..2031ded53b2 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 590289feb65..6662582dac7 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 d90d413bffa..dcc5cc6d967 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/scene_data_providers/physx_scene_data_provider.py b/source/isaaclab_physx/isaaclab_physx/scene_data_providers/physx_scene_data_provider.py index 9a88660da49..ec4c64d8f8c 100644 --- 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 @@ -42,8 +42,7 @@ class PhysxSceneDataProvider(BaseSceneDataProvider): - 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) + - Newton model/state (built locally from the scene's :class:`ClonePlan` when required) """ # ---- Environment discovery / metadata ------------------------------------------------- @@ -129,7 +128,7 @@ def __init__(self, stage, simulation_context) -> None: self._last_newton_model_build_elapsed_ms: float | None = None if self._needs_newton_sync: - self._build_newton_model_from_clone_plans() + self._build_newton_model_from_clone_plan() self._setup_rigid_body_view() # ---- Newton model + PhysX view setup -------------------------------------------------- @@ -150,53 +149,55 @@ def _refresh_newton_model_if_needed(self) -> None: 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._build_newton_model_from_clone_plan() 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"``. + def _build_newton_model_from_clone_plan(self) -> None: + """Build Newton model and state from the scene's :class:`ClonePlan`. + + Reads the plan :meth:`InteractiveScene.clone_environments` publishes on + :class:`SimulationContext`, validates the flat ``(sources, destinations, mask)`` + shape :func:`isaaclab_newton.cloner.newton_visualizer_prebuild` expects, and + caches the resulting model/state. Per-env positions are read off + ``xformOp:translate`` on the env-level prims derived from the first destination + 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.") + plan = self._simulation_context.get_clone_plan() + if plan is None: + raise RuntimeError("No clone plan 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]}") + if len(plan.sources) != len(plan.destinations): + raise RuntimeError( + f"Clone plan sources and destinations disagree: {len(plan.sources)} != {len(plan.destinations)}" + ) + if plan.clone_mask.dim() != 2 or plan.clone_mask.size(0) != len(plan.sources): + raise RuntimeError( + f"Clone plan mask shape {tuple(plan.clone_mask.shape)} does not match {len(plan.sources)} sources." + ) + + # Drop all-False rows (possible when ``num_prototypes > num_envs``). 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]) + for i, (source_path, destination) in enumerate(zip(plan.sources, plan.destinations)): + if not plan.clone_mask[i].any(): + continue + sources.append(source_path) + destinations.append(destination) + mask_rows.append(plan.clone_mask[i : i + 1]) if not sources: - raise RuntimeError("All clone-plan prototype rows are empty.") + raise RuntimeError("All clone-plan source rows are empty.") mask = torch.cat(mask_rows, dim=0) + num_envs = plan.clone_mask.size(1) # 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] + "{}" + env_path_template = destinations[0].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)) @@ -239,7 +240,7 @@ def _build_newton_model_from_clone_plans(self) -> None: self._clear_newton_model_state() except Exception as exc: source = "error" - logger.error("[PhysxSceneDataProvider] Failed to build Newton model from clone plans: %s", exc) + logger.error("[PhysxSceneDataProvider] Failed to build Newton model from clone plan: %s", exc) self._clear_newton_model_state() finally: self._last_newton_model_build_elapsed_ms = (time.perf_counter() - start_t) * 1000.0 diff --git a/source/isaaclab_physx/isaaclab_physx/sim/__init__.pyi b/source/isaaclab_physx/isaaclab_physx/sim/__init__.pyi index abc8d0087af..9a522d96ff8 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 bb0e51a19b4..6d2a05bf803 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 6579fee6356..e6bdea28d24 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 6e8a48b2f11..1a3e833d61d 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 35b066fa873..2c8121a5cbe 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 de65e850179..697109c7d03 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 @@ -3,16 +3,38 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""PhysX FrameView with Fabric GPU acceleration.""" +"""PhysX FrameView with Fabric GPU acceleration. + +Design: + +* Three persistent ``PrimSelection`` instances differing only in per-attribute access + mode (one for each of {trans_ro, world_rw, local_rw}). +* ``omni:fabric:localMatrix`` is read/written directly — no software composition of + ``inv(parent_world) * child_world`` for ``get_local_poses``/``set_local_poses``. +* View → Fabric index mapping is an integer Warp array computed from the selection's + ordered path list (``selection.GetPaths()``); no custom attributes are written to + prims. +* Read/write happens via ``wp.indexedfabricarray``, so the view-to-fabric mapping is + baked into the array itself and the kernels just dereference ``ifa[view_index]``. +* World ↔ local consistency is maintained: + - After ``set_local_poses``: the view is marked dirty; the next world read + fires a Warp kernel that recomputes ``child_world = parent_world * + child_local`` for this view's children. Tracking is per-view so that + multiple views on the same stage don't clear each other's dirty flag. + - After ``set_world_poses``: a Warp kernel recomputes child localMatrix from + parent worldMatrix on the fly using a parent indexed fabric array, so the + next ``get_local_poses`` returns consistent values. +""" from __future__ import annotations import logging +from typing import ClassVar import torch import warp as wp -from pxr import Usd +from pxr import Usd, UsdGeom import isaaclab.sim as sim_utils from isaaclab.app.settings_manager import SettingsManager @@ -23,6 +45,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). @@ -44,54 +72,132 @@ 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. + fallback and non-accelerated operations (visibility, scales when + Fabric is disabled). + + When Fabric is enabled, world-pose, local-pose, and scale operations + use Warp kernels operating on ``omni:fabric:worldMatrix`` and + ``omni:fabric:localMatrix``. All other operations delegate to the + internal USD view. + + Every accessor for a selection's indexed fabric array + (:meth:`_get_world_ro_array`, :meth:`_get_local_ro_array`, + :meth:`_get_world_rw_array`, :meth:`_get_local_rw_array`, + :meth:`_get_parent_world_ro_array`) calls :meth:`PrepareForReuse` on its + backing ``PrimSelection`` to detect Fabric topology changes that would + require rebuilding the view→fabric index mapping. ``PrepareForReuse`` + is idempotent and cheap in the steady state (returns ``False`` with no + rebuild), so the per-access cost is negligible. The two sync helpers + (:meth:`_sync_local_from_world`, :meth:`_sync_world_from_local_if_dirty`) + refresh ``trans_sel_ro`` at most once per call to avoid the redundant + USDRT round-trip that would otherwise occur when ``_world_ifa_ro``, + ``_local_ifa_ro`` and ``_parent_world_ifa_ro`` are read consecutively. Pose getters return :class:`~isaaclab.utils.warp.ProxyArray`. Setters accept ``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)`` so + # that a recycled ``stage_id`` paired with a fresh ``fabric_id`` never reuses a + # stale handle. ``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 +242,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 +276,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 +292,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 +310,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 +393,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 +426,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 +440,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 +457,390 @@ 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_device = "cuda:0" + 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." + ) + 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)`` so a recycled ``stage_id`` paired with a fresh + # Fabric attachment never returns a stale handle. 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 + cache_key = (self._stage_id, fabric_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. + 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), + self._fabric_empty_2d_array_sentinel, + 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]] = [] + for path in unique_parent_paths: + prim = usd_stage.GetPrimAtPath(path) + tf = xform_cache.GetLocalToWorldTransform(prim) + 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)]) + 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_unit_scale = wp.array( + [[1.0, 1.0, 1.0]] * len(unique_parent_paths), + 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_unit_scale, + 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 + 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 1e917e938c2..9cc172addf5 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 @ git+https://github.com/newton-physics/newton.git@v1.2.0rc2", ], } diff --git a/source/isaaclab_physx/test/assets/test_articulation.py b/source/isaaclab_physx/test/assets/test_articulation.py index 3687dae5961..227c091a165 100644 --- a/source/isaaclab_physx/test/assets/test_articulation.py +++ b/source/isaaclab_physx/test/assets/test_articulation.py @@ -92,7 +92,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 +116,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( @@ -204,7 +204,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 +260,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 +316,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 +381,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 +445,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 +499,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 +559,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. @@ -618,7 +611,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 +640,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 +665,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 +772,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 +833,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 +856,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 +913,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 +1007,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 +1066,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 +1159,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 +1221,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 +1254,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 +1289,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 +1340,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 +1355,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 +1398,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 +1408,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 +1445,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 +1460,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 +1519,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 +1536,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 +1554,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 +1602,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 +1727,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 +1805,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 +1823,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 +1842,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 +2092,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) diff --git a/source/isaaclab_physx/test/assets/test_rigid_object.py b/source/isaaclab_physx/test/assets/test_rigid_object.py index 6a2211787e1..b7f422c4f2f 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 8401cc395a0..f42f6dfcb08 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 e85a4a8415c..c075821bb98 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 00772e6cb0d..685d9204b3e 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 631e9ba118d..28559f9b6da 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 4bfba07d99e..f90c740f17e 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 4376c0e0b8e..5d36edae3f9 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,254 @@ 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) 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 8a9a65b18d7..00000000000 --- 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 6b5ae668f03..df9fe2b0361 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 0c4c4323ced..ad62d198ad0 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 ae311dd1071..00000000000 --- 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 9efbf82b8bf..00000000000 --- 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 94a128b6416..00000000000 --- 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 a23b7c7322b..00000000000 --- 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 cd4dc7e674d..00000000000 --- 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 e69de29bb2d..00000000000 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 59e71ddc398..00000000000 --- 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 273ae57d2cb..219e89c4eb2 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.5.37" # Description title = "Isaac Lab Environments" diff --git a/source/isaaclab_tasks/docs/CHANGELOG.rst b/source/isaaclab_tasks/docs/CHANGELOG.rst index 2044f807afc..6c3de163c11 100644 --- a/source/isaaclab_tasks/docs/CHANGELOG.rst +++ b/source/isaaclab_tasks/docs/CHANGELOG.rst @@ -1,6 +1,106 @@ Changelog --------- +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/cartpole/cartpole_camera_presets_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/direct/cartpole/cartpole_camera_presets_env_cfg.py index 4c27674ff7c..00d11f233ff 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/inhand_manipulation/inhand_manipulation_env.py b/source/isaaclab_tasks/isaaclab_tasks/direct/inhand_manipulation/inhand_manipulation_env.py index 5969d8c9c4b..bd178f3745e 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/manager_based/drone_arl/mdp/__init__.pyi b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/mdp/__init__.pyi index d54142b0609..3085421b05d 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 a9072367c83..5765177a465 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 00000000000..3fc33a9328b --- /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 00000000000..992521fdd47 --- /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 c8b8048bd68..e084eabcd01 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 ce635cc544d..a691df9e2d1 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_tasks/isaaclab_tasks/manager_based/drone_arl/navigation/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/navigation/__init__.py new file mode 100644 index 00000000000..a2a3a87e623 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/navigation/__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 + +"""Drone NTNU navigation environments.""" diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/navigation/config/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/navigation/config/__init__.py new file mode 100644 index 00000000000..61d66a54093 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/drone_arl/navigation/config/__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 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 00000000000..7e798426040 --- /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 00000000000..460a3056908 --- /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 00000000000..078d89875f7 --- /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 00000000000..aeddab56e5f --- /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 00000000000..c1465d64bef --- /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 00000000000..1990750eefc --- /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 00000000000..6b4039e254a --- /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 00000000000..711a9e99074 --- /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, size=1, 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 00000000000..c42610dc10b --- /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/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 a78e4a15116..de61f13b8a9 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/manipulation/assemble_trocar/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/assemble_trocar/__init__.py new file mode 100644 index 00000000000..624d0226981 --- /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 00000000000..cd8c26c840a --- /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 00000000000..40594872603 --- /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 00000000000..540b0edbc3a --- /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 00000000000..b130a12a8a5 --- /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 00000000000..81c60741b78 --- /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 numpy as np + +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 * np.pi / 180.0, + "left_hand_middle_0_joint": -60.0 * np.pi / 180.0, + "left_hand_thumb_0_joint": 0.0, + "left_hand_index_1_joint": -40.0 * np.pi / 180.0, + "left_hand_middle_1_joint": -40.0 * np.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 * np.pi / 180.0, + "right_hand_middle_0_joint": 60.0 * np.pi / 180.0, + "right_hand_thumb_0_joint": 0.0, + "right_hand_index_1_joint": 40.0 * np.pi / 180.0, + "right_hand_middle_1_joint": 40.0 * np.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 00000000000..50e58134f14 --- /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_tasks/isaaclab_tasks/manager_based/manipulation/assemble_trocar/mdp/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/assemble_trocar/mdp/__init__.py new file mode 100644 index 00000000000..d428ed46f7b --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/assemble_trocar/mdp/__init__.py @@ -0,0 +1,10 @@ +# 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 + +"""MDP utilities for the assemble_trocar task.""" + +from isaaclab.utils.module import lazy_export + +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 00000000000..92214471ac0 --- /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 00000000000..06c037ba3d6 --- /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 00000000000..504d9caba67 --- /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 00000000000..12b70ae473b --- /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/dexsuite/mdp/rewards.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/mdp/rewards.py index 8a5a013ea84..9e8b084afba 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/pick_place/mdp/__init__.pyi b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/pick_place/mdp/__init__.pyi index 1421a52bc54..de6c777b49c 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 c3e030b33ec..cedc6507d78 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/test/golden_images/cartpole/newton-ovrtx_renderer-rgb.png b/source/isaaclab_tasks/test/golden_images/cartpole/newton-ovrtx_renderer-rgb.png index e47c06e2ca7..f35e82ae658 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 791497af827..5a53a6f517a 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 87104cb8716..583746565af 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 7d05e4a7adb..b33b4e8fd83 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 6b4f8389da0..27d490c4b23 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 5199099a758..8e51c396efa 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 544e2ffd450..0395c5f3d11 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 c3b229d3487..ca7d0753189 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 46ce5933fb8..7fefafde048 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 2e2f6cb257a..b5d197da550 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 16b6b73ec7f..90a2440d093 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 b0a81304d1b..0a6d3e09769 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 d6e87b16a11..e8d16133a54 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 ddffaebf072..97cf4a8487f 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 a25340e96b0..39a18ee2dc1 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 572a1759a30..5b972abb61c 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 687917f13e3..a5a024b8a9d 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 788064900f8..1c80f668e74 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 623c1e08c23..0400c3386e6 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 f76d43364ab..15afbee806b 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 4c534f674c8..00000000000 --- 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 947103618b9..876f53ffd43 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 3856e6ec134..295bc656a90 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 bd6a3c7f6c7..1f412e0f023 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 f94a63d5758..a15d09bebb0 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 fa5f36f658e..9c3a7813cd8 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 43131f70cfc..457a2a26d96 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-fix-newton-cam-pos-type.rst b/source/isaaclab_visualizers/changelog.d/jichuanh-fix-newton-cam-pos-type.rst new file mode 100644 index 00000000000..6c6525acadf --- /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 00000000000..c9f55b413e0 --- /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 ced2935897a..701a21c7998 100644 --- a/source/isaaclab_visualizers/isaaclab_visualizers/kit/kit_visualizer.py +++ b/source/isaaclab_visualizers/isaaclab_visualizers/kit/kit_visualizer.py @@ -172,7 +172,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.""" 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 00000000000..9d222ed71a6 --- /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 8c8bb0bed9d..aeafc29bd26 100644 --- a/source/isaaclab_visualizers/isaaclab_visualizers/newton/newton_visualizer.py +++ b/source/isaaclab_visualizers/isaaclab_visualizers/newton/newton_visualizer.py @@ -13,11 +13,13 @@ 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 from isaaclab_visualizers.newton_adapter import apply_viewer_visible_worlds, resolve_visible_env_indices +from .newton_visualization_markers import render_newton_visualization_markers from .newton_visualizer_cfg import NewtonVisualizerCfg logger = logging.getLogger(__name__) @@ -268,6 +270,7 @@ 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: """Initialize viewer resources and bind scene data provider. @@ -335,8 +338,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", @@ -374,6 +379,7 @@ def step(self, dt: float) -> None: self._update_camera_from_usd_path() self._state = self._scene_data_provider.get_newton_state() + num_envs = int(self._scene_data_provider.get_metadata().get("num_envs", 0)) contacts = None if self._viewer.show_contacts: @@ -401,6 +407,10 @@ def step(self, dt: float) -> None: self._viewer.log_contacts(contacts, self._state) except RuntimeError as exc: logger.debug(f"[NewtonVisualizer] Failed to log contacts: {exc}") + if self.cfg.enable_markers: + render_newton_visualization_markers( + self._viewer, self._resolved_visible_env_ids, num_envs=num_envs + ) self._viewer.end_frame() else: self._viewer._update() @@ -454,7 +464,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 +486,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 5390802df69..5600ad2ee9c 100644 --- a/source/isaaclab_visualizers/isaaclab_visualizers/rerun/rerun_visualizer.py +++ b/source/isaaclab_visualizers/isaaclab_visualizers/rerun/rerun_visualizer.py @@ -20,6 +20,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 .rerun_visualizer_cfg import RerunVisualizerCfg @@ -133,6 +134,7 @@ 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: """Initialize rerun viewer and bind scene data provider. @@ -196,8 +198,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", @@ -235,16 +239,22 @@ def step(self, dt: float) -> None: self._update_camera_from_usd_path() self._state = self._scene_data_provider.get_newton_state() + num_envs = int(self._scene_data_provider.get_metadata().get("num_envs", 0)) 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 +333,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 a629ab8b2fe..44a0e92c1d7 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 @@ -129,6 +130,8 @@ 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: """Initialize viewer resources and bind scene data provider. @@ -151,8 +154,12 @@ def initialize(self, scene_data_provider: BaseSceneDataProvider) -> None: 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_meta + ) + num_visualized_envs = ( + len(self._resolved_visible_env_ids) if self._resolved_visible_env_ids is not None else num_envs_meta + ) viewer_url = _viser_web_viewer_url(self.cfg.port) self._log_initialization_table( logger=logger, @@ -183,10 +190,26 @@ def step(self, dt: float) -> None: self._apply_pending_camera_pose() self._state = self._scene_data_provider.get_newton_state() + num_envs = int(self._scene_data_provider.get_metadata().get("num_envs", 0)) 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 +246,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 fc120619787..9ad52a71236 100644 --- a/source/isaaclab_visualizers/setup.py +++ b/source/isaaclab_visualizers/setup.py @@ -17,16 +17,16 @@ "kit": [], "newton": [ "warp-lang", - "newton @ git+https://github.com/newton-physics/newton.git@a27277ed49d6f307b8a1e4c394be7e1d14965a62", + "newton @ 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 @ 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 @ git+https://github.com/newton-physics/newton.git@v1.2.0rc2", "viser>=1.0.16", ], } diff --git a/tools/conftest.py b/tools/conftest.py index bf92d62f6c4..55b00ce44af 100644 --- a/tools/conftest.py +++ b/tools/conftest.py @@ -33,16 +33,16 @@ 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 diff --git a/tools/template/templates/extension/config/extension.toml b/tools/template/templates/extension/config/extension.toml index dbe4b064fbc..c23cf2de128 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 66832541e5c..aece6deba34 100644 --- a/tools/test_settings.py +++ b/tools/test_settings.py @@ -62,6 +62,11 @@ "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, } """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 1676fdc8f90..9944580034e 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", @@ -82,10 +83,10 @@ 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", + "mujoco==3.8.0", + "mujoco-warp==3.8.0.1", + "newton @ git+https://github.com/newton-physics/newton.git@v1.2.0rc2", "PyOpenGL-accelerate==3.1.10" ] }, # ================================================================================