From c123279a07d202c550f5075cb1aef767fd212488 Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Mon, 23 Mar 2026 22:20:22 -0700 Subject: [PATCH 01/11] unify more functions --- source/isaaclab/config/extension.toml | 2 +- source/isaaclab/docs/CHANGELOG.rst | 16 + .../isaaclab/isaaclab/envs/mdp/__init__.pyi | 2 + source/isaaclab/isaaclab/envs/mdp/events.py | 340 ++++++++++++++---- source/isaaclab_newton/config/extension.toml | 2 +- source/isaaclab_newton/docs/CHANGELOG.rst | 17 + .../assets/articulation/articulation.py | 184 ++++++++++ .../assets/articulation/articulation_data.py | 10 + .../assets/rigid_object/rigid_object.py | 189 ++++++++++ .../assets/rigid_object/rigid_object_data.py | 10 + .../rigid_object_collection.py | 188 ++++++++++ .../rigid_object_collection_data.py | 8 + .../test/assets/test_articulation.py | 58 +++ .../test/assets/test_rigid_object.py | 31 +- .../assets/test_rigid_object_collection.py | 35 +- source/isaaclab_physx/config/extension.toml | 2 +- source/isaaclab_physx/docs/CHANGELOG.rst | 17 + .../assets/articulation/articulation.py | 59 +++ .../assets/rigid_object/rigid_object.py | 59 +++ .../rigid_object_collection.py | 59 +++ .../test/assets/test_articulation.py | 38 ++ 21 files changed, 1225 insertions(+), 101 deletions(-) diff --git a/source/isaaclab/config/extension.toml b/source/isaaclab/config/extension.toml index 4920025410e2..e19107cefd71 100644 --- a/source/isaaclab/config/extension.toml +++ b/source/isaaclab/config/extension.toml @@ -1,7 +1,7 @@ [package] # Note: Semantic Versioning is used: https://semver.org/ -version = "4.5.30" +version = "4.5.31" # Description title = "Isaac Lab framework for Robot Learning" diff --git a/source/isaaclab/docs/CHANGELOG.rst b/source/isaaclab/docs/CHANGELOG.rst index 3554916d36ae..bb1728e6a8b9 100644 --- a/source/isaaclab/docs/CHANGELOG.rst +++ b/source/isaaclab/docs/CHANGELOG.rst @@ -1,6 +1,22 @@ Changelog --------- +4.5.31 (2026-04-13) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added :class:`~isaaclab.envs.mdp.randomize_rigid_body_inertia` event term for + randomizing body inertia tensors. Supports diagonal-only and full 3x3 modes. + +Changed +^^^^^^^ + +* Updated :class:`~isaaclab.envs.mdp.randomize_rigid_body_material` to use + backend-specific APIs (PhysX 3-tuple vs Newton separate friction/restitution). + + 4.5.30 (2026-04-13) ~~~~~~~~~~~~~~~~~~~ diff --git a/source/isaaclab/isaaclab/envs/mdp/__init__.pyi b/source/isaaclab/isaaclab/envs/mdp/__init__.pyi index 399035f2c09c..adf609106188 100644 --- a/source/isaaclab/isaaclab/envs/mdp/__init__.pyi +++ b/source/isaaclab/isaaclab/envs/mdp/__init__.pyi @@ -55,6 +55,7 @@ __all__ = [ "randomize_physics_scene_gravity", "randomize_rigid_body_collider_offsets", "randomize_rigid_body_com", + "randomize_rigid_body_inertia", "randomize_rigid_body_mass", "randomize_rigid_body_material", "randomize_rigid_body_scale", @@ -195,6 +196,7 @@ from .events import ( randomize_physics_scene_gravity, randomize_rigid_body_collider_offsets, randomize_rigid_body_com, + randomize_rigid_body_inertia, randomize_rigid_body_mass, randomize_rigid_body_material, randomize_rigid_body_scale, diff --git a/source/isaaclab/isaaclab/envs/mdp/events.py b/source/isaaclab/isaaclab/envs/mdp/events.py index 18c9b5eff63a..46937d357058 100644 --- a/source/isaaclab/isaaclab/envs/mdp/events.py +++ b/source/isaaclab/isaaclab/envs/mdp/events.py @@ -29,7 +29,13 @@ from isaaclab.utils.version import compare_versions, get_isaac_sim_version if TYPE_CHECKING: + from isaaclab_newton.assets import Articulation as NewtonArticulation + from isaaclab_newton.assets import RigidObject as NewtonRigidObject + from isaaclab_newton.assets import RigidObjectCollection as NewtonRigidObjectCollection + from isaaclab_physx.assets import Articulation as PhysXArticulation from isaaclab_physx.assets import DeformableObject + from isaaclab_physx.assets import RigidObject as PhysXRigidObject + from isaaclab_physx.assets import RigidObjectCollection as PhysXRigidObjectCollection from isaaclab.assets import Articulation, RigidObject from isaaclab.envs import ManagerBasedEnv @@ -158,6 +164,15 @@ class randomize_rigid_body_material(ManagerTermBase): values. The number of materials is specified by ``num_buckets``. The materials are generated by sampling uniform random values from the given ranges. + Automatically detects the active physics backend (PhysX or Newton) and applies the appropriate + material randomization strategy: + + - **PhysX**: Uses the 3-tuple material format (static_friction, dynamic_friction, restitution) + and applies via the unified ``set_material_properties_index`` method. + - **Newton**: Uses separate friction (mu) and restitution values, applying them via + ``set_friction_index`` and ``set_restitution_index`` methods. Newton uses a single + friction coefficient (mu), so only the static friction value is used. + The material properties are then assigned to the geometries of the asset. The assignment is done by creating a random integer tensor of shape (num_instances, max_num_shapes) where ``num_instances`` is the number of assets spawned and ``max_num_shapes`` is the maximum number of shapes in the asset (over @@ -203,25 +218,9 @@ def __init__(self, cfg: EventTermCfg, env: ManagerBasedEnv): f" with type: '{type(self.asset)}'." ) - # obtain number of shapes per body (needed for indexing the material properties correctly) - # note: this is a workaround since the Articulation does not provide a direct way to obtain the number of shapes - # per body. We use the physics simulation view to obtain the number of shapes per body. - if isinstance(self.asset, BaseArticulation) and self.asset_cfg.body_ids != slice(None): - self.num_shapes_per_body = [] - for link_path in self.asset.root_view.link_paths[0]: - link_physx_view = self.asset._physics_sim_view.create_rigid_body_view(link_path) # type: ignore - self.num_shapes_per_body.append(link_physx_view.max_shapes) - # ensure the parsing is correct - num_shapes = sum(self.num_shapes_per_body) - expected_shapes = self.asset.root_view.max_shapes - if num_shapes != expected_shapes: - raise ValueError( - "Randomization term 'randomize_rigid_body_material' failed to parse the number of shapes per body." - f" Expected total shapes: {expected_shapes}, but got: {num_shapes}." - ) - else: - # in this case, we don't need to do special indexing - self.num_shapes_per_body = None + # detect physics backend and initialize backend-specific state + manager_name = env.sim.physics_manager.__name__.lower() + self._backend = "newton" if "newton" in manager_name else "physx" # obtain parameters for sampling friction and restitution values static_friction_range = cfg.params.get("static_friction_range", (1.0, 1.0)) @@ -232,8 +231,7 @@ def __init__(self, cfg: EventTermCfg, env: ManagerBasedEnv): # sample material properties from the given ranges # note: we only sample the materials once during initialization # afterwards these are randomly assigned to the geometries of the asset - range_list = [static_friction_range, dynamic_friction_range, restitution_range] - ranges = torch.tensor(range_list, device="cpu") + ranges = torch.tensor([static_friction_range, dynamic_friction_range, restitution_range], device="cpu") self.material_buckets = math_utils.sample_uniform(ranges[:, 0], ranges[:, 1], (num_buckets, 3), device="cpu") # ensure dynamic friction is always less than static friction @@ -241,6 +239,62 @@ def __init__(self, cfg: EventTermCfg, env: ManagerBasedEnv): if make_consistent: self.material_buckets[:, 1] = torch.min(self.material_buckets[:, 0], self.material_buckets[:, 1]) + # initialize backend-specific state (shape indices, cached materials) + use_body_ids = isinstance(self.asset, BaseArticulation) and self.asset_cfg.body_ids != slice(None) + if self._backend == "newton": + self._init_newton(use_body_ids) + else: + self._init_physx(use_body_ids) + + def _init_physx(self, use_body_ids: bool) -> None: + """Initialize PhysX-specific state: shape indices and cached materials.""" + asset: PhysXArticulation | PhysXRigidObject | PhysXRigidObjectCollection = self.asset # type: ignore[assignment] + + # compute shape indices + if use_body_ids: + # note: workaround since Articulation doesn't expose shapes per body directly + num_shapes_per_body = [] + for link_path in asset.root_view.link_paths[0]: # type: ignore[union-attr] + link_physx_view = asset.root_view.create_rigid_body_view(link_path) # type: ignore + num_shapes_per_body.append(link_physx_view.max_shapes) + shape_indices_list = [] + for body_id in self.asset_cfg.body_ids: + start_idx = sum(num_shapes_per_body[:body_id]) + end_idx = start_idx + num_shapes_per_body[body_id] + shape_indices_list.extend(range(start_idx, end_idx)) + self.shape_indices = torch.tensor(shape_indices_list, dtype=torch.long) + else: + self.shape_indices = torch.arange(asset.root_view.max_shapes, dtype=torch.long) + + # cache default materials + self.default_materials = wp.to_torch(asset.root_view.get_material_properties()).clone() + + def _init_newton(self, use_body_ids: bool) -> None: + """Initialize Newton-specific state: shape indices and cached materials.""" + asset: NewtonArticulation | NewtonRigidObject | NewtonRigidObjectCollection = self.asset # type: ignore[assignment] + + # compute shape indices + if use_body_ids: + num_shapes_per_body = asset.num_shapes_per_body # type: ignore[union-attr] + shape_indices_list = [] + for body_id in self.asset_cfg.body_ids: + start_idx = sum(num_shapes_per_body[:body_id]) + end_idx = start_idx + num_shapes_per_body[body_id] + shape_indices_list.extend(range(start_idx, end_idx)) + self.shape_indices = torch.tensor(shape_indices_list, dtype=torch.long) + else: + self.shape_indices = torch.arange(asset.num_shapes, dtype=torch.long) + + # cache default materials + # TODO(newton-material-api): We access internal bindings `_sim_bind_shape_material_*` directly + # because Newton's friction model (single mu) differs from PhysX (static/dynamic friction). + # This is not ideal - we should either: + # 1. Add a unified getter API that abstracts backend differences, or + # 2. Use Newton's selection API (e.g., root_view.get_attribute) for querying + # For now, internal code accesses the bindings directly while we iterate on the design. + self.default_friction = wp.to_torch(asset.data._sim_bind_shape_material_mu).clone() + self.default_restitution = wp.to_torch(asset.data._sim_bind_shape_material_restitution).clone() + def __call__( self, env: ManagerBasedEnv, @@ -252,38 +306,46 @@ def __call__( asset_cfg: SceneEntityCfg, make_consistent: bool = False, ): + # determine device based on backend + device = env.device if self._backend == "newton" else "cpu" # physx uses cpu to process material randomization + # resolve environment ids if env_ids is None: - env_ids = torch.arange(env.scene.num_envs, device="cpu", dtype=torch.int32) + env_ids = torch.arange(env.scene.num_envs, device=device, dtype=torch.int32) else: - env_ids = env_ids.cpu() + env_ids = env_ids.to(device) # randomly assign material IDs to the geometries - total_num_shapes = self.asset.root_view.max_shapes - bucket_ids = torch.randint(0, num_buckets, (len(env_ids), total_num_shapes), device="cpu") - material_samples = self.material_buckets[bucket_ids] - - # retrieve material buffer from the physics simulation - materials = wp.to_torch(self.asset.root_view.get_material_properties()) + total_num_shapes = len(self.shape_indices) + bucket_ids = torch.randint(0, num_buckets, (len(env_ids), total_num_shapes), device=device) + shape_idx = self.shape_indices.to(device) + material_samples = self.material_buckets.to(device)[bucket_ids] - # update material buffer with new samples - if self.num_shapes_per_body is not None: - # sample material properties from the given ranges - for body_id in self.asset_cfg.body_ids: - # obtain indices of shapes for the body - start_idx = sum(self.num_shapes_per_body[:body_id]) - end_idx = start_idx + self.num_shapes_per_body[body_id] - # assign the new materials - # material samples are of shape: num_env_ids x total_num_shapes x 3 - materials[env_ids, start_idx:end_idx] = material_samples[:, start_idx:end_idx] + # dispatch to backend-specific implementation + if self._backend == "newton": + self._call_newton(env_ids, shape_idx, material_samples) else: - # assign all the materials - materials[env_ids] = material_samples[:] + self._call_physx(env_ids, shape_idx, material_samples) - # apply to simulation - self.asset.root_view.set_material_properties( - wp.from_torch(materials, dtype=wp.float32), wp.from_torch(env_ids, dtype=wp.int32) - ) + def _call_physx(self, env_ids: torch.Tensor, shape_idx: torch.Tensor, material_samples: torch.Tensor) -> None: + """Apply material randomization via PhysX's 3-tuple material format.""" + # update cached material buffer with new samples (vectorized) + self.default_materials[env_ids[:, None], shape_idx] = material_samples + + # apply to simulation via PhysX's unified material API + asset: PhysXRigidObject | PhysXArticulation | PhysXRigidObjectCollection = self.asset # type: ignore[assignment] + asset.set_material_properties_index(materials=self.default_materials, env_ids=env_ids) + + def _call_newton(self, env_ids: torch.Tensor, shape_idx: torch.Tensor, material_samples: torch.Tensor) -> None: + """Apply material randomization via Newton's native friction/restitution API.""" + # update cached default buffers with new samples (vectorized) + self.default_friction[env_ids[:, None], shape_idx] = material_samples[..., 0] + self.default_restitution[env_ids[:, None], shape_idx] = material_samples[..., 2] + + # apply via Newton's setter methods (handles kernel write + notification) + asset: NewtonRigidObject | NewtonArticulation | NewtonRigidObjectCollection = self.asset # type: ignore[assignment] + asset.set_friction_mask(friction=self.default_friction) + asset.set_restitution_mask(restitution=self.default_restitution) class randomize_rigid_body_mass(ManagerTermBase): @@ -405,6 +467,144 @@ def __call__( ) +class randomize_rigid_body_inertia(ManagerTermBase): + """Randomize the inertia tensor of rigid bodies by adding, scaling, or setting values. + + This function modifies body inertia tensors independently of mass. The inertia tensor + is a 3x3 symmetric matrix stored as 9 elements: ``[Ixx, Ixy, Ixz, Iyx, Iyy, Iyz, Izx, Izy, Izz]``. + + Two modes are supported via the :attr:`diagonal_only` parameter: + + - **diagonal_only=True** (default): Only modifies diagonal elements (Ixx, Iyy, Izz at + indices 0, 4, 8). This is useful for adding numerical stability (armature/regularization) + without changing rotational coupling between axes. The diagonal elements represent + resistance to rotation about each principal axis. + + - **diagonal_only=False**: Modifies all 9 elements of the inertia tensor. This can + simulate manufacturing variations or asymmetric mass distributions. Off-diagonal + elements represent coupling between rotations about different axes. + + .. note:: + Unlike :class:`randomize_rigid_body_mass` which recomputes inertia based on mass + ratios, this function modifies inertia directly without affecting mass. + """ + + def __init__(self, cfg: EventTermCfg, env: ManagerBasedEnv): + """Initialize the term. + + Args: + cfg: The configuration of the event term. + env: The environment instance. + + Raises: + ValueError: If the operation is not supported. + ValueError: If the lower bound is negative or zero when not allowed for scale operation. + ValueError: If the upper bound is less than the lower bound. + """ + from isaaclab.assets import BaseArticulation, BaseRigidObject + + super().__init__(cfg, env) + + # extract the used quantities (to enable type-hinting) + self.asset_cfg: SceneEntityCfg = cfg.params["asset_cfg"] + self.asset: RigidObject | Articulation = env.scene[self.asset_cfg.name] + + if not isinstance(self.asset, (BaseRigidObject, BaseArticulation)): + raise ValueError( + f"Randomization term 'randomize_rigid_body_inertia' not supported for asset: '{self.asset_cfg.name}'" + f" with type: '{type(self.asset)}'." + ) + + # check for valid operation + if cfg.params["operation"] == "scale": + if "inertia_distribution_params" in cfg.params: + _validate_scale_range( + cfg.params["inertia_distribution_params"], "inertia_distribution_params", allow_zero=False + ) + elif cfg.params["operation"] not in ("abs", "add"): + raise ValueError( + f"Randomization term 'randomize_rigid_body_inertia' does not support operation:" + f" '{cfg.params['operation']}'." + ) + + self.default_inertia = None + # cache inertia indices: diagonal (0, 4, 8) for regularization, or all elements + diagonal_only = cfg.params.get("diagonal_only", True) + self._inertia_idx = torch.tensor([0, 4, 8], device=self.asset.device) if diagonal_only else slice(None) + + def __call__( + self, + env: ManagerBasedEnv, + env_ids: torch.Tensor | None, + asset_cfg: SceneEntityCfg, + inertia_distribution_params: tuple[float, float], + operation: Literal["add", "scale", "abs"] = "add", + distribution: Literal["uniform", "log_uniform", "gaussian"] = "uniform", + diagonal_only: bool = True, + ): + """Randomize body inertia tensors. + + Args: + env: The environment instance. + env_ids: The environment indices to randomize. If None, all environments are randomized. + asset_cfg: The asset configuration specifying the asset and body names. + inertia_distribution_params: Distribution parameters as a tuple of two floats + ``(low, high)`` for sampling inertia modification values. + operation: The operation to apply. Options: ``"add"``, ``"scale"``, ``"abs"``. + Defaults to ``"add"`` which is typical for regularization/armature. + distribution: The distribution to sample from. Options: ``"uniform"``, + ``"log_uniform"``, ``"gaussian"``. Defaults to ``"uniform"``. + diagonal_only: If True, only modify diagonal elements (Ixx, Iyy, Izz) for + numerical stability. If False, modify all 9 elements. Defaults to True. + """ + # store default inertia on first call for repeatable randomization + if self.default_inertia is None: + self.default_inertia = wp.to_torch(self.asset.data.body_inertia).clone() + + # resolve environment ids + if env_ids is None: + env_ids = torch.arange(env.scene.num_envs, device=self.asset.device, dtype=torch.int32) + else: + env_ids = env_ids.to(self.asset.device) + + # resolve body indices + if self.asset_cfg.body_ids == slice(None): + body_ids = torch.arange(self.asset.num_bodies, dtype=torch.int32, device=self.asset.device) + else: + body_ids = torch.tensor(self.asset_cfg.body_ids, dtype=torch.int32, device=self.asset.device) + + # get default inertias for affected envs/bodies (advanced indexing creates a copy) + # shape: (len(env_ids), len(body_ids), 9) + inertias = self.default_inertia[env_ids[:, None], body_ids] + + # resolve the distribution function + if distribution == "uniform": + dist_fn = math_utils.sample_uniform + elif distribution == "log_uniform": + dist_fn = math_utils.sample_log_uniform + elif distribution == "gaussian": + dist_fn = math_utils.sample_gaussian + else: + raise NotImplementedError( + f"Unknown distribution: '{distribution}' for inertia randomization." + " Please use 'uniform', 'log_uniform', or 'gaussian'." + ) + + # sample random values once per (env, body) - shape: (len(env_ids), len(body_ids)) + random_values = dist_fn(*inertia_distribution_params, (len(env_ids), len(body_ids)), device=self.asset.device) + + # apply the operation with the SAME random value per body + if operation == "add": + inertias[:, :, self._inertia_idx] += random_values[..., None] + elif operation == "scale": + inertias[:, :, self._inertia_idx] *= random_values[..., None] + elif operation == "abs": + inertias[:, :, self._inertia_idx] = random_values[..., None] + + # set the inertia tensors into the physics simulation + self.asset.set_inertias_index(inertias=inertias, body_ids=body_ids, env_ids=env_ids) + + def randomize_rigid_body_com( env: ManagerBasedEnv, env_ids: torch.Tensor | None, @@ -802,14 +1002,26 @@ def __init__(self, cfg: EventTermCfg, env: ManagerBasedEnv): # extract the used quantities (to enable type-hinting) self.asset_cfg: SceneEntityCfg = cfg.params["asset_cfg"] - self.asset: RigidObject | Articulation = env.scene[self.asset_cfg.name] + self.asset: Articulation = env.scene[self.asset_cfg.name] + + # detect physics backend + manager_name = env.sim.physics_manager.__name__.lower() + self._backend = "newton" if "newton" in manager_name else "physx" + # cache default values (common to both backends) self.default_joint_friction_coeff = wp.to_torch(self.asset.data.joint_friction_coeff).clone() - self.default_dynamic_joint_friction_coeff = wp.to_torch(self.asset.data.joint_dynamic_friction_coeff).clone() - self.default_viscous_joint_friction_coeff = wp.to_torch(self.asset.data.joint_viscous_friction_coeff).clone() self.default_joint_armature = wp.to_torch(self.asset.data.joint_armature).clone() self.default_joint_pos_limits = wp.to_torch(self.asset.data.joint_pos_limits).clone() + # cache dynamic/viscous friction (PhysX only - Newton only has static friction) + if self._backend == "physx": + self.default_dynamic_joint_friction_coeff = wp.to_torch( + self.asset.data.joint_dynamic_friction_coeff + ).clone() + self.default_viscous_joint_friction_coeff = wp.to_torch( + self.asset.data.joint_viscous_friction_coeff + ).clone() + # check for valid operation if cfg.params["operation"] == "scale": if "friction_distribution_params" in cfg.params: @@ -867,8 +1079,14 @@ def __call__( # Always set static friction (indexed once) static_friction_coeff = friction_coeff[env_ids_for_slice, joint_ids] - # if isaacsim version is lower than 5.0.0 we can set only the static friction coefficient - if get_isaac_sim_version().major >= 5: + if self._backend == "newton": + # Newton only supports static friction coefficient + self.asset.write_joint_friction_coefficient_to_sim_index( + joint_friction_coeff=static_friction_coeff, + joint_ids=joint_ids, + env_ids=env_ids, + ) + else: # Randomize raw tensors dynamic_friction_coeff = _randomize_prop_by_op( self.default_dynamic_joint_friction_coeff.clone(), @@ -897,19 +1115,15 @@ def __call__( # Index once at the end dynamic_friction_coeff = dynamic_friction_coeff[env_ids_for_slice, joint_ids] viscous_friction_coeff = viscous_friction_coeff[env_ids_for_slice, joint_ids] - else: - # For versions < 5.0.0, we do not set these values - dynamic_friction_coeff = None - viscous_friction_coeff = None - - # Single write call for all versions - self.asset.write_joint_friction_coefficient_to_sim_index( - joint_friction_coeff=static_friction_coeff, - joint_dynamic_friction_coeff=dynamic_friction_coeff, - joint_viscous_friction_coeff=viscous_friction_coeff, - joint_ids=joint_ids, - env_ids=env_ids, - ) + + # Single write call for all versions + self.asset.write_joint_friction_coefficient_to_sim_index( + joint_friction_coeff=static_friction_coeff, + joint_dynamic_friction_coeff=dynamic_friction_coeff, + joint_viscous_friction_coeff=viscous_friction_coeff, + joint_ids=joint_ids, + env_ids=env_ids, + ) # joint armature if armature_distribution_params is not None: diff --git a/source/isaaclab_newton/config/extension.toml b/source/isaaclab_newton/config/extension.toml index 9cfd4c5fc105..5fbc20c99341 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.11" +version = "0.5.12" # 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 1417eb1163ea..98b8afa5b3d2 100644 --- a/source/isaaclab_newton/docs/CHANGELOG.rst +++ b/source/isaaclab_newton/docs/CHANGELOG.rst @@ -1,6 +1,23 @@ Changelog --------- +0.5.12 (2026-04-13) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added ``set_friction_index/mask`` and ``set_restitution_index/mask`` methods to + Newton assets for native material property randomization. + +Changed +^^^^^^^ + +* Removed ``shape_material_mu`` and ``shape_material_restitution`` data properties. +* Removed hardcoded ``default_body_armature`` from Newton cloner. Use + :class:`~isaaclab.envs.mdp.randomize_rigid_body_inertia` event term instead. + + 0.5.11 (2026-04-13) ~~~~~~~~~~~~~~~~~~~ diff --git a/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation.py b/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation.py index 9d62dc0bbed1..251d91fc11ed 100644 --- a/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation.py +++ b/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation.py @@ -169,6 +169,11 @@ def num_shapes_per_body(self) -> list[int]: self._num_shapes_per_body.append(len(shapes)) return self._num_shapes_per_body + @property + def num_shapes(self) -> int: + """Total number of collision shapes in the articulation.""" + return self.data._num_shapes + @property def joint_names(self) -> list[str]: """Ordered names of joints in articulation.""" @@ -2268,6 +2273,168 @@ def set_inertias_mask( # tell the physics engine that some of the body properties have been updated SimulationManager.add_model_change(SolverNotifyFlags.BODY_INERTIAL_PROPERTIES) + def set_friction_index( + self, + *, + friction: torch.Tensor | wp.array, + shape_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + ) -> None: + """Set friction coefficient (mu) for collision shapes using indices. + + .. note:: + This method expects partial data. + + .. tip:: + Both the index and mask methods have dedicated optimized implementations. Performance is similar for both. + However, to allow graphed pipelines, the mask method must be used. + + Args: + friction: Friction coefficient for shapes. Shape is (len(env_ids), len(shape_ids)). + shape_ids: Shape indices. If None, all shapes are updated. + env_ids: Environment indices. If None, all environments are updated. + """ + # resolve all indices + env_ids = self._resolve_env_ids(env_ids) + shape_ids = self._resolve_shape_ids(shape_ids) + self.assert_shape_and_dtype(friction, (env_ids.shape[0], shape_ids.shape[0]), wp.float32, "friction") + # Warp kernels can ingest torch tensors directly, so we don't need to convert to warp arrays here. + wp.launch( + shared_kernels.write_2d_data_to_buffer_with_indices, + dim=(env_ids.shape[0], shape_ids.shape[0]), + inputs=[ + friction, + env_ids, + shape_ids, + ], + outputs=[ + self.data._sim_bind_shape_material_mu, + ], + device=self.device, + ) + # tell the physics engine that some of the shape properties have been updated + SimulationManager.add_model_change(SolverNotifyFlags.SHAPE_PROPERTIES) + + def set_friction_mask( + self, + *, + friction: torch.Tensor | wp.array, + env_mask: wp.array | None = None, + ) -> None: + """Set friction coefficient (mu) for collision shapes using masks. + + .. note:: + This method expects full data. + + .. tip:: + Both the index and mask methods have dedicated optimized implementations. Performance is similar for both. + However, to allow graphed pipelines, the mask method must be used. + + Args: + friction: Friction coefficient for all shapes. Shape is (num_instances, num_shapes). + env_mask: Environment mask. If None, then all the instances are updated. Shape is (num_instances,). + """ + # resolve masks + env_mask = self._resolve_mask(env_mask, self._ALL_ENV_MASK) + shape_mask = self._ALL_SHAPE_MASK + self.assert_shape_and_dtype_mask(friction, (env_mask, shape_mask), wp.float32, "friction") + wp.launch( + shared_kernels.write_2d_data_to_buffer_with_mask, + dim=(env_mask.shape[0], shape_mask.shape[0]), + inputs=[ + friction, + env_mask, + shape_mask, + ], + outputs=[ + self.data._sim_bind_shape_material_mu, + ], + device=self.device, + ) + # tell the physics engine that some of the shape properties have been updated + SimulationManager.add_model_change(SolverNotifyFlags.SHAPE_PROPERTIES) + + def set_restitution_index( + self, + *, + restitution: torch.Tensor | wp.array, + shape_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + ) -> None: + """Set restitution coefficient for collision shapes using indices. + + .. note:: + This method expects partial data. + + .. tip:: + Both the index and mask methods have dedicated optimized implementations. Performance is similar for both. + However, to allow graphed pipelines, the mask method must be used. + + Args: + restitution: Restitution coefficient for shapes. Shape is (len(env_ids), len(shape_ids)). + shape_ids: Shape indices. If None, all shapes are updated. + env_ids: Environment indices. If None, all environments are updated. + """ + # resolve all indices + env_ids = self._resolve_env_ids(env_ids) + shape_ids = self._resolve_shape_ids(shape_ids) + self.assert_shape_and_dtype(restitution, (env_ids.shape[0], shape_ids.shape[0]), wp.float32, "restitution") + # Warp kernels can ingest torch tensors directly, so we don't need to convert to warp arrays here. + wp.launch( + shared_kernels.write_2d_data_to_buffer_with_indices, + dim=(env_ids.shape[0], shape_ids.shape[0]), + inputs=[ + restitution, + env_ids, + shape_ids, + ], + outputs=[ + self.data._sim_bind_shape_material_restitution, + ], + device=self.device, + ) + # tell the physics engine that some of the shape properties have been updated + SimulationManager.add_model_change(SolverNotifyFlags.SHAPE_PROPERTIES) + + def set_restitution_mask( + self, + *, + restitution: torch.Tensor | wp.array, + env_mask: wp.array | None = None, + ) -> None: + """Set restitution coefficient for collision shapes using masks. + + .. note:: + This method expects full data. + + .. tip:: + Both the index and mask methods have dedicated optimized implementations. Performance is similar for both. + However, to allow graphed pipelines, the mask method must be used. + + Args: + restitution: Restitution coefficient for all shapes. Shape is (num_instances, num_shapes). + env_mask: Environment mask. If None, then all the instances are updated. Shape is (num_instances,). + """ + # resolve masks + env_mask = self._resolve_mask(env_mask, self._ALL_ENV_MASK) + shape_mask = self._ALL_SHAPE_MASK + self.assert_shape_and_dtype_mask(restitution, (env_mask, shape_mask), wp.float32, "restitution") + wp.launch( + shared_kernels.write_2d_data_to_buffer_with_mask, + dim=(env_mask.shape[0], shape_mask.shape[0]), + inputs=[ + restitution, + env_mask, + shape_mask, + ], + outputs=[ + self.data._sim_bind_shape_material_restitution, + ], + device=self.device, + ) + # tell the physics engine that some of the shape properties have been updated + SimulationManager.add_model_change(SolverNotifyFlags.SHAPE_PROPERTIES) + def set_joint_position_target_index( self, *, @@ -3234,6 +3401,8 @@ def _create_buffers(self): self._ALL_JOINT_MASK = wp.ones((self.num_joints,), dtype=wp.bool, device=self.device) self._ALL_BODY_INDICES = wp.array(np.arange(self.num_bodies, dtype=np.int32), device=self.device) self._ALL_BODY_MASK = wp.ones((self.num_bodies,), dtype=wp.bool, device=self.device) + self._ALL_SHAPE_INDICES = wp.array(np.arange(self.num_shapes, dtype=np.int32), device=self.device) + self._ALL_SHAPE_MASK = wp.ones((self.num_shapes,), dtype=wp.bool, device=self.device) self._ALL_FIXED_TENDON_INDICES = wp.array(np.arange(self.num_fixed_tendons, dtype=np.int32), device=self.device) self._ALL_FIXED_TENDON_MASK = wp.ones((self.num_fixed_tendons,), dtype=wp.bool, device=self.device) self._ALL_SPATIAL_TENDON_INDICES = wp.array( @@ -3731,6 +3900,21 @@ def _resolve_body_ids(self, body_ids: Sequence[int] | torch.Tensor | wp.array | return self._ALL_BODY_INDICES return body_ids + def _resolve_shape_ids(self, shape_ids: Sequence[int] | torch.Tensor | wp.array | None) -> wp.array | torch.Tensor: + """Resolve shape indices to a warp array or tensor. + + Args: + shape_ids: Shape indices. If None, then all indices are used. + + Returns: + A warp array of shape indices or a tensor of shape indices. + """ + if isinstance(shape_ids, list): + return wp.array(shape_ids, dtype=wp.int32, device=self.device) + if (shape_ids is None) or (shape_ids == slice(None)): + return self._ALL_SHAPE_INDICES + return shape_ids + def _resolve_fixed_tendon_ids( self, tendon_ids: Sequence[int] | torch.Tensor | wp.array | None ) -> wp.array | torch.Tensor: 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 4dc9507dbe59..f0129e77b454 100644 --- a/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation_data.py +++ b/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation_data.py @@ -1323,6 +1323,16 @@ def _create_simulation_bindings(self) -> None: self._sim_bind_joint_velocity_target = wp.zeros( (self._num_instances, 0), dtype=wp.float32, device=self.device ) + # -- shape material properties (for collision shapes) + self._sim_bind_shape_material_mu = self._root_view.get_attribute( + "shape_material_mu", SimulationManager.get_model() + )[:, 0] + self._sim_bind_shape_material_restitution = self._root_view.get_attribute( + "shape_material_restitution", SimulationManager.get_model() + )[:, 0] + self._num_shapes = ( + self._sim_bind_shape_material_mu.shape[1] if len(self._sim_bind_shape_material_mu.shape) > 1 else 1 + ) def _create_buffers(self) -> None: """Create buffers for the root data.""" 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 5e02e3622985..3e01289557b3 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 @@ -80,6 +80,11 @@ def num_bodies(self) -> int: """ return 1 + @property + def num_shapes(self) -> int: + """Total number of collision shapes in the rigid object.""" + return self.data._num_shapes + @property def body_names(self) -> list[str]: """Ordered names of bodies in the rigid object.""" @@ -984,6 +989,170 @@ def set_inertias_mask( # tell the physics engine that some of the body properties have been updated SimulationManager.add_model_change(SolverNotifyFlags.BODY_INERTIAL_PROPERTIES) + def set_friction_index( + self, + *, + friction: torch.Tensor | wp.array, + shape_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + ) -> None: + """Set friction coefficient (mu) for collision shapes using indices. + + .. note:: + This method expects partial data. + + .. tip:: + Both the index and mask methods have dedicated optimized implementations. Performance is similar for both. + However, to allow graphed pipelines, the mask method must be used. + + Args: + friction: Friction coefficient for shapes. Shape is (len(env_ids), len(shape_ids)). + shape_ids: Shape indices. If None, all shapes are updated. + env_ids: Environment indices. If None, all environments are updated. + """ + # resolve all indices + env_ids = self._resolve_env_ids(env_ids) + shape_ids = self._resolve_shape_ids(shape_ids) + self.assert_shape_and_dtype(friction, (env_ids.shape[0], shape_ids.shape[0]), wp.float32, "friction") + # Warp kernels can ingest torch tensors directly, so we don't need to convert to warp arrays here. + wp.launch( + shared_kernels.write_2d_data_to_buffer_with_indices, + dim=(env_ids.shape[0], shape_ids.shape[0]), + inputs=[ + friction, + env_ids, + shape_ids, + ], + outputs=[ + self.data._sim_bind_shape_material_mu, + ], + device=self.device, + ) + # tell the physics engine that some of the shape properties have been updated + SimulationManager.add_model_change(SolverNotifyFlags.SHAPE_PROPERTIES) + + def set_friction_mask( + self, + *, + friction: torch.Tensor | wp.array, + env_mask: wp.array | None = None, + ) -> None: + """Set friction coefficient (mu) for collision shapes using masks. + + .. note:: + This method expects full data. + + .. tip:: + Both the index and mask methods have dedicated optimized implementations. Performance is similar for both. + However, to allow graphed pipelines, the mask method must be used. + + Args: + friction: Friction coefficient for all shapes. Shape is (num_instances, num_shapes). + env_mask: Environment mask. If None, then all the instances are updated. Shape is (num_instances,). + """ + # resolve masks + if env_mask is None: + env_mask = self._ALL_ENV_MASK + shape_mask = self._ALL_SHAPE_MASK + self.assert_shape_and_dtype_mask(friction, (env_mask, shape_mask), wp.float32, "friction") + wp.launch( + shared_kernels.write_2d_data_to_buffer_with_mask, + dim=(env_mask.shape[0], shape_mask.shape[0]), + inputs=[ + friction, + env_mask, + shape_mask, + ], + outputs=[ + self.data._sim_bind_shape_material_mu, + ], + device=self.device, + ) + # tell the physics engine that some of the shape properties have been updated + SimulationManager.add_model_change(SolverNotifyFlags.SHAPE_PROPERTIES) + + def set_restitution_index( + self, + *, + restitution: torch.Tensor | wp.array, + shape_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + ) -> None: + """Set restitution coefficient for collision shapes using indices. + + .. note:: + This method expects partial data. + + .. tip:: + Both the index and mask methods have dedicated optimized implementations. Performance is similar for both. + However, to allow graphed pipelines, the mask method must be used. + + Args: + restitution: Restitution coefficient for shapes. Shape is (len(env_ids), len(shape_ids)). + shape_ids: Shape indices. If None, all shapes are updated. + env_ids: Environment indices. If None, all environments are updated. + """ + # resolve all indices + env_ids = self._resolve_env_ids(env_ids) + shape_ids = self._resolve_shape_ids(shape_ids) + self.assert_shape_and_dtype(restitution, (env_ids.shape[0], shape_ids.shape[0]), wp.float32, "restitution") + # Warp kernels can ingest torch tensors directly, so we don't need to convert to warp arrays here. + wp.launch( + shared_kernels.write_2d_data_to_buffer_with_indices, + dim=(env_ids.shape[0], shape_ids.shape[0]), + inputs=[ + restitution, + env_ids, + shape_ids, + ], + outputs=[ + self.data._sim_bind_shape_material_restitution, + ], + device=self.device, + ) + # tell the physics engine that some of the shape properties have been updated + SimulationManager.add_model_change(SolverNotifyFlags.SHAPE_PROPERTIES) + + def set_restitution_mask( + self, + *, + restitution: torch.Tensor | wp.array, + env_mask: wp.array | None = None, + ) -> None: + """Set restitution coefficient for collision shapes using masks. + + .. note:: + This method expects full data. + + .. tip:: + Both the index and mask methods have dedicated optimized implementations. Performance is similar for both. + However, to allow graphed pipelines, the mask method must be used. + + Args: + restitution: Restitution coefficient for all shapes. Shape is (num_instances, num_shapes). + env_mask: Environment mask. If None, then all the instances are updated. Shape is (num_instances,). + """ + # resolve masks + if env_mask is None: + env_mask = self._ALL_ENV_MASK + shape_mask = self._ALL_SHAPE_MASK + self.assert_shape_and_dtype_mask(restitution, (env_mask, shape_mask), wp.float32, "restitution") + wp.launch( + shared_kernels.write_2d_data_to_buffer_with_mask, + dim=(env_mask.shape[0], shape_mask.shape[0]), + inputs=[ + restitution, + env_mask, + shape_mask, + ], + outputs=[ + self.data._sim_bind_shape_material_restitution, + ], + device=self.device, + ) + # tell the physics engine that some of the shape properties have been updated + SimulationManager.add_model_change(SolverNotifyFlags.SHAPE_PROPERTIES) + """ Internal helper. """ @@ -1070,6 +1239,8 @@ def _create_buffers(self): self._ALL_ENV_MASK = wp.ones((self.num_instances,), dtype=wp.bool, device=self.device) self._ALL_BODY_INDICES = wp.array(np.arange(self.num_bodies, dtype=np.int32), device=self.device) self._ALL_BODY_MASK = wp.ones((self.num_bodies,), dtype=wp.bool, device=self.device) + self._ALL_SHAPE_INDICES = wp.array(np.arange(self.num_shapes, dtype=np.int32), device=self.device) + self._ALL_SHAPE_MASK = wp.ones((self.num_shapes,), dtype=wp.bool, device=self.device) # external wrench composer self._instantaneous_wrench_composer = WrenchComposer(self) @@ -1128,6 +1299,24 @@ def _resolve_body_ids(self, body_ids: Sequence[int] | torch.Tensor | wp.array | return wp.array(body_ids, dtype=wp.int32, device=self.device) return body_ids + def _resolve_shape_ids(self, shape_ids: Sequence[int] | torch.Tensor | wp.array | None) -> wp.array | torch.Tensor: + """Resolve shape indices to a warp array or tensor. + + .. note:: + We do not need to convert torch tensors to warp arrays since they never get passed to the TensorAPI views. + + Args: + shape_ids: Shape indices. If None, then all indices are used. + + Returns: + A warp array of shape indices or a tensor of shape indices. + """ + if (shape_ids is None) or (shape_ids == slice(None)): + return self._ALL_SHAPE_INDICES + elif isinstance(shape_ids, list): + return wp.array(shape_ids, dtype=wp.int32, device=self.device) + return shape_ids + """ Internal simulation callbacks. """ 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 2af92df4592c..3ca059d5633c 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 @@ -735,6 +735,16 @@ def _create_simulation_bindings(self) -> None: self._sim_bind_body_external_wrench = self._root_view.get_attribute("body_f", SimulationManager.get_state_0())[ :, 0 ] + # -- shape material properties (for collision shapes) + self._sim_bind_shape_material_mu = self._root_view.get_attribute( + "shape_material_mu", SimulationManager.get_model() + )[:, 0] + self._sim_bind_shape_material_restitution = self._root_view.get_attribute( + "shape_material_restitution", SimulationManager.get_model() + )[:, 0] + self._num_shapes = ( + self._sim_bind_shape_material_mu.shape[1] if len(self._sim_bind_shape_material_mu.shape) > 1 else 1 + ) def _create_buffers(self) -> None: """Create buffers for the root data.""" 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 82bdf7a03003..5f368d8957cd 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 @@ -111,6 +111,11 @@ def num_bodies(self) -> int: """Number of bodies in the rigid object collection.""" return len(self.body_names) + @property + def num_shapes(self) -> int: + """Total number of collision shapes in the rigid object collection.""" + return self.data._num_shapes + @property def body_names(self) -> list[str]: """Ordered names of bodies in the rigid object collection.""" @@ -1050,6 +1055,170 @@ def set_inertias_mask( # Tell the physics engine that some of the body properties have been updated SimulationManager.add_model_change(SolverNotifyFlags.BODY_INERTIAL_PROPERTIES) + def set_friction_index( + self, + *, + friction: torch.Tensor | wp.array, + shape_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + ) -> None: + """Set friction coefficient (mu) for collision shapes using indices. + + .. note:: + This method expects partial data. + + .. tip:: + Both the index and mask methods have dedicated optimized implementations. Performance is similar for both. + However, to allow graphed pipelines, the mask method must be used. + + Args: + friction: Friction coefficient for shapes. Shape is (len(env_ids), len(shape_ids)). + shape_ids: Shape indices. If None, all shapes are updated. + env_ids: Environment indices. If None, all environments are updated. + """ + # resolve all indices + env_ids = self._resolve_env_ids(env_ids) + shape_ids = self._resolve_shape_ids(shape_ids) + self.assert_shape_and_dtype(friction, (env_ids.shape[0], shape_ids.shape[0]), wp.float32, "friction") + # Write to consolidated buffer + wp.launch( + shared_kernels.write_2d_data_to_buffer_with_indices, + dim=(env_ids.shape[0], shape_ids.shape[0]), + inputs=[ + friction, + env_ids, + shape_ids, + ], + outputs=[ + self.data._sim_bind_shape_material_mu, + ], + device=self.device, + ) + # Tell the physics engine that some of the shape properties have been updated + SimulationManager.add_model_change(SolverNotifyFlags.SHAPE_PROPERTIES) + + def set_friction_mask( + self, + *, + friction: torch.Tensor | wp.array, + env_mask: wp.array | None = None, + ) -> None: + """Set friction coefficient (mu) for collision shapes using masks. + + .. note:: + This method expects full data. + + .. tip:: + Both the index and mask methods have dedicated optimized implementations. Performance is similar for both. + However, to allow graphed pipelines, the mask method must be used. + + Args: + friction: Friction coefficient for all shapes. Shape is (num_instances, num_shapes). + env_mask: Environment mask. If None, then all the instances are updated. Shape is (num_instances,). + """ + # resolve masks + if env_mask is None: + env_mask = self._ALL_ENV_MASK + shape_mask = self._ALL_SHAPE_MASK + self.assert_shape_and_dtype_mask(friction, (env_mask, shape_mask), wp.float32, "friction") + wp.launch( + shared_kernels.write_2d_data_to_buffer_with_mask, + dim=(env_mask.shape[0], shape_mask.shape[0]), + inputs=[ + friction, + env_mask, + shape_mask, + ], + outputs=[ + self.data._sim_bind_shape_material_mu, + ], + device=self.device, + ) + # Tell the physics engine that some of the shape properties have been updated + SimulationManager.add_model_change(SolverNotifyFlags.SHAPE_PROPERTIES) + + def set_restitution_index( + self, + *, + restitution: torch.Tensor | wp.array, + shape_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + ) -> None: + """Set restitution coefficient for collision shapes using indices. + + .. note:: + This method expects partial data. + + .. tip:: + Both the index and mask methods have dedicated optimized implementations. Performance is similar for both. + However, to allow graphed pipelines, the mask method must be used. + + Args: + restitution: Restitution coefficient for shapes. Shape is (len(env_ids), len(shape_ids)). + shape_ids: Shape indices. If None, all shapes are updated. + env_ids: Environment indices. If None, all environments are updated. + """ + # resolve all indices + env_ids = self._resolve_env_ids(env_ids) + shape_ids = self._resolve_shape_ids(shape_ids) + self.assert_shape_and_dtype(restitution, (env_ids.shape[0], shape_ids.shape[0]), wp.float32, "restitution") + # Write to consolidated buffer + wp.launch( + shared_kernels.write_2d_data_to_buffer_with_indices, + dim=(env_ids.shape[0], shape_ids.shape[0]), + inputs=[ + restitution, + env_ids, + shape_ids, + ], + outputs=[ + self.data._sim_bind_shape_material_restitution, + ], + device=self.device, + ) + # Tell the physics engine that some of the shape properties have been updated + SimulationManager.add_model_change(SolverNotifyFlags.SHAPE_PROPERTIES) + + def set_restitution_mask( + self, + *, + restitution: torch.Tensor | wp.array, + env_mask: wp.array | None = None, + ) -> None: + """Set restitution coefficient for collision shapes using masks. + + .. note:: + This method expects full data. + + .. tip:: + Both the index and mask methods have dedicated optimized implementations. Performance is similar for both. + However, to allow graphed pipelines, the mask method must be used. + + Args: + restitution: Restitution coefficient for all shapes. Shape is (num_instances, num_shapes). + env_mask: Environment mask. If None, then all the instances are updated. Shape is (num_instances,). + """ + # resolve masks + if env_mask is None: + env_mask = self._ALL_ENV_MASK + shape_mask = self._ALL_SHAPE_MASK + self.assert_shape_and_dtype_mask(restitution, (env_mask, shape_mask), wp.float32, "restitution") + wp.launch( + shared_kernels.write_2d_data_to_buffer_with_mask, + dim=(env_mask.shape[0], shape_mask.shape[0]), + inputs=[ + restitution, + env_mask, + shape_mask, + ], + outputs=[ + self.data._sim_bind_shape_material_restitution, + ], + device=self.device, + ) + # Tell the physics engine that some of the shape properties have been updated + SimulationManager.add_model_change(SolverNotifyFlags.SHAPE_PROPERTIES) + """ Internal helper. """ @@ -1137,8 +1306,12 @@ def _create_buffers(self): self._ALL_BODY_INDICES = wp.array( np.arange(self.num_bodies, dtype=np.int32), device=self.device, dtype=wp.int32 ) + self._ALL_SHAPE_INDICES = wp.array( + np.arange(self.num_shapes, dtype=np.int32), device=self.device, dtype=wp.int32 + ) self._ALL_ENV_MASK = wp.ones((self.num_instances,), dtype=wp.bool, device=self.device) self._ALL_BODY_MASK = wp.ones((self.num_bodies,), dtype=wp.bool, device=self.device) + self._ALL_SHAPE_MASK = wp.ones((self.num_shapes,), dtype=wp.bool, device=self.device) # external wrench composer self._instantaneous_wrench_composer = WrenchComposer(self) @@ -1195,6 +1368,21 @@ def _resolve_body_ids(self, body_ids) -> wp.array: return wp.from_torch(body_ids.to(torch.int32), dtype=wp.int32) return body_ids + def _resolve_shape_ids(self, shape_ids: Sequence[int] | torch.Tensor | wp.array | None) -> wp.array | torch.Tensor: + """Resolve shape indices to a warp array or tensor. + + Args: + shape_ids: Shape indices. If None, then all indices are used. + + Returns: + A warp array of shape indices or a tensor of shape indices. + """ + if (shape_ids is None) or (shape_ids == slice(None)): + return self._ALL_SHAPE_INDICES + elif isinstance(shape_ids, list): + return wp.array(shape_ids, dtype=wp.int32, device=self.device) + return shape_ids + def _resolve_env_mask(self, env_mask: wp.array | None) -> wp.array | torch.Tensor: """Resolve environment mask to indices via torch.nonzero.""" if env_mask is not None: diff --git a/source/isaaclab_newton/isaaclab_newton/assets/rigid_object_collection/rigid_object_collection_data.py b/source/isaaclab_newton/isaaclab_newton/assets/rigid_object_collection/rigid_object_collection_data.py index 3fbc1801322f..6b3c53580f50 100644 --- a/source/isaaclab_newton/isaaclab_newton/assets/rigid_object_collection/rigid_object_collection_data.py +++ b/source/isaaclab_newton/isaaclab_newton/assets/rigid_object_collection/rigid_object_collection_data.py @@ -608,6 +608,14 @@ def _create_simulation_bindings(self) -> None: device=_body_inertia_raw.device, copy=False, ) + # -- shape material properties (for collision shapes) + self._sim_bind_shape_material_mu = self._root_view.get_attribute("shape_material_mu", model)[:, :, 0] + self._sim_bind_shape_material_restitution = self._root_view.get_attribute("shape_material_restitution", model)[ + :, :, 0 + ] + self._num_shapes = ( + self._sim_bind_shape_material_mu.shape[1] if len(self._sim_bind_shape_material_mu.shape) > 1 else 1 + ) def _create_buffers(self) -> None: """Create buffers for computing and caching derived quantities.""" diff --git a/source/isaaclab_newton/test/assets/test_articulation.py b/source/isaaclab_newton/test/assets/test_articulation.py index 867f0fe8eba1..57c80c30ad4a 100644 --- a/source/isaaclab_newton/test/assets/test_articulation.py +++ b/source/isaaclab_newton/test/assets/test_articulation.py @@ -2487,5 +2487,63 @@ def _patched_simulate(cls): ) +@pytest.mark.parametrize("add_ground_plane", [True]) +@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) + articulation, _ = generate_articulation( + articulation_cfg=articulation_cfg, num_articulations=num_articulations, device=device + ) + + # Play the simulator + sim.reset() + + # Get actual number of shapes from the articulation + num_shapes = articulation.num_shapes + + # Test 1: Set all shapes using index method (passing shape_ids explicitly) + shape_ids = torch.arange(num_shapes, dtype=torch.int32, device=device) + env_ids = torch.arange(num_articulations, dtype=torch.int32, device=device) + friction = torch.empty(num_articulations, num_shapes, device=device).uniform_(0.4, 0.8) + restitution = torch.empty(num_articulations, num_shapes, device=device).uniform_(0.0, 0.2) + + articulation.set_friction_index(friction=friction, shape_ids=shape_ids, env_ids=env_ids) + articulation.set_restitution_index(restitution=restitution, shape_ids=shape_ids, env_ids=env_ids) + + # Simulate physics + sim.step() + articulation.update(sim.cfg.dt) + + # Get material properties via internal bindings (for test verification only) + mu = wp.to_torch(articulation.data._sim_bind_shape_material_mu) + restitution_check = wp.to_torch(articulation.data._sim_bind_shape_material_restitution) + + # Check if material properties are set correctly + torch.testing.assert_close(mu, friction) + torch.testing.assert_close(restitution_check, restitution) + + # Test 2: Set subset of shapes using index method + if num_shapes > 1: + subset_shape_ids = torch.tensor([0], dtype=torch.int32, device=device) + subset_friction = torch.empty(num_articulations, 1, device=device).uniform_(0.1, 0.2) + subset_restitution = torch.empty(num_articulations, 1, device=device).uniform_(0.5, 0.6) + + articulation.set_friction_index(friction=subset_friction, shape_ids=subset_shape_ids, env_ids=env_ids) + articulation.set_restitution_index(restitution=subset_restitution, shape_ids=subset_shape_ids, env_ids=env_ids) + + sim.step() + articulation.update(sim.cfg.dt) + + # Check only the subset was updated + mu_updated = wp.to_torch(articulation.data._sim_bind_shape_material_mu) + restitution_updated = wp.to_torch(articulation.data._sim_bind_shape_material_restitution) + torch.testing.assert_close(mu_updated[:, 0:1], subset_friction) + torch.testing.assert_close(restitution_updated[:, 0:1], subset_restitution) + + if __name__ == "__main__": pytest.main([__file__, "-v", "--maxfail=1"]) diff --git a/source/isaaclab_newton/test/assets/test_rigid_object.py b/source/isaaclab_newton/test/assets/test_rigid_object.py index 9cc1717d8c5f..4df40aaeed00 100644 --- a/source/isaaclab_newton/test/assets/test_rigid_object.py +++ b/source/isaaclab_newton/test/assets/test_rigid_object.py @@ -589,7 +589,6 @@ def test_reset_rigid_object(num_cubes, device): assert torch.count_nonzero(wp.to_torch(cube_object._permanent_wrench_composer.composed_torque)) == 0 -@pytest.mark.skip(reason="Newton friction/restitution/material not yet supported") @pytest.mark.parametrize("num_cubes", [1, 2]) @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.isaacsim_ci @@ -603,30 +602,30 @@ def test_rigid_body_set_material_properties(num_cubes, device): # Play sim sim.reset() - # Set material properties - static_friction = torch.FloatTensor(num_cubes, 1).uniform_(0.4, 0.8) - dynamic_friction = torch.FloatTensor(num_cubes, 1).uniform_(0.4, 0.8) - restitution = torch.FloatTensor(num_cubes, 1).uniform_(0.0, 0.2) - - materials = torch.cat([static_friction, dynamic_friction, restitution], dim=-1) + # Set material properties using Newton's native API + # Get actual number of shapes from the object + num_shapes = cube_object.num_shapes + shape_ids = torch.arange(num_shapes, dtype=torch.int32, device=device) + env_ids = torch.arange(num_cubes, dtype=torch.int32, device=device) + friction = torch.empty(num_cubes, num_shapes, device=device).uniform_(0.4, 0.8) + restitution = torch.empty(num_cubes, num_shapes, device=device).uniform_(0.0, 0.2) - indices = torch.tensor(range(num_cubes), dtype=torch.int32) - # Add friction to cube - cube_object.root_view.set_material_properties( - wp.from_torch(materials, dtype=wp.float32), wp.from_torch(indices, dtype=wp.int32) - ) + # Use Newton's native friction/restitution API with shape_ids + cube_object.set_friction_index(friction=friction, shape_ids=shape_ids, env_ids=env_ids) + cube_object.set_restitution_index(restitution=restitution, shape_ids=shape_ids, env_ids=env_ids) # Simulate physics - # perform rendering sim.step() # update object cube_object.update(sim.cfg.dt) - # Get material properties - materials_to_check = wp.to_torch(cube_object.root_view.get_material_properties()) + # Get material properties via internal bindings (for test verification only) + mu = wp.to_torch(cube_object.data._sim_bind_shape_material_mu) + restitution_check = wp.to_torch(cube_object.data._sim_bind_shape_material_restitution) # Check if material properties are set correctly - torch.testing.assert_close(materials_to_check.reshape(num_cubes, 3), materials) + torch.testing.assert_close(mu, friction) + torch.testing.assert_close(restitution_check, restitution) @pytest.mark.skip(reason="Newton friction/restitution/material not yet supported") 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 a1086d4371ec..0d51f25675f1 100644 --- a/source/isaaclab_newton/test/assets/test_rigid_object_collection.py +++ b/source/isaaclab_newton/test/assets/test_rigid_object_collection.py @@ -522,42 +522,39 @@ def test_reset_object_collection(num_envs, num_cubes, device): ) -@pytest.mark.skip(reason="Newton doesn't support friction/restitution/material properties yet") @pytest.mark.parametrize("num_envs", [1, 3]) @pytest.mark.parametrize("num_cubes", [1, 2]) @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) def test_set_material_properties(num_envs, num_cubes, device): - """Test getting and setting material properties of rigid object.""" + """Test getting and setting material properties of rigid object collection.""" with _newton_sim_context(device, auto_add_lighting=True) as sim: sim._app_control_on_stop_handle = None object_collection, _ = generate_cubes_scene(num_envs=num_envs, num_cubes=num_cubes, device=device) sim.reset() - # Set material properties - static_friction = torch.FloatTensor(num_envs, num_cubes, 1).uniform_(0.4, 0.8) - dynamic_friction = torch.FloatTensor(num_envs, num_cubes, 1).uniform_(0.4, 0.8) - restitution = torch.FloatTensor(num_envs, num_cubes, 1).uniform_(0.0, 0.2) + # Set material properties using Newton's native API + # Get actual number of shapes from the collection + num_shapes = object_collection.num_shapes + shape_ids = torch.arange(num_shapes, dtype=torch.int32, device=device) + env_ids = torch.arange(num_envs, dtype=torch.int32, device=device) + friction = torch.empty(num_envs, num_shapes, device=device).uniform_(0.4, 0.8) + restitution = torch.empty(num_envs, num_shapes, device=device).uniform_(0.0, 0.2) - materials = torch.cat([static_friction, dynamic_friction, restitution], dim=-1) - - # Add friction to cube - indices = torch.tensor(range(num_cubes * num_envs), dtype=torch.int) - object_collection.root_view.set_material_properties( - object_collection.reshape_data_to_view_3d(wp.from_torch(materials, dtype=wp.float32), 3, device="cpu"), - wp.from_torch(indices, dtype=wp.int32), - ) + # Use Newton's native friction/restitution API with shape_ids + object_collection.set_friction_index(friction=friction, shape_ids=shape_ids, env_ids=env_ids) + object_collection.set_restitution_index(restitution=restitution, shape_ids=shape_ids, env_ids=env_ids) # Perform simulation sim.step() object_collection.update(sim.cfg.dt) - # Get material properties - materials_to_check = object_collection.root_view.get_material_properties() + # Get material properties via internal bindings (for test verification only) + mu = wp.to_torch(object_collection.data._sim_bind_shape_material_mu) + restitution_check = wp.to_torch(object_collection.data._sim_bind_shape_material_restitution) # Check if material properties are set correctly - torch.testing.assert_close( - wp.to_torch(object_collection.reshape_view_to_data_3d(materials_to_check, 3, device="cpu")), materials - ) + torch.testing.assert_close(mu, friction) + torch.testing.assert_close(restitution_check, restitution) @pytest.mark.parametrize("num_envs", [1, 3]) diff --git a/source/isaaclab_physx/config/extension.toml b/source/isaaclab_physx/config/extension.toml index ee1f4696f61f..a814911ea827 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.14" +version = "0.5.15" # 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 0f1e959d3f3d..4d9715d22857 100644 --- a/source/isaaclab_physx/docs/CHANGELOG.rst +++ b/source/isaaclab_physx/docs/CHANGELOG.rst @@ -1,6 +1,23 @@ Changelog --------- +0.5.15 (2026-04-13) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added :meth:`~isaaclab_physx.assets.RigidObject.set_material_properties_index`, + :meth:`~isaaclab_physx.assets.RigidObject.set_material_properties_mask`, + :meth:`~isaaclab_physx.assets.Articulation.set_material_properties_index`, + :meth:`~isaaclab_physx.assets.Articulation.set_material_properties_mask`, + :meth:`~isaaclab_physx.assets.RigidObjectCollection.set_material_properties_index`, and + :meth:`~isaaclab_physx.assets.RigidObjectCollection.set_material_properties_mask` + methods for setting collision shape material properties (friction, restitution). + These methods follow the standard ``_index``/``_mask`` pattern, providing a unified + API across PhysX and Newton backends. + + 0.5.14 (2026-04-06) ~~~~~~~~~~~~~~~~~~~ diff --git a/source/isaaclab_physx/isaaclab_physx/assets/articulation/articulation.py b/source/isaaclab_physx/isaaclab_physx/assets/articulation/articulation.py index cf7d1f95d5ca..2610ff1e4f6f 100644 --- a/source/isaaclab_physx/isaaclab_physx/assets/articulation/articulation.py +++ b/source/isaaclab_physx/isaaclab_physx/assets/articulation/articulation.py @@ -2218,6 +2218,65 @@ def set_inertias_mask( # Set full data to True to ensure the right code path is taken inside the kernel. self.set_inertias_index(inertias=inertias, body_ids=body_ids, env_ids=env_ids, full_data=True) + def set_material_properties_index( + self, + *, + materials: torch.Tensor | wp.array, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + ) -> None: + """Set material properties (friction and restitution) for collision shapes using indices. + + The material properties consist of static friction, dynamic friction, and restitution + coefficients for each collision shape. + + .. note:: + This method expects partial data. + + .. tip:: + Both the index and mask methods have dedicated optimized implementations. Performance is similar for both. + However, to allow graphed pipelines, the mask method must be used. + + Args: + materials: Material properties for all shapes. Shape is (len(env_ids), max_shapes, 3) + where the 3 values are [static_friction, dynamic_friction, restitution]. + env_ids: Environment indices. If None, all environments are updated. + """ + # resolve all indices + env_ids = self._resolve_env_ids(env_ids) + # convert materials to warp array if needed (root_view requires warp arrays) + if isinstance(materials, torch.Tensor): + materials = wp.from_torch(materials, dtype=wp.float32) + # set material properties via root view + self.root_view.set_material_properties(materials, env_ids) + + def set_material_properties_mask( + self, + *, + materials: torch.Tensor | wp.array, + env_mask: wp.array | None = None, + ) -> None: + """Set material properties (friction and restitution) for collision shapes using masks. + + The material properties consist of static friction, dynamic friction, and restitution + coefficients for each collision shape. + + .. note:: + This method expects full data. + + .. tip:: + For maximum performance we recommend using the index method. This is because in PhysX, the tensor API + is only supporting indexing, hence masks need to be converted to indices. + + Args: + materials: Material properties for all shapes. Shape is ``(num_instances, max_shapes, 3)`` + where the 3 values are ``[static_friction, dynamic_friction, restitution]``. + env_mask: Environment mask. If None, then all the instances are updated. Shape is (num_instances,). + """ + # Resolve masks to indices + env_ids = self._resolve_env_mask(env_mask) + # Call the index method + self.set_material_properties_index(materials=materials, env_ids=env_ids) + def set_joint_position_target_index( self, *, diff --git a/source/isaaclab_physx/isaaclab_physx/assets/rigid_object/rigid_object.py b/source/isaaclab_physx/isaaclab_physx/assets/rigid_object/rigid_object.py index b549da96b787..586750eaa6a3 100644 --- a/source/isaaclab_physx/isaaclab_physx/assets/rigid_object/rigid_object.py +++ b/source/isaaclab_physx/isaaclab_physx/assets/rigid_object/rigid_object.py @@ -912,6 +912,65 @@ def set_inertias_mask( # Set full data to True to ensure the right code path is taken inside the kernel. self.set_inertias_index(inertias=inertias, body_ids=body_ids, env_ids=env_ids, full_data=True) + def set_material_properties_index( + self, + *, + materials: torch.Tensor | wp.array, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + ) -> None: + """Set material properties (friction and restitution) for collision shapes using indices. + + The material properties consist of static friction, dynamic friction, and restitution + coefficients for each collision shape. + + .. note:: + This method expects partial data. + + .. tip:: + Both the index and mask methods have dedicated optimized implementations. Performance is similar for both. + However, to allow graphed pipelines, the mask method must be used. + + Args: + materials: Material properties for all shapes. Shape is (len(env_ids), max_shapes, 3) + where the 3 values are [static_friction, dynamic_friction, restitution]. + env_ids: Environment indices. If None, all environments are updated. + """ + # resolve all indices + env_ids = self._resolve_env_ids(env_ids) + # convert materials to warp array if needed (root_view requires warp arrays) + if isinstance(materials, torch.Tensor): + materials = wp.from_torch(materials, dtype=wp.float32) + # set material properties via root view + self.root_view.set_material_properties(materials, env_ids) + + def set_material_properties_mask( + self, + *, + materials: torch.Tensor | wp.array, + env_mask: wp.array | None = None, + ) -> None: + """Set material properties (friction and restitution) for collision shapes using masks. + + The material properties consist of static friction, dynamic friction, and restitution + coefficients for each collision shape. + + .. note:: + This method expects full data. + + .. tip:: + For maximum performance we recommend using the index method. This is because in PhysX, the tensor API + is only supporting indexing, hence masks need to be converted to indices. + + Args: + materials: Material properties for all shapes. Shape is ``(num_instances, max_shapes, 3)`` + where the 3 values are ``[static_friction, dynamic_friction, restitution]``. + env_mask: Environment mask. If None, then all the instances are updated. Shape is (num_instances,). + """ + # Resolve masks to indices + env_ids = self._resolve_env_mask(env_mask) + # Call the index method + self.set_material_properties_index(materials=materials, env_ids=env_ids) + """ Internal helper. """ 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 877a44261293..c84eac2eef45 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 @@ -1053,6 +1053,65 @@ def set_inertias_mask( # Set full data to True to ensure the right code path is taken inside the kernel. self.set_inertias_index(inertias=inertias, body_ids=body_ids, env_ids=env_ids, full_data=True) + def set_material_properties_index( + self, + *, + materials: torch.Tensor | wp.array, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + ) -> None: + """Set material properties (friction and restitution) for collision shapes using indices. + + The material properties consist of static friction, dynamic friction, and restitution + coefficients for each collision shape. + + .. note:: + This method expects partial data. + + .. tip:: + Both the index and mask methods have dedicated optimized implementations. Performance is similar for both. + However, to allow graphed pipelines, the mask method must be used. + + Args: + materials: Material properties for all shapes. Shape is (len(env_ids), max_shapes, 3) + where the 3 values are [static_friction, dynamic_friction, restitution]. + env_ids: Environment indices. If None, all environments are updated. + """ + # resolve all indices + env_ids = self._resolve_env_ids(env_ids) + # convert materials to warp array if needed (root_view requires warp arrays) + if isinstance(materials, torch.Tensor): + materials = wp.from_torch(materials, dtype=wp.float32) + # set material properties via root view + self.root_view.set_material_properties(materials, env_ids) + + def set_material_properties_mask( + self, + *, + materials: torch.Tensor | wp.array, + env_mask: wp.array | None = None, + ) -> None: + """Set material properties (friction and restitution) for collision shapes using masks. + + The material properties consist of static friction, dynamic friction, and restitution + coefficients for each collision shape. + + .. note:: + This method expects full data. + + .. tip:: + For maximum performance we recommend using the index method. This is because in PhysX, the tensor API + is only supporting indexing, hence masks need to be converted to indices. + + Args: + materials: Material properties for all shapes. Shape is ``(num_instances, max_shapes, 3)`` + where the 3 values are ``[static_friction, dynamic_friction, restitution]``. + env_mask: Environment mask. If None, then all the instances are updated. Shape is (num_instances,). + """ + # Resolve masks to indices + env_ids = self._resolve_env_mask(env_mask) + # Call the index method + self.set_material_properties_index(materials=materials, env_ids=env_ids) + """ Helper functions. """ diff --git a/source/isaaclab_physx/test/assets/test_articulation.py b/source/isaaclab_physx/test/assets/test_articulation.py index f1e31795c130..9726a7bd3a80 100644 --- a/source/isaaclab_physx/test/assets/test_articulation.py +++ b/source/isaaclab_physx/test/assets/test_articulation.py @@ -2245,5 +2245,43 @@ def test_write_joint_frictions_to_sim(sim, num_articulations, device, add_ground assert torch.allclose(joint_friction_coeff_sim_2, friction_2.cpu()) +@pytest.mark.parametrize("add_ground_plane", [True]) +@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) + articulation, _ = generate_articulation( + articulation_cfg=articulation_cfg, num_articulations=num_articulations, device=device + ) + + # Play the simulator + sim.reset() + + # Get number of shapes from the articulation + max_shapes = articulation.root_view.max_shapes + + # Generate random material properties: (static_friction, dynamic_friction, restitution) + materials = torch.empty(num_articulations, max_shapes, 3, device="cpu").uniform_(0.0, 1.0) + # Ensure dynamic friction <= static friction + materials[..., 1] = torch.min(materials[..., 0], materials[..., 1]) + + # Use PhysX's material properties API + env_ids = torch.arange(num_articulations, dtype=torch.int32) + articulation.set_material_properties_index(materials=materials, env_ids=env_ids) + + # Simulate physics + sim.step() + articulation.update(sim.cfg.dt) + + # Get material properties from simulation + materials_check = wp.to_torch(articulation.root_view.get_material_properties()) + + # Check if material properties are set correctly + torch.testing.assert_close(materials_check, materials) + + if __name__ == "__main__": pytest.main([__file__, "-v", "--maxfail=1"]) From 07a98d446bc29cf76f75b16c5ea0f0b687428dcc Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Mon, 23 Mar 2026 22:26:13 -0700 Subject: [PATCH 02/11] fix linting --- source/isaaclab/isaaclab/envs/mdp/events.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/source/isaaclab/isaaclab/envs/mdp/events.py b/source/isaaclab/isaaclab/envs/mdp/events.py index 46937d357058..a92f2d7c36c0 100644 --- a/source/isaaclab/isaaclab/envs/mdp/events.py +++ b/source/isaaclab/isaaclab/envs/mdp/events.py @@ -26,7 +26,7 @@ import isaaclab.utils.math as math_utils from isaaclab.actuators import ImplicitActuator from isaaclab.managers import EventTermCfg, ManagerTermBase, SceneEntityCfg -from isaaclab.utils.version import compare_versions, get_isaac_sim_version +from isaaclab.utils.version import compare_versions if TYPE_CHECKING: from isaaclab_newton.assets import Articulation as NewtonArticulation From cc98f1277cba613377aaaccc02c77a0a86b6a28b Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Tue, 24 Mar 2026 14:05:43 -0700 Subject: [PATCH 03/11] address comment --- source/isaaclab_newton/docs/CHANGELOG.rst | 7 ---- .../assets/articulation/articulation.py | 4 +- .../test/assets/test_rigid_object.py | 38 +++++++++++++++++++ 3 files changed, 40 insertions(+), 9 deletions(-) diff --git a/source/isaaclab_newton/docs/CHANGELOG.rst b/source/isaaclab_newton/docs/CHANGELOG.rst index 98b8afa5b3d2..6b5861137b01 100644 --- a/source/isaaclab_newton/docs/CHANGELOG.rst +++ b/source/isaaclab_newton/docs/CHANGELOG.rst @@ -10,13 +10,6 @@ Added * Added ``set_friction_index/mask`` and ``set_restitution_index/mask`` methods to Newton assets for native material property randomization. -Changed -^^^^^^^ - -* Removed ``shape_material_mu`` and ``shape_material_restitution`` data properties. -* Removed hardcoded ``default_body_armature`` from Newton cloner. Use - :class:`~isaaclab.envs.mdp.randomize_rigid_body_inertia` event term instead. - 0.5.11 (2026-04-13) ~~~~~~~~~~~~~~~~~~~ diff --git a/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation.py b/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation.py index 251d91fc11ed..b94394ecd8c6 100644 --- a/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation.py +++ b/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation.py @@ -3909,10 +3909,10 @@ def _resolve_shape_ids(self, shape_ids: Sequence[int] | torch.Tensor | wp.array Returns: A warp array of shape indices or a tensor of shape indices. """ - if isinstance(shape_ids, list): - return wp.array(shape_ids, dtype=wp.int32, device=self.device) if (shape_ids is None) or (shape_ids == slice(None)): return self._ALL_SHAPE_INDICES + elif isinstance(shape_ids, list): + return wp.array(shape_ids, dtype=wp.int32, device=self.device) return shape_ids def _resolve_fixed_tendon_ids( diff --git a/source/isaaclab_physx/test/assets/test_rigid_object.py b/source/isaaclab_physx/test/assets/test_rigid_object.py index 6cea115105e1..431bbf0fab55 100644 --- a/source/isaaclab_physx/test/assets/test_rigid_object.py +++ b/source/isaaclab_physx/test/assets/test_rigid_object.py @@ -604,6 +604,44 @@ def test_rigid_body_set_material_properties(num_cubes, device): torch.testing.assert_close(materials_to_check.reshape(num_cubes, 3), materials) +@pytest.mark.parametrize("num_cubes", [1, 2]) +@pytest.mark.parametrize("device", ["cuda:0", "cpu"]) +@pytest.mark.isaacsim_ci +def test_set_material_properties_index(num_cubes, device): + """Test setting material properties using the set_material_properties_index API.""" + with build_simulation_context( + device=device, gravity_enabled=True, add_ground_plane=True, auto_add_lighting=True + ) as sim: + sim._app_control_on_stop_handle = None + # Generate cubes scene + cube_object, _ = generate_cubes_scene(num_cubes=num_cubes, device=device) + + # Play sim + sim.reset() + + # Get number of shapes + max_shapes = cube_object.root_view.max_shapes + + # Generate random material properties: (static_friction, dynamic_friction, restitution) + materials = torch.empty(num_cubes, max_shapes, 3, device="cpu").uniform_(0.0, 1.0) + # Ensure dynamic friction <= static friction + materials[..., 1] = torch.min(materials[..., 0], materials[..., 1]) + + # Use the new set_material_properties_index API + env_ids = torch.arange(num_cubes, dtype=torch.int32) + cube_object.set_material_properties_index(materials=materials, env_ids=env_ids) + + # Simulate physics + sim.step() + cube_object.update(sim.cfg.dt) + + # Get material properties from simulation + materials_check = wp.to_torch(cube_object.root_view.get_material_properties()) + + # Check if material properties are set correctly + torch.testing.assert_close(materials_check, materials) + + @pytest.mark.parametrize("num_cubes", [1, 2]) @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.isaacsim_ci From f13095b02a74a6556150493ca4b5da64a9ba9a28 Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Fri, 10 Apr 2026 00:10:12 -0700 Subject: [PATCH 04/11] add more event for newton physx parity --- source/isaaclab/docs/CHANGELOG.rst | 7 + source/isaaclab/isaaclab/envs/mdp/events.py | 151 ++++++++++++------ .../test/assets/test_articulation.py | 59 ++++++- 3 files changed, 163 insertions(+), 54 deletions(-) diff --git a/source/isaaclab/docs/CHANGELOG.rst b/source/isaaclab/docs/CHANGELOG.rst index bb1728e6a8b9..88342fc2a69d 100644 --- a/source/isaaclab/docs/CHANGELOG.rst +++ b/source/isaaclab/docs/CHANGELOG.rst @@ -15,6 +15,13 @@ Changed * Updated :class:`~isaaclab.envs.mdp.randomize_rigid_body_material` to use backend-specific APIs (PhysX 3-tuple vs Newton separate friction/restitution). +* Converted :func:`~isaaclab.envs.mdp.randomize_rigid_body_collider_offsets` from a + plain function to a :class:`~isaaclab.managers.ManagerTermBase` class with automatic + backend detection. PhysX uses rest/contact offsets directly; Newton maps them to + ``shape_margin`` and ``shape_gap``. +* Updated :func:`~isaaclab.envs.mdp.randomize_rigid_body_com` to detect the active + physics backend and pass position-only (vec3) for Newton vs full pose (pos + quat) + for PhysX. 4.5.30 (2026-04-13) diff --git a/source/isaaclab/isaaclab/envs/mdp/events.py b/source/isaaclab/isaaclab/envs/mdp/events.py index a92f2d7c36c0..8851c82cb9e8 100644 --- a/source/isaaclab/isaaclab/envs/mdp/events.py +++ b/source/isaaclab/isaaclab/envs/mdp/events.py @@ -638,71 +638,116 @@ def randomize_rigid_body_com( ranges[:, 0], ranges[:, 1], (len(env_ids), 3), device=asset.device ).unsqueeze(1) - # get the current com of the bodies (num_assets, num_bodies) + # get the current com of the bodies (num_assets, num_bodies, 7) coms = wp.to_torch(asset.data.body_com_pose_b).clone() - # Randomize the com in range + # Randomize the com position coms[env_ids[:, None], body_ids, :3] += rand_samples - # Set the new coms - asset.set_coms_index(coms=coms, env_ids=env_ids) - - -def randomize_rigid_body_collider_offsets( - env: ManagerBasedEnv, - env_ids: torch.Tensor | None, - asset_cfg: SceneEntityCfg, - rest_offset_distribution_params: tuple[float, float] | None = None, - contact_offset_distribution_params: tuple[float, float] | None = None, - distribution: Literal["uniform", "log_uniform", "gaussian"] = "uniform", -): - """Randomize the collider parameters of rigid bodies in an asset by adding, scaling, or setting random values. + # Newton's set_coms_index expects position-only (vec3f), while PhysX expects + # the full pose (pos + quat). Detect backend to pass the correct shape. + manager_name = env.sim.physics_manager.__name__.lower() + if "newton" in manager_name: + asset.set_coms_index(coms=coms[..., :3], env_ids=env_ids) + else: + asset.set_coms_index(coms=coms, env_ids=env_ids) - This function allows randomizing the collider parameters of the asset, such as rest and contact offsets. - These correspond to the physics engine collider properties that affect the collision checking. - The function samples random values from the given distribution parameters and applies the operation to - the collider properties. It then sets the values into the physics simulation. If the distribution parameters - are not provided for a particular property, the function does not modify the property. +class randomize_rigid_body_collider_offsets(ManagerTermBase): + """Randomize the collider parameters of rigid bodies by setting random values. - Currently, the distribution parameters are applied as absolute values. + Supports both PhysX (rest/contact offset) and Newton (shape_margin/shape_gap). + For Newton, ``rest_offset`` maps to ``shape_margin`` and ``contact_offset`` is + converted to ``shape_gap`` via ``gap = contact_offset - margin``. .. tip:: - This function uses CPU tensors to assign the collision properties. It is recommended to use this function - only during the initialization of the environment. + It is recommended to use this function only during environment initialization. """ - # extract the used quantities (to enable type-hinting) - asset: RigidObject | Articulation = env.scene[asset_cfg.name] - # resolve environment ids - if env_ids is None: - env_ids = torch.arange(env.scene.num_envs, device="cpu") + def __init__(self, cfg: EventTermCfg, env: ManagerBasedEnv): + from isaaclab.assets import BaseArticulation, BaseRigidObject - # sample collider properties from the given ranges and set into the physics simulation - # -- rest offsets - if rest_offset_distribution_params is not None: - rest_offset = wp.to_torch(asset.root_view.get_rest_offsets()).clone() - rest_offset = _randomize_prop_by_op( - rest_offset, - rest_offset_distribution_params, - None, - slice(None), - operation="abs", - distribution=distribution, - ) - asset.root_view.set_rest_offsets(rest_offset, env_ids.cpu()) - # -- contact offsets - if contact_offset_distribution_params is not None: - contact_offset = wp.to_torch(asset.root_view.get_contact_offsets()).clone() - contact_offset = _randomize_prop_by_op( - contact_offset, - contact_offset_distribution_params, - None, - slice(None), - operation="abs", - distribution=distribution, - ) - asset.root_view.set_contact_offsets(contact_offset, env_ids.cpu()) + super().__init__(cfg, env) + + self.asset_cfg: SceneEntityCfg = cfg.params["asset_cfg"] + self.asset: RigidObject | Articulation = env.scene[self.asset_cfg.name] + + if not isinstance(self.asset, (BaseRigidObject, BaseArticulation)): + raise ValueError( + f"Randomization term 'randomize_rigid_body_collider_offsets' not supported for asset:" + f" '{self.asset_cfg.name}' with type: '{type(self.asset)}'." + ) + + if "newton" in env.sim.physics_manager.__name__.lower(): + self._init_newton() + else: + self._init_physx() + + def _init_physx(self) -> None: + asset = self.asset + self._default_rest = wp.to_torch(asset.root_view.get_rest_offsets()).clone() # type: ignore[union-attr] + self._default_contact = wp.to_torch(asset.root_view.get_contact_offsets()).clone() # type: ignore[union-attr] + self._env_ids_device = "cpu" + self._write_rest = lambda data, ids: asset.root_view.set_rest_offsets(data, ids) # type: ignore[union-attr] + self._write_contact = lambda data, ids: asset.root_view.set_contact_offsets(data, ids) # type: ignore[union-attr] + + def _init_newton(self) -> None: + import isaaclab_newton.physics.newton_manager as nm + from newton.solvers import SolverNotifyFlags + + notify = lambda: nm.NewtonManager.add_model_change(SolverNotifyFlags.SHAPE_PROPERTIES) # noqa: E731 + + model = nm.NewtonManager.get_model() + margin_buf = self.asset._root_view.get_attribute("shape_margin", model)[:, 0] # type: ignore[union-attr] + gap_buf = self.asset._root_view.get_attribute("shape_gap", model)[:, 0] # type: ignore[union-attr] + + self._default_rest = wp.to_torch(margin_buf).clone() + self._default_contact = wp.to_torch(gap_buf).clone() + self._env_ids_device = self._default_rest.device + margin_view = wp.to_torch(margin_buf) + gap_view = wp.to_torch(gap_buf) + + def _write_margin(data: torch.Tensor, ids: torch.Tensor) -> None: + self._default_rest[ids] = data[ids] + margin_view[ids] = data[ids] + notify() + + def _write_gap(data: torch.Tensor, ids: torch.Tensor) -> None: + gap = torch.clamp(data - self._default_rest, min=0.0) + self._default_contact[ids] = gap[ids] + gap_view[ids] = gap[ids] + notify() + + self._write_rest = _write_margin + self._write_contact = _write_gap + + def __call__( + self, + env: ManagerBasedEnv, + env_ids: torch.Tensor | None, + asset_cfg: SceneEntityCfg, + rest_offset_distribution_params: tuple[float, float] | None = None, + contact_offset_distribution_params: tuple[float, float] | None = None, + distribution: Literal["uniform", "log_uniform", "gaussian"] = "uniform", + ): + if env_ids is None: + env_ids = torch.arange(env.scene.num_envs, device=self._env_ids_device) + else: + env_ids = env_ids.to(self._env_ids_device) + + if rest_offset_distribution_params is not None: + data = _randomize_prop_by_op( + self._default_rest.clone(), rest_offset_distribution_params, + None, slice(None), operation="abs", distribution=distribution, + ) + self._write_rest(data, env_ids) + + if contact_offset_distribution_params is not None: + data = _randomize_prop_by_op( + self._default_contact.clone(), contact_offset_distribution_params, + None, slice(None), operation="abs", distribution=distribution, + ) + self._write_contact(data, env_ids) class randomize_physics_scene_gravity(ManagerTermBase): diff --git a/source/isaaclab_newton/test/assets/test_articulation.py b/source/isaaclab_newton/test/assets/test_articulation.py index 57c80c30ad4a..cc7f2b0305a3 100644 --- a/source/isaaclab_newton/test/assets/test_articulation.py +++ b/source/isaaclab_newton/test/assets/test_articulation.py @@ -2486,7 +2486,6 @@ def _patched_simulate(cls): f"body_q was stale when collide() ran: diff={diff:.4f}m, jq={jq_root.tolist()}, bq={bq_root.tolist()}" ) - @pytest.mark.parametrize("add_ground_plane", [True]) @pytest.mark.parametrize("num_articulations", [1, 2]) @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @@ -2544,6 +2543,64 @@ def test_set_material_properties(sim, num_articulations, device, add_ground_plan torch.testing.assert_close(mu_updated[:, 0:1], subset_friction) torch.testing.assert_close(restitution_updated[:, 0:1], subset_restitution) +@pytest.mark.parametrize("num_articulations", [2]) +@pytest.mark.parametrize("device", ["cuda:0"]) +@pytest.mark.parametrize("add_ground_plane", [True]) +@pytest.mark.parametrize("articulation_type", ["anymal"]) +@pytest.mark.isaacsim_ci +def test_randomize_rigid_body_com(sim, num_articulations, device, add_ground_plane, articulation_type): + """Test that randomize_rigid_body_com modifies CoM and affects simulation dynamics.""" + articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) + articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) + + sim.reset() + assert articulation.is_initialized + + original_com = wp.to_torch(articulation.data.body_com_pos_b).clone() + + com_offset = torch.zeros(num_articulations, articulation.num_bodies, 3, device=device) + com_offset[..., 0] = 0.5 + new_com = original_com + com_offset + env_ids = torch.arange(num_articulations, device=device, dtype=torch.int32) + articulation.set_coms_index(coms=new_com, env_ids=env_ids) + + updated_com = wp.to_torch(articulation.data.body_com_pos_b) + torch.testing.assert_close(updated_com, new_com, atol=1e-5, rtol=1e-5) + + +@pytest.mark.parametrize("num_articulations", [2]) +@pytest.mark.parametrize("device", ["cuda:0"]) +@pytest.mark.parametrize("add_ground_plane", [True]) +@pytest.mark.parametrize("articulation_type", ["anymal"]) +@pytest.mark.isaacsim_ci +def test_randomize_rigid_body_collider_offsets(sim, num_articulations, device, add_ground_plane, articulation_type): + """Test that Newton collider offset randomization (shape_margin, shape_gap) takes effect.""" + articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) + articulation, _ = generate_articulation(articulation_cfg, num_articulations, device=device) + + sim.reset() + assert articulation.is_initialized + + model = SimulationManager.get_model() + original_margin = wp.to_torch(articulation.root_view.get_attribute("shape_margin", model)).clone() + original_gap = wp.to_torch(articulation.root_view.get_attribute("shape_gap", model)).clone() + + new_margin = original_margin.clone() + new_margin[:, 0] += 0.01 + articulation.root_view.set_attribute("shape_margin", model, wp.from_torch(new_margin, dtype=wp.float32)) + + new_gap = original_gap.clone() + new_gap[:, 0] += 0.005 + articulation.root_view.set_attribute("shape_gap", model, wp.from_torch(new_gap, dtype=wp.float32)) + + with wp.ScopedDevice(device): + SimulationManager._solver.notify_model_changed(SolverNotifyFlags.SHAPE_PROPERTIES) + + updated_margin = wp.to_torch(articulation.root_view.get_attribute("shape_margin", model)) + updated_gap = wp.to_torch(articulation.root_view.get_attribute("shape_gap", model)) + torch.testing.assert_close(updated_margin, new_margin) + torch.testing.assert_close(updated_gap, new_gap) + if __name__ == "__main__": pytest.main([__file__, "-v", "--maxfail=1"]) From 5e0379d642188b8769796d92bee4c28d30d98c41 Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Fri, 10 Apr 2026 00:10:34 -0700 Subject: [PATCH 05/11] pass linter --- source/isaaclab/isaaclab/envs/mdp/events.py | 16 ++++++++++++---- .../test/assets/test_articulation.py | 2 ++ 2 files changed, 14 insertions(+), 4 deletions(-) diff --git a/source/isaaclab/isaaclab/envs/mdp/events.py b/source/isaaclab/isaaclab/envs/mdp/events.py index 8851c82cb9e8..4821ec9b2d4a 100644 --- a/source/isaaclab/isaaclab/envs/mdp/events.py +++ b/source/isaaclab/isaaclab/envs/mdp/events.py @@ -737,15 +737,23 @@ def __call__( if rest_offset_distribution_params is not None: data = _randomize_prop_by_op( - self._default_rest.clone(), rest_offset_distribution_params, - None, slice(None), operation="abs", distribution=distribution, + self._default_rest.clone(), + rest_offset_distribution_params, + None, + slice(None), + operation="abs", + distribution=distribution, ) self._write_rest(data, env_ids) if contact_offset_distribution_params is not None: data = _randomize_prop_by_op( - self._default_contact.clone(), contact_offset_distribution_params, - None, slice(None), operation="abs", distribution=distribution, + self._default_contact.clone(), + contact_offset_distribution_params, + None, + slice(None), + operation="abs", + distribution=distribution, ) self._write_contact(data, env_ids) diff --git a/source/isaaclab_newton/test/assets/test_articulation.py b/source/isaaclab_newton/test/assets/test_articulation.py index cc7f2b0305a3..50d726402ab3 100644 --- a/source/isaaclab_newton/test/assets/test_articulation.py +++ b/source/isaaclab_newton/test/assets/test_articulation.py @@ -2486,6 +2486,7 @@ def _patched_simulate(cls): f"body_q was stale when collide() ran: diff={diff:.4f}m, jq={jq_root.tolist()}, bq={bq_root.tolist()}" ) + @pytest.mark.parametrize("add_ground_plane", [True]) @pytest.mark.parametrize("num_articulations", [1, 2]) @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @@ -2543,6 +2544,7 @@ def test_set_material_properties(sim, num_articulations, device, add_ground_plan torch.testing.assert_close(mu_updated[:, 0:1], subset_friction) torch.testing.assert_close(restitution_updated[:, 0:1], subset_restitution) + @pytest.mark.parametrize("num_articulations", [2]) @pytest.mark.parametrize("device", ["cuda:0"]) @pytest.mark.parametrize("add_ground_plane", [True]) From 2d0d97535f5a9f4ca8db234dd503014ca99f071e Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Fri, 10 Apr 2026 01:55:32 -0700 Subject: [PATCH 06/11] support newton randomize_rigid_body_collider_offsets --- source/isaaclab/isaaclab/envs/mdp/events.py | 191 +++++++++++++++----- 1 file changed, 142 insertions(+), 49 deletions(-) diff --git a/source/isaaclab/isaaclab/envs/mdp/events.py b/source/isaaclab/isaaclab/envs/mdp/events.py index 4821ec9b2d4a..fbc91bffe9ce 100644 --- a/source/isaaclab/isaaclab/envs/mdp/events.py +++ b/source/isaaclab/isaaclab/envs/mdp/events.py @@ -656,16 +656,42 @@ def randomize_rigid_body_com( class randomize_rigid_body_collider_offsets(ManagerTermBase): """Randomize the collider parameters of rigid bodies by setting random values. - Supports both PhysX (rest/contact offset) and Newton (shape_margin/shape_gap). - For Newton, ``rest_offset`` maps to ``shape_margin`` and ``contact_offset`` is - converted to ``shape_gap`` via ``gap = contact_offset - margin``. + This function allows randomizing the collider parameters of the asset, such as rest and contact offsets. + These correspond to the physics engine collider properties that affect collision checking. + + Automatically detects the active physics backend (PhysX or Newton) and applies the appropriate + collider offset randomization strategy: + + - **PhysX**: Uses rest offset and contact offset directly via the PhysX tensor API + (``root_view.set_rest_offsets`` / ``root_view.set_contact_offsets``). + - **Newton**: Maps PhysX concepts to Newton's geometry properties. PhysX ``rest_offset`` + maps to Newton ``shape_margin``, and PhysX ``contact_offset`` is converted to Newton + ``shape_gap`` via ``gap = contact_offset - margin``. + See the `Newton collision schema`_ for details on this mapping. + + The function samples random values from the given distribution parameters and applies them + as absolute values to the collider properties. If the distribution parameters are not + provided for a particular property, the function does not modify it. + + .. _Newton collision schema: https://newton-physics.github.io/newton/latest/concepts/collisions.html .. tip:: - It is recommended to use this function only during environment initialization. + This function uses CPU tensors (PhysX) or GPU tensors (Newton) to assign the collision + properties. It is recommended to use this function only during the initialization of + the environment. """ def __init__(self, cfg: EventTermCfg, env: ManagerBasedEnv): - from isaaclab.assets import BaseArticulation, BaseRigidObject + """Initialize the term. + + Args: + cfg: The configuration of the event term. + env: The environment instance. + + Raises: + ValueError: If the asset is not a RigidObject or an Articulation. + """ + from isaaclab.assets import BaseArticulation, BaseRigidObject # noqa: PLC0415 super().__init__(cfg, env) @@ -678,48 +704,40 @@ def __init__(self, cfg: EventTermCfg, env: ManagerBasedEnv): f" '{self.asset_cfg.name}' with type: '{type(self.asset)}'." ) - if "newton" in env.sim.physics_manager.__name__.lower(): + manager_name = env.sim.physics_manager.__name__.lower() + self._backend = "newton" if "newton" in manager_name else "physx" + + if self._backend == "newton": self._init_newton() else: self._init_physx() def _init_physx(self) -> None: - asset = self.asset - self._default_rest = wp.to_torch(asset.root_view.get_rest_offsets()).clone() # type: ignore[union-attr] - self._default_contact = wp.to_torch(asset.root_view.get_contact_offsets()).clone() # type: ignore[union-attr] - self._env_ids_device = "cpu" - self._write_rest = lambda data, ids: asset.root_view.set_rest_offsets(data, ids) # type: ignore[union-attr] - self._write_contact = lambda data, ids: asset.root_view.set_contact_offsets(data, ids) # type: ignore[union-attr] + """Initialize PhysX-specific state: cache default offsets.""" + asset: PhysXArticulation | PhysXRigidObject | PhysXRigidObjectCollection = self.asset # type: ignore[assignment] + self.default_rest_offsets = wp.to_torch(asset.root_view.get_rest_offsets()).clone() + self.default_contact_offsets = wp.to_torch(asset.root_view.get_contact_offsets()).clone() def _init_newton(self) -> None: - import isaaclab_newton.physics.newton_manager as nm - from newton.solvers import SolverNotifyFlags - - notify = lambda: nm.NewtonManager.add_model_change(SolverNotifyFlags.SHAPE_PROPERTIES) # noqa: E731 - - model = nm.NewtonManager.get_model() - margin_buf = self.asset._root_view.get_attribute("shape_margin", model)[:, 0] # type: ignore[union-attr] - gap_buf = self.asset._root_view.get_attribute("shape_gap", model)[:, 0] # type: ignore[union-attr] - - self._default_rest = wp.to_torch(margin_buf).clone() - self._default_contact = wp.to_torch(gap_buf).clone() - self._env_ids_device = self._default_rest.device - margin_view = wp.to_torch(margin_buf) - gap_view = wp.to_torch(gap_buf) + """Initialize Newton-specific state: bind to shape_margin and shape_gap.""" + import isaaclab_newton.physics.newton_manager as newton_manager_module # noqa: PLC0415 + from newton.solvers import SolverNotifyFlags # noqa: PLC0415 - def _write_margin(data: torch.Tensor, ids: torch.Tensor) -> None: - self._default_rest[ids] = data[ids] - margin_view[ids] = data[ids] - notify() + self._newton_manager = newton_manager_module.NewtonManager + self._notify_shape_properties = SolverNotifyFlags.SHAPE_PROPERTIES - def _write_gap(data: torch.Tensor, ids: torch.Tensor) -> None: - gap = torch.clamp(data - self._default_rest, min=0.0) - self._default_contact[ids] = gap[ids] - gap_view[ids] = gap[ids] - notify() + asset: NewtonArticulation | NewtonRigidObject | NewtonRigidObjectCollection = self.asset # type: ignore[assignment] + model = self._newton_manager.get_model() + # TODO(newton-collider-api): We access root_view.get_attribute directly to create + # bindings for shape_margin and shape_gap because Newton doesn't yet expose dedicated + # APIs for these properties on its asset classes. This mirrors the approach used for + # material properties in randomize_rigid_body_material. When Newton adds proper + # getter/setter APIs for collision geometry properties, update this code. + self._sim_bind_shape_margin = asset._root_view.get_attribute("shape_margin", model)[:, 0] + self._sim_bind_shape_gap = asset._root_view.get_attribute("shape_gap", model)[:, 0] - self._write_rest = _write_margin - self._write_contact = _write_gap + self.default_margin = wp.to_torch(self._sim_bind_shape_margin).clone() + self.default_gap = wp.to_torch(self._sim_bind_shape_gap).clone() def __call__( self, @@ -730,32 +748,107 @@ def __call__( contact_offset_distribution_params: tuple[float, float] | None = None, distribution: Literal["uniform", "log_uniform", "gaussian"] = "uniform", ): + if self._backend == "newton": + self._call_newton( + env, env_ids, rest_offset_distribution_params, contact_offset_distribution_params, distribution + ) + else: + self._call_physx( + env, env_ids, rest_offset_distribution_params, contact_offset_distribution_params, distribution + ) + + def _call_physx( + self, + env: ManagerBasedEnv, + env_ids: torch.Tensor | None, + rest_offset_params: tuple[float, float] | None, + contact_offset_params: tuple[float, float] | None, + distribution: Literal["uniform", "log_uniform", "gaussian"], + ) -> None: + """Apply collider offset randomization via PhysX's tensor API.""" + asset: PhysXRigidObject | PhysXArticulation | PhysXRigidObjectCollection = self.asset # type: ignore[assignment] if env_ids is None: - env_ids = torch.arange(env.scene.num_envs, device=self._env_ids_device) + env_ids = torch.arange(env.scene.num_envs, device="cpu") else: - env_ids = env_ids.to(self._env_ids_device) + env_ids = env_ids.cpu() - if rest_offset_distribution_params is not None: - data = _randomize_prop_by_op( - self._default_rest.clone(), - rest_offset_distribution_params, + if rest_offset_params is not None: + rest_offset = self.default_rest_offsets.clone() + rest_offset = _randomize_prop_by_op( + rest_offset, + rest_offset_params, None, slice(None), operation="abs", distribution=distribution, ) - self._write_rest(data, env_ids) + asset.root_view.set_rest_offsets(rest_offset, env_ids) - if contact_offset_distribution_params is not None: - data = _randomize_prop_by_op( - self._default_contact.clone(), - contact_offset_distribution_params, + if contact_offset_params is not None: + contact_offset = self.default_contact_offsets.clone() + contact_offset = _randomize_prop_by_op( + contact_offset, + contact_offset_params, None, slice(None), operation="abs", distribution=distribution, ) - self._write_contact(data, env_ids) + asset.root_view.set_contact_offsets(contact_offset, env_ids) + + def _call_newton( + self, + env: ManagerBasedEnv, + env_ids: torch.Tensor | None, + rest_offset_params: tuple[float, float] | None, + contact_offset_params: tuple[float, float] | None, + distribution: Literal["uniform", "log_uniform", "gaussian"], + ) -> None: + """Apply collider offset randomization via Newton's shape geometry properties. + + Maps PhysX concepts to Newton (see ``SchemaResolverPhysx`` in newton): + + - ``rest_offset`` → ``shape_margin`` (Newton margin) + - ``contact_offset`` → ``shape_gap`` (Newton gap = contact_offset - rest_offset) + """ + device = env.device + if env_ids is None: + env_ids = torch.arange(env.scene.num_envs, device=device, dtype=torch.int32) + else: + env_ids = env_ids.to(device) + + margin_view = wp.to_torch(self._sim_bind_shape_margin) + + if rest_offset_params is not None: + margin = self.default_margin.clone() + margin = _randomize_prop_by_op( + margin, + rest_offset_params, + None, + slice(None), + operation="abs", + distribution=distribution, + ) + self.default_margin[env_ids] = margin[env_ids] + margin_view[env_ids] = margin[env_ids] + + if contact_offset_params is not None: + current_margin = self.default_margin + contact_offset = torch.zeros_like(self.default_gap) + contact_offset = _randomize_prop_by_op( + contact_offset, + contact_offset_params, + None, + slice(None), + operation="abs", + distribution=distribution, + ) + gap = torch.clamp(contact_offset - current_margin, min=0.0) + self.default_gap[env_ids] = gap[env_ids] + gap_view = wp.to_torch(self._sim_bind_shape_gap) + gap_view[env_ids] = gap[env_ids] + + self._newton_manager.add_model_change(self._notify_shape_properties) class randomize_physics_scene_gravity(ManagerTermBase): From caca5c2445343aa36a55c83b8f1830644e090f8f Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Fri, 10 Apr 2026 02:06:34 -0700 Subject: [PATCH 07/11] add note --- source/isaaclab/isaaclab/envs/mdp/events.py | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/source/isaaclab/isaaclab/envs/mdp/events.py b/source/isaaclab/isaaclab/envs/mdp/events.py index fbc91bffe9ce..ab6cd744a1ab 100644 --- a/source/isaaclab/isaaclab/envs/mdp/events.py +++ b/source/isaaclab/isaaclab/envs/mdp/events.py @@ -645,12 +645,15 @@ def randomize_rigid_body_com( coms[env_ids[:, None], body_ids, :3] += rand_samples # Newton's set_coms_index expects position-only (vec3f), while PhysX expects - # the full pose (pos + quat). Detect backend to pass the correct shape. + # the full pose (pos + quat). + # NOTE: On Newton (MuJoCo Warp), runtime COM changes may cause simulation instability + # because notify_model_changed(BODY_INERTIAL_PROPERTIES) does not fully recompute the + # mass matrix after body_ipos changes. Use with caution until this is fixed upstream. manager_name = env.sim.physics_manager.__name__.lower() if "newton" in manager_name: - asset.set_coms_index(coms=coms[..., :3], env_ids=env_ids) + asset.set_coms_index(coms=coms[:, body_ids, :3], body_ids=body_ids, env_ids=env_ids) else: - asset.set_coms_index(coms=coms, env_ids=env_ids) + asset.set_coms_index(coms=coms[:, body_ids], body_ids=body_ids, env_ids=env_ids) class randomize_rigid_body_collider_offsets(ManagerTermBase): From ecd79faba2c8604644e987ae3907465357d8d95c Mon Sep 17 00:00:00 2001 From: Antoine Richard Date: Fri, 10 Apr 2026 17:22:04 +0200 Subject: [PATCH 08/11] Address PR review: split MDPs, revert asset setters, use view-level APIs - Revert all friction/restitution/material setter additions from Newton and PhysX asset classes (9 files). Material property operations stay at the view level. - Split randomize_rigid_body_material into backend-specific classes: PhysX uses bucket-based 3-tuple materials via root_view; Newton samples friction/restitution continuously per shape via view-level attribute bindings. - Convert randomize_rigid_body_com from function to ManagerTermBase class with cached defaults for repeatable randomization. - Split randomize_rigid_body_collider_offsets into backend-specific classes: PhysX uses rest/contact offsets; Newton maps to shape_margin/shape_gap. - Add BaseRigidObjectCollection support to randomize_rigid_body_inertia. - Fix _physics_sim_view bug (was incorrectly using root_view). - Restore shape-count validation in PhysX material init. - Rewrite all tests to use view-level APIs instead of asset setters. - Un-skip Newton rigid object material tests; skip friction/restitution behavior tests that are incompatible with MuJoCo physics model. - Remove stale Newton/PhysX changelog entries, rewrite isaaclab entry. --- source/isaaclab/docs/CHANGELOG.rst | 22 +- source/isaaclab/isaaclab/envs/mdp/events.py | 687 ++++++++++-------- source/isaaclab_newton/docs/CHANGELOG.rst | 1 + .../assets/articulation/articulation.py | 184 ----- .../assets/articulation/articulation_data.py | 10 - .../assets/rigid_object/rigid_object.py | 189 ----- .../assets/rigid_object/rigid_object_data.py | 10 - .../rigid_object_collection.py | 188 ----- .../rigid_object_collection_data.py | 8 - .../test/assets/test_articulation.py | 46 +- .../test/assets/test_rigid_object.py | 111 ++- .../assets/test_rigid_object_collection.py | 36 +- source/isaaclab_physx/docs/CHANGELOG.rst | 1 + .../assets/articulation/articulation.py | 59 -- .../assets/rigid_object/rigid_object.py | 59 -- .../rigid_object_collection.py | 59 -- .../test/assets/test_articulation.py | 6 +- .../test/assets/test_rigid_object.py | 10 +- 18 files changed, 516 insertions(+), 1170 deletions(-) diff --git a/source/isaaclab/docs/CHANGELOG.rst b/source/isaaclab/docs/CHANGELOG.rst index 88342fc2a69d..97966a2cfbd7 100644 --- a/source/isaaclab/docs/CHANGELOG.rst +++ b/source/isaaclab/docs/CHANGELOG.rst @@ -8,20 +8,24 @@ Added ^^^^^ * Added :class:`~isaaclab.envs.mdp.randomize_rigid_body_inertia` event term for - randomizing body inertia tensors. Supports diagonal-only and full 3x3 modes. + randomizing body inertia tensors independently of mass. Supports diagonal-only + (Ixx, Iyy, Izz) and full 3x3 modes. Changed ^^^^^^^ -* Updated :class:`~isaaclab.envs.mdp.randomize_rigid_body_material` to use - backend-specific APIs (PhysX 3-tuple vs Newton separate friction/restitution). +* Split :class:`~isaaclab.envs.mdp.randomize_rigid_body_material` into + backend-specific implementations. PhysX uses bucket-based 3-tuple materials via the + tensor API; Newton samples friction and restitution continuously per shape via + view-level attribute bindings. +* Converted :func:`~isaaclab.envs.mdp.randomize_rigid_body_com` from a plain function + to a :class:`~isaaclab.managers.ManagerTermBase` class with repeatable randomization + from cached defaults. Newton passes position-only (vec3); PhysX passes full pose + (pos + quat). * Converted :func:`~isaaclab.envs.mdp.randomize_rigid_body_collider_offsets` from a - plain function to a :class:`~isaaclab.managers.ManagerTermBase` class with automatic - backend detection. PhysX uses rest/contact offsets directly; Newton maps them to - ``shape_margin`` and ``shape_gap``. -* Updated :func:`~isaaclab.envs.mdp.randomize_rigid_body_com` to detect the active - physics backend and pass position-only (vec3) for Newton vs full pose (pos + quat) - for PhysX. + plain function to a :class:`~isaaclab.managers.ManagerTermBase` class with + backend-specific implementations. PhysX uses rest/contact offsets directly; Newton + maps them to ``shape_margin`` and ``shape_gap``. 4.5.30 (2026-04-13) diff --git a/source/isaaclab/isaaclab/envs/mdp/events.py b/source/isaaclab/isaaclab/envs/mdp/events.py index ab6cd744a1ab..60849f651ea3 100644 --- a/source/isaaclab/isaaclab/envs/mdp/events.py +++ b/source/isaaclab/isaaclab/envs/mdp/events.py @@ -29,13 +29,7 @@ from isaaclab.utils.version import compare_versions if TYPE_CHECKING: - from isaaclab_newton.assets import Articulation as NewtonArticulation - from isaaclab_newton.assets import RigidObject as NewtonRigidObject - from isaaclab_newton.assets import RigidObjectCollection as NewtonRigidObjectCollection - from isaaclab_physx.assets import Articulation as PhysXArticulation from isaaclab_physx.assets import DeformableObject - from isaaclab_physx.assets import RigidObject as PhysXRigidObject - from isaaclab_physx.assets import RigidObjectCollection as PhysXRigidObjectCollection from isaaclab.assets import Articulation, RigidObject from isaaclab.envs import ManagerBasedEnv @@ -157,36 +151,219 @@ def randomize_rigid_body_scale( op_order_spec.default = Vt.TokenArray(["xformOp:translate", "xformOp:orient", "xformOp:scale"]) +class _RandomizeRigidBodyMaterialPhysx: + """PhysX backend implementation for material randomization. + + Uses the bucket-based approach required by PhysX's 64000 unique material limit. + Materials are pre-sampled into buckets and randomly assigned to shapes. + """ + + def __init__( + self, cfg: EventTermCfg, env: ManagerBasedEnv, asset: RigidObject | Articulation, asset_cfg: SceneEntityCfg + ): + from isaaclab.assets import BaseArticulation + + # obtain parameters for sampling friction and restitution values + static_friction_range = cfg.params.get("static_friction_range", (1.0, 1.0)) + dynamic_friction_range = cfg.params.get("dynamic_friction_range", (1.0, 1.0)) + restitution_range = cfg.params.get("restitution_range", (0.0, 0.0)) + num_buckets = int(cfg.params.get("num_buckets", 1)) + + # sample material properties from the given ranges + # note: we only sample the materials once during initialization + # afterwards these are randomly assigned to the geometries of the asset + range_list = [static_friction_range, dynamic_friction_range, restitution_range] + ranges = torch.tensor(range_list, device="cpu") + self.material_buckets = math_utils.sample_uniform(ranges[:, 0], ranges[:, 1], (num_buckets, 3), device="cpu") + + # ensure dynamic friction is always less than static friction + make_consistent = cfg.params.get("make_consistent", False) + if make_consistent: + self.material_buckets[:, 1] = torch.min(self.material_buckets[:, 0], self.material_buckets[:, 1]) + + self.asset = asset + self.asset_cfg = asset_cfg + + # obtain number of shapes per body (needed for indexing the material properties correctly) + # note: this is a workaround since the Articulation does not provide a direct way to obtain the number of shapes + # per body. We use the physics simulation view to obtain the number of shapes per body. + if isinstance(asset, BaseArticulation) and asset_cfg.body_ids != slice(None): + self.num_shapes_per_body = [] + for link_path in asset.root_view.link_paths[0]: + link_physx_view = asset._physics_sim_view.create_rigid_body_view(link_path) # type: ignore + self.num_shapes_per_body.append(link_physx_view.max_shapes) + # ensure the parsing is correct + num_shapes = sum(self.num_shapes_per_body) + expected_shapes = asset.root_view.max_shapes + if num_shapes != expected_shapes: + raise ValueError( + "Randomization term 'randomize_rigid_body_material' failed to parse the number of shapes per body." + f" Expected total shapes: {expected_shapes}, but got: {num_shapes}." + ) + else: + # in this case, we don't need to do special indexing + self.num_shapes_per_body = None + + def __call__( + self, + env: ManagerBasedEnv, + env_ids: torch.Tensor | None, + static_friction_range: tuple[float, float], + dynamic_friction_range: tuple[float, float], + restitution_range: tuple[float, float], + num_buckets: int, + asset_cfg: SceneEntityCfg, + make_consistent: bool = False, + ): + # resolve environment ids + if env_ids is None: + env_ids = torch.arange(env.scene.num_envs, device="cpu", dtype=torch.int32) + else: + env_ids = env_ids.cpu() + + # randomly assign material IDs to the geometries + total_num_shapes = self.asset.root_view.max_shapes + bucket_ids = torch.randint(0, num_buckets, (len(env_ids), total_num_shapes), device="cpu") + material_samples = self.material_buckets[bucket_ids] + + # retrieve material buffer from the physics simulation + materials = wp.to_torch(self.asset.root_view.get_material_properties()) + + # update material buffer with new samples + if self.num_shapes_per_body is not None: + # sample material properties from the given ranges + for body_id in self.asset_cfg.body_ids: + # obtain indices of shapes for the body + start_idx = sum(self.num_shapes_per_body[:body_id]) + end_idx = start_idx + self.num_shapes_per_body[body_id] + # assign the new materials + # material samples are of shape: num_env_ids x total_num_shapes x 3 + materials[env_ids, start_idx:end_idx] = material_samples[:, start_idx:end_idx] + else: + # assign all the materials + materials[env_ids] = material_samples[:] + + # apply to simulation + self.asset.root_view.set_material_properties( + wp.from_torch(materials, dtype=wp.float32), wp.from_torch(env_ids, dtype=wp.int32) + ) + + +class _RandomizeRigidBodyMaterialNewton: + """Newton backend implementation for material randomization. + + Newton can assign arbitrary friction/restitution per shape (no bucket limitation). + Samples friction (mu) and restitution continuously from the given ranges. + Newton uses a single friction coefficient (mu), so ``dynamic_friction_range`` + and ``num_buckets`` are ignored. + """ + + def __init__( + self, cfg: EventTermCfg, env: ManagerBasedEnv, asset: RigidObject | Articulation, asset_cfg: SceneEntityCfg + ): + import isaaclab_newton.physics.newton_manager as newton_manager_module # noqa: PLC0415 + from newton.solvers import SolverNotifyFlags # noqa: PLC0415 + + from isaaclab.assets import BaseArticulation + + self.asset = asset + self.asset_cfg = asset_cfg + self._newton_manager = newton_manager_module.NewtonManager + self._notify_shape_properties = SolverNotifyFlags.SHAPE_PROPERTIES + + # cache friction/restitution ranges for continuous per-shape sampling + self._static_friction_range = cfg.params.get("static_friction_range", (1.0, 1.0)) + self._restitution_range = cfg.params.get("restitution_range", (0.0, 0.0)) + + # compute shape indices for body-specific randomization + if isinstance(asset, BaseArticulation) and asset_cfg.body_ids != slice(None): + num_shapes_per_body = asset.num_shapes_per_body + shape_indices_list = [] + for body_id in asset_cfg.body_ids: + start_idx = sum(num_shapes_per_body[:body_id]) + end_idx = start_idx + num_shapes_per_body[body_id] + shape_indices_list.extend(range(start_idx, end_idx)) + self._shape_indices = torch.tensor(shape_indices_list, dtype=torch.long) + else: + total_shapes = sum(asset.num_shapes_per_body) + self._shape_indices = torch.arange(total_shapes, dtype=torch.long) + + # get friction/restitution view-level bindings + model = self._newton_manager.get_model() + self._friction_binding = asset._root_view.get_attribute("shape_material_mu", model)[:, 0] # type: ignore + self._restitution_binding = asset._root_view.get_attribute("shape_material_restitution", model)[:, 0] # type: ignore + + # cache defaults + self._default_friction = wp.to_torch(self._friction_binding).clone() + self._default_restitution = wp.to_torch(self._restitution_binding).clone() + + def __call__( + self, + env: ManagerBasedEnv, + env_ids: torch.Tensor | None, + static_friction_range: tuple[float, float], + dynamic_friction_range: tuple[float, float], + restitution_range: tuple[float, float], + num_buckets: int, + asset_cfg: SceneEntityCfg, + make_consistent: bool = False, + ): + device = env.device + # resolve environment ids + if env_ids is None: + env_ids = torch.arange(env.scene.num_envs, device=device, dtype=torch.int32) + else: + env_ids = env_ids.to(device) + + num_shapes = len(self._shape_indices) + shape_idx = self._shape_indices.to(device) + + # sample friction (mu) and restitution continuously per shape + friction_range = torch.tensor(self._static_friction_range, device=device) + restitution_range_t = torch.tensor(self._restitution_range, device=device) + friction_samples = math_utils.sample_uniform( + friction_range[0], friction_range[1], (len(env_ids), num_shapes), device=device + ) + restitution_samples = math_utils.sample_uniform( + restitution_range_t[0], restitution_range_t[1], (len(env_ids), num_shapes), device=device + ) + + # update cached buffers for the affected env_ids + self._default_friction[env_ids[:, None], shape_idx] = friction_samples + self._default_restitution[env_ids[:, None], shape_idx] = restitution_samples + + # write only the affected env_ids to the warp binding + friction_view = wp.to_torch(self._friction_binding) + restitution_view = wp.to_torch(self._restitution_binding) + friction_view[env_ids[:, None], shape_idx] = friction_samples + restitution_view[env_ids[:, None], shape_idx] = restitution_samples + + # notify the physics engine + self._newton_manager.add_model_change(self._notify_shape_properties) + + class randomize_rigid_body_material(ManagerTermBase): """Randomize the physics materials on all geometries of the asset. This function creates a set of physics materials with random static friction, dynamic friction, and restitution - values. The number of materials is specified by ``num_buckets``. The materials are generated by sampling - uniform random values from the given ranges. + values and assigns them to the geometries of the asset. - Automatically detects the active physics backend (PhysX or Newton) and applies the appropriate - material randomization strategy: + Automatically detects the active physics backend (PhysX or Newton) and delegates to + the appropriate backend-specific implementation: - **PhysX**: Uses the 3-tuple material format (static_friction, dynamic_friction, restitution) - and applies via the unified ``set_material_properties_index`` method. - - **Newton**: Uses separate friction (mu) and restitution values, applying them via - ``set_friction_index`` and ``set_restitution_index`` methods. Newton uses a single - friction coefficient (mu), so only the static friction value is used. - - The material properties are then assigned to the geometries of the asset. The assignment is done by - creating a random integer tensor of shape (num_instances, max_num_shapes) where ``num_instances`` - is the number of assets spawned and ``max_num_shapes`` is the maximum number of shapes in the asset (over - all bodies). The integer values are used as indices to select the material properties from the - material buckets. + with bucket-based assignment (limited to 64000 unique materials). Applied via the PhysX + tensor API (``root_view.set_material_properties``). + - **Newton**: Samples friction (mu) and restitution continuously per shape (no bucket + limitation). Newton uses a single friction coefficient, so ``dynamic_friction_range`` + and ``num_buckets`` are ignored. Applied directly to Newton's view-level bindings. If the flag ``make_consistent`` is set to ``True``, the dynamic friction is set to be less than or equal to - the static friction. This obeys the physics constraint on friction values. However, it may not always be - essential for the application. Thus, the flag is set to ``False`` by default. + the static friction (PhysX only). This obeys the physics constraint on friction values. .. attention:: - This function uses CPU tensors to assign the material properties. It is recommended to use this function - only during the initialization of the environment. Otherwise, it may lead to a significant performance - overhead. + On PhysX, this function uses CPU tensors to assign the material properties. It is recommended to + use this function only during the initialization of the environment. .. note:: PhysX only allows 64000 unique physics materials in the scene. If the number of materials exceeds this @@ -218,82 +395,12 @@ def __init__(self, cfg: EventTermCfg, env: ManagerBasedEnv): f" with type: '{type(self.asset)}'." ) - # detect physics backend and initialize backend-specific state + # detect physics backend and instantiate the appropriate implementation manager_name = env.sim.physics_manager.__name__.lower() - self._backend = "newton" if "newton" in manager_name else "physx" - - # obtain parameters for sampling friction and restitution values - static_friction_range = cfg.params.get("static_friction_range", (1.0, 1.0)) - dynamic_friction_range = cfg.params.get("dynamic_friction_range", (1.0, 1.0)) - restitution_range = cfg.params.get("restitution_range", (0.0, 0.0)) - num_buckets = int(cfg.params.get("num_buckets", 1)) - - # sample material properties from the given ranges - # note: we only sample the materials once during initialization - # afterwards these are randomly assigned to the geometries of the asset - ranges = torch.tensor([static_friction_range, dynamic_friction_range, restitution_range], device="cpu") - self.material_buckets = math_utils.sample_uniform(ranges[:, 0], ranges[:, 1], (num_buckets, 3), device="cpu") - - # ensure dynamic friction is always less than static friction - make_consistent = cfg.params.get("make_consistent", False) - if make_consistent: - self.material_buckets[:, 1] = torch.min(self.material_buckets[:, 0], self.material_buckets[:, 1]) - - # initialize backend-specific state (shape indices, cached materials) - use_body_ids = isinstance(self.asset, BaseArticulation) and self.asset_cfg.body_ids != slice(None) - if self._backend == "newton": - self._init_newton(use_body_ids) - else: - self._init_physx(use_body_ids) - - def _init_physx(self, use_body_ids: bool) -> None: - """Initialize PhysX-specific state: shape indices and cached materials.""" - asset: PhysXArticulation | PhysXRigidObject | PhysXRigidObjectCollection = self.asset # type: ignore[assignment] - - # compute shape indices - if use_body_ids: - # note: workaround since Articulation doesn't expose shapes per body directly - num_shapes_per_body = [] - for link_path in asset.root_view.link_paths[0]: # type: ignore[union-attr] - link_physx_view = asset.root_view.create_rigid_body_view(link_path) # type: ignore - num_shapes_per_body.append(link_physx_view.max_shapes) - shape_indices_list = [] - for body_id in self.asset_cfg.body_ids: - start_idx = sum(num_shapes_per_body[:body_id]) - end_idx = start_idx + num_shapes_per_body[body_id] - shape_indices_list.extend(range(start_idx, end_idx)) - self.shape_indices = torch.tensor(shape_indices_list, dtype=torch.long) - else: - self.shape_indices = torch.arange(asset.root_view.max_shapes, dtype=torch.long) - - # cache default materials - self.default_materials = wp.to_torch(asset.root_view.get_material_properties()).clone() - - def _init_newton(self, use_body_ids: bool) -> None: - """Initialize Newton-specific state: shape indices and cached materials.""" - asset: NewtonArticulation | NewtonRigidObject | NewtonRigidObjectCollection = self.asset # type: ignore[assignment] - - # compute shape indices - if use_body_ids: - num_shapes_per_body = asset.num_shapes_per_body # type: ignore[union-attr] - shape_indices_list = [] - for body_id in self.asset_cfg.body_ids: - start_idx = sum(num_shapes_per_body[:body_id]) - end_idx = start_idx + num_shapes_per_body[body_id] - shape_indices_list.extend(range(start_idx, end_idx)) - self.shape_indices = torch.tensor(shape_indices_list, dtype=torch.long) + if "newton" in manager_name: + self._impl = _RandomizeRigidBodyMaterialNewton(cfg, env, self.asset, self.asset_cfg) else: - self.shape_indices = torch.arange(asset.num_shapes, dtype=torch.long) - - # cache default materials - # TODO(newton-material-api): We access internal bindings `_sim_bind_shape_material_*` directly - # because Newton's friction model (single mu) differs from PhysX (static/dynamic friction). - # This is not ideal - we should either: - # 1. Add a unified getter API that abstracts backend differences, or - # 2. Use Newton's selection API (e.g., root_view.get_attribute) for querying - # For now, internal code accesses the bindings directly while we iterate on the design. - self.default_friction = wp.to_torch(asset.data._sim_bind_shape_material_mu).clone() - self.default_restitution = wp.to_torch(asset.data._sim_bind_shape_material_restitution).clone() + self._impl = _RandomizeRigidBodyMaterialPhysx(cfg, env, self.asset, self.asset_cfg) def __call__( self, @@ -306,46 +413,16 @@ def __call__( asset_cfg: SceneEntityCfg, make_consistent: bool = False, ): - # determine device based on backend - device = env.device if self._backend == "newton" else "cpu" # physx uses cpu to process material randomization - - # resolve environment ids - if env_ids is None: - env_ids = torch.arange(env.scene.num_envs, device=device, dtype=torch.int32) - else: - env_ids = env_ids.to(device) - - # randomly assign material IDs to the geometries - total_num_shapes = len(self.shape_indices) - bucket_ids = torch.randint(0, num_buckets, (len(env_ids), total_num_shapes), device=device) - shape_idx = self.shape_indices.to(device) - material_samples = self.material_buckets.to(device)[bucket_ids] - - # dispatch to backend-specific implementation - if self._backend == "newton": - self._call_newton(env_ids, shape_idx, material_samples) - else: - self._call_physx(env_ids, shape_idx, material_samples) - - def _call_physx(self, env_ids: torch.Tensor, shape_idx: torch.Tensor, material_samples: torch.Tensor) -> None: - """Apply material randomization via PhysX's 3-tuple material format.""" - # update cached material buffer with new samples (vectorized) - self.default_materials[env_ids[:, None], shape_idx] = material_samples - - # apply to simulation via PhysX's unified material API - asset: PhysXRigidObject | PhysXArticulation | PhysXRigidObjectCollection = self.asset # type: ignore[assignment] - asset.set_material_properties_index(materials=self.default_materials, env_ids=env_ids) - - def _call_newton(self, env_ids: torch.Tensor, shape_idx: torch.Tensor, material_samples: torch.Tensor) -> None: - """Apply material randomization via Newton's native friction/restitution API.""" - # update cached default buffers with new samples (vectorized) - self.default_friction[env_ids[:, None], shape_idx] = material_samples[..., 0] - self.default_restitution[env_ids[:, None], shape_idx] = material_samples[..., 2] - - # apply via Newton's setter methods (handles kernel write + notification) - asset: NewtonRigidObject | NewtonArticulation | NewtonRigidObjectCollection = self.asset # type: ignore[assignment] - asset.set_friction_mask(friction=self.default_friction) - asset.set_restitution_mask(restitution=self.default_restitution) + self._impl( + env, + env_ids, + static_friction_range, + dynamic_friction_range, + restitution_range, + num_buckets, + asset_cfg, + make_consistent, + ) class randomize_rigid_body_mass(ManagerTermBase): @@ -501,7 +578,7 @@ def __init__(self, cfg: EventTermCfg, env: ManagerBasedEnv): ValueError: If the lower bound is negative or zero when not allowed for scale operation. ValueError: If the upper bound is less than the lower bound. """ - from isaaclab.assets import BaseArticulation, BaseRigidObject + from isaaclab.assets import BaseArticulation, BaseRigidObject, BaseRigidObjectCollection super().__init__(cfg, env) @@ -509,7 +586,7 @@ def __init__(self, cfg: EventTermCfg, env: ManagerBasedEnv): self.asset_cfg: SceneEntityCfg = cfg.params["asset_cfg"] self.asset: RigidObject | Articulation = env.scene[self.asset_cfg.name] - if not isinstance(self.asset, (BaseRigidObject, BaseArticulation)): + if not isinstance(self.asset, (BaseRigidObject, BaseArticulation, BaseRigidObjectCollection)): raise ValueError( f"Randomization term 'randomize_rigid_body_inertia' not supported for asset: '{self.asset_cfg.name}'" f" with type: '{type(self.asset)}'." @@ -605,83 +682,19 @@ def __call__( self.asset.set_inertias_index(inertias=inertias, body_ids=body_ids, env_ids=env_ids) -def randomize_rigid_body_com( - env: ManagerBasedEnv, - env_ids: torch.Tensor | None, - com_range: dict[str, tuple[float, float]], - asset_cfg: SceneEntityCfg, -): +class randomize_rigid_body_com(ManagerTermBase): """Randomize the center of mass (CoM) of rigid bodies by adding a random value sampled from the given ranges. - .. note:: - This function does not properly track the original CoM values. It is recommended to use this function - only once, during the initialization of the environment. - """ - # extract the used quantities (to enable type-hinting) - asset: Articulation = env.scene[asset_cfg.name] - # resolve environment ids - if env_ids is None: - env_ids = torch.arange(env.scene.num_envs, device=asset.device) - else: - env_ids = env_ids.to(asset.device) - - # resolve body indices - if asset_cfg.body_ids == slice(None): - body_ids = torch.arange(asset.num_bodies, dtype=torch.int, device=asset.device) - else: - body_ids = torch.tensor(asset_cfg.body_ids, dtype=torch.int, device=asset.device) - - # sample random CoM values - range_list = [com_range.get(key, (0.0, 0.0)) for key in ["x", "y", "z"]] - ranges = torch.tensor(range_list, device=asset.device) - rand_samples = math_utils.sample_uniform( - ranges[:, 0], ranges[:, 1], (len(env_ids), 3), device=asset.device - ).unsqueeze(1) - - # get the current com of the bodies (num_assets, num_bodies, 7) - coms = wp.to_torch(asset.data.body_com_pose_b).clone() - - # Randomize the com position - coms[env_ids[:, None], body_ids, :3] += rand_samples - - # Newton's set_coms_index expects position-only (vec3f), while PhysX expects - # the full pose (pos + quat). - # NOTE: On Newton (MuJoCo Warp), runtime COM changes may cause simulation instability - # because notify_model_changed(BODY_INERTIAL_PROPERTIES) does not fully recompute the - # mass matrix after body_ipos changes. Use with caution until this is fixed upstream. - manager_name = env.sim.physics_manager.__name__.lower() - if "newton" in manager_name: - asset.set_coms_index(coms=coms[:, body_ids, :3], body_ids=body_ids, env_ids=env_ids) - else: - asset.set_coms_index(coms=coms[:, body_ids], body_ids=body_ids, env_ids=env_ids) - - -class randomize_rigid_body_collider_offsets(ManagerTermBase): - """Randomize the collider parameters of rigid bodies by setting random values. - - This function allows randomizing the collider parameters of the asset, such as rest and contact offsets. - These correspond to the physics engine collider properties that affect collision checking. - - Automatically detects the active physics backend (PhysX or Newton) and applies the appropriate - collider offset randomization strategy: - - - **PhysX**: Uses rest offset and contact offset directly via the PhysX tensor API - (``root_view.set_rest_offsets`` / ``root_view.set_contact_offsets``). - - **Newton**: Maps PhysX concepts to Newton's geometry properties. PhysX ``rest_offset`` - maps to Newton ``shape_margin``, and PhysX ``contact_offset`` is converted to Newton - ``shape_gap`` via ``gap = contact_offset - margin``. - See the `Newton collision schema`_ for details on this mapping. - - The function samples random values from the given distribution parameters and applies them - as absolute values to the collider properties. If the distribution parameters are not - provided for a particular property, the function does not modify it. + This class tracks the original CoM values and randomizes from those defaults on each call, + ensuring repeatable randomization across resets. - .. _Newton collision schema: https://newton-physics.github.io/newton/latest/concepts/collisions.html + Automatically detects the active physics backend: - .. tip:: - This function uses CPU tensors (PhysX) or GPU tensors (Newton) to assign the collision - properties. It is recommended to use this function only during the initialization of - the environment. + - **PhysX**: Passes the full CoM pose (position + quaternion) to ``set_coms_index``. + - **Newton**: Passes position-only (vec3) to ``set_coms_index``. Note that on Newton + (MuJoCo Warp), runtime CoM changes may cause simulation instability because + ``notify_model_changed(BODY_INERTIAL_PROPERTIES)`` does not fully recompute the + mass matrix after ``body_ipos`` changes. Use with caution until this is fixed upstream. """ def __init__(self, cfg: EventTermCfg, env: ManagerBasedEnv): @@ -690,57 +703,69 @@ def __init__(self, cfg: EventTermCfg, env: ManagerBasedEnv): Args: cfg: The configuration of the event term. env: The environment instance. - - Raises: - ValueError: If the asset is not a RigidObject or an Articulation. """ - from isaaclab.assets import BaseArticulation, BaseRigidObject # noqa: PLC0415 - super().__init__(cfg, env) self.asset_cfg: SceneEntityCfg = cfg.params["asset_cfg"] self.asset: RigidObject | Articulation = env.scene[self.asset_cfg.name] - if not isinstance(self.asset, (BaseRigidObject, BaseArticulation)): - raise ValueError( - f"Randomization term 'randomize_rigid_body_collider_offsets' not supported for asset:" - f" '{self.asset_cfg.name}' with type: '{type(self.asset)}'." - ) - + # detect physics backend manager_name = env.sim.physics_manager.__name__.lower() - self._backend = "newton" if "newton" in manager_name else "physx" + self._is_newton = "newton" in manager_name - if self._backend == "newton": - self._init_newton() + self.default_com = None + + def __call__( + self, + env: ManagerBasedEnv, + env_ids: torch.Tensor | None, + com_range: dict[str, tuple[float, float]], + asset_cfg: SceneEntityCfg, + ): + # store default CoM on first call for repeatable randomization + if self.default_com is None: + self.default_com = wp.to_torch(self.asset.data.body_com_pose_b).clone() + + # resolve environment ids + if env_ids is None: + env_ids = torch.arange(env.scene.num_envs, device=self.asset.device) else: - self._init_physx() + env_ids = env_ids.to(self.asset.device) - def _init_physx(self) -> None: - """Initialize PhysX-specific state: cache default offsets.""" - asset: PhysXArticulation | PhysXRigidObject | PhysXRigidObjectCollection = self.asset # type: ignore[assignment] - self.default_rest_offsets = wp.to_torch(asset.root_view.get_rest_offsets()).clone() - self.default_contact_offsets = wp.to_torch(asset.root_view.get_contact_offsets()).clone() + # resolve body indices + if self.asset_cfg.body_ids == slice(None): + body_ids = torch.arange(self.asset.num_bodies, dtype=torch.int, device=self.asset.device) + else: + body_ids = torch.tensor(self.asset_cfg.body_ids, dtype=torch.int, device=self.asset.device) + + # sample random CoM values + range_list = [com_range.get(key, (0.0, 0.0)) for key in ["x", "y", "z"]] + ranges = torch.tensor(range_list, device=self.asset.device) + rand_samples = math_utils.sample_uniform( + ranges[:, 0], ranges[:, 1], (len(env_ids), 3), device=self.asset.device + ).unsqueeze(1) + + # start from defaults and add random offsets + coms = self.default_com.clone() + coms[env_ids[:, None], body_ids, :3] += rand_samples + + # Newton expects position-only (vec3f), PhysX expects the full pose (pos + quat) + if self._is_newton: + self.asset.set_coms_index(coms=coms[:, body_ids, :3], body_ids=body_ids, env_ids=env_ids) + else: + self.asset.set_coms_index(coms=coms[:, body_ids], body_ids=body_ids, env_ids=env_ids) - def _init_newton(self) -> None: - """Initialize Newton-specific state: bind to shape_margin and shape_gap.""" - import isaaclab_newton.physics.newton_manager as newton_manager_module # noqa: PLC0415 - from newton.solvers import SolverNotifyFlags # noqa: PLC0415 - self._newton_manager = newton_manager_module.NewtonManager - self._notify_shape_properties = SolverNotifyFlags.SHAPE_PROPERTIES +class _RandomizeRigidBodyColliderOffsetsPhysx: + """PhysX backend implementation for collider offset randomization. - asset: NewtonArticulation | NewtonRigidObject | NewtonRigidObjectCollection = self.asset # type: ignore[assignment] - model = self._newton_manager.get_model() - # TODO(newton-collider-api): We access root_view.get_attribute directly to create - # bindings for shape_margin and shape_gap because Newton doesn't yet expose dedicated - # APIs for these properties on its asset classes. This mirrors the approach used for - # material properties in randomize_rigid_body_material. When Newton adds proper - # getter/setter APIs for collision geometry properties, update this code. - self._sim_bind_shape_margin = asset._root_view.get_attribute("shape_margin", model)[:, 0] - self._sim_bind_shape_gap = asset._root_view.get_attribute("shape_gap", model)[:, 0] + Uses rest offset and contact offset directly via the PhysX tensor API. + """ - self.default_margin = wp.to_torch(self._sim_bind_shape_margin).clone() - self.default_gap = wp.to_torch(self._sim_bind_shape_gap).clone() + def __init__(self, asset: RigidObject | Articulation): + self.asset = asset + self.default_rest_offsets = wp.to_torch(asset.root_view.get_rest_offsets()).clone() + self.default_contact_offsets = wp.to_torch(asset.root_view.get_contact_offsets()).clone() def __call__( self, @@ -751,69 +776,73 @@ def __call__( contact_offset_distribution_params: tuple[float, float] | None = None, distribution: Literal["uniform", "log_uniform", "gaussian"] = "uniform", ): - if self._backend == "newton": - self._call_newton( - env, env_ids, rest_offset_distribution_params, contact_offset_distribution_params, distribution - ) - else: - self._call_physx( - env, env_ids, rest_offset_distribution_params, contact_offset_distribution_params, distribution - ) - - def _call_physx( - self, - env: ManagerBasedEnv, - env_ids: torch.Tensor | None, - rest_offset_params: tuple[float, float] | None, - contact_offset_params: tuple[float, float] | None, - distribution: Literal["uniform", "log_uniform", "gaussian"], - ) -> None: - """Apply collider offset randomization via PhysX's tensor API.""" - asset: PhysXRigidObject | PhysXArticulation | PhysXRigidObjectCollection = self.asset # type: ignore[assignment] if env_ids is None: env_ids = torch.arange(env.scene.num_envs, device="cpu") else: env_ids = env_ids.cpu() - if rest_offset_params is not None: + if rest_offset_distribution_params is not None: rest_offset = self.default_rest_offsets.clone() rest_offset = _randomize_prop_by_op( rest_offset, - rest_offset_params, + rest_offset_distribution_params, None, slice(None), operation="abs", distribution=distribution, ) - asset.root_view.set_rest_offsets(rest_offset, env_ids) + self.asset.root_view.set_rest_offsets(rest_offset, env_ids) - if contact_offset_params is not None: + if contact_offset_distribution_params is not None: contact_offset = self.default_contact_offsets.clone() contact_offset = _randomize_prop_by_op( contact_offset, - contact_offset_params, + contact_offset_distribution_params, None, slice(None), operation="abs", distribution=distribution, ) - asset.root_view.set_contact_offsets(contact_offset, env_ids) + self.asset.root_view.set_contact_offsets(contact_offset, env_ids) - def _call_newton( + +class _RandomizeRigidBodyColliderOffsetsNewton: + """Newton backend implementation for collider offset randomization. + + Maps PhysX concepts to Newton's geometry properties: + + - ``rest_offset`` -> ``shape_margin`` (Newton margin) + - ``contact_offset`` -> ``shape_gap`` (Newton gap = contact_offset - margin) + + See the `Newton collision schema`_ for details. + + .. _Newton collision schema: https://newton-physics.github.io/newton/latest/concepts/collisions.html + """ + + def __init__(self, asset: RigidObject | Articulation): + import isaaclab_newton.physics.newton_manager as newton_manager_module # noqa: PLC0415 + from newton.solvers import SolverNotifyFlags # noqa: PLC0415 + + self.asset = asset + self._newton_manager = newton_manager_module.NewtonManager + self._notify_shape_properties = SolverNotifyFlags.SHAPE_PROPERTIES + + model = self._newton_manager.get_model() + self._sim_bind_shape_margin = asset._root_view.get_attribute("shape_margin", model)[:, 0] # type: ignore + self._sim_bind_shape_gap = asset._root_view.get_attribute("shape_gap", model)[:, 0] # type: ignore + + self.default_margin = wp.to_torch(self._sim_bind_shape_margin).clone() + self.default_gap = wp.to_torch(self._sim_bind_shape_gap).clone() + + def __call__( self, env: ManagerBasedEnv, env_ids: torch.Tensor | None, - rest_offset_params: tuple[float, float] | None, - contact_offset_params: tuple[float, float] | None, - distribution: Literal["uniform", "log_uniform", "gaussian"], - ) -> None: - """Apply collider offset randomization via Newton's shape geometry properties. - - Maps PhysX concepts to Newton (see ``SchemaResolverPhysx`` in newton): - - - ``rest_offset`` → ``shape_margin`` (Newton margin) - - ``contact_offset`` → ``shape_gap`` (Newton gap = contact_offset - rest_offset) - """ + asset_cfg: SceneEntityCfg, + rest_offset_distribution_params: tuple[float, float] | None = None, + contact_offset_distribution_params: tuple[float, float] | None = None, + distribution: Literal["uniform", "log_uniform", "gaussian"] = "uniform", + ): device = env.device if env_ids is None: env_ids = torch.arange(env.scene.num_envs, device=device, dtype=torch.int32) @@ -822,11 +851,11 @@ def _call_newton( margin_view = wp.to_torch(self._sim_bind_shape_margin) - if rest_offset_params is not None: + if rest_offset_distribution_params is not None: margin = self.default_margin.clone() margin = _randomize_prop_by_op( margin, - rest_offset_params, + rest_offset_distribution_params, None, slice(None), operation="abs", @@ -835,12 +864,12 @@ def _call_newton( self.default_margin[env_ids] = margin[env_ids] margin_view[env_ids] = margin[env_ids] - if contact_offset_params is not None: + if contact_offset_distribution_params is not None: current_margin = self.default_margin contact_offset = torch.zeros_like(self.default_gap) contact_offset = _randomize_prop_by_op( contact_offset, - contact_offset_params, + contact_offset_distribution_params, None, slice(None), operation="abs", @@ -854,6 +883,80 @@ def _call_newton( self._newton_manager.add_model_change(self._notify_shape_properties) +class randomize_rigid_body_collider_offsets(ManagerTermBase): + """Randomize the collider parameters of rigid bodies by setting random values. + + This function allows randomizing the collider parameters of the asset, such as rest and contact offsets. + These correspond to the physics engine collider properties that affect collision checking. + + Automatically detects the active physics backend (PhysX or Newton) and delegates to + the appropriate backend-specific implementation: + + - **PhysX**: Uses rest offset and contact offset directly via the PhysX tensor API + (``root_view.set_rest_offsets`` / ``root_view.set_contact_offsets``). + - **Newton**: Maps PhysX concepts to Newton's geometry properties. PhysX ``rest_offset`` + maps to Newton ``shape_margin``, and PhysX ``contact_offset`` is converted to Newton + ``shape_gap`` via ``gap = contact_offset - margin``. + + The function samples random values from the given distribution parameters and applies them + as absolute values to the collider properties. If the distribution parameters are not + provided for a particular property, the function does not modify it. + + .. tip:: + This function uses CPU tensors (PhysX) or GPU tensors (Newton) to assign the collision + properties. It is recommended to use this function only during the initialization of + the environment. + """ + + def __init__(self, cfg: EventTermCfg, env: ManagerBasedEnv): + """Initialize the term. + + Args: + cfg: The configuration of the event term. + env: The environment instance. + + Raises: + ValueError: If the asset is not a RigidObject or an Articulation. + """ + from isaaclab.assets import BaseArticulation, BaseRigidObject + + super().__init__(cfg, env) + + self.asset_cfg: SceneEntityCfg = cfg.params["asset_cfg"] + self.asset: RigidObject | Articulation = env.scene[self.asset_cfg.name] + + if not isinstance(self.asset, (BaseRigidObject, BaseArticulation)): + raise ValueError( + f"Randomization term 'randomize_rigid_body_collider_offsets' not supported for asset:" + f" '{self.asset_cfg.name}' with type: '{type(self.asset)}'." + ) + + # detect physics backend and instantiate the appropriate implementation + manager_name = env.sim.physics_manager.__name__.lower() + if "newton" in manager_name: + self._impl = _RandomizeRigidBodyColliderOffsetsNewton(self.asset) + else: + self._impl = _RandomizeRigidBodyColliderOffsetsPhysx(self.asset) + + def __call__( + self, + env: ManagerBasedEnv, + env_ids: torch.Tensor | None, + asset_cfg: SceneEntityCfg, + rest_offset_distribution_params: tuple[float, float] | None = None, + contact_offset_distribution_params: tuple[float, float] | None = None, + distribution: Literal["uniform", "log_uniform", "gaussian"] = "uniform", + ): + self._impl( + env, + env_ids, + asset_cfg, + rest_offset_distribution_params, + contact_offset_distribution_params, + distribution, + ) + + class randomize_physics_scene_gravity(ManagerTermBase): """Randomize gravity by adding, scaling, or setting random values. diff --git a/source/isaaclab_newton/docs/CHANGELOG.rst b/source/isaaclab_newton/docs/CHANGELOG.rst index 6b5861137b01..371ce6f91e2a 100644 --- a/source/isaaclab_newton/docs/CHANGELOG.rst +++ b/source/isaaclab_newton/docs/CHANGELOG.rst @@ -22,6 +22,7 @@ Added registration, wildcard body matching, and zero-copy transform views. + 0.5.10 (2026-04-05) ~~~~~~~~~~~~~~~~~~~ diff --git a/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation.py b/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation.py index b94394ecd8c6..9d62dc0bbed1 100644 --- a/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation.py +++ b/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation.py @@ -169,11 +169,6 @@ def num_shapes_per_body(self) -> list[int]: self._num_shapes_per_body.append(len(shapes)) return self._num_shapes_per_body - @property - def num_shapes(self) -> int: - """Total number of collision shapes in the articulation.""" - return self.data._num_shapes - @property def joint_names(self) -> list[str]: """Ordered names of joints in articulation.""" @@ -2273,168 +2268,6 @@ def set_inertias_mask( # tell the physics engine that some of the body properties have been updated SimulationManager.add_model_change(SolverNotifyFlags.BODY_INERTIAL_PROPERTIES) - def set_friction_index( - self, - *, - friction: torch.Tensor | wp.array, - shape_ids: Sequence[int] | torch.Tensor | wp.array | None = None, - env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, - ) -> None: - """Set friction coefficient (mu) for collision shapes using indices. - - .. note:: - This method expects partial data. - - .. tip:: - Both the index and mask methods have dedicated optimized implementations. Performance is similar for both. - However, to allow graphed pipelines, the mask method must be used. - - Args: - friction: Friction coefficient for shapes. Shape is (len(env_ids), len(shape_ids)). - shape_ids: Shape indices. If None, all shapes are updated. - env_ids: Environment indices. If None, all environments are updated. - """ - # resolve all indices - env_ids = self._resolve_env_ids(env_ids) - shape_ids = self._resolve_shape_ids(shape_ids) - self.assert_shape_and_dtype(friction, (env_ids.shape[0], shape_ids.shape[0]), wp.float32, "friction") - # Warp kernels can ingest torch tensors directly, so we don't need to convert to warp arrays here. - wp.launch( - shared_kernels.write_2d_data_to_buffer_with_indices, - dim=(env_ids.shape[0], shape_ids.shape[0]), - inputs=[ - friction, - env_ids, - shape_ids, - ], - outputs=[ - self.data._sim_bind_shape_material_mu, - ], - device=self.device, - ) - # tell the physics engine that some of the shape properties have been updated - SimulationManager.add_model_change(SolverNotifyFlags.SHAPE_PROPERTIES) - - def set_friction_mask( - self, - *, - friction: torch.Tensor | wp.array, - env_mask: wp.array | None = None, - ) -> None: - """Set friction coefficient (mu) for collision shapes using masks. - - .. note:: - This method expects full data. - - .. tip:: - Both the index and mask methods have dedicated optimized implementations. Performance is similar for both. - However, to allow graphed pipelines, the mask method must be used. - - Args: - friction: Friction coefficient for all shapes. Shape is (num_instances, num_shapes). - env_mask: Environment mask. If None, then all the instances are updated. Shape is (num_instances,). - """ - # resolve masks - env_mask = self._resolve_mask(env_mask, self._ALL_ENV_MASK) - shape_mask = self._ALL_SHAPE_MASK - self.assert_shape_and_dtype_mask(friction, (env_mask, shape_mask), wp.float32, "friction") - wp.launch( - shared_kernels.write_2d_data_to_buffer_with_mask, - dim=(env_mask.shape[0], shape_mask.shape[0]), - inputs=[ - friction, - env_mask, - shape_mask, - ], - outputs=[ - self.data._sim_bind_shape_material_mu, - ], - device=self.device, - ) - # tell the physics engine that some of the shape properties have been updated - SimulationManager.add_model_change(SolverNotifyFlags.SHAPE_PROPERTIES) - - def set_restitution_index( - self, - *, - restitution: torch.Tensor | wp.array, - shape_ids: Sequence[int] | torch.Tensor | wp.array | None = None, - env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, - ) -> None: - """Set restitution coefficient for collision shapes using indices. - - .. note:: - This method expects partial data. - - .. tip:: - Both the index and mask methods have dedicated optimized implementations. Performance is similar for both. - However, to allow graphed pipelines, the mask method must be used. - - Args: - restitution: Restitution coefficient for shapes. Shape is (len(env_ids), len(shape_ids)). - shape_ids: Shape indices. If None, all shapes are updated. - env_ids: Environment indices. If None, all environments are updated. - """ - # resolve all indices - env_ids = self._resolve_env_ids(env_ids) - shape_ids = self._resolve_shape_ids(shape_ids) - self.assert_shape_and_dtype(restitution, (env_ids.shape[0], shape_ids.shape[0]), wp.float32, "restitution") - # Warp kernels can ingest torch tensors directly, so we don't need to convert to warp arrays here. - wp.launch( - shared_kernels.write_2d_data_to_buffer_with_indices, - dim=(env_ids.shape[0], shape_ids.shape[0]), - inputs=[ - restitution, - env_ids, - shape_ids, - ], - outputs=[ - self.data._sim_bind_shape_material_restitution, - ], - device=self.device, - ) - # tell the physics engine that some of the shape properties have been updated - SimulationManager.add_model_change(SolverNotifyFlags.SHAPE_PROPERTIES) - - def set_restitution_mask( - self, - *, - restitution: torch.Tensor | wp.array, - env_mask: wp.array | None = None, - ) -> None: - """Set restitution coefficient for collision shapes using masks. - - .. note:: - This method expects full data. - - .. tip:: - Both the index and mask methods have dedicated optimized implementations. Performance is similar for both. - However, to allow graphed pipelines, the mask method must be used. - - Args: - restitution: Restitution coefficient for all shapes. Shape is (num_instances, num_shapes). - env_mask: Environment mask. If None, then all the instances are updated. Shape is (num_instances,). - """ - # resolve masks - env_mask = self._resolve_mask(env_mask, self._ALL_ENV_MASK) - shape_mask = self._ALL_SHAPE_MASK - self.assert_shape_and_dtype_mask(restitution, (env_mask, shape_mask), wp.float32, "restitution") - wp.launch( - shared_kernels.write_2d_data_to_buffer_with_mask, - dim=(env_mask.shape[0], shape_mask.shape[0]), - inputs=[ - restitution, - env_mask, - shape_mask, - ], - outputs=[ - self.data._sim_bind_shape_material_restitution, - ], - device=self.device, - ) - # tell the physics engine that some of the shape properties have been updated - SimulationManager.add_model_change(SolverNotifyFlags.SHAPE_PROPERTIES) - def set_joint_position_target_index( self, *, @@ -3401,8 +3234,6 @@ def _create_buffers(self): self._ALL_JOINT_MASK = wp.ones((self.num_joints,), dtype=wp.bool, device=self.device) self._ALL_BODY_INDICES = wp.array(np.arange(self.num_bodies, dtype=np.int32), device=self.device) self._ALL_BODY_MASK = wp.ones((self.num_bodies,), dtype=wp.bool, device=self.device) - self._ALL_SHAPE_INDICES = wp.array(np.arange(self.num_shapes, dtype=np.int32), device=self.device) - self._ALL_SHAPE_MASK = wp.ones((self.num_shapes,), dtype=wp.bool, device=self.device) self._ALL_FIXED_TENDON_INDICES = wp.array(np.arange(self.num_fixed_tendons, dtype=np.int32), device=self.device) self._ALL_FIXED_TENDON_MASK = wp.ones((self.num_fixed_tendons,), dtype=wp.bool, device=self.device) self._ALL_SPATIAL_TENDON_INDICES = wp.array( @@ -3900,21 +3731,6 @@ def _resolve_body_ids(self, body_ids: Sequence[int] | torch.Tensor | wp.array | return self._ALL_BODY_INDICES return body_ids - def _resolve_shape_ids(self, shape_ids: Sequence[int] | torch.Tensor | wp.array | None) -> wp.array | torch.Tensor: - """Resolve shape indices to a warp array or tensor. - - Args: - shape_ids: Shape indices. If None, then all indices are used. - - Returns: - A warp array of shape indices or a tensor of shape indices. - """ - if (shape_ids is None) or (shape_ids == slice(None)): - return self._ALL_SHAPE_INDICES - elif isinstance(shape_ids, list): - return wp.array(shape_ids, dtype=wp.int32, device=self.device) - return shape_ids - def _resolve_fixed_tendon_ids( self, tendon_ids: Sequence[int] | torch.Tensor | wp.array | None ) -> wp.array | torch.Tensor: 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 f0129e77b454..4dc9507dbe59 100644 --- a/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation_data.py +++ b/source/isaaclab_newton/isaaclab_newton/assets/articulation/articulation_data.py @@ -1323,16 +1323,6 @@ def _create_simulation_bindings(self) -> None: self._sim_bind_joint_velocity_target = wp.zeros( (self._num_instances, 0), dtype=wp.float32, device=self.device ) - # -- shape material properties (for collision shapes) - self._sim_bind_shape_material_mu = self._root_view.get_attribute( - "shape_material_mu", SimulationManager.get_model() - )[:, 0] - self._sim_bind_shape_material_restitution = self._root_view.get_attribute( - "shape_material_restitution", SimulationManager.get_model() - )[:, 0] - self._num_shapes = ( - self._sim_bind_shape_material_mu.shape[1] if len(self._sim_bind_shape_material_mu.shape) > 1 else 1 - ) def _create_buffers(self) -> None: """Create buffers for the root data.""" 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 3e01289557b3..5e02e3622985 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 @@ -80,11 +80,6 @@ def num_bodies(self) -> int: """ return 1 - @property - def num_shapes(self) -> int: - """Total number of collision shapes in the rigid object.""" - return self.data._num_shapes - @property def body_names(self) -> list[str]: """Ordered names of bodies in the rigid object.""" @@ -989,170 +984,6 @@ def set_inertias_mask( # tell the physics engine that some of the body properties have been updated SimulationManager.add_model_change(SolverNotifyFlags.BODY_INERTIAL_PROPERTIES) - def set_friction_index( - self, - *, - friction: torch.Tensor | wp.array, - shape_ids: Sequence[int] | torch.Tensor | wp.array | None = None, - env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, - ) -> None: - """Set friction coefficient (mu) for collision shapes using indices. - - .. note:: - This method expects partial data. - - .. tip:: - Both the index and mask methods have dedicated optimized implementations. Performance is similar for both. - However, to allow graphed pipelines, the mask method must be used. - - Args: - friction: Friction coefficient for shapes. Shape is (len(env_ids), len(shape_ids)). - shape_ids: Shape indices. If None, all shapes are updated. - env_ids: Environment indices. If None, all environments are updated. - """ - # resolve all indices - env_ids = self._resolve_env_ids(env_ids) - shape_ids = self._resolve_shape_ids(shape_ids) - self.assert_shape_and_dtype(friction, (env_ids.shape[0], shape_ids.shape[0]), wp.float32, "friction") - # Warp kernels can ingest torch tensors directly, so we don't need to convert to warp arrays here. - wp.launch( - shared_kernels.write_2d_data_to_buffer_with_indices, - dim=(env_ids.shape[0], shape_ids.shape[0]), - inputs=[ - friction, - env_ids, - shape_ids, - ], - outputs=[ - self.data._sim_bind_shape_material_mu, - ], - device=self.device, - ) - # tell the physics engine that some of the shape properties have been updated - SimulationManager.add_model_change(SolverNotifyFlags.SHAPE_PROPERTIES) - - def set_friction_mask( - self, - *, - friction: torch.Tensor | wp.array, - env_mask: wp.array | None = None, - ) -> None: - """Set friction coefficient (mu) for collision shapes using masks. - - .. note:: - This method expects full data. - - .. tip:: - Both the index and mask methods have dedicated optimized implementations. Performance is similar for both. - However, to allow graphed pipelines, the mask method must be used. - - Args: - friction: Friction coefficient for all shapes. Shape is (num_instances, num_shapes). - env_mask: Environment mask. If None, then all the instances are updated. Shape is (num_instances,). - """ - # resolve masks - if env_mask is None: - env_mask = self._ALL_ENV_MASK - shape_mask = self._ALL_SHAPE_MASK - self.assert_shape_and_dtype_mask(friction, (env_mask, shape_mask), wp.float32, "friction") - wp.launch( - shared_kernels.write_2d_data_to_buffer_with_mask, - dim=(env_mask.shape[0], shape_mask.shape[0]), - inputs=[ - friction, - env_mask, - shape_mask, - ], - outputs=[ - self.data._sim_bind_shape_material_mu, - ], - device=self.device, - ) - # tell the physics engine that some of the shape properties have been updated - SimulationManager.add_model_change(SolverNotifyFlags.SHAPE_PROPERTIES) - - def set_restitution_index( - self, - *, - restitution: torch.Tensor | wp.array, - shape_ids: Sequence[int] | torch.Tensor | wp.array | None = None, - env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, - ) -> None: - """Set restitution coefficient for collision shapes using indices. - - .. note:: - This method expects partial data. - - .. tip:: - Both the index and mask methods have dedicated optimized implementations. Performance is similar for both. - However, to allow graphed pipelines, the mask method must be used. - - Args: - restitution: Restitution coefficient for shapes. Shape is (len(env_ids), len(shape_ids)). - shape_ids: Shape indices. If None, all shapes are updated. - env_ids: Environment indices. If None, all environments are updated. - """ - # resolve all indices - env_ids = self._resolve_env_ids(env_ids) - shape_ids = self._resolve_shape_ids(shape_ids) - self.assert_shape_and_dtype(restitution, (env_ids.shape[0], shape_ids.shape[0]), wp.float32, "restitution") - # Warp kernels can ingest torch tensors directly, so we don't need to convert to warp arrays here. - wp.launch( - shared_kernels.write_2d_data_to_buffer_with_indices, - dim=(env_ids.shape[0], shape_ids.shape[0]), - inputs=[ - restitution, - env_ids, - shape_ids, - ], - outputs=[ - self.data._sim_bind_shape_material_restitution, - ], - device=self.device, - ) - # tell the physics engine that some of the shape properties have been updated - SimulationManager.add_model_change(SolverNotifyFlags.SHAPE_PROPERTIES) - - def set_restitution_mask( - self, - *, - restitution: torch.Tensor | wp.array, - env_mask: wp.array | None = None, - ) -> None: - """Set restitution coefficient for collision shapes using masks. - - .. note:: - This method expects full data. - - .. tip:: - Both the index and mask methods have dedicated optimized implementations. Performance is similar for both. - However, to allow graphed pipelines, the mask method must be used. - - Args: - restitution: Restitution coefficient for all shapes. Shape is (num_instances, num_shapes). - env_mask: Environment mask. If None, then all the instances are updated. Shape is (num_instances,). - """ - # resolve masks - if env_mask is None: - env_mask = self._ALL_ENV_MASK - shape_mask = self._ALL_SHAPE_MASK - self.assert_shape_and_dtype_mask(restitution, (env_mask, shape_mask), wp.float32, "restitution") - wp.launch( - shared_kernels.write_2d_data_to_buffer_with_mask, - dim=(env_mask.shape[0], shape_mask.shape[0]), - inputs=[ - restitution, - env_mask, - shape_mask, - ], - outputs=[ - self.data._sim_bind_shape_material_restitution, - ], - device=self.device, - ) - # tell the physics engine that some of the shape properties have been updated - SimulationManager.add_model_change(SolverNotifyFlags.SHAPE_PROPERTIES) - """ Internal helper. """ @@ -1239,8 +1070,6 @@ def _create_buffers(self): self._ALL_ENV_MASK = wp.ones((self.num_instances,), dtype=wp.bool, device=self.device) self._ALL_BODY_INDICES = wp.array(np.arange(self.num_bodies, dtype=np.int32), device=self.device) self._ALL_BODY_MASK = wp.ones((self.num_bodies,), dtype=wp.bool, device=self.device) - self._ALL_SHAPE_INDICES = wp.array(np.arange(self.num_shapes, dtype=np.int32), device=self.device) - self._ALL_SHAPE_MASK = wp.ones((self.num_shapes,), dtype=wp.bool, device=self.device) # external wrench composer self._instantaneous_wrench_composer = WrenchComposer(self) @@ -1299,24 +1128,6 @@ def _resolve_body_ids(self, body_ids: Sequence[int] | torch.Tensor | wp.array | return wp.array(body_ids, dtype=wp.int32, device=self.device) return body_ids - def _resolve_shape_ids(self, shape_ids: Sequence[int] | torch.Tensor | wp.array | None) -> wp.array | torch.Tensor: - """Resolve shape indices to a warp array or tensor. - - .. note:: - We do not need to convert torch tensors to warp arrays since they never get passed to the TensorAPI views. - - Args: - shape_ids: Shape indices. If None, then all indices are used. - - Returns: - A warp array of shape indices or a tensor of shape indices. - """ - if (shape_ids is None) or (shape_ids == slice(None)): - return self._ALL_SHAPE_INDICES - elif isinstance(shape_ids, list): - return wp.array(shape_ids, dtype=wp.int32, device=self.device) - return shape_ids - """ Internal simulation callbacks. """ 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 3ca059d5633c..2af92df4592c 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 @@ -735,16 +735,6 @@ def _create_simulation_bindings(self) -> None: self._sim_bind_body_external_wrench = self._root_view.get_attribute("body_f", SimulationManager.get_state_0())[ :, 0 ] - # -- shape material properties (for collision shapes) - self._sim_bind_shape_material_mu = self._root_view.get_attribute( - "shape_material_mu", SimulationManager.get_model() - )[:, 0] - self._sim_bind_shape_material_restitution = self._root_view.get_attribute( - "shape_material_restitution", SimulationManager.get_model() - )[:, 0] - self._num_shapes = ( - self._sim_bind_shape_material_mu.shape[1] if len(self._sim_bind_shape_material_mu.shape) > 1 else 1 - ) def _create_buffers(self) -> None: """Create buffers for the root data.""" 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 5f368d8957cd..82bdf7a03003 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 @@ -111,11 +111,6 @@ def num_bodies(self) -> int: """Number of bodies in the rigid object collection.""" return len(self.body_names) - @property - def num_shapes(self) -> int: - """Total number of collision shapes in the rigid object collection.""" - return self.data._num_shapes - @property def body_names(self) -> list[str]: """Ordered names of bodies in the rigid object collection.""" @@ -1055,170 +1050,6 @@ def set_inertias_mask( # Tell the physics engine that some of the body properties have been updated SimulationManager.add_model_change(SolverNotifyFlags.BODY_INERTIAL_PROPERTIES) - def set_friction_index( - self, - *, - friction: torch.Tensor | wp.array, - shape_ids: Sequence[int] | torch.Tensor | wp.array | None = None, - env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, - ) -> None: - """Set friction coefficient (mu) for collision shapes using indices. - - .. note:: - This method expects partial data. - - .. tip:: - Both the index and mask methods have dedicated optimized implementations. Performance is similar for both. - However, to allow graphed pipelines, the mask method must be used. - - Args: - friction: Friction coefficient for shapes. Shape is (len(env_ids), len(shape_ids)). - shape_ids: Shape indices. If None, all shapes are updated. - env_ids: Environment indices. If None, all environments are updated. - """ - # resolve all indices - env_ids = self._resolve_env_ids(env_ids) - shape_ids = self._resolve_shape_ids(shape_ids) - self.assert_shape_and_dtype(friction, (env_ids.shape[0], shape_ids.shape[0]), wp.float32, "friction") - # Write to consolidated buffer - wp.launch( - shared_kernels.write_2d_data_to_buffer_with_indices, - dim=(env_ids.shape[0], shape_ids.shape[0]), - inputs=[ - friction, - env_ids, - shape_ids, - ], - outputs=[ - self.data._sim_bind_shape_material_mu, - ], - device=self.device, - ) - # Tell the physics engine that some of the shape properties have been updated - SimulationManager.add_model_change(SolverNotifyFlags.SHAPE_PROPERTIES) - - def set_friction_mask( - self, - *, - friction: torch.Tensor | wp.array, - env_mask: wp.array | None = None, - ) -> None: - """Set friction coefficient (mu) for collision shapes using masks. - - .. note:: - This method expects full data. - - .. tip:: - Both the index and mask methods have dedicated optimized implementations. Performance is similar for both. - However, to allow graphed pipelines, the mask method must be used. - - Args: - friction: Friction coefficient for all shapes. Shape is (num_instances, num_shapes). - env_mask: Environment mask. If None, then all the instances are updated. Shape is (num_instances,). - """ - # resolve masks - if env_mask is None: - env_mask = self._ALL_ENV_MASK - shape_mask = self._ALL_SHAPE_MASK - self.assert_shape_and_dtype_mask(friction, (env_mask, shape_mask), wp.float32, "friction") - wp.launch( - shared_kernels.write_2d_data_to_buffer_with_mask, - dim=(env_mask.shape[0], shape_mask.shape[0]), - inputs=[ - friction, - env_mask, - shape_mask, - ], - outputs=[ - self.data._sim_bind_shape_material_mu, - ], - device=self.device, - ) - # Tell the physics engine that some of the shape properties have been updated - SimulationManager.add_model_change(SolverNotifyFlags.SHAPE_PROPERTIES) - - def set_restitution_index( - self, - *, - restitution: torch.Tensor | wp.array, - shape_ids: Sequence[int] | torch.Tensor | wp.array | None = None, - env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, - ) -> None: - """Set restitution coefficient for collision shapes using indices. - - .. note:: - This method expects partial data. - - .. tip:: - Both the index and mask methods have dedicated optimized implementations. Performance is similar for both. - However, to allow graphed pipelines, the mask method must be used. - - Args: - restitution: Restitution coefficient for shapes. Shape is (len(env_ids), len(shape_ids)). - shape_ids: Shape indices. If None, all shapes are updated. - env_ids: Environment indices. If None, all environments are updated. - """ - # resolve all indices - env_ids = self._resolve_env_ids(env_ids) - shape_ids = self._resolve_shape_ids(shape_ids) - self.assert_shape_and_dtype(restitution, (env_ids.shape[0], shape_ids.shape[0]), wp.float32, "restitution") - # Write to consolidated buffer - wp.launch( - shared_kernels.write_2d_data_to_buffer_with_indices, - dim=(env_ids.shape[0], shape_ids.shape[0]), - inputs=[ - restitution, - env_ids, - shape_ids, - ], - outputs=[ - self.data._sim_bind_shape_material_restitution, - ], - device=self.device, - ) - # Tell the physics engine that some of the shape properties have been updated - SimulationManager.add_model_change(SolverNotifyFlags.SHAPE_PROPERTIES) - - def set_restitution_mask( - self, - *, - restitution: torch.Tensor | wp.array, - env_mask: wp.array | None = None, - ) -> None: - """Set restitution coefficient for collision shapes using masks. - - .. note:: - This method expects full data. - - .. tip:: - Both the index and mask methods have dedicated optimized implementations. Performance is similar for both. - However, to allow graphed pipelines, the mask method must be used. - - Args: - restitution: Restitution coefficient for all shapes. Shape is (num_instances, num_shapes). - env_mask: Environment mask. If None, then all the instances are updated. Shape is (num_instances,). - """ - # resolve masks - if env_mask is None: - env_mask = self._ALL_ENV_MASK - shape_mask = self._ALL_SHAPE_MASK - self.assert_shape_and_dtype_mask(restitution, (env_mask, shape_mask), wp.float32, "restitution") - wp.launch( - shared_kernels.write_2d_data_to_buffer_with_mask, - dim=(env_mask.shape[0], shape_mask.shape[0]), - inputs=[ - restitution, - env_mask, - shape_mask, - ], - outputs=[ - self.data._sim_bind_shape_material_restitution, - ], - device=self.device, - ) - # Tell the physics engine that some of the shape properties have been updated - SimulationManager.add_model_change(SolverNotifyFlags.SHAPE_PROPERTIES) - """ Internal helper. """ @@ -1306,12 +1137,8 @@ def _create_buffers(self): self._ALL_BODY_INDICES = wp.array( np.arange(self.num_bodies, dtype=np.int32), device=self.device, dtype=wp.int32 ) - self._ALL_SHAPE_INDICES = wp.array( - np.arange(self.num_shapes, dtype=np.int32), device=self.device, dtype=wp.int32 - ) self._ALL_ENV_MASK = wp.ones((self.num_instances,), dtype=wp.bool, device=self.device) self._ALL_BODY_MASK = wp.ones((self.num_bodies,), dtype=wp.bool, device=self.device) - self._ALL_SHAPE_MASK = wp.ones((self.num_shapes,), dtype=wp.bool, device=self.device) # external wrench composer self._instantaneous_wrench_composer = WrenchComposer(self) @@ -1368,21 +1195,6 @@ def _resolve_body_ids(self, body_ids) -> wp.array: return wp.from_torch(body_ids.to(torch.int32), dtype=wp.int32) return body_ids - def _resolve_shape_ids(self, shape_ids: Sequence[int] | torch.Tensor | wp.array | None) -> wp.array | torch.Tensor: - """Resolve shape indices to a warp array or tensor. - - Args: - shape_ids: Shape indices. If None, then all indices are used. - - Returns: - A warp array of shape indices or a tensor of shape indices. - """ - if (shape_ids is None) or (shape_ids == slice(None)): - return self._ALL_SHAPE_INDICES - elif isinstance(shape_ids, list): - return wp.array(shape_ids, dtype=wp.int32, device=self.device) - return shape_ids - def _resolve_env_mask(self, env_mask: wp.array | None) -> wp.array | torch.Tensor: """Resolve environment mask to indices via torch.nonzero.""" if env_mask is not None: diff --git a/source/isaaclab_newton/isaaclab_newton/assets/rigid_object_collection/rigid_object_collection_data.py b/source/isaaclab_newton/isaaclab_newton/assets/rigid_object_collection/rigid_object_collection_data.py index 6b3c53580f50..3fbc1801322f 100644 --- a/source/isaaclab_newton/isaaclab_newton/assets/rigid_object_collection/rigid_object_collection_data.py +++ b/source/isaaclab_newton/isaaclab_newton/assets/rigid_object_collection/rigid_object_collection_data.py @@ -608,14 +608,6 @@ def _create_simulation_bindings(self) -> None: device=_body_inertia_raw.device, copy=False, ) - # -- shape material properties (for collision shapes) - self._sim_bind_shape_material_mu = self._root_view.get_attribute("shape_material_mu", model)[:, :, 0] - self._sim_bind_shape_material_restitution = self._root_view.get_attribute("shape_material_restitution", model)[ - :, :, 0 - ] - self._num_shapes = ( - self._sim_bind_shape_material_mu.shape[1] if len(self._sim_bind_shape_material_mu.shape) > 1 else 1 - ) def _create_buffers(self) -> None: """Create buffers for computing and caching derived quantities.""" diff --git a/source/isaaclab_newton/test/assets/test_articulation.py b/source/isaaclab_newton/test/assets/test_articulation.py index 50d726402ab3..50474990838b 100644 --- a/source/isaaclab_newton/test/assets/test_articulation.py +++ b/source/isaaclab_newton/test/assets/test_articulation.py @@ -2493,7 +2493,7 @@ def _patched_simulate(cls): @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.""" + """Test getting and setting material properties (friction/restitution) via view-level APIs.""" articulation_cfg = generate_articulation_cfg(articulation_type=articulation_type) articulation, _ = generate_articulation( articulation_cfg=articulation_cfg, num_articulations=num_articulations, device=device @@ -2502,47 +2502,47 @@ def test_set_material_properties(sim, num_articulations, device, add_ground_plan # Play the simulator sim.reset() - # Get actual number of shapes from the articulation - num_shapes = articulation.num_shapes + # Get friction/restitution bindings via view-level API + model = SimulationManager.get_model() + friction_binding = articulation._root_view.get_attribute("shape_material_mu", model)[:, 0] + restitution_binding = articulation._root_view.get_attribute("shape_material_restitution", model)[:, 0] + num_shapes = friction_binding.shape[1] - # Test 1: Set all shapes using index method (passing shape_ids explicitly) - shape_ids = torch.arange(num_shapes, dtype=torch.int32, device=device) - env_ids = torch.arange(num_articulations, dtype=torch.int32, device=device) + # Test 1: Set all shapes via in-place writes to the warp binding friction = torch.empty(num_articulations, num_shapes, device=device).uniform_(0.4, 0.8) restitution = torch.empty(num_articulations, num_shapes, device=device).uniform_(0.0, 0.2) - articulation.set_friction_index(friction=friction, shape_ids=shape_ids, env_ids=env_ids) - articulation.set_restitution_index(restitution=restitution, shape_ids=shape_ids, env_ids=env_ids) + wp.to_torch(friction_binding)[:] = friction + wp.to_torch(restitution_binding)[:] = restitution + SimulationManager.add_model_change(SolverNotifyFlags.SHAPE_PROPERTIES) # Simulate physics sim.step() articulation.update(sim.cfg.dt) - # Get material properties via internal bindings (for test verification only) - mu = wp.to_torch(articulation.data._sim_bind_shape_material_mu) - restitution_check = wp.to_torch(articulation.data._sim_bind_shape_material_restitution) - - # Check if material properties are set correctly + # Verify by reading back from the binding + mu = wp.to_torch(friction_binding) + restitution_check = wp.to_torch(restitution_binding) torch.testing.assert_close(mu, friction) torch.testing.assert_close(restitution_check, restitution) - # Test 2: Set subset of shapes using index method + # Test 2: Set subset of shapes (only shape 0) if num_shapes > 1: - subset_shape_ids = torch.tensor([0], dtype=torch.int32, device=device) - subset_friction = torch.empty(num_articulations, 1, device=device).uniform_(0.1, 0.2) - subset_restitution = torch.empty(num_articulations, 1, device=device).uniform_(0.5, 0.6) + subset_friction = torch.empty(num_articulations, device=device).uniform_(0.1, 0.2) + subset_restitution = torch.empty(num_articulations, device=device).uniform_(0.5, 0.6) - articulation.set_friction_index(friction=subset_friction, shape_ids=subset_shape_ids, env_ids=env_ids) - articulation.set_restitution_index(restitution=subset_restitution, shape_ids=subset_shape_ids, env_ids=env_ids) + wp.to_torch(friction_binding)[:, 0] = subset_friction + wp.to_torch(restitution_binding)[:, 0] = subset_restitution + SimulationManager.add_model_change(SolverNotifyFlags.SHAPE_PROPERTIES) sim.step() articulation.update(sim.cfg.dt) # Check only the subset was updated - mu_updated = wp.to_torch(articulation.data._sim_bind_shape_material_mu) - restitution_updated = wp.to_torch(articulation.data._sim_bind_shape_material_restitution) - torch.testing.assert_close(mu_updated[:, 0:1], subset_friction) - torch.testing.assert_close(restitution_updated[:, 0:1], subset_restitution) + mu_updated = wp.to_torch(friction_binding) + restitution_updated = wp.to_torch(restitution_binding) + torch.testing.assert_close(mu_updated[:, 0], subset_friction) + torch.testing.assert_close(restitution_updated[:, 0], subset_restitution) @pytest.mark.parametrize("num_articulations", [2]) diff --git a/source/isaaclab_newton/test/assets/test_rigid_object.py b/source/isaaclab_newton/test/assets/test_rigid_object.py index 4df40aaeed00..c1d01f4164fb 100644 --- a/source/isaaclab_newton/test/assets/test_rigid_object.py +++ b/source/isaaclab_newton/test/assets/test_rigid_object.py @@ -593,7 +593,7 @@ def test_reset_rigid_object(num_cubes, device): @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.isaacsim_ci def test_rigid_body_set_material_properties(num_cubes, device): - """Test getting and setting material properties of rigid object.""" + """Test getting and setting material properties of rigid object via view-level APIs.""" with _newton_sim_context(device, gravity_enabled=True, add_ground_plane=True, auto_add_lighting=True) as sim: sim._app_control_on_stop_handle = None # Generate cubes scene @@ -602,33 +602,48 @@ def test_rigid_body_set_material_properties(num_cubes, device): # Play sim sim.reset() - # Set material properties using Newton's native API - # Get actual number of shapes from the object - num_shapes = cube_object.num_shapes - shape_ids = torch.arange(num_shapes, dtype=torch.int32, device=device) - env_ids = torch.arange(num_cubes, dtype=torch.int32, device=device) + # Get friction/restitution bindings via view-level API + model = SimulationManager.get_model() + friction_binding = cube_object._root_view.get_attribute("shape_material_mu", model)[:, 0] + restitution_binding = cube_object._root_view.get_attribute("shape_material_restitution", model)[:, 0] + num_shapes = friction_binding.shape[1] + + # Set material properties via in-place writes to the warp binding friction = torch.empty(num_cubes, num_shapes, device=device).uniform_(0.4, 0.8) restitution = torch.empty(num_cubes, num_shapes, device=device).uniform_(0.0, 0.2) - # Use Newton's native friction/restitution API with shape_ids - cube_object.set_friction_index(friction=friction, shape_ids=shape_ids, env_ids=env_ids) - cube_object.set_restitution_index(restitution=restitution, shape_ids=shape_ids, env_ids=env_ids) + wp.to_torch(friction_binding)[:] = friction + wp.to_torch(restitution_binding)[:] = restitution + SimulationManager.add_model_change(SolverNotifyFlags.SHAPE_PROPERTIES) # Simulate physics sim.step() - # update object cube_object.update(sim.cfg.dt) - # Get material properties via internal bindings (for test verification only) - mu = wp.to_torch(cube_object.data._sim_bind_shape_material_mu) - restitution_check = wp.to_torch(cube_object.data._sim_bind_shape_material_restitution) - - # Check if material properties are set correctly + # Verify by reading back from the binding + mu = wp.to_torch(friction_binding) + restitution_check = wp.to_torch(restitution_binding) torch.testing.assert_close(mu, friction) torch.testing.assert_close(restitution_check, restitution) -@pytest.mark.skip(reason="Newton friction/restitution/material not yet supported") +def _set_newton_material_properties(cube_object, friction_val, restitution_val, device): + """Helper to set material properties via Newton view-level APIs.""" + model = SimulationManager.get_model() + friction_binding = cube_object._root_view.get_attribute("shape_material_mu", model)[:, 0] + restitution_binding = cube_object._root_view.get_attribute("shape_material_restitution", model)[:, 0] + num_envs = friction_binding.shape[0] + num_shapes = friction_binding.shape[1] + + friction_tensor = torch.full((num_envs, num_shapes), friction_val, device=device) + restitution_tensor = torch.full((num_envs, num_shapes), restitution_val, device=device) + + wp.to_torch(friction_binding)[:] = friction_tensor + wp.to_torch(restitution_binding)[:] = restitution_tensor + SimulationManager.add_model_change(SolverNotifyFlags.SHAPE_PROPERTIES) + + +@pytest.mark.skip(reason="MuJoCo contact at height=0 does not settle the same as PhysX — cube falls on z-axis") @pytest.mark.parametrize("num_cubes", [1, 2]) @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.isaacsim_ci @@ -639,11 +654,12 @@ def test_rigid_body_no_friction(num_cubes, device): # Generate cubes scene cube_object, _ = generate_cubes_scene(num_cubes=num_cubes, height=0.0, device=device) - # Create ground plane with no friction + # Create ground plane with near-zero friction + # Note: MuJoCo requires friction >= MJ_MINMU (1e-5), so we use 1e-4 cfg = sim_utils.GroundPlaneCfg( physics_material=materials.RigidBodyMaterialCfg( - static_friction=0.0, - dynamic_friction=0.0, + static_friction=1e-4, + dynamic_friction=1e-4, restitution=0.0, ) ) @@ -652,17 +668,8 @@ def test_rigid_body_no_friction(num_cubes, device): # Play sim sim.reset() - # Set material friction properties to be all zero - static_friction = torch.zeros(num_cubes, 1) - dynamic_friction = torch.zeros(num_cubes, 1) - restitution = torch.FloatTensor(num_cubes, 1).uniform_(0.0, 0.2) - - cube_object_materials = torch.cat([static_friction, dynamic_friction, restitution], dim=-1) - indices = torch.tensor(range(num_cubes), dtype=torch.int32) - - cube_object.root_view.set_material_properties( - wp.from_torch(cube_object_materials, dtype=wp.float32), wp.from_torch(indices, dtype=wp.int32) - ) + # Set material friction to near-zero via view-level API + _set_newton_material_properties(cube_object, friction_val=1e-4, restitution_val=0.0, device=device) # Set initial velocity # Initial velocity in X to get the block moving @@ -689,7 +696,7 @@ def test_rigid_body_no_friction(num_cubes, device): ) -@pytest.mark.skip(reason="Newton friction/restitution/material not yet supported") +@pytest.mark.skip(reason="MuJoCo uses Coulomb friction (single mu), no static/dynamic distinction") @pytest.mark.parametrize("num_cubes", [1, 2]) @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.isaacsim_ci @@ -710,7 +717,7 @@ def test_rigid_body_with_static_friction(num_cubes, device): cfg = sim_utils.GroundPlaneCfg( physics_material=materials.RigidBodyMaterialCfg( static_friction=static_friction_coefficient, - dynamic_friction=static_friction_coefficient, # This shouldn't be required but is due to a bug in PhysX + dynamic_friction=static_friction_coefficient, ) ) cfg.func("/World/GroundPlane", cfg) @@ -718,19 +725,12 @@ def test_rigid_body_with_static_friction(num_cubes, device): # Play sim sim.reset() - # Set static friction to be non-zero - # Dynamic friction also needs to be zero due to a bug in PhysX - static_friction = torch.Tensor([[static_friction_coefficient]] * num_cubes) - dynamic_friction = torch.Tensor([[static_friction_coefficient]] * num_cubes) - restitution = torch.zeros(num_cubes, 1) - - cube_object_materials = torch.cat([static_friction, dynamic_friction, restitution], dim=-1) - - indices = torch.tensor(range(num_cubes), dtype=torch.int32) - - # Add friction to cube - cube_object.root_view.set_material_properties( - wp.from_torch(cube_object_materials, dtype=wp.float32), wp.from_torch(indices, dtype=wp.int32) + # Set friction via view-level API + _set_newton_material_properties( + cube_object, + friction_val=static_friction_coefficient, + restitution_val=0.0, + device=device, ) # let everything settle @@ -776,7 +776,7 @@ def test_rigid_body_with_static_friction(num_cubes, device): assert (wp.to_torch(cube_object.data.root_pos_w)[..., 0] - initial_root_pos[..., 0] > 0.02).all() -@pytest.mark.skip(reason="Newton friction/restitution/material not yet supported") +@pytest.mark.skip(reason="MuJoCo restitution model differs from PhysX — inelastic collisions still bounce") @pytest.mark.parametrize("num_cubes", [1, 2]) @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.isaacsim_ci @@ -799,7 +799,7 @@ def test_rigid_body_with_restitution(num_cubes, device): elif expected_collision_type == "partially_elastic": restitution_coefficient = 0.5 - # Create ground plane such that has a restitution of 1.0 (perfectly elastic collision) + # Create ground plane cfg = sim_utils.GroundPlaneCfg( physics_material=materials.RigidBodyMaterialCfg( restitution=restitution_coefficient, @@ -807,8 +807,6 @@ def test_rigid_body_with_restitution(num_cubes, device): ) cfg.func("/World/GroundPlane", cfg) - indices = torch.tensor(range(num_cubes), dtype=torch.int32) - # Play sim sim.reset() @@ -823,15 +821,12 @@ def test_rigid_body_with_restitution(num_cubes, device): cube_object.write_root_pose_to_sim_index(root_pose=root_pose) cube_object.write_root_velocity_to_sim_index(root_velocity=root_vel) - static_friction = torch.zeros(num_cubes, 1) - dynamic_friction = torch.zeros(num_cubes, 1) - restitution = torch.Tensor([[restitution_coefficient]] * num_cubes) - - cube_object_materials = torch.cat([static_friction, dynamic_friction, restitution], dim=-1) - - # Add restitution to cube - cube_object.root_view.set_material_properties( - wp.from_torch(cube_object_materials, dtype=wp.float32), wp.from_torch(indices, dtype=wp.int32) + # Set restitution via view-level API + _set_newton_material_properties( + cube_object, + friction_val=0.0, + restitution_val=restitution_coefficient, + device=device, ) curr_z_velocity = wp.to_torch(cube_object.data.root_lin_vel_w)[:, 2].clone() 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 0d51f25675f1..7d4a0be7cb98 100644 --- a/source/isaaclab_newton/test/assets/test_rigid_object_collection.py +++ b/source/isaaclab_newton/test/assets/test_rigid_object_collection.py @@ -526,33 +526,37 @@ def test_reset_object_collection(num_envs, num_cubes, device): @pytest.mark.parametrize("num_cubes", [1, 2]) @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) def test_set_material_properties(num_envs, num_cubes, device): - """Test getting and setting material properties of rigid object collection.""" + """Test getting and setting material properties of rigid object collection via view-level APIs.""" with _newton_sim_context(device, auto_add_lighting=True) as sim: sim._app_control_on_stop_handle = None object_collection, _ = generate_cubes_scene(num_envs=num_envs, num_cubes=num_cubes, device=device) sim.reset() - # Set material properties using Newton's native API - # Get actual number of shapes from the collection - num_shapes = object_collection.num_shapes - shape_ids = torch.arange(num_shapes, dtype=torch.int32, device=device) - env_ids = torch.arange(num_envs, dtype=torch.int32, device=device) - friction = torch.empty(num_envs, num_shapes, device=device).uniform_(0.4, 0.8) - restitution = torch.empty(num_envs, num_shapes, device=device).uniform_(0.0, 0.2) + # Get friction/restitution bindings via view-level API + # The collection's _root_view stores data in flat view order: (num_envs * num_cubes, ...) + model = SimulationManager.get_model() + friction_raw = object_collection._root_view.get_attribute("shape_material_mu", model) + restitution_raw = object_collection._root_view.get_attribute("shape_material_restitution", model) - # Use Newton's native friction/restitution API with shape_ids - object_collection.set_friction_index(friction=friction, shape_ids=shape_ids, env_ids=env_ids) - object_collection.set_restitution_index(restitution=restitution, shape_ids=shape_ids, env_ids=env_ids) + # Shape is (num_envs * num_cubes, num_shapes_per_body, 1) — slice off trailing dim + friction_binding = friction_raw[:, :, 0] + restitution_binding = restitution_raw[:, :, 0] + + # Generate random values matching the flat view shape + friction = torch.empty_like(wp.to_torch(friction_binding)).uniform_(0.4, 0.8) + restitution = torch.empty_like(wp.to_torch(restitution_binding)).uniform_(0.0, 0.2) + + wp.to_torch(friction_binding)[:] = friction + wp.to_torch(restitution_binding)[:] = restitution + SimulationManager.add_model_change(SolverNotifyFlags.SHAPE_PROPERTIES) # Perform simulation sim.step() object_collection.update(sim.cfg.dt) - # Get material properties via internal bindings (for test verification only) - mu = wp.to_torch(object_collection.data._sim_bind_shape_material_mu) - restitution_check = wp.to_torch(object_collection.data._sim_bind_shape_material_restitution) - - # Check if material properties are set correctly + # Verify by reading back from the binding + mu = wp.to_torch(friction_binding) + restitution_check = wp.to_torch(restitution_binding) torch.testing.assert_close(mu, friction) torch.testing.assert_close(restitution_check, restitution) diff --git a/source/isaaclab_physx/docs/CHANGELOG.rst b/source/isaaclab_physx/docs/CHANGELOG.rst index 4d9715d22857..077963af1127 100644 --- a/source/isaaclab_physx/docs/CHANGELOG.rst +++ b/source/isaaclab_physx/docs/CHANGELOG.rst @@ -32,6 +32,7 @@ Fixed remain visually updated after resuming. + 0.5.13 (2026-03-25) ~~~~~~~~~~~~~~~~~~~ diff --git a/source/isaaclab_physx/isaaclab_physx/assets/articulation/articulation.py b/source/isaaclab_physx/isaaclab_physx/assets/articulation/articulation.py index 2610ff1e4f6f..cf7d1f95d5ca 100644 --- a/source/isaaclab_physx/isaaclab_physx/assets/articulation/articulation.py +++ b/source/isaaclab_physx/isaaclab_physx/assets/articulation/articulation.py @@ -2218,65 +2218,6 @@ def set_inertias_mask( # Set full data to True to ensure the right code path is taken inside the kernel. self.set_inertias_index(inertias=inertias, body_ids=body_ids, env_ids=env_ids, full_data=True) - def set_material_properties_index( - self, - *, - materials: torch.Tensor | wp.array, - env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, - ) -> None: - """Set material properties (friction and restitution) for collision shapes using indices. - - The material properties consist of static friction, dynamic friction, and restitution - coefficients for each collision shape. - - .. note:: - This method expects partial data. - - .. tip:: - Both the index and mask methods have dedicated optimized implementations. Performance is similar for both. - However, to allow graphed pipelines, the mask method must be used. - - Args: - materials: Material properties for all shapes. Shape is (len(env_ids), max_shapes, 3) - where the 3 values are [static_friction, dynamic_friction, restitution]. - env_ids: Environment indices. If None, all environments are updated. - """ - # resolve all indices - env_ids = self._resolve_env_ids(env_ids) - # convert materials to warp array if needed (root_view requires warp arrays) - if isinstance(materials, torch.Tensor): - materials = wp.from_torch(materials, dtype=wp.float32) - # set material properties via root view - self.root_view.set_material_properties(materials, env_ids) - - def set_material_properties_mask( - self, - *, - materials: torch.Tensor | wp.array, - env_mask: wp.array | None = None, - ) -> None: - """Set material properties (friction and restitution) for collision shapes using masks. - - The material properties consist of static friction, dynamic friction, and restitution - coefficients for each collision shape. - - .. note:: - This method expects full data. - - .. tip:: - For maximum performance we recommend using the index method. This is because in PhysX, the tensor API - is only supporting indexing, hence masks need to be converted to indices. - - Args: - materials: Material properties for all shapes. Shape is ``(num_instances, max_shapes, 3)`` - where the 3 values are ``[static_friction, dynamic_friction, restitution]``. - env_mask: Environment mask. If None, then all the instances are updated. Shape is (num_instances,). - """ - # Resolve masks to indices - env_ids = self._resolve_env_mask(env_mask) - # Call the index method - self.set_material_properties_index(materials=materials, env_ids=env_ids) - def set_joint_position_target_index( self, *, diff --git a/source/isaaclab_physx/isaaclab_physx/assets/rigid_object/rigid_object.py b/source/isaaclab_physx/isaaclab_physx/assets/rigid_object/rigid_object.py index 586750eaa6a3..b549da96b787 100644 --- a/source/isaaclab_physx/isaaclab_physx/assets/rigid_object/rigid_object.py +++ b/source/isaaclab_physx/isaaclab_physx/assets/rigid_object/rigid_object.py @@ -912,65 +912,6 @@ def set_inertias_mask( # Set full data to True to ensure the right code path is taken inside the kernel. self.set_inertias_index(inertias=inertias, body_ids=body_ids, env_ids=env_ids, full_data=True) - def set_material_properties_index( - self, - *, - materials: torch.Tensor | wp.array, - env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, - ) -> None: - """Set material properties (friction and restitution) for collision shapes using indices. - - The material properties consist of static friction, dynamic friction, and restitution - coefficients for each collision shape. - - .. note:: - This method expects partial data. - - .. tip:: - Both the index and mask methods have dedicated optimized implementations. Performance is similar for both. - However, to allow graphed pipelines, the mask method must be used. - - Args: - materials: Material properties for all shapes. Shape is (len(env_ids), max_shapes, 3) - where the 3 values are [static_friction, dynamic_friction, restitution]. - env_ids: Environment indices. If None, all environments are updated. - """ - # resolve all indices - env_ids = self._resolve_env_ids(env_ids) - # convert materials to warp array if needed (root_view requires warp arrays) - if isinstance(materials, torch.Tensor): - materials = wp.from_torch(materials, dtype=wp.float32) - # set material properties via root view - self.root_view.set_material_properties(materials, env_ids) - - def set_material_properties_mask( - self, - *, - materials: torch.Tensor | wp.array, - env_mask: wp.array | None = None, - ) -> None: - """Set material properties (friction and restitution) for collision shapes using masks. - - The material properties consist of static friction, dynamic friction, and restitution - coefficients for each collision shape. - - .. note:: - This method expects full data. - - .. tip:: - For maximum performance we recommend using the index method. This is because in PhysX, the tensor API - is only supporting indexing, hence masks need to be converted to indices. - - Args: - materials: Material properties for all shapes. Shape is ``(num_instances, max_shapes, 3)`` - where the 3 values are ``[static_friction, dynamic_friction, restitution]``. - env_mask: Environment mask. If None, then all the instances are updated. Shape is (num_instances,). - """ - # Resolve masks to indices - env_ids = self._resolve_env_mask(env_mask) - # Call the index method - self.set_material_properties_index(materials=materials, env_ids=env_ids) - """ Internal helper. """ 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 c84eac2eef45..877a44261293 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 @@ -1053,65 +1053,6 @@ def set_inertias_mask( # Set full data to True to ensure the right code path is taken inside the kernel. self.set_inertias_index(inertias=inertias, body_ids=body_ids, env_ids=env_ids, full_data=True) - def set_material_properties_index( - self, - *, - materials: torch.Tensor | wp.array, - env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, - ) -> None: - """Set material properties (friction and restitution) for collision shapes using indices. - - The material properties consist of static friction, dynamic friction, and restitution - coefficients for each collision shape. - - .. note:: - This method expects partial data. - - .. tip:: - Both the index and mask methods have dedicated optimized implementations. Performance is similar for both. - However, to allow graphed pipelines, the mask method must be used. - - Args: - materials: Material properties for all shapes. Shape is (len(env_ids), max_shapes, 3) - where the 3 values are [static_friction, dynamic_friction, restitution]. - env_ids: Environment indices. If None, all environments are updated. - """ - # resolve all indices - env_ids = self._resolve_env_ids(env_ids) - # convert materials to warp array if needed (root_view requires warp arrays) - if isinstance(materials, torch.Tensor): - materials = wp.from_torch(materials, dtype=wp.float32) - # set material properties via root view - self.root_view.set_material_properties(materials, env_ids) - - def set_material_properties_mask( - self, - *, - materials: torch.Tensor | wp.array, - env_mask: wp.array | None = None, - ) -> None: - """Set material properties (friction and restitution) for collision shapes using masks. - - The material properties consist of static friction, dynamic friction, and restitution - coefficients for each collision shape. - - .. note:: - This method expects full data. - - .. tip:: - For maximum performance we recommend using the index method. This is because in PhysX, the tensor API - is only supporting indexing, hence masks need to be converted to indices. - - Args: - materials: Material properties for all shapes. Shape is ``(num_instances, max_shapes, 3)`` - where the 3 values are ``[static_friction, dynamic_friction, restitution]``. - env_mask: Environment mask. If None, then all the instances are updated. Shape is (num_instances,). - """ - # Resolve masks to indices - env_ids = self._resolve_env_mask(env_mask) - # Call the index method - self.set_material_properties_index(materials=materials, env_ids=env_ids) - """ Helper functions. """ diff --git a/source/isaaclab_physx/test/assets/test_articulation.py b/source/isaaclab_physx/test/assets/test_articulation.py index 9726a7bd3a80..cf6f86be0670 100644 --- a/source/isaaclab_physx/test/assets/test_articulation.py +++ b/source/isaaclab_physx/test/assets/test_articulation.py @@ -2268,9 +2268,11 @@ def test_set_material_properties(sim, num_articulations, device, add_ground_plan # Ensure dynamic friction <= static friction materials[..., 1] = torch.min(materials[..., 0], materials[..., 1]) - # Use PhysX's material properties API + # Set material properties via the PhysX view-level API env_ids = torch.arange(num_articulations, dtype=torch.int32) - articulation.set_material_properties_index(materials=materials, env_ids=env_ids) + articulation.root_view.set_material_properties( + wp.from_torch(materials, dtype=wp.float32), wp.from_torch(env_ids, dtype=wp.int32) + ) # Simulate physics sim.step() diff --git a/source/isaaclab_physx/test/assets/test_rigid_object.py b/source/isaaclab_physx/test/assets/test_rigid_object.py index 431bbf0fab55..a41f5a39f083 100644 --- a/source/isaaclab_physx/test/assets/test_rigid_object.py +++ b/source/isaaclab_physx/test/assets/test_rigid_object.py @@ -607,8 +607,8 @@ def test_rigid_body_set_material_properties(num_cubes, device): @pytest.mark.parametrize("num_cubes", [1, 2]) @pytest.mark.parametrize("device", ["cuda:0", "cpu"]) @pytest.mark.isaacsim_ci -def test_set_material_properties_index(num_cubes, device): - """Test setting material properties using the set_material_properties_index API.""" +def test_set_material_properties_via_view(num_cubes, device): + """Test setting material properties via the PhysX view-level API.""" with build_simulation_context( device=device, gravity_enabled=True, add_ground_plane=True, auto_add_lighting=True ) as sim: @@ -627,9 +627,11 @@ def test_set_material_properties_index(num_cubes, device): # Ensure dynamic friction <= static friction materials[..., 1] = torch.min(materials[..., 0], materials[..., 1]) - # Use the new set_material_properties_index API + # Set material properties via the PhysX view-level API env_ids = torch.arange(num_cubes, dtype=torch.int32) - cube_object.set_material_properties_index(materials=materials, env_ids=env_ids) + cube_object.root_view.set_material_properties( + wp.from_torch(materials, dtype=wp.float32), wp.from_torch(env_ids, dtype=wp.int32) + ) # Simulate physics sim.step() From fed0a733d5b6a7782dcacd65d101f4122d7bff58 Mon Sep 17 00:00:00 2001 From: Antoine Richard Date: Fri, 10 Apr 2026 17:38:40 +0200 Subject: [PATCH 09/11] Address review findings from automated code review - Fix randomize_rigid_body_com to pass partial data (env_ids subset) to set_coms_index, matching the pattern used by mass/inertia terms. - Remove dead _default_friction/_default_restitution writes in Newton material implementation (values were written but never read back). - Guard Newton collider offset notification to only fire when params are actually provided. - Fix changelog Sphinx roles for converted function-to-class items. --- source/isaaclab/docs/CHANGELOG.rst | 12 ++++++------ source/isaaclab/isaaclab/envs/mdp/events.py | 16 +++++----------- 2 files changed, 11 insertions(+), 17 deletions(-) diff --git a/source/isaaclab/docs/CHANGELOG.rst b/source/isaaclab/docs/CHANGELOG.rst index 97966a2cfbd7..ea796e1a506f 100644 --- a/source/isaaclab/docs/CHANGELOG.rst +++ b/source/isaaclab/docs/CHANGELOG.rst @@ -18,14 +18,14 @@ Changed backend-specific implementations. PhysX uses bucket-based 3-tuple materials via the tensor API; Newton samples friction and restitution continuously per shape via view-level attribute bindings. -* Converted :func:`~isaaclab.envs.mdp.randomize_rigid_body_com` from a plain function - to a :class:`~isaaclab.managers.ManagerTermBase` class with repeatable randomization +* Converted ``randomize_rigid_body_com`` from a plain function to a + :class:`~isaaclab.managers.ManagerTermBase` class with repeatable randomization from cached defaults. Newton passes position-only (vec3); PhysX passes full pose (pos + quat). -* Converted :func:`~isaaclab.envs.mdp.randomize_rigid_body_collider_offsets` from a - plain function to a :class:`~isaaclab.managers.ManagerTermBase` class with - backend-specific implementations. PhysX uses rest/contact offsets directly; Newton - maps them to ``shape_margin`` and ``shape_gap``. +* Converted ``randomize_rigid_body_collider_offsets`` from a plain function to a + :class:`~isaaclab.managers.ManagerTermBase` class with backend-specific + implementations. PhysX uses rest/contact offsets directly; Newton maps them to + ``shape_margin`` and ``shape_gap``. 4.5.30 (2026-04-13) diff --git a/source/isaaclab/isaaclab/envs/mdp/events.py b/source/isaaclab/isaaclab/envs/mdp/events.py index 60849f651ea3..3781df73cfdc 100644 --- a/source/isaaclab/isaaclab/envs/mdp/events.py +++ b/source/isaaclab/isaaclab/envs/mdp/events.py @@ -293,10 +293,6 @@ def __init__( self._friction_binding = asset._root_view.get_attribute("shape_material_mu", model)[:, 0] # type: ignore self._restitution_binding = asset._root_view.get_attribute("shape_material_restitution", model)[:, 0] # type: ignore - # cache defaults - self._default_friction = wp.to_torch(self._friction_binding).clone() - self._default_restitution = wp.to_torch(self._restitution_binding).clone() - def __call__( self, env: ManagerBasedEnv, @@ -328,10 +324,6 @@ def __call__( restitution_range_t[0], restitution_range_t[1], (len(env_ids), num_shapes), device=device ) - # update cached buffers for the affected env_ids - self._default_friction[env_ids[:, None], shape_idx] = friction_samples - self._default_restitution[env_ids[:, None], shape_idx] = restitution_samples - # write only the affected env_ids to the warp binding friction_view = wp.to_torch(self._friction_binding) restitution_view = wp.to_torch(self._restitution_binding) @@ -750,10 +742,11 @@ def __call__( coms[env_ids[:, None], body_ids, :3] += rand_samples # Newton expects position-only (vec3f), PhysX expects the full pose (pos + quat) + # note: pass partial data of shape (len(env_ids), len(body_ids), ...) to match the API if self._is_newton: - self.asset.set_coms_index(coms=coms[:, body_ids, :3], body_ids=body_ids, env_ids=env_ids) + self.asset.set_coms_index(coms=coms[env_ids[:, None], body_ids, :3], body_ids=body_ids, env_ids=env_ids) else: - self.asset.set_coms_index(coms=coms[:, body_ids], body_ids=body_ids, env_ids=env_ids) + self.asset.set_coms_index(coms=coms[env_ids[:, None], body_ids], body_ids=body_ids, env_ids=env_ids) class _RandomizeRigidBodyColliderOffsetsPhysx: @@ -880,7 +873,8 @@ def __call__( gap_view = wp.to_torch(self._sim_bind_shape_gap) gap_view[env_ids] = gap[env_ids] - self._newton_manager.add_model_change(self._notify_shape_properties) + if rest_offset_distribution_params is not None or contact_offset_distribution_params is not None: + self._newton_manager.add_model_change(self._notify_shape_properties) class randomize_rigid_body_collider_offsets(ManagerTermBase): From ba77aa5d3664fd2bf690f01cbc187378f6d5853f Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Fri, 10 Apr 2026 18:16:38 -0700 Subject: [PATCH 10/11] fix event --- source/isaaclab/isaaclab/envs/mdp/events.py | 18 ++++++++---------- 1 file changed, 8 insertions(+), 10 deletions(-) diff --git a/source/isaaclab/isaaclab/envs/mdp/events.py b/source/isaaclab/isaaclab/envs/mdp/events.py index 3781df73cfdc..071a918b11d3 100644 --- a/source/isaaclab/isaaclab/envs/mdp/events.py +++ b/source/isaaclab/isaaclab/envs/mdp/events.py @@ -262,10 +262,9 @@ def __init__( self, cfg: EventTermCfg, env: ManagerBasedEnv, asset: RigidObject | Articulation, asset_cfg: SceneEntityCfg ): import isaaclab_newton.physics.newton_manager as newton_manager_module # noqa: PLC0415 + from isaaclab_newton.assets import Articulation as NewtonArticulation # noqa: PLC0415 from newton.solvers import SolverNotifyFlags # noqa: PLC0415 - from isaaclab.assets import BaseArticulation - self.asset = asset self.asset_cfg = asset_cfg self._newton_manager = newton_manager_module.NewtonManager @@ -275,8 +274,13 @@ def __init__( self._static_friction_range = cfg.params.get("static_friction_range", (1.0, 1.0)) self._restitution_range = cfg.params.get("restitution_range", (0.0, 0.0)) + # get friction/restitution view-level bindings + model = self._newton_manager.get_model() + self._friction_binding = asset._root_view.get_attribute("shape_material_mu", model)[:, 0] # type: ignore + self._restitution_binding = asset._root_view.get_attribute("shape_material_restitution", model)[:, 0] # type: ignore + # compute shape indices for body-specific randomization - if isinstance(asset, BaseArticulation) and asset_cfg.body_ids != slice(None): + if isinstance(asset, NewtonArticulation) and asset_cfg.body_ids != slice(None): num_shapes_per_body = asset.num_shapes_per_body shape_indices_list = [] for body_id in asset_cfg.body_ids: @@ -285,13 +289,7 @@ def __init__( shape_indices_list.extend(range(start_idx, end_idx)) self._shape_indices = torch.tensor(shape_indices_list, dtype=torch.long) else: - total_shapes = sum(asset.num_shapes_per_body) - self._shape_indices = torch.arange(total_shapes, dtype=torch.long) - - # get friction/restitution view-level bindings - model = self._newton_manager.get_model() - self._friction_binding = asset._root_view.get_attribute("shape_material_mu", model)[:, 0] # type: ignore - self._restitution_binding = asset._root_view.get_attribute("shape_material_restitution", model)[:, 0] # type: ignore + self._shape_indices = torch.arange(self._friction_binding.shape[1], dtype=torch.long) def __call__( self, From f08d567ecd7e1ab323ac143b2467f907e62fb639 Mon Sep 17 00:00:00 2001 From: Antoine Richard Date: Mon, 13 Apr 2026 13:30:21 +0200 Subject: [PATCH 11/11] Fix PhysX collider offset randomization passing torch to warp API The PhysX view set_rest_offsets/set_contact_offsets methods expect warp arrays on CPU after the warp migration. Convert both data and env_ids to warp before calling the view API, matching the pattern used by all other PhysX view setters. --- source/isaaclab/isaaclab/envs/mdp/events.py | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/source/isaaclab/isaaclab/envs/mdp/events.py b/source/isaaclab/isaaclab/envs/mdp/events.py index 071a918b11d3..5de8b83a66d0 100644 --- a/source/isaaclab/isaaclab/envs/mdp/events.py +++ b/source/isaaclab/isaaclab/envs/mdp/events.py @@ -768,9 +768,10 @@ def __call__( distribution: Literal["uniform", "log_uniform", "gaussian"] = "uniform", ): if env_ids is None: - env_ids = torch.arange(env.scene.num_envs, device="cpu") + env_ids = torch.arange(env.scene.num_envs, device="cpu", dtype=torch.int32) else: - env_ids = env_ids.cpu() + env_ids = env_ids.to(device="cpu", dtype=torch.int32) + wp_env_ids = wp.from_torch(env_ids, dtype=wp.int32) if rest_offset_distribution_params is not None: rest_offset = self.default_rest_offsets.clone() @@ -782,7 +783,7 @@ def __call__( operation="abs", distribution=distribution, ) - self.asset.root_view.set_rest_offsets(rest_offset, env_ids) + self.asset.root_view.set_rest_offsets(wp.from_torch(rest_offset), wp_env_ids) if contact_offset_distribution_params is not None: contact_offset = self.default_contact_offsets.clone() @@ -794,7 +795,7 @@ def __call__( operation="abs", distribution=distribution, ) - self.asset.root_view.set_contact_offsets(contact_offset, env_ids) + self.asset.root_view.set_contact_offsets(wp.from_torch(contact_offset), wp_env_ids) class _RandomizeRigidBodyColliderOffsetsNewton: