diff --git a/scripts/tutorials/01_assets/run_deformable_object.py b/scripts/tutorials/01_assets/run_deformable_object.py index 4d2583974b96..8c056d907a1c 100644 --- a/scripts/tutorials/01_assets/run_deformable_object.py +++ b/scripts/tutorials/01_assets/run_deformable_object.py @@ -22,6 +22,7 @@ # add argparse arguments parser = argparse.ArgumentParser(description="Tutorial on interacting with a deformable object.") +parser.add_argument("--backend", type=str, default="physx", choices=["physx", "newton"], help="Physics backend.") # append AppLauncher cli args AppLauncher.add_app_launcher_args(parser) # demos should open Kit visualizer by default @@ -37,14 +38,10 @@ import torch import warp as wp -from isaaclab_physx.assets import DeformableObject, DeformableObjectCfg - -# deformables supported in PhysX -from isaaclab_physx.sim import DeformableBodyMaterialCfg, DeformableBodyPropertiesCfg import isaaclab.sim as sim_utils import isaaclab.utils.math as math_utils -from isaaclab.sim import SimulationContext +from isaaclab.assets import DeformableObject, DeformableObjectCfg def design_scene(): @@ -59,20 +56,22 @@ def design_scene(): # Create a dictionary for the scene entities scene_entities = {} - # Create separate groups called "Origin0", "Origin1", ... - # Each group will have a robot in it + # Create separate groups called "env_0", "env_1", ... + # Newton's scene loader requires the "env_\d+" naming convention to + # detect per-environment Xforms and replicate them as separate worlds. origins = [[0.25, 0.25, 0.0], [-0.25, 0.25, 0.0], [0.25, -0.25, 0.0], [-0.25, -0.25, 0.0]] for i, origin in enumerate(origins): - sim_utils.create_prim(f"/World/Origin{i}", "Xform", translation=origin) + sim_utils.create_prim(f"/World/env_{i}", "Xform", translation=origin) # 3D Deformable Object cfg = DeformableObjectCfg( - prim_path="/World/Origin.*/Cube", + prim_path="/World/env_.*/Cube", spawn=sim_utils.MeshCuboidCfg( size=(0.2, 0.2, 0.2), - deformable_props=DeformableBodyPropertiesCfg(), + deformable_props=sim_utils.DeformableBodyPropertiesCfg(rest_offset=0.0, contact_offset=0.001), visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.5, 0.1, 0.0)), - physics_material=DeformableBodyMaterialCfg(poissons_ratio=0.4, youngs_modulus=1e5), + physics_material=sim_utils.DeformableBodyMaterialCfg(poissons_ratio=0.4, youngs_modulus=1e5, density=500.0), + # physics_material=sim_utils.SurfaceDeformableBodyMaterialCfg(poissons_ratio=0.4, youngs_modulus=1e4, surface_thickness=0.001, surface_bend_stiffness=1e0, surface_shear_stiffness=1e0, surface_stretch_stiffness=1e0), ), init_state=DeformableObjectCfg.InitialStateCfg(pos=(0.0, 0.0, 1.0)), debug_vis=True, @@ -158,8 +157,17 @@ def run_simulator(sim: sim_utils.SimulationContext, entities: dict, origins: tor def main(): """Main function.""" # Load kit helper - sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) - sim = SimulationContext(sim_cfg) + if args_cli.backend == "newton": + from isaaclab_contrib.deformable import register_hooks as _register_deformable_hooks + _register_deformable_hooks() + from isaaclab_contrib.deformable.newton_manager_cfg import VBDSolverCfg + from isaaclab_newton.physics import NewtonCfg + physics_cfg = NewtonCfg(solver_cfg=VBDSolverCfg(iterations=10), num_substeps=4) + else: + from isaaclab_physx.physics import PhysxCfg + physics_cfg = PhysxCfg() + sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device, physics=physics_cfg) + sim = sim_utils.SimulationContext(sim_cfg) # Set main camera sim.set_camera_view(eye=[2.0, 2.0, 2.0], target=[0.0, 0.0, 0.75]) # Design scene diff --git a/source/isaaclab/isaaclab/assets/__init__.pyi b/source/isaaclab/isaaclab/assets/__init__.pyi index 727eae33002b..55b42b8819ab 100644 --- a/source/isaaclab/isaaclab/assets/__init__.pyi +++ b/source/isaaclab/isaaclab/assets/__init__.pyi @@ -21,6 +21,11 @@ __all__ = [ "RigidObjectCollection", "RigidObjectCollectionCfg", "RigidObjectCollectionData", + "BaseDeformableObject", + "BaseDeformableObjectData", + "DeformableObject", + "DeformableObjectCfg", + "DeformableObjectData", ] from .articulation import ( @@ -46,3 +51,10 @@ from .rigid_object_collection import ( RigidObjectCollectionCfg, RigidObjectCollectionData, ) +from .deformable_object import ( + BaseDeformableObject, + BaseDeformableObjectData, + DeformableObject, + DeformableObjectCfg, + DeformableObjectData, +) diff --git a/source/isaaclab/isaaclab/assets/deformable_object/__init__.py b/source/isaaclab/isaaclab/assets/deformable_object/__init__.py new file mode 100644 index 000000000000..525b6ae5fd98 --- /dev/null +++ b/source/isaaclab/isaaclab/assets/deformable_object/__init__.py @@ -0,0 +1,10 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-module for deformable object assets.""" + +from isaaclab.utils.module import lazy_export + +lazy_export() diff --git a/source/isaaclab/isaaclab/assets/deformable_object/__init__.pyi b/source/isaaclab/isaaclab/assets/deformable_object/__init__.pyi new file mode 100644 index 000000000000..ebe78d5b6dbc --- /dev/null +++ b/source/isaaclab/isaaclab/assets/deformable_object/__init__.pyi @@ -0,0 +1,18 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from .base_deformable_object import BaseDeformableObject +from .base_deformable_object_data import BaseDeformableObjectData +from .deformable_object import DeformableObject +from .deformable_object_cfg import DeformableObjectCfg +from .deformable_object_data import DeformableObjectData + +__all__ = [ + "BaseDeformableObject", + "BaseDeformableObjectData", + "DeformableObject", + "DeformableObjectCfg", + "DeformableObjectData", +] diff --git a/source/isaaclab/isaaclab/assets/deformable_object/base_deformable_object.py b/source/isaaclab/isaaclab/assets/deformable_object/base_deformable_object.py new file mode 100644 index 000000000000..4dae9b2a2b2e --- /dev/null +++ b/source/isaaclab/isaaclab/assets/deformable_object/base_deformable_object.py @@ -0,0 +1,368 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import warnings +from abc import abstractmethod +from collections.abc import Sequence +from typing import TYPE_CHECKING + +import torch +import warp as wp + +import isaaclab.utils.math as math_utils +from isaaclab.assets.asset_base import AssetBase + +if TYPE_CHECKING: + from .base_deformable_object_data import BaseDeformableObjectData + from .deformable_object_cfg import DeformableObjectCfg + + +class BaseDeformableObject(AssetBase): + """Abstract base class for deformable object assets. + + Deformable objects are assets that can be deformed in the simulation. They are typically used for + soft bodies, such as stuffed animals, food items, and cloth. + + Unlike rigid object assets, deformable objects have a more complex structure and require additional + handling for simulation. The state of a deformable object comprises of its nodal positions and + velocities, and not the object's root position and orientation. The nodal positions and velocities + are in the simulation frame. + + Soft bodies can be `partially kinematic`_, where some nodes are driven by kinematic targets, and + the rest are simulated. The kinematic targets are the desired positions of the nodes, and the + simulation drives the nodes towards these targets. + + .. _partially kinematic: https://nvidia-omniverse.github.io/PhysX/physx/5.4.1/docs/SoftBodies.html#kinematic-soft-bodies + """ + + cfg: DeformableObjectCfg + """Configuration instance for the deformable object.""" + + __backend_name__: str = "base" + """The name of the backend for the deformable object.""" + + def __init__(self, cfg: DeformableObjectCfg): + """Initialize the deformable object. + + Args: + cfg: A configuration instance. + """ + super().__init__(cfg) + + """ + Properties + """ + + @property + @abstractmethod + def data(self) -> BaseDeformableObjectData: + """Data container for the deformable object.""" + raise NotImplementedError() + + @property + @abstractmethod + def num_instances(self) -> int: + """Number of instances of the asset.""" + raise NotImplementedError() + + @property + @abstractmethod + def num_bodies(self) -> int: + """Number of bodies in the asset. + + This is always 1 since each object is a single deformable body. + """ + raise NotImplementedError() + + @property + @abstractmethod + def max_sim_vertices_per_body(self) -> int: + """The maximum number of simulation mesh vertices per deformable body.""" + raise NotImplementedError() + + """ + Operations. + """ + + @abstractmethod + def reset(self, env_ids: Sequence[int] | None = None, env_mask: wp.array | None = None) -> None: + """Reset the deformable object. + + Args: + env_ids: Environment indices. If None, then all indices are used. + env_mask: Environment mask. If None, then all the instances are updated. + Shape is (num_instances,). + """ + raise NotImplementedError() + + @abstractmethod + def write_data_to_sim(self): + """Write data to the simulator.""" + raise NotImplementedError() + + @abstractmethod + def update(self, dt: float): + """Update the internal buffers. + + Args: + dt: The amount of time passed from last :meth:`update` call [s]. + """ + raise NotImplementedError() + + """ + Operations - Write to simulation (index variants). + """ + + def write_nodal_state_to_sim_index( + self, + nodal_state: torch.Tensor | wp.array, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + full_data: bool = False, + ) -> None: + """Set the nodal state over selected environment indices into the simulation. + + The nodal state comprises of the nodal positions and velocities. Since these are nodes, + the velocity only has a translational component. All the quantities are in the simulation frame. + + Args: + nodal_state: Nodal state in simulation frame [m, m/s]. + Shape is (len(env_ids), max_sim_vertices_per_body, 6) + or (num_instances, max_sim_vertices_per_body, 6). + env_ids: Environment indices. If None, then all indices are used. + full_data: Whether to expect full data. Defaults to False. + """ + # Convert warp to torch if needed + if isinstance(nodal_state, wp.array): + nodal_state = wp.to_torch(nodal_state) + # set into simulation + self.write_nodal_pos_to_sim_index(nodal_state[..., :3], env_ids=env_ids, full_data=full_data) + self.write_nodal_velocity_to_sim_index(nodal_state[..., 3:], env_ids=env_ids, full_data=full_data) + + @abstractmethod + def write_nodal_pos_to_sim_index( + self, + nodal_pos: torch.Tensor | wp.array, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + full_data: bool = False, + ) -> None: + """Set the nodal positions over selected environment indices into the simulation. + + Args: + nodal_pos: Nodal positions in simulation frame [m]. + Shape is (len(env_ids), max_sim_vertices_per_body, 3) + or (num_instances, max_sim_vertices_per_body, 3). + env_ids: Environment indices. If None, then all indices are used. + full_data: Whether to expect full data. Defaults to False. + """ + raise NotImplementedError() + + @abstractmethod + def write_nodal_velocity_to_sim_index( + self, + nodal_vel: torch.Tensor | wp.array, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + full_data: bool = False, + ) -> None: + """Set the nodal velocity over selected environment indices into the simulation. + + Args: + nodal_vel: Nodal velocities in simulation frame [m/s]. + Shape is (len(env_ids), max_sim_vertices_per_body, 3) + or (num_instances, max_sim_vertices_per_body, 3). + env_ids: Environment indices. If None, then all indices are used. + full_data: Whether to expect full data. Defaults to False. + """ + raise NotImplementedError() + + @abstractmethod + def write_nodal_kinematic_target_to_sim_index( + self, + targets: torch.Tensor | wp.array, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + full_data: bool = False, + ) -> None: + """Set the kinematic targets of the simulation mesh for the deformable bodies using indices. + + The kinematic targets comprise of individual nodal positions of the simulation mesh + for the deformable body and a flag indicating whether the node is kinematically driven or not. + The positions are in the simulation frame. + + .. note:: + The flag is set to 0.0 for kinematically driven nodes and 1.0 for free nodes. + + Args: + targets: The kinematic targets comprising of nodal positions and flags [m]. + Shape is (len(env_ids), max_sim_vertices_per_body, 4) + or (num_instances, max_sim_vertices_per_body, 4). + env_ids: Environment indices. If None, then all indices are used. + full_data: Whether to expect full data. Defaults to False. + """ + raise NotImplementedError() + + """ + Operations - Write to simulation (mask variants). + """ + + def write_nodal_state_to_sim_mask( + self, + nodal_state: torch.Tensor | wp.array, + env_mask: wp.array | None = None, + ) -> None: + """Set the nodal state over selected environment mask into the simulation. + + Args: + nodal_state: Nodal state in simulation frame [m, m/s]. + Shape is (num_instances, max_sim_vertices_per_body, 6). + env_mask: Environment mask. If None, then all indices are used. + """ + if env_mask is not None: + env_ids = wp.nonzero(env_mask) + else: + env_ids = self._ALL_INDICES + self.write_nodal_state_to_sim_index(nodal_state, env_ids=env_ids, full_data=True) + + def write_nodal_pos_to_sim_mask( + self, + nodal_pos: torch.Tensor | wp.array, + env_mask: wp.array | None = None, + ) -> None: + """Set the nodal positions over selected environment mask into the simulation. + + Args: + nodal_pos: Nodal positions in simulation frame [m]. + Shape is (num_instances, max_sim_vertices_per_body, 3). + env_mask: Environment mask. If None, then all indices are used. + """ + if env_mask is not None: + env_ids = wp.nonzero(env_mask) + else: + env_ids = self._ALL_INDICES + self.write_nodal_pos_to_sim_index(nodal_pos, env_ids=env_ids, full_data=True) + + def write_nodal_velocity_to_sim_mask( + self, + nodal_vel: torch.Tensor | wp.array, + env_mask: wp.array | None = None, + ) -> None: + """Set the nodal velocity over selected environment mask into the simulation. + + Args: + nodal_vel: Nodal velocities in simulation frame [m/s]. + Shape is (num_instances, max_sim_vertices_per_body, 3). + env_mask: Environment mask. If None, then all indices are used. + """ + if env_mask is not None: + env_ids = wp.nonzero(env_mask) + else: + env_ids = self._ALL_INDICES + self.write_nodal_velocity_to_sim_index(nodal_vel, env_ids=env_ids, full_data=True) + + def write_nodal_kinematic_target_to_sim_mask( + self, + targets: torch.Tensor | wp.array, + env_mask: wp.array | None = None, + ) -> None: + """Set the kinematic targets of the simulation mesh for the deformable bodies using mask. + + Args: + targets: The kinematic targets comprising of nodal positions and flags [m]. + Shape is (num_instances, max_sim_vertices_per_body, 4). + env_mask: Environment mask. If None, then all indices are used. + """ + if env_mask is not None: + env_ids = wp.nonzero(env_mask) + else: + env_ids = self._ALL_INDICES + self.write_nodal_kinematic_target_to_sim_index(targets, env_ids=env_ids, full_data=True) + + """ + Operations - Deprecated wrappers. + """ + + def write_nodal_state_to_sim( + self, + nodal_state: torch.Tensor | wp.array, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + ) -> None: + """Deprecated. Please use :meth:`write_nodal_state_to_sim_index` instead.""" + warnings.warn( + "The method 'write_nodal_state_to_sim' is deprecated. Please use 'write_nodal_state_to_sim_index' instead.", + DeprecationWarning, + stacklevel=2, + ) + self.write_nodal_state_to_sim_index(nodal_state, env_ids=env_ids) + + def write_nodal_kinematic_target_to_sim( + self, + targets: torch.Tensor | wp.array, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + ) -> None: + """Deprecated. Please use :meth:`write_nodal_kinematic_target_to_sim_index` instead.""" + warnings.warn( + "The method 'write_nodal_kinematic_target_to_sim' is deprecated." + " Please use 'write_nodal_kinematic_target_to_sim_index' instead.", + DeprecationWarning, + stacklevel=2, + ) + self.write_nodal_kinematic_target_to_sim_index(targets, env_ids=env_ids) + + def write_nodal_pos_to_sim( + self, + nodal_pos: torch.Tensor | wp.array, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + ) -> None: + """Deprecated. Please use :meth:`write_nodal_pos_to_sim_index` instead.""" + warnings.warn( + "The method 'write_nodal_pos_to_sim' is deprecated. Please use 'write_nodal_pos_to_sim_index' instead.", + DeprecationWarning, + stacklevel=2, + ) + self.write_nodal_pos_to_sim_index(nodal_pos, env_ids=env_ids) + + def write_nodal_velocity_to_sim( + self, + nodal_vel: torch.Tensor | wp.array, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + ) -> None: + """Deprecated. Please use :meth:`write_nodal_velocity_to_sim_index` instead.""" + warnings.warn( + "The method 'write_nodal_velocity_to_sim' is deprecated." + " Please use 'write_nodal_velocity_to_sim_index' instead.", + DeprecationWarning, + stacklevel=2, + ) + self.write_nodal_velocity_to_sim_index(nodal_vel, env_ids=env_ids) + + """ + Operations - Helper. + """ + + def transform_nodal_pos( + self, nodal_pos: torch.Tensor, pos: torch.Tensor | None = None, quat: torch.Tensor | None = None + ) -> torch.Tensor: + """Transform the nodal positions based on the pose transformation. + + This function computes the transformation of the nodal positions based on the pose transformation. + It multiplies the nodal positions with the rotation matrix of the pose and adds the translation. + Internally, it calls the :meth:`isaaclab.utils.math.transform_points` function. + + Args: + nodal_pos: The nodal positions in the simulation frame [m]. + Shape is (N, max_sim_vertices_per_body, 3). + pos: The position transformation [m]. Shape is (N, 3). + Defaults to None, in which case the position is assumed to be zero. + quat: The orientation transformation as quaternion (x, y, z, w). Shape is (N, 4). + Defaults to None, in which case the orientation is assumed to be identity. + + Returns: + The transformed nodal positions [m]. Shape is (N, max_sim_vertices_per_body, 3). + """ + # offset the nodal positions to center them around the origin + mean_nodal_pos = nodal_pos.mean(dim=1, keepdim=True) + nodal_pos = nodal_pos - mean_nodal_pos + # transform the nodal positions based on the pose around the origin + return math_utils.transform_points(nodal_pos, pos, quat) + mean_nodal_pos diff --git a/source/isaaclab/isaaclab/assets/deformable_object/base_deformable_object_data.py b/source/isaaclab/isaaclab/assets/deformable_object/base_deformable_object_data.py new file mode 100644 index 000000000000..76e62702bad8 --- /dev/null +++ b/source/isaaclab/isaaclab/assets/deformable_object/base_deformable_object_data.py @@ -0,0 +1,117 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +from abc import ABC, abstractmethod + +import warp as wp + + +class BaseDeformableObjectData(ABC): + """Abstract data container for a deformable object. + + This class defines the interface for deformable object data in the simulation. + The data includes the nodal states of the root deformable body in the object. + The data is stored in the simulation world frame unless otherwise specified. + + The data is lazily updated, meaning that the data is only updated when it is accessed. + This is useful when the data is expensive to compute or retrieve. The data is updated + when the timestamp of the buffer is older than the current simulation timestamp. + """ + + def __init__(self, device: str): + """Initialize the deformable object data. + + Args: + device: The device used for processing. + """ + self.device = device + # Set initial time stamp + self._sim_timestamp = 0.0 + + def update(self, dt: float): + """Update the data for the deformable object. + + Args: + dt: The time step for the update [s]. This must be a positive value. + """ + # update the simulation timestamp + self._sim_timestamp += dt + + ## + # Defaults. + ## + + default_nodal_state_w: wp.array = None + """Default nodal state ``[nodal_pos, nodal_vel]`` in simulation world frame. + Shape is (num_instances, max_sim_vertices_per_body) with dtype vec6f. + """ + + ## + # Kinematic commands. + ## + + nodal_kinematic_target: wp.array = None + """Simulation mesh kinematic targets for the deformable bodies. + Shape is (num_instances, max_sim_vertices_per_body) with dtype vec4f. + + The kinematic targets are used to drive the simulation mesh vertices to the target positions. + The targets are stored as (x, y, z, is_not_kinematic) where "is_not_kinematic" is a binary + flag indicating whether the vertex is kinematic or not. The flag is set to 0 for kinematic vertices + and 1 for non-kinematic vertices. + """ + + ## + # Properties. + ## + + @property + @abstractmethod + def nodal_pos_w(self) -> wp.array: + """Nodal positions in simulation world frame [m]. + Shape is (num_instances, max_sim_vertices_per_body) vec3f. + """ + raise NotImplementedError() + + @property + @abstractmethod + def nodal_vel_w(self) -> wp.array: + """Nodal velocities in simulation world frame [m/s]. + Shape is (num_instances, max_sim_vertices_per_body) vec3f. + """ + raise NotImplementedError() + + @property + @abstractmethod + def nodal_state_w(self) -> wp.array: + """Nodal state ``[nodal_pos, nodal_vel]`` in simulation world frame [m, m/s]. + Shape is (num_instances, max_sim_vertices_per_body) vec6f. + """ + raise NotImplementedError() + + ## + # Derived properties. + ## + + @property + @abstractmethod + def root_pos_w(self) -> wp.array: + """Root position from nodal positions of the simulation mesh for the deformable bodies + in simulation world frame [m]. Shape is (num_instances,) vec3f. + + This quantity is computed as the mean of the nodal positions. + """ + raise NotImplementedError() + + @property + @abstractmethod + def root_vel_w(self) -> wp.array: + """Root velocity from vertex velocities for the deformable bodies in simulation + world frame [m/s]. Shape is (num_instances,) vec3f. + + This quantity is computed as the mean of the nodal velocities. + """ + raise NotImplementedError() diff --git a/source/isaaclab/isaaclab/assets/deformable_object/deformable_object.py b/source/isaaclab/isaaclab/assets/deformable_object/deformable_object.py new file mode 100644 index 000000000000..3bd41c8728be --- /dev/null +++ b/source/isaaclab/isaaclab/assets/deformable_object/deformable_object.py @@ -0,0 +1,27 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +from typing import TYPE_CHECKING + +from isaaclab.utils.backend_utils import FactoryBase + +from .base_deformable_object import BaseDeformableObject +from .base_deformable_object_data import BaseDeformableObjectData + +if TYPE_CHECKING: + from isaaclab_physx.assets.deformable_object import DeformableObject as PhysXDeformableObject + from isaaclab_physx.assets.deformable_object import DeformableObjectData as PhysXDeformableObjectData + + +class DeformableObject(FactoryBase, BaseDeformableObject): + """Factory for creating deformable object instances.""" + + data: BaseDeformableObjectData | PhysXDeformableObjectData + + def __new__(cls, *args, **kwargs) -> BaseDeformableObject | PhysXDeformableObject: + """Create a new instance of a deformable object based on the backend.""" + return super().__new__(cls, *args, **kwargs) diff --git a/source/isaaclab/isaaclab/assets/deformable_object/deformable_object_cfg.py b/source/isaaclab/isaaclab/assets/deformable_object/deformable_object_cfg.py new file mode 100644 index 000000000000..7dbf060c27b6 --- /dev/null +++ b/source/isaaclab/isaaclab/assets/deformable_object/deformable_object_cfg.py @@ -0,0 +1,32 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +from typing import TYPE_CHECKING + +from isaaclab.assets.asset_base_cfg import AssetBaseCfg +from isaaclab.markers import VisualizationMarkersCfg +from isaaclab.markers.config import DEFORMABLE_TARGET_MARKER_CFG +from isaaclab.utils import configclass + +if TYPE_CHECKING: + from .deformable_object import DeformableObject + + +@configclass +class DeformableObjectCfg(AssetBaseCfg): + """Configuration parameters for a deformable object.""" + + class_type: type[DeformableObject] | str = "{DIR}.deformable_object:DeformableObject" + + visualizer_cfg: VisualizationMarkersCfg = DEFORMABLE_TARGET_MARKER_CFG.replace( + prim_path="/Visuals/DeformableTarget" + ) + """The configuration object for the visualization markers. Defaults to DEFORMABLE_TARGET_MARKER_CFG. + + .. note:: + This attribute is only used when debug visualization is enabled. + """ diff --git a/source/isaaclab/isaaclab/assets/deformable_object/deformable_object_data.py b/source/isaaclab/isaaclab/assets/deformable_object/deformable_object_data.py new file mode 100644 index 000000000000..35567aceb847 --- /dev/null +++ b/source/isaaclab/isaaclab/assets/deformable_object/deformable_object_data.py @@ -0,0 +1,25 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +from typing import TYPE_CHECKING + +from isaaclab.utils.backend_utils import FactoryBase + +from .base_deformable_object_data import BaseDeformableObjectData + +if TYPE_CHECKING: + from isaaclab_physx.assets.deformable_object.deformable_object_data import ( + DeformableObjectData as PhysXDeformableObjectData, + ) + + +class DeformableObjectData(FactoryBase): + """Factory for creating deformable object data instances.""" + + def __new__(cls, *args, **kwargs) -> BaseDeformableObjectData | PhysXDeformableObjectData: + """Create a new instance of a deformable object data based on the backend.""" + return super().__new__(cls, *args, **kwargs) diff --git a/source/isaaclab/isaaclab/sim/__init__.pyi b/source/isaaclab/isaaclab/sim/__init__.pyi index a718ccdcb989..b559da1ec4f4 100644 --- a/source/isaaclab/isaaclab/sim/__init__.pyi +++ b/source/isaaclab/isaaclab/sim/__init__.pyi @@ -22,11 +22,13 @@ __all__ = [ "activate_contact_sensors", "define_articulation_root_properties", "define_collision_properties", + "define_deformable_body_properties", "define_mass_properties", "define_mesh_collision_properties", "define_rigid_body_properties", "modify_articulation_root_properties", "modify_collision_properties", + "modify_deformable_body_properties", "modify_fixed_tendon_properties", "modify_joint_drive_properties", "modify_mass_properties", @@ -39,6 +41,7 @@ __all__ = [ "CollisionPropertiesCfg", "ConvexDecompositionPropertiesCfg", "ConvexHullPropertiesCfg", + "DeformableBodyPropertiesCfg", "FixedTendonPropertiesCfg", "JointDrivePropertiesCfg", "MassPropertiesCfg", @@ -50,6 +53,7 @@ __all__ = [ "TriangleMeshSimplificationPropertiesCfg", "SpawnerCfg", "RigidObjectSpawnerCfg", + "DeformableObjectSpawnerCfg", "spawn_from_mjcf", "spawn_from_urdf", "spawn_from_usd", @@ -68,8 +72,11 @@ __all__ = [ "LightCfg", "SphereLightCfg", "spawn_rigid_body_material", + "spawn_deformable_body_material", "PhysicsMaterialCfg", "RigidBodyMaterialCfg", + "DeformableBodyMaterialCfg", + "SurfaceDeformableBodyMaterialCfg", "spawn_from_mdl_file", "spawn_preview_surface", "GlassMdlCfg", @@ -188,11 +195,13 @@ from .schemas import ( activate_contact_sensors, define_articulation_root_properties, define_collision_properties, + define_deformable_body_properties, define_mass_properties, define_mesh_collision_properties, define_rigid_body_properties, modify_articulation_root_properties, modify_collision_properties, + modify_deformable_body_properties, modify_fixed_tendon_properties, modify_joint_drive_properties, modify_mass_properties, @@ -205,6 +214,7 @@ from .schemas import ( CollisionPropertiesCfg, ConvexDecompositionPropertiesCfg, ConvexHullPropertiesCfg, + DeformableBodyPropertiesCfg, FixedTendonPropertiesCfg, JointDrivePropertiesCfg, MassPropertiesCfg, @@ -218,6 +228,7 @@ from .schemas import ( from .spawners import ( SpawnerCfg, RigidObjectSpawnerCfg, + DeformableObjectSpawnerCfg, spawn_from_mjcf, spawn_from_urdf, spawn_from_usd, @@ -236,8 +247,11 @@ from .spawners import ( LightCfg, SphereLightCfg, spawn_rigid_body_material, + spawn_deformable_body_material, PhysicsMaterialCfg, RigidBodyMaterialCfg, + DeformableBodyMaterialCfg, + SurfaceDeformableBodyMaterialCfg, spawn_from_mdl_file, spawn_preview_surface, GlassMdlCfg, diff --git a/source/isaaclab/isaaclab/sim/schemas/__init__.pyi b/source/isaaclab/isaaclab/sim/schemas/__init__.pyi index f413b3ded12d..947d4ab47c92 100644 --- a/source/isaaclab/isaaclab/sim/schemas/__init__.pyi +++ b/source/isaaclab/isaaclab/sim/schemas/__init__.pyi @@ -10,11 +10,13 @@ __all__ = [ "activate_contact_sensors", "define_articulation_root_properties", "define_collision_properties", + "define_deformable_body_properties", "define_mass_properties", "define_mesh_collision_properties", "define_rigid_body_properties", "modify_articulation_root_properties", "modify_collision_properties", + "modify_deformable_body_properties", "modify_fixed_tendon_properties", "modify_joint_drive_properties", "modify_mass_properties", @@ -27,6 +29,7 @@ __all__ = [ "CollisionPropertiesCfg", "ConvexDecompositionPropertiesCfg", "ConvexHullPropertiesCfg", + "DeformableBodyPropertiesCfg", "FixedTendonPropertiesCfg", "JointDrivePropertiesCfg", "MassPropertiesCfg", @@ -45,11 +48,13 @@ from .schemas import ( activate_contact_sensors, define_articulation_root_properties, define_collision_properties, + define_deformable_body_properties, define_mass_properties, define_mesh_collision_properties, define_rigid_body_properties, modify_articulation_root_properties, modify_collision_properties, + modify_deformable_body_properties, modify_fixed_tendon_properties, modify_joint_drive_properties, modify_mass_properties, @@ -64,6 +69,7 @@ from .schemas_cfg import ( CollisionPropertiesCfg, ConvexDecompositionPropertiesCfg, ConvexHullPropertiesCfg, + DeformableBodyPropertiesCfg, FixedTendonPropertiesCfg, JointDrivePropertiesCfg, MassPropertiesCfg, diff --git a/source/isaaclab/isaaclab/sim/schemas/schemas.py b/source/isaaclab/isaaclab/sim/schemas/schemas.py index 0f97b542e031..6cf142e69990 100644 --- a/source/isaaclab/isaaclab/sim/schemas/schemas.py +++ b/source/isaaclab/isaaclab/sim/schemas/schemas.py @@ -10,14 +10,19 @@ import math from typing import Any -from pxr import Usd, UsdPhysics +import numpy as np +import warp as wp + +from pxr import Sdf, Usd, UsdGeom, UsdPhysics from isaaclab.sim.utils.stage import get_current_stage from isaaclab.utils.string import to_camel_case from ..utils import ( apply_nested, + create_prim, find_global_fixed_joint_prim, + get_all_matching_child_prims, safe_set_attribute_on_usd_prim, safe_set_attribute_on_usd_schema, ) @@ -1013,3 +1018,321 @@ def modify_mesh_collision_properties( # success return True + + +""" +Deformable body properties. +""" + + +@wp.kernel +def _fix_tet_winding_kernel( + points: wp.array(dtype=wp.vec3), + tet_indices: wp.array(ndim=2, dtype=wp.int32), +): + """Flip any tet with negative signed volume by swapping its last two vertex indices. + + ``UsdGeom.TetMesh`` and :meth:`UsdGeom.TetMesh.ComputeSurfaceFaces` require a + right-handed tet winding (positive signed volume). Swapping indices 2 and 3 + reverses the orientation without changing which four vertices form the tet. + """ + i = wp.tid() + v0 = tet_indices[i, 0] + v1 = tet_indices[i, 1] + v2 = tet_indices[i, 2] + v3 = tet_indices[i, 3] + p0 = points[v0] + e1 = points[v1] - p0 + e2 = points[v2] - p0 + e3 = points[v3] - p0 + signed_volume = wp.dot(e1, wp.cross(e2, e3)) + if signed_volume < 0.0: + tet_indices[i, 2] = v3 + tet_indices[i, 3] = v2 + + +def define_deformable_body_properties( + prim_path: str, + cfg: schemas_cfg.DeformableBodyPropertiesCfg, + stage: Usd.Stage | None = None, + deformable_type: str = "volume", + sim_mesh_prim_path: str | None = None, +): + """Apply the deformable body schema on the input prim and set its properties. The input prim should + have a visual surface mesh as child. Volume deformables will have their simulation tetrahedral mesh + automatically computed from the surface mesh of the input prim. Surface deformables simply copy the visual mesh + as simulation mesh. + + See :func:`modify_deformable_body_properties` for more details on how the properties are set. + + .. note:: + If the input prim is not a mesh, this function will traverse the prim and find the first mesh + under it. If no mesh or multiple meshes are found, an error is raised. This is because the deformable + body schema can only be applied to a single mesh. + + Args: + prim_path: The prim path where to apply the deformable body schema. + cfg: The configuration for the deformable body. + stage: The stage where to find the prim. Defaults to None, in which case the + current stage is used. + deformable_type: The type of the deformable body (surface or volume). + This is used to determine which USD API to use for the deformable body. Defaults to "volume". + sim_mesh_prim_path: Optional override for the simulation mesh creation prim path. + If None, it is set to ``{prim_path}/sim_mesh`` for surface deformables + and ``{prim_path}/sim_tetmesh`` for volume deformables. + + Raises: + ValueError: When the prim path is not valid. + ValueError: When the prim has no mesh or multiple meshes. + RuntimeError: When setting the deformable body properties fails. + """ + from omni.physx.scripts import deformableUtils + + # get stage handle + if stage is None: + stage = get_current_stage() + + # get USD prim + root_prim = stage.GetPrimAtPath(prim_path) + # check if prim path is valid + if not root_prim.IsValid(): + raise ValueError(f"Prim path '{prim_path}' is not valid.") + + # traverse the prim and get the visual mesh. If none or multiple meshes are found, raise error. + matching_prims = get_all_matching_child_prims(prim_path, lambda p: p.GetTypeName() == "Mesh") + # check if the visual surface mesh is valid + if len(matching_prims) == 0: + raise ValueError(f"Could not find any visual mesh in '{prim_path}'. Please check asset.") + if len(matching_prims) > 1: + # get list of all meshes found + mesh_paths = [p.GetPrimPath() for p in matching_prims] + raise ValueError( + f"Found multiple visual meshes in '{prim_path}': {mesh_paths}." + " Deformable body schema can only be applied to one mesh for now." + ) + vis_mesh_prim = matching_prims[0] + + # check if the prim is valid + if not vis_mesh_prim.IsValid(): + raise ValueError(f"Mesh prim path '{vis_mesh_prim.GetPrimPath()}' is not valid.") + + # remove potential previous configuration + deformableUtils.remove_deformable_body(stage, prim_path) + + # create and set simulation/root prim properties based on the type of the deformable mesh (surface vs volume) + sim_mesh_prim_path = prim_path + "/sim_mesh" if sim_mesh_prim_path is None else sim_mesh_prim_path + # extract visual surface mesh vertices and faces + vertices = np.array(vis_mesh_prim.GetAttribute("points").Get()) + faces = np.array(vis_mesh_prim.GetAttribute("faceVertexIndices").Get()).flatten() + face_counts = np.array(vis_mesh_prim.GetAttribute("faceVertexCounts").Get()) + if deformable_type == "surface": + # create simulation mesh as copy of visual mesh + sim_mesh_prim = create_prim( + sim_mesh_prim_path, + prim_type="Mesh", + attributes={ + "points": vertices, + "faceVertexIndices": faces, + "faceVertexCounts": face_counts, + }, + stage=stage, + ) + + # apply sim API + if not sim_mesh_prim.ApplyAPI("OmniPhysicsSurfaceDeformableSimAPI"): + raise RuntimeError(f"Failed to set surface deformable body API on prim '{sim_mesh_prim_path}'.") + + # apply collision API + if not sim_mesh_prim.ApplyAPI(UsdPhysics.CollisionAPI): + raise RuntimeError(f"Failed to set surface deformable collision API on prim '{sim_mesh_prim_path}'.") + + # set rest-shape attributes required by OmniPhysicsSurfaceDeformableSimAPI + sim_mesh_prim.GetAttribute("omniphysics:restShapePoints").Set(vertices) + sim_mesh_prim.GetAttribute("omniphysics:restTriVtxIndices").Set(faces) + + elif deformable_type == "volume": + from pytetwild import tetrahedralize + + tet_mesh_points, tet_mesh_indices = tetrahedralize( + vertices, + faces.reshape(-1, 3), + edge_length_fac=0.2, + simplify=False, + epsilon=1e-2, + coarsen=True, + ) + # pytetwild's default ordering does not guarantee positive signed volume, which + # ``UsdGeom.TetMesh`` and ``ComputeSurfaceFaces`` require. Flip any inverted tets. + device = "cpu" + _tet_points_wp = wp.array(tet_mesh_points.astype(np.float32), dtype=wp.vec3, device=device) + _tet_indices_wp = wp.array( + np.asarray(tet_mesh_indices, dtype=np.int32).reshape(-1, 4), dtype=wp.int32, device=device + ) + wp.launch( + _fix_tet_winding_kernel, + dim=_tet_indices_wp.shape[0], + inputs=[_tet_points_wp, _tet_indices_wp], + device=device, + ) + tet_mesh_indices = _tet_indices_wp.numpy() + sim_mesh_prim = create_prim( + sim_mesh_prim_path, + prim_type="TetMesh", + attributes={ + "points": tet_mesh_points, + "tetVertexIndices": tet_mesh_indices, + }, + stage=stage, + ) + + # apply sim API + if not sim_mesh_prim.ApplyAPI("OmniPhysicsVolumeDeformableSimAPI"): + raise RuntimeError(f"Failed to set volume deformable body API on prim '{sim_mesh_prim_path}'.") + + # apply collision API + if not sim_mesh_prim.ApplyAPI(UsdPhysics.CollisionAPI): + raise RuntimeError(f"Failed to set volume deformable collision API on prim '{sim_mesh_prim_path}'.") + + # set surface faces and rest-shape attributes required by OmniPhysicsVolumeDeformableSimAPI + surface_face_indices = UsdGeom.TetMesh.ComputeSurfaceFaces( + UsdGeom.TetMesh(sim_mesh_prim), Usd.TimeCode.Default() + ) + UsdGeom.TetMesh(sim_mesh_prim).GetSurfaceFaceVertexIndicesAttr().Set(surface_face_indices) + sim_mesh_prim.GetAttribute("omniphysics:restShapePoints").Set(tet_mesh_points) + sim_mesh_prim.GetAttribute("omniphysics:restTetVtxIndices").Set(tet_mesh_indices) + + else: + raise ValueError( + f"""Unsupported deformable type: '{deformable_type}'. + Only surface and volume deformables are supported.""" + ) + + # TODO: Temporary solution: Overwrite visual mesh with tet mesh surface points or copy + # surface sim mesh to vis mesh. In the future we can have separate visual from simulation mesh. + # This currently does not work if an asset is loaded where the visual mesh is not the simulation mesh surface. + vis_mesh = UsdGeom.Mesh(vis_mesh_prim) + if deformable_type == "volume": + tet_mesh_prim = UsdGeom.TetMesh(sim_mesh_prim) + surface_indices = tet_mesh_prim.GetSurfaceFaceVertexIndicesAttr().Get() + if surface_indices is None or len(surface_indices) == 0: + raise ValueError( + f"Deformable body at '{prim_path}' has no surface indices on its TetMesh prim; " + "cannot sync to visual mesh." + ) + vis_mesh.GetPointsAttr().Set(tet_mesh_prim.GetPointsAttr().Get()) + vis_mesh.GetFaceVertexIndicesAttr().Set(np.asarray(surface_indices).flatten()) + vis_mesh.GetFaceVertexCountsAttr().Set([3] * len(surface_indices)) + else: + sim_mesh = UsdGeom.Mesh(sim_mesh_prim) + vis_mesh.GetFaceVertexIndicesAttr().Set(sim_mesh.GetFaceVertexIndicesAttr().Get()) + vis_mesh.GetFaceVertexCountsAttr().Set(sim_mesh.GetFaceVertexCountsAttr().Get()) + + # bind visual to sim mesh by applying bind pose deformable pose API + purposes = ["bindPose"] + vis_mesh_prim.ApplyAPI("OmniPhysicsDeformablePoseAPI", "default") + vis_mesh_prim.CreateAttribute("deformablePose:default:omniphysics:purposes", Sdf.ValueTypeNames.TokenArray).Set( + purposes + ) + points = UsdGeom.PointBased(vis_mesh_prim).GetPointsAttr().Get() + vis_mesh_prim.CreateAttribute("deformablePose:default:omniphysics:points", Sdf.ValueTypeNames.Point3fArray).Set( + points + ) + + sim_mesh_prim.ApplyAPI("OmniPhysicsDeformablePoseAPI", "default") + sim_mesh_prim.CreateAttribute("deformablePose:default:omniphysics:purposes", Sdf.ValueTypeNames.TokenArray).Set( + purposes + ) + + # disable simulation mesh for rendering + UsdGeom.Imageable(sim_mesh_prim).GetPurposeAttr().Set(UsdGeom.Tokens.guide) + + # apply deformable body api + if not root_prim.ApplyAPI("OmniPhysicsDeformableBodyAPI"): + raise RuntimeError(f"Failed to set deformable body API on prim '{prim_path}'.") + + # set deformable body properties + modify_deformable_body_properties(prim_path, cfg, stage) + + +@apply_nested +def modify_deformable_body_properties( + prim_path: str, cfg: schemas_cfg.DeformableBodyPropertiesCfg, stage: Usd.Stage | None = None +): + """Modify deformable body parameters for a deformable body prim. + + A `deformable body`_ is a single body (either surface or volume deformable) that can be simulated by PhysX + or Newton. Unlike rigid bodies, deformable bodies support relative motion of the nodes in the mesh. + Consequently, they can be used to simulate deformations under applied forces. + + PhysX deformable body simulation employs Finite Element Analysis (FEA) to simulate the deformations of the mesh. + It uses two meshes to represent the deformable body: + + 1. **Simulation mesh**: This mesh is used for the simulation and is the one that is deformed by the solver. + 2. **Collision mesh**: This mesh only needs to match the surface of the simulation mesh and is used for + collision detection. + + For most applications, we assume that the above two meshes are computed from the "render mesh" of the deformable + body. The render mesh is the mesh that is visible in the scene and is used for rendering purposes. It is composed + of triangles, while the simulation mesh is composed of tetrahedrons for volume deformables, + and triangles for surface deformables. + + We apply similar design choices to the simulation in Newton with a separate visual, simulation and collision mesh. + + .. caution:: + The deformable body schema is still under development by the Omniverse team. The current implementation + works with the PhysX schemas shipped with Isaac Sim 6.0.0 onwards. It may change in future releases. + + .. note:: + This function is decorated with :func:`apply_nested` that sets the properties to all the prims + (that have the schema applied on them) under the input prim path. + + .. _deformable body: https://nvidia-omniverse.github.io/PhysX/physx/5.6.1/docs/DeformableVolume.html + .. _PhysxDeformableBodyAPI: https://docs.omniverse.nvidia.com/kit/docs/omni_usd_schema_physics/latest/physxschema/annotated.html + + Args: + prim_path: The prim path to the deformable body. + cfg: The configuration for the deformable body. + stage: The stage where to find the prim. Defaults to None, in which case the + current stage is used. + + Returns: + True if the properties were successfully set, False otherwise. + """ + # get stage handle + if stage is None: + stage = get_current_stage() + + # get deformable-body USD prim + deformable_body_prim = stage.GetPrimAtPath(prim_path) + # check if the prim is valid + if not deformable_body_prim.IsValid(): + return False + # check if deformable body API is applied + if "OmniPhysicsDeformableBodyAPI" not in deformable_body_prim.GetAppliedSchemas(): + return False + + # apply customization to deformable API + if "PhysxBaseDeformableBodyAPI" not in deformable_body_prim.GetAppliedSchemas(): + deformable_body_prim.AddAppliedSchema("PhysxBaseDeformableBodyAPI") + + # ensure PhysX collision API is applied on the collision mesh + if "PhysxCollisionAPI" not in deformable_body_prim.GetAppliedSchemas(): + deformable_body_prim.AddAppliedSchema("PhysxCollisionAPI") + + # convert to dict + cfg = cfg.to_dict() + # set into PhysX API + if cfg["kinematic_enabled"]: + logger.warning( + "Kinematic deformable bodies are not fully supported in the current version of Omni Physics. " + "Setting kinematic_enabled to True may lead to unexpected behavior." + ) + # prefixes for each attribute (collision attributes: physxCollision:*, and physxDeformable:* for rest) + property_prefixes = cfg["_property_prefix"] + for prefix, attr_list in property_prefixes.items(): + for attr_name in attr_list: + safe_set_attribute_on_usd_prim( + deformable_body_prim, f"{prefix}:{to_camel_case(attr_name, 'cC')}", cfg[attr_name], camel_case=False + ) + # success + return True diff --git a/source/isaaclab/isaaclab/sim/schemas/schemas_cfg.py b/source/isaaclab/isaaclab/sim/schemas/schemas_cfg.py index 69cbc8bd5304..f30981168273 100644 --- a/source/isaaclab/isaaclab/sim/schemas/schemas_cfg.py +++ b/source/isaaclab/isaaclab/sim/schemas/schemas_cfg.py @@ -5,6 +5,7 @@ from __future__ import annotations +import dataclasses from typing import Literal from isaaclab.utils import configclass @@ -547,3 +548,156 @@ class SDFMeshPropertiesCfg(MeshCollisionPropertiesCfg): Default value is 6. Range: [0, inf) """ + + +@configclass +class OmniPhysicsPropertiesCfg: + """OmniPhysics properties for a deformable body. + + These properties are set with the prefix ``omniphysics:``. For example, to set the mass of the + deformable body, you would set the property ``omniphysics:mass``. + + See the OmniPhysics documentation for more information on the available properties. + """ + + deformable_body_enabled: bool | None = None + """Enables deformable body.""" + + kinematic_enabled: bool = False + """Enables kinematic body. Defaults to False, which means that the body is not kinematic.""" + + mass: float | None = None + """The material mass in [kg]. Defaults to None, in which case the material density is used to compute the mass.""" + + +@configclass +class PhysXDeformableBodyPropertiesCfg: + """PhysX-specific properties for a deformable body. + + These properties are set with the prefix ``physxDeformableBody:`` + + For more information on the available properties, please refer to the `documentation `_. + """ + + solver_position_iteration_count: int = 16 + """Number of the solver positional iterations per step. Range is [1,255], default to 16.""" + + linear_damping: float | None = None + """Linear damping coefficient, in units of [1/s] and constrained to the range [0, inf).""" + + max_linear_velocity: float | None = None + """Maximum allowable linear velocity for the deformable body, in units of distance/second and constrained to the + range [0, inf). A negative value allows the simulation to choose suitable a per vertex value dynamically, + currently only supported for surface deformables. This can help prevent surface-surface intersections.""" + + settling_damping: float | None = None + """Additional damping applied when a vertex's velocity falls below :attr:`settling_threshold`. + Specified in units of [1/s] and constrained to the range [0, inf).""" + + settling_threshold: float | None = None + """Velocity threshold below which :attr:`settling_damping` is applied in addition to standard damping. + Specified in units of distance/second and constrained to the range [0, inf).""" + + sleep_threshold: float | None = None + """Velocity threshold below which a vertex becomes a candidate for sleeping. + Specified in units of distance/seconds and constrained to the range [0, inf).""" + + max_depenetration_velocity: float | None = None + """Maximum velocity that the solver may apply to resolve intersections. + Specified in units of distance/seconds and constrained to the range [0, inf).""" + + self_collision: bool | None = None + """Enables self-collisions for the deformable body, preventing self-intersections.""" + + self_collision_filter_distance: float | None = None + r"""Distance below which self-collision is disabled [m]. + + The default value of -inf indicates that the simulation selects a suitable value. + Constrained to range [:attr:`rest_offset` \* 2, inf]. + """ + + enable_speculative_c_c_d: bool | None = None + """Enables dynamic adjustment of contact offset based on velocity (speculative continuous collision detection).""" + + disable_gravity: bool | None = None + """Disables gravity for the deformable body.""" + + # specific to surface deformables + collision_pair_update_frequency: int | None = None + """Determines how often surface-to-surface collision pairs are updated during each time step. + Increasing this value results in more frequent updates to the contact pairs, which provides better contact points. + + For example, a value of 2 means collision pairs are updated twice per time step: + once at the beginning and once in the middle of the time step (i.e., during the middle solver iteration). + If set to 0, the solver adaptively determines when to update the surface-to-surface contact pairs, + instead of using a fixed frequency. + + Valid range: [1, :attr:`solver_position_iteration_count`]. + """ + + collision_iteration_multiplier: float | None = None + """Determines how many collision subiterations are used in each solver iteration. + By default, collision constraints are applied once per solver iteration. + Increasing this value applies collision constraints more frequently within each solver iteration. + + For example, a value of 2 means collision constraints are applied twice per solver iteration + (i.e., collision constraints are applied 2 x :attr:`solver_position_iteration_count` times per time step). + Increasing this value does not update collision pairs more frequently; + refer to :attr:`collision_pair_update_frequency` for that. + + Valid range: [1, :attr:`solver_position_iteration_count` / 2]. + """ + + +@configclass +class PhysXCollisionPropertiesCfg: + """PhysX-specific collision properties for a deformable body. + + These properties are set with the prefix ``physxCollision:``. + + See the PhysX documentation for more information on the available properties. + """ + + contact_offset: float | None = None + """Contact offset for the collision shape [m]. + + The collision detector generates contact points as soon as two shapes get closer than the sum of their + contact offsets. This quantity should be non-negative which means that contact generation can potentially start + before the shapes actually penetrate. + """ + + rest_offset: float | None = None + """Rest offset for the collision shape [m]. + + The rest offset quantifies how close a shape gets to others at rest, At rest, the distance between two + vertically stacked objects is the sum of their rest offsets. If a pair of shapes have a positive rest + offset, the shapes will be separated at rest by an air gap. + """ + + +@configclass +class DeformableBodyPropertiesCfg( + OmniPhysicsPropertiesCfg, PhysXDeformableBodyPropertiesCfg, PhysXCollisionPropertiesCfg +): + """Properties to apply to a deformable body. + + A deformable body is a body that can deform under forces, both surface and volume deformables. + The configuration allows users to specify the properties of the deformable body, + such as the solver iteration counts, damping, and self-collision. + + An FEM-based deformable body is created by providing a collision mesh and simulation mesh. The collision mesh + is used for collision detection and the simulation mesh is used for simulation. + + See :meth:`modify_deformable_body_properties` for more information. + + .. note:: + If the values are :obj:`None`, they are not modified. This is useful when you want to set only a subset of + the properties and leave the rest as-is. + """ + + _property_prefix: dict[str, list[str]] = { + "omniphysics": [field.name for field in dataclasses.fields(OmniPhysicsPropertiesCfg)], + "physxDeformableBody": [field.name for field in dataclasses.fields(PhysXDeformableBodyPropertiesCfg)], + "physxCollision": [field.name for field in dataclasses.fields(PhysXCollisionPropertiesCfg)], + } + """Mapping between the property prefixes and the properties that fall under each prefix.""" diff --git a/source/isaaclab/isaaclab/sim/spawners/__init__.pyi b/source/isaaclab/isaaclab/sim/spawners/__init__.pyi index dae1a432b47e..11724f4024fd 100644 --- a/source/isaaclab/isaaclab/sim/spawners/__init__.pyi +++ b/source/isaaclab/isaaclab/sim/spawners/__init__.pyi @@ -6,6 +6,7 @@ __all__ = [ "SpawnerCfg", "RigidObjectSpawnerCfg", + "DeformableObjectSpawnerCfg", "spawn_from_mjcf", "spawn_from_urdf", "spawn_from_usd", @@ -24,8 +25,11 @@ __all__ = [ "LightCfg", "SphereLightCfg", "spawn_rigid_body_material", + "spawn_deformable_body_material", "PhysicsMaterialCfg", "RigidBodyMaterialCfg", + "DeformableBodyMaterialCfg", + "SurfaceDeformableBodyMaterialCfg", "spawn_from_mdl_file", "spawn_preview_surface", "GlassMdlCfg", @@ -67,7 +71,7 @@ __all__ = [ "MultiUsdFileCfg", ] -from .spawner_cfg import SpawnerCfg, RigidObjectSpawnerCfg +from .spawner_cfg import SpawnerCfg, RigidObjectSpawnerCfg, DeformableObjectSpawnerCfg from .from_files import ( spawn_from_mjcf, spawn_from_urdf, @@ -91,8 +95,11 @@ from .lights import ( ) from .materials import ( spawn_rigid_body_material, + spawn_deformable_body_material, PhysicsMaterialCfg, RigidBodyMaterialCfg, + DeformableBodyMaterialCfg, + SurfaceDeformableBodyMaterialCfg, spawn_from_mdl_file, spawn_preview_surface, GlassMdlCfg, diff --git a/source/isaaclab/isaaclab/sim/spawners/from_files/from_files.py b/source/isaaclab/isaaclab/sim/spawners/from_files/from_files.py index be7bd6e7074e..c05cc7a6981c 100644 --- a/source/isaaclab/isaaclab/sim/spawners/from_files/from_files.py +++ b/source/isaaclab/isaaclab/sim/spawners/from_files/from_files.py @@ -13,7 +13,7 @@ # deformables only supported on PhysX backend from isaaclab_physx.sim import schemas as schemas_physx -from isaaclab_physx.sim.spawners.materials import SurfaceDeformableBodyMaterialCfg +from isaaclab.sim.spawners.materials import SurfaceDeformableBodyMaterialCfg from pxr import Gf, Sdf, Usd, UsdGeom diff --git a/source/isaaclab/isaaclab/sim/spawners/materials/__init__.pyi b/source/isaaclab/isaaclab/sim/spawners/materials/__init__.pyi index 93142ddab389..d5c4768e6731 100644 --- a/source/isaaclab/isaaclab/sim/spawners/materials/__init__.pyi +++ b/source/isaaclab/isaaclab/sim/spawners/materials/__init__.pyi @@ -5,8 +5,11 @@ __all__ = [ "spawn_rigid_body_material", + "spawn_deformable_body_material", "PhysicsMaterialCfg", "RigidBodyMaterialCfg", + "DeformableBodyMaterialCfg", + "SurfaceDeformableBodyMaterialCfg", "spawn_from_mdl_file", "spawn_preview_surface", "GlassMdlCfg", @@ -15,10 +18,12 @@ __all__ = [ "VisualMaterialCfg", ] -from .physics_materials import spawn_rigid_body_material +from .physics_materials import spawn_rigid_body_material, spawn_deformable_body_material from .physics_materials_cfg import ( PhysicsMaterialCfg, RigidBodyMaterialCfg, + DeformableBodyMaterialCfg, + SurfaceDeformableBodyMaterialCfg, ) from .visual_materials import spawn_from_mdl_file, spawn_preview_surface from .visual_materials_cfg import GlassMdlCfg, MdlFileCfg, PreviewSurfaceCfg, VisualMaterialCfg diff --git a/source/isaaclab/isaaclab/sim/spawners/materials/physics_materials.py b/source/isaaclab/isaaclab/sim/spawners/materials/physics_materials.py index 240a0ff00746..d8786b5bfa17 100644 --- a/source/isaaclab/isaaclab/sim/spawners/materials/physics_materials.py +++ b/source/isaaclab/isaaclab/sim/spawners/materials/physics_materials.py @@ -5,16 +5,13 @@ from __future__ import annotations -from typing import TYPE_CHECKING - from pxr import Usd, UsdPhysics, UsdShade from isaaclab.sim.utils import clone, safe_set_attribute_on_usd_prim, safe_set_attribute_on_usd_schema from isaaclab.sim.utils.stage import get_current_stage from isaaclab.utils.string import to_camel_case -if TYPE_CHECKING: - from . import physics_materials_cfg +from . import physics_materials_cfg @clone @@ -74,3 +71,69 @@ def spawn_rigid_body_material(prim_path: str, cfg: physics_materials_cfg.RigidBo safe_set_attribute_on_usd_prim(prim, f"physxMaterial:{to_camel_case(attr_name, 'cC')}", value, camel_case=False) # return the prim return prim + + +@clone +def spawn_deformable_body_material(prim_path: str, cfg: physics_materials_cfg.DeformableBodyMaterialCfg) -> Usd.Prim: + """Create material with deformable-body physics properties. + + Deformable body materials are used to define the physical properties to meshes of a deformable body. These + include the friction and deformable body properties. For more information on deformable body material, + please refer to the documentation on `PxFEMSoftBodyMaterial`_. + + .. note:: + This function is decorated with :func:`clone` that resolves prim path into list of paths + if the input prim path is a regex pattern. This is done to support spawning multiple assets + from a single and cloning the USD prim at the given path expression. + + Args: + prim_path: The prim path or pattern to spawn the asset at. If the prim path is a regex pattern, + then the asset is spawned at all the matching prim paths. + cfg: The configuration for the physics material. + + Returns: + The spawned deformable body material prim. + + Raises: + ValueError: When a prim already exists at the specified prim path and is not a material. + + .. _PxFEMSoftBodyMaterial: https://nvidia-omniverse.github.io/PhysX/physx/5.4.1/_api_build/structPxFEMSoftBodyMaterialModel.html + """ + # get stage handle + stage = get_current_stage() + + # create material prim if no prim exists + if not stage.GetPrimAtPath(prim_path).IsValid(): + _ = UsdShade.Material.Define(stage, prim_path) + + # obtain prim + prim = stage.GetPrimAtPath(prim_path) + # check if prim is a material + if not prim.IsA(UsdShade.Material): + raise ValueError(f"A prim already exists at path: '{prim_path}' but is not a material.") + # ensure PhysX deformable body material API is applied + applied = prim.GetAppliedSchemas() + if "OmniPhysicsDeformableMaterialAPI" not in applied: + prim.AddAppliedSchema("OmniPhysicsDeformableMaterialAPI") + if "PhysxDeformableMaterialAPI" not in applied: + prim.AddAppliedSchema("PhysxDeformableMaterialAPI") + # surface deformable material API + is_surface_deformable = isinstance(cfg, physics_materials_cfg.SurfaceDeformableBodyMaterialCfg) + if is_surface_deformable: + if "OmniPhysicsSurfaceDeformableMaterialAPI" not in applied: + prim.AddAppliedSchema("OmniPhysicsSurfaceDeformableMaterialAPI") + if "PhysxSurfaceDeformableMaterialAPI" not in applied: + prim.AddAppliedSchema("PhysxSurfaceDeformableMaterialAPI") + + # convert to dict + cfg = cfg.to_dict() + del cfg["func"] + # set into PhysX API, gather prefixes for each attribute + property_prefixes = cfg["_property_prefix"] + for prefix, attr_list in property_prefixes.items(): + for attr_name in attr_list: + safe_set_attribute_on_usd_prim( + prim, f"{prefix}:{to_camel_case(attr_name, 'cC')}", cfg[attr_name], camel_case=False + ) + # return the prim + return prim diff --git a/source/isaaclab/isaaclab/sim/spawners/materials/physics_materials_cfg.py b/source/isaaclab/isaaclab/sim/spawners/materials/physics_materials_cfg.py index dde9aec6d905..2dea999fd073 100644 --- a/source/isaaclab/isaaclab/sim/spawners/materials/physics_materials_cfg.py +++ b/source/isaaclab/isaaclab/sim/spawners/materials/physics_materials_cfg.py @@ -5,6 +5,7 @@ from __future__ import annotations +import dataclasses from collections.abc import Callable from dataclasses import MISSING from typing import Literal @@ -77,3 +78,157 @@ class RigidBodyMaterialCfg(PhysicsMaterialCfg): Irrelevant if compliant contacts are disabled when :obj:`compliant_contact_stiffness` is set to zero and rigid contacts are active. """ + + +@configclass +class OmniPhysicsDeformableMaterialCfg: + """OmniPhysics material properties for a deformable body. + + These properties are set with the prefix ``omniphysics:``. For example, to set the density of the + deformable body, you would set the property ``omniphysics:density``. + + See the OmniPhysics documentation for more information on the available properties. + """ + + density: float = 1000.0 + """The material density in [kg/m^3]. Defaults to 1000.0 kg/m^3, which is the density of water.""" + + static_friction: float = 0.25 + """The static friction. Defaults to 0.25.""" + + dynamic_friction: float = 0.25 + """The dynamic friction. Defaults to 0.25.""" + + youngs_modulus: float = 1000000.0 + """The Young's modulus, which defines the body's stiffness. Defaults to 1[MPa]. + + The Young's modulus is a measure of the material's ability to deform under stress. It is measured in Pascals ([Pa]). + """ + + poissons_ratio: float = 0.45 + """The Poisson's ratio which defines the body's volume preservation. Defaults to 0.45. + + The Poisson's ratio is a measure of the material's ability to expand in the lateral direction when compressed + in the axial direction. It is a dimensionless number between 0 and 0.5. Using a value of 0.5 will make the + material incompressible. + """ + + +@configclass +class OmniPhysicsSurfaceDeformableMaterialCfg(OmniPhysicsDeformableMaterialCfg): + """OmniPhysics material properties for a surface deformable body, + extending on :class:`OmniPhysicsDeformableMaterialCfg` with additional parameters for surface deformable bodies. + + These properties are set with the prefix ``omniphysics:``. + For example, to set the surface thickness of the surface deformable body, + you would set the property ``omniphysics:surfaceThickness``. + + See the OmniPhysics documentation for more information on the available properties. + """ + + surface_thickness: float = 0.01 + """The thickness of the deformable body's surface. Defaults to 0.01 meters ([m]).""" + + surface_stretch_stiffness: float = 0.0 + """The stretch stiffness of the deformable body's surface. Defaults to 0.0.""" + + surface_shear_stiffness: float = 0.0 + """The shear stiffness of the deformable body's surface. Defaults to 0.0.""" + + surface_bend_stiffness: float = 0.0 + """The bend stiffness of the deformable body's surface. Defaults to 0.0.""" + + bend_damping: float = 0.0 + """The bend damping for the deformable body's surface. Defaults to 0.0.""" + + +@configclass +class PhysXDeformableMaterialCfg: + """PhysX-specific material properties for a deformable body. + + These properties are set with the prefix ``physxDeformableBody:``. + For example, to set the elasticity damping of the deformable body, + you would set the property ``physxDeformableBody:elasticityDamping``. + + See the PhysX documentation for more information on the available properties. + """ + + elasticity_damping: float = 0.005 + """The elasticity damping for the deformable material. Defaults to 0.005.""" + + +@configclass +class NewtonDeformableMaterialCfg: + """Newton-specific material properties for a deformable body. + + These properties are set with the prefix ``newton:``. + For example, to set the elasticity damping of the deformable body, + you would set the property ``newton:tri_ke``. + + See the Newton documentation for more information on the available properties. + """ + + particle_radius: float = 0.008 + """Particle radius [m] (controls rigid body-particle contact distance). Used by Newton backend only.""" + + # -- Cloth (triangle surface mesh) parameters + + tri_ke: float = 1e4 + """Triangle area-preserving stiffness [Pa]. Used by Newton backend for cloth meshes.""" + + tri_ka: float = 1e4 + """Triangle area stiffness [Pa]. Used by Newton backend for cloth meshes.""" + + tri_kd: float = 1.5e-6 + """Triangle area damping. Used by Newton backend for cloth meshes.""" + + edge_ke: float = 5.0 + """Bending stiffness. Used by Newton backend for cloth meshes.""" + + edge_kd: float = 1e-2 + """Bending damping. Used by Newton backend for cloth meshes.""" + + # -- Volumetric (tetrahedral FEM) parameters + + k_damp: float = 0.0 + """Damping stiffness for tetrahedral elements. Defaults to 0.0.""" + + +@configclass +class DeformableBodyMaterialCfg( + PhysicsMaterialCfg, + OmniPhysicsDeformableMaterialCfg, + PhysXDeformableMaterialCfg, + NewtonDeformableMaterialCfg, +): + """Physics material parameters for deformable bodies. + + See :meth:`spawn_deformable_body_material` for more information. + """ + + func: Callable | str = "{DIR}.physics_materials:spawn_deformable_body_material" + + _property_prefix: dict[str, list[str]] = { + "omniphysics": [field.name for field in dataclasses.fields(OmniPhysicsDeformableMaterialCfg)], + "physxDeformableBody": [field.name for field in dataclasses.fields(PhysXDeformableMaterialCfg)], + "newton": [field.name for field in dataclasses.fields(NewtonDeformableMaterialCfg)], + } + """Mapping between the property prefixes and the properties that fall under each prefix.""" + + +@configclass +class SurfaceDeformableBodyMaterialCfg(DeformableBodyMaterialCfg, OmniPhysicsSurfaceDeformableMaterialCfg): + """Physics material parameters for surface deformable bodies, + extending on :class:`DeformableBodyMaterialCfg` with additional parameters for surface deformable bodies. + + See :meth:`spawn_deformable_body_material` for more information. + """ + + func: Callable | str = "{DIR}.physics_materials:spawn_deformable_body_material" + + _property_prefix: dict[str, list[str]] = { + "omniphysics": [field.name for field in dataclasses.fields(OmniPhysicsSurfaceDeformableMaterialCfg)], + "physxDeformableBody": [field.name for field in dataclasses.fields(PhysXDeformableMaterialCfg)], + "newton": [field.name for field in dataclasses.fields(NewtonDeformableMaterialCfg)], + } + """Extend DeformableBodyMaterialCfg properties under each prefix.""" diff --git a/source/isaaclab/isaaclab/sim/spawners/meshes/meshes.py b/source/isaaclab/isaaclab/sim/spawners/meshes/meshes.py index 75ead1c8fb67..27cbc0070ab4 100644 --- a/source/isaaclab/isaaclab/sim/spawners/meshes/meshes.py +++ b/source/isaaclab/isaaclab/sim/spawners/meshes/meshes.py @@ -11,16 +11,12 @@ import trimesh import trimesh.transformations -# deformables only supported on PhysX backend -from isaaclab_physx.sim import schemas as schemas_physx -from isaaclab_physx.sim.spawners.materials import DeformableBodyMaterialCfg, SurfaceDeformableBodyMaterialCfg - from pxr import Usd, UsdPhysics from isaaclab.sim import schemas from isaaclab.sim.utils import bind_physics_material, bind_visual_material, clone, create_prim, get_current_stage -from ..materials import RigidBodyMaterialCfg +from ..materials import DeformableBodyMaterialCfg, RigidBodyMaterialCfg, SurfaceDeformableBodyMaterialCfg if TYPE_CHECKING: from . import meshes_cfg @@ -394,7 +390,7 @@ def _spawn_mesh_geom_from_mesh( if cfg.deformable_props is not None: # apply deformable body properties deformable_type = "surface" if isinstance(cfg.physics_material, SurfaceDeformableBodyMaterialCfg) else "volume" - schemas_physx.define_deformable_body_properties( + schemas.define_deformable_body_properties( prim_path, cfg.deformable_props, stage=stage, deformable_type=deformable_type ) if cfg.mass_props is not None: diff --git a/source/isaaclab/isaaclab/sim/spawners/meshes/meshes_cfg.py b/source/isaaclab/isaaclab/sim/spawners/meshes/meshes_cfg.py index a9e7e44586d0..18f7a5b7fc13 100644 --- a/source/isaaclab/isaaclab/sim/spawners/meshes/meshes_cfg.py +++ b/source/isaaclab/isaaclab/sim/spawners/meshes/meshes_cfg.py @@ -9,11 +9,8 @@ from dataclasses import MISSING from typing import Literal -# deformables only supported on PhysX backend -from isaaclab_physx.sim.spawners.spawner_cfg import DeformableObjectSpawnerCfg - from isaaclab.sim.spawners import materials -from isaaclab.sim.spawners.spawner_cfg import RigidObjectSpawnerCfg +from isaaclab.sim.spawners.spawner_cfg import DeformableObjectSpawnerCfg, RigidObjectSpawnerCfg from isaaclab.utils import configclass diff --git a/source/isaaclab/isaaclab/sim/spawners/spawner_cfg.py b/source/isaaclab/isaaclab/sim/spawners/spawner_cfg.py index 7a803ad0e0dd..ea82664e157d 100644 --- a/source/isaaclab/isaaclab/sim/spawners/spawner_cfg.py +++ b/source/isaaclab/isaaclab/sim/spawners/spawner_cfg.py @@ -100,3 +100,25 @@ class RigidObjectSpawnerCfg(SpawnerCfg): This adds the PhysxContactReporter API to all the rigid bodies in the given prim path and its children. """ + + +@configclass +class DeformableObjectSpawnerCfg(SpawnerCfg): + """Configuration parameters for spawning a deformable asset. + + Unlike rigid objects, deformable objects are affected by forces and can deform when subjected to + external forces. This class is used to configure the properties of the deformable object. + + Deformable bodies don't have a separate collision mesh. The collision mesh is the same as the visual mesh. + The collision properties such as rest and collision offsets are specified in the :attr:`deformable_props`. + + Note: + By default, all properties are set to None. This means that no properties will be added or modified + to the prim outside of the properties available by default when spawning the prim. + """ + + mass_props: schemas.MassPropertiesCfg | None = None + """Mass properties.""" + + deformable_props: schemas.DeformableBodyPropertiesCfg | None = None + """Deformable body properties. Only supported on PhysX backend for now.""" diff --git a/source/isaaclab/isaaclab/sim/utils/prims.py b/source/isaaclab/isaaclab/sim/utils/prims.py index d54ca890d2b7..c8395530e6d3 100644 --- a/source/isaaclab/isaaclab/sim/utils/prims.py +++ b/source/isaaclab/isaaclab/sim/utils/prims.py @@ -910,7 +910,25 @@ def add_usd_reference( def _add_reference_to_prim(prim: Usd.Prim) -> Usd.Prim: """Helper function to add a reference to a prim.""" - success_bool = prim.GetReferences().AddReference(usd_path) + # ``spawn_from_usd`` documents that "if a default prim is not specified, + # then the asset is spawned at the root prim." USD's AddReference() + # without an explicit prim path uses defaultPrim; when that is missing + # the reference silently composes nothing. We fall back to the first + # child of the pseudo-root so the referenced content actually appears. + ref_stage = Usd.Stage.Open(usd_path) + default_prim = ref_stage.GetDefaultPrim() if ref_stage else None + if default_prim and default_prim.IsValid(): + success_bool = prim.GetReferences().AddReference(usd_path) + else: + # Find the first child of the pseudo-root to use as reference target + ref_prim_path = None + if ref_stage: + for child in ref_stage.GetPseudoRoot().GetChildren(): + ref_prim_path = child.GetPath().pathString + break + if ref_prim_path is None: + raise RuntimeError(f"USD file at '{usd_path}' has no defaultPrim and no root-level prims to reference.") + success_bool = prim.GetReferences().AddReference(usd_path, ref_prim_path) if not success_bool: raise RuntimeError( f"Unable to add USD reference to the prim at path: {prim_path} from the USD file at path: {usd_path}" diff --git a/source/isaaclab_contrib/isaaclab_contrib/deformable/__init__.py b/source/isaaclab_contrib/isaaclab_contrib/deformable/__init__.py new file mode 100644 index 000000000000..ea5b8ef7a5af --- /dev/null +++ b/source/isaaclab_contrib/isaaclab_contrib/deformable/__init__.py @@ -0,0 +1,117 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Experimental Newton deformable module. + +Importing this module makes deformable config classes available immediately. +Heavy runtime imports (NewtonManager, DeformableObject, etc.) and hook +registration are deferred until first access so that ``pxr`` is not loaded +at config-resolution time (before Kit/SimulationApp is started). +""" + +# Lightweight config-only imports -- no pxr dependency +from .newton_manager_cfg import CoupledSolverCfg, NewtonModelCfg, VBDSolverCfg + +# --------------------------------------------------------------------------- +# Deferred heavy imports & hook registration +# --------------------------------------------------------------------------- +_hooks_registered = False + +# Names that are deferred and resolved via __getattr__ +_DEFERRED_NAMES = { + "NewtonManager", + "DeformableObject", + "DeformableRegistryEntry", + "DeformableObjectData", + "CoupledSolver", + "per_world_deformable_hook", + "post_replicate_deformable_hook", + "apply_model_cfg", + "sync_particles_to_usd", + "create_coupled_solver", + "create_vbd_solver", + "register_hooks", +} + + +def _do_deferred_imports(): + """Import all heavy symbols and register hooks. + + Called once on first access to any deferred name. + """ + global _hooks_registered + + from isaaclab_newton.physics import NewtonManager + + from .cloner_hooks import per_world_deformable_hook, post_replicate_deformable_hook + from .coupled_solver import CoupledSolver + from .deformable_object import DeformableObject, DeformableRegistryEntry + from .deformable_object_data import DeformableObjectData + from .model_cfg_hook import apply_model_cfg + from .particle_sync import setup_fabric_particle_sync, sync_particles_to_usd + from .solver_factories import create_coupled_solver, create_vbd_solver + + # Inject into module namespace so subsequent accesses are direct + g = globals() + g["NewtonManager"] = NewtonManager + g["DeformableObject"] = DeformableObject + g["DeformableRegistryEntry"] = DeformableRegistryEntry + g["DeformableObjectData"] = DeformableObjectData + g["CoupledSolver"] = CoupledSolver + g["per_world_deformable_hook"] = per_world_deformable_hook + g["post_replicate_deformable_hook"] = post_replicate_deformable_hook + g["apply_model_cfg"] = apply_model_cfg + g["sync_particles_to_usd"] = sync_particles_to_usd + g["setup_fabric_particle_sync"] = setup_fabric_particle_sync + g["create_coupled_solver"] = create_coupled_solver + g["create_vbd_solver"] = create_vbd_solver + + # Register hooks with NewtonManager + if not _hooks_registered: + _hooks_registered = True + _register_hooks_impl(NewtonManager, create_vbd_solver, create_coupled_solver, + sync_particles_to_usd, setup_fabric_particle_sync, + per_world_deformable_hook, + post_replicate_deformable_hook, apply_model_cfg) + + # Register the Newton DeformableObject backend with the factory + from isaaclab.assets.deformable_object.deformable_object import DeformableObject as DeformableObjectFactory + DeformableObjectFactory.register("newton", DeformableObject) + + +def _register_hooks_impl(NewtonManager, create_vbd_solver, create_coupled_solver, + sync_particles_to_usd, setup_fabric_particle_sync, + per_world_deformable_hook, + post_replicate_deformable_hook, apply_model_cfg): + """Register all deformable hooks with NewtonManager.""" + NewtonManager.register_solver_factory("vbd", create_vbd_solver) + NewtonManager.register_solver_factory("coupled", create_coupled_solver) + NewtonManager._particle_sync_fn = sync_particles_to_usd + NewtonManager._post_start_simulation_fn = setup_fabric_particle_sync + if per_world_deformable_hook not in NewtonManager._per_world_builder_hooks: + NewtonManager._per_world_builder_hooks.append(per_world_deformable_hook) + if post_replicate_deformable_hook not in NewtonManager._post_replicate_hooks: + NewtonManager._post_replicate_hooks.append(post_replicate_deformable_hook) + NewtonManager._post_finalize_model_fn = apply_model_cfg + + +def register_hooks() -> None: + """Register all deformable hooks with :class:`NewtonManager`. + + This is called automatically on first access to any deferred symbol + and can be called again after :meth:`NewtonManager.clear` to + re-register hooks that were wiped. + """ + global _hooks_registered + # Force re-registration so hooks survive NewtonManager.clear() + _hooks_registered = False + _do_deferred_imports() + + +def __getattr__(name: str): + if name in _DEFERRED_NAMES: + _do_deferred_imports() + return globals()[name] + raise AttributeError(f"module {__name__!r} has no attribute {name!r}") diff --git a/source/isaaclab_contrib/isaaclab_contrib/deformable/cloner_hooks.py b/source/isaaclab_contrib/isaaclab_contrib/deformable/cloner_hooks.py new file mode 100644 index 000000000000..20848ecfb83f --- /dev/null +++ b/source/isaaclab_contrib/isaaclab_contrib/deformable/cloner_hooks.py @@ -0,0 +1,40 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Cloner hooks for registering deformable bodies during Newton world replication.""" + +from __future__ import annotations + +from newton import ModelBuilder + +from .deformable_object import add_deformable_entry_to_builder + + +def per_world_deformable_hook(builder: ModelBuilder, world_idx: int, env_position: list[float]) -> None: + """Per-world builder hook: add all deformable bodies from the registry. + + Args: + builder: The Newton model builder. + world_idx: The world/environment index. + env_position: World position [x, y, z] for this environment. + """ + from isaaclab_newton.physics import NewtonManager + + for entry in NewtonManager._deformable_registry: + add_deformable_entry_to_builder(builder, entry, world_idx, env_position) + + +def post_replicate_deformable_hook(builder: ModelBuilder) -> None: + """Post-replicate hook: call ``builder.color()`` if deformable bodies are present. + + Required by the VBD solver for parallel vertex colouring. + + Args: + builder: The Newton model builder. + """ + from isaaclab_newton.physics import NewtonManager + + if NewtonManager._deformable_registry: + builder.color() diff --git a/source/isaaclab_contrib/isaaclab_contrib/deformable/coupled_solver.py b/source/isaaclab_contrib/isaaclab_contrib/deformable/coupled_solver.py new file mode 100644 index 000000000000..b8e580351cba --- /dev/null +++ b/source/isaaclab_contrib/isaaclab_contrib/deformable/coupled_solver.py @@ -0,0 +1,346 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Coupled solver that alternates a rigid-body solver and VBD (cloth) per substep. + +Supports two coupling modes (selected via :attr:`CoupledSolverCfg.coupling_mode`): + +- ``"one_way"`` (default): Rigid solver advances first, then VBD reads the updated + body poses for cloth-body contacts. The rigid solver does not feel particle contact forces. +- ``"two_way"``: Same-substep two-way coupling with normal + Coulomb friction forces. + Contact detection runs first, reaction forces (normal and tangential friction) are + injected into ``body_f``, then the rigid solver reads ``body_f`` and feels resistance + from the deformable object. The friction reaction provides the force needed for the + actuators to carry the object against gravity during a lift. + +The rigid solver can be either :class:`SolverFeatherstone` or :class:`SolverMuJoCo`. +""" + +from __future__ import annotations + +import inspect +import logging +from typing import TYPE_CHECKING, Literal + +import warp as wp +from newton import CollisionPipeline, Contacts, Control, Model, State +from newton.solvers import SolverFeatherstone, SolverMuJoCo, SolverVBD + +if TYPE_CHECKING: + from .newton_manager_cfg import CoupledSolverCfg + +logger = logging.getLogger(__name__) + +# Fixed upper bound on contact slots for the reaction kernel. The kernel is +# launched with this many threads; threads beyond the actual contact count +# early-exit immediately so over-allocating is cheap. +_MAX_REACTION_CONTACTS: int = 2048 + + +# --------------------------------------------------------------------------- +# Warp kernels for two-way coupling +# --------------------------------------------------------------------------- + + +@wp.kernel +def _kernel_body_particle_reaction( + contact_count: wp.array(dtype=wp.int32), + contact_particle: wp.array(dtype=wp.int32), + contact_shape: wp.array(dtype=wp.int32), + contact_body_pos: wp.array(dtype=wp.vec3), + contact_body_vel: wp.array(dtype=wp.vec3), + contact_normal: wp.array(dtype=wp.vec3), + particle_q: wp.array(dtype=wp.vec3), + particle_q_prev: wp.array(dtype=wp.vec3), + particle_radius: wp.array(dtype=wp.float32), + body_q: wp.array(dtype=wp.transform), + body_qd: wp.array(dtype=wp.spatial_vector), + body_com: wp.array(dtype=wp.vec3), + shape_body: wp.array(dtype=wp.int32), + shape_material_mu: wp.array(dtype=wp.float32), + soft_contact_ke: float, + soft_contact_mu: float, + friction_epsilon: float, + dt: float, + body_f: wp.array(dtype=wp.spatial_vector), +): + """Newton's-third-law reaction (normal + Coulomb friction) from soft particles onto rigid bodies. + + Mirrors the complete contact model from ``evaluate_body_particle_contact()`` + in ``newton/_src/solvers/vbd/rigid_vbd_kernels.py``. One thread per contact + slot; threads beyond the actual contact count early-exit. + """ + tid = wp.tid() + if tid >= contact_count[0]: + return + + p_idx = contact_particle[tid] + s_idx = contact_shape[tid] + body_idx = shape_body[s_idx] + if body_idx < 0: + return + + X_wb = body_q[body_idx] + bx = wp.transform_point(X_wb, contact_body_pos[tid]) + n = contact_normal[tid] + + penetration = -(wp.dot(n, particle_q[p_idx] - bx) - particle_radius[p_idx]) + if penetration <= 0.0: + return + + normal_load = soft_contact_ke * penetration + f_on_particle = n * normal_load + + com_w = wp.transform_point(X_wb, body_com[body_idx]) + mu = wp.sqrt(soft_contact_mu * shape_material_mu[s_idx]) + + if mu > 0.0: + body_v_s = body_qd[body_idx] + body_lin_v = wp.spatial_top(body_v_s) + body_ang_v = wp.spatial_bottom(body_v_s) + r = bx - com_w + bv = body_lin_v + wp.cross(body_ang_v, r) + wp.transform_vector(X_wb, contact_body_vel[tid]) + + dx = particle_q[p_idx] - particle_q_prev[p_idx] + relative_translation = dx - bv * dt + + dot_nu = wp.dot(n, relative_translation) + u_t = relative_translation - n * dot_nu + u_norm = wp.length(u_t) + eps_u = friction_epsilon * dt + + if u_norm > 0.0: + if u_norm > eps_u: + f1_over_x = 1.0 / u_norm + else: + f1_over_x = (-u_norm / eps_u + 2.0) / eps_u + f_on_particle = f_on_particle - (mu * normal_load * f1_over_x) * u_t + + reaction = -f_on_particle + torque = wp.cross(bx - com_w, reaction) + + wp.atomic_add( + body_f, + body_idx, + wp.spatial_vector( + reaction[0], + reaction[1], + reaction[2], + torque[0], + torque[1], + torque[2], + ), + ) + + +# --------------------------------------------------------------------------- +# CoupledSolver +# --------------------------------------------------------------------------- + +CouplingMode = Literal["one_way", "two_way"] + + +class CoupledSolver: + """Coupled rigid-body + VBD solver for rigid-body/cloth interaction. + + Supports two coupling modes: + + **one_way** (default): + + 1. Clear forces. + 2. Rigid step (Featherstone or MuJoCo). + 3. Collision detection. + 4. VBD step (particles only). + + **two_way** (same-substep two-way coupling with normal + friction): + + 1. Clear forces. + 2. Collision detection. + 3. Inject contact reaction forces (normal + Coulomb friction) into ``body_f``. + 4. Rigid step (reads ``body_f`` -- fingers feel resistance). + 5. VBD step (uses same contacts). + """ + + def __init__( + self, + model: Model, + cfg: CoupledSolverCfg, + collision_pipeline: CollisionPipeline, + contacts: Contacts, + ): + """Initialize the coupled solver. + + Args: + model: The Newton model. + cfg: Coupled solver configuration containing rigid solver, VBD, + and coupling mode settings. + collision_pipeline: Collision pipeline for cloth-body contacts. + contacts: Contacts buffer for the collision pipeline. + """ + self._model = model + self._coupling_mode = cfg.coupling_mode + + # --- Build rigid solver from config --- + rigid_solver_cfg = cfg.rigid_solver_cfg + if hasattr(rigid_solver_cfg, "to_dict"): + rigid_solver_cfg = rigid_solver_cfg.to_dict() + rigid_solver_type = rigid_solver_cfg.get("solver_type", "mujoco_warp") + self._rigid_solver_type = rigid_solver_type + self._is_featherstone = rigid_solver_type == "featherstone" + + if rigid_solver_type == "mujoco_warp": + valid_keys = set(inspect.signature(SolverMuJoCo.__init__).parameters) - {"self", "model"} + rigid_kwargs = {k: v for k, v in rigid_solver_cfg.items() if k in valid_keys} + logger.info("Coupled: Creating SolverMuJoCo with args: %s", rigid_kwargs) + self.rigid_solver = SolverMuJoCo(model, **rigid_kwargs) + elif rigid_solver_type == "featherstone": + valid_keys = set(inspect.signature(SolverFeatherstone.__init__).parameters) - {"self", "model"} + rigid_kwargs = {k: v for k, v in rigid_solver_cfg.items() if k in valid_keys} + logger.info("Coupled: Creating SolverFeatherstone with args: %s", rigid_kwargs) + self.rigid_solver = SolverFeatherstone(model, **rigid_kwargs) + else: + raise ValueError(f"Unsupported rigid solver type for coupled solver: {rigid_solver_type}") + + # --- Build VBD solver from config --- + vbd_cfg = cfg.vbd_cfg + if hasattr(vbd_cfg, "to_dict"): + vbd_cfg = vbd_cfg.to_dict() + valid_keys = set(inspect.signature(SolverVBD.__init__).parameters) - {"self", "model"} + vbd_kwargs = {k: v for k, v in vbd_cfg.items() if k in valid_keys} + vbd_kwargs["integrate_with_external_rigid_solver"] = True + self.vbd = SolverVBD(model, **vbd_kwargs) + + # Collision pipeline and contacts buffer (owned by this solver) + self.collision_pipeline = collision_pipeline + self.contacts = contacts + + logger.info( + "CoupledSolver initialized: %s + VBD(%s), coupling_mode=%s", + rigid_solver_type, + {k: v for k, v in vbd_kwargs.items() if k != "integrate_with_external_rigid_solver"}, + cfg.coupling_mode, + ) + + def rebuild_bvh(self, state: State) -> None: + """Rebuild BVH for VBD collision detection.""" + self.vbd.rebuild_bvh(state) + + def step( + self, + state_in: State, + state_out: State, + control: Control, + contacts: Contacts | None, + dt: float, + ) -> None: + """One coupled substep. + + Args: + state_in: Current state (read/write). + state_out: Next state (write). + control: Joint-level control inputs. + contacts: Ignored -- the solver uses its own internal contacts. + dt: Substep timestep [s]. + """ + if self._coupling_mode == "one_way": + self._step_one_way(state_in, state_out, control, dt) + else: + self._step_two_way(state_in, state_out, control, dt) + + def _step_one_way(self, state_in: State, state_out: State, control: Control, dt: float) -> None: + """One-way coupling: rigid step, then collide, then VBD.""" + # 1. Clear forces + state_in.clear_forces() + state_out.clear_forces() + + # 2. Rigid-body step + self._rigid_step(state_in, state_out, control, dt) + + # 3. Clear spurious particle forces from rigid step + state_in.particle_f.zero_() + + # 4. Collision detection (cloth-body contacts) + self.collision_pipeline.collide(state_in, self.contacts) + + # 5. VBD step -- particles only, reads updated rigid poses + self.vbd.step(state_in, state_out, control, self.contacts, dt) + + def _step_two_way(self, state_in: State, state_out: State, control: Control, dt: float) -> None: + """Two-way coupling: collide, inject reactions into body_f, rigid step, VBD step.""" + # 1. Clear forces + state_in.clear_forces() + state_out.clear_forces() + + # 2. Collision detection BEFORE rigid step + self.collision_pipeline.collide(state_in, self.contacts) + + # 3. Inject contact reaction forces into body_f + if state_in.body_f is not None: + self._apply_reactions(state_in, dt) + + # 4. Rigid-body step (reads body_f for soft-contact reactions) + self._rigid_step(state_in, state_out, control, dt) + + # 5. Clear spurious particle forces from rigid step + state_in.particle_f.zero_() + + # 6. VBD step -- uses same contacts detected in step 2 + self.vbd.step(state_in, state_out, control, self.contacts, dt) + + def _rigid_step(self, state_in: State, state_out: State, control: Control, dt: float) -> None: + """Advance rigid bodies with the configured sub-solver.""" + model = self._model + + saved_particle_count = model.particle_count + model.particle_count = 0 + + self.rigid_solver.step(state_in, state_out, control, None, dt) + + model.particle_count = saved_particle_count + + def _apply_reactions(self, state: State, dt: float) -> None: + """Launch the reaction kernel to inject normal + friction forces into body_f. + + .. note:: + ``particle_q_prev`` receives the same array as ``particle_q``, so the + friction term's relative displacement (``dx = q - q_prev``) is always + zero. Friction is therefore computed from body velocity only. Maintaining + a separate ``q_prev`` snapshot would give physically correct Coulomb + friction but requires an extra buffer copy per substep. Acceptable for + now since one-way coupling is the primary use case. + """ + model = self._model + contacts = self.contacts + + wp.launch( + _kernel_body_particle_reaction, + dim=_MAX_REACTION_CONTACTS, + inputs=[ + contacts.soft_contact_count, + contacts.soft_contact_particle, + contacts.soft_contact_shape, + contacts.soft_contact_body_pos, + contacts.soft_contact_body_vel, + contacts.soft_contact_normal, + state.particle_q, + state.particle_q, + model.particle_radius, + state.body_q, + state.body_qd, + model.body_com, + model.shape_body, + model.shape_material_mu, + float(model.soft_contact_ke), + float(model.soft_contact_mu), + float(self.vbd.friction_epsilon), + float(dt), + state.body_f, + ], + ) + + def notify_model_changed(self, change: int) -> None: + """Forward model-change notifications to both sub-solvers.""" + self.rigid_solver.notify_model_changed(change) + self.vbd.notify_model_changed(change) diff --git a/source/isaaclab_contrib/isaaclab_contrib/deformable/deformable_object.py b/source/isaaclab_contrib/isaaclab_contrib/deformable/deformable_object.py new file mode 100644 index 000000000000..e12bbd7fad85 --- /dev/null +++ b/source/isaaclab_contrib/isaaclab_contrib/deformable/deformable_object.py @@ -0,0 +1,733 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import logging +from collections.abc import Sequence +from dataclasses import dataclass, field +from typing import TYPE_CHECKING + +import numpy as np +import torch +import warp as wp +from isaaclab_newton.physics import NewtonManager as SimulationManager + +import isaaclab.sim as sim_utils +from isaaclab.assets.deformable_object.base_deformable_object import BaseDeformableObject +from isaaclab.markers import VisualizationMarkers +from isaaclab.physics import PhysicsEvent + +from .deformable_object_data import DeformableObjectData +from .kernels import ( + compute_nodal_state_w, + enforce_kinematic_targets, + scatter_particles_vec3f_index, + set_kinematic_flags_to_one, + vec6f, +) + + +@dataclass +class DeformableRegistryEntry: + """Entry in the deformable body registry. + + Registered by :class:`DeformableObject` during ``__init__``, consumed by + ``newton_physics_replicate`` inside the per-world ``begin_world``/``end_world`` loop. + After replication, ``particle_offsets`` and ``particles_per_body`` are filled in + so the asset can bind to the correct particle ranges. + """ + + prim_path: str + sim_mesh_prim_path: str + vis_mesh_prim_path: str + vertices: list + indices: list + init_pos: tuple[float, float, float] + init_rot: tuple[float, float, float, float] # (x, y, z, w) + deformable_type: str | None = None # "volume" or "surface" + # Cloth params + density: float = 0.02 + tri_ke: float = 1e4 + tri_ka: float = 1e4 + tri_kd: float = 1.5e-6 + edge_ke: float = 5.0 + edge_kd: float = 1e-2 + particle_radius: float = 0.008 + # Tet params + k_mu: float = 1e5 + k_lambda: float = 1e5 + k_damp: float = 0.0 + # Filled by newton_physics_replicate: + particle_offsets: list[int] = field(default_factory=list) + particles_per_body: int = 0 + + +if TYPE_CHECKING: + from isaaclab.assets.deformable_object.deformable_object_cfg import DeformableObjectCfg + +logger = logging.getLogger(__name__) + + +def add_deformable_entry_to_builder( + builder, + entry: DeformableRegistryEntry, + env_idx: int, + env_position: list[float], +) -> None: + """Add a deformable registry entry to a Newton ``ModelBuilder`` for one environment. + + Depending on the deformable type (``"volume"`` or ``"surface"``), calls + ``builder.add_soft_mesh()`` or ``builder.add_cloth_mesh()`` with the mesh + data and material properties stored in the registry entry. + + Also records the particle offset for the instance and, on the first + environment, records the per-body particle count. + + Args: + builder: The Newton ``ModelBuilder``. + entry: A :class:`DeformableRegistryEntry` with mesh data and config. + env_idx: The environment index. + env_position: World position [x, y, z] [m] for this environment. + """ + before_count = getattr(builder, "particle_count", 0) + + body_pos = wp.vec3( + entry.init_pos[0] + env_position[0], + entry.init_pos[1] + env_position[1], + entry.init_pos[2] + env_position[2], + ) + body_rot = wp.quat(entry.init_rot[0], entry.init_rot[1], entry.init_rot[2], entry.init_rot[3]) + + if entry.deformable_type == "volume": + builder.add_soft_mesh( + pos=body_pos, + rot=body_rot, + scale=1.0, + vel=wp.vec3(0.0, 0.0, 0.0), + vertices=entry.vertices, + indices=entry.indices, + density=entry.density, + k_mu=entry.k_mu, + k_lambda=entry.k_lambda, + k_damp=entry.k_damp, + particle_radius=entry.particle_radius, + ) + elif entry.deformable_type == "surface": + builder.add_cloth_mesh( + pos=body_pos, + rot=body_rot, + scale=1.0, + vel=wp.vec3(0.0, 0.0, 0.0), + vertices=entry.vertices, + indices=entry.indices, + density=entry.density, + tri_ke=entry.tri_ke, + tri_ka=entry.tri_ka, + tri_kd=entry.tri_kd, + edge_ke=entry.edge_ke, + edge_kd=entry.edge_kd, + particle_radius=entry.particle_radius, + ) + else: + raise ValueError( + f"Invalid deformable type '{entry.deformable_type}' for registry entry " + f"with prim path '{entry.prim_path}'" + ) + + after_count = getattr(builder, "particle_count", 0) + delta = after_count - before_count + + entry.particle_offsets.append(before_count) + if env_idx == 0: + entry.particles_per_body = delta + + +class DeformableObject(BaseDeformableObject): + """A deformable object asset class (Newton backend). + + This class manages cloth/deformable bodies in the Newton physics engine. Newton stores all + particles in flat arrays (``state.particle_q``, ``state.particle_qd``). This class builds + a per-instance indexing layer on top of those flat arrays, enabling the standard + :class:`BaseDeformableObject` interface for reading/writing nodal state. + + The cloth mesh is added to the Newton :class:`ModelBuilder` during the ``MODEL_INIT`` phase. + The mesh data is read from the USD prim at :attr:`cfg.prim_path`, and cloth simulation + parameters (density, stiffness, etc.) come from :attr:`DeformableObjectCfg`. + """ + + cfg: DeformableObjectCfg + """Configuration instance for the deformable object.""" + + __backend_name__: str = "newton" + """The name of the backend for the deformable object.""" + + def __init__(self, cfg: DeformableObjectCfg): + """Initialize the deformable object. + + Args: + cfg: A configuration instance. + """ + super().__init__(cfg) + + # initialize deformable type to None, should be set to either surface or volume on initialization + self._deformable_type: str | None = None + + # Read mesh from the spawned USD prim and register in the deformable registry. + self._registry_entry = self._register_deformable() + + # Register custom vec6f type for nodal state validation. + self._DTYPE_TO_TORCH_TRAILING_DIMS = {**self._DTYPE_TO_TORCH_TRAILING_DIMS, vec6f: (6,)} + + """ + Properties + """ + + @property + def data(self) -> DeformableObjectData: + return self._data + + @property + def num_instances(self) -> int: + return self._num_instances + + @property + def num_bodies(self) -> int: + """Number of bodies in the asset. + + This is always 1 since each object is a single deformable body. + """ + return 1 + + @property + def max_sim_vertices_per_body(self) -> int: + """The maximum number of simulation mesh vertices per deformable body.""" + return self._particles_per_body + + """ + Operations. + """ + + def reset(self, env_ids: Sequence[int] | None = None, env_mask: wp.array | None = None) -> None: + """Reset the deformable object. + + No-op to match the PhysX deformable object convention. + + Args: + env_ids: Environment indices. If None, then all indices are used. + env_mask: Environment mask. If None, then all the instances are updated. + Shape is (num_instances,). + """ + pass + + def write_data_to_sim(self): + """Apply kinematic targets to the Newton simulation. + + Reads the stored kinematic target buffer and enforces it on particles: + kinematic particles (flag=0) get inv_mass=0, particle_flags=0, target position, + and zero velocity; free particles (flag=1) get their original inv_mass and + particle_flags=1 (ACTIVE) restored. + + Writes to both ``state_0`` and ``state_1`` so kinematic positions survive + the state swaps that happen between substeps. + """ + if self._data.nodal_kinematic_target is None or self._default_particle_inv_mass is None: + return + + model = SimulationManager._model + if model is None: + return + + for state in (SimulationManager._state_0, SimulationManager._state_1): + if state is None: + continue + wp.launch( + enforce_kinematic_targets, + dim=(self._num_instances, self._particles_per_body), + inputs=[ + self._data.nodal_kinematic_target, + self._particle_offsets, + self._default_particle_inv_mass, + ], + outputs=[ + state.particle_q, + state.particle_qd, + model.particle_inv_mass, + model.particle_flags, + ], + device=self.device, + ) + + def update(self, dt: float): + self._data.update(dt) + + """ + Operations - Write to simulation. + """ + + def write_nodal_pos_to_sim_index( + self, + nodal_pos: torch.Tensor | wp.array, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + full_data: bool = False, + ) -> None: + """Set the nodal positions over selected environment indices into the simulation. + + Args: + nodal_pos: Nodal positions in simulation frame [m]. + Shape is (len(env_ids), max_sim_vertices_per_body, 3) + or (num_instances, max_sim_vertices_per_body, 3). + env_ids: Environment indices. If None, then all indices are used. + full_data: Whether to expect full data. Defaults to False. + """ + env_ids = self._resolve_env_ids(env_ids) + if full_data: + self.assert_shape_and_dtype( + nodal_pos, (self.num_instances, self._particles_per_body), wp.vec3f, "nodal_pos" + ) + else: + self.assert_shape_and_dtype(nodal_pos, (env_ids.shape[0], self._particles_per_body), wp.vec3f, "nodal_pos") + if isinstance(nodal_pos, torch.Tensor): + nodal_pos = wp.from_torch(nodal_pos.contiguous(), dtype=wp.vec3f) + + # Scatter into both Newton states + for state in (SimulationManager._state_0, SimulationManager._state_1): + if state is not None and state.particle_q is not None: + wp.launch( + scatter_particles_vec3f_index, + dim=(env_ids.shape[0], self._particles_per_body), + inputs=[nodal_pos, env_ids, self._particle_offsets, full_data], + outputs=[state.particle_q], + device=self.device, + ) + + # Invalidate data caches + self._data._nodal_pos_w.timestamp = -1.0 + self._data._nodal_state_w.timestamp = -1.0 + self._data._root_pos_w.timestamp = -1.0 + + def write_nodal_velocity_to_sim_index( + self, + nodal_vel: torch.Tensor | wp.array, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + full_data: bool = False, + ) -> None: + """Set the nodal velocity over selected environment indices into the simulation. + + Args: + nodal_vel: Nodal velocities in simulation frame [m/s]. + Shape is (len(env_ids), max_sim_vertices_per_body, 3) + or (num_instances, max_sim_vertices_per_body, 3). + env_ids: Environment indices. If None, then all indices are used. + full_data: Whether to expect full data. Defaults to False. + """ + env_ids = self._resolve_env_ids(env_ids) + if full_data: + self.assert_shape_and_dtype( + nodal_vel, (self.num_instances, self._particles_per_body), wp.vec3f, "nodal_vel" + ) + else: + self.assert_shape_and_dtype(nodal_vel, (env_ids.shape[0], self._particles_per_body), wp.vec3f, "nodal_vel") + if isinstance(nodal_vel, torch.Tensor): + nodal_vel = wp.from_torch(nodal_vel.contiguous(), dtype=wp.vec3f) + + # Scatter into both Newton states + for state in (SimulationManager._state_0, SimulationManager._state_1): + if state is not None and state.particle_qd is not None: + wp.launch( + scatter_particles_vec3f_index, + dim=(env_ids.shape[0], self._particles_per_body), + inputs=[nodal_vel, env_ids, self._particle_offsets, full_data], + outputs=[state.particle_qd], + device=self.device, + ) + + # Invalidate data caches + self._data._nodal_vel_w.timestamp = -1.0 + self._data._nodal_state_w.timestamp = -1.0 + self._data._root_vel_w.timestamp = -1.0 + + def write_nodal_kinematic_target_to_sim_index( + self, + targets: torch.Tensor | wp.array, + env_ids: Sequence[int] | torch.Tensor | wp.array | None = None, + full_data: bool = False, + ) -> None: + """Set the kinematic targets of the simulation mesh for the deformable bodies. + + Newton has no native kinematic target API. Instead: + - Kinematic (flag=0.0): set ``particle_inv_mass`` to 0, write target pos, zero vel + - Free (flag=1.0): restore original ``particle_inv_mass`` + + Args: + targets: The kinematic targets comprising of nodal positions and flags [m]. + Shape is (len(env_ids), max_sim_vertices_per_body, 4) + or (num_instances, max_sim_vertices_per_body, 4). + env_ids: Environment indices. If None, then all indices are used. + full_data: Whether to expect full data. Defaults to False. + """ + env_ids = self._resolve_env_ids(env_ids) + if full_data: + self.assert_shape_and_dtype(targets, (self.num_instances, self._particles_per_body), wp.vec4f, "targets") + else: + self.assert_shape_and_dtype(targets, (env_ids.shape[0], self._particles_per_body), wp.vec4f, "targets") + if isinstance(targets, torch.Tensor): + if targets.dim() == 2: + targets = targets.unsqueeze(0) + targets = wp.from_torch(targets.contiguous(), dtype=wp.vec4f) + + # Store kinematic targets in our data buffer + if self._data.nodal_kinematic_target is not None: + targets_torch = wp.to_torch(targets) + buffer_torch = wp.to_torch(self._data.nodal_kinematic_target) + env_ids_torch = wp.to_torch(env_ids).long() + if full_data: + buffer_torch[env_ids_torch] = targets_torch[env_ids_torch] + else: + buffer_torch[env_ids_torch] = targets_torch + + """ + Internal helper. + """ + + def _resolve_env_ids(self, env_ids): + """Resolve environment indices to a warp int32 array.""" + if env_ids is None or (isinstance(env_ids, slice) and env_ids == slice(None)): + return self._ALL_INDICES + elif isinstance(env_ids, list): + return wp.array(env_ids, dtype=wp.int32, device=self.device) + elif isinstance(env_ids, torch.Tensor): + return wp.from_torch(env_ids.to(torch.int32), dtype=wp.int32) + return env_ids + + def _register_deformable(self) -> DeformableRegistryEntry: + """Read mesh from the spawned USD prim and register in NewtonManager's deformable registry. + + Returns: + The registry entry (also stored on NewtonManager._deformable_registry). + + Note: + pxr imports are deferred to this method (not module level) so that + ``resolve_task_config`` can import the env-cfg module before Kit + starts without polluting the ``pxr`` module cache. + """ + from pxr import Gf, UsdGeom, UsdShade + + # Find the first spawned mesh prim in regex path + template_prim = sim_utils.find_first_matching_prim(self.cfg.prim_path) + if template_prim is None: + raise RuntimeError(f"Failed to find prim for expression: '{self.cfg.prim_path}'.") + template_prim_path = template_prim.GetPrimPath() + + # Discover sim / visual mesh prims under the template. + def _is_sim_mesh(prim) -> bool: + return any("DeformableSimAPI" in api for api in prim.GetAppliedSchemas()) + + tet_prims = sim_utils.get_all_matching_child_prims(template_prim_path, lambda p: p.GetTypeName() == "TetMesh") + mesh_prims = sim_utils.get_all_matching_child_prims(template_prim_path, lambda p: p.GetTypeName() == "Mesh") + + # When the spawn config uses a surface deformable material, the PhysX + # spawner may still create a TetMesh proxy (sim_tetmesh) alongside the + # original Mesh. Newton reads the original Mesh directly, so ignore + # the PhysX TetMesh proxy and treat this as a surface deformable. + _is_surface_material = ( + self.cfg.spawn is not None + and getattr(self.cfg.spawn, "physics_material", None) is not None + and "Surface" in type(self.cfg.spawn.physics_material).__name__ + ) + + if len(tet_prims) > 1: + raise ValueError( + f"Found multiple TetMesh prims under '{template_prim_path}': " + f"{[p.GetPrimPath() for p in tet_prims]}." + " Deformable body schema supports only one simulation mesh per asset." + ) + + # Pick simulation and visual mesh prims. + if len(tet_prims) == 1 and not _is_surface_material: + deformable_type = "volume" + mesh_prim = tet_prims[0] + vis_candidates = [p for p in mesh_prims if not _is_sim_mesh(p)] + elif len(mesh_prims) > 0: + deformable_type = "surface" + sim_candidates = [p for p in mesh_prims if _is_sim_mesh(p)] + vis_candidates = [p for p in mesh_prims if not _is_sim_mesh(p)] + if len(sim_candidates) > 1: + raise ValueError( + f"Found multiple simulation Mesh prims under '{template_prim_path}': " + f"{[p.GetPrimPath() for p in sim_candidates]}." + " Deformable body schema supports only one simulation mesh per asset." + ) + if _is_surface_material and vis_candidates: + # Newton reads the original authored Mesh directly. The PhysX + # sim_mesh proxy has no points at scene-construction time, so + # prefer the original mesh when using a surface deformable material. + mesh_prim = vis_candidates[0] + vis_candidates = [] # visual == sim, no separate embedding target + elif sim_candidates: + mesh_prim = sim_candidates[0] + else: + mesh_prim = vis_candidates[0] + vis_candidates = [] # visual == sim, no separate embedding target + else: + raise ValueError( + f"Could not find any surface or volume mesh in '{template_prim_path}'. Please check asset." + ) + + # BUG FIX: vis_mesh_prim fallback for empty vis_candidates. + # When vis_candidates is empty the visual mesh IS the simulation mesh + # (e.g. a plain surface cloth with no separate visual embedding). + vis_mesh_prim = vis_candidates[0] if vis_candidates else mesh_prim + vis_mesh_prim_path = str(vis_mesh_prim.GetPrimPath()) + vis_mesh_prim_path = self.cfg.prim_path + vis_mesh_prim_path[len(template_prim_path.pathString) :] + sim_mesh_prim_path = str(mesh_prim.GetPrimPath()) + sim_mesh_prim_path = self.cfg.prim_path + sim_mesh_prim_path[len(template_prim_path.pathString) :] + logger.info("Registered visual UsdGeom.Mesh at %s.", vis_mesh_prim_path) + + # Bake the template prim's xform directly into the vertex positions. + xform_cache = UsdGeom.XformCache() + mesh_to_parent_frame = ( + xform_cache.GetLocalToWorldTransform(mesh_prim) + * xform_cache.GetLocalToWorldTransform(template_prim.GetParent()).GetInverse() + ) + + def _bake_points(raw_pts) -> list[wp.vec3]: + out = [] + for p in raw_pts: + q = mesh_to_parent_frame.Transform(Gf.Vec3d(float(p[0]), float(p[1]), float(p[2]))) + out.append(wp.vec3(float(q[0]), float(q[1]), float(q[2]))) + return out + + if deformable_type == "volume": + tet_mesh = UsdGeom.TetMesh(mesh_prim) + pts = tet_mesh.GetPointsAttr().Get() + vertices = _bake_points(pts) + raw_tet_indices = tet_mesh.GetTetVertexIndicesAttr().Get() + indices = [] + for vec4i in raw_tet_indices: + indices.extend([int(vec4i[0]), int(vec4i[1]), int(vec4i[2]), int(vec4i[3])]) + logger.info("Registered UsdGeom.TetMesh: %d vertices, %d tetrahedra.", len(pts), len(indices) // 4) + else: # surface + usd_mesh = UsdGeom.Mesh(mesh_prim) + pts = usd_mesh.GetPointsAttr().Get() + vertices = _bake_points(pts) + indices = list(usd_mesh.GetFaceVertexIndicesAttr().Get()) + logger.info("Registered UsdGeom.Mesh: %d vertices.", len(pts)) + + # BUG FIX: init_pos/init_rot are already baked into the vertices by the Xform + # transform above. Setting them to identity prevents add_cloth_mesh/add_soft_mesh + # from applying them a second time. + # Note: add_deformable_entry_to_builder passes init_rot directly to + # wp.quat(x, y, z, w), so identity must be (0, 0, 0, 1) not (1, 0, 0, 0). + init_pos = (0.0, 0.0, 0.0) + init_rot = (0.0, 0.0, 0.0, 1.0) + + # Look up the bound deformable physics material + if not template_prim.HasAPI(UsdShade.MaterialBindingAPI): + raise ValueError( + f"Template prim '{template_prim_path}' must have a UsdShade.MaterialBindingAPI applied" + " with a physics material target that has 'OmniPhysicsDeformableMaterialAPI' applied." + ) + material_targets = UsdShade.MaterialBindingAPI(template_prim).GetDirectBindingRel("physics").GetTargets() + stage = template_prim.GetStage() + material_prim = None + for mat_path in material_targets: + mat_prim = stage.GetPrimAtPath(mat_path) + if "OmniPhysicsDeformableMaterialAPI" in mat_prim.GetAppliedSchemas(): + material_prim = mat_prim + break + if material_prim is None: + raise ValueError( + f"Could not find a physics material with 'OmniPhysicsDeformableMaterialAPI' applied" + f" among the physics material targets of '{template_prim_path}'." + ) + density = material_prim.GetAttribute("omniphysics:density").Get() + youngs_modulus = material_prim.GetAttribute("omniphysics:youngsModulus").Get() + poissons_ratio = material_prim.GetAttribute("omniphysics:poissonsRatio").Get() + # Convert Young's modulus and Poisson's ratio to Lame parameters for Newton + k_mu = youngs_modulus / (2 * (1 + poissons_ratio)) + k_lambda = (youngs_modulus * poissons_ratio) / ((1 + poissons_ratio) * (1 - 2 * poissons_ratio)) + + tri_ke = material_prim.GetAttribute("newton:triKe").Get() + tri_ka = material_prim.GetAttribute("newton:triKa").Get() + tri_kd = material_prim.GetAttribute("newton:triKd").Get() + edge_ke = material_prim.GetAttribute("newton:edgeKe").Get() + edge_kd = material_prim.GetAttribute("newton:edgeKd").Get() + particle_radius = material_prim.GetAttribute("newton:particleRadius").Get() + k_damp = material_prim.GetAttribute("newton:kDamp").Get() + + entry = DeformableRegistryEntry( + prim_path=self.cfg.prim_path, + sim_mesh_prim_path=sim_mesh_prim_path, + vis_mesh_prim_path=vis_mesh_prim_path, + vertices=vertices, + indices=indices, + deformable_type=deformable_type, + init_pos=init_pos, + init_rot=init_rot, + density=density, + tri_ke=tri_ke, + tri_ka=tri_ka, + tri_kd=tri_kd, + edge_ke=edge_ke, + edge_kd=edge_kd, + particle_radius=particle_radius, + k_mu=k_mu, + k_lambda=k_lambda, + k_damp=k_damp, + ) + SimulationManager._deformable_registry.append(entry) + self._deformable_type = deformable_type + return entry + + def _initialize_impl(self): + """Initialize physics handles and buffers after the Newton model is ready.""" + entry = self._registry_entry + self._num_instances = len(entry.particle_offsets) + self._particles_per_body = entry.particles_per_body + self._recorded_particle_offsets = entry.particle_offsets + + if self._num_instances == 0: + raise RuntimeError( + f"No deformable body instances found for '{self.cfg.prim_path}'. " + "Ensure newton_physics_replicate or MODEL_INIT processed the registry." + ) + + logger.info("Newton deformable object initialized at: %s", self.cfg.prim_path) + logger.info("Number of instances: %d", self._num_instances) + logger.info("Particles per body: %d", self._particles_per_body) + + # Build particle offset array on device + self._particle_offsets = wp.array(self._recorded_particle_offsets, dtype=wp.int32, device=self.device) + + # Create data container + self._data = DeformableObjectData( + particle_offsets=self._particle_offsets, + particles_per_body=self._particles_per_body, + num_instances=self._num_instances, + device=self.device, + ) + + # Bind simulation state arrays + state = SimulationManager._state_0 + if state is not None: + self._data.bind_simulation_state(state.particle_q, state.particle_qd) + + # Create buffers + self._create_buffers() + + # Update data once + self.update(0.0) + + # Register rebind callback for full resets + self._physics_ready_handle = SimulationManager.register_callback( + lambda _: self._rebind_state(), + PhysicsEvent.PHYSICS_READY, + name=f"deformable_object_rebind_{self.cfg.prim_path}", + ) + + def _rebind_state(self) -> None: + """Rebind state arrays after a full simulation reset.""" + state = SimulationManager._state_0 + if state is not None and hasattr(self, "_data"): + self._data.bind_simulation_state(state.particle_q, state.particle_qd) + + def _create_buffers(self): + """Create buffers for storing data.""" + # Constants + self._ALL_INDICES = wp.array(np.arange(self._num_instances, dtype=np.int32), device=self.device) + + # Snapshot default positions from current state (after finalize + FK) + state = SimulationManager._state_0 + if state is not None and state.particle_q is not None: + from .kernels import gather_particles_vec3f + + self._default_nodal_pos_w = wp.zeros( + (self._num_instances, self._particles_per_body), dtype=wp.vec3f, device=self.device + ) + wp.launch( + gather_particles_vec3f, + dim=(self._num_instances, self._particles_per_body), + inputs=[state.particle_q, self._particle_offsets, self._particles_per_body], + outputs=[self._default_nodal_pos_w], + device=self.device, + ) + + # Compute default nodal state as vec6f (positions + zero velocities) + nodal_velocities = wp.zeros( + (self._num_instances, self._particles_per_body), dtype=wp.vec3f, device=self.device + ) + self._data.default_nodal_state_w = wp.zeros( + (self._num_instances, self._particles_per_body), dtype=vec6f, device=self.device + ) + wp.launch( + compute_nodal_state_w, + dim=(self._num_instances, self._particles_per_body), + inputs=[self._default_nodal_pos_w, nodal_velocities], + outputs=[self._data.default_nodal_state_w], + device=self.device, + ) + else: + self._default_nodal_pos_w = None + + # Snapshot default particle_inv_mass for kinematic target restoration + model = SimulationManager._model + if model is not None and hasattr(model, "particle_inv_mass") and model.particle_inv_mass is not None: + self._default_particle_inv_mass = wp.clone(model.particle_inv_mass) + else: + self._default_particle_inv_mass = None + + # Kinematic targets -- allocate and initialize with free flags + self._data.nodal_kinematic_target = wp.zeros( + (self._num_instances, self._particles_per_body), dtype=wp.vec4f, device=self.device + ) + wp.launch( + set_kinematic_flags_to_one, + dim=(self._num_instances * self._particles_per_body,), + inputs=[self._data.nodal_kinematic_target.reshape((self._num_instances * self._particles_per_body,))], + device=self.device, + ) + + # Set up the model parameters + model = SimulationManager._model + if model is not None: + if hasattr(model, "edge_rest_angle"): + model.edge_rest_angle.zero_() + + """ + Internal simulation callbacks. + """ + + def _set_debug_vis_impl(self, debug_vis: bool): + if debug_vis: + if not hasattr(self, "target_visualizer"): + self.target_visualizer = VisualizationMarkers(self.cfg.visualizer_cfg) + self.target_visualizer.set_visibility(True) + else: + if hasattr(self, "target_visualizer"): + self.target_visualizer.set_visibility(False) + + def _debug_vis_callback(self, event): + num_enabled = 0 + if self._deformable_type == "volume": + kinematic_target_torch = wp.to_torch(self.data.nodal_kinematic_target) + targets_enabled = kinematic_target_torch[:, :, 3] == 0.0 + num_enabled = int(torch.sum(targets_enabled).item()) + if num_enabled == 0: + positions = torch.tensor([[0.0, 0.0, -10.0]], device=self.device) + else: + positions = kinematic_target_torch[targets_enabled][..., :3] + self.target_visualizer.visualize(positions) + + def _clear_callbacks(self) -> None: + """Clears all registered callbacks.""" + super()._clear_callbacks() + if hasattr(self, "_physics_ready_handle") and self._physics_ready_handle is not None: + self._physics_ready_handle.deregister() + self._physics_ready_handle = None + + def _invalidate_initialize_callback(self, event): + """Invalidates the scene elements.""" + super()._invalidate_initialize_callback(event) diff --git a/source/isaaclab_contrib/isaaclab_contrib/deformable/deformable_object_data.py b/source/isaaclab_contrib/isaaclab_contrib/deformable/deformable_object_data.py new file mode 100644 index 000000000000..052c84309b6b --- /dev/null +++ b/source/isaaclab_contrib/isaaclab_contrib/deformable/deformable_object_data.py @@ -0,0 +1,157 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import warp as wp + +from isaaclab.assets.deformable_object.base_deformable_object_data import BaseDeformableObjectData +from isaaclab.utils.buffers import TimestampedBufferWarp as TimestampedBuffer + +from .kernels import compute_mean_vec3f_over_vertices, compute_nodal_state_w, gather_particles_vec3f, vec6f + + +class DeformableObjectData(BaseDeformableObjectData): + """Data container for a deformable object (Newton backend). + + Newton stores all particles in flat arrays (``model.particle_q``, ``state.particle_qd``). + This data class builds a per-instance view by gathering from the flat arrays using + precomputed offsets. + + The data is lazily updated, meaning that the data is only updated when it is accessed. + """ + + def __init__( + self, + particle_offsets: wp.array, + particles_per_body: int, + num_instances: int, + device: str, + ): + """Initialize the Newton deformable object data. + + Args: + particle_offsets: Per-instance start offset into the flat particle array. + Shape is (num_instances,) with dtype int32. + particles_per_body: Number of particles per deformable body instance. + num_instances: Number of deformable body instances. + device: The device used for processing. + """ + super().__init__(device) + + # Store direct references (no weakref) — set later by the asset class when state is available + self._particle_q: wp.array | None = None + self._particle_qd: wp.array | None = None + + # Store dimensions and indexing + self._particle_offsets = particle_offsets + self._particles_per_body = particles_per_body + self._num_instances = num_instances + + # Initialize lazy buffers + self._nodal_pos_w = TimestampedBuffer((num_instances, particles_per_body), device, wp.vec3f) + self._nodal_vel_w = TimestampedBuffer((num_instances, particles_per_body), device, wp.vec3f) + self._nodal_state_w = TimestampedBuffer((num_instances, particles_per_body), device, vec6f) + self._root_pos_w = TimestampedBuffer((num_instances,), device, wp.vec3f) + self._root_vel_w = TimestampedBuffer((num_instances,), device, wp.vec3f) + + def bind_simulation_state(self, particle_q: wp.array, particle_qd: wp.array) -> None: + """Bind the simulation state arrays for lazy reads. + + Called by the asset class after the Newton model is built and states are available. + + Args: + particle_q: Flat particle positions from Newton state. Shape is (total_particles,) vec3f. + particle_qd: Flat particle velocities from Newton state. Shape is (total_particles,) vec3f. + """ + self._particle_q = particle_q + self._particle_qd = particle_qd + + ## + # Properties. + ## + + @property + def nodal_pos_w(self) -> wp.array: + """Nodal positions in simulation world frame [m]. Shape is (num_instances, particles_per_body) vec3f.""" + if self._nodal_pos_w.timestamp < self._sim_timestamp: + wp.launch( + gather_particles_vec3f, + dim=(self._num_instances, self._particles_per_body), + inputs=[self._particle_q, self._particle_offsets, self._particles_per_body], + outputs=[self._nodal_pos_w.data], + device=self.device, + ) + self._nodal_pos_w.timestamp = self._sim_timestamp + return self._nodal_pos_w.data + + @property + def nodal_vel_w(self) -> wp.array: + """Nodal velocities in simulation world frame [m/s]. Shape is (num_instances, particles_per_body) vec3f.""" + if self._nodal_vel_w.timestamp < self._sim_timestamp: + wp.launch( + gather_particles_vec3f, + dim=(self._num_instances, self._particles_per_body), + inputs=[self._particle_qd, self._particle_offsets, self._particles_per_body], + outputs=[self._nodal_vel_w.data], + device=self.device, + ) + self._nodal_vel_w.timestamp = self._sim_timestamp + return self._nodal_vel_w.data + + @property + def nodal_state_w(self) -> wp.array: + """Nodal state ``[nodal_pos, nodal_vel]`` in simulation world frame [m, m/s]. + + Shape is (num_instances, particles_per_body) vec6f. + """ + if self._nodal_state_w.timestamp < self._sim_timestamp: + wp.launch( + compute_nodal_state_w, + dim=(self._num_instances, self._particles_per_body), + inputs=[self.nodal_pos_w, self.nodal_vel_w], + outputs=[self._nodal_state_w.data], + device=self.device, + ) + self._nodal_state_w.timestamp = self._sim_timestamp + return self._nodal_state_w.data + + ## + # Derived properties. + ## + + @property + def root_pos_w(self) -> wp.array: + """Root position from nodal positions [m]. Shape is (num_instances,) vec3f. + + This quantity is computed as the mean of the nodal positions. + """ + if self._root_pos_w.timestamp < self._sim_timestamp: + wp.launch( + compute_mean_vec3f_over_vertices, + dim=(self._num_instances,), + inputs=[self.nodal_pos_w, self._particles_per_body], + outputs=[self._root_pos_w.data], + device=self.device, + ) + self._root_pos_w.timestamp = self._sim_timestamp + return self._root_pos_w.data + + @property + def root_vel_w(self) -> wp.array: + """Root velocity from nodal velocities [m/s]. Shape is (num_instances,) vec3f. + + This quantity is computed as the mean of the nodal velocities. + """ + if self._root_vel_w.timestamp < self._sim_timestamp: + wp.launch( + compute_mean_vec3f_over_vertices, + dim=(self._num_instances,), + inputs=[self.nodal_vel_w, self._particles_per_body], + outputs=[self._root_vel_w.data], + device=self.device, + ) + self._root_vel_w.timestamp = self._sim_timestamp + return self._root_vel_w.data diff --git a/source/isaaclab_contrib/isaaclab_contrib/deformable/kernels.py b/source/isaaclab_contrib/isaaclab_contrib/deformable/kernels.py new file mode 100644 index 000000000000..76b6a9532156 --- /dev/null +++ b/source/isaaclab_contrib/isaaclab_contrib/deformable/kernels.py @@ -0,0 +1,191 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Warp kernels for Newton deformable object gather/scatter operations.""" + +import warp as wp + +vec6f = wp.types.vector(length=6, dtype=wp.float32) + + +@wp.kernel +def gather_particles_vec3f( + src: wp.array(dtype=wp.vec3f), + offsets: wp.array(dtype=wp.int32), + num_particles: int, + dst: wp.array2d(dtype=wp.vec3f), +): + """Gather particle data from a flat array into a per-instance 2D array. + + Args: + src: Flat source particle array (all instances concatenated). Shape is (total_particles,). + offsets: Per-instance start offset into the flat array. Shape is (num_instances,). + num_particles: Number of particles per instance. + dst: Output 2D array. Shape is (num_instances, num_particles). + """ + i, j = wp.tid() + dst[i, j] = src[offsets[i] + j] + + +@wp.kernel +def scatter_particles_vec3f_index( + src: wp.array2d(dtype=wp.vec3f), + env_ids: wp.array(dtype=wp.int32), + offsets: wp.array(dtype=wp.int32), + full_data: bool, + dst: wp.array(dtype=wp.vec3f), +): + """Scatter per-instance particle data into the flat simulation array using indices. + + Args: + src: Input 2D array. Shape is (len(env_ids), num_particles) or (num_instances, num_particles). + env_ids: Environment indices to scatter to. Shape is (num_selected,). + offsets: Per-instance start offset into the flat array. Shape is (num_instances,). + full_data: If True, index src with env_ids[i]; otherwise index src with i. + dst: Flat destination particle array. Shape is (total_particles,). + """ + i, j = wp.tid() + env_id = env_ids[i] + if full_data: + dst[offsets[env_id] + j] = src[env_id, j] + else: + dst[offsets[env_id] + j] = src[i, j] + + +@wp.kernel +def compute_nodal_state_w( + nodal_pos: wp.array2d(dtype=wp.vec3f), + nodal_vel: wp.array2d(dtype=wp.vec3f), + nodal_state: wp.array2d(dtype=vec6f), +): + """Concatenate nodal positions and velocities into a 6-element state vector. + + Args: + nodal_pos: Input array of nodal positions. Shape is (num_instances, num_vertices). + nodal_vel: Input array of nodal velocities. Shape is (num_instances, num_vertices). + nodal_state: Output array where concatenated state vectors are written. + Shape is (num_instances, num_vertices). + """ + i, j = wp.tid() + p = nodal_pos[i, j] + v = nodal_vel[i, j] + nodal_state[i, j] = vec6f(p[0], p[1], p[2], v[0], v[1], v[2]) + + +@wp.kernel +def compute_mean_vec3f_over_vertices( + data: wp.array2d(dtype=wp.vec3f), + num_vertices: int, + result: wp.array(dtype=wp.vec3f), +): + """Compute the mean of vec3f data over the vertex dimension. + + Args: + data: Input array of vec3f data. Shape is (num_instances, num_vertices). + num_vertices: Number of vertices per instance. + result: Output array where mean values are written. Shape is (num_instances,). + """ + i = wp.tid() + acc = wp.vec3f(0.0, 0.0, 0.0) + for j in range(num_vertices): + acc = acc + data[i, j] + result[i] = acc / float(num_vertices) + + +@wp.kernel +def scatter_zero_vel_index( + env_ids: wp.array(dtype=wp.int32), + offsets: wp.array(dtype=wp.int32), + num_particles: int, + dst: wp.array(dtype=wp.vec3f), +): + """Zero the velocity of particles for selected environments. + + Args: + env_ids: Environment indices to zero velocities for. Shape is (num_selected,). + offsets: Per-instance start offset into the flat array. Shape is (num_instances,). + num_particles: Number of particles per instance. + dst: Flat destination velocity array. Shape is (total_particles,). + """ + i, j = wp.tid() + env_id = env_ids[i] + dst[offsets[env_id] + j] = wp.vec3f(0.0, 0.0, 0.0) + + +@wp.kernel +def scatter_default_pos_index( + default_pos: wp.array2d(dtype=wp.vec3f), + env_ids: wp.array(dtype=wp.int32), + offsets: wp.array(dtype=wp.int32), + dst: wp.array(dtype=wp.vec3f), +): + """Scatter default positions for selected environments into the flat simulation array. + + Args: + default_pos: Default positions per instance. Shape is (num_instances, num_particles). + env_ids: Environment indices to reset. Shape is (num_selected,). + offsets: Per-instance start offset into the flat array. Shape is (num_instances,). + dst: Flat destination particle array. Shape is (total_particles,). + """ + i, j = wp.tid() + env_id = env_ids[i] + dst[offsets[env_id] + j] = default_pos[env_id, j] + + +@wp.kernel +def set_kinematic_flags_to_one( + data: wp.array(dtype=wp.vec4f), +): + """Set the w-component (kinematic flag) of all vec4f entries to 1.0. + + This is used to initialize all vertices as non-kinematic (free) nodes. + + Args: + data: Input/output array of vec4f kinematic targets. Shape is (N*V,). + """ + i = wp.tid() + v = data[i] + data[i] = wp.vec4f(v[0], v[1], v[2], 1.0) + + +@wp.kernel +def enforce_kinematic_targets( + targets: wp.array2d(dtype=wp.vec4f), + offsets: wp.array(dtype=wp.int32), + default_inv_mass: wp.array(dtype=wp.float32), + particle_q: wp.array(dtype=wp.vec3f), + particle_qd: wp.array(dtype=wp.vec3f), + particle_inv_mass: wp.array(dtype=wp.float32), + particle_flags: wp.array(dtype=wp.int32), +): + """Enforce kinematic targets on Newton particles. + + For each particle, reads the kinematic target flag (w-component): + - flag == 0.0 (kinematic): set inv_mass to 0, particle_flags to 0, write target position, zero velocity. + - flag != 0.0 (free): restore the default inv_mass and set particle_flags to 1 (ACTIVE). + + Args: + targets: Per-instance kinematic targets. Shape is (num_instances, particles_per_body). + Each vec4f contains (target_x, target_y, target_z, flag). + offsets: Per-instance start offset into the flat particle array. + default_inv_mass: Saved default inverse masses. Shape is (total_particles,). + particle_q: Flat particle positions to write. Shape is (total_particles,). + particle_qd: Flat particle velocities to write. Shape is (total_particles,). + particle_inv_mass: Flat particle inverse masses to write. Shape is (total_particles,). + particle_flags: Flat particle flags to write. Shape is (total_particles,). + 0 = kinematic (solver skips integration), 1 = ACTIVE. + """ + i, j = wp.tid() + t = targets[i, j] + flat_idx = offsets[i] + j + flag = t[3] + if flag == 0.0: + particle_inv_mass[flat_idx] = 0.0 + particle_flags[flat_idx] = 0 + particle_q[flat_idx] = wp.vec3f(t[0], t[1], t[2]) + particle_qd[flat_idx] = wp.vec3f(0.0, 0.0, 0.0) + else: + particle_inv_mass[flat_idx] = default_inv_mass[flat_idx] + particle_flags[flat_idx] = 1 diff --git a/source/isaaclab_contrib/isaaclab_contrib/deformable/model_cfg_hook.py b/source/isaaclab_contrib/isaaclab_contrib/deformable/model_cfg_hook.py new file mode 100644 index 000000000000..261341bc1c5b --- /dev/null +++ b/source/isaaclab_contrib/isaaclab_contrib/deformable/model_cfg_hook.py @@ -0,0 +1,52 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Hook for applying global Newton model parameters after builder finalization.""" + +from __future__ import annotations + +import logging + +logger = logging.getLogger(__name__) + + +def apply_model_cfg() -> None: + """Apply global model parameters from :class:`NewtonModelCfg` to the finalized model. + + Sets ``soft_contact_ke/kd/mu`` and optionally overrides per-shape + ``shape_material_ke/kd/mu`` on the Newton model. This hook is always + executed (not gated behind contact attributes) to ensure contact + parameters are consistently applied. + """ + from isaaclab_newton.physics import NewtonManager + + from isaaclab.physics import PhysicsManager + + cfg = PhysicsManager._cfg + if cfg is None or not hasattr(cfg, "model_cfg") or cfg.model_cfg is None: + return + + model = NewtonManager._model + if model is None: + return + + model_cfg = cfg.model_cfg + model.soft_contact_ke = float(model_cfg.soft_contact_ke) + model.soft_contact_kd = float(model_cfg.soft_contact_kd) + model.soft_contact_mu = float(model_cfg.soft_contact_mu) + + if model_cfg.shape_material_ke is not None: + model.shape_material_ke.fill_(float(model_cfg.shape_material_ke)) + if model_cfg.shape_material_kd is not None: + model.shape_material_kd.fill_(float(model_cfg.shape_material_kd)) + if model_cfg.shape_material_mu is not None: + model.shape_material_mu.fill_(float(model_cfg.shape_material_mu)) + + logger.info( + "Applied NewtonModelCfg: soft_contact_ke=%.1f, soft_contact_kd=%.4f, soft_contact_mu=%.2f", + model_cfg.soft_contact_ke, + model_cfg.soft_contact_kd, + model_cfg.soft_contact_mu, + ) diff --git a/source/isaaclab_contrib/isaaclab_contrib/deformable/newton_manager_cfg.py b/source/isaaclab_contrib/isaaclab_contrib/deformable/newton_manager_cfg.py new file mode 100644 index 000000000000..a87b3f9668d8 --- /dev/null +++ b/source/isaaclab_contrib/isaaclab_contrib/deformable/newton_manager_cfg.py @@ -0,0 +1,171 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configuration classes for VBD, coupled solver, and global Newton model parameters.""" + +from __future__ import annotations + +from isaaclab_newton.physics.newton_manager_cfg import MJWarpSolverCfg, NewtonSolverCfg + +from isaaclab.utils import configclass + + +@configclass +class VBDSolverCfg(NewtonSolverCfg): + """Configuration for the Vertex Block Descent (VBD) solver. + + Supports particle simulation (cloth, soft bodies) and coupled rigid-body systems. + Requires ``ModelBuilder.color()`` to be called before ``finalize()`` to build + the parallel vertex colouring needed by the solver. + """ + + solver_type: str = "vbd" + + iterations: int = 10 + """Number of VBD iterations per substep.""" + + integrate_with_external_rigid_solver: bool = False + """Whether rigid bodies are integrated by an external solver (one-way coupling). + + Set to ``True`` when coupling cloth with a separate rigid-body solver + (e.g. ``SolverFeatherstone``) so that VBD only integrates the cloth particles. + """ + + particle_enable_self_contact: bool = False + """Whether to enable cloth self-contact.""" + + particle_self_contact_radius: float = 0.005 + """Particle radius used for self-contact detection [m].""" + + particle_self_contact_margin: float = 0.005 + """Self-contact detection margin [m]. Should be >= particle_self_contact_radius.""" + + particle_collision_detection_interval: int = -1 + """Controls how frequently particle self-contact detection is applied. + + If set to a value < 0, collision detection is only performed once before the + initialization step. If set to 0, collision detection is applied twice: once + before and once immediately after initialization. If set to a value ``k`` >= 1, + collision detection is applied before every ``k`` VBD iterations. + """ + + particle_vertex_contact_buffer_size: int = 32 + """Preallocation size for each vertex's vertex-triangle collision buffer.""" + + particle_edge_contact_buffer_size: int = 64 + """Preallocation size for each edge's edge-edge collision buffer.""" + + particle_topological_contact_filter_threshold: int = 2 + """Maximum topological distance (in rings) below which self-contacts are discarded. + + Only used when ``particle_enable_self_contact`` is ``True``. + Increase to suppress contacts between closely connected mesh elements. + Values > 3 significantly increase computation time. + """ + + particle_rest_shape_contact_exclusion_radius: float = 0.0 + """World-space distance threshold for filtering topologically close primitives [m]. + + Candidate self-contacts whose rest-configuration separation is shorter than + this value are ignored. Only used when ``particle_enable_self_contact`` is ``True``. + """ + + rigid_contact_k_start: float = 1.0e2 + """Initial stiffness seed for all rigid body contacts (body-body and body-particle) [N/m]. + + Used by the AVBD rigid contact solver. Increase to make rigid contacts stiffer. + """ + + +@configclass +class CoupledSolverCfg(NewtonSolverCfg): + """Configuration for the coupled rigid-body + VBD solver. + + Alternates a rigid-body solver and a cloth solver (:class:`SolverVBD`) per + substep. The coupling direction is controlled by :attr:`coupling_mode`: + + - ``"one_way"`` (default): Rigid solver advances first, then VBD reads + the updated body poses. The rigid solver does not feel particle contacts. + - ``"two_way"``: Same-substep two-way coupling with normal + Coulomb + friction. Contact detection runs first, reaction forces are injected + into ``body_f``, then the rigid solver reads ``body_f`` and feels + resistance from the deformable object. The friction reaction lets + actuators carry the object against gravity during a lift. + + The rigid-body solver is selected by :attr:`rigid_solver_cfg`: + + - :class:`MJWarpSolverCfg` -- MuJoCo Warp (default, recommended for stability) + - :class:`FeatherstoneSolverCfg` -- Newton Featherstone + """ + + solver_type: str = "coupled" + + rigid_solver_cfg: NewtonSolverCfg = MJWarpSolverCfg() + """Rigid-body sub-solver configuration. Can be :class:`MJWarpSolverCfg` or + :class:`FeatherstoneSolverCfg`.""" + + vbd_cfg: VBDSolverCfg = VBDSolverCfg(integrate_with_external_rigid_solver=True) + """VBD sub-solver configuration for cloth/particle dynamics.""" + + soft_contact_margin: float = 0.01 + """Soft-contact detection margin for the CollisionPipeline [m].""" + + coupling_mode: str = "two_way" + """Coupling direction between the rigid and VBD solvers. + + - ``"one_way"``: Rigid -> cloth only (default, existing behavior). + - ``"two_way"``: Same-substep two-way coupling with normal + Coulomb friction. + """ + + +@configclass +class NewtonModelCfg: + """Global Newton model parameters. + + These parameters are applied to the ``newton.Model`` after finalization. + They control model-level contact behavior shared across all objects. + """ + + soft_contact_ke: float = 1.0e3 + """Body-particle contact stiffness [N/m]. + + Controls how stiff the penalty force is when cloth/soft-body particles + contact rigid body shapes. The effective stiffness per contact is the + average of this value and the rigid shape's material stiffness. + """ + + soft_contact_kd: float = 1.0e-2 + """Body-particle contact damping [N*s/m].""" + + soft_contact_mu: float = 0.5 + """Body-particle contact friction coefficient. + + The effective friction per contact is ``sqrt(soft_contact_mu * shape_material_mu)``. + Increase for better grip (e.g. gripper picking up cloth). + """ + + shape_material_ke: float | None = None + """Per-shape contact stiffness override [N/m]. + + When set, all collision shapes in the model will have their contact + stiffness overwritten to this value. If ``None`` (default), the + per-shape values parsed from USD/MJCF are kept. + """ + + shape_material_kd: float | None = None + """Per-shape contact damping override [N*s/m]. + + When set, all collision shapes in the model will have their contact + damping overwritten to this value. If ``None`` (default), the + per-shape values parsed from USD/MJCF are kept. + """ + + shape_material_mu: float | None = None + """Per-shape friction coefficient override [dimensionless]. + + When set, all collision shapes in the model will have their friction + coefficient overwritten to this value. If ``None`` (default), the + per-shape values parsed from USD/MJCF are kept. + """ diff --git a/source/isaaclab_contrib/isaaclab_contrib/deformable/particle_sync.py b/source/isaaclab_contrib/isaaclab_contrib/deformable/particle_sync.py new file mode 100644 index 000000000000..21b93795ef86 --- /dev/null +++ b/source/isaaclab_contrib/isaaclab_contrib/deformable/particle_sync.py @@ -0,0 +1,175 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Particle sync utilities for writing Newton particle positions to USD/Fabric for Kit rendering.""" + +from __future__ import annotations + +import logging +import re + +import warp as wp + +from isaaclab.physics import PhysicsManager + +logger = logging.getLogger(__name__) + +_usdrt = None # cached usdrt module (avoid re-importing every render frame) +_sync_warned = False # True after the first sync failure has been logged at warning level + + +@wp.kernel(enable_backward=False) +def _sync_particle_points( + fabric_points: wp.fabricarrayarray(dtype=wp.vec3f), + fabric_world_matrices: wp.fabricarray(dtype=wp.mat44d), + offsets: wp.fabricarray(dtype=wp.uint32), + counts: wp.fabricarray(dtype=wp.uint32), + particle_q: wp.array(dtype=wp.vec3f), +): + """Write Newton particle positions into Fabric mesh point arrays as local-frame points. + + Newton stores particle positions in world space in ``state.particle_q``. The Fabric + ``points`` attribute on a ``UsdGeom.Mesh`` is local-space -- Kit multiplies by the + mesh prim's resolved ``omni:fabric:worldMatrix`` at render time. + + This kernel inverts the mesh prim's world matrix to convert each world-space particle + position into local-space before writing. + """ + i = wp.tid() + offset = int(offsets[i]) + num_points = int(counts[i]) + + # Un-transpose Fabric's stored matrix to get the standard homogeneous form + world_matrix = wp.transpose(wp.mat44f(fabric_world_matrices[i])) + inv_world_matrix = wp.inverse(world_matrix) + + for j in range(num_points): + wp_in = particle_q[offset + j] + # Apply inverse transform to homogeneous point (w=1). + fabric_points[i][j] = wp.vec3f( + inv_world_matrix[0, 0] * wp_in[0] + + inv_world_matrix[0, 1] * wp_in[1] + + inv_world_matrix[0, 2] * wp_in[2] + + inv_world_matrix[0, 3], + inv_world_matrix[1, 0] * wp_in[0] + + inv_world_matrix[1, 1] * wp_in[1] + + inv_world_matrix[1, 2] * wp_in[2] + + inv_world_matrix[1, 3], + inv_world_matrix[2, 0] * wp_in[0] + + inv_world_matrix[2, 1] * wp_in[1] + + inv_world_matrix[2, 2] * wp_in[2] + + inv_world_matrix[2, 3], + ) + + +def sync_particles_to_usd() -> None: + """Write Newton particle_q to Fabric mesh point arrays for Kit viewport rendering. + + For each deformable body whose mesh prim carries a ``newton:particleOffset`` + attribute, this function copies the corresponding slice of ``state_0.particle_q`` + into the Fabric ``points`` array so the Kit viewport reflects the current + deformation. + + No-op when there is no ``_usdrt_stage``, no simulation state, or no + deformable bodies registered. + """ + from isaaclab_newton.physics import NewtonManager + + if ( + NewtonManager._usdrt_stage is None + or NewtonManager._state_0 is None + or not NewtonManager._deformable_registry + or NewtonManager._state_0.particle_q is None + ): + return + if not NewtonManager._particles_dirty: + return + pq = NewtonManager._state_0.particle_q + try: + global _usdrt + if _usdrt is None: + import usdrt + + _usdrt = usdrt + usdrt = _usdrt + + selection = NewtonManager._usdrt_stage.SelectPrims( + require_attrs=[ + (usdrt.Sdf.ValueTypeNames.Point3fArray, "points", usdrt.Usd.Access.ReadWrite), + (usdrt.Sdf.ValueTypeNames.UInt, NewtonManager._newton_particle_offset_attr, usdrt.Usd.Access.Read), + (usdrt.Sdf.ValueTypeNames.UInt, NewtonManager._newton_particle_count_attr, usdrt.Usd.Access.Read), + (usdrt.Sdf.ValueTypeNames.Matrix4d, "omni:fabric:worldMatrix", usdrt.Usd.Access.Read), + ], + device=str(PhysicsManager._device), + ) + if selection.GetCount() == 0: + return + fabric_points = wp.fabricarrayarray(data=selection, attrib="points", dtype=wp.vec3f) + fabric_offsets = wp.fabricarray(data=selection, attrib=NewtonManager._newton_particle_offset_attr) + fabric_counts = wp.fabricarray(data=selection, attrib=NewtonManager._newton_particle_count_attr) + fabric_world_matrices = wp.fabricarray(data=selection, attrib="omni:fabric:worldMatrix") + wp.launch( + _sync_particle_points, + dim=selection.GetCount(), + inputs=[fabric_points, fabric_world_matrices, fabric_offsets, fabric_counts, pq], + device=PhysicsManager._device, + ) + NewtonManager._particles_dirty = False + except Exception as exc: + global _sync_warned + if not _sync_warned: + logger.warning("[sync_particles_to_usd] %s", exc) + _sync_warned = True + else: + logger.debug("[sync_particles_to_usd] %s", exc) + + +def setup_fabric_particle_sync() -> None: + """Set up Fabric attributes for deformable particle sync after simulation starts. + + For each deformable registry entry, this function: + + 1. Resolves regex prim paths to concrete per-instance visual mesh paths. + 2. Creates per-instance ``newton:particleOffset`` and ``newton:particleCount`` + Fabric attributes so :func:`sync_particles_to_usd` can find the right slice + of ``particle_q`` and iterate the correct number of particles per body. + 3. Triggers an initial particle sync. + + .. note:: + Visual mesh topology is already overwritten to match the simulation mesh + at scene construction time in :func:`define_deformable_body_properties`. + """ + import usdrt + + from isaaclab.sim.utils.stage import get_current_stage + from isaaclab_newton.physics import NewtonManager + + if NewtonManager._usdrt_stage is None: + NewtonManager._usdrt_stage = get_current_stage(fabric=True) + + stage = get_current_stage() + for entry in NewtonManager._deformable_registry: + for inst_idx, offset in enumerate(entry.particle_offsets): + # Resolve regex pattern to concrete instance path of visual mesh + resolved_vis = re.sub(r"(?<=[Ee]nv_)\.\*", str(inst_idx), entry.vis_mesh_prim_path) + resolved_vis = re.sub(r"\.\*", str(inst_idx), resolved_vis) + vis_prim = stage.GetPrimAtPath(resolved_vis) + + if not vis_prim or not vis_prim.IsValid(): + logger.warning("[setup_fabric_particle_sync] vis prim not found at %s", resolved_vis) + continue + + # Create per-instance particle offset and count attributes on the visual mesh + # prim so the Fabric sync kernel can find the right slice of particle_q + # and iterate only over this body's particles (counts vary across bodies). + fab_prim = NewtonManager._usdrt_stage.GetPrimAtPath(vis_prim.GetPath().pathString) + fab_prim.CreateAttribute(NewtonManager._newton_particle_offset_attr, usdrt.Sdf.ValueTypeNames.UInt, True) + fab_prim.GetAttribute(NewtonManager._newton_particle_offset_attr).Set(offset) + fab_prim.CreateAttribute(NewtonManager._newton_particle_count_attr, usdrt.Sdf.ValueTypeNames.UInt, True) + fab_prim.GetAttribute(NewtonManager._newton_particle_count_attr).Set(entry.particles_per_body) + + NewtonManager._mark_particles_dirty() + if NewtonManager._particle_sync_fn is not None: + NewtonManager._particle_sync_fn() diff --git a/source/isaaclab_contrib/isaaclab_contrib/deformable/solver_factories.py b/source/isaaclab_contrib/isaaclab_contrib/deformable/solver_factories.py new file mode 100644 index 000000000000..3ea81d6a91e2 --- /dev/null +++ b/source/isaaclab_contrib/isaaclab_contrib/deformable/solver_factories.py @@ -0,0 +1,71 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Solver factory functions for VBD and coupled solvers. + +These factories are registered with :class:`NewtonManager` via +:meth:`NewtonManager.register_solver_factory` so the manager can create +VBD and coupled solvers without hard-coding their logic. +""" + +from __future__ import annotations + +import inspect +import logging + +from newton.solvers import SolverVBD + +logger = logging.getLogger(__name__) + + +def create_vbd_solver(manager_cls, cfg_dict: dict, solver_cfg) -> None: + """Create and assign a VBD solver on the Newton manager. + + Args: + manager_cls: The :class:`NewtonManager` class (not an instance). + cfg_dict: Solver configuration dictionary with ``solver_type`` already popped. + solver_cfg: The original solver configuration object. + """ + manager_cls._use_single_state = False + solver_sig = inspect.signature(SolverVBD.__init__) + valid_solver_args = set(solver_sig.parameters.keys()) - {"self", "model"} + filtered_cfg = {k: v for k, v in cfg_dict.items() if k in valid_solver_args} + logger.info("VBD: Creating SolverVBD with args: %s", filtered_cfg) + logger.info("VBD: model particle_count=%s", getattr(manager_cls._model, "particle_count", "N/A")) + num_groups = len(getattr(manager_cls._model, "particle_color_groups", [])) + logger.info("VBD: model particle_color_groups has %d groups", num_groups) + manager_cls._solver = SolverVBD(manager_cls._model, **filtered_cfg) + logger.info("VBD: SolverVBD created successfully") + + # VBD may need its own collision pipeline for body-particle contacts. + # If the solver_cfg has collision attributes, set up the pipeline. + if hasattr(solver_cfg, "particle_enable_self_contact") and solver_cfg.particle_enable_self_contact: + manager_cls._needs_collision_pipeline = True + manager_cls._initialize_contacts() + + +def create_coupled_solver(manager_cls, cfg_dict: dict, solver_cfg) -> None: + """Create and assign a coupled rigid-body + VBD solver on the Newton manager. + + Args: + manager_cls: The :class:`NewtonManager` class (not an instance). + cfg_dict: Solver configuration dictionary with ``solver_type`` already popped. + solver_cfg: The original solver configuration object. + """ + from .coupled_solver import CoupledSolver + + manager_cls._use_single_state = False + manager_cls._soft_contact_margin = solver_cfg.soft_contact_margin + + # Initialize collision pipeline to pass into the coupled solver + manager_cls._needs_collision_pipeline = True + manager_cls._initialize_contacts() + + manager_cls._solver = CoupledSolver( + model=manager_cls._model, + cfg=solver_cfg, + collision_pipeline=manager_cls._collision_pipeline, + contacts=manager_cls._contacts, + ) diff --git a/source/isaaclab_contrib/test/deformable/test_deformable_object.py b/source/isaaclab_contrib/test/deformable/test_deformable_object.py new file mode 100644 index 000000000000..786c58e62ddc --- /dev/null +++ b/source/isaaclab_contrib/test/deformable/test_deformable_object.py @@ -0,0 +1,343 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# ignore private usage of variables warning +# pyright: reportPrivateUsage=none + + +"""Launch Isaac Sim Simulator first.""" + +from isaaclab.app import AppLauncher + +# launch omniverse app +simulation_app = AppLauncher(headless=True).app + +"""Rest everything follows.""" + +import pytest +import torch +import warp as wp +from flaky import flaky + +from isaaclab_contrib.deformable import DeformableObject, VBDSolverCfg, register_hooks +from isaaclab_newton.physics import NewtonCfg + +import isaaclab.sim as sim_utils +import isaaclab.utils.math as math_utils +from isaaclab.assets.deformable_object import DeformableObjectCfg +from isaaclab.sim import SimulationCfg, build_simulation_context + +NEWTON_VBD_CFG = SimulationCfg( + physics=NewtonCfg( + solver_cfg=VBDSolverCfg(iterations=3), + num_substeps=2, + ), +) + + +def _newton_sim_context(device="cuda:0", gravity_enabled=True): + """Helper to create a Newton VBD simulation context.""" + NEWTON_VBD_CFG.device = device + NEWTON_VBD_CFG.gravity = (0.0, 0.0, -9.81) if gravity_enabled else (0.0, 0.0, 0.0) + return build_simulation_context(device=device, sim_cfg=NEWTON_VBD_CFG, auto_add_lighting=True) + + +def generate_cubes_scene( + num_cubes: int = 1, + height: float = 1.0, + device: str = "cuda:0", +) -> DeformableObject: + """Generate a scene with deformable tet-mesh cubes. + + Args: + num_cubes: Number of cubes to generate. + height: Height of the cubes. + device: Device to use for the simulation. + + Returns: + The deformable object representing the cubes. + """ + origins = torch.tensor([(i * 1.0, 0, height) for i in range(num_cubes)]).to(device) + for i, origin in enumerate(origins): + sim_utils.create_prim(f"/World/env_{i}", "Xform", translation=origin) + + cube_object_cfg = DeformableObjectCfg( + prim_path="/World/env_.*/Cube", + spawn=sim_utils.MeshCuboidCfg( + size=(0.1, 0.1, 0.1), + deformable_props=sim_utils.DeformableBodyPropertiesCfg(), + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.2, 0.8, 0.2)), + physics_material=sim_utils.DeformableBodyMaterialCfg( + density=500.0, + youngs_modulus=2.5e4, + poissons_ratio=0.25, + ), + ), + init_state=DeformableObjectCfg.InitialStateCfg( + pos=(0.0, 0.0, height), + rot=(1.0, 0.0, 0.0, 0.0), + ), + ) + cube_object = DeformableObject(cfg=cube_object_cfg) + return cube_object + + +@pytest.fixture +def sim(): + """Create Newton VBD simulation context.""" + register_hooks() + with _newton_sim_context() as sim: + sim._app_control_on_stop_handle = None + yield sim + + +@pytest.mark.parametrize("num_cubes", [1, 2]) +def test_initialization(sim, num_cubes): + """Test initialization of Newton deformable objects.""" + cube_object = generate_cubes_scene(num_cubes=num_cubes) + + sim.reset() + + assert cube_object.is_initialized + assert cube_object.num_instances == num_cubes + assert cube_object.max_sim_vertices_per_body > 0 + + particles_per_body = cube_object.max_sim_vertices_per_body + + # nodal_state_w: (N, V, 6) + nodal_state = wp.to_torch(cube_object.data.nodal_state_w) + assert nodal_state.shape == (num_cubes, particles_per_body, 6) + + # nodal_pos_w: (N, V, 3) + nodal_pos = wp.to_torch(cube_object.data.nodal_pos_w) + assert nodal_pos.shape == (num_cubes, particles_per_body, 3) + + # nodal_vel_w: (N, V, 3) + nodal_vel = wp.to_torch(cube_object.data.nodal_vel_w) + assert nodal_vel.shape == (num_cubes, particles_per_body, 3) + + # root_pos_w: (N, 3) + root_pos = wp.to_torch(cube_object.data.root_pos_w) + assert root_pos.shape == (num_cubes, 3) + + # root_vel_w: (N, 3) + root_vel = wp.to_torch(cube_object.data.root_vel_w) + assert root_vel.shape == (num_cubes, 3) + + +@pytest.mark.parametrize("num_cubes", [1, 2]) +def test_set_nodal_state(sim, num_cubes): + """Test setting the state of the deformable object.""" + cube_object = generate_cubes_scene(num_cubes=num_cubes) + + sim.reset() + + for state_type_to_randomize in ["nodal_pos_w", "nodal_vel_w"]: + state_dict = { + "nodal_pos_w": torch.zeros_like(wp.to_torch(cube_object.data.nodal_pos_w)), + "nodal_vel_w": torch.zeros_like(wp.to_torch(cube_object.data.nodal_vel_w)), + } + + for _ in range(5): + state_dict[state_type_to_randomize] = torch.randn( + num_cubes, cube_object.max_sim_vertices_per_body, 3, device=sim.device + ) + + for _ in range(5): + nodal_state = torch.cat( + [ + state_dict["nodal_pos_w"], + state_dict["nodal_vel_w"], + ], + dim=-1, + ) + cube_object.write_nodal_state_to_sim_index(nodal_state) + + torch.testing.assert_close( + wp.to_torch(cube_object.data.nodal_state_w), nodal_state, rtol=1e-5, atol=1e-5 + ) + + sim.step() + cube_object.update(sim.cfg.dt) + + +@pytest.mark.parametrize("num_cubes", [2, 4]) +def test_write_partial_env_ids(sim, num_cubes): + """Test writing to a subset of environments using env_ids.""" + cube_object = generate_cubes_scene(num_cubes=num_cubes) + + sim.reset() + + particles_per_body = cube_object.max_sim_vertices_per_body + default_pos = wp.to_torch(cube_object.data.nodal_pos_w).clone() + + # Write new positions only for env 0 + new_pos = torch.randn(1, particles_per_body, 3, device=sim.device) + cube_object.write_nodal_pos_to_sim_index(new_pos, env_ids=torch.tensor([0], device=sim.device)) + cube_object.update(sim.cfg.dt) + + read_pos = wp.to_torch(cube_object.data.nodal_pos_w) + + # env 0 should have new positions + torch.testing.assert_close(read_pos[0], new_pos[0], rtol=1e-5, atol=1e-5) + + # other envs should be unchanged + torch.testing.assert_close(read_pos[1:], default_pos[1:], rtol=1e-5, atol=1e-5) + + +@pytest.mark.parametrize("num_cubes", [1, 2]) +@pytest.mark.parametrize("randomize_pos", [True, False]) +@pytest.mark.parametrize("randomize_rot", [True, False]) +@flaky(max_runs=3, min_passes=1) +def test_set_nodal_state_with_applied_transform(num_cubes, randomize_pos, randomize_rot): + """Test setting the state of the deformable object with applied transform. + + Applies random position/rotation transforms to the default nodal state, + writes it to simulation, steps with no gravity, and verifies the mean + nodal position (root_pos_w) matches the expected transformed centroid. + """ + register_hooks() + cfg = SimulationCfg( + physics=NewtonCfg( + solver_cfg=VBDSolverCfg(iterations=3), + num_substeps=2, + ), + ) + cfg.device = "cuda:0" + cfg.gravity = (0.0, 0.0, 0.0) + + with build_simulation_context(device="cuda:0", sim_cfg=cfg, auto_add_lighting=True) as sim: + sim._app_control_on_stop_handle = None + cube_object = generate_cubes_scene(num_cubes=num_cubes, height=5.0) + sim.reset() + + for _ in range(5): + nodal_state = wp.to_torch(cube_object.data.default_nodal_state_w).clone() + mean_nodal_pos_default = nodal_state[..., :3].mean(dim=1) + + if randomize_pos: + pos_w = 0.5 * torch.rand(cube_object.num_instances, 3, device=sim.device) + pos_w[:, 2] += 0.5 + else: + pos_w = None + if randomize_rot: + quat_w = math_utils.random_orientation(cube_object.num_instances, device=sim.device) + else: + quat_w = None + + # transform_nodal_pos: center, rotate, translate, un-center + nodal_pos = nodal_state[..., :3] + mean_pos = nodal_pos.mean(dim=1, keepdim=True) + centered = nodal_pos - mean_pos + nodal_state[..., :3] = math_utils.transform_points(centered, pos_w, quat_w) + mean_pos + mean_nodal_pos_init = nodal_state[..., :3].mean(dim=1) + + if pos_w is None: + torch.testing.assert_close(mean_nodal_pos_init, mean_nodal_pos_default, rtol=1e-5, atol=1e-5) + else: + torch.testing.assert_close(mean_nodal_pos_init, mean_nodal_pos_default + pos_w, rtol=1e-5, atol=1e-5) + + cube_object.write_nodal_state_to_sim_index(nodal_state) + + for _ in range(50): + sim.step() + cube_object.update(sim.cfg.dt) + + torch.testing.assert_close( + wp.to_torch(cube_object.data.root_pos_w), mean_nodal_pos_init, rtol=1e-4, atol=1e-4 + ) + + +def test_freefall_analytical(sim): + """Test that one step of free-fall matches the inertia target prediction. + + VBD computes an inertia target per substep (h = sub_dt):: + + v_new = v + g * h + x_new = x + v_new * h + + then optimizes elastic + contact potentials around it. For free-fall + (no contacts, negligible elastic forces over one step), the final + position equals the inertia target. + + Starting from rest (v_0 = 0) with N substeps of h = dt/N: + + substep 1: v_1 = g*h, dx_1 = g*h^2 (1 * g*h^2) + substep 2: v_2 = 2*g*h, dx_2 = 2*g*h^2 (2 * g*h^2) + ... + substep k: v_k = k*g*h, dx_k = k*g*h^2 (k * g*h^2) + + total dz = g*h^2 * (1 + 2 + ... + N) = g*h^2 * N*(N+1)/2 + + """ + g = -9.81 + dt = 1.0 / 60.0 + num_substeps = 2 + sub_dt = dt / num_substeps + expected_dz = g * sub_dt**2 * num_substeps * (num_substeps + 1) / 2 + + cube_object = generate_cubes_scene(num_cubes=1, height=5.0) + sim.reset() + + x0 = wp.to_torch(cube_object.data.nodal_pos_w).clone() + sim.step() + cube_object.update(sim.cfg.dt) + x1 = wp.to_torch(cube_object.data.nodal_pos_w) + + dz = x1[..., 2] - x0[..., 2] + # Every vertex should have the same Z displacement under uniform gravity + torch.testing.assert_close(dz, torch.full_like(dz, expected_dz), rtol=1e-2, atol=1e-5) + + +@pytest.mark.parametrize("num_cubes", [2, 4]) +def test_set_kinematic_targets(sim, num_cubes): + """Test setting kinematic targets for the deformable object. + + Env 0 is kinematically constrained (all vertices pinned at default positions, + flag=0). Other envs are free (flag=1) and fall under gravity. After several + steps, env 0 should stay in place while the others have fallen. + """ + cube_object = generate_cubes_scene(num_cubes=num_cubes, height=5.0) + + sim.reset() + + particles_per_body = cube_object.max_sim_vertices_per_body + default_state = wp.to_torch(cube_object.data.default_nodal_state_w) + default_pos = default_state[..., :3].clone() + + # Build kinematic target buffer: (N, V, 4) = [x, y, z, flag] + nodal_kinematic_targets = torch.zeros(num_cubes, particles_per_body, 4, device=sim.device) + + for _ in range(5): + # Restore default state + cube_object.write_nodal_state_to_sim_index(default_state) + + # Env 0: pin all vertices at default positions (flag=0 = kinematic) + nodal_kinematic_targets[0, :, :3] = default_pos[0] + nodal_kinematic_targets[0, :, 3] = 0.0 + + # Other envs: free (flag=1) + nodal_kinematic_targets[1:, :, :3] = default_pos[1:] + nodal_kinematic_targets[1:, :, 3] = 1.0 + + cube_object.write_nodal_kinematic_target_to_sim_index(nodal_kinematic_targets) + + for _ in range(20): + cube_object.write_data_to_sim() + sim.step() + cube_object.update(sim.cfg.dt) + + # Env 0 should stay at default position (kinematically constrained) + torch.testing.assert_close( + wp.to_torch(cube_object.data.nodal_pos_w)[0], + default_pos[0], + rtol=1e-5, + atol=1e-5, + ) + + # Other envs should have fallen + final_root_z = wp.to_torch(cube_object.data.root_pos_w)[1:, 2] + default_root_z = default_pos[1:, :, 2].mean(dim=1) + assert torch.all(final_root_z < default_root_z) diff --git a/source/isaaclab_contrib/test/deformable/test_rigid_deformable_coupling.py b/source/isaaclab_contrib/test/deformable/test_rigid_deformable_coupling.py new file mode 100644 index 000000000000..c466b773f8ac --- /dev/null +++ b/source/isaaclab_contrib/test/deformable/test_rigid_deformable_coupling.py @@ -0,0 +1,275 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# ignore private usage of variables warning +# pyright: reportPrivateUsage=none + + +"""Launch Isaac Sim Simulator first.""" + +from isaaclab.app import AppLauncher + +# launch omniverse app +simulation_app = AppLauncher(headless=True).app + +"""Rest everything follows.""" + +import pytest +import warp as wp + +from isaaclab_contrib.deformable import CoupledSolverCfg, DeformableObject, VBDSolverCfg, register_hooks +from isaaclab_newton.assets import Articulation +from isaaclab_newton.physics import MJWarpSolverCfg, NewtonCfg + +import isaaclab.sim as sim_utils +from isaaclab.assets.deformable_object import DeformableObjectCfg +from isaaclab.sim import SimulationCfg, build_simulation_context + +from isaaclab_assets import FRANKA_PANDA_CFG # isort:skip + +COUPLED_ONE_WAY_CFG = SimulationCfg( + dt=1.0 / 60.0, + physics=NewtonCfg( + solver_cfg=CoupledSolverCfg( + rigid_solver_cfg=MJWarpSolverCfg( + njmax=40, + nconmax=20, + ls_iterations=20, + cone="pyramidal", + impratio=1, + ls_parallel=False, + integrator="implicitfast", + ), + vbd_cfg=VBDSolverCfg( + iterations=3, + integrate_with_external_rigid_solver=True, + particle_enable_self_contact=False, + particle_collision_detection_interval=-1, + ), + soft_contact_margin=0.01, + coupling_mode="one_way", + ), + num_substeps=5, + use_cuda_graph=True, + ), +) + +COUPLED_TWO_WAY_CFG = SimulationCfg( + dt=1.0 / 60.0, + physics=NewtonCfg( + solver_cfg=CoupledSolverCfg( + rigid_solver_cfg=MJWarpSolverCfg( + njmax=40, + nconmax=20, + ls_iterations=20, + cone="pyramidal", + impratio=1, + ls_parallel=False, + integrator="implicitfast", + ), + vbd_cfg=VBDSolverCfg( + iterations=3, + integrate_with_external_rigid_solver=True, + particle_enable_self_contact=False, + particle_collision_detection_interval=-1, + ), + soft_contact_margin=0.01, + coupling_mode="two_way", + ), + num_substeps=5, + use_cuda_graph=True, + ), +) + + +def _coupled_sim_context(cfg: SimulationCfg, device="cuda:0"): + """Helper to create a coupled solver simulation context.""" + cfg.device = device + return build_simulation_context(device=device, sim_cfg=cfg, auto_add_lighting=True) + + +@pytest.fixture +def sim_one_way(): + """Create a one-way coupled solver simulation context.""" + register_hooks() + with _coupled_sim_context(COUPLED_ONE_WAY_CFG) as sim: + sim._app_control_on_stop_handle = None + yield sim + + +@pytest.fixture +def sim_two_way(): + """Create a two-way coupled solver simulation context.""" + register_hooks() + with _coupled_sim_context(COUPLED_TWO_WAY_CFG) as sim: + sim._app_control_on_stop_handle = None + yield sim + + +def generate_robot_and_two_cubes( + colliding_cube_pos: tuple = (0.3, 0.0, 1.0), + free_cube_pos: tuple = (2.0, 0.0, 1.0), + device: str = "cuda:0", +) -> tuple[Articulation, DeformableObject, DeformableObject]: + """Generate a scene with one Franka robot and two deformable cubes. + + A single env contains a robot and two cube objects at different positions. + One cube is placed above the robot arm (will collide), the other is placed + far away (falls freely). + + Args: + colliding_cube_pos: Position of the cube above the robot arm. + free_cube_pos: Position of the cube that falls freely. + device: Device to use. + + Returns: + Tuple of (robot, colliding_cube, free_cube). + """ + sim_utils.create_prim("/World/env_0", "Xform", translation=(0.0, 0.0, 0.0)) + + cfg = sim_utils.GroundPlaneCfg() + cfg.func("/World/defaultGroundPlane", cfg) + + robot_cfg = FRANKA_PANDA_CFG.replace(prim_path="/World/env_.*/Robot") + robot = Articulation(robot_cfg) + + colliding_cube = DeformableObject( + cfg=DeformableObjectCfg( + prim_path="/World/env_.*/cube_collide", + spawn=sim_utils.MeshCuboidCfg( + size=(0.05, 0.05, 0.05), + deformable_props=sim_utils.DeformableBodyPropertiesCfg(), + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.2, 0.8, 0.2)), + physics_material=sim_utils.DeformableBodyMaterialCfg( + density=500.0, + youngs_modulus=2.5e5, + poissons_ratio=0.25, + particle_radius=0.005, + ), + ), + init_state=DeformableObjectCfg.InitialStateCfg(pos=colliding_cube_pos), + ) + ) + + free_cube = DeformableObject( + cfg=DeformableObjectCfg( + prim_path="/World/env_.*/cube_free", + spawn=sim_utils.MeshCuboidCfg( + size=(0.05, 0.05, 0.05), + deformable_props=sim_utils.DeformableBodyPropertiesCfg(), + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.8, 0.2, 0.2)), + physics_material=sim_utils.DeformableBodyMaterialCfg( + density=500.0, + youngs_modulus=2.5e4, + poissons_ratio=0.25, + particle_radius=0.005, + ), + ), + init_state=DeformableObjectCfg.InitialStateCfg(pos=free_cube_pos), + ) + ) + + return robot, colliding_cube, free_cube + + +def test_smoke_one_way(sim_one_way): + """Smoke test: coupled solver (one-way) initializes and steps without crash.""" + robot, colliding_cube, free_cube = generate_robot_and_two_cubes() + sim_one_way.reset() + + assert robot.is_initialized + assert colliding_cube.is_initialized + assert free_cube.is_initialized + + initial_z_collide = wp.to_torch(colliding_cube.data.root_pos_w)[0, 2].item() + initial_z_free = wp.to_torch(free_cube.data.root_pos_w)[0, 2].item() + + for _ in range(10): + sim_one_way.step() + robot.update(sim_one_way.cfg.dt) + colliding_cube.update(sim_one_way.cfg.dt) + free_cube.update(sim_one_way.cfg.dt) + + # Both cubes should have fallen under gravity + assert wp.to_torch(colliding_cube.data.root_pos_w)[0, 2].item() < initial_z_collide - 0.01 + assert wp.to_torch(free_cube.data.root_pos_w)[0, 2].item() < initial_z_free - 0.01 + + +def test_smoke_two_way(sim_two_way): + """Smoke test: coupled solver (two-way) initializes and steps without crash.""" + robot, colliding_cube, free_cube = generate_robot_and_two_cubes() + sim_two_way.reset() + + assert robot.is_initialized + assert colliding_cube.is_initialized + assert free_cube.is_initialized + + initial_z_collide = wp.to_torch(colliding_cube.data.root_pos_w)[0, 2].item() + initial_z_free = wp.to_torch(free_cube.data.root_pos_w)[0, 2].item() + + for _ in range(10): + sim_two_way.step() + robot.update(sim_two_way.cfg.dt) + colliding_cube.update(sim_two_way.cfg.dt) + free_cube.update(sim_two_way.cfg.dt) + + # Both cubes should have fallen under gravity + assert wp.to_torch(colliding_cube.data.root_pos_w)[0, 2].item() < initial_z_collide - 0.01 + assert wp.to_torch(free_cube.data.root_pos_w)[0, 2].item() < initial_z_free - 0.01 + + +def test_deformable_deflected_by_rigid_contact(sim_one_way): + """Test that a cube falling onto the robot is deflected horizontally. + + Two cubes start at the same height (Z=1.0m). Cube 0 (env 0) is placed + above the robot arm at X=0.3m. Cube 1 (env 1) is shifted +2m in X away + from the robot so it falls freely. + + Expected timeline (dt=1/60s): + + - Steps 0-15: Both cubes in free-fall, identical Z trajectories. + No horizontal motion. Z drops from ~1.0m to ~0.65m. + - Step ~20: Cube 0 hits the robot arm at Z~0.54m. Contact deflects it + horizontally (X starts increasing). Cube 1 is still in free-fall. + - Step ~28: Cube 1 reaches the ground (Z~0.005m) and stops. Zero + horizontal displacement. + - Steps 20-40: Cube 0 continues falling while sliding off the arm, + gaining horizontal velocity. Reaches the ground around step 40. + - Steps 40-70: Cube 0 slides along the ground, decelerating due to + friction. Settles around X~1.0m. + - Steps 70+: Both cubes at rest on the ground. + + The test asserts that cube 0 has a significantly larger horizontal + displacement than cube 1 (which should be ~zero). + """ + robot, colliding_cube, free_cube = generate_robot_and_two_cubes( + colliding_cube_pos=(0.3, 0.0, 1.0), # above robot arm + free_cube_pos=(2.0, 0.0, 1.0), # far from robot, same height + ) + sim_one_way.reset() + + initial_xy_collide = wp.to_torch(colliding_cube.data.root_pos_w)[0, :2].clone() + initial_xy_free = wp.to_torch(free_cube.data.root_pos_w)[0, :2].clone() + + # Free-fall from 1.0m takes sqrt(2*1.0/9.81) ~ 0.45s ~ 27 steps at dt=1/60. + # Collision with the robot arm happens around step 20 (Z~0.5m). + # 120 steps (2s) gives ample time for collision, bounce, and settling. + for _ in range(120): + sim_one_way.step() + robot.update(sim_one_way.cfg.dt) + colliding_cube.update(sim_one_way.cfg.dt) + free_cube.update(sim_one_way.cfg.dt) + + final_xy_collide = wp.to_torch(colliding_cube.data.root_pos_w)[0, :2] + final_xy_free = wp.to_torch(free_cube.data.root_pos_w)[0, :2] + + displacement_collide = (final_xy_collide - initial_xy_collide).norm().item() + displacement_free = (final_xy_free - initial_xy_free).norm().item() + + # Colliding cube should be deflected; free cube should fall straight + assert displacement_collide > displacement_free + 0.01, ( + f"Colliding cube should be deflected more than free cube: " + f"collide={displacement_collide:.4f}, free={displacement_free:.4f}" + ) diff --git a/source/isaaclab_newton/isaaclab_newton/cloner/newton_replicate.py b/source/isaaclab_newton/isaaclab_newton/cloner/newton_replicate.py index 2b257de9aec5..a46a3349417b 100644 --- a/source/isaaclab_newton/isaaclab_newton/cloner/newton_replicate.py +++ b/source/isaaclab_newton/isaaclab_newton/cloner/newton_replicate.py @@ -64,6 +64,24 @@ def _build_newton_builder_from_mapping( # The prototype is built from env_0 in absolute world coordinates. # add_builder xforms are deltas from env_0 so positions don't get double-counted. env0_pos = positions[0] + + # Deformable prim paths are handled by per_world_builder_hooks, not add_usd. + # Resolve the regex prim_path patterns to concrete env_0 paths so add_usd + # can skip them via ignore_paths. + import re + + _deformable_ignore_paths: list[str] = [] + for entry in NewtonManager._deformable_registry: + pat = re.compile(entry.prim_path.replace(".*", "[^/]*") + "$") + for src_path in sources: + # Check if any prim under this source matches the deformable pattern + prim = stage.GetPrimAtPath(src_path) + if prim.IsValid(): + for child in Usd.PrimRange(prim): + child_path = str(child.GetPath()) + if pat.match(child_path): + _deformable_ignore_paths.append(child_path) + protos: dict[str, ModelBuilder] = {} for src_path in sources: p = NewtonManager.create_builder(up_axis=up_axis) @@ -74,6 +92,7 @@ def _build_newton_builder_from_mapping( load_visual_shapes=True, skip_mesh_approximation=True, schema_resolvers=schema_resolvers, + ignore_paths=_deformable_ignore_paths if _deformable_ignore_paths else None, ) if simplify_meshes: p.approximate_meshes("convex_hull", keep_visual_shapes=True) @@ -109,9 +128,18 @@ def _build_newton_builder_from_mapping( local_site_map[label] = [[] for _ in range(num_worlds)] for proto_shape_idx in proto_shape_indices: local_site_map[label][col].append(offset + proto_shape_idx) + + # Run per-world builder hooks (e.g. deformable body registration). + for hook in NewtonManager._per_world_builder_hooks: + hook(builder, col, positions[col].tolist()) + # end the world context builder.end_world() + # Run post-replicate hooks (e.g. builder.color() for deformable coloring). + for hook in NewtonManager._post_replicate_hooks: + hook(builder) + site_index_map = { **global_site_map, **{label: (None, per_world) for label, per_world in local_site_map.items()}, diff --git a/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py b/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py index 8a752c8d4c8e..b824cc07ec3f 100644 --- a/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py +++ b/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py @@ -11,6 +11,7 @@ import ctypes import inspect import logging +from collections.abc import Callable from typing import TYPE_CHECKING import numpy as np @@ -120,6 +121,9 @@ class NewtonManager(PhysicsManager): _newton_index_attr = "newton:index" _clone_physics_only = False _transforms_dirty: bool = False + _particles_dirty: bool = False + _newton_particle_offset_attr = "newton:particleOffset" + _newton_particle_count_attr = "newton:particleCount" # cubric GPU transform hierarchy (replaces CPU update_world_xforms) _cubric = None @@ -132,6 +136,17 @@ class NewtonManager(PhysicsManager): # Views list for assets to register their views _views: list = [] + # Deformable body registry and extension hooks. + # Experimental deformable support registers callbacks here so the manager + # and cloner can invoke them without hard-coding deformable logic. + _deformable_registry: list = [] + _solver_factories: dict[str, Callable] = {} + _particle_sync_fn: Callable | None = None + _post_finalize_model_fn: Callable | None = None + _post_start_simulation_fn: Callable | None = None + _per_world_builder_hooks: list = [] + _post_replicate_hooks: list = [] + # CL: Cloning / Replication logic # TODO: These attributes support cloning-specific logic and should be moved into a cloner class # Pending site requests from sensors. @@ -193,6 +208,8 @@ def forward(cls) -> None: def pre_render(cls) -> None: """Flush deferred Fabric writes before cameras/visualizers read the scene.""" cls.sync_transforms_to_usd() + if cls._particle_sync_fn is not None: + cls._particle_sync_fn() @classmethod def sync_transforms_to_usd(cls) -> None: @@ -294,13 +311,47 @@ def sync_transforms_to_usd(cls) -> None: @classmethod def _mark_transforms_dirty(cls) -> None: - """Flag that physics state has changed and Fabric needs re-sync. + """Flag that rigid-body transforms have changed and Fabric needs re-sync. - Called by :meth:`_simulate` after stepping. The actual sync is deferred - to :meth:`sync_transforms_to_usd`, which runs at render cadence. + The actual sync is deferred to :meth:`sync_transforms_to_usd`, + which runs at render cadence via :meth:`pre_render`. """ cls._transforms_dirty = True + @classmethod + def _mark_particles_dirty(cls) -> None: + """Flag that particle positions have changed and Fabric needs re-sync. + + The actual sync is deferred to the particle sync callback (if registered), + which runs at render cadence via :meth:`pre_render`. + """ + cls._particles_dirty = True + + @classmethod + def _mark_state_dirty(cls) -> None: + """Flag that all physics state has changed and Fabric needs re-sync. + + Convenience method that marks both transforms and particles dirty. + Called by :meth:`_simulate` after stepping. + """ + cls._mark_transforms_dirty() + cls._mark_particles_dirty() + + @classmethod + def register_solver_factory(cls, solver_type: str, factory: Callable) -> None: + """Register an external solver factory. + + Experimental extensions can register custom solver constructors that + will be invoked by :meth:`initialize_solver` when the configured + ``solver_type`` matches *solver_type*. + + Args: + solver_type: Solver type string that triggers this factory. + factory: Callable ``(cls, cfg_dict, solver_cfg)`` that creates + and assigns the solver on the manager. + """ + cls._solver_factories[solver_type] = factory + @classmethod def step(cls) -> None: """Step the physics simulation.""" @@ -339,7 +390,7 @@ def step(cls) -> None: if cfg is not None and cfg.use_cuda_graph and cls._graph is not None and "cuda" in device: # type: ignore[union-attr] wp.capture_launch(cls._graph) if cls._usdrt_stage is not None: - cls._mark_transforms_dirty() + cls._mark_state_dirty() else: with wp.ScopedDevice(device): cls._simulate() @@ -404,6 +455,7 @@ def clear(cls): cls._newton_stage_path = None cls._usdrt_stage = None cls._transforms_dirty = False + cls._particles_dirty = False cls._up_axis = "Z" cls._model_changes = set() cls._cl_pending_sites = {} @@ -411,6 +463,13 @@ def clear(cls): cls._pending_extended_state_attributes = set() cls._pending_extended_contact_attributes = set() cls._views = [] + cls._deformable_registry = [] + cls._solver_factories = {} + cls._particle_sync_fn = None + cls._post_finalize_model_fn = None + cls._post_start_simulation_fn = None + cls._per_world_builder_hooks = [] + cls._post_replicate_hooks = [] @classmethod def set_builder(cls, builder: ModelBuilder) -> None: @@ -646,6 +705,10 @@ def start_simulation(cls) -> None: if cls._pending_extended_contact_attributes: cls._model.request_contact_attributes(*cls._pending_extended_contact_attributes) cls._pending_extended_contact_attributes = set() + + if cls._post_finalize_model_fn is not None: + cls._post_finalize_model_fn() + cls._state_0 = cls._model.state() cls._state_1 = cls._model.state() cls._control = cls._model.control() @@ -659,23 +722,53 @@ def start_simulation(cls) -> None: import usdrt body_paths = getattr(cls._model, "body_label", None) or getattr(cls._model, "body_key", None) - if body_paths is None: - raise RuntimeError("NewtonManager: model has no body_label/body_key, skipping USD/Fabric sync for RTX.") - cls._usdrt_stage = get_current_stage(fabric=True) - for i, prim_path in enumerate(body_paths): - prim = cls._usdrt_stage.GetPrimAtPath(prim_path) - prim.CreateAttribute(cls._newton_index_attr, usdrt.Sdf.ValueTypeNames.UInt, True) - prim.GetAttribute(cls._newton_index_attr).Set(i) - # Tag with PhysicsRigidBodyAPI so cubric's eRigidBody mode - # applies Inverse propagation (preserves Newton's world - # transforms and derives local) instead of Forward. - prim.AddAppliedSchema("PhysicsRigidBodyAPI") - xformable_prim = usdrt.Rt.Xformable(prim) - if not xformable_prim.HasWorldXform(): - xformable_prim.SetWorldXformFromUsd() - - cls._mark_transforms_dirty() - cls.sync_transforms_to_usd() + if not body_paths: + logger.warning( + "NewtonManager: model has no rigid bodies (body_label/body_key is empty). " + "USD/Fabric body sync for RTX is skipped. " + "Particle-only scenes (e.g. cloth) must register their own USD mesh update." + ) + cls._usdrt_stage = None + else: + cls._usdrt_stage = get_current_stage(fabric=True) + for i, prim_path in enumerate(body_paths): + prim = cls._usdrt_stage.GetPrimAtPath(prim_path) + prim.CreateAttribute(cls._newton_index_attr, usdrt.Sdf.ValueTypeNames.UInt, True) + prim.GetAttribute(cls._newton_index_attr).Set(i) + # Tag with PhysicsRigidBodyAPI so cubric's eRigidBody mode + # applies Inverse propagation (preserves Newton's world + # transforms and derives local) instead of Forward. + prim.AddAppliedSchema("PhysicsRigidBodyAPI") + xformable_prim = usdrt.Rt.Xformable(prim) + if not xformable_prim.HasWorldXform(): + xformable_prim.SetWorldXformFromUsd() + + cls._mark_transforms_dirty() + cls.sync_transforms_to_usd() + + # Setup Fabric particle sync for deformable bodies. + if cls._deformable_registry and cls._post_start_simulation_fn is not None: + cls._post_start_simulation_fn() + + @classmethod + def _get_deformable_ignore_paths(cls) -> list[str]: + """Return USD prim paths to skip when calling ``builder.add_usd``. + + For each registered deformable body, both the simulation mesh (which + carries ``UsdPhysics.CollisionAPI``) and the visual mesh are returned. + The sim mesh must be skipped so Newton does not create a redundant + static mesh collider alongside the particles produced by + ``add_soft_mesh``. The visual mesh is skipped so Newton does not + treat it as a collider — Kit reads it directly from USD for rendering. + + Paths may contain regex patterns; Newton's ``add_usd`` matches them + via :func:`re.match`. + """ + paths: list[str] = [] + for entry in cls._deformable_registry: + paths.append(entry.sim_mesh_prim_path) + paths.append(entry.vis_mesh_prim_path) + return paths @classmethod def instantiate_builder_from_stage(cls): @@ -707,12 +800,20 @@ def instantiate_builder_from_stage(cls): schema_resolvers = [SchemaResolverNewton(), SchemaResolverPhysx()] + # Deformable sim/visual mesh paths must be skipped by ``add_usd`` + # so they don't get duplicated as static colliders. + deformable_ignore_paths = cls._get_deformable_ignore_paths() + if not env_paths: # No env Xforms — flat loading - builder.add_usd(stage, schema_resolvers=schema_resolvers) + builder.add_usd(stage, ignore_paths=deformable_ignore_paths, schema_resolvers=schema_resolvers) + for hook in cls._per_world_builder_hooks: + hook(builder, 0, [0.0, 0.0, 0.0]) + for hook in cls._post_replicate_hooks: + hook(builder) else: # Load everything except the env subtrees (ground plane, lights, etc.) - ignore_paths = [path for _, path in env_paths] + ignore_paths = [path for _, path in env_paths] + deformable_ignore_paths builder.add_usd(stage, ignore_paths=ignore_paths, schema_resolvers=schema_resolvers) # Build a prototype from the first env (all envs assumed identical) @@ -721,6 +822,7 @@ def instantiate_builder_from_stage(cls): proto.add_usd( stage, root_path=proto_path, + ignore_paths=deformable_ignore_paths, schema_resolvers=schema_resolvers, ) @@ -752,8 +854,13 @@ def instantiate_builder_from_stage(cls): local_site_map[label] = [[] for _ in range(num_worlds)] for proto_shape_idx in proto_shape_indices: local_site_map[label][col].append(offset + proto_shape_idx) + for hook in cls._per_world_builder_hooks: + hook(builder, col, list(pos)) builder.end_world() + for hook in cls._post_replicate_hooks: + hook(builder) + cls._cl_site_index_map = { **global_site_map, **{label: (None, per_world) for label, per_world in local_site_map.items()}, @@ -833,6 +940,8 @@ def initialize_solver(cls) -> None: elif cls._solver_type == "featherstone": cls._use_single_state = False cls._solver = SolverFeatherstone(cls._model, **cfg_dict) + elif cls._solver_type in cls._solver_factories: + cls._solver_factories[cls._solver_type](cls, cfg_dict, solver_cfg) else: raise ValueError(f"Invalid solver type: {cls._solver_type}") @@ -1024,6 +1133,10 @@ def _simulate_physics_only(cls) -> None: The caller (``step()``) is responsible for calling ``sync_transforms_to_usd()`` eagerly after ``wp.capture_launch``. """ + # Rebuild BVH once per step for solvers that require it (e.g. VBD cloth). + if hasattr(cls._solver, "rebuild_bvh"): + cls._solver.rebuild_bvh(cls._state_0) + if cls._needs_collision_pipeline: cls._collision_pipeline.collide(cls._state_0, cls._contacts) contacts = cls._contacts @@ -1070,12 +1183,12 @@ def _simulate(cls) -> None: """Run one simulation step with substeps and USD sync. Delegates physics work to :meth:`_simulate_physics_only` and then - marks transforms dirty for the next render-cadence sync. + marks state dirty for the next render-cadence sync. """ cls._simulate_physics_only() if cls._usdrt_stage is not None: - cls._mark_transforms_dirty() + cls._mark_state_dirty() @classmethod def get_solver_convergence_steps(cls) -> dict[str, float | int]: diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/pick_cloth/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/direct/pick_cloth/__init__.py new file mode 100644 index 000000000000..60e4873363eb --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/pick_cloth/__init__.py @@ -0,0 +1,23 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import gymnasium as gym + +from . import agents + +## +# Register Gym environments. +## + + +gym.register( + id="Isaac-Pick-Cloth-Direct-v0", + entry_point=f"{__name__}.pick_cloth_env:PickClothEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.pick_cloth_env_cfg:PickClothEnvCfg", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:PPORunnerCfg", + }, +) diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/pick_cloth/agents/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/direct/pick_cloth/agents/__init__.py new file mode 100644 index 000000000000..460a30569089 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/pick_cloth/agents/__init__.py @@ -0,0 +1,4 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/pick_cloth/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/direct/pick_cloth/agents/rsl_rl_ppo_cfg.py new file mode 100644 index 000000000000..52fb8eab25f7 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/pick_cloth/agents/rsl_rl_ppo_cfg.py @@ -0,0 +1,38 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.utils import configclass + +from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg + + +@configclass +class PPORunnerCfg(RslRlOnPolicyRunnerCfg): + num_steps_per_env = 24 + max_iterations = 300 + save_interval = 50 + experiment_name = "pick_cloth_direct" + policy = RslRlPpoActorCriticCfg( + init_noise_std=1.0, + actor_obs_normalization=False, + critic_obs_normalization=False, + actor_hidden_dims=[64, 64], + critic_hidden_dims=[64, 64], + activation="elu", + ) + algorithm = RslRlPpoAlgorithmCfg( + value_loss_coef=1.0, + use_clipped_value_loss=True, + clip_param=0.2, + entropy_coef=0.005, + num_learning_epochs=5, + num_mini_batches=4, + learning_rate=1.0e-3, + schedule="adaptive", + gamma=0.99, + lam=0.95, + desired_kl=0.01, + max_grad_norm=1.0, + ) diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/pick_cloth/pick_cloth_env.py b/source/isaaclab_tasks/isaaclab_tasks/direct/pick_cloth/pick_cloth_env.py new file mode 100644 index 000000000000..a62b66150bb9 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/pick_cloth/pick_cloth_env.py @@ -0,0 +1,477 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Pick-Cloth environment: Franka robot interacts with cloth using a coupled solver.""" + +from __future__ import annotations + +import logging +from collections.abc import Sequence + +import torch +import warp as wp + +from isaaclab_contrib.deformable import register_hooks as _register_deformable_hooks + +_register_deformable_hooks() + +from pxr import Gf, UsdGeom + +import isaaclab.sim as sim_utils +from isaaclab.assets import Articulation +from isaaclab.assets.deformable_object import DeformableObject +from isaaclab.envs import DirectRLEnv +from isaaclab.sim.spawners.from_files import GroundPlaneCfg, spawn_ground_plane +from isaaclab.sim.spawners.shapes import SphereCfg, spawn_sphere +from isaaclab.sim.utils.stage import get_current_stage + +from .pick_cloth_env_cfg import PickClothEnvCfg + +logger = logging.getLogger(__name__) + + +class PickClothEnv(DirectRLEnv): + cfg: PickClothEnvCfg + + def __init__(self, cfg: PickClothEnvCfg, render_mode: str | None = None, **kwargs): + self._has_robot = cfg.robot_cfg is not None + + # Without a robot the coupled solver (rigid + VBD) is unnecessary and will + # fail because there are no rigid bodies. Swap to VBD-only automatically. + if not self._has_robot: + from isaaclab_contrib.deformable.newton_manager_cfg import CoupledSolverCfg + + physics_cfg = cfg.sim.physics + if hasattr(physics_cfg, "solver_cfg") and isinstance(physics_cfg.solver_cfg, CoupledSolverCfg): + physics_cfg.solver_cfg = physics_cfg.solver_cfg.vbd_cfg + + # For velocity control, override actuator gains before the robot is spawned: + # zero stiffness (no position tracking), high damping (velocity-tracking gain). + # Featherstone torque: tau = ke*(pos_target - q) + kd*(vel_target - qd) + # With ke=0: tau = kd*(vel_target - qd) -- proportional velocity control. + if self._has_robot and cfg.control_mode == "velocity": + for actuator in cfg.robot_cfg.actuators.values(): + actuator.stiffness = 0.0 + actuator.damping = 200.0 + + super().__init__(cfg, render_mode, **kwargs) + + if self._has_robot: + self._arm_joint_idx, _ = self.robot.find_joints(self.cfg.arm_joint_names) + self._default_joint_pos = wp.to_torch(self.robot.data.default_joint_pos).clone() + + self.joint_pos = wp.to_torch(self.robot.data.joint_pos) + self.joint_vel = wp.to_torch(self.robot.data.joint_vel) + + # Find EE body index for reward computation + ee_body_idx, _ = self.robot.find_bodies("panda_hand") + self._ee_body_idx = int(ee_body_idx[0]) + + # Keyboard controls (Newton viewer) + self._request_reset = False + self._gripper_closed = False + self._reset_key_registered = False + self._newton_viewer_gl = None + + # Finger joint indices and limits + if self._has_robot: + self._finger_joint_idx, _ = self.robot.find_joints(["panda_finger_joint1", "panda_finger_joint2"]) + + # ------------------------------------------------------------------ + # Optional interactive IK -- Newton backend only + # ------------------------------------------------------------------ + self._ik_available = False + if self._has_robot and cfg.interactive_ik: + self._setup_interactive_ik() + + logger.info( + "PickClothEnv: has_robot=%s, control_mode=%s, action_scale=%s, interactive_ik=%s", + self._has_robot, + self.cfg.control_mode, + cfg.action_scale, + self._ik_available, + ) + + _SPHERE_PRIM_PATH = "/World/ik_target" + + def _setup_interactive_ik(self): + """Initialize Newton IK solver and the draggable target sphere.""" + try: + import newton + import newton.ik as ik + from isaaclab_newton.physics import NewtonManager + + newton_model = NewtonManager._model + if newton_model is None: + logger.info("[PickClothEnv] Newton model not available; IK disabled.") + return + + ee_body_idx, _ = self.robot.find_bodies("panda_hand") + self._ee_ik_index = int(ee_body_idx[0]) + + # Write robot default joint positions into model before FK + default_jpos = wp.to_torch(self.robot.data.default_joint_pos)[0] + joint_q_torch = wp.to_torch(newton_model.joint_q) + n_robot = min(default_jpos.shape[0], joint_q_torch.shape[0]) + joint_q_torch[:n_robot] = default_jpos[:n_robot] + + # Compute initial EE transform via FK + ik_state = newton_model.state() + newton.eval_fk(newton_model, newton_model.joint_q, newton_model.joint_qd, ik_state) + body_q_np = ik_state.body_q.numpy() + self._ee_tf = wp.transform(*body_q_np[self._ee_ik_index]) + ee_pos = wp.transform_get_translation(self._ee_tf) + + # IK objectives + ee_rot = wp.transform_get_rotation(self._ee_tf) + self._pos_obj = ik.IKObjectivePosition( + link_index=self._ee_ik_index, + link_offset=wp.vec3(0.0, 0.0, 0.0), + target_positions=wp.array([ee_pos], dtype=wp.vec3), + ) + self._rot_obj = ik.IKObjectiveRotation( + link_index=self._ee_ik_index, + link_offset_rotation=wp.quat_identity(), + target_rotations=wp.array([wp.vec4(ee_rot[0], ee_rot[1], ee_rot[2], ee_rot[3])], dtype=wp.vec4), + ) + self._joint_limit_obj = ik.IKObjectiveJointLimit( + joint_limit_lower=newton_model.joint_limit_lower, + joint_limit_upper=newton_model.joint_limit_upper, + weight=0.0, + ) + + self._ik_joint_q = wp.array(newton_model.joint_q, shape=(1, newton_model.joint_coord_count)) + self._ik_solver = ik.IKSolver( + model=newton_model, + n_problems=1, + objectives=[self._pos_obj, self._rot_obj, self._joint_limit_obj], + jacobian_mode=ik.IKJacobianType.ANALYTIC, + ) + self._newton_model = newton_model + self._ik_available = True + self._newton_viewer_gl = None # lazily resolved in _apply_action + logger.info("[PickClothEnv] Newton IK initialized (EE index=%d)", self._ee_ik_index) + + # Spawn IK target sphere and teleport to EE position + self._stage = get_current_stage() + spawn_sphere( + self._SPHERE_PRIM_PATH, + SphereCfg( + radius=0.05, + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 0.0, 0.0)), + ), + translation=(float(ee_pos[0]), float(ee_pos[1]), float(ee_pos[2])), + ) + self._sphere_prim = self._stage.GetPrimAtPath(self._SPHERE_PRIM_PATH) + except Exception as exc: + logger.info("[PickClothEnv] IK not available: %s", exc) + + def _apply_ik_action(self): + """Read gizmo target, solve IK, and set joint position targets.""" + # Lazily find Newton viewer for gizmo + if self._newton_viewer_gl is None: + try: + from isaaclab_visualizers.newton import NewtonVisualizer + + for v in self.sim.visualizers: + if isinstance(v, NewtonVisualizer) and v._viewer is not None: + self._newton_viewer_gl = v._viewer + break + except Exception: + pass + + if self._newton_viewer_gl is not None: + self._register_reset_key() + + # Patch begin_frame to register the draggable gizmo each render step + _orig_bf = self._newton_viewer_gl.begin_frame + _tf = self._ee_tf + _viewer = self._newton_viewer_gl + + def _begin_frame_with_gizmo(time, _orig=_orig_bf, _v=_viewer, _t=_tf): + _orig(time) + _v.log_gizmo("ik_target", _t) + + self._newton_viewer_gl.begin_frame = _begin_frame_with_gizmo + logger.info("[PickClothEnv] Newton viewer gizmo registered") + else: + logger.warning("[PickClothEnv] NewtonViewerGL not found in sim.visualizers") + + if self._newton_viewer_gl is not None: + # Render a red sphere at the IK target position + device = self._newton_viewer_gl.device + target_pos_vec = wp.transform_get_translation(self._ee_tf) + self._newton_viewer_gl.log_points( + "ik_target_sphere", + points=wp.array([target_pos_vec], dtype=wp.vec3, device=device), + radii=wp.array([0.05], dtype=wp.float32, device=device), + colors=wp.array([wp.vec3(1.0, 0.0, 0.0)], dtype=wp.vec3, device=device), + ) + + # Sync USD sphere prim to the (possibly gizmo-updated) target position + target_pos = wp.transform_get_translation(self._ee_tf) + xform = UsdGeom.Xformable(self._sphere_prim) + for op in xform.GetOrderedXformOps(): + if op.GetOpType() == UsdGeom.XformOp.TypeTranslate: + op.Set(Gf.Vec3d(float(target_pos[0]), float(target_pos[1]), float(target_pos[2]))) + break + + # Warm-start IK from current joint state + current_q = wp.to_torch(self.robot.data.joint_pos)[0] + ik_q_torch = wp.to_torch(self._ik_joint_q) + n_robot = current_q.shape[0] + ik_q_torch[0, :n_robot] = current_q[:n_robot] + + self._pos_obj.set_target_position(0, wp.transform_get_translation(self._ee_tf)) + ee_rot = wp.transform_get_rotation(self._ee_tf) + self._rot_obj.set_target_rotation(0, ee_rot) + self._ik_solver.step(self._ik_joint_q, self._ik_joint_q, iterations=24) + + solved_arm_q = wp.to_torch(self._ik_joint_q)[0, self._arm_joint_idx] + self.robot.set_joint_position_target_index(target=solved_arm_q.unsqueeze(0), joint_ids=self._arm_joint_idx) + + # Finger control: closed=0.0, open=0.04 (joint limits) + self._apply_finger_targets() + + def _apply_finger_targets(self): + """Set finger joint targets based on gripper state (G key toggle).""" + finger_pos = 0.0 if self._gripper_closed else 0.04 + finger_target = torch.full( + (self.num_envs, len(self._finger_joint_idx)), + finger_pos, + dtype=torch.float32, + device=self.device, + ) + self.robot.set_joint_position_target_index(target=finger_target, joint_ids=self._finger_joint_idx) + + def _try_find_viewer_for_reset_key(self): + """Lazily find Newton viewer and register R-key reset (non-IK path).""" + if self._reset_key_registered: + return + try: + from isaaclab_visualizers.newton import NewtonVisualizer + + for v in self.sim.visualizers: + if isinstance(v, NewtonVisualizer) and v._viewer is not None: + self._newton_viewer_gl = v._viewer + self._register_reset_key() + return + except Exception: + pass + + def _register_reset_key(self): + """Register R key to trigger environment reset in Newton viewer.""" + if self._reset_key_registered or self._newton_viewer_gl is None: + return + import pyglet.window.key as key + + def _on_key(symbol, modifiers, _self=self): + if symbol == key.R: + _self._request_reset = True + logger.info("[PickClothEnv] Reset requested via R key") + elif symbol == key.G: + _self._gripper_closed = not _self._gripper_closed + logger.info("[PickClothEnv] Gripper %s via G key", "closed" if _self._gripper_closed else "open") + + self._newton_viewer_gl.renderer.register_key_press(_on_key) + self._reset_key_registered = True + logger.info("[PickClothEnv] R key (reset) and G key (gripper toggle) registered") + + def _setup_scene(self): + # Robot (optional) + if self._has_robot: + self.robot = Articulation(self.cfg.robot_cfg) + + # Cloth asset (triangle surface mesh) + self.cloth = DeformableObject(self.cfg.cloth) + + # Ground plane + spawn_ground_plane(prim_path="/World/ground", cfg=GroundPlaneCfg()) + + light_cfg = sim_utils.DomeLightCfg(intensity=2000.0, color=(0.75, 0.75, 0.75)) + light_cfg.func("/World/Light", light_cfg) + + # Clone environments + self.scene.clone_environments(copy_from_source=False) + if self._has_robot: + self.scene.articulations["robot"] = self.robot + + if self._has_robot and self.cfg.disable_robot_ground_collision: + self._register_ground_collision_disable() + + def _register_ground_collision_disable(self): + """Register a PHYSICS_READY callback to disable ground-robot collision. + + Sets the ground plane's ``shape_collision_group`` to 0 in Newton, which + disables rigid-body collisions while keeping soft (particle) contacts active. + """ + from isaaclab_newton.physics import NewtonManager + + from isaaclab.physics import PhysicsEvent + + def _disable(payload=None): + model = NewtonManager._model + if model is None: + return + labels = list(model.shape_label) + groups = model.shape_collision_group.numpy() + for i, label in enumerate(labels): + if "ground" in label.lower() or "defaultgroundplane" in label.lower(): + groups[i] = 0 + model.shape_collision_group.assign(wp.array(groups, dtype=int, device=model.shape_collision_group.device)) + logger.info( + "Disabled ground-robot collision for shapes: %s", + [labels[i] for i, g in enumerate(groups) if g == 0], + ) + + NewtonManager.register_callback(_disable, PhysicsEvent.PHYSICS_READY) + + # --- RL interface --- + + def _pre_physics_step(self, actions: torch.Tensor) -> None: + self.actions = actions.clone() + + def _apply_action(self) -> None: + # Lazily register R-key reset when Newton viewer is available (non-IK path) + if not self._reset_key_registered and not self._ik_available: + self._try_find_viewer_for_reset_key() + + if not self._has_robot: + return + if self._ik_available: + self._apply_ik_action() + return + if self.cfg.control_mode == "velocity": + # Velocity control: actions are target joint velocities [rad/s] + vel_targets = self.actions * self.cfg.action_scale + self.robot.set_joint_velocity_target_index(target=vel_targets, joint_ids=self._arm_joint_idx) + else: + # Position control: actions are offsets from default pose [rad] + pos_targets = self._default_joint_pos[:, self._arm_joint_idx] + self.actions * self.cfg.action_scale + self.robot.set_joint_position_target_index(target=pos_targets, joint_ids=self._arm_joint_idx) + self._apply_finger_targets() + + def _get_observations(self) -> dict: + self.cloth.update(self.step_dt) + + # Cloth centroid: mean of all nodal positions + nodal_pos = wp.to_torch(self.cloth.data.nodal_pos_w) # (num_envs, num_particles, 3) + self._cloth_centroid = nodal_pos.mean(dim=1) # (num_envs, 3) + + if self._has_robot: + obs = torch.cat( + ( + self.joint_pos[:, self._arm_joint_idx], # (num_envs, 7) + self.joint_vel[:, self._arm_joint_idx], # (num_envs, 7) + self._cloth_centroid, # (num_envs, 3) + ), + dim=-1, + ) + else: + obs = self._cloth_centroid # (num_envs, 3) + return {"policy": obs} + + def _get_rewards(self) -> torch.Tensor: + # Cloth height reward -- encourage lifting + cloth_height = self._cloth_centroid[:, 2] # z-component + rew_cloth_height = self.cfg.rew_scale_cloth_height * cloth_height + + if self._has_robot: + # EE-to-cloth distance penalty -- encourage reaching toward cloth + ee_pos = wp.to_torch(self.robot.data.body_pos_w)[:, self._ee_body_idx] # (num_envs, 3) + ee_cloth_dist = torch.norm(ee_pos - self._cloth_centroid, dim=-1) + rew_ee_cloth_dist = self.cfg.rew_scale_ee_cloth_dist * ee_cloth_dist + + # Joint velocity penalty + rew_joint_vel = self.cfg.rew_scale_joint_vel * torch.sum( + torch.abs(self.joint_vel[:, self._arm_joint_idx]), dim=-1 + ) + return rew_cloth_height + rew_ee_cloth_dist + rew_joint_vel + + return rew_cloth_height + + def _get_dones(self) -> tuple[torch.Tensor, torch.Tensor]: + if self._has_robot: + self.joint_pos = wp.to_torch(self.robot.data.joint_pos) + self.joint_vel = wp.to_torch(self.robot.data.joint_vel) + + time_out = self.episode_length_buf >= self.max_episode_length - 1 + + # R key in Newton viewer triggers reset for all envs + if self._request_reset: + time_out = torch.ones_like(time_out) + self._request_reset = False + + terminated = torch.zeros_like(time_out) + return terminated, time_out + + def _reset_idx(self, env_ids: Sequence[int] | None): + if env_ids is None or len(env_ids) == 0: + return + super()._reset_idx(env_ids) + + if self._has_robot: + # Reset robot joint state to defaults + joint_pos = wp.to_torch(self.robot.data.default_joint_pos)[env_ids].clone() + joint_vel = wp.to_torch(self.robot.data.default_joint_vel)[env_ids].clone() + + # Root pose -- add env origins for world frame + default_root_pose = wp.to_torch(self.robot.data.default_root_pose)[env_ids].clone() + default_root_pose[:, :3] += self.scene.env_origins[env_ids] + default_root_vel = wp.to_torch(self.robot.data.default_root_vel)[env_ids].clone() + + # Update cached views + self.joint_pos[env_ids] = joint_pos + self.joint_vel[env_ids] = joint_vel + + # Write robot state to simulation + self.robot.write_root_pose_to_sim_index(root_pose=default_root_pose, env_ids=env_ids) + self.robot.write_root_velocity_to_sim_index(root_velocity=default_root_vel, env_ids=env_ids) + self.robot.write_joint_position_to_sim_index(position=joint_pos, env_ids=env_ids) + self.robot.write_joint_velocity_to_sim_index(velocity=joint_vel, env_ids=env_ids) + + # Run FK so body_q matches the reset joint_q before the next simulation step. + self.sim.forward() + + # Sync rigid-body state from state_0 to state_1 for reset envs only. + # VBD (with integrate_with_external_rigid_solver=True) reads body_q from state_out, + # which is state_1 in the first substep. Without this sync, state_1 retains the + # pre-reset robot pose and cloth particles see stale body contacts. + from isaaclab_newton.physics import NewtonManager as SimulationManager + + s0 = SimulationManager.get_state_0() + s1 = SimulationManager.get_state_1() + model = SimulationManager.get_model() + + if s0.body_q is not None and s1.body_q is not None: + bodies_per_env = model.body_count // model.world_count + s0_bq = wp.to_torch(s0.body_q).view(model.world_count, bodies_per_env, -1) + s1_bq = wp.to_torch(s1.body_q).view(model.world_count, bodies_per_env, -1) + s1_bq[env_ids] = s0_bq[env_ids] + + if s0.body_qd is not None and s1.body_qd is not None: + bodies_per_env = model.body_count // model.world_count + s0_bqd = wp.to_torch(s0.body_qd).view(model.world_count, bodies_per_env, -1) + s1_bqd = wp.to_torch(s1.body_qd).view(model.world_count, bodies_per_env, -1) + s1_bqd[env_ids] = s0_bqd[env_ids] + + if s0.joint_q is not None and s1.joint_q is not None: + dofs_per_env = model.joint_dof_count // model.world_count + s0_jq = wp.to_torch(s0.joint_q).view(model.world_count, dofs_per_env) + s1_jq = wp.to_torch(s1.joint_q).view(model.world_count, dofs_per_env) + s1_jq[env_ids] = s0_jq[env_ids] + + if s0.joint_qd is not None and s1.joint_qd is not None: + dofs_per_env = model.joint_dof_count // model.world_count + s0_jqd = wp.to_torch(s0.joint_qd).view(model.world_count, dofs_per_env) + s1_jqd = wp.to_torch(s1.joint_qd).view(model.world_count, dofs_per_env) + s1_jqd[env_ids] = s0_jqd[env_ids] + + # Reset cloth: restore default nodal state (positions + zero velocities). + # reset() is a no-op (matches PhysX convention); state is restored via write. + env_ids_list = env_ids.cpu().tolist() if hasattr(env_ids, "cpu") else list(env_ids) + default_state = wp.to_torch(self.cloth.data.default_nodal_state_w) + self.cloth.write_nodal_state_to_sim_index(default_state, env_ids=env_ids_list) + self.cloth.reset(env_ids=env_ids_list) diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/pick_cloth/pick_cloth_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/direct/pick_cloth/pick_cloth_env_cfg.py new file mode 100644 index 000000000000..6e31fc663346 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/pick_cloth/pick_cloth_env_cfg.py @@ -0,0 +1,206 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configuration for the Pick-Cloth environment: Franka robot + cloth with coupled solver.""" + +import importlib.util +import os.path + +from isaaclab_contrib.deformable.newton_manager_cfg import CoupledSolverCfg, NewtonModelCfg, VBDSolverCfg +from isaaclab_newton.physics import FeatherstoneSolverCfg, MJWarpSolverCfg, NewtonCfg +from isaaclab_visualizers.newton import NewtonVisualizerCfg + +import isaaclab.sim as sim_utils +from isaaclab.assets.deformable_object import DeformableObjectCfg +from isaaclab.envs import DirectRLEnvCfg +from isaaclab.scene import InteractiveSceneCfg +from isaaclab.sim import SimulationCfg +from isaaclab.utils import configclass + +from isaaclab_tasks.utils import PresetCfg, preset + +from isaaclab_assets.robots.franka import FRANKA_PANDA_CFG, FRANKA_PANDA_HIGH_PD_CFG + +# Locate shirt USD from Newton package (defer import to avoid pxr before SimulationApp). +_newton_spec = importlib.util.find_spec("newton") +_SHIRT_USD = os.path.join( + os.path.dirname(_newton_spec.origin), + "examples", + "assets", + "unisex_shirt.usd", +) + +@configclass +class DeformableNewtonCfg(NewtonCfg): + """NewtonCfg extended with model-level contact parameters for deformable objects. + + Uses a distinct class name so that ``_is_kitless_physics`` does not + match it, ensuring Kit is launched for USD deformable spawning. + """ + + model_cfg: NewtonModelCfg | None = None + """Global Newton model parameters applied after builder finalization.""" + + +MODEL_CFG = NewtonModelCfg( + soft_contact_ke=1e4, + soft_contact_kd=1e-2, + soft_contact_mu=0.5, +) + + +@configclass +class PickClothPhysicsCfg(PresetCfg): + """Physics presets for the Pick-Cloth environment. + + Presets: + - ``default`` / ``newton`` / ``newton_mjwarp``: MuJoCo Warp rigid solver + VBD cloth (recommended). + - ``newton_featherstone``: Featherstone rigid solver + VBD cloth. + - ``cloth_only``: VBD cloth only, no rigid-body solver. + """ + + default: DeformableNewtonCfg = DeformableNewtonCfg( + solver_cfg=CoupledSolverCfg( + rigid_solver_cfg=MJWarpSolverCfg( + njmax=40, + nconmax=20, + ls_iterations=20, + cone="pyramidal", + impratio=1, + ls_parallel=False, + integrator="implicitfast", + ccd_iterations=100, + ), + vbd_cfg=VBDSolverCfg( + iterations=5, + particle_enable_self_contact=True, + particle_self_contact_radius=2e-3, # good for substeps=10 + particle_self_contact_margin=2e-3, + particle_topological_contact_filter_threshold=1, + particle_rest_shape_contact_exclusion_radius=0.0, + particle_vertex_contact_buffer_size=16, + particle_edge_contact_buffer_size=20, + particle_collision_detection_interval=-1, + integrate_with_external_rigid_solver=True, + ), + soft_contact_margin=0.01, + coupling_mode="one_way", + ), + model_cfg=MODEL_CFG, + num_substeps=10, + use_cuda_graph=True, + ) + + newton: DeformableNewtonCfg = default + newton_mjwarp: DeformableNewtonCfg = default + + newton_featherstone: DeformableNewtonCfg = DeformableNewtonCfg( + solver_cfg=CoupledSolverCfg( + rigid_solver_cfg=FeatherstoneSolverCfg(), + vbd_cfg=VBDSolverCfg( + iterations=5, + particle_enable_self_contact=True, + particle_self_contact_radius=1e-4, + particle_self_contact_margin=2e-3, + particle_topological_contact_filter_threshold=1, + particle_rest_shape_contact_exclusion_radius=0.0, + particle_vertex_contact_buffer_size=16, + particle_edge_contact_buffer_size=20, + particle_collision_detection_interval=-1, + integrate_with_external_rigid_solver=True, + ), + soft_contact_margin=0.01, + ), + model_cfg=MODEL_CFG, + num_substeps=30, + use_cuda_graph=True, + ) + + +@configclass +class PickClothEnvCfg(DirectRLEnvCfg): + # env + decimation = 2 + episode_length_s = 5.0 + # With robot: obs = joint_pos(7) + joint_vel(7) + cloth_centroid(3) = 17, act = 7 + # Without robot (robot_cfg=None): obs = cloth_centroid(3) = 3, act = 0 + action_space = 7 + observation_space = 17 + state_space = 0 + + # simulation + sim: SimulationCfg = SimulationCfg( + dt=1 / 60, + render_interval=decimation, + physics=PickClothPhysicsCfg(), + visualizer_cfgs=NewtonVisualizerCfg(), + ) + + # scene + scene: InteractiveSceneCfg = InteractiveSceneCfg( + num_envs=1, + env_spacing=4.0, + replicate_physics=True, + ) + + # robot (use presets=cloth_only to run without a robot) + robot_cfg = preset( + default=FRANKA_PANDA_CFG.replace(prim_path="/World/envs/env_.*/Robot"), + franka_high_pd=FRANKA_PANDA_HIGH_PD_CFG.replace(prim_path="/World/envs/env_.*/Robot"), + cloth_only=None, + ) + + # joint names to control (7 arm joints, excluding fingers) + arm_joint_names = ["panda_joint[1-7]"] + + # control mode: "position" (PD, actions are joint position offsets [rad]) + # "velocity" (P on velocity, actions are joint velocity targets [rad/s]) + control_mode: str = "position" + + # action scale applied to raw actions before use as targets + action_scale = 0.5 + + # cloth asset -- shirt mesh loaded from Newton assets + cloth: DeformableObjectCfg = DeformableObjectCfg( + prim_path="/World/envs/env_.*/cloth", + spawn=sim_utils.UsdFileCfg( + usd_path=_SHIRT_USD, + scale=(0.01, 0.01, 0.01), # shirt USD vertices are in cm -> convert to meters + deformable_props=sim_utils.DeformableBodyPropertiesCfg(), + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.2, 0.2, 0.8)), + physics_material=sim_utils.SurfaceDeformableBodyMaterialCfg( + density=0.02, + tri_ke=1e4, + tri_ka=1e4, + tri_kd=1.5e-6, + edge_ke=5.0, + edge_kd=1e-2, + particle_radius=0.01, + ), + ), + init_state=DeformableObjectCfg.InitialStateCfg( + pos=(0.9, 1.25, 0.20), # in front of robot, reachable height + rot=(1.0, 0.0, 0.0, 0.0), + ), + ) + + # disable rigid-body collision between robot and ground plane + disable_robot_ground_collision: bool = True + """When True, set the ground plane's collision group to 0 in Newton so the + robot arm does not collide with the ground. Soft (particle) contacts are + unaffected. Defaults to True.""" + + # interactive IK: when True, spawn a draggable sphere and solve IK each step + interactive_ik: bool = False + + # reward scales + rew_scale_cloth_height = 5.0 + """Reward for lifting cloth centroid higher [per m].""" + + rew_scale_ee_cloth_dist = -2.0 + """Penalty for EE-to-cloth-centroid distance [per m].""" + + rew_scale_joint_vel = -0.01 + """Penalty for joint velocities.""" diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/pick_vbd_cube/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/direct/pick_vbd_cube/__init__.py new file mode 100644 index 000000000000..c49f1466bc9e --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/pick_vbd_cube/__init__.py @@ -0,0 +1,23 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import gymnasium as gym + +from . import agents + +## +# Register Gym environments. +## + + +gym.register( + id="Isaac-Pick-VBD-Cube-Direct-v0", + entry_point=f"{__name__}.pick_vbd_cube_env:PickVBDCubeEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.pick_vbd_cube_env_cfg:PickVBDCubeEnvCfg", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:PPORunnerCfg", + }, +) diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/pick_vbd_cube/agents/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/direct/pick_vbd_cube/agents/__init__.py new file mode 100644 index 000000000000..460a30569089 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/pick_vbd_cube/agents/__init__.py @@ -0,0 +1,4 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/pick_vbd_cube/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/direct/pick_vbd_cube/agents/rsl_rl_ppo_cfg.py new file mode 100644 index 000000000000..7d6515919c44 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/pick_vbd_cube/agents/rsl_rl_ppo_cfg.py @@ -0,0 +1,38 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.utils import configclass + +from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg + + +@configclass +class PPORunnerCfg(RslRlOnPolicyRunnerCfg): + num_steps_per_env = 24 + max_iterations = 300 + save_interval = 50 + experiment_name = "pick_vbd_cube_direct" + policy = RslRlPpoActorCriticCfg( + init_noise_std=1.0, + actor_obs_normalization=False, + critic_obs_normalization=False, + actor_hidden_dims=[64, 64], + critic_hidden_dims=[64, 64], + activation="elu", + ) + algorithm = RslRlPpoAlgorithmCfg( + value_loss_coef=1.0, + use_clipped_value_loss=True, + clip_param=0.2, + entropy_coef=0.005, + num_learning_epochs=5, + num_mini_batches=4, + learning_rate=1.0e-3, + schedule="adaptive", + gamma=0.99, + lam=0.95, + desired_kl=0.01, + max_grad_norm=1.0, + ) diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/pick_vbd_cube/pick_vbd_cube_env.py b/source/isaaclab_tasks/isaaclab_tasks/direct/pick_vbd_cube/pick_vbd_cube_env.py new file mode 100644 index 000000000000..279471be8a70 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/pick_vbd_cube/pick_vbd_cube_env.py @@ -0,0 +1,387 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Pick-VBD-Cube environment: Franka robot interacts with a deformable cube using a coupled solver.""" + +from __future__ import annotations + +import logging +from collections.abc import Sequence + +import torch +import warp as wp + +from isaaclab_contrib.deformable import register_hooks as _register_deformable_hooks + +_register_deformable_hooks() + +from pxr import Gf, UsdGeom + +import isaaclab.sim as sim_utils +from isaaclab.assets import Articulation +from isaaclab.assets.deformable_object import DeformableObject +from isaaclab.envs import DirectRLEnv +from isaaclab.sim.spawners.from_files import GroundPlaneCfg, spawn_ground_plane +from isaaclab.sim.spawners.shapes import SphereCfg, spawn_sphere +from isaaclab.sim.utils.stage import get_current_stage + +from .pick_vbd_cube_env_cfg import PickVBDCubeEnvCfg + +logger = logging.getLogger(__name__) + + +class PickVBDCubeEnv(DirectRLEnv): + cfg: PickVBDCubeEnvCfg + + def __init__(self, cfg: PickVBDCubeEnvCfg, render_mode: str | None = None, **kwargs): + # For velocity control, override actuator gains before the robot is spawned + if cfg.control_mode == "velocity": + for actuator in cfg.robot_cfg.actuators.values(): + actuator.stiffness = 0.0 + actuator.damping = 200.0 + + super().__init__(cfg, render_mode, **kwargs) + + self._arm_joint_idx, _ = self.robot.find_joints(self.cfg.arm_joint_names) + self._default_joint_pos = wp.to_torch(self.robot.data.default_joint_pos).clone() + + self.joint_pos = wp.to_torch(self.robot.data.joint_pos) + self.joint_vel = wp.to_torch(self.robot.data.joint_vel) + + # Find EE body index for reward computation + ee_body_idx, _ = self.robot.find_bodies("panda_hand") + self._ee_body_idx = int(ee_body_idx[0]) + + # Keyboard controls (Newton viewer) + self._request_reset = False + self._gripper_closed = False + self._reset_key_registered = False + self._newton_viewer_gl = None + + # Finger joint indices + self._finger_joint_idx, _ = self.robot.find_joints(["panda_finger_joint1", "panda_finger_joint2"]) + + # Optional interactive IK + self._ik_available = False + if cfg.interactive_ik: + self._setup_interactive_ik() + + logger.info( + "PickVBDCubeEnv: control_mode=%s, action_scale=%s, interactive_ik=%s", + self.cfg.control_mode, + cfg.action_scale, + self._ik_available, + ) + + _SPHERE_PRIM_PATH = "/World/ik_target" + + def _setup_interactive_ik(self): + """Initialize Newton IK solver and the draggable target sphere.""" + try: + import newton + import newton.ik as ik + from isaaclab_newton.physics import NewtonManager + + newton_model = NewtonManager._model + if newton_model is None: + logger.info("[PickVBDCubeEnv] Newton model not available; IK disabled.") + return + + ee_body_idx, _ = self.robot.find_bodies("panda_hand") + self._ee_ik_index = int(ee_body_idx[0]) + + default_jpos = wp.to_torch(self.robot.data.default_joint_pos)[0] + joint_q_torch = wp.to_torch(newton_model.joint_q) + n_robot = min(default_jpos.shape[0], joint_q_torch.shape[0]) + joint_q_torch[:n_robot] = default_jpos[:n_robot] + + ik_state = newton_model.state() + newton.eval_fk(newton_model, newton_model.joint_q, newton_model.joint_qd, ik_state) + body_q_np = ik_state.body_q.numpy() + self._ee_tf = wp.transform(*body_q_np[self._ee_ik_index]) + ee_pos = wp.transform_get_translation(self._ee_tf) + + ee_rot = wp.transform_get_rotation(self._ee_tf) + self._pos_obj = ik.IKObjectivePosition( + link_index=self._ee_ik_index, + link_offset=wp.vec3(0.0, 0.0, 0.0), + target_positions=wp.array([ee_pos], dtype=wp.vec3), + ) + self._rot_obj = ik.IKObjectiveRotation( + link_index=self._ee_ik_index, + link_offset_rotation=wp.quat_identity(), + target_rotations=wp.array([wp.vec4(ee_rot[0], ee_rot[1], ee_rot[2], ee_rot[3])], dtype=wp.vec4), + ) + self._joint_limit_obj = ik.IKObjectiveJointLimit( + joint_limit_lower=newton_model.joint_limit_lower, + joint_limit_upper=newton_model.joint_limit_upper, + weight=0.0, + ) + + self._ik_joint_q = wp.array(newton_model.joint_q, shape=(1, newton_model.joint_coord_count)) + self._ik_solver = ik.IKSolver( + model=newton_model, + n_problems=1, + objectives=[self._pos_obj, self._rot_obj, self._joint_limit_obj], + jacobian_mode=ik.IKJacobianType.ANALYTIC, + ) + self._newton_model = newton_model + self._ik_available = True + self._newton_viewer_gl = None + logger.info("[PickVBDCubeEnv] Newton IK initialized (EE index=%d)", self._ee_ik_index) + + self._stage = get_current_stage() + spawn_sphere( + self._SPHERE_PRIM_PATH, + SphereCfg( + radius=0.05, + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 0.0, 0.0)), + ), + translation=(float(ee_pos[0]), float(ee_pos[1]), float(ee_pos[2])), + ) + self._sphere_prim = self._stage.GetPrimAtPath(self._SPHERE_PRIM_PATH) + except Exception as exc: + logger.info("[PickVBDCubeEnv] IK not available: %s", exc) + + def _apply_ik_action(self): + """Read gizmo target, solve IK, and set joint position targets.""" + if self._newton_viewer_gl is None: + try: + from isaaclab_visualizers.newton import NewtonVisualizer + + for v in self.sim.visualizers: + if isinstance(v, NewtonVisualizer) and v._viewer is not None: + self._newton_viewer_gl = v._viewer + break + except Exception: + pass + + if self._newton_viewer_gl is not None: + self._register_reset_key() + + _orig_bf = self._newton_viewer_gl.begin_frame + _tf = self._ee_tf + _viewer = self._newton_viewer_gl + + def _begin_frame_with_gizmo(time, _orig=_orig_bf, _v=_viewer, _t=_tf): + _orig(time) + _v.log_gizmo("ik_target", _t) + + self._newton_viewer_gl.begin_frame = _begin_frame_with_gizmo + logger.info("[PickVBDCubeEnv] Newton viewer gizmo registered") + else: + logger.warning("[PickVBDCubeEnv] NewtonViewerGL not found in sim.visualizers") + + if self._newton_viewer_gl is not None: + device = self._newton_viewer_gl.device + target_pos_vec = wp.transform_get_translation(self._ee_tf) + self._newton_viewer_gl.log_points( + "ik_target_sphere", + points=wp.array([target_pos_vec], dtype=wp.vec3, device=device), + radii=wp.array([0.05], dtype=wp.float32, device=device), + colors=wp.array([wp.vec3(1.0, 0.0, 0.0)], dtype=wp.vec3, device=device), + ) + + target_pos = wp.transform_get_translation(self._ee_tf) + xform = UsdGeom.Xformable(self._sphere_prim) + for op in xform.GetOrderedXformOps(): + if op.GetOpType() == UsdGeom.XformOp.TypeTranslate: + op.Set(Gf.Vec3d(float(target_pos[0]), float(target_pos[1]), float(target_pos[2]))) + break + + current_q = wp.to_torch(self.robot.data.joint_pos)[0] + ik_q_torch = wp.to_torch(self._ik_joint_q) + n_robot = current_q.shape[0] + ik_q_torch[0, :n_robot] = current_q[:n_robot] + + self._pos_obj.set_target_position(0, wp.transform_get_translation(self._ee_tf)) + ee_rot = wp.transform_get_rotation(self._ee_tf) + self._rot_obj.set_target_rotation(0, ee_rot) + self._ik_solver.step(self._ik_joint_q, self._ik_joint_q, iterations=24) + + solved_arm_q = wp.to_torch(self._ik_joint_q)[0, self._arm_joint_idx] + self.robot.set_joint_position_target_index(target=solved_arm_q.unsqueeze(0), joint_ids=self._arm_joint_idx) + self._apply_finger_targets() + + def _apply_finger_targets(self): + """Set finger joint targets based on gripper state (G key toggle).""" + finger_pos = 0.0 if self._gripper_closed else 0.04 + finger_target = torch.full( + (self.num_envs, len(self._finger_joint_idx)), + finger_pos, + dtype=torch.float32, + device=self.device, + ) + self.robot.set_joint_position_target_index(target=finger_target, joint_ids=self._finger_joint_idx) + + def _try_find_viewer_for_reset_key(self): + """Lazily find Newton viewer and register R-key reset (non-IK path).""" + if self._reset_key_registered: + return + try: + from isaaclab_visualizers.newton import NewtonVisualizer + + for v in self.sim.visualizers: + if isinstance(v, NewtonVisualizer) and v._viewer is not None: + self._newton_viewer_gl = v._viewer + self._register_reset_key() + return + except Exception: + pass + + def _register_reset_key(self): + """Register R key to trigger environment reset in Newton viewer.""" + if self._reset_key_registered or self._newton_viewer_gl is None: + return + import pyglet.window.key as key + + def _on_key(symbol, modifiers, _self=self): + if symbol == key.R: + _self._request_reset = True + logger.info("[PickVBDCubeEnv] Reset requested via R key") + elif symbol == key.G: + _self._gripper_closed = not _self._gripper_closed + logger.info("[PickVBDCubeEnv] Gripper %s via G key", "closed" if _self._gripper_closed else "open") + + self._newton_viewer_gl.renderer.register_key_press(_on_key) + self._reset_key_registered = True + logger.info("[PickVBDCubeEnv] R key (reset) and G key (gripper toggle) registered") + + def _setup_scene(self): + self.robot = Articulation(self.cfg.robot_cfg) + self.cube = DeformableObject(self.cfg.cube) + + spawn_ground_plane(prim_path="/World/ground", cfg=GroundPlaneCfg()) + + light_cfg = sim_utils.DomeLightCfg(intensity=2000.0, color=(0.75, 0.75, 0.75)) + light_cfg.func("/World/Light", light_cfg) + + self.scene.clone_environments(copy_from_source=False) + self.scene.articulations["robot"] = self.robot + + if self.cfg.disable_robot_ground_collision: + self._register_ground_collision_disable() + + def _register_ground_collision_disable(self): + """Register a PHYSICS_READY callback to disable ground-robot collision. + + Sets the ground plane's ``shape_collision_group`` to 0 in Newton, which + disables rigid-body collisions while keeping soft (particle) contacts active. + """ + from isaaclab_newton.physics import NewtonManager + + from isaaclab.physics import PhysicsEvent + + def _disable(payload=None): + model = NewtonManager._model + if model is None: + return + labels = list(model.shape_label) + groups = model.shape_collision_group.numpy() + for i, label in enumerate(labels): + if "ground" in label.lower() or "defaultgroundplane" in label.lower(): + groups[i] = 0 + model.shape_collision_group.assign(wp.array(groups, dtype=int, device=model.shape_collision_group.device)) + logger.info( + "Disabled ground-robot collision for shapes: %s", + [labels[i] for i, g in enumerate(groups) if g == 0], + ) + + NewtonManager.register_callback(_disable, PhysicsEvent.PHYSICS_READY) + + # --- RL interface --- + + def _pre_physics_step(self, actions: torch.Tensor) -> None: + self.actions = actions.clone() + + def _apply_action(self) -> None: + if not self._reset_key_registered and not self._ik_available: + self._try_find_viewer_for_reset_key() + + if self._ik_available: + self._apply_ik_action() + return + if self.cfg.control_mode == "velocity": + vel_targets = self.actions * self.cfg.action_scale + self.robot.set_joint_velocity_target_index(target=vel_targets, joint_ids=self._arm_joint_idx) + else: + pos_targets = self._default_joint_pos[:, self._arm_joint_idx] + self.actions * self.cfg.action_scale + self.robot.set_joint_position_target_index(target=pos_targets, joint_ids=self._arm_joint_idx) + self._apply_finger_targets() + + def _get_observations(self) -> dict: + self.cube.update(self.step_dt) + + # Cube centroid: mean of all nodal positions + nodal_pos = wp.to_torch(self.cube.data.nodal_pos_w) # (num_envs, num_particles, 3) + self._cube_centroid = nodal_pos.mean(dim=1) # (num_envs, 3) + self._object_pos = self._cube_centroid + + obs = torch.cat( + ( + self.joint_pos[:, self._arm_joint_idx], # (num_envs, 7) + self.joint_vel[:, self._arm_joint_idx], # (num_envs, 7) + self._cube_centroid, # (num_envs, 3) + ), + dim=-1, + ) + return {"policy": obs} + + def _get_rewards(self) -> torch.Tensor: + # Cube height reward + cube_height = self._cube_centroid[:, 2] + rew_cube_height = self.cfg.rew_scale_cube_height * cube_height + + # EE-to-cube distance penalty + ee_pos = wp.to_torch(self.robot.data.body_pos_w)[:, self._ee_body_idx] + ee_cube_dist = torch.norm(ee_pos - self._cube_centroid, dim=-1) + rew_ee_cube_dist = self.cfg.rew_scale_ee_cube_dist * ee_cube_dist + + # Joint velocity penalty + rew_joint_vel = self.cfg.rew_scale_joint_vel * torch.sum( + torch.abs(self.joint_vel[:, self._arm_joint_idx]), dim=-1 + ) + return rew_cube_height + rew_ee_cube_dist + rew_joint_vel + + def _get_dones(self) -> tuple[torch.Tensor, torch.Tensor]: + self.joint_pos = wp.to_torch(self.robot.data.joint_pos) + self.joint_vel = wp.to_torch(self.robot.data.joint_vel) + + time_out = self.episode_length_buf >= self.max_episode_length - 1 + + if self._request_reset: + time_out = torch.ones_like(time_out) + self._request_reset = False + + terminated = torch.zeros_like(time_out) + return terminated, time_out + + def _reset_idx(self, env_ids: Sequence[int] | None): + if env_ids is None or len(env_ids) == 0: + return + super()._reset_idx(env_ids) + + # Reset robot + joint_pos = wp.to_torch(self.robot.data.default_joint_pos)[env_ids].clone() + joint_vel = wp.to_torch(self.robot.data.default_joint_vel)[env_ids].clone() + + default_root_pose = wp.to_torch(self.robot.data.default_root_pose)[env_ids].clone() + default_root_pose[:, :3] += self.scene.env_origins[env_ids] + default_root_vel = wp.to_torch(self.robot.data.default_root_vel)[env_ids].clone() + + self.joint_pos[env_ids] = joint_pos + self.joint_vel[env_ids] = joint_vel + + self.robot.write_root_pose_to_sim_index(root_pose=default_root_pose, env_ids=env_ids) + self.robot.write_root_velocity_to_sim_index(root_velocity=default_root_vel, env_ids=env_ids) + self.robot.write_joint_position_to_sim_index(position=joint_pos, env_ids=env_ids) + self.robot.write_joint_velocity_to_sim_index(velocity=joint_vel, env_ids=env_ids) + + # Reset deformable cube: restore default nodal state (positions + zero velocities). + # reset() is a no-op (matches PhysX convention); state is restored via write. + env_ids_list = env_ids.cpu().tolist() if hasattr(env_ids, "cpu") else list(env_ids) + default_state = wp.to_torch(self.cube.data.default_nodal_state_w) + self.cube.write_nodal_state_to_sim_index(default_state, env_ids=env_ids_list) + self.cube.reset(env_ids=env_ids_list) diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/pick_vbd_cube/pick_vbd_cube_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/direct/pick_vbd_cube/pick_vbd_cube_env_cfg.py new file mode 100644 index 000000000000..bf6fc03cfa70 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/pick_vbd_cube/pick_vbd_cube_env_cfg.py @@ -0,0 +1,173 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Configuration for the Pick-VBD-Cube environment: Franka robot + deformable cube with coupled solver.""" + +from isaaclab_contrib.deformable.newton_manager_cfg import CoupledSolverCfg, NewtonModelCfg, VBDSolverCfg +from isaaclab_newton.physics import FeatherstoneSolverCfg, MJWarpSolverCfg, NewtonCfg +from isaaclab_visualizers.newton import NewtonVisualizerCfg + +import isaaclab.sim as sim_utils +from isaaclab.assets.deformable_object import DeformableObjectCfg +from isaaclab.envs import DirectRLEnvCfg +from isaaclab.scene import InteractiveSceneCfg +from isaaclab.sim import SimulationCfg +from isaaclab.utils import configclass + +from isaaclab_tasks.utils import PresetCfg, preset + +from isaaclab_assets.robots.franka import FRANKA_PANDA_CFG, FRANKA_PANDA_HIGH_PD_CFG + +@configclass +class DeformableNewtonCfg(NewtonCfg): + """NewtonCfg extended with model-level contact parameters for deformable objects. + + Uses a distinct class name so that ``_is_kitless_physics`` does not + match it, ensuring Kit is launched for USD deformable spawning. + """ + + model_cfg: NewtonModelCfg | None = None + """Global Newton model parameters applied after builder finalization.""" + + +MODEL_CFG = NewtonModelCfg( + soft_contact_ke=1e4, + soft_contact_kd=1e-2, + soft_contact_mu=1.0, +) + + +@configclass +class PickVBDCubePhysicsCfg(PresetCfg): + """Physics presets for the Pick-VBD-Cube environment. + + Presets: + - ``default`` / ``newton`` / ``newton_mjwarp``: MuJoCo Warp rigid solver + VBD deformable (recommended). + - ``newton_featherstone``: Featherstone rigid solver + VBD deformable. + """ + + default: DeformableNewtonCfg = DeformableNewtonCfg( + solver_cfg=CoupledSolverCfg( + rigid_solver_cfg=MJWarpSolverCfg( + njmax=40, + nconmax=20, + ls_iterations=20, + cone="pyramidal", + impratio=1, + ls_parallel=False, + integrator="implicitfast", + ccd_iterations=100, + ), + vbd_cfg=VBDSolverCfg( + iterations=5, + integrate_with_external_rigid_solver=True, + particle_enable_self_contact=False, + particle_collision_detection_interval=-1, + ), + soft_contact_margin=0.01, + ), + model_cfg=MODEL_CFG, + num_substeps=10, + use_cuda_graph=True, + ) + + newton: DeformableNewtonCfg = default + newton_mjwarp: DeformableNewtonCfg = default + + newton_featherstone: DeformableNewtonCfg = DeformableNewtonCfg( + solver_cfg=CoupledSolverCfg( + rigid_solver_cfg=FeatherstoneSolverCfg(), + vbd_cfg=VBDSolverCfg( + iterations=5, + integrate_with_external_rigid_solver=True, + particle_enable_self_contact=False, + particle_collision_detection_interval=-1, + ), + soft_contact_margin=0.01, + coupling_mode="two_way", + ), + model_cfg=MODEL_CFG, + num_substeps=30, + use_cuda_graph=True, + ) + + +@configclass +class PickVBDCubeEnvCfg(DirectRLEnvCfg): + # env + decimation = 2 + episode_length_s = 5.0 + # obs = joint_pos(7) + joint_vel(7) + cube_centroid(3) = 17, act = 7 + action_space = 7 + observation_space = 17 + state_space = 0 + + # simulation + sim: SimulationCfg = SimulationCfg( + dt=1 / 60, + render_interval=decimation, + physics=PickVBDCubePhysicsCfg(), + visualizer_cfgs=NewtonVisualizerCfg(), + ) + + # scene + scene: InteractiveSceneCfg = InteractiveSceneCfg( + num_envs=1, + env_spacing=4.0, + replicate_physics=True, + ) + + # robot + robot_cfg = preset( + default=FRANKA_PANDA_CFG.replace(prim_path="/World/envs/env_.*/Robot"), + franka_high_pd=FRANKA_PANDA_HIGH_PD_CFG.replace(prim_path="/World/envs/env_.*/Robot"), + ) + + # joint names to control (7 arm joints, excluding fingers) + arm_joint_names = ["panda_joint[1-7]"] + + # control mode: "position" or "velocity" + control_mode: str = "position" + + # action scale applied to raw actions before use as targets + action_scale = 0.5 + + # deformable cube (VBD) + cube: DeformableObjectCfg = DeformableObjectCfg( + prim_path="/World/envs/env_.*/cube", + spawn=sim_utils.MeshCuboidCfg( + size=(0.05, 0.05, 0.05), + deformable_props=sim_utils.DeformableBodyPropertiesCfg(), + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.2, 0.8, 0.2)), + physics_material=sim_utils.DeformableBodyMaterialCfg( + density=500.0, + youngs_modulus=2.5e5, + poissons_ratio=0.25, + particle_radius=0.005, + ), + ), + init_state=DeformableObjectCfg.InitialStateCfg( + pos=(0.25, 0.0, 0.05), + ), + ) + + # disable rigid-body collision between robot and ground plane + disable_robot_ground_collision: bool = True + """When True, set the ground plane's collision group to 0 in Newton so the + robot arm does not collide with the ground. Soft (particle) contacts are + unaffected. Defaults to True.""" + + # interactive IK: when True, spawn a draggable sphere and solve IK each step + interactive_ik: bool = False + + # reward scales + rew_scale_cube_height = 5.0 + """Reward for lifting cube centroid higher [per m].""" + + rew_scale_ee_cube_dist = -2.0 + """Penalty for EE-to-cube-centroid distance [per m].""" + + rew_scale_joint_vel = -0.01 + """Penalty for joint velocities."""