diff --git a/agent_context/topics/data-assets/data-assets.md b/agent_context/topics/data-assets/data-assets.md index 5da2d69c88..a2bf2edda1 100644 --- a/agent_context/topics/data-assets/data-assets.md +++ b/agent_context/topics/data-assets/data-assets.md @@ -255,3 +255,10 @@ under the same lock used by preparation. Historical URDF preparation and model-fidelity metadata are owned by `embodichain_benchmarks.assembly.robot_reference`, not the dataset resolver or a combined arm/gripper catalog preset. + +The five classes in `assets/rl_assets.py` use the same archive cache. The +`official_rl` entry point registers four robot entries and the `G1RoughTerrains` +scene entry through the shared `embodichain.assets` catalog. CLI discovery and +`get_data_class()` resolve the same class identities. The G1 Rough asset does not replace +`UnitreeG1Locomotion`; their joint counts and provenance differ. Publication +status is external to code registration and must be verified before release. diff --git a/agent_context/topics/rl-learning/rl-learning.md b/agent_context/topics/rl-learning/rl-learning.md index bcadd198e4..8576d2aacd 100644 --- a/agent_context/topics/rl-learning/rl-learning.md +++ b/agent_context/topics/rl-learning/rl-learning.md @@ -211,3 +211,20 @@ Use `tests/learning/` for routing, algorithms, buffers, gradients and checkpoint contracts; run native/distributed suites only for those resource boundaries. Missing names point to registries/discovery, disappearing gradients to early detachment, and reset/terminal issues to the selected environment protocol. + +## Rough terrain and dexterous manipulation tasks + +`locomotion/velocity/g1_rough.py` registers the 37-action G1 task (`UnitreeG1RoughRL-v1`). +Its terrain curriculum comes from `G1RoughTerrains` and its robot comes from +`UnitreeG1Rough`, independently of the 29-joint G1 flat archive. Its catalog +exposes a single `default` deployment (`env.yaml` + `agents/ppo.yaml`) running +`physics: newton` with `solver_type: dexuni` and Newton heightfield contacts. + +`manipulation/dexterous/` registers five USD-native `EmbodiedEnv` tasks +(`ShadowPenFixedRL-v1`, `ShadowPenContinuousRL-v1`, `LeapCubeFaceTurnRL-v1`, +`LeapCubeReorientRL-v1`, `LeapCubeRotateZAxisRL-v1`) backed by +`DexterousManipulationEnv`. Runtime scene loading uses `SimulationManager.add_usd()` +and public `Robot`/`Articulation`/`RigidObject` facades with zero MJCF dependency; +each task catalog exposes a single `default` deployment (`env.yaml` + +`agents/ppo.yaml`) running `physics: newton` with `solver_type: dexuni` (GPU +MJWarp CUDA). diff --git a/agent_context/topics/rl-learning/training.md b/agent_context/topics/rl-learning/training.md index 23f03049d0..fca7931bef 100644 --- a/agent_context/topics/rl-learning/training.md +++ b/agent_context/topics/rl-learning/training.md @@ -43,6 +43,14 @@ Variable-horizon APG transition budgets use the actual update count to check an explicit restored LR horizon. Legacy standard/APG-v1 files restore available fields and report missing metadata or RNG. `checkpoint.py` owns shared validation and atomic persistence. +Optional environment training state uses the same v2 file and is collected per +rank. Environments implement `get_training_state`, side-effect-free +`validate_training_state`, and `load_training_state`; validation runs before +learning state changes, and restoration precedes the fresh episode reset. +Missing state in older files produces a warning for environments with these +hooks. G1 Rough uses them for terrain assignments and its task generator, +requiring the original terrain map and per-rank environment count. Its first +reset preserves the restored assignments; physical episodes still restart. Configuration snapshots are frozen from loaded data on the first save; later saves update the run manifest without rewriting those snapshots. Final saves reuse the current update's periodic or best checkpoint. Subsequent saves and final diff --git a/docs/source/api_reference/public_api.rst b/docs/source/api_reference/public_api.rst index 527e63d5a7..b29afdb3f0 100644 --- a/docs/source/api_reference/public_api.rst +++ b/docs/source/api_reference/public_api.rst @@ -5990,6 +5990,278 @@ native listener binds all IPv4 interfaces and lives until World teardown. :no-index: +embodichain.data.assets.rl_assets +--------------------------------- + +Robot, reset-state and frozen-terrain archives use the standard asset cache. + +.. currentmodule:: embodichain.data.assets.rl_assets + +.. autosummary:: + + UnitreeG1Rough + G1RoughTerrains + ShadowHandPen + LeapHandCubeFaceTurn + LeapHandCube + +.. autoclass:: UnitreeG1Rough + :members: + +.. autoclass:: G1RoughTerrains + :members: + +.. autoclass:: ShadowHandPen + :members: + +.. autoclass:: LeapHandCubeFaceTurn + :members: + +.. autoclass:: LeapHandCube + :members: + + +embodichain_tasks.locomotion.velocity.g1_rough +---------------------------------------------- + +The G1 Rough environment registers the 37-action rough-terrain locomotion task with terrain height observations. + +.. currentmodule:: embodichain_tasks.locomotion.velocity.g1_rough + +.. autosummary:: + + UnitreeG1RoughEnv + +.. autoclass:: UnitreeG1RoughEnv + :members: + + +embodichain_tasks.locomotion.velocity.rough +------------------------------------------- + +Load rough-terrain velocity task configurations from a supplied scene bundle. The loader validates robot joint order, shared initial states and terrain inputs. + +.. currentmodule:: embodichain_tasks.locomotion.velocity.rough + +.. autosummary:: + + load_rough_env_cfg + +.. autofunction:: load_rough_env_cfg + + +embodichain_tasks.locomotion.velocity.rough.config +-------------------------------------------------- + +Packaged task definitions retain policy joint order, observation dimensions and fixed physics/control periods. + +.. currentmodule:: embodichain_tasks.locomotion.velocity.rough.config + +.. autosummary:: + + RoughVelocityConfig + load_config + +.. autoclass:: RoughVelocityConfig + :members: + +.. autofunction:: load_config + + +embodichain_tasks.locomotion.velocity.rough.curriculum +------------------------------------------------------ + +Terrain assignment and selected-row curriculum updates use terminal positions and commands before resetting each episode. + +.. currentmodule:: embodichain_tasks.locomotion.velocity.rough.curriculum + +.. autosummary:: + + TerrainCurriculum + +.. autoclass:: TerrainCurriculum + :members: + + +embodichain_tasks.locomotion.velocity.rough.environment +------------------------------------------------------- + +Resolve the rough-terrain deployment against exported robot and terrain assets, with optional training randomization and contact generation selection. + +.. currentmodule:: embodichain_tasks.locomotion.velocity.rough.environment + +.. autosummary:: + + load_rough_env_cfg + +.. autofunction:: load_rough_env_cfg + + +embodichain_tasks.locomotion.velocity.rough.mdp +----------------------------------------------- + +Pure Torch state, action, observation, reward and termination functions share the rough-terrain task definitions. Device constants cache fixed values and reward joint selectors. + +.. currentmodule:: embodichain_tasks.locomotion.velocity.rough.mdp + +.. autosummary:: + + RoughVelocityState + RoughVelocityReward + RoughVelocityConstants + action_target + observation_terms + build_observation + compute_rewards + compute_termination + dc_motor_effort + update_contact_times + +.. autoclass:: RoughVelocityState + :members: + +.. autoclass:: RoughVelocityReward + :members: + +.. autoclass:: RoughVelocityConstants + :members: + +.. autofunction:: action_target + +.. autofunction:: observation_terms + +.. autofunction:: build_observation + +.. autofunction:: compute_rewards + +.. autofunction:: compute_termination + +.. autofunction:: dc_motor_effort + +.. autofunction:: update_contact_times + + +embodichain_tasks.locomotion.velocity.rough.runtime +--------------------------------------------------- + +The runtime adapts the shared task definitions to EmbodiChain managers, state and sensors. Explicit motors retain the configured outer physics cadence; implicit torque telemetry is sampled before the final physical step. + +.. currentmodule:: embodichain_tasks.locomotion.velocity.rough.runtime + +.. autosummary:: + + RoughVelocityEnv + +.. autoclass:: RoughVelocityEnv + :members: + + +embodichain_tasks.locomotion.velocity.rough.terrain +--------------------------------------------------- + +The scanner evaluates 187 torso-relative terrain rays and replays a fixed-buffer CUDA graph when the geometry and buffers remain unchanged. + +.. currentmodule:: embodichain_tasks.locomotion.velocity.rough.terrain + +.. autosummary:: + + HeightScanner + +.. autoclass:: HeightScanner + :members: + + +embodichain_tasks.manipulation.dexterous +---------------------------------------- + +USD-native Shadow Hand and LEAP Hand dexterous manipulation RL environments built on ``EmbodiedEnv``. + +.. currentmodule:: embodichain_tasks.manipulation.dexterous + +.. autosummary:: + + LeapCubeFaceTurnEnv + LeapCubeReorientEnv + LeapCubeRotateZAxisEnv + ShadowPenContinuousEnv + ShadowPenFixedEnv + + +embodichain_tasks.manipulation.dexterous.leap_cube_face_turn +------------------------------------------------------------ + +LEAP Hand top-layer articulated cube face-turning environment (``LeapCubeFaceTurnRL-v1``). + +.. currentmodule:: embodichain_tasks.manipulation.dexterous.leap_cube_face_turn + +.. autosummary:: + + LeapCubeFaceTurnEnv + +.. autoclass:: LeapCubeFaceTurnEnv + :members: + + +embodichain_tasks.manipulation.dexterous.leap_cube_reorient +----------------------------------------------------------- + +LEAP Hand in-hand cube 3D orientation tracking environment (``LeapCubeReorientRL-v1``). + +.. currentmodule:: embodichain_tasks.manipulation.dexterous.leap_cube_reorient + +.. autosummary:: + + LeapCubeReorientEnv + +.. autoclass:: LeapCubeReorientEnv + :members: + + +embodichain_tasks.manipulation.dexterous.leap_cube_rotate_z +----------------------------------------------------------- + +LEAP Hand continuous cube Z-axis rotation environment (``LeapCubeRotateZAxisRL-v1``). + +.. currentmodule:: embodichain_tasks.manipulation.dexterous.leap_cube_rotate_z + +.. autosummary:: + + LeapCubeRotateZAxisEnv + +.. autoclass:: LeapCubeRotateZAxisEnv + :members: + + +embodichain_tasks.manipulation.dexterous.shadow_pen_continuous +-------------------------------------------------------------- + +Shadow Hand continuous multi-pose pen-spinning environment (``ShadowPenContinuousRL-v1``). + +.. currentmodule:: embodichain_tasks.manipulation.dexterous.shadow_pen_continuous + +.. autosummary:: + + ShadowPenContinuousEnv + +.. autoclass:: ShadowPenContinuousEnv + :members: + + +embodichain_tasks.manipulation.dexterous.shadow_pen_fixed +--------------------------------------------------------- + +Shadow Hand fixed-grasp pen-spinning environment (``ShadowPenFixedRL-v1``). + +.. currentmodule:: embodichain_tasks.manipulation.dexterous.shadow_pen_fixed + +.. autosummary:: + + ShadowPenFixedEnv + +.. autoclass:: ShadowPenFixedEnv + :members: + + Parallel-jaw hand configuration and composition ---------------------------------------------- diff --git a/docs/source/overview/rl/dexterous.md b/docs/source/overview/rl/dexterous.md new file mode 100644 index 0000000000..c9a396fd47 --- /dev/null +++ b/docs/source/overview/rl/dexterous.md @@ -0,0 +1,48 @@ +# Pen and cube manipulation + +Five USD-native dexterous manipulation tasks inherit from `EmbodiedEnv` (`DexterousManipulationEnv`) and integrate with the task catalog, `run-task`, `train-rl`, and `eval-policy`. Each task loads its physical scene directly from packaged USD (`.usdc`) via `SimulationManager.add_usd()` with zero runtime MJCF dependency. + +| Task directory | Registered environment | Actions | Actor / critic observations | Asset | +|---|---|---:|---:|---| +| `shadow_pen_fixed` | `ShadowPenFixedRL-v1` | 18 | 87 / 87 | `ShadowHandPen` | +| `shadow_pen_continuous` | `ShadowPenContinuousRL-v1` | 18 | 87 / 87 | `ShadowHandPen` | +| `leap_cube_face_turn` | `LeapCubeFaceTurnRL-v1` | 16 | 65 / 65 | `LeapHandCubeFaceTurn` | +| `leap_cube_reorient` | `LeapCubeReorientRL-v1` | 16 | 57 / 128 | `LeapHandCube` | +| `leap_cube_rotate_z` | `LeapCubeRotateZAxisRL-v1` | 16 | 32 / 105 | `LeapHandCube` | + +## Configuration and CLI Usage + +Each task directory under `extensions/embodichain_tasks/configs/tasks/manipulation/dexterous//` provides a single `default` catalog deployment (`env.yaml` and `agents/ppo.yaml`) configured with `physics: newton` and `solver_type: dexuni` (`use_mujoco_cpu: false`, `use_mujoco_contacts: true`, GPU MuJoCo Warp on CUDA with full scene joint ownership). + +```bash +embodichain data download --name ShadowHandPen +embodichain show-task embodichain_tasks:shadow_pen_fixed +embodichain run-task --gym_config extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/env.yaml +embodichain train-rl --config extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/agents/ppo.yaml +``` + +Continue training from a saved checkpoint with `embodichain train-rl --config CONFIG --resume latest --run RUN_DIRECTORY`, setting `trainer.iterations` to the cumulative update target. Evaluate a saved checkpoint via `embodichain eval-policy RUN_DIRECTORY --checkpoint latest --num-envs 64 --episodes 64 --renderer no-render`. + +## Task Semantics and Evaluation + +Fixed and continuous pen-spinning share the `ShadowHandPen` USD scene; continuous pen-spinning samples across 22 initial grasp poses. Cube face-turning (`LeapCubeFaceTurnRL-v1`) models the cube as a single-hinge articulation (`face_turn`) and applies passive detent torque before each physics substep. Cube reorientation (`LeapCubeReorientRL-v1`) and Z-axis rotation (`LeapCubeRotateZAxisRL-v1`) construct asymmetric actor and privileged critic observation groups from public `Robot` and `RigidObject` state, site transforms parsed from USD, and pre-integration PD actuator forces. + +Evaluation reports `eval/success_rate` for `ShadowPenFixedRL-v1`, `ShadowPenContinuousRL-v1`, `LeapCubeFaceTurnRL-v1`, and `LeapCubeReorientRL-v1`: an episode counts as successful if at least one task success event occurs before termination or truncation (`info["success"]`), while per-step pulses remain in `info["metrics"]["success"]`. `LeapCubeRotateZAxisRL-v1` optimizes unbounded angular velocity without a binary success threshold; evaluate `rotation`, `angular_velocity_z`, `retained`, episode length, and return instead. + +## USD Physical Fidelity and Solver Settings + +All five environments import fixed tendons, joint limits, actuator `ctrlrange`, PD `target_ke`/`target_kd`, site local poses, and collision filter pairs directly from `.usdc`. + +`LeapCubeFaceTurnRL-v1` reserves `njmax: 2048` constraint rows per world (`512` for the remaining tasks). Each `env.yaml` preserves authored solver settings (`20/50` Newton/line-search iterations on Shadow Hand tasks; `5/8` on LEAP Hand tasks) and sets `arena_space: 0.0` with `separate_worlds: true` so execution-model geometry stays spatially uniform across parallel worlds. + +## DexUni Training and Evaluation Baseline + +Sustained single-GPU PPO training, `--resume latest` continuation (`+10` updates), and `64`-episode `eval-policy` results under `env.yaml` (`physics: newton`, `solver_type: dexuni`, `4096` parallel environments, single NVIDIA RTX PRO 5000 Blackwell 72 GB GPU): + +| Environment | Updates (`train + resume`) | Transitions | Steady SPS | Train `episode_reward_avg_100` (`start -> end`) | `eval-policy` (`64` episodes, `--checkpoint latest`) | +|---|---:|---:|---:|---|---| +| `ShadowPenFixedRL-v1` | `150 + 10` | `26,214,400` | `54,106` | `-50.49 -> +47.94` | `avg_reward=+74.26`, `avg_length=500.0`, `retained=100%`, `error=0.143 rad`, `rotation=2.40 rad` | +| `ShadowPenContinuousRL-v1` | `150 + 10` | `52,428,800` | `57,265` | `-39.46 -> +14.47` | `avg_reward=+9.64`, `avg_length=485.8`, `retained=96.9%`, `error=0.229 rad`, `rotation=0.82 rad` | +| `LeapCubeFaceTurnRL-v1` | `150 + 10` | `26,214,400` | `55,562` | `-0.47 -> +216.71` | `avg_reward=+255.53`, `avg_length=500.0`, `success_rate=100%`, `success_count=12.52`, `rotation=20.56 rad` | +| `LeapCubeReorientRL-v1` | `300 + 10` | `50,790,400` | `48,218` | `-4.58 -> +172.64` | `avg_reward=+180.98`, `avg_length=997.6`, `retained=98.4%`, `success_rate=3.1%`, `position_error=0.020 m` | +| `LeapCubeRotateZAxisRL-v1` | `300 + 10` | `50,790,400` | `94,216` | `-5.56 -> +27.40` | `avg_reward=+29.35`, `avg_length=497.5`, `retained=98.4%`, `rotation=29.43 rad`, `angular_velocity_z=1.39 rad/s` | diff --git a/docs/source/overview/rl/index.rst b/docs/source/overview/rl/index.rst index c147bde91e..9090e51396 100644 --- a/docs/source/overview/rl/index.rst +++ b/docs/source/overview/rl/index.rst @@ -88,6 +88,7 @@ See also train_script.md multi_gpu.md locomotion.md + dexterous.md See Also -------- diff --git a/docs/source/overview/rl/locomotion.md b/docs/source/overview/rl/locomotion.md index 0d328ac7c4..84b370be3f 100644 --- a/docs/source/overview/rl/locomotion.md +++ b/docs/source/overview/rl/locomotion.md @@ -5,6 +5,7 @@ Locomotion environments use the same task registration, configuration loader, ac | Task configuration directory | Environment ID | Action / actor / critic dimensions | |---|---|---| | `locomotion/velocity/g1_flat` | `UnitreeG1FlatRL-v1` | 29 / 98 / 113 | +| `locomotion/velocity/g1_rough` | `UnitreeG1RoughRL-v1` | 37 / 310 / 310 | | `locomotion/velocity/h1_2_flat` | `UnitreeH1_2FlatRL-v1` | 27 / 92 / 107 | | `locomotion/velocity/go1_flat` | `UnitreeGo1FlatRL-v1` | 12 / 48 / 72 | | `locomotion/velocity/go2_flat` | `UnitreeGo2FlatRL-v1` | 12 / 47 / 74 | @@ -13,7 +14,7 @@ Locomotion environments use the same task registration, configuration loader, ac | `classic_control/humanoid` | `HumanoidRun-v1` | 17 / 63 / 63 | Paths in this table are relative to `embodichain_tasks/configs/tasks/`. -Each directory contains `env.yaml` and `agents/ppo.yaml` for the default backend, plus `env.newton.yaml` and `agents/ppo.newton.yaml` for Newton. Capability registration is `RL`. +Flat-terrain velocity tasks and `humanoid` ship `env.yaml` and `agents/ppo.yaml` for the default backend alongside `env.newton.yaml` and `agents/ppo.newton.yaml` for Newton; `g1_rough` ships a single Newton DexUni deployment in `env.yaml` and `agents/ppo.yaml`. Capability registration is `RL`. The six velocity task deployments declare the generic `joint_position.default_offset@1` action contract. The contract describes the @@ -46,6 +47,40 @@ Velocity tasks convert normalized joint actions to offsets around the task's def Humanoid uses clipped effort actions (`[-0.4, 0.4]`) multiplied by per-joint motor gears and limited by joint effort bounds. The benchmark's progress potential divides distance by the physics timestep; it deliberately retains that original reward scaling. The maximum episode length is 900 control steps at 60 Hz. +## G1 rough terrain with DexUni + +`g1_rough/env.yaml` (`g1_rough/agents/ppo.yaml`) configures +`UnitreeG1RoughRL-v1` on `physics: newton` with `solver_type: dexuni` +(`joint_mode: dynamic`, `contact_mode: auto`, `use_mujoco_cpu: false`, +`use_mujoco_contacts: false`) and Newton explicit heightfield contact generation +(`broad_phase: explicit`, `reduce_contacts: true`, `max_triangle_pairs: 2500000`). + +The separate `UnitreeG1Rough` and `G1RoughTerrains` asset packages provide the +37-joint model and terrain inputs. They do not replace `UnitreeG1Locomotion`. + +Use `embodichain train-rl --config CONFIG --resume CHECKPOINT` to restore learning +state and the saved terrain levels/types. `trainer.iterations` is the cumulative +update target. The first reset uses the restored terrain assignments without +advancing the curriculum; later episode resets resume normal progression. +Physical state and partial episodes restart. Curriculum state is stored per rank +in the shared schema-v2 checkpoint and validated before learning state changes. +Restore requires the original terrain map and per-rank environment count. +The local G1 validation uses a single training process. Older checkpoints without +environment state warn that terrain assignments cannot be restored. + +`g1_rough/env.yaml` sets `env.episode_context_in_info: false` to avoid +building per-row provenance dictionaries on every control step. Episode state, +curriculum restoration and recorder metadata remain active. Set the option to +`true` when a consumer needs the context snapshot in step/reset info; this is +the shared environment default. + +On a single NVIDIA RTX PRO 5000 Blackwell (72 GB) GPU at `4096` parallel +environments, `g1_rough/agents/ppo.yaml` (`150 + 10` resumed updates, +`15,728,640` transitions) sustains `67,576` steady SPS (`84,131` peak resumed +SPS), improving `episode_length_avg_100` from `24.0` to `39.09` (`eval-policy` +over `64` episodes: `avg_length=45.25`, `avg_reward=-3.40`, +`linear_velocity_error=0.649`, `root_height=0.789 m`). + ## PPO configuration For example, the prepared Go2 training entry is: diff --git a/embodichain/data/assets/__init__.py b/embodichain/data/assets/__init__.py index b62f1ef505..0607cb8f17 100644 --- a/embodichain/data/assets/__init__.py +++ b/embodichain/data/assets/__init__.py @@ -30,3 +30,6 @@ from .locomotion_assets import * from .task_assets import * from .so101_assets import * + + +from .rl_assets import * diff --git a/embodichain/data/assets/rl_assets.py b/embodichain/data/assets/rl_assets.py new file mode 100644 index 0000000000..4ffad5f1e6 --- /dev/null +++ b/embodichain/data/assets/rl_assets.py @@ -0,0 +1,94 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + + +"""Model and terrain archives for dexterous manipulation and rough locomotion.""" + +from __future__ import annotations + +import open3d as o3d + +from embodichain.data.constants import ( + EMBODICHAIN_DEFAULT_DATA_ROOT, + EMBODICHAIN_DOWNLOAD_PREFIX, +) +from embodichain.data.dataset import EmbodiChainDataset + +__all__ = [ + "UnitreeG1Rough", + "G1RoughTerrains", + "ShadowHandPen", + "LeapHandCubeFaceTurn", + "LeapHandCube", +] + + +class _RLAsset(EmbodiChainDataset): + """Download an archive through the standard asset cache.""" + + category: str = "robot_assets" + archive_md5: str + + def __init__(self, data_root: str | None = None) -> None: + name = type(self).__name__ + descriptor = o3d.data.DataDescriptor( + f"{EMBODICHAIN_DOWNLOAD_PREFIX}{self.category}/{name}.zip", self.archive_md5 + ) + super().__init__( + name, + descriptor, + EMBODICHAIN_DEFAULT_DATA_ROOT if data_root is None else data_root, + ) + + +class UnitreeG1Rough(_RLAsset): + """Bundled model inputs for UnitreeG1Rough.""" + + archive_md5 = "b2ff7af5456458a38f96c03dc4ae39db" + + +class G1RoughTerrains(_RLAsset): + """Bundled model inputs for G1RoughTerrains.""" + + category = "scene_assets" + archive_md5 = "3799830bd532aa7047708f749fe462c3" + + +class ShadowHandPen(_RLAsset): + """Bundled model inputs for ShadowHandPen.""" + + archive_md5 = "5dc61ae6cfe2f993c2f27eba8103cb89" + + +class LeapHandCubeFaceTurn(_RLAsset): + """Bundled model inputs for LeapHandCubeFaceTurn.""" + + archive_md5 = "0dcc05a60e258a305af4e5bb49fc3e2c" + + +class LeapHandCube(_RLAsset): + """Bundled model inputs for LeapHandCube.""" + + archive_md5 = "96ecb448c72a8b21f8daba32fa3e4a41" + + +_ASSET_CATALOG: dict[str, dict[str, str]] = { + name: { + "class_path": f"{__name__}:{name}", + "category": "scene" if name == "G1RoughTerrains" else "robot", + } + for name in __all__ +} diff --git a/embodichain/lab/sim/sim_manager.py b/embodichain/lab/sim/sim_manager.py index b211809a1a..63e127e5c3 100644 --- a/embodichain/lab/sim/sim_manager.py +++ b/embodichain/lab/sim/sim_manager.py @@ -1102,6 +1102,7 @@ def asset_uids(self) -> List[str]: uid_list = ( ["default_plane"] if getattr(self.sim_config, "enable_ground_plane", True) + and not getattr(self, "_ground_plane_removed", False) else [] ) uid_list.extend(list(self._lights.keys())) @@ -2067,7 +2068,11 @@ def prepare(self) -> None: self._env = result.get_arena("default") self._arenas = [result.get_arena(name) for name in scene.arena_names] self.__dict__.pop("arena_offsets", None) - if self._default_plane is None and self.sim_config.enable_ground_plane: + if ( + self._default_plane is None + and self.sim_config.enable_ground_plane + and "default_plane" in scene + ): self._bind_default_plane(scene.handles("default_plane")[0]) # Runtime readiness belongs to the SimulationManager. Keep this and @@ -2937,6 +2942,7 @@ def _declare_spawn_default_plane(self) -> None: per_env=False, ) self._spawn_default_plane_descriptor = descriptor + self._ground_plane_removed = False if hasattr(descriptor, "render_categories"): descriptor.render_categories = self._GROUND_RENDER_CATEGORY @@ -3012,6 +3018,25 @@ def configure_ground_plane_collision( if handles: self._bind_default_plane(handles[0]) + def remove_ground_plane(self) -> None: + """Remove the default global ground plane before preparing scene physics. + + Use this when an imported scene stage already authors its own per-arena + ground geometry and solver ownership domain. + + Raises: + RuntimeError: The scene physics has already been prepared. + """ + if self.spawn_result is not None: + raise RuntimeError( + "The default ground plane must be removed before prepare()." + ) + if self._spawn_scene is not None and "default_plane" in self._spawn_scene: + self._spawn_scene.remove("default_plane") + self._spawn_default_plane_descriptor = None + self._default_plane = None + self._ground_plane_removed = True + def set_default_global_lighting(self) -> None: """Set default global lighting for the scene. @@ -6922,6 +6947,33 @@ def _clear_newton_articulation_dynamics(batch: Any) -> None: f"status {status}." ) + def clear_articulation_dynamics( + self, + env_ids: Sequence[int] | torch.Tensor | None = None, + ) -> None: + """Clear dynamic state and solver history for all articulations in selected worlds. + + Args: + env_ids: Environment indices whose articulation dynamics should be cleared. + Defaults to all environments when ``None``. + """ + articulation_uids = tuple(self._robots) + tuple(self._articulations) + if not articulation_uids: + return + if self.physics.name == "newton": + batch = self._newton_articulation_reset_batch(articulation_uids, env_ids) + self._clear_newton_articulation_dynamics(batch) + return + ids = ( + None + if env_ids is None + else torch.as_tensor(env_ids, dtype=torch.long).reshape(-1).tolist() + ) + for robot in self._robots.values(): + robot.clear_dynamics(env_ids=ids) + for articulation in self._articulations.values(): + articulation.clear_dynamics(env_ids=ids) + def export_usd(self, fpath: str) -> bool: """Export the current simulation scene to a USD file. diff --git a/embodichain/learning/rl/checkpoint.py b/embodichain/learning/rl/checkpoint.py index ee3c810265..539d8da155 100644 --- a/embodichain/learning/rl/checkpoint.py +++ b/embodichain/learning/rl/checkpoint.py @@ -389,6 +389,25 @@ def _validate(trainer: Any, payload: Any, scheduler: str) -> list[str]: normalizers = payload["normalizer_by_rank"][rank] expected = _policy_normalizers(policy) _validate_state_layout(expected, normalizers, "rank normalizer") + environment_states = payload.get("environment_state_by_rank") + environment_state = None + if environment_states is not None: + if ( + not isinstance(environment_states, list) + or len(environment_states) != world_size + ): + raise ValueError( + "Checkpoint environment state must contain one entry per rank." + ) + environment_state = environment_states[rank] + if environment_state is not None: + validate = getattr(trainer.env, "validate_training_state", None) + load = getattr(trainer.env, "load_training_state", None) + if not callable(validate) or not callable(load): + raise ValueError("Environment cannot restore checkpoint training state.") + validate(environment_state) + elif callable(getattr(trainer.env, "get_training_state", None)): + unavailable.append("environment training state") normalizer = getattr(trainer, "observation_normalizer", None) saved_normalizer = payload.get("observation_normalizer") if (normalizer is None) != (saved_normalizer is None): @@ -444,6 +463,8 @@ def _save(trainer: Any, path: str | Path, extra: Mapping[str, Any]) -> str: "rng": _capture_rng(trainer.algorithm.device), "normalizer": _policy_normalizers(policy), } + get_state = getattr(trainer.env, "get_training_state", None) + local["environment"] = get_state() if callable(get_state) else None except Exception as exc: error = exc _sync_errors(error, distributed) @@ -461,6 +482,7 @@ def _save(trainer: Any, path: str | Path, extra: Mapping[str, Any]) -> str: "world_size": len(ranks), "rng_by_rank": [value["rng"] for value in ranks], "normalizer_by_rank": [value["normalizer"] for value in ranks], + "environment_state_by_rank": [value["environment"] for value in ranks], "global_step": trainer.global_step, "num_updates": trainer.num_updates, "policy": policy.state_dict(), @@ -520,6 +542,9 @@ def _load(trainer: Any, path: str | Path, scheduler: str) -> dict[str, Any]: if context is not None and "gym" in context and "gym" in original: # Retain the source identity across successive snapshot-based resumes. context["gym"] = deepcopy(original["gym"]) + environment_states = payload.get("environment_state_by_rank") + if environment_states is not None and environment_states[rank] is not None: + trainer.env.load_training_state(environment_states[rank]) if hasattr(trainer.collector, "obs_td"): trainer.collector.obs_td = trainer.collector._reset_env() trainer.curr_ret.zero_() diff --git a/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/README.md b/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/README.md new file mode 100644 index 0000000000..8d85dc87ab --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/README.md @@ -0,0 +1,24 @@ +# Unitree G1 Rough-Terrain Locomotion + +Track planar velocity and yaw-rate commands with the 37-DOF Unitree G1 humanoid across a heightfield terrain curriculum with 187 local elevation samples. +Gym ID: `UnitreeG1RoughRL-v1`. + +[env.yaml](env.yaml) and [agents/ppo.yaml](agents/ppo.yaml) configure the `default` catalog deployment on `physics: newton` with `solver_type: dexuni` (`joint_mode: dynamic`, `contact_mode: auto`, `use_mujoco_cpu: false`, `use_mujoco_contacts: false`) and Newton explicit heightfield contact generation (`broad_phase: explicit`, `reduce_contacts: true`, `max_triangle_pairs: 2500000`). Simulation runs 0.005 s outer physics steps, two solver substeps, four outer steps per control interval (50 Hz control), 100 Newton solver iterations, and 50 line-search iterations. + +The task loads the 37-DOF `UnitreeG1Rough` robot archive (`g1.usda`) and the `G1RoughTerrains` curriculum (`training/terrain.npz`). The 29-DOF `UnitreeG1Locomotion` flat-terrain asset remains unchanged. + +```bash +embodichain show-task embodichain_tasks:g1_rough +embodichain run-task --gym_config extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/env.yaml +embodichain train-rl --config extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/agents/ppo.yaml +``` + +## Evaluation + +Evaluate a saved checkpoint with its training run directory: + +```bash +embodichain eval-policy outputs/unitree_g1_rough_ppo/ --checkpoint latest --num-envs 64 --episodes 64 --renderer no-render +``` + +Check linear/angular velocity command tracking, upright posture, tilt terminations, terrain curriculum progression, and episode survival. Terminal success is not applicable to this continuous velocity-tracking task. diff --git a/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/agents/ppo.yaml b/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/agents/ppo.yaml new file mode 100644 index 0000000000..5b54ff7488 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/agents/ppo.yaml @@ -0,0 +1,59 @@ +trainer: + exp_name: unitree_g1_rough_ppo + gym_config: extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/env.yaml + seed: 42 + device: cuda:0 + headless: true + num_envs: 4096 + iterations: 3000 + buffer_size: 24 + enable_eval: false + save_frequency_updates: 100 + use_wandb: false + wandb_project_name: embodichain-locomotion +policy: + name: actor_critic + actor_obs_normalization: true + critic_obs_normalization: true + obs_groups: + actor: + - policy + critic: + - critic + actor: + type: mlp + network_cfg: + hidden_sizes: + - 512 + - 256 + - 128 + activation: elu + critic: + type: mlp + network_cfg: + hidden_sizes: + - 512 + - 256 + - 128 + activation: elu + initial_action_std: 1.0 +algorithm: + name: ppo + cfg: + optimizer: + name: adam + learning_rate: 0.001 + schedule: adaptive + desired_kl: 0.01 + minimum_learning_rate: 1.0e-05 + maximum_learning_rate: 0.01 + n_epochs: 5 + num_mini_batches: 4 + batch_size: 24576 + gamma: 0.99 + gae_lambda: 0.95 + clip_coef: 0.2 + ent_coef: 0.01 + vf_coef: 1.0 + use_clipped_value_loss: true + max_grad_norm: 1.0 diff --git a/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/catalog.yaml b/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/catalog.yaml new file mode 100644 index 0000000000..770e5709eb --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/catalog.yaml @@ -0,0 +1,12 @@ +task_key: g1_rough +title: Unitree G1 Rough-Terrain Locomotion +summary: Track planar velocity and yaw-rate commands with the 37-DOF Unitree G1 humanoid + across a heightfield terrain curriculum. +tags: +- locomotion +- reinforcement_learning +- humanoid +default_deployment: default +deployments: + default: + config: env.yaml diff --git a/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/env.yaml b/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/env.yaml new file mode 100644 index 0000000000..e9c1afe057 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/env.yaml @@ -0,0 +1,198 @@ +physics: newton +id: UnitreeG1RoughRL-v1 +max_episode_steps: 1000 +num_envs: 8 +scene_node_capacity: 262144 +headless: true +device: cuda:0 +arena_space: 0.0 +render_cfg: + renderer: no-render +physics_config: + mesh_bvh_constructor: cubql + sync_to_renderer: false + gravity: + - 0.0 + - 0.0 + - -9.81 + num_substeps: 2 + physics_dt: 0.005 + collision_cfg: + broad_phase: explicit + reduce_contacts: true + max_triangle_pairs: 2500000 + update_interval: null + solver_cfg: + solver_type: dexuni + joint_mode: dynamic + contact_mode: auto + mujoco_options: + use_mujoco_cpu: false + use_mujoco_contacts: false + njmax: 300 + nconmax: 300 + iterations: 100 + ls_iterations: 50 + integrator: implicitfast + separate_worlds: true + tolerance: 1.0e-06 + solver: newton + cone: pyramidal + impratio: 1.0 + ccd_iterations: 35 + collision_options: + broad_phase: explicit + reduce_contacts: true + max_triangle_pairs: 2500000 +env: + episode_context_in_info: false + sim_steps_per_control: 4 + control_parts: + - locomotion + events: {} + observations: + policy: + func: velocity_locomotion_observation + mode: add + name: policy + params: + privileged: false + enable_corruption: false + critic: + func: velocity_locomotion_observation + mode: add + name: critic + params: + privileged: true + enable_corruption: false + rewards: + total: + func: velocity_locomotion_total_reward + mode: add + weight: 1.0 + params: {} + actions: + joint_position: + contract: joint_position.default_offset@1 + params: {} + extensions: + deterministic_reset: false + randomization: true + origin: + - 0.0 + - 0.0 + - 0.0 + terrain: G1RoughTerrains/training/terrain.npz +robot: + uid: robot + fpath: UnitreeG1Rough/g1.usda + build_pk_chain: false + init_pos: + - 0.0 + - 0.0 + - 0.7400000095367432 + init_qpos: + - -0.20000000298023224 + - 0.0 + - 0.0 + - 0.41999998688697815 + - -0.23000000417232513 + - 0.0 + - -0.20000000298023224 + - 0.0 + - 0.0 + - 0.41999998688697815 + - -0.23000000417232513 + - 0.0 + - 0.0 + - 0.3499999940395355 + - 0.1599999964237213 + - 0.0 + - 0.8700000047683716 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 1.0 + - 0.5199999809265137 + - 0.3499999940395355 + - -0.1599999964237213 + - 0.0 + - 0.8700000047683716 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - 0.0 + - -1.0 + - -0.5199999809265137 + control_parts: + locomotion: + - left_hip_pitch_joint + - left_hip_roll_joint + - left_hip_yaw_joint + - left_knee_joint + - left_ankle_pitch_joint + - left_ankle_roll_joint + - right_hip_pitch_joint + - right_hip_roll_joint + - right_hip_yaw_joint + - right_knee_joint + - right_ankle_pitch_joint + - right_ankle_roll_joint + - torso_joint + - left_shoulder_pitch_joint + - left_shoulder_roll_joint + - left_shoulder_yaw_joint + - left_elbow_pitch_joint + - left_elbow_roll_joint + - left_five_joint + - left_six_joint + - left_three_joint + - left_four_joint + - left_zero_joint + - left_one_joint + - left_two_joint + - right_shoulder_pitch_joint + - right_shoulder_roll_joint + - right_shoulder_yaw_joint + - right_elbow_pitch_joint + - right_elbow_roll_joint + - right_five_joint + - right_six_joint + - right_three_joint + - right_four_joint + - right_zero_joint + - right_one_joint + - right_two_joint + asset_physics_mode: overlay + root_props: + fixed_base: false + self_collision_enabled: false + min_position_iters: 4 + min_velocity_iters: 0 + attrs: + collision_props: + preserve_mesh_transform: true + margin: 0.0 + gap: 0.01 + material_props: + ke: 160000.0 + kd: 1100.0 + dynamic_friction: 0.8 +sensor: +- uid: locomotion_contacts + sensor_type: ContactSensor + articulation_cfg_list: + - articulation_uid: robot + link_name_list: [] + filter_need_both_actor: false + max_contacts_per_env: 512 +light: {} +background: [] +rigid_object: [] +rigid_object_group: [] +articulation: [] diff --git a/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/task.json b/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/task.json new file mode 100644 index 0000000000..eeec297f30 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/task.json @@ -0,0 +1,1222 @@ +{ + "schema": "unitree-velocity-task", + "official_task": "Unitree-G1-Rough", + "robot": { + "joint_names": [ + "left_hip_pitch_joint", + "left_hip_roll_joint", + "left_hip_yaw_joint", + "left_knee_joint", + "left_ankle_pitch_joint", + "left_ankle_roll_joint", + "right_hip_pitch_joint", + "right_hip_roll_joint", + "right_hip_yaw_joint", + "right_knee_joint", + "right_ankle_pitch_joint", + "right_ankle_roll_joint", + "torso_joint", + "left_shoulder_pitch_joint", + "left_shoulder_roll_joint", + "left_shoulder_yaw_joint", + "left_elbow_pitch_joint", + "left_elbow_roll_joint", + "left_five_joint", + "left_six_joint", + "left_three_joint", + "left_four_joint", + "left_zero_joint", + "left_one_joint", + "left_two_joint", + "right_shoulder_pitch_joint", + "right_shoulder_roll_joint", + "right_shoulder_yaw_joint", + "right_elbow_pitch_joint", + "right_elbow_roll_joint", + "right_five_joint", + "right_six_joint", + "right_three_joint", + "right_four_joint", + "right_zero_joint", + "right_one_joint", + "right_two_joint" + ], + "body_names": [ + "pelvis", + "left_hip_pitch_link", + "left_hip_roll_link", + "left_hip_yaw_link", + "left_knee_link", + "left_ankle_pitch_link", + "left_ankle_roll_link", + "pelvis_contour_link", + "right_hip_pitch_link", + "right_hip_roll_link", + "right_hip_yaw_link", + "right_knee_link", + "right_ankle_pitch_link", + "right_ankle_roll_link", + "torso_link", + "head_link", + "imu_link", + "left_shoulder_pitch_link", + "left_shoulder_roll_link", + "left_shoulder_yaw_link", + "left_elbow_pitch_link", + "left_elbow_roll_link", + "left_palm_link", + "left_five_link", + "left_six_link", + "left_three_link", + "left_four_link", + "left_zero_link", + "left_one_link", + "left_two_link", + "logo_link", + "right_shoulder_pitch_link", + "right_shoulder_roll_link", + "right_shoulder_yaw_link", + "right_elbow_pitch_link", + "right_elbow_roll_link", + "right_palm_link", + "right_five_link", + "right_six_link", + "right_three_link", + "right_four_link", + "right_zero_link", + "right_one_link", + "right_two_link" + ], + "default_joint_position": [ + -0.20000000298023224, + 0.0, + 0.0, + 0.41999998688697815, + -0.23000000417232513, + 0.0, + -0.20000000298023224, + 0.0, + 0.0, + 0.41999998688697815, + -0.23000000417232513, + 0.0, + 0.0, + 0.3499999940395355, + 0.1599999964237213, + 0.0, + 0.8700000047683716, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 1.0, + 0.5199999809265137, + 0.3499999940395355, + -0.1599999964237213, + 0.0, + 0.8700000047683716, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + -1.0, + -0.5199999809265137 + ], + "action_scale": [ + 0.5, + 0.5, + 0.5, + 0.5, + 0.5, + 0.5, + 0.5, + 0.5, + 0.5, + 0.5, + 0.5, + 0.5, + 0.5, + 0.5, + 0.5, + 0.5, + 0.5, + 0.5, + 0.5, + 0.5, + 0.5, + 0.5, + 0.5, + 0.5, + 0.5, + 0.5, + 0.5, + 0.5, + 0.5, + 0.5, + 0.5, + 0.5, + 0.5, + 0.5, + 0.5, + 0.5, + 0.5 + ], + "root_height": 0.7400000095367432, + "soft_joint_position_limit_factor": 0.9, + "stiffness": [ + 200.0, + 150.0, + 150.0, + 200.0, + 20.0, + 20.0, + 200.0, + 150.0, + 150.0, + 200.0, + 20.0, + 20.0, + 200.0, + 40.0, + 40.0, + 40.0, + 40.0, + 40.0, + 40.0, + 40.0, + 40.0, + 40.0, + 40.0, + 40.0, + 40.0, + 40.0, + 40.0, + 40.0, + 40.0, + 40.0, + 40.0, + 40.0, + 40.0, + 40.0, + 40.0, + 40.0, + 40.0 + ], + "damping": [ + 5.0, + 5.0, + 5.0, + 5.0, + 2.0, + 2.0, + 5.0, + 5.0, + 5.0, + 5.0, + 2.0, + 2.0, + 5.0, + 10.0, + 10.0, + 10.0, + 10.0, + 10.0, + 10.0, + 10.0, + 10.0, + 10.0, + 10.0, + 10.0, + 10.0, + 10.0, + 10.0, + 10.0, + 10.0, + 10.0, + 10.0, + 10.0, + 10.0, + 10.0, + 10.0, + 10.0, + 10.0 + ], + "effort_limit": [ + 300.0, + 300.0, + 300.0, + 300.0, + 20.0, + 20.0, + 300.0, + 300.0, + 300.0, + 300.0, + 20.0, + 20.0, + 300.0, + 300.0, + 300.0, + 300.0, + 300.0, + 300.0, + 300.0, + 300.0, + 300.0, + 300.0, + 300.0, + 300.0, + 300.0, + 300.0, + 300.0, + 300.0, + 300.0, + 300.0, + 300.0, + 300.0, + 300.0, + 300.0, + 300.0, + 300.0, + 300.0 + ], + "velocity_limit": [ + 32.0, + 32.0, + 32.0, + 20.000001907348633, + 53.0, + 53.0, + 32.0, + 32.0, + 32.0, + 20.000001907348633, + 53.0, + 53.0, + 32.0, + 53.0, + 53.0, + 53.0, + 53.0, + 53.0, + 23.0, + 23.0, + 23.0, + 23.0, + 23.0, + 23.0, + 23.0, + 53.0, + 53.0, + 53.0, + 53.0, + 53.0, + 23.0, + 23.0, + 23.0, + 23.0, + 23.0, + 23.0, + 23.0 + ], + "armature": [ + 0.009999999776482582, + 0.009999999776482582, + 0.009999999776482582, + 0.009999999776482582, + 0.009999999776482582, + 0.009999999776482582, + 0.009999999776482582, + 0.009999999776482582, + 0.009999999776482582, + 0.009999999776482582, + 0.009999999776482582, + 0.009999999776482582, + 0.009999999776482582, + 0.009999999776482582, + 0.009999999776482582, + 0.009999999776482582, + 0.009999999776482582, + 0.009999999776482582, + 0.0010000000474974513, + 0.0010000000474974513, + 0.0010000000474974513, + 0.0010000000474974513, + 0.0010000000474974513, + 0.0010000000474974513, + 0.0010000000474974513, + 0.009999999776482582, + 0.009999999776482582, + 0.009999999776482582, + 0.009999999776482582, + 0.009999999776482582, + 0.0010000000474974513, + 0.0010000000474974513, + 0.0010000000474974513, + 0.0010000000474974513, + 0.0010000000474974513, + 0.0010000000474974513, + 0.0010000000474974513 + ], + "soft_joint_pos_limits": [ + [ + -2.0799996852874756, + 2.7799999713897705 + ], + [ + -0.12049996852874756, + 2.3904995918273926 + ], + [ + -2.4749996662139893, + 2.4749996662139893 + ], + [ + -0.19090032577514648, + 2.4009101390838623 + ], + [ + -0.6094999313354492, + 0.6594998836517334 + ], + [ + -0.23561996221542358, + 0.23561996221542358 + ], + [ + -2.0799996852874756, + 2.7799999713897705 + ], + [ + -2.3904995918273926, + 0.12049996852874756 + ], + [ + -2.4749996662139893, + 2.4749996662139893 + ], + [ + -0.19090032577514648, + 2.4009101390838623 + ], + [ + -0.6094999313354492, + 0.6594998836517334 + ], + [ + -0.23561996221542358, + 0.23561996221542358 + ], + [ + -2.3561999797821045, + 2.3561999797821045 + ], + [ + -2.67911958694458, + 2.5045199394226074 + ], + [ + -1.3962149620056152, + 2.0595147609710693 + ], + [ + -2.3561999797821045, + 2.3561999797821045 + ], + [ + -0.04441988468170166, + 3.238419532775879 + ], + [ + -1.8848698139190674, + 1.8848698139190674 + ], + [ + -1.7329999208450317, + 0.1929999589920044 + ], + [ + -1.747999906539917, + -0.09200000762939453 + ], + [ + -1.7329999208450317, + 0.1929999589920044 + ], + [ + -1.747999906539917, + -0.09200000762939453 + ], + [ + -0.4712381362915039, + 0.4712381362915039 + ], + [ + -0.8899999856948853, + 1.090000033378601 + ], + [ + 0.09200000762939453, + 1.747999906539917 + ], + [ + -2.67911958694458, + 2.5045199394226074 + ], + [ + -2.0595147609710693, + 1.3962149620056152 + ], + [ + -2.3561999797821045, + 2.3561999797821045 + ], + [ + -0.04441988468170166, + 3.238419532775879 + ], + [ + -1.8848698139190674, + 1.8848698139190674 + ], + [ + -0.1929999589920044, + 1.7329999208450317 + ], + [ + 0.09200000762939453, + 1.747999906539917 + ], + [ + -0.1929999589920044, + 1.7329999208450317 + ], + [ + 0.09200000762939453, + 1.747999906539917 + ], + [ + -0.4712381362915039, + 0.4712381362915039 + ], + [ + -1.090000033378601, + 0.8899999856948853 + ], + [ + -1.747999906539917, + -0.09200000762939453 + ] + ] + }, + "physics": { + "physics_dt": 0.005, + "control_dt": 0.02, + "episode_length_s": 20.0, + "num_substeps": 2 + }, + "observations": { + "base_lin_vel": { + "noise": { + "n_min": -0.1, + "n_max": 0.1 + }, + "clip": null, + "scale": null + }, + "base_ang_vel": { + "noise": { + "n_min": -0.2, + "n_max": 0.2 + }, + "clip": null, + "scale": null + }, + "projected_gravity": { + "noise": { + "n_min": -0.05, + "n_max": 0.05 + }, + "clip": null, + "scale": null + }, + "velocity_commands": { + "noise": null, + "clip": null, + "scale": null + }, + "joint_pos": { + "noise": { + "n_min": -0.01, + "n_max": 0.01 + }, + "clip": null, + "scale": null + }, + "joint_vel": { + "noise": { + "n_min": -1.5, + "n_max": 1.5 + }, + "clip": null, + "scale": null + }, + "actions": { + "noise": null, + "clip": null, + "scale": null + }, + "height_scan": { + "noise": { + "n_min": -0.1, + "n_max": 0.1 + }, + "clip": [ + -1.0, + 1.0 + ], + "scale": null + } + }, + "rewards": { + "track_lin_vel_xy_exp": { + "function": "track_lin_vel_xy_yaw_frame_exp", + "params": { + "std": 0.5 + }, + "joint_ids": [ + 0, + 1, + 2, + 3, + 4, + 5, + 6, + 7, + 8, + 9, + 10, + 11, + 12, + 13, + 14, + 15, + 16, + 17, + 18, + 19, + 20, + 21, + 22, + 23, + 24, + 25, + 26, + 27, + 28, + 29, + 30, + 31, + 32, + 33, + 34, + 35, + 36 + ], + "weight": 1.0 + }, + "track_ang_vel_z_exp": { + "function": "track_ang_vel_z_world_exp", + "params": { + "std": 0.5 + }, + "joint_ids": [ + 0, + 1, + 2, + 3, + 4, + 5, + 6, + 7, + 8, + 9, + 10, + 11, + 12, + 13, + 14, + 15, + 16, + 17, + 18, + 19, + 20, + 21, + 22, + 23, + 24, + 25, + 26, + 27, + 28, + 29, + 30, + 31, + 32, + 33, + 34, + 35, + 36 + ], + "weight": 2.0 + }, + "ang_vel_xy_l2": { + "function": "ang_vel_xy_l2", + "params": {}, + "joint_ids": [ + 0, + 1, + 2, + 3, + 4, + 5, + 6, + 7, + 8, + 9, + 10, + 11, + 12, + 13, + 14, + 15, + 16, + 17, + 18, + 19, + 20, + 21, + 22, + 23, + 24, + 25, + 26, + 27, + 28, + 29, + 30, + 31, + 32, + 33, + 34, + 35, + 36 + ], + "weight": -0.05 + }, + "dof_torques_l2": { + "function": "joint_torques_l2", + "params": {}, + "joint_ids": [ + 0, + 1, + 2, + 3, + 4, + 5, + 6, + 7, + 8, + 9, + 10, + 11 + ], + "weight": -1.5e-07 + }, + "dof_acc_l2": { + "function": "joint_acc_l2", + "params": {}, + "joint_ids": [ + 0, + 1, + 2, + 3, + 6, + 7, + 8, + 9 + ], + "weight": -1.25e-07 + }, + "action_rate_l2": { + "function": "action_rate_l2", + "params": {}, + "joint_ids": [ + 0, + 1, + 2, + 3, + 4, + 5, + 6, + 7, + 8, + 9, + 10, + 11, + 12, + 13, + 14, + 15, + 16, + 17, + 18, + 19, + 20, + 21, + 22, + 23, + 24, + 25, + 26, + 27, + 28, + 29, + 30, + 31, + 32, + 33, + 34, + 35, + 36 + ], + "weight": -0.005 + }, + "feet_air_time": { + "function": "feet_air_time_positive_biped", + "params": { + "threshold": 0.4 + }, + "joint_ids": [ + 0, + 1, + 2, + 3, + 4, + 5, + 6, + 7, + 8, + 9, + 10, + 11, + 12, + 13, + 14, + 15, + 16, + 17, + 18, + 19, + 20, + 21, + 22, + 23, + 24, + 25, + 26, + 27, + 28, + 29, + 30, + 31, + 32, + 33, + 34, + 35, + 36 + ], + "weight": 0.25 + }, + "flat_orientation_l2": { + "function": "flat_orientation_l2", + "params": {}, + "joint_ids": [ + 0, + 1, + 2, + 3, + 4, + 5, + 6, + 7, + 8, + 9, + 10, + 11, + 12, + 13, + 14, + 15, + 16, + 17, + 18, + 19, + 20, + 21, + 22, + 23, + 24, + 25, + 26, + 27, + 28, + 29, + 30, + 31, + 32, + 33, + 34, + 35, + 36 + ], + "weight": -1.0 + }, + "dof_pos_limits": { + "function": "joint_pos_limits", + "params": {}, + "joint_ids": [ + 4, + 5, + 10, + 11 + ], + "weight": -1.0 + }, + "termination_penalty": { + "function": "is_terminated", + "params": {}, + "joint_ids": [ + 0, + 1, + 2, + 3, + 4, + 5, + 6, + 7, + 8, + 9, + 10, + 11, + 12, + 13, + 14, + 15, + 16, + 17, + 18, + 19, + 20, + 21, + 22, + 23, + 24, + 25, + 26, + 27, + 28, + 29, + 30, + 31, + 32, + 33, + 34, + 35, + 36 + ], + "weight": -200.0 + }, + "feet_slide": { + "function": "feet_slide", + "params": {}, + "joint_ids": [ + 0, + 1, + 2, + 3, + 4, + 5, + 6, + 7, + 8, + 9, + 10, + 11, + 12, + 13, + 14, + 15, + 16, + 17, + 18, + 19, + 20, + 21, + 22, + 23, + 24, + 25, + 26, + 27, + 28, + 29, + 30, + 31, + 32, + 33, + 34, + 35, + 36 + ], + "weight": -0.1 + }, + "joint_deviation_hip": { + "function": "joint_deviation_l1", + "params": {}, + "joint_ids": [ + 1, + 2, + 7, + 8 + ], + "weight": -0.1 + }, + "joint_deviation_arms": { + "function": "joint_deviation_l1", + "params": {}, + "joint_ids": [ + 13, + 14, + 15, + 16, + 17, + 25, + 26, + 27, + 28, + 29 + ], + "weight": -0.1 + }, + "joint_deviation_fingers": { + "function": "joint_deviation_l1", + "params": {}, + "joint_ids": [ + 18, + 19, + 20, + 21, + 22, + 23, + 24, + 30, + 31, + 32, + 33, + 34, + 35, + 36 + ], + "weight": -0.05 + }, + "joint_deviation_torso": { + "function": "joint_deviation_l1", + "params": {}, + "joint_ids": [ + 12 + ], + "weight": -0.1 + } + }, + "commands": { + "twist": { + "resampling_time_range": [ + 10.0, + 10.0 + ], + "heading_command": true, + "heading_control_stiffness": 0.5, + "rel_standing_envs": 0.02, + "rel_heading_envs": 1.0, + "ranges": { + "lin_vel_x": [ + 0.0, + 1.0 + ], + "lin_vel_y": [ + -0.0, + 0.0 + ], + "ang_vel_z": [ + -1.0, + 1.0 + ], + "heading": [ + -3.141592653589793, + 3.141592653589793 + ] + }, + "vel_xy_success_threshold": 0.5, + "vel_yaw_success_threshold": 0.8 + } + }, + "events": { + "physics_material": { + "params": { + "static_friction_range": [ + 0.8, + 0.8 + ], + "dynamic_friction_range": [ + 0.6, + 0.6 + ], + "restitution_range": [ + 0.0, + 0.0 + ], + "num_buckets": 64 + }, + "body_names": ".*", + "mode": "startup", + "interval_range_s": null + }, + "add_base_mass": { + "params": { + "mass_distribution_params": [ + 0.8, + 1.25 + ], + "operation": "scale", + "distribution": "log_uniform" + }, + "body_names": "torso_link", + "mode": "startup", + "interval_range_s": null + }, + "base_external_force_torque": { + "params": { + "force_range": [ + 0.0, + 0.0 + ], + "torque_range": [ + -0.0, + 0.0 + ] + }, + "body_names": "torso_link", + "mode": "reset", + "interval_range_s": null + }, + "reset_base": { + "params": { + "pose_range": { + "x": [ + -0.5, + 0.5 + ], + "y": [ + -0.5, + 0.5 + ], + "yaw": [ + -3.14, + 3.14 + ], + "z": [ + 0.0, + 0.0 + ] + }, + "velocity_range": { + "x": [ + -0.5, + 0.5 + ], + "y": [ + -0.5, + 0.5 + ], + "z": [ + -0.5, + 0.5 + ], + "roll": [ + -0.5, + 0.5 + ], + "pitch": [ + -0.5, + 0.5 + ], + "yaw": [ + -0.5, + 0.5 + ] + } + }, + "body_names": null, + "mode": "reset", + "interval_range_s": null + }, + "reset_robot_joints": { + "params": { + "position_range": [ + 1.0, + 1.0 + ], + "velocity_range": [ + 0.0, + 0.0 + ] + }, + "body_names": null, + "mode": "reset", + "interval_range_s": null + }, + "push_robot": { + "params": { + "velocity_range": { + "x": [ + -0.5, + 0.5 + ], + "y": [ + -0.5, + 0.5 + ] + } + }, + "body_names": null, + "mode": "interval", + "interval_range_s": [ + 10.0, + 15.0 + ] + } + }, + "feet": [ + "left_ankle_roll_link", + "right_ankle_roll_link" + ], + "illegal_bodies": [ + "torso_link" + ] +} diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_face_turn/README.md b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_face_turn/README.md new file mode 100644 index 0000000000..aee086b69e --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_face_turn/README.md @@ -0,0 +1,22 @@ +# LEAP Hand Cube Face Turn + +Turn the top articulated layer of a Rubik-style cube to discrete 90-degree targets with the 16-DOF LEAP Hand. +Gym ID: `LeapCubeFaceTurnRL-v1`. + +[env.yaml](env.yaml) and [agents/ppo.yaml](agents/ppo.yaml) configure the `default` catalog deployment. The scene loads directly from `LeapHandCubeFaceTurn` (`usd/leap_cube_top_layer.usdc`) via `SimulationManager.add_usd()`, running `physics: newton` with `solver_type: dexuni` on GPU MuJoCo Warp CUDA (`use_mujoco_cpu: false`, `use_mujoco_contacts: true`). + +```bash +embodichain show-task embodichain_tasks:leap_cube_face_turn +embodichain run-task --gym_config extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_face_turn/env.yaml +embodichain train-rl --config extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_face_turn/agents/ppo.yaml +``` + +## Evaluation + +Evaluate a saved checkpoint with its training run directory: + +```bash +embodichain eval-policy outputs/leap_cube_face_turn_ppo/ --checkpoint latest --episodes 32 --num-envs 32 --renderer no-render +``` + +Episode `info["success"]` records whether at least one target milestone was achieved before termination or truncation. diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_face_turn/agents/ppo.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_face_turn/agents/ppo.yaml new file mode 100644 index 0000000000..042a7a778c --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_face_turn/agents/ppo.yaml @@ -0,0 +1,55 @@ +trainer: + exp_name: leap_cube_face_turn_ppo + gym_config: extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_face_turn/env.yaml + seed: 42 + device: cuda:0 + headless: true + num_envs: 4096 + iterations: 430 + buffer_size: 40 + enable_eval: false + save_frequency_updates: 100 + use_wandb: false + wandb_project_name: embodichain-dexterous +policy: + name: actor_critic + actor_obs_normalization: true + critic_obs_normalization: true + obs_groups: + actor: + - policy + critic: + - policy + actor: + type: mlp + network_cfg: + hidden_sizes: + - 512 + - 256 + - 128 + activation: elu + critic: + type: mlp + network_cfg: + hidden_sizes: + - 512 + - 256 + - 128 + activation: elu + initial_action_std: 0.5 +algorithm: + name: ppo + cfg: + optimizer: + name: adam + learning_rate: 0.0003 + n_epochs: 4 + num_mini_batches: 32 + batch_size: 5120 + gamma: 0.99 + gae_lambda: 0.95 + clip_coef: 0.3 + ent_coef: 0.01 + vf_coef: 0.5 + use_clipped_value_loss: true + max_grad_norm: 1.0 diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_face_turn/catalog.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_face_turn/catalog.yaml new file mode 100644 index 0000000000..4277a56390 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_face_turn/catalog.yaml @@ -0,0 +1,12 @@ +task_key: leap_cube_face_turn +title: LEAP Hand Cube Face Turn +summary: Turn the top articulated layer of a Rubik-style cube to discrete 90-degree + targets with the 16-DOF LEAP Hand. +tags: +- manipulation +- dexterous +- reinforcement_learning +default_deployment: default +deployments: + default: + config: env.yaml diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_face_turn/env.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_face_turn/env.yaml new file mode 100644 index 0000000000..1688c0d02f --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_face_turn/env.yaml @@ -0,0 +1,59 @@ +physics: newton +scene_node_capacity: 262144 +id: LeapCubeFaceTurnRL-v1 +max_episode_steps: 500 +num_envs: 8 +headless: true +device: cuda:0 +arena_space: 0.0 +physics_config: + sync_to_renderer: false + gravity: + - 0.0 + - 0.0 + - -9.81 + num_substeps: 1 + collision_cfg: null + solver_cfg: + solver_type: dexuni + joint_mode: dynamic + contact_mode: auto + mujoco_options: + use_mujoco_cpu: false + use_mujoco_contacts: true + update_data_interval: 0 + separate_worlds: true + solver: newton + integrator: euler + cone: pyramidal + impratio: 1.0 + iterations: 5 + ls_iterations: 8 + ccd_iterations: 10 + tolerance: 1.0e-08 + ls_tolerance: 0.01 + ccd_tolerance: 1.0e-06 + njmax: 2048 + nconmax: 256 + physics_dt: 0.005 +env: + episode_context_in_info: false + sim_steps_per_control: 10 + control_parts: + - hand + events: {} + observations: {} + rewards: {} + actions: {} + extensions: {} +robot: + uid: leap_hand + build_pk_chain: false + asset_physics_mode: preserve + source_pose_mode: preserve +sensor: [] +light: {} +background: [] +rigid_object: [] +rigid_object_group: [] +articulation: [] diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_reorient/README.md b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_reorient/README.md new file mode 100644 index 0000000000..af9a62ae19 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_reorient/README.md @@ -0,0 +1,22 @@ +# LEAP Hand Cube Reorientation + +Reorient a rigid cube in-hand to consecutive 3D target orientations with the 16-DOF LEAP Hand. +Gym ID: `LeapCubeReorientRL-v1`. + +[env.yaml](env.yaml) and [agents/ppo.yaml](agents/ppo.yaml) configure the `default` catalog deployment. The scene loads directly from `LeapHandCube` (`usd/M4.usdc`) via `SimulationManager.add_usd()`, running `physics: newton` with `solver_type: dexuni` on GPU MuJoCo Warp CUDA (`use_mujoco_cpu: false`, `use_mujoco_contacts: true`). + +```bash +embodichain show-task embodichain_tasks:leap_cube_reorient +embodichain run-task --gym_config extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_reorient/env.yaml +embodichain train-rl --config extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_reorient/agents/ppo.yaml +``` + +## Evaluation + +Evaluate a saved checkpoint with its training run directory: + +```bash +embodichain eval-policy outputs/leap_cube_reorient_ppo/ --checkpoint latest --episodes 32 --num-envs 32 --renderer no-render +``` + +Episode `info["success"]` records whether at least one target milestone was achieved before termination or truncation. diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_reorient/agents/ppo.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_reorient/agents/ppo.yaml new file mode 100644 index 0000000000..a773ded6eb --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_reorient/agents/ppo.yaml @@ -0,0 +1,59 @@ +trainer: + exp_name: leap_cube_reorient_ppo + gym_config: extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_reorient/env.yaml + seed: 42 + device: cuda:0 + headless: true + num_envs: 4096 + iterations: 920 + buffer_size: 40 + enable_eval: false + save_frequency_updates: 100 + use_wandb: false + wandb_project_name: embodichain-dexterous +policy: + name: actor_critic + actor_obs_normalization: true + critic_obs_normalization: true + obs_groups: + actor: + - policy + critic: + - critic + actor: + type: mlp + network_cfg: + hidden_sizes: + - 512 + - 256 + - 128 + activation: elu + critic: + type: mlp + network_cfg: + hidden_sizes: + - 512 + - 256 + - 128 + activation: elu + initial_action_std: 0.5 +algorithm: + name: ppo + cfg: + optimizer: + name: adam + learning_rate: 0.0001 + schedule: adaptive + desired_kl: 0.02 + minimum_learning_rate: 1.0e-05 + maximum_learning_rate: 0.0003 + n_epochs: 4 + num_mini_batches: 32 + batch_size: 5120 + gamma: 0.99 + gae_lambda: 0.95 + clip_coef: 0.3 + ent_coef: 0.001 + vf_coef: 0.5 + use_clipped_value_loss: true + max_grad_norm: 1.0 diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_reorient/catalog.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_reorient/catalog.yaml new file mode 100644 index 0000000000..ba977c44a7 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_reorient/catalog.yaml @@ -0,0 +1,12 @@ +task_key: leap_cube_reorient +title: LEAP Hand Cube Reorientation +summary: Reorient a rigid cube in-hand to consecutive 3D target orientations with + the 16-DOF LEAP Hand. +tags: +- manipulation +- dexterous +- reinforcement_learning +default_deployment: default +deployments: + default: + config: env.yaml diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_reorient/env.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_reorient/env.yaml new file mode 100644 index 0000000000..ee3814f23a --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_reorient/env.yaml @@ -0,0 +1,59 @@ +physics: newton +scene_node_capacity: 262144 +id: LeapCubeReorientRL-v1 +max_episode_steps: 1000 +num_envs: 8 +headless: true +device: cuda:0 +arena_space: 0.0 +physics_config: + sync_to_renderer: false + gravity: + - 0.0 + - 0.0 + - -9.81 + num_substeps: 1 + collision_cfg: null + solver_cfg: + solver_type: dexuni + joint_mode: dynamic + contact_mode: auto + mujoco_options: + use_mujoco_cpu: false + use_mujoco_contacts: true + update_data_interval: 0 + separate_worlds: true + solver: newton + integrator: euler + cone: pyramidal + impratio: 1.0 + iterations: 5 + ls_iterations: 8 + ccd_iterations: 10 + tolerance: 1.0e-08 + ls_tolerance: 0.01 + ccd_tolerance: 1.0e-06 + njmax: 512 + nconmax: 256 + physics_dt: 0.01 +env: + episode_context_in_info: false + sim_steps_per_control: 5 + control_parts: + - hand + events: {} + observations: {} + rewards: {} + actions: {} + extensions: {} +robot: + uid: leap_hand + build_pk_chain: false + asset_physics_mode: preserve + source_pose_mode: preserve +sensor: [] +light: {} +background: [] +rigid_object: [] +rigid_object_group: [] +articulation: [] diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_rotate_z/README.md b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_rotate_z/README.md new file mode 100644 index 0000000000..aec5c75063 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_rotate_z/README.md @@ -0,0 +1,22 @@ +# LEAP Hand Cube Z-Axis Rotation + +Continuously rotate a rigid cube around the world Z-axis in-hand with the 16-DOF LEAP Hand. +Gym ID: `LeapCubeRotateZAxisRL-v1`. + +[env.yaml](env.yaml) and [agents/ppo.yaml](agents/ppo.yaml) configure the `default` catalog deployment. The scene loads directly from `LeapHandCube` (`usd/M5.usdc`) via `SimulationManager.add_usd()`, running `physics: newton` with `solver_type: dexuni` on GPU MuJoCo Warp CUDA (`use_mujoco_cpu: false`, `use_mujoco_contacts: true`). + +```bash +embodichain show-task embodichain_tasks:leap_cube_rotate_z +embodichain run-task --gym_config extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_rotate_z/env.yaml +embodichain train-rl --config extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_rotate_z/agents/ppo.yaml +``` + +## Evaluation + +Evaluate a saved checkpoint with its training run directory: + +```bash +embodichain eval-policy outputs/leap_cube_rotate_z_ppo/ --checkpoint latest --episodes 32 --num-envs 32 --renderer no-render +``` + +Continuous rotation has no binary target threshold; `info["success"]` is omitted from episode evaluation. diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_rotate_z/agents/ppo.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_rotate_z/agents/ppo.yaml new file mode 100644 index 0000000000..b9443c1dd1 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_rotate_z/agents/ppo.yaml @@ -0,0 +1,55 @@ +trainer: + exp_name: leap_cube_rotate_z_ppo + gym_config: extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_rotate_z/env.yaml + seed: 42 + device: cuda:0 + headless: true + num_envs: 4096 + iterations: 650 + buffer_size: 40 + enable_eval: false + save_frequency_updates: 100 + use_wandb: false + wandb_project_name: embodichain-dexterous +policy: + name: actor_critic + actor_obs_normalization: true + critic_obs_normalization: true + obs_groups: + actor: + - policy + critic: + - critic + actor: + type: mlp + network_cfg: + hidden_sizes: + - 512 + - 256 + - 128 + activation: elu + critic: + type: mlp + network_cfg: + hidden_sizes: + - 512 + - 256 + - 128 + activation: elu + initial_action_std: 0.5 +algorithm: + name: ppo + cfg: + optimizer: + name: adam + learning_rate: 0.0003 + n_epochs: 4 + num_mini_batches: 32 + batch_size: 5120 + gamma: 0.97 + gae_lambda: 0.95 + clip_coef: 0.3 + ent_coef: 0.01 + vf_coef: 0.5 + use_clipped_value_loss: true + max_grad_norm: 1.0 diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_rotate_z/catalog.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_rotate_z/catalog.yaml new file mode 100644 index 0000000000..c5546261fe --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_rotate_z/catalog.yaml @@ -0,0 +1,12 @@ +task_key: leap_cube_rotate_z +title: LEAP Hand Cube Z-Axis Rotation +summary: Continuously rotate a rigid cube around the world Z-axis in-hand with the + 16-DOF LEAP Hand. +tags: +- manipulation +- dexterous +- reinforcement_learning +default_deployment: default +deployments: + default: + config: env.yaml diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_rotate_z/env.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_rotate_z/env.yaml new file mode 100644 index 0000000000..c71e08a003 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_rotate_z/env.yaml @@ -0,0 +1,59 @@ +physics: newton +scene_node_capacity: 262144 +id: LeapCubeRotateZAxisRL-v1 +max_episode_steps: 500 +num_envs: 8 +headless: true +device: cuda:0 +arena_space: 0.0 +physics_config: + sync_to_renderer: false + gravity: + - 0.0 + - 0.0 + - -9.81 + num_substeps: 1 + collision_cfg: null + solver_cfg: + solver_type: dexuni + joint_mode: dynamic + contact_mode: auto + mujoco_options: + use_mujoco_cpu: false + use_mujoco_contacts: true + update_data_interval: 0 + separate_worlds: true + solver: newton + integrator: euler + cone: pyramidal + impratio: 1.0 + iterations: 5 + ls_iterations: 8 + ccd_iterations: 10 + tolerance: 1.0e-08 + ls_tolerance: 0.01 + ccd_tolerance: 1.0e-06 + njmax: 512 + nconmax: 256 + physics_dt: 0.01 +env: + episode_context_in_info: false + sim_steps_per_control: 5 + control_parts: + - hand + events: {} + observations: {} + rewards: {} + actions: {} + extensions: {} +robot: + uid: leap_hand + build_pk_chain: false + asset_physics_mode: preserve + source_pose_mode: preserve +sensor: [] +light: {} +background: [] +rigid_object: [] +rigid_object_group: [] +articulation: [] diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/README.md b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/README.md new file mode 100644 index 0000000000..90657284ef --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/README.md @@ -0,0 +1,22 @@ +# Shadow Hand Continuous Pen Spin + +Continuously spin a pen through sequential orientation targets with the 24-DOF Shadow Hand. +Gym ID: `ShadowPenContinuousRL-v1`. + +[env.yaml](env.yaml) and [agents/ppo.yaml](agents/ppo.yaml) configure the `default` catalog deployment. The scene loads directly from `ShadowHandPen` (`usd/shadow_pen.usdc`) via `SimulationManager.add_usd()`, running `physics: newton` with `solver_type: dexuni` on GPU MuJoCo Warp CUDA (`use_mujoco_cpu: false`, `use_mujoco_contacts: true`). + +```bash +embodichain show-task embodichain_tasks:shadow_pen_continuous +embodichain run-task --gym_config extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/env.yaml +embodichain train-rl --config extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/agents/ppo.yaml +``` + +## Evaluation + +Evaluate a saved checkpoint with its training run directory: + +```bash +embodichain eval-policy outputs/shadow_pen_continuous_ppo/ --checkpoint latest --episodes 32 --num-envs 32 --renderer no-render +``` + +Episode `info["success"]` records whether at least one target milestone was achieved before termination or truncation. diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/agents/ppo.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/agents/ppo.yaml new file mode 100644 index 0000000000..0ac124562c --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/agents/ppo.yaml @@ -0,0 +1,59 @@ +trainer: + exp_name: shadow_pen_continuous_ppo + gym_config: extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/env.yaml + seed: 42 + device: cuda:0 + headless: true + num_envs: 4096 + iterations: 310 + buffer_size: 80 + enable_eval: false + save_frequency_updates: 100 + use_wandb: false + wandb_project_name: embodichain-dexterous +policy: + name: actor_critic + actor_obs_normalization: true + critic_obs_normalization: true + obs_groups: + actor: + - policy + critic: + - policy + actor: + type: mlp + network_cfg: + hidden_sizes: + - 512 + - 256 + - 128 + activation: elu + critic: + type: mlp + network_cfg: + hidden_sizes: + - 512 + - 256 + - 128 + activation: elu + initial_action_std: 0.5 +algorithm: + name: ppo + cfg: + optimizer: + name: adam + learning_rate: 0.0001 + schedule: adaptive + desired_kl: 0.03 + minimum_learning_rate: 1.0e-05 + maximum_learning_rate: 0.0003 + n_epochs: 4 + num_mini_batches: 64 + batch_size: 5120 + gamma: 0.995 + gae_lambda: 0.95 + clip_coef: 0.3 + ent_coef: 0.03 + vf_coef: 0.5 + use_clipped_value_loss: true + max_grad_norm: 1.0 diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/catalog.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/catalog.yaml new file mode 100644 index 0000000000..25963f3f2a --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/catalog.yaml @@ -0,0 +1,12 @@ +task_key: shadow_pen_continuous +title: Shadow Hand Continuous Pen Spin +summary: Continuously spin a pen through sequential orientation targets with the 24-DOF + Shadow Hand. +tags: +- manipulation +- dexterous +- reinforcement_learning +default_deployment: default +deployments: + default: + config: env.yaml diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/env.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/env.yaml new file mode 100644 index 0000000000..d90c8a4324 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/env.yaml @@ -0,0 +1,59 @@ +physics: newton +scene_node_capacity: 262144 +id: ShadowPenContinuousRL-v1 +max_episode_steps: 500 +num_envs: 8 +headless: true +device: cuda:0 +arena_space: 0.0 +physics_config: + sync_to_renderer: false + gravity: + - 0.0 + - 0.0 + - -9.81 + num_substeps: 1 + collision_cfg: null + solver_cfg: + solver_type: dexuni + joint_mode: dynamic + contact_mode: auto + mujoco_options: + use_mujoco_cpu: false + use_mujoco_contacts: true + update_data_interval: 0 + separate_worlds: true + solver: newton + integrator: euler + cone: pyramidal + impratio: 1.0 + iterations: 20 + ls_iterations: 50 + ccd_iterations: 10 + tolerance: 1.0e-08 + ls_tolerance: 0.01 + ccd_tolerance: 1.0e-06 + njmax: 512 + nconmax: 256 + physics_dt: 0.002 +env: + episode_context_in_info: false + sim_steps_per_control: 20 + control_parts: + - hand + events: {} + observations: {} + rewards: {} + actions: {} + extensions: {} +robot: + uid: shadow_hand + build_pk_chain: false + asset_physics_mode: preserve + source_pose_mode: preserve +sensor: [] +light: {} +background: [] +rigid_object: [] +rigid_object_group: [] +articulation: [] diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/README.md b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/README.md new file mode 100644 index 0000000000..51e8d77ba1 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/README.md @@ -0,0 +1,22 @@ +# Shadow Hand Fixed-Target Pen Spin + +Spin a pen in-hand to sampled target orientations with the 24-DOF Shadow Hand. +Gym ID: `ShadowPenFixedRL-v1`. + +[env.yaml](env.yaml) and [agents/ppo.yaml](agents/ppo.yaml) configure the `default` catalog deployment. The scene loads directly from `ShadowHandPen` (`usd/shadow_pen.usdc`) via `SimulationManager.add_usd()`, running `physics: newton` with `solver_type: dexuni` on GPU MuJoCo Warp CUDA (`use_mujoco_cpu: false`, `use_mujoco_contacts: true`). + +```bash +embodichain show-task embodichain_tasks:shadow_pen_fixed +embodichain run-task --gym_config extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/env.yaml +embodichain train-rl --config extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/agents/ppo.yaml +``` + +## Evaluation + +Evaluate a saved checkpoint with its training run directory: + +```bash +embodichain eval-policy outputs/shadow_pen_fixed_ppo/ --checkpoint latest --episodes 32 --num-envs 32 --renderer no-render +``` + +Episode `info["success"]` records whether at least one target milestone was achieved before termination or truncation. diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/agents/ppo.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/agents/ppo.yaml new file mode 100644 index 0000000000..d94aebc011 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/agents/ppo.yaml @@ -0,0 +1,55 @@ +trainer: + exp_name: shadow_pen_fixed_ppo + gym_config: extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/env.yaml + seed: 42 + device: cuda:0 + headless: true + num_envs: 4096 + iterations: 650 + buffer_size: 40 + enable_eval: false + save_frequency_updates: 100 + use_wandb: false + wandb_project_name: embodichain-dexterous +policy: + name: actor_critic + actor_obs_normalization: true + critic_obs_normalization: true + obs_groups: + actor: + - policy + critic: + - policy + actor: + type: mlp + network_cfg: + hidden_sizes: + - 512 + - 256 + - 128 + activation: elu + critic: + type: mlp + network_cfg: + hidden_sizes: + - 512 + - 256 + - 128 + activation: elu + initial_action_std: 0.5 +algorithm: + name: ppo + cfg: + optimizer: + name: adam + learning_rate: 0.0001 + n_epochs: 4 + num_mini_batches: 32 + batch_size: 5120 + gamma: 0.99 + gae_lambda: 0.95 + clip_coef: 0.3 + ent_coef: 0.002 + vf_coef: 0.5 + use_clipped_value_loss: true + max_grad_norm: 1.0 diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/catalog.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/catalog.yaml new file mode 100644 index 0000000000..4ba04cffee --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/catalog.yaml @@ -0,0 +1,12 @@ +task_key: shadow_pen_fixed +title: Shadow Hand Fixed-Target Pen Spin +summary: Spin a pen in-hand to sampled target orientations with the 24-DOF Shadow + Hand. +tags: +- manipulation +- dexterous +- reinforcement_learning +default_deployment: default +deployments: + default: + config: env.yaml diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/env.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/env.yaml new file mode 100644 index 0000000000..277fbc2249 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/env.yaml @@ -0,0 +1,59 @@ +physics: newton +scene_node_capacity: 262144 +id: ShadowPenFixedRL-v1 +max_episode_steps: 500 +num_envs: 8 +headless: true +device: cuda:0 +arena_space: 0.0 +physics_config: + sync_to_renderer: false + gravity: + - 0.0 + - 0.0 + - -9.81 + num_substeps: 1 + collision_cfg: null + solver_cfg: + solver_type: dexuni + joint_mode: dynamic + contact_mode: auto + mujoco_options: + use_mujoco_cpu: false + use_mujoco_contacts: true + update_data_interval: 0 + separate_worlds: true + solver: newton + integrator: euler + cone: pyramidal + impratio: 1.0 + iterations: 20 + ls_iterations: 50 + ccd_iterations: 10 + tolerance: 1.0e-08 + ls_tolerance: 0.01 + ccd_tolerance: 1.0e-06 + njmax: 512 + nconmax: 256 + physics_dt: 0.002 +env: + episode_context_in_info: false + sim_steps_per_control: 20 + control_parts: + - hand + events: {} + observations: {} + rewards: {} + actions: {} + extensions: {} +robot: + uid: shadow_hand + build_pk_chain: false + asset_physics_mode: preserve + source_pose_mode: preserve +sensor: [] +light: {} +background: [] +rigid_object: [] +rigid_object_group: [] +articulation: [] diff --git a/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/_embodichain.py b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/_embodichain.py index 515a479006..dcd24dee7f 100644 --- a/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/_embodichain.py +++ b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/_embodichain.py @@ -79,6 +79,7 @@ class EmbodiChainVelocityEnv(EmbodiedEnv): orientation_link_name: str | None = None imu_offset: tuple[float, float, float] | None = None explicit_pd_effort_control = False + _default_action_clip: float | None = 1.0 def __init__(self, cfg: EmbodiedEnvCfg | None = None, **kwargs) -> None: rewards = ( @@ -108,6 +109,7 @@ def __init__(self, cfg: EmbodiedEnvCfg | None = None, **kwargs) -> None: "preserve_order": True, "offset": self.velocity_task_config.default_joint_position, "scale": self.velocity_task_config.action_scale, + "clip": self._default_action_clip, } for name, value in mapping.items(): term_cfg.params.setdefault(name, value) @@ -181,32 +183,7 @@ def _init_sim_state(self, **kwargs) -> None: dtype=torch.float32, device=self.device, ) - sensor = self.get_sensor("locomotion_contacts") - self._foot_contacts = sensor.create_history( - "feet", - sensor.get_actor_ids(self.robot.uid, self.foot_link_names), - counterpart_ids=sensor.get_actor_ids("default_plane"), - # These flat scenes have only one static contact surface. The query - # can omit static counterpart identities, represented by -1. - include_unknown_counterpart=True, - ) - self._illegal_contacts = sensor.create_history( - "illegal", - sensor.get_actor_ids(self.robot.uid, self.illegal_contact_link_names), - ) - if "self_collision_count" in self._state_fields: - robot_ids = sensor.get_actor_ids(self.robot.uid, self.robot.link_names) - self._self_contacts = sensor.create_history( - "self", - robot_ids, - counterpart_ids=robot_ids, - force_threshold=float( - config.data.get("rewards", {}) - .get("self_collisions", {}) - .get("params", {}) - .get("force_threshold", 0.0) - ), - ) + self._initialize_contact_histories() self._locomotion_generator = self.get_generator("locomotion.commands_and_noise") action_shape = (self.num_envs, config.action_dim) foot_shape = (self.num_envs, len(self.foot_link_names)) @@ -267,6 +244,36 @@ def _init_sim_state(self, **kwargs) -> None: dtype=np.float32, ) + def _initialize_contact_histories(self) -> None: + """Register the generic task's substep force and contact-time histories.""" + config = self.velocity_task_config + sensor = self.get_sensor("locomotion_contacts") + self._foot_contacts = sensor.create_history( + "feet", + sensor.get_actor_ids(self.robot.uid, self.foot_link_names), + counterpart_ids=sensor.get_actor_ids("default_plane"), + # These flat scenes have only one static contact surface. The query + # can omit static counterpart identities, represented by -1. + include_unknown_counterpart=True, + ) + self._illegal_contacts = sensor.create_history( + "illegal", + sensor.get_actor_ids(self.robot.uid, self.illegal_contact_link_names), + ) + if "self_collision_count" in self._state_fields: + robot_ids = sensor.get_actor_ids(self.robot.uid, self.robot.link_names) + self._self_contacts = sensor.create_history( + "self", + robot_ids, + counterpart_ids=robot_ids, + force_threshold=float( + config.data.get("rewards", {}) + .get("self_collisions", {}) + .get("params", {}) + .get("force_threshold", 0.0) + ), + ) + def _bind_locomotion_action_state(self) -> None: """Bind task state to the default-position action term buffers. @@ -307,6 +314,7 @@ def _bind_locomotion_action_state(self) -> None: f"Locomotion action term must expose {required_state}; " f"missing {missing_state}." ) + self._locomotion_action_term = action_term self.locomotion_action = action_term.raw_actions self.last_locomotion_action = action_term.previous_raw_actions self.encoder_bias = action_term.position_bias @@ -547,7 +555,7 @@ def _update_heading_commands(self) -> None: ) self.command.masked_fill_(self._standing_env.unsqueeze(-1), 0.0) - def _update_sim_state(self, **kwargs) -> None: + def _update_sim_state(self, **kwargs) -> dict[str, torch.Tensor] | None: super()._update_sim_state(**kwargs) state = None if { @@ -590,6 +598,7 @@ def _update_sim_state(self, **kwargs) -> None: self._advance_command_timer() self._update_heading_commands() self._invalidate_task_cache() + return state def _initialize_episode( self, @@ -631,14 +640,7 @@ def _initialize_episode( joint_position = constant( self.velocity_task_config.default_joint_position, pose ).expand(count, -1) - self.robot.set_state( - env_ids=ids, - joint_ids=self.policy_joint_ids, - root_pose=pose, - qpos=joint_position, - target_qpos=joint_position, - clear_dynamics=True, - ) + self._write_episode_initial_state(ids, pose, joint_position) self._episode_step.index_fill_(0, ids, 0) self._previous_joint_velocity.index_fill_(0, ids, 0.0) self._joint_acceleration.index_fill_(0, ids, 0.0) @@ -653,6 +655,22 @@ def _initialize_episode( self.reward_command[ids] = self.command[ids] self._invalidate_task_cache() + def _write_episode_initial_state( + self, + env_ids: torch.Tensor, + root_pose: torch.Tensor, + joint_position: torch.Tensor, + ) -> None: + """Write the sampled default root and joint state.""" + self.robot.set_state( + env_ids=env_ids, + joint_ids=self.policy_joint_ids, + root_pose=root_pose, + qpos=joint_position, + target_qpos=joint_position, + clear_dynamics=True, + ) + def _invalidate_task_cache(self) -> None: self._state_cache = None self._reward_cache = None diff --git a/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/g1_rough.py b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/g1_rough.py new file mode 100644 index 0000000000..be16dacd10 --- /dev/null +++ b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/g1_rough.py @@ -0,0 +1,41 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Unitree G1 rough-terrain velocity task.""" + +from __future__ import annotations + +from embodichain.lab.gym.utils.registration import register_env +from .rough.config import load_config +from .rough.runtime import RoughVelocityEnv + +__all__ = ["UnitreeG1RoughEnv"] + +_CONFIG = load_config("g1_rough") + + +@register_env( + "UnitreeG1RoughRL-v1", max_episode_steps=1000, override=True, supports_rl=True +) +class UnitreeG1RoughEnv(RoughVelocityEnv): + """Track planar velocity commands with the 37-DOF G1 model on rough terrain.""" + + velocity_task_config = _CONFIG + foot_link_names = tuple(_CONFIG.data["feet"]) + foot_offsets = ((0.0, 0.0, 0.0),) * len(foot_link_names) + illegal_contact_link_names = tuple(_CONFIG.data["illegal_bodies"]) + orientation_link_name = "torso_link" + explicit_pd_effort_control = False diff --git a/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/__init__.py b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/__init__.py new file mode 100644 index 0000000000..73cb831e90 --- /dev/null +++ b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/__init__.py @@ -0,0 +1,23 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Rough-terrain velocity task computations and runtime adapter.""" + +from __future__ import annotations + +from .environment import load_rough_env_cfg + +__all__ = ["load_rough_env_cfg"] diff --git a/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/_frame.py b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/_frame.py new file mode 100644 index 0000000000..070f8f3610 --- /dev/null +++ b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/_frame.py @@ -0,0 +1,82 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- +"""Fuse reference frame vectors while retaining independently owned snapshots.""" + +from __future__ import annotations + +import torch +import warp as wp + +from embodichain.utils.device_utils import current_warp_stream + +__all__: list[str] = [] + +wp.set_module_options({"enable_fp_fusion": False, "enable_backward": False}) + + +@wp.func +def _inverse(q: wp.vec3, w: float, value: wp.vec3) -> wp.vec3: + tangent = wp.cross(q, value) * 2.0 + return value - w * tangent + wp.cross(q, tangent) + + +@wp.kernel(enable_backward=False) +def _frame_vectors( + quaternion: wp.array2d(dtype=float), + linear: wp.array2d(dtype=float), + angular: wp.array2d(dtype=float), + output: wp.array3d(dtype=float), +): + row = wp.tid() + x, y, z, w = ( + quaternion[row, 0], + quaternion[row, 1], + quaternion[row, 2], + quaternion[row, 3], + ) + xyz = wp.vec3(x, y, z) + velocity = wp.vec3(linear[row, 0], linear[row, 1], linear[row, 2]) + omega = wp.vec3(angular[row, 0], angular[row, 1], angular[row, 2]) + yaw = wp.atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z)) + v0 = _inverse(xyz, w, velocity) + v1 = _inverse(xyz, w, omega) + v2 = _inverse(xyz, w, wp.vec3(0.0, 0.0, -1.0)) + v3 = _inverse(wp.vec3(0.0, 0.0, wp.sin(yaw / 2.0)), wp.cos(yaw / 2.0), velocity) + for column in range(3): + output[row, 0, column] = v0[column] + output[row, 1, column] = v1[column] + output[row, 2, column] = v2[column] + output[row, 3, column] = v3[column] + + +def _reference_frame_vectors( + quaternion: torch.Tensor, linear: torch.Tensor, angular: torch.Tensor +) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor, torch.Tensor]: + """Return fresh body-frame vectors and yaw-frame linear velocity.""" + output = linear.new_empty((linear.shape[0], 4, 3)) + wp.launch( + _frame_vectors, + dim=linear.shape[0], + inputs=[ + wp.from_torch(quaternion), + wp.from_torch(linear), + wp.from_torch(angular), + wp.from_torch(output), + ], + device=str(linear.device), + stream=current_warp_stream(linear.device), + ) + return tuple(output.unbind(dim=1)) diff --git a/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/config.py b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/config.py new file mode 100644 index 0000000000..948084b1bb --- /dev/null +++ b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/config.py @@ -0,0 +1,62 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Rough-terrain velocity task settings.""" + +from __future__ import annotations + +from dataclasses import MISSING + +from embodichain.utils import configclass + +from ..contracts._config import load_task_data + +__all__ = ["RoughVelocityConfig", "load_config"] + + +@configclass +class RoughVelocityConfig: + """Policy dimensions, timing, model parameters and resolved task terms.""" + + data: dict = MISSING + joint_names: tuple[str, ...] = MISSING + default_joint_position: tuple[float, ...] = MISSING + action_scale: tuple[float, ...] = MISSING + action_dim: int = MISSING + actor_observation_dim: int = MISSING + critic_observation_dim: int = MISSING + physics_dt: float = 0.005 + control_dt: float = 0.02 + max_episode_steps: int = 1000 + + +def load_config(task: str = "g1_rough") -> RoughVelocityConfig: + """Load a packaged rough-terrain velocity definition in policy joint order.""" + data = load_task_data(task) + robot = data["robot"] + dimension = len(robot["joint_names"]) + observation_dimension = 12 + 3 * dimension + (187 if dimension == 37 else 0) + cfg = RoughVelocityConfig( + data=data, + joint_names=tuple(robot["joint_names"]), + default_joint_position=tuple(robot["default_joint_position"]), + action_scale=tuple(robot["action_scale"]), + action_dim=dimension, + actor_observation_dim=observation_dimension, + critic_observation_dim=observation_dimension, + ) + cfg.validate() + return cfg diff --git a/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/curriculum.py b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/curriculum.py new file mode 100644 index 0000000000..9614a31cad --- /dev/null +++ b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/curriculum.py @@ -0,0 +1,149 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Terrain assignment and distance curriculum for rough-terrain locomotion.""" + +from __future__ import annotations + +import torch + +__all__ = ["TerrainCurriculum"] + + +class TerrainCurriculum: + """Assign terrain columns and advance rows at episode resets. + + Args: + origins: Shared terrain origins, with shape (rows, columns, 3). + num_envs: Number of independent robot instances. + max_init_level: Maximum initial row, inclusive. + terrain_length: Length of one terrain tile along X, in meters. + episode_length_s: Time limit used by the commanded-distance rule. + generator: Random generator for initial and overflow row assignment. + """ + + def __init__( + self, + origins: torch.Tensor, + num_envs: int, + max_init_level: int, + terrain_length: float, + episode_length_s: float, + generator: torch.Generator, + ) -> None: + self.origins = origins + self.generator = generator + self.terrain_length = terrain_length + self.episode_length_s = episode_length_s + self.max_level, columns = origins.shape[:2] + self.levels = torch.randint( + 0, + min(max_init_level, self.max_level - 1) + 1, + (num_envs,), + device=origins.device, + generator=generator, + ) + self.types = torch.div( + torch.arange(num_envs, device=origins.device), + num_envs / columns, + rounding_mode="floor", + ).long() + self.env_origins = origins[self.levels, self.types].clone() + + def state_dict(self) -> dict[str, torch.Tensor]: + """Return terrain assignments and the shared task generator state.""" + return { + "levels": self.levels.detach().cpu().clone(), + "types": self.types.detach().cpu().clone(), + "origins": self.origins.detach().cpu().clone(), + "generator": self.generator.get_state().cpu().clone(), + } + + def validate_state_dict(self, state: dict[str, torch.Tensor]) -> None: + """Check saved assignments without changing the current curriculum. + + Args: + state: Terrain assignments, map origins and task RNG state. + + Raises: + ValueError: The terrain map, instance count or indices differ. + """ + levels = state["levels"].to(self.levels.device) + types = state["types"].to(self.types.device) + if levels.shape != self.levels.shape or types.shape != self.types.shape: + raise ValueError("Terrain curriculum instance count differs") + if not torch.equal(state["origins"].to(self.origins), self.origins): + raise ValueError("Terrain curriculum map differs") + if levels.dtype != torch.long or types.dtype != torch.long: + raise ValueError("Terrain curriculum indices must be int64") + if bool(((levels < 0) | (levels >= self.max_level)).any()): + raise ValueError("Terrain curriculum level is out of range") + if bool(((types < 0) | (types >= self.origins.shape[1])).any()): + raise ValueError("Terrain curriculum type is out of range") + torch.Generator(device=self.generator.device).set_state( + state["generator"].cpu() + ) + + def load_state_dict(self, state: dict[str, torch.Tensor]) -> None: + """Restore assignments after checking the terrain map and instance count. + + Args: + state: Terrain assignments, map origins and task RNG state. + """ + self.validate_state_dict(state) + levels = state["levels"].to(self.levels.device) + types = state["types"].to(self.types.device) + self.generator.set_state(state["generator"].cpu()) + self.levels.copy_(levels) + self.types.copy_(types) + self.env_origins.copy_(self.origins[levels, types]) + + def update( + self, + env_ids: torch.Tensor, + root_positions: torch.Tensor, + commands: torch.Tensor, + ) -> None: + """Use pre-reset positions and commands to update selected rows. + + Args: + env_ids: Instances whose episodes are being reset. + root_positions: Positions for all instances, in terrain coordinates. + commands: Current linear X/Y and yaw velocity commands for all instances. + """ + distance = torch.linalg.vector_norm( + root_positions[env_ids, :2] - self.env_origins[env_ids, :2], dim=1 + ) + move_up = distance > self.terrain_length / 2 + move_down = distance < ( + torch.linalg.vector_norm(commands[env_ids, :2], dim=1) + * self.episode_length_s + * 0.5 + ) + move_down &= ~move_up + levels = self.levels[env_ids] + move_up.long() - move_down.long() + random_levels = torch.randint( + self.max_level, + levels.shape, + device=levels.device, + generator=self.generator, + ) + self.levels[env_ids] = torch.where( + levels >= self.max_level, random_levels, levels.clamp_min(0) + ) + self.env_origins[env_ids] = self.origins[ + self.levels[env_ids], self.types[env_ids] + ] diff --git a/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/environment.py b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/environment.py new file mode 100644 index 0000000000..1969837126 --- /dev/null +++ b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/environment.py @@ -0,0 +1,165 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Build rough-terrain velocity environments from frozen scene assets.""" + +from __future__ import annotations + +import json +from importlib.resources import files +from pathlib import Path + +import numpy as np +import yaml + +from embodichain.lab.gym.envs import EmbodiedEnvCfg +from embodichain.lab.gym.utils.gym_utils import config_to_cfg, get_manager_modules + +from .config import load_config + +__all__ = ["load_rough_env_cfg"] + + +def load_rough_env_cfg( + robot: str, + asset_bundle: str | Path, + *, + num_envs: int = 1, + seed: int = 42, + training_randomization: bool = False, + use_mujoco_contacts: bool | None = None, + initial_state_file: str | Path | None = None, +) -> EmbodiedEnvCfg: + """Load G1 Rough with shared evaluation or training terrain. + + Args: + robot: ``g1``. + asset_bundle: Directory containing the exported robot USDs and, for G1, + ``terrain.npz`` and ``terrain.json`` with evaluation origins or the + complete training terrain and curriculum definition. + num_envs: Independent physics worlds. + seed: Environment random seed. + training_randomization: Enable observation noise, startup mass/COM + events, random reset and interval velocity pushes. + use_mujoco_contacts: Select native contacts or Newton contacts while + retaining MJWarp dynamics. ``None`` keeps the packaged setting. + initial_state_file: Shared deterministic benchmark states. Rows repeat + in environment ID order and apply again on episode resets. + + Returns: + The resolved EmbodiChain environment configuration. + + Raises: + ValueError: The robot selector is unknown or bundle metadata is invalid. + FileNotFoundError: A required scene asset is missing. + """ + tasks = {"g1": "g1_rough"} + if robot not in tasks: + raise ValueError(f"Unknown robot selector: {robot!r}") + bundle = Path(asset_bundle).expanduser().resolve() + asset = bundle / f"{robot}.usda" + if not asset.is_file(): + raise FileNotFoundError(asset) + resource = files("embodichain_tasks.configs").joinpath( + "tasks", "locomotion", "velocity", tasks[robot], "env.yaml" + ) + data = yaml.safe_load(resource.read_text()) + data["num_envs"] = num_envs + data["robot"]["fpath"] = str(asset) + solver_cfg = data["physics_config"]["solver_cfg"] + mj_target = ( + solver_cfg.setdefault("mujoco_options", {}) + if solver_cfg.get("solver_type") == "dexuni" + else solver_cfg + ) + if use_mujoco_contacts is None: + use_mujoco_contacts = mj_target["use_mujoco_contacts"] + mj_target["use_mujoco_contacts"] = use_mujoco_contacts + if use_mujoco_contacts: + data["physics_config"]["collision_cfg"] = None + solver_cfg.pop("collision_options", None) + # Newton contact mode sets dynamic friction to 0.8, while native MJWarp + # contact mode retains the imported sliding friction of 1.0. + data["robot"]["attrs"]["material_props"]["dynamic_friction"] = ( + 1.0 if use_mujoco_contacts else 0.8 + ) + extension = data["env"]["extensions"] + extension["deterministic_reset"] = not training_randomization + extension["randomization"] = training_randomization + if initial_state_file is not None: + if training_randomization: + raise ValueError( + "Shared initial states require deterministic benchmark mode." + ) + states = json.loads(Path(initial_state_file).read_text()) + task = load_config(tasks[robot]) + if ( + states.get("schema") != "rough_velocity.initial_states@1" + or states.get("robot") != robot + or states.get("joint_names") != list(task.joint_names) + ): + raise ValueError( + "Initial states must match the target robot and policy joint order." + ) + rows = states["initial_states"] + if not rows: + raise ValueError("Initial state list must be nonempty.") + limits = np.asarray(task.data["robot"]["soft_joint_pos_limits"]) + for row in rows: + for key, size in ( + ("root_pose", 7), + ("root_velocity", 6), + ("qpos", task.action_dim), + ("qvel", task.action_dim), + ): + values = np.asarray(row[key]) + if values.shape != (size,) or not np.isfinite(values).all(): + raise ValueError(f"Invalid initial state field: {key}") + if not np.isclose(np.linalg.norm(row["root_pose"][3:]), 1.0, atol=1e-6): + raise ValueError( + "Initial root quaternion must be normalized in XYZW order." + ) + position = np.asarray(row["qpos"]) + if np.any(position < limits[:, 0] - 1e-7) or np.any( + position > limits[:, 1] + 1e-7 + ): + raise ValueError("Initial joint positions exceed the task soft limits.") + extension["initial_states"] = rows + for term in data["env"]["observations"].values(): + term["params"]["enable_corruption"] = training_randomization + if robot == "g1": + terrain = bundle / "terrain.npz" + if not terrain.is_file(): + raise FileNotFoundError(terrain) + extension["terrain"] = str(terrain) + metadata = json.loads((bundle / "terrain.json").read_text()) + if training_randomization: + required_keys = ( + "terrain_origins", + "max_init_terrain_level", + "generator", + ) + if not all(key in metadata for key in required_keys): + raise ValueError( + "G1 training requires terrain.json with terrain_origins, " + "max_init_terrain_level, and generator." + ) + extension["terrain_course"] = metadata + else: + extension["origin"] = metadata["env_origins"][0] + cfg = config_to_cfg(data, manager_modules=get_manager_modules()) + cfg.seed = seed + return cfg diff --git a/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/mdp.py b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/mdp.py new file mode 100644 index 0000000000..0383ffea26 --- /dev/null +++ b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/mdp.py @@ -0,0 +1,339 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Tensor computations for rough-terrain velocity task definitions.""" + +from __future__ import annotations + +from dataclasses import dataclass +from typing import TYPE_CHECKING + +import torch + +if TYPE_CHECKING: + from .config import RoughVelocityConfig + +__all__ = [ + "RoughVelocityState", + "RoughVelocityReward", + "RoughVelocityConstants", + "action_target", + "observation_terms", + "build_observation", + "compute_rewards", + "compute_termination", + "dc_motor_effort", + "update_contact_times", +] + + +@dataclass +class RoughVelocityState: + """Measured tensors in policy joint order and XYZW quaternion convention.""" + + base_lin_vel_b: torch.Tensor + base_ang_vel_b: torch.Tensor + projected_gravity_b: torch.Tensor + base_lin_vel_yaw: torch.Tensor + base_ang_vel_w: torch.Tensor + command: torch.Tensor + joint_pos: torch.Tensor + joint_vel: torch.Tensor + joint_acc: torch.Tensor + applied_torque: torch.Tensor + action: torch.Tensor + previous_action: torch.Tensor + foot_vel_w: torch.Tensor + foot_force_history: torch.Tensor + current_air_time: torch.Tensor + current_contact_time: torch.Tensor + last_air_time: torch.Tensor + illegal_force_history: torch.Tensor + height_scan: torch.Tensor + episode_step: torch.Tensor + root_height: torch.Tensor + + +@dataclass(frozen=True) +class RoughVelocityReward: + """Reward values before and after weight and control-period multiplication.""" + + raw_terms: dict[str, torch.Tensor] + weighted_terms: dict[str, torch.Tensor] + total: torch.Tensor + + +@dataclass(frozen=True) +class RoughVelocityConstants: + """Device tensors for the fixed task configuration.""" + + default_joint_position: torch.Tensor + action_scale: torch.Tensor + joint_limits: torch.Tensor + observation_scales: dict[str, torch.Tensor] + reward_joint_ids: dict[str, torch.Tensor | None] + + @classmethod + def from_config( + cls, config: RoughVelocityConfig, reference: torch.Tensor + ) -> RoughVelocityConstants: + """Materialize constants once on the environment device and dtype.""" + return cls( + reference.new_tensor(config.default_joint_position), + reference.new_tensor(config.action_scale), + reference.new_tensor(config.data["robot"]["soft_joint_pos_limits"]), + { + name: reference.new_tensor(spec["scale"]) + for name, spec in config.data["observations"].items() + if spec["scale"] is not None + }, + { + name: ( + None + if spec["joint_ids"] is None + else torch.tensor( + spec["joint_ids"], device=reference.device, dtype=torch.long + ) + ) + for name, spec in config.data["rewards"].items() + }, + ) + + +def action_target( + config: RoughVelocityConfig, + action: torch.Tensor, + *, + constants: RoughVelocityConstants | None = None, +) -> torch.Tensor: + """Apply the task default joint offset and scale without clipping.""" + scale = ( + action.new_tensor(config.action_scale) + if constants is None + else constants.action_scale + ) + default = ( + action.new_tensor(config.default_joint_position) + if constants is None + else constants.default_joint_position + ) + return action * scale + default + + +def observation_terms( + config: RoughVelocityConfig, + state: RoughVelocityState, + *, + constants: RoughVelocityConstants | None = None, +) -> dict[str, torch.Tensor]: + """Return policy terms in their declared order before noise and clipping.""" + default = ( + state.joint_pos.new_tensor(config.default_joint_position) + if constants is None + else constants.default_joint_position + ) + terms = { + "base_lin_vel": state.base_lin_vel_b, + "base_ang_vel": state.base_ang_vel_b, + "projected_gravity": state.projected_gravity_b, + "velocity_commands": state.command, + "joint_pos": state.joint_pos - default, + "joint_vel": state.joint_vel, + "actions": state.action, + } + if config.action_dim == 37: + terms["height_scan"] = state.height_scan + return terms + + +def build_observation( + config: RoughVelocityConfig, + state: RoughVelocityState, + *, + noise: dict[str, torch.Tensor] | None = None, + constants: RoughVelocityConstants | None = None, +) -> torch.Tensor: + """Process term noise, clipping and scaling in declared order. + + Args: + config: Fixed rough-terrain velocity task definition. + state: Measured state tensors. + noise: Optional realized additive noise per term. + constants: Optional device tensors for the fixed task configuration. + + Returns: + The shared actor and critic policy observation. + """ + values = [] + for name, term in observation_terms(config, state, constants=constants).items(): + spec = config.data["observations"][name] + if noise is not None and name in noise: + term = term + noise[name] + if spec["clip"] is not None: + term = term.clamp(*spec["clip"]) + if spec["scale"] is not None: + term = term * ( + term.new_tensor(spec["scale"]) + if constants is None + else constants.observation_scales[name] + ) + values.append(term) + return torch.cat(values, dim=-1) + + +def compute_termination( + config: RoughVelocityConfig, state: RoughVelocityState +) -> tuple[torch.Tensor, torch.Tensor]: + """Return illegal body contact and the independent time limit.""" + terminated = state.illegal_force_history.norm(dim=-1).amax(dim=(1, 2)) > 1.0 + return terminated, state.episode_step >= config.max_episode_steps + + +def compute_rewards( + config: RoughVelocityConfig, + state: RoughVelocityState, + terminated: torch.Tensor, + *, + constants: RoughVelocityConstants | None = None, +) -> RoughVelocityReward: + """Evaluate active reward terms, including joint selections and step duration.""" + raw = {} + default = ( + state.joint_pos.new_tensor(config.default_joint_position) + if constants is None + else constants.default_joint_position + ) + limits = ( + state.joint_pos.new_tensor(config.data["robot"]["soft_joint_pos_limits"]) + if constants is None + else constants.joint_limits + ) + for name, spec in config.data["rewards"].items(): + function = spec["function"] + params = spec["params"] + joints = ( + spec["joint_ids"] if constants is None else constants.reward_joint_ids[name] + ) + if function.startswith("track_lin_vel"): + velocity = ( + state.base_lin_vel_yaw + if "yaw_frame" in function + else state.base_lin_vel_b + ) + value = torch.exp( + -(state.command[:, :2] - velocity[:, :2]).square().sum(-1) + / params["std"] ** 2 + ) + elif function.startswith("track_ang_vel"): + velocity = ( + state.base_ang_vel_w if "world" in function else state.base_ang_vel_b + ) + value = torch.exp( + -(state.command[:, 2] - velocity[:, 2]).square() / params["std"] ** 2 + ) + elif function == "lin_vel_z_l2": + value = state.base_lin_vel_b[:, 2].square() + elif function == "ang_vel_xy_l2": + value = state.base_ang_vel_b[:, :2].square().sum(-1) + elif function == "joint_torques_l2": + value = state.applied_torque[:, joints].square().sum(-1) + elif function == "joint_acc_l2": + value = state.joint_acc[:, joints].square().sum(-1) + elif function == "action_rate_l2": + value = (state.action - state.previous_action).square().sum(-1) + elif function == "feet_air_time": + first = (state.current_contact_time > 0.0) & ( + state.current_contact_time < config.control_dt + config.physics_dt * 0.5 + ) + value = ((state.last_air_time - params["threshold"]) * first).sum(-1) + value = value * (state.command[:, :2].norm(dim=-1) > 0.1) + elif function == "feet_air_time_positive_biped": + contact = state.current_contact_time > 0.0 + mode_time = torch.where( + contact, state.current_contact_time, state.current_air_time + ) + single = contact.sum(-1) == 1 + value = ( + torch.where(single[:, None], mode_time, 0.0) + .amin(-1) + .clamp(max=params["threshold"]) + ) + value = value * (state.command[:, :2].norm(dim=-1) > 0.1) + elif function == "flat_orientation_l2": + value = state.projected_gravity_b[:, :2].square().sum(-1) + elif function == "joint_pos_limits": + value = ( + (limits[:, 0] - state.joint_pos).clamp(min=0.0) + + (state.joint_pos - limits[:, 1]).clamp(min=0.0) + )[:, joints].sum(-1) + elif function == "is_terminated": + value = terminated.to(state.joint_pos.dtype) + elif function == "feet_slide": + contacts = state.foot_force_history.norm(dim=-1).amax(dim=1) > 1.0 + value = (state.foot_vel_w[:, :, :2].norm(dim=-1) * contacts).sum(-1) + elif function == "joint_deviation_l1": + value = (state.joint_pos - default).abs()[:, joints].sum(-1) + else: + raise ValueError(f"Unsupported reward function: {function}") + raw[name] = value + weighted = { + name: value * config.data["rewards"][name]["weight"] * config.control_dt + for name, value in raw.items() + } + return RoughVelocityReward( + raw, weighted, torch.stack(tuple(weighted.values())).sum(0) + ) + + +def dc_motor_effort( + desired_effort: torch.Tensor, + velocity: torch.Tensor, + effort_limit: torch.Tensor, + velocity_limit: torch.Tensor, + saturation_effort: torch.Tensor, +) -> torch.Tensor: + """Apply the reference DCMotor torque-speed envelope, including overspeed braking.""" + maximum_velocity = velocity_limit * (1.0 + effort_limit / saturation_effort) + velocity = velocity.clamp(-maximum_velocity, maximum_velocity) + maximum = torch.minimum( + saturation_effort * (1.0 - velocity / velocity_limit), effort_limit + ) + minimum = torch.maximum( + saturation_effort * (-1.0 - velocity / velocity_limit), -effort_limit + ) + return desired_effort.clamp(minimum, maximum) + + +def update_contact_times( + force: torch.Tensor, + elapsed: float, + current_air: torch.Tensor, + current_contact: torch.Tensor, + last_air: torch.Tensor, + last_contact: torch.Tensor, +) -> None: + """Update reference air and contact timers from summed normal forces in place.""" + contact = force.norm(dim=-1) > 0.0 + last_air.copy_( + torch.where(contact & (current_air > 0.0), current_air + elapsed, last_air) + ) + last_contact.copy_( + torch.where( + ~contact & (current_contact > 0.0), current_contact + elapsed, last_contact + ) + ) + current_air.copy_(torch.where(contact, 0.0, current_air + elapsed)) + current_contact.copy_(torch.where(contact, current_contact + elapsed, 0.0)) diff --git a/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/runtime.py b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/runtime.py new file mode 100644 index 0000000000..f37aa69ddd --- /dev/null +++ b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/runtime.py @@ -0,0 +1,783 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""EmbodiChain and DexSim runtime adapter for rough-terrain velocity tasks.""" + +from __future__ import annotations + +from dataclasses import replace +from functools import partial +from pathlib import Path +from typing import Any, Sequence + +import gymnasium as gym +import numpy as np +import torch +from tensordict import TensorDict + +from embodichain.lab.sim.types import EnvAction, EnvObs +from embodichain.utils.math import quat_apply_inverse + +from .._embodichain import EmbodiChainVelocityEnv +from .mdp import ( + RoughVelocityState, + RoughVelocityConstants, + build_observation, + observation_terms, + compute_rewards, + compute_termination, + dc_motor_effort, + update_contact_times, +) +from .terrain import HeightScanner +from .curriculum import TerrainCurriculum + +__all__ = ["RoughVelocityEnv"] + + +class RoughVelocityEnv(EmbodiChainVelocityEnv): + """Run rough-terrain velocity locomotion on shared training or evaluation terrain. + + Deterministic evaluation disables startup mass/COM randomization and + interval pushes. ``randomization=True`` enables those training + events. Training loads the complete rough terrain and curriculum; fixed + scenes load evaluation origins. Both use the shared triangle mesh for + height rays and Newton heightfield rasterization for collision. + """ + + _default_action_clip = None + state_type = RoughVelocityState + compute_rewards_fn = staticmethod(compute_rewards) + compute_termination_fn = staticmethod(compute_termination) + + def _initialize_contact_histories(self) -> None: + """Use this task's control-frame histories and normal-force sampling.""" + + def _invalidate_task_cache(self) -> None: + super()._invalidate_task_cache() + self._observation_cache = None + + def _prepare_scene(self, **kwargs: Any) -> None: + from dexsim.spawn import NewtonCollisionDesc, GeometryDesc + + rough = self.velocity_task_config.action_dim == 37 + self._reference_terrain = None + geometry = None + contact = NewtonCollisionDesc( + mu=1.0, margin=0.0, gap=0.01, ke=160000.0, kd=1100.0 + ) + if rough: + from embodichain.data import get_data_path + import json + + path = Path(get_data_path(self.cfg.terrain)) + if getattr(self.cfg, "randomization", False) and not getattr( + self.cfg, "terrain_course", None + ): + self.cfg.terrain_course = json.loads( + path.with_suffix(".json").read_text() + ) + terrain = np.load(path) + self._reference_terrain = (terrain["vertices"], terrain["triangles"]) + geometry = GeometryDesc.mesh( + vertices=terrain["vertices"], triangles=terrain["triangles"] + ) + contact.convert_to_heightfield = True + contact.heightfield_resolution = float(terrain["resolution"]) + self.sim.configure_ground_plane_collision(newton=contact, geometry=geometry) + self._graph_motor = None + if self.explicit_pd_effort_control and self.sim.is_newton_backend: + self._graph_motor = self.sim._register_joint_dc_motor_control( + self.robot.uid, self.velocity_task_config.data["robot"] + ) + super()._prepare_scene(**kwargs) + + def _init_sim_state(self, **kwargs: Any) -> None: + self._reference_events_deferred = True + self._reference_scan_reuse = None + super()._init_sim_state(**kwargs) + from ...managers.observations import velocity_locomotion_observation + from ...managers.rewards import velocity_locomotion_total_reward + + expected = { + id(self.cfg.observations): velocity_locomotion_observation, + id(self.cfg.rewards): velocity_locomotion_total_reward, + } + self._reference_task_managers_only = set(self.sensors) == { + "locomotion_contacts" + } and all( + self._configured_functor_callable(term) is expected[id(collection)] + for collection, _, term in self._iter_configured_functors() + if id(collection) in expected + ) + config = self.velocity_task_config + self._reference_constants = RoughVelocityConstants.from_config( + config, self.locomotion_action + ) + self._reference_origin = self.locomotion_action.new_tensor( + getattr(self.cfg, "origin", (0.0, 0.0, 0.0)) + ) + self.compute_rewards_fn = partial( + compute_rewards, constants=self._reference_constants + ) + self.single_action_space = gym.spaces.Box( + -np.inf, np.inf, (config.action_dim,), dtype=np.float32 + ) + self._reference_sensor = self.get_sensor("locomotion_contacts") + names = (*self.foot_link_names, *self.illegal_contact_link_names) + self._tracked_actor_ids = self._reference_sensor.get_actor_ids( + self.robot.uid, names + ).to(torch.long) + self._tracked_keys = ( + torch.arange(self.num_envs, device=self.device)[:, None] * (2**32) + + self._tracked_actor_ids + ).flatten() + self._sorted_keys, self._sorted_rows = self._tracked_keys.sort() + bodies = len(names) + self._normal_force = torch.zeros((self.num_envs, bodies, 3), device=self.device) + self._normal_history = torch.zeros( + (self.num_envs, 3, bodies, 3), device=self.device + ) + self._air_time = torch.zeros((self.num_envs, bodies), device=self.device) + self._contact_time = torch.zeros_like(self._air_time) + self._last_air_time = torch.zeros_like(self._air_time) + self._last_contact_time = torch.zeros_like(self._air_time) + self._applied_effort = torch.zeros_like(self.locomotion_action) + self._previous_physics_joint_velocity = torch.zeros_like(self.locomotion_action) + self._position_target = self._applied_effort.clone() + self._kp = torch.tensor(config.data["robot"]["stiffness"], device=self.device) + self._kd = torch.tensor(config.data["robot"]["damping"], device=self.device) + self._effort_limit = torch.tensor( + config.data["robot"]["effort_limit"], device=self.device + ) + self._velocity_limit = torch.tensor( + config.data["robot"]["velocity_limit"], device=self.device + ) + if self._graph_motor is not None: + import warp as wp + + target = wp.to_torch(self._graph_motor.targets) + target.copy_(self._position_target) + self._position_target = target + self._applied_effort = wp.to_torch(self._graph_motor.effort) + self._previous_physics_joint_velocity = wp.to_torch( + self._graph_motor.velocity + ) + self._push_remaining = torch.empty(self.num_envs, device=self.device).uniform_( + 10.0, 15.0, generator=self._locomotion_generator + ) + self._height_scanner = None + if self._reference_terrain is not None: + from dexsim.engine.newton_physics.backend_registry import get_newton_backend + + backend = get_newton_backend(self.sim.spawn_result.world) + self._height_scanner = HeightScanner( + *self._reference_terrain, + self.num_envs, + self.device, + physics_model=backend.model, + physics_state=backend.state_0, + ) + self._reference_initial_states = ( + { + name: torch.tensor( + [row[name] for row in self.cfg.initial_states], + device=self.device, + dtype=torch.float32, + ) + for name in ("root_pose", "root_velocity", "qpos", "qvel") + } + if getattr(self.cfg, "initial_states", None) is not None + else None + ) + self._observation_cache: torch.Tensor | None = None + self._reference_terrain_course = None + terrain_course = getattr(self.cfg, "terrain_course", None) + if terrain_course is not None: + origins = torch.tensor( + terrain_course["terrain_origins"], + device=self.device, + dtype=torch.float32, + ) + self._reference_terrain_course = TerrainCurriculum( + origins, + self.num_envs, + terrain_course["max_init_terrain_level"], + terrain_course["generator"]["size"][0], + self.max_episode_steps * self.step_dt, + self._locomotion_generator, + ) + if getattr(self.cfg, "randomization", False): + self._randomize_startup() + self._reference_events_deferred = False + + def _randomize_startup(self) -> None: + """Apply reference log-uniform mass/inertia and additive COM events.""" + spec = self.velocity_task_config.data["events"]["add_base_mass"] + name = spec["body_names"] + minimum, maximum = spec["params"]["mass_distribution_params"] + scale = ( + torch.empty((self.num_envs, 1), device=self.device) + .uniform_( + np.log(minimum), np.log(maximum), generator=self._locomotion_generator + ) + .exp_() + ) + self.robot.set_inertia( + self.robot.get_inertia(link_names=[name]) * scale[:, :, None], + link_names=[name], + ) + self.robot.set_mass( + self.robot.get_mass(link_names=[name]) * scale, link_names=[name] + ) + com_spec = self.velocity_task_config.data["events"].get("base_com") + if com_spec is not None: + pose = self.robot.get_com_pose(link_names=[name]).clone() + for axis, key in enumerate(("x", "y", "z")): + pose[:, :, axis] += torch.empty_like(pose[:, :, axis]).uniform_( + *com_spec["params"]["com_range"][key], + generator=self._locomotion_generator, + ) + self.robot.set_com_pose(pose, link_names=[name]) + + def _step_action(self, action: torch.Tensor) -> torch.Tensor: + """Write processed action targets through the Spawn batch state API.""" + self._position_target.copy_(self._locomotion_action_term.processed_actions) + if not self.explicit_pd_effort_control: + self.robot.body_data.articulation_view.apply_state( + None, self.policy_joint_ids, target_qpos=self._position_target + ) + self._last_action_trace = self.action_manager.action_trace() + return action + + def _advance_physics(self) -> None: + """Update actuator effort at 200 Hz and sample lazy contacts at 50 Hz.""" + sensors = [ + sensor + for sensor in self.sensors.values() + if sensor.requires_substep_update + and getattr(getattr(sensor, "cfg", None), "sample_phase", None) is None + ] + for sensor in sensors: + sensor.begin_control_step() + + def sample_substep(dt: float) -> None: + for sensor in sensors: + sensor.update_physics_step(dt) + + after_substep = sample_substep if sensors else None + if getattr(self, "_graph_motor", None) is not None: + for _ in range(self.cfg.sim_steps_per_control): + self.sim.update( + self.physics_dt, + 1, + after_substep=after_substep, + render_final_step=False, + ) + self._sample_reference_contacts() + return + for step in range(self.cfg.sim_steps_per_control): + if ( + not self.explicit_pd_effort_control + and step < self.cfg.sim_steps_per_control - 1 + ): + # Implicit drives run in the solver. Only the final pre-step + # effort and velocity are consumed by control-step rewards. + self.sim.update( + self.physics_dt, + 1, + after_substep=after_substep, + render_final_step=False, + ) + continue + state = self.robot.body_data.fetch_state() + velocity = state["qvel"][:, self.policy_joint_ids] + if step == self.cfg.sim_steps_per_control - 1: + self._previous_physics_joint_velocity.copy_(velocity) + effort = ( + self._kp + * (self._position_target - state["qpos"][:, self.policy_joint_ids]) + - self._kd * velocity + ) + if self.explicit_pd_effort_control: + effort = dc_motor_effort( + effort, + velocity, + self._effort_limit, + self._velocity_limit, + self._effort_limit, + ) + self.robot.set_qf(effort, joint_ids=self.policy_joint_ids) + else: + effort = effort.clamp(-self._effort_limit, self._effort_limit) + self._applied_effort.copy_(effort) + self.sim.update( + self.physics_dt, 1, after_substep=after_substep, render_final_step=False + ) + self._sample_reference_contacts() + + def _sample_reference_contacts(self) -> None: + """Reduce all counterpart normal impulses to tracked body net forces.""" + if getattr(self, "_reference_defer_observations", False) and not ( + self._reference_sensor.requires_substep_update + ): + solver_dt = self.physics_dt / self.cfg.sim_cfg.physics_cfg.num_substeps + self._reference_sensor._reduce_normal_impulses( + self._sorted_keys, self._sorted_rows, self._normal_force, solver_dt + ) + else: + self._reference_sensor.update() + data = self._reference_sensor.get_data() + ids = data["user_ids"].to(torch.long) + valid = data["is_valid"] + solver_dt = self.physics_dt / self.cfg.sim_cfg.physics_cfg.num_substeps + force = data["normal"] * data["impulse"][:, :, None] / solver_dt + env = torch.arange(self.num_envs, device=self.device)[:, None, None] + keys = (env * (2**32) + ids).flatten() + index = torch.searchsorted(self._sorted_keys, keys).clamp( + max=self._sorted_keys.numel() - 1 + ) + match = (self._sorted_keys[index] == keys) & valid[:, :, None].expand_as( + ids + ).flatten() + forces = torch.stack((-force, force), dim=2).flatten(0, 2) + self._normal_force.zero_() + self._normal_force.view(-1, 3).index_add_( + 0, self._sorted_rows[index], torch.where(match[:, None], forces, 0.0) + ) + self._normal_history[:, 1:] = self._normal_history[:, :-1].clone() + self._normal_history[:, 0].copy_(self._normal_force) + update_contact_times( + self._normal_force, + self.step_dt, + self._air_time, + self._contact_time, + self._last_air_time, + self._last_contact_time, + ) + + def _update_sim_state(self, **kwargs: Any) -> None: + data = super()._update_sim_state(**kwargs) + # Explicit PD effort actuators update articulation data once per + # decimation block; implicit-drive robots update it per physics step. + if not self.explicit_pd_effort_control: + self._joint_acceleration.copy_( + (self._previous_joint_velocity - self._previous_physics_joint_velocity) + / self.physics_dt + ) + if data is not None: + self._state_cache = self._make_reference_state(data) + self._observation_cache = None + + def _advance_command_timer(self) -> None: + if not getattr(self, "_reference_events_deferred", False): + super()._advance_command_timer() + + def _update_heading_commands(self) -> None: + if not getattr(self, "_reference_events_deferred", False): + super()._update_heading_commands() + + def _apply_reference_interval_events(self) -> None: + """Apply velocity pushes after rewards and episode resets.""" + if getattr(self.cfg, "randomization", False): + self._push_remaining.sub_(self.step_dt) + ids = (self._push_remaining <= 0.0).nonzero().flatten() + if ids.numel(): + velocity = self.robot.body_data.root_vel[ids].clone() + velocity[:, :2].uniform_( + -0.5, 0.5, generator=self._locomotion_generator + ) + heights = self.get_velocity_locomotion_state().height_scan + self.robot.set_state(env_ids=ids, root_velocity=velocity) + # Velocity writes preserve the ray origins and terrain geometry. + self._state_cache = None + self._reference_scan_reuse = heights + self._observation_cache = None + self._push_remaining[ids] = torch.empty( + ids.numel(), device=self.device + ).uniform_(10.0, 15.0, generator=self._locomotion_generator) + + def step( + self, + action: EnvAction, + *, + policy_observations_only: bool = False, + **kwargs: Any, + ) -> tuple[EnvObs, torch.Tensor, torch.Tensor, torch.Tensor, dict[str, Any]]: + """Compute rewards and resets before commands, pushes and observations.""" + self._reference_defer_observations = ( + policy_observations_only + and getattr(self, "_reference_task_managers_only", True) + and not any( + ( + getattr(self, "rollout_buffer", None) is not None, + getattr(self, "dataset_manager", None) is not None, + getattr(self, "_traj_buffer", None) is not None, + bool( + getattr( + getattr(self, "event_manager", None), "available_modes", () + ) + ), + ) + ) + ) + self._reference_policy_observations_only = ( + self._reference_defer_observations + and getattr(self, "_reference_task_managers_only", False) + ) + self._reference_events_deferred = True + try: + try: + _, rewards, terminated, truncated, info = super().step(action, **kwargs) + finally: + self._reference_events_deferred = False + self._reference_defer_observations = False + self._advance_command_timer() + self._update_heading_commands() + self._apply_reference_interval_events() + return self.get_obs(**kwargs), rewards, terminated, truncated, info + finally: + self._reference_policy_observations_only = False + + def _get_sensor_obs(self, **kwargs: Any) -> TensorDict: + """Read task-owned contact samples without querying an invalid reset state. + + Contact rows are sampled by ``_sample_reference_contacts`` after physics + and cleared by the sensor reset. Other sensors retain their normal + observation update lifecycle. + """ + self.sim.render_camera_group(self._camera_group_ids) + obs = TensorDict({}, batch_size=[self.num_envs], device=self.device) + for name, sensor in self.sensors.items(): + if ( + sensor is not self._reference_sensor + and not sensor.requires_substep_update + and getattr(getattr(sensor, "cfg", None), "sample_phase", None) is None + ): + sensor.update(fetch_only=True) + obs[name] = sensor.get_data() + return obs + + def get_obs(self, **kwargs: Any) -> EnvObs: + """Defer unused training observations until commands and resets finish.""" + if getattr(self, "_reference_defer_observations", False): + return {} + if getattr(self, "_reference_policy_observations_only", False): + obs = TensorDict({}, batch_size=[self.num_envs], device=self.device) + return self._extend_obs(obs, **kwargs) + return super().get_obs(**kwargs) + + def get_training_state(self) -> dict[str, Any]: + """Return curriculum state for continuation from fresh episodes.""" + course = self._reference_terrain_course + return {"terrain_course": course.state_dict() if course is not None else None} + + def validate_training_state(self, state: dict[str, Any]) -> None: + """Validate saved terrain state before the trainer restores learning state. + + Args: + state: Environment state returned by :meth:`get_training_state`. + + Raises: + ValueError: The saved curriculum does not match this environment. + """ + course = self._reference_terrain_course + saved = state["terrain_course"] + if (course is None) != (saved is None): + raise ValueError("Checkpoint terrain curriculum configuration differs") + if course is not None: + course.validate_state_dict(saved) + + def load_training_state(self, state: dict[str, Any]) -> None: + """Restore the curriculum without advancing it on the first reset. + + Args: + state: Environment state returned by :meth:`get_training_state`. + """ + self.validate_training_state(state) + course = self._reference_terrain_course + saved = state["terrain_course"] + if course is not None: + course.load_state_dict(saved) + self._reference_resume_reset = torch.ones( + self.num_envs, dtype=torch.bool, device=self.device + ) + + def _before_episode_reset(self, env_ids: torch.Tensor, **kwargs: Any) -> None: + super()._before_episode_reset(env_ids, **kwargs) + course = self._reference_terrain_course + if course is None: + return + eligible = self._elapsed_steps[env_ids] > 0 + resume_reset = getattr(self, "_reference_resume_reset", None) + if resume_reset is not None: + eligible &= ~resume_reset[env_ids] + resume_reset[env_ids] = False + update_ids = env_ids[eligible] + if update_ids.numel(): + course.update( + update_ids, self.robot.body_data.root_pose[:, :3], self.reward_command + ) + + def _initialize_episode( + self, env_ids: Sequence[int] | None = None, **kwargs: Any + ) -> None: + ids = ( + torch.arange(self.num_envs, device=self.device) + if env_ids is None + else torch.as_tensor(env_ids, device=self.device, dtype=torch.long) + ) + if ids.numel() == 0: + return + previous_deferred = getattr(self, "_reference_events_deferred", False) + self._reference_events_deferred = True + try: + super()._initialize_episode(env_ids=env_ids, **kwargs) + finally: + self._reference_events_deferred = previous_deferred + deterministic = getattr(self.cfg, "deterministic_reset", True) + pose = self._reference_reset_pose + self._reference_reset_pose = None + if deterministic: + pose.zero_() + pose[:, 6] = 1.0 + pose[:, 2] = self.velocity_task_config.data["robot"]["root_height"] + origin = ( + self._reference_terrain_course.env_origins[ids] + if self._reference_terrain_course is not None + else getattr(self, "_reference_origin", None) + ) + if origin is None: + origin = pose.new_tensor(getattr(self.cfg, "origin", (0.0, 0.0, 0.0))) + pose[:, :3] += origin + constants = getattr(self, "_reference_constants", None) + default_position = ( + constants.default_joint_position + if constants is not None + else pose.new_tensor(self.velocity_task_config.default_joint_position) + ) + joint = default_position.expand(ids.numel(), -1).clone() + velocity = torch.zeros((ids.numel(), 6), device=self.device) + if not deterministic: + bounds = self.velocity_task_config.data["events"]["reset_robot_joints"][ + "params" + ]["position_range"] + joint *= torch.empty_like(joint).uniform_( + *bounds, generator=self._locomotion_generator + ) + velocity.uniform_(-0.5, 0.5, generator=self._locomotion_generator) + limits = ( + constants.joint_limits + if constants is not None + else joint.new_tensor( + self.velocity_task_config.data["robot"]["soft_joint_pos_limits"] + ) + ) + joint = joint.clamp(limits[:, 0], limits[:, 1]) + joint_velocity = torch.zeros_like(joint) + if self._reference_initial_states is not None: + selected = ids % self._reference_initial_states["root_pose"].shape[0] + pose = self._reference_initial_states["root_pose"][selected] + velocity = self._reference_initial_states["root_velocity"][selected] + joint = self._reference_initial_states["qpos"][selected] + joint_velocity = self._reference_initial_states["qvel"][selected] + self.robot.set_state( + env_ids=ids, + joint_ids=self.policy_joint_ids, + root_pose=pose, + qpos=joint, + qvel=joint_velocity, + target_qpos=joint, + root_velocity=velocity, + clear_dynamics=True, + ) + self._position_target[ids] = joint + for buffer in ( + self._normal_force, + self._normal_history, + self._air_time, + self._contact_time, + self._last_air_time, + self._last_contact_time, + self._applied_effort, + self._previous_physics_joint_velocity, + ): + buffer.index_fill_(0, ids, 0.0) + self._push_remaining[ids] = torch.empty( + ids.numel(), device=self.device + ).uniform_(10.0, 15.0, generator=self._locomotion_generator) + self._observation_cache = None + self._invalidate_task_cache() + + def _write_episode_initial_state( + self, + env_ids: torch.Tensor, + root_pose: torch.Tensor, + joint_position: torch.Tensor, + ) -> None: + """Keep sampled RNG values until the reference reset writes final state.""" + self._reference_reset_pose = root_pose.clone() + + def get_velocity_locomotion_state(self) -> RoughVelocityState: + """Fetch policy-ordered task state and actual torso height rays.""" + if self._state_cache is not None: + if ( + self._height_scanner is not None + and self._state_cache.height_scan.shape[-1] == 0 + and not getattr(self, "_reference_defer_observations", False) + ): + self._state_cache = replace( + self._state_cache, height_scan=self._compute_reference_height_scan() + ) + return self._state_cache + self._state_cache = self._make_reference_state( + self.robot.body_data.fetch_state() + ) + return self._state_cache + + def _make_reference_state( + self, data: dict[str, torch.Tensor] + ) -> RoughVelocityState: + """Build task state from the current control frame's articulation read.""" + quat = data["root_pose"][:, 3:7] + if self.device.type == "cuda" and not any( + value.requires_grad + for value in (quat, data["root_lin_vel"], data["root_ang_vel"]) + ): + from ._frame import _reference_frame_vectors + + linear_b, angular_b, gravity_b, linear_yaw = _reference_frame_vectors( + quat, data["root_lin_vel"], data["root_ang_vel"] + ) + else: + gravity = torch.zeros_like(data["root_lin_vel"]) + gravity[:, 2] = -1.0 + x, y, z, w = quat.unbind(-1) + yaw = torch.atan2(2 * (w * z + x * y), 1 - 2 * (y.square() + z.square())) + yaw_quat = torch.zeros_like(quat) + yaw_quat[:, 2] = torch.sin(yaw / 2) + yaw_quat[:, 3] = torch.cos(yaw / 2) + linear_b = quat_apply_inverse(quat, data["root_lin_vel"]) + angular_b = quat_apply_inverse(quat, data["root_ang_vel"]) + gravity_b = quat_apply_inverse(quat, gravity) + linear_yaw = quat_apply_inverse(yaw_quat, data["root_lin_vel"]) + count = len(self.foot_link_names) + heights = data["qpos"].new_empty((self.num_envs, 0)) + if self._height_scanner is not None and not getattr( + self, "_reference_defer_observations", False + ): + heights = self._compute_reference_height_scan() + return RoughVelocityState( + base_lin_vel_b=linear_b, + base_ang_vel_b=angular_b, + projected_gravity_b=gravity_b, + base_lin_vel_yaw=linear_yaw, + base_ang_vel_w=data["root_ang_vel"], + command=self.reward_command, + joint_pos=data["qpos"][:, self.policy_joint_ids], + joint_vel=data["qvel"][:, self.policy_joint_ids], + joint_acc=self._joint_acceleration, + applied_torque=self._applied_effort, + action=self.locomotion_action, + previous_action=self.last_locomotion_action, + foot_vel_w=self.robot.body_data.body_link_vel[:, self.foot_link_ids, :3], + foot_force_history=self._normal_history[:, :, :count], + current_air_time=self._air_time[:, :count], + current_contact_time=self._contact_time[:, :count], + last_air_time=self._last_air_time[:, :count], + illegal_force_history=self._normal_history[:, :, count:], + height_scan=heights, + episode_step=self._episode_step, + root_height=data["root_pose"][:, 2], + ) + + def _compute_reference_height_scan(self) -> torch.Tensor: + """Reuse height rays across velocity-only writes.""" + reused = getattr(self, "_reference_scan_reuse", None) + self._reference_scan_reuse = None + return ( + reused + if reused is not None + else self._height_scanner.compute( + self.robot.body_data.body_link_pose[:, self.orientation_link_id] + ) + ) + + def build_velocity_locomotion_observations( + self, *, enable_corruption: bool = False + ) -> tuple[torch.Tensor, torch.Tensor]: + """Return the same reference policy tensor to actor and critic.""" + if self._observation_cache is None: + state = replace(self.get_velocity_locomotion_state(), command=self.command) + noise = {} + if enable_corruption and not getattr( + self, "_reference_events_deferred", False + ): + for name, term in observation_terms( + self.velocity_task_config, + state, + constants=self._reference_constants, + ).items(): + spec = self.velocity_task_config.data["observations"][name]["noise"] + if spec: + noise[name] = torch.empty_like(term).uniform_( + spec["n_min"], + spec["n_max"], + generator=self._locomotion_generator, + ) + self._observation_cache = build_observation( + self.velocity_task_config, + state, + noise=noise, + constants=self._reference_constants, + ) + return self._observation_cache, self._observation_cache + + def _extend_obs(self, obs: EnvObs, **kwargs: Any) -> EnvObs: + """Generate fresh noise per observation pass and share it with the critic.""" + self._observation_cache = None + return super()._extend_obs(obs, **kwargs) + + def compute_task_state( + self, **kwargs: Any + ) -> tuple[torch.Tensor, torch.Tensor, dict[str, torch.Tensor]]: + """Return body-contact failures and reference frame tracking errors.""" + state = self.get_velocity_locomotion_state() + terminated, _ = compute_termination(self.velocity_task_config, state) + velocity = ( + state.base_lin_vel_yaw + if self.velocity_task_config.action_dim == 37 + else state.base_lin_vel_b + ) + angular = ( + state.base_ang_vel_w + if self.velocity_task_config.action_dim == 37 + else state.base_ang_vel_b + ) + return ( + torch.zeros_like(terminated), + terminated, + { + "linear_velocity_error": (state.command[:, :2] - velocity[:, :2]).norm( + dim=-1 + ), + "yaw_rate_error": (state.command[:, 2] - angular[:, 2]).abs(), + "root_height": state.root_height, + "maximum_illegal_contact_force": state.illegal_force_history.norm( + dim=-1 + ).amax(dim=(1, 2)), + }, + ) diff --git a/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/terrain.py b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/terrain.py new file mode 100644 index 0000000000..038341f1c6 --- /dev/null +++ b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/terrain.py @@ -0,0 +1,214 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Yaw-aligned height rays over collision terrain.""" + +from __future__ import annotations + +import numpy as np +import newton +import torch +import warp as wp + +__all__ = ["HeightScanner"] + + +@wp.kernel(enable_backward=False) +def _prepare_rays( + pose: wp.array2d(dtype=wp.float32), + pattern: wp.array(dtype=wp.vec3), + origins: wp.array(dtype=wp.vec3), + directions: wp.array(dtype=wp.vec3), +): + env, ray = wp.tid() + x, y, z, w = pose[env, 3], pose[env, 4], pose[env, 5], pose[env, 6] + yaw = wp.atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z)) + rotation = wp.quat(0.0, 0.0, wp.sin(yaw * 0.5), wp.cos(yaw * 0.5)) + point = pattern[ray] + wp.vec3(0.0, 0.0, 20.0) + index = env * pattern.shape[0] + ray + origins[index] = wp.quat_rotate(rotation, point) + wp.vec3( + pose[env, 0], pose[env, 1], pose[env, 2] + ) + directions[index] = wp.vec3(0.0, 0.0, -1.0) + + +@wp.kernel(enable_backward=False) +def _resolve_heights( + pose: wp.array2d(dtype=wp.float32), + origins: wp.array(dtype=wp.vec3), + distances: wp.array(dtype=wp.float32), + height: wp.array2d(dtype=wp.float32), +): + env, ray = wp.tid() + index = env * height.shape[1] + ray + distance = distances[index] + value = wp.float32(wp.inf) + if distance >= 0.0 and distance <= 1000000.0: + value = pose[env, 2] - (origins[index][2] - distance) - 0.5 + height[env, ray] = value + + +@wp.kernel +def _scan( + mesh: wp.uint64, + pose: wp.array2d(dtype=wp.float32), + pattern: wp.array(dtype=wp.vec3), + height: wp.array2d(dtype=wp.float32), +): + env, ray = wp.tid() + x, y, z, w = pose[env, 3], pose[env, 4], pose[env, 5], pose[env, 6] + yaw = wp.atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z)) + p = pattern[ray] + start = wp.vec3( + pose[env, 0] + wp.cos(yaw) * p[0] - wp.sin(yaw) * p[1], + pose[env, 1] + wp.sin(yaw) * p[0] + wp.cos(yaw) * p[1], + pose[env, 2] + 20.0, + ) + direction = wp.vec3(0.0, 0.0, -1.0) + query = wp.mesh_query_ray(mesh, start, direction, 1000000.0) + value = wp.float32(wp.inf) + if query.result: + value = pose[env, 2] - (start[2] - query.t) - 0.5 + height[env, ray] = value + + +class HeightScanner: + """187 terrain rays in the torso yaw frame, XY grid order and height offset.""" + + def __init__( + self, + vertices: np.ndarray, + triangles: np.ndarray, + num_envs: int, + device: torch.device, + *, + physics_model: newton.Model | None = None, + physics_state: newton.State | None = None, + ) -> None: + self.physics_model = physics_model + self.mesh = ( + None + if physics_model is not None + else wp.Mesh( + points=wp.array(vertices, dtype=wp.vec3, device=str(device)), + indices=wp.array( + triangles.reshape(-1), dtype=wp.int32, device=str(device) + ), + ) + ) + x = torch.arange(-0.8, 0.8 + 1.0e-9, 0.1, device=device) + y = torch.arange(-0.5, 0.5 + 1.0e-9, 0.1, device=device) + xx, yy = torch.meshgrid(x, y, indexing="xy") + pattern = torch.stack( + (xx.flatten(), yy.flatten(), torch.zeros_like(xx.flatten())), -1 + ) + self.pattern = wp.from_torch(pattern, dtype=wp.vec3) + self.height = torch.empty((num_envs, 187), device=device) + self._pose = torch.empty((num_envs, 7), device=device) + self._pose_warp = wp.from_torch(self._pose) + self._graph = None + self._scan_stream = wp.Stream(str(device)) if device.type == "cuda" else None + self._torch_stream = ( + wp.stream_to_torch(self._scan_stream) + if self._scan_stream is not None + else None + ) + if physics_model is not None: + if physics_state is None: + raise ValueError("The physics height scanner requires the model state.") + physics_model.bvh_build_shapes( + physics_state, + shape_flags=newton.ShapeFlags.COLLIDE_SHAPES + | newton.ShapeFlags.VISIBLE, + ) + count = num_envs * 187 + self._origins = wp.empty(count, dtype=wp.vec3, device=str(device)) + self._directions = wp.empty(count, dtype=wp.vec3, device=str(device)) + self._distances = wp.empty(count, dtype=wp.float32, device=str(device)) + self._worlds = wp.full(count, -1, dtype=wp.int32, device=str(device)) + + def compute(self, pose: torch.Tensor) -> torch.Tensor: + """Return torso height minus terrain hit height minus 0.5 m. + + Physics-backed deployments query the global collision heightfield through + the same Newton BVH ray interface as the reference sensor. + """ + if pose.is_cuda: + caller = torch.cuda.current_stream(pose.device) + self._torch_stream.wait_stream(caller) + with ( + wp.ScopedStream(self._scan_stream), + torch.cuda.stream(self._torch_stream), + ): + self._pose.copy_(pose) + if self._graph is None: + self._compute_height() + with wp.ScopedCapture(stream=self._scan_stream) as capture: + self._compute_height() + self._graph = capture.graph + else: + wp.capture_launch(self._graph, stream=self._scan_stream) + caller.wait_stream(self._torch_stream) + else: + self._pose.copy_(pose) + self._compute_height() + return self.height + + def _compute_height(self) -> None: + """Launch the fixed-buffer ray pipeline without changing its geometry.""" + if self.physics_model is not None: + wp.launch( + _prepare_rays, + dim=self.height.shape, + inputs=[ + self._pose_warp, + self.pattern, + self._origins, + self._directions, + ], + device=str(self._pose.device), + ) + newton.intersect_ray( + self.physics_model, + ray_origins=self._origins, + ray_directions=self._directions, + ray_worlds=self._worlds, + enable_global_world=False, + out_dist=self._distances, + ) + wp.launch( + _resolve_heights, + dim=self.height.shape, + inputs=[ + self._pose_warp, + self._origins, + self._distances, + wp.from_torch(self.height), + ], + device=str(self._pose.device), + ) + return + wp.launch( + _scan, + dim=self.height.shape, + inputs=[ + self.mesh.id, + self._pose_warp, + self.pattern, + wp.from_torch(self.height), + ], + device=str(self._pose.device), + ) diff --git a/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/__init__.py b/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/__init__.py new file mode 100644 index 0000000000..d4b0816a0a --- /dev/null +++ b/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/__init__.py @@ -0,0 +1,31 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +from .leap_cube_face_turn import LeapCubeFaceTurnEnv +from .leap_cube_reorient import LeapCubeReorientEnv +from .leap_cube_rotate_z import LeapCubeRotateZAxisEnv +from .shadow_pen_continuous import ShadowPenContinuousEnv +from .shadow_pen_fixed import ShadowPenFixedEnv + +__all__ = [ + "LeapCubeFaceTurnEnv", + "LeapCubeReorientEnv", + "LeapCubeRotateZAxisEnv", + "ShadowPenContinuousEnv", + "ShadowPenFixedEnv", +] diff --git a/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_leap_tasks.py b/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_leap_tasks.py new file mode 100644 index 0000000000..1955a97ac3 --- /dev/null +++ b/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_leap_tasks.py @@ -0,0 +1,486 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Torch MDP definitions for the three retained LEAP manipulation tasks. + +Cube reorientation and rotation equations are adapted from MuJoCo Playground, +Copyright 2025 DeepMind Technologies Limited, under Apache 2.0. +The physics bridge supplies named tensors; this module does not step physics. +Quaternion tensors use wxyz throughout. +""" + +from __future__ import annotations + +import math +from collections.abc import Mapping + +import torch +from torch import Tensor + +__all__ = ["LeapTask", "quat_multiply", "quat_matrix", "quat_integrate"] + +TASK_IDS = ("LeapCubeFaceTurn", "LeapCubeReorient", "LeapCubeRotateZAxis") + + +def quat_multiply(a: Tensor, b: Tensor) -> Tensor: + """Multiply batched wxyz quaternions.""" + aw, av = a[..., :1], a[..., 1:] + bw, bv = b[..., :1], b[..., 1:] + return torch.cat( + ( + aw * bw - (av * bv).sum(-1, keepdim=True), + aw * bv + bw * av + torch.linalg.cross(av, bv, dim=-1), + ), + dim=-1, + ) + + +def _normalize(q: Tensor) -> Tensor: + return q / q.norm(dim=-1, keepdim=True).clamp_min(1e-12) + + +def _inverse(q: Tensor) -> Tensor: + return torch.cat((q[..., :1], -q[..., 1:]), dim=-1) + + +def quat_matrix(q: Tensor) -> Tensor: + """Return batched rotation matrices for normalized wxyz quaternions.""" + w, x, y, z = q.unbind(-1) + return torch.stack( + ( + 1 - 2 * (y * y + z * z), + 2 * (x * y - z * w), + 2 * (x * z + y * w), + 2 * (x * y + z * w), + 1 - 2 * (x * x + z * z), + 2 * (y * z - x * w), + 2 * (x * z - y * w), + 2 * (y * z + x * w), + 1 - 2 * (x * x + y * y), + ), + dim=-1, + ).reshape(*q.shape[:-1], 3, 3) + + +def quat_integrate(q: Tensor, velocity: Tensor, dt: float) -> Tensor: + """Integrate local angular velocity by right-multiplying the increment.""" + angle = velocity.norm(dim=-1, keepdim=True) * dt + factor = 0.5 * dt * torch.sinc(angle / (2 * math.pi)) + delta = torch.cat((torch.cos(angle / 2), velocity * factor), dim=-1) + return _normalize(quat_multiply(q, delta)) + + +def _orientation_error(q: Tensor, goal: Tensor) -> Tensor: + diff = _normalize(quat_multiply(q, _inverse(goal))) + return 2 * torch.asin(diff[..., 1:].norm(dim=-1).clamp(max=1)) + + +def _linear_tolerance(error: Tensor, bound: float, margin: float) -> Tensor: + # Playground tolerance uses value_at_margin=0.1 for the linear sigmoid. + return (1 - 0.9 * (error - bound).clamp_min(0) / margin).clamp_min(0) + + +class LeapTask: + """Maintain task state and evaluate batched source-compatible LEAP MDPs. + + Args: + task_id: One of LeapCubeFaceTurn, LeapCubeReorient, LeapCubeRotateZAxis. + num_envs: Number of parallel environments. + qpos0: Source MJCF home qpos; face turn uses the saved settled qpos. + ctrl0: Source actuator home targets in the 16-joint policy order. + lower: Actuator control lower bounds. + upper: Actuator control upper bounds. + device: Device shared with physics state tensors. + seed: Seed for task reset and observation noise. + noise_level: Multiplier for the reference observation noise. + """ + + def __init__( + self, + task_id: str, + num_envs: int, + qpos0: Tensor, + ctrl0: Tensor, + lower: Tensor, + upper: Tensor, + device: str | torch.device = "cpu", + seed: int = 42, + noise_level: float = 1.0, + ) -> None: + task_id = {"M3": TASK_IDS[0], "M4": TASK_IDS[1], "M5": TASK_IDS[2]}.get( + task_id, task_id + ) + if task_id not in TASK_IDS: + raise ValueError(f"Unknown LEAP task: {task_id}") + self.task_id = task_id + self.num_envs = num_envs + self.device = torch.device(device) + self.dtype = torch.float32 + self.qpos0, self.ctrl0, self.lower, self.upper = ( + torch.as_tensor(x, device=self.device, dtype=self.dtype).clone() + for x in (qpos0, ctrl0, lower, upper) + ) + self.face_turn = task_id == "LeapCubeFaceTurn" + self.reorient = task_id == "LeapCubeReorient" + expected_q = 24 if self.face_turn else 23 + if self.qpos0.shape != (expected_q,) or self.ctrl0.shape != (16,): + raise ValueError(f"Expected qpos ({expected_q},) and ctrl (16,)") + if self.lower.shape != (16,) or self.upper.shape != (16,): + raise ValueError("Expected 16 actuator control bounds") + self.default_pose = self.qpos0[:16].clone() + self.action_dim = 16 + self.actor_dim = 65 if self.face_turn else (57 if self.reorient else 32) + self.critic_dim = 65 if self.face_turn else (128 if self.reorient else 105) + self.control_dt = 0.05 + self.sim_dt = 0.005 if self.face_turn else 0.01 + self.substeps = round(self.control_dt / self.sim_dt) + self.episode_length = 1000 if self.reorient else 500 + self.noise_level = noise_level + self.generator = torch.Generator(device=self.device).manual_seed(seed) + self.last_action = self._zeros(16) + self.last_last_action = self._zeros(16) + self.motor_targets = self._zeros(16) + self.initial_pos = self._zeros(3) + self.initial_quat = self._zeros(4) + self.goal_quat = self._zeros(4) + self.goal_velocity = self._zeros(3) + self.goal_angle = self._zeros() + self.previous_face_angle = self._zeros() + self.rotation = self._zeros() + self.held = torch.zeros(num_envs, dtype=torch.long, device=self.device) + self.success_count = torch.zeros_like(self.held) + self.steps = torch.zeros_like(self.held) + self.steps_since_success = torch.zeros_like(self.held) + self.goal_quat[:, 0] = 1 + + def _zeros(self, *shape: int) -> Tensor: + return torch.zeros( + (self.num_envs, *shape), device=self.device, dtype=self.dtype + ) + + def _rand(self, shape: tuple[int, ...], normal: bool = False) -> Tensor: + function = torch.randn if normal else torch.rand + return function( + shape, device=self.device, dtype=self.dtype, generator=self.generator + ) + + def _ids(self, env_ids: Tensor | None) -> Tensor: + if env_ids is None: + return torch.arange(self.num_envs, device=self.device) + return torch.as_tensor(env_ids, device=self.device, dtype=torch.long) + + def _uniform_quat(self, count: int) -> Tensor: + u, v, w = self._rand((count, 3)).unbind(-1) + return torch.stack( + ( + (1 - u).sqrt() * torch.sin(2 * math.pi * v), + (1 - u).sqrt() * torch.cos(2 * math.pi * v), + u.sqrt() * torch.sin(2 * math.pi * w), + u.sqrt() * torch.cos(2 * math.pi * w), + ), + dim=-1, + ) + + def reset(self, env_ids: Tensor | None = None) -> dict[str, Tensor]: + """Reset selected task slots and return their source-ordered states. + + The bridge writes qpos/qvel/ctrl, refreshes kinematics, and calls observe + on only these slots. All returned arrays have len(env_ids) rows. + """ + ids = self._ids(env_ids) + count = ids.numel() + qpos = self.qpos0.expand(count, -1).clone() + qvel = torch.zeros( + (count, self.qpos0.numel() - 1), device=self.device, dtype=self.dtype + ) + if self.face_turn: + qpos[:, :16] += 0.02 * self._rand((count, 16), normal=True) + qpos[:, 16:19] += 0.006 * (self._rand((count, 3)) - 0.5) + ctrl = self.ctrl0.expand(count, -1).clone() + else: + qpos[:, :16] = ( + self.default_pose + 0.1 * self._rand((count, 16), normal=True) + ).clamp(self.lower, self.upper) + qpos[:, 16:19] = qpos.new_tensor([0.1, 0, 0.05]) + 0.02 * ( + self._rand((count, 3)) - 0.5 + ) + qpos[:, 19:23] = self._uniform_quat(count) + ctrl = qpos[:, :16].clone() + self.initial_pos[ids] = qpos[:, 16:19] + self.initial_quat[ids] = qpos[:, 19:23] + self.motor_targets[ids] = ctrl + self.last_action[ids] = 0 + self.last_last_action[ids] = 0 + self.goal_velocity[ids] = 0 + self.goal_angle[ids] = math.pi / 2 + self.previous_face_angle[ids] = qpos[:, 23] if self.face_turn else 0 + self.rotation[ids] = 0 + self.held[ids] = 0 + self.success_count[ids] = 0 + self.steps[ids] = 0 + self.steps_since_success[ids] = 0 + if self.reorient: + self.goal_quat[ids] = self._uniform_quat(count) + else: + self.goal_quat[ids] = qpos.new_tensor([1, 0, 0, 0]) + return { + "env_ids": ids, + "qpos": qpos, + "qvel": qvel, + "ctrl": ctrl, + "goal_quat": self.goal_quat[ids].clone(), + } + + def control_targets(self, action: Tensor) -> Tensor: + """Map normalized policy actions to source actuator targets.""" + if action.shape != (self.num_envs, 16): + raise ValueError(f"Expected actions ({self.num_envs}, 16)") + if self.face_turn: + target = (self.ctrl0 + 0.8 * action).clamp(self.lower, self.upper) + elif self.reorient: + target = (self.motor_targets + 0.5 * action).clamp(self.lower, self.upper) + else: + target = self.default_pose + 0.6 * action + self.motor_targets.copy_(target) + return target + + def face_detent(self, face_angle: Tensor) -> Tensor: + """Return passive face torque, recomputed before each physics substep.""" + offset = face_angle - torch.round(face_angle / (math.pi / 2)) * (math.pi / 2) + return -0.01 * offset + + def observe( + self, state: Mapping[str, Tensor], env_ids: Tensor | None = None + ) -> dict[str, Tensor]: + """Build observations from full-batch physics tensors for selected slots. + + M3 requires source-ordered qpos/qvel. M4/M5 require hand_qpos, + hand_qvel, actuator_force, cube_pos, cube_quat, cube_linvel, + cube_angvel, palm_pos, and fingertip_pos. Fingertips are flattened + thumb/index/middle/ring positions expressed in the grasp-site frame. + """ + ids = self._ids(env_ids) + selected = {key: value[ids] for key, value in state.items()} + if self.face_turn: + qpos, qvel = selected["qpos"], selected["qvel"] + delta = self.goal_angle[ids] - qpos[:, 23] + obs = torch.cat( + ( + qpos[:, :16], + 0.05 * qvel[:, :16], + self.last_action[ids], + 10 * (qpos[:, 16:19] - self.initial_pos[ids]), + qpos[:, 19:23], + 0.1 * qvel[:, 16:22], + torch.stack( + (delta.sin(), delta.cos(), delta / math.pi, qvel[:, 22] * 0.1), + dim=-1, + ), + ), + dim=-1, + ) + return {"state": obs, "privileged_state": obs} + q, dq = selected["hand_qpos"], selected["hand_qvel"] + noisy_q = q + self.noise_level * 0.05 * (2 * self._rand(tuple(q.shape)) - 1) + pos, quat = selected["cube_pos"], selected["cube_quat"] + pos_error = selected["palm_pos"] - pos + if self.reorient: + noisy_pos = pos + self.noise_level * 0.02 * ( + 2 * self._rand(tuple(pos.shape)) - 1 + ) + noisy_quat = _normalize( + quat + + self.noise_level * 0.1 * self._rand(tuple(quat.shape), normal=True) + ) + diff = quat_multiply(noisy_quat, _inverse(self.goal_quat[ids])) + ori_features = quat_matrix(diff).flatten(1)[:, 3:] + clean_diff = quat_multiply(quat, _inverse(self.goal_quat[ids])) + clean_ori = quat_matrix(clean_diff).flatten(1)[:, 3:] + obs = torch.cat( + ( + noisy_q, + noisy_q - self.motor_targets[ids], + selected["palm_pos"] - noisy_pos, + ori_features, + self.last_action[ids], + ), + dim=-1, + ) + perturbation = selected.get( + "perturbation_direction", q.new_zeros((len(ids), 6)) + ) + applied = selected.get("cube_external_force", q.new_zeros((len(ids), 6))) + critic = torch.cat( + ( + obs, + q, + dq, + selected["fingertip_pos"], + pos_error, + clean_ori, + selected["cube_linvel"], + selected["cube_angvel"], + perturbation, + applied, + ), + dim=-1, + ) + else: + obs = torch.cat((noisy_q, self.last_action[ids]), dim=-1) + critic = torch.cat( + ( + obs, + q, + dq, + selected["actuator_force"], + selected["fingertip_pos"], + pos_error, + quat, + selected["cube_angvel"], + selected["cube_linvel"], + ), + dim=-1, + ) + return {"state": obs, "privileged_state": critic} + + def after_step( + self, state: Mapping[str, Tensor], action: Tensor + ) -> tuple[dict[str, Tensor], Tensor, Tensor, Tensor, dict[str, Tensor]]: + """Evaluate one control interval after physics and retain task counters. + + No slots reset automatically. M4 updates goal_quat after constructing + the returned observation, preserving the source implementation order. + The bridge may copy goal_quat to the visual goal after this call. + """ + if self.face_turn: + qpos, qvel = state["qpos"], state["qvel"] + pos, quat, angle = qpos[:, 16:19], qpos[:, 19:23], qpos[:, 23] + drift = (pos - self.initial_pos).norm(dim=-1) + terminated = ( + (pos[:, 2] < -0.03) | (drift > 0.15) | ~torch.isfinite(qpos).all(-1) + ) + error = (self.goal_angle - angle).abs() + previous_error = (self.goal_angle - self.previous_face_angle).abs() + base_angle = 2 * torch.acos( + (quat * self.initial_quat).sum(-1).abs().clamp(0, 1) + ) + good = ( + (error < 0.1) + & (qvel[:, 22].abs() < 1.5) + & (base_angle < 0.5) + & ~terminated + ) + self.held.copy_(torch.where(good, self.held + 1, 0)) + success = self.held >= 20 + cost = 0.02 * (action - self.last_action).square().mean(-1) + reward = ( + 5 * (previous_error - error) + + (2 * torch.exp(-3 * error) - 0.2 * base_angle - 5 * drift - cost) + * self.control_dt + + 10 * success.float() + - 5 * terminated.float() + ) + increment = angle - self.previous_face_angle + self.rotation += torch.atan2(increment.sin(), increment.cos()) + self.previous_face_angle.copy_(angle) + self.goal_angle += success.float() * (math.pi / 2) + self.held.masked_fill_(success, 0) + self.success_count += success.long() + self.last_last_action.copy_(self.last_action) + self.last_action.copy_(action) + obs = self.observe(state) + metrics = { + "success": success.float(), + "error": error, + "base_angle": base_angle, + "drift": drift, + "rotation": self.rotation.clone(), + "retained": (~terminated).float(), + "face_angle": angle.clone(), + "face_velocity": qvel[:, 22].clone(), + } + else: + pos, quat = state["cube_pos"], state["cube_quat"] + terminated = pos[:, 2] < -0.05 + if self.reorient: + for name in ( + "hand_qpos", + "hand_qvel", + "cube_pos", + "cube_quat", + "cube_linvel", + "cube_angvel", + ): + terminated = terminated | ~torch.isfinite(state[name]).all(-1) + error = _orientation_error(quat, self.goal_quat) + success = error < 0.1 + self.success_count += success.long() + self.steps_since_success.copy_( + torch.where(success, 0, self.steps_since_success + 1) + ) + pos_error = (state["palm_pos"] - pos).norm(dim=-1) + pose_cost = (state["hand_qpos"] - self.default_pose).square().sum(-1) + rate_cost = (action - self.last_action).square().sum(-1) + ( + action - 2 * self.last_action + self.last_last_action + ).square().sum(-1) + energy = (state["hand_qvel"].abs() * state["actuator_force"].abs()).sum( + -1 + ) + reward = ( + 5 * _linear_tolerance(error, 0.2, math.pi) + + 0.5 * _linear_tolerance(pos_error, 0.02, 0.05) + - 100 * terminated.float() + - 0.5 * pose_cost + - 0.001 * rate_cost + - 0.001 * energy + ) * self.control_dt + 100 * success.float() + obs = self.observe(state) + self.goal_velocity.copy_( + torch.where( + success[:, None], + 1 + 4 * self._rand((self.num_envs, 3)), + 0.8 * self.goal_velocity, + ) + ) + self.goal_quat.copy_( + quat_integrate( + self.goal_quat, self.goal_velocity, 2 * self.control_dt + ) + ) + metrics = { + "success": success.float(), + "error": error, + "position_error": pos_error, + "retained": (~terminated).float(), + "steps_since_success": self.steps_since_success.clone(), + } + else: + angvel = state["cube_angvel"][:, 2] + reward = (angvel - 100 * terminated.float()) * self.control_dt + self.rotation += angvel * self.control_dt + obs = self.observe(state) + metrics = { + "angular_velocity_z": angvel.clone(), + "rotation": self.rotation.clone(), + "retained": (~terminated).float(), + } + self.last_last_action.copy_(self.last_action) + self.last_action.copy_(action) + self.steps += 1 + truncated = (self.steps >= self.episode_length) & ~terminated + metrics["success_count"] = self.success_count.clone() + metrics["steps"] = self.steps.clone() + return obs, reward, terminated, truncated, metrics diff --git a/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_runtime.py b/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_runtime.py new file mode 100644 index 0000000000..973522e770 --- /dev/null +++ b/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_runtime.py @@ -0,0 +1,876 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""USD-native dexterous manipulation environments built on :class:`EmbodiedEnv`.""" + +from __future__ import annotations + +from collections.abc import Callable, Sequence +from dataclasses import dataclass +import math +from pathlib import Path +from typing import Any + +import gymnasium as gym +import numpy as np +from tensordict import TensorDict +import torch + +from dexsim.engine.newton_physics.runtime_control import GraphRuntimeControl +from embodichain.data import get_data_path +from embodichain.lab.gym.envs import EmbodiedEnv +from embodichain.lab.sim.cfg import ( + JointDrivePropertiesCfg, + RobotCfg, + UsdImportCfg, + UsdSourceCfg, +) +from embodichain.lab.sim.objects import Articulation, RigidObject, Robot +from embodichain.lab.sim.spawn.usd_import import _parse_usd_source, _source_path +from embodichain.lab.sim.types import EnvAction, EnvObs +from embodichain.learning.rl.policy_evaluation import PolicyViewerCameraCfg + +from ._leap_tasks import LeapTask +from ._shadow_task import ShadowState, ShadowTask + +__all__ = ["DexterousManipulationEnv", "ENV_NAMES", "TASK_SPECS", "rotate_wxyz"] + +ENV_NAMES: dict[str, str] = { + "M1": "ShadowPenFixedRL-v1", + "M2": "ShadowPenContinuousRL-v1", + "M3": "LeapCubeFaceTurnRL-v1", + "M4": "LeapCubeReorientRL-v1", + "M5": "LeapCubeRotateZAxisRL-v1", +} + + +@dataclass(frozen=True) +class _TaskSpec: + task_id: str + env_id: str + usd_asset: str + initial_asset: str + ctrl_dt: float + sim_dt: float + max_episode_steps: int + action_dim: int + policy_dim: int + critic_dim: int + has_episode_success: bool + viewer_eye_offset: tuple[float, float, float] + viewer_target_height: float + + +TASK_SPECS: dict[str, _TaskSpec] = { + "M1": _TaskSpec( + task_id="M1", + env_id="ShadowPenFixedRL-v1", + usd_asset="ShadowHandPen/usd/shadow_pen.usdc", + initial_asset="ShadowHandPen/ShadowPenSpin-initial.npz", + ctrl_dt=0.04, + sim_dt=0.002, + max_episode_steps=500, + action_dim=18, + policy_dim=87, + critic_dim=87, + has_episode_success=True, + viewer_eye_offset=(0.35, -0.35, 0.22), + viewer_target_height=0.20, + ), + "M2": _TaskSpec( + task_id="M2", + env_id="ShadowPenContinuousRL-v1", + usd_asset="ShadowHandPen/usd/shadow_pen.usdc", + initial_asset="ShadowHandPen/ShadowPenSpin-initial.npz", + ctrl_dt=0.04, + sim_dt=0.002, + max_episode_steps=500, + action_dim=18, + policy_dim=87, + critic_dim=87, + has_episode_success=True, + viewer_eye_offset=(0.35, -0.35, 0.22), + viewer_target_height=0.20, + ), + "M3": _TaskSpec( + task_id="M3", + env_id="LeapCubeFaceTurnRL-v1", + usd_asset="LeapHandCubeFaceTurn/usd/leap_cube_top_layer.usdc", + initial_asset="LeapHandCubeFaceTurn/LeapCubeFaceTurn-initial.npz", + ctrl_dt=0.05, + sim_dt=0.005, + max_episode_steps=500, + action_dim=16, + policy_dim=65, + critic_dim=65, + has_episode_success=True, + viewer_eye_offset=(0.32, -0.32, 0.20), + viewer_target_height=0.05, + ), + "M4": _TaskSpec( + task_id="M4", + env_id="LeapCubeReorientRL-v1", + usd_asset="LeapHandCube/usd/M4.usdc", + initial_asset="LeapHandCube/M4-initial.npz", + ctrl_dt=0.05, + sim_dt=0.01, + max_episode_steps=1000, + action_dim=16, + policy_dim=57, + critic_dim=128, + has_episode_success=True, + viewer_eye_offset=(0.34, -0.34, 0.22), + viewer_target_height=0.08, + ), + "M5": _TaskSpec( + task_id="M5", + env_id="LeapCubeRotateZAxisRL-v1", + usd_asset="LeapHandCube/usd/M5.usdc", + initial_asset="LeapHandCube/M5-initial.npz", + ctrl_dt=0.05, + sim_dt=0.01, + max_episode_steps=500, + action_dim=16, + policy_dim=32, + critic_dim=105, + has_episode_success=False, + viewer_eye_offset=(0.32, -0.32, 0.20), + viewer_target_height=0.08, + ), +} + + +class _DexUniSceneJointOwnership(GraphRuntimeControl): + """Assign all rigid and kinematic scene joints to DexUni's MuJoCo branch.""" + + def __init__(self) -> None: + self._assigned: tuple[int, ...] | None = None + + def initialize_model(self, model: Any, result: Any) -> None: + from dexsim.engine.newton_physics.backend_registry import get_newton_backend + + backend = get_newton_backend(result.world) + if backend is None or backend.solver_type != "dexuni": + return + cfg = backend.cfg.solver_cfg + if ( + cfg.mujoco_articulations is None + and cfg.mujoco_articulation_paths is None + and (cfg.mujoco_joints is None or cfg.mujoco_joints == self._assigned) + ): + self._assigned = tuple(range(model.joint_count)) + cfg.mujoco_joints = self._assigned + + def __call__( + self, context: Any, substep_index: int, substep_count: int, substep_dt: float + ) -> None: + del context, substep_index, substep_count, substep_dt + + def prepare_episode_reset( + self, arena_indices: tuple[int, ...] + ) -> Callable[[], None]: + del arena_indices + return lambda: None + + +def rotate_wxyz(q: torch.Tensor, v: torch.Tensor) -> torch.Tensor: + """Rotate 3D vectors ``v`` by normalized ``wxyz`` quaternions ``q``.""" + xyz = q[..., 1:] + t = 2.0 * torch.cross(xyz, v, dim=-1) + return v + q[..., :1] * t + torch.cross(xyz, t, dim=-1) + + +def _xyzw_to_wxyz(quat_xyzw: torch.Tensor) -> torch.Tensor: + return torch.cat((quat_xyzw[..., 3:4], quat_xyzw[..., :3]), dim=-1) + + +def _wxyz_to_xyzw(quat_wxyz: torch.Tensor) -> torch.Tensor: + return torch.cat((quat_wxyz[..., 1:4], quat_wxyz[..., :1]), dim=-1) + + +def _invert_wxyz(quat_wxyz: torch.Tensor) -> torch.Tensor: + return torch.cat((quat_wxyz[..., :1], -quat_wxyz[..., 1:]), dim=-1) + + +class DexterousManipulationEnv(EmbodiedEnv): + """Base USD-native :class:`EmbodiedEnv` for Shadow Hand and LEAP Hand RL tasks.""" + + task_id: str = "M1" + + def __init__(self, cfg: Any = None, **kwargs: Any) -> None: + if self.task_id not in TASK_SPECS: + raise ValueError( + f"Unknown dexterous manipulation task_id: {self.task_id!r}" + ) + self._spec = TASK_SPECS[self.task_id] + if cfg is not None: + if not getattr(cfg, "control_parts", None) and not getattr( + cfg, "active_joint_ids", None + ): + cfg.control_parts = ["hand"] + for field_name in ("events", "observations", "rewards", "actions"): + val = getattr(cfg, field_name, None) + if ( + val is not None + and isinstance(val, object) + and hasattr(val, "__dict__") + and not vars(val) + ): + setattr(cfg, field_name, None) + self.policy_viewer_camera_cfg = PolicyViewerCameraCfg( + eye_offset=self._spec.viewer_eye_offset, + target_height=self._spec.viewer_target_height, + ) + super().__init__(cfg, **kwargs) + + @property + def is_shadow(self) -> bool: + """Return whether this task uses the Shadow Hand pen-spinning scene.""" + return self.task_id in ("M1", "M2") + + def _declare_robot(self, **kwargs: Any) -> Robot: + del kwargs + self.sim.remove_ground_plane() + usd_path = Path(get_data_path(self._spec.usd_asset)) + if not usd_path.is_file(): + raise FileNotFoundError( + f"Missing dexterous manipulation USD asset: {usd_path}" + ) + self._usd_path = usd_path + + scene_desc = _parse_usd_source( + UsdSourceCfg(file_path=str(usd_path)), name="scene", namespace=None + ) + hand_art = next( + (art for art in scene_desc.articulations if art.fixed_base), None + ) + if hand_art is None: + raise ValueError(f"No fixed-base hand articulation found in {usd_path}") + + dof_joints = [j for j in hand_art.joints if j.joint_type.name != "FIXED"] + actuated_joints = [ + j for j in dof_joints if j.newton is not None and j.newton.target_mode == 1 + ] + self._actuated_joint_names = [j.name for j in actuated_joints] + policy_joint_names = ( + self._actuated_joint_names[2:] + if self.is_shadow + else self._actuated_joint_names + ) + self._usd_ctrlrange = np.asarray( + [j.newton.actuator_ctrlrange for j in actuated_joints], dtype=np.float32 + ) + self._usd_kp = np.asarray( + [float(j.newton.target_ke or 0.0) for j in actuated_joints], + dtype=np.float32, + ) + self._usd_kd = np.asarray( + [float(j.newton.target_kd or 0.0) for j in actuated_joints], + dtype=np.float32, + ) + self._usd_sites: dict[str, tuple[str, tuple[float, float, float]]] = {} + for site in getattr(hand_art, "sites", ()): + pos = tuple( + float(v) for v in np.asarray(site.local_pose, dtype=np.float32)[:3, 3] + ) + self._usd_sites[site.name] = (site.link_name, pos) + + default_uid = "shadow_hand" if self.is_shadow else "leap_hand" + cfg_robot = getattr(self.cfg, "robot", None) + robot_uid = ( + cfg_robot.uid + if cfg_robot is not None and getattr(cfg_robot, "uid", None) + else default_uid + ) + robot_cfg = RobotCfg( + uid=robot_uid, + build_pk_chain=False, + asset_physics_mode="preserve", + source_pose_mode="preserve", + enable_sensor=self.cfg.enable_sensor, + control_parts={"hand": policy_joint_names}, + joint_drive_props=JointDrivePropertiesCfg( + stiffness=None, + damping=None, + max_effort=None, + max_velocity=None, + friction=None, + drive_type=None, + ), + ) + self._robot_sensor_cfgs = [] + if ( + self.sim.is_newton_backend + and self.sim._active_newton_solver_type == "dexuni" + ): + self.sim._spawn_scene.builder.add_runtime_control( + _DexUniSceneJointOwnership() + ) + self.sim.add_usd( + UsdImportCfg( + name="scene", + source=UsdSourceCfg(file_path=str(usd_path)), + asset_cfgs={_source_path(hand_art): robot_cfg}, + ) + ) + return self.sim.get_robot(robot_uid) + + def _init_sim_state(self, **kwargs: Any) -> None: + super()._init_sim_state(**kwargs) + spec = self._spec + expected_decimation = round(spec.ctrl_dt / spec.sim_dt) + if not math.isclose(self.physics_dt, spec.sim_dt, abs_tol=1e-9): + raise ValueError( + f"{spec.env_id} requires physics_dt={spec.sim_dt}, got {self.physics_dt}." + ) + if self.cfg.sim_steps_per_control != expected_decimation: + raise ValueError( + f"{spec.env_id} requires sim_steps_per_control={expected_decimation}, " + f"got {self.cfg.sim_steps_per_control}." + ) + + self.action_dim = spec.action_dim + self.policy_dim = spec.policy_dim + self.critic_dim = spec.critic_dim + self.single_action_space = gym.spaces.Box( + -1.0, 1.0, shape=(self.action_dim,), dtype=np.float32 + ) + + self._actuated_joint_ids = [ + self.robot.joint_names.index(name) for name in self._actuated_joint_names + ] + self._actuated_joint_ids_tensor = torch.tensor( + self._actuated_joint_ids, dtype=torch.long, device=self.device + ) + self._kp = torch.as_tensor( + self._usd_kp, dtype=torch.float32, device=self.device + ) + self._kd = torch.as_tensor( + self._usd_kd, dtype=torch.float32, device=self.device + ) + + detached_uids = [self.robot.uid] + self._object: RigidObject | None = None + self._goal_obj: RigidObject | None = None + self._cube_art: Articulation | None = None + self._face_joint_id: int = 0 + + if self.is_shadow: + self._object = self.sim.get_rigid_object("object") + detached_uids.append("object") + self._palm_link_idx = self.robot.link_names.index("robot0:palm") + elif self.task_id == "M3": + self._cube_art = self.sim.get_articulation("cube") + self._face_joint_id = self._cube_art.joint_names.index("face_turn") + self._cube_root_com = self._cube_art.get_com_pose( + link_names=self._cube_art.root_link_name + )[:, 0, :3] + detached_uids.append("cube") + else: + self._object = self.sim.get_rigid_object("cube") + detached_uids.append("cube") + if "goal" in self.sim._rigid_objects: + self._goal_obj = self.sim.get_rigid_object("goal") + self._goal_pos = self._goal_obj.get_local_pose()[:1, :3].clone() + detached_uids.append("goal") + + self.add_detached_uids_for_reset(detached_uids) + + if self.task_id in ("M4", "M5"): + site_names = ("grasp_site", "th_tip", "if_tip", "mf_tip", "rf_tip") + self._site_link_indices = torch.tensor( + [ + self.robot.link_names.index(self._usd_sites[name][0]) + for name in site_names + ], + dtype=torch.long, + device=self.device, + ) + self._site_local_pos = torch.tensor( + [self._usd_sites[name][1] for name in site_names], + dtype=torch.float32, + device=self.device, + ) + + seed = 42 if self.cfg.seed is None else int(self.cfg.seed) + initial_path = Path(get_data_path(spec.initial_asset)) + if self.is_shadow: + self.task = ShadowTask.from_assets( + initial_path.parent, + self.num_envs, + variant=self.task_id, + device=self.device, + lower=self._usd_ctrlrange[:, 0], + upper=self._usd_ctrlrange[:, 1], + ) + self._reset_generator = torch.Generator(device=self.device).manual_seed( + seed + ) + else: + with np.load(initial_path) as initial: + qpos0 = initial["qpos"].copy() + ctrl0 = initial["ctrl"].copy() + leap_task_name = { + "M3": "LeapCubeFaceTurn", + "M4": "LeapCubeReorient", + "M5": "LeapCubeRotateZAxis", + }[self.task_id] + self.task = LeapTask( + leap_task_name, + self.num_envs, + qpos0, + ctrl0, + self._usd_ctrlrange[:, 0], + self._usd_ctrlrange[:, 1], + device=self.device, + seed=seed, + ) + self._reset_generator = self.task.generator + + self._ctrl = torch.zeros( + (self.num_envs, len(self._actuated_joint_ids)), + dtype=torch.float32, + device=self.device, + ) + self._actuator_force = torch.zeros( + (self.num_envs, len(self._actuated_joint_ids)), + dtype=torch.float32, + device=self.device, + ) + self._raw_action = torch.zeros( + (self.num_envs, self.action_dim), dtype=torch.float32, device=self.device + ) + self._policy_obs = torch.zeros( + (self.num_envs, self.policy_dim), dtype=torch.float32, device=self.device + ) + self._critic_obs = torch.zeros( + (self.num_envs, self.critic_dim), dtype=torch.float32, device=self.device + ) + self._step_reward = torch.zeros( + self.num_envs, dtype=torch.float32, device=self.device + ) + self._step_terminated = torch.zeros( + self.num_envs, dtype=torch.bool, device=self.device + ) + self._step_truncated = torch.zeros( + self.num_envs, dtype=torch.bool, device=self.device + ) + self._episode_success = torch.zeros( + self.num_envs, dtype=torch.bool, device=self.device + ) + self._step_metrics: dict[str, torch.Tensor] = {} + + def _set_seed(self, seed: int) -> int: + effective = int(seed) + self.cfg.seed = effective + if hasattr(self, "_reset_generator"): + self._reset_generator.manual_seed(effective) + for name, generator in getattr(self, "_component_generators", {}).items(): + generator.manual_seed(self._component_seed(name, effective)) + event_manager = getattr(self, "event_manager", None) + if event_manager is not None: + event_manager.set_seed(effective) + return effective + + def get_policy_viewer_target_pose(self) -> tuple[float, ...]: + """Return environment 0's manipulated object world pose ``(x, y, z, qx, qy, qz, qw)``.""" + if self._object is not None: + pose = self._object.get_local_pose()[0].detach().clone() + elif self._cube_art is not None: + pose = self._cube_art.get_local_pose()[0].detach().clone() + else: + pose = self.robot.get_local_pose()[0].detach().clone() + pose[:3] = pose[:3] + self.sim.arena_offsets[0].to( + pose.device, dtype=pose.dtype + ) + return tuple(float(v) for v in pose.cpu().tolist()) + + def _local_link_poses(self) -> torch.Tensor: + """Return robot link poses in local arena coordinates ``(N, L, 7)`` (xyzw).""" + link_poses = self.robot.body_data.body_link_pose + root_local = self.robot.get_local_pose()[:, :3] + offset = link_poses[:, 0, :3] - root_local + if bool((offset.abs() > 1e-6).any()): + link_poses = link_poses.clone() + link_poses[:, :, :3] = link_poses[:, :, :3] - offset[:, None, :] + return link_poses + + def _read_physical_state(self) -> Any: + """Read batched simulation state via public Robot/Articulation/RigidObject facades.""" + hand_qpos = self.robot.get_qpos() + hand_qvel = self.robot.get_qvel() + + if self.is_shadow: + assert self._object is not None + obj_pose_xyzw = self._object.get_local_pose() + obj_pos = obj_pose_xyzw[:, :3] + obj_quat_wxyz = _xyzw_to_wxyz(obj_pose_xyzw[:, 3:7]) + obj_linvel = self._object.body_data.lin_vel + obj_world_angvel = self._object.body_data.ang_vel + obj_local_angvel = rotate_wxyz( + _invert_wxyz(obj_quat_wxyz), obj_world_angvel + ) + qpos = torch.cat((hand_qpos, obj_pos, obj_quat_wxyz), dim=-1) + qvel = torch.cat((hand_qvel, obj_linvel, obj_local_angvel), dim=-1) + palm_pos = self._local_link_poses()[:, self._palm_link_idx, :3] + return ShadowState(qpos, qvel, palm_pos) + + if self.task_id == "M3": + assert self._cube_art is not None + cube_pose_xyzw = self._cube_art.get_local_pose() + cube_pos = cube_pose_xyzw[:, :3] + cube_quat_wxyz = _xyzw_to_wxyz(cube_pose_xyzw[:, 3:7]) + cube_linvel = self._cube_art.body_data.root_lin_vel + cube_world_angvel = self._cube_art.body_data.root_ang_vel + # MJCF freejoint translation is measured at the body origin. + cube_linvel = cube_linvel - torch.cross( + cube_world_angvel, + rotate_wxyz(cube_quat_wxyz, self._cube_root_com), + dim=-1, + ) + cube_local_angvel = rotate_wxyz( + _invert_wxyz(cube_quat_wxyz), cube_world_angvel + ) + face_qpos = self._cube_art.get_qpos()[ + :, self._face_joint_id : self._face_joint_id + 1 + ] + face_qvel = self._cube_art.get_qvel()[ + :, self._face_joint_id : self._face_joint_id + 1 + ] + return { + "qpos": torch.cat( + (hand_qpos, cube_pos, cube_quat_wxyz, face_qpos), dim=-1 + ), + "qvel": torch.cat( + (hand_qvel, cube_linvel, cube_local_angvel, face_qvel), dim=-1 + ), + } + + assert self._object is not None + cube_pose_xyzw = self._object.get_local_pose() + cube_pos = cube_pose_xyzw[:, :3] + cube_quat_wxyz = _xyzw_to_wxyz(cube_pose_xyzw[:, 3:7]) + cube_linvel = self._object.body_data.lin_vel + cube_world_angvel = self._object.body_data.ang_vel + cube_local_angvel = rotate_wxyz(_invert_wxyz(cube_quat_wxyz), cube_world_angvel) + + link_poses = self._local_link_poses()[:, self._site_link_indices, :] + site_link_pos = link_poses[:, :, :3] + site_link_quat_wxyz = _xyzw_to_wxyz(link_poses[:, :, 3:7]) + site_world_pos = site_link_pos + rotate_wxyz( + site_link_quat_wxyz, self._site_local_pos.unsqueeze(0) + ) + grasp_pos = site_world_pos[:, 0, :] + palm_quat_inv = _invert_wxyz(site_link_quat_wxyz[:, 0, :]) + tip_offsets = site_world_pos[:, 1:, :] - grasp_pos[:, None, :] + tips_in_grasp = rotate_wxyz(palm_quat_inv[:, None, :], tip_offsets).reshape( + self.num_envs, 12 + ) + + return { + "qpos": torch.cat((hand_qpos, cube_pos, cube_quat_wxyz), dim=-1), + "qvel": torch.cat((hand_qvel, cube_linvel, cube_local_angvel), dim=-1), + "hand_qpos": hand_qpos, + "hand_qvel": hand_qvel, + "actuator_force": self._actuator_force.clone(), + "cube_pos": cube_pos, + "cube_quat": cube_quat_wxyz, + "cube_linvel": cube_linvel, + "cube_angvel": cube_world_angvel, + "palm_pos": grasp_pos, + "fingertip_pos": tips_in_grasp, + } + + def _sync_goal_object(self, env_ids: torch.Tensor | None = None) -> None: + if self._goal_obj is None or self.task_id != "M4": + return + if env_ids is None: + goal_quat_xyzw = _wxyz_to_xyzw(self.task.goal_quat) + goal_pose = torch.cat( + (self._goal_pos.expand(self.num_envs, -1), goal_quat_xyzw), dim=-1 + ) + self._goal_obj.set_local_pose(goal_pose) + else: + goal_quat_xyzw = _wxyz_to_xyzw(self.task.goal_quat[env_ids]) + goal_pose = torch.cat( + (self._goal_pos.expand(len(env_ids), -1), goal_quat_xyzw), dim=-1 + ) + self._goal_obj.set_local_pose(goal_pose, env_ids=env_ids) + + def _initialize_episode( + self, env_ids: Sequence[int] | torch.Tensor | None = None, **kwargs: Any + ) -> None: + super()._initialize_episode(env_ids=env_ids, **kwargs) + ids = ( + torch.arange(self.num_envs, device=self.device, dtype=torch.long) + if env_ids is None + else torch.as_tensor(env_ids, device=self.device, dtype=torch.long) + ) + if not ids.numel(): + return + + if self.is_shadow: + qpos, _, ctrl = self.task.reset(ids, generator=self._reset_generator) + hand_qpos = qpos[:, :24] + obj_pos = qpos[:, 24:27] + obj_quat_xyzw = _wxyz_to_xyzw(qpos[:, 27:31]) + assert self._object is not None + zeros_3 = torch.zeros( + (len(ids), 3), dtype=torch.float32, device=self.device + ) + self._object.set_local_pose( + torch.cat((obj_pos, obj_quat_xyzw), dim=-1), env_ids=ids + ) + self._object.set_velocity(lin_vel=zeros_3, ang_vel=zeros_3, env_ids=ids) + else: + initial = self.task.reset(ids) + qpos = initial["qpos"] + ctrl = initial["ctrl"] + hand_qpos = qpos[:, :16] + cube_pos = qpos[:, 16:19] + cube_quat_xyzw = _wxyz_to_xyzw(qpos[:, 19:23]) + if self.task_id == "M3": + assert self._cube_art is not None + face_qpos = qpos[:, 23:24] + self._cube_art.set_local_pose( + torch.cat((cube_pos, cube_quat_xyzw), dim=-1), env_ids=ids + ) + self._cube_art.set_root_velocity( + torch.zeros((len(ids), 6), dtype=torch.float32, device=self.device), + env_ids=ids, + ) + self._cube_art.set_qpos( + face_qpos, + joint_ids=[self._face_joint_id], + env_ids=ids, + target=False, + ) + self._cube_art.set_qvel( + torch.zeros_like(face_qpos), + joint_ids=[self._face_joint_id], + env_ids=ids, + target=False, + ) + self._cube_art.set_qf( + torch.zeros_like(face_qpos), + joint_ids=[self._face_joint_id], + env_ids=ids, + ) + else: + assert self._object is not None + zeros_3 = torch.zeros( + (len(ids), 3), dtype=torch.float32, device=self.device + ) + self._object.set_local_pose( + torch.cat((cube_pos, cube_quat_xyzw), dim=-1), env_ids=ids + ) + self._object.set_velocity(lin_vel=zeros_3, ang_vel=zeros_3, env_ids=ids) + self._sync_goal_object(ids) + + zero_hand_qvel = torch.zeros_like(hand_qpos) + self.robot.set_qpos(hand_qpos, env_ids=ids, target=False) + for target in (False, True): + self.robot.set_qvel(zero_hand_qvel, env_ids=ids, target=target) + self.robot.set_qf(zero_hand_qvel, env_ids=ids) + # clear_articulation_dynamics syncs Newton state_0 (including all world + # articulations and standalone rigid objects) into the MuJoCo solver + # data and refreshes forward kinematics; restore actuator position + # targets right after clearing solver state. + self.sim.clear_articulation_dynamics(env_ids=ids) + self._ctrl[ids] = ctrl + self._actuator_force[ids] = 0.0 + self.robot.set_qpos( + ctrl, joint_ids=self._actuated_joint_ids, env_ids=ids, target=True + ) + + self._raw_action[ids] = 0.0 + self._episode_success[ids] = False + self._step_terminated[ids] = False + self._step_truncated[ids] = False + + state = self._read_physical_state() + if self.is_shadow: + obs_all = self.task.observe(state) + self._policy_obs[ids] = obs_all[ids] + self._critic_obs[ids] = obs_all[ids] + else: + raw = self.task.observe(state, env_ids=ids) + self._policy_obs[ids] = raw["state"] + self._critic_obs[ids] = raw["privileged_state"] + + def _preprocess_action(self, action: EnvAction) -> EnvAction: + return action + + def _step_action(self, action: EnvAction) -> EnvAction: + if not isinstance(action, torch.Tensor): + raise TypeError(f"Expected Tensor action, got {type(action).__name__}") + if action.ndim != 2 or action.shape != (self.num_envs, self.action_dim): + raise ValueError( + f"Expected action shape ({self.num_envs}, {self.action_dim}), got {tuple(action.shape)}" + ) + if not bool(torch.isfinite(action).all()): + raise FloatingPointError("Non-finite policy action") + clipped = action.clamp(-1.0, 1.0) + self._raw_action.copy_(clipped) + if self.is_shadow: + targets = self.task.action_targets(clipped) + else: + targets = self.task.control_targets(clipped) + self._ctrl.copy_(targets) + self.robot.set_qpos(targets, joint_ids=self._actuated_joint_ids, target=True) + return action + + def _advance_physics(self) -> None: + if self.is_shadow: + super()._advance_physics() + return + sensors = [ + sensor + for sensor in self.sensors.values() + if sensor.requires_substep_update + and getattr(getattr(sensor, "cfg", None), "sample_phase", None) is None + ] + for sensor in sensors: + sensor.begin_control_step() + + def sample_substep(dt: float) -> None: + for sensor in sensors: + sensor.update_physics_step(dt) + + after_substep = sample_substep if sensors else None + if self.task_id == "M3": + assert self._cube_art is not None + for _ in range(self.cfg.sim_steps_per_control): + face_angle = self._cube_art.get_qpos()[:, self._face_joint_id] + face_force = self.task.face_detent(face_angle) + self._cube_art.set_qf( + face_force[:, None], joint_ids=[self._face_joint_id] + ) + self.sim.update( + self.physics_dt, + 1, + after_substep=after_substep, + render_final_step=False, + ) + return + if self.task_id in ("M4", "M5"): + substeps = self.cfg.sim_steps_per_control + if substeps > 1: + self.sim.update( + self.physics_dt, + substeps - 1, + after_substep=after_substep, + render_final_step=False, + ) + qpos_pre = self.robot.get_qpos()[:, self._actuated_joint_ids_tensor] + qvel_pre = self.robot.get_qvel()[:, self._actuated_joint_ids_tensor] + self._actuator_force.copy_( + self._kp * (self._ctrl - qpos_pre) - self._kd * qvel_pre + ) + self.sim.update( + self.physics_dt, 1, after_substep=after_substep, render_final_step=False + ) + return + + def _update_sim_state(self, **kwargs: Any) -> None: + super()._update_sim_state(**kwargs) + state = self._read_physical_state() + qpos_check = state.qpos if isinstance(state, ShadowState) else state["qpos"] + if not bool(torch.isfinite(qpos_check).all()): + raise FloatingPointError("Non-finite physical state") + + next_elapsed = self._elapsed_steps + 1 + if self.is_shadow: + obs, reward, terminated, metrics = self.task.step(state, self._raw_action) + self._policy_obs.copy_(obs) + self._critic_obs.copy_(obs) + truncated = (next_elapsed >= self.max_episode_steps) & ~terminated + else: + raw_obs, reward, terminated, _, metrics = self.task.after_step( + state, self._raw_action + ) + self._policy_obs.copy_(raw_obs["state"]) + self._critic_obs.copy_(raw_obs["privileged_state"]) + truncated = (next_elapsed >= self.max_episode_steps) & ~terminated + if self.task_id == "M4": + self._sync_goal_object() + + self._step_reward.copy_(reward) + self._step_terminated.copy_(terminated.bool()) + self._step_truncated.copy_(truncated.bool()) + self._step_metrics = {key: value.clone() for key, value in metrics.items()} + if "success" in metrics: + self._episode_success |= metrics["success"].bool() + + def get_obs(self, **kwargs: Any) -> EnvObs: + obs = TensorDict( + {"policy": self._policy_obs.clone(), "critic": self._critic_obs.clone()}, + batch_size=[self.num_envs], + device=self.device, + ) + if self.sensors: + sensor_obs = self._get_sensor_obs(**kwargs) + if len(sensor_obs.keys()) > 0: + obs["sensor"] = sensor_obs + if self.cfg.observations and self.observation_manager is not None: + obs = self.observation_manager.compute(obs) + return obs + + def compute_task_state( + self, **kwargs: Any + ) -> tuple[torch.Tensor, torch.Tensor, dict[str, Any]]: + del kwargs + return ( + self._episode_success.clone(), + self._step_terminated.clone(), + {key: value.clone() for key, value in self._step_metrics.items()}, + ) + + def is_task_success(self, **kwargs: Any) -> torch.Tensor: + del kwargs + return self._episode_success.clone() + + def get_info(self, **kwargs: Any) -> dict[str, Any]: + info = super().get_info(**kwargs) + if not self._spec.has_episode_success: + info.pop("success", None) + return info + + def evaluate(self, **kwargs: Any) -> dict[str, Any]: + info = self.get_info(**kwargs) + eval_dict: dict[str, Any] = {} + if "success" in info: + eval_dict["success"] = info["success"][0].item() + for key, value in info.get("metrics", {}).items(): + eval_dict[key] = value + return eval_dict + + def get_reward( + self, obs: EnvObs, action: EnvAction, info: dict[str, Any] + ) -> torch.Tensor: + del obs, action, info + return self._step_reward.clone() + + def _get_termination_flags( + self, obs: EnvObs, info: dict[str, Any] + ) -> tuple[torch.Tensor, torch.Tensor]: + del obs, info + terminated = self._step_terminated.clone() + truncated = self._step_truncated.clone() + if self.cfg.ignore_terminations: + terminated[:] = False + truncated = self._elapsed_steps >= self.max_episode_steps + return terminated, truncated diff --git a/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_shadow_task.py b/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_shadow_task.py new file mode 100644 index 0000000000..f423a8aa21 --- /dev/null +++ b/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_shadow_task.py @@ -0,0 +1,374 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- +from __future__ import annotations + +import math +from collections.abc import Sequence +from pathlib import Path +from typing import NamedTuple + +import torch + +from embodichain.utils import configclass + +__all__ = ["ShadowState", "ShadowTask", "ShadowTaskCfg", "shadow_config"] + + +class ShadowState(NamedTuple): + """Batched simulation state in the source MJCF coordinate convention. + + Args: + qpos: Shape ``(N, 31)``; 24 hand angles, world object position, + then the object's world orientation as a wxyz quaternion. + qvel: Shape ``(N, 30)``; 24 hand velocities, world object linear + velocity, then object-frame angular velocity. + palm_pos: World position of the ``robot0:palm`` body, shape ``(N, 3)``. + """ + + qpos: torch.Tensor + qvel: torch.Tensor + palm_pos: torch.Tensor + + +@configclass +class ShadowTaskCfg: + """Task parameters retained from the two selected reference policies.""" + + variant: str = "M1" + ctrl_dt: float = 0.04 + sim_dt: float = 0.002 + episode_length: int = 500 + random_phase_starts: bool = False + action_smoothing_beta: float = 1.0 + goal_mode: str = "hold" + goal_step_rad: float = math.pi / 12 + quarter_hold_steps: int = 2 + static_goal_reward_scale: float = 2.0 + rotation_reward_scale: float = 3.0 + segment_progress_scale: float = 5.0 + segment_bonus: float = 5.0 + drop_penalty: float = 50.0 + spin_direction: float = -1.0 + maximum_tilt_deg: float = 45.0 + + +def shadow_config(variant: str) -> ShadowTaskCfg: + """Return the selected fixed-grasp or continuous-spin configuration.""" + if variant == "M1": + return ShadowTaskCfg() + if variant == "M2": + return ShadowTaskCfg( + variant="M2", + random_phase_starts=True, + action_smoothing_beta=0.7, + goal_mode="milestone", + static_goal_reward_scale=0.0, + ) + raise ValueError(f"Unknown Shadow task variant: {variant}") + + +class ShadowTask: + """Compute Shadow pen MDP terms without advancing a physics engine. + + Call ``reset`` and write its selected rows into the simulation. After a + forward-kinematics update, call ``observe``. For each control interval, + compute ``action_targets``, advance the simulation for 20 substeps, and call + ``step`` with the resulting state and the same raw actions. The bridge owns + the 500-step time limit and resets; ``step`` returns physical termination. + """ + + action_size = 18 + observation_size = 87 + qpos_size = 31 + qvel_size = 30 + axis_length = 0.2 + + def __init__( + self, + cfg: ShadowTaskCfg, + num_envs: int, + q0: torch.Tensor, + ctrl0: torch.Tensor, + phase_starts: torch.Tensor, + lower: torch.Tensor, + upper: torch.Tensor, + ) -> None: + if num_envs < 1 or q0.shape != (31,) or ctrl0.shape != (20,): + raise ValueError("Expected N >= 1, q0[31], and ctrl0[20]") + if phase_starts.ndim != 2 or phase_starts.shape[1] != 31: + raise ValueError("Expected phase_starts[num_phases, 31]") + if lower.shape != (20,) or upper.shape != (20,): + raise ValueError("Expected 20 actuator control bounds") + if cfg.goal_mode not in ("hold", "milestone"): + raise ValueError("goal_mode must be hold or milestone") + self.cfg = cfg + self.num_envs = num_envs + self.device = q0.device + self.dtype = q0.dtype + self.q0 = q0.clone() + self.ctrl0 = ctrl0.to(q0).clone() + self.phase_starts = phase_starts.to(q0).clone() + self.lower = lower.to(q0).clone() + self.upper = upper.to(q0).clone() + self.initial_pos = q0.new_zeros((num_envs, 3)) + self.goal = q0.new_full((num_envs,), cfg.goal_step_rad) + self.held = torch.zeros(num_envs, dtype=torch.int64, device=self.device) + self.turns = self.held.clone() + self.rotation = q0.new_zeros(num_envs) + self.last_heading = q0.new_zeros(num_envs) + self.last_action = q0.new_zeros((num_envs, self.action_size)) + self.motor_targets = self.ctrl0.expand(num_envs, -1).clone() + self.phase_index = torch.full( + (num_envs,), -1, dtype=torch.int64, device=self.device + ) + + @classmethod + def from_assets( + cls, + asset_root: str | Path, + num_envs: int, + variant: str = "M1", + device: torch.device | str = "cpu", + dtype: torch.dtype = torch.float32, + *, + lower: torch.Tensor | Sequence[float], + upper: torch.Tensor | Sequence[float], + ) -> ShadowTask: + """Load retained reset arrays with USD-authored actuator control bounds.""" + import numpy as np + + assets = Path(asset_root) + with np.load(assets / "ShadowPenSpin-initial.npz") as initial: + q0 = torch.as_tensor(initial["qpos"].copy(), device=device, dtype=dtype) + ctrl0 = torch.as_tensor(initial["ctrl"].copy(), device=device, dtype=dtype) + with np.load(assets / "ShadowPenSpinPhases-initial-states.npz") as phases: + starts = torch.as_tensor(phases["qpos"].copy(), device=device, dtype=dtype) + lower_tensor = torch.as_tensor(lower, device=device, dtype=dtype) + upper_tensor = torch.as_tensor(upper, device=device, dtype=dtype) + return cls( + shadow_config(variant), + num_envs, + q0, + ctrl0, + starts, + lower_tensor, + upper_tensor, + ) + + @staticmethod + def pen_axis(qpos: torch.Tensor) -> torch.Tensor: + """Return the world unit vector along the object's local positive Z axis.""" + quat = qpos[:, 27:31] + quat = quat / torch.linalg.vector_norm(quat, dim=-1, keepdim=True) + w, x, y, z = quat.unbind(-1) + return torch.stack( + (2 * (x * z + w * y), 2 * (y * z - w * x), 1 - 2 * (x * x + y * y)), + dim=-1, + ) + + @classmethod + def heading(cls, qpos: torch.Tensor) -> torch.Tensor: + """Return the world XY heading of the pen's longitudinal axis.""" + axis = cls.pen_axis(qpos) + return torch.atan2(axis[:, 1], axis[:, 0]) + + def reset( + self, + env_ids: torch.Tensor | None = None, + *, + generator: torch.Generator | None = None, + qpos: torch.Tensor | None = None, + ) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor]: + """Sample selected reset states and clear only their task memory. + + Args: + env_ids: Rows to reset; omission resets all environments. + generator: Generator on the same device as the task tensors. + qpos: Optional exact source-coordinate reset rows, used for fixed + state comparisons. When supplied, no reset noise is added. + + Returns: + Selected ``qpos[K,31]``, zero ``qvel[K,30]``, and ``ctrl[K,20]``. + """ + if env_ids is None: + env_ids = torch.arange(self.num_envs, device=self.device) + env_ids = torch.as_tensor(env_ids, device=self.device, dtype=torch.int64) + count = env_ids.numel() + phases = torch.full_like(env_ids, -1) + if qpos is None: + if self.cfg.random_phase_starts: + phases = torch.randint( + self.phase_starts.shape[0], + (count,), + device=self.device, + generator=generator, + ) + qpos = self.phase_starts[phases].clone() + sign = ( + 2 + * torch.randint( + 2, (count, 1), device=self.device, generator=generator + ) + - 1 + ) + qpos[:, 27:31] *= sign + else: + qpos = self.q0.expand(count, -1).clone() + qpos[:, 24:27] += ( + 0.006 + * torch.rand( + (count, 3), + dtype=self.dtype, + device=self.device, + generator=generator, + ) + - 0.003 + ) + qpos[:, :24] += 0.02 * torch.randn( + (count, 24), + dtype=self.dtype, + device=self.device, + generator=generator, + ) + else: + qpos = qpos.to(self.q0).clone() + if qpos.shape != (count, self.qpos_size): + raise ValueError("Exact reset qpos must have one row per env_id") + self.initial_pos[env_ids] = qpos[:, 24:27] + self.goal[env_ids] = self.cfg.goal_step_rad + self.held[env_ids] = 0 + self.turns[env_ids] = 0 + self.rotation[env_ids] = 0 + self.last_heading[env_ids] = self.heading(qpos) + self.last_action[env_ids] = 0 + self.motor_targets[env_ids] = self.ctrl0 + self.phase_index[env_ids] = phases + return ( + qpos, + qpos.new_zeros((count, self.qvel_size)), + self.ctrl0.expand(count, -1).clone(), + ) + + def action_targets(self, actions: torch.Tensor) -> torch.Tensor: + """Map raw policy actions to all 20 position-actuator targets.""" + beta = self.cfg.action_smoothing_beta + smoothed = beta * actions + (1 - beta) * self.last_action + motor = torch.cat((actions.new_zeros((self.num_envs, 2)), smoothed), dim=-1) + return (self.upper + self.lower) / 2 + motor.clamp(-1, 1) * ( + self.upper - self.lower + ) / 2 + + def observe(self, state: ShadowState) -> torch.Tensor: + """Return the reference policy's 87-dimensional observation.""" + delta = self.goal - self.rotation + return torch.cat( + ( + state.qpos[:, :24], + 0.05 * state.qvel[:, :24], + self.last_action, + 10 * (state.qpos[:, 24:27] - self.initial_pos), + state.qpos[:, 27:31], + 0.1 * state.qvel[:, 24:30], + torch.stack((torch.sin(delta), torch.cos(delta)), dim=-1), + 10 * (state.qpos[:, 24:27] - state.palm_pos), + torch.stack( + ( + delta / math.pi, + self.held / self.cfg.quarter_hold_steps, + self.rotation / (2 * math.pi) - self.turns, + ), + dim=-1, + ), + ), + dim=-1, + ) + + def step( + self, state: ShadowState, actions: torch.Tensor + ) -> tuple[torch.Tensor, torch.Tensor, torch.Tensor, dict[str, torch.Tensor]]: + """Update task memory after physics and return observation and MDP terms. + + Args: + state: State after applying ``action_targets(actions)`` for one + control interval. + actions: The raw actions passed to ``action_targets`` before physics. + + Returns: + Observation, reward, physical termination, and reference task metrics. + Time-limit truncation is handled by the simulation bridge. + """ + cfg = self.cfg + beta = cfg.action_smoothing_beta + smoothed = beta * actions + (1 - beta) * self.last_action + ctrl = self.action_targets(actions) + pos = state.qpos[:, 24:27] + drift = torch.linalg.vector_norm(pos - self.initial_pos, dim=-1) + axis = self.pen_axis(state.qpos) + gap = self.axis_length * axis[:, 2].abs() + done = ( + (pos[:, 2] < 0.04) + | (drift > 0.20) + | ~torch.isfinite(state.qpos).all(-1) + | ( + gap + > self.axis_length * math.sin(math.radians(cfg.maximum_tilt_deg)) + 1e-6 + ) + ) + heading = torch.atan2(axis[:, 1], axis[:, 0]) + difference = heading - self.last_heading + increment = torch.atan2(torch.sin(difference), torch.cos(difference)) + increment = increment * cfg.spin_direction + old_rotation = self.rotation + rotation = old_rotation + increment + spin = increment / cfg.ctrl_dt + cost = 0.02 * (smoothed - self.last_action).square().mean(-1) + horizontal = (1 - (gap / 0.2).square()).clamp(0, 1) + reward = ( + cfg.rotation_reward_scale * spin * horizontal - 15 * gap - 5 * drift - cost + ) * cfg.ctrl_dt - cfg.drop_penalty * done.to(self.dtype) + turns = torch.floor(rotation.clamp_min(0) / (2 * math.pi)) + success = (turns > self.turns) & (gap < 0.05) & ~done + reward = reward + 5 * success.to(self.dtype) + error = (self.goal - rotation).abs() + previous_error = (self.goal - old_rotation).abs() + good = (error < 0.1) & (spin.abs() < 1.5) & (gap < 0.05) & ~done + held = torch.where(good, self.held + 1, 0) + segment = held >= cfg.quarter_hold_steps + if cfg.goal_mode == "milestone": + segment = (rotation >= self.goal) & (gap < 0.05) & ~done + reward = ( + reward + + cfg.segment_progress_scale * (previous_error - error) + + cfg.static_goal_reward_scale * torch.exp(-2 * error) * cfg.ctrl_dt + + cfg.segment_bonus * segment.to(self.dtype) + ) + self.goal = self.goal + segment.to(self.dtype) * cfg.goal_step_rad + self.held = torch.where(segment, 0, held) + self.turns = self.turns + success.to(torch.int64) + self.rotation = rotation + self.last_heading = heading + self.last_action = smoothed + self.motor_targets = ctrl + metrics = { + "success": success.to(self.dtype), + "error": error, + "rotation": rotation, + "horizontal_gap": gap, + "retained": (~done).to(self.dtype), + "reward_spin": spin, + "segment_success": segment.to(self.dtype), + } + return self.observe(state), reward, done, metrics diff --git a/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/leap_cube_face_turn.py b/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/leap_cube_face_turn.py new file mode 100644 index 0000000000..df999f1686 --- /dev/null +++ b/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/leap_cube_face_turn.py @@ -0,0 +1,32 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +from embodichain.lab.gym.utils.registration import register_env + +from ._runtime import DexterousManipulationEnv + +__all__ = ["LeapCubeFaceTurnEnv"] + + +@register_env( + "LeapCubeFaceTurnRL-v1", max_episode_steps=500, override=True, supports_rl=True +) +class LeapCubeFaceTurnEnv(DexterousManipulationEnv): + """LEAP Hand top-layer articulated cube face-turning environment.""" + + task_id = "M3" diff --git a/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/leap_cube_reorient.py b/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/leap_cube_reorient.py new file mode 100644 index 0000000000..0e7d1e91df --- /dev/null +++ b/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/leap_cube_reorient.py @@ -0,0 +1,32 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +from embodichain.lab.gym.utils.registration import register_env + +from ._runtime import DexterousManipulationEnv + +__all__ = ["LeapCubeReorientEnv"] + + +@register_env( + "LeapCubeReorientRL-v1", max_episode_steps=1000, override=True, supports_rl=True +) +class LeapCubeReorientEnv(DexterousManipulationEnv): + """LEAP Hand in-hand 3D cube reorientation environment.""" + + task_id = "M4" diff --git a/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/leap_cube_rotate_z.py b/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/leap_cube_rotate_z.py new file mode 100644 index 0000000000..ccf8ae403c --- /dev/null +++ b/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/leap_cube_rotate_z.py @@ -0,0 +1,32 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +from embodichain.lab.gym.utils.registration import register_env + +from ._runtime import DexterousManipulationEnv + +__all__ = ["LeapCubeRotateZAxisEnv"] + + +@register_env( + "LeapCubeRotateZAxisRL-v1", max_episode_steps=500, override=True, supports_rl=True +) +class LeapCubeRotateZAxisEnv(DexterousManipulationEnv): + """LEAP Hand continuous Z-axis cube rotation environment.""" + + task_id = "M5" diff --git a/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/shadow_pen_continuous.py b/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/shadow_pen_continuous.py new file mode 100644 index 0000000000..e22ee31cb8 --- /dev/null +++ b/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/shadow_pen_continuous.py @@ -0,0 +1,32 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +from embodichain.lab.gym.utils.registration import register_env + +from ._runtime import DexterousManipulationEnv + +__all__ = ["ShadowPenContinuousEnv"] + + +@register_env( + "ShadowPenContinuousRL-v1", max_episode_steps=500, override=True, supports_rl=True +) +class ShadowPenContinuousEnv(DexterousManipulationEnv): + """Multi-phase continuous Shadow Hand pen spinning environment.""" + + task_id = "M2" diff --git a/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/shadow_pen_fixed.py b/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/shadow_pen_fixed.py new file mode 100644 index 0000000000..14ba25b85a --- /dev/null +++ b/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/shadow_pen_fixed.py @@ -0,0 +1,32 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +from embodichain.lab.gym.utils.registration import register_env + +from ._runtime import DexterousManipulationEnv + +__all__ = ["ShadowPenFixedEnv"] + + +@register_env( + "ShadowPenFixedRL-v1", max_episode_steps=500, override=True, supports_rl=True +) +class ShadowPenFixedEnv(DexterousManipulationEnv): + """Fixed-grasp Shadow Hand pen rotation environment.""" + + task_id = "M1" diff --git a/pyproject.toml b/pyproject.toml index 0ff92ba08c..f2d62f659f 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -108,6 +108,7 @@ VLP16 = "embodichain_sensors.vlp16:VLP16_REGISTRATION" [project.entry-points."embodichain.assets"] official_robots = "embodichain_robots._asset_catalog:ROBOT_ASSETS" +official_rl = "embodichain.data.assets.rl_assets:_ASSET_CATALOG" [tool.uv.sources] bpy = { index = "blender" } diff --git a/tests/data/test_download.py b/tests/data/test_download.py index ee51cdc2b9..e73f7163b0 100644 --- a/tests/data/test_download.py +++ b/tests/data/test_download.py @@ -414,3 +414,27 @@ def capture(self, prefix, descriptor, root): assert list(descriptor.urls) == expected_urls assert descriptor.md5 == md5 assert root == expected_root + + +@pytest.mark.no_sim +@pytest.mark.parametrize( + "name,category", + [ + ("UnitreeG1Rough", "robot"), + ("G1RoughTerrains", "scene"), + ("ShadowHandPen", "robot"), + ("LeapHandCubeFaceTurn", "robot"), + ("LeapHandCube", "robot"), + ], +) +def test_rl_assets_share_cli_and_dataset_resolution(name: str, category: str) -> None: + from embodichain.data.assets import rl_assets + + registry = download.get_registry() + expected = getattr(rl_assets, name) + assert dict(registry[category])[name] is expected + assert get_data_class(name) is expected + assert ( + sum(asset_name == name for rows in registry.values() for asset_name, _ in rows) + == 1 + ) diff --git a/tests/gym/envs/task_program/test_asset_materialization.py b/tests/gym/envs/task_program/test_asset_materialization.py index 15c7f14d7d..99a8a66434 100644 --- a/tests/gym/envs/task_program/test_asset_materialization.py +++ b/tests/gym/envs/task_program/test_asset_materialization.py @@ -583,6 +583,8 @@ def test_manifest_inventory_and_provenance_are_validated_without_optional_digest def test_packaged_asset_id_is_not_shadowed_during_bundling( deployment, tmp_path, monkeypatch ): + from embodichain.data import get_data_path + source, requirements, resolved = deployment shadow = source.parent / "Dataset" shadow.mkdir() @@ -591,7 +593,10 @@ def test_packaged_asset_id_is_not_shadowed_during_bundling( (data_root / "Dataset").mkdir(parents=True) shutil.copy2(resolved.assets["source"].visual_path, data_root / "Dataset/mesh.obj") monkeypatch.setattr( - "embodichain.data.constants.EMBODICHAIN_DEFAULT_DATA_ROOT", str(data_root) + "embodichain.data.get_data_path", + lambda path: ( + str(data_root / path) if path == "Dataset/mesh.obj" else get_data_path(path) + ), ) env_path = source.parent / "env.yaml" env = yaml.safe_load(env_path.read_text()) diff --git a/tests/gym/envs/tasks/test_dexterous_metrics.py b/tests/gym/envs/tasks/test_dexterous_metrics.py new file mode 100644 index 0000000000..760507027a --- /dev/null +++ b/tests/gym/envs/tasks/test_dexterous_metrics.py @@ -0,0 +1,416 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +from contextlib import nullcontext +import math +from types import SimpleNamespace +from typing import Any, Callable +from unittest.mock import Mock, call + +import gymnasium as gym +import numpy as np +import pytest +from tensordict import TensorDict +import torch + +from embodichain.lab.gym.envs.episode_context import EpisodeContext +from embodichain.learning.rl.evaluation import evaluate_episodes +from embodichain_tasks.manipulation.dexterous._runtime import ( + DexterousManipulationEnv, + TASK_SPECS, +) +from embodichain_tasks.manipulation.dexterous._shadow_task import ShadowState + + +class _Policy(torch.nn.Module): + obs_dim = 1 + action_dim = 1 + actor_obs_groups = ("policy",) + + def __init__(self, action_dim: int = 1) -> None: + super().__init__() + self.action_dim = action_dim + self.anchor = torch.nn.Parameter(torch.zeros(())) + + def get_action(self, data: Any, deterministic: bool = False) -> Any: + del deterministic + data["action"] = torch.zeros(data.batch_size[0], self.action_dim) + return data + + +@pytest.fixture +def env_factory() -> Callable[[str], DexterousManipulationEnv]: + """Build a lightweight CPU DexterousManipulationEnv exercising real step/reset hooks.""" + + def make(task_id: str) -> DexterousManipulationEnv: + spec = TASK_SPECS[task_id] + env = DexterousManipulationEnv.__new__(DexterousManipulationEnv) + env.task_id = task_id + env._spec = spec + env._device = torch.device("cpu") + env._num_envs = 2 + env.max_episode_steps = spec.max_episode_steps + env.action_dim = spec.action_dim + env.policy_dim = spec.policy_dim + env.critic_dim = spec.critic_dim + env.single_action_space = gym.spaces.Box( + -1.0, 1.0, (spec.action_dim,), dtype=np.float32 + ) + env.single_observation_space = gym.spaces.Dict( + { + "policy": gym.spaces.Box( + -np.inf, np.inf, (spec.policy_dim,), dtype=np.float32 + ), + "critic": gym.spaces.Box( + -np.inf, np.inf, (spec.critic_dim,), dtype=np.float32 + ), + } + ) + env.sim_cfg = SimpleNamespace(physics_dt=spec.sim_dt) + env.cfg = SimpleNamespace( + seed=42, + ignore_terminations=False, + episode_context_in_info=False, + sim_steps_per_control=round(spec.ctrl_dt / spec.sim_dt), + events=None, + observations=None, + rewards=None, + actions=None, + dataset=None, + ) + env.episode_context = EpisodeContext(2) + env.episode_success_status = torch.zeros(2, dtype=torch.bool) + env._profiler = SimpleNamespace(section=lambda *args, **kwargs: nullcontext()) + env._elapsed_steps = torch.zeros(2, dtype=torch.long) + env._detached_uids_for_reset = ["hand", "obj"] + env.sensors = {} + env.event_manager = None + env.observation_manager = None + env.action_manager = None + env.reward_manager = None + env.dataset_manager = None + env.rollout_buffer = None + env._traj_buffer = None + env._is_Keyboard = False + + env.sim = SimpleNamespace( + device=torch.device("cpu"), + is_rt_enabled=False, + reset_objects_state=Mock(), + clear_articulation_dynamics=Mock(), + update=Mock(), + render_frame=lambda **kwargs: nullcontext(), + arena_offsets=torch.zeros(2, 3), + ) + hand_dof = 24 if env.is_shadow else 16 + act_count = 20 if env.is_shadow else 16 + env._actuated_joint_ids = list(range(act_count)) + env._actuated_joint_ids_tensor = torch.arange(act_count, dtype=torch.long) + env._kp = torch.ones(act_count) + env._kd = torch.zeros(act_count) + env._ctrl = torch.zeros(2, act_count) + env._actuator_force = torch.zeros(2, act_count) + env._raw_action = torch.zeros(2, spec.action_dim) + env._policy_obs = torch.zeros(2, spec.policy_dim) + env._critic_obs = torch.zeros(2, spec.critic_dim) + env._step_reward = torch.zeros(2) + env._step_terminated = torch.zeros(2, dtype=torch.bool) + env._step_truncated = torch.zeros(2, dtype=torch.bool) + env._episode_success = torch.zeros(2, dtype=torch.bool) + env._step_metrics = {} + env._reset_generator = torch.Generator().manual_seed(0) + + env.robot = SimpleNamespace( + uid="hand", + get_qpos=lambda: torch.zeros(2, hand_dof), + get_qvel=lambda: torch.zeros(2, hand_dof), + get_qf=lambda: torch.zeros(2, hand_dof), + set_qpos=Mock(), + set_qvel=Mock(), + set_qf=Mock(), + ) + env._object = SimpleNamespace( + set_local_pose=Mock(), + set_velocity=Mock(), + get_local_pose=lambda: torch.tensor( + [[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]] * 2 + ), + ) + env._goal_obj = None + env._cube_art = ( + SimpleNamespace( + set_local_pose=Mock(), + set_root_velocity=Mock(), + set_qpos=Mock(), + set_qvel=Mock(), + set_qf=Mock(), + get_qpos=lambda: torch.zeros(2, 1), + ) + if task_id == "M3" + else None + ) + env._face_joint_id = 0 + + if env.is_shadow: + env._read_physical_state = lambda: ShadowState( + torch.zeros(2, 31), torch.zeros(2, 30), torch.zeros(2, 3) + ) + else: + env._read_physical_state = lambda: {"qpos": torch.zeros(2, 24)} + + def task_reset(ids, *, generator=None): + n = len(ids) + if env.is_shadow: + assert generator is env._reset_generator + qpos = torch.zeros(n, 31) + qpos[:, 27] = 1.0 + return qpos, torch.zeros(n, 30), torch.zeros(n, act_count) + q_len = 24 if task_id == "M3" else 23 + qpos = torch.zeros(n, q_len) + qpos[:, 19] = 1.0 + return { + "qpos": qpos, + "qvel": torch.zeros(n, q_len - 1), + "ctrl": torch.zeros(n, act_count), + } + + def observe(state, env_ids=None): + del state + if env.is_shadow: + return torch.zeros(2, spec.policy_dim) + n = 2 if env_ids is None else len(env_ids) + return { + "state": torch.zeros(n, spec.policy_dim), + "privileged_state": torch.zeros(n, spec.critic_dim), + } + + def step_fn(state, actions): + del state, actions + # Environment 0 succeeds on step 0 and terminates after 3 steps; + # environment 1 never succeeds and terminates after 4 steps. + success = (env._elapsed_steps == 0) & torch.tensor([True, False]) + done = env._elapsed_steps >= torch.tensor([2, 3]) + metrics = {"rotation": env._elapsed_steps.float()} + if task_id != "M5": + metrics["success"] = success.float() + if env.is_shadow: + return ( + torch.zeros(2, spec.policy_dim), + torch.ones(2), + done, + metrics, + ) + return ( + { + "state": torch.zeros(2, spec.policy_dim), + "privileged_state": torch.zeros(2, spec.critic_dim), + }, + torch.ones(2), + done, + torch.zeros(2, dtype=torch.bool), + metrics, + ) + + env.task = SimpleNamespace( + reset=task_reset, + observe=observe, + action_targets=lambda x: torch.zeros(x.shape[0], act_count), + control_targets=lambda x: torch.zeros(x.shape[0], act_count), + face_detent=lambda a: torch.zeros_like(a), + step=step_fn, + after_step=step_fn, + ) + env.reset(seed=42) + return env + + return make + + +@pytest.mark.no_sim +@pytest.mark.parametrize("task_id", ["M1", "M2", "M3", "M4"]) +def test_success_survives_until_autoreset_and_reaches_evaluation( + env_factory: Callable[[str], DexterousManipulationEnv], task_id: str +) -> None: + """An early success pulse persists across the episode until reset.""" + env = env_factory(task_id) + result = evaluate_episodes( + policy=_Policy(env.action_dim), + env=env, + num_episodes=2, + device="cpu", + ) + assert result["eval/success_rate"] == 0.5 + assert result["eval/metrics/success"] == 0.0 + assert result["eval/avg_length"] == 3.5 + + +@pytest.mark.no_sim +@pytest.mark.parametrize("task_id", ["M1", "M3"]) +def test_partial_reset_clears_only_selected_success_and_keeps_terminal_info( + env_factory: Callable[[str], DexterousManipulationEnv], task_id: str +) -> None: + """Selective reset clears only target rows and rewinds the reset generator on explicit seed.""" + env = env_factory(task_id) + env._episode_success[:] = True + trainer_rng = torch.random.get_rng_state() + env.reset(seed=19, options={"reset_ids": torch.tensor([0])}) + assert env._reset_generator.initial_seed() == 19 + assert torch.equal(torch.random.get_rng_state(), trainer_rng) + assert env._episode_success.tolist() == [False, True] + env._elapsed_steps[:] = torch.tensor([2, 1]) + _, _, terminated, _, info = env.step(torch.zeros(2, env.action_dim)) + assert terminated.tolist() == [True, False] + assert info["success"].tolist() == [False, True] + env.reset() + assert not env._episode_success.any() + assert info["success"].tolist() == [False, True] + + +@pytest.mark.no_sim +def test_continuous_rotation_has_metrics_without_invented_binary_success( + env_factory: Callable[[str], DexterousManipulationEnv], +) -> None: + """M5 omits binary episode success while reporting continuous rotation metrics.""" + env = env_factory("M5") + result = evaluate_episodes( + policy=_Policy(env.action_dim), + env=env, + num_episodes=2, + device="cpu", + ) + assert math.isnan(result["eval/success_rate"]) + assert math.isfinite(result["eval/metrics/rotation"]) + + +@pytest.mark.no_sim +@pytest.mark.parametrize("task_id", list(TASK_SPECS)) +@pytest.mark.parametrize("ignore_terminations", [False, True]) +def test_time_limit_respects_ignored_terminations( + env_factory: Callable[[str], DexterousManipulationEnv], + task_id: str, + ignore_terminations: bool, +) -> None: + """Only effective failures suppress time-limit resets and bootstrapping.""" + from embodichain.learning.rl.algo.common import compute_gae + + env = env_factory(task_id) + env.cfg.ignore_terminations = ignore_terminations + env._elapsed_steps.fill_(env.max_episode_steps - 1) + env.task.step = lambda state, actions: ( + torch.zeros(2, env.policy_dim), + torch.ones(2), + torch.tensor([True, False]), + {}, + ) + env.task.after_step = lambda state, actions: ( + { + "state": torch.zeros(2, env.policy_dim), + "privileged_state": torch.zeros(2, env.critic_dim), + }, + torch.ones(2), + torch.tensor([True, False]), + torch.zeros(2, dtype=torch.bool), + {}, + ) + _, reward, terminated, truncated, _ = env.step(torch.zeros(2, env.action_dim)) + assert terminated.tolist() == [not ignore_terminations, False] + assert truncated.tolist() == [ignore_terminations, True] + assert env._elapsed_steps.tolist() == [0, 0] + rollout = TensorDict( + { + "value": torch.full((2, 2), 10.0), + "reward": torch.stack((reward, torch.zeros(2)), dim=1), + "done": torch.stack( + (terminated | truncated, torch.zeros(2, dtype=torch.bool)), dim=1 + ), + "terminated": torch.stack( + (terminated, torch.zeros(2, dtype=torch.bool)), dim=1 + ), + "truncated": torch.stack( + (truncated, torch.zeros(2, dtype=torch.bool)), dim=1 + ), + }, + batch_size=[2, 2], + ) + _, returns = compute_gae(rollout, gamma=0.9, gae_lambda=0.95) + expected = torch.tensor([10.0 if ignore_terminations else 1.0, 10.0]) + torch.testing.assert_close(returns[:, 0], expected) + + +@pytest.mark.no_sim +@pytest.mark.parametrize("task_id", list(TASK_SPECS)) +def test_physics_samples_opted_in_sensors( + env_factory: Callable[[str], DexterousManipulationEnv], task_id: str +) -> None: + """Custom stepping samples each physics interval, including batched updates.""" + env = env_factory(task_id) + sensor = Mock(requires_substep_update=True, cfg=SimpleNamespace(sample_phase=None)) + passive = Mock(requires_substep_update=False) + scheduled = Mock( + requires_substep_update=True, cfg=SimpleNamespace(sample_phase="physics") + ) + env.sensors = {"contacts": sensor, "camera": passive, "scheduled": scheduled} + + def advance(dt, count, *, after_substep=None, **kwargs): + for _ in range(count): + if after_substep is not None: + after_substep(dt) + + env.sim.update = advance + env._advance_physics() + sensor.begin_control_step.assert_called_once_with() + assert ( + sensor.update_physics_step.call_args_list + == [call(env.physics_dt)] * env.cfg.sim_steps_per_control + ) + passive.begin_control_step.assert_not_called() + passive.update_physics_step.assert_not_called() + scheduled.begin_control_step.assert_not_called() + scheduled.update_physics_step.assert_not_called() + + +@pytest.mark.no_sim +def test_face_turn_freejoint_velocity_uses_body_origin( + env_factory: Callable[[str], DexterousManipulationEnv], +) -> None: + """Convert COM velocity using each root's orientation without mutating state.""" + env = env_factory("M3") + env._cube_root_com = torch.tensor([[0.0, 0.0, -0.01]] * 2) + half_angle = math.sqrt(0.5) + # Row 1 turns the local -Z COM offset into world -X by rotating about Y. + env._cube_art.get_local_pose = lambda: torch.tensor( + [[0, 0, 0, 0, 0, 0, 1], [0, 0, 0, 0, half_angle, 0, half_angle]] + ) + com_velocity = torch.tensor([[0.0, 0.01, 0.0], [0.0, 0.02, 0.0]]) + env._cube_art.body_data = SimpleNamespace( + root_lin_vel=com_velocity.clone(), + root_ang_vel=torch.tensor([[1.0, 0.0, 0.0], [0.0, 0.0, 1.0]]), + ) + env._cube_art.get_qvel = lambda: torch.full((2, 1), 0.5) + + state = DexterousManipulationEnv._read_physical_state(env) + + # v_origin = v_com - omega x r; the second cross product is -0.01 Y. + torch.testing.assert_close( + state["qvel"][:, 16:19], torch.tensor([[0.0, 0.0, 0.0], [0.0, 0.03, 0.0]]) + ) + torch.testing.assert_close( + state["qvel"][:, 19:22], torch.tensor([[1.0, 0.0, 0.0], [-1.0, 0.0, 0.0]]) + ) + torch.testing.assert_close(state["qvel"][:, 22], torch.full((2,), 0.5)) + torch.testing.assert_close(env._cube_art.body_data.root_lin_vel, com_velocity) diff --git a/tests/gym/envs/tasks/test_dexterous_usd.py b/tests/gym/envs/tasks/test_dexterous_usd.py new file mode 100644 index 0000000000..329cabd0cc --- /dev/null +++ b/tests/gym/envs/tasks/test_dexterous_usd.py @@ -0,0 +1,139 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +from __future__ import annotations + +import argparse +import inspect +from pathlib import Path +from types import SimpleNamespace + +import pytest + +from embodichain.cli._task_catalog import _catalog_from_args +from embodichain.lab.gym.utils.gym_utils import config_to_cfg, get_manager_modules +from embodichain.lab.sim import cfg as sim_cfg +from embodichain.utils.utility import load_config +from embodichain_tasks.manipulation.dexterous import _runtime, _shadow_task + + +def test_dexuni_scene_joint_ownership_assigns_all_joints( + monkeypatch: pytest.MonkeyPatch, +) -> None: + from dexsim.engine.newton_physics import backend_registry + from embodichain_tasks.manipulation.dexterous._runtime import ( + _DexUniSceneJointOwnership, + ) + + solver_cfg = SimpleNamespace( + mujoco_articulations=None, + mujoco_articulation_paths=None, + mujoco_joints=None, + ) + backend = SimpleNamespace( + solver_type="dexuni", cfg=SimpleNamespace(solver_cfg=solver_cfg) + ) + monkeypatch.setattr(backend_registry, "get_newton_backend", lambda world: backend) + + control = _DexUniSceneJointOwnership() + control.initialize_model( + SimpleNamespace(joint_count=12), SimpleNamespace(world=object()) + ) + assert solver_cfg.mujoco_joints == tuple(range(12)) + + explicit_cfg = SimpleNamespace( + mujoco_articulations=None, + mujoco_articulation_paths=("/World/hand",), + mujoco_joints=None, + ) + backend.cfg = SimpleNamespace(solver_cfg=explicit_cfg) + control.initialize_model( + SimpleNamespace(joint_count=12), SimpleNamespace(world=object()) + ) + assert explicit_cfg.mujoco_joints is None + + +def test_dexterous_and_rough_catalog_configs_use_single_dexuni_deployment( + monkeypatch: pytest.MonkeyPatch, +) -> None: + for module in (_runtime, _shadow_task): + source = inspect.getsource(module) + assert "from_xml_path" not in source + assert "ElementTree" not in source + assert ".xml" not in source + + worktree_config_root = Path.cwd() / "extensions/embodichain_tasks/configs/tasks" + if worktree_config_root.is_dir(): + config_root = worktree_config_root + else: + config_root = Path(_runtime.__file__).resolve().parents[3] / "configs/tasks" + if not config_root.is_dir(): + config_root = Path(_runtime.__file__).resolve().parents[2] / "configs/tasks" + + tasks = _catalog_from_args( + argparse.Namespace(config_root=[f"embodichain_tasks={config_root}"]) + ) + catalog = {task.qualified_key: task for task in tasks} + expected = { + "embodichain_tasks:shadow_pen_fixed": "manipulation/dexterous/shadow_pen_fixed", + "embodichain_tasks:shadow_pen_continuous": "manipulation/dexterous/shadow_pen_continuous", + "embodichain_tasks:leap_cube_face_turn": "manipulation/dexterous/leap_cube_face_turn", + "embodichain_tasks:leap_cube_reorient": "manipulation/dexterous/leap_cube_reorient", + "embodichain_tasks:leap_cube_rotate_z": "manipulation/dexterous/leap_cube_rotate_z", + "embodichain_tasks:g1_rough": "locomotion/velocity/g1_rough", + } + + monkeypatch.setattr(sim_cfg, "get_data_path", lambda value: value) + manager_modules = [ + *get_manager_modules(), + "embodichain_tasks.locomotion.managers.observations", + "embodichain_tasks.locomotion.managers.rewards", + ] + for selector, rel_dir in expected.items(): + assert selector in catalog + entry = catalog[selector] + assert entry.default_deployment == "default" + deps = {d.name: d for d in entry.deployments} + assert set(deps) == {"default"} + dep = deps["default"] + assert dep.physics == "newton" + assert len(dep.agent_refs) == 1 + + task_dir = config_root / rel_dir + env_path = task_dir / "env.yaml" + ppo_path = task_dir / "agents/ppo.yaml" + assert env_path.is_file() + assert ppo_path.is_file() + assert not (task_dir / "env.newton.yaml").exists() + assert not (task_dir / "env.mjwarp.yaml").exists() + assert not (task_dir / "agents/ppo.newton.yaml").exists() + assert not (task_dir / "agents/ppo.mjwarp.yaml").exists() + + env_raw = load_config(env_path) + agent_raw = load_config(ppo_path) + assert Path(agent_raw["trainer"]["gym_config"]).name == "env.yaml" + algo = agent_raw["algorithm"]["cfg"] + assert algo["batch_size"] == ( + agent_raw["trainer"]["num_envs"] + * agent_raw["trainer"]["buffer_size"] + // algo["num_mini_batches"] + ) + cfg = config_to_cfg( + env_raw, source_path=env_path, manager_modules=manager_modules + ) + assert isinstance(cfg.sim_cfg.physics_cfg, sim_cfg.NewtonPhysicsCfg) + solver = cfg.sim_cfg.physics_cfg.to_dexsim_cfg(gpu_id=0).solver_cfg + assert solver.solver_type == "dexuni" diff --git a/tests/gym/envs/tasks/test_rough_velocity.py b/tests/gym/envs/tasks/test_rough_velocity.py new file mode 100644 index 0000000000..27aacb332d --- /dev/null +++ b/tests/gym/envs/tasks/test_rough_velocity.py @@ -0,0 +1,1095 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Verify packaged rough-terrain velocity definitions, curriculum and runtime.""" + +from __future__ import annotations + +import json +from contextlib import nullcontext +from copy import copy +from pathlib import Path +from types import SimpleNamespace +from unittest.mock import Mock, call + +import numpy as np +import pytest +import torch +from dexsim.spawn import ( + CollisionDesc, + GeometryDesc, + NewtonCollisionDesc, + ObjectDesc, + RenderDesc, +) + +from embodichain.lab.gym.envs.episode_context import EpisodeContext +from embodichain.lab.gym.envs.managers.action_manager import ActionTermCfg +from embodichain.lab.gym.envs.managers.actions import DefaultJointPositionAction +from embodichain.lab.sim.sim_manager import SimulationManager, SimulationManagerCfg +from embodichain_tasks.locomotion.velocity.rough.curriculum import ( + TerrainCurriculum, +) +from embodichain_tasks.locomotion.velocity.rough.config import load_config +from embodichain_tasks.locomotion.velocity.rough.environment import ( + load_rough_env_cfg, +) +from embodichain_tasks.locomotion.velocity.rough.runtime import RoughVelocityEnv +from embodichain_tasks.locomotion.velocity.rough.terrain import HeightScanner +from embodichain_tasks.locomotion.velocity.rough.mdp import ( + RoughVelocityConstants, + RoughVelocityState, + action_target, + build_observation, + compute_rewards, + compute_termination, + dc_motor_effort, + update_contact_times, +) + +__all__ = [] + + +@pytest.mark.no_sim +@pytest.mark.parametrize("explicit_pd", [False, True]) +def test_runtime_uses_configured_action_targets(explicit_pd: bool) -> None: + """Configured scale, offset and encoder bias reach both drive modes.""" + config = load_config("g1_rough") + env = object.__new__(RoughVelocityEnv) + env._num_envs = 2 + env.sim = SimpleNamespace(device=torch.device("cpu")) + env.robot = SimpleNamespace( + joint_names=list(config.joint_names), + cfg=SimpleNamespace(init_qpos=config.default_joint_position), + body_data=SimpleNamespace( + articulation_view=SimpleNamespace(apply_state=Mock()) + ), + ) + term = DefaultJointPositionAction( + ActionTermCfg( + params={ + "joint_names": config.joint_names, + "preserve_order": True, + "offset": -0.3, + "scale": 0.1, + "clip": None, + } + ), + env, + ) + env.action_manager = SimpleNamespace( + get_term=lambda name: term, action_trace=lambda: None + ) + env._bind_locomotion_action_state() + env.encoder_bias[:] = torch.tensor([[0.02], [0.04]]) + action = torch.tensor([[1.0], [2.0]]).expand(2, config.action_dim) + term.process_actions(action) + env.velocity_task_config = config + env._reference_constants = RoughVelocityConstants.from_config(config, action) + env.explicit_pd_effort_control = explicit_pd + env.policy_joint_ids = torch.arange(config.action_dim) + env._position_target = torch.empty_like(action) + + env._step_action(action) + + # offset + scale * action - bias: -0.3 + 0.1 * [1, 2] - [0.02, 0.04]. + expected = torch.tensor([[-0.22], [-0.14]]).expand_as(action) + torch.testing.assert_close(env._position_target, expected) + if not explicit_pd: + applied = env.robot.body_data.articulation_view.apply_state.call_args + torch.testing.assert_close(applied.kwargs["target_qpos"], expected) + + +@pytest.mark.no_sim +def test_reference_observation_preserves_reset_contact_rows() -> None: + """Observation reads preserve cleared contact rows and update other sensors.""" + calls = [] + contacts = SimpleNamespace( + update=lambda **kwargs: pytest.fail("reset contacts must not be queried"), + get_data=lambda: {"is_valid": torch.zeros((2, 1), dtype=torch.bool)}, + ) + camera = SimpleNamespace( + requires_substep_update=False, + update=lambda **kwargs: calls.append(kwargs), + get_data=lambda: torch.ones((2, 1)), + ) + scheduled = Mock( + requires_substep_update=False, cfg=SimpleNamespace(sample_phase="physics") + ) + scheduled.get_data.return_value = torch.zeros((2, 1)) + env = SimpleNamespace( + sim=SimpleNamespace(render_camera_group=lambda ids: calls.append(ids)), + _camera_group_ids=[3], + num_envs=2, + device=torch.device("cpu"), + sensors={ + "locomotion_contacts": contacts, + "camera": camera, + "scheduled": scheduled, + }, + _reference_sensor=contacts, + ) + obs = RoughVelocityEnv._get_sensor_obs(env) + assert not obs["locomotion_contacts"]["is_valid"].any() + assert obs["camera"].eq(1).all() + assert obs["scheduled"].eq(0).all() + scheduled.update.assert_not_called() + assert calls == [[3], {"fetch_only": True}] + + +@pytest.mark.no_sim +def test_reference_uses_control_frame_contacts_without_parent_histories() -> None: + """Reference normal-force histories replace the generic substep histories.""" + from embodichain_tasks.locomotion.velocity._embodichain import ( + EmbodiChainVelocityEnv, + ) + + registered = [] + sensor = SimpleNamespace( + get_actor_ids=lambda *args: torch.tensor([[1]]), + create_history=lambda name, *args, **kwargs: registered.append(name), + ) + env = object.__new__(EmbodiChainVelocityEnv) + env.get_sensor = lambda name: sensor + env.robot = SimpleNamespace(uid="robot") + env._state_fields = frozenset() + env._initialize_contact_histories() + assert registered == ["feet", "illegal"] + registered.clear() + reference = object.__new__(RoughVelocityEnv) + reference.get_sensor = lambda name: sensor + reference._initialize_contact_histories() + assert registered == [] + + +@pytest.mark.no_sim +@pytest.mark.parametrize("native_go2", [False, True]) +def test_joint_acceleration_matches_reference_update_interval( + monkeypatch, native_go2 +) -> None: + """Match G1's final 5 ms derivative and Go2's fused 20 ms derivative.""" + from embodichain_tasks.locomotion.velocity._embodichain import ( + EmbodiChainVelocityEnv, + ) + + env = object.__new__(RoughVelocityEnv) + env.sim_cfg = SimpleNamespace(physics_dt=0.005) + env.cfg = SimpleNamespace(sim_steps_per_control=4) + env.policy_joint_ids = torch.arange(2) + env.explicit_pd_effort_control = native_go2 + velocities = torch.tensor( + [[[0.0, 0.0]], [[4.0, -2.0]], [[-4.0, 3.0]], [[2.0, -1.0]], [[3.0, 1.0]]] + ) + step = 0 + sampled_steps = [] + + def fetch_state(): + sampled_steps.append(step) + return {"qpos": torch.zeros(1, 2), "qvel": velocities[step]} + + def advance(dt, count, **kwargs): + nonlocal step + assert dt == 0.005 and count == 1 + step += 1 + if kwargs.get("after_substep") is not None: + kwargs["after_substep"](dt) + + def update_control_state(self, **kwargs): + current = fetch_state()["qvel"] + self._joint_acceleration.copy_( + (current - self._previous_joint_velocity) / self.step_dt + ) + self._previous_joint_velocity.copy_(current) + + env.robot = SimpleNamespace( + body_data=SimpleNamespace(fetch_state=fetch_state), set_qf=lambda *a, **kw: None + ) + env.sim = SimpleNamespace(update=advance) + sensor = Mock(requires_substep_update=True, cfg=SimpleNamespace(sample_phase=None)) + env.sensors = {"contacts": sensor} + env._position_target = torch.zeros(1, 2) + env._kp = torch.ones(2) + env._kd = torch.ones(2) + env._effort_limit = torch.full((2,), 100.0) + env._velocity_limit = torch.full((2,), 30.0) + env._applied_effort = torch.zeros(1, 2) + env._previous_joint_velocity = torch.zeros(1, 2) + env._previous_physics_joint_velocity = torch.zeros(1, 2) + env._joint_acceleration = torch.zeros(1, 2) + env._sample_reference_contacts = lambda: None + monkeypatch.setattr( + EmbodiChainVelocityEnv, "_update_sim_state", update_control_state + ) + env._advance_physics() + sensor.begin_control_step.assert_called_once_with() + assert sensor.update_physics_step.call_args_list == [call(0.005)] * 4 + assert sampled_steps == ([0, 1, 2, 3] if native_go2 else [3]) + torch.testing.assert_close(env._applied_effort, -velocities[3]) + env._update_sim_state() + expected = [[150.0, 50.0]] if native_go2 else [[200.0, 400.0]] + torch.testing.assert_close(env._joint_acceleration, torch.tensor(expected)) + + +@pytest.mark.no_sim +def test_graph_motor_samples_substep_sensors() -> None: + """Graph-driven motors preserve the sensor control-step lifecycle.""" + env = object.__new__(RoughVelocityEnv) + env.sim_cfg = SimpleNamespace(physics_dt=0.005) + env.cfg = SimpleNamespace(sim_steps_per_control=4) + env._graph_motor = object() + sensor = Mock(requires_substep_update=True, cfg=SimpleNamespace(sample_phase=None)) + scheduled = Mock( + requires_substep_update=True, cfg=SimpleNamespace(sample_phase="physics") + ) + env.sensors = {"contacts": sensor, "scheduled": scheduled} + env._sample_reference_contacts = Mock() + + def advance(dt, count, *, after_substep=None, **kwargs): + for _ in range(count): + if after_substep is not None: + after_substep(dt) + + env.sim = SimpleNamespace(update=advance) + env._advance_physics() + sensor.begin_control_step.assert_called_once_with() + assert sensor.update_physics_step.call_args_list == [call(0.005)] * 4 + scheduled.begin_control_step.assert_not_called() + scheduled.update_physics_step.assert_not_called() + env._sample_reference_contacts.assert_called_once_with() + + +@pytest.mark.no_sim +def test_control_state_read_is_reused_by_reference_managers() -> None: + """Acceleration and task managers consume one live read per physics frame.""" + env = object.__new__(RoughVelocityEnv) + env._num_envs = 1 + env.sim = SimpleNamespace(device=torch.device("cpu")) + env.sim_cfg = SimpleNamespace(physics_dt=0.005) + env.cfg = SimpleNamespace(sim_steps_per_control=4, events=None) + env._state_fields = frozenset({"joint_acc"}) + env.explicit_pd_effort_control = True + env.policy_joint_ids = torch.arange(2) + env.foot_link_ids = torch.arange(1) + env.foot_link_names = ("foot",) + env._reference_events_deferred = True + env._height_scanner = None + env._episode_step = torch.zeros(1, dtype=torch.long) + env._global_control_step = 0 + env._gait_process = torch.zeros(1) + env._gait_frequency = torch.zeros(1) + env.command = torch.zeros(1, 3) + env.reward_command = torch.zeros(1, 3) + for name in ( + "_joint_acceleration", + "_previous_joint_velocity", + "_applied_effort", + "locomotion_action", + "last_locomotion_action", + ): + setattr(env, name, torch.zeros(1, 2)) + env._normal_history = torch.zeros(1, 3, 1, 3) + for name in ("_air_time", "_contact_time", "_last_air_time"): + setattr(env, name, torch.zeros(1, 1)) + velocity = torch.ones(1, 2) + reads = [] + + def fetch_state(): + reads.append(1) + return dict( + qpos=torch.zeros(1, 2), + qvel=velocity.clone(), + root_pose=torch.tensor([[0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]]), + root_lin_vel=torch.zeros(1, 3), + root_ang_vel=torch.zeros(1, 3), + ) + + env.robot = SimpleNamespace( + body_data=SimpleNamespace( + fetch_state=fetch_state, body_link_vel=torch.zeros(1, 1, 6) + ) + ) + for frame in (1, 2): + velocity.fill_(frame) + env._update_sim_state() + state = env.get_velocity_locomotion_state() + assert len(reads) == frame + torch.testing.assert_close(state.joint_vel, velocity) + torch.testing.assert_close(state.joint_acc, torch.full((1, 2), 50.0)) + + +@pytest.mark.no_sim +@pytest.mark.parametrize("task, dimension", [("g1_rough", 310)]) +def test_rough_velocity_mdp_contracts_and_cached_constants( + task: str, dimension: int +) -> None: + """Verify observation clipping/scaling, termination, and cached reward parity.""" + config = load_config(task) + n = 4 + dof = config.action_dim + default_qpos = torch.tensor(config.default_joint_position, dtype=torch.float32) + limits = torch.tensor( + config.data["robot"]["soft_joint_pos_limits"], dtype=torch.float32 + ) + limit_term, limit_spec = next( + (k, v) + for k, v in config.data["rewards"].items() + if v["function"] == "joint_pos_limits" + ) + term_penalty = next( + k for k, v in config.data["rewards"].items() if v["function"] == "is_terminated" + ) + limit_joint = limit_spec["joint_ids"][0] + joint_pos = default_qpos.repeat(n, 1) + joint_pos[1, limit_joint] = limits[limit_joint, 1] + 0.15 + illegal = torch.zeros((n, 3, 2, 3), dtype=torch.float32) + illegal[2, 1, 0, 2] = 2.5 + foot_force = torch.zeros((n, 3, 2, 3), dtype=torch.float32) + foot_force[1, 0, 0, 2] = 5.0 + state = RoughVelocityState( + base_lin_vel_b=torch.tensor( + [[0.5, 0.0, 0.0], [0.2, -0.1, 0.3], [0.0, 0.0, 0.0], [0.4, 0.1, -0.2]] + ), + base_ang_vel_b=torch.tensor( + [[0.0, 0.0, 0.3], [0.2, -0.1, 0.0], [0.0, 0.0, 0.0], [0.1, 0.1, -0.2]] + ), + projected_gravity_b=torch.tensor( + [[0.0, 0.0, -1.0], [0.1, -0.1, -0.99], [0.0, 0.0, -1.0], [0.2, 0.0, -0.98]] + ), + base_lin_vel_yaw=torch.tensor( + [[0.5, 0.0, 0.0], [0.1, 0.0, 0.0], [0.0, 0.0, 0.0], [0.4, 0.1, 0.0]] + ), + base_ang_vel_w=torch.tensor( + [[0.0, 0.0, 0.3], [0.0, 0.0, 0.1], [0.0, 0.0, 0.0], [0.0, 0.0, -0.2]] + ), + command=torch.tensor( + [[0.5, 0.0, 0.3], [0.5, 0.0, 0.0], [0.0, 0.0, 0.0], [0.4, 0.1, -0.2]] + ), + joint_pos=joint_pos, + joint_vel=torch.full((n, dof), 0.25), + joint_acc=torch.full((n, dof), 1.5), + applied_torque=torch.full((n, dof), 2.0), + action=torch.full((n, dof), 0.5), + previous_action=torch.full((n, dof), 0.1), + foot_vel_w=torch.tensor([[[0.2, 0.1, 0.0], [0.0, 0.0, 0.0]]]).repeat(n, 1, 1), + foot_force_history=foot_force, + current_air_time=torch.tensor( + [[0.3, 0.0], [0.0, 0.2], [0.1, 0.1], [0.25, 0.0]] + ), + current_contact_time=torch.tensor( + [[0.0, 0.2], [0.02, 0.0], [0.0, 0.0], [0.0, 0.15]] + ), + last_air_time=torch.full((n, 2), 0.35), + illegal_force_history=illegal, + height_scan=torch.linspace(-2.0, 2.0, n * 187).reshape(n, 187), + episode_step=torch.tensor([10, 20, 30, 1000]), + root_height=torch.full((n,), 0.78), + ) + noise = {"height_scan": torch.full((n, 187), 0.05)} + constants = RoughVelocityConstants.from_config(config, state.joint_pos) + obs_uncached = build_observation(config, state, noise=noise, constants=None) + obs_cached = build_observation(config, state, noise=noise, constants=constants) + assert obs_uncached.shape == (n, dimension) + torch.testing.assert_close(obs_cached, obs_uncached, rtol=0, atol=0) + # Height scan clips to [-1.0, 1.0] before scaling. + assert obs_uncached[:, -187:].abs().max().item() <= 1.0 + 1e-6 + + terminated, truncated = compute_termination(config, state) + assert terminated.tolist() == [False, False, True, False] + assert truncated.tolist() == [False, False, False, True] + + rew_uncached = compute_rewards(config, state, terminated, constants=None) + rew_cached = compute_rewards(config, state, terminated, constants=constants) + assert set(rew_uncached.raw_terms) == set(config.data["rewards"]) + for name in config.data["rewards"]: + torch.testing.assert_close( + rew_cached.raw_terms[name], rew_uncached.raw_terms[name], rtol=0, atol=0 + ) + expected_total = sum( + rew_uncached.raw_terms[name] + * config.data["rewards"][name]["weight"] + * config.control_dt + for name in config.data["rewards"] + ) + torch.testing.assert_close(rew_uncached.total, expected_total, rtol=1e-6, atol=1e-6) + torch.testing.assert_close(rew_cached.total, rew_uncached.total, rtol=0, atol=0) + assert rew_uncached.raw_terms[limit_term][1].item() == pytest.approx(0.15) + assert rew_uncached.raw_terms[term_penalty][2].item() == 1.0 + + +@pytest.mark.no_sim +@pytest.mark.parametrize("task", ["g1_rough"]) +def test_reference_action_retains_large_policy_output(task: str) -> None: + """Outputs above one must reach the reference offset mapping unchanged.""" + config = load_config(task) + action = torch.full((2, config.action_dim), 3.0) + target = action_target(config, action) + expected = torch.tensor(config.default_joint_position) + 3 * torch.tensor( + config.action_scale + ) + torch.testing.assert_close(target, expected.expand_as(target)) + + +@pytest.mark.no_sim +def test_motor_brakes_at_overspeed() -> None: + """The four-quadrant envelope retains opposing torque beyond free speed.""" + result = dc_motor_effort( + torch.tensor([100.0, 100.0, -100.0, -100.0]), + torch.tensor([0.0, 30.0, -30.0, -90.0]), + torch.tensor(20.0), + torch.tensor(30.0), + torch.tensor(20.0), + ) + torch.testing.assert_close(result, torch.tensor([20.0, 0.0, 0.0, 20.0])) + + +@pytest.mark.no_sim +def test_contact_transition_keeps_elapsed_air_and_contact_time() -> None: + """Landing and liftoff include the elapsed sensor interval in the last mode.""" + air = torch.tensor([[0.1, 0.0]]) + contact = torch.tensor([[0.0, 0.2]]) + last_air = torch.zeros_like(air) + last_contact = torch.zeros_like(air) + update_contact_times( + torch.tensor([[[0.0, 0.0, 0.00001], [0.0, 0.0, 0.0]]]), + 0.02, + air, + contact, + last_air, + last_contact, + ) + torch.testing.assert_close(air, torch.tensor([[0.0, 0.02]])) + torch.testing.assert_close(contact, torch.tensor([[0.02, 0.0]])) + torch.testing.assert_close(last_air, torch.tensor([[0.12, 0.0]])) + torch.testing.assert_close(last_contact, torch.tensor([[0.0, 0.22]])) + + +@pytest.mark.no_sim +def test_ground_replacement_updates_declared_geometry() -> None: + """Ground redeclaration must retain the modified render and collision geometry.""" + original = GeometryDesc.plane(10.0) + descriptor = ObjectDesc( + name="ground", + collisions=[CollisionDesc.from_geometry(original)], + renders=[RenderDesc.from_geometry(original)], + ) + declared = copy(descriptor) + sim = object.__new__(SimulationManager) + sim.sim_config = SimulationManagerCfg() + sim._spawn_default_plane_descriptor = descriptor + scene = Mock() + scene.builder.is_finalized = False + scene.handles.return_value = () + sim._spawn_scene = scene + mesh = GeometryDesc.mesh( + vertices=np.array([[0, 0, 0], [1, 0, 0], [0, 1, 1]], dtype=np.float32), + triangles=np.array([[0, 1, 2]], dtype=np.int32), + ) + collision = NewtonCollisionDesc( + convert_to_heightfield=True, heightfield_resolution=0.1 + ) + sim.configure_ground_plane_collision(newton=collision, geometry=mesh) + scene.remove.assert_called_once_with("default_plane") + scene.declare.assert_called_once_with("rigid_object", "default_plane", descriptor) + assert declared.collisions[0].geometry_type == mesh.geometry_type + assert declared.collisions[0].newton is collision + np.testing.assert_array_equal(declared.collisions[0].vertices, mesh.vertices) + np.testing.assert_array_equal(declared.renders[0].triangles, mesh.triangles) + sim._spawn_scene = SimpleNamespace( + builder=SimpleNamespace(is_finalized=True, result=object()) + ) + with pytest.raises(RuntimeError, match="before prepare"): + sim.configure_ground_plane_collision(enabled=False) + + +@pytest.mark.no_sim +def test_remove_ground_plane_before_prepare() -> None: + """Remove the default ground plane cleanly before prepare and guard post-prepare calls.""" + from unittest.mock import MagicMock + + sim = object.__new__(SimulationManager) + sim.sim_config = SimulationManagerCfg() + for key in ( + "_lights", + "_sensors", + "_robots", + "_rigid_objects", + "_rigid_object_groups", + "_deformable_objects", + "_fluid_objects", + "_articulations", + ): + setattr(sim, key, {}) + assert sim.asset_uids == ["default_plane"] + scene = MagicMock() + scene.builder.is_finalized = False + scene.handles.return_value = () + scene.__contains__.return_value = True + sim._spawn_scene = scene + sim._spawn_default_plane_descriptor = object() + sim.remove_ground_plane() + scene.remove.assert_called_once_with("default_plane") + assert sim._spawn_default_plane_descriptor is None + assert sim.asset_uids == [] + + sim._spawn_scene = SimpleNamespace( + builder=SimpleNamespace(is_finalized=True, result=object()) + ) + with pytest.raises(RuntimeError, match="before prepare"): + sim.remove_ground_plane() + + +@pytest.mark.no_sim +def test_load_rough_env_cfg_resolves_training_and_evaluation_bundles( + tmp_path: Path, +) -> None: + """Resolve explicit G1 asset bundles for evaluation and training modes.""" + (tmp_path / "g1.usda").write_text("#usda 1.0\n") + np.savez( + tmp_path / "terrain.npz", + vertices=np.zeros((3, 3), dtype=np.float32), + triangles=np.array([[0, 1, 2]], dtype=np.int32), + resolution=np.float32(0.1), + ) + (tmp_path / "terrain.json").write_text( + json.dumps( + { + "env_origins": [[1.0, 2.0, 0.5]], + "terrain_origins": [[[0.0, 0.0, 0.0]]], + "max_init_terrain_level": 0, + "generator": {"size": [8.0, 8.0]}, + } + ) + ) + eval_cfg = load_rough_env_cfg("g1", tmp_path, num_envs=2, seed=7) + assert eval_cfg.num_envs == 2 + assert eval_cfg.seed == 7 + assert eval_cfg.extensions["deterministic_reset"] is True + assert eval_cfg.extensions["origin"] == [1.0, 2.0, 0.5] + + train_cfg = load_rough_env_cfg( + "g1", tmp_path, num_envs=4, training_randomization=True + ) + assert train_cfg.extensions["randomization"] is True + assert "terrain_origins" in train_cfg.extensions["terrain_course"] + + +@pytest.mark.no_sim +def test_height_scanner_keeps_yaw_frame_and_xy_order() -> None: + """Check all 187 rays against an analytic inclined plane after a yaw rotation.""" + vertices = np.array( + [[-5, -5, -1.5], [5, -5, -0.5], [5, 5, 1.5], [-5, 5, 0.5]], dtype=np.float32 + ) + scanner = HeightScanner( + vertices, + np.array([[0, 1, 2], [0, 2, 3]], dtype=np.int32), + 1, + torch.device("cpu"), + ) + pose = torch.tensor([[0.0, 0.0, 1.0, 0.0, 0.0, 2**-0.5, 2**-0.5]]) + result = scanner.compute(pose) + x, y = torch.meshgrid( + torch.arange(-0.8, 0.8 + 1e-9, 0.1), + torch.arange(-0.5, 0.5 + 1e-9, 0.1), + indexing="xy", + ) + expected = 0.5 + 0.1 * y.flatten() - 0.2 * x.flatten() + torch.testing.assert_close(result[0], expected, atol=5e-6, rtol=0) + + +@pytest.mark.no_sim +def test_height_scanner_queries_physics_heightfield_instead_of_source_mesh() -> None: + """Read the collision surface when its height differs from the source mesh.""" + import newton + + heightfield = newton.Heightfield( + data=np.ones((3, 3), dtype=np.float32), + nrow=3, + ncol=3, + hx=2.0, + hy=2.0, + min_z=1.0, + max_z=1.0, + ) + builder = newton.ModelBuilder() + builder.add_shape_heightfield(heightfield=heightfield) + model = builder.finalize(device="cpu") + # The source mesh is at z=0; the collision heightfield is at z=1 m. + vertices = np.array( + [[-2, -2, 0], [2, -2, 0], [2, 2, 0], [-2, 2, 0]], dtype=np.float32 + ) + scanner = HeightScanner( + vertices, + np.array([[0, 1, 2], [0, 2, 3]], dtype=np.int32), + 1, + torch.device("cpu"), + physics_model=model, + physics_state=model.state(), + ) + pose = torch.tensor([[0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]]) + expected = torch.full((1, 187), -0.5) # torso_z - terrain_z - 0.5 m + torch.testing.assert_close(scanner.compute(pose), expected, atol=5e-6, rtol=0) + + +@pytest.mark.no_sim +def test_contact_impulse_uses_internal_solver_interval() -> None: + """0.025 N s from a 2.5 ms solver interval is 10 N; invalid slots stay zero.""" + env = object.__new__(RoughVelocityEnv) + env._num_envs = 1 + env._device = torch.device("cpu") + env.sim = SimpleNamespace(device=torch.device("cpu")) + env.sim_cfg = SimpleNamespace( + physics_dt=0.005, physics_cfg=SimpleNamespace(num_substeps=2) + ) + env.cfg = SimpleNamespace(sim_cfg=env.sim_cfg, sim_steps_per_control=4) + data = { + "user_ids": torch.tensor([[[0, 1], [0, 1]]]), + "is_valid": torch.tensor([[True, False]]), + "normal": torch.tensor([[[0.0, 0.0, 1.0], [float("nan"), 0.0, 0.0]]]), + "impulse": torch.tensor([[0.025, float("nan")]]), + } + env._reference_sensor = SimpleNamespace(update=lambda: None, get_data=lambda: data) + env._sorted_keys = torch.tensor([1]) + env._sorted_rows = torch.tensor([0]) + env._normal_force = torch.zeros(1, 1, 3) + env._normal_history = torch.zeros(1, 3, 1, 3) + env._air_time = torch.zeros(1, 1) + env._contact_time = torch.zeros(1, 1) + env._last_air_time = torch.zeros(1, 1) + env._last_contact_time = torch.zeros(1, 1) + env._sample_reference_contacts() + torch.testing.assert_close(env._normal_force, torch.tensor([[[0.0, 0.0, 10.0]]])) + torch.testing.assert_close(env._contact_time, torch.tensor([[0.02]])) + + +@pytest.mark.no_sim +def test_rough_curriculum_updates_and_preserves_untouched_rows() -> None: + """Verify promotion, demotion, floor/overflow bounds, and row isolation.""" + rows, cols = 5, 3 + grid_x, grid_y = torch.meshgrid( + torch.arange(rows, dtype=torch.float32) * 10.0, + torch.arange(cols, dtype=torch.float32) * 10.0, + indexing="ij", + ) + origins = torch.stack((grid_x, grid_y, torch.zeros_like(grid_x)), dim=-1) + curriculum = TerrainCurriculum( + origins, 6, 2, 8.0, 20.0, torch.Generator().manual_seed(7) + ) + assert curriculum.types.tolist() == [0, 0, 1, 1, 2, 2] + assert int(curriculum.levels.min()) >= 0 and int(curriculum.levels.max()) <= 2 + + curriculum.levels[:] = torch.tensor([2, 2, 2, 0, 4, 3]) + curriculum.env_origins.copy_(origins[curriculum.levels, curriculum.types]) + before_levels = curriculum.levels.clone() + before_origins = curriculum.env_origins.clone() + + # Selected rows [0, 1, 2, 3, 4]; row 5 is untouched: + # - row 0: distance 5.0 > 4.0 -> promote 2 -> 3 + # - row 1: distance 1.0 < 0.4 * 20 * 0.5 = 4.0 -> demote 2 -> 1 + # - row 2: distance 3.0 in [2.0, 4.0] (cmd=0.2 -> threshold 2.0) -> stay 2 + # - row 3: level 0 demotion clamps at 0 + # - row 4: level 4 promotion overflows max_level=5 -> wraps into [0, 4] + positions = curriculum.env_origins.clone() + positions[0, 0] += 5.0 + positions[1, 0] += 1.0 + positions[2, 0] += 3.0 + positions[3, 0] += 0.5 + positions[4, 0] += 5.0 + commands = torch.tensor( + [ + [0.4, 0.0, 0.0], + [0.4, 0.0, 0.0], + [0.2, 0.0, 0.0], + [0.4, 0.0, 0.0], + [0.4, 0.0, 0.0], + [0.8, 0.0, 0.0], + ] + ) + selected = torch.tensor([0, 1, 2, 3, 4]) + curriculum.update(selected, positions, commands) + + assert curriculum.levels[:4].tolist() == [3, 1, 2, 0] + assert 0 <= int(curriculum.levels[4]) < rows + assert int(curriculum.levels[5]) == int(before_levels[5]) + torch.testing.assert_close(curriculum.env_origins[5], before_origins[5]) + torch.testing.assert_close( + curriculum.env_origins[selected], + origins[curriculum.levels[selected], curriculum.types[selected]], + ) + + +@pytest.mark.no_sim +def test_rough_reset_curriculum_uses_terminal_step_command(monkeypatch) -> None: + """A timeout must use the terminal pose and its reward command.""" + origins = torch.zeros((10, 1, 3)) + curriculum = TerrainCurriculum( + origins, 1, 5, 8.0, 20.0, torch.Generator().manual_seed(42) + ) + curriculum.levels[:] = 3 + env = object.__new__(RoughVelocityEnv) + env._device = torch.device("cpu") + env.sim = SimpleNamespace(device=torch.device("cpu")) + env._num_envs = 1 + env._elapsed_steps = torch.ones(1, dtype=torch.long) + env.episode_context = EpisodeContext(1) + env._profiler = SimpleNamespace(section=lambda *args, **kwargs: nullcontext()) + env.is_task_success = lambda: torch.zeros(1, dtype=torch.bool) + env.sensors = {} + env._detached_uids_for_reset = () + env._reference_terrain_course = curriculum + env.robot = SimpleNamespace( + body_data=SimpleNamespace( + root_pose=torch.tensor([[2.0, 0.0, 0.74, 0.0, 0.0, 0.0, 1.0]]) + ) + ) + env.reward_command = torch.tensor([[0.4, 0.0, 0.0]]) + env.command = torch.tensor([[0.1, 0.0, 0.0]]) + + def reset_objects_state(**kwargs): + env.robot.body_data.root_pose[0, 0] = 6.0 + + env.sim.reset_objects_state = reset_objects_state + + class ResetRecorded(Exception): + pass + + def stop_after_curriculum(self, *args, **kwargs): + raise ResetRecorded + + from embodichain_tasks.locomotion.velocity._embodichain import ( + EmbodiChainVelocityEnv, + ) + + monkeypatch.setattr( + EmbodiChainVelocityEnv, "_before_episode_reset", lambda *args, **kwargs: None + ) + monkeypatch.setattr( + EmbodiChainVelocityEnv, "_initialize_episode", stop_after_curriculum + ) + with pytest.raises(ResetRecorded): + env.reset(options={"reset_ids": [0]}) + # Two meters is below the old command's four-meter threshold; the next + # command's one-meter threshold would incorrectly keep the level unchanged. + assert curriculum.levels.item() == 2 + + +@pytest.mark.no_sim +def test_reference_step_orders_events_after_rewards_and_resets(monkeypatch) -> None: + """Commands and pushes must precede only the returned observation.""" + from embodichain_tasks.locomotion.velocity._embodichain import ( + EmbodiChainVelocityEnv, + ) + + events = [] + env = object.__new__(RoughVelocityEnv) + + def parent_step(self, action, **kwargs): + self._advance_command_timer() + self._update_heading_commands() + events.extend(["rewards", "reset"]) + return ( + {}, + torch.ones(1), + torch.zeros(1, dtype=torch.bool), + torch.zeros(1, dtype=torch.bool), + {}, + ) + + monkeypatch.setattr(EmbodiChainVelocityEnv, "step", parent_step) + monkeypatch.setattr( + EmbodiChainVelocityEnv, + "_advance_command_timer", + lambda self: events.append("commands"), + ) + monkeypatch.setattr( + EmbodiChainVelocityEnv, + "_update_heading_commands", + lambda self: events.append("heading"), + ) + monkeypatch.setattr( + RoughVelocityEnv, + "_apply_reference_interval_events", + lambda self: events.append("push"), + ) + monkeypatch.setattr( + RoughVelocityEnv, + "get_obs", + lambda self, **kwargs: events.append("observation") or {}, + ) + env.step(torch.zeros(1, 37)) + assert events == ["rewards", "reset", "commands", "heading", "push", "observation"] + assert not env._reference_events_deferred + + +@pytest.mark.no_sim +@pytest.mark.parametrize("recording", [False, True]) +@pytest.mark.parametrize("task_managers", [False, True]) +def test_training_step_defers_only_unused_observations( + monkeypatch, recording, task_managers +) -> None: + from embodichain_tasks.locomotion.velocity._embodichain import ( + EmbodiChainVelocityEnv, + ) + + env = object.__new__(RoughVelocityEnv) + env.cfg = SimpleNamespace(events=SimpleNamespace()) + env._num_envs = 2 + env.sim = SimpleNamespace(device=torch.device("cpu")) + env.event_manager = SimpleNamespace(available_modes=[]) + env.rollout_buffer = object() if recording else None + env.dataset_manager = None + env._traj_buffer = None + env._reference_task_managers_only = task_managers + calls = [] + raw_reads = [] + generator = torch.Generator().manual_seed(42) + + def observe(self, **kwargs): + calls.append("observe") + noise = ( + torch.rand(2, 3, generator=generator) + if not self._reference_events_deferred + else 0 + ) + return {"policy": torch.ones(2, 3) + noise} + + def raw_observe(self, **kwargs): + raw_reads.append(True) + return observe(self, **kwargs) + + def parent_step(self, action, **kwargs): + self.get_obs() + self.get_obs() + return ( + {}, + torch.ones(2), + torch.zeros(2, dtype=torch.bool), + torch.zeros(2, dtype=torch.bool), + {}, + ) + + monkeypatch.setattr(EmbodiChainVelocityEnv, "get_obs", raw_observe) + monkeypatch.setattr( + RoughVelocityEnv, "_extend_obs", lambda self, obs, **kwargs: observe(self) + ) + monkeypatch.setattr(EmbodiChainVelocityEnv, "step", parent_step) + monkeypatch.setattr(RoughVelocityEnv, "_advance_command_timer", lambda self: None) + monkeypatch.setattr(RoughVelocityEnv, "_update_heading_commands", lambda self: None) + monkeypatch.setattr( + RoughVelocityEnv, "_apply_reference_interval_events", lambda self: None + ) + + obs, *_ = env.step(torch.zeros(2, 12), policy_observations_only=True) + + expected_generator = torch.Generator().manual_seed(42) + expected = torch.ones(2, 3) + torch.rand(2, 3, generator=expected_generator) + torch.testing.assert_close(obs["policy"], expected, rtol=0, atol=0) + assert torch.equal(generator.get_state(), expected_generator.get_state()) + assert len(calls) == (3 if recording or not task_managers else 1) + assert len(raw_reads) == (3 if recording or not task_managers else 0) + assert not env._reference_defer_observations + + +@pytest.mark.no_sim +@pytest.mark.parametrize("task", ["g1_rough"]) +def test_reference_reset_writes_once_and_preserves_sampled_state(monkeypatch, task): + """Deferred parent writes preserve final states, RNG and untouched rows.""" + from embodichain.lab.gym.envs import EmbodiedEnv + from embodichain_tasks.locomotion.velocity._embodichain import ( + EmbodiChainVelocityEnv, + ) + + monkeypatch.setattr(EmbodiedEnv, "_initialize_episode", lambda *a, **kw: None) + config = load_config(task) + rows = torch.tensor([1, 3]) + + def make_env(legacy: bool): + env = object.__new__(RoughVelocityEnv) + env._device = torch.device("cpu") + env._num_envs = 4 + env.sim = SimpleNamespace(device=env._device) + env.cfg = SimpleNamespace(deterministic_reset=False) + env.velocity_task_config = config + env.policy_joint_ids = torch.arange(config.action_dim) + env._reference_terrain_course = None + env._reference_initial_states = None + env._reference_events_deferred = False + env._locomotion_generator = torch.Generator().manual_seed(42) + env.command = torch.ones((4, 3)) + env.reward_command = torch.zeros((4, 3)) + env._resample_commands = lambda ids: env.command.__setitem__( + ids, torch.rand((len(ids), 3), generator=env._locomotion_generator) + ) + env._update_heading_commands = lambda: None + for name in ( + "_episode_step", + "_previous_joint_velocity", + "_joint_acceleration", + "_previous_root_velocity_w", + "_root_acceleration_w", + "_filtered_base_lin_vel_b", + "_filtered_base_ang_vel_b", + "_foot_peak_height", + "_foot_swing_height_cost", + "_position_target", + "_normal_force", + "_normal_history", + "_air_time", + "_contact_time", + "_last_air_time", + "_last_contact_time", + "_applied_effort", + "_previous_physics_joint_velocity", + "_push_remaining", + ): + setattr( + env, + name, + ( + torch.zeros((4, config.action_dim)) + if name == "_position_target" + else torch.zeros(4) + ), + ) + pose = torch.zeros((4, 7)) + pose[:, 6] = 1 + writes = [] + + def set_state(**fields): + writes.append( + { + k: v.clone() if isinstance(v, torch.Tensor) else v + for k, v in fields.items() + } + ) + pose[fields["env_ids"]] = fields["root_pose"] + + env.robot = SimpleNamespace( + set_state=set_state, body_data=SimpleNamespace(root_pose=pose) + ) + if legacy: + + def parent_write(ids, root_pose, joints): + env._reference_reset_pose = root_pose.clone() + EmbodiChainVelocityEnv._write_episode_initial_state( + env, ids, root_pose, joints + ) + + env._write_episode_initial_state = parent_write + return env, writes + + legacy, old_writes = make_env(True) + current, new_writes = make_env(False) + legacy._initialize_episode(rows) + current._initialize_episode(rows) + assert len(old_writes) == 2 + assert len(new_writes) == 1 + for name, value in old_writes[-1].items(): + if isinstance(value, torch.Tensor): + torch.testing.assert_close(new_writes[-1][name], value, rtol=0, atol=0) + else: + assert new_writes[-1][name] == value + assert torch.equal( + current._locomotion_generator.get_state(), + legacy._locomotion_generator.get_state(), + ) + torch.testing.assert_close( + current.robot.body_data.root_pose[[0, 2]], + torch.tensor([[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]]).expand(2, -1), + ) + + +@pytest.mark.gpu +@pytest.mark.no_sim +def test_height_scanner_graph_replays_changed_poses() -> None: + """Captured rays read current poses and match the eager physics query.""" + import newton + import warp as wp + + device = torch.device("cuda:0") + vertices = np.array( + [[-3, -3, 0], [3, -3, 0], [3, 3, 0], [-3, 3, 0]], dtype=np.float32 + ) + triangles = np.array([[0, 1, 2], [0, 2, 3]], dtype=np.int32) + builder = newton.ModelBuilder() + builder.add_shape_plane() + model = builder.finalize(str(device)) + state = model.state() + scanner = HeightScanner( + vertices, triangles, 2, device, physics_model=model, physics_state=state + ) + previous = None + graph = None + for shift in (-0.07, 0.0, 0.11): + pose = torch.tensor( + [[shift, 0, 0.8 + shift, 0, 0, 0, 1], [0, shift, 1.1 + shift, 0, 0, 0, 1]], + device=device, + ) + captured = scanner.compute(pose).clone() + torch.cuda.synchronize() + if graph is None: + graph = scanner._graph + assert graph is not None and scanner._graph is graph + with wp.ScopedStream(scanner._scan_stream): + scanner._compute_height() + torch.cuda.synchronize() + torch.testing.assert_close(captured, scanner.height, rtol=0, atol=0) + assert bool(torch.isfinite(captured).all()) + if previous is not None: + assert not torch.equal(captured, previous) + previous = captured + + +@pytest.mark.no_sim +def test_curriculum_checkpoint_restores_assignments_and_rejects_other_maps() -> None: + origins = torch.arange(36, dtype=torch.float32).reshape(4, 3, 3) + + def make(count=6): + return TerrainCurriculum( + origins.clone(), count, 3, 8.0, 20.0, torch.Generator().manual_seed(4) + ) + + source = make() + source.levels.fill_(3) + source.env_origins.copy_(source.origins[source.levels, source.types]) + saved = source.state_dict() + target = make() + target.load_state_dict(saved) + torch.testing.assert_close(target.levels, source.levels) + torch.testing.assert_close(target.env_origins, source.env_origins) + torch.testing.assert_close( + target.generator.get_state(), source.generator.get_state() + ) + with pytest.raises(ValueError, match="instance count"): + make(3).load_state_dict(saved) + different_map = make() + different_map.origins.add_(1) + with pytest.raises(ValueError, match="map differs"): + different_map.load_state_dict(saved) + env = SimpleNamespace( + _reference_terrain_course=target, num_envs=6, device=torch.device("cpu") + ) + env.validate_training_state = ( + lambda state: RoughVelocityEnv.validate_training_state(env, state) + ) + RoughVelocityEnv.load_training_state(env, {"terrain_course": saved}) + assert env._reference_resume_reset.all() + + before = target.state_dict() + invalid = {**saved, "levels": torch.full_like(saved["levels"], -1)} + with pytest.raises(ValueError, match="out of range"): + env.validate_training_state({"terrain_course": invalid}) + for key, value in before.items(): + torch.testing.assert_close(value, target.state_dict()[key], rtol=0, atol=0) diff --git a/tests/gym/envs/tasks/test_rough_velocity_frame.py b/tests/gym/envs/tasks/test_rough_velocity_frame.py new file mode 100644 index 0000000000..e7a0ad4fb2 --- /dev/null +++ b/tests/gym/envs/tasks/test_rough_velocity_frame.py @@ -0,0 +1,79 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- +"""Compare fused reference vectors with the established Torch mathematics.""" + +from __future__ import annotations + +import pytest +import torch +import warp as wp + +from embodichain.utils.math import quat_apply_inverse +from embodichain_tasks.locomotion.velocity.rough._frame import _reference_frame_vectors + +__all__: list[str] = [] +pytestmark = pytest.mark.no_sim + + +@pytest.mark.parametrize("device", ["cpu", "cuda:0"]) +def test_frame_vectors_preserve_math_strides_and_snapshot_ownership( + device: str, +) -> None: + if device.startswith("cuda") and not torch.cuda.is_available(): + pytest.skip("CUDA is unavailable") + wp.init() + generator = torch.Generator(device=device).manual_seed(42) + pose = torch.randn((2048, 7), generator=generator, device=device) + quaternion = pose[:, 3:] + quaternion /= quaternion.norm(dim=-1, keepdim=True) + quaternion[:4] = torch.tensor( + [ + [0.0, 0.0, 0.0, 1.0], + [0.0, 0.0, 1.0, 0.0], + [0.0, 1.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0], + ], + device=device, + ) + linear = torch.randn((2048, 3), generator=generator, device=device) * 10 + angular = torch.randn((2048, 3), generator=generator, device=device) * 10 + x, y, z, w = quaternion.unbind(-1) + yaw = torch.atan2(2 * (w * z + x * y), 1 - 2 * (y.square() + z.square())) + yaw_quaternion = torch.zeros_like(quaternion) + yaw_quaternion[:, 2] = torch.sin(yaw / 2) + yaw_quaternion[:, 3] = torch.cos(yaw / 2) + gravity = torch.zeros_like(linear) + gravity[:, 2] = -1 + expected = ( + quat_apply_inverse(quaternion, linear), + quat_apply_inverse(quaternion, angular), + quat_apply_inverse(quaternion, gravity), + quat_apply_inverse(yaw_quaternion, linear), + ) + before = [value.clone() for value in (pose, linear, angular)] + actual = _reference_frame_vectors(quaternion, linear, angular) + wp.synchronize_device(device) + for a, b in zip(actual, expected, strict=True): + torch.testing.assert_close(a, b, atol=1e-5, rtol=1e-6) + saved = [value.clone() for value in actual] + _reference_frame_vectors( + quaternion, torch.zeros_like(linear), torch.zeros_like(angular) + ) + wp.synchronize_device(device) + for a, b in zip(actual, saved, strict=True): + torch.testing.assert_close(a, b, atol=0, rtol=0) + for a, b in zip((pose, linear, angular), before, strict=True): + torch.testing.assert_close(a, b, atol=0, rtol=0) diff --git a/tests/gym/envs/tasks/test_unitree_locomotion.py b/tests/gym/envs/tasks/test_unitree_locomotion.py index 81652d9fca..7c44195267 100644 --- a/tests/gym/envs/tasks/test_unitree_locomotion.py +++ b/tests/gym/envs/tasks/test_unitree_locomotion.py @@ -348,3 +348,56 @@ def test_policy_viewer_target_pose_preserves_world_xyzw_and_copies_state(): assert target.tolist() == pytest.approx(pose[0].tolist()) target[:] = 0 assert pose[0].tolist() == pytest.approx([1, 2, 3, 0, 0, 0.6, 0.8]) + + +@pytest.mark.parametrize( + "rough,override,expected", + [ + (False, {}, 0.5), + (True, {}, 1.0), + (False, {"clip": None}, 1.0), + (True, {"clip": 0.5}, 0.25), + ], +) +def test_environment_action_clipping_defaults_and_overrides( + monkeypatch, rough, override, expected +): + from dataclasses import dataclass + from types import SimpleNamespace, MethodType + from embodichain.lab.gym.envs import EmbodiedEnv + from embodichain.lab.gym.envs.managers.actions import DefaultJointPositionAction + from embodichain_tasks.locomotion.velocity._embodichain import ( + EmbodiChainVelocityEnv, + ) + from embodichain_tasks.locomotion.velocity.rough.runtime import RoughVelocityEnv + + @dataclass + class State: + qpos: torch.Tensor + + cls = RoughVelocityEnv if rough else EmbodiChainVelocityEnv + host = object.__new__(cls) + host.state_type = State + host.velocity_task_config = SimpleNamespace( + data={}, joint_names=["joint"], default_joint_position=[0.0], action_scale=[0.5] + ) + params = dict(override) + cfg = SimpleNamespace( + robot=object(), actions={"joint_position": SimpleNamespace(params=params)} + ) + monkeypatch.setattr(EmbodiedEnv, "__init__", lambda *args, **kwargs: None) + cls.__init__(host, cfg) + term = SimpleNamespace( + num_envs=1, + action_dim=1, + _raw_actions=torch.zeros(1, 1), + _previous_raw_actions=torch.zeros(1, 1), + _processed_actions=torch.zeros(1, 1), + _offset=torch.tensor(params["offset"]), + _scale=torch.tensor(params["scale"]), + position_bias=torch.zeros(1, 1), + _clip=params["clip"], + ) + term._store_raw = MethodType(DefaultJointPositionAction._store_raw, term) + DefaultJointPositionAction.process_actions(term, torch.tensor([[2.0]])) + assert term._processed_actions.item() == expected diff --git a/tests/learning/test_rl_resume.py b/tests/learning/test_rl_resume.py index 6a16d8a463..d0a45e403f 100644 --- a/tests/learning/test_rl_resume.py +++ b/tests/learning/test_rl_resume.py @@ -99,6 +99,94 @@ def _assert_state_equal(first, second) -> None: assert first == second +def _attach_curriculum(trainer: Trainer, level: int) -> None: + """Expose rank-local task state on the existing lightweight environment.""" + env = trainer.env + env.levels = torch.full((env.num_envs,), level, dtype=torch.long) + + def validate(state): + if state["levels"].shape != env.levels.shape: + raise ValueError("Curriculum instance count differs") + + def load(state): + validate(state) + env.levels.copy_(state["levels"]) + + env.get_training_state = lambda: {"levels": env.levels.clone()} + env.validate_training_state = validate + env.load_training_state = load + + +def test_v2_restores_environment_state_before_reset(tmp_path, monkeypatch): + source = _trainer(tmp_path / "source") + _attach_curriculum(source, 3) + source.train(16) + path = source.save_checkpoint() + payload = torch.load(path, weights_only=True) + assert payload["schema_version"] == 2 + assert len(payload["environment_state_by_rank"]) == 1 + restored = _trainer(tmp_path / "restored") + _attach_curriculum(restored, 0) + reset = restored.collector._reset_env + + def check_reset(): + torch.testing.assert_close(restored.env.levels, source.env.levels) + return reset() + + monkeypatch.setattr(restored.collector, "_reset_env", check_reset) + restored.load_checkpoint(path) + _assert_state_equal(source.policy.state_dict(), restored.policy.state_dict()) + _assert_state_equal(source.algorithm.state_dict(), restored.algorithm.state_dict()) + restored.train(32) + assert restored.num_updates == 2 + again = torch.load(restored.save_checkpoint(), weights_only=True) + assert again["schema_version"] == 2 + _assert_state_equal( + payload["environment_state_by_rank"], again["environment_state_by_rank"] + ) + + +@pytest.mark.parametrize("defect", ["rank_count", "instance_count", "loader"]) +def test_environment_state_rejected_before_learning_state_changes(tmp_path, defect): + source = _trainer(tmp_path / "source") + _attach_curriculum(source, 3) + source.train(16) + payload = torch.load(source.save_checkpoint(), weights_only=True) + restored = _trainer(tmp_path / "restored") + _attach_curriculum(restored, 0) + if defect == "rank_count": + payload["environment_state_by_rank"] = [] + elif defect == "instance_count": + payload["environment_state_by_rank"][0]["levels"] = torch.zeros(3) + else: + del restored.env.load_training_state + path = tmp_path / "invalid.pt" + torch.save(payload, path) + policy = deepcopy(restored.policy.state_dict()) + optimizer = deepcopy(restored.algorithm.state_dict()) + with pytest.raises(ValueError, match="[Ee]nvironment|[Cc]urriculum"): + restored.load_checkpoint(path) + _assert_state_equal(policy, restored.policy.state_dict()) + _assert_state_equal(optimizer, restored.algorithm.state_dict()) + assert restored.num_updates == 0 + assert not restored.env.levels.any() + + +def test_older_v2_reports_missing_environment_state(tmp_path): + source = _trainer(tmp_path / "source") + source.train(16) + payload = torch.load(source.save_checkpoint(), weights_only=True) + payload.pop("environment_state_by_rank") + path = tmp_path / "older-v2.pt" + torch.save(payload, path) + restored = _trainer(tmp_path / "restored") + _attach_curriculum(restored, 0) + with pytest.warns(UserWarning, match="environment training state"): + restored.load_checkpoint(path) + assert restored.num_updates == 1 + _assert_state_equal(source.policy.state_dict(), restored.policy.state_dict()) + + @pytest.mark.parametrize("algorithm", ["ppo", "grpo"]) @pytest.mark.parametrize( "scheduler", diff --git a/tests/learning/test_rl_resume_distributed.py b/tests/learning/test_rl_resume_distributed.py index a792f89484..3af281702c 100644 --- a/tests/learning/test_rl_resume_distributed.py +++ b/tests/learning/test_rl_resume_distributed.py @@ -42,6 +42,19 @@ def _components(rank: int, directory: Path, mode: str) -> Trainer: env = PointMassEnv(num_envs=4, max_episode_steps=4) + env.level = rank + 2 + env.get_training_state = lambda: {"rank": rank, "level": env.level} + + def validate(state): + if state["rank"] != rank: + raise ValueError("Environment state belongs to another rank") + + def load(state): + validate(state) + env.level = state["level"] + + env.validate_training_state = validate + env.load_training_state = load policy = ActorCritic( 14, 2, @@ -165,7 +178,9 @@ def fail_export(*args, **kwargs): else None ) restored = _components(rank, directory / f"{mode}_resumed", mode) + restored.env.level = -1 restored.load_checkpoint(path) + assert restored.env.level == rank + 2 assert restored.best_eval_value == trainer.best_eval_value assert restored.best_checkpoint_path is None actual_rng = random.random(), np.random.rand(), torch.rand(3) @@ -195,6 +210,10 @@ def fail_export(*args, **kwargs): assert Path(restored.best_checkpoint_path).is_file() payload = torch.load(path, weights_only=True) assert len(payload["rng_by_rank"]) == 2 + assert payload["environment_state_by_rank"] == [ + {"rank": 0, "level": 2}, + {"rank": 1, "level": 3}, + ] if mode != "grpo": assert not torch.equal( payload["normalizer_by_rank"][0]["actor_obs_normalizer.mean"],