diff --git a/source/isaaclab_assets/changelog.d/task-cleanup-dex-part03.minor.rst b/source/isaaclab_assets/changelog.d/task-cleanup-dex-part03.minor.rst new file mode 100644 index 000000000000..6c8250b50c79 --- /dev/null +++ b/source/isaaclab_assets/changelog.d/task-cleanup-dex-part03.minor.rst @@ -0,0 +1,9 @@ +Added +^^^^^ + +* Added the dexterous-hand actuated-joint and fingertip body-name lists + (:obj:`~isaaclab_assets.robots.shadow_hand.SHADOW_ACTUATED_JOINT_NAMES`, + :obj:`~isaaclab_assets.robots.shadow_hand.SHADOW_FINGERTIP_BODY_NAMES`, + :obj:`~isaaclab_assets.robots.allegro.ALLEGRO_ACTUATED_JOINT_NAMES`, + :obj:`~isaaclab_assets.robots.allegro.ALLEGRO_FINGERTIP_BODY_NAMES`) to the + robot asset modules so tasks can reference them from a single source. diff --git a/source/isaaclab_assets/isaaclab_assets/robots/allegro.py b/source/isaaclab_assets/isaaclab_assets/robots/allegro.py index 10d96e277770..b2669f2dd283 100644 --- a/source/isaaclab_assets/isaaclab_assets/robots/allegro.py +++ b/source/isaaclab_assets/isaaclab_assets/robots/allegro.py @@ -66,3 +66,32 @@ soft_joint_pos_limit_factor=1.0, ) """Configuration of Allegro Hand robot.""" + + +ALLEGRO_FINGERTIP_BODY_NAMES: list[str] = [ + "index_link_3", + "middle_link_3", + "ring_link_3", + "thumb_link_3", +] +"""Allegro Hand fingertip body names.""" + +ALLEGRO_ACTUATED_JOINT_NAMES: list[str] = [ + "index_joint_0", + "middle_joint_0", + "ring_joint_0", + "thumb_joint_0", + "index_joint_1", + "index_joint_2", + "index_joint_3", + "middle_joint_1", + "middle_joint_2", + "middle_joint_3", + "ring_joint_1", + "ring_joint_2", + "ring_joint_3", + "thumb_joint_1", + "thumb_joint_2", + "thumb_joint_3", +] +"""Allegro Hand actuated joint names, in the Direct task's actuation order.""" diff --git a/source/isaaclab_assets/isaaclab_assets/robots/shadow_hand.py b/source/isaaclab_assets/isaaclab_assets/robots/shadow_hand.py index 696b8f9b5a4d..b348cc32a57e 100644 --- a/source/isaaclab_assets/isaaclab_assets/robots/shadow_hand.py +++ b/source/isaaclab_assets/isaaclab_assets/robots/shadow_hand.py @@ -82,3 +82,37 @@ soft_joint_pos_limit_factor=1.0, ) """Configuration of Shadow Hand robot.""" + + +SHADOW_FINGERTIP_BODY_NAMES: list[str] = [ + "robot0_ffdistal", + "robot0_mfdistal", + "robot0_rfdistal", + "robot0_lfdistal", + "robot0_thdistal", +] +"""Shadow Hand fingertip body names (identical on every backend asset).""" + +SHADOW_ACTUATED_JOINT_NAMES: list[str] = [ + "robot0_WRJ1", + "robot0_WRJ0", + "robot0_FFJ3", + "robot0_FFJ2", + "robot0_FFJ1", + "robot0_MFJ3", + "robot0_MFJ2", + "robot0_MFJ1", + "robot0_RFJ3", + "robot0_RFJ2", + "robot0_RFJ1", + "robot0_LFJ4", + "robot0_LFJ3", + "robot0_LFJ2", + "robot0_LFJ1", + "robot0_THJ4", + "robot0_THJ3", + "robot0_THJ2", + "robot0_THJ1", + "robot0_THJ0", +] +"""Shadow Hand actuated joint names, in the Direct task's actuation order.""" diff --git a/source/isaaclab_tasks/changelog.d/task-cleanup-dex-part03.minor.rst b/source/isaaclab_tasks/changelog.d/task-cleanup-dex-part03.minor.rst new file mode 100644 index 000000000000..9a453bb3de5d --- /dev/null +++ b/source/isaaclab_tasks/changelog.d/task-cleanup-dex-part03.minor.rst @@ -0,0 +1,13 @@ +Added +^^^^^ + +* Added behavioral-success metrics and threshold-independent episode-error + diagnostics to the dexterous reorientation environments. + +Fixed +^^^^^ + +* Fixed dexterous hand resets that could initialize joints below their lower + position limits. Reset joint positions now sample uniformly across the full + joint range; previously the distribution was biased toward the lower half of + the range. diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/handover/handover_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/core/handover/handover_env_cfg.py index a3d7126ce0bd..bbf23f6204e0 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/handover/handover_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/handover/handover_env_cfg.py @@ -19,7 +19,7 @@ from isaaclab.sim.spawners.materials.physics_materials_cfg import RigidBodyMaterialCfg from isaaclab.utils.configclass import configclass -from isaaclab_tasks.core.reorient.config.shadow_hand.shadow_hand_env_cfg import ShadowHandRobotCfg +from isaaclab_tasks.core.reorient.config.shadow_hand.shadow_hand_common import ShadowHandRobotCfg from isaaclab_tasks.utils import PresetCfg, preset from isaaclab_assets.robots.shadow_hand import SHADOW_HAND_CFG diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/reorient/config/allegro_hand/allegro_hand_common.py b/source/isaaclab_tasks/isaaclab_tasks/core/reorient/config/allegro_hand/allegro_hand_common.py new file mode 100644 index 000000000000..a3ed24dc514d --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/core/reorient/config/allegro_hand/allegro_hand_common.py @@ -0,0 +1,114 @@ +# 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 + +"""Allegro Hand identity shared by the Direct and manager-based reorientation tasks. + +Asset and marker configurations, joint/body name lists, backend physics +presets, and the sim mixin. No task tunables. +""" + +from isaaclab_newton.physics import MJWarpSolverCfg, NewtonCfg +from isaaclab_ovphysx.physics import OvPhysxCfg +from isaaclab_physx.physics import PhysxCfg + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, RigidObjectCfg +from isaaclab.markers import VisualizationMarkersCfg +from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR +from isaaclab.utils.configclass import configclass + +from isaaclab_tasks.utils import PresetCfg + +from isaaclab_assets.robots.allegro import ALLEGRO_HAND_CFG + + +@configclass +class ObjectCfg(PresetCfg): + physx = RigidObjectCfg( + prim_path="/World/envs/env_.*/object", + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Blocks/DexCube/dex_cube_instanceable.usd", + rigid_props=sim_utils.RigidBodyPropertiesCfg( + kinematic_enabled=False, + disable_gravity=False, + enable_gyroscopic_forces=True, + solver_position_iteration_count=8, + solver_velocity_iteration_count=0, + sleep_threshold=0.005, + stabilization_threshold=0.0025, + max_depenetration_velocity=1000.0, + ), + mass_props=sim_utils.MassPropertiesCfg(density=400.0), + scale=(1.2, 1.2, 1.2), + ), + init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, -0.17, 0.56), rot=(0.0, 0.0, 0.0, 1.0)), + ) + newton_mjwarp = ArticulationCfg( + prim_path="/World/envs/env_.*/object", + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Blocks/DexCube/dex_cube_instanceable.usd", + mass_props=sim_utils.MassPropertiesCfg(density=400.0), + scale=(1.2, 1.2, 1.2), + ), + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.0, -0.17, 0.565), rot=(0.0, 0.0, 0.0, 1.0), joint_pos={}, joint_vel={} + ), + actuators={}, + articulation_root_prim_path="", + ) + ovphysx = RigidObjectCfg( + prim_path="/World/envs/env_.*/object", + spawn=sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Blocks/DexCube/dex_cube_instanceable.usd", + rigid_props=sim_utils.RigidBodyPropertiesCfg( + kinematic_enabled=False, + disable_gravity=False, + enable_gyroscopic_forces=True, + solver_position_iteration_count=8, + solver_velocity_iteration_count=0, + sleep_threshold=0.005, + stabilization_threshold=0.0025, + max_depenetration_velocity=1000.0, + ), + mass_props=sim_utils.MassPropertiesCfg(density=400.0), + scale=(1.2, 1.2, 1.2), + ), + init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, -0.17, 0.56), rot=(0.0, 0.0, 0.0, 1.0)), + ) + default = newton_mjwarp + + +@configclass +class PhysicsCfg(PresetCfg): + physx = PhysxCfg( + bounce_threshold_velocity=0.2, + ) + newton_mjwarp = NewtonCfg( + solver_cfg=MJWarpSolverCfg( + integrator="implicitfast", + njmax=80, + nconmax=70, + impratio=10.0, + cone="elliptic", + update_data_interval=2, + ), + num_substeps=2, + ) + ovphysx = OvPhysxCfg() + default = newton_mjwarp + + +# Scene pieces shared verbatim by the manager-based variant. +ROBOT_CFG = ALLEGRO_HAND_CFG.replace(prim_path="/World/envs/env_.*/Robot") +OBJECT_CFG = ObjectCfg() +GOAL_OBJECT_CFG = VisualizationMarkersCfg( + prim_path="/Visuals/goal_marker", + markers={ + "goal": sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Blocks/DexCube/dex_cube_instanceable.usd", + scale=(1.2, 1.2, 1.2), + ) + }, +) diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/reorient/config/allegro_hand/allegro_hand_direct_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/core/reorient/config/allegro_hand/allegro_hand_direct_env_cfg.py index 8f32326763f6..66c9ab696fb8 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/reorient/config/allegro_hand/allegro_hand_direct_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/reorient/config/allegro_hand/allegro_hand_direct_env_cfg.py @@ -3,104 +3,23 @@ # # SPDX-License-Identifier: BSD-3-Clause - -from isaaclab_newton.physics import MJWarpSolverCfg, NewtonCfg -from isaaclab_ovphysx.physics import OvPhysxCfg -from isaaclab_physx.physics import PhysxCfg - -import isaaclab.sim as sim_utils -from isaaclab.assets import ArticulationCfg, RigidObjectCfg +from isaaclab.assets import ArticulationCfg from isaaclab.envs import DirectRLEnvCfg from isaaclab.markers import VisualizationMarkersCfg from isaaclab.scene import InteractiveSceneCfg from isaaclab.sim import SimulationCfg -from isaaclab.sim.spawners.materials.physics_materials_cfg import RigidBodyMaterialCfg -from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR +from isaaclab.sim.spawners.materials import RigidBodyMaterialBaseCfg from isaaclab.utils.configclass import configclass -from isaaclab_tasks.utils import PresetCfg - -from isaaclab_assets.robots.allegro import ALLEGRO_HAND_CFG - +from isaaclab_tasks.core.reorient.config.allegro_hand.allegro_hand_common import ( + GOAL_OBJECT_CFG, + OBJECT_CFG, + ROBOT_CFG, + ObjectCfg, + PhysicsCfg, +) -@configclass -class ObjectCfg(PresetCfg): - physx = RigidObjectCfg( - prim_path="/World/envs/env_.*/object", - spawn=sim_utils.UsdFileCfg( - usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Blocks/DexCube/dex_cube_instanceable.usd", - rigid_props=sim_utils.RigidBodyPropertiesCfg( - kinematic_enabled=False, - disable_gravity=False, - enable_gyroscopic_forces=True, - solver_position_iteration_count=8, - solver_velocity_iteration_count=0, - sleep_threshold=0.005, - stabilization_threshold=0.0025, - max_depenetration_velocity=1000.0, - ), - mass_props=sim_utils.MassPropertiesCfg(density=400.0), - scale=(1.2, 1.2, 1.2), - ), - init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, -0.17, 0.56), rot=(0.0, 0.0, 0.0, 1.0)), - ) - newton_mjwarp = ArticulationCfg( - prim_path="/World/envs/env_.*/object", - spawn=sim_utils.UsdFileCfg( - usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Blocks/DexCube/dex_cube_instanceable.usd", - mass_props=sim_utils.MassPropertiesCfg(density=400.0), - scale=(1.2, 1.2, 1.2), - ), - init_state=ArticulationCfg.InitialStateCfg( - pos=(0.0, -0.17, 0.565), rot=(0.0, 0.0, 0.0, 1.0), joint_pos={}, joint_vel={} - ), - actuators={}, - articulation_root_prim_path="", - ) - ovphysx = RigidObjectCfg( - prim_path="/World/envs/env_.*/object", - spawn=sim_utils.UsdFileCfg( - usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Blocks/DexCube/dex_cube_instanceable.usd", - rigid_props=sim_utils.RigidBodyPropertiesCfg( - kinematic_enabled=False, - disable_gravity=False, - enable_gyroscopic_forces=True, - solver_position_iteration_count=8, - solver_velocity_iteration_count=0, - sleep_threshold=0.005, - stabilization_threshold=0.0025, - max_depenetration_velocity=1000.0, - ), - mass_props=sim_utils.MassPropertiesCfg(density=400.0), - scale=(1.2, 1.2, 1.2), - ), - init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, -0.17, 0.56), rot=(0.0, 0.0, 0.0, 1.0)), - ) - default = physx - - -@configclass -class PhysicsCfg(PresetCfg): - physx = PhysxCfg( - bounce_threshold_velocity=0.2, - ) - newton_mjwarp = NewtonCfg( - solver_cfg=MJWarpSolverCfg( - solver="newton", - integrator="implicitfast", - njmax=80, - nconmax=70, - impratio=10.0, - cone="elliptic", - update_data_interval=2, - iterations=100, - # save_to_mjcf="AllegroHand.xml", - ), - num_substeps=2, - debug_mode=False, - ) - ovphysx = OvPhysxCfg() - default = physx +from isaaclab_assets.robots.allegro import ALLEGRO_ACTUATED_JOINT_NAMES, ALLEGRO_FINGERTIP_BODY_NAMES @configclass @@ -113,59 +32,30 @@ class AllegroHandEnvCfg(DirectRLEnvCfg): state_space = 0 asymmetric_obs = False obs_type = "full" - # simulation + + # simulation — values mirrored by the manager cfg (guarded by the value-parity test) sim: SimulationCfg = SimulationCfg( dt=1 / 120, - render_interval=decimation, - physics_material=RigidBodyMaterialCfg( - static_friction=1.0, - dynamic_friction=1.0, - ), + render_interval=4, + physics_material=RigidBodyMaterialBaseCfg(static_friction=1.0, dynamic_friction=1.0), physics=PhysicsCfg(), ) # robot - robot_cfg: ArticulationCfg = ALLEGRO_HAND_CFG.replace(prim_path="/World/envs/env_.*/Robot") + robot_cfg: ArticulationCfg = ROBOT_CFG - actuated_joint_names = [ - "index_joint_0", - "middle_joint_0", - "ring_joint_0", - "thumb_joint_0", - "index_joint_1", - "index_joint_2", - "index_joint_3", - "middle_joint_1", - "middle_joint_2", - "middle_joint_3", - "ring_joint_1", - "ring_joint_2", - "ring_joint_3", - "thumb_joint_1", - "thumb_joint_2", - "thumb_joint_3", - ] - fingertip_body_names = [ - "index_link_3", - "middle_link_3", - "ring_link_3", - "thumb_link_3", - ] + actuated_joint_names = ALLEGRO_ACTUATED_JOINT_NAMES + fingertip_body_names = ALLEGRO_FINGERTIP_BODY_NAMES # in-hand object - object_cfg: ObjectCfg = ObjectCfg() + object_cfg: ObjectCfg = OBJECT_CFG # goal object - goal_object_cfg: VisualizationMarkersCfg = VisualizationMarkersCfg( - prim_path="/Visuals/goal_marker", - markers={ - "goal": sim_utils.UsdFileCfg( - usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Blocks/DexCube/dex_cube_instanceable.usd", - scale=(1.2, 1.2, 1.2), - ) - }, - ) + goal_object_cfg: VisualizationMarkersCfg = GOAL_OBJECT_CFG # scene scene: InteractiveSceneCfg = InteractiveSceneCfg( - num_envs=8192, env_spacing=0.75, replicate_physics=True, clone_in_fabric=True + num_envs=8192, + env_spacing=0.75, + replicate_physics=True, + clone_in_fabric=True, ) # reset reset_position_noise = 0.01 # range of position at reset @@ -176,8 +66,8 @@ class AllegroHandEnvCfg(DirectRLEnvCfg): rot_reward_scale = 1.0 rot_eps = 0.1 action_penalty_scale = -0.0002 - reach_goal_bonus = 250 - fall_penalty = 0 + reach_goal_bonus = 250.0 + fall_penalty = 0.0 fall_dist = 0.24 vel_obs_scale = 0.2 success_tolerance = 0.2 diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/reorient/config/shadow_hand/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/core/reorient/config/shadow_hand/__init__.py index 5135565424d8..cd1fc9220363 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/reorient/config/shadow_hand/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/reorient/config/shadow_hand/__init__.py @@ -22,7 +22,7 @@ entry_point=reorient_direct_entry, disable_env_checker=True, kwargs={ - "env_cfg_entry_point": f"{__name__}.shadow_hand_env_cfg:ShadowHandEnvCfg", + "env_cfg_entry_point": f"{__name__}.shadow_hand_direct_env_cfg:ShadowHandEnvCfg", "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_cfg.yaml", "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:ShadowHandPPORunnerCfg", "skrl_cfg_entry_point": f"{agents.__name__}:skrl_ppo_cfg.yaml", @@ -34,7 +34,7 @@ entry_point=reorient_direct_entry, disable_env_checker=True, kwargs={ - "env_cfg_entry_point": f"{__name__}.shadow_hand_env_cfg:ShadowHandOpenAIEnvCfg", + "env_cfg_entry_point": f"{__name__}.shadow_hand_direct_env_cfg:ShadowHandOpenAIEnvCfg", "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_ff_cfg.yaml", "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:ShadowHandAsymFFPPORunnerCfg", "skrl_cfg_entry_point": f"{agents.__name__}:skrl_ff_ppo_cfg.yaml", @@ -46,8 +46,9 @@ entry_point=reorient_direct_entry, disable_env_checker=True, kwargs={ - "env_cfg_entry_point": f"{__name__}.shadow_hand_env_cfg:ShadowHandOpenAIEnvCfg", + "env_cfg_entry_point": f"{__name__}.shadow_hand_direct_env_cfg:ShadowHandOpenAIEnvCfg", "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_lstm_cfg.yaml", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:ShadowHandAsymLSTMPPORunnerCfg", }, ) diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/reorient/config/shadow_hand/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/core/reorient/config/shadow_hand/agents/rsl_rl_ppo_cfg.py index f5573ae71190..3994988a3b0a 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/reorient/config/shadow_hand/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/reorient/config/shadow_hand/agents/rsl_rl_ppo_cfg.py @@ -5,7 +5,7 @@ from isaaclab.utils.configclass import configclass -from isaaclab_rl.rsl_rl import RslRlMLPModelCfg, RslRlOnPolicyRunnerCfg, RslRlPpoAlgorithmCfg +from isaaclab_rl.rsl_rl import RslRlMLPModelCfg, RslRlOnPolicyRunnerCfg, RslRlPpoAlgorithmCfg, RslRlRNNModelCfg @configclass @@ -47,6 +47,7 @@ class ShadowHandAsymFFPPORunnerCfg(RslRlOnPolicyRunnerCfg): max_iterations = 10000 save_interval = 250 experiment_name = "shadow_hand_openai_ff" + obs_groups = {"actor": ["policy"], "critic": ["critic"]} actor = RslRlMLPModelCfg( hidden_dims=[400, 400, 200, 100], activation="elu", @@ -74,6 +75,30 @@ class ShadowHandAsymFFPPORunnerCfg(RslRlOnPolicyRunnerCfg): ) +@configclass +class ShadowHandAsymLSTMPPORunnerCfg(ShadowHandAsymFFPPORunnerCfg): + """RSL-RL recurrent policy configuration for the asymmetric OpenAI observations.""" + + experiment_name = "shadow_hand_openai_lstm" + actor = RslRlRNNModelCfg( + hidden_dims=[400, 400, 200, 100], + activation="elu", + obs_normalization=True, + distribution_cfg=RslRlMLPModelCfg.GaussianDistributionCfg(init_std=1.0), + rnn_type="lstm", + rnn_hidden_dim=256, + rnn_num_layers=1, + ) + critic = RslRlRNNModelCfg( + hidden_dims=[512, 512, 256, 128], + activation="elu", + obs_normalization=True, + rnn_type="lstm", + rnn_hidden_dim=256, + rnn_num_layers=1, + ) + + @configclass class ShadowHandCameraFFPPORunnerCfg(RslRlOnPolicyRunnerCfg): num_steps_per_env = 64 diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/reorient/config/shadow_hand/shadow_hand_camera_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/core/reorient/config/shadow_hand/shadow_hand_camera_env_cfg.py index ae5e80d9fb60..5ce3614bbf76 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/reorient/config/shadow_hand/shadow_hand_camera_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/reorient/config/shadow_hand/shadow_hand_camera_env_cfg.py @@ -11,7 +11,7 @@ from isaaclab.utils.configclass import configclass from isaaclab_tasks.core.reorient.config.shadow_hand.feature_extractor import FeatureExtractorCfg -from isaaclab_tasks.core.reorient.config.shadow_hand.shadow_hand_env_cfg import ShadowHandEnvCfg +from isaaclab_tasks.core.reorient.config.shadow_hand.shadow_hand_direct_env_cfg import ShadowHandEnvCfg from isaaclab_tasks.utils import PresetCfg from isaaclab_tasks.utils.presets import MultiBackendRendererCfg @@ -121,6 +121,18 @@ class ShadowHandCameraEnvCfg(ShadowHandEnvCfg): observation_space = 164 + 27 # state observation + vision CNN embedding state_space = 187 + 27 # asymmetric states + vision CNN embedding + def __post_init__(self): + # The vision env renders through the Isaac RTX tiled camera, whose render + # products require the Fabric cloning path. The Newton backend disables Fabric + # cloning (see the base env's ``clone_in_fabric`` scene preset), so under Newton + # the ``rgb`` annotator has no render products for ``num_envs > 1`` and the + # default RGB/depth/semantic render fails. Default the vision env to PhysX so it + # renders out of the box; Newton stays selectable via ``physics=newton_mjwarp`` + # for the depth-only Newton-warp-renderer benchmark path (``presets=newton_renderer``). + super().__post_init__() + for backend_cfg in (self.sim.physics, self.robot_cfg, self.object_cfg): + backend_cfg.default = backend_cfg.physx + def validate_config(self): """Check renderer/data-type and feature-extractor compatibility.""" renderer_type = getattr(self.tiled_camera.renderer_cfg, "renderer_type", None) diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/reorient/config/shadow_hand/shadow_hand_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/core/reorient/config/shadow_hand/shadow_hand_common.py similarity index 64% rename from source/isaaclab_tasks/isaaclab_tasks/core/reorient/config/shadow_hand/shadow_hand_env_cfg.py rename to source/isaaclab_tasks/isaaclab_tasks/core/reorient/config/shadow_hand/shadow_hand_common.py index f6bdae2bd0d8..88436d5aa672 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/reorient/config/shadow_hand/shadow_hand_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/reorient/config/shadow_hand/shadow_hand_common.py @@ -3,20 +3,24 @@ # # SPDX-License-Identifier: BSD-3-Clause +"""Shadow Hand identity shared by the Direct and manager-based reorientation tasks. + +Asset and marker configurations, joint/body name lists, backend physics and +domain-randomization presets, and the sim mixins. No task tunables: reward +scales and thresholds live inline in the workflow configuration files. +""" + from isaaclab_newton.physics import KaminoSolverCfg, MJWarpSolverCfg, NewtonCfg +from isaaclab_ovphysx.physics import OvPhysxCfg from isaaclab_physx.physics import PhysxCfg import isaaclab.envs.mdp as mdp import isaaclab.sim as sim_utils from isaaclab.actuators import ImplicitActuatorCfg from isaaclab.assets import ArticulationCfg, RigidObjectCfg -from isaaclab.envs import DirectRLEnvCfg from isaaclab.managers import EventTermCfg as EventTerm from isaaclab.managers import SceneEntityCfg from isaaclab.markers import VisualizationMarkersCfg -from isaaclab.scene import InteractiveSceneCfg -from isaaclab.sim import SimulationCfg -from isaaclab.sim.spawners.materials.physics_materials_cfg import RigidBodyMaterialCfg from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR from isaaclab.utils.configclass import configclass from isaaclab.utils.noise import GaussianNoiseCfg, NoiseModelWithAdditiveBiasCfg @@ -134,7 +138,8 @@ class PhysxEventCfg: class ShadowHandEventCfg(PresetCfg): physx = PhysxEventCfg() newton_mjwarp = NewtonEventCfg() - default = physx + ovphysx = physx # OvPhysX is PhysX-based; reuse the PhysX randomization terms + default = newton_mjwarp newton_kamino = newton_mjwarp @@ -207,7 +212,17 @@ class ShadowHandRobotCfg(PresetCfg): }, soft_joint_pos_limit_factor=1.0, ) - default = physx + ovphysx = SHADOW_HAND_CFG.replace( + prim_path="/World/envs/env_.*/Robot", + # OVPhysX does not expose the fixed-tendon runtime API, so spawn without tendon overrides. + spawn=SHADOW_HAND_CFG.spawn.replace(fixed_tendons_props=None), + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.0, 0.0, 0.5), + rot=(0.0, 0.0, 0.0, 1.0), + joint_pos={".*": 0.0}, + ), + ) + default = newton_mjwarp newton_kamino = newton_mjwarp @@ -247,25 +262,8 @@ class ObjectCfg(PresetCfg): actuators={}, articulation_root_prim_path="", ) - default = physx - newton_kamino = newton_mjwarp - - -@configclass -class ShadowHandSceneCfg(PresetCfg): - """Scene configuration presets for the shadow hand environment. - - PhysX supports ``clone_in_fabric=True`` for faster scene cloning via the Fabric layer. - Newton does not support Fabric cloning, so ``clone_in_fabric`` must be ``False``. - """ - - physx: InteractiveSceneCfg = InteractiveSceneCfg( - num_envs=8192, env_spacing=0.75, replicate_physics=True, clone_in_fabric=True - ) - newton_mjwarp: InteractiveSceneCfg = InteractiveSceneCfg( - num_envs=8192, env_spacing=0.75, replicate_physics=True, clone_in_fabric=False - ) - default: InteractiveSceneCfg = physx + ovphysx = physx # OvPhysX is PhysX-based; use the rigid-body cube, not Newton's articulation + default = newton_mjwarp newton_kamino = newton_mjwarp @@ -278,152 +276,40 @@ class PhysicsCfg(PresetCfg): ) newton_mjwarp = NewtonCfg( solver_cfg=MJWarpSolverCfg( - solver="newton", integrator="implicitfast", njmax=200, nconmax=70, impratio=10.0, cone="elliptic", update_data_interval=2, - iterations=100, ), num_substeps=2, - debug_mode=False, ) - default = physx + ovphysx = OvPhysxCfg() + default = newton_mjwarp newton_kamino = NewtonCfg(solver_cfg=KaminoSolverCfg(max_contacts_per_world=128)) -@configclass -class ShadowHandEnvCfg(DirectRLEnvCfg): - # env - decimation = 2 - episode_length_s = 10.0 - action_space = 20 - observation_space = 157 # (full) - state_space = 0 - asymmetric_obs = False - obs_type = "full" - - # simulation - sim: SimulationCfg = SimulationCfg( - dt=1 / 120, - render_interval=decimation, - physics_material=RigidBodyMaterialCfg(static_friction=1.0, dynamic_friction=1.0), - physics=PhysicsCfg(), - ) - # robot - robot_cfg: ShadowHandRobotCfg = ShadowHandRobotCfg() - actuated_joint_names = [ - "robot0_WRJ1", - "robot0_WRJ0", - "robot0_FFJ3", - "robot0_FFJ2", - "robot0_FFJ1", - "robot0_MFJ3", - "robot0_MFJ2", - "robot0_MFJ1", - "robot0_RFJ3", - "robot0_RFJ2", - "robot0_RFJ1", - "robot0_LFJ4", - "robot0_LFJ3", - "robot0_LFJ2", - "robot0_LFJ1", - "robot0_THJ4", - "robot0_THJ3", - "robot0_THJ2", - "robot0_THJ1", - "robot0_THJ0", - ] - fingertip_body_names = [ - "robot0_ffdistal", - "robot0_mfdistal", - "robot0_rfdistal", - "robot0_lfdistal", - "robot0_thdistal", - ] - - # in-hand object - object_cfg: ObjectCfg = ObjectCfg() - # goal object - goal_object_cfg: VisualizationMarkersCfg = VisualizationMarkersCfg( - prim_path="/Visuals/goal_marker", - markers={ - "goal": sim_utils.UsdFileCfg( - usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Blocks/DexCube/dex_cube_instanceable.usd", - scale=(1.0, 1.0, 1.0), - ) - }, - ) - # scene — use ShadowHandSceneCfg so that presets=newton_mjwarp disables clone_in_fabric automatically - scene: ShadowHandSceneCfg = ShadowHandSceneCfg() - - # reset - reset_position_noise = 0.01 # range of position at reset - reset_dof_pos_noise = 0.2 # range of dof pos at reset - reset_dof_vel_noise = 0.0 # range of dof vel at reset - # reward scales - dist_reward_scale = -10.0 - rot_reward_scale = 1.0 - rot_eps = 0.1 - action_penalty_scale = -0.0002 - reach_goal_bonus = 250 - fall_penalty = 0 - fall_dist = 0.24 - vel_obs_scale = 0.2 - success_tolerance = 0.1 - max_consecutive_success = 0 - success_count_threshold: int = 1 - """Minimum number of goals reached in an episode to count it as a successful episode.""" - av_factor = 0.1 - act_moving_average = 1.0 - force_torque_obs_scale = 10.0 - - -@configclass -class ShadowHandOpenAIEnvCfg(ShadowHandEnvCfg): - # env - decimation = 3 - episode_length_s = 8.0 - action_space = 20 - observation_space = 42 - state_space = 187 - asymmetric_obs = True - obs_type = "openai" - # simulation - sim: SimulationCfg = SimulationCfg( - dt=1 / 60, - render_interval=decimation, - physics_material=RigidBodyMaterialCfg(static_friction=1.0, dynamic_friction=1.0), - physics=PhysicsCfg(), - ) - # reset - reset_position_noise = 0.01 # range of position at reset - reset_dof_pos_noise = 0.2 # range of dof pos at reset - reset_dof_vel_noise = 0.0 # range of dof vel at reset - # reward scales - dist_reward_scale = -10.0 - rot_reward_scale = 1.0 - rot_eps = 0.1 - action_penalty_scale = -0.0002 - reach_goal_bonus = 250 - fall_penalty = -50 - vel_obs_scale = 0.2 - success_tolerance = 0.4 - max_consecutive_success = 50 - av_factor = 0.1 - act_moving_average = 0.3 - force_torque_obs_scale = 10.0 - # domain randomization config - events: ShadowHandEventCfg = ShadowHandEventCfg() - # at every time-step add gaussian noise + bias. The bias is a gaussian sampled at reset - action_noise_model: NoiseModelWithAdditiveBiasCfg = NoiseModelWithAdditiveBiasCfg( - noise_cfg=GaussianNoiseCfg(mean=0.0, std=0.05, operation="add"), - bias_noise_cfg=GaussianNoiseCfg(mean=0.0, std=0.015, operation="abs"), - ) - # at every time-step add gaussian noise + bias. The bias is a gaussian sampled at reset - observation_noise_model: NoiseModelWithAdditiveBiasCfg = NoiseModelWithAdditiveBiasCfg( - noise_cfg=GaussianNoiseCfg(mean=0.0, std=0.002, operation="add"), - bias_noise_cfg=GaussianNoiseCfg(mean=0.0, std=0.0001, operation="abs"), - ) +# Scene pieces shared verbatim by the manager-based variants. +ROBOT_CFG = ShadowHandRobotCfg() +OBJECT_CFG = ObjectCfg() +GOAL_OBJECT_CFG = VisualizationMarkersCfg( + prim_path="/Visuals/goal_marker", + markers={ + "goal": sim_utils.UsdFileCfg( + usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Blocks/DexCube/dex_cube_instanceable.usd", + scale=(1.0, 1.0, 1.0), + ) + }, +) + + +# Per-step gaussian noise + reset-sampled bias, shared verbatim by the manager-based variant. +OPENAI_ACTION_NOISE_CFG = NoiseModelWithAdditiveBiasCfg( + noise_cfg=GaussianNoiseCfg(mean=0.0, std=0.05, operation="add"), + bias_noise_cfg=GaussianNoiseCfg(mean=0.0, std=0.015, operation="abs"), +) +OPENAI_OBSERVATION_NOISE_CFG = NoiseModelWithAdditiveBiasCfg( + noise_cfg=GaussianNoiseCfg(mean=0.0, std=0.002, operation="add"), + bias_noise_cfg=GaussianNoiseCfg(mean=0.0, std=0.0001, operation="abs"), +) diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/reorient/config/shadow_hand/shadow_hand_direct_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/core/reorient/config/shadow_hand/shadow_hand_direct_env_cfg.py new file mode 100644 index 000000000000..8a65002bca88 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/core/reorient/config/shadow_hand/shadow_hand_direct_env_cfg.py @@ -0,0 +1,143 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.envs import DirectRLEnvCfg +from isaaclab.markers import VisualizationMarkersCfg +from isaaclab.scene import InteractiveSceneCfg +from isaaclab.sim import SimulationCfg +from isaaclab.sim.spawners.materials import RigidBodyMaterialBaseCfg +from isaaclab.utils.configclass import configclass +from isaaclab.utils.noise import NoiseModelWithAdditiveBiasCfg + +from isaaclab_tasks.core.reorient.config.shadow_hand.shadow_hand_common import ( + GOAL_OBJECT_CFG, + OBJECT_CFG, + OPENAI_ACTION_NOISE_CFG, + OPENAI_OBSERVATION_NOISE_CFG, + ROBOT_CFG, + ObjectCfg, + PhysicsCfg, + ShadowHandEventCfg, + ShadowHandRobotCfg, +) +from isaaclab_tasks.utils import preset + +from isaaclab_assets.robots.shadow_hand import SHADOW_ACTUATED_JOINT_NAMES, SHADOW_FINGERTIP_BODY_NAMES + + +@configclass +class ShadowHandSceneCfg(InteractiveSceneCfg): + """Shadow Direct scene defaults. + + ``clone_in_fabric`` is the only backend-varying field: PhysX/OvPhysX use Fabric + cloning for speed; Newton does not support it. The per-backend value is selected + inline at the scene field via :func:`~isaaclab_tasks.utils.preset`. + """ + + num_envs = 8192 + env_spacing = 0.75 + replicate_physics = True + + +@configclass +class ShadowHandEnvCfg(DirectRLEnvCfg): + # env + decimation = 2 + episode_length_s = 10.0 + action_space = 20 + observation_space = 157 # (full) + state_space = 0 + asymmetric_obs = False + obs_type = "full" + + # simulation — values mirrored by the manager cfg (guarded by the value-parity test) + sim: SimulationCfg = SimulationCfg( + dt=1 / 120, + render_interval=2, + physics_material=RigidBodyMaterialBaseCfg(static_friction=1.0, dynamic_friction=1.0), + physics=PhysicsCfg(), + ) + + # robot + robot_cfg: ShadowHandRobotCfg = ROBOT_CFG + actuated_joint_names = SHADOW_ACTUATED_JOINT_NAMES + fingertip_body_names = SHADOW_FINGERTIP_BODY_NAMES + + # in-hand object + object_cfg: ObjectCfg = OBJECT_CFG + # goal object + goal_object_cfg: VisualizationMarkersCfg = GOAL_OBJECT_CFG + # scene — clone_in_fabric is the only backend-varying field (Newton cannot use Fabric cloning) + scene: InteractiveSceneCfg = preset( + default=ShadowHandSceneCfg(clone_in_fabric=False), + physx=ShadowHandSceneCfg(clone_in_fabric=True), + ovphysx=ShadowHandSceneCfg(clone_in_fabric=True), + newton_mjwarp=ShadowHandSceneCfg(clone_in_fabric=False), + newton_kamino=ShadowHandSceneCfg(clone_in_fabric=False), + ) + + # reset + reset_position_noise = 0.01 # range of position at reset + reset_dof_pos_noise = 0.2 # range of dof pos at reset + reset_dof_vel_noise = 0.0 # range of dof vel at reset + # reward scales + dist_reward_scale = -10.0 + rot_reward_scale = 1.0 + rot_eps = 0.1 + action_penalty_scale = -0.0002 + reach_goal_bonus = 250.0 + fall_penalty = 0.0 + fall_dist = 0.24 + vel_obs_scale = 0.2 + success_tolerance = 0.1 + max_consecutive_success = 0 + success_count_threshold: int = 1 + """Minimum number of goals reached in an episode to count it as a successful episode.""" + av_factor = 0.1 + act_moving_average = 1.0 + force_torque_obs_scale = 10.0 + + +@configclass +class ShadowHandOpenAIEnvCfg(ShadowHandEnvCfg): + # env + decimation = 3 + episode_length_s = 8.0 + action_space = 20 + observation_space = 42 + state_space = 187 + asymmetric_obs = True + obs_type = "openai" + + # simulation — values mirrored by the manager cfg (guarded by the value-parity test) + sim: SimulationCfg = SimulationCfg( + dt=1 / 60, + render_interval=3, + physics_material=RigidBodyMaterialBaseCfg(static_friction=1.0, dynamic_friction=1.0), + physics=PhysicsCfg(), + ) + # reset + reset_position_noise = 0.01 # range of position at reset + reset_dof_pos_noise = 0.2 # range of dof pos at reset + reset_dof_vel_noise = 0.0 # range of dof vel at reset + # reward scales + dist_reward_scale = -10.0 + rot_reward_scale = 1.0 + rot_eps = 0.1 + action_penalty_scale = -0.0002 + reach_goal_bonus = 250.0 + fall_penalty = -50.0 + vel_obs_scale = 0.2 + success_tolerance = 0.4 + max_consecutive_success = 50 + av_factor = 0.1 + act_moving_average = 0.3 + force_torque_obs_scale = 10.0 + # domain randomization config + events: ShadowHandEventCfg = ShadowHandEventCfg() + # at every time-step add gaussian noise + bias. The bias is a gaussian sampled at reset + action_noise_model: NoiseModelWithAdditiveBiasCfg = OPENAI_ACTION_NOISE_CFG + # at every time-step add gaussian noise + bias. The bias is a gaussian sampled at reset + observation_noise_model: NoiseModelWithAdditiveBiasCfg = OPENAI_OBSERVATION_NOISE_CFG diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/reorient/mdp/__init__.pyi b/source/isaaclab_tasks/isaaclab_tasks/core/reorient/mdp/__init__.pyi index e835f887dd8a..3221965e78a6 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/reorient/mdp/__init__.pyi +++ b/source/isaaclab_tasks/isaaclab_tasks/core/reorient/mdp/__init__.pyi @@ -10,6 +10,8 @@ __all__ = [ "success_bonus", "track_orientation_inv_l2", "track_pos_l2", + "evaluate_reorient_success", + "reorient_reward", "max_consecutive_success", "object_away_from_goal", "object_away_from_robot", @@ -17,6 +19,12 @@ __all__ = [ from .commands import ReorientCommand, ReorientCommandCfg from .observations import goal_quat_diff -from .rewards import success_bonus, track_orientation_inv_l2, track_pos_l2 +from .rewards import ( + reorient_reward, + evaluate_reorient_success, + success_bonus, + track_orientation_inv_l2, + track_pos_l2, +) from .terminations import max_consecutive_success, object_away_from_goal, object_away_from_robot from isaaclab.envs.mdp import * diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/reorient/mdp/rewards.py b/source/isaaclab_tasks/isaaclab_tasks/core/reorient/mdp/rewards.py index 9974462578f4..b5e71b6a0f57 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/reorient/mdp/rewards.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/reorient/mdp/rewards.py @@ -98,3 +98,97 @@ def track_orientation_inv_l2( dtheta = math_utils.quat_error_magnitude(asset.data.root_quat_w.torch, goal_quat_w) return 1.0 / (dtheta + rot_eps) + + +@torch.jit.script +def evaluate_reorient_success( + object_quat: torch.Tensor, target_quat: torch.Tensor, success_tolerance: float +) -> tuple[torch.Tensor, torch.Tensor]: + """Evaluate reorientation success while exposing its physical error. + + This is the single per-step success evaluation: callers reuse the returned + flags and errors for the reward, the episode bookkeeping, and the + episode-minimum error tracking instead of recomputing the quaternion math. + + Args: + object_quat: Object ``(x, y, z, w)`` orientations. + target_quat: Target ``(x, y, z, w)`` orientations. + success_tolerance: Maximum successful orientation error [rad]. + + Returns: + Per-environment success flags and orientation errors [rad]. + """ + orientation_error = math_utils.quat_error_magnitude(object_quat, target_quat) + return orientation_error <= success_tolerance, orientation_error + + +@torch.jit.script +def reorient_reward( + reset_buf: torch.Tensor, + reset_goal_buf: torch.Tensor, + successes: torch.Tensor, + consecutive_successes: torch.Tensor, + object_pos: torch.Tensor, + target_pos: torch.Tensor, + goal_reached: torch.Tensor, + rotation_distance: torch.Tensor, + actions: torch.Tensor, + distance_scale: float, + rotation_scale: float, + rotation_epsilon: float, + action_penalty_scale: float, + success_bonus: float, + fall_distance: float, + fall_penalty: float, + averaging_factor: float, +) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor, torch.Tensor]: + """Compute the Direct reorientation reward and success state transition. + + The success evaluation is not recomputed here: callers pass the flags and + orientation errors from :func:`evaluate_reorient_success`, computed once + per step. + + Args: + reset_buf: Current episode-reset flags. + reset_goal_buf: Current goal-reset flags. + successes: Goals reached in each episode. + consecutive_successes: Moving-average success count. + object_pos: Object positions in the environment frame [m]. + target_pos: Goal positions in the environment frame [m]. + goal_reached: Per-environment success flags for this step. + rotation_distance: Per-environment orientation errors [rad]. + actions: Normalized joint actions. + distance_scale: Position-distance reward scale [1/m]. + rotation_scale: Orientation reward scale [rad]. + rotation_epsilon: Orientation reward regularizer [rad]. + action_penalty_scale: Squared-action reward scale. + success_bonus: Reward added when a goal is reached. + fall_distance: Object-to-goal termination distance [m]. + fall_penalty: Reward added when the object is out of reach. + averaging_factor: Consecutive-success moving-average factor. + + Returns: + Reward, goal-reset flags, episode success counts, and moving-average + consecutive successes. + """ + goal_distance = torch.linalg.norm(object_pos - target_pos, ord=2, dim=-1) + goal_resets = reset_goal_buf | goal_reached + successes = successes + goal_resets + fell = goal_distance >= fall_distance + reward = ( + goal_distance * distance_scale + + rotation_scale / (rotation_distance + rotation_epsilon) + + actions.square().sum(dim=-1) * action_penalty_scale + + goal_resets.to(goal_distance.dtype) * success_bonus + + fell.to(goal_distance.dtype) * fall_penalty + ) + resets = reset_buf | fell + num_resets = resets.sum() + finished_successes = (successes * resets).sum() + mean_successes = finished_successes / num_resets.clamp_min(1) + consecutive_successes = torch.where( + num_resets > 0, + averaging_factor * mean_successes + (1.0 - averaging_factor) * consecutive_successes, + consecutive_successes, + ) + return reward, goal_resets, successes, consecutive_successes diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/reorient/reorient_common.py b/source/isaaclab_tasks/isaaclab_tasks/core/reorient/reorient_common.py new file mode 100644 index 000000000000..946f1d2e2f08 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/core/reorient/reorient_common.py @@ -0,0 +1,32 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Task-family identity shared by the Direct and manager-based reorientation tasks. + +Geometry constants only — task tunables live inline in the workflow +configuration files, and robot identity lives in the per-robot ``*_common`` +modules. +""" + +GOAL_MARKER_POSITION: tuple[float, float, float] = (-0.2, -0.45, 0.68) +"""Fixed goal-marker display position [m], environment frame (state-based tasks).""" + +CAMERA_GOAL_MARKER_POSITION: tuple[float, float, float] = (-0.2, 0.1, 0.6) +"""Goal-marker display position [m] for the camera tasks. + +Deviates from :data:`GOAL_MARKER_POSITION` so the goal cube sits inside the +tiled camera's frustum. +""" + +IN_HAND_POS_OFFSET: tuple[float, float, float] = (0.0, 0.0, -0.04) +"""Offset from the object's default position to the in-hand goal anchor [m]. + +Defines the Direct/manager goal-position parity: the Direct environments and +the manager command terms derive the same in-hand target point from the +object's default root position plus this offset. +""" + +CAMERA_PLAY_NUM_ENVS: int = 64 +"""Camera-task environment count for checkpoint playback.""" diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/reorient/reorient_direct_env.py b/source/isaaclab_tasks/isaaclab_tasks/core/reorient/reorient_direct_env.py index 5a632dc52ed2..70aee8c4cb53 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/reorient/reorient_direct_env.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/reorient/reorient_direct_env.py @@ -9,7 +9,6 @@ from collections.abc import Sequence from typing import TYPE_CHECKING -import numpy as np import torch import isaaclab.sim as sim_utils @@ -19,19 +18,15 @@ from isaaclab.markers import VisualizationMarkers from isaaclab.sensors import JointWrenchSensor, JointWrenchSensorCfg from isaaclab.sim.spawners.from_files import GroundPlaneCfg, spawn_ground_plane -from isaaclab.utils.math import ( - quat_conjugate, - quat_from_angle_axis, - quat_mul, - sample_uniform, - saturate, - scale_transform, - unscale_transform, -) +from isaaclab.utils.math import quat_conjugate, quat_mul, sample_uniform, saturate, scale_transform, unscale_transform + +from isaaclab_tasks.core.reorient.mdp.rewards import evaluate_reorient_success, reorient_reward +from isaaclab_tasks.core.reorient.reorient_common import GOAL_MARKER_POSITION, IN_HAND_POS_OFFSET +from isaaclab_tasks.core.utils import EpisodeErrorRecorder, randomize_rotation, sample_joint_positions_within_limits if TYPE_CHECKING: from isaaclab_tasks.core.reorient.config.allegro_hand.allegro_hand_direct_env_cfg import AllegroHandEnvCfg - from isaaclab_tasks.core.reorient.config.shadow_hand.shadow_hand_env_cfg import ShadowHandEnvCfg + from isaaclab_tasks.core.reorient.config.shadow_hand.shadow_hand_direct_env_cfg import ShadowHandEnvCfg class ReorientDirectEnv(DirectRLEnv): @@ -40,61 +35,58 @@ class ReorientDirectEnv(DirectRLEnv): def __init__(self, cfg: AllegroHandEnvCfg | ShadowHandEnvCfg, render_mode: str | None = None, **kwargs): super().__init__(cfg, render_mode, **kwargs) + # -- robot introspection: joints, bodies, limits -- self.num_hand_dofs = self.hand.num_joints - - # buffers for position targets - self.hand_dof_targets = torch.zeros((self.num_envs, self.num_hand_dofs), dtype=torch.float, device=self.device) - self.prev_targets = torch.zeros((self.num_envs, self.num_hand_dofs), dtype=torch.float, device=self.device) - self.cur_targets = torch.zeros((self.num_envs, self.num_hand_dofs), dtype=torch.float, device=self.device) - - # list of actuated joints - self.actuated_dof_indices = list() - for joint_name in cfg.actuated_joint_names: - self.actuated_dof_indices.append(self.hand.joint_names.index(joint_name)) - self.actuated_dof_indices.sort() - - # finger bodies - self.finger_bodies = list() - for body_name in self.cfg.fingertip_body_names: - self.finger_bodies.append(self.hand.body_names.index(body_name)) - self.finger_bodies.sort() + self.actuated_dof_indices, _ = self.hand.find_joints(cfg.actuated_joint_names) + if len(self.actuated_dof_indices) != len(cfg.actuated_joint_names): + raise ValueError( + f"Expected {len(cfg.actuated_joint_names)} actuated joints, found {len(self.actuated_dof_indices)}." + ) + self.finger_bodies, fingertip_body_names = self.hand.find_bodies(self.cfg.fingertip_body_names) + if len(self.finger_bodies) != len(self.cfg.fingertip_body_names): + raise ValueError( + f"Expected {len(self.cfg.fingertip_body_names)} fingertip bodies, found {len(self.finger_bodies)}." + ) self.num_fingertips = len(self.finger_bodies) - self.finger_wrench_bodies = [] if getattr(self, "_joint_wrench_sensor", None) is not None: - for body_name in self.cfg.fingertip_body_names: + for body_name in fingertip_body_names: self.finger_wrench_bodies.append(self._joint_wrench_sensor.body_names.index(body_name)) self.finger_wrench_bodies.sort() - - # joint limits joint_pos_limits = self.hand.data.joint_limits.torch.to(self.device) self.hand_dof_lower_limits = joint_pos_limits[..., 0] self.hand_dof_upper_limits = joint_pos_limits[..., 1] - # track goal resets - self.reset_goal_buf = torch.zeros(self.num_envs, dtype=torch.bool, device=self.device) - # used to compare object position + # -- actuation targets (EMA-smoothed joint position targets) -- + self.prev_targets = torch.zeros((self.num_envs, self.num_hand_dofs), dtype=torch.float, device=self.device) + self.cur_targets = torch.zeros((self.num_envs, self.num_hand_dofs), dtype=torch.float, device=self.device) + + # -- goal and success state -- + # in-hand target = object default position + shared offset (mirrors ReorientCommand) self.in_hand_pos = self.object.data.default_root_pose.torch[:, 0:3].clone() - self.in_hand_pos[:, 2] -= 0.04 - # default goal positions + self.in_hand_pos += torch.tensor(IN_HAND_POS_OFFSET, dtype=torch.float, device=self.device) self.goal_rot = torch.zeros((self.num_envs, 4), dtype=torch.float, device=self.device) - self.goal_rot[:, 0] = 1.0 + self.goal_rot[:, 3] = 1.0 # identity quaternion in (x, y, z, w) layout self.goal_pos = torch.zeros((self.num_envs, 3), dtype=torch.float, device=self.device) - self.goal_pos[:, :] = torch.tensor([-0.2, -0.45, 0.68], device=self.device) - # initialize goal marker - self.goal_markers = VisualizationMarkers(self.cfg.goal_object_cfg) - - # track successes + self.goal_pos[:, :] = torch.tensor(GOAL_MARKER_POSITION, device=self.device) + self.reset_goal_buf = torch.zeros(self.num_envs, dtype=torch.bool, device=self.device) self.successes = torch.zeros(self.num_envs, dtype=torch.float, device=self.device) self.consecutive_successes = torch.zeros(1, dtype=torch.float, device=self.device) self._last_episode_success = torch.zeros(self.num_envs, dtype=torch.bool, device=self.device) - # unit tensors + # -- per-step evaluation state and diagnostics -- + # written once per step in :meth:`_get_dones`; the reward and metrics reuse them + self._success_flags = torch.zeros(self.num_envs, dtype=torch.bool, device=self.device) + self._orientation_error_buf = torch.full((self.num_envs,), torch.inf, device=self.device) + self._orientation_error = EpisodeErrorRecorder(self.num_envs, self.device) + + # -- reset randomization constants -- self.x_unit_tensor = torch.tensor([1, 0, 0], dtype=torch.float, device=self.device).repeat((self.num_envs, 1)) self.y_unit_tensor = torch.tensor([0, 1, 0], dtype=torch.float, device=self.device).repeat((self.num_envs, 1)) self.z_unit_tensor = torch.tensor([0, 0, 1], dtype=torch.float, device=self.device).repeat((self.num_envs, 1)) - # bind write methods + # -- visualization and articulation write handles -- + self.goal_markers = VisualizationMarkers(self.cfg.goal_object_cfg) self._set_joint_pos_target = self.hand.set_joint_position_target_index self._write_obj_root_pose = self.object.write_root_pose_to_sim_index self._write_obj_root_vel = self.object.write_root_velocity_to_sim_index @@ -190,32 +182,31 @@ def _update_fingertip_force_sensors(self) -> None: self.fingertip_force_sensors = torch.cat((force, torque), dim=-1) def _get_rewards(self) -> torch.Tensor: - ( - total_reward, - self.reset_goal_buf, - self.successes[:], - self.consecutive_successes[:], - ) = compute_rewards( + # the success flags and orientation errors were computed this step by + # :meth:`_get_dones`; the recorder and the reward reuse them + self._orientation_error.update(self._orientation_error_buf) + total_reward, goal_resets, successes, consecutive_successes = reorient_reward( self.reset_buf, self.reset_goal_buf, self.successes, self.consecutive_successes, - self.max_episode_length, self.object_pos, - self.object_rot, self.in_hand_pos, - self.goal_rot, + self._success_flags, + self._orientation_error_buf, + self.actions, self.cfg.dist_reward_scale, self.cfg.rot_reward_scale, self.cfg.rot_eps, - self.actions, self.cfg.action_penalty_scale, - self.cfg.success_tolerance, self.cfg.reach_goal_bonus, self.cfg.fall_dist, self.cfg.fall_penalty, self.cfg.av_factor, ) + self.reset_goal_buf.copy_(goal_resets) + self.successes[:] = successes + self.consecutive_successes[:] = consecutive_successes if "log" not in self.extras: self.extras["log"] = dict() @@ -235,11 +226,15 @@ def _get_dones(self) -> tuple[torch.Tensor, torch.Tensor]: goal_dist = torch.linalg.norm(self.object_pos - self.in_hand_pos, ord=2, dim=-1) out_of_reach = goal_dist >= self.cfg.fall_dist + # single per-step success evaluation; the reward and metrics reuse these buffers + self._success_flags, self._orientation_error_buf = evaluate_reorient_success( + self.object_rot, self.goal_rot, self.cfg.success_tolerance + ) + if self.cfg.max_consecutive_success > 0: - # Reset progress (episode length buf) on goal envs if max_consecutive_success > 0 - rot_dist = rotation_distance(self.object_rot, self.goal_rot) + # reset progress (episode length buf) on goal environments self.episode_length_buf = torch.where( - torch.abs(rot_dist) <= self.cfg.success_tolerance, + self._success_flags, torch.zeros_like(self.episode_length_buf), self.episode_length_buf, ) @@ -253,9 +248,10 @@ def _get_dones(self) -> tuple[torch.Tensor, torch.Tensor]: def _reset_idx(self, env_ids: Sequence[int]): # Episode counts as successful when goals reached >= cfg.success_count_threshold. self._last_episode_success[env_ids] = self.successes[env_ids] >= self.cfg.success_count_threshold - self.extras.setdefault("log", {})["Metrics/success_rate"] = ( - self._last_episode_success[env_ids].float().mean().item() - ) + # 0-dim device tensor: avoids a host sync here; consumers read it at logging cadence + self.extras.setdefault("log", {})["Metrics/success_rate"] = self._last_episode_success[env_ids].float().mean() + for statistic, value in self._orientation_error.reset(env_ids).items(): + self.extras["log"][f"Diagnostics/episode_min_orientation_error_{statistic}"] = value super()._reset_idx(env_ids) @@ -281,19 +277,15 @@ def _reset_idx(self, env_ids: Sequence[int]): self._write_obj_root_vel(root_velocity=object_default_vel, env_ids=env_ids) # reset hand - delta_max = self.hand_dof_upper_limits[env_ids] - self.hand.data.default_joint_pos.torch[env_ids] - delta_min = self.hand_dof_lower_limits[env_ids] - self.hand.data.default_joint_pos.torch[env_ids] - - dof_pos_noise = sample_uniform(-1.0, 1.0, (len(env_ids), self.num_hand_dofs), device=self.device) - rand_delta = delta_min + (delta_max - delta_min) * 0.5 * dof_pos_noise - dof_pos = self.hand.data.default_joint_pos.torch[env_ids] + self.cfg.reset_dof_pos_noise * rand_delta + default_dof_pos = self.hand.data.default_joint_pos.torch[env_ids] + dof_limits = self.hand.data.joint_limits.torch[env_ids] + dof_pos = sample_joint_positions_within_limits(default_dof_pos, dof_limits, self.cfg.reset_dof_pos_noise) dof_vel_noise = sample_uniform(-1.0, 1.0, (len(env_ids), self.num_hand_dofs), device=self.device) dof_vel = self.hand.data.default_joint_vel.torch[env_ids] + self.cfg.reset_dof_vel_noise * dof_vel_noise self.prev_targets[env_ids] = dof_pos self.cur_targets[env_ids] = dof_pos - self.hand_dof_targets[env_ids] = dof_pos self._set_joint_pos_target(target=dof_pos, env_ids=env_ids) self._write_hand_joint_pos(position=dof_pos, env_ids=env_ids) @@ -317,6 +309,7 @@ def _reset_target_pose(self, env_ids): self.reset_goal_buf[env_ids] = 0 def _compute_intermediate_values(self): + """Refresh the torch-side state snapshots consumed by the observation and reward paths.""" # data for hand self.fingertip_pos = self.hand.data.body_pos_w.torch[:, self.finger_bodies] self.fingertip_rot = self.hand.data.body_quat_w.torch[:, self.finger_bodies] @@ -405,75 +398,3 @@ def compute_full_state(self): dim=-1, ) return states - - -@torch.jit.script -def randomize_rotation(rand0, rand1, x_unit_tensor, y_unit_tensor): - return quat_mul( - quat_from_angle_axis(rand0 * np.pi, x_unit_tensor), quat_from_angle_axis(rand1 * np.pi, y_unit_tensor) - ) - - -@torch.jit.script -def rotation_distance(object_rot, target_rot): - # Orientation alignment for the cube in hand and goal cube - quat_diff = quat_mul(object_rot, quat_conjugate(target_rot)) - return 2.0 * torch.asin(torch.clamp(torch.linalg.norm(quat_diff[:, 0:3], ord=2, dim=-1), max=1.0)) - - -@torch.jit.script -def compute_rewards( - reset_buf: torch.Tensor, - reset_goal_buf: torch.Tensor, - successes: torch.Tensor, - consecutive_successes: torch.Tensor, - max_episode_length: float, - object_pos: torch.Tensor, - object_rot: torch.Tensor, - target_pos: torch.Tensor, - target_rot: torch.Tensor, - dist_reward_scale: float, - rot_reward_scale: float, - rot_eps: float, - actions: torch.Tensor, - action_penalty_scale: float, - success_tolerance: float, - reach_goal_bonus: float, - fall_dist: float, - fall_penalty: float, - av_factor: float, -): - goal_dist = torch.linalg.norm(object_pos - target_pos, ord=2, dim=-1) - rot_dist = rotation_distance(object_rot, target_rot) - - dist_rew = goal_dist * dist_reward_scale - rot_rew = 1.0 / (torch.abs(rot_dist) + rot_eps) * rot_reward_scale - - action_penalty = torch.sum(actions**2, dim=-1) - - # Total reward is: position distance + orientation alignment + action regularization + success bonus + fall penalty - reward = dist_rew + rot_rew + action_penalty * action_penalty_scale - - # Find out which envs hit the goal and update successes count - goal_resets = torch.where(torch.abs(rot_dist) <= success_tolerance, torch.ones_like(reset_goal_buf), reset_goal_buf) - successes = successes + goal_resets - - # Success bonus: orientation is within `success_tolerance` of goal orientation - reward = torch.where(goal_resets == 1, reward + reach_goal_bonus, reward) - - # Fall penalty: distance to the goal is larger than a threshold - reward = torch.where(goal_dist >= fall_dist, reward + fall_penalty, reward) - - # Check env termination conditions, including maximum success number - resets = torch.where(goal_dist >= fall_dist, torch.ones_like(reset_buf), reset_buf) - - num_resets = torch.sum(resets) - finished_cons_successes = torch.sum(successes * resets.float()) - - cons_successes = torch.where( - num_resets > 0, - av_factor * finished_cons_successes / num_resets + (1.0 - av_factor) * consecutive_successes, - consecutive_successes, - ) - - return reward, goal_resets, successes, cons_successes diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/utils.py b/source/isaaclab_tasks/isaaclab_tasks/core/utils.py new file mode 100644 index 000000000000..3a3790615ed9 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/core/utils.py @@ -0,0 +1,137 @@ +# Copyright (c) 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 + +"""Shared utilities for core learning tasks.""" + +from collections.abc import Sequence + +import numpy as np +import torch + +import isaaclab.utils.math as math_utils +from isaaclab.utils.math import quat_from_angle_axis, quat_mul + + +class EpisodeErrorRecorder: + """Record the minimum physical error reached in each episode. + + The recorder deliberately contains no success threshold. This keeps the + measured task error separate from the policy that converts it to a success + result. + """ + + def __init__(self, num_envs: int, device: str | torch.device): + """Initialize per-environment error buffers. + + Args: + num_envs: Number of parallel environments. + device: Device on which to store the buffers. + """ + self.minimum_error = torch.full((num_envs,), torch.inf, device=device) + self._has_sample = torch.zeros(num_envs, dtype=torch.bool, device=device) + + def update(self, error: torch.Tensor) -> None: + """Record one error sample for every environment. + + Args: + error: Per-environment physical errors, in task-defined units. + + Raises: + ValueError: If :paramref:`error` does not match the recorder shape. + """ + if error.shape != self.minimum_error.shape: + raise ValueError(f"Expected error shape {self.minimum_error.shape}, got {error.shape}.") + finite = torch.isfinite(error) + # non-finite samples keep the running minimum; boolean advanced indexing would + # force a host synchronization every step, so substitute-and-minimum instead + torch.minimum(self.minimum_error, torch.where(finite, error, self.minimum_error), out=self.minimum_error) + self._has_sample |= finite + + def reset(self, env_ids: Sequence[int] | torch.Tensor | slice | None = None) -> dict[str, torch.Tensor]: + """Summarize and clear completed episodes. + + Args: + env_ids: Environments whose episodes completed, or ``None`` for all. + + Returns: + Mean, median, and 90th-percentile episode-minimum errors as 0-dim + device tensors, so logging them does not force a host + synchronization in the reset path. The result is empty when none of + the selected environments has a sample. + """ + if env_ids is None: + env_ids = slice(None) + valid = self._has_sample[env_ids] + values = self.minimum_error[env_ids][valid] + statistics = {} + if values.numel() > 0: + statistics = { + "mean": values.mean(), + "median": values.median(), + "p90": torch.quantile(values, 0.9), + } + self.minimum_error[env_ids] = torch.inf + self._has_sample[env_ids] = False + return statistics + + +def sample_joint_positions_within_limits( + default_position: torch.Tensor, + limits: torch.Tensor, + noise_scale: float, +) -> torch.Tensor: + """Sample reset positions between each joint's default position and limits. + + Args: + default_position: Default joint positions [m or rad, depending on joint type], shape ``(..., J)``. + limits: Lower and upper joint-position limits [m or rad, depending on joint type], shape ``(..., J, 2)``. + noise_scale: Dimensionless interpolation scale from the default position toward the sampled limits. + + Returns: + Sampled joint positions [m or rad, depending on joint type], shape ``(..., J)``. + + Raises: + ValueError: If :paramref:`noise_scale` is outside ``[0, 1]``. + """ + if not 0.0 <= noise_scale <= 1.0: + raise ValueError(f"Expected noise_scale in [0, 1], got {noise_scale}.") + position_sample = math_utils.sample_uniform( + -1.0, + 1.0, + default_position.shape, + device=default_position.device, + ) + position_fraction = 0.5 * (position_sample + 1.0) + position_delta = limits[..., 0] - default_position + position_delta = position_delta + (limits[..., 1] - limits[..., 0]) * position_fraction + joint_position = default_position + noise_scale * position_delta + return torch.clamp(joint_position, min=limits[..., 0], max=limits[..., 1]) + + +def random_xy_rotation(count: int, device: str | torch.device) -> torch.Tensor: + """Sample the Direct tasks' sequential random X/Y rotation. + + Args: + count: Number of rotations to sample. + device: Device on which to sample. + + Returns: + Sampled ``(x, y, z, w)`` unit quaternions, shape ``(count, 4)``. + """ + random_values = math_utils.sample_uniform(-1.0, 1.0, (count, 2), device=device) + x_unit = torch.tensor([1.0, 0.0, 0.0], device=device).repeat(count, 1) + y_unit = torch.tensor([0.0, 1.0, 0.0], device=device).repeat(count, 1) + return math_utils.quat_mul( + math_utils.quat_from_angle_axis(random_values[:, 0] * torch.pi, x_unit), + math_utils.quat_from_angle_axis(random_values[:, 1] * torch.pi, y_unit), + ) + + +@torch.jit.script +def randomize_rotation(rand0, rand1, x_unit_tensor, y_unit_tensor): + """Compose ``[-pi, pi]``-scaled random X- and Y-axis rotations into ``(x, y, z, w)`` quaternions.""" + return quat_mul( + quat_from_angle_axis(rand0 * np.pi, x_unit_tensor), quat_from_angle_axis(rand1 * np.pi, y_unit_tensor) + ) diff --git a/source/isaaclab_tasks/test/core/test_core_utils.py b/source/isaaclab_tasks/test/core/test_core_utils.py new file mode 100644 index 000000000000..08f3f223a7b2 --- /dev/null +++ b/source/isaaclab_tasks/test/core/test_core_utils.py @@ -0,0 +1,96 @@ +# Copyright (c) 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 + +"""Tests for utilities shared by core tasks.""" + +import pytest +import torch + +import isaaclab_tasks.core.utils as core_utils + + +def test_sample_joint_positions_within_limits_interpolates_endpoints(monkeypatch): + """Verify reset-noise scales map samples between defaults and limit endpoints.""" + default_position = torch.tensor([[0.1, -0.2]]) + limits = torch.tensor([[[-1.0, 1.0], [-2.0, 2.0]]]) + monkeypatch.setattr( + core_utils.math_utils, + "sample_uniform", + lambda *args, **kwargs: torch.tensor([[-1.0, 1.0]]), + ) + + assert torch.equal(core_utils.sample_joint_positions_within_limits(default_position, limits, 0.0), default_position) + assert torch.allclose( + core_utils.sample_joint_positions_within_limits(default_position, limits, 0.2), + torch.tensor([[-0.12, 0.24]]), + ) + assert torch.equal( + core_utils.sample_joint_positions_within_limits(default_position, limits, 1.0), + torch.tensor([[-1.0, 2.0]]), + ) + + +@pytest.mark.parametrize("noise_scale", (-1.0e-6, 1.000001)) +def test_sample_joint_positions_within_limits_rejects_invalid_scale(noise_scale): + """Verify interpolation scales outside the supported interval are rejected.""" + with pytest.raises(ValueError, match="Expected noise_scale in"): + core_utils.sample_joint_positions_within_limits( + torch.zeros(1, 1), + torch.tensor([[[-1.0, 1.0]]]), + noise_scale, + ) + + +def test_episode_error_recorder_reports_threshold_independent_statistics(): + """Verify episode error summaries contain no success-threshold policy.""" + recorder = core_utils.EpisodeErrorRecorder(num_envs=3, device="cpu") + recorder.update(torch.tensor([0.3, 0.2, 0.4])) + recorder.update(torch.tensor([0.1, 0.25, 0.35])) + + statistics = recorder.reset(torch.tensor([0, 1, 2])) + + # values are 0-dim device tensors (sync-free logging); compare via item() + assert all(isinstance(v, torch.Tensor) and v.ndim == 0 for v in statistics.values()) + assert {k: v.item() for k, v in statistics.items()} == pytest.approx( + {"mean": 0.21666667, "median": 0.2, "p90": 0.32} + ) + assert torch.isinf(recorder.minimum_error).all() + + +def test_episode_error_recorder_skips_episodes_without_samples(): + """Verify initial resets do not emit non-finite diagnostic values.""" + recorder = core_utils.EpisodeErrorRecorder(num_envs=2, device="cpu") + + assert recorder.reset(torch.tensor([0, 1])) == {} + + +def test_episode_error_recorder_update_matches_masked_indexing_reference(): + """Verify the sync-free update equals boolean-mask indexing on NaN/inf/finite mixes.""" + + def reference_update(minimum_error, has_sample, error): + finite = torch.isfinite(error) + minimum_error[finite] = torch.minimum(minimum_error[finite], error[finite]) + has_sample |= finite + + num_envs = 6 + recorder = core_utils.EpisodeErrorRecorder(num_envs=num_envs, device="cpu") + reference_minimum = torch.full((num_envs,), torch.inf) + reference_has_sample = torch.zeros(num_envs, dtype=torch.bool) + nan, inf = float("nan"), float("inf") + samples = [ + torch.tensor([0.5, nan, inf, -inf, 0.4, nan]), + torch.tensor([nan, 0.3, inf, 0.2, 0.6, nan]), + torch.tensor([0.1, inf, -inf, 0.7, nan, nan]), + ] + + for error in samples: + recorder.update(error) + reference_update(reference_minimum, reference_has_sample, error) + + assert torch.equal(recorder.minimum_error, reference_minimum) + assert torch.equal(recorder._has_sample, reference_has_sample) + # env 5 never received a finite sample and must stay excluded from the statistics + assert recorder.minimum_error[5] == torch.inf + assert not recorder._has_sample[5] diff --git a/source/isaaclab_tasks/test/core/test_dexterous_task_math.py b/source/isaaclab_tasks/test/core/test_dexterous_task_math.py new file mode 100644 index 000000000000..67810cdd9540 --- /dev/null +++ b/source/isaaclab_tasks/test/core/test_dexterous_task_math.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 + +"""Behavior tests for the dexterous-task math helpers. + +Each test asserts behavior on hand-computed cases (known rotations, distances, +and layouts) so quaternion-layout or formula regressions fail loudly without +re-implementing the tested math as a reference. +""" + +import math + +import pytest +import torch + +import isaaclab.utils.math as math_utils + +from isaaclab_tasks.core.reorient.mdp.rewards import evaluate_reorient_success + +_DEVICES = ["cpu"] + (["cuda:0"] if torch.cuda.is_available() else []) + +_SIN45 = math.sin(math.pi / 4) +_COS45 = math.cos(math.pi / 4) +# (x, y, z, w) storage everywhere +_IDENTITY = (0.0, 0.0, 0.0, 1.0) +_ROT90_X = (_SIN45, 0.0, 0.0, _COS45) +_ROT180_Z = (0.0, 0.0, 1.0, 0.0) + + +def _quats(device, *quats): + return torch.tensor(quats, dtype=torch.float32, device=device) + + +@pytest.mark.parametrize("device", _DEVICES) +def test_rotation_distance_recovers_known_angles(device): + _, distance = evaluate_reorient_success( + _quats(device, _IDENTITY, _ROT90_X, _ROT180_Z), _quats(device, _IDENTITY, _IDENTITY, _IDENTITY), 0.4 + ) + torch.testing.assert_close(distance, torch.tensor([0.0, math.pi / 2, math.pi], device=device), atol=1e-5, rtol=0.0) + + +@pytest.mark.parametrize("device", _DEVICES) +def test_reorient_success_thresholds_on_rotation_distance(device): + success, distance = evaluate_reorient_success( + _quats(device, _IDENTITY, _ROT90_X), _quats(device, _IDENTITY, _IDENTITY), 0.4 + ) + assert success.tolist() == [True, False] + torch.testing.assert_close(distance, torch.tensor([0.0, math.pi / 2], device=device), atol=1e-5, rtol=0.0) + + +@pytest.mark.parametrize("device", _DEVICES) +def test_goal_quat_error_composes_the_conjugate_product(device): + # error(q, identity) = q; error(q, q) = identity; the 180-degree case exercises w = 0. + # This is the composition the goal_quat_diff and object_goal observation terms rely on. + asset = _quats(device, _ROT90_X, _ROT90_X, _ROT180_Z) + goal = _quats(device, _IDENTITY, _ROT90_X, _IDENTITY) + out = math_utils.quat_mul(asset, math_utils.quat_conjugate(goal)) + torch.testing.assert_close(out[0], torch.tensor(_ROT90_X, device=device), atol=1e-6, rtol=0.0) + torch.testing.assert_close(out[1], torch.tensor(_IDENTITY, device=device), atol=1e-6, rtol=0.0) + # 180-degree rotations keep w = 0, so quat_unique must leave them unchanged + unique = math_utils.quat_unique(out) + torch.testing.assert_close(unique[2].abs(), torch.tensor(_ROT180_Z, device=device).abs(), atol=1e-6, rtol=0.0) + + +@pytest.mark.parametrize("device", _DEVICES) +def test_goal_quat_error_flips_sign_for_negative_real_part(device): + # error(270-degree X rotation, identity) has w = -cos45 < 0, exercising the sign flip + rot270_x = (_SIN45, 0.0, 0.0, -_COS45) + out = math_utils.quat_mul(_quats(device, rot270_x), math_utils.quat_conjugate(_quats(device, _IDENTITY))) + torch.testing.assert_close(out[0], torch.tensor(rot270_x, device=device), atol=1e-6, rtol=0.0) + unique = math_utils.quat_unique(out) + expected = (-_SIN45, 0.0, 0.0, _COS45) + torch.testing.assert_close(unique[0], torch.tensor(expected, device=device), atol=1e-6, rtol=0.0)