diff --git a/docs/source/_static/css/environment-browser.js b/docs/source/_static/css/environment-browser.js index 02dd2940055e..93fcd005568d 100644 --- a/docs/source/_static/css/environment-browser.js +++ b/docs/source/_static/css/environment-browser.js @@ -21,13 +21,13 @@ ["Isaac-Fourbar-Pole-Swingup", "rsl_rl", "newton_kamino", "", ""], ["Isaac-Humanoid", "rl_games,rsl_rl,skrl,sb3", "isaacsim_physx,newton_mjwarp", "", ""], ["Isaac-Humanoid-Direct", "rl_games,rsl_rl,skrl", "isaacsim_physx,newton_mjwarp,ovphysx", "", ""], - ["Isaac-Lift-Cloth-Franka", "rsl_rl", "newton_mjwarp_vbd_proxy,ovphysx", "", ""], - ["Isaac-Lift-Cloth-Franka-Camera", "rsl_rl", "newton_mjwarp_vbd_proxy,ovphysx", "isaacsim_rtx,newton_renderer,ovrtx", ""], + ["Isaac-Lift-Cloth-Franka", "rsl_rl", "newton_mjwarp_vbd_proxy", "", "ik,joint"], + ["Isaac-Lift-Cloth-Franka-Camera", "rsl_rl", "newton_mjwarp_vbd_proxy", "isaacsim_rtx,newton_renderer,ovrtx", "ik,joint"], ["Isaac-Lift-Franka", "rsl_rl", "isaacsim_physx,newton_mjwarp", "", "cube,shapes"], ["Isaac-Lift-KukaAllegro", "rsl_rl", "isaacsim_physx,newton_mjwarp,ovphysx", "", "cube,shapes"], ["Isaac-Lift-KukaAllegro-Camera", "rsl_rl", "isaacsim_physx,newton_mjwarp,ovphysx", "isaacsim_rtx,newton_renderer,ovrtx", "albedo128,albedo256,albedo64,cube,depth128,depth256,depth64,duo_camera,raycaster_depth128,raycaster_depth256,raycaster_depth64,rgb128,rgb256,rgb64,semantic_segmentation128,semantic_segmentation256,semantic_segmentation64,shapes,simple_shading_constant_diffuse128,simple_shading_constant_diffuse256,simple_shading_constant_diffuse64,simple_shading_diffuse_mdl128,simple_shading_diffuse_mdl256,simple_shading_diffuse_mdl64,simple_shading_full_mdl128,simple_shading_full_mdl256,simple_shading_full_mdl64,single_camera"], - ["Isaac-Lift-Soft-Franka", "rsl_rl", "isaacsim_physx,newton_mjwarp_vbd_proxy,ovphysx", "", ""], - ["Isaac-Lift-Soft-Franka-Camera", "rsl_rl", "isaacsim_physx,newton_mjwarp_vbd_proxy,ovphysx", "isaacsim_rtx,newton_renderer,ovrtx", ""], + ["Isaac-Lift-Soft-Franka", "rsl_rl", "isaacsim_physx,newton_mjwarp_vbd_proxy", "", "ik,joint"], + ["Isaac-Lift-Soft-Franka-Camera", "rsl_rl", "isaacsim_physx,newton_mjwarp_vbd_proxy", "isaacsim_rtx,newton_renderer,ovrtx", "ik,joint"], ["Isaac-Open-Drawer-Franka", "rl_games,rsl_rl,skrl", "", "", ""], ["Isaac-Open-Drawer-Franka-Direct", "rl_games,rsl_rl,skrl", "isaacsim_physx,newton_mjwarp,ovphysx", "", ""], ["Isaac-Pendulum-Direct", "rl_games,skrl", "", "", ""], 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 6f5b46bcb912..9faf0758f78b 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 @@ -272,7 +272,7 @@ The core Franka soft-body task demonstrates the proxy configuration: .. literalinclude:: ../../../../../../source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/franka_soft_env_cfg.py :language: python :start-at: newton_mjwarp_vbd_proxy: NewtonCfg - :end-before: isaacsim_physx: PhysxCfg = PhysxCfg() + :end-before: isaacsim_physx: PhysxCfg = PhysxCfg( :dedent: 4 What the selectors do: diff --git a/docs/source/overview/environments.rst b/docs/source/overview/environments.rst index b8d38a83dccc..c2f1882fe427 100644 --- a/docs/source/overview/environments.rst +++ b/docs/source/overview/environments.rst @@ -232,24 +232,24 @@ for the lift-cube environment: | |lift-cube| | |lift-cube-link| | Pick a cube and bring it to a sampled target position with the Franka robot | | +-------------------------+------------------------------+-----------------------------------------------------------------------------+-----------------------------------------+ | |lift-soft-franka| | |lift-soft-franka-link| | Pick a deformable soft body and bring it to a sampled target position with | **physics=** ``isaacsim_physx``, | - | | | the Franka robot | ``newton_mjwarp_vbd_proxy``, | - | | | | ``ovphysx`` | + | | | the Franka robot | ``newton_mjwarp_vbd_proxy`` | + | | | | **presets=** ``ik``, ``joint`` | +-------------------------+------------------------------+-----------------------------------------------------------------------------+-----------------------------------------+ | |lift-soft-franka| | |lift-soft-franka-cam-link| | Camera (vision) variant of the soft-body lift task using RGB observations | **physics=** ``isaacsim_physx``, | - | | | | ``newton_mjwarp_vbd_proxy``, | - | | | | ``ovphysx`` | + | | | | ``newton_mjwarp_vbd_proxy`` | | | | | **renderer=** ``isaacsim_rtx``, | | | | | ``newton_renderer``, ``ovrtx`` | + | | | | **presets=** ``ik``, ``joint`` | +-------------------------+------------------------------+-----------------------------------------------------------------------------+-----------------------------------------+ | |lift-cloth-franka| | |lift-cloth-franka-link| | Lift a deformable cloth from a table with the Franka robot | **physics=** | - | | | | ``newton_mjwarp_vbd_proxy``, | - | | | | ``ovphysx`` | + | | | | ``newton_mjwarp_vbd_proxy`` | + | | | | **presets=** ``ik``, ``joint`` | +-------------------------+------------------------------+-----------------------------------------------------------------------------+-----------------------------------------+ | |lift-cloth-franka| | |lift-cloth-franka-cam-link| | Camera (vision) variant of the cloth lift task using RGB observations | **physics=** | - | | | | ``newton_mjwarp_vbd_proxy``, | - | | | | ``ovphysx`` | + | | | | ``newton_mjwarp_vbd_proxy`` | | | | | **renderer=** ``isaacsim_rtx``, | | | | | ``newton_renderer``, ``ovrtx`` | + | | | | **presets=** ``ik``, ``joint`` | +-------------------------+------------------------------+-----------------------------------------------------------------------------+-----------------------------------------+ | |stack-cube| | |stack-cube-link| | Stack three cubes (bottom to top: blue, red, green) with the Franka robot. | **physics=** ``isaacsim_physx``, | | | | Blueprint env used for the NVIDIA Isaac GR00T blueprint for synthetic | ``newton_mjwarp`` | @@ -1093,12 +1093,14 @@ including disabling runtime perturbations used for training. * - Isaac-Lift-Cloth-Franka - Manager Based - **rsl_rl** (PPO) - - **physics=** ``newton_mjwarp_vbd_proxy``, ``ovphysx`` + - | **physics=** ``newton_mjwarp_vbd_proxy`` + | **presets=** ``ik``, ``joint`` * - Isaac-Lift-Cloth-Franka-Camera - Manager Based - **rsl_rl** (PPO) - - | **physics=** ``newton_mjwarp_vbd_proxy``, ``ovphysx`` + - | **physics=** ``newton_mjwarp_vbd_proxy`` | **renderer=** ``isaacsim_rtx``, ``newton_renderer``, ``ovrtx`` + | **presets=** ``ik``, ``joint`` * - Isaac-Lift-Franka - Manager Based - **rsl_rl** (PPO) @@ -1118,12 +1120,14 @@ including disabling runtime perturbations used for training. * - Isaac-Lift-Soft-Franka - Manager Based - **rsl_rl** (PPO) - - **physics=** ``isaacsim_physx``, ``newton_mjwarp_vbd_proxy``, ``ovphysx`` + - | **physics=** ``isaacsim_physx``, ``newton_mjwarp_vbd_proxy`` + | **presets=** ``ik``, ``joint`` * - Isaac-Lift-Soft-Franka-Camera - Manager Based - **rsl_rl** (PPO) - - | **physics=** ``isaacsim_physx``, ``newton_mjwarp_vbd_proxy``, ``ovphysx`` + - | **physics=** ``isaacsim_physx``, ``newton_mjwarp_vbd_proxy`` | **renderer=** ``isaacsim_rtx``, ``newton_renderer``, ``ovrtx`` + | **presets=** ``ik``, ``joint`` * - Isaac-Open-Drawer-Franka - Manager Based - **rl_games** (PPO), **rsl_rl** (PPO), **skrl** (PPO) diff --git a/scripts/environments/state_machine/lift_franka_soft.py b/scripts/environments/state_machine/lift_franka_soft.py index bd7da899719b..6ba98adce128 100644 --- a/scripts/environments/state_machine/lift_franka_soft.py +++ b/scripts/environments/state_machine/lift_franka_soft.py @@ -11,59 +11,40 @@ .. code-block:: bash + # Kitless run with the Newton OpenGL viewer (default). uv run python scripts/environments/state_machine/lift_franka_soft.py -""" + # Headless. + uv run python scripts/environments/state_machine/lift_franka_soft.py --viz none -"""Launch Omniverse Toolkit first.""" +""" import argparse - -from isaaclab.app import AppLauncher - -# add argparse arguments -parser = argparse.ArgumentParser(description="Pick and lift a deformable with a robotic arm.") -parser.add_argument("--num_envs", type=int, default=1, help="Number of environments to simulate.") -parser.add_argument("--task", type=str, default="Isaac-Lift-Soft-Franka", help="The task to run.") -parser.add_argument("--video", action="store_true", default=False, help="Record a video of the rollout.") -parser.add_argument("--video_length", type=int, default=500, help="Length of the recorded video (in env steps).") -parser.add_argument( - "--video_folder", - type=str, - default="videos/lift_franka_soft", - help="Directory to write recorded videos into.", -) -# append AppLauncher cli args -AppLauncher.add_app_launcher_args(parser) -# parse the arguments -args_cli = parser.parse_args() - -# RecordVideo needs an rgb_array render mode, which in turn requires cameras to be enabled. -if args_cli.video: - args_cli.enable_cameras = True - -# launch omniverse app -app_launcher = AppLauncher(args_cli) -simulation_app = app_launcher.app - -# disable metrics assembler due to scene graph instancing -from isaaclab.sim.utils import disable_extension - -disable_extension("omni.usd.metrics.assembler.ui") - -"""Rest everything else.""" - -import os +import sys from collections.abc import Sequence import gymnasium as gym import torch import warp as wp +from isaaclab.app import add_launcher_args, launch_simulation from isaaclab.assets.deformable_object.deformable_object_data import DeformableObjectData +from isaaclab.visualizers import VisualizerCfg import isaaclab_tasks # noqa: F401 -from isaaclab_tasks.utils.parse_cfg import parse_env_cfg +from isaaclab_tasks.core.lift.config.franka_soft.franka_soft_env_cfg import ActionsCfg +from isaaclab_tasks.utils import resolve_task_config, setup_preset_cli + +# add argparse arguments +parser = argparse.ArgumentParser(description="Pick and lift a deformable with a robotic arm.") +parser.add_argument("--num_envs", type=int, default=1, help="Number of environments to simulate.") +parser.add_argument("--num_steps", type=int, default=1000, help="Number of environment steps to run.") +parser.add_argument("--task", type=str, default="Isaac-Lift-Soft-Franka", help="The task to run.") +add_launcher_args(parser) +# the task runs on Newton, so default to the kitless viewer +parser.set_defaults(visualizer=["newton"]) +args_cli, hydra_args = setup_preset_cli(parser) +sys.argv = [sys.argv[0]] + hydra_args # initialize warp wp.init() @@ -185,7 +166,7 @@ class PickSmWaitTime: REST = wp.constant(0.2) APPROACH_ABOVE_OBJECT = wp.constant(1.0) - APPROACH_OBJECT = wp.constant(1.0) + APPROACH_OBJECT = wp.constant(1.5) GRASP_OBJECT = wp.constant(1.0) LIFT_OBJECT = wp.constant(1.5) OPEN_GRIPPER = wp.constant(0.0) @@ -280,89 +261,76 @@ 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 - # 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: - video_folder = os.path.abspath(args_cli.video_folder) - os.makedirs(video_folder, exist_ok=True) - env = gym.wrappers.RecordVideo( - env, - video_folder=video_folder, - step_trigger=lambda step: step == 0, - video_length=args_cli.video_length, - disable_logger=True, - ) - print(f"[INFO] Recording video to {video_folder} (length={args_cli.video_length} steps)") - - # reset environment at start - env.reset() - - # create action buffers (position + quaternion) - actions = torch.zeros(env.unwrapped.action_space.shape, device=env.unwrapped.device) - actions[:, 3] = 1.0 - # desired rotation after grasping - desired_orientation = torch.zeros((env.unwrapped.num_envs, 4), device=env.unwrapped.device) - desired_orientation[:, 0] = 1.0 - - # Top-down approach: identity quaternion (wxyz, w=1) aligns panda_hand with the Franka root, - # giving the canonical top-down grasp pose. The bar lies along world-X, so the gripper - # closes across its short side without any wrist twist. - object_grasp_orientation = torch.zeros((env.unwrapped.num_envs, 4), device=env.unwrapped.device) - object_grasp_orientation[:, 0] = 1.0 - # Grasp at the deformable's centre of mass. - object_local_grasp_position = torch.tensor([0.0, 0.0, 0.0], device=env.unwrapped.device) - - # create state machine - pick_sm = PickAndLiftSm(env_cfg.sim.dt * env_cfg.decimation, env.unwrapped.num_envs, env.unwrapped.device) - - while simulation_app.is_running(): - # run everything in inference mode - with torch.inference_mode(): - # step environment - dones = env.step(actions)[-2] - - # observations - # -- end-effector frame - ee_frame_sensor = env.unwrapped.scene["ee_frame"] - tcp_rest_position = ( - ee_frame_sensor.data.target_pos_w.torch[..., 0, :].clone() - env.unwrapped.scene.env_origins - ) - tcp_rest_orientation = ee_frame_sensor.data.target_quat_w.torch[..., 0, :].clone() - # -- object frame - object_data: DeformableObjectData = env.unwrapped.scene["deformable"].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] - - # advance state machine - actions = pick_sm.compute( - torch.cat([tcp_rest_position, tcp_rest_orientation], dim=-1), - torch.cat([object_position, object_grasp_orientation], dim=-1), - torch.cat([desired_position, desired_orientation], dim=-1), - ) - - # reset state machine - if dones.any(): - pick_sm.reset_idx(dones.nonzero(as_tuple=False).squeeze(-1)) - - # close the environment - env.close() + # parse configuration via Hydra, so presets can be selected on the CLI (e.g. presets=isaacsim_physx) + env_cfg, _ = resolve_task_config(args_cli.task, "") + env_cfg.sim.device = args_cli.device + env_cfg.scene.num_envs = args_cli.num_envs + # the state machine emits absolute end-effector poses, so pick the IK action preset; the env + # defaults to relative joint targets, which RL trains on. + env_cfg.actions = ActionsCfg().ik + env_cfg.viewer.eye = (1.3, 0.6, 0.5) + env_cfg.viewer.lookat = (0.5, 0.0, 0.05) + env_cfg.sim.default_visualizer_cfg = VisualizerCfg(eye=env_cfg.viewer.eye, lookat=env_cfg.viewer.lookat) + + with launch_simulation(env_cfg, args_cli): + env = gym.make(args_cli.task, cfg=env_cfg) + + # reset environment at start + env.reset() + + # create action buffers (position + quaternion) + actions = torch.zeros(env.unwrapped.action_space.shape, device=env.unwrapped.device) + actions[:, 3] = 1.0 + # desired rotation after grasping + desired_orientation = torch.zeros((env.unwrapped.num_envs, 4), device=env.unwrapped.device) + desired_orientation[:, 0] = 1.0 + + # Top-down approach: identity quaternion (wxyz, w=1) aligns panda_hand with the Franka root, + # giving the canonical top-down grasp pose. The bar lies along world-X, so the gripper + # closes across its short side without any wrist twist. + object_grasp_orientation = torch.zeros((env.unwrapped.num_envs, 4), device=env.unwrapped.device) + object_grasp_orientation[:, 0] = 1.0 + # Grasp 1 cm below the deformable's centre of mass, so the fingers close around its lower half. + object_local_grasp_position = torch.tensor([0.0, 0.0, -0.01], device=env.unwrapped.device) + + # create state machine + pick_sm = PickAndLiftSm(env_cfg.sim.dt * env_cfg.decimation, env.unwrapped.num_envs, env.unwrapped.device) + + for _ in range(args_cli.num_steps): + # run everything in inference mode + with torch.inference_mode(): + # step environment + dones = env.step(actions)[-2] + + # observations + # -- end-effector frame + ee_frame_sensor = env.unwrapped.scene["ee_frame"] + tcp_rest_position = ( + ee_frame_sensor.data.target_pos_w.torch[..., 0, :].clone() - env.unwrapped.scene.env_origins + ) + tcp_rest_orientation = ee_frame_sensor.data.target_quat_w.torch[..., 0, :].clone() + # -- object frame + object_data: DeformableObjectData = env.unwrapped.scene["deformable"].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] + + # advance state machine + actions = pick_sm.compute( + torch.cat([tcp_rest_position, tcp_rest_orientation], dim=-1), + torch.cat([object_position, object_grasp_orientation], dim=-1), + torch.cat([desired_position, desired_orientation], dim=-1), + ) + + # reset state machine + if dones.any(): + pick_sm.reset_idx(dones.nonzero(as_tuple=False).squeeze(-1)) + + # close the environment + env.close() if __name__ == "__main__": - # run the main function main() - # close sim app - simulation_app.close() diff --git a/source/isaaclab/isaaclab/sim/spawners/meshes/meshes.py b/source/isaaclab/isaaclab/sim/spawners/meshes/meshes.py index 22e851b6aed3..faaac56f6fcf 100644 --- a/source/isaaclab/isaaclab/sim/spawners/meshes/meshes.py +++ b/source/isaaclab/isaaclab/sim/spawners/meshes/meshes.py @@ -420,6 +420,12 @@ def _spawn_mesh_geom_from_mesh( if not is_rigid_material: raise ValueError("Rigid properties require a rigid physics material.") + # refine the surface for deformable primitives + if cfg.deformable_props is not None: + max_edge = 0.3 * float(np.linalg.norm(mesh.bounding_box.extents)) + vertices, faces = trimesh.remesh.subdivide_to_size(mesh.vertices, mesh.faces, max_edge=max_edge) + mesh = trimesh.Trimesh(vertices=vertices, faces=faces, process=False) + # create all the paths we need for clarity geom_prim_path = prim_path + "/geometry" mesh_prim_path = geom_prim_path + "/mesh" diff --git a/source/isaaclab_contrib/changelog.d/mym-lift-soft.minor.rst b/source/isaaclab_contrib/changelog.d/mym-lift-soft.minor.rst new file mode 100644 index 000000000000..0909867a3b55 --- /dev/null +++ b/source/isaaclab_contrib/changelog.d/mym-lift-soft.minor.rst @@ -0,0 +1,6 @@ +Added +^^^^^ + +* Added :attr:`~isaaclab_contrib.deformable.VBDSolverCfg.rigid_body_particle_contact_buffer_size` + to size the per-body particle contact list. Contacts past the buffer are dropped from the body's + reaction list, which pushes the particles without recoiling the body and injects energy. 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 70d04ed2f7fd..af4607860def 100644 --- a/source/isaaclab_contrib/isaaclab_contrib/deformable/newton_manager_cfg.py +++ b/source/isaaclab_contrib/isaaclab_contrib/deformable/newton_manager_cfg.py @@ -118,6 +118,16 @@ class VBDSolverCfg(NewtonModelSolverCfg): rigid_contact_k_start: float = 1.0e2 """Initial stiffness seed for all rigid body contacts [N/m].""" + rigid_body_particle_contact_buffer_size: int = 256 + """Per-body capacity of the body-particle soft-contact list. + + Contacts past this count are dropped from the body's reaction list: the particles are still + pushed but the body does not recoil, injecting energy. Newton prints ``Per-body particle + contact buffer overflowed N > size`` on overflow; raise this above the observed ``N``. Only + used when VBD integrates the rigid bodies itself, i.e. + :attr:`integrate_with_external_rigid_solver` is ``False``. + """ + @configclass class CoupledMJWarpVBDSolverCfg(NewtonModelSolverCfg): diff --git a/source/isaaclab_newton/changelog.d/mym-lift-soft.minor.rst b/source/isaaclab_newton/changelog.d/mym-lift-soft.minor.rst new file mode 100644 index 000000000000..f89e4eaf6d17 --- /dev/null +++ b/source/isaaclab_newton/changelog.d/mym-lift-soft.minor.rst @@ -0,0 +1,6 @@ +Added +^^^^^ + +* Added :attr:`~isaaclab_newton.physics.NewtonCollisionPipelineCfg.enable_rigid_soft_full_surface_contact` + to generate edge and triangle-interior soft contacts against rigid SDFs, so rigid features that + pass between soft vertices are caught. diff --git a/source/isaaclab_newton/isaaclab_newton/physics/newton_collision_cfg.py b/source/isaaclab_newton/isaaclab_newton/physics/newton_collision_cfg.py index 9e75d3153308..2f9a56c13fa0 100644 --- a/source/isaaclab_newton/isaaclab_newton/physics/newton_collision_cfg.py +++ b/source/isaaclab_newton/isaaclab_newton/physics/newton_collision_cfg.py @@ -150,6 +150,17 @@ class NewtonCollisionPipelineCfg: Defaults to ``0.01`` (same as Newton's default). """ + enable_rigid_soft_full_surface_contact: bool = False + """Whether to generate soft contacts over the full soft-mesh surface against rigid SDFs. + + When ``True``, Newton adds edge and triangle-interior soft contacts (in addition to the + per-vertex particle contacts) so rigid features that pass between soft vertices are caught. + Analytic shapes (boxes, capsules, spheres) are full-surface-capable without an SDF; any + participating mesh/convex collider must carry a volume SDF. + + Defaults to ``False`` (same as Newton's default). + """ + requires_grad: bool | None = None """Whether to enable gradient computation for collision. diff --git a/source/isaaclab_ovphysx/changelog.d/mym-lift-soft.skip b/source/isaaclab_ovphysx/changelog.d/mym-lift-soft.skip new file mode 100644 index 000000000000..e69de29bb2d1 diff --git a/source/isaaclab_ovphysx/test/tasks/test_lift_franka_soft_deformable.py b/source/isaaclab_ovphysx/test/tasks/test_lift_franka_soft_deformable.py deleted file mode 100644 index acde2b54cc9a..000000000000 --- a/source/isaaclab_ovphysx/test/tasks/test_lift_franka_soft_deformable.py +++ /dev/null @@ -1,145 +0,0 @@ -# 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 - -"""Task-level smoke tests for OVPhysX volume and surface deformables.""" - -from __future__ import annotations - -import gymnasium as gym -import ovphysx.types # noqa: F401 -import pytest -import torch -import warp as wp -from isaaclab_ovphysx import tensor_types as TT # noqa: E402 - -from isaaclab.sim import SimulationContext # noqa: E402 - -import isaaclab_tasks # noqa: F401, E402 -from isaaclab_tasks.core.lift.config.franka_soft.franka_cloth_env_cfg import FrankaClothEnvCfg # noqa: E402 -from isaaclab_tasks.core.lift.config.franka_soft.franka_soft_env_cfg import FrankaSoftEnvCfg # noqa: E402 -from isaaclab_tasks.utils.hydra import resolve_presets # noqa: E402 - -wp.init() - -_NUM_ENVS = 2 - - -def _configure_deformable_lift_ovphysx_smoke( - cfg_cls: type[FrankaSoftEnvCfg], -) -> FrankaSoftEnvCfg: - """Build a minimal multi-environment OvPhysX deformable-lift task.""" - cfg = resolve_presets(cfg_cls(), ("ovphysx",)) - cfg.sim.device = "cuda:0" - cfg.scene.num_envs = _NUM_ENVS - - # Keep these smokes focused on the stock task deformable and shared MDP data - # path while avoiding unrelated external props. - cfg.scene.table = None - cfg.scene.sky_light = None - cfg.scene.ground = None - cfg.commands.deformable_pose.debug_vis = False - cfg.ui_window_class_type = None - return cfg - - -@pytest.mark.skipif(not torch.cuda.is_available(), reason="OVPhysX deformables require CUDA") -@pytest.mark.isaacsim_ci -def test_lift_franka_soft_task_reads_and_steps_volume_deformable(): - """Reset and step finite soft-lift observations and deformable state.""" - env = None - try: - cfg = _configure_deformable_lift_ovphysx_smoke(FrankaSoftEnvCfg) - env = gym.make("Isaac-Lift-Soft-Franka", cfg=cfg) - env.unwrapped.sim._app_control_on_stop_handle = None - - obs, _ = env.reset() - policy_obs = obs["policy"] - assert policy_obs.shape[0] == _NUM_ENVS - assert torch.isfinite(policy_obs).all() - - deformable = env.unwrapped.scene["deformable"] - assert deformable.is_initialized - assert deformable.num_instances == _NUM_ENVS - assert deformable.max_sim_vertices_per_body > 0 - assert torch.isfinite(deformable.data.nodal_state_w.torch).all() - - ee_frame = env.unwrapped.scene["ee_frame"] - assert torch.isfinite(ee_frame.data.target_pos_w.torch).all() - - targets = deformable.data.nodal_kinematic_target - assert targets is not None - expected_targets = targets.torch.clone() - updated_targets = expected_targets[:1].clone() - updated_targets[..., 3] = 1.0 - updated_targets[:, :, :3] = deformable.data.nodal_pos_w.torch[:1] + torch.tensor( - [0.0, 0.0, 0.03], device=env.unwrapped.device - ) - updated_targets[:, :, 3] = 0.0 - deformable.write_nodal_kinematic_target_to_sim_index( - updated_targets, env_ids=torch.tensor([0], device=env.unwrapped.device) - ) - expected_targets[0] = updated_targets[0] - readback_targets = wp.to_torch(deformable.root_view.get_attribute(TT.DEFORMABLE_SIM_KINEMATIC_TARGET)) - torch.testing.assert_close(readback_targets, expected_targets, rtol=1e-5, atol=1e-5) - - arm_action = env.unwrapped.action_manager.get_term("arm_action") - ee_pos_curr, ee_quat_curr = arm_action._compute_frame_pose() - actions = torch.zeros(env.action_space.shape, device=env.unwrapped.device) - actions[:, :3] = ee_pos_curr - actions[:, 3:7] = ee_quat_curr - for _ in range(3): - obs, reward, terminated, time_out, _ = env.step(actions) - assert torch.isfinite(obs["policy"]).all() - assert torch.isfinite(reward).all() - assert torch.isfinite(deformable.data.nodal_state_w.torch).all() - assert torch.isfinite(deformable.data.root_pos_w.torch).all() - assert torch.isfinite(ee_frame.data.target_pos_w.torch).all() - assert not terminated.any() - assert not time_out.any() - finally: - try: - if env is not None: - env.close() - finally: - SimulationContext.clear_instance() - - -@pytest.mark.skipif(not torch.cuda.is_available(), reason="OVPhysX deformables require CUDA") -@pytest.mark.isaacsim_ci -def test_lift_franka_cloth_task_reads_and_steps_surface_deformable(): - """Reset and step finite cloth-lift observations and deformable state.""" - env = None - try: - cfg = _configure_deformable_lift_ovphysx_smoke(FrankaClothEnvCfg) - env = gym.make("Isaac-Lift-Cloth-Franka", cfg=cfg) - env.unwrapped.sim._app_control_on_stop_handle = None - - obs, _ = env.reset() - assert obs["policy"].shape[0] == _NUM_ENVS - assert torch.isfinite(obs["policy"]).all() - - deformable = env.unwrapped.scene["deformable"] - assert deformable.is_initialized - assert deformable.num_instances == _NUM_ENVS - assert deformable.max_sim_vertices_per_body > 0 - assert deformable.data.nodal_kinematic_target is None - assert torch.isfinite(deformable.data.nodal_state_w.torch).all() - assert torch.isfinite(deformable.data.root_pos_w.torch).all() - - actions = torch.zeros(env.action_space.shape, device=env.unwrapped.device) - for _ in range(3): - obs, reward, terminated, time_out, _ = env.step(actions) - assert torch.isfinite(obs["policy"]).all() - assert torch.isfinite(reward).all() - assert torch.isfinite(deformable.data.nodal_state_w.torch).all() - assert torch.isfinite(deformable.data.root_pos_w.torch).all() - assert not terminated.any() - assert not time_out.any() - finally: - try: - if env is not None: - env.close() - finally: - SimulationContext.clear_instance() diff --git a/source/isaaclab_tasks/changelog.d/mym-lift-soft.major.rst b/source/isaaclab_tasks/changelog.d/mym-lift-soft.major.rst new file mode 100644 index 000000000000..72bbdfeadc07 --- /dev/null +++ b/source/isaaclab_tasks/changelog.d/mym-lift-soft.major.rst @@ -0,0 +1,22 @@ +Added +^^^^^ + +* Added deformable-specific commands, observations, rewards, events, terminations, and curricula, + plus ``joint`` and ``ik`` action presets, to the Franka soft-beam and cloth lift environments. + +Changed +^^^^^^^ + +* **Breaking:** Changed the default action space to relative joint-position control. Use + ``presets=ik`` for task-space inverse-kinematics control; integrations using the cloth + environments' previous absolute joint targets must update their actions. +* **Breaking:** Changed the non-camera ``rsl_rl`` experiment name from ``franka_deformable`` to + ``franka_soft``. Update log and checkpoint paths that refer to ``logs/rsl_rl/franka_deformable``. +* Re-tuned the robot, scenes, contact handling, control rate, and ``rsl_rl`` configuration for stable + gravity-based training across the supported physics backends. + +Removed +^^^^^^^ + +* **Breaking:** Removed the unsupported ``ovphysx`` preset from the Franka soft-beam and cloth lift + environments. Use ``isaacsim_physx`` for the soft-beam task or ``newton_mjwarp_vbd_proxy`` for either task. diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/agents/rsl_rl_ppo_cfg.py index b884b0f16152..61c7f8d1f05f 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/agents/rsl_rl_ppo_cfg.py @@ -9,7 +9,6 @@ RslRlCNNModelCfg, RslRlMLPModelCfg, RslRlOnPolicyRunnerCfg, - RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg, ) @@ -32,24 +31,31 @@ @configclass class FrankaDeformablePPORunnerCfg(RslRlOnPolicyRunnerCfg): num_steps_per_env = 24 - max_iterations = 50000 + max_iterations = 3000 save_interval = 50 - experiment_name = "franka_deformable" - policy = RslRlPpoActorCriticCfg( - init_noise_std=1.0, - actor_obs_normalization=False, - critic_obs_normalization=False, - actor_hidden_dims=[256, 128, 64], - critic_hidden_dims=[256, 128, 64], + experiment_name = "franka_soft" + obs_groups = { + "actor": ["policy"], + "critic": ["policy"], + } + actor = RslRlMLPModelCfg( + hidden_dims=[256, 128, 64], activation="elu", + obs_normalization=True, + distribution_cfg=RslRlMLPModelCfg.GaussianDistributionCfg(init_std=1.0), + ) + critic = RslRlMLPModelCfg( + hidden_dims=[256, 128, 64], + activation="elu", + obs_normalization=True, ) - algorithm = ALGO_CFG + algorithm = ALGO_CFG.replace(learning_rate=1.0e-3) @configclass class FrankaDeformableCameraPPORunnerCfg(RslRlOnPolicyRunnerCfg): num_steps_per_env = 24 - max_iterations = 50000 + max_iterations = 5000 save_interval = 50 experiment_name = "franka_deformable_camera" obs_groups = { diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/franka_cloth_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/franka_cloth_env_cfg.py index a0779f790a3f..cd1f81803dc7 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/franka_cloth_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/franka_cloth_env_cfg.py @@ -7,15 +7,12 @@ from __future__ import annotations -from isaaclab_newton.physics import MJWarpSolverCfg, NewtonCfg, NewtonShapeCfg +from isaaclab_newton.physics import MJWarpSolverCfg, NewtonCfg, NewtonCollisionPipelineCfg from isaaclab_newton.sim.schemas import NewtonDeformableBodyPropertiesCfg from isaaclab_newton.sim.spawners.materials import NewtonSurfaceDeformableBodyMaterialCfg -from isaaclab_ovphysx.physics import OvPhysxCfg -from isaaclab_physx.sim.schemas import PhysxDeformableBodyPropertiesCfg -from isaaclab_physx.sim.spawners.materials import PhysxSurfaceDeformableBodyMaterialCfg import isaaclab.sim as sim_utils -from isaaclab.assets import AssetBaseCfg +from isaaclab.assets import RigidObjectCfg from isaaclab.assets.deformable_object import DeformableObjectCfg from isaaclab.managers import EventTermCfg as EventTerm from isaaclab.managers import SceneEntityCfg @@ -25,9 +22,9 @@ from isaaclab_contrib.coupling import CouplerEntryCfg, CouplerProxyCfg, CouplerProxyMappingCfg from isaaclab_contrib.deformable.newton_manager_cfg import NewtonModelCfg, VBDSolverCfg -from isaaclab_tasks.core.lift import mdp from isaaclab_tasks.utils import PresetCfg +from ... import mdp from .franka_soft_env_cfg import ( FRANKA_CAMERA_CFG, FrankaCameraObservationsCfg, @@ -42,12 +39,6 @@ # Scene definition ## -ROBOT_SHAPE_MATERIAL_MU = 100.0 -"""Franka collision-shape friction coefficient [dimensionless] used for Newton cloth contact.""" - -ROBOT_SHAPE_MATERIAL_BODY_NAMES = ".*" -"""Franka body-name regex receiving :data:`ROBOT_SHAPE_MATERIAL_MU`.""" - @configclass class PhysicsCfg(PresetCfg): @@ -58,18 +49,16 @@ class PhysicsCfg(PresetCfg): CouplerEntryCfg( name="rigid", solver_cfg=MJWarpSolverCfg( - njmax=40, - nconmax=20, cone="elliptic", ls_iterations=20, integrator="implicitfast", - ccd_iterations=100, ), - bodies=[r"/World/envs/env_.*/Robot"], + # the cube is a rigid body, so it must be owned by the rigid entry + bodies=[r"/World/envs/env_.*/Robot", r"/World/envs/env_.*/Cube"], ), CouplerEntryCfg( name="soft", - solver_cfg=VBDSolverCfg(iterations=10), + solver_cfg=VBDSolverCfg(iterations=10, rigid_body_particle_contact_buffer_size=1024), all_particles=True, include_static_shapes=True, ), @@ -79,26 +68,22 @@ class PhysicsCfg(PresetCfg): source="rigid", destination="soft", bodies=[ - r"/World/envs/env_.*/Robot/panda_hand", - r"/World/envs/env_.*/Robot/panda_(left|right)finger", + r"/World/envs/env_.*/Robot/Geometry/.*panda_hand", + r"/World/envs/env_.*/Robot/Geometry/.*panda_(left|right)finger", + r"/World/envs/env_.*/Cube", ], collide_interval=1, + collision_pipeline=NewtonCollisionPipelineCfg( + enable_rigid_soft_full_surface_contact=True, + ), ) ], iterations=1, - model_cfg=NewtonModelCfg( - soft_contact_ke=1e3, - soft_contact_kd=1e-5, - soft_contact_mu=0.5, - ), + model_cfg=NewtonModelCfg(soft_contact_mu=10.0), ), - default_shape_cfg=NewtonShapeCfg(ke=1e3, kd=1e-5, mu=1e-4), - num_substeps=10, - use_cuda_graph=True, + num_substeps=2, ) - ovphysx: OvPhysxCfg = OvPhysxCfg() - default = newton_mjwarp_vbd_proxy @@ -111,43 +96,21 @@ class DeformableCfg(PresetCfg): init_state=DeformableObjectCfg.InitialStateCfg(pos=(0.4, 0.0, 0.2)), spawn=sim_utils.MeshRectangleCfg( size=(0.2, 0.2), - resolution=(30, 30), + resolution=(10, 10), deformable_props=NewtonDeformableBodyPropertiesCfg(), visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.95, 0.85, 0.1)), physics_material=NewtonSurfaceDeformableBodyMaterialCfg( - density=50.0, - particle_radius=0.005, + density=10.0, + particle_radius=0.0025, tri_ke=5e2, tri_ka=5e2, tri_kd=1e-3, - edge_ke=2.0, + edge_ke=0.5, edge_kd=1e-3, ), ), ) - ovphysx: DeformableObjectCfg = DeformableObjectCfg( - prim_path="{ENV_REGEX_NS}/Deformable", - init_state=DeformableObjectCfg.InitialStateCfg(pos=(0.4, 0.0, 0.2)), - spawn=sim_utils.MeshRectangleCfg( - size=(0.2, 0.2), - resolution=(30, 30), - deformable_props=PhysxDeformableBodyPropertiesCfg(), - visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.95, 0.85, 0.1)), - physics_material=PhysxSurfaceDeformableBodyMaterialCfg( - density=50.0, - youngs_modulus=2000.0, - poissons_ratio=0.25, - surface_thickness=0.005, - surface_stretch_stiffness=0.8, - surface_shear_stiffness=0.7, - surface_bend_stiffness=0.6, - elasticity_damping=0.03, - bend_damping=0.04, - ), - ), - ) - default = newton_mjwarp_vbd_proxy @@ -157,36 +120,29 @@ class FrankaClothSceneCfg(_FrankaSoftSceneCfg): deformable: DeformableCfg = DeformableCfg() - # Static collidable cube the cloth drops onto (sits on the table top at z = 0). - cube: AssetBaseCfg = AssetBaseCfg( + # Collidable cube the cloth drapes onto (sits on the table top at z = 0). Kinematic so the + # reset event can move it under the randomized cloth without it being simulated. + cube: RigidObjectCfg = RigidObjectCfg( prim_path="{ENV_REGEX_NS}/Cube", - init_state=AssetBaseCfg.InitialStateCfg(pos=(0.45, 0.0, 0.04)), + init_state=RigidObjectCfg.InitialStateCfg(pos=(0.45, 0.0, 0.04)), spawn=sim_utils.CuboidCfg( - size=(0.03, 0.01, 0.08), + size=(0.01, 0.03, 0.08), + rigid_props=sim_utils.RigidBodyPropertiesCfg(kinematic_enabled=True, disable_gravity=True), + mass_props=sim_utils.MassPropertiesCfg(mass=1.0), collision_props=sim_utils.CollisionPropertiesCfg(), visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.2, 0.2, 0.25)), ), ) - def __post_init__(self) -> None: - super().__post_init__() - - # increase franka gripper stiffness - self.robot.actuators["panda_hand"].effort_limit_sim = 500.0 - self.robot.actuators["panda_hand"].stiffness = 2000.0 - self.robot.actuators["panda_hand"].damping = 100.0 - @configclass class FrankaClothScenePresetCfg(PresetCfg): """Preset config for the Franka surface deformable scene.""" newton_mjwarp_vbd_proxy: FrankaClothSceneCfg = FrankaClothSceneCfg( - num_envs=128, env_spacing=2.5, replicate_physics=True + num_envs=2048, env_spacing=2.5, replicate_physics=True ) - ovphysx: FrankaClothSceneCfg = FrankaClothSceneCfg(num_envs=128, env_spacing=2.5, replicate_physics=True) - default = newton_mjwarp_vbd_proxy @@ -198,54 +154,22 @@ class FrankaClothCameraSceneCfg(FrankaClothSceneCfg): @configclass -class ActionsCfg: - """7-dim arm joint position + 1-dim binary gripper.""" - - arm_action = mdp.JointPositionActionCfg( - asset_name="robot", joint_names=["panda_joint.*"], scale=0.1, use_default_offset=True - ) - gripper_action = mdp.BinaryJointPositionActionCfg( - asset_name="robot", - joint_names=["panda_finger.*"], - open_command_expr={"panda_finger_.*": 0.05}, - close_command_expr={"panda_finger_.*": 0.0}, - ) - - -@configclass -class EventCfg(FrankaSoftEventCfg): +class FrankaClothEventCfg(FrankaSoftEventCfg): """Reset and startup events for the Franka cloth environment.""" - robot_physics_material = EventTerm( - func=mdp.randomize_rigid_body_material, - mode="startup", + # Replaces the base term so the cube follows the randomized cloth position. + reset_deformable = EventTerm( + func=mdp.reset_deformable_over_support, + mode="reset", params={ - "asset_cfg": SceneEntityCfg("robot", body_names=ROBOT_SHAPE_MATERIAL_BODY_NAMES), - "static_friction_range": (ROBOT_SHAPE_MATERIAL_MU, ROBOT_SHAPE_MATERIAL_MU), - "dynamic_friction_range": (ROBOT_SHAPE_MATERIAL_MU, ROBOT_SHAPE_MATERIAL_MU), - "restitution_range": (0.0, 0.0), - "num_buckets": 1, + "position_range": {"x": (-0.1, 0.1), "y": (-0.25, 0.25), "z": (0.0, 0.0)}, + "support_offset_range": {"x": (-0.02, 0.02), "y": (-0.02, 0.02)}, + "asset_cfg": SceneEntityCfg("deformable"), + "support_cfg": SceneEntityCfg("cube"), }, ) -def _make_ovphysx_event_cfg() -> EventCfg: - """Create cloth events that select all robot shapes on OvPhysX.""" - cfg = EventCfg() - cfg.robot_physics_material.params["asset_cfg"] = SceneEntityCfg("robot") - return cfg - - -@configclass -class EventPresetCfg(PresetCfg): - """Preset config for Franka cloth startup and reset events.""" - - newton_mjwarp_vbd_proxy: EventCfg = EventCfg() - ovphysx: EventCfg = _make_ovphysx_event_cfg() - - default = newton_mjwarp_vbd_proxy - - ## # Environment configuration ## @@ -255,22 +179,12 @@ class EventPresetCfg(PresetCfg): class FrankaClothEnvCfg(FrankaSoftEnvCfg): """Manager-based RL environment: Franka Panda lifting a surface deformable.""" - # Scene settings scene: FrankaClothScenePresetCfg = FrankaClothScenePresetCfg() - # Basic settings - actions: ActionsCfg = ActionsCfg() - # MDP settings - events: EventPresetCfg = EventPresetCfg() + events: FrankaClothEventCfg = FrankaClothEventCfg() def __post_init__(self) -> None: - # general settings - self.decimation = 1 - self.episode_length_s = 5.0 - - # simulation settings - self.sim.dt = 1 / 60.0 - self.sim.render_interval = self.decimation - + super().__post_init__() + # override the soft-beam physics with the cloth presets self.sim.physics = PhysicsCfg() diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/franka_soft_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/franka_soft_env_cfg.py index 610ca1bae507..0e6524f61b37 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/franka_soft_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/franka_soft_env_cfg.py @@ -3,25 +3,29 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""Configuration for the Franka deformable lifting environment.""" +"""Configuration for the Franka deformable (soft beam) lifting environment.""" from __future__ import annotations -from isaaclab_newton.physics import MJWarpSolverCfg, NewtonCfg, NewtonShapeCfg +from isaaclab_newton.physics import ( + MJWarpSolverCfg, + NewtonCfg, + NewtonCollisionPipelineCfg, +) from isaaclab_newton.sim.schemas import NewtonDeformableBodyPropertiesCfg from isaaclab_newton.sim.spawners.materials import NewtonDeformableBodyMaterialCfg -from isaaclab_ovphysx.physics import OvPhysxCfg from isaaclab_physx.physics import PhysxCfg -from isaaclab_physx.sim.schemas import PhysxDeformableBodyPropertiesCfg +from isaaclab_physx.sim.schemas import PhysxCollisionCfg, PhysxDeformableBodyPropertiesCfg from isaaclab_physx.sim.spawners.materials import PhysxDeformableBodyMaterialCfg import isaaclab.sim as sim_utils +from isaaclab.actuators import ImplicitActuatorCfg from isaaclab.assets import ArticulationCfg, AssetBaseCfg from isaaclab.assets.deformable_object import DeformableObjectCfg -from isaaclab.controllers.differential_ik_cfg import DifferentialIKControllerCfg +from isaaclab.controllers import DifferentialIKControllerCfg from isaaclab.envs import ManagerBasedRLEnvCfg from isaaclab.envs import mdp as env_mdp -from isaaclab.envs.mdp.actions.actions_cfg import DifferentialInverseKinematicsActionCfg +from isaaclab.managers import CurriculumTermCfg as CurrTerm from isaaclab.managers import EventTermCfg as EventTerm from isaaclab.managers import ObservationGroupCfg as ObsGroup from isaaclab.managers import ObservationTermCfg as ObsTerm @@ -33,9 +37,10 @@ from isaaclab.scene import InteractiveSceneCfg from isaaclab.sensors import CameraCfg, FrameTransformerCfg from isaaclab.sensors.frame_transformer.frame_transformer_cfg import OffsetCfg -from isaaclab.sim.spawners.from_files.from_files_cfg import GroundPlaneCfg, UsdFileCfg +from isaaclab.sim.spawners.from_files.from_files_cfg import GroundPlaneCfg from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR from isaaclab.utils.configclass import configclass +from isaaclab.visualizers import VisualizerCfg from isaaclab_contrib.coupling import ( CouplerEntryCfg, @@ -47,15 +52,16 @@ VBDSolverCfg, ) -from isaaclab_tasks.core.lift import mdp from isaaclab_tasks.utils import PresetCfg from isaaclab_tasks.utils.presets import MultiBackendRendererCfg +from ... import mdp + ## # Pre-defined configs ## -from isaaclab_assets.robots.franka import FRANKA_PANDA_CFG # isort:skip +from isaaclab_assets.robots.franka import FRANKA_PANDA_MENAGERIE_CFG # isort:skip ## @@ -64,8 +70,16 @@ # Shared volume material parameters. The Newton config below uses the equivalent Lame parameters. -YOUNGS_MODULUS = 8e4 -POISSONS_RATIO = 0.25 +YOUNGS_MODULUS = 2e5 +POISSONS_RATIO = 0.3 + +# Table collider whose top surface sits at z = 0. Spawned invisible: the command term's success +# visualizer draws it instead, tinted by whether the goal is reached. +TABLE_SPAWN_CFG = sim_utils.CuboidCfg( + size=(1.3, 0.9, 1.05), + collision_props=sim_utils.CollisionPropertiesCfg(), + visible=False, +) FRANKA_CAMERA_CFG = CameraCfg( @@ -91,14 +105,14 @@ class DeformableCfg(PresetCfg): prim_path="{ENV_REGEX_NS}/Deformable", init_state=DeformableObjectCfg.InitialStateCfg(pos=(0.5, 0.0, 0.05)), spawn=sim_utils.MeshCuboidCfg( - size=(0.3, 0.05, 0.05), + size=(0.3, 0.04, 0.04), deformable_props=NewtonDeformableBodyPropertiesCfg(), - visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.95, 0.85, 0.1)), + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.45, 0.45, 0.85)), physics_material=NewtonDeformableBodyMaterialCfg( - density=300.0, + density=1000.0, k_mu=YOUNGS_MODULUS / (2.0 * (1.0 + POISSONS_RATIO)), k_lambda=(YOUNGS_MODULUS * POISSONS_RATIO / ((1.0 + POISSONS_RATIO) * (1.0 - 2.0 * POISSONS_RATIO))), - particle_radius=0.01, + particle_radius=0.0025, ), ), ) @@ -107,22 +121,21 @@ class DeformableCfg(PresetCfg): prim_path="{ENV_REGEX_NS}/Deformable", init_state=DeformableObjectCfg.InitialStateCfg(pos=(0.5, 0.0, 0.05)), spawn=sim_utils.MeshCuboidCfg( - size=(0.3, 0.05, 0.05), + size=(0.3, 0.04, 0.04), deformable_props=PhysxDeformableBodyPropertiesCfg(), - visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.95, 0.85, 0.1)), + collision_props=[PhysxCollisionCfg(rest_offset=0.0025, contact_offset=0.01)], + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.45, 0.45, 0.85)), physics_material=PhysxDeformableBodyMaterialCfg( - density=300.0, + density=1000.0, youngs_modulus=YOUNGS_MODULUS, poissons_ratio=POISSONS_RATIO, static_friction=10.0, - dynamic_friction=5.0, + dynamic_friction=10.0, ), ), ) isaacsim_physx = physx - ovphysx: DeformableObjectCfg = physx - default = newton_mjwarp_vbd_proxy @@ -142,7 +155,7 @@ class PhysicsCfg(PresetCfg): ), CouplerEntryCfg( name="soft", - solver_cfg=VBDSolverCfg(iterations=10), + solver_cfg=VBDSolverCfg(iterations=10, rigid_body_particle_contact_buffer_size=256), all_particles=True, include_static_shapes=True, ), @@ -152,26 +165,27 @@ class PhysicsCfg(PresetCfg): source="rigid", destination="soft", bodies=[ - r"/World/envs/env_.*/Robot/panda_hand", - r"/World/envs/env_.*/Robot/panda_(left|right)finger", + r"/World/envs/env_.*/Robot/Geometry/.*panda_hand", + r"/World/envs/env_.*/Robot/Geometry/.*panda_(left|right)finger", ], - collide_interval=5, + collide_interval=1, + collision_pipeline=NewtonCollisionPipelineCfg( + enable_rigid_soft_full_surface_contact=True, + ), ) ], iterations=1, - model_cfg=NewtonModelCfg( - soft_contact_ke=1e4, - soft_contact_kd=1e-5, - soft_contact_mu=5.0, - ), + model_cfg=NewtonModelCfg(soft_contact_ke=8.0e3, soft_contact_mu=10.0), ), - default_shape_cfg=NewtonShapeCfg(ke=4e4, kd=1e-5, mu=5.0), - num_substeps=10, + num_substeps=2, + ) + + isaacsim_physx: PhysxCfg = PhysxCfg( + friction_offset_threshold=0.005, + friction_correlation_distance=0.01, ) - isaacsim_physx: PhysxCfg = PhysxCfg() - ovphysx: OvPhysxCfg = OvPhysxCfg() - physx: PhysxAutoCfg = PhysxAutoCfg(isaacsim_physx=isaacsim_physx, ovphysx=ovphysx) + physx: PhysxAutoCfg = PhysxAutoCfg(isaacsim_physx=isaacsim_physx) default = newton_mjwarp_vbd_proxy @@ -185,15 +199,18 @@ class PhysicsCfg(PresetCfg): class _FrankaSoftSceneCfg(InteractiveSceneCfg): """Scene for the Franka deformable environment.""" - robot: ArticulationCfg = FRANKA_PANDA_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + robot: ArticulationCfg = FRANKA_PANDA_MENAGERIE_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") # end-effector frame for reward shaping ee_frame: FrameTransformerCfg = FrameTransformerCfg( - prim_path="/World/envs/env_.*/Robot/panda_link0", + prim_path="/World/envs/env_.*/Robot/Geometry/panda_link0", debug_vis=False, target_frames=[ FrameTransformerCfg.FrameCfg( - prim_path="/World/envs/env_.*/Robot/panda_hand", + prim_path=( + "/World/envs/env_.*/Robot/Geometry/panda_link0/panda_link1/panda_link2/panda_link3/" + "panda_link4/panda_link5/panda_link6/panda_link7/panda_hand" + ), name="end_effector", offset=OffsetCfg(pos=[0.0, 0.0, 0.1034]), ), @@ -202,13 +219,13 @@ class _FrankaSoftSceneCfg(InteractiveSceneCfg): deformable: 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 - # sits directly on it when placed at z = 0.05. + # static table collider with its top surface at z = 0. Kept invisible: the success + # visualizer renders the visible table, colored by whether the goal is reached + # (see CommandsCfg). table: AssetBaseCfg = AssetBaseCfg( prim_path="{ENV_REGEX_NS}/Table", - init_state=AssetBaseCfg.InitialStateCfg(pos=[0.5, 0.0, 0.0], rot=[0.0, 0.0, 0.707, 0.707]), - spawn=UsdFileCfg(usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Mounts/SeattleLabTable/table_instanceable.usd"), + init_state=AssetBaseCfg.InitialStateCfg(pos=[0.5, 0.0, -0.525]), + spawn=TABLE_SPAWN_CFG, ) # ground plane @@ -228,14 +245,50 @@ class _FrankaSoftSceneCfg(InteractiveSceneCfg): ) def __post_init__(self) -> None: - # disable gravity on the arm so the low-PD actuators do not need to fight gravity sag, - # which is the dominant source of steady-state IK tracking error. - self.robot.spawn.rigid_props.disable_gravity = True - - # increase franka gripper stiffness - self.robot.actuators["panda_hand"].effort_limit_sim = 500.0 - self.robot.actuators["panda_hand"].stiffness = 1000.0 - self.robot.actuators["panda_hand"].damping = 100.0 + self.robot.actuators = { + # inspired by libfranka's joint_impedance_control.cpp + "panda_arm": ImplicitActuatorCfg( + joint_names_expr=["panda_joint[1-7]"], + effort_limit_sim={"panda_joint[1-4]": 87.0, "panda_joint[5-7]": 12.0}, + velocity_limit_sim={"panda_joint[1-4]": 2.175, "panda_joint[5-7]": 2.61}, + # velocity_limit_sim={"panda_joint[1-4]": 20.0, "panda_joint[5-7]": 25.0}, + stiffness={ + "panda_joint[1-4]": 600.0, + "panda_joint5": 250.0, + "panda_joint6": 150.0, + "panda_joint7": 50.0, + }, + damping={ + "panda_joint[1-4]": 50.0, + "panda_joint5": 30.0, + "panda_joint6": 25.0, + "panda_joint7": 15.0, + }, + armature={ + "panda_joint[1-2]": 0.6057, + "panda_joint[3-4]": 0.4625, + "panda_joint[5-7]": 0.2055, + }, + ), + "panda_hand": ImplicitActuatorCfg( + joint_names_expr=["panda_finger_joint1"], + effort_limit_sim=70.0, + velocity_limit=0.2, + velocity_limit_sim=2.0, + stiffness=350.0, + damping=175.0, + armature=0.1, + ), + "panda_finger2_passive": ImplicitActuatorCfg( + joint_names_expr=["panda_finger_joint2"], + effort_limit_sim=1.0, + velocity_limit=0.2, + velocity_limit_sim=2.0, + stiffness=0.0, + damping=0.0, + armature=0.1, + ), + } @configclass @@ -254,12 +307,12 @@ class _FrankaSoftCameraSceneCfg(_FrankaSoftSceneCfg): class CommandsCfg: """Commands for the deformable goal pose (xyz + identity quat in robot root frame).""" - deformable_pose = mdp.UniformPoseCommandCfg( + deformable_pose = mdp.DeformableUniformPoseCommandCfg( asset_name="robot", - body_name="panda_hand", + object_name="deformable", resampling_time_range=(5.0, 5.0), debug_vis=True, - ranges=mdp.UniformPoseCommandCfg.Ranges( + ranges=mdp.DeformableUniformPoseCommandCfg.Ranges( pos_x=(0.4, 0.6), pos_y=(-0.25, 0.25), pos_z=(0.25, 0.5), @@ -267,16 +320,16 @@ class CommandsCfg: pitch=(0.0, 0.0), yaw=(0.0, 0.0), ), - # Render the goal as a transparent colored sphere (a point) instead of a coordinate frame. - goal_pose_visualizer_cfg=VisualizationMarkersCfg( - prim_path="/Visuals/Command/goal_pose", + # the invisible table is drawn by these markers, tinted green once the goal is reached + success_vis_asset_name="table", + success_visualizer_cfg=VisualizationMarkersCfg( + prim_path="/Visuals/SuccessMarkers", markers={ - "sphere": sim_utils.SphereCfg( - radius=0.03, - visual_material=sim_utils.PreviewSurfaceCfg( - diffuse_color=(0.1, 0.9, 0.2), - opacity=0.4, - ), + "failure": TABLE_SPAWN_CFG.replace( + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.8, 0.5, 0.5)), visible=True + ), + "success": TABLE_SPAWN_CFG.replace( + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.5, 0.8, 0.5)), visible=True ), }, ), @@ -284,10 +337,21 @@ class CommandsCfg: @configclass -class ActionsCfg: +class _JointActionsCfg: + """7-dim relative joint-position arm targets + 1-dim limit-rescaled gripper.""" + + arm_action = mdp.RelativeJointPositionActionCfg(asset_name="robot", joint_names=["panda_joint.*"], scale=0.03) + + gripper_action = mdp.JointPositionToLimitsActionCfg( + asset_name="robot", joint_names=["panda_finger_joint1"], rescale_to_limits=True + ) + + +@configclass +class _IkActionsCfg: """7-dim absolute end-effector pose (xyz + quaternion) via differential IK + 1-dim binary gripper.""" - arm_action = DifferentialInverseKinematicsActionCfg( + arm_action = mdp.DifferentialInverseKinematicsActionCfg( asset_name="robot", joint_names=["panda_joint.*"], body_name="panda_hand", @@ -297,19 +361,31 @@ class ActionsCfg: ik_method="dls", ik_params={"lambda_val": 0.6}, ), - body_offset=DifferentialInverseKinematicsActionCfg.OffsetCfg(pos=[0.0, 0.0, 0.107]), + body_offset=mdp.DifferentialInverseKinematicsActionCfg.OffsetCfg(pos=[0.0, 0.0, 0.107]), ) + gripper_action = mdp.BinaryJointPositionActionCfg( asset_name="robot", - joint_names=["panda_finger.*"], - open_command_expr={"panda_finger_.*": 0.05}, - close_command_expr={"panda_finger_.*": 0.0}, + joint_names=["panda_finger_joint1"], + open_command_expr={"panda_finger_joint1": 0.04}, + close_command_expr={"panda_finger_joint1": 0.015}, ) +@configclass +class ActionsCfg(PresetCfg): + """Action-space presets: joint-space for RL, task-space IK for scripted end-effector control.""" + + joint: _JointActionsCfg = _JointActionsCfg() + + ik: _IkActionsCfg = _IkActionsCfg() + + default = joint + + @configclass class ObservationsCfg: - """Policy observations: joint state, deformable COM in robot frame, target, last action.""" + """Policy observations: relative joint state, sampled deformable points, target command, and last action.""" @configclass class PolicyCfg(ObsGroup): @@ -394,84 +470,111 @@ class EventCfg: 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)}, + "position_range": {"x": (-0.15, 0.1), "y": (-0.2, 0.2), "z": (0.0, 0.0)}, "velocity_range": {}, "asset_cfg": SceneEntityCfg("deformable"), }, ) + variable_gravity = EventTerm( + func=mdp.randomize_physics_scene_gravity, + mode="reset", + params={ + "gravity_distribution_params": ([0.0, 0.0, -9.81], [0.0, 0.0, -9.81]), + "operation": "abs", + }, + ) + @configclass class RewardsCfg: """Lift-to-target reward for a deformable object.""" reaching_deformable = RewTerm( - func=mdp.deformable_ee_distance, + func=mdp.deformable_com_ee_distance, params={"std": 0.1, "asset_cfg": SceneEntityCfg("deformable")}, weight=5.0, ) + lifting_deformable = RewTerm( - func=mdp.deformable_lifted, - params={"minimal_height": 0.04, "asset_cfg": SceneEntityCfg("deformable")}, + func=mdp.deformable_lifting, + params={"std": 0.1, "minimal_height": 0.02, "asset_cfg": SceneEntityCfg("deformable")}, weight=5.0, ) + deformable_goal_tracking = RewTerm( - func=mdp.deformable_com_goal_distance, + func=mdp.DeformableComGoalDistance, params={ "std": 0.3, - "minimal_height": 0.075, + "minimal_height": 0.0, "command_name": "deformable_pose", + "success_threshold": 0.05, "asset_cfg": SceneEntityCfg("deformable"), }, - weight=16.0, + weight=2.0, ) - deformable_goal_tracking_fine_grained = RewTerm( - func=mdp.deformable_com_goal_distance, + + success_bonus = RewTerm( + func=mdp.deformable_com_goal_reached, params={ - "std": 0.05, - "minimal_height": 0.075, + "minimal_height": 0.0, "command_name": "deformable_pose", + "success_threshold": 0.05, "asset_cfg": SceneEntityCfg("deformable"), }, - weight=5.0, + weight=20.0, ) - action_rate = RewTerm(func=mdp.action_rate_l2, weight=-1e-2) - gripper_close = RewTerm( - func=mdp.gripper_close_action, - params={"action_name": "gripper_action"}, - weight=-1.0, + action_rate = RewTerm(func=mdp.action_rate_l2, weight=-1e-3) + + +@configclass +class CurriculumCfg: + """Ramp the action-rate penalty once the policy has learned to lift (matches rigid recipe).""" + + action_rate = CurrTerm( + func=mdp.modify_reward_weight, params={"term_name": "action_rate", "weight": -1e-1, "num_steps": 15000} + ) + + # Since we use 24 steps per env, 10000 steps correspond to 10000/24 = 416.67 learning iterations + gravity = CurrTerm( + func=mdp.gravity_range_linear, + params={ + "event_name": "variable_gravity", + "start_gravity_z": -0.0001, + "end_gravity_z": -9.81, + "start_step": 0, + "end_step": 10000, + }, ) - joint_vel = RewTerm(func=mdp.joint_vel_l2, weight=-1e-2) - joint_torque = RewTerm(func=mdp.joint_torques_l2, weight=-1e-4) - joint_acc = RewTerm(func=mdp.joint_acc_l2, weight=-1e-4) @configclass class TerminationsCfg: - """Time out + table bounds/drop termination.""" + """Time out + workspace bounds termination.""" time_out = DoneTerm(func=mdp.time_out, time_out=True) - deformable_outside_table = DoneTerm( - func=mdp.deformable_outside_table_bounds, + deformable_out_of_bounds = DoneTerm( + func=mdp.deformable_outside_bounds, params={ "x_bounds": (0.0, 1.0), "y_bounds": (-0.5, 0.5), + "z_bounds": (-0.02, 1.0), "asset_cfg": SceneEntityCfg("deformable"), }, ) - deformable_dropped = DoneTerm( - func=mdp.deformable_com_below_minimum, - params={"minimum_height": -0.1, "asset_cfg": SceneEntityCfg("deformable")}, - ) - ee_below_table = DoneTerm( func=mdp.ee_below_minimum, params={"minimum_height": 0.0, "ee_frame_cfg": SceneEntityCfg("ee_frame")}, ) + joint_vel_out_of_limit = DoneTerm( + func=mdp.joint_vel_out_of_sim_limit, + params={"asset_cfg": SceneEntityCfg("robot")}, + ) + ## # Environment configuration @@ -481,15 +584,13 @@ class TerminationsCfg: @configclass class FrankaSoftSceneCfg(PresetCfg): newton_mjwarp_vbd_proxy: _FrankaSoftSceneCfg = _FrankaSoftSceneCfg( - num_envs=128, env_spacing=2.5, replicate_physics=True + num_envs=2048, env_spacing=2.0, replicate_physics=True ) # PhysX does not support replicating physics for deformable objects - physx: _FrankaSoftSceneCfg = _FrankaSoftSceneCfg(num_envs=128, env_spacing=2.5, replicate_physics=False) + physx: _FrankaSoftSceneCfg = _FrankaSoftSceneCfg(num_envs=2048, env_spacing=2.0, replicate_physics=False) isaacsim_physx = physx - ovphysx: _FrankaSoftSceneCfg = _FrankaSoftSceneCfg(num_envs=128, env_spacing=2.5, replicate_physics=True) - default = newton_mjwarp_vbd_proxy @@ -498,16 +599,16 @@ class FrankaSoftCameraSceneCfg(PresetCfg): """Scene presets for visual Franka soft lifting.""" newton_mjwarp_vbd_proxy: _FrankaSoftCameraSceneCfg = _FrankaSoftCameraSceneCfg( - num_envs=128, env_spacing=2.5, replicate_physics=True + num_envs=128, env_spacing=2.0, replicate_physics=True ) - physx: _FrankaSoftCameraSceneCfg = _FrankaSoftCameraSceneCfg(num_envs=128, env_spacing=2.5, replicate_physics=False) + physx: _FrankaSoftCameraSceneCfg = _FrankaSoftCameraSceneCfg(num_envs=128, env_spacing=2.0, replicate_physics=False) isaacsim_physx = physx default = newton_mjwarp_vbd_proxy @configclass class FrankaSoftEnvCfg(ManagerBasedRLEnvCfg): - """Manager-based RL environment: Franka Panda lifting a volume deformable.""" + """Manager-based RL environment: Franka Panda lifting a soft beam to a target pose.""" # Scene settings scene: FrankaSoftSceneCfg = FrankaSoftSceneCfg() @@ -519,18 +620,31 @@ class FrankaSoftEnvCfg(ManagerBasedRLEnvCfg): rewards: RewardsCfg = RewardsCfg() terminations: TerminationsCfg = TerminationsCfg() events: EventCfg = EventCfg() + curriculum: CurriculumCfg = CurriculumCfg() def __post_init__(self) -> None: # general settings - self.decimation = 1 + self.decimation = 4 self.episode_length_s = 5.0 # simulation settings - self.sim.dt = 1 / 60.0 + self.sim.dt = 1.0 / 120 self.sim.render_interval = self.decimation - self.sim.gravity = (0.0, 0.0, 0.0) + self.sim.gravity = (0.0, 0.0, -9.81) self.sim.physics = PhysicsCfg() + self.viewer.eye = (0.75, 0.25, 0.65) + self.viewer.lookat = (0.0, 0.75, 0.4) + self.sim.default_visualizer_cfg = VisualizerCfg(eye=self.viewer.eye, lookat=self.viewer.lookat) + + self.video_recorder.window_width = 1920 + self.video_recorder.window_height = 1080 + + def play_mode(self): + super().play_mode() + if self.curriculum is not None: + self.curriculum.gravity = None + @configclass class FrankaSoftCameraEnvCfg(FrankaSoftEnvCfg): diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/lift/mdp/__init__.pyi b/source/isaaclab_tasks/isaaclab_tasks/core/lift/mdp/__init__.pyi index 93cb88f1efd8..7141e29219d2 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/lift/mdp/__init__.pyi +++ b/source/isaaclab_tasks/isaaclab_tasks/core/lift/mdp/__init__.pyi @@ -17,6 +17,16 @@ __all__ = [ "reset_to_target", "get_reset_state", "set_reset_state", + "joint_vel_out_of_sim_limit", + "deformable_outside_bounds", + "deformable_lifting", + "deformable_fingertip_distance", + "deformable_com_goal_reached", + "deformable_com_ee_distance", + "DeformableComGoalDistance", + "reset_deformable_over_support", + "gravity_range_linear", + "DeformableUniformPoseCommandCfg", "ObjectUniformPoseCommandCfg", "DifficultyScheduler", "initial_final_interpolate_fn", @@ -40,20 +50,19 @@ __all__ = [ "position_command_progress", "success_reward", "abnormal_robot_state", - "deformable_com_below_minimum", - "deformable_outside_table_bounds", "ee_below_minimum", "object_reached_goal", "out_of_bound", ] -from .commands import ObjectUniformPoseCommandCfg -from .curriculums import DifficultyScheduler, initial_final_interpolate_fn +from .commands import DeformableUniformPoseCommandCfg, ObjectUniformPoseCommandCfg +from .curriculums import DifficultyScheduler, gravity_range_linear, initial_final_interpolate_fn from .events import ( SuccessMonitor, conditional_reset, grasp_travel_distance, mesh_clearance, + reset_deformable_over_support, reset_joints_shared_offset, reset_to_target, slab_clearance, @@ -70,8 +79,13 @@ from .observations import ( vision_camera, ) from .rewards import ( + DeformableComGoalDistance, contacts, contact_count, + deformable_com_ee_distance, + deformable_com_goal_reached, + deformable_fingertip_distance, + deformable_lifting, deformable_com_goal_distance, deformable_ee_distance, deformable_lifted, @@ -84,10 +98,10 @@ from .rewards import ( success_reward, ) from .terminations import ( + deformable_outside_bounds, abnormal_robot_state, - deformable_com_below_minimum, - deformable_outside_table_bounds, ee_below_minimum, + joint_vel_out_of_sim_limit, object_reached_goal, out_of_bound, ) diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/lift/mdp/commands/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/core/lift/mdp/commands/__init__.py index 83f55101029b..e14e0f6d52c5 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/lift/mdp/commands/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/lift/mdp/commands/__init__.py @@ -3,4 +3,6 @@ # # SPDX-License-Identifier: BSD-3-Clause -from .pose_commands_cfg import * # noqa: F401, F403 +from isaaclab.utils.module import lazy_export + +lazy_export() diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/lift/mdp/commands/__init__.pyi b/source/isaaclab_tasks/isaaclab_tasks/core/lift/mdp/commands/__init__.pyi index 50695b343506..0a9bc6289bb3 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/lift/mdp/commands/__init__.pyi +++ b/source/isaaclab_tasks/isaaclab_tasks/core/lift/mdp/commands/__init__.pyi @@ -4,7 +4,8 @@ # SPDX-License-Identifier: BSD-3-Clause __all__ = [ + "DeformableUniformPoseCommandCfg", "ObjectUniformPoseCommandCfg", ] -from .pose_commands_cfg import ObjectUniformPoseCommandCfg +from .pose_commands_cfg import DeformableUniformPoseCommandCfg, ObjectUniformPoseCommandCfg diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/lift/mdp/commands/pose_commands.py b/source/isaaclab_tasks/isaaclab_tasks/core/lift/mdp/commands/pose_commands.py index b0165e452ee3..a21173c8f3e9 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/lift/mdp/commands/pose_commands.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/lift/mdp/commands/pose_commands.py @@ -13,15 +13,16 @@ import torch +from isaaclab.assets import AssetBaseCfg from isaaclab.managers import CommandTerm from isaaclab.utils.leapp import POSE7_ELEMENT_NAMES from isaaclab.utils.math import combine_frame_transforms, compute_pose_error, quat_from_euler_xyz, quat_unique if TYPE_CHECKING: - from isaaclab.assets import Articulation, RigidObject + from isaaclab.assets import Articulation, DeformableObject, RigidObject from isaaclab.envs import ManagerBasedEnv - from . import pose_commands_cfg as dex_cmd_cfgs + from .pose_commands_cfg import DeformableUniformPoseCommandCfg, ObjectUniformPoseCommandCfg class ObjectUniformPoseCommand(CommandTerm): @@ -49,10 +50,10 @@ class ObjectUniformPoseCommand(CommandTerm): and optional visualization settings. """ - cfg: dex_cmd_cfgs.ObjectUniformPoseCommandCfg + cfg: ObjectUniformPoseCommandCfg """Configuration for the command generator.""" - def __init__(self, cfg: dex_cmd_cfgs.ObjectUniformPoseCommandCfg, env: ManagerBasedEnv): + def __init__(self, cfg: ObjectUniformPoseCommandCfg, env: ManagerBasedEnv): """Initialize the command generator class. Args: @@ -65,10 +66,16 @@ def __init__(self, cfg: dex_cmd_cfgs.ObjectUniformPoseCommandCfg, env: ManagerBa # extract the robot and body index for which the command is generated self.robot: Articulation = env.scene[cfg.asset_name] self.object: RigidObject = env.scene[cfg.object_name] + self.success_vis_asset: RigidObject | AssetBaseCfg | None if cfg.success_vis_asset_name in env.scene.keys(): - self.success_vis_asset: RigidObject = env.scene[cfg.success_vis_asset_name] + self.success_vis_asset = env.scene[cfg.success_vis_asset_name] else: self.success_vis_asset = None + if isinstance(self.success_vis_asset, AssetBaseCfg): + offset = torch.tensor(self.success_vis_asset.init_state.pos, device=self.device) + self._static_success_vis_pos_w = env.scene.env_origins + offset + else: + self._static_success_vis_pos_w = None # create buffers # -- commands: (x, y, z, qx, qy, qz, qw) in root frame @@ -134,9 +141,13 @@ def _update_metrics(self): self.metrics["orientation_error"] = torch.linalg.norm(rot_error, dim=-1) success_id &= self.metrics["orientation_error"] < 0.5 if self.success_vis_asset is not None: - self.success_visualizer.visualize( - self.success_vis_asset.data.root_pos_w.torch, marker_indices=success_id.int() - ) + self.success_visualizer.visualize(self._get_success_vis_pos_w(), marker_indices=success_id.int()) + + def _get_success_vis_pos_w(self) -> torch.Tensor: + """Return the success visualization positions in the world frame.""" + if self._static_success_vis_pos_w is not None: + return self._static_success_vis_pos_w + return self.success_vis_asset.data.root_pos_w.torch def _resample_command(self, env_ids: Sequence[int]): # sample new pose targets @@ -194,3 +205,40 @@ def _debug_vis_callback(self, event): self.goal_visualizer.visualize(self.pose_command_w[:, :3], marker_indices=success_id + 1) # -- current object position self.curr_visualizer.visualize(obj_pos, marker_indices=success_id + 1) + + +class DeformableUniformPoseCommand(ObjectUniformPoseCommand): + """Uniform position command for a deformable object, tracked by its center of mass. + + Deformable objects expose no root orientation, so the target is tracked with the COM + (:attr:`~isaaclab.assets.DeformableObject.data.root_pos_w`) and only ``position_only`` + commands are supported. + """ + + cfg: DeformableUniformPoseCommandCfg + """Configuration for the command generator.""" + + object: DeformableObject + """The deformable object tracked by the command.""" + + def __init__(self, cfg: DeformableUniformPoseCommandCfg, env: ManagerBasedEnv): + if not cfg.position_only: + raise ValueError("DeformableUniformPoseCommand only supports position_only commands.") + super().__init__(cfg, env) + + def _update_metrics(self): + # transform command from base frame to simulation world frame + self.pose_command_w[:, :3], self.pose_command_w[:, 3:] = combine_frame_transforms( + self.robot.data.root_pos_w.torch, + self.robot.data.root_quat_w.torch, + self.pose_command_b[:, :3], + self.pose_command_b[:, 3:], + ) + com_w = self.object.data.root_pos_w.torch + self.metrics["position_error"] = torch.linalg.norm(self.pose_command_w[:, :3] - com_w, dim=-1) + + if self.success_vis_asset is None: + return + # same success radius as the goal markers of the base class + success_id = (self.metrics["position_error"] < 0.05).int() + self.success_visualizer.visualize(self._get_success_vis_pos_w(), marker_indices=success_id) diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/lift/mdp/commands/pose_commands_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/core/lift/mdp/commands/pose_commands_cfg.py index b43523c45738..5836818e4627 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/lift/mdp/commands/pose_commands_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/lift/mdp/commands/pose_commands_cfg.py @@ -13,7 +13,7 @@ from isaaclab.utils.configclass import configclass if TYPE_CHECKING: - from .pose_commands import ObjectUniformPoseCommand + from .pose_commands import DeformableUniformPoseCommand, ObjectUniformPoseCommand ALIGN_MARKER_CFG = VisualizationMarkersCfg( markers={ @@ -94,3 +94,10 @@ class Ranges: prim_path="/Visuals/SuccessMarkers", markers={} ) """The configuration for the success visualization marker. User needs to add the markers""" + + +@configclass +class DeformableUniformPoseCommandCfg(ObjectUniformPoseCommandCfg): + """Configuration for the deformable uniform pose command generator.""" + + class_type: type["DeformableUniformPoseCommand"] | str = "{DIR}.pose_commands:DeformableUniformPoseCommand" diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/lift/mdp/curriculums.py b/source/isaaclab_tasks/isaaclab_tasks/core/lift/mdp/curriculums.py index 5bc6129e1986..8a3884fad20c 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/lift/mdp/curriculums.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/lift/mdp/curriculums.py @@ -83,3 +83,26 @@ def __call__( # Python float: downstream ADR terms compare and interpolate host-side self.difficulty_frac = (torch.mean(self.current_adr_difficulties) / max(max_difficulty, 1)).item() return self.difficulty_frac + + +def gravity_range_linear( + env: ManagerBasedRLEnv, + _env_ids: Sequence[int], + event_name: str, + start_gravity_z: float, + end_gravity_z: float, + start_step: int, + end_step: int, +) -> dict[str, float]: + """Linearly ramp an event's deterministic vertical gravity [m/s^2].""" + if end_step <= start_step: + raise ValueError("end_step must be greater than start_step.") + + alpha = (env.common_step_counter - start_step) / (end_step - start_step) + alpha = min(max(alpha, 0.0), 1.0) + gravity_z = start_gravity_z + alpha * (end_gravity_z - start_gravity_z) + gravity = [0.0, 0.0, gravity_z] + event_cfg = env.event_manager.get_term_cfg(event_name) + event_cfg.params["gravity_distribution_params"] = (gravity, gravity.copy()) + env.event_manager.set_term_cfg(event_name, event_cfg) + return {"gravity_z": gravity_z} diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/lift/mdp/events.py b/source/isaaclab_tasks/isaaclab_tasks/core/lift/mdp/events.py index cc4f86a2a3ef..0d9a367b56b8 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/lift/mdp/events.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/lift/mdp/events.py @@ -31,7 +31,7 @@ if TYPE_CHECKING: from collections.abc import Sequence - from isaaclab.assets import Articulation + from isaaclab.assets import Articulation, DeformableObject, RigidObject from isaaclab.envs import ManagerBasedEnv, ManagerBasedRLEnv from .events_cfg import GraspTravelDistanceCfg, MeshClearanceCfg, SlabClearanceCfg, SuccessMonitorCfg @@ -897,3 +897,50 @@ def __call__(self, env: ManagerBasedEnv, env_ids: torch.Tensor) -> torch.Tensor: device=env.device, ) return wp.to_torch(out_min) >= self.cfg.min_clearance + + +def reset_deformable_over_support( + env: ManagerBasedEnv, + env_ids: torch.Tensor, + position_range: dict[str, tuple[float, float]], + support_offset_range: dict[str, tuple[float, float]], + asset_cfg: SceneEntityCfg = SceneEntityCfg("deformable"), + support_cfg: SceneEntityCfg = SceneEntityCfg("cube"), +) -> None: + """Reset a deformable object and keep a support body underneath it. + + The deformable is displaced from its default nodal state by a sample from + :paramref:`position_range`. The support receives the same planar displacement plus an + independent sample from :paramref:`support_offset_range`, so it stays under the deformable + while still varying between resets. + + Args: + env: The environment instance. + env_ids: The environment indices to reset. + position_range: Deformable displacement bounds [m] keyed by ``x``, ``y``, ``z``. + support_offset_range: Support jitter bounds [m] keyed by ``x``, ``y``, applied on top of + the deformable's displacement. + asset_cfg: Scene entity of the deformable object to reset. + support_cfg: Scene entity of the rigid support body to keep underneath. + """ + deformable: DeformableObject = env.scene[asset_cfg.name] + support: RigidObject = env.scene[support_cfg.name] + + # shared planar displacement, so the support tracks the deformable + ranges = torch.tensor([position_range.get(key, (0.0, 0.0)) for key in ("x", "y", "z")], device=deformable.device) + offset = sample_uniform(ranges[:, 0], ranges[:, 1], (len(env_ids), 3), device=deformable.device) + + nodal_state = deformable.data.default_nodal_state_w.torch[env_ids].clone() + nodal_state[..., :3] += offset.unsqueeze(1) + deformable.write_nodal_state_to_sim_index(nodal_state, env_ids=env_ids) + + ranges = torch.tensor([support_offset_range.get(key, (0.0, 0.0)) for key in ("x", "y")], device=support.device) + jitter = sample_uniform(ranges[:, 0], ranges[:, 1], (len(env_ids), 2), device=support.device) + + root_pose = support.data.default_root_pose.torch[env_ids].clone() + root_pose[:, :3] += env.scene.env_origins[env_ids] + root_pose[:, :2] += offset[:, :2] + jitter + support.write_root_pose_to_sim_index(root_pose=root_pose, env_ids=env_ids) + support.write_root_velocity_to_sim_index( + root_velocity=torch.zeros_like(support.data.default_root_vel.torch[env_ids]), env_ids=env_ids + ) diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/lift/mdp/observations.py b/source/isaaclab_tasks/isaaclab_tasks/core/lift/mdp/observations.py index a7b253906b0b..69b280ee3ae3 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/lift/mdp/observations.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/lift/mdp/observations.py @@ -9,7 +9,6 @@ from typing import TYPE_CHECKING import torch -import warp as wp from isaaclab.managers import ManagerTermBase, SceneEntityCfg from isaaclab.utils.math import quat_apply, quat_apply_inverse, quat_inv, quat_mul, subtract_frame_transforms @@ -269,8 +268,8 @@ def deformable_com_in_robot_root_frame( """Position of the deformable object's COM in the robot's root frame [m].""" asset: DeformableObject = 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 = asset.data.root_pos_w.torch + com_b, _ = subtract_frame_transforms(robot.data.root_pos_w.torch, robot.data.root_quat_w.torch, com_w) return com_b diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/lift/mdp/rewards.py b/source/isaaclab_tasks/isaaclab_tasks/core/lift/mdp/rewards.py index 54cdff0fa54b..848c0ca3457a 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/lift/mdp/rewards.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/lift/mdp/rewards.py @@ -10,9 +10,8 @@ from typing import TYPE_CHECKING import torch -import warp as wp -from isaaclab.managers import ManagerTermBase, SceneEntityCfg +from isaaclab.managers import ManagerTermBase, RewardTermCfg, SceneEntityCfg from isaaclab.utils import math as math_utils from isaaclab.utils.math import combine_frame_transforms, compute_pose_error @@ -375,27 +374,158 @@ def deformable_lifted( minimal_height: float, asset_cfg: SceneEntityCfg = SceneEntityCfg("deformable"), ) -> torch.Tensor: - """Reward if the deformable COM is above a minimum height.""" + """Reward if the deformable COM is above a minimum height. + + Args: + env: The environment instance. + minimal_height: Minimum COM height [m]. + asset_cfg: The deformable object 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] + com_z = asset.data.root_pos_w.torch[:, 2] return torch.where(com_z > minimal_height, 1.0, 0.0) +def deformable_lifting( + env: ManagerBasedRLEnv, + std: float, + minimal_height: float, + asset_cfg: SceneEntityCfg = SceneEntityCfg("deformable"), +) -> torch.Tensor: + """Reward raising the deformable COM above ``minimal_height`` [m] using a tanh kernel with scale ``std`` [m]. + + Dense analogue of :func:`deformable_lifted`: ungated and continuous, so it supplies a smooth + upward gradient rather than a binary step. Returns ``0`` at or below ``minimal_height`` and + saturates toward ``1``. + + Args: + env: The environment instance. + std: The tanh kernel standard deviation [m]. + minimal_height: Minimum COM height [m]. + asset_cfg: The deformable object entity. + + Returns: + Reward tensor with shape ``(num_envs,)``. + """ + asset: DeformableObject = env.scene[asset_cfg.name] + com_z = asset.data.root_pos_w.torch[:, 2] + height = (com_z - minimal_height).clamp(min=0.0) + return torch.tanh(height / std) + + def deformable_ee_distance( env: ManagerBasedRLEnv, std: float, asset_cfg: SceneEntityCfg = SceneEntityCfg("deformable"), ee_frame_cfg: SceneEntityCfg = SceneEntityCfg("ee_frame"), ) -> torch.Tensor: - """Reward reaching the deformable's nearest nodal point with the end-effector.""" + """Reward reaching the deformable's nearest nodal point with the end-effector. + + Args: + env: The environment instance. + std: The tanh kernel standard deviation [m]. + asset_cfg: The deformable object entity. + ee_frame_cfg: The end-effector frame entity. + + Returns: + Reward tensor with shape ``(num_envs,)``. + """ asset: DeformableObject = 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) - ee_w = wp.to_torch(ee_frame.data.target_pos_w)[..., 0, :] + nodal_pos_w = asset.data.nodal_pos_w.torch + ee_w = ee_frame.data.target_pos_w.torch[..., 0, :] distance = torch.linalg.norm(nodal_pos_w - ee_w.unsqueeze(1), dim=2).min(dim=1).values return 1.0 - torch.tanh(distance / std) +def deformable_com_ee_distance( + env: ManagerBasedRLEnv, + std: float, + asset_cfg: SceneEntityCfg = SceneEntityCfg("deformable"), + ee_frame_cfg: SceneEntityCfg = SceneEntityCfg("ee_frame"), +) -> torch.Tensor: + """Reward reaching the deformable's center of mass with the end-effector using a tanh kernel. + + Uses the COM (:attr:`~isaaclab.assets.DeformableObject.data.root_pos_w`) rather than the nearest + node, so the gripper is drawn to the object's middle. For an elongated body (e.g. a beam) this + steers the grasp toward the center instead of an end, keeping the object balanced when lifted. + + Args: + env: The environment instance. + std: The tanh kernel standard deviation [m]. + asset_cfg: The deformable object entity. + ee_frame_cfg: The end-effector frame entity. + + Returns: + Reward tensor with shape ``(num_envs,)``. + """ + asset: DeformableObject = env.scene[asset_cfg.name] + ee_frame: FrameTransformer = env.scene[ee_frame_cfg.name] + com_w = asset.data.root_pos_w.torch + ee_w = ee_frame.data.target_pos_w.torch[..., 0, :] + distance = torch.linalg.norm(com_w - ee_w, dim=1) + return 1.0 - torch.tanh(distance / std) + + +def deformable_fingertip_distance( + env: ManagerBasedRLEnv, + std: float, + asset_cfg: SceneEntityCfg = SceneEntityCfg("deformable"), + robot_cfg: SceneEntityCfg = SceneEntityCfg("robot"), + target_com: bool = False, +) -> torch.Tensor: + """Reward closing the gripper around the deformable using a tanh kernel with scale ``std`` [m]. + + Each selected finger body is rewarded for approaching the nearest deformable node + (:attr:`~isaaclab.assets.DeformableObject.data.nodal_pos_w`), + so grasping any part of the soft body is credited rather than only its center. Supplies the grasp + gradient that the EE-reach reward lacks. When ``target_com`` is set, each finger is instead drawn + to the object's center of mass, biasing the grasp to the middle of an elongated body (e.g. a beam). + + Args: + env: The environment instance. + std: The tanh kernel standard deviation [m]. + asset_cfg: The deformable object entity. + robot_cfg: The robot entity with ``body_ids`` selecting the finger bodies. + target_com: If ``True``, target the COM instead of the nearest node. + + Returns: + Reward tensor with shape ``(num_envs,)``. + """ + asset: DeformableObject = env.scene[asset_cfg.name] + robot: Articulation = env.scene[robot_cfg.name] + # target points in world frame: COM (num_envs, 1, 3) or all nodes (num_envs, num_nodes, 3) + if target_com: + target_w = asset.data.root_pos_w.torch.unsqueeze(1) + else: + target_w = asset.data.nodal_pos_w.torch + # selected finger bodies in world frame: (num_envs, num_fingers, 3) + finger_pos_w = robot.data.body_pos_w.torch[:, robot_cfg.body_ids] + # nearest target to each finger: (num_envs, num_fingers) + distance = torch.linalg.norm(finger_pos_w.unsqueeze(2) - target_w.unsqueeze(1), dim=3) + nearest = distance.min(dim=2).values + return (1.0 - torch.tanh(nearest / std)).mean(dim=1) + + +def _deformable_com_goal_metrics( + env: ManagerBasedRLEnv, + minimal_height: float, + command_name: str, + robot_cfg: SceneEntityCfg, + asset_cfg: SceneEntityCfg, +) -> tuple[torch.Tensor, torch.Tensor]: + """Compute deformable COM goal distance and lifted state.""" + robot: Articulation = env.scene[robot_cfg.name] + asset: DeformableObject = env.scene[asset_cfg.name] + command = env.command_manager.get_command(command_name) + des_pos_w, _ = combine_frame_transforms(robot.data.root_pos_w.torch, robot.data.root_quat_w.torch, command[:, :3]) + com_w = asset.data.root_pos_w.torch + return torch.linalg.norm(des_pos_w - com_w, dim=1), com_w[:, 2] > minimal_height + + def deformable_com_goal_distance( env: ManagerBasedRLEnv, std: float, @@ -404,16 +534,50 @@ def deformable_com_goal_distance( robot_cfg: SceneEntityCfg = SceneEntityCfg("robot"), asset_cfg: SceneEntityCfg = SceneEntityCfg("deformable"), ) -> torch.Tensor: - """Reward tracking of the goal position by the deformable's COM.""" - robot: Articulation = env.scene[robot_cfg.name] - asset: DeformableObject = env.scene[asset_cfg.name] - command = env.command_manager.get_command(command_name) - des_pos_w, _ = combine_frame_transforms( - wp.to_torch(robot.data.root_pos_w), wp.to_torch(robot.data.root_quat_w), command[:, :3] - ) - com_w = wp.to_torch(asset.data.root_pos_w) - distance = torch.linalg.norm(des_pos_w - com_w, dim=1) - return (com_w[:, 2] > minimal_height) * (1.0 - torch.tanh(distance / std)) + """Reward tracking the goal position with the lifted deformable COM.""" + distance, is_lifted = _deformable_com_goal_metrics(env, minimal_height, command_name, robot_cfg, asset_cfg) + return is_lifted.float() * (1.0 - torch.tanh(distance / std)) + + +class DeformableComGoalDistance(ManagerTermBase): + """Reward deformable COM goal tracking and log episode success.""" + + def __init__(self, cfg: RewardTermCfg, env: ManagerBasedRLEnv): + super().__init__(cfg, env) + self._succeeded = torch.zeros(env.num_envs, dtype=torch.bool, device=env.device) + + def reset(self, env_ids: Sequence[int] | None = None) -> None: + if env_ids is None: + env_ids = slice(None) + self._env.extras.setdefault("log", {})["Metrics/success_rate"] = self._succeeded[env_ids].float().mean().item() + self._succeeded[env_ids] = False + + def __call__( + self, + env: ManagerBasedRLEnv, + std: float, + minimal_height: float, + command_name: str, + success_threshold: float, + robot_cfg: SceneEntityCfg = SceneEntityCfg("robot"), + asset_cfg: SceneEntityCfg = SceneEntityCfg("deformable"), + ) -> torch.Tensor: + distance, is_lifted = _deformable_com_goal_metrics(env, minimal_height, command_name, robot_cfg, asset_cfg) + self._succeeded |= is_lifted & (distance < success_threshold) + return is_lifted.float() * (1.0 - torch.tanh(distance / std)) + + +def deformable_com_goal_reached( + env: ManagerBasedRLEnv, + minimal_height: float, + command_name: str, + success_threshold: float, + robot_cfg: SceneEntityCfg = SceneEntityCfg("robot"), + asset_cfg: SceneEntityCfg = SceneEntityCfg("deformable"), +) -> torch.Tensor: + """Reward the deformable COM for reaching the lifted goal.""" + distance, is_lifted = _deformable_com_goal_metrics(env, minimal_height, command_name, robot_cfg, asset_cfg) + return (is_lifted & (distance < success_threshold)).float() def gripper_close_action(env: ManagerBasedRLEnv, action_name: str = "gripper_action") -> torch.Tensor: diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/lift/mdp/terminations.py b/source/isaaclab_tasks/isaaclab_tasks/core/lift/mdp/terminations.py index 166bd9e805db..add6bdfd4287 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/lift/mdp/terminations.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/lift/mdp/terminations.py @@ -88,36 +88,52 @@ def object_reached_goal( return torch.linalg.norm(des_pos_w - object.data.root_pos_w.torch[:, :3], dim=1) < threshold -def deformable_com_below_minimum( +def ee_below_minimum( env: ManagerBasedRLEnv, minimum_height: float, - asset_cfg: SceneEntityCfg = SceneEntityCfg("deformable"), + ee_frame_cfg: SceneEntityCfg = SceneEntityCfg("ee_frame"), ) -> torch.Tensor: - """Return whether the deformable object's COM is below the minimum height [m].""" - asset: DeformableObject = env.scene[asset_cfg.name] - return wp.to_torch(asset.data.root_pos_w)[:, 2] < minimum_height + """Return whether the end-effector is below the minimum environment-frame height [m].""" + ee_frame: FrameTransformer = env.scene[ee_frame_cfg.name] + ee_z = wp.to_torch(ee_frame.data.target_pos_w)[..., 0, 2] - env.scene.env_origins[:, 2] + return ee_z < minimum_height -def deformable_outside_table_bounds( +def deformable_outside_bounds( env: ManagerBasedRLEnv, x_bounds: tuple[float, float], y_bounds: tuple[float, float], + z_bounds: tuple[float, float], asset_cfg: SceneEntityCfg = SceneEntityCfg("deformable"), ) -> torch.Tensor: - """Return whether any deformable node left the table footprint [m].""" + """Terminate if any deformable nodal point leaves the allowed workspace box. + + Covers both leaving the table footprint (x, y) and being dropped off it (z). + + 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]. + z_bounds: Allowed z-position range in the environment frame [m]. + asset_cfg: The deformable object 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]) - return torch.any(outside_x | outside_y, dim=1) + nodal_pos = asset.data.nodal_pos_w.torch - env.scene.env_origins.unsqueeze(1) + lower = torch.tensor([x_bounds[0], y_bounds[0], z_bounds[0]], device=nodal_pos.device) + upper = torch.tensor([x_bounds[1], y_bounds[1], z_bounds[1]], device=nodal_pos.device) + return ((nodal_pos < lower) | (nodal_pos > upper)).flatten(1).any(dim=1) -def ee_below_minimum( - env: ManagerBasedRLEnv, - minimum_height: float, - ee_frame_cfg: SceneEntityCfg = SceneEntityCfg("ee_frame"), +def joint_vel_out_of_sim_limit( + env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot") ) -> torch.Tensor: - """Return whether the end-effector is below the minimum environment-frame height [m].""" - ee_frame: FrameTransformer = env.scene[ee_frame_cfg.name] - ee_z = wp.to_torch(ee_frame.data.target_pos_w)[..., 0, 2] - env.scene.env_origins[:, 2] - return ee_z < minimum_height + """Terminate when joint velocities exceed actuator simulator limits [m/s or rad/s, depending on joint type].""" + asset: Articulation = env.scene[asset_cfg.name] + joint_ids = asset_cfg.joint_ids if asset_cfg.joint_ids is not None else slice(None) + limits = torch.full_like(asset.data.joint_vel.torch, torch.inf) + for actuator in asset.actuators.values(): + limits[:, actuator.joint_indices] = actuator.velocity_limit_sim + return torch.any(torch.abs(asset.data.joint_vel.torch[:, joint_ids]) > limits[:, joint_ids], dim=1) diff --git a/source/isaaclab_tasks/test/core/test_franka_deformable_ovphysx_cfg.py b/source/isaaclab_tasks/test/core/test_franka_deformable_ovphysx_cfg.py deleted file mode 100644 index 7ae19992fda8..000000000000 --- a/source/isaaclab_tasks/test/core/test_franka_deformable_ovphysx_cfg.py +++ /dev/null @@ -1,78 +0,0 @@ -# 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 - -"""Sim-free regression tests for the Franka deformable OvPhysX presets.""" - -import pytest -from isaaclab_ovphysx.physics import OvPhysxCfg -from isaaclab_physx.sim.schemas import PhysxDeformableBodyPropertiesCfg -from isaaclab_physx.sim.spawners.materials import ( - PhysxDeformableBodyMaterialCfg, - PhysxSurfaceDeformableBodyMaterialCfg, -) - -from isaaclab_tasks.core.lift.config.franka_soft.franka_cloth_env_cfg import ( - FrankaClothCameraEnvCfg, - FrankaClothEnvCfg, -) -from isaaclab_tasks.core.lift.config.franka_soft.franka_soft_env_cfg import FrankaSoftCameraEnvCfg, FrankaSoftEnvCfg -from isaaclab_tasks.utils.hydra import resolve_presets - - -def test_soft_task_ovphysx_preset_selects_complete_authored_scene(): - """Test that the soft-task OvPhysX preset selects a fully authored scene.""" - cfg = resolve_presets(FrankaSoftEnvCfg(), ("ovphysx",)) - - assert isinstance(cfg.sim.physics, OvPhysxCfg) - assert cfg.scene.replicate_physics is True - assert isinstance(cfg.scene.deformable.spawn.deformable_props, PhysxDeformableBodyPropertiesCfg) - assert isinstance(cfg.scene.deformable.spawn.physics_material, PhysxDeformableBodyMaterialCfg) - - -def test_cloth_task_ovphysx_preset_selects_complete_authored_scene(): - """Test that the cloth-task OvPhysX preset selects a fully authored scene.""" - cfg = resolve_presets(FrankaClothEnvCfg(), ("ovphysx",)) - - assert isinstance(cfg.sim.physics, OvPhysxCfg) - assert cfg.scene.replicate_physics is True - assert isinstance(cfg.scene.deformable.spawn.deformable_props, PhysxDeformableBodyPropertiesCfg) - assert isinstance(cfg.scene.deformable.spawn.physics_material, PhysxSurfaceDeformableBodyMaterialCfg) - assert cfg.events.robot_physics_material.params["asset_cfg"].body_names is None - - -@pytest.mark.parametrize( - ("env_cfg_type", "material_type"), - [ - (FrankaSoftCameraEnvCfg, PhysxDeformableBodyMaterialCfg), - (FrankaClothCameraEnvCfg, PhysxSurfaceDeformableBodyMaterialCfg), - ], -) -def test_camera_task_ovphysx_preset_selects_complete_authored_scene(env_cfg_type, material_type): - """Test that each camera-task OvPhysX preset selects a fully authored scene.""" - cfg = resolve_presets(env_cfg_type(), ("ovphysx",)) - - assert isinstance(cfg.sim.physics, OvPhysxCfg) - assert cfg.scene.replicate_physics is True - assert isinstance(cfg.scene.deformable.spawn.deformable_props, PhysxDeformableBodyPropertiesCfg) - assert isinstance(cfg.scene.deformable.spawn.physics_material, material_type) - - -def test_cloth_rendering_variant_applies_deterministic_overrides(): - """Test that the rendering test applies overrides after preset resolution.""" - from rendering_test_utils import _configure_franka_camera_test_env_cfg - - expected_range = { - "x": (0.0, 0.0), - "y": (0.0, 0.0), - "z": (0.0, 0.0), - } - - cfg = resolve_presets(FrankaClothCameraEnvCfg(), ("ovphysx",)) - _configure_franka_camera_test_env_cfg(cfg, "rgb") - - assert cfg.scene.num_envs == 4 - assert cfg.scene.replicate_physics is True - assert cfg.scene.base_camera.data_types == ["rgb"] - assert cfg.events.reset_deformable.params["position_range"] == expected_range