diff --git a/docs/source/overview/environments.rst b/docs/source/overview/environments.rst index e71e8f977823..514eb04a73af 100644 --- a/docs/source/overview/environments.rst +++ b/docs/source/overview/environments.rst @@ -387,7 +387,7 @@ for the lift-cube environment: .. |cube-shadow-link| replace:: :isaaclab-source:`Isaac-Reorient-Cube-Shadow-Direct ` .. |cube-shadow-ff-link| replace:: :isaaclab-source:`Isaac-Reorient-Cube-Shadow-OpenAI-FF-Direct ` .. |cube-shadow-lstm-link| replace:: :isaaclab-source:`Isaac-Reorient-Cube-Shadow-OpenAI-LSTM-Direct ` -.. |cube-shadow-vis-link| replace:: :isaaclab-source:`Isaac-Reorient-Cube-Shadow-Camera-Direct ` +.. |cube-shadow-vis-link| replace:: :isaaclab-source:`Isaac-Reorient-Cube-Shadow-Camera-Direct ` .. |agibot_place_mug-link| replace:: :isaaclab-source:`IsaacContrib-Place-Mug-Agibot-Left-Arm-RmpFlow ` .. |agibot_place_toy-link| replace:: :isaaclab-source:`IsaacContrib-Place-Toy2Box-Agibot-Right-Arm-RmpFlow ` .. |reach_openarm_bi-link| replace:: :isaaclab-source:`IsaacContrib-Reach-OpenArmBi ` diff --git a/source/isaaclab/changelog.d/dexterous-env-convergence.rst b/source/isaaclab/changelog.d/dexterous-env-convergence.rst new file mode 100644 index 000000000000..f8191a7f7281 --- /dev/null +++ b/source/isaaclab/changelog.d/dexterous-env-convergence.rst @@ -0,0 +1,6 @@ +Fixed +^^^^^ + +* Fixed :meth:`~isaaclab.envs.DirectRLEnv.reset` to store the observation buffer like + :meth:`~isaaclab.envs.DirectRLEnv.step`, and exposed it on the + multi-agent-to-single-agent adapter. diff --git a/source/isaaclab/isaaclab/envs/direct_rl_env.py b/source/isaaclab/isaaclab/envs/direct_rl_env.py index e99c1cc8afc3..a65b64cd239e 100644 --- a/source/isaaclab/isaaclab/envs/direct_rl_env.py +++ b/source/isaaclab/isaaclab/envs/direct_rl_env.py @@ -386,7 +386,9 @@ def reset(self, seed: int | None = None, options: dict[str, Any] | None = None) self.sim.render() # return observations - return self._get_observations(), self.extras + # store the buffer like step() does, so consumers can read the latest observations + self.obs_buf = self._get_observations() + return self.obs_buf, self.extras def step(self, action: torch.Tensor) -> VecEnvStepReturn: """Execute one time-step of the environment's dynamics. diff --git a/source/isaaclab/isaaclab/envs/utils/marl.py b/source/isaaclab/isaaclab/envs/utils/marl.py index 55c25acd0b8c..adc43e7d39d2 100644 --- a/source/isaaclab/isaaclab/envs/utils/marl.py +++ b/source/isaaclab/isaaclab/envs/utils/marl.py @@ -81,22 +81,39 @@ def __init__(self, env: DirectMARLEnv) -> None: ) self.action_space = gym.vector.utils.batch_space(self.single_action_space, self.num_envs) - def reset(self, seed: int | None = None, options: dict[str, Any] | None = None) -> tuple[VecEnvObs, dict]: - obs, extras = self.env.reset(seed, options) + # latest converted observations, refreshed by reset() and step() + self._obs_buf: VecEnvObs = {} - # use environment state as observation - if self._state_as_observation: - obs = {"policy": self.env.state()} - # concatenate agents' observations + @property + def episode_length_buf(self) -> torch.Tensor: + """Episode lengths from the wrapped multi-agent environment.""" + return self.env.episode_length_buf + + @episode_length_buf.setter + def episode_length_buf(self, value: torch.Tensor) -> None: + # copy in place so holders of the wrapped environment's buffer stay in sync + self.env.episode_length_buf.copy_(value) + + @property + def obs_buf(self) -> VecEnvObs: + """Latest observations from the wrapped multi-agent environment.""" + return self._obs_buf + + def _convert_observations(self, obs: dict[AgentID, ObsType]) -> VecEnvObs: + """Convert multi-agent observations to the single-agent policy observation.""" # FIXME: This implementation assumes the spaces are fundamental ones. Fix it to support composite spaces - else: - obs = { - "policy": torch.cat( - [obs[agent].reshape(self.num_envs, -1) for agent in self.env.possible_agents], dim=-1 - ) - } + if self._state_as_observation: + return {"policy": self.env.state()} + return { + "policy": torch.cat( + [obs[agent].reshape(self.num_envs, -1) for agent in self.env.possible_agents], dim=-1 + ) + } - return obs, extras + def reset(self, seed: int | None = None, options: dict[str, Any] | None = None) -> tuple[VecEnvObs, dict]: + obs, extras = self.env.reset(seed, options) + self._obs_buf = self._convert_observations(obs) + return self._obs_buf, extras def step(self, action: torch.Tensor) -> VecEnvStepReturn: # split single-agent actions to build the multi-agent ones @@ -111,24 +128,14 @@ def step(self, action: torch.Tensor) -> VecEnvStepReturn: # step the environment obs, rewards, terminated, time_outs, extras = self.env.step(_actions) - # use environment state as observation - if self._state_as_observation: - obs = {"policy": self.env.state()} - # concatenate agents' observations - # FIXME: This implementation assumes the spaces are fundamental ones. Fix it to support composite spaces - else: - obs = { - "policy": torch.cat( - [obs[agent].reshape(self.num_envs, -1) for agent in self.env.possible_agents], dim=-1 - ) - } + self._obs_buf = self._convert_observations(obs) # process environment outputs to return single-agent data rewards = sum(rewards.values()) terminated = math.prod(terminated.values()).to(dtype=torch.bool) time_outs = math.prod(time_outs.values()).to(dtype=torch.bool) - return obs, rewards, terminated, time_outs, extras + return self._obs_buf, rewards, terminated, time_outs, extras def render(self, recompute: bool = False) -> np.ndarray | None: return self.env.render(recompute) diff --git a/source/isaaclab/test/envs/test_marl_utils.py b/source/isaaclab/test/envs/test_marl_utils.py new file mode 100644 index 000000000000..d3dab2ed2f0f --- /dev/null +++ b/source/isaaclab/test/envs/test_marl_utils.py @@ -0,0 +1,109 @@ +# 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 types import SimpleNamespace + +import gymnasium as gym +import torch + +from isaaclab.envs.utils.marl import multi_agent_to_single_agent + + +class _FakeMultiAgentEnv: + possible_agents = ["agent_0", "agent_1"] + observation_spaces = { + "agent_0": gym.spaces.Box(low=-1.0, high=1.0, shape=(2,)), + "agent_1": gym.spaces.Box(low=-1.0, high=1.0, shape=(1,)), + } + action_spaces = { + "agent_0": gym.spaces.Box(low=-1.0, high=1.0, shape=(1,)), + "agent_1": gym.spaces.Box(low=-1.0, high=1.0, shape=(1,)), + } + render_mode = None + + def __init__(self): + self.unwrapped = self + self.cfg = SimpleNamespace(state_space=2) + self.state_space = gym.spaces.Box(low=-1.0, high=1.0, shape=(2,)) + self.sim = object() + self.scene = SimpleNamespace(num_envs=2) + self.episode_length_buf = torch.tensor([1, 2]) + self.obs_dict = { + "agent_0": torch.tensor([[1.0, 2.0], [3.0, 4.0]]), + "agent_1": torch.tensor([[5.0], [6.0]]), + } + + def reset(self, seed=None, options=None): + return self.obs_dict, {} + + def step(self, actions): + # shift the observations so a step is distinguishable from a reset + self.obs_dict = {agent: obs + 10.0 for agent, obs in self.obs_dict.items()} + rewards = {agent: torch.zeros(2) for agent in self.possible_agents} + dones = {agent: torch.zeros(2, dtype=torch.bool) for agent in self.possible_agents} + return self.obs_dict, rewards, dones, dones, {} + + def state(self): + return torch.tensor([[7.0, 8.0], [9.0, 10.0]]) + + def close(self): + pass + + +def test_multi_agent_to_single_agent_reset_concatenates_agents(): + """The adapter reset should concatenate the agents' observations.""" + env = multi_agent_to_single_agent(_FakeMultiAgentEnv()) + + observations, _ = env.reset() + + torch.testing.assert_close(observations["policy"], torch.tensor([[1.0, 2.0, 5.0], [3.0, 4.0, 6.0]])) + + +def test_multi_agent_to_single_agent_reset_can_use_state(): + """The adapter reset should support the state-as-observation mode.""" + env = multi_agent_to_single_agent(_FakeMultiAgentEnv(), state_as_observation=True) + + observations, _ = env.reset() + + torch.testing.assert_close(observations["policy"], torch.tensor([[7.0, 8.0], [9.0, 10.0]])) + + +def test_multi_agent_to_single_agent_forwards_episode_lengths(): + """RSL-RL episode randomization should update the wrapped environment buffer in place.""" + source_env = _FakeMultiAgentEnv() + wrapped_buffer = source_env.episode_length_buf + env = multi_agent_to_single_agent(source_env) + episode_lengths = torch.tensor([3, 4]) + + env.episode_length_buf = episode_lengths + + torch.testing.assert_close(env.episode_length_buf, episode_lengths) + torch.testing.assert_close(source_env.episode_length_buf, episode_lengths) + # written in place, so references taken before the assignment observe the new values + assert source_env.episode_length_buf is wrapped_buffer + + +def test_multi_agent_to_single_agent_exposes_latest_observations(): + """The public observation buffer should track the observations last returned by reset and step.""" + env = multi_agent_to_single_agent(_FakeMultiAgentEnv()) + + reset_obs, _ = env.reset() + torch.testing.assert_close(env.obs_buf["policy"], reset_obs["policy"]) + torch.testing.assert_close(env.obs_buf["policy"], torch.tensor([[1.0, 2.0, 5.0], [3.0, 4.0, 6.0]])) + + step_obs = env.step(torch.zeros(2, 2))[0] + torch.testing.assert_close(env.obs_buf["policy"], step_obs["policy"]) + torch.testing.assert_close(env.obs_buf["policy"], torch.tensor([[11.0, 12.0, 15.0], [13.0, 14.0, 16.0]])) + + +def test_multi_agent_to_single_agent_state_observation_tracks_steps(): + """The state-as-observation mode should also refresh the buffer on every transition.""" + env = multi_agent_to_single_agent(_FakeMultiAgentEnv(), state_as_observation=True) + + reset_obs, _ = env.reset() + torch.testing.assert_close(env.obs_buf["policy"], reset_obs["policy"]) + + step_obs = env.step(torch.zeros(2, 2))[0] + torch.testing.assert_close(env.obs_buf["policy"], step_obs["policy"]) diff --git a/source/isaaclab_assets/changelog.d/task-cleanup-dex-part04.minor.rst b/source/isaaclab_assets/changelog.d/task-cleanup-dex-part04.minor.rst new file mode 100644 index 000000000000..fb0341f86e7a --- /dev/null +++ b/source/isaaclab_assets/changelog.d/task-cleanup-dex-part04.minor.rst @@ -0,0 +1,5 @@ +Added +^^^^^ + +* Added ``SHADOW_HAND_NEWTON_CFG``, the Newton (MJWarp) Shadow Hand configuration, + shared by the reorientation and handover tasks. diff --git a/source/isaaclab_assets/isaaclab_assets/robots/shadow_hand.py b/source/isaaclab_assets/isaaclab_assets/robots/shadow_hand.py index b348cc32a57e..3d3c96876c2f 100644 --- a/source/isaaclab_assets/isaaclab_assets/robots/shadow_hand.py +++ b/source/isaaclab_assets/isaaclab_assets/robots/shadow_hand.py @@ -7,7 +7,8 @@ The following configurations are available: -* :obj:`SHADOW_HAND_CFG`: Shadow Hand with implicit actuator model. +* :obj:`SHADOW_HAND_CFG`: Shadow Hand on the PhysX asset with implicit actuator model. +* :obj:`SHADOW_HAND_NEWTON_CFG`: Shadow Hand on the Newton (MJWarp) asset. Reference: @@ -81,7 +82,97 @@ }, soft_joint_pos_limit_factor=1.0, ) -"""Configuration of Shadow Hand robot.""" +"""Configuration of the Shadow Hand robot on the PhysX asset.""" + + +SHADOW_HAND_NEWTON_CFG = ArticulationCfg( + spawn=sim_utils.UsdFileCfg( + # Newton/MuJoCo use a separate USD schema; this asset renumbers the finger + # joints (+1) relative to the PhysX asset above (e.g. FFJ4 vs FFJ3, LFJ5 vs LFJ4). + usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/ShadowRobot/ShadowHandNewton/shadow_hand_instanceable.usda", + activate_contact_sensors=False, + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=True, + retain_accelerations=True, + max_depenetration_velocity=1000.0, + ), + articulation_props=sim_utils.ArticulationRootPropertiesCfg(enabled_self_collisions=True), + joint_drive_props=sim_utils.JointDrivePropertiesCfg(drive_type="force", ensure_drives_exist=True), + fixed_tendons_props=sim_utils.FixedTendonPropertiesCfg(damping=0.1), + ), + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.0, 0.0, 0.5), + # WARNING(Octi): Newton's import_usd.py bakes the USD body xformOp rotation into + # joint_X_p for the root fixed joint, which cancels with the matching localPose1 + # rotation in joint_X_c during FK (joint_X_p * inv(joint_X_c) ≈ identity). This + # discards the root body's native USD orientation, so we must re-apply it here as a + # spawn rotation. PhysX or USD does not have this issue. Remove once Newton fixes root joint + # transform handling in import_usd.py. + rot=(0.0, 0.0, -0.70710678118, 0.70710678118), + joint_pos={".*": 0.0}, + ), + actuators={ + # Drives the joints named by :obj:`SHADOW_ACTUATED_JOINT_NAMES`, which resolve on this + # asset despite its +1 finger renumbering. + # + # The per-finger ``J1``/``J2`` pair is coupled by a fixed tendon (``coef=[1, 1]``) that + # the MJWarp solver currently skips, so the configuration is what holds the pair + # together: both ends are driven with identical gains and effort limits, which + # reproduces the 1:1 coupling. Keep them symmetric and in the same actuator group -- + # driving one end while clamping the other makes the two fight, and an uncapped effort + # limit on either end diverges to NaN within a few hundred steps. + # + # Known limitation: the ``J4`` knuckle abduction joints (and ``LFJ5``) are left + # undriven, so the fingers cannot spread laterally. Correcting that requires resolving + # the skipped tendon first and is deferred to a follow-up. + "fingers": ImplicitActuatorCfg( + joint_names_expr=[ + "robot0_WR.*", + "robot0_(FF|MF|RF|LF|TH)J(3|2|1)", + "robot0_(LF|TH)J4", + "robot0_THJ0", + ], + effort_limit_sim={ + "robot0_WRJ1": 4.785, + "robot0_WRJ0": 2.175, + "robot0_(FF|MF|RF|LF)J1": 0.7245, + "robot0_FFJ(3|2)": 0.9, + "robot0_MFJ(3|2)": 0.9, + "robot0_RFJ(3|2)": 0.9, + "robot0_LFJ(4|3|2)": 0.9, + "robot0_THJ4": 2.3722, + "robot0_THJ3": 1.45, + "robot0_THJ(2|1)": 0.99, + "robot0_THJ0": 0.81, + }, + # Default gains match the PhysX cfg (wrists 5.0/0.5, fingers 1.0/0.1). Tasks that + # need more joint authority override these -- e.g. the handover catch on MJWarp + # raises them to 20.0/2.0, since MJWarp's implicit-PD path lacks PhysX's + # fixed-tendon limit stiffness + solver-iteration torque amplification. + stiffness={ + "robot0_WRJ.*": 5.0, + "robot0_(FF|MF|RF|LF|TH)J(3|2|1)": 1.0, + "robot0_(LF|TH)J4": 1.0, + "robot0_THJ0": 1.0, + }, + damping={ + "robot0_WRJ.*": 0.5, + "robot0_(FF|MF|RF|LF|TH)J(3|2|1)": 0.1, + "robot0_(LF|TH)J4": 0.1, + "robot0_THJ0": 0.1, + }, + friction=1e-2, + armature=2e-3, + ), + }, + soft_joint_pos_limit_factor=1.0, +) +"""Configuration of the Shadow Hand robot on the Newton (MJWarp) asset. + +The Newton USD renumbers the finger joints (+1) relative to :obj:`SHADOW_HAND_CFG`, but the +names in :obj:`SHADOW_ACTUATED_JOINT_NAMES` resolve on both assets, so the two backends share +one actuated-joint list. Gains default to the PhysX values; tasks override them as needed. +""" SHADOW_FINGERTIP_BODY_NAMES: list[str] = [ @@ -115,4 +206,7 @@ "robot0_THJ1", "robot0_THJ0", ] -"""Shadow Hand actuated joint names, in the Direct task's actuation order.""" +"""Shadow Hand actuated joint names, in the Direct task's actuation order. + +These names resolve on both the PhysX and Newton assets, so every backend shares this list. +""" diff --git a/source/isaaclab_experimental/changelog.d/dexterous-env-convergence.rst b/source/isaaclab_experimental/changelog.d/dexterous-env-convergence.rst new file mode 100644 index 000000000000..6a70f438d6c7 --- /dev/null +++ b/source/isaaclab_experimental/changelog.d/dexterous-env-convergence.rst @@ -0,0 +1,6 @@ +Fixed +^^^^^ + +* Fixed :meth:`~isaaclab_experimental.envs.DirectRLEnvWarp.step` and + :meth:`~isaaclab_experimental.envs.DirectRLEnvWarp.reset` to store observations in + ``obs_buf``. diff --git a/source/isaaclab_experimental/isaaclab_experimental/envs/direct_rl_env_warp.py b/source/isaaclab_experimental/isaaclab_experimental/envs/direct_rl_env_warp.py index 025feab3d930..27fae3ef67e2 100644 --- a/source/isaaclab_experimental/isaaclab_experimental/envs/direct_rl_env_warp.py +++ b/source/isaaclab_experimental/isaaclab_experimental/envs/direct_rl_env_warp.py @@ -372,7 +372,9 @@ def reset(self, seed: int | None = None, options: dict[str, Any] | None = None) # return observations self._get_observations() - return {"policy": self.torch_obs_buf.clone()}, self.extras + # store the returned buffer so RslRlVecEnvWrapper.get_observations() can read env.obs_buf + self.obs_buf = {"policy": self.torch_obs_buf.clone()} + return self.obs_buf, self.extras @Timer(name="env_step", msg="Step took:", enable=DEBUG_TIMER_STEP or DEBUG_TIMERS) def step(self, action: torch.Tensor) -> VecEnvStepReturn: @@ -455,8 +457,10 @@ def step(self, action: torch.Tensor) -> VecEnvStepReturn: self._post_step_visualize() # return observations, rewards, resets and extras + # store the returned buffer so RslRlVecEnvWrapper.get_observations() can read env.obs_buf + self.obs_buf = {"policy": self.torch_obs_buf.clone()} return ( - {"policy": self.torch_obs_buf.clone()}, + self.obs_buf, self.torch_reward_buf, self.torch_reset_terminated, self.torch_reset_time_outs, diff --git a/source/isaaclab_rl/changelog.d/dexterous-env-convergence.minor.rst b/source/isaaclab_rl/changelog.d/dexterous-env-convergence.minor.rst new file mode 100644 index 000000000000..fbaa872507d7 --- /dev/null +++ b/source/isaaclab_rl/changelog.d/dexterous-env-convergence.minor.rst @@ -0,0 +1,7 @@ +Changed +^^^^^^^ + +* Changed :meth:`~isaaclab_rl.rsl_rl.RslRlVecEnvWrapper.get_observations` to read the + environment-owned observation buffer instead of private environment methods. The + returned observations now match the latest reset/step returns, including observation + noise. diff --git a/source/isaaclab_rl/isaaclab_rl/rsl_rl/vecenv_wrapper.py b/source/isaaclab_rl/isaaclab_rl/rsl_rl/vecenv_wrapper.py index 5a3fdcd47164..88c8a7ddfcee 100644 --- a/source/isaaclab_rl/isaaclab_rl/rsl_rl/vecenv_wrapper.py +++ b/source/isaaclab_rl/isaaclab_rl/rsl_rl/vecenv_wrapper.py @@ -171,11 +171,7 @@ def reset(self) -> tuple[TensorDict, dict]: # noqa: D102 def get_observations(self) -> TensorDict: """Returns the current observations of the environment.""" - if hasattr(self.unwrapped, "observation_manager"): - obs_dict = self.unwrapped.observation_manager.compute() - else: - obs_dict = self.unwrapped._get_observations() - return TensorDict(obs_dict, batch_size=[self.num_envs]) + return TensorDict(self.unwrapped.obs_buf, batch_size=[self.num_envs]) def step(self, actions: torch.Tensor) -> tuple[TensorDict, torch.Tensor, torch.Tensor, dict]: # clip actions diff --git a/source/isaaclab_rl/test/test_rsl_rl_wrapper_observations.py b/source/isaaclab_rl/test/test_rsl_rl_wrapper_observations.py new file mode 100644 index 000000000000..dba6b064047c --- /dev/null +++ b/source/isaaclab_rl/test/test_rsl_rl_wrapper_observations.py @@ -0,0 +1,76 @@ +# 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 + +"""Kit-free checks for the observation contract of :class:`RslRlVecEnvWrapper`. + +The wrapper reads the environment-owned ``obs_buf`` instead of calling the environment's +private ``_get_observations``. These tests pin that contract without a simulator, so a +regression is caught before the Kit-dependent wrapper tests run. +""" + +import inspect + +import torch +from tensordict import TensorDict + +from isaaclab.envs import DirectRLEnv + +from isaaclab_rl.rsl_rl import RslRlVecEnvWrapper + + +class _FakeEnv: + """Minimal stand-in exposing only what :meth:`get_observations` reads.""" + + def __init__(self): + self.unwrapped = self + self.obs_buf = {"policy": torch.tensor([[1.0, 2.0], [3.0, 4.0]])} + + +def _make_wrapper(env: _FakeEnv, num_envs: int = 2) -> RslRlVecEnvWrapper: + """Build a wrapper without ``__init__``, which requires a real environment and a live sim.""" + wrapper = object.__new__(RslRlVecEnvWrapper) + wrapper.env = env + wrapper.num_envs = num_envs + return wrapper + + +def test_get_observations_returns_the_environment_buffer(): + """The wrapper should hand back the environment's own observation buffer.""" + env = _FakeEnv() + wrapper = _make_wrapper(env) + + observations = wrapper.get_observations() + + assert isinstance(observations, TensorDict) + torch.testing.assert_close(observations["policy"], env.obs_buf["policy"]) + + +def test_get_observations_tracks_buffer_updates(): + """Successive reads should reflect the latest reset/step observations.""" + env = _FakeEnv() + wrapper = _make_wrapper(env) + + env.obs_buf = {"policy": torch.tensor([[5.0, 6.0], [7.0, 8.0]])} + + torch.testing.assert_close(wrapper.get_observations()["policy"], env.obs_buf["policy"]) + + +def test_get_observations_does_not_use_private_environment_methods(): + """The wrapper must not reach into the environment's private observation API.""" + env = _FakeEnv() + + def _private_call(): + raise AssertionError("get_observations() must not call the private _get_observations()") + + env._get_observations = _private_call + wrapper = _make_wrapper(env) + + wrapper.get_observations() + + +def test_direct_rl_env_stores_the_observation_buffer(): + """The environment side of the contract: reset and step both publish ``obs_buf``.""" + assert "self.obs_buf" in inspect.getsource(DirectRLEnv.reset) + assert "self.obs_buf" in inspect.getsource(DirectRLEnv.step) diff --git a/source/isaaclab_tasks/changelog.d/task-cleanup-dex-part04.minor.rst b/source/isaaclab_tasks/changelog.d/task-cleanup-dex-part04.minor.rst new file mode 100644 index 000000000000..d14a54fd7fbc --- /dev/null +++ b/source/isaaclab_tasks/changelog.d/task-cleanup-dex-part04.minor.rst @@ -0,0 +1,22 @@ +Added +^^^^^ + +* Added an RSL-RL training configuration and success metrics to the Shadow handover + Direct task. +* Added OVPhysX physics presets to the handover and camera Direct environments. + +Changed +^^^^^^^ + +* Changed the default physics backend of the Shadow handover Direct task from PhysX to + Newton (MJWarp). Pass ``physics=physx`` for the previous backend. + +Fixed +^^^^^ + +* Fixed handover construction on Newton, which raised ``No joints found for actuator + group``. +* Fixed the Shadow hand root orientation on Newton, which left both palms rotated + 90 degrees. +* Fixed the handover goal orientation, which was initialized to a 180-degree rotation + instead of identity. diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/handover/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/core/handover/__init__.py index 19a42486b912..410f83ced7a3 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/handover/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/handover/__init__.py @@ -21,6 +21,7 @@ disable_env_checker=True, kwargs={ "env_cfg_entry_point": f"{__name__}.handover_env_cfg:HandoverEnvCfg", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:HandoverPPORunnerCfg", "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_cfg.yaml", "skrl_cfg_entry_point": f"{agents.__name__}:skrl_ppo_cfg.yaml", "skrl_ippo_cfg_entry_point": f"{agents.__name__}:skrl_ippo_cfg.yaml", diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/handover/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/core/handover/agents/rsl_rl_ppo_cfg.py new file mode 100644 index 000000000000..75d1b4c0f98f --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/core/handover/agents/rsl_rl_ppo_cfg.py @@ -0,0 +1,44 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.utils.configclass import configclass + +from isaaclab_rl.rsl_rl import RslRlMLPModelCfg, RslRlOnPolicyRunnerCfg, RslRlPpoAlgorithmCfg + + +@configclass +class HandoverPPORunnerCfg(RslRlOnPolicyRunnerCfg): + """RSL-RL PPO configuration for the single-agent view of Shadow Hand handover.""" + + num_steps_per_env = 16 + max_iterations = 5000 + save_interval = 250 + experiment_name = "handover" + obs_groups = {"actor": ["policy"], "critic": ["policy"]} + actor = RslRlMLPModelCfg( + hidden_dims=[512, 512, 256, 128], + activation="elu", + obs_normalization=True, + distribution_cfg=RslRlMLPModelCfg.GaussianDistributionCfg(init_std=1.0), + ) + critic = RslRlMLPModelCfg( + hidden_dims=[512, 512, 256, 128], + activation="elu", + obs_normalization=True, + ) + algorithm = RslRlPpoAlgorithmCfg( + value_loss_coef=1.0, + use_clipped_value_loss=True, + clip_param=0.2, + entropy_coef=0.005, + num_learning_epochs=5, + num_mini_batches=4, + learning_rate=5.0e-4, + schedule="adaptive", + gamma=0.99, + lam=0.95, + desired_kl=0.016, + max_grad_norm=1.0, + ) diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/handover/handover_common.py b/source/isaaclab_tasks/isaaclab_tasks/core/handover/handover_common.py new file mode 100644 index 000000000000..a5bec600e21c --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/core/handover/handover_common.py @@ -0,0 +1,57 @@ +# 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 + +"""Shared task-defining parameters for the handover family. + +The Direct environment and the manager counterpart inherit the same +shared structural definitions so the two variants cannot drift apart. Every +scalar task parameter reads as a flat ``cfg.`` field on the env +configuration; non-scalar items (joint/body-name lists, the goal offset, and +marker templates) stay module-level constants. +""" + +import isaaclab.sim as sim_utils +from isaaclab.markers import VisualizationMarkersCfg + +from isaaclab_assets.robots.shadow_hand import ( + SHADOW_ACTUATED_JOINT_NAMES as ACTUATED_JOINT_NAMES, +) +from isaaclab_assets.robots.shadow_hand import ( + SHADOW_FINGERTIP_BODY_NAMES as FINGERTIP_BODY_NAMES, +) + +__all__ = [ + "ACTUATED_JOINT_NAMES", + "FINGERTIP_BODY_NAMES", + "GOAL_MARKER_CFG", + "GOAL_POSITION_OFFSET", + "OBJECT_RADIUS", +] + + +OBJECT_RADIUS: float = 0.0335 +"""Hand-over object sphere radius [m], also used for the goal marker.""" + +GOAL_POSITION_OFFSET: tuple[float, float, float] = (0.0, -0.25, 0.0) +"""Goal-position offset from the object's default position [m]. + +The Direct environment and the manager command derive the same fixed goal +point from the object's default root position plus this offset. +""" + +GOAL_MARKER_CFG = VisualizationMarkersCfg( + prim_path="/Visuals/goal_marker", + markers={ + "goal": sim_utils.SphereCfg( + radius=OBJECT_RADIUS, + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.4, 0.3, 1.0)), + ), + }, +) +"""Goal-marker template shared by the Direct environment and the manager command term. + +Consumers relying on a different prim path use ``replace`` +on this template; configclass deep-copies defaults, so sharing the instance is safe. +""" diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/handover/handover_env.py b/source/isaaclab_tasks/isaaclab_tasks/core/handover/handover_env.py index 4c587bf9d899..a0a8630fedf6 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/handover/handover_env.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/handover/handover_env.py @@ -8,7 +8,6 @@ from collections.abc import Sequence -import numpy as np import torch import isaaclab.sim as sim_utils @@ -17,17 +16,12 @@ from isaaclab.envs import DirectMARLEnv from isaaclab.markers import VisualizationMarkers 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.handover.handover_common import GOAL_POSITION_OFFSET from isaaclab_tasks.core.handover.handover_env_cfg import HandoverEnvCfg +from isaaclab_tasks.core.handover.mdp.rewards import evaluate_handover_success, handover_reward +from isaaclab_tasks.core.utils import EpisodeErrorRecorder, randomize_rotation, sample_joint_positions_within_limits class HandoverEnv(DirectMARLEnv): @@ -53,33 +47,39 @@ def __init__(self, cfg: HandoverEnvCfg, render_mode: str | None = None, **kwargs ) # list of actuated joints - self.actuated_dof_indices = list() - for joint_name in cfg.actuated_joint_names: - self.actuated_dof_indices.append(self.right_hand.joint_names.index(joint_name)) - self.actuated_dof_indices.sort() + self.actuated_dof_indices, _ = self.right_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)}." + ) # finger bodies - self.finger_bodies = list() - for body_name in self.cfg.fingertip_body_names: - self.finger_bodies.append(self.right_hand.body_names.index(body_name)) - self.finger_bodies.sort() + self.finger_bodies, _ = self.right_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) # joint limits - joint_pos_limits = self.right_hand.data.joint_limits.torch.to(self.device) + joint_pos_limits = self.right_hand.data.joint_limits.torch self.hand_dof_lower_limits = joint_pos_limits[..., 0] self.hand_dof_upper_limits = joint_pos_limits[..., 1] # default goal positions 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.0, -0.64, 0.54], device=self.device) + # goal = object default position + shared offset (mirrors HandoverCommand.__init__) + self.goal_pos[:, :] = self.object.data.default_root_pose.torch[:, :3] + torch.tensor( + GOAL_POSITION_OFFSET, dtype=torch.float, device=self.device + ) # initialize goal marker self.goal_markers = VisualizationMarkers(self.cfg.goal_object_cfg) # Sticky per-env flag: True once the object reached the goal within threshold. self._episode_succeeded = torch.zeros(self.num_envs, dtype=torch.bool, device=self.device) + self._goal_distance = EpisodeErrorRecorder(self.num_envs, self.device) # unit tensors for sampling goal/object rotations about the x and y axes self.x_unit_tensor = torch.tensor([1, 0, 0], dtype=torch.float, device=self.device).repeat((self.num_envs, 1)) @@ -192,17 +192,21 @@ def _get_states(self) -> torch.Tensor: def _get_rewards(self) -> dict[str, torch.Tensor]: # compute reward - goal_dist = torch.linalg.norm(self.object_pos - self.goal_pos, ord=2, dim=-1) - rew_dist = 2 * torch.exp(-self.cfg.dist_reward_scale * goal_dist) + succeeded, goal_dist = evaluate_handover_success( + self.object_pos, self.goal_pos, self.cfg.success_distance_threshold + ) + self._goal_distance.update(goal_dist) + rew_dist = handover_reward(goal_dist, self.cfg.dist_reward_scale) - # log reward components + # log as tensors, not .item(): a per-step host sync stalls the GPU if "log" not in self.extras: self.extras["log"] = dict() + goal_dist_mean = goal_dist.mean() self.extras["log"]["dist_reward"] = rew_dist.mean() - self.extras["log"]["dist_goal"] = goal_dist.mean() - self.extras["log"]["Metrics/goal_distance"] = goal_dist.mean().item() + self.extras["log"]["dist_goal"] = goal_dist_mean + self.extras["log"]["Metrics/goal_distance"] = goal_dist_mean # Sticky per-env success: True once the object reached the goal within threshold. - self._episode_succeeded |= goal_dist < self.cfg.success_distance_threshold + self._episode_succeeded |= succeeded return {"right_hand": rew_dist, "left_hand": rew_dist} @@ -222,9 +226,10 @@ def _reset_idx(self, env_ids: Sequence[int] | torch.Tensor | None): if env_ids is None: env_ids = self.right_hand._ALL_INDICES # Flush per-episode success (sticky binary: object ever reached the goal within threshold). - self.extras.setdefault("log", {})["Metrics/success_rate"] = ( - self._episode_succeeded[env_ids].float().mean().item() - ) + # 0-dim device tensor, for the same reason + self.extras.setdefault("log", {})["Metrics/success_rate"] = self._episode_succeeded[env_ids].float().mean() + for statistic, value in self._goal_distance.reset(env_ids).items(): + self.extras["log"][f"Diagnostics/episode_min_goal_distance_{statistic}"] = value self._episode_succeeded[env_ids] = False # reset articulation and rigid body attributes super()._reset_idx(env_ids) @@ -251,12 +256,9 @@ def _reset_idx(self, env_ids: Sequence[int] | torch.Tensor | None): self.object.write_root_velocity_to_sim_index(root_velocity=object_default_vel, env_ids=env_ids) # reset right hand - delta_max = self.hand_dof_upper_limits[env_ids] - self.right_hand.data.default_joint_pos.torch[env_ids] - delta_min = self.hand_dof_lower_limits[env_ids] - self.right_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.right_hand.data.default_joint_pos.torch[env_ids] + self.cfg.reset_dof_pos_noise * rand_delta + default_dof_pos = self.right_hand.data.default_joint_pos.torch[env_ids] + dof_limits = self.right_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.right_hand.data.default_joint_vel.torch[env_ids] + self.cfg.reset_dof_vel_noise * dof_vel_noise @@ -269,12 +271,9 @@ def _reset_idx(self, env_ids: Sequence[int] | torch.Tensor | None): self.right_hand.write_joint_velocity_to_sim_index(velocity=dof_vel, env_ids=env_ids) # reset left hand - delta_max = self.hand_dof_upper_limits[env_ids] - self.left_hand.data.default_joint_pos.torch[env_ids] - delta_min = self.hand_dof_lower_limits[env_ids] - self.left_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.left_hand.data.default_joint_pos.torch[env_ids] + self.cfg.reset_dof_pos_noise * rand_delta + default_dof_pos = self.left_hand.data.default_joint_pos.torch[env_ids] + dof_limits = self.left_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.left_hand.data.default_joint_vel.torch[env_ids] + self.cfg.reset_dof_vel_noise * dof_vel_noise @@ -328,10 +327,3 @@ def _compute_intermediate_values(self): self.object_rot = self.object.data.root_quat_w.torch self.object_linvel = self.object.data.root_lin_vel_w.torch self.object_angvel = self.object.data.root_ang_vel_w.torch - - -@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) - ) 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 bbf23f6204e0..7ecaf8fce0ee 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 @@ -3,12 +3,14 @@ # # SPDX-License-Identifier: BSD-3-Clause +import torch from isaaclab_newton.physics import 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 +import isaaclab.utils.math as math_utils from isaaclab.assets import ArticulationCfg, RigidObjectCfg from isaaclab.envs import DirectMARLEnvCfg from isaaclab.managers import EventTermCfg as EventTerm @@ -16,13 +18,18 @@ 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.sim.spawners.materials import RigidBodyMaterialBaseCfg from isaaclab.utils.configclass import configclass -from isaaclab_tasks.core.reorient.config.shadow_hand.shadow_hand_common import ShadowHandRobotCfg +from isaaclab_tasks.core.handover.handover_common import ( + ACTUATED_JOINT_NAMES, + FINGERTIP_BODY_NAMES, + GOAL_MARKER_CFG, + OBJECT_RADIUS, +) from isaaclab_tasks.utils import PresetCfg, preset -from isaaclab_assets.robots.shadow_hand import SHADOW_HAND_CFG +from isaaclab_assets.robots.shadow_hand import SHADOW_HAND_CFG, SHADOW_HAND_NEWTON_CFG @configclass @@ -124,61 +131,62 @@ class EventCfg: ) -# Reuse the single-agent Shadow Hand Newton port (USD path, ``rot`` reapplication -# workaround, effort limits, joint regex). The multi-agent variant only diverges -# in actuator gains (stiffness/damping bumped for the catch task) and adds a -# ``distal_passive`` override for the J0 USD-baked values. -_SHADOW_HAND_NEWTON_CFG = ShadowHandRobotCfg().newton_mjwarp - - def _shadow_hand_cfg( prim_path: str, init_pos: tuple[float, float, float], init_rot: tuple[float, float, float, float], ) -> PresetCfg: - """Per-hand Shadow Hand preset (PhysX and Newton MJWarp variants). + """Per-hand Shadow Hand preset (PhysX, Newton MJWarp, and OVPhysX variants). - Both variants are placed at *prim_path* with the same init pose; per-hand + All variants are placed at *prim_path* with the same init pose; per-hand differences (right vs left) come from the caller's *prim_path* / *init_pos* / *init_rot* — the gain tuning is identical on both hands. - The Newton variant layers two :class:`~isaaclab.actuators.ImplicitActuatorCfg` - overrides on top of the single-agent Newton port: - - * ``fingers`` actuator: ``stiffness=20.0`` / ``damping=2.0`` (vs PhysX's - ``5.0`` / ``0.5`` on wrists and ``1.0`` / ``0.1`` on fingers). PhysX layers - ``fixed_tendons_props(limit_stiffness=30, damping=0.1)`` and runs - ``solver_position_iteration_count=8`` per substep — both amplify the - effective torque per unit nominal gain. Newton's MJWarp implicit-PD path - has neither, so a larger nominal gain is needed for comparable joint - authority. ``20.0`` / ``2.0`` is the smallest tested setting at which - MAPPO learns the catch (mean reward at iter 200 / 2048 envs goes from - ~27 at PhysX-mirrored gains to ~777). - * ``distal_passive`` on the four ``robot0_(FF|MF|RF|LF)J0`` joints with - ``stiffness=10.0`` / ``damping=0.1``. The Newton USD bakes - ``stiffness=286 / damping=57`` on these joints from the MJCF→USD - translation, which fights the ``MjcTendon`` coupling and bounces the - ball. ``stiffness=10`` (~1/3 of PhysX's ``limit_stiffness=30``) keeps - the joints near-passive while the tendon constraint dominates. + The Newton variant reuses the shared actuator defined on ``SHADOW_HAND_NEWTON_CFG``, + raising its gains to ``20.0`` / ``2.0`` -- the catch needs more joint authority than + reorientation. The scalar replaces the per-joint gain mapping, so it applies to the whole + actuator group. """ physx_cfg = SHADOW_HAND_CFG.replace(prim_path=prim_path).replace( init_state=ArticulationCfg.InitialStateCfg(pos=init_pos, rot=init_rot, joint_pos={".*": 0.0}) ) - newton_cfg = _SHADOW_HAND_NEWTON_CFG.replace( + # Newton's importer bakes the asset's root orientation into the root joint (see the note on + # SHADOW_HAND_NEWTON_CFG.init_state), so the task rotation must compose with it rather than + # replace it — replacing leaves both palms rotated 90 degrees. + newton_rot = tuple( + math_utils.quat_mul( + torch.tensor(init_rot, dtype=torch.float64), + torch.tensor(SHADOW_HAND_NEWTON_CFG.init_state.rot, dtype=torch.float64), + ).tolist() + ) + newton_mjwarp_cfg = SHADOW_HAND_NEWTON_CFG.replace( prim_path=prim_path, - init_state=_SHADOW_HAND_NEWTON_CFG.init_state.replace(pos=init_pos, rot=init_rot), + init_state=SHADOW_HAND_NEWTON_CFG.init_state.replace(pos=init_pos, rot=newton_rot), actuators={ - "fingers": _SHADOW_HAND_NEWTON_CFG.actuators["fingers"].replace(stiffness=20.0, damping=2.0), - "distal_passive": ImplicitActuatorCfg( - joint_names_expr=["robot0_(FF|MF|RF|LF)J0"], - stiffness=10.0, - damping=0.1, - friction=1e-2, - armature=2e-3, - ), + **SHADOW_HAND_NEWTON_CFG.actuators, + "fingers": SHADOW_HAND_NEWTON_CFG.actuators["fingers"].replace(stiffness=20.0, damping=2.0), }, ) - return preset(default=physx_cfg, physx=physx_cfg, newton_mjwarp=newton_cfg) + ovphysx_cfg = SHADOW_HAND_CFG.replace( + prim_path=prim_path, + # 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=SHADOW_HAND_CFG.init_state.replace(pos=init_pos, rot=init_rot), + ) + return preset(default=newton_mjwarp_cfg, physx=physx_cfg, newton_mjwarp=newton_mjwarp_cfg, ovphysx=ovphysx_cfg) + + +# Per-hand presets shared by the Direct environment and the manager scene. +RIGHT_HAND_CFG = _shadow_hand_cfg( + prim_path="/World/envs/env_.*/RightRobot", + init_pos=(0.0, 0.0, 0.5), + init_rot=(0.0, 0.0, 0.0, 1.0), +) +LEFT_HAND_CFG = _shadow_hand_cfg( + prim_path="/World/envs/env_.*/LeftRobot", + init_pos=(0.0, -1.0, 0.5), + init_rot=(0.0, 0.0, 1.0, 0.0), +) @configclass @@ -196,7 +204,7 @@ class ObjectCfg(PresetCfg): physx = RigidObjectCfg( prim_path="/World/envs/env_.*/object", spawn=sim_utils.SphereCfg( - radius=0.0335, + radius=OBJECT_RADIUS, visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.8, 1.0, 0.0)), physics_material=sim_utils.RigidBodyMaterialCfg(static_friction=0.7), rigid_props=sim_utils.RigidBodyPropertiesCfg( @@ -217,7 +225,7 @@ class ObjectCfg(PresetCfg): newton_mjwarp = RigidObjectCfg( prim_path="/World/envs/env_.*/object", spawn=sim_utils.SphereCfg( - radius=0.0335, + radius=OBJECT_RADIUS, visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.8, 1.0, 0.0)), rigid_props=sim_utils.RigidBodyPropertiesCfg( kinematic_enabled=False, @@ -229,7 +237,8 @@ class ObjectCfg(PresetCfg): ), init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, -0.39, 0.54), rot=(0.0, 0.0, 0.0, 1.0)), ) - default = physx + ovphysx = physx + default = newton_mjwarp @configclass @@ -255,13 +264,16 @@ class PhysicsCfg(PresetCfg): nconmax=70, impratio=10.0, cone="elliptic", - update_data_interval=2, + update_data_interval=4, ccd_iterations=50, # bumped from default 35 for multi-finger contact geometry ), - num_substeps=2, + # 4 substeps (vs reorient's 2): sustained ball-palm contact drives a small fraction of + # envs to NaN at 2. + num_substeps=4, debug_mode=False, ) - default = physx + ovphysx = OvPhysxCfg() + default = newton_mjwarp @configclass @@ -274,69 +286,24 @@ class HandoverEnvCfg(DirectMARLEnvCfg): observation_spaces = {"right_hand": 157, "left_hand": 157} state_space = 290 - # 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, - ), + physics_material=RigidBodyMaterialBaseCfg(static_friction=1.0, dynamic_friction=1.0), physics=PhysicsCfg(), ) + # robot - right_robot_cfg: PresetCfg = _shadow_hand_cfg( - prim_path="/World/envs/env_.*/RightRobot", - init_pos=(0.0, 0.0, 0.5), - init_rot=(0.0, 0.0, 0.0, 1.0), - ) - left_robot_cfg: PresetCfg = _shadow_hand_cfg( - prim_path="/World/envs/env_.*/LeftRobot", - init_pos=(0.0, -1.0, 0.5), - init_rot=(0.0, 0.0, 1.0, 0.0), - ) - 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", - ] + right_robot_cfg: PresetCfg = RIGHT_HAND_CFG + left_robot_cfg: PresetCfg = LEFT_HAND_CFG + actuated_joint_names = ACTUATED_JOINT_NAMES + fingertip_body_names = FINGERTIP_BODY_NAMES # in-hand object object_cfg: ObjectCfg = ObjectCfg() # goal object - goal_object_cfg: VisualizationMarkersCfg = VisualizationMarkersCfg( - prim_path="/Visuals/goal_marker", - markers={ - "goal": sim_utils.SphereCfg( - radius=0.0335, - visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.4, 0.3, 1.0)), - ), - }, - ) + goal_object_cfg: VisualizationMarkersCfg = GOAL_MARKER_CFG # scene scene: InteractiveSceneCfg = InteractiveSceneCfg(num_envs=2048, env_spacing=1.5, replicate_physics=True) diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/handover/mdp/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/core/handover/mdp/__init__.py new file mode 100644 index 000000000000..3a693209a889 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/core/handover/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 + +"""Manager-based MDP terms for the Shadow Hand handover task.""" + +from isaaclab.utils.module import lazy_export + +lazy_export() diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/handover/mdp/__init__.pyi b/source/isaaclab_tasks/isaaclab_tasks/core/handover/mdp/__init__.pyi new file mode 100644 index 000000000000..8100050074e1 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/core/handover/mdp/__init__.pyi @@ -0,0 +1,12 @@ +# 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__ = [ + "handover_reward", + "evaluate_handover_success", +] + +from .rewards import evaluate_handover_success, handover_reward +from isaaclab.envs.mdp import * diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/handover/mdp/rewards.py b/source/isaaclab_tasks/isaaclab_tasks/core/handover/mdp/rewards.py new file mode 100644 index 000000000000..c31065214a65 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/core/handover/mdp/rewards.py @@ -0,0 +1,33 @@ +# 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 terms for the manager-based handover task.""" + +from __future__ import annotations + +import torch + + +def handover_reward(goal_distance: torch.Tensor, distance_scale: float) -> torch.Tensor: + """Return one hand's Direct reward for the current object-goal distance.""" + return 2.0 * torch.exp(-distance_scale * goal_distance) + + +@torch.jit.script +def evaluate_handover_success( + object_position: torch.Tensor, target_position: torch.Tensor, success_distance_threshold: float +) -> tuple[torch.Tensor, torch.Tensor]: + """Evaluate handover success while exposing its physical error. + + Args: + object_position: Object positions [m]. + target_position: Goal positions [m]. + success_distance_threshold: Exclusive successful goal-distance threshold [m]. + + Returns: + Per-environment success flags and object-to-goal distances [m]. + """ + goal_distance = torch.linalg.norm(object_position - target_position, ord=2, dim=-1) + return goal_distance < success_distance_threshold, goal_distance 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 b708f10dce01..95a6cc7e599d 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 @@ -58,10 +58,10 @@ gym.register( id="Isaac-Reorient-Cube-Shadow-Camera-Direct", - entry_point=f"{__name__}.shadow_hand_camera_env:ShadowHandCameraEnv", + entry_point=f"{__name__}.shadow_hand_direct_camera_env:ShadowHandCameraEnv", disable_env_checker=True, kwargs={ - "env_cfg_entry_point": f"{__name__}.shadow_hand_camera_env_cfg:ShadowHandCameraEnvCfg", + "env_cfg_entry_point": f"{__name__}.shadow_hand_direct_camera_env_cfg:ShadowHandCameraEnvCfg", "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:ShadowHandCameraFFPPORunnerCfg", "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_camera_cfg.yaml", }, @@ -69,10 +69,10 @@ gym.register( id="Isaac-Reorient-Cube-Shadow-Camera-Benchmark-Direct", - entry_point=f"{__name__}.shadow_hand_camera_env:ShadowHandCameraEnv", + entry_point=f"{__name__}.shadow_hand_direct_camera_env:ShadowHandCameraEnv", disable_env_checker=True, kwargs={ - "env_cfg_entry_point": f"{__name__}.shadow_hand_camera_env_cfg:ShadowHandCameraBenchmarkEnvCfg", + "env_cfg_entry_point": f"{__name__}.shadow_hand_direct_camera_env_cfg:ShadowHandCameraBenchmarkEnvCfg", "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:ShadowHandCameraFFPPORunnerCfg", "rl_games_cfg_entry_point": f"{agents.__name__}:rl_games_ppo_camera_cfg.yaml", }, 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 3994988a3b0a..b5dd6e7c3071 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 @@ -105,6 +105,7 @@ class ShadowHandCameraFFPPORunnerCfg(RslRlOnPolicyRunnerCfg): max_iterations = 50000 save_interval = 250 experiment_name = "shadow_hand_camera" + obs_groups = {"actor": ["policy"], "critic": ["critic"]} actor = RslRlMLPModelCfg( hidden_dims=[1024, 512, 512, 256, 128], activation="elu", diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/reorient/config/shadow_hand/shadow_hand_common.py b/source/isaaclab_tasks/isaaclab_tasks/core/reorient/config/shadow_hand/shadow_hand_common.py index 88436d5aa672..61cddbbf355d 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/reorient/config/shadow_hand/shadow_hand_common.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/reorient/config/shadow_hand/shadow_hand_common.py @@ -16,7 +16,6 @@ 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.managers import EventTermCfg as EventTerm from isaaclab.managers import SceneEntityCfg @@ -27,7 +26,10 @@ from isaaclab_tasks.utils import PresetCfg -from isaaclab_assets.robots.shadow_hand import SHADOW_HAND_CFG +from isaaclab_assets.robots.shadow_hand import ( + SHADOW_HAND_CFG, + SHADOW_HAND_NEWTON_CFG, +) @configclass @@ -152,66 +154,10 @@ class ShadowHandRobotCfg(PresetCfg): joint_pos={".*": 0.0}, ) ) - newton_mjwarp = ArticulationCfg( - prim_path="/World/envs/env_.*/Robot", - spawn=sim_utils.UsdFileCfg( - # newton/mujoco have separate usd schema - usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/ShadowRobot/ShadowHandNewton/shadow_hand_instanceable.usda", - activate_contact_sensors=False, - rigid_props=sim_utils.RigidBodyPropertiesCfg( - disable_gravity=True, - retain_accelerations=True, - max_depenetration_velocity=1000.0, - ), - articulation_props=sim_utils.ArticulationRootPropertiesCfg(enabled_self_collisions=True), - joint_drive_props=sim_utils.JointDrivePropertiesCfg(drive_type="force", ensure_drives_exist=True), - fixed_tendons_props=sim_utils.FixedTendonPropertiesCfg(damping=0.1), - ), - init_state=ArticulationCfg.InitialStateCfg( - pos=(0.0, 0.0, 0.5), - # WARNING(Octi): Newton's import_usd.py bakes the USD body xformOp rotation into - # joint_X_p for the root fixed joint, which cancels with the matching localPose1 - # rotation in joint_X_c during FK (joint_X_p * inv(joint_X_c) ≈ identity). This - # discards the root body's native USD orientation, so we must re-apply it here as a - # spawn rotation. PhysX or USD does not have this issue. Remove once Newton fixes root joint - # transform handling in import_usd.py. - rot=(0.0, 0.0, -0.70710678118, 0.70710678118), - joint_pos={".*": 0.0}, - ), - actuators={ - "fingers": ImplicitActuatorCfg( - joint_names_expr=["robot0_WR.*", "robot0_(FF|MF|RF|LF|TH)J(3|2|1)", "robot0_(LF|TH)J4", "robot0_THJ0"], - effort_limit_sim={ - "robot0_WRJ1": 4.785, - "robot0_WRJ0": 2.175, - "robot0_(FF|MF|RF|LF)J1": 0.7245, - "robot0_FFJ(3|2)": 0.9, - "robot0_MFJ(3|2)": 0.9, - "robot0_RFJ(3|2)": 0.9, - "robot0_LFJ(4|3|2)": 0.9, - "robot0_THJ4": 2.3722, - "robot0_THJ3": 1.45, - "robot0_THJ(2|1)": 0.99, - "robot0_THJ0": 0.81, - }, - stiffness={ - "robot0_WRJ.*": 5.0, - "robot0_(FF|MF|RF|LF|TH)J(3|2|1)": 1.0, - "robot0_(LF|TH)J4": 1.0, - "robot0_THJ0": 1.0, - }, - damping={ - "robot0_WRJ.*": 0.5, - "robot0_(FF|MF|RF|LF|TH)J(3|2|1)": 0.1, - "robot0_(LF|TH)J4": 0.1, - "robot0_THJ0": 0.1, - }, - friction=1e-2, - armature=2e-3, - ), - }, - soft_joint_pos_limit_factor=1.0, - ) + # Newton robot lives in the asset (see isaaclab_assets.robots.shadow_hand); reorient + # uses its default gains. The handover task consumes the same asset cfg and overrides + # only the finger gains. + newton_mjwarp = SHADOW_HAND_NEWTON_CFG.replace(prim_path="/World/envs/env_.*/Robot") 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. @@ -293,6 +239,7 @@ class PhysicsCfg(PresetCfg): # 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={ diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/reorient/config/shadow_hand/shadow_hand_camera_env.py b/source/isaaclab_tasks/isaaclab_tasks/core/reorient/config/shadow_hand/shadow_hand_direct_camera_env.py similarity index 78% rename from source/isaaclab_tasks/isaaclab_tasks/core/reorient/config/shadow_hand/shadow_hand_camera_env.py rename to source/isaaclab_tasks/isaaclab_tasks/core/reorient/config/shadow_hand/shadow_hand_direct_camera_env.py index 1e562001a164..d424b46400e5 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/reorient/config/shadow_hand/shadow_hand_camera_env.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/reorient/config/shadow_hand/shadow_hand_direct_camera_env.py @@ -14,13 +14,15 @@ from isaaclab import cloner from isaaclab.assets import Articulation, RigidObject from isaaclab.sensors import Camera -from isaaclab.utils.math import quat_apply, scale_transform +from isaaclab.utils.math import scale_transform from isaaclab_tasks.core.reorient.config.shadow_hand.feature_extractor import FeatureExtractor +from isaaclab_tasks.core.reorient.mdp.observations import compute_cube_keypoints +from isaaclab_tasks.core.reorient.reorient_common import CAMERA_GOAL_MARKER_POSITION from isaaclab_tasks.core.reorient.reorient_direct_env import ReorientDirectEnv if TYPE_CHECKING: - from isaaclab_tasks.core.reorient.config.shadow_hand.shadow_hand_camera_env_cfg import ShadowHandCameraEnvCfg + from isaaclab_tasks.core.reorient.config.shadow_hand.shadow_hand_direct_camera_env_cfg import ShadowHandCameraEnvCfg class ShadowHandCameraEnv(ReorientDirectEnv): @@ -40,7 +42,7 @@ def __init__(self, cfg: ShadowHandCameraEnvCfg, render_mode: str | None = None, width=self.cfg.tiled_camera.width, ) # hide goal cubes - self.goal_pos[:, :] = torch.tensor([-0.2, 0.1, 0.6], device=self.device) + self.goal_pos[:, :] = torch.tensor(CAMERA_GOAL_MARKER_POSITION, device=self.device) # keypoints buffer self.gt_keypoints = torch.ones(self.num_envs, 8, 3, dtype=torch.float32, device=self.device) self.goal_keypoints = torch.ones(self.num_envs, 8, 3, dtype=torch.float32, device=self.device) @@ -66,7 +68,7 @@ def _setup_scene(self): def _compute_image_observations(self): # generate ground truth keypoints for in-hand cube - compute_keypoints(pose=torch.cat((self.object_pos, self.object_rot), dim=1), out=self.gt_keypoints) + compute_cube_keypoints(pose=torch.cat((self.object_pos, self.object_rot), dim=1), out=self.gt_keypoints) object_pose = torch.cat([self.object_pos, self.gt_keypoints.view(-1, 24)], dim=-1) @@ -78,7 +80,7 @@ def _compute_image_observations(self): self.embeddings = embeddings.clone().detach() # compute keypoints for goal cube - compute_keypoints( + compute_cube_keypoints( pose=torch.cat((torch.zeros_like(self.goal_pos), self.goal_rot), dim=-1), out=self.goal_keypoints ) @@ -136,34 +138,3 @@ def _get_observations(self) -> dict: observations = {"policy": obs, "critic": state} return observations - - -@torch.jit.script -def compute_keypoints( - pose: torch.Tensor, - num_keypoints: int = 8, - size: tuple[float, float, float] = (2 * 0.03, 2 * 0.03, 2 * 0.03), - out: torch.Tensor | None = None, -): - """Computes positions of 8 corner keypoints of a cube. - - Args: - pose: Position and orientation of the center of the cube. Shape is (N, 7) - num_keypoints: Number of keypoints to compute. Default = 8 - size: Length of X, Y, Z dimensions of cube. Default = [0.06, 0.06, 0.06] - out: Buffer to store keypoints. If None, a new buffer will be created. - """ - num_envs = pose.shape[0] - if out is None: - out = torch.ones(num_envs, num_keypoints, 3, dtype=torch.float32, device=pose.device) - else: - out[:] = 1.0 - for i in range(num_keypoints): - # which dimensions to negate - n = [((i >> k) & 1) == 0 for k in range(3)] - corner_loc = ([(1 if n[k] else -1) * s / 2 for k, s in enumerate(size)],) - corner = torch.tensor(corner_loc, dtype=torch.float32, device=pose.device) * out[:, i, :] - # express corner position in the world frame - out[:, i, :] = pose[:, :3] + quat_apply(pose[:, 3:7], corner) - - return out 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_direct_camera_env_cfg.py similarity index 100% rename from source/isaaclab_tasks/isaaclab_tasks/core/reorient/config/shadow_hand/shadow_hand_camera_env_cfg.py rename to source/isaaclab_tasks/isaaclab_tasks/core/reorient/config/shadow_hand/shadow_hand_direct_camera_env_cfg.py diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/reorient/mdp/observations.py b/source/isaaclab_tasks/isaaclab_tasks/core/reorient/mdp/observations.py index b54b2beeab9f..8bff42fb7b41 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/reorient/mdp/observations.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/reorient/mdp/observations.py @@ -21,6 +21,82 @@ from .commands import ReorientCommand +CUBE_HALF_SIZE: tuple[float, float, float] = (0.03, 0.03, 0.03) +"""Half side lengths [m] of the reorientation cube.""" + + +def _cube_corner_offsets( + size: tuple[float, float, float], num_keypoints: int, device: torch.device | str +) -> torch.Tensor: + """Corner offsets [m] from the cube center; corner index bits select the +/- half side per axis.""" + signs = torch.tensor( + [[1 - 2 * ((corner >> axis) & 1) for axis in range(3)] for corner in range(num_keypoints)], + dtype=torch.float32, + device=device, + ) + half_size = torch.tensor(size, dtype=torch.float32, device=device) / 2.0 + return signs * half_size + + +def compute_cube_keypoints( + pose: torch.Tensor, + num_keypoints: int = 8, + size: tuple[float, float, float] = (2 * 0.03, 2 * 0.03, 2 * 0.03), + out: torch.Tensor | None = None, +) -> torch.Tensor: + """Compute cube-corner positions for batched poses. + + Args: + pose: Cube center poses ``(x, y, z, qx, qy, qz, qw)`` [m, unit quaternion]. + num_keypoints: Number of binary-sign corners to compute. + size: Cube side lengths along each axis [m]. + out: Optional output buffer [m], shape ``(num_envs, num_keypoints, 3)``. + + Returns: + Cube-corner positions [m], shape ``(num_envs, num_keypoints, 3)``. + """ + # Vectorized over corners: the earlier implementation looped over the eight corners, + # allocating a tensor and calling quat_apply once per corner. The corner sign-offsets + # are pose-independent, so they are built once and all num_keypoints corners are rotated + # by the pose in a single batched quat_apply — mathematically identical, no Python loop. + num_envs = pose.shape[0] + corners = _cube_corner_offsets(size, num_keypoints, pose.device) + # Broadcast each env's quaternion across its corners and rotate every offset at once. + rotated = math_utils.quat_apply( + pose[:, None, 3:7].expand(num_envs, num_keypoints, 4), corners.expand(num_envs, num_keypoints, 3) + ) + # Translate the rotated offsets by the cube-center position to get world-frame corners. + keypoints = pose[:, None, 0:3] + rotated + if out is None: + return keypoints + out.copy_(keypoints) + return out + + +def cube_keypoints_from_quat( + quat: torch.Tensor, + half_size: tuple[float, float, float] = CUBE_HALF_SIZE, + num_keypoints: int = 8, +) -> torch.Tensor: + """Rotation-only cube-corner offsets [m] from batched ``(x, y, z, w)`` orientations. + + Args: + quat: Cube orientations, shape ``(num_envs, 4)``. + half_size: Cube half side lengths along each axis [m]. + num_keypoints: Number of binary-sign corners to compute. + + Returns: + Flattened corner offsets [m], shape ``(num_envs, num_keypoints * 3)``. + """ + num_envs = quat.shape[0] + size = (2.0 * half_size[0], 2.0 * half_size[1], 2.0 * half_size[2]) + corners = _cube_corner_offsets(size, num_keypoints, quat.device) + rotated = math_utils.quat_apply( + quat[:, None, :].expand(num_envs, num_keypoints, 4), corners.expand(num_envs, num_keypoints, 3) + ) + return rotated.reshape(num_envs, num_keypoints * 3) + + def goal_quat_diff( env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg, command_name: str, make_quat_unique: bool ) -> torch.Tensor: diff --git a/source/isaaclab_tasks/test/core/test_dexterous_task_math.py b/source/isaaclab_tasks/test/core/test_dexterous_task_math.py index 67810cdd9540..0beb76ae7cd8 100644 --- a/source/isaaclab_tasks/test/core/test_dexterous_task_math.py +++ b/source/isaaclab_tasks/test/core/test_dexterous_task_math.py @@ -17,6 +17,8 @@ import isaaclab.utils.math as math_utils +from isaaclab_tasks.core.handover.mdp.rewards import evaluate_handover_success, handover_reward +from isaaclab_tasks.core.reorient.mdp.observations import compute_cube_keypoints, cube_keypoints_from_quat from isaaclab_tasks.core.reorient.mdp.rewards import evaluate_reorient_success _DEVICES = ["cpu"] + (["cuda:0"] if torch.cuda.is_available() else []) @@ -73,3 +75,53 @@ def test_goal_quat_error_flips_sign_for_negative_real_part(device): 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) + + +@pytest.mark.parametrize("device", _DEVICES) +def test_handover_success_measures_env_frame_distance(device): + object_pos = torch.tensor([[0.0, 0.0, 3.0], [0.0, 0.0, 0.0]], device=device) + goal_pos = torch.tensor([[0.0, 0.0, 3.05], [1.0, 0.0, 0.0]], device=device) + success, distance = evaluate_handover_success(object_pos, goal_pos, 0.1) + torch.testing.assert_close(distance, torch.tensor([0.05, 1.0], device=device), atol=1e-6, rtol=0.0) + assert success.tolist() == [True, False] + + +@pytest.mark.parametrize("device", _DEVICES) +def test_handover_reward_falls_off_exponentially(device): + scale = 20.0 + distance = torch.tensor([0.0, math.log(2.0) / scale], device=device) + reward = handover_reward(distance, scale) + # 2 * exp(-scale * d): d = 0 -> 2.0; d = ln(2)/scale -> 1.0 + torch.testing.assert_close(reward, torch.tensor([2.0, 1.0], device=device), atol=1e-6, rtol=1e-6) + + +@pytest.mark.parametrize("device", _DEVICES) +def test_cube_keypoints_identity_pose_gives_half_side_corners(device): + pose = torch.zeros(1, 7, device=device) + pose[0, 6] = 1.0 # identity orientation (x, y, z, w) + + keypoints = compute_cube_keypoints(pose, size=(0.4, 0.6, 0.8)) + + corners = {tuple(round(c, 3) for c in corner) for corner in keypoints[0].tolist()} + expected = {(sx * 0.2, sy * 0.3, sz * 0.4) for sx in (1, -1) for sy in (1, -1) for sz in (1, -1)} + assert corners == expected + + +@pytest.mark.parametrize("device", _DEVICES) +def test_cube_keypoints_write_into_optional_out_buffer(device): + pose = torch.zeros(2, 7, device=device) + pose[:, 6] = 1.0 + out = torch.full((2, 8, 3), torch.nan, dtype=torch.float32, device=device) + result = compute_cube_keypoints(pose, out=out) + assert result is out + assert not out.isnan().any() + torch.testing.assert_close(out, compute_cube_keypoints(pose)) + + +@pytest.mark.parametrize("device", _DEVICES) +def test_goal_keypoints_are_rotation_only_offsets(device): + quat = _quats(device, _IDENTITY) + flattened = cube_keypoints_from_quat(quat, half_size=(0.2, 0.3, 0.4)) + corners = {tuple(round(c, 3) for c in corner) for corner in flattened.view(8, 3).tolist()} + expected = {(sx * 0.2, sy * 0.3, sz * 0.4) for sx in (1, -1) for sy in (1, -1) for sz in (1, -1)} + assert corners == expected diff --git a/source/isaaclab_tasks/test/core/test_shadow_hand_camera_presets.py b/source/isaaclab_tasks/test/core/test_shadow_hand_camera_presets.py index 85314ede2eaf..cad2801dab19 100644 --- a/source/isaaclab_tasks/test/core/test_shadow_hand_camera_presets.py +++ b/source/isaaclab_tasks/test/core/test_shadow_hand_camera_presets.py @@ -36,7 +36,7 @@ from isaaclab.renderers import RendererCfg # noqa: E402 -from isaaclab_tasks.core.reorient.config.shadow_hand.shadow_hand_camera_env_cfg import ( # noqa: E402 +from isaaclab_tasks.core.reorient.config.shadow_hand.shadow_hand_direct_camera_env_cfg import ( # noqa: E402 ShadowHandCameraEnvCfg, ) from isaaclab_tasks.utils.hydra import collect_presets # noqa: E402 diff --git a/source/isaaclab_tasks/test/rendering_test_utils.py b/source/isaaclab_tasks/test/rendering_test_utils.py index 763882944cb4..aa6c9c1ccbf1 100644 --- a/source/isaaclab_tasks/test/rendering_test_utils.py +++ b/source/isaaclab_tasks/test/rendering_test_utils.py @@ -1204,8 +1204,8 @@ def rendering_test_shadow_hand( from isaaclab.utils.configclass import configclass - from isaaclab_tasks.core.reorient.config.shadow_hand.shadow_hand_camera_env import ShadowHandCameraEnv - from isaaclab_tasks.core.reorient.config.shadow_hand.shadow_hand_camera_env_cfg import ( + from isaaclab_tasks.core.reorient.config.shadow_hand.shadow_hand_direct_camera_env import ShadowHandCameraEnv + from isaaclab_tasks.core.reorient.config.shadow_hand.shadow_hand_direct_camera_env_cfg import ( ShadowHandCameraEnvCfg, ShadowHandTiledCameraCfg, _ShadowHandBaseTiledCameraCfg, @@ -1297,8 +1297,8 @@ def rendering_test_shadow_hand_yellow_bg( """Golden render test for the Shadow Hand environment with a yellow camera background (RGB only).""" from isaaclab.utils.configclass import configclass - from isaaclab_tasks.core.reorient.config.shadow_hand.shadow_hand_camera_env import ShadowHandCameraEnv - from isaaclab_tasks.core.reorient.config.shadow_hand.shadow_hand_camera_env_cfg import ( + from isaaclab_tasks.core.reorient.config.shadow_hand.shadow_hand_direct_camera_env import ShadowHandCameraEnv + from isaaclab_tasks.core.reorient.config.shadow_hand.shadow_hand_direct_camera_env_cfg import ( ShadowHandCameraEnvCfg, ShadowHandTiledCameraCfg, _ShadowHandBaseTiledCameraCfg,