diff --git a/docs/source/api/lab_contrib/isaaclab_contrib.deformable.rst b/docs/source/api/lab_contrib/isaaclab_contrib.deformable.rst index 0267a922c5c8..0b81c4ebd718 100644 --- a/docs/source/api/lab_contrib/isaaclab_contrib.deformable.rst +++ b/docs/source/api/lab_contrib/isaaclab_contrib.deformable.rst @@ -11,10 +11,13 @@ isaaclab_contrib.deformable deformable_object_data.DeformableObjectData newton_manager_cfg.VBDSolverCfg newton_manager_cfg.CoupledMJWarpVBDSolverCfg + newton_manager_cfg.ProxyCoupledMJWarpVBDSolverCfg newton_manager_cfg.CoupledFeatherstoneVBDSolverCfg newton_manager_cfg.NewtonModelCfg + newton_manager_cfg.CoupledNewtonCfg vbd_manager.NewtonVBDManager coupled_mjwarp_vbd_manager.NewtonCoupledMJWarpVBDManager + proxy_coupled_mjwarp_vbd_manager.NewtonProxyCoupledMJWarpVBDManager coupled_featherstone_vbd_manager.NewtonCoupledFeatherstoneVBDManager Deformable Object @@ -44,6 +47,11 @@ Newton Solver Configurations :show-inheritance: :exclude-members: __init__ +.. autoclass:: isaaclab_contrib.deformable.newton_manager_cfg.ProxyCoupledMJWarpVBDSolverCfg + :members: + :show-inheritance: + :exclude-members: __init__ + .. autoclass:: isaaclab_contrib.deformable.newton_manager_cfg.CoupledFeatherstoneVBDSolverCfg :members: :show-inheritance: @@ -54,6 +62,11 @@ Newton Solver Configurations :show-inheritance: :exclude-members: __init__ +.. autoclass:: isaaclab_contrib.deformable.newton_manager_cfg.CoupledNewtonCfg + :members: + :show-inheritance: + :exclude-members: __init__ + Newton Solver Managers ---------------------- @@ -67,6 +80,11 @@ Newton Solver Managers :inherited-members: :show-inheritance: +.. autoclass:: isaaclab_contrib.deformable.proxy_coupled_mjwarp_vbd_manager.NewtonProxyCoupledMJWarpVBDManager + :members: + :inherited-members: + :show-inheritance: + .. autoclass:: isaaclab_contrib.deformable.coupled_featherstone_vbd_manager.NewtonCoupledFeatherstoneVBDManager :members: :inherited-members: diff --git a/docs/source/overview/core-concepts/physical-backends/newton/using-vbd-solver.rst b/docs/source/overview/core-concepts/physical-backends/newton/using-vbd-solver.rst index 71f5742fb275..510dc696212c 100644 --- a/docs/source/overview/core-concepts/physical-backends/newton/using-vbd-solver.rst +++ b/docs/source/overview/core-concepts/physical-backends/newton/using-vbd-solver.rst @@ -17,10 +17,19 @@ before it works well with VBD. VBD is usually exposed through a task-specific physics preset rather than a general ``newton_vbd`` preset. Deformable-only scenes can use :class:`~isaaclab_contrib.deformable.VBDSolverCfg` directly. Robot or -rigid-body scenes usually use -:class:`~isaaclab_contrib.deformable.CoupledMJWarpVBDSolverCfg` or -:class:`~isaaclab_contrib.deformable.CoupledFeatherstoneVBDSolverCfg` so one -solver advances rigid bodies and VBD advances deformable particles. +rigid-body scenes usually use one of the coupled configs so one solver advances +rigid bodies and VBD advances deformable particles: + +* :class:`~isaaclab_contrib.deformable.CoupledMJWarpVBDSolverCfg` — alternates + the rigid (MJWarp) and VBD substeps. Use it when the same robot should both + contact and feel the deformable. +* :class:`~isaaclab_contrib.deformable.ProxyCoupledMJWarpVBDSolverCfg` — + partitions the model between an MJWarp entry and a VBD entry, exposing + selected rigid bodies as *proxies* in the VBD view via lagged impulses (see + :ref:`newton-vbd-proxy-coupling` below). Use it when only a few rigid bodies + (e.g. a gripper) need to interact with the deformable. +* :class:`~isaaclab_contrib.deformable.CoupledFeatherstoneVBDSolverCfg` — + alternates Featherstone and VBD; supports kinematic one-way coupling. Start from a Supported Deformable Task -------------------------------------- @@ -207,6 +216,101 @@ The rigid solver parameters still matter. For example, MJWarp's ``nconmax`` and :doc:`mjwarp-solver` for the MJWarp-side parameters. +.. _newton-vbd-proxy-coupling: + +Proxy-Coupled MJWarp + VBD +-------------------------- + +:class:`~isaaclab_contrib.deformable.ProxyCoupledMJWarpVBDSolverCfg` is an +alternative MJWarp + VBD coupling that wraps Newton's +:class:`newton.solvers.SolverCoupledProxy`. Instead of alternating two +full-model substeps, the model is **partitioned** between an MJWarp entry and a +VBD entry, and selected rigid bodies are exposed to VBD as *proxies* — virtual +copies that VBD collides against. Contact feedback is returned to MJWarp as +lagged impulses. This typically scales better than the alternating coupling +when only a small set of rigid bodies (e.g. the fingers of a gripper) actually +needs to touch the deformable, since the bulk of the articulation is solved +purely by MJWarp without seeing the particle contacts. + +The Franka soft-body task ships a ``newton_mjwarp_vbd_proxy`` preset (the new +default for ``Isaac-Lift-Soft-Franka-v0``) that demonstrates the typical +configuration: + +.. literalinclude:: ../../../../../../source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift_franka_soft/franka_soft_env_cfg.py + :language: python + :start-at: newton_mjwarp_vbd_proxy: CoupledNewtonCfg + :end-before: physx: PhysxCfg = PhysxCfg() + :dedent: 4 + +What the selectors do: + +* ``mjwarp_bodies`` and ``vbd_bodies`` partition every body in the model. Each + entry is either a :class:`~isaaclab.managers.SceneEntityCfg` (resolved + against the scene's ``prim_path``, optionally narrowed by ``body_names``) or + a raw prim-path regex string matched against ``model.body_label`` (e.g. + ``"/World/envs/env_.*/Robot"``). Joints inherit their child body's owner; + shapes inherit their body's owner. Static shapes (world geometry) always go + to VBD so the proxy collision pipeline can test against the ground. A body + matching both partitions, or matching neither, is an error. +* ``proxy_bodies`` selects the (rigid) MJWarp bodies that VBD should collide + against. Only bodies that own at least one + ``newton.ShapeFlags.COLLIDE_SHAPES`` shape are kept. For + :class:`~isaaclab.managers.SceneEntityCfg` entries, ``body_names`` is + **required** here since proxies must be a strict subset of the asset. +* In the snippet above, the entire Franka articulation is routed to MJWarp, + the deformable particles are owned by VBD, and only the ``panda_hand`` and + the two fingers are exposed as proxies — so VBD only ever sees three rigid + proxies regardless of how many links the arm has. + +Key proxy-specific parameters: + +.. list-table:: + :header-rows: 1 + :widths: 30 70 + + * - Parameter + - Description + * - ``proxy_mode`` + - Default: ``"lagged"``. ``"lagged"`` syncs source begin poses and end + velocities, then rewinds lagged feedback before the destination solve. + ``"staggered"`` syncs source end poses and end velocities directly. + Lagged is the safer default; staggered can be tighter but is more + sensitive to timestep. + * - ``proxy_iterations`` + - Default: ``1``. Number of relaxation iterations per coupled substep. + Increase it when proxy contact feedback needs more accuracy. + * - ``proxy_collide_interval`` + - Default: ``1``. How often (in proxy passes) the proxy collision + pipeline rebuilds candidate pairs. Increase it for cheaper but + slightly staler proxy contacts. + * - ``proxy_mass_scale`` + - Default: ``1.0``. Multiplier for the virtual inertia of proxy bodies + in the VBD view. Increase it to make proxies behave more like fixed + obstacles to VBD. + +Because the proxy solver resolves +:class:`~isaaclab.managers.SceneEntityCfg` selectors against the scene at +solver-build time, the coupled preset must be wrapped in a +:class:`~isaaclab_contrib.deformable.CoupledNewtonCfg` (a thin +:class:`~isaaclab_newton.physics.NewtonCfg` subclass with a ``scene_cfg`` and a +``model_cfg`` field). The env's ``__post_init__`` is responsible for setting +``self.sim.physics.scene_cfg = self.scene`` — the Franka soft env does this +automatically via the :class:`~isaaclab_tasks.utils.PresetCfg` plumbing. + +Try the demo: + +.. code-block:: bash + + # zero-agent visual smoke test (default preset is now the proxy-coupled one) + ./isaaclab.sh -p scripts/environments/zero_agent.py --task Isaac-Lift-Soft-Franka-v0 --num_envs 1 --visualizer kit + + # scripted pick-and-lift via state machine + ./isaaclab.sh -p scripts/environments/state_machine/lift_franka_soft.py --num_envs 1 + + # explicitly select the alternating-substep preset instead + ./isaaclab.sh -p scripts/environments/zero_agent.py --task Isaac-Lift-Soft-Franka-v0 --num_envs 1 presets=newton_mjwarp_vbd + + Contact and Material Parameters ------------------------------- diff --git a/scripts/environments/state_machine/lift_franka_soft.py b/scripts/environments/state_machine/lift_franka_soft.py index a9561718a591..22d3e7dbdca4 100644 --- a/scripts/environments/state_machine/lift_franka_soft.py +++ b/scripts/environments/state_machine/lift_franka_soft.py @@ -63,8 +63,6 @@ from isaaclab.assets.deformable_object.deformable_object_data import DeformableObjectData import isaaclab_tasks # noqa: F401 -from isaaclab_tasks.manager_based.manipulation.lift_franka_soft.franka_cloth_env_cfg import FrankaClothEnvCfg -from isaaclab_tasks.manager_based.manipulation.lift_franka_soft.franka_soft_env_cfg import FrankaSoftEnvCfg from isaaclab_tasks.utils.parse_cfg import parse_env_cfg # initialize warp @@ -284,26 +282,14 @@ def compute(self, ee_pose: torch.Tensor, object_pose: torch.Tensor, des_object_p def main(): # create environment render_mode = "rgb_array" if args_cli.video else None - if args_cli.task == "Isaac-Lift-Soft-Franka-v0": - # parse configuration - env_cfg: FrankaSoftEnvCfg = parse_env_cfg( - "Isaac-Lift-Soft-Franka-v0", - device=args_cli.device, - num_envs=args_cli.num_envs, - ) - env_cfg.viewer.eye = (2.1, 1.0, 1.3) - env = gym.make("Isaac-Lift-Soft-Franka-v0", cfg=env_cfg, render_mode=render_mode) - elif args_cli.task == "Isaac-Lift-Soft-Franka-Cloth-v0": - # parse configuration - env_cfg: FrankaClothEnvCfg = parse_env_cfg( - "Isaac-Lift-Cloth-Franka-v0", - device=args_cli.device, - num_envs=args_cli.num_envs, - ) - env_cfg.viewer.eye = (2.1, 1.0, 1.3) - env = gym.make("Isaac-Lift-Cloth-Franka-v0", cfg=FrankaClothEnvCfg(), render_mode=render_mode) - else: - raise ValueError(f"Unknown task: {args_cli.task}") + # parse configuration + env_cfg = parse_env_cfg( + args_cli.task, + device=args_cli.device, + num_envs=args_cli.num_envs, + ) + env_cfg.viewer.eye = (2.1, 1.0, 1.3) + env = gym.make(args_cli.task, cfg=env_cfg, render_mode=render_mode) # wrap for video recording if args_cli.video: @@ -353,12 +339,12 @@ def main(): ) tcp_rest_orientation = ee_frame_sensor.data.target_quat_w.torch[..., 0, :].clone() # -- object frame - object_data: DeformableObjectData = env.unwrapped.scene["deformable"].data + object_data: DeformableObjectData = env.unwrapped.scene["object"].data object_position = object_data.root_pos_w.torch - env.unwrapped.scene.env_origins object_position += object_local_grasp_position # -- target object frame - desired_position = env.unwrapped.command_manager.get_command("deformable_pose")[..., :3] + desired_position = env.unwrapped.command_manager.get_command("object_pose")[..., :3] # advance state machine actions = pick_sm.compute( diff --git a/source/isaaclab_contrib/changelog.d/feat-proxycoupledsolver-prim-path-strings.minor.rst b/source/isaaclab_contrib/changelog.d/feat-proxycoupledsolver-prim-path-strings.minor.rst new file mode 100644 index 000000000000..86e4fa0e0527 --- /dev/null +++ b/source/isaaclab_contrib/changelog.d/feat-proxycoupledsolver-prim-path-strings.minor.rst @@ -0,0 +1,16 @@ +Added +^^^^^ + +* Added :class:`~isaaclab_contrib.deformable.newton_manager_cfg.ProxyCoupledMJWarpVBDSolverCfg` + and the matching + :class:`~isaaclab_contrib.deformable.proxy_coupled_mjwarp_vbd_manager.NewtonProxyCoupledMJWarpVBDManager`, + wrapping :class:`newton.solvers.experimental.coupled.SolverCoupledProxy` to + split simulation between MuJoCo Warp (rigids/articulations) and VBD + (particles/deformables), with selected MJWarp bodies exposed as proxies in + the VBD view. + +* Added support for raw prim-path regex strings (e.g. ``"/World/envs/env_.*/MyCube"``) + in the body-selector lists of + :class:`~isaaclab_contrib.deformable.newton_manager_cfg.ProxyCoupledMJWarpVBDSolverCfg`, + alongside :class:`~isaaclab.managers.SceneEntityCfg` entries. Useful for + claiming rigid assets that aren't registered as named scene entities. diff --git a/source/isaaclab_contrib/isaaclab_contrib/deformable/coupled_featherstone_vbd_manager.py b/source/isaaclab_contrib/isaaclab_contrib/deformable/coupled_featherstone_vbd_manager.py index 312d9fc69f10..8a4160d97119 100644 --- a/source/isaaclab_contrib/isaaclab_contrib/deformable/coupled_featherstone_vbd_manager.py +++ b/source/isaaclab_contrib/isaaclab_contrib/deformable/coupled_featherstone_vbd_manager.py @@ -7,7 +7,6 @@ from __future__ import annotations -import inspect import logging from typing import TYPE_CHECKING @@ -21,7 +20,6 @@ from .deformable_object import ( add_deformable_entry_to_builder, - clear_deformable_builder_hooks, install_deformable_builder_hooks, ) from .kernels import _kernel_body_particle_reaction @@ -82,11 +80,6 @@ def step(cls) -> None: NewtonManager._model_changes = set() super().step() - @classmethod - def _solver_specific_clear(cls): - """Clear VBD-specific state.""" - clear_deformable_builder_hooks() - @classmethod def _get_deformable_ignore_paths(cls) -> list[str]: """Return USD prim paths to skip when calling ``builder.add_usd``. @@ -299,13 +292,10 @@ def _build_solver(cls, model: Model, solver_cfg: CoupledFeatherstoneVBDSolverCfg """ cls._coupling_mode = solver_cfg.coupling_mode - valid = set(inspect.signature(SolverFeatherstone.__init__).parameters) - {"self", "model"} - kwargs = {k: v for k, v in solver_cfg.rigid_solver_cfg.to_dict().items() if k in valid} - cls._rigid_solver = SolverFeatherstone(model, **kwargs) - - valid = set(inspect.signature(SolverVBD.__init__).parameters) - {"self", "model"} - kwargs = {k: v for k, v in solver_cfg.soft_solver_cfg.to_dict().items() if k in valid} - cls._soft_solver = SolverVBD(model, **kwargs) + cls._rigid_solver = SolverFeatherstone( + model, **cls._filter_solver_kwargs(SolverFeatherstone, solver_cfg.rigid_solver_cfg) + ) + cls._soft_solver = SolverVBD(model, **cls._filter_solver_kwargs(SolverVBD, solver_cfg.soft_solver_cfg)) # Dummy solver for the newtonmanager NewtonManager._solver = SolverBase(model) @@ -339,8 +329,12 @@ def _step_solver( cls._step_kinematic(state_in, state_out, control, substep_dt) elif cls._coupling_mode == "one_way": cls._step_one_way(state_in, state_out, control, substep_dt) - else: + elif cls._coupling_mode == "two_way": cls._step_two_way(state_in, state_out, control, substep_dt) + else: + raise ValueError( + f"Unknown coupling_mode={cls._coupling_mode!r}; expected one of {{'kinematic', 'one_way', 'two_way'}}." + ) @classmethod def _simulate_physics_only(cls) -> None: diff --git a/source/isaaclab_contrib/isaaclab_contrib/deformable/coupled_mjwarp_vbd_manager.py b/source/isaaclab_contrib/isaaclab_contrib/deformable/coupled_mjwarp_vbd_manager.py index 5097284bafc7..cd84f80ec273 100644 --- a/source/isaaclab_contrib/isaaclab_contrib/deformable/coupled_mjwarp_vbd_manager.py +++ b/source/isaaclab_contrib/isaaclab_contrib/deformable/coupled_mjwarp_vbd_manager.py @@ -7,7 +7,6 @@ from __future__ import annotations -import inspect import logging from typing import TYPE_CHECKING @@ -21,7 +20,6 @@ from .deformable_object import ( add_deformable_entry_to_builder, - clear_deformable_builder_hooks, install_deformable_builder_hooks, ) from .kernels import _kernel_body_particle_reaction @@ -82,11 +80,6 @@ def step(cls) -> None: NewtonManager._model_changes = set() super().step() - @classmethod - def _solver_specific_clear(cls): - """Clear VBD-specific state.""" - clear_deformable_builder_hooks() - @classmethod def _get_deformable_ignore_paths(cls) -> list[str]: """Return USD prim paths to skip when calling ``builder.add_usd``. @@ -299,13 +292,8 @@ def _build_solver(cls, model: Model, solver_cfg: CoupledMJWarpVBDSolverCfg) -> N """ cls._coupling_mode = solver_cfg.coupling_mode - valid = set(inspect.signature(SolverMuJoCo.__init__).parameters) - {"self", "model"} - kwargs = {k: v for k, v in solver_cfg.rigid_solver_cfg.to_dict().items() if k in valid} - cls._rigid_solver = SolverMuJoCo(model, **kwargs) - - valid = set(inspect.signature(SolverVBD.__init__).parameters) - {"self", "model"} - kwargs = {k: v for k, v in solver_cfg.soft_solver_cfg.to_dict().items() if k in valid} - cls._soft_solver = SolverVBD(model, **kwargs) + cls._rigid_solver = SolverMuJoCo(model, **cls._filter_solver_kwargs(SolverMuJoCo, solver_cfg.rigid_solver_cfg)) + cls._soft_solver = SolverVBD(model, **cls._filter_solver_kwargs(SolverVBD, solver_cfg.soft_solver_cfg)) # Dummy solver for the newtonmanager NewtonManager._solver = SolverBase(model) @@ -328,8 +316,10 @@ def _step_solver( """ if cls._coupling_mode == "one_way": cls._step_one_way(state_in, state_out, control, substep_dt) - else: + elif cls._coupling_mode == "two_way": cls._step_two_way(state_in, state_out, control, substep_dt) + else: + raise ValueError(f"Unknown coupling_mode={cls._coupling_mode!r}; expected one of {{'one_way', 'two_way'}}.") @classmethod def _simulate_physics_only(cls) -> None: diff --git a/source/isaaclab_contrib/isaaclab_contrib/deformable/newton_manager_cfg.py b/source/isaaclab_contrib/isaaclab_contrib/deformable/newton_manager_cfg.py index e9358ff0be23..aa52efb49b96 100644 --- a/source/isaaclab_contrib/isaaclab_contrib/deformable/newton_manager_cfg.py +++ b/source/isaaclab_contrib/isaaclab_contrib/deformable/newton_manager_cfg.py @@ -9,27 +9,29 @@ from typing import TYPE_CHECKING -from isaaclab_newton.physics import FeatherstoneSolverCfg, MJWarpSolverCfg, NewtonSolverCfg +from isaaclab_newton.physics import FeatherstoneSolverCfg, MJWarpSolverCfg, NewtonCfg, NewtonSolverCfg +from isaaclab.managers import SceneEntityCfg from isaaclab.utils.configclass import configclass if TYPE_CHECKING: from isaaclab_newton.physics import NewtonManager + from isaaclab.scene import InteractiveSceneCfg + @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. + Supports cloth, soft bodies, and coupled rigid-body systems. Requires + ``ModelBuilder.color()`` before ``finalize()`` to build the vertex coloring. """ class_type: type[NewtonManager] | str = "{DIR}.vbd_manager:NewtonVBDManager" """Manager class for the VBD solver.""" - solver_type: str = "vbd" + requires_graph_coloring: bool = True iterations: int = 10 """Number of VBD iterations per substep.""" @@ -37,8 +39,8 @@ class VBDSolverCfg(NewtonSolverCfg): 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. + Set to ``True`` when coupling cloth with a separate rigid-body solver so VBD + only integrates the cloth particles. """ particle_enable_self_contact: bool = False @@ -51,12 +53,10 @@ class VBDSolverCfg(NewtonSolverCfg): """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. + """How often 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. + ``< 0``: once before initialization. ``0``: once before and once after + initialization. ``k >= 1``: before every ``k`` VBD iterations. """ particle_vertex_contact_buffer_size: int = 32 @@ -68,48 +68,33 @@ class VBDSolverCfg(NewtonSolverCfg): 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. + Only used when ``particle_enable_self_contact`` is ``True``. 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]. + """Rest-configuration separation threshold for filtering 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``. + 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. - """ + """Initial stiffness seed for all rigid body contacts [N/m].""" @configclass class CoupledMJWarpVBDSolverCfg(NewtonSolverCfg): - """Configuration for the coupled rigid-body MJWarp + VBD solver. - - Alternates a rigid-body solver (:class:`MJWarpSolverCfg`) and a soft-body solver (:class:`SolverVBD`) per - substep. The coupling direction is controlled by :attr:`coupling_mode`: + """Configuration for the coupled MJWarp + VBD solver. - - ``"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. + Alternates a rigid-body solver (:class:`MJWarpSolverCfg`) and VBD per substep. + The coupling direction is controlled by :attr:`coupling_mode`. """ class_type: type[NewtonManager] | str = "{DIR}.coupled_mjwarp_vbd_manager:NewtonCoupledMJWarpVBDManager" - """Manager class for the VBD solver.""" - - solver_type: str = "coupledmjwarpvbd" + """Manager class for the coupled MJWarp + VBD solver.""" rigid_solver_cfg: MJWarpSolverCfg = MJWarpSolverCfg() - """Rigid-body sub-solver configuration for :class:`MJWarpSolverCfg`.""" + """Rigid-body sub-solver configuration.""" soft_solver_cfg: VBDSolverCfg = VBDSolverCfg(integrate_with_external_rigid_solver=True) """VBD sub-solver configuration for cloth/particle dynamics.""" @@ -117,36 +102,90 @@ class CoupledMJWarpVBDSolverCfg(NewtonSolverCfg): coupling_mode: str = "two_way" """Coupling direction between the rigid and VBD solvers. - - ``"one_way"``: Rigid -> soft only (default, existing behavior). + - ``"one_way"``: Rigid -> soft only. - ``"two_way"``: Same-substep two-way coupling with normal + Coulomb friction. """ @configclass -class CoupledFeatherstoneVBDSolverCfg(NewtonSolverCfg): - """Configuration for the coupled rigid-body Featherstone + VBD solver. +class ProxyCoupledMJWarpVBDSolverCfg(NewtonSolverCfg): + """Configuration for the proxy-coupled MJWarp + VBD solver. - Alternates a rigid-body solver (:class:`FeatherstoneSolverCfg`) and a soft-body solver (:class:`SolverVBD`) per - substep. The coupling direction is controlled by :attr:`coupling_mode`: + Wraps Newton's :class:`newton.solvers.experimental.coupled.SolverCoupledProxy` + (lagged-impulse virtual-proxy coupling) with MuJoCo Warp as the rigid sub-solver and VBD as + the soft sub-solver. Selected MuJoCo bodies are exposed as proxy bodies in + the VBD view so VBD detects contacts against them and returns feedback + wrenches to MuJoCo via lagged impulses. - - ``"kinematic"`` (default): Rigid -> soft only. Rigid bodies are kinematically updated by the rigid solver, - then VBD reads the updated body poses and reacts to them. The rigid solver does not feel particle contacts. - - ``"one_way"``: 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. + Body selectors are either :class:`~isaaclab.managers.SceneEntityCfg` + (scoped by the asset's ``prim_path``, optionally narrowed by ``body_names`` + full-matched against body short names) or raw prim-path regex strings + (e.g. ``"/World/envs/env_.*/MyCube"``) matched against ``model.body_label`` + via ``^(/|$)``. """ - class_type: type[NewtonManager] | str = "{DIR}.coupled_featherstone_vbd_manager:NewtonCoupledFeatherstoneVBDManager" - """Manager class for the VBD solver.""" + class_type: type[NewtonManager] | str = "{DIR}.proxy_coupled_mjwarp_vbd_manager:NewtonProxyCoupledMJWarpVBDManager" + """Manager class for the proxy-coupled MJWarp + VBD solver.""" + + requires_graph_coloring: bool = True - solver_type: str = "coupledfeatherstonevbd" + mjwarp_cfg: MJWarpSolverCfg = MJWarpSolverCfg() + """MuJoCo Warp sub-solver configuration.""" + + vbd_cfg: VBDSolverCfg = VBDSolverCfg(integrate_with_external_rigid_solver=True) + """VBD sub-solver configuration; defaults to external rigid integration since + rigid bodies live in the MuJoCo entry.""" + + mjwarp_bodies: list[SceneEntityCfg | str] = [] + """Selectors whose bodies/joints/shapes go to the MuJoCo entry. + + Joints inherit their child body's owner; shapes inherit their body's + owner; static shapes (``body == -1``) always go to the VBD entry. + """ + + vbd_bodies: list[SceneEntityCfg | str] = [] + """Selectors routed to the VBD entry (see :attr:`mjwarp_bodies`).""" + + proxy_bodies: list[SceneEntityCfg | str] = [] + """Selectors naming MuJoCo bodies to expose as proxies in the VBD view. + + For :class:`SceneEntityCfg` entries, ``body_names`` is **required** + (proxies are a subset, not the whole asset); raw strings are accepted + as-is. Matched bodies are filtered to those owning at least one + ``newton.ShapeFlags.COLLIDE_SHAPES`` shape. Empty list = no proxies. + """ + + proxy_mode: str = "lagged" + """Proxy transfer mode passed to :class:`newton.solvers.experimental.coupled.SolverCoupledProxy.Proxy`. + + - ``"lagged"``: syncs source begin poses and end velocities, then rewinds + lagged feedback before the destination solve. + - ``"staggered"``: syncs source end poses and end velocities directly. + """ + + proxy_iterations: int = 1 + """Number of relaxation iterations per coupled substep.""" + + proxy_collide_interval: int = 1 + """Collision-detection refresh interval (in proxy passes).""" + + proxy_mass_scale: float = 1.0 + """Mass / inertia scale applied to destination proxy bodies (virtual inertia).""" + + +@configclass +class CoupledFeatherstoneVBDSolverCfg(NewtonSolverCfg): + """Configuration for the coupled Featherstone + VBD solver. + + Alternates a rigid-body solver (:class:`FeatherstoneSolverCfg`) and VBD per + substep. The coupling direction is controlled by :attr:`coupling_mode`. + """ + + class_type: type[NewtonManager] | str = "{DIR}.coupled_featherstone_vbd_manager:NewtonCoupledFeatherstoneVBDManager" + """Manager class for the coupled Featherstone + VBD solver.""" rigid_solver_cfg: FeatherstoneSolverCfg = FeatherstoneSolverCfg() - """Rigid-body sub-solver configuration for :class:`FeatherstoneSolverCfg`.""" + """Rigid-body sub-solver configuration.""" soft_solver_cfg: VBDSolverCfg = VBDSolverCfg(integrate_with_external_rigid_solver=True) """VBD sub-solver configuration for cloth/particle dynamics.""" @@ -154,58 +193,61 @@ class CoupledFeatherstoneVBDSolverCfg(NewtonSolverCfg): coupling_mode: str = "kinematic" """Coupling direction between the rigid and VBD solvers. - - ``"kinematic"``: Rigid -> soft only (default) - - ``"one_way"``: Rigid -> soft only (existing behavior). - - ``"two_way"``: Same-substep two-way coupling with normal + Coulomb friction. + Accepts the same values as :attr:`CoupledMJWarpVBDSolverCfg.coupling_mode`, + plus ``"kinematic"`` (rigid -> soft only, rigid bodies kinematically updated). """ @configclass class NewtonModelCfg: - """Global Newton model parameters. + """Global Newton model parameters applied after builder finalization. - These parameters are applied to the ``newton.Model`` after finalization. - They control model-level contact behavior shared across all objects. + These control model-level contact behavior shared across all objects. """ soft_contact_ke: float = 1.0e3 - """Body-particle contact stiffness [N/m]. + """Body-particle and particle self-contact stiffness [N/m]. - Controls the stiffness of the penalty force of contacts between cloth/soft-body particles - and rigid body shapes, and self-contacts of cloth/soft-body particles. The effective stiffness per contact is the - average of this value and the rigid shape's material stiffness. + Effective per-contact stiffness 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. + """Body-particle contact friction coefficient [dimensionless]. - The effective friction per contact is ``sqrt(soft_contact_mu * shape_material_mu)``. - Increase for better grip (e.g. gripper picking up cloth). + Effective per-contact friction is ``sqrt(soft_contact_mu * shape_material_mu)``. """ 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. - """ + """Per-shape contact stiffness override [N/m]. ``None`` keeps USD/MJCF values.""" shape_material_kd: float | None = None - """Per-shape contact damping override [N*s/m]. + """Per-shape contact damping override [N*s/m]. ``None`` keeps USD/MJCF values.""" + + shape_material_mu: float | None = None + """Per-shape friction coefficient override [dimensionless]. ``None`` keeps USD/MJCF values.""" + - 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. +@configclass +class CoupledNewtonCfg(NewtonCfg): + """:class:`NewtonCfg` extended for coupled-solver setups. + + Adds :attr:`model_cfg` for global model parameters and :attr:`scene_cfg` so + the manager can resolve :class:`~isaaclab.managers.SceneEntityCfg` selectors + against the scene at solver-build time. + + Uses a distinct class name so :func:`_is_kitless_physics` does not match it, + ensuring Kit is launched for USD deformable/coupled spawning. """ - shape_material_mu: float | None = None - """Per-shape friction coefficient override [dimensionless]. + model_cfg: NewtonModelCfg | None = None + """Global Newton model parameters applied after builder finalization.""" + + scene_cfg: InteractiveSceneCfg | None = None + """Scene cfg used by coupled solvers to resolve scene-entity selectors. - 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. + Set to ``self.scene`` from the env's ``__post_init__``. """ diff --git a/source/isaaclab_contrib/isaaclab_contrib/deformable/proxy_coupled_mjwarp_vbd_manager.py b/source/isaaclab_contrib/isaaclab_contrib/deformable/proxy_coupled_mjwarp_vbd_manager.py new file mode 100644 index 000000000000..cf79f2c8076d --- /dev/null +++ b/source/isaaclab_contrib/isaaclab_contrib/deformable/proxy_coupled_mjwarp_vbd_manager.py @@ -0,0 +1,273 @@ +# 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 + +"""Proxy-coupled MJWarp + VBD Newton manager. + +Wraps :class:`newton.solvers.SolverCoupledProxy` with MuJoCo Warp as the rigid +sub-solver and VBD as the soft sub-solver, exposing selected MuJoCo bodies as +proxies in the VBD view. +""" + +from __future__ import annotations + +import re +from typing import TYPE_CHECKING + +from isaaclab_newton.physics.newton_manager import NewtonManager +from newton import CollisionPipeline, Model, ShapeFlags +from newton.solvers import SolverMuJoCo, SolverVBD +from newton.solvers.experimental.coupled import SolverCoupledProxy + +from isaaclab.managers import SceneEntityCfg +from isaaclab.physics import PhysicsManager + +from .newton_manager_cfg import CoupledNewtonCfg, ProxyCoupledMJWarpVBDSolverCfg +from .vbd_manager import NewtonVBDManager + +if TYPE_CHECKING: + from isaaclab.scene import InteractiveSceneCfg + + +class NewtonProxyCoupledMJWarpVBDManager(NewtonVBDManager): + """Newton manager wrapping :class:`newton.solvers.SolverCoupledProxy` with an MJWarp+VBD split. + + Bodies/joints/shapes are partitioned between the two entries; all particles + are solved by VBD. + """ + + @classmethod + def _build_solver(cls, model: Model, solver_cfg: ProxyCoupledMJWarpVBDSolverCfg) -> None: + mjc_kw = cls._filter_solver_kwargs(SolverMuJoCo, solver_cfg.mjwarp_cfg) + vbd_kw = cls._filter_solver_kwargs(SolverVBD, solver_cfg.vbd_cfg) + + outer_cfg = PhysicsManager._cfg + scene_cfg = outer_cfg.scene_cfg if isinstance(outer_cfg, CoupledNewtonCfg) else None + + mjc_bodies, vbd_bodies, mjc_joints, vbd_joints, mjc_shapes, vbd_shapes = cls._partition_model_by_entities( + model, + solver_cfg.mjwarp_bodies, + solver_cfg.vbd_bodies, + scene_cfg, + ) + vbd_particles = list(range(model.particle_count)) + + proxy_body_ids = cls._select_proxy_bodies(model, solver_cfg.proxy_bodies, scene_cfg) + if solver_cfg.proxy_bodies and not proxy_body_ids: + raise ValueError( + f"ProxyCoupledMJWarpVBDSolverCfg.proxy_bodies={solver_cfg.proxy_bodies!r} resolved to " + "zero bodies after filtering for `ShapeFlags.COLLIDE_SHAPES`. Rigid bodies would not be " + "visible to VBD; check that the selected bodies own at least one collidable shape." + ) + + entries = [ + SolverCoupledProxy.Entry( + name="mjc", + solver=lambda v, _kw=mjc_kw: SolverMuJoCo(model=v, **_kw), + bodies=mjc_bodies, + joints=mjc_joints, + shapes=mjc_shapes, + ), + SolverCoupledProxy.Entry( + name="vbd", + solver=lambda v, _kw=vbd_kw: SolverVBD(model=v, **_kw), + bodies=vbd_bodies, + joints=vbd_joints, + particles=vbd_particles, + shapes=vbd_shapes, + ), + ] + + proxies: list[SolverCoupledProxy.Proxy] = [] + if proxy_body_ids: + proxies.append( + SolverCoupledProxy.Proxy( + source="mjc", + destination="vbd", + bodies=proxy_body_ids, + mode=solver_cfg.proxy_mode, + mass_scale=float(solver_cfg.proxy_mass_scale), + collision_pipeline=lambda destination_model: CollisionPipeline( + destination_model, + broad_phase="explicit", + ), + collide_interval=int(solver_cfg.proxy_collide_interval), + ) + ) + + NewtonManager._solver = SolverCoupledProxy( + model=model, + entries=entries, + coupling=SolverCoupledProxy.Config( + proxies=proxies, + iterations=int(solver_cfg.proxy_iterations), + ), + ) + NewtonManager._use_single_state = False + NewtonManager._needs_collision_pipeline = False + + @classmethod + def _resolve_entity_to_body_ids( + cls, + model: Model, + spec: SceneEntityCfg | str, + scene_cfg: InteractiveSceneCfg | None, + field: str, + ) -> list[int]: + """Resolve one selector to ``model.body_label`` indices. + + Strings are matched directly via ``^(/|$)``. :class:`SceneEntityCfg` + looks up the asset's ``prim_path`` on ``scene_cfg`` and (optionally) + full-matches ``body_names`` regexes against the body short name. + + Raises: + ValueError: Asset missing on ``scene_cfg``; ``body_names`` pattern + with zero matches; or a string with zero matches. + """ + if isinstance(spec, str): + prim_path, patterns, spec_repr = spec, None, f"prim-path regex {spec!r}" + else: + asset_cfg = getattr(scene_cfg, spec.name, None) if scene_cfg is not None else None + if asset_cfg is None or not hasattr(asset_cfg, "prim_path"): + raise ValueError( + f"ProxyCoupledMJWarpVBDSolverCfg.{field}: scene entity {spec.name!r} " + f"is not on the attached scene cfg (or lacks `prim_path`)." + ) + prim_path = asset_cfg.prim_path + patterns = [spec.body_names] if isinstance(spec.body_names, str) else spec.body_names + spec_repr = f"asset {spec.name!r}" + + asset_re = re.compile(rf"^{prim_path}(/|$)") + # Treat patterns=None as ".*" so the loop is uniform across both branches. + compiled = [re.compile(p) for p in (patterns if patterns is not None else [r".*"])] + matched = [False] * len(compiled) + body_ids: list[int] = [] + for b in range(int(model.body_count)): + lbl = model.body_label[b] + if not asset_re.match(lbl): + continue + short = lbl.rsplit("/", 1)[-1] + hit = next((i for i, rx in enumerate(compiled) if rx.fullmatch(short)), None) + if hit is None: + continue + matched[hit] = True + body_ids.append(b) + + if patterns is not None: + unmatched = [p for p, ok in zip(patterns, matched) if not ok] + if unmatched: + raise ValueError( + f"ProxyCoupledMJWarpVBDSolverCfg.{field}: {spec_repr} has no bodies matching {unmatched}." + ) + elif isinstance(spec, str) and not body_ids: + # Strings have no asset-cfg safety net — zero matches is almost always a typo. + raise ValueError( + f"ProxyCoupledMJWarpVBDSolverCfg.{field}: {spec_repr} matched no bodies in " + f"`model.body_label` (labels are full post-clone prim paths)." + ) + return body_ids + + @classmethod + def _partition_model_by_entities( + cls, + model: Model, + mjwarp_bodies: list[SceneEntityCfg | str], + vbd_bodies: list[SceneEntityCfg | str], + scene_cfg: InteractiveSceneCfg | None, + ) -> tuple[list[int], list[int], list[int], list[int], list[int], list[int]]: + """Split bodies/joints/shapes between the MJWarp and VBD entries. + + Joints/shapes inherit their (child) body's owner. Static shapes + (``body == -1``) always go to VBD so its proxy collision pipeline + tests rigid proxies against the world. + + Raises: + ValueError: A body matches both partitions or neither. + """ + mjc_owned: set[int] = set() + for spec in mjwarp_bodies: + mjc_owned.update(cls._resolve_entity_to_body_ids(model, spec, scene_cfg, "mjwarp_bodies")) + vbd_owned: set[int] = set() + for spec in vbd_bodies: + vbd_owned.update(cls._resolve_entity_to_body_ids(model, spec, scene_cfg, "vbd_bodies")) + + def _preview(ids: list[int]) -> str: + return ", ".join(f"{b}:{model.body_label[b]!r}" for b in ids[:5]) + + if overlap := sorted(mjc_owned & vbd_owned): + raise ValueError( + f"ProxyCoupledMJWarpVBDSolverCfg: {len(overlap)} bodies match both " + f"`mjwarp_bodies` and `vbd_bodies` (first few: {_preview(overlap)})." + ) + unclaimed = [b for b in range(int(model.body_count)) if b not in mjc_owned and b not in vbd_owned] + if unclaimed: + raise ValueError( + f"ProxyCoupledMJWarpVBDSolverCfg: {len(unclaimed)} bodies unclaimed by " + f"`mjwarp_bodies`/`vbd_bodies` (first few: {_preview(unclaimed)})." + ) + + mjc_joints: list[int] = [] + vbd_joints: list[int] = [] + if int(model.joint_count): + for j, c in enumerate(model.joint_child.numpy()): + child = int(c) + if child in mjc_owned: + mjc_joints.append(j) + elif child in vbd_owned: + vbd_joints.append(j) + + mjc_shapes: list[int] = [] + vbd_shapes: list[int] = [] + if int(model.shape_count): + for s, b in enumerate(model.shape_body.numpy()): + body = int(b) + if body < 0 or body in vbd_owned: + vbd_shapes.append(s) + elif body in mjc_owned: + mjc_shapes.append(s) + + return sorted(mjc_owned), sorted(vbd_owned), mjc_joints, vbd_joints, mjc_shapes, vbd_shapes + + @classmethod + def _select_proxy_bodies( + cls, + model: Model, + proxy_bodies: list[SceneEntityCfg | str], + scene_cfg: InteractiveSceneCfg | None, + ) -> list[int]: + """Resolve proxy bodies, filtered to those owning a ``COLLIDE_SHAPES`` shape. + + Raises: + ValueError: A :class:`SceneEntityCfg` entry has ``body_names=None`` + (proxies must be a subset, not the whole asset). + """ + if not proxy_bodies: + return [] + + shape_count = int(model.shape_count) + collide_flag = int(ShapeFlags.COLLIDE_SHAPES) + collide_bodies: set[int] = set() + if shape_count: + shape_body_np = model.shape_body.numpy() + shape_flags_np = model.shape_flags.numpy() + collide_bodies = { + int(shape_body_np[s]) + for s in range(shape_count) + if int(shape_body_np[s]) >= 0 and int(shape_flags_np[s]) & collide_flag + } + + proxy_ids: list[int] = [] + seen: set[int] = set() + for spec in proxy_bodies: + if isinstance(spec, SceneEntityCfg) and spec.body_names is None: + raise ValueError( + f"ProxyCoupledMJWarpVBDSolverCfg.proxy_bodies entry {spec.name!r} requires " + f"`body_names` (proxies must be a subset of the asset)." + ) + for body_id in cls._resolve_entity_to_body_ids(model, spec, scene_cfg, "proxy_bodies"): + if body_id in collide_bodies and body_id not in seen: + seen.add(body_id) + proxy_ids.append(body_id) + + return proxy_ids diff --git a/source/isaaclab_contrib/isaaclab_contrib/deformable/vbd_manager.py b/source/isaaclab_contrib/isaaclab_contrib/deformable/vbd_manager.py index 88380f078673..e74daabcc3cd 100644 --- a/source/isaaclab_contrib/isaaclab_contrib/deformable/vbd_manager.py +++ b/source/isaaclab_contrib/isaaclab_contrib/deformable/vbd_manager.py @@ -7,7 +7,6 @@ from __future__ import annotations -import inspect import logging from typing import TYPE_CHECKING @@ -271,8 +270,7 @@ def _build_solver(cls, model: Model, solver_cfg: VBDSolverCfg) -> None: VBD always uses Newton's :class:`CollisionPipeline` and steps with separate input/output states, so the flags are fixed. """ - valid = set(inspect.signature(SolverVBD.__init__).parameters) - {"self", "model"} - kwargs = {k: v for k, v in solver_cfg.to_dict().items() if k in valid} + kwargs = cls._filter_solver_kwargs(SolverVBD, solver_cfg) NewtonManager._solver = SolverVBD(model, **kwargs) NewtonManager._use_single_state = False NewtonManager._needs_collision_pipeline = True diff --git a/source/isaaclab_contrib/test/deformable/test_proxy_coupled_mjwarp_vbd.py b/source/isaaclab_contrib/test/deformable/test_proxy_coupled_mjwarp_vbd.py new file mode 100644 index 000000000000..127912c42b7e --- /dev/null +++ b/source/isaaclab_contrib/test/deformable/test_proxy_coupled_mjwarp_vbd.py @@ -0,0 +1,362 @@ +# 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 + +"""Pure-Python unit tests for proxy-coupled MJWarp+VBD partitioning logic. + +These tests intentionally do NOT launch :class:`isaaclab.app.AppLauncher`. +:class:`NewtonProxyCoupledMJWarpVBDManager`'s body/joint/shape partitioning and +proxy-body resolution are static class methods that operate on a Newton +:class:`newton.Model` and an :class:`isaaclab.scene.InteractiveSceneCfg`, so +they can be tested against minimal fakes without spinning up Isaac Sim. + +For the end-to-end smoke (solver builds and steps), see the existing env +smoke tests on ``Isaac-Lift-Soft-Franka-v0`` in +``isaaclab_tasks/test/test_environments.py``. +""" + +from __future__ import annotations + +from dataclasses import dataclass, field + +import numpy as np +import pytest +from newton import ShapeFlags + +from isaaclab.managers import SceneEntityCfg + +from isaaclab_contrib.deformable.proxy_coupled_mjwarp_vbd_manager import NewtonProxyCoupledMJWarpVBDManager + +## +# Fakes +## + + +@dataclass +class _FakeArray: + """Minimal stand-in for a Newton/warp array exposing ``.numpy()``.""" + + data: np.ndarray + + def numpy(self) -> np.ndarray: + return self.data + + +@dataclass +class _FakeModel: + """Minimal stand-in for :class:`newton.Model`. + + Only the attributes consulted by the partitioning/selection helpers are + populated. Joint/shape arrays default to zero-length numpy arrays when + there are no joints/shapes to model. + """ + + body_count: int + body_label: list[str] + joint_count: int = 0 + joint_child: _FakeArray = field(default_factory=lambda: _FakeArray(np.zeros(0, dtype=np.int32))) + shape_count: int = 0 + shape_body: _FakeArray = field(default_factory=lambda: _FakeArray(np.zeros(0, dtype=np.int32))) + shape_flags: _FakeArray = field(default_factory=lambda: _FakeArray(np.zeros(0, dtype=np.int32))) + + +@dataclass +class _FakeAsset: + """Stand-in for a scene asset cfg with the ``prim_path`` field consulted by the manager.""" + + prim_path: str + + +@dataclass +class _FakeSceneCfg: + """Stand-in for :class:`InteractiveSceneCfg`. Asset attributes are looked up by ``getattr``.""" + + robot: _FakeAsset | None = None + other: _FakeAsset | None = None + + +## +# Helpers +## + + +def _model_with_two_bodies( + *, + with_shapes: bool = False, + with_joints: bool = False, + extra_static_shape: bool = False, +) -> _FakeModel: + """Build a 2-body Franka-like model under ``/World/envs/env_0/Robot``. + + Args: + with_shapes: Attach one COLLIDE_SHAPES shape to each body. + with_joints: Attach one joint whose child is body 1. + extra_static_shape: Add an extra shape with ``body == -1``. + """ + body_count = 2 + body_label = [ + "/World/envs/env_0/Robot/panda_link0", + "/World/envs/env_0/Robot/panda_hand", + ] + + shape_body = np.zeros(0, dtype=np.int32) + shape_flags = np.zeros(0, dtype=np.int32) + shape_count = 0 + if with_shapes: + owners = [0, 1] + if extra_static_shape: + owners.append(-1) + shape_body = np.asarray(owners, dtype=np.int32) + shape_flags = np.full(len(owners), int(ShapeFlags.COLLIDE_SHAPES), dtype=np.int32) + shape_count = len(owners) + + joint_child = np.zeros(0, dtype=np.int32) + joint_count = 0 + if with_joints: + joint_child = np.asarray([1], dtype=np.int32) + joint_count = 1 + + return _FakeModel( + body_count=body_count, + body_label=body_label, + joint_count=joint_count, + joint_child=_FakeArray(joint_child), + shape_count=shape_count, + shape_body=_FakeArray(shape_body), + shape_flags=_FakeArray(shape_flags), + ) + + +def _robot_scene() -> _FakeSceneCfg: + """A scene cfg with a single ``robot`` asset at the conventional prim path.""" + return _FakeSceneCfg(robot=_FakeAsset(prim_path="/World/envs/env_.*/Robot")) + + +## +# _resolve_entity_to_body_ids +## + + +def test_resolve_entity_no_body_names_returns_all_under_asset(): + model = _model_with_two_bodies() + body_ids = NewtonProxyCoupledMJWarpVBDManager._resolve_entity_to_body_ids( + model, + SceneEntityCfg("robot"), + _robot_scene(), + field="mjwarp_bodies", + ) + assert body_ids == [0, 1] + + +def test_resolve_entity_body_names_filter_by_regex(): + """Patterns full-match against the short name (segment after last ``/``).""" + model = _model_with_two_bodies() + body_ids = NewtonProxyCoupledMJWarpVBDManager._resolve_entity_to_body_ids( + model, + SceneEntityCfg("robot", body_names=["panda_hand"]), + _robot_scene(), + field="proxy_bodies", + ) + assert body_ids == [1] + + +def test_resolve_entity_asset_missing_on_scene_cfg_raises(): + model = _model_with_two_bodies() + with pytest.raises(ValueError, match="not on the attached scene cfg"): + NewtonProxyCoupledMJWarpVBDManager._resolve_entity_to_body_ids( + model, + SceneEntityCfg("missing_asset"), + _robot_scene(), + field="mjwarp_bodies", + ) + + +def test_resolve_entity_unmatched_body_names_raises(): + model = _model_with_two_bodies() + with pytest.raises(ValueError, match="no bodies matching"): + NewtonProxyCoupledMJWarpVBDManager._resolve_entity_to_body_ids( + model, + SceneEntityCfg("robot", body_names=["nonexistent_link"]), + _robot_scene(), + field="proxy_bodies", + ) + + +## +# _partition_model_by_entities +## + + +def test_partition_splits_bodies_joints_shapes(): + """Bodies, joints, and shapes are split by their assigned partition.""" + model = _model_with_two_bodies(with_shapes=True, with_joints=True, extra_static_shape=True) + scene = _FakeSceneCfg( + robot=_FakeAsset(prim_path="/World/envs/env_.*/Robot"), + other=_FakeAsset(prim_path="/World/envs/env_.*/Robot"), + ) + + mjc_b, vbd_b, mjc_j, vbd_j, mjc_s, vbd_s = NewtonProxyCoupledMJWarpVBDManager._partition_model_by_entities( + model, + mjwarp_bodies=[SceneEntityCfg("robot", body_names=["panda_link0"])], + vbd_bodies=[SceneEntityCfg("other", body_names=["panda_hand"])], + scene_cfg=scene, + ) + + assert mjc_b == [0] + assert vbd_b == [1] + # Joint 0's child is body 1 (VBD partition) → the joint index lands in VBD. + assert mjc_j == [] + assert vbd_j == [0] + # Shape 0 → body 0 (MJC). Shape 1 → body 1 (VBD). Shape 2 → body -1 (static, → VBD). + assert mjc_s == [0] + assert vbd_s == [1, 2] + + +def test_partition_overlapping_bodies_raises(): + model = _model_with_two_bodies() + scene = _FakeSceneCfg( + robot=_FakeAsset(prim_path="/World/envs/env_.*/Robot"), + other=_FakeAsset(prim_path="/World/envs/env_.*/Robot"), + ) + with pytest.raises(ValueError, match="match both"): + NewtonProxyCoupledMJWarpVBDManager._partition_model_by_entities( + model, + mjwarp_bodies=[SceneEntityCfg("robot")], + vbd_bodies=[SceneEntityCfg("other", body_names=["panda_hand"])], + scene_cfg=scene, + ) + + +def test_partition_unclaimed_bodies_raises(): + model = _model_with_two_bodies() + with pytest.raises(ValueError, match="unclaimed"): + NewtonProxyCoupledMJWarpVBDManager._partition_model_by_entities( + model, + mjwarp_bodies=[SceneEntityCfg("robot", body_names=["panda_link0"])], + vbd_bodies=[], + scene_cfg=_robot_scene(), + ) + + +## +# _select_proxy_bodies +## + + +def test_select_proxy_bodies_filters_to_collide_shapes(): + """Only bodies with at least one ``COLLIDE_SHAPES``-flagged shape become proxies.""" + model = _model_with_two_bodies(with_shapes=True) + # Knock out body 0's collide flag — only body 1 should remain. + model.shape_flags = _FakeArray(np.asarray([0, int(ShapeFlags.COLLIDE_SHAPES)], dtype=np.int32)) + + proxy_ids = NewtonProxyCoupledMJWarpVBDManager._select_proxy_bodies( + model, + proxy_bodies=[SceneEntityCfg("robot", body_names=["panda_link0", "panda_hand"])], + scene_cfg=_robot_scene(), + ) + assert proxy_ids == [1] + + +def test_select_proxy_bodies_requires_body_names(): + """For ``SceneEntityCfg`` entries — proxies must be a subset, not the whole asset.""" + model = _model_with_two_bodies(with_shapes=True) + with pytest.raises(ValueError, match="requires `body_names`"): + NewtonProxyCoupledMJWarpVBDManager._select_proxy_bodies( + model, + proxy_bodies=[SceneEntityCfg("robot")], + scene_cfg=_robot_scene(), + ) + + +def test_select_proxy_bodies_empty_input_returns_empty(): + """No ``proxy_bodies`` entries → no proxies (short-circuit before model lookups).""" + proxy_ids = NewtonProxyCoupledMJWarpVBDManager._select_proxy_bodies( + model=_FakeModel(body_count=0, body_label=[]), + proxy_bodies=[], + scene_cfg=None, + ) + assert proxy_ids == [] + + +def test_select_proxy_bodies_deduplicates_across_entries(): + """Multiple entries matching the same body produce a single proxy entry.""" + model = _model_with_two_bodies(with_shapes=True) + proxy_ids = NewtonProxyCoupledMJWarpVBDManager._select_proxy_bodies( + model, + proxy_bodies=[ + SceneEntityCfg("robot", body_names=["panda_hand"]), + SceneEntityCfg("robot", body_names=["panda_hand"]), + ], + scene_cfg=_robot_scene(), + ) + assert proxy_ids == [1] + + +## +# Raw prim-path strings as selectors +## + + +def test_resolve_string_prefix_claims_all_bodies_under_path(): + """A raw prim-path string claims every body whose label matches ``^(/|$)``.""" + model = _model_with_two_bodies() + # Asset-prefix regex over the whole robot. + body_ids = NewtonProxyCoupledMJWarpVBDManager._resolve_entity_to_body_ids( + model, + spec="/World/envs/env_.*/Robot", + scene_cfg=None, + field="mjwarp_bodies", + ) + assert body_ids == [0, 1] + + +def test_resolve_string_narrows_to_a_single_body(): + """A specific prim-path string claims only the body with that exact label.""" + model = _model_with_two_bodies() + body_ids = NewtonProxyCoupledMJWarpVBDManager._resolve_entity_to_body_ids( + model, + spec="/World/envs/env_.*/Robot/panda_hand", + scene_cfg=None, + field="proxy_bodies", + ) + assert body_ids == [1] + + +def test_resolve_string_no_matches_raises(): + """A raw prim-path string with zero matches is treated as a typo.""" + model = _model_with_two_bodies() + with pytest.raises(ValueError, match="matched no bodies"): + NewtonProxyCoupledMJWarpVBDManager._resolve_entity_to_body_ids( + model, + spec="/World/envs/env_.*/Nonexistent", + scene_cfg=None, + field="mjwarp_bodies", + ) + + +def test_partition_accepts_mixed_string_and_scene_entity(): + """``mjwarp_bodies`` / ``vbd_bodies`` accept a mix of strings and ``SceneEntityCfg``.""" + model = _model_with_two_bodies(with_shapes=True, with_joints=True) + mjc_b, vbd_b, _, _, _, _ = NewtonProxyCoupledMJWarpVBDManager._partition_model_by_entities( + model, + mjwarp_bodies=[SceneEntityCfg("robot", body_names=["panda_link0"])], + vbd_bodies=["/World/envs/env_.*/Robot/panda_hand"], + scene_cfg=_robot_scene(), + ) + assert mjc_b == [0] + assert vbd_b == [1] + + +def test_select_proxy_bodies_accepts_string_without_body_names(): + """A raw prim-path string in ``proxy_bodies`` bypasses the ``body_names`` requirement.""" + model = _model_with_two_bodies(with_shapes=True) + proxy_ids = NewtonProxyCoupledMJWarpVBDManager._select_proxy_bodies( + model, + proxy_bodies=["/World/envs/env_.*/Robot/panda_hand"], + scene_cfg=None, + ) + assert proxy_ids == [1] diff --git a/source/isaaclab_newton/changelog.d/feat-proxycoupledsolver.rst b/source/isaaclab_newton/changelog.d/feat-proxycoupledsolver.rst new file mode 100644 index 000000000000..585f0864c5f4 --- /dev/null +++ b/source/isaaclab_newton/changelog.d/feat-proxycoupledsolver.rst @@ -0,0 +1,10 @@ +Changed +^^^^^^^ + +* Extracted the repeated solver-kwargs filtering pattern from + :class:`~isaaclab_newton.physics.NewtonFeatherstoneManager`, + :class:`~isaaclab_newton.physics.NewtonMJWarpManager`, and + :class:`~isaaclab_newton.physics.NewtonXPBDManager` into a shared + :meth:`~isaaclab_newton.physics.NewtonManager._filter_solver_kwargs` helper, + so :class:`NewtonManager` subclasses can reuse it when forwarding + ``solver_cfg`` fields to a Newton solver constructor. diff --git a/source/isaaclab_newton/isaaclab_newton/physics/featherstone_manager.py b/source/isaaclab_newton/isaaclab_newton/physics/featherstone_manager.py index a1e918990c19..275d89efacbf 100644 --- a/source/isaaclab_newton/isaaclab_newton/physics/featherstone_manager.py +++ b/source/isaaclab_newton/isaaclab_newton/physics/featherstone_manager.py @@ -7,8 +7,6 @@ from __future__ import annotations -import inspect - from newton import Model from newton.solvers import SolverFeatherstone @@ -29,8 +27,7 @@ def _build_solver(cls, model: Model, solver_cfg: FeatherstoneSolverCfg) -> None: Featherstone always uses Newton's :class:`CollisionPipeline` and steps with separate input/output states, so the flags are fixed. """ - valid = set(inspect.signature(SolverFeatherstone.__init__).parameters) - {"self", "model"} - kwargs = {k: v for k, v in solver_cfg.to_dict().items() if k in valid} + kwargs = cls._filter_solver_kwargs(SolverFeatherstone, solver_cfg) NewtonManager._solver = SolverFeatherstone(model, **kwargs) NewtonManager._use_single_state = False NewtonManager._needs_collision_pipeline = True diff --git a/source/isaaclab_newton/isaaclab_newton/physics/mjwarp_manager.py b/source/isaaclab_newton/isaaclab_newton/physics/mjwarp_manager.py index 01cf6f1996e2..311e83e70f01 100644 --- a/source/isaaclab_newton/isaaclab_newton/physics/mjwarp_manager.py +++ b/source/isaaclab_newton/isaaclab_newton/physics/mjwarp_manager.py @@ -7,7 +7,6 @@ from __future__ import annotations -import inspect import logging import numpy as np @@ -40,8 +39,7 @@ def _build_solver(cls, model: Model, solver_cfg: MJWarpSolverCfg) -> None: forwarded. Sets :attr:`NewtonManager._needs_collision_pipeline` to ``True`` only when ``use_mujoco_contacts=False``. """ - valid = set(inspect.signature(SolverMuJoCo.__init__).parameters) - {"self", "model"} - kwargs = {k: v for k, v in solver_cfg.to_dict().items() if k in valid} + kwargs = cls._filter_solver_kwargs(SolverMuJoCo, solver_cfg) NewtonManager._solver = SolverMuJoCo(model, **kwargs) NewtonManager._use_single_state = True NewtonManager._needs_collision_pipeline = not solver_cfg.use_mujoco_contacts diff --git a/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py b/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py index 500d07b726e7..b1592f73c543 100644 --- a/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py +++ b/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py @@ -9,6 +9,7 @@ import contextlib import ctypes +import inspect import logging from abc import abstractmethod from collections.abc import Callable @@ -1166,6 +1167,17 @@ def _build_solver(cls, model: Model, solver_cfg) -> None: """ raise NotImplementedError("NewtonManager subclasses must implement _build_solver()") + @staticmethod + def _filter_solver_kwargs(solver_cls: type, solver_cfg) -> dict: + """Return cfg fields that match ``solver_cls.__init__`` parameters. + + Drops keys that the solver constructor doesn't accept (e.g. cfg-only + metadata like ``solver_type`` / ``class_type``). ``self`` and ``model`` + are always excluded — ``model`` is passed positionally at construction. + """ + valid = set(inspect.signature(solver_cls.__init__).parameters) - {"self", "model"} + return {k: v for k, v in solver_cfg.to_dict().items() if k in valid} + @classmethod def _step_solver( cls, state_0: State, state_1: State, control: Control, contacts: Contacts | None, substep_dt: float diff --git a/source/isaaclab_newton/isaaclab_newton/physics/xpbd_manager.py b/source/isaaclab_newton/isaaclab_newton/physics/xpbd_manager.py index 40022ef7e0d2..284287e06689 100644 --- a/source/isaaclab_newton/isaaclab_newton/physics/xpbd_manager.py +++ b/source/isaaclab_newton/isaaclab_newton/physics/xpbd_manager.py @@ -7,8 +7,6 @@ from __future__ import annotations -import inspect - from newton import Model from newton.solvers import SolverXPBD @@ -29,8 +27,7 @@ def _build_solver(cls, model: Model, solver_cfg: XPBDSolverCfg) -> None: XPBD always uses Newton's :class:`CollisionPipeline` and steps with separate input/output states, so the flags are fixed. """ - valid = set(inspect.signature(SolverXPBD.__init__).parameters) - {"self", "model"} - kwargs = {k: v for k, v in solver_cfg.to_dict().items() if k in valid} + kwargs = cls._filter_solver_kwargs(SolverXPBD, solver_cfg) NewtonManager._solver = SolverXPBD(model, **kwargs) NewtonManager._use_single_state = False NewtonManager._needs_collision_pipeline = True diff --git a/source/isaaclab_tasks/changelog.d/feat-proxycoupledsolver.minor.rst b/source/isaaclab_tasks/changelog.d/feat-proxycoupledsolver.minor.rst new file mode 100644 index 000000000000..a5d6de563a48 --- /dev/null +++ b/source/isaaclab_tasks/changelog.d/feat-proxycoupledsolver.minor.rst @@ -0,0 +1,19 @@ +Changed +^^^^^^^ + +* **Breaking:** Renamed the deformable scene entity and its MDP terms in + ``lift_franka_soft`` from ``deformable`` to ``object`` to align with the + rigid lift task. Affects ``Isaac-Lift-Soft-Franka-v0`` and the cloth + variant: scene entry ``scene.deformable`` -> ``scene.object``, command + ``deformable_pose`` -> ``object_pose``, and MDP functions + ``deformable_ee_distance``, ``deformable_lifted``, + ``deformable_com_goal_distance``, ``deformable_com_in_robot_root_frame``, + ``deformable_com_below_minimum``, ``deformable_outside_table_bounds``, + ``DeformableSampledPointsInRobotRootFrame`` -> ``object_*`` / + ``ObjectSampledPointsInRobotRootFrame``. Update env configs, checkpoints, + and RL configs accordingly. + +* Migrated ``lift_franka_soft`` (rigid + cloth variants) from + ``DeformableNewtonCfg`` to + :class:`~isaaclab_contrib.deformable.newton_manager_cfg.CoupledNewtonCfg` + with the proxy-coupled MJWarp + VBD solver as the default. diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift_franka_soft/franka_cloth_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift_franka_soft/franka_cloth_env_cfg.py index 1f89cf012c2a..d1233003189d 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift_franka_soft/franka_cloth_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift_franka_soft/franka_cloth_env_cfg.py @@ -10,6 +10,8 @@ from isaaclab_newton.physics import MJWarpSolverCfg from isaaclab_newton.sim.schemas import NewtonDeformableBodyPropertiesCfg from isaaclab_newton.sim.spawners.materials import NewtonSurfaceDeformableBodyMaterialCfg +from isaaclab_visualizers.kit import KitVisualizerCfg +from isaaclab_visualizers.newton import NewtonVisualizerCfg import isaaclab.sim as sim_utils from isaaclab.assets import AssetBaseCfg @@ -19,13 +21,18 @@ from isaaclab.managers import SceneEntityCfg from isaaclab.utils.configclass import configclass -from isaaclab_contrib.deformable.newton_manager_cfg import CoupledMJWarpVBDSolverCfg, NewtonModelCfg, VBDSolverCfg +from isaaclab_contrib.deformable.newton_manager_cfg import ( + CoupledMJWarpVBDSolverCfg, + CoupledNewtonCfg, + NewtonModelCfg, + VBDSolverCfg, +) from isaaclab_tasks.utils import PresetCfg from . import mdp -from .franka_soft_env_cfg import DeformableNewtonCfg, FrankaSoftEnvCfg, _FrankaSoftSceneCfg from .franka_soft_env_cfg import EventCfg as FrankaSoftEventCfg +from .franka_soft_env_cfg import FrankaSoftEnvCfg, _FrankaSoftSceneCfg ## # Scene definition @@ -42,7 +49,7 @@ class PhysicsCfg(PresetCfg): # Newton physics: MJWarp rigid + VBD soft, one-way coupled # (matches newton/examples/softbody/example_softbody_franka.py) - newton_mjwarp_vdb: DeformableNewtonCfg = DeformableNewtonCfg( + newton_mjwarp_vdb: CoupledNewtonCfg = CoupledNewtonCfg( solver_cfg=CoupledMJWarpVBDSolverCfg( rigid_solver_cfg=MJWarpSolverCfg( njmax=40, @@ -108,7 +115,7 @@ class DeformableCfg(PresetCfg): class FrankaClothSceneCfg(_FrankaSoftSceneCfg): """Scene for the Franka surface deformable environment.""" - deformable: DeformableCfg = DeformableCfg() + object: DeformableCfg = DeformableCfg() # static collidable cubes the cloth drops onto (sits on the table top at z = 0). # Modeled as a static asset (no rigid body / no DOFs) so adding it does not @@ -143,33 +150,33 @@ class ActionsCfg: class RewardsCfg: """Lift-to-target reward for a deformable object.""" - reaching_deformable = RewTerm( - func=mdp.deformable_ee_distance, - params={"std": 0.1, "asset_cfg": SceneEntityCfg("deformable")}, + reaching_object = RewTerm( + func=mdp.object_ee_distance, + params={"std": 0.1, "asset_cfg": SceneEntityCfg("object")}, weight=5.0, ) - lifting_deformable = RewTerm( - func=mdp.deformable_lifted, - params={"minimal_height": 0.04, "asset_cfg": SceneEntityCfg("deformable")}, + lifting_object = RewTerm( + func=mdp.object_lifted, + params={"minimal_height": 0.04, "asset_cfg": SceneEntityCfg("object")}, weight=5.0, ) - deformable_goal_tracking = RewTerm( - func=mdp.deformable_com_goal_distance, + object_goal_tracking = RewTerm( + func=mdp.object_com_goal_distance, params={ "std": 0.3, "minimal_height": 0.075, - "command_name": "deformable_pose", - "asset_cfg": SceneEntityCfg("deformable"), + "command_name": "object_pose", + "asset_cfg": SceneEntityCfg("object"), }, weight=16.0, ) - deformable_goal_tracking_fine_grained = RewTerm( - func=mdp.deformable_com_goal_distance, + object_goal_tracking_fine_grained = RewTerm( + func=mdp.object_com_goal_distance, params={ "std": 0.05, "minimal_height": 0.075, - "command_name": "deformable_pose", - "asset_cfg": SceneEntityCfg("deformable"), + "command_name": "object_pose", + "asset_cfg": SceneEntityCfg("object"), }, weight=5.0, ) @@ -228,12 +235,8 @@ def __post_init__(self) -> None: self.sim.dt = 1 / 60.0 self.sim.render_interval = self.decimation - # viewer settings - self.viewer.origin_type = "asset_root" - self.viewer.asset_name = "robot" - self.viewer.env_index = 0 - self.viewer.eye = (1.25, -1.5, 0.6) - self.viewer.resolution = (1920, 1080) + view = dict(eye=(1.25, -1.5, 0.6), lookat=(0.0, 0.0, 0.0), window_width=1920, window_height=1080) + self.sim.visualizer_cfgs = [KitVisualizerCfg(**view), NewtonVisualizerCfg(**view)] self.sim.physics = PhysicsCfg() # increase franka gripper stiffness diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift_franka_soft/franka_soft_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift_franka_soft/franka_soft_env_cfg.py index 9aba49d303c9..836876e7f6b9 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift_franka_soft/franka_soft_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift_franka_soft/franka_soft_env_cfg.py @@ -13,12 +13,14 @@ from __future__ import annotations -from isaaclab_newton.physics import MJWarpSolverCfg, NewtonCfg +from isaaclab_newton.physics import MJWarpSolverCfg from isaaclab_newton.sim.schemas import NewtonDeformableBodyPropertiesCfg from isaaclab_newton.sim.spawners.materials import NewtonDeformableBodyMaterialCfg from isaaclab_physx.physics import PhysxCfg from isaaclab_physx.sim.schemas import PhysxDeformableBodyPropertiesCfg from isaaclab_physx.sim.spawners.materials import PhysxDeformableBodyMaterialCfg +from isaaclab_visualizers.kit import KitVisualizerCfg +from isaaclab_visualizers.newton import NewtonVisualizerCfg import isaaclab.sim as sim_utils from isaaclab.assets import ArticulationCfg, AssetBaseCfg @@ -40,7 +42,13 @@ from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR from isaaclab.utils.configclass import configclass -from isaaclab_contrib.deformable.newton_manager_cfg import CoupledMJWarpVBDSolverCfg, NewtonModelCfg, VBDSolverCfg +from isaaclab_contrib.deformable.newton_manager_cfg import ( + CoupledMJWarpVBDSolverCfg, + CoupledNewtonCfg, + NewtonModelCfg, + ProxyCoupledMJWarpVBDSolverCfg, + VBDSolverCfg, +) from isaaclab_tasks.utils import PresetCfg @@ -63,18 +71,6 @@ POISSONS_RATIO = 0.25 -@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.""" - - @configclass class DeformableCfg(PresetCfg): """Preset config for the deformable object, matching the Newton example.""" @@ -112,14 +108,16 @@ class DeformableCfg(PresetCfg): ), ) - default = newton_mjwarp_vbd + newton_mjwarp_vbd_proxy = newton_mjwarp_vbd + + default = newton_mjwarp_vbd_proxy @configclass class PhysicsCfg(PresetCfg): # Newton physics: MJWarp rigid + VBD soft, one-way coupled # (matches newton/examples/softbody/example_softbody_franka.py) - newton_mjwarp_vbd: DeformableNewtonCfg = DeformableNewtonCfg( + newton_mjwarp_vbd: CoupledNewtonCfg = CoupledNewtonCfg( solver_cfg=CoupledMJWarpVBDSolverCfg( rigid_solver_cfg=MJWarpSolverCfg( njmax=40, @@ -148,12 +146,40 @@ class PhysicsCfg(PresetCfg): shape_material_mu=5.0, ), num_substeps=10, - use_cuda_graph=True, + ) + + newton_mjwarp_vbd_proxy: CoupledNewtonCfg = CoupledNewtonCfg( + solver_cfg=ProxyCoupledMJWarpVBDSolverCfg( + mjwarp_cfg=MJWarpSolverCfg( + cone="elliptic", + ls_parallel=True, + ls_iterations=20, + integrator="implicitfast", + ), + vbd_cfg=VBDSolverCfg( + iterations=10, + ), + mjwarp_bodies=["/World/envs/env_.*/Robot"], + proxy_bodies=[ + "/World/envs/env_.*/Robot/panda_hand", + "/World/envs/env_.*/Robot/panda_(left|right)finger", + ], + proxy_collide_interval=5, + ), + model_cfg=NewtonModelCfg( + soft_contact_ke=1e4, + soft_contact_kd=1e-5, + soft_contact_mu=5.0, + shape_material_ke=4e4, + shape_material_kd=1e-5, + shape_material_mu=5.0, + ), + num_substeps=10, ) physx: PhysxCfg = PhysxCfg() - default = newton_mjwarp_vbd + default = newton_mjwarp_vbd_proxy ## @@ -180,7 +206,7 @@ class _FrankaSoftSceneCfg(InteractiveSceneCfg): ], ) - deformable: DeformableCfg = DeformableCfg() + object: DeformableCfg = DeformableCfg() # static table matching the Newton example: half-extents (0.4, 0.4, 0.1) → top at z = 0.2 # NOTE: SeattleLabTable USD has its origin on the top surface, so the deformable object @@ -231,7 +257,7 @@ def __post_init__(self) -> None: class CommandsCfg: """Commands for the deformable goal pose (xyz + identity quat in robot root frame).""" - deformable_pose = mdp.UniformPoseCommandCfg( + object_pose = mdp.UniformPoseCommandCfg( asset_name="robot", body_name="panda_hand", resampling_time_range=(5.0, 5.0), @@ -292,11 +318,11 @@ class ObservationsCfg: class PolicyCfg(ObsGroup): joint_pos = ObsTerm(func=mdp.joint_pos_rel) joint_vel = ObsTerm(func=mdp.joint_vel_rel) - deformable_sampled_points = ObsTerm( - func=mdp.DeformableSampledPointsInRobotRootFrame, - params={"asset_cfg": SceneEntityCfg("deformable"), "num_points": 20}, + object_sampled_points = ObsTerm( + func=mdp.ObjectSampledPointsInRobotRootFrame, + params={"asset_cfg": SceneEntityCfg("object"), "num_points": 20}, ) - target_position = ObsTerm(func=mdp.generated_commands, params={"command_name": "deformable_pose"}) + target_position = ObsTerm(func=mdp.generated_commands, params={"command_name": "object_pose"}) actions = ObsTerm(func=mdp.last_action) def __post_init__(self) -> None: @@ -316,13 +342,13 @@ class EventCfg: params={"position_range": (0.9, 1.1), "velocity_range": (0.0, 0.0)}, ) - reset_deformable = EventTerm( + reset_object = EventTerm( func=mdp.reset_nodal_state_uniform, mode="reset", params={ "position_range": {"x": (-0.05, 0.05), "y": (-0.05, 0.05), "z": (0.0, 0.0)}, "velocity_range": {}, - "asset_cfg": SceneEntityCfg("deformable"), + "asset_cfg": SceneEntityCfg("object"), }, ) @@ -331,33 +357,33 @@ class EventCfg: class RewardsCfg: """Lift-to-target reward for a deformable object.""" - reaching_deformable = RewTerm( - func=mdp.deformable_ee_distance, - params={"std": 0.1, "asset_cfg": SceneEntityCfg("deformable")}, + reaching_object = RewTerm( + func=mdp.object_ee_distance, + params={"std": 0.1, "asset_cfg": SceneEntityCfg("object")}, weight=5.0, ) - lifting_deformable = RewTerm( - func=mdp.deformable_lifted, - params={"minimal_height": 0.04, "asset_cfg": SceneEntityCfg("deformable")}, + lifting_object = RewTerm( + func=mdp.object_lifted, + params={"minimal_height": 0.04, "asset_cfg": SceneEntityCfg("object")}, weight=5.0, ) - deformable_goal_tracking = RewTerm( - func=mdp.deformable_com_goal_distance, + object_goal_tracking = RewTerm( + func=mdp.object_com_goal_distance, params={ "std": 0.3, "minimal_height": 0.075, - "command_name": "deformable_pose", - "asset_cfg": SceneEntityCfg("deformable"), + "command_name": "object_pose", + "asset_cfg": SceneEntityCfg("object"), }, weight=16.0, ) - deformable_goal_tracking_fine_grained = RewTerm( - func=mdp.deformable_com_goal_distance, + object_goal_tracking_fine_grained = RewTerm( + func=mdp.object_com_goal_distance, params={ "std": 0.05, "minimal_height": 0.075, - "command_name": "deformable_pose", - "asset_cfg": SceneEntityCfg("deformable"), + "command_name": "object_pose", + "asset_cfg": SceneEntityCfg("object"), }, weight=5.0, ) @@ -379,18 +405,18 @@ class TerminationsCfg: time_out = DoneTerm(func=mdp.time_out, time_out=True) - deformable_outside_table = DoneTerm( - func=mdp.deformable_outside_table_bounds, + object_outside_table = DoneTerm( + func=mdp.object_outside_table_bounds, params={ "x_bounds": (0.0, 1.0), "y_bounds": (-0.5, 0.5), - "asset_cfg": SceneEntityCfg("deformable"), + "asset_cfg": SceneEntityCfg("object"), }, ) - deformable_dropped = DoneTerm( - func=mdp.deformable_com_below_minimum, - params={"minimum_height": -0.1, "asset_cfg": SceneEntityCfg("deformable")}, + object_dropped = DoneTerm( + func=mdp.object_com_below_minimum, + params={"minimum_height": -0.1, "asset_cfg": SceneEntityCfg("object")}, ) ee_below_table = DoneTerm( @@ -411,7 +437,9 @@ class FrankaSoftSceneCfg(PresetCfg): # PhysX does not support replicating physics for deformable objects physx: _FrankaSoftSceneCfg = _FrankaSoftSceneCfg(num_envs=128, env_spacing=2.5, replicate_physics=False) - default = newton_mjwarp_vbd + newton_mjwarp_vbd_proxy = newton_mjwarp_vbd + + default = newton_mjwarp_vbd_proxy @configclass @@ -440,9 +468,5 @@ def __post_init__(self) -> None: self.sim.gravity = (0.0, 0.0, 0.0) self.sim.physics = PhysicsCfg() - # viewer settings - self.viewer.origin_type = "asset_root" - self.viewer.asset_name = "robot" - self.viewer.env_index = 0 - self.viewer.eye = (1.25, -1.5, 0.75) - self.viewer.resolution = (1920, 1080) + view = dict(eye=(1.25, -1.5, 0.75), lookat=(0.0, 0.0, 0.0), window_width=1920, window_height=1080) + self.sim.visualizer_cfgs = [KitVisualizerCfg(**view), NewtonVisualizerCfg(**view)] diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift_franka_soft/mdp/__init__.pyi b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift_franka_soft/mdp/__init__.pyi index bb8b3af19852..d0aa5d596cee 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift_franka_soft/mdp/__init__.pyi +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift_franka_soft/mdp/__init__.pyi @@ -4,25 +4,28 @@ # SPDX-License-Identifier: BSD-3-Clause __all__ = [ - "deformable_com_below_minimum", - "deformable_ee_distance", - "deformable_com_goal_distance", - "deformable_com_in_robot_root_frame", - "DeformableSampledPointsInRobotRootFrame", - "deformable_lifted", - "deformable_outside_table_bounds", "ee_below_minimum", "gripper_close_action", + "object_com_below_minimum", + "object_com_goal_distance", + "object_com_in_robot_root_frame", + "object_ee_distance", + "object_lifted", + "object_outside_table_bounds", + "ObjectSampledPointsInRobotRootFrame", ] -from .observations import DeformableSampledPointsInRobotRootFrame, deformable_com_in_robot_root_frame +from .observations import ( + ObjectSampledPointsInRobotRootFrame, + object_com_in_robot_root_frame, +) from .rewards import ( - deformable_com_below_minimum, - deformable_ee_distance, - deformable_com_goal_distance, - deformable_lifted, - deformable_outside_table_bounds, ee_below_minimum, gripper_close_action, + object_com_below_minimum, + object_com_goal_distance, + object_ee_distance, + object_lifted, + object_outside_table_bounds, ) from isaaclab.envs.mdp import * diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift_franka_soft/mdp/observations.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift_franka_soft/mdp/observations.py index a1aa2fa0aa64..fc91dd4f4452 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift_franka_soft/mdp/observations.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift_franka_soft/mdp/observations.py @@ -11,82 +11,82 @@ from typing import TYPE_CHECKING import torch -import warp as wp from isaaclab.managers import ManagerTermBase, SceneEntityCfg from isaaclab.utils.math import subtract_frame_transforms +from .rewards import _com_w, _points_w + if TYPE_CHECKING: - from isaaclab.assets import Articulation, DeformableObject + from isaaclab.assets import Articulation from isaaclab.envs import ManagerBasedRLEnv -def deformable_com_in_robot_root_frame( +def object_com_in_robot_root_frame( env: ManagerBasedRLEnv, - asset_cfg: SceneEntityCfg = SceneEntityCfg("deformable"), + asset_cfg: SceneEntityCfg, robot_cfg: SceneEntityCfg = SceneEntityCfg("robot"), ) -> torch.Tensor: - """Position of the deformable object's COM in the robot's root frame [m]. - - The COM is the mean of the deformable's nodal positions (see - :attr:`~isaaclab.assets.DeformableObject.data.root_pos_w`). + """Position of the asset's COM in the robot's root frame [m]. Returns: Tensor of shape ``(num_envs, 3)``. """ - asset: DeformableObject = env.scene[asset_cfg.name] + asset = env.scene[asset_cfg.name] robot: Articulation = env.scene[robot_cfg.name] - com_w = wp.to_torch(asset.data.root_pos_w) - com_b, _ = subtract_frame_transforms(wp.to_torch(robot.data.root_pos_w), wp.to_torch(robot.data.root_quat_w), com_w) + com_w = _com_w(asset) + com_b, _ = subtract_frame_transforms(robot.data.root_pos_w.torch, robot.data.root_quat_w.torch, com_w) return com_b -class DeformableSampledPointsInRobotRootFrame(ManagerTermBase): - """Sampled deformable nodal points expressed in the robot's root frame. +class ObjectSampledPointsInRobotRootFrame(ManagerTermBase): + """Sampled asset point positions expressed in the robot's root frame. - The point indices are sampled on reset, then reused within the episode so + The point indices are sampled on reset and reused within the episode so each observed point follows the same material node over time. """ def __init__(self, cfg, env: ManagerBasedRLEnv): super().__init__(cfg, env) - self.asset_cfg: SceneEntityCfg = cfg.params.get("asset_cfg", SceneEntityCfg("deformable")) + self.asset_cfg: SceneEntityCfg = cfg.params["asset_cfg"] self.robot_cfg: SceneEntityCfg = cfg.params.get("robot_cfg", SceneEntityCfg("robot")) self.num_points: int = cfg.params.get("num_points", 20) - asset: DeformableObject = env.scene[self.asset_cfg.name] - self.num_nodes = asset.data.nodal_pos_w.shape[1] - self.node_ids = torch.empty(env.num_envs, self.num_points, dtype=torch.long, device=env.device) + asset = env.scene[self.asset_cfg.name] + self.num_source_points = _points_w(asset).shape[1] + self.point_ids = torch.empty(env.num_envs, self.num_points, dtype=torch.long, device=env.device) self.reset() def reset(self, env_ids: Sequence[int] | None = None) -> None: - """Resample observed deformable nodes for the selected environments.""" + """Resample observed point indices for the selected environments.""" if env_ids is None: env_ids = slice(None) num_envs = self.num_envs else: num_envs = len(env_ids) - if self.num_points <= self.num_nodes: - self.node_ids[env_ids] = ( - torch.rand((num_envs, self.num_nodes), device=self.device).topk(self.num_points, dim=1).indices + if self.num_points <= self.num_source_points: + self.point_ids[env_ids] = ( + torch.rand((num_envs, self.num_source_points), device=self.device).topk(self.num_points, dim=1).indices ) else: - self.node_ids[env_ids] = torch.randint(self.num_nodes, (num_envs, self.num_points), device=self.device) + self.point_ids[env_ids] = torch.randint( + self.num_source_points, (num_envs, self.num_points), device=self.device + ) def __call__( self, env: ManagerBasedRLEnv, - asset_cfg: SceneEntityCfg = SceneEntityCfg("deformable"), + asset_cfg: SceneEntityCfg, robot_cfg: SceneEntityCfg = SceneEntityCfg("robot"), num_points: int = 20, ) -> torch.Tensor: - """Sample deformable nodal positions in the robot's root frame. + """Sample asset point positions in the robot's root frame. Args: env: The environment instance. - asset_cfg: The deformable object entity. + asset_cfg: The deformable entity. robot_cfg: The robot entity providing the reference frame. num_points: Number of sampled points. @@ -94,15 +94,13 @@ def __call__( Flattened tensor of shape ``(num_envs, 3 * num_points)`` with sampled point positions [m] in the robot root frame. """ - asset: DeformableObject = env.scene[asset_cfg.name] + asset = env.scene[asset_cfg.name] robot: Articulation = env.scene[robot_cfg.name] if num_points != self.num_points: - raise ValueError( - f"Requested {num_points} deformable points, but this term was initialized with {self.num_points}." - ) + raise ValueError(f"Requested {num_points} points, but this term was initialized with {self.num_points}.") - nodal_pos_w = asset.data.nodal_pos_w.torch - sampled_points_w = nodal_pos_w.gather(1, self.node_ids.unsqueeze(-1).expand(-1, -1, 3)) + points_w = _points_w(asset) + sampled_points_w = points_w.gather(1, self.point_ids.unsqueeze(-1).expand(-1, -1, 3)) flat_sampled_points_w = sampled_points_w.reshape(-1, 3) root_pos_w = robot.data.root_pos_w.torch.unsqueeze(1).expand(-1, num_points, -1) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift_franka_soft/mdp/rewards.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift_franka_soft/mdp/rewards.py index 9811051cd370..923ced51d492 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift_franka_soft/mdp/rewards.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/lift_franka_soft/mdp/rewards.py @@ -21,72 +21,82 @@ from isaaclab.sensors import FrameTransformer -def deformable_lifted( +def _points_w(asset: DeformableObject) -> torch.Tensor: + """Return deformable nodal positions in world frame, shape ``[num_envs, K, 3]`` [m].""" + return wp.to_torch(asset.data.nodal_pos_w) + + +def _com_w(asset: DeformableObject) -> torch.Tensor: + """Return the deformable's centre of mass in world frame, shape ``[num_envs, 3]`` [m].""" + return wp.to_torch(asset.data.root_pos_w) + + +def object_lifted( env: ManagerBasedRLEnv, minimal_height: float, - asset_cfg: SceneEntityCfg = SceneEntityCfg("deformable"), + asset_cfg: SceneEntityCfg, ) -> torch.Tensor: - """Reward if the deformable COM is above a minimum height. + """Reward if the asset's COM is above a minimum height. Args: env: The environment instance. minimal_height: Minimum COM height [m]. - asset_cfg: The deformable object entity. + asset_cfg: The deformable entity. Returns: Reward tensor with shape ``(num_envs,)``. """ - asset: DeformableObject = env.scene[asset_cfg.name] - com_z = wp.to_torch(asset.data.root_pos_w)[:, 2] + asset = env.scene[asset_cfg.name] + com_z = _com_w(asset)[:, 2] return torch.where(com_z > minimal_height, 1.0, 0.0) -def deformable_ee_distance( +def object_ee_distance( env: ManagerBasedRLEnv, std: float, - asset_cfg: SceneEntityCfg = SceneEntityCfg("deformable"), + asset_cfg: SceneEntityCfg, ee_frame_cfg: SceneEntityCfg = SceneEntityCfg("ee_frame"), ) -> torch.Tensor: - """Reward reaching the deformable's nearest nodal point with the end-effector. + """Reward reaching the asset's nearest point with the end-effector. Args: env: The environment instance. std: The tanh kernel standard deviation [m]. - asset_cfg: The deformable object entity. + asset_cfg: The deformable entity. ee_frame_cfg: The end-effector frame entity. Returns: Reward tensor with shape ``(num_envs,)``. """ - asset: DeformableObject = env.scene[asset_cfg.name] + asset = env.scene[asset_cfg.name] ee_frame: FrameTransformer = env.scene[ee_frame_cfg.name] - nodal_pos_w = wp.to_torch(asset.data.nodal_pos_w) + points_w = _points_w(asset) ee_w = wp.to_torch(ee_frame.data.target_pos_w)[..., 0, :] - distance = torch.linalg.norm(nodal_pos_w - ee_w.unsqueeze(1), dim=2).min(dim=1).values + distance = torch.linalg.norm(points_w - ee_w.unsqueeze(1), dim=2).min(dim=1).values return 1.0 - torch.tanh(distance / std) -def deformable_com_goal_distance( +def object_com_goal_distance( env: ManagerBasedRLEnv, std: float, minimal_height: float, command_name: str, + asset_cfg: SceneEntityCfg, robot_cfg: SceneEntityCfg = SceneEntityCfg("robot"), - asset_cfg: SceneEntityCfg = SceneEntityCfg("deformable"), ) -> torch.Tensor: - """Reward tracking of the goal position by the deformable's COM (tanh kernel). + """Reward tracking of the goal position by the asset's COM (tanh kernel). Only credits when the COM is above ``minimal_height`` (i.e. the object is lifted). The command is interpreted as ``[x, y, z, qw, qx, qy, qz]`` in the robot's root frame. """ robot: Articulation = env.scene[robot_cfg.name] - asset: DeformableObject = env.scene[asset_cfg.name] + asset = env.scene[asset_cfg.name] command = env.command_manager.get_command(command_name) des_pos_b = command[:, :3] des_pos_w, _ = combine_frame_transforms( wp.to_torch(robot.data.root_pos_w), wp.to_torch(robot.data.root_quat_w), des_pos_b ) - com_w = wp.to_torch(asset.data.root_pos_w) + com_w = _com_w(asset) distance = torch.linalg.norm(des_pos_w - com_w, dim=1) return (com_w[:, 2] > minimal_height) * (1.0 - torch.tanh(distance / std)) @@ -109,38 +119,38 @@ def gripper_close_action(env: ManagerBasedRLEnv, action_name: str = "gripper_act return torch.any(gripper_action < 0.0, dim=1).float() -def deformable_com_below_minimum( +def object_com_below_minimum( env: ManagerBasedRLEnv, minimum_height: float, - asset_cfg: SceneEntityCfg = SceneEntityCfg("deformable"), + asset_cfg: SceneEntityCfg, ) -> torch.Tensor: - """Termination signal when the deformable's COM falls below ``minimum_height`` [m].""" - asset: DeformableObject = env.scene[asset_cfg.name] - com_z = wp.to_torch(asset.data.root_pos_w)[:, 2] + """Termination signal when the asset's COM falls below ``minimum_height`` [m].""" + asset = env.scene[asset_cfg.name] + com_z = _com_w(asset)[:, 2] return com_z < minimum_height -def deformable_outside_table_bounds( +def object_outside_table_bounds( env: ManagerBasedRLEnv, x_bounds: tuple[float, float], y_bounds: tuple[float, float], - asset_cfg: SceneEntityCfg = SceneEntityCfg("deformable"), + asset_cfg: SceneEntityCfg, ) -> torch.Tensor: - """Terminate if any deformable nodal point leaves the table footprint. + """Terminate if any asset point leaves the table footprint. Args: env: The environment instance. x_bounds: Allowed x-position range in the environment frame [m]. y_bounds: Allowed y-position range in the environment frame [m]. - asset_cfg: The deformable object entity. + asset_cfg: The deformable entity. Returns: Boolean tensor with shape ``(num_envs,)``. """ - asset: DeformableObject = env.scene[asset_cfg.name] - nodal_pos = wp.to_torch(asset.data.nodal_pos_w) - env.scene.env_origins.unsqueeze(1) - outside_x = (nodal_pos[..., 0] < x_bounds[0]) | (nodal_pos[..., 0] > x_bounds[1]) - outside_y = (nodal_pos[..., 1] < y_bounds[0]) | (nodal_pos[..., 1] > y_bounds[1]) + asset = env.scene[asset_cfg.name] + points = _points_w(asset) - env.scene.env_origins.unsqueeze(1) + outside_x = (points[..., 0] < x_bounds[0]) | (points[..., 0] > x_bounds[1]) + outside_y = (points[..., 1] < y_bounds[0]) | (points[..., 1] > y_bounds[1]) return torch.any(outside_x | outside_y, dim=1)