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..ea796e1a506f 100644 --- a/source/isaaclab/docs/CHANGELOG.rst +++ b/source/isaaclab/docs/CHANGELOG.rst @@ -1,6 +1,33 @@ Changelog --------- +4.5.31 (2026-04-13) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added :class:`~isaaclab.envs.mdp.randomize_rigid_body_inertia` event term for + randomizing body inertia tensors independently of mass. Supports diagonal-only + (Ixx, Iyy, Izz) and full 3x3 modes. + +Changed +^^^^^^^ + +* 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 ``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 ``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/__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..5de8b83a66d0 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_physx.assets import DeformableObject @@ -151,69 +151,50 @@ def randomize_rigid_body_scale( op_order_spec.default = Vt.TokenArray(["xformOp:translate", "xformOp:orient", "xformOp:scale"]) -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. - - 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. - - 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. +class _RandomizeRigidBodyMaterialPhysx: + """PhysX backend implementation for material randomization. - .. 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. - - .. note:: - PhysX only allows 64000 unique physics materials in the scene. If the number of materials exceeds this - limit, the simulation will crash. Due to this reason, we sample the materials only once during initialization. - Afterwards, these materials are randomly assigned to the geometries of the asset. + 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): - """Initialize the term. - - Args: - cfg: The configuration of the event term. - env: The environment instance. + def __init__( + self, cfg: EventTermCfg, env: ManagerBasedEnv, asset: RigidObject | Articulation, asset_cfg: SceneEntityCfg + ): + from isaaclab.assets import BaseArticulation - Raises: - ValueError: If the asset is not a RigidObject or an Articulation. - """ - from isaaclab.assets import BaseArticulation, BaseRigidObject + # 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)) - super().__init__(cfg, env) + # 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") - # 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] + # 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]) - if not isinstance(self.asset, (BaseRigidObject, BaseArticulation)): - raise ValueError( - f"Randomization term 'randomize_rigid_body_material' not supported for asset: '{self.asset_cfg.name}'" - f" with type: '{type(self.asset)}'." - ) + 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(self.asset, BaseArticulation) and self.asset_cfg.body_ids != slice(None): + if isinstance(asset, BaseArticulation) and 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 + 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 = self.asset.root_view.max_shapes + 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." @@ -223,24 +204,6 @@ def __init__(self, cfg: EventTermCfg, env: ManagerBasedEnv): # in this case, we don't need to do special indexing self.num_shapes_per_body = None - # 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]) - def __call__( self, env: ManagerBasedEnv, @@ -286,6 +249,172 @@ def __call__( ) +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 isaaclab_newton.assets import Articulation as NewtonArticulation # noqa: PLC0415 + from newton.solvers import SolverNotifyFlags # noqa: PLC0415 + + 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)) + + # 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, 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: + 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(self._friction_binding.shape[1], dtype=torch.long) + + 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 + ) + + # 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 and assigns them to the geometries of the asset. + + 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) + 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 (PhysX only). This obeys the physics constraint on friction values. + + .. attention:: + 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 + limit, the simulation will crash. Due to this reason, we sample the materials only once during initialization. + Afterwards, these materials are randomly assigned to the geometries of the asset. + """ + + 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) + + # 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_material' not supported for asset: '{self.asset_cfg.name}'" + f" 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 = _RandomizeRigidBodyMaterialNewton(cfg, env, self.asset, self.asset_cfg) + else: + self._impl = _RandomizeRigidBodyMaterialPhysx(cfg, env, self.asset, self.asset_cfg) + + 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, + ): + 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): """Randomize the mass of the bodies by adding, scaling, or setting random values. @@ -405,104 +534,420 @@ def __call__( ) -def randomize_rigid_body_com( - env: ManagerBasedEnv, - env_ids: torch.Tensor | None, - com_range: dict[str, tuple[float, float]], - asset_cfg: SceneEntityCfg, -): - """Randomize the center of mass (CoM) of rigid bodies by adding a random value sampled from the given ranges. +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:: - 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. + Unlike :class:`randomize_rigid_body_mass` which recomputes inertia based on mass + ratios, this function modifies inertia directly without affecting mass. """ - # 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) + def __init__(self, cfg: EventTermCfg, env: ManagerBasedEnv): + """Initialize the term. - # 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) + Args: + cfg: The configuration of the event term. + env: The environment instance. - # get the current com of the bodies (num_assets, num_bodies) - coms = wp.to_torch(asset.data.body_com_pose_b).clone() + 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, BaseRigidObjectCollection - # Randomize the com in range - coms[env_ids[:, None], body_ids, :3] += rand_samples + super().__init__(cfg, env) - # Set the new coms - asset.set_coms_index(coms=coms, env_ids=env_ids) + # 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, BaseRigidObjectCollection)): + raise ValueError( + f"Randomization term 'randomize_rigid_body_inertia' not supported for asset: '{self.asset_cfg.name}'" + f" with type: '{type(self.asset)}'." + ) -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. + # 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) + + +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. + + This class tracks the original CoM values and randomizes from those defaults on each call, + ensuring repeatable randomization across resets. + + Automatically detects the active physics backend: + + - **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): + """Initialize the term. + + Args: + cfg: The configuration of the event term. + env: The environment instance. + """ + super().__init__(cfg, env) + + self.asset_cfg: SceneEntityCfg = cfg.params["asset_cfg"] + self.asset: RigidObject | Articulation = env.scene[self.asset_cfg.name] + + # detect physics backend + manager_name = env.sim.physics_manager.__name__.lower() + self._is_newton = "newton" in manager_name + + 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: + 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.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) + # 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[env_ids[:, None], body_ids, :3], body_ids=body_ids, env_ids=env_ids) + else: + self.asset.set_coms_index(coms=coms[env_ids[:, None], body_ids], body_ids=body_ids, env_ids=env_ids) + + +class _RandomizeRigidBodyColliderOffsetsPhysx: + """PhysX backend implementation for collider offset randomization. + + Uses rest offset and contact offset directly via the PhysX tensor API. + """ + + 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, + 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="cpu", dtype=torch.int32) + else: + 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() + rest_offset = _randomize_prop_by_op( + rest_offset, + rest_offset_distribution_params, + None, + slice(None), + operation="abs", + distribution=distribution, + ) + 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() + contact_offset = _randomize_prop_by_op( + contact_offset, + contact_offset_distribution_params, + None, + slice(None), + operation="abs", + distribution=distribution, + ) + self.asset.root_view.set_contact_offsets(wp.from_torch(contact_offset), wp_env_ids) + + +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, + 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) + else: + env_ids = env_ids.to(device) + + margin_view = wp.to_torch(self._sim_bind_shape_margin) + + if rest_offset_distribution_params is not None: + margin = self.default_margin.clone() + margin = _randomize_prop_by_op( + margin, + rest_offset_distribution_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_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_distribution_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] + + 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): + """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 the collision checking. + These correspond to the physics engine collider properties that affect 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. + 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``. - Currently, the distribution parameters are applied as absolute values. + 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 to assign the collision properties. It is recommended to use this function - only during the initialization of the environment. + 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. """ - # 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): + """Initialize the term. - # 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, + 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, - 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, + distribution, ) - asset.root_view.set_contact_offsets(contact_offset, env_ids.cpu()) class randomize_physics_scene_gravity(ManagerTermBase): @@ -802,14 +1247,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 +1324,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 +1360,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..371ce6f91e2a 100644 --- a/source/isaaclab_newton/docs/CHANGELOG.rst +++ b/source/isaaclab_newton/docs/CHANGELOG.rst @@ -1,6 +1,16 @@ 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. + + 0.5.11 (2026-04-13) ~~~~~~~~~~~~~~~~~~~ @@ -12,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/test/assets/test_articulation.py b/source/isaaclab_newton/test/assets/test_articulation.py index 867f0fe8eba1..50474990838b 100644 --- a/source/isaaclab_newton/test/assets/test_articulation.py +++ b/source/isaaclab_newton/test/assets/test_articulation.py @@ -2487,5 +2487,122 @@ 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) 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 + ) + + # Play the simulator + sim.reset() + + # 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 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) + + 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) + + # 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 (only shape 0) + if num_shapes > 1: + 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) + + 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(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]) +@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"]) diff --git a/source/isaaclab_newton/test/assets/test_rigid_object.py b/source/isaaclab_newton/test/assets/test_rigid_object.py index 9cc1717d8c5f..c1d01f4164fb 100644 --- a/source/isaaclab_newton/test/assets/test_rigid_object.py +++ b/source/isaaclab_newton/test/assets/test_rigid_object.py @@ -589,12 +589,11 @@ 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 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 @@ -603,33 +602,48 @@ 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) + # 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] - materials = torch.cat([static_friction, dynamic_friction, restitution], dim=-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) - 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) - ) + wp.to_torch(friction_binding)[:] = friction + wp.to_torch(restitution_binding)[:] = restitution + SimulationManager.add_model_change(SolverNotifyFlags.SHAPE_PROPERTIES) # 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()) + # 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) + - # Check if material properties are set correctly - torch.testing.assert_close(materials_to_check.reshape(num_cubes, 3), materials) +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) -@pytest.mark.skip(reason="Newton friction/restitution/material not yet supported") + 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 @@ -640,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, ) ) @@ -653,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 @@ -690,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 @@ -711,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) @@ -719,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 @@ -777,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 @@ -800,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, @@ -808,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() @@ -824,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 a1086d4371ec..7d4a0be7cb98 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,43 @@ 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 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 - 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) + # 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) - materials = torch.cat([static_friction, dynamic_friction, restitution], dim=-1) + # 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] - # 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), - ) + # 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 - materials_to_check = object_collection.root_view.get_material_properties() - - # 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 - ) + # 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.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..077963af1127 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) ~~~~~~~~~~~~~~~~~~~ @@ -15,6 +32,7 @@ Fixed remain visually updated after resuming. + 0.5.13 (2026-03-25) ~~~~~~~~~~~~~~~~~~~ diff --git a/source/isaaclab_physx/test/assets/test_articulation.py b/source/isaaclab_physx/test/assets/test_articulation.py index f1e31795c130..cf6f86be0670 100644 --- a/source/isaaclab_physx/test/assets/test_articulation.py +++ b/source/isaaclab_physx/test/assets/test_articulation.py @@ -2245,5 +2245,45 @@ 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]) + + # Set material properties via the PhysX view-level API + env_ids = torch.arange(num_articulations, dtype=torch.int32) + 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() + 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"]) diff --git a/source/isaaclab_physx/test/assets/test_rigid_object.py b/source/isaaclab_physx/test/assets/test_rigid_object.py index 6cea115105e1..a41f5a39f083 100644 --- a/source/isaaclab_physx/test/assets/test_rigid_object.py +++ b/source/isaaclab_physx/test/assets/test_rigid_object.py @@ -604,6 +604,46 @@ 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_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: + 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]) + + # Set material properties via the PhysX view-level API + env_ids = torch.arange(num_cubes, dtype=torch.int32) + 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() + 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