From 4a547f0a506c26237686b5f65c8eb8bae961276d Mon Sep 17 00:00:00 2001 From: Mike Yan Michelis Date: Fri, 31 Jul 2026 19:22:10 +0200 Subject: [PATCH 1/6] Retune Franka deformable lift for stable grasping The Franka deformable lift tasks could not grasp reliably. The gripper tunneled through the beam between soft vertices, and the shared lift MDP terms offered no deformable-aware rewards or divergence guards, so diverged environments kept poisoning the rollout. Add a task-local mdp package with deformable-aware rewards, observations, terminations, events, a gravity curriculum, and a pose command that tracks the deformable center of mass. Retune the beam material, the collider contact and rest offsets, and the arm and hand actuator gains so the gripper no longer crushes the deformable. Enable full-surface rigid-soft contact backed by signed-distance fields on the gripper so contacts are caught between vertices. Give the cloth task a kinematic rigid support that is registered with the coupler so the cloth no longer passes through it. Select the action space through a preset, with joint-space targets as the default and task-space inverse kinematics available through presets=ik. Support the above in Newton with NewtonShapeSDFCfg to provision volume SDFs on collider shapes selected by label regex, a collision pipeline flag for full-surface rigid-soft contact, and a configurable per-body particle contact buffer so contacts are not silently dropped. Full-surface contact on the coupled proxy solver additionally requires Newton with proxy-body contact harvesting, tracked in newton-physics/newton#3756. --- .../lab_newton/isaaclab_newton.physics.rst | 6 + docs/source/overview/environments.rst | 156 ++++----- .../state_machine/lift_franka_soft.py | 228 ++++++------- .../changelog.d/mym-lift-soft.minor.rst | 6 + .../deformable/newton_manager_cfg.py | 10 + .../changelog.d/mym-lift-soft.minor.rst | 9 + .../isaaclab_newton/physics/__init__.pyi | 2 + .../physics/newton_collision_cfg.py | 11 + .../isaaclab_newton/physics/newton_manager.py | 34 ++ .../physics/newton_manager_cfg.py | 35 ++ .../changelog.d/mym-lift-soft.minor.rst | 53 +++ .../franka_soft/agents/rsl_rl_ppo_cfg.py | 26 +- .../franka_soft/franka_cloth_env_cfg.py | 89 +++-- .../config/franka_soft/franka_soft_env_cfg.py | 297 ++++++++++++----- .../lift/config/franka_soft/mdp/__init__.py | 10 + .../lift/config/franka_soft/mdp/__init__.pyi | 64 ++++ .../config/franka_soft/mdp/curriculums.py | 75 +++++ .../lift/config/franka_soft/mdp/events.py | 181 +++++++++++ .../config/franka_soft/mdp/observations.py | 117 +++++++ .../config/franka_soft/mdp/pose_commands.py | 82 +++++ .../lift/config/franka_soft/mdp/rewards.py | 304 ++++++++++++++++++ .../config/franka_soft/mdp/terminations.py | 158 +++++++++ .../core/lift/config/franka_soft/mdp/utils.py | 95 ++++++ 23 files changed, 1718 insertions(+), 330 deletions(-) create mode 100644 source/isaaclab_contrib/changelog.d/mym-lift-soft.minor.rst create mode 100644 source/isaaclab_newton/changelog.d/mym-lift-soft.minor.rst create mode 100644 source/isaaclab_tasks/changelog.d/mym-lift-soft.minor.rst create mode 100644 source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/__init__.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/__init__.pyi create mode 100644 source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/curriculums.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/events.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/observations.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/pose_commands.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/rewards.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/terminations.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/utils.py diff --git a/docs/source/api/lab_newton/isaaclab_newton.physics.rst b/docs/source/api/lab_newton/isaaclab_newton.physics.rst index 8570e55f8806..b53add0c744c 100644 --- a/docs/source/api/lab_newton/isaaclab_newton.physics.rst +++ b/docs/source/api/lab_newton/isaaclab_newton.physics.rst @@ -18,6 +18,7 @@ NewtonCollisionPipelineCfg HydroelasticSDFCfg NewtonShapeCfg + NewtonShapeSDFCfg NewtonMJWarpManager NewtonXPBDManager NewtonFeatherstoneManager @@ -87,6 +88,11 @@ Physics Configuration :show-inheritance: :exclude-members: __init__ +.. autoclass:: NewtonShapeSDFCfg + :members: + :show-inheritance: + :exclude-members: __init__ + Solver Managers --------------- diff --git a/docs/source/overview/environments.rst b/docs/source/overview/environments.rst index c7c4357dc75f..25e279c72822 100644 --- a/docs/source/overview/environments.rst +++ b/docs/source/overview/environments.rst @@ -967,7 +967,7 @@ including disabling runtime perturbations used for training. * - Isaac-Ant - Manager Based - **rl_games** (PPO), **rsl_rl** (PPO), **skrl** (PPO), **sb3** (PPO) - - **physics=** ``newton_kamino``, ``newton_mjwarp``, ``physx`` + - **physics=** ``isaacsim_physx``, ``newton_kamino``, ``newton_mjwarp``, ``physx`` * - Isaac-Ant-Direct - Direct - **rl_games** (PPO), **rsl_rl** (PPO), **skrl** (PPO) @@ -975,11 +975,11 @@ including disabling runtime perturbations used for training. * - Isaac-Cartpole - Manager Based - **rl_games** (PPO), **rsl_rl** (PPO), **skrl** (PPO), **sb3** (PPO) - - **physics=** ``newton_kamino``, ``newton_mjwarp``, ``ovphysx``, ``physx`` + - **physics=** ``isaacsim_physx``, ``newton_kamino``, ``newton_mjwarp``, ``ovphysx``, ``physx`` * - Isaac-Cartpole-Camera - Manager Based - **rl_games** (PPO, FEATURE), **rsl_rl** (PPO, FEATURE) - - | **physics=** ``newton_kamino``, ``newton_mjwarp``, ``ovphysx``, ``physx`` + - | **physics=** ``isaacsim_physx``, ``newton_kamino``, ``newton_mjwarp``, ``ovphysx``, ``physx`` | **renderer=** ``isaacsim_rtx``, ``newton_renderer``, ``ovrtx``, ``rtx`` | **presets=** ``albedo``, ``depth``, ``resnet18``, ``rgb``, ``semantic_segmentation``, ``simple_shading_constant_diffuse``, ``simple_shading_diffuse_mdl``, ``simple_shading_full_mdl``, ``theia_tiny`` * - Isaac-Cartpole-Camera-Direct @@ -999,7 +999,7 @@ including disabling runtime perturbations used for training. * - Isaac-Humanoid - Manager Based - **rl_games** (PPO), **rsl_rl** (PPO), **skrl** (PPO), **sb3** (PPO) - - **physics=** ``newton_mjwarp``, ``physx`` + - **physics=** ``isaacsim_physx``, ``newton_mjwarp``, ``physx`` * - Isaac-Humanoid-Direct - Direct - **rl_games** (PPO), **rsl_rl** (PPO), **skrl** (PPO) @@ -1007,12 +1007,14 @@ including disabling runtime perturbations used for training. * - Isaac-Lift-Cloth-Franka - Manager Based - **rsl_rl** (PPO) - - **physics=** ``newton_mjwarp_vbd_proxy`` + - | **physics=** ``newton_mjwarp_vbd_proxy`` + | **presets=** ``ik``, ``joint`` * - Isaac-Lift-Cloth-Franka-Camera - Manager Based - **rsl_rl** (PPO) - | **physics=** ``newton_mjwarp_vbd_proxy`` | **renderer=** ``isaacsim_rtx``, ``newton_renderer``, ``ovrtx``, ``rtx`` + | **presets=** ``ik``, ``joint`` * - Isaac-Lift-Cube-Franka - Manager Based - **rl_games** (PPO), **rsl_rl** (PPO), **skrl** (PPO), **sb3** (PPO) @@ -1020,32 +1022,34 @@ including disabling runtime perturbations used for training. * - Isaac-Lift-Franka - Manager Based - **rsl_rl** (PPO) - - | **physics=** ``newton_mjwarp``, ``physx`` + - | **physics=** ``isaacsim_physx``, ``newton_mjwarp``, ``physx`` | **presets=** ``cube``, ``shapes`` * - Isaac-Lift-KukaAllegro - Manager Based - **rsl_rl** (PPO) - - | **physics=** ``newton_mjwarp``, ``ovphysx``, ``isaacsim_physx`` + - | **physics=** ``isaacsim_physx``, ``newton_mjwarp``, ``ovphysx``, ``physx`` | **presets=** ``cube``, ``shapes`` * - Isaac-Lift-KukaAllegro-Camera - Manager Based - **rsl_rl** (PPO) - - | **physics=** ``newton_mjwarp``, ``ovphysx``, ``physx`` + - | **physics=** ``isaacsim_physx``, ``newton_mjwarp``, ``ovphysx``, ``physx`` | **renderer=** ``isaacsim_rtx``, ``newton_renderer``, ``ovrtx``, ``rtx`` | **presets=** ``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 - Manager Based - **rsl_rl** (PPO) - - **physics=** ``newton_mjwarp_vbd_proxy``, ``physx`` + - | **physics=** ``isaacsim_physx``, ``newton_mjwarp_vbd_proxy``, ``physx`` + | **presets=** ``ik``, ``joint`` * - Isaac-Lift-Soft-Franka-Camera - Manager Based - **rsl_rl** (PPO) - - | **physics=** ``newton_mjwarp_vbd_proxy``, ``physx`` + - | **physics=** ``isaacsim_physx``, ``newton_mjwarp_vbd_proxy``, ``physx`` | **renderer=** ``isaacsim_rtx``, ``newton_renderer``, ``ovrtx``, ``rtx`` + | **presets=** ``ik``, ``joint`` * - Isaac-Open-Drawer-Franka - Manager Based - **rl_games** (PPO), **rsl_rl** (PPO), **skrl** (PPO) - - + - **presets=** ``isaacsim_physx`` * - Isaac-Open-Drawer-Franka-Direct - Direct - **rl_games** (PPO), **rsl_rl** (PPO), **skrl** (PPO) @@ -1057,16 +1061,16 @@ including disabling runtime perturbations used for training. * - Isaac-Reach-Franka - Manager Based - **rl_games** (PPO), **rsl_rl** (PPO), **skrl** (PPO) - - | **physics=** ``isaacsim_physx``, ``newton_kamino``, ``newton_mjwarp``, ``ovphysx`` + - | **physics=** ``isaacsim_physx``, ``newton_kamino``, ``newton_mjwarp``, ``ovphysx``, ``physx`` | **presets=** ``diffik``, ``joint_pos``, ``newton_ik`` * - Isaac-Reach-Franka-OSC - Manager Based - **rsl_rl** (PPO) - - **physics=** ``isaacsim_physx``, ``newton_kamino``, ``newton_mjwarp``, ``ovphysx`` + - **physics=** ``isaacsim_physx``, ``newton_kamino``, ``newton_mjwarp``, ``ovphysx``, ``physx`` * - Isaac-Reach-UR10 - Manager Based - **rl_games** (PPO), **rsl_rl** (PPO), **skrl** (PPO) - - **physics=** ``isaacsim_physx``, ``newton_kamino``, ``newton_mjwarp``, ``ovphysx`` + - **physics=** ``isaacsim_physx``, ``newton_kamino``, ``newton_mjwarp``, ``ovphysx``, ``physx`` * - Isaac-Reorient-Cube-Allegro - Manager Based - **rl_games** (PPO), **rsl_rl** (PPO), **skrl** (PPO) @@ -1074,89 +1078,89 @@ including disabling runtime perturbations used for training. * - Isaac-Reorient-Cube-Allegro-Direct - Direct - **rl_games** (PPO), **rsl_rl** (PPO), **skrl** (PPO) - - **physics=** ``newton_mjwarp``, ``ovphysx``, ``physx`` + - **physics=** ``isaacsim_physx``, ``newton_mjwarp``, ``ovphysx``, ``physx`` * - Isaac-Reorient-Cube-Shadow-Camera-Direct - Direct - **rl_games** (PPO), **rsl_rl** (PPO) - - | **physics=** ``newton_kamino``, ``newton_mjwarp``, ``ovphysx``, ``physx`` + - | **physics=** ``isaacsim_physx``, ``newton_kamino``, ``newton_mjwarp``, ``ovphysx``, ``physx`` | **renderer=** ``isaacsim_rtx``, ``newton_renderer``, ``ovrtx``, ``rtx`` | **presets=** ``albedo``, ``depth``, ``full``, ``rgb``, ``semantic_segmentation``, ``simple_shading_constant_diffuse``, ``simple_shading_diffuse_mdl``, ``simple_shading_full_mdl`` * - Isaac-Reorient-Cube-Shadow-Direct - Direct - **rl_games** (PPO), **rsl_rl** (PPO), **skrl** (PPO) - - **physics=** ``newton_kamino``, ``newton_mjwarp``, ``ovphysx``, ``physx`` + - **physics=** ``isaacsim_physx``, ``newton_kamino``, ``newton_mjwarp``, ``ovphysx``, ``physx`` * - Isaac-Reorient-Cube-Shadow-OpenAI-FF-Direct - Direct - **rl_games** (PPO), **rsl_rl** (PPO), **skrl** (PPO) - - **physics=** ``newton_kamino``, ``newton_mjwarp``, ``ovphysx``, ``physx`` + - **physics=** ``isaacsim_physx``, ``newton_kamino``, ``newton_mjwarp``, ``ovphysx``, ``physx`` * - Isaac-Reorient-Cube-Shadow-OpenAI-LSTM-Direct - Direct - **rl_games** (PPO), **rsl_rl** (PPO) - - **physics=** ``newton_kamino``, ``newton_mjwarp``, ``ovphysx``, ``physx`` + - **physics=** ``isaacsim_physx``, ``newton_kamino``, ``newton_mjwarp``, ``ovphysx``, ``physx`` * - Isaac-Reorient-Franka - Manager Based - **rsl_rl** (PPO) - - | **physics=** ``newton_mjwarp``, ``physx`` + - | **physics=** ``isaacsim_physx``, ``newton_mjwarp``, ``physx`` | **presets=** ``cube``, ``shapes`` * - Isaac-Reorient-KukaAllegro - Manager Based - **rsl_rl** (PPO) - - | **physics=** ``newton_mjwarp``, ``ovphysx``, ``physx`` + - | **physics=** ``isaacsim_physx``, ``newton_mjwarp``, ``ovphysx``, ``physx`` | **presets=** ``cube``, ``shapes`` * - Isaac-Reorient-KukaAllegro-Camera - Manager Based - **rsl_rl** (PPO) - - | **physics=** ``newton_mjwarp``, ``ovphysx``, ``physx`` + - | **physics=** ``isaacsim_physx``, ``newton_mjwarp``, ``ovphysx``, ``physx`` | **renderer=** ``isaacsim_rtx``, ``newton_renderer``, ``ovrtx``, ``rtx`` | **presets=** ``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-Shadow-Handover-Direct - Direct - - **rl_games** (PPO), **skrl** (PPO, IPPO, MAPPO) - - **physics=** ``newton_mjwarp``, ``physx`` + - **rl_games** (PPO), **rsl_rl** (PPO), **skrl** (PPO, IPPO, MAPPO) + - **physics=** ``isaacsim_physx``, ``newton_mjwarp``, ``ovphysx``, ``physx`` * - Isaac-Velocity-Flat-AnymalD - Manager Based - **rsl_rl** (PPO, DISTILLATION, DISTILLATION_RECURRENT, RECURRENT), **skrl** (PPO) - - **physics=** ``newton_mjwarp``, ``ovphysx``, ``physx`` + - **physics=** ``isaacsim_physx``, ``newton_mjwarp``, ``ovphysx``, ``physx`` * - Isaac-Velocity-Flat-Cassie - Manager Based - **rsl_rl** (PPO), **skrl** (PPO) - - **physics=** ``newton_kamino``, ``newton_mjwarp``, ``physx`` + - **physics=** ``isaacsim_physx``, ``newton_kamino``, ``newton_mjwarp``, ``physx`` * - Isaac-Velocity-Flat-G1 - Manager Based - **rsl_rl** (PPO), **skrl** (PPO) - - **physics=** ``newton_kamino``, ``newton_mjwarp``, ``physx`` + - **physics=** ``isaacsim_physx``, ``newton_kamino``, ``newton_mjwarp``, ``physx`` * - Isaac-Velocity-Flat-H1 - Manager Based - **rsl_rl** (PPO), **skrl** (PPO) - - **physics=** ``newton_kamino``, ``newton_mjwarp``, ``physx`` + - **physics=** ``isaacsim_physx``, ``newton_kamino``, ``newton_mjwarp``, ``physx`` * - Isaac-Velocity-Flat-Spot - Manager Based - **rsl_rl** (PPO), **skrl** (PPO) - - **physics=** ``newton_kamino``, ``newton_mjwarp``, ``physx`` + - **physics=** ``isaacsim_physx``, ``newton_kamino``, ``newton_mjwarp``, ``physx`` * - Isaac-Velocity-Flat-UnitreeGo2 - Manager Based - **rsl_rl** (PPO), **skrl** (PPO) - - **physics=** ``newton_kamino``, ``newton_mjwarp``, ``physx`` + - **physics=** ``isaacsim_physx``, ``newton_kamino``, ``newton_mjwarp``, ``physx`` * - Isaac-Velocity-Rough-AnymalD - Manager Based - **rsl_rl** (PPO), **skrl** (PPO) - - **physics=** ``newton_mjwarp``, ``ovphysx``, ``physx`` + - **physics=** ``isaacsim_physx``, ``newton_mjwarp``, ``ovphysx``, ``physx`` * - Isaac-Velocity-Rough-Cassie - Manager Based - **rsl_rl** (PPO), **skrl** (PPO) - - **physics=** ``newton_mjwarp``, ``ovphysx``, ``physx`` + - **physics=** ``isaacsim_physx``, ``newton_mjwarp``, ``ovphysx``, ``physx`` * - Isaac-Velocity-Rough-G1 - Manager Based - **rsl_rl** (PPO), **skrl** (PPO) - - **physics=** ``newton_mjwarp``, ``ovphysx``, ``physx`` + - **physics=** ``isaacsim_physx``, ``newton_mjwarp``, ``ovphysx``, ``physx`` * - Isaac-Velocity-Rough-H1 - Manager Based - **rsl_rl** (PPO), **skrl** (PPO) - - **physics=** ``newton_mjwarp``, ``ovphysx``, ``physx`` + - **physics=** ``isaacsim_physx``, ``newton_mjwarp``, ``ovphysx``, ``physx`` * - Isaac-Velocity-Rough-UnitreeGo2 - Manager Based - **rsl_rl** (PPO), **skrl** (PPO) - - **physics=** ``newton_mjwarp``, ``ovphysx``, ``physx`` + - **physics=** ``isaacsim_physx``, ``newton_mjwarp``, ``ovphysx``, ``physx`` * - IsaacContrib-Assemble-Trocar-G129-Dex3 - Manager Based - **rlinf** (PPO) @@ -1221,11 +1225,11 @@ including disabling runtime perturbations used for training. * - IsaacContrib-DrLegs-HoldPose - Manager Based - **rsl_rl** (PPO) - - **physics=** ``newton_kamino``, ``physx`` + - **physics=** ``isaacsim_physx``, ``newton_kamino``, ``physx`` * - IsaacContrib-DrLegs-Walk - Manager Based - **rsl_rl** (PPO) - - **physics=** ``newton_kamino``, ``physx`` + - **physics=** ``isaacsim_physx``, ``newton_kamino``, ``physx`` * - IsaacContrib-ExhaustPipe-GR1T2-Pink-IK-Abs - Manager Based - @@ -1285,7 +1289,7 @@ including disabling runtime perturbations used for training. * - IsaacContrib-Navigation-Flat-AnymalC - Manager Based - **rsl_rl** (PPO), **skrl** (PPO) - - + - **presets=** ``isaacsim_physx`` * - IsaacContrib-NutPour-GR1T2-Pink-IK-Abs - Manager Based - @@ -1293,11 +1297,11 @@ including disabling runtime perturbations used for training. * - IsaacContrib-Open-Drawer-Franka-IK-Abs - Manager Based - - - + - **presets=** ``isaacsim_physx`` * - IsaacContrib-Open-Drawer-Franka-IK-Rel - Manager Based - - - + - **presets=** ``isaacsim_physx`` * - IsaacContrib-Open-Drawer-OpenArm - Manager Based - **rl_games** (PPO), **rsl_rl** (PPO) @@ -1325,15 +1329,15 @@ including disabling runtime perturbations used for training. * - IsaacContrib-Place-Mug-Agibot-Left-Arm-RmpFlow - Manager Based - - - **physics=** ``newton_mjwarp``, ``physx`` + - **physics=** ``isaacsim_physx``, ``newton_mjwarp``, ``physx`` * - IsaacContrib-Place-Toy2Box-Agibot-Right-Arm-RmpFlow - Manager Based - - - **physics=** ``newton_mjwarp``, ``physx`` + - **physics=** ``isaacsim_physx``, ``newton_mjwarp``, ``physx`` * - IsaacContrib-Reach-Franka-IK-Abs - Manager Based - - - **physics=** ``isaacsim_physx``, ``newton_kamino``, ``newton_mjwarp``, ``ovphysx`` + - **physics=** ``isaacsim_physx``, ``newton_kamino``, ``newton_mjwarp``, ``ovphysx``, ``physx`` * - IsaacContrib-Reach-OpenArm - Manager Based - **rl_games** (PPO), **rsl_rl** (PPO), **skrl** (PPO) @@ -1345,66 +1349,66 @@ including disabling runtime perturbations used for training. * - IsaacContrib-Stack-Cube-Bin-Franka-IK-Rel-Mimic - Manager Based - - - **physics=** ``newton_mjwarp``, ``physx`` + - **physics=** ``isaacsim_physx``, ``newton_mjwarp``, ``physx`` * - IsaacContrib-Stack-Cube-BlueGreen-Franka-IK-Rel - Manager Based - - - **physics=** ``newton_mjwarp``, ``physx`` + - **physics=** ``isaacsim_physx``, ``newton_mjwarp``, ``physx`` * - IsaacContrib-Stack-Cube-BlueGreenRed-Franka-IK-Rel - Manager Based - - - **physics=** ``newton_mjwarp``, ``physx`` + - **physics=** ``isaacsim_physx``, ``newton_mjwarp``, ``physx`` * - IsaacContrib-Stack-Cube-Franka - Manager Based - - - **physics=** ``newton_mjwarp``, ``physx`` + - **physics=** ``isaacsim_physx``, ``newton_mjwarp``, ``physx`` * - IsaacContrib-Stack-Cube-Franka-IK-Abs - Manager Based - - - **physics=** ``newton_mjwarp``, ``physx`` + - **physics=** ``isaacsim_physx``, ``newton_mjwarp``, ``physx`` * - IsaacContrib-Stack-Cube-Franka-IK-Rel - Manager Based - - - **physics=** ``newton_mjwarp``, ``physx`` + - **physics=** ``isaacsim_physx``, ``newton_mjwarp``, ``physx`` * - IsaacContrib-Stack-Cube-Franka-IK-Rel-Blueprint - Manager Based - - - **physics=** ``newton_mjwarp``, ``physx`` + - **physics=** ``isaacsim_physx``, ``newton_mjwarp``, ``physx`` * - IsaacContrib-Stack-Cube-Franka-IK-Rel-Skillgen - Manager Based - - - **physics=** ``newton_mjwarp``, ``physx`` + - **physics=** ``isaacsim_physx``, ``newton_mjwarp``, ``physx`` * - IsaacContrib-Stack-Cube-Franka-IK-Rel-Visuomotor - Manager Based - - - **physics=** ``newton_mjwarp``, ``physx`` + - **physics=** ``isaacsim_physx``, ``newton_mjwarp``, ``physx`` * - IsaacContrib-Stack-Cube-Franka-IK-Rel-Visuomotor-Cosmos - Manager Based - - - **physics=** ``newton_mjwarp``, ``physx`` + - **physics=** ``isaacsim_physx``, ``newton_mjwarp``, ``physx`` * - IsaacContrib-Stack-Cube-Galbot-Left-Arm-Gripper-RmpFlow - Manager Based - - - **physics=** ``newton_mjwarp``, ``physx`` + - **physics=** ``isaacsim_physx``, ``newton_mjwarp``, ``physx`` * - IsaacContrib-Stack-Cube-Galbot-Left-Arm-Gripper-Visuomotor - Manager Based - - - | **physics=** ``newton_mjwarp``, ``physx`` + - | **physics=** ``isaacsim_physx``, ``newton_mjwarp``, ``physx`` | **renderer=** ``isaacsim_rtx``, ``newton_renderer``, ``ovrtx``, ``rtx`` * - IsaacContrib-Stack-Cube-Galbot-Left-Arm-Gripper-Visuomotor-Joint-Position - Manager Based - - - | **physics=** ``newton_mjwarp``, ``physx`` + - | **physics=** ``isaacsim_physx``, ``newton_mjwarp``, ``physx`` | **renderer=** ``isaacsim_rtx``, ``newton_renderer``, ``ovrtx``, ``rtx`` * - IsaacContrib-Stack-Cube-Galbot-Left-Arm-Gripper-Visuomotor-RmpFlow - Manager Based - - - | **physics=** ``newton_mjwarp``, ``physx`` + - | **physics=** ``isaacsim_physx``, ``newton_mjwarp``, ``physx`` | **renderer=** ``isaacsim_rtx``, ``newton_renderer``, ``ovrtx``, ``rtx`` * - IsaacContrib-Stack-Cube-Galbot-Right-Arm-Suction-RmpFlow - Manager Based - - - **physics=** ``newton_mjwarp``, ``physx`` + - **physics=** ``isaacsim_physx``, ``newton_mjwarp``, ``physx`` * - IsaacContrib-Stack-Cube-Instance-Randomize-Franka - Manager Based - @@ -1416,31 +1420,31 @@ including disabling runtime perturbations used for training. * - IsaacContrib-Stack-Cube-RedGreen-Franka-IK-Rel - Manager Based - - - **physics=** ``newton_mjwarp``, ``physx`` + - **physics=** ``isaacsim_physx``, ``newton_mjwarp``, ``physx`` * - IsaacContrib-Stack-Cube-RedGreenBlue-Franka-IK-Rel - Manager Based - - - **physics=** ``newton_mjwarp``, ``physx`` + - **physics=** ``isaacsim_physx``, ``newton_mjwarp``, ``physx`` * - IsaacContrib-Stack-Cube-SO101-IK-Abs-v0 - Manager Based - - - **physics=** ``newton_mjwarp``, ``physx`` + - **physics=** ``isaacsim_physx``, ``newton_mjwarp``, ``physx`` * - IsaacContrib-Stack-Cube-SO101-Joint-Teleop-v0 - Manager Based - - - **physics=** ``newton_mjwarp``, ``physx`` + - **physics=** ``isaacsim_physx``, ``newton_mjwarp``, ``physx`` * - IsaacContrib-Stack-Cube-SO101-v0 - Manager Based - - - **physics=** ``newton_mjwarp``, ``physx`` + - **physics=** ``isaacsim_physx``, ``newton_mjwarp``, ``physx`` * - IsaacContrib-Stack-Cube-UR10-Long-Suction-IK-Rel - Manager Based - - - **physics=** ``newton_mjwarp``, ``physx`` + - **physics=** ``isaacsim_physx``, ``newton_mjwarp``, ``physx`` * - IsaacContrib-Stack-Cube-UR10-Short-Suction-IK-Rel - Manager Based - - - **physics=** ``newton_mjwarp``, ``physx`` + - **physics=** ``isaacsim_physx``, ``newton_mjwarp``, ``physx`` * - IsaacContrib-TrackPositionNoObstacles-ARL-Robot-1 - Manager Based - **rl_games** (PPO), **rsl_rl** (PPO), **skrl** (PPO) @@ -1448,15 +1452,15 @@ including disabling runtime perturbations used for training. * - IsaacContrib-Tracking-LocoManip-Digit - Manager Based - **rsl_rl** (PPO) - - **physics=** ``physx`` + - **physics=** ``isaacsim_physx``, ``physx`` * - IsaacContrib-Velocity-Flat-AnymalB - Manager Based - **rsl_rl** (PPO), **skrl** (PPO) - - **physics=** ``newton_kamino``, ``newton_mjwarp``, ``physx`` + - **physics=** ``isaacsim_physx``, ``newton_kamino``, ``newton_mjwarp``, ``physx`` * - IsaacContrib-Velocity-Flat-AnymalC - Manager Based - **rl_games** (PPO), **rsl_rl** (PPO), **skrl** (PPO) - - **physics=** ``newton_kamino``, ``newton_mjwarp``, ``physx`` + - **physics=** ``isaacsim_physx``, ``newton_kamino``, ``newton_mjwarp``, ``physx`` * - IsaacContrib-Velocity-Flat-AnymalC-Direct - Direct - **rl_games** (PPO), **rsl_rl** (PPO), **skrl** (PPO) @@ -1464,23 +1468,23 @@ including disabling runtime perturbations used for training. * - IsaacContrib-Velocity-Flat-Digit - Manager Based - **rsl_rl** (PPO) - - **physics=** ``physx`` + - **physics=** ``isaacsim_physx``, ``physx`` * - IsaacContrib-Velocity-Flat-UnitreeA1 - Manager Based - **rsl_rl** (PPO), **skrl** (PPO), **sb3** (PPO) - - **physics=** ``newton_kamino``, ``newton_mjwarp``, ``physx`` + - **physics=** ``isaacsim_physx``, ``newton_kamino``, ``newton_mjwarp``, ``physx`` * - IsaacContrib-Velocity-Flat-UnitreeGo1 - Manager Based - **rsl_rl** (PPO), **skrl** (PPO) - - **physics=** ``newton_kamino``, ``newton_mjwarp``, ``physx`` + - **physics=** ``isaacsim_physx``, ``newton_kamino``, ``newton_mjwarp``, ``physx`` * - IsaacContrib-Velocity-Rough-AnymalB - Manager Based - **rsl_rl** (PPO), **skrl** (PPO) - - **physics=** ``newton_mjwarp``, ``ovphysx``, ``physx`` + - **physics=** ``isaacsim_physx``, ``newton_mjwarp``, ``ovphysx``, ``physx`` * - IsaacContrib-Velocity-Rough-AnymalC - Manager Based - **rl_games** (PPO), **rsl_rl** (PPO), **skrl** (PPO) - - **physics=** ``newton_mjwarp``, ``ovphysx``, ``physx`` + - **physics=** ``isaacsim_physx``, ``newton_mjwarp``, ``ovphysx``, ``physx`` * - IsaacContrib-Velocity-Rough-AnymalC-Direct - Direct - **rl_games** (PPO), **rsl_rl** (PPO), **skrl** (PPO) @@ -1488,14 +1492,14 @@ including disabling runtime perturbations used for training. * - IsaacContrib-Velocity-Rough-Digit - Manager Based - **rsl_rl** (PPO) - - **physics=** ``physx`` + - **physics=** ``isaacsim_physx``, ``physx`` * - IsaacContrib-Velocity-Rough-UnitreeA1 - Manager Based - **rsl_rl** (PPO), **skrl** (PPO), **sb3** (PPO) - - **physics=** ``newton_mjwarp``, ``ovphysx``, ``physx`` + - **physics=** ``isaacsim_physx``, ``newton_mjwarp``, ``ovphysx``, ``physx`` * - IsaacContrib-Velocity-Rough-UnitreeGo1 - Manager Based - **rsl_rl** (PPO), **skrl** (PPO) - - **physics=** ``newton_mjwarp``, ``ovphysx``, ``physx`` + - **physics=** ``isaacsim_physx``, ``newton_mjwarp``, ``ovphysx``, ``physx`` .. END-AUTO-GENERATED: comprehensive-environment-list diff --git a/scripts/environments/state_machine/lift_franka_soft.py b/scripts/environments/state_machine/lift_franka_soft.py index bd7da899719b..dd88db4a184a 100644 --- a/scripts/environments/state_machine/lift_franka_soft.py +++ b/scripts/environments/state_machine/lift_franka_soft.py @@ -11,19 +11,38 @@ .. 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 + + # Record a video. Requires Isaac Sim, since RecordVideo goes through the Kit RTX viewport. + uv run python scripts/environments/state_machine/lift_franka_soft.py --video -"""Launch Omniverse Toolkit first.""" +""" import argparse +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 -from isaaclab.app import AppLauncher +import isaaclab_tasks # noqa: F401 +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.") 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).") @@ -33,37 +52,17 @@ 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. +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 + +# RecordVideo needs an rgb_array render mode, which is the Kit RTX viewport: enable cameras and +# request the Kit visualizer so launch_simulation starts Isaac Sim. 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 -from collections.abc import Sequence - -import gymnasium as gym -import torch -import warp as wp - -from isaaclab.assets.deformable_object.deformable_object_data import DeformableObjectData - -import isaaclab_tasks # noqa: F401 -from isaaclab_tasks.utils.parse_cfg import parse_env_cfg + args_cli.visualizer = ["kit"] # initialize warp wp.init() @@ -185,7 +184,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) @@ -282,87 +281,92 @@ 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), + # 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 + # frame the deformable at (0.5, 0.0, 0.05) rather than the world origin. ``viewer`` drives the + # Kit viewport and the video recorder; ``default_visualizer_cfg`` drives the Newton/Kit + # visualizer window, which otherwise falls back to VisualizerCfg's (4.0, -4.0, 3.0). + 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, 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, ) - - # reset state machine - if dones.any(): - pick_sm.reset_idx(dones.nonzero(as_tuple=False).squeeze(-1)) - - # close the environment - env.close() + 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 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_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..3f9fd2104285 --- /dev/null +++ b/source/isaaclab_newton/changelog.d/mym-lift-soft.minor.rst @@ -0,0 +1,9 @@ +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. +* Added :class:`~isaaclab_newton.physics.NewtonShapeSDFCfg` and + :attr:`~isaaclab_newton.physics.NewtonCfg.sdf_shape_cfgs` to provision volume SDFs on collider + shapes selected by label regex, as required by full-surface rigid-soft contact. diff --git a/source/isaaclab_newton/isaaclab_newton/physics/__init__.pyi b/source/isaaclab_newton/isaaclab_newton/physics/__init__.pyi index 46cc26e9318f..fa541baaafa8 100644 --- a/source/isaaclab_newton/isaaclab_newton/physics/__init__.pyi +++ b/source/isaaclab_newton/isaaclab_newton/physics/__init__.pyi @@ -17,6 +17,7 @@ __all__ = [ "NewtonCollisionPipelineCfg", "NewtonManager", "NewtonShapeCfg", + "NewtonShapeSDFCfg", "NewtonSolverCfg", "NewtonXPBDManager", "XPBDSolverCfg", @@ -35,6 +36,7 @@ from .newton_manager import NewtonManager from .newton_manager_cfg import ( NewtonCfg, NewtonShapeCfg, + NewtonShapeSDFCfg, NewtonSolverCfg, ) from .xpbd_manager import NewtonXPBDManager 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..3a7b810208d6 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. + Requires a volume SDF on every participating rigid mesh/convex shape; provision these via + :attr:`~isaaclab_newton.physics.NewtonShapeSDFCfg` on :attr:`NewtonCfg.sdf_shape_cfgs`. + + 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_newton/isaaclab_newton/physics/newton_manager.py b/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py index e32907729683..9dd604288034 100644 --- a/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py +++ b/source/isaaclab_newton/isaaclab_newton/physics/newton_manager.py @@ -1026,6 +1026,35 @@ def _prepare_builder_for_finalize(cls, builder: ModelBuilder) -> None: The default implementation is a no-op. """ + @classmethod + def _provision_shape_sdfs(cls, builder: ModelBuilder) -> None: + """Request volume SDFs on collider shapes selected by :attr:`NewtonCfg.sdf_shape_cfgs`. + + Newton retains per-shape SDF requests on the builder (``shape_force_sdf`` and + ``shape_sdf_max_resolution``) until :meth:`ModelBuilder.finalize` generates the SDF data. + Rather than intercepting shape creation, this matches the already-populated shape labels + by regex and flips the retained flags, equivalent to calling + ``ShapeConfig.configure_sdf(force_sdf=True, max_resolution=...)`` at add time. Required by + :attr:`NewtonCollisionPipelineCfg.enable_rigid_soft_full_surface_contact`. + """ + cfg = PhysicsManager._cfg + if not isinstance(cfg, NewtonCfg) or not cfg.sdf_shape_cfgs: + return + + labels = list(getattr(builder, "shape_label", ()) or ()) + if not labels: + return + + for sdf_cfg in cfg.sdf_shape_cfgs: + if not sdf_cfg.shape_label_patterns: + continue + matched, _ = resolve_matching_names(sdf_cfg.shape_label_patterns, labels, raise_when_no_match=False) + for index in matched: + builder.shape_force_sdf[index] = True + if sdf_cfg.max_resolution is not None: + builder.shape_sdf_max_resolution[index] = sdf_cfg.max_resolution + builder.shape_sdf_target_voxel_size[index] = None + @classmethod def cl_register_site(cls, body_pattern: str | None, xform: wp.transform, *, per_world: bool = False) -> str: """Register a site request for injection into prototypes before replication. @@ -1338,6 +1367,11 @@ def start_simulation(cls) -> None: cls._builder.request_state_attributes(*cls._pending_extended_state_attributes) NewtonManager._pending_extended_state_attributes = set() cls._prepare_builder_for_finalize(cls._builder) + # Provision volume SDFs on selected collider shapes (e.g. gripper) before finalize so + # full-surface rigid-soft contact has the SDFs it requires. Runs after the subclass hook + # so replicated shape labels are present, and unconditionally so subclasses that override + # _prepare_builder_for_finalize without calling super() still get it. + cls._provision_shape_sdfs(cls._builder) with Timer(name="newton_finalize_builder", msg="Finalize builder took:"): NewtonManager._model = cls._builder.finalize(device=device) cls._model.set_gravity(cls._gravity_vector) diff --git a/source/isaaclab_newton/isaaclab_newton/physics/newton_manager_cfg.py b/source/isaaclab_newton/isaaclab_newton/physics/newton_manager_cfg.py index 8747d667d3ce..091a825c8a7c 100644 --- a/source/isaaclab_newton/isaaclab_newton/physics/newton_manager_cfg.py +++ b/source/isaaclab_newton/isaaclab_newton/physics/newton_manager_cfg.py @@ -8,6 +8,7 @@ from __future__ import annotations import logging +from dataclasses import field from typing import TYPE_CHECKING, Literal from isaaclab.physics import PhysicsCfg @@ -98,6 +99,32 @@ class NewtonShapeCfg: """ +@configclass +class NewtonShapeSDFCfg: + """Provisions a volume SDF on selected rigid collider shapes before finalize. + + Full-surface rigid-soft contact + (:attr:`~isaaclab_newton.physics.NewtonCollisionPipelineCfg.enable_rigid_soft_full_surface_contact`) + requires an SDF on every participating rigid mesh/convex shape. Newton retains per-shape SDF + requests on the builder until finalize; this config targets shapes by full label regex and + forwards to ``ModelBuilder.ShapeConfig.configure_sdf(force_sdf=True)`` semantics. + """ + + shape_label_patterns: list[str] = field(default_factory=list) + """Full Newton shape-label regexes; every matched collider shape receives a volume SDF. + + Matching is a full-label regex against ``Model.shape_label`` (append ``.*`` to match a body's + descendant collider shapes, e.g. ``r"/World/envs/env_.*/Robot/panda_hand.*"``). + """ + + max_resolution: int | None = None + """Maximum SDF grid resolution [voxels], must be divisible by 8. + + ``None`` builds the SDF at Newton's default resolution (``force_sdf`` only), which is the + lightest way to provision the SDF needed for full-surface contact. + """ + + @configclass class NewtonCfg(PhysicsCfg): """Configuration for Newton physics manager. @@ -169,6 +196,14 @@ class NewtonCfg(PhysicsCfg): :class:`NewtonShapeCfg` for the declared fields. """ + sdf_shape_cfgs: list[NewtonShapeSDFCfg] = field(default_factory=list) + """Per-shape volume SDF provisioning applied to the builder before finalize. + + Each entry selects collider shapes by label regex and requests a volume SDF on them, as + required by :attr:`NewtonCollisionPipelineCfg.enable_rigid_soft_full_surface_contact`. + Defaults to an empty list (no SDFs provisioned beyond Newton's own heuristics). + """ + simplify_meshes: bool = True """Whether Newton replication simplifies mesh colliders to convex hulls. diff --git a/source/isaaclab_tasks/changelog.d/mym-lift-soft.minor.rst b/source/isaaclab_tasks/changelog.d/mym-lift-soft.minor.rst new file mode 100644 index 000000000000..f90ad6e01a17 --- /dev/null +++ b/source/isaaclab_tasks/changelog.d/mym-lift-soft.minor.rst @@ -0,0 +1,53 @@ +Added +^^^^^ + +* Added a task-local ``mdp`` package for the Franka deformable lift environments, providing + deformable-aware rewards, observations, terminations, events, curricula, and a pose command that + tracks the deformable center of mass. +* Added an ``ik`` action preset to the Franka deformable lift environments that drives the arm with + an absolute end-effector pose through a differential inverse-kinematics controller. Select it + with ``presets=ik``. +* Added a gravity curriculum to the Franka deformable lift environments that linearly ramps the + vertical gravity up to -9.81 m/s^2 over the first environment steps, so the policy learns to + grasp before it has to hold the object up. The soft-beam tasks ramp from near zero over 10000 + steps, the cloth tasks from -1.0 m/s^2 over 20000 steps. +* Added terminations for a diverged solve (non-finite deformable or robot state) and for joint + velocities beyond the simulation limits, so unrecoverable environments reset instead of poisoning + the rollout. + +Changed +^^^^^^^ + +* **Breaking:** Changed the Franka deformable lift environments to select their action space + through a :class:`~isaaclab_tasks.utils.PresetCfg`, with ``joint`` (relative joint-position arm + targets plus a limit-rescaled gripper) as the new default. ``Isaac-Lift-Soft-Franka`` and + ``Isaac-Lift-Soft-Franka-Camera`` previously used absolute task-space differential inverse + kinematics, and ``Isaac-Lift-Cloth-Franka`` and ``Isaac-Lift-Cloth-Franka-Camera`` previously + used absolute joint-position targets. Append ``presets=ik`` to the run command to get the + task-space inverse-kinematics action space back. +* **Breaking:** Changed the rsl_rl ``experiment_name`` of ``Isaac-Lift-Soft-Franka`` and + ``Isaac-Lift-Cloth-Franka`` from ``franka_deformable`` to ``franka_soft``. New runs are written to + ``logs/rsl_rl/franka_soft``; move existing ``logs/rsl_rl/franka_deformable`` run directories there + to resume from an older checkpoint. +* Changed the Franka deformable lift environments to simulate under real gravity: gravity is no + longer disabled on the robot, and the vertical gravity of ``Isaac-Lift-Soft-Franka`` and + ``Isaac-Lift-Soft-Franka-Camera`` is no longer zeroed. +* Changed the simulation step of the Franka deformable lift environments from 1/60 s to 1/120 s + with a decimation of 4, which halves the policy control rate from 60 Hz to 30 Hz, and raised the + default number of environments of ``Isaac-Lift-Soft-Franka`` from 128 to 2048. +* Changed the table of the Franka deformable lift environments from the ``SeattleLabTable`` USD + asset to an invisible cuboid collider whose top surface sits at ``z = 0``. The goal command's + success visualizer draws the table instead, tinted by whether the goal is reached. +* Changed the rsl_rl PPO configuration of ``Isaac-Lift-Soft-Franka`` and + ``Isaac-Lift-Cloth-Franka`` to the actor/critic model configurations, using + :class:`~isaaclab_rl.rsl_rl.RslRlMLPModelCfg` with observation normalization and explicit + ``obs_groups``, a learning rate of 1e-3, and ``max_iterations`` lowered from 50000 to 5000. +* Retuned ``Isaac-Lift-Soft-Franka`` and ``Isaac-Lift-Soft-Franka-Camera`` for stable grasping: a + stiffer and denser beam, a smaller particle radius, explicit collider contact and rest offsets, + and full-surface rigid-soft contact with signed-distance fields on the gripper. +* Retuned the Franka arm and hand actuator gains of the Franka deformable lift environments, adding + realistic armature and a slower, weaker gripper so it settles on the object instead of crushing + it. This replaces the previous per-task gripper overrides, so the cloth tasks now use the same + gains as the soft-beam tasks. +* Changed ``Isaac-Lift-Cloth-Franka`` to place the cloth over a kinematic rigid support that is + registered with the coupler, so the cloth no longer passes through it. 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..3150aa9b4782 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,18 +31,25 @@ @configclass class FrankaDeformablePPORunnerCfg(RslRlOnPolicyRunnerCfg): num_steps_per_env = 24 - max_iterations = 50000 + max_iterations = 5000 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 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 4ece3c02f0e5..4245bc02aa52 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,24 +7,25 @@ from __future__ import annotations -from isaaclab_newton.physics import MJWarpSolverCfg, NewtonCfg, NewtonShapeCfg +from isaaclab_newton.physics import MJWarpSolverCfg, NewtonCfg from isaaclab_newton.sim.schemas import NewtonDeformableBodyPropertiesCfg from isaaclab_newton.sim.spawners.materials import NewtonSurfaceDeformableBodyMaterialCfg 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 CurriculumTermCfg as CurrTerm from isaaclab.managers import EventTermCfg as EventTerm from isaaclab.managers import SceneEntityCfg from isaaclab.sensors import CameraCfg from isaaclab.utils.configclass import configclass from isaaclab_contrib.coupling import CouplerEntryCfg, CouplerProxyCfg, CouplerProxyMappingCfg -from isaaclab_contrib.deformable.newton_manager_cfg import NewtonModelCfg, VBDSolverCfg +from isaaclab_contrib.deformable.newton_manager_cfg import 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, @@ -55,18 +56,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, ), @@ -78,20 +77,15 @@ class PhysicsCfg(PresetCfg): bodies=[ r"/World/envs/env_.*/Robot/panda_hand", r"/World/envs/env_.*/Robot/panda_(left|right)finger", + r"/World/envs/env_.*/Cube", ], + # detect contact every substep so the gripper stops at the cloth surface collide_interval=1, ) ], iterations=1, - model_cfg=NewtonModelCfg( - soft_contact_ke=1e3, - soft_contact_kd=1e-5, - soft_contact_mu=0.5, - ), ), - default_shape_cfg=NewtonShapeCfg(ke=1e3, kd=1e-5, mu=1e-4), - num_substeps=10, - use_cuda_graph=True, + num_substeps=2, ) default = newton_mjwarp_vbd_proxy @@ -130,12 +124,15 @@ 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), + 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)), ), @@ -150,24 +147,36 @@ class FrankaClothCameraSceneCfg(FrankaClothSceneCfg): @configclass -class ActionsCfg: - """7-dim arm joint position + 1-dim binary gripper.""" +class CurriculumCfg: + """Ramp the action-rate penalty once the policy has learned to lift (matches rigid recipe).""" - arm_action = mdp.JointPositionActionCfg( - asset_name="robot", joint_names=["panda_joint.*"], scale=0.1, use_default_offset=True + action_rate = CurrTerm( + func=mdp.modify_reward_weight, params={"term_name": "action_rate", "weight": -1e-2, "num_steps": 50000} ) - 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}, + + # Since we use 24 steps per env, 20000 steps correspond to 20000/24 = 833.33 learning iterations + gravity = CurrTerm( + func=mdp.modify_gravity_linear, + params={"start_gravity_z": -1.0, "end_gravity_z": -9.81, "start_step": 0, "end_step": 20000}, ) @configclass -class EventCfg(FrankaSoftEventCfg): +class FrankaClothEventCfg(FrankaSoftEventCfg): """Reset and startup events for the Franka cloth environment.""" + # Replaces the base term so the cube follows the randomized cloth position. + reset_deformable = EventTerm( + func=mdp.reset_deformable_over_support, + mode="reset", + params={ + "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"), + }, + ) + robot_physics_material = EventTerm( func=mdp.randomize_rigid_body_material, mode="startup", @@ -192,27 +201,15 @@ class FrankaClothEnvCfg(FrankaSoftEnvCfg): # Scene settings scene: FrankaClothSceneCfg = FrankaClothSceneCfg(num_envs=128, env_spacing=2.5, replicate_physics=True) - # Basic settings - actions: ActionsCfg = ActionsCfg() # MDP settings - events: EventCfg = EventCfg() + events: FrankaClothEventCfg = FrankaClothEventCfg() + curriculum: CurriculumCfg = CurriculumCfg() 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() - # increase franka gripper stiffness - self.scene.robot.actuators["panda_hand"].effort_limit_sim = 500.0 - self.scene.robot.actuators["panda_hand"].stiffness = 2000.0 - self.scene.robot.actuators["panda_hand"].damping = 100.0 - @configclass class FrankaClothCameraEnvCfg(FrankaClothEnvCfg): 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 a3bad98d214b..ea7664389116 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,24 +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, + NewtonShapeSDFCfg, +) 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.schemas import PhysxCollisionCfg, PhysxDeformableBodyPropertiesCfg from isaaclab_physx.sim.spawners.materials import PhysxDeformableBodyMaterialCfg import isaaclab.sim as sim_utils 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 @@ -32,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, @@ -46,10 +52,11 @@ 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 ## @@ -63,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( @@ -90,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, ), ), ) @@ -106,15 +121,16 @@ 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.0005, contact_offset=0.005)], + 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, + static_friction=1.0, + dynamic_friction=1.0, ), ), ) @@ -139,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,21 +168,32 @@ class PhysicsCfg(PresetCfg): r"/World/envs/env_.*/Robot/panda_hand", r"/World/envs/env_.*/Robot/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=5.0e3), ), - default_shape_cfg=NewtonShapeCfg(ke=4e4, kd=1e-5, mu=5.0), - num_substeps=10, + sdf_shape_cfgs=[ + NewtonShapeSDFCfg( + shape_label_patterns=[ + r"/World/envs/env_.*/Robot/panda_hand/collisions/collisions", + r"/World/envs/env_.*/Robot/panda_(left|right)finger/collisions/collisions", + ], + # ~2.3 mm voxels on the fingers; finer isn't needed for contact resolution. + max_resolution=8, + ) + ], + num_substeps=2, ) - isaacsim_physx: PhysxCfg = PhysxCfg() + isaacsim_physx: PhysxCfg = PhysxCfg( + friction_offset_threshold=0.001, + friction_correlation_distance=0.005, + ) physx: PhysxAutoCfg = PhysxAutoCfg(isaacsim_physx=isaacsim_physx) default = newton_mjwarp_vbd_proxy @@ -198,13 +225,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 @@ -224,14 +251,29 @@ 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 + # Re-tuned Franka actuators: stiff arm gains with realistic armature so the low-inertia + # default gains do not let the fingers tunnel through the soft body, and a slower, weaker + # gripper so it settles on the beam surface instead of crushing it. Velocity limits are + # required by the joint_vel_out_of_sim_limit termination. Scoped here rather than in + # FRANKA_PANDA_CFG so the other Franka tasks keep the stock asset. + shoulder = self.robot.actuators["panda_shoulder"] + shoulder.velocity_limit_sim = 2.175 + shoulder.stiffness = 600.0 + shoulder.damping = 50.0 + shoulder.armature = {"panda_joint[1-2]": 0.6057, "panda_joint[3-4]": 0.4625} + + forearm = self.robot.actuators["panda_forearm"] + forearm.velocity_limit_sim = 2.61 + forearm.stiffness = {"panda_joint5": 250.0, "panda_joint6": 150.0, "panda_joint7": 50.0} + forearm.damping = {"panda_joint5": 30.0, "panda_joint6": 25.0, "panda_joint7": 15.0} + forearm.armature = 0.2055 + + hand = self.robot.actuators["panda_hand"] + hand.effort_limit_sim = 70.0 + hand.velocity_limit_sim = 0.2 + hand.stiffness = 750.0 + hand.damping = 175.0 + hand.armature = 0.1 @configclass @@ -250,12 +292,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), @@ -263,16 +305,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 ), }, ), @@ -280,10 +322,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.*"], 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", @@ -293,16 +346,28 @@ 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}, + close_command_expr={"panda_finger_.*": 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.""" @@ -390,7 +455,7 @@ 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"), }, @@ -399,48 +464,79 @@ class EventCfg: @configclass class RewardsCfg: - """Lift-to-target reward for a deformable object.""" - + """Deformable analogue of the winning rigid-cube lift recipe. + + The table top sits at z = 0 and the beam's rest COM at ~0.05 m. The dense lift reward gates at + 0.02 m, i.e. below rest, so it stays graded across the full lift range and only vanishes when + the beam is pressed into the table. The goal-tracking terms gate at 0.0 m, so they are + effectively always active. Success = COM within 5 cm of the goal position. + """ + + # The hand targets the COM so the grasp lands mid-beam (targeting the nearest node lets the + # gripper grab an end, which turns the beam into an unstable pole). The fingers target the + # nearest surface node instead: targeting the COM with both fingers is maximized only when + # both fingertips reach the object's center, i.e. by closing and indenting the beam. 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_delta = RewTerm( + func=mdp.deformable_com_goal_distance_delta, + params={ + "minimal_height": 0.0, + "command_name": "deformable_pose", + "asset_cfg": SceneEntityCfg("deformable"), + }, + weight=500.0, + ) + deformable_goal_tracking = RewTerm( func=mdp.deformable_com_goal_distance, 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=10.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-4) + + +@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-2, "num_steps": 40000} + ) + + # Since we use 24 steps per env, 10000 steps correspond to 10000/24 = 416.67 learning iterations + gravity = CurrTerm( + func=mdp.modify_gravity_linear, + params={"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 @@ -468,6 +564,25 @@ class TerminationsCfg: 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")}, + ) + + # real failure, not a time out: a diverged solve must bootstrap as a termination + deformable_invalid = DoneTerm( + func=mdp.deformable_state_invalid, + params={"asset_cfg": SceneEntityCfg("deformable")}, + ) + + # The measured divergence poisons the beam too, so deformable_invalid resets that case. This + # covers a robot-only divergence: every reward term is deformable-driven and sanitized, and the + # other robot terminations fail open on NaN, so nothing else would ever reset the environment. + robot_invalid = DoneTerm( + func=mdp.robot_state_invalid, + params={"asset_cfg": SceneEntityCfg("robot")}, + ) + ## # Environment configuration @@ -477,11 +592,11 @@ 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 default = newton_mjwarp_vbd_proxy @@ -492,16 +607,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() @@ -512,19 +627,29 @@ class FrankaSoftEnvCfg(ManagerBasedRLEnvCfg): # MDP settings rewards: RewardsCfg = RewardsCfg() terminations: TerminationsCfg = TerminationsCfg() + # Parent reset events + per-env material domain randomization. events: EventCfg = EventCfg() + # Ramp the action-rate penalty once the policy has learned to lift. + 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 + @configclass class FrankaSoftCameraEnvCfg(FrankaSoftEnvCfg): diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/__init__.py new file mode 100644 index 000000000000..188dd9bacd92 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/__init__.py @@ -0,0 +1,10 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""This sub-module contains the functions that are specific to the deformable lift environments.""" + +from isaaclab.utils.module import lazy_export + +lazy_export() diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/__init__.pyi b/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/__init__.pyi new file mode 100644 index 000000000000..ac119215daa7 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/__init__.pyi @@ -0,0 +1,64 @@ +# 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 + +__all__ = [ + # observations + "deformable_com_in_robot_root_frame", + "DeformableSampledPointsInRobotRootFrame", + # rewards + "deformable_lifted", + "deformable_lifting", + "deformable_ee_distance", + "deformable_com_ee_distance", + "deformable_fingertip_distance", + "deformable_com_goal_distance", + "deformable_com_goal_distance_delta", + "deformable_com_goal_reached", + # terminations + "deformable_com_below_minimum", + "deformable_nodal_vel_above_maximum", + "deformable_outside_table_bounds", + "deformable_state_invalid", + "joint_vel_out_of_sim_limit", + "robot_state_invalid", + # events + "randomize_deformable_material", + "reset_deformable_over_support", + # commands + "DeformableUniformPoseCommand", + "DeformableUniformPoseCommandCfg", + # curriculums + "modify_gravity_linear", +] + +from .curriculums import modify_gravity_linear +from .events import randomize_deformable_material, reset_deformable_over_support +from .observations import ( + DeformableSampledPointsInRobotRootFrame, + deformable_com_in_robot_root_frame, +) +from .pose_commands import ( + DeformableUniformPoseCommand, + DeformableUniformPoseCommandCfg, +) +from .rewards import ( + deformable_com_ee_distance, + deformable_com_goal_distance, + deformable_com_goal_distance_delta, + deformable_com_goal_reached, + deformable_ee_distance, + deformable_fingertip_distance, + deformable_lifted, + deformable_lifting, +) +from .terminations import ( + deformable_com_below_minimum, + deformable_nodal_vel_above_maximum, + deformable_outside_table_bounds, + deformable_state_invalid, + joint_vel_out_of_sim_limit, + robot_state_invalid, +) +from isaaclab_tasks.core.lift.mdp import * diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/curriculums.py b/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/curriculums.py new file mode 100644 index 000000000000..6c89fb35da09 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/curriculums.py @@ -0,0 +1,75 @@ +# 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 + +"""Curriculum functions for the deformable lift tasks.""" + +from __future__ import annotations + +from collections.abc import Sequence +from typing import TYPE_CHECKING + +import isaaclab.sim as sim_utils +from isaaclab.managers import CurriculumTermCfg, ManagerTermBase + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + + +class modify_gravity_linear(ManagerTermBase): + """Curriculum that linearly ramps the vertical gravity toward its target value. + + The vertical gravity component is interpolated from :paramref:`start_gravity_z` to + :paramref:`end_gravity_z` [m/s^2] as the global step counter advances from + :paramref:`start_step` to :paramref:`end_step`, then held constant. This lets the policy + first learn under near-weightless dynamics before full gravity is applied. + + The active physics backend is detected automatically (PhysX or Newton). + """ + + def __init__(self, cfg: CurriculumTermCfg, env: ManagerBasedRLEnv): + super().__init__(cfg, env) + + manager_name = env.sim.physics_manager.__name__.lower() + if "newton" in manager_name: + self._backend = "newton" + import isaaclab_newton.physics.newton_manager as newton_manager_module # noqa: PLC0415 + from newton import ModelFlags # noqa: PLC0415 + + self._newton_manager = newton_manager_module.NewtonManager + self._notify_model_properties = ModelFlags.MODEL_PROPERTIES + else: + self._backend = "physx" + import carb # noqa: PLC0415 + + self._carb = carb + self._physics_sim_view = sim_utils.SimulationContext.instance().physics_sim_view + + def __call__( + self, + env: ManagerBasedRLEnv, + env_ids: Sequence[int], + start_gravity_z: float, + end_gravity_z: float, + start_step: int, + end_step: int, + ) -> float: + # linearly interpolate the vertical gravity based on training progress + alpha = (env.common_step_counter - start_step) / max(end_step - start_step, 1) + alpha = min(max(alpha, 0.0), 1.0) + gravity_z = start_gravity_z + alpha * (end_gravity_z - start_gravity_z) + + if self._backend == "newton": + import warp as wp # noqa: PLC0415 + + model = self._newton_manager.get_model() + if model is None or model.gravity is None: + raise RuntimeError("Newton model is not initialized. Cannot modify gravity.") + # write to all worlds so gravity stays consistent regardless of per-env reset timing + wp.to_torch(model.gravity)[:, 2] = gravity_z + self._newton_manager.add_model_change(self._notify_model_properties) + else: + self._physics_sim_view.set_gravity(self._carb.Float3(0.0, 0.0, gravity_z)) + + return gravity_z diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/events.py b/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/events.py new file mode 100644 index 000000000000..89eb1d4cd3f3 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/events.py @@ -0,0 +1,181 @@ +# 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 + +"""Events for the deformable lift environments.""" + +from __future__ import annotations + +from typing import TYPE_CHECKING + +import torch +import warp as wp + +from isaaclab.managers import SceneEntityCfg +from isaaclab.utils import math as math_utils + +if TYPE_CHECKING: + from isaaclab.assets import DeformableObject, RigidObject + from isaaclab.envs import ManagerBasedEnv + + +@wp.kernel +def _write_tet_materials( + tet_indices: wp.array2d(dtype=wp.int32), + particle_offset0: wp.int32, + particles_per_body: wp.int32, + k_mu: wp.array(dtype=wp.float32), + k_lambda: wp.array(dtype=wp.float32), + tet_materials: wp.array2d(dtype=wp.float32), +): + """Write per-env Lame parameters into the shared tet-material array (leaves k_damp untouched).""" + t = wp.tid() + # Map the tet to its env via its first particle index (contiguous under replicate_physics). + e = (tet_indices[t, 0] - particle_offset0) // particles_per_body + tet_materials[t, 0] = k_mu[e] + tet_materials[t, 1] = k_lambda[e] + + +@wp.kernel +def _scale_particle_mass( + offsets: wp.array(dtype=wp.int32), + density_scale: wp.array(dtype=wp.float32), + spawn_mass: wp.array(dtype=wp.float32), + particle_mass: wp.array(dtype=wp.float32), + particle_inv_mass: wp.array(dtype=wp.float32), +): + """Scale free particle masses by the per-env density ratio; skip kinematic particles.""" + e, j = wp.tid() + flat_idx = offsets[e] + j + if particle_inv_mass[flat_idx] == 0.0: + return + m = spawn_mass[flat_idx] * density_scale[e] + particle_mass[flat_idx] = m + particle_inv_mass[flat_idx] = 1.0 / m + + +def randomize_deformable_material( + env: ManagerBasedEnv, + env_ids: torch.Tensor | None, + asset_cfg: SceneEntityCfg = SceneEntityCfg("deformable"), + youngs_modulus_range: tuple[float, float] = (7e4, 5e5), + density_range: tuple[float, float] = (100.0, 1000.0), + poissons_ratio: float = 0.25, +) -> None: + """Randomize the deformable object's material stiffness and density per environment. + + Startup event that samples a Young's modulus and density independently for every + environment instance and writes the corresponding Lame parameters into the Newton + tetrahedral materials and the particle masses. The tetrahedral damping term + ``k_damp`` is preserved. Kinematic particles keep their infinite mass. + + Args: + env: The environment instance. + env_ids: Unused; all instances are randomized at startup. + asset_cfg: Scene entity of the deformable object to randomize. + youngs_modulus_range: Sampling bounds for the Young's modulus [Pa]. + density_range: Sampling bounds for the mass density [kg/m^3]. + poissons_ratio: Poisson's ratio [dimensionless] used to convert to Lame parameters. + """ + # Imported here, not at module scope: pulling ``newton`` imports ``pxr`` and a second USD + # runtime, which must not happen before the Kit app has started. + from isaaclab_newton.physics import NewtonManager + from newton import ModelFlags + + asset = env.scene[asset_cfg.name] + device = env.device + num_instances = asset.num_instances + + model = NewtonManager.get_model() + if model is None: + return + + nu = poissons_ratio + + # Sample per-env Young's modulus and density, then convert E to Lame parameters. + youngs = torch.empty(num_instances, device=device).uniform_(*youngs_modulus_range) + density = torch.empty(num_instances, device=device).uniform_(*density_range) + k_mu = youngs / (2.0 * (1.0 + nu)) + k_lambda = youngs * nu / ((1.0 + nu) * (1.0 - 2.0 * nu)) + + k_mu_wp = wp.from_torch(k_mu.contiguous(), dtype=wp.float32) + k_lambda_wp = wp.from_torch(k_lambda.contiguous(), dtype=wp.float32) + + particle_offset0 = int(asset._recorded_particle_offsets[0]) + particles_per_body = asset._particles_per_body + + wp.launch( + _write_tet_materials, + dim=(model.tet_materials.shape[0],), + inputs=[model.tet_indices, particle_offset0, particles_per_body, k_mu_wp, k_lambda_wp], + outputs=[model.tet_materials], + device=device, + ) + + # Scale masses by density relative to the spawn baseline (spawn mass already encodes it). + spawn_density = asset.cfg.spawn.physics_material.density + density_scale = (density / spawn_density).contiguous() + density_scale_wp = wp.from_torch(density_scale, dtype=wp.float32) + spawn_mass = wp.clone(model.particle_mass) + + wp.launch( + _scale_particle_mass, + dim=(num_instances, particles_per_body), + inputs=[asset._particle_offsets, density_scale_wp, spawn_mass], + outputs=[model.particle_mass, model.particle_inv_mass], + device=device, + ) + + # Refresh the asset's cached inverse-mass snapshot used by the kinematic-target restore. + asset._default_particle_inv_mass = wp.clone(model.particle_inv_mass) + + # notify the solver that model properties changed, else the randomization is ignored + NewtonManager.add_model_change(ModelFlags.MODEL_PROPERTIES) + + +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 = math_utils.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(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 = math_utils.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/config/franka_soft/mdp/observations.py b/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/observations.py new file mode 100644 index 000000000000..c38ba710f7d2 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/observations.py @@ -0,0 +1,117 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Observation functions for the deformable lift tasks.""" + +from __future__ import annotations + +from collections.abc import Sequence +from typing import TYPE_CHECKING + +import torch + +from isaaclab.managers import ManagerTermBase, SceneEntityCfg +from isaaclab.utils.math import subtract_frame_transforms + +from .utils import _com_w, _nodal_pos_w + +if TYPE_CHECKING: + from isaaclab.assets import Articulation, DeformableObject + from isaaclab.envs import ManagerBasedRLEnv + from isaaclab.managers import ObservationTermCfg + + +def deformable_com_in_robot_root_frame( + env: ManagerBasedRLEnv, + asset_cfg: SceneEntityCfg = SceneEntityCfg("deformable"), + 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`). + + Returns: + Tensor of shape ``(num_envs, 3)``. + """ + asset: DeformableObject = env.scene[asset_cfg.name] + robot: Articulation = env.scene[robot_cfg.name] + 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. + + The point indices are sampled on reset, then reused within the episode so + each observed point follows the same material node over time. + """ + + def __init__(self, cfg: ObservationTermCfg, env: ManagerBasedRLEnv): + super().__init__(cfg, env) + + self.asset_cfg: SceneEntityCfg = cfg.params.get("asset_cfg", SceneEntityCfg("deformable")) + 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) + self.reset() + + def reset(self, env_ids: Sequence[int] | None = None) -> None: + """Resample observed deformable nodes 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 + ) + else: + self.node_ids[env_ids] = torch.randint(self.num_nodes, (num_envs, self.num_points), device=self.device) + + def __call__( + self, + env: ManagerBasedRLEnv, + asset_cfg: SceneEntityCfg = SceneEntityCfg("deformable"), + robot_cfg: SceneEntityCfg = SceneEntityCfg("robot"), + num_points: int = 20, + ) -> torch.Tensor: + """Sample deformable nodal positions in the robot's root frame. + + Args: + env: The environment instance. + asset_cfg: The deformable object entity. + robot_cfg: The robot entity providing the reference frame. + num_points: Number of sampled points. + + Returns: + 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] + 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}." + ) + + nodal_pos_w = _nodal_pos_w(asset) + sampled_points_w = nodal_pos_w.gather(1, self.node_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) + root_quat_w = robot.data.root_quat_w.torch.unsqueeze(1).expand(-1, num_points, -1) + sampled_points_b, _ = subtract_frame_transforms( + root_pos_w.reshape(-1, 3), + root_quat_w.reshape(-1, 4), + flat_sampled_points_w, + ) + return sampled_points_b.view(env.num_envs, -1) diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/pose_commands.py b/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/pose_commands.py new file mode 100644 index 000000000000..2cda5216a49c --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/pose_commands.py @@ -0,0 +1,82 @@ +# 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 + +"""Pose command terms for the deformable lift tasks.""" + +from __future__ import annotations + +from typing import TYPE_CHECKING + +import torch + +from isaaclab.assets import AssetBaseCfg +from isaaclab.utils.configclass import configclass +from isaaclab.utils.math import combine_frame_transforms + +from isaaclab_tasks.core.dexsuite.mdp.commands.pose_commands import ObjectUniformPoseCommand +from isaaclab_tasks.core.dexsuite.mdp.commands.pose_commands_cfg import ObjectUniformPoseCommandCfg + +if TYPE_CHECKING: + from isaaclab.assets import DeformableObject + from isaaclab.envs import ManagerBasedEnv + + +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. + + The success visualizer asset may be a static asset (``AssetBaseCfg``), which has no + runtime view. In that case its world position is the fixed spawn offset from the + environment origins. + """ + + 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) + + # static assets are stored as their config, so their world position is constant + 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 + + 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() + if self._static_success_vis_pos_w is not None: + vis_pos_w = self._static_success_vis_pos_w + else: + vis_pos_w = self.success_vis_asset.data.root_pos_w.torch + self.success_visualizer.visualize(vis_pos_w, marker_indices=success_id) + + +@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/config/franka_soft/mdp/rewards.py b/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/rewards.py new file mode 100644 index 000000000000..1a1e9a4df875 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/rewards.py @@ -0,0 +1,304 @@ +# 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 + +"""Reward functions for the deformable lift tasks.""" + +from __future__ import annotations + +from collections.abc import Sequence +from typing import TYPE_CHECKING + +import torch + +from isaaclab.managers import ManagerTermBase, RewardTermCfg, SceneEntityCfg +from isaaclab.utils.math import combine_frame_transforms + +from .utils import _body_pos_w, _com_w, _ee_pos_w, _nodal_pos_w + +if TYPE_CHECKING: + from isaaclab.assets import Articulation, DeformableObject + from isaaclab.envs import ManagerBasedRLEnv + from isaaclab.sensors import FrameTransformer + + +def deformable_lifted( + env: ManagerBasedRLEnv, + minimal_height: float, + asset_cfg: SceneEntityCfg = SceneEntityCfg("deformable"), +) -> torch.Tensor: + """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 = _com_w(asset)[:, 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 = _com_w(asset)[:, 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. + + 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 = _nodal_pos_w(asset) + ee_w = _ee_pos_w(ee_frame) + 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 = _com_w(asset) + ee_w = _ee_pos_w(ee_frame) + 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 = _com_w(asset).unsqueeze(1) + else: + target_w = _nodal_pos_w(asset) + # selected finger bodies in world frame: (num_envs, num_fingers, 3) + finger_pos_w = _body_pos_w(robot, 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) + + +class deformable_com_goal_distance(ManagerTermBase): + """Reward tracking of the goal position by the deformable's COM (tanh kernel). + + Only credits when the COM is above ``minimal_height`` [m] (i.e. the object is lifted). + The command is interpreted as ``[x, y, z, qw, qx, qy, qz]`` in the robot's root frame. + + If ``success_threshold`` is provided in the term params, this also tracks per-episode + success (sticky binary: COM ever within ``success_threshold`` [m] of the commanded goal + while lifted above ``minimal_height``) and logs the mean across environments under + ``Metrics/success_rate`` on reset. + """ + + def __init__(self, cfg: RewardTermCfg, env: ManagerBasedRLEnv): + super().__init__(cfg, env) + self._track_success = cfg.params.get("success_threshold") is not None + if self._track_success: + self._succeeded = torch.zeros(env.num_envs, dtype=torch.bool, device=env.device) + + def reset(self, env_ids: Sequence[int] | None = None): + if env_ids is None: + env_ids = slice(None) + if self._track_success: + 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, + robot_cfg: SceneEntityCfg = SceneEntityCfg("robot"), + asset_cfg: SceneEntityCfg = SceneEntityCfg("deformable"), + success_threshold: float | None = None, + ) -> torch.Tensor: + 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 = _com_w(asset) + distance = torch.linalg.norm(des_pos_w - com_w, dim=1) + is_lifted = com_w[:, 2] > minimal_height + if success_threshold is not None: + self._succeeded |= is_lifted & (distance < success_threshold) + return is_lifted.float() * (1.0 - torch.tanh(distance / std)) + + +class deformable_com_goal_distance_delta(ManagerTermBase): + """Reward progress of the deformable COM toward the commanded goal. + + Returns the per-step decrease in the COM-to-goal distance (previous minus current), + gated so it only credits while the COM is lifted above ``minimal_height`` [m]. The stored + distance is re-baselined on the first step after reset so the reset teleport does not + produce a spurious reward. Success tracking matches :class:`deformable_com_goal_distance`. + """ + + def __init__(self, cfg: RewardTermCfg, env: ManagerBasedRLEnv): + super().__init__(cfg, env) + self._prev_distance = torch.zeros(env.num_envs, device=env.device) + self._needs_baseline = torch.ones(env.num_envs, dtype=torch.bool, device=env.device) + self._track_success = cfg.params.get("success_threshold") is not None + if self._track_success: + self._succeeded = torch.zeros(env.num_envs, dtype=torch.bool, device=env.device) + + def reset(self, env_ids: Sequence[int] | None = None): + if env_ids is None: + env_ids = slice(None) + self._needs_baseline[env_ids] = True + if self._track_success: + 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, + minimal_height: float, + command_name: str, + robot_cfg: SceneEntityCfg = SceneEntityCfg("robot"), + asset_cfg: SceneEntityCfg = SceneEntityCfg("deformable"), + success_threshold: float | None = None, + ) -> torch.Tensor: + 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 = _com_w(asset) + distance = torch.linalg.norm(des_pos_w - com_w, dim=1) + self._prev_distance = torch.where(self._needs_baseline, distance, self._prev_distance) + self._needs_baseline[:] = False + is_lifted = com_w[:, 2] > minimal_height + if success_threshold is not None: + self._succeeded |= is_lifted & (distance < success_threshold) + delta = self._prev_distance - distance + self._prev_distance = distance + return is_lifted.float() * delta + + +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: + """Per-step success bonus for holding the deformable COM at the goal. + + Returns ``1.0`` while the COM is within ``success_threshold`` [m] of the commanded goal + position and lifted above ``minimal_height`` [m], else ``0.0``. Matches the condition + tracked as ``Metrics/success_rate`` in :class:`deformable_com_goal_distance`. + + Args: + env: The environment instance. + minimal_height: Minimum COM height for the bonus to apply [m]. + command_name: Name of the goal-pose command term. + success_threshold: Maximum COM-to-goal distance counted as success [m]. + robot_cfg: The robot entity providing the goal reference frame. + asset_cfg: The deformable object entity. + + Returns: + Reward tensor with shape ``(num_envs,)`` valued in ``{0, 1}``. + """ + 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 = _com_w(asset) + distance = torch.linalg.norm(des_pos_w - com_w, dim=1) + is_lifted = com_w[:, 2] > minimal_height + return (is_lifted & (distance < success_threshold)).float() diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/terminations.py b/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/terminations.py new file mode 100644 index 000000000000..65eb246e865c --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/terminations.py @@ -0,0 +1,158 @@ +# 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 + +"""Termination functions for the deformable lift tasks. + +The functions can be passed to the :class:`isaaclab.managers.TerminationTermCfg` object to enable +the termination introduced by the function. +""" + +from __future__ import annotations + +from typing import TYPE_CHECKING + +import torch + +from isaaclab.managers import SceneEntityCfg + +if TYPE_CHECKING: + from isaaclab.assets import Articulation, DeformableObject + from isaaclab.envs import ManagerBasedRLEnv + + +def deformable_com_below_minimum( + env: ManagerBasedRLEnv, + minimum_height: float, + asset_cfg: SceneEntityCfg = SceneEntityCfg("deformable"), +) -> torch.Tensor: + """Termination signal when the deformable's COM falls below ``minimum_height`` [m].""" + asset: DeformableObject = env.scene[asset_cfg.name] + com_z = asset.data.root_pos_w.torch[:, 2] + return com_z < minimum_height + + +def deformable_outside_table_bounds( + env: ManagerBasedRLEnv, + x_bounds: tuple[float, float], + y_bounds: tuple[float, float], + asset_cfg: SceneEntityCfg = SceneEntityCfg("deformable"), +) -> torch.Tensor: + """Terminate if any deformable nodal 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. + + Returns: + Boolean tensor with shape ``(num_envs,)``. + """ + asset: DeformableObject = env.scene[asset_cfg.name] + nodal_pos = asset.data.nodal_pos_w.torch - 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) + + +def deformable_nodal_vel_above_maximum( + env: ManagerBasedRLEnv, + maximum_velocity: float, + asset_cfg: SceneEntityCfg = SceneEntityCfg("deformable"), +) -> torch.Tensor: + """Terminate when any deformable node moves faster than ``maximum_velocity`` [m/s]. + + Guards against solver blow-up, where penalty contact ejects nodes at implausible speeds. + + Args: + env: The environment instance. + maximum_velocity: Maximum allowed nodal speed [m/s]. + asset_cfg: The deformable object entity. + + Returns: + Boolean tensor with shape ``(num_envs,)``. + """ + asset: DeformableObject = env.scene[asset_cfg.name] + speed = torch.linalg.norm(asset.data.nodal_vel_w.torch, dim=-1) + return speed.max(dim=1).values > maximum_velocity + + +def deformable_state_invalid( + env: ManagerBasedRLEnv, + asset_cfg: SceneEntityCfg = SceneEntityCfg("deformable"), + position_limit: float = 1.0e4, +) -> torch.Tensor: + """Terminate when the deformable state is no longer numerically valid. + + Guards against a diverging solve: once any node position or velocity turns non-finite, the + center of mass (the mean over nodes) is non-finite too, so every reward and observation reading + it is poisoned for the rest of the episode. Terminating resets the environment so training can + continue. + + This reads the raw state, unlike the sanitized accessors used by the reward and observation + terms, which would otherwise mask the divergence. Node positions beyond ``position_limit`` are + also flagged, since a blow-up passes through large finite values before it overflows to + infinity. Unlike :func:`deformable_outside_table_bounds`, which only checks x and y, this covers + all three axes. + + Args: + env: The environment instance. + asset_cfg: The deformable object entity. + position_limit: Maximum absolute nodal position component treated as valid [m]. + + Returns: + Boolean tensor with shape ``(num_envs,)``. + """ + asset: DeformableObject = env.scene[asset_cfg.name] + nodal_pos_w = asset.data.nodal_pos_w.torch + nodal_vel_w = asset.data.nodal_vel_w.torch + valid = torch.isfinite(nodal_pos_w).flatten(1).all(dim=1) + valid &= torch.isfinite(nodal_vel_w).flatten(1).all(dim=1) + valid &= (nodal_pos_w.abs() <= position_limit).flatten(1).all(dim=1) + return ~valid + + +def robot_state_invalid( + env: ManagerBasedRLEnv, + asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), + position_limit: float = 1.0e4, +) -> torch.Tensor: + """Terminate when the robot state is no longer numerically valid. + + Robot-side analogue of :func:`deformable_state_invalid`. The coupled rigid/soft solve can turn + an environment's whole state non-finite in a single step, and MuJoCo Warp warm-starts the next + substep from that state, so the environment stays dead until it is reset. The other robot + terminations fail open here, since ``abs(NaN) > limit`` and ``NaN < limit`` are both ``False``. + + This reads the raw state, unlike the sanitized accessors used by the reward and observation + terms, which would otherwise mask the divergence. + + Args: + env: The environment instance. + asset_cfg: The robot entity. + position_limit: Maximum absolute body position component treated as valid [m]. + + Returns: + Boolean tensor with shape ``(num_envs,)``. + """ + asset: Articulation = env.scene[asset_cfg.name] + body_pos_w = asset.data.body_pos_w.torch + valid = torch.isfinite(asset.data.joint_pos.torch).all(dim=1) + valid &= torch.isfinite(asset.data.joint_vel.torch).all(dim=1) + valid &= torch.isfinite(body_pos_w).flatten(1).all(dim=1) + valid &= (body_pos_w.abs() <= position_limit).flatten(1).all(dim=1) + return ~valid + + +def joint_vel_out_of_sim_limit( + env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot") +) -> torch.Tensor: + """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/isaaclab_tasks/core/lift/config/franka_soft/mdp/utils.py b/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/utils.py new file mode 100644 index 000000000000..8c8e1ce91e05 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/utils.py @@ -0,0 +1,95 @@ +# 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 + +"""Sanitized state accessors shared by the deformable lift MDP terms. + +The coupled rigid/soft solve can diverge and turn a whole environment's state non-finite. Measured +behaviour is a single-step event in one environment out of thousands: the robot joint state, the +robot body poses, the end-effector frame and every deformable node all become ``NaN`` at once, with +no growth in the preceding steps. Every reward or observation reading that state then returns +``NaN``, and RL libraries check the returned rewards and observations, so one diverged environment +aborts the whole run. + +Terminating on the divergence is not enough on its own. In +:meth:`~isaaclab.envs.ManagerBasedRLEnv.step` the reward manager runs after the termination manager +but *before* the environments are reset, so rewards for the terminating step are still computed from +the diverged state. Observations are computed after the reset and are normally clean, but the +pre-reset paths (an active recorder term, or ``compute_final_obs``) also read the diverged state. + +Reward terms, and the deformable observation terms, therefore read state through the helpers below, +which replace non-finite entries with ``0.0``. This places a diverged body at the world origin, +yielding a finite but meaningless value for exactly one step. That is intentional and acceptable, +because :func:`~isaaclab_tasks.core.lift.config.franka_soft.mdp.deformable_state_invalid` and +:func:`~isaaclab_tasks.core.lift.config.franka_soft.mdp.robot_state_invalid` flag the same step from +the raw state and the environment is reset immediately. + +The robot's root pose is deliberately left raw: the Franka is fixed-base, so body 0 is welded and +its transform has no joint-state dependence, keeping it finite while every descendant body goes +non-finite. A floating-base variant of this task would have to sanitize it too. +""" + +from __future__ import annotations + +from typing import TYPE_CHECKING + +import torch + +if TYPE_CHECKING: + from isaaclab.assets import Articulation, DeformableObject + from isaaclab.sensors import FrameTransformer + + +def _finite(value: torch.Tensor) -> torch.Tensor: + """Copy of ``value`` with every non-finite entry replaced by ``0.0``.""" + return torch.nan_to_num(value, nan=0.0, posinf=0.0, neginf=0.0) + + +def _com_w(asset: DeformableObject) -> torch.Tensor: + """Sanitized world-frame center of mass of a deformable object [m]. + + Args: + asset: The deformable object entity. + + Returns: + Tensor of shape ``(num_envs, 3)`` with non-finite entries replaced by ``0.0``. + """ + return _finite(asset.data.root_pos_w.torch) + + +def _nodal_pos_w(asset: DeformableObject) -> torch.Tensor: + """Sanitized world-frame nodal positions of a deformable object [m]. + + Args: + asset: The deformable object entity. + + Returns: + Tensor of shape ``(num_envs, num_nodes, 3)`` with non-finite entries replaced by ``0.0``. + """ + return _finite(asset.data.nodal_pos_w.torch) + + +def _body_pos_w(asset: Articulation, body_ids: slice | list[int]) -> torch.Tensor: + """Sanitized world-frame positions of the selected robot bodies [m]. + + Args: + asset: The articulation entity. + body_ids: Indices of the bodies to read. + + Returns: + Tensor of shape ``(num_envs, num_bodies, 3)`` with non-finite entries replaced by ``0.0``. + """ + return _finite(asset.data.body_pos_w.torch[:, body_ids]) + + +def _ee_pos_w(sensor: FrameTransformer) -> torch.Tensor: + """Sanitized world-frame position of the first target frame of a frame transformer [m]. + + Args: + sensor: The frame transformer sensor. + + Returns: + Tensor of shape ``(num_envs, 3)`` with non-finite entries replaced by ``0.0``. + """ + return _finite(sensor.data.target_pos_w.torch[..., 0, :]) From 32f647431968e262eb261a2e084e8caf034a3203 Mon Sep 17 00:00:00 2001 From: Mike Yan Michelis Date: Fri, 31 Jul 2026 19:56:38 +0200 Subject: [PATCH 2/6] Style: Minimize comments --- .../config/franka_soft/franka_soft_env_cfg.py | 28 ++++--------------- 1 file changed, 6 insertions(+), 22 deletions(-) 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 ea7664389116..74272210af1e 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 @@ -251,11 +251,7 @@ class _FrankaSoftSceneCfg(InteractiveSceneCfg): ) def __post_init__(self) -> None: - # Re-tuned Franka actuators: stiff arm gains with realistic armature so the low-inertia - # default gains do not let the fingers tunnel through the soft body, and a slower, weaker - # gripper so it settles on the beam surface instead of crushing it. Velocity limits are - # required by the joint_vel_out_of_sim_limit termination. Scoped here rather than in - # FRANKA_PANDA_CFG so the other Franka tasks keep the stock asset. + # Re-tuned Franka actuators, most importantly with realistic velocity limits. shoulder = self.robot.actuators["panda_shoulder"] shoulder.velocity_limit_sim = 2.175 shoulder.stiffness = 600.0 @@ -464,18 +460,8 @@ class EventCfg: @configclass class RewardsCfg: - """Deformable analogue of the winning rigid-cube lift recipe. - - The table top sits at z = 0 and the beam's rest COM at ~0.05 m. The dense lift reward gates at - 0.02 m, i.e. below rest, so it stays graded across the full lift range and only vanishes when - the beam is pressed into the table. The goal-tracking terms gate at 0.0 m, so they are - effectively always active. Success = COM within 5 cm of the goal position. - """ - - # The hand targets the COM so the grasp lands mid-beam (targeting the nearest node lets the - # gripper grab an end, which turns the beam into an unstable pole). The fingers target the - # nearest surface node instead: targeting the COM with both fingers is maximized only when - # both fingertips reach the object's center, i.e. by closing and indenting the beam. + """Lift-to-target reward for a deformable object.""" + reaching_deformable = RewTerm( func=mdp.deformable_com_ee_distance, params={"std": 0.1, "asset_cfg": SceneEntityCfg("deformable")}, @@ -529,13 +515,13 @@ 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-2, "num_steps": 40000} + func=mdp.modify_reward_weight, params={"term_name": "action_rate", "weight": -1e-2, "num_steps": 15000} ) # Since we use 24 steps per env, 10000 steps correspond to 10000/24 = 416.67 learning iterations gravity = CurrTerm( func=mdp.modify_gravity_linear, - params={"start_gravity_z": -0.0001, "end_gravity_z": -9.81, "start_step": 0, "end_step": 10000}, + params={"start_gravity_z": -0.0001, "end_gravity_z": -9.81, "start_step": 0, "end_step": 5000}, ) @@ -627,9 +613,7 @@ class FrankaSoftEnvCfg(ManagerBasedRLEnvCfg): # MDP settings rewards: RewardsCfg = RewardsCfg() terminations: TerminationsCfg = TerminationsCfg() - # Parent reset events + per-env material domain randomization. events: EventCfg = EventCfg() - # Ramp the action-rate penalty once the policy has learned to lift. curriculum: CurriculumCfg = CurriculumCfg() def __post_init__(self) -> None: @@ -660,5 +644,5 @@ class FrankaSoftCameraEnvCfg(FrankaSoftEnvCfg): def __post_init__(self) -> None: super().__post_init__() - # Warm up the RTX render product/annotator (Newton skips the PhysX assets_loading render loop). + # Warm up the RTX render product/annotator (Newton skips the PhysX assets_loading render loop), helps with passing rendering tests. self.num_rerenders_on_reset = 2 From b9cc2c93fe572ed43672fac17d42fd89fe724be9 Mon Sep 17 00:00:00 2001 From: Mike Yan Michelis Date: Fri, 31 Jul 2026 20:16:14 +0200 Subject: [PATCH 3/6] Style: Unnecessary invalidrobot state reset is removed (still trains without) --- .../franka_soft/agents/rsl_rl_ppo_cfg.py | 2 +- .../config/franka_soft/franka_soft_env_cfg.py | 8 ----- .../lift/config/franka_soft/mdp/__init__.pyi | 2 -- .../config/franka_soft/mdp/terminations.py | 32 ------------------- .../core/lift/config/franka_soft/mdp/utils.py | 5 ++- 5 files changed, 3 insertions(+), 46 deletions(-) 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 3150aa9b4782..15d0d85f78d7 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 @@ -55,7 +55,7 @@ class FrankaDeformablePPORunnerCfg(RslRlOnPolicyRunnerCfg): @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_soft_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/franka_soft_env_cfg.py index 74272210af1e..18618e2b9444 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 @@ -561,14 +561,6 @@ class TerminationsCfg: params={"asset_cfg": SceneEntityCfg("deformable")}, ) - # The measured divergence poisons the beam too, so deformable_invalid resets that case. This - # covers a robot-only divergence: every reward term is deformable-driven and sanitized, and the - # other robot terminations fail open on NaN, so nothing else would ever reset the environment. - robot_invalid = DoneTerm( - func=mdp.robot_state_invalid, - params={"asset_cfg": SceneEntityCfg("robot")}, - ) - ## # Environment configuration diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/__init__.pyi b/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/__init__.pyi index ac119215daa7..9911d8e295c0 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/__init__.pyi +++ b/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/__init__.pyi @@ -22,7 +22,6 @@ __all__ = [ "deformable_outside_table_bounds", "deformable_state_invalid", "joint_vel_out_of_sim_limit", - "robot_state_invalid", # events "randomize_deformable_material", "reset_deformable_over_support", @@ -59,6 +58,5 @@ from .terminations import ( deformable_outside_table_bounds, deformable_state_invalid, joint_vel_out_of_sim_limit, - robot_state_invalid, ) from isaaclab_tasks.core.lift.mdp import * diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/terminations.py b/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/terminations.py index 65eb246e865c..5ae6db57093a 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/terminations.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/terminations.py @@ -114,38 +114,6 @@ def deformable_state_invalid( return ~valid -def robot_state_invalid( - env: ManagerBasedRLEnv, - asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), - position_limit: float = 1.0e4, -) -> torch.Tensor: - """Terminate when the robot state is no longer numerically valid. - - Robot-side analogue of :func:`deformable_state_invalid`. The coupled rigid/soft solve can turn - an environment's whole state non-finite in a single step, and MuJoCo Warp warm-starts the next - substep from that state, so the environment stays dead until it is reset. The other robot - terminations fail open here, since ``abs(NaN) > limit`` and ``NaN < limit`` are both ``False``. - - This reads the raw state, unlike the sanitized accessors used by the reward and observation - terms, which would otherwise mask the divergence. - - Args: - env: The environment instance. - asset_cfg: The robot entity. - position_limit: Maximum absolute body position component treated as valid [m]. - - Returns: - Boolean tensor with shape ``(num_envs,)``. - """ - asset: Articulation = env.scene[asset_cfg.name] - body_pos_w = asset.data.body_pos_w.torch - valid = torch.isfinite(asset.data.joint_pos.torch).all(dim=1) - valid &= torch.isfinite(asset.data.joint_vel.torch).all(dim=1) - valid &= torch.isfinite(body_pos_w).flatten(1).all(dim=1) - valid &= (body_pos_w.abs() <= position_limit).flatten(1).all(dim=1) - return ~valid - - def joint_vel_out_of_sim_limit( env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot") ) -> torch.Tensor: diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/utils.py b/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/utils.py index 8c8e1ce91e05..c24590c4a908 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/utils.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/utils.py @@ -21,9 +21,8 @@ Reward terms, and the deformable observation terms, therefore read state through the helpers below, which replace non-finite entries with ``0.0``. This places a diverged body at the world origin, yielding a finite but meaningless value for exactly one step. That is intentional and acceptable, -because :func:`~isaaclab_tasks.core.lift.config.franka_soft.mdp.deformable_state_invalid` and -:func:`~isaaclab_tasks.core.lift.config.franka_soft.mdp.robot_state_invalid` flag the same step from -the raw state and the environment is reset immediately. +because :func:`~isaaclab_tasks.core.lift.config.franka_soft.mdp.deformable_state_invalid` flags the +same step from the raw state and the environment is reset immediately. The robot's root pose is deliberately left raw: the Franka is fixed-base, so body 0 is welded and its transform has no joint-state dependence, keeping it finite while every descendant body goes From 86df345bbe0c99b741947975488a4022cbba8970 Mon Sep 17 00:00:00 2001 From: Mike Yan Michelis Date: Fri, 31 Jul 2026 20:35:44 +0200 Subject: [PATCH 4/6] Style: Unify termination cfg --- .../config/franka_soft/franka_soft_env_cfg.py | 12 +++----- .../lift/config/franka_soft/mdp/__init__.pyi | 6 ++-- .../config/franka_soft/mdp/terminations.py | 29 +++++++------------ 3 files changed, 17 insertions(+), 30 deletions(-) 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 18618e2b9444..59167f0d70a7 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 @@ -527,24 +527,20 @@ class CurriculumCfg: @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")}, diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/__init__.pyi b/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/__init__.pyi index 9911d8e295c0..9e95d647564d 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/__init__.pyi +++ b/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/__init__.pyi @@ -17,9 +17,8 @@ __all__ = [ "deformable_com_goal_distance_delta", "deformable_com_goal_reached", # terminations - "deformable_com_below_minimum", "deformable_nodal_vel_above_maximum", - "deformable_outside_table_bounds", + "deformable_outside_bounds", "deformable_state_invalid", "joint_vel_out_of_sim_limit", # events @@ -53,9 +52,8 @@ from .rewards import ( deformable_lifting, ) from .terminations import ( - deformable_com_below_minimum, deformable_nodal_vel_above_maximum, - deformable_outside_table_bounds, + deformable_outside_bounds, deformable_state_invalid, joint_vel_out_of_sim_limit, ) diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/terminations.py b/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/terminations.py index 5ae6db57093a..ab04713cca25 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/terminations.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/terminations.py @@ -22,29 +22,22 @@ from isaaclab.envs import ManagerBasedRLEnv -def deformable_com_below_minimum( - env: ManagerBasedRLEnv, - minimum_height: float, - asset_cfg: SceneEntityCfg = SceneEntityCfg("deformable"), -) -> torch.Tensor: - """Termination signal when the deformable's COM falls below ``minimum_height`` [m].""" - asset: DeformableObject = env.scene[asset_cfg.name] - com_z = asset.data.root_pos_w.torch[:, 2] - return com_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: - """Terminate if any deformable nodal point leaves the table footprint. + """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: @@ -52,9 +45,9 @@ def deformable_outside_table_bounds( """ asset: DeformableObject = env.scene[asset_cfg.name] nodal_pos = asset.data.nodal_pos_w.torch - 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) + 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 deformable_nodal_vel_above_maximum( @@ -94,8 +87,8 @@ def deformable_state_invalid( This reads the raw state, unlike the sanitized accessors used by the reward and observation terms, which would otherwise mask the divergence. Node positions beyond ``position_limit`` are also flagged, since a blow-up passes through large finite values before it overflows to - infinity. Unlike :func:`deformable_outside_table_bounds`, which only checks x and y, this covers - all three axes. + infinity. Unlike :func:`deformable_outside_bounds`, whose limits are task bounds, this one is a + numerical sanity check. Args: env: The environment instance. From f30618e920af144572d443ccf0de22db026c51d4 Mon Sep 17 00:00:00 2001 From: Mike Yan Michelis Date: Fri, 31 Jul 2026 20:46:50 +0200 Subject: [PATCH 5/6] Style: Unnecessary deformable invalid state --- .../config/franka_soft/franka_soft_env_cfg.py | 6 ---- .../lift/config/franka_soft/mdp/__init__.pyi | 2 -- .../config/franka_soft/mdp/terminations.py | 35 ------------------- .../core/lift/config/franka_soft/mdp/utils.py | 13 +++---- 4 files changed, 4 insertions(+), 52 deletions(-) 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 59167f0d70a7..66f31cc49101 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 @@ -551,12 +551,6 @@ class TerminationsCfg: params={"asset_cfg": SceneEntityCfg("robot")}, ) - # real failure, not a time out: a diverged solve must bootstrap as a termination - deformable_invalid = DoneTerm( - func=mdp.deformable_state_invalid, - params={"asset_cfg": SceneEntityCfg("deformable")}, - ) - ## # Environment configuration diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/__init__.pyi b/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/__init__.pyi index 9e95d647564d..dc8132a8f4cf 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/__init__.pyi +++ b/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/__init__.pyi @@ -19,7 +19,6 @@ __all__ = [ # terminations "deformable_nodal_vel_above_maximum", "deformable_outside_bounds", - "deformable_state_invalid", "joint_vel_out_of_sim_limit", # events "randomize_deformable_material", @@ -54,7 +53,6 @@ from .rewards import ( from .terminations import ( deformable_nodal_vel_above_maximum, deformable_outside_bounds, - deformable_state_invalid, joint_vel_out_of_sim_limit, ) from isaaclab_tasks.core.lift.mdp import * diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/terminations.py b/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/terminations.py index ab04713cca25..e3905aab3c75 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/terminations.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/terminations.py @@ -72,41 +72,6 @@ def deformable_nodal_vel_above_maximum( return speed.max(dim=1).values > maximum_velocity -def deformable_state_invalid( - env: ManagerBasedRLEnv, - asset_cfg: SceneEntityCfg = SceneEntityCfg("deformable"), - position_limit: float = 1.0e4, -) -> torch.Tensor: - """Terminate when the deformable state is no longer numerically valid. - - Guards against a diverging solve: once any node position or velocity turns non-finite, the - center of mass (the mean over nodes) is non-finite too, so every reward and observation reading - it is poisoned for the rest of the episode. Terminating resets the environment so training can - continue. - - This reads the raw state, unlike the sanitized accessors used by the reward and observation - terms, which would otherwise mask the divergence. Node positions beyond ``position_limit`` are - also flagged, since a blow-up passes through large finite values before it overflows to - infinity. Unlike :func:`deformable_outside_bounds`, whose limits are task bounds, this one is a - numerical sanity check. - - Args: - env: The environment instance. - asset_cfg: The deformable object entity. - position_limit: Maximum absolute nodal position component treated as valid [m]. - - Returns: - Boolean tensor with shape ``(num_envs,)``. - """ - asset: DeformableObject = env.scene[asset_cfg.name] - nodal_pos_w = asset.data.nodal_pos_w.torch - nodal_vel_w = asset.data.nodal_vel_w.torch - valid = torch.isfinite(nodal_pos_w).flatten(1).all(dim=1) - valid &= torch.isfinite(nodal_vel_w).flatten(1).all(dim=1) - valid &= (nodal_pos_w.abs() <= position_limit).flatten(1).all(dim=1) - return ~valid - - def joint_vel_out_of_sim_limit( env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot") ) -> torch.Tensor: diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/utils.py b/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/utils.py index c24590c4a908..d04d4fec372b 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/utils.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/utils.py @@ -12,17 +12,12 @@ ``NaN``, and RL libraries check the returned rewards and observations, so one diverged environment aborts the whole run. -Terminating on the divergence is not enough on its own. In -:meth:`~isaaclab.envs.ManagerBasedRLEnv.step` the reward manager runs after the termination manager -but *before* the environments are reset, so rewards for the terminating step are still computed from -the diverged state. Observations are computed after the reset and are normally clean, but the -pre-reset paths (an active recorder term, or ``compute_final_obs``) also read the diverged state. - Reward terms, and the deformable observation terms, therefore read state through the helpers below, which replace non-finite entries with ``0.0``. This places a diverged body at the world origin, -yielding a finite but meaningless value for exactly one step. That is intentional and acceptable, -because :func:`~isaaclab_tasks.core.lift.config.franka_soft.mdp.deformable_state_invalid` flags the -same step from the raw state and the environment is reset immediately. +yielding finite but meaningless values. The task has no termination on numerical validity: the +bounds terminations fail open on ``NaN`` (both ``NaN < lower`` and ``NaN > upper`` are ``False``), +so a diverged environment runs to its time out and is reset there. Training tolerates this, since +the event is rare and the sanitized rewards stay finite throughout. The robot's root pose is deliberately left raw: the Franka is fixed-base, so body 0 is welded and its transform has no joint-state dependence, keeping it finite while every descendant body goes From fd44b981c7d0df1e8778d7c1cd3d09c73fc90fc1 Mon Sep 17 00:00:00 2001 From: Mike Yan Michelis Date: Mon, 3 Aug 2026 13:52:32 +0200 Subject: [PATCH 6/6] Fix: Policy still trains without delta position reward --- .../config/franka_soft/franka_soft_env_cfg.py | 10 ---- .../lift/config/franka_soft/mdp/__init__.pyi | 4 -- .../lift/config/franka_soft/mdp/rewards.py | 54 ------------------- .../config/franka_soft/mdp/terminations.py | 22 -------- 4 files changed, 90 deletions(-) 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 66f31cc49101..53b4e04aae3d 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 @@ -474,16 +474,6 @@ class RewardsCfg: weight=5.0, ) - deformable_goal_tracking_delta = RewTerm( - func=mdp.deformable_com_goal_distance_delta, - params={ - "minimal_height": 0.0, - "command_name": "deformable_pose", - "asset_cfg": SceneEntityCfg("deformable"), - }, - weight=500.0, - ) - deformable_goal_tracking = RewTerm( func=mdp.deformable_com_goal_distance, params={ diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/__init__.pyi b/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/__init__.pyi index dc8132a8f4cf..0cf4f671172e 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/__init__.pyi +++ b/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/__init__.pyi @@ -14,10 +14,8 @@ __all__ = [ "deformable_com_ee_distance", "deformable_fingertip_distance", "deformable_com_goal_distance", - "deformable_com_goal_distance_delta", "deformable_com_goal_reached", # terminations - "deformable_nodal_vel_above_maximum", "deformable_outside_bounds", "joint_vel_out_of_sim_limit", # events @@ -43,7 +41,6 @@ from .pose_commands import ( from .rewards import ( deformable_com_ee_distance, deformable_com_goal_distance, - deformable_com_goal_distance_delta, deformable_com_goal_reached, deformable_ee_distance, deformable_fingertip_distance, @@ -51,7 +48,6 @@ from .rewards import ( deformable_lifting, ) from .terminations import ( - deformable_nodal_vel_above_maximum, deformable_outside_bounds, joint_vel_out_of_sim_limit, ) diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/rewards.py b/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/rewards.py index 1a1e9a4df875..bf76c35c28ee 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/rewards.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/rewards.py @@ -215,60 +215,6 @@ def __call__( return is_lifted.float() * (1.0 - torch.tanh(distance / std)) -class deformable_com_goal_distance_delta(ManagerTermBase): - """Reward progress of the deformable COM toward the commanded goal. - - Returns the per-step decrease in the COM-to-goal distance (previous minus current), - gated so it only credits while the COM is lifted above ``minimal_height`` [m]. The stored - distance is re-baselined on the first step after reset so the reset teleport does not - produce a spurious reward. Success tracking matches :class:`deformable_com_goal_distance`. - """ - - def __init__(self, cfg: RewardTermCfg, env: ManagerBasedRLEnv): - super().__init__(cfg, env) - self._prev_distance = torch.zeros(env.num_envs, device=env.device) - self._needs_baseline = torch.ones(env.num_envs, dtype=torch.bool, device=env.device) - self._track_success = cfg.params.get("success_threshold") is not None - if self._track_success: - self._succeeded = torch.zeros(env.num_envs, dtype=torch.bool, device=env.device) - - def reset(self, env_ids: Sequence[int] | None = None): - if env_ids is None: - env_ids = slice(None) - self._needs_baseline[env_ids] = True - if self._track_success: - 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, - minimal_height: float, - command_name: str, - robot_cfg: SceneEntityCfg = SceneEntityCfg("robot"), - asset_cfg: SceneEntityCfg = SceneEntityCfg("deformable"), - success_threshold: float | None = None, - ) -> torch.Tensor: - 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 = _com_w(asset) - distance = torch.linalg.norm(des_pos_w - com_w, dim=1) - self._prev_distance = torch.where(self._needs_baseline, distance, self._prev_distance) - self._needs_baseline[:] = False - is_lifted = com_w[:, 2] > minimal_height - if success_threshold is not None: - self._succeeded |= is_lifted & (distance < success_threshold) - delta = self._prev_distance - distance - self._prev_distance = distance - return is_lifted.float() * delta - - def deformable_com_goal_reached( env: ManagerBasedRLEnv, minimal_height: float, diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/terminations.py b/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/terminations.py index e3905aab3c75..1a27cb071d84 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/terminations.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/lift/config/franka_soft/mdp/terminations.py @@ -50,28 +50,6 @@ def deformable_outside_bounds( return ((nodal_pos < lower) | (nodal_pos > upper)).flatten(1).any(dim=1) -def deformable_nodal_vel_above_maximum( - env: ManagerBasedRLEnv, - maximum_velocity: float, - asset_cfg: SceneEntityCfg = SceneEntityCfg("deformable"), -) -> torch.Tensor: - """Terminate when any deformable node moves faster than ``maximum_velocity`` [m/s]. - - Guards against solver blow-up, where penalty contact ejects nodes at implausible speeds. - - Args: - env: The environment instance. - maximum_velocity: Maximum allowed nodal speed [m/s]. - asset_cfg: The deformable object entity. - - Returns: - Boolean tensor with shape ``(num_envs,)``. - """ - asset: DeformableObject = env.scene[asset_cfg.name] - speed = torch.linalg.norm(asset.data.nodal_vel_w.torch, dim=-1) - return speed.max(dim=1).values > maximum_velocity - - def joint_vel_out_of_sim_limit( env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot") ) -> torch.Tensor: