From b1908b94ea6edd40dba6b8692c282179af389020 Mon Sep 17 00:00:00 2001 From: acrlw <13927622+acrlw@users.noreply.github.com> Date: Wed, 21 Oct 2026 17:42:15 +0800 Subject: [PATCH 01/24] feat(tasks): add rough-terrain and dexterous RL task candidates --- .../topics/data-assets/data-assets.md | 6 + .../topics/rl-learning/rl-learning.md | 14 + docs/source/api_reference/public_api.rst | 265 + docs/source/overview/rl/dexterous.md | 44 + docs/source/overview/rl/index.rst | 1 + embodichain/data/assets/__init__.py | 3 + embodichain/data/assets/rl_assets.py | 94 + .../g1_isaac_rough/agents/ppo.newton.yaml | 59 + .../velocity/g1_isaac_rough/catalog.yaml | 11 + .../velocity/g1_isaac_rough/env.newton.yaml | 188 + .../velocity/g1_isaac_rough/rsl_rl.json | 65 + .../velocity/g1_isaac_rough/task.json | 1224 +++++ .../agents/ppo.dexuni.yaml | 53 + .../agents/ppo.mujoco_warp.yaml | 53 + .../leap_cube_reorient/agents/ppo.dexuni.yaml | 53 + .../agents/ppo.mujoco_warp.yaml | 53 + .../leap_cube_rotate_z/agents/ppo.dexuni.yaml | 53 + .../agents/ppo.mujoco_warp.yaml | 53 + .../agents/ppo.dexuni.yaml | 53 + .../agents/ppo.mujoco_warp.yaml | 53 + .../shadow_pen_fixed/agents/ppo.dexuni.yaml | 53 + .../agents/ppo.mujoco_warp.yaml | 53 + .../locomotion/velocity/_embodichain.py | 86 +- .../locomotion/velocity/g1_isaac_rough.py | 41 + .../locomotion/velocity/isaac/README.md | 31 + .../locomotion/velocity/isaac/__init__.py | 23 + .../locomotion/velocity/isaac/_frame.py | 82 + .../locomotion/velocity/isaac/config.py | 60 + .../locomotion/velocity/isaac/curriculum.py | 101 + .../locomotion/velocity/isaac/environment.py | 156 + .../locomotion/velocity/isaac/mdp.py | 339 ++ .../locomotion/velocity/isaac/rsl_rl.py | 97 + .../locomotion/velocity/isaac/runtime.py | 697 +++ .../locomotion/velocity/isaac/terrain.py | 214 + .../manipulation/dexterous/__init__.py | 18 + .../manipulation/dexterous/_audit_model.py | 497 ++ .../manipulation/dexterous/_audit_tendons.py | 186 + .../manipulation/dexterous/_dexuni_adapter.py | 230 + .../manipulation/dexterous/_leap_tasks.py | 486 ++ .../manipulation/dexterous/_runtime.py | 697 +++ .../manipulation/dexterous/_shadow_task.py | 375 ++ .../dexterous/leap_cube_face_turn.py | 31 + .../dexterous/leap_cube_reorient.py | 31 + .../dexterous/leap_cube_rotate_z.py | 31 + .../dexterous/shadow_pen_continuous.py | 31 + .../dexterous/shadow_pen_fixed.py | 31 + pyproject.toml | 1 + .../envs/tasks/fixtures/g1_isaac_rough.json | 4697 +++++++++++++++++ .../tasks/fixtures/terrain_curriculum.json | 3566 +++++++++++++ tests/gym/envs/tasks/test_isaac_velocity.py | 796 +++ .../envs/tasks/test_isaac_velocity_frame.py | 79 + 51 files changed, 16179 insertions(+), 35 deletions(-) create mode 100644 docs/source/overview/rl/dexterous.md create mode 100644 embodichain/data/assets/rl_assets.py create mode 100644 extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/agents/ppo.newton.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/catalog.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/env.newton.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/rsl_rl.json create mode 100644 extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/task.json create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_face_turn/agents/ppo.dexuni.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_face_turn/agents/ppo.mujoco_warp.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_reorient/agents/ppo.dexuni.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_reorient/agents/ppo.mujoco_warp.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_rotate_z/agents/ppo.dexuni.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_rotate_z/agents/ppo.mujoco_warp.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/agents/ppo.dexuni.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/agents/ppo.mujoco_warp.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/agents/ppo.dexuni.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/agents/ppo.mujoco_warp.yaml create mode 100644 extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/g1_isaac_rough.py create mode 100644 extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/README.md create mode 100644 extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/__init__.py create mode 100644 extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/_frame.py create mode 100644 extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/config.py create mode 100644 extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/curriculum.py create mode 100644 extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/environment.py create mode 100644 extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/mdp.py create mode 100644 extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/rsl_rl.py create mode 100644 extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/runtime.py create mode 100644 extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/terrain.py create mode 100644 extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/__init__.py create mode 100644 extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_audit_model.py create mode 100644 extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_audit_tendons.py create mode 100644 extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_dexuni_adapter.py create mode 100644 extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_leap_tasks.py create mode 100644 extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_runtime.py create mode 100644 extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_shadow_task.py create mode 100644 extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/leap_cube_face_turn.py create mode 100644 extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/leap_cube_reorient.py create mode 100644 extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/leap_cube_rotate_z.py create mode 100644 extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/shadow_pen_continuous.py create mode 100644 extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/shadow_pen_fixed.py create mode 100644 tests/gym/envs/tasks/fixtures/g1_isaac_rough.json create mode 100644 tests/gym/envs/tasks/fixtures/terrain_curriculum.json create mode 100644 tests/gym/envs/tasks/test_isaac_velocity.py create mode 100644 tests/gym/envs/tasks/test_isaac_velocity_frame.py diff --git a/agent_context/topics/data-assets/data-assets.md b/agent_context/topics/data-assets/data-assets.md index 5da2d69c88..1db8e0f883 100644 --- a/agent_context/topics/data-assets/data-assets.md +++ b/agent_context/topics/data-assets/data-assets.md @@ -255,3 +255,9 @@ 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 CLI +adds four robot entries and the `G1RoughTerrains` scene entry, based on each +class's archive category. 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..852c837798 100644 --- a/agent_context/topics/rl-learning/rl-learning.md +++ b/agent_context/topics/rl-learning/rl-learning.md @@ -211,3 +211,17 @@ 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 task candidates + +`locomotion/velocity/g1_isaac_rough.py` registers the 37-action G1 task. +Its frozen curriculum comes from `G1RoughTerrains`; the robot comes from +`UnitreeG1Rough`, independently of the 29-joint G1 flat archive. + +`manipulation/dexterous/` contains five task-named learning-env factories and +shared internal physics/MDP adapters. Matching task-first agent configurations +select `mujoco_warp` or `dexuni` explicitly. The declared Dict observation +spaces match returned actor/critic groups. Source MJCF audits run at creation. +Version-specific backend adapters remain local to the task collection pending +engine-owned public import/state interfaces. See the user-facing dexterous +RL page for current training and checkpoint-continuation boundaries. diff --git a/docs/source/api_reference/public_api.rst b/docs/source/api_reference/public_api.rst index 527e63d5a7..f56012cb6b 100644 --- a/docs/source/api_reference/public_api.rst +++ b/docs/source/api_reference/public_api.rst @@ -163,6 +163,36 @@ Prepared robot snapshots preserve resource trees and source notices for relocata .. autofunction:: snapshot_urdf_resources +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.data.assets.demo_assets ----------------------------------- @@ -4879,6 +4909,241 @@ reference-state resets and terminal snapshots before automatic row resets. .. automodule:: embodichain_tasks.locomotion.tracking.g1_motion :members: +embodichain_tasks.locomotion.velocity.g1_isaac_rough +---------------------------------------------------- + +The G1 Rough environment registers the 37-action reference task with terrain height observations. + +.. currentmodule:: embodichain_tasks.locomotion.velocity.g1_isaac_rough + +.. autosummary:: + + UnitreeG1IsaacRoughEnv + +.. autoclass:: UnitreeG1IsaacRoughEnv + :members: + + +embodichain_tasks.locomotion.velocity.isaac +------------------------------------------- + +Load reference task configurations from a supplied scene bundle. The loader validates robot joint order, shared initial states and terrain inputs. + +.. currentmodule:: embodichain_tasks.locomotion.velocity.isaac + +.. autosummary:: + + load_reference_env_cfg + +.. autofunction:: load_reference_env_cfg + + +embodichain_tasks.locomotion.velocity.isaac.config +-------------------------------------------------- + +Packaged task definitions retain policy joint order, observation dimensions and fixed physics/control periods. + +.. currentmodule:: embodichain_tasks.locomotion.velocity.isaac.config + +.. autosummary:: + + IsaacVelocityConfig + load_config + +.. autoclass:: IsaacVelocityConfig + :members: + +.. autofunction:: load_config + + +embodichain_tasks.locomotion.velocity.isaac.curriculum +------------------------------------------------------ + +Terrain assignment and selected-row curriculum updates use terminal positions and commands before resetting each episode. + +.. currentmodule:: embodichain_tasks.locomotion.velocity.isaac.curriculum + +.. autosummary:: + + ReferenceTerrainCurriculum + +.. autoclass:: ReferenceTerrainCurriculum + :members: + + +embodichain_tasks.locomotion.velocity.isaac.environment +------------------------------------------------------- + +Resolve the reference deployment against exported robot and terrain assets, with optional training randomization and contact generation selection. + +.. currentmodule:: embodichain_tasks.locomotion.velocity.isaac.environment + +.. autosummary:: + + load_reference_env_cfg + +.. autofunction:: load_reference_env_cfg + + +embodichain_tasks.locomotion.velocity.isaac.mdp +----------------------------------------------- + +Pure Torch state, action, observation, reward and termination functions share the reference task definitions. Device constants cache fixed values and reward joint selectors. + +.. currentmodule:: embodichain_tasks.locomotion.velocity.isaac.mdp + +.. autosummary:: + + IsaacVelocityState + IsaacVelocityReward + IsaacVelocityConstants + action_target + observation_terms + build_observation + compute_rewards + compute_termination + dc_motor_effort + update_contact_times + +.. autoclass:: IsaacVelocityState + :members: + +.. autoclass:: IsaacVelocityReward + :members: + +.. autoclass:: IsaacVelocityConstants + :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.isaac.rsl_rl +-------------------------------------------------- + +The RSL-RL adapter preserves unbounded policy actions, automatic selected-row resets and timeout bootstrapping. CUDA calls share the existing simulation stream with ordered entry and exit dependencies. + +.. currentmodule:: embodichain_tasks.locomotion.velocity.isaac.rsl_rl + +.. autosummary:: + + ReferenceRslRlEnv + +.. autoclass:: ReferenceRslRlEnv + :members: + + +embodichain_tasks.locomotion.velocity.isaac.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.isaac.runtime + +.. autosummary:: + + IsaacVelocityEnv + +.. autoclass:: IsaacVelocityEnv + :members: + + +embodichain_tasks.locomotion.velocity.isaac.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.isaac.terrain + +.. autosummary:: + + ReferenceHeightScanner + +.. autoclass:: ReferenceHeightScanner + :members: + + +embodichain_tasks.manipulation.dexterous.leap_cube_face_turn +------------------------------------------------------------ + +Create the leap cube face turn vector environment with an explicit DexSim solver and packaged model inputs. + +.. currentmodule:: embodichain_tasks.manipulation.dexterous.leap_cube_face_turn + +.. autosummary:: + + make_env + +.. autofunction:: make_env + + +embodichain_tasks.manipulation.dexterous.leap_cube_reorient +----------------------------------------------------------- + +Create the leap cube reorient vector environment with an explicit DexSim solver and packaged model inputs. + +.. currentmodule:: embodichain_tasks.manipulation.dexterous.leap_cube_reorient + +.. autosummary:: + + make_env + +.. autofunction:: make_env + + +embodichain_tasks.manipulation.dexterous.leap_cube_rotate_z +----------------------------------------------------------- + +Create the leap cube rotate z vector environment with an explicit DexSim solver and packaged model inputs. + +.. currentmodule:: embodichain_tasks.manipulation.dexterous.leap_cube_rotate_z + +.. autosummary:: + + make_env + +.. autofunction:: make_env + + +embodichain_tasks.manipulation.dexterous.shadow_pen_continuous +-------------------------------------------------------------- + +Create the shadow pen continuous vector environment with an explicit DexSim solver and packaged model inputs. + +.. currentmodule:: embodichain_tasks.manipulation.dexterous.shadow_pen_continuous + +.. autosummary:: + + make_env + +.. autofunction:: make_env + + +embodichain_tasks.manipulation.dexterous.shadow_pen_fixed +--------------------------------------------------------- + +Create the shadow pen fixed vector environment with an explicit DexSim solver and packaged model inputs. + +.. currentmodule:: embodichain_tasks.manipulation.dexterous.shadow_pen_fixed + +.. autosummary:: + + make_env + +.. autofunction:: make_env + + embodichain_tasks.manipulation.push_cube ---------------------------------------- diff --git a/docs/source/overview/rl/dexterous.md b/docs/source/overview/rl/dexterous.md new file mode 100644 index 0000000000..1bb4aff559 --- /dev/null +++ b/docs/source/overview/rl/dexterous.md @@ -0,0 +1,44 @@ +# Pen and cube manipulation + +Five vector tasks use the standard learning-environment interface and PPO trainer. +Each deployment selects its solver explicitly and obtains models and initial +states through the EmbodiChain asset registry. + +| Task directory | Registered environment | Actions | Actor / critic observations | Asset | +|---|---|---:|---:|---| +| `shadow_pen_fixed` | `ShadowPenFixedDexSim-v1` | 18 | 87 / 87 | `ShadowHandPen` | +| `shadow_pen_continuous` | `ShadowPenContinuousDexSim-v1` | 18 | 87 / 87 | `ShadowHandPen` | +| `leap_cube_face_turn` | `LeapCubeFaceTurnDexSim-v1` | 16 | 65 / 65 | `LeapHandCubeFaceTurn` | +| `leap_cube_reorient` | `LeapCubeReorientDexSim-v1` | 16 | 57 / 128 | `LeapHandCube` | +| `leap_cube_rotate_z` | `LeapCubeRotateZAxisDexSim-v1` | 16 | 32 / 105 | `LeapHandCube` | + +Training configurations are under +`extensions/embodichain_tasks/configs/tasks/manipulation/dexterous//agents/`. +`ppo.mujoco_warp.yaml` and `ppo.dexuni.yaml` select the corresponding solver. +The default configurations run 32 environments for ten updates and save +checkpoints. For example: + +```bash +embodichain data download --name ShadowHandPen +embodichain train-rl --config extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/agents/ppo.mujoco_warp.yaml +``` + +Increase `trainer.iterations` and `trainer.num_envs` for a longer experiment. +Policy and optimizer state are saved by the standard trainer. Checkpoint +evaluation uses the generated RUN manifest through `embodichain eval-policy`. +The core training CLI does not currently expose an optimizer-resume option; +programmatic continuation must restore policy, optimizer and trainer counters. + +The fixed and continuous pen tasks share their model. Continuous spinning +samples from 22 retained initial poses. Cube face turning retains the passive +top-layer joint and updates its detent on every physics substep. Reorientation +and Z-axis rotation use separate actor and critic observation groups. +Selective reset preserves unselected physical rows; completed rows return the +next initial observation with terminal reward and done flags. + +The runtime currently contains version-specific MJCF import and compact-state +adapters for DexSim/Newton. Model audits check imported masses, inertia, +actuators, tendons and collision masks before stepping. These adapters need +engine-owned public interfaces before this task collection can be treated as +a backend-independent release. The local validation versions and publication +status must accompany a release; short-run checkpoints are not converged policies. 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/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..e437292ca3 --- /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 = "9353ed3cd8888a2d38df32196a8322a5" + + +class G1RoughTerrains(_RLAsset): + """Bundled model inputs for G1RoughTerrains.""" + + category = "scene_assets" + archive_md5 = "3799830bd532aa7047708f749fe462c3" + + +class ShadowHandPen(_RLAsset): + """Bundled model inputs for ShadowHandPen.""" + + archive_md5 = "3730a16ed282a92d663245fd56e07a96" + + +class LeapHandCubeFaceTurn(_RLAsset): + """Bundled model inputs for LeapHandCubeFaceTurn.""" + + archive_md5 = "e148431937375c1469704f1c0b9b4a15" + + +class LeapHandCube(_RLAsset): + """Bundled model inputs for LeapHandCube.""" + + archive_md5 = "1fa070d259d36c5d8b1f45a11187bfa8" + + +_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/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/agents/ppo.newton.yaml b/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/agents/ppo.newton.yaml new file mode 100644 index 0000000000..959285b8eb --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/agents/ppo.newton.yaml @@ -0,0 +1,59 @@ +trainer: + exp_name: g1_rough_ppo_mujoco_warp + gym_config: extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/env.newton.yaml + seed: 42 + device: cuda:0 + headless: true + num_envs: 64 + iterations: 20 + buffer_size: 24 + enable_eval: false + save_frequency_updates: 10 + 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: 384 + 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_isaac_rough/catalog.yaml b/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/catalog.yaml new file mode 100644 index 0000000000..bc8e151c04 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/catalog.yaml @@ -0,0 +1,11 @@ +task_key: g1_isaac_rough +title: Unitree G1 Rough-Terrain Locomotion +summary: G1 locomotion with a frozen terrain curriculum and height observations. +tags: +- locomotion +- reinforcement_learning +- benchmark +default_deployment: newton +deployments: + newton: + config: env.newton.yaml diff --git a/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/env.newton.yaml b/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/env.newton.yaml new file mode 100644 index 0000000000..88dc52a341 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/env.newton.yaml @@ -0,0 +1,188 @@ +physics: newton +id: UnitreeG1IsaacRoughRL-v1 +max_episode_steps: 1000 +num_envs: 1 +scene_node_capacity: 65535 +headless: true +device: cuda:0 +arena_space: 0.0 +render_cfg: + renderer: no-render +physics_config: + sync_to_renderer: false + gravity: + - 0.0 + - 0.0 + - -9.81 + num_substeps: 2 + collision_cfg: + broad_phase: explicit + reduce_contacts: true + max_triangle_pairs: 2500000 + update_interval: null + solver_cfg: + solver_type: mujoco_warp + njmax: 300 + nconmax: 300 + iterations: 100 + ls_iterations: 50 + integrator: implicitfast + separate_worlds: true + tolerance: 1.0e-06 + solver: newton + use_mujoco_contacts: false + cone: pyramidal + impratio: 1.0 + ccd_iterations: 35 + physics_dt: 0.005 +env: + 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: + reference_deterministic_reset: false + reference_randomization: true + reference_origin: + - 0.0 + - 0.0 + - 0.0 + reference_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_isaac_rough/rsl_rl.json b/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/rsl_rl.json new file mode 100644 index 0000000000..76eb553363 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/rsl_rl.json @@ -0,0 +1,65 @@ +{ + "seed": 42, + "device": "cuda:0", + "num_steps_per_env": 24, + "max_iterations": 5000, + "obs_groups": { + "actor": [ + "policy" + ], + "critic": [ + "policy" + ] + }, + "check_for_nan": true, + "save_interval": 50, + "experiment_name": "g1_rough", + "logger": "tensorboard", + "class_name": "OnPolicyRunner", + "algorithm": { + "class_name": "PPO", + "num_learning_epochs": 5, + "num_mini_batches": 4, + "learning_rate": 0.001, + "schedule": "adaptive", + "gamma": 0.99, + "lam": 0.95, + "entropy_coef": 0.008, + "desired_kl": 0.01, + "max_grad_norm": 1.0, + "optimizer": "adam", + "value_loss_coef": 1.0, + "use_clipped_value_loss": true, + "clip_param": 0.2, + "normalize_advantage_per_mini_batch": false, + "share_cnn_encoders": false, + "rnd_cfg": null, + "symmetry_cfg": null + }, + "actor": { + "class_name": "MLPModel", + "hidden_dims": [ + 512, + 256, + 128 + ], + "activation": "elu", + "obs_normalization": false, + "distribution_cfg": { + "class_name": "GaussianDistribution", + "init_std": 1.0, + "std_type": "scalar" + } + }, + "critic": { + "class_name": "MLPModel", + "hidden_dims": [ + 512, + 256, + 128 + ], + "activation": "elu", + "obs_normalization": false, + "distribution_cfg": null + } +} diff --git a/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/task.json b/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/task.json new file mode 100644 index 0000000000..a23933d249 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/task.json @@ -0,0 +1,1224 @@ +{ + "reference": { + "isaac_lab": "v3.0.0-EA", + "task": "Isaac-Velocity-Rough-G1" + }, + "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/agents/ppo.dexuni.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_face_turn/agents/ppo.dexuni.yaml new file mode 100644 index 0000000000..703b1ae2ca --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_face_turn/agents/ppo.dexuni.yaml @@ -0,0 +1,53 @@ +trainer: + exp_name: leap_cube_face_turn_dexuni + learning_env: + name: LeapCubeFaceTurnDexSim-v1 + cfg: + solver_kind: dexuni + cuda_graph: true + seed: 42 + device: cuda:0 + num_envs: 32 + iterations: 10 + buffer_size: 24 + enable_eval: false + save_freq: 768 + use_wandb: false +policy: + name: actor_critic + actor_obs_normalization: true + critic_obs_normalization: true + initial_log_std: -0.693147 + actor: + type: mlp + network_cfg: + hidden_sizes: + - 256 + - 256 + activation: elu + critic: + type: mlp + network_cfg: + hidden_sizes: + - 256 + - 256 + activation: elu + obs_groups: + actor: + - obs + critic: + - obs +algorithm: + name: ppo + cfg: + optimizer: + name: adam + learning_rate: 0.0003 + n_epochs: 4 + batch_size: 256 + gamma: 0.99 + gae_lambda: 0.95 + clip_coef: 0.2 + ent_coef: 0.01 + vf_coef: 0.5 + max_grad_norm: 1.0 diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_face_turn/agents/ppo.mujoco_warp.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_face_turn/agents/ppo.mujoco_warp.yaml new file mode 100644 index 0000000000..11202c6af7 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_face_turn/agents/ppo.mujoco_warp.yaml @@ -0,0 +1,53 @@ +trainer: + exp_name: leap_cube_face_turn_mujoco_warp + learning_env: + name: LeapCubeFaceTurnDexSim-v1 + cfg: + solver_kind: mujoco_warp + cuda_graph: true + seed: 42 + device: cuda:0 + num_envs: 32 + iterations: 10 + buffer_size: 24 + enable_eval: false + save_freq: 768 + use_wandb: false +policy: + name: actor_critic + actor_obs_normalization: true + critic_obs_normalization: true + initial_log_std: -0.693147 + actor: + type: mlp + network_cfg: + hidden_sizes: + - 256 + - 256 + activation: elu + critic: + type: mlp + network_cfg: + hidden_sizes: + - 256 + - 256 + activation: elu + obs_groups: + actor: + - obs + critic: + - obs +algorithm: + name: ppo + cfg: + optimizer: + name: adam + learning_rate: 0.0003 + n_epochs: 4 + batch_size: 256 + gamma: 0.99 + gae_lambda: 0.95 + clip_coef: 0.2 + ent_coef: 0.01 + vf_coef: 0.5 + max_grad_norm: 1.0 diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_reorient/agents/ppo.dexuni.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_reorient/agents/ppo.dexuni.yaml new file mode 100644 index 0000000000..35ec3b5f33 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_reorient/agents/ppo.dexuni.yaml @@ -0,0 +1,53 @@ +trainer: + exp_name: leap_cube_reorient_dexuni + learning_env: + name: LeapCubeReorientDexSim-v1 + cfg: + solver_kind: dexuni + cuda_graph: true + seed: 42 + device: cuda:0 + num_envs: 32 + iterations: 10 + buffer_size: 24 + enable_eval: false + save_freq: 768 + use_wandb: false +policy: + name: actor_critic + actor_obs_normalization: true + critic_obs_normalization: true + initial_log_std: -0.693147 + actor: + type: mlp + network_cfg: + hidden_sizes: + - 256 + - 256 + activation: elu + critic: + type: mlp + network_cfg: + hidden_sizes: + - 256 + - 256 + activation: elu + obs_groups: + actor: + - obs + critic: + - critic_obs +algorithm: + name: ppo + cfg: + optimizer: + name: adam + learning_rate: 0.0003 + n_epochs: 4 + batch_size: 256 + gamma: 0.99 + gae_lambda: 0.95 + clip_coef: 0.2 + ent_coef: 0.01 + vf_coef: 0.5 + max_grad_norm: 1.0 diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_reorient/agents/ppo.mujoco_warp.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_reorient/agents/ppo.mujoco_warp.yaml new file mode 100644 index 0000000000..0647ed5d18 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_reorient/agents/ppo.mujoco_warp.yaml @@ -0,0 +1,53 @@ +trainer: + exp_name: leap_cube_reorient_mujoco_warp + learning_env: + name: LeapCubeReorientDexSim-v1 + cfg: + solver_kind: mujoco_warp + cuda_graph: true + seed: 42 + device: cuda:0 + num_envs: 32 + iterations: 10 + buffer_size: 24 + enable_eval: false + save_freq: 768 + use_wandb: false +policy: + name: actor_critic + actor_obs_normalization: true + critic_obs_normalization: true + initial_log_std: -0.693147 + actor: + type: mlp + network_cfg: + hidden_sizes: + - 256 + - 256 + activation: elu + critic: + type: mlp + network_cfg: + hidden_sizes: + - 256 + - 256 + activation: elu + obs_groups: + actor: + - obs + critic: + - critic_obs +algorithm: + name: ppo + cfg: + optimizer: + name: adam + learning_rate: 0.0003 + n_epochs: 4 + batch_size: 256 + gamma: 0.99 + gae_lambda: 0.95 + clip_coef: 0.2 + ent_coef: 0.01 + vf_coef: 0.5 + max_grad_norm: 1.0 diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_rotate_z/agents/ppo.dexuni.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_rotate_z/agents/ppo.dexuni.yaml new file mode 100644 index 0000000000..7353f2e49f --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_rotate_z/agents/ppo.dexuni.yaml @@ -0,0 +1,53 @@ +trainer: + exp_name: leap_cube_rotate_z_dexuni + learning_env: + name: LeapCubeRotateZAxisDexSim-v1 + cfg: + solver_kind: dexuni + cuda_graph: true + seed: 42 + device: cuda:0 + num_envs: 32 + iterations: 10 + buffer_size: 24 + enable_eval: false + save_freq: 768 + use_wandb: false +policy: + name: actor_critic + actor_obs_normalization: true + critic_obs_normalization: true + initial_log_std: -0.693147 + actor: + type: mlp + network_cfg: + hidden_sizes: + - 256 + - 256 + activation: elu + critic: + type: mlp + network_cfg: + hidden_sizes: + - 256 + - 256 + activation: elu + obs_groups: + actor: + - obs + critic: + - critic_obs +algorithm: + name: ppo + cfg: + optimizer: + name: adam + learning_rate: 0.0003 + n_epochs: 4 + batch_size: 256 + gamma: 0.99 + gae_lambda: 0.95 + clip_coef: 0.2 + ent_coef: 0.01 + vf_coef: 0.5 + max_grad_norm: 1.0 diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_rotate_z/agents/ppo.mujoco_warp.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_rotate_z/agents/ppo.mujoco_warp.yaml new file mode 100644 index 0000000000..214e25f61d --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_rotate_z/agents/ppo.mujoco_warp.yaml @@ -0,0 +1,53 @@ +trainer: + exp_name: leap_cube_rotate_z_mujoco_warp + learning_env: + name: LeapCubeRotateZAxisDexSim-v1 + cfg: + solver_kind: mujoco_warp + cuda_graph: true + seed: 42 + device: cuda:0 + num_envs: 32 + iterations: 10 + buffer_size: 24 + enable_eval: false + save_freq: 768 + use_wandb: false +policy: + name: actor_critic + actor_obs_normalization: true + critic_obs_normalization: true + initial_log_std: -0.693147 + actor: + type: mlp + network_cfg: + hidden_sizes: + - 256 + - 256 + activation: elu + critic: + type: mlp + network_cfg: + hidden_sizes: + - 256 + - 256 + activation: elu + obs_groups: + actor: + - obs + critic: + - critic_obs +algorithm: + name: ppo + cfg: + optimizer: + name: adam + learning_rate: 0.0003 + n_epochs: 4 + batch_size: 256 + gamma: 0.99 + gae_lambda: 0.95 + clip_coef: 0.2 + ent_coef: 0.01 + vf_coef: 0.5 + max_grad_norm: 1.0 diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/agents/ppo.dexuni.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/agents/ppo.dexuni.yaml new file mode 100644 index 0000000000..97a793868b --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/agents/ppo.dexuni.yaml @@ -0,0 +1,53 @@ +trainer: + exp_name: shadow_pen_continuous_dexuni + learning_env: + name: ShadowPenContinuousDexSim-v1 + cfg: + solver_kind: dexuni + cuda_graph: true + seed: 42 + device: cuda:0 + num_envs: 32 + iterations: 10 + buffer_size: 24 + enable_eval: false + save_freq: 768 + use_wandb: false +policy: + name: actor_critic + actor_obs_normalization: true + critic_obs_normalization: true + initial_log_std: -0.693147 + actor: + type: mlp + network_cfg: + hidden_sizes: + - 256 + - 256 + activation: elu + critic: + type: mlp + network_cfg: + hidden_sizes: + - 256 + - 256 + activation: elu + obs_groups: + actor: + - obs + critic: + - obs +algorithm: + name: ppo + cfg: + optimizer: + name: adam + learning_rate: 0.0003 + n_epochs: 4 + batch_size: 256 + gamma: 0.99 + gae_lambda: 0.95 + clip_coef: 0.2 + ent_coef: 0.01 + vf_coef: 0.5 + max_grad_norm: 1.0 diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/agents/ppo.mujoco_warp.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/agents/ppo.mujoco_warp.yaml new file mode 100644 index 0000000000..d5a1b77ccb --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/agents/ppo.mujoco_warp.yaml @@ -0,0 +1,53 @@ +trainer: + exp_name: shadow_pen_continuous_mujoco_warp + learning_env: + name: ShadowPenContinuousDexSim-v1 + cfg: + solver_kind: mujoco_warp + cuda_graph: true + seed: 42 + device: cuda:0 + num_envs: 32 + iterations: 10 + buffer_size: 24 + enable_eval: false + save_freq: 768 + use_wandb: false +policy: + name: actor_critic + actor_obs_normalization: true + critic_obs_normalization: true + initial_log_std: -0.693147 + actor: + type: mlp + network_cfg: + hidden_sizes: + - 256 + - 256 + activation: elu + critic: + type: mlp + network_cfg: + hidden_sizes: + - 256 + - 256 + activation: elu + obs_groups: + actor: + - obs + critic: + - obs +algorithm: + name: ppo + cfg: + optimizer: + name: adam + learning_rate: 0.0003 + n_epochs: 4 + batch_size: 256 + gamma: 0.99 + gae_lambda: 0.95 + clip_coef: 0.2 + ent_coef: 0.01 + vf_coef: 0.5 + max_grad_norm: 1.0 diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/agents/ppo.dexuni.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/agents/ppo.dexuni.yaml new file mode 100644 index 0000000000..306a3d2097 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/agents/ppo.dexuni.yaml @@ -0,0 +1,53 @@ +trainer: + exp_name: shadow_pen_fixed_dexuni + learning_env: + name: ShadowPenFixedDexSim-v1 + cfg: + solver_kind: dexuni + cuda_graph: true + seed: 42 + device: cuda:0 + num_envs: 32 + iterations: 10 + buffer_size: 24 + enable_eval: false + save_freq: 768 + use_wandb: false +policy: + name: actor_critic + actor_obs_normalization: true + critic_obs_normalization: true + initial_log_std: -0.693147 + actor: + type: mlp + network_cfg: + hidden_sizes: + - 256 + - 256 + activation: elu + critic: + type: mlp + network_cfg: + hidden_sizes: + - 256 + - 256 + activation: elu + obs_groups: + actor: + - obs + critic: + - obs +algorithm: + name: ppo + cfg: + optimizer: + name: adam + learning_rate: 0.0003 + n_epochs: 4 + batch_size: 256 + gamma: 0.99 + gae_lambda: 0.95 + clip_coef: 0.2 + ent_coef: 0.01 + vf_coef: 0.5 + max_grad_norm: 1.0 diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/agents/ppo.mujoco_warp.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/agents/ppo.mujoco_warp.yaml new file mode 100644 index 0000000000..cd71adda11 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/agents/ppo.mujoco_warp.yaml @@ -0,0 +1,53 @@ +trainer: + exp_name: shadow_pen_fixed_mujoco_warp + learning_env: + name: ShadowPenFixedDexSim-v1 + cfg: + solver_kind: mujoco_warp + cuda_graph: true + seed: 42 + device: cuda:0 + num_envs: 32 + iterations: 10 + buffer_size: 24 + enable_eval: false + save_freq: 768 + use_wandb: false +policy: + name: actor_critic + actor_obs_normalization: true + critic_obs_normalization: true + initial_log_std: -0.693147 + actor: + type: mlp + network_cfg: + hidden_sizes: + - 256 + - 256 + activation: elu + critic: + type: mlp + network_cfg: + hidden_sizes: + - 256 + - 256 + activation: elu + obs_groups: + actor: + - obs + critic: + - obs +algorithm: + name: ppo + cfg: + optimizer: + name: adam + learning_rate: 0.0003 + n_epochs: 4 + batch_size: 256 + gamma: 0.99 + gae_lambda: 0.95 + clip_coef: 0.2 + ent_coef: 0.01 + vf_coef: 0.5 + max_grad_norm: 1.0 diff --git a/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/_embodichain.py b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/_embodichain.py index 515a479006..604af6aa08 100644 --- a/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/_embodichain.py +++ b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/_embodichain.py @@ -108,6 +108,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": None, } for name, value in mapping.items(): term_cfg.params.setdefault(name, value) @@ -181,32 +182,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 +243,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. @@ -547,7 +553,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 +596,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 +638,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 +653,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_isaac_rough.py b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/g1_isaac_rough.py new file mode 100644 index 0000000000..b2c12ab642 --- /dev/null +++ b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/g1_isaac_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. +# ---------------------------------------------------------------------------- + +"""Isaac Lab 3.0 EA g1 velocity task on DexSim.""" + +from __future__ import annotations + +from embodichain.lab.gym.utils.registration import register_env +from .isaac.config import load_config +from .isaac.runtime import IsaacVelocityEnv + +__all__ = ["UnitreeG1IsaacRoughEnv"] + +_CONFIG = load_config("g1_isaac_rough") + + +@register_env( + "UnitreeG1IsaacRoughRL-v1", max_episode_steps=1000, override=True, supports_rl=True +) +class UnitreeG1IsaacRoughEnv(IsaacVelocityEnv): + """Run the reference g1 policy interface and rewards.""" + + 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/isaac/README.md b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/README.md new file mode 100644 index 0000000000..615e5950ba --- /dev/null +++ b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/README.md @@ -0,0 +1,31 @@ +# G1 rough-terrain velocity task + +The task controls 37 joints and exposes 310 policy observations, including 187 +terrain height samples. It uses the independent `UnitreeG1Rough` robot archive +and the complete `G1RoughTerrains` curriculum. The existing 29-joint +`UnitreeG1Locomotion` asset remains unchanged. + +The runnable deployment is +`extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/env.newton.yaml`. +It explicitly selects Newton contact generation with the MuJoCo Warp solver, +0.005 s outer physics steps, two solver substeps and four outer steps per +control interval. It retains the reference task's 100 solver iterations and +50 line-search iterations. + +Train through the unified CLI: + +```bash +embodichain train-rl --config extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/agents/ppo.newton.yaml +``` + +The default smoke configuration uses 64 environments and 20 updates. Increase +these fields for longer training. Archive availability and the matching +DexSim selected-link property APIs are required. The package loader supports +explicit G1 reference bundles for reproducible terrain and initial-state checks. +`ReferenceRslRlEnv` retains the previously validated RSL-RL interface. + +The native MuJoCo Warp heightfield collision path remains unsuitable for this +terrain because the retained official-runtime reproducer produces nonfinite +states. The default deployment therefore selects Newton contacts. DexUni's +G1 external-contact path still requires the separately retained dispatch +adapter; it is not enabled by this deployment. diff --git a/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/__init__.py b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/__init__.py new file mode 100644 index 0000000000..781fed98bf --- /dev/null +++ b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/__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. +# ---------------------------------------------------------------------------- + +"""Isaac reference velocity task computations and runtime adapter.""" + +from __future__ import annotations + +from .environment import load_reference_env_cfg + +__all__ = ["load_reference_env_cfg"] diff --git a/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/_frame.py b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/_frame.py new file mode 100644 index 0000000000..070f8f3610 --- /dev/null +++ b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/_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/isaac/config.py b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/config.py new file mode 100644 index 0000000000..4bc702bb05 --- /dev/null +++ b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/config.py @@ -0,0 +1,60 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Fixed Isaac Lab 3.0 EA velocity task settings.""" + +from __future__ import annotations + +from dataclasses import MISSING + +from embodichain.utils import configclass + +from ..contracts._config import load_task_data + +__all__ = ["IsaacVelocityConfig", "load_config"] + + +@configclass +class IsaacVelocityConfig: + """Policy dimensions, timing, model parameters and resolved reference 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) -> IsaacVelocityConfig: + """Load one packaged reference definition, retaining 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) + return IsaacVelocityConfig( + 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, + ) diff --git a/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/curriculum.py b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/curriculum.py new file mode 100644 index 0000000000..ddc923eba0 --- /dev/null +++ b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/curriculum.py @@ -0,0 +1,101 @@ +# ---------------------------------------------------------------------------- +# 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 matching Isaac G1 Rough.""" + +from __future__ import annotations + +import torch + +__all__ = ["ReferenceTerrainCurriculum"] + + +class ReferenceTerrainCurriculum: + """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 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/isaac/environment.py b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/environment.py new file mode 100644 index 0000000000..ddb648751e --- /dev/null +++ b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/environment.py @@ -0,0 +1,156 @@ +# ---------------------------------------------------------------------------- +# 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 reference environments from explicitly supplied 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_reference_env_cfg"] + + +def load_reference_env_cfg( + robot: str, + reference_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``. + reference_bundle: Directory containing the exported robot USDs and, + for G1, terrain.npz and terrain.json with evaluation origins or the + complete reference training terrain and curriculum definition. + num_envs: Independent physics worlds. + seed: Environment random seed. + training_randomization: Enable reference 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`` uses the EA reference 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. + FileNotFoundError: A required reference asset is missing. + """ + tasks = {"g1": "g1_isaac_rough"} + if robot not in tasks: + raise ValueError(f"Unknown reference robot: {robot!r}") + bundle = Path(reference_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.newton.yaml" + ) + data = yaml.safe_load(resource.read_text()) + data["num_envs"] = num_envs + data["robot"]["fpath"] = str(asset) + if use_mujoco_contacts is None: + use_mujoco_contacts = data["physics_config"]["solver_cfg"][ + "use_mujoco_contacts" + ] + data["physics_config"]["solver_cfg"]["use_mujoco_contacts"] = use_mujoco_contacts + if use_mujoco_contacts: + data["physics_config"]["collision_cfg"] = None + # The EA startup material event changes Newton mu to 0.8, while its native + # MJWarp model 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["reference_deterministic_reset"] = not training_randomization + extension["reference_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") != "isaac_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 reference 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 reference soft limits." + ) + extension["reference_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["reference_terrain"] = str(terrain) + metadata = json.loads((bundle / "terrain.json").read_text()) + if training_randomization: + if metadata.get("schema") != "isaac_velocity.rough_training_terrain@1": + raise ValueError( + "G1 training requires the complete reference Rough terrain bundle." + ) + extension["reference_terrain_course"] = metadata + else: + extension["reference_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/isaac/mdp.py b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/mdp.py new file mode 100644 index 0000000000..451a669d3e --- /dev/null +++ b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/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 the Isaac Lab 3.0 EA velocity task definitions.""" + +from __future__ import annotations + +from dataclasses import dataclass +from typing import TYPE_CHECKING + +import torch + +if TYPE_CHECKING: + from .config import IsaacVelocityConfig + +__all__ = [ + "IsaacVelocityState", + "IsaacVelocityReward", + "IsaacVelocityConstants", + "action_target", + "observation_terms", + "build_observation", + "compute_rewards", + "compute_termination", + "dc_motor_effort", + "update_contact_times", +] + + +@dataclass +class IsaacVelocityState: + """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 IsaacVelocityReward: + """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 IsaacVelocityConstants: + """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: IsaacVelocityConfig, reference: torch.Tensor + ) -> IsaacVelocityConstants: + """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: IsaacVelocityConfig, + action: torch.Tensor, + *, + constants: IsaacVelocityConstants | None = None, +) -> torch.Tensor: + """Apply the reference 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: IsaacVelocityConfig, + state: IsaacVelocityState, + *, + constants: IsaacVelocityConstants | None = None, +) -> dict[str, torch.Tensor]: + """Return reference 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: IsaacVelocityConfig, + state: IsaacVelocityState, + *, + noise: dict[str, torch.Tensor] | None = None, + constants: IsaacVelocityConstants | None = None, +) -> torch.Tensor: + """Process term noise, clipping and scaling in reference order. + + Args: + config: Fixed reference task definition. + state: Measured state tensors. + noise: Optional realized additive noise per term for cross-stack replay. + 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: IsaacVelocityConfig, state: IsaacVelocityState +) -> 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: IsaacVelocityConfig, + state: IsaacVelocityState, + terminated: torch.Tensor, + *, + constants: IsaacVelocityConstants | None = None, +) -> IsaacVelocityReward: + """Evaluate active reference 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 reference reward function: {function}") + raw[name] = value + weighted = { + name: value * config.data["rewards"][name]["weight"] * config.control_dt + for name, value in raw.items() + } + return IsaacVelocityReward( + 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/isaac/rsl_rl.py b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/rsl_rl.py new file mode 100644 index 0000000000..d6ee0b8ea6 --- /dev/null +++ b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/rsl_rl.py @@ -0,0 +1,97 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""RSL-RL 5.4 VecEnv interface for the reference task deployments.""" + +from __future__ import annotations + +from contextlib import contextmanager +from typing import TYPE_CHECKING, Any + +import torch +import warp as wp +from tensordict import TensorDict + +if TYPE_CHECKING: + from .runtime import IsaacVelocityEnv + +__all__ = ["ReferenceRslRlEnv"] + + +@contextmanager +def _simulation_stream(device: torch.device): + """Run a policy call on the simulation stream with ordered handoffs.""" + if device.type != "cuda": + yield + return + caller = torch.cuda.current_stream(device) + stream = wp.stream_to_torch(str(device)) + same_stream = caller.cuda_stream == stream.cuda_stream + if not same_stream: + stream.wait_stream(caller) + try: + with torch.cuda.stream(stream): + yield + finally: + if not same_stream: + caller.wait_stream(stream) + + +class ReferenceRslRlEnv: + """Expose reference policy observations and timeout bootstrapping to RSL-RL.""" + + def __init__(self, env: IsaacVelocityEnv) -> None: + self.env = env + self.num_envs = env.num_envs + self.num_actions = env.velocity_task_config.action_dim + self.max_episode_length = env.max_episode_steps + self.device = env.device + self.cfg = env.cfg + + @property + def episode_length_buf(self) -> torch.Tensor: + """Current control-step counters used by the RSL-RL runner.""" + return self.env._elapsed_steps + + @episode_length_buf.setter + def episode_length_buf(self, value: torch.Tensor) -> None: + self.env._elapsed_steps.copy_(value) + self.env._episode_step.copy_(value) + + def get_observations(self) -> TensorDict: + """Return the shared actor/critic reference policy group.""" + with _simulation_stream(self.device): + obs = self.env.get_obs() + return TensorDict({"policy": obs["policy"]}, batch_size=[self.num_envs]) + + def step( + self, actions: torch.Tensor + ) -> tuple[TensorDict, torch.Tensor, torch.Tensor, dict[str, Any]]: + """Forward unbounded actions and keep time limits separate from failures.""" + with _simulation_stream(self.device): + obs, rewards, terminated, truncated, info = self.env.step( + actions, policy_observations_only=True + ) + return ( + TensorDict({"policy": obs["policy"]}, batch_size=[self.num_envs]), + rewards, + terminated | truncated, + {"time_outs": truncated}, + ) + + def close(self) -> None: + """Close the owned simulation environment.""" + self.env.close() diff --git a/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/runtime.py b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/runtime.py new file mode 100644 index 0000000000..dec4140251 --- /dev/null +++ b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/runtime.py @@ -0,0 +1,697 @@ +# ---------------------------------------------------------------------------- +# 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 adapter for fixed Isaac velocity definitions.""" + +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 ( + IsaacVelocityState, + IsaacVelocityConstants, + build_observation, + observation_terms, + action_target, + compute_rewards, + compute_termination, + dc_motor_effort, + update_contact_times, +) +from .terrain import ReferenceHeightScanner +from .curriculum import ReferenceTerrainCurriculum + +__all__ = ["IsaacVelocityEnv"] + + +class IsaacVelocityEnv(EmbodiChainVelocityEnv): + """Run the reference policy interface on shared training or evaluation terrain. + + The benchmark disables startup mass/COM and interval pushes on both stacks. + ``reference_randomization=True`` enables those reference training events. + Training loads the complete reference Rough terrain and curriculum; fixed + scenes load evaluation origins. Both use the shared triangle mesh for + rays and Newton rasterization for collision. + """ + + state_type = IsaacVelocityState + 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.reference_terrain)) + if getattr(self.cfg, "reference_randomization", False) and not getattr( + self.cfg, "reference_terrain_course", None + ): + self.cfg.reference_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.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 = IsaacVelocityConstants.from_config( + config, self.locomotion_action + ) + self._reference_origin = self.locomotion_action.new_tensor( + getattr(self.cfg, "reference_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 = ReferenceHeightScanner( + *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.reference_initial_states], + device=self.device, + dtype=torch.float32, + ) + for name in ("root_pose", "root_velocity", "qpos", "qvel") + } + if getattr(self.cfg, "reference_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, "reference_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 = ReferenceTerrainCurriculum( + 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, "reference_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 unbounded reference targets through the Spawn batch state API.""" + self._position_target.copy_( + action_target( + self.velocity_task_config, + self.locomotion_action, + constants=self._reference_constants, + ) + ) + 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.""" + 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, 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, 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, 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) + # The EA native Go2 actuator path updates articulation data once per + # decimation block. G1 updates it after each individual 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 reference velocity pushes after rewards and episode resets.""" + if getattr(self.cfg, "reference_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_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 _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 + if self._reference_terrain_course is not None: + self._reference_terrain_course.update( + ids, self.robot.body_data.root_pose[:, :3], self.reward_command + ) + 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, "reference_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, "reference_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) -> IsaacVelocityState: + """Fetch policy-ordered reference 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] + ) -> IsaacVelocityState: + """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 IsaacVelocityState( + 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/isaac/terrain.py b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/terrain.py new file mode 100644 index 0000000000..fe59fd94cc --- /dev/null +++ b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/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 reference collision terrain.""" + +from __future__ import annotations + +import numpy as np +import newton +import torch +import warp as wp + +__all__ = ["ReferenceHeightScanner"] + + +@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 ReferenceHeightScanner: + """187 terrain rays with the reference torso frame, XY order and 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..a948cec2c5 --- /dev/null +++ b/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/__init__.py @@ -0,0 +1,18 @@ +# ---------------------------------------------------------------------------- +# 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 + +__all__: list[str] = [] diff --git a/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_audit_model.py b/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_audit_model.py new file mode 100644 index 0000000000..3c95f9520b --- /dev/null +++ b/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_audit_model.py @@ -0,0 +1,497 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""CPU comparison of source and solver-exported manipulation MJCF models.""" + +from __future__ import annotations + +from collections.abc import Mapping, Sequence +import re +from typing import Any + +import mujoco +import numpy as np + +__all__ = ["audit_model"] + + +def _rotation(quat: np.ndarray) -> np.ndarray: + matrix = np.empty(9) + mujoco.mju_quat2Mat(matrix, quat) + return matrix.reshape(3, 3) + + +def _contact_reason(model: Any, first: int, second: int) -> str: + """Return the MuJoCo static geom-pair filter, or ``eligible``.""" + if any( + {int(a), int(b)} == {first, second} + for a, b in zip(model.pair_geom1, model.pair_geom2) + ): + return "explicit_pair" + b1, b2 = int(model.geom_bodyid[first]), int(model.geom_bodyid[second]) + + def has_dynamic_ancestor(body: int) -> bool: + while body: + if model.body_dofnum[body]: + return True + body = int(model.body_parentid[body]) + return False + + if not has_dynamic_ancestor(b1) and not has_dynamic_ancestor(b2): + return "static_static" + w1, w2 = int(model.body_weldid[b1]), int(model.body_weldid[b2]) + if w1 == w2: + return "same_weld" + if not ( + int(model.opt.disableflags) & int(mujoco.mjtDisableBit.mjDSBL_FILTERPARENT) + ): + p1 = int(model.body_weldid[model.body_parentid[w1]]) + p2 = int(model.body_weldid[model.body_parentid[w2]]) + if w1 and w2 and (w1 == p2 or w2 == p1): + return "parent_child" + if not ( + int(model.geom_contype[first]) & int(model.geom_conaffinity[second]) + or int(model.geom_contype[second]) & int(model.geom_conaffinity[first]) + ): + return "collision_masks" + lo, hi = sorted((b1, b2)) + if (lo << 16) + hi in model.exclude_signature: + return "explicit_exclude" + return "eligible" + + +def audit_model( + source: Any, + imported: Any, + joint_map: Mapping[int, int], + actuator_map: Sequence[int], + body_map: Mapping[str, int], + *, + atol: float = 2e-6, + rtol: float = 2e-5, +) -> dict[str, Any]: + """Compare physical arrays after matching source entities to imported ones. + + Args: + source: Source MuJoCo model with runtime option overrides applied. + imported: MuJoCo model exported by the active Newton solver. + joint_map: Source joint index to imported joint index. + actuator_map: Imported actuator indices in source actuator order. + body_map: Source body names to imported body indices. + atol: Absolute numeric tolerance. + rtol: Relative numeric tolerance. + + Returns: + JSON-compatible checks, mismatches, collision mapping and option flags. + This function reports differences without silently changing the model. + """ + checks: list[dict[str, Any]] = [] + + def compare( + name: str, + expected: object, + actual: object, + *, + ignored_indices: Sequence[int] = (), + reason: str | None = None, + ) -> None: + a, b = np.asarray(expected), np.asarray(actual) + same_shape = a.shape == b.shape + raw_matches = bool(same_shape and np.allclose(a, b, atol=atol, rtol=rtol)) + semantic_a, semantic_b = a.copy(), b.copy() + if same_shape and ignored_indices: + semantic_a.flat[list(ignored_indices)] = 0 + semantic_b.flat[list(ignored_indices)] = 0 + difference = ( + float(np.max(np.abs(a.astype(float) - b.astype(float)))) + if same_shape and a.size + else 0.0 + ) + checks.append( + { + "field": name, + "matches": bool( + same_shape + and np.allclose(semantic_a, semantic_b, atol=atol, rtol=rtol) + ), + "raw_matches": raw_matches, + "equivalence_reason": ( + reason if not raw_matches and ignored_indices else None + ), + "max_abs_error": difference if same_shape else None, + "source_shape": list(a.shape), + "imported_shape": list(b.shape), + "source": a.tolist() if a.size <= 9 else None, + "imported": b.tolist() if b.size <= 9 else None, + } + ) + + for field in ( + "gravity", + "timestep", + "integrator", + "solver", + "iterations", + "ls_iterations", + "ccd_iterations", + "cone", + "impratio", + "tolerance", + "ls_tolerance", + "ccd_tolerance", + "enableflags", + "disableflags", + ): + compare( + f"option.{field}", getattr(source.opt, field), getattr(imported.opt, field) + ) + + bmap = {0: 0} + for source_id in range(1, source.nbody): + name = source.body(source_id).name + if name not in body_map: + checks.append( + {"field": f"body[{name or source_id}].mapping", "matches": False} + ) + continue + imported_id = body_map[name] + bmap[source_id] = imported_id + for field in ("body_mass", "body_ipos", "body_pos"): + ignored = ( + (0, 1, 2) + if ( + field == "body_ipos" + and source.body_mass[source_id] == 0 + and imported.body_mass[imported_id] == 0 + ) + else () + ) + compare( + f"body[{name}].{field}", + getattr(source, field)[source_id], + getattr(imported, field)[imported_id], + ignored_indices=ignored, + reason="Zero-mass body center of mass does not enter dynamics.", + ) + compare( + f"body[{name}].rotation", + _rotation(source.body_quat[source_id]), + _rotation(imported.body_quat[imported_id]), + ) + # Principal-axis eigenvectors may change sign/order while the tensor agrees. + ra, rb = _rotation(source.body_iquat[source_id]), _rotation( + imported.body_iquat[imported_id] + ) + compare( + f"body[{name}].inertia_tensor", + ra @ np.diag(source.body_inertia[source_id]) @ ra.T, + rb @ np.diag(imported.body_inertia[imported_id]) @ rb.T, + ) + + dof_map = [] + for source_id in range(source.njnt): + imported_id = joint_map[source_id] + name = source.joint(source_id).name or str(source_id) + for field in ( + "jnt_type", + "jnt_pos", + "jnt_axis", + "jnt_limited", + "jnt_range", + "jnt_actfrclimited", + "jnt_actfrcrange", + "jnt_solref", + "jnt_solimp", + "jnt_stiffness", + ): + ignored = () + if ( + field == "jnt_range" + and not source.jnt_limited[source_id] + and not imported.jnt_limited[imported_id] + ): + ignored = (0, 1) + if ( + field == "jnt_actfrcrange" + and not source.jnt_actfrclimited[source_id] + and not imported.jnt_actfrclimited[imported_id] + ): + ignored = (0, 1) + compare( + f"joint[{name}].{field}", + getattr(source, field)[source_id], + getattr(imported, field)[imported_id], + ignored_indices=ignored, + reason="Range is inactive because both limit flags are disabled.", + ) + width = (6, 3, 1, 1)[int(source.jnt_type[source_id])] + sa, ia = int(source.jnt_dofadr[source_id]), int( + imported.jnt_dofadr[imported_id] + ) + dof_map.extend(range(ia, ia + width)) + for field in ("dof_damping", "dof_armature", "dof_frictionloss"): + compare( + f"joint[{name}].{field}", + getattr(source, field)[sa : sa + width], + getattr(imported, field)[ia : ia + width], + ) + amap = np.asarray(actuator_map, dtype=int) + for field in ( + "actuator_gainprm", + "actuator_biasprm", + "actuator_gear", + "actuator_ctrlrange", + "actuator_ctrllimited", + "actuator_forcerange", + "actuator_forcelimited", + "actuator_trntype", + "actuator_dyntype", + "actuator_gaintype", + "actuator_biastype", + ): + compare(field, getattr(source, field), getattr(imported, field)[amap]) + + # Verify collision geoms by name first; source unnamed geoms use body-local + # type and position. Visual-only removals are expected from parse_visuals=False. + collision_map: dict[int, int] = {} + source_collision = np.flatnonzero(source.geom_contype | source.geom_conaffinity) + imported_collision = np.flatnonzero( + imported.geom_contype | imported.geom_conaffinity + ) + used: set[int] = set() + for sid in source_collision: + name = source.geom(int(sid)).name + candidates = [ + int(i) + for i in imported_collision + if int(i) not in used + and int(imported.geom_bodyid[i]) + == bmap.get(int(source.geom_bodyid[sid]), -1) + ] + named = [ + i + for i in candidates + if name + and ( + imported.geom(i).name == name + or imported.geom(i).name.endswith("_" + name) + or re.sub(r"_\d+$", "", imported.geom(i).name).split("/")[-1] == name + or re.sub(r"_\d+$", "", imported.geom(i).name).endswith("_" + name) + ) + ] + if len(named) == 1: + target = named[0] + else: + geometric = [ + i + for i in candidates + if imported.geom_type[i] == source.geom_type[sid] + and np.allclose( + imported.geom_pos[i], source.geom_pos[sid], atol=atol, rtol=rtol + ) + ] + if len(geometric) != 1: + checks.append( + { + "field": f"geom[{name or sid}].mapping", + "matches": False, + "candidates": candidates, + } + ) + continue + target = geometric[0] + used.add(target) + collision_map[int(sid)] = target + is_mesh = int(source.geom_type[sid]) == int(mujoco.mjtGeom.mjGEOM_MESH) + for field in ( + "geom_type", + "geom_pos", + "geom_size", + "geom_friction", + "geom_condim", + "geom_solref", + "geom_solimp", + "geom_margin", + "geom_gap", + "geom_priority", + "geom_solmix", + ): + ignored = () + reason = None + if is_mesh and field in ("geom_pos", "geom_size"): + ignored = (0, 1, 2) + reason = ( + "Mesh principal frame may change; body-frame vertices are checked." + ) + if field == "geom_size": + kind = int(source.geom_type[sid]) + if kind == int(mujoco.mjtGeom.mjGEOM_PLANE): + ignored, reason = ( + 0, + 1, + 2, + ), "Plane size only controls visualization." + elif kind in ( + int(mujoco.mjtGeom.mjGEOM_CAPSULE), + int(mujoco.mjtGeom.mjGEOM_CYLINDER), + ): + ignored, reason = ( + 2, + ), "Capsule/cylinder collision uses radius and half-length." + elif kind == int(mujoco.mjtGeom.mjGEOM_SPHERE): + ignored, reason = (1, 2), "Sphere collision uses only radius." + compare( + f"geom[{name or sid}].{field}", + getattr(source, field)[sid], + getattr(imported, field)[target], + ignored_indices=ignored, + reason=reason, + ) + compare( + f"geom[{name or sid}].rotation", + _rotation(source.geom_quat[sid]), + _rotation(imported.geom_quat[target]), + ignored_indices=tuple(range(9)) if is_mesh else (), + reason="Mesh principal frame may change; body-frame vertices are checked.", + ) + if is_mesh: + source_mesh, imported_mesh = int(source.geom_dataid[sid]), int( + imported.geom_dataid[target] + ) + source_start, imported_start = ( + source.mesh_vertadr[source_mesh], + imported.mesh_vertadr[imported_mesh], + ) + va = source.mesh_vert[ + source_start : source_start + source.mesh_vertnum[source_mesh] + ] + vb = imported.mesh_vert[ + imported_start : imported_start + imported.mesh_vertnum[imported_mesh] + ] + va = va @ _rotation(source.geom_quat[sid]).T + source.geom_pos[sid] + vb = ( + vb @ _rotation(imported.geom_quat[target]).T + imported.geom_pos[target] + ) + compare( + f"geom[{name or sid}].body_frame_mesh_bounds", + np.stack((va.min(0), va.max(0))), + np.stack((vb.min(0), vb.max(0))), + ) + # Newton can recenter/reorder the same convex mesh. Compare both + # point sets in the body frame without requiring matching indices. + distances = np.linalg.norm(va[:, None] - vb[None, :], axis=-1) + vertex_error = max( + float(distances.min(0).max()), float(distances.min(1).max()) + ) + checks.append( + { + "field": f"geom[{name or sid}].body_frame_mesh_vertices", + "matches": vertex_error <= atol, + "max_abs_error": vertex_error, + "source_vertex_count": len(va), + "imported_vertex_count": len(vb), + } + ) + compare("collision_geom_count", len(source_collision), len(imported_collision)) + source_excludes = { + tuple( + sorted( + ( + bmap.get(int(signature) >> 16, -1), + bmap.get(int(signature) & 0xFFFF, -1), + ) + ) + ) + for signature in source.exclude_signature + } + imported_excludes = { + tuple(sorted((int(signature) >> 16, int(signature) & 0xFFFF))) + for signature in imported.exclude_signature + } + pair_differences = [] + mapped = sorted(collision_map) + for offset, first in enumerate(mapped): + for second in mapped[offset + 1 :]: + expected = _contact_reason(source, first, second) + actual = _contact_reason( + imported, collision_map[first], collision_map[second] + ) + if (expected in ("eligible", "explicit_pair")) != ( + actual in ("eligible", "explicit_pair") + ): + pair_differences.append( + { + "source_geoms": [ + source.geom(first).name or first, + source.geom(second).name or second, + ], + "source_filter": expected, + "imported_filter": actual, + } + ) + inverse_bmap = {v: k for k, v in bmap.items()} + extra_excludes = [] + for first, second in sorted(imported_excludes - source_excludes): + sa, sb = inverse_bmap.get(first), inverse_bmap.get(second) + reasons: dict[str, int] = {} + if sa is not None and sb is not None: + for ga in np.flatnonzero(source.geom_bodyid == sa): + for gb in np.flatnonzero(source.geom_bodyid == sb): + why = _contact_reason(source, int(ga), int(gb)) + reasons[why] = reasons.get(why, 0) + 1 + extra_excludes.append( + { + "imported_bodies": [first, second], + "source_bodies": [ + source.body(sa).name if sa is not None else None, + source.body(sb).name if sb is not None else None, + ], + "source_geom_pair_filters": reasons, + "redundant": sa is not None + and sb is not None + and not reasons.get("eligible", 0), + } + ) + checks.append( + { + "field": "contact_excludes", + "matches": not pair_differences + and len(collision_map) == len(source_collision), + "raw_matches": source_excludes == imported_excludes, + "missing_source_pairs": sorted(source_excludes - imported_excludes), + "additional_imported_pairs": sorted(imported_excludes - source_excludes), + "additional_imported_pair_details": extra_excludes, + "effective_geom_pair_differences": pair_differences, + "equivalence_reason": "Static geom-pair eligibility includes explicit pairs, welds, parent filtering, masks and excludes.", + } + ) + mismatches = [item for item in checks if not item["matches"]] + return { + "status": "passed" if not mismatches else "differences", + "atol": atol, + "rtol": rtol, + "check_count": len(checks), + "mismatches": mismatches, + "checks": checks, + "equivalent_differences": [ + item + for item in checks + if item["matches"] and not item.get("raw_matches", True) + ], + "collision_geom_map": collision_map, + "source_disableflags": int(source.opt.disableflags), + "imported_disableflags": int(imported.opt.disableflags), + } diff --git a/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_audit_tendons.py b/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_audit_tendons.py new file mode 100644 index 0000000000..48191481d6 --- /dev/null +++ b/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_audit_tendons.py @@ -0,0 +1,186 @@ +# ---------------------------------------------------------------------------- +# 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 fixed-tendon topology and physical properties across MJCF export.""" + +from __future__ import annotations + +from collections.abc import Mapping +from typing import Any + +import mujoco +import numpy as np + +__all__ = ["audit_tendons"] + + +def audit_tendons( + source: Any, + imported: Any, + joint_map: Mapping[int, int], + *, + atol: float = 2e-6, + rtol: float = 2e-5, +) -> dict[str, Any]: + """Compare every fixed tendon after mapping joint indices. + + Args: + source: Source MuJoCo model. + imported: MuJoCo model exported by the active solver. + joint_map: Source joint index to imported joint index. + atol: Absolute numeric tolerance. + rtol: Relative numeric tolerance. + + Returns: + JSON-compatible per-tendon address ranges, ordered joint/coefficient + records, physical-property checks, and the overall status. A non-joint + wrap is reported as unsupported instead of being treated as equivalent. + """ + rows: list[dict[str, Any]] = [] + joint_wrap = int(mujoco.mjtWrap.mjWRAP_JOINT) + imported_names = [imported.tendon(i).name for i in range(imported.ntendon)] + used: set[int] = set() + for sid in range(source.ntendon): + name = source.tendon(sid).name + candidates = [ + iid + for iid, actual_name in enumerate(imported_names) + if iid not in used + and ( + actual_name == name + or actual_name.endswith("_" + name) + or actual_name.endswith("/" + name) + ) + ] + if len(candidates) != 1: + rows.append( + { + "name": name, + "matches": False, + "mapping_error": candidates, + } + ) + continue + iid = candidates[0] + used.add(iid) + sa, ia = int(source.tendon_adr[sid]), int(imported.tendon_adr[iid]) + sn, inn = int(source.tendon_num[sid]), int(imported.tendon_num[iid]) + source_path, imported_path = [], [] + supported = True + for model, start, count, destination, remap in ( + (source, sa, sn, source_path, True), + (imported, ia, inn, imported_path, False), + ): + for wid in range(start, start + count): + kind = int(model.wrap_type[wid]) + target = int(model.wrap_objid[wid]) + if kind != joint_wrap: + supported = False + destination.append( + { + "wrap_index": wid, + "wrap_type": kind, + "joint_index": target, + "joint_name": ( + model.joint(target).name if kind == joint_wrap else None + ), + "mapped_joint_index": ( + joint_map.get(target, -1) + if remap and kind == joint_wrap + else target + ), + "coefficient": float(model.wrap_prm[wid]), + } + ) + topology_matches = ( + supported + and sn == inn + and all( + expected["mapped_joint_index"] == actual["mapped_joint_index"] + and expected["wrap_type"] == actual["wrap_type"] + and np.isclose( + expected["coefficient"], + actual["coefficient"], + atol=atol, + rtol=rtol, + ) + for expected, actual in zip(source_path, imported_path) + ) + ) + properties = [] + for field in ( + "tendon_limited", + "tendon_range", + "tendon_stiffness", + "tendon_damping", + "tendon_frictionloss", + "tendon_margin", + "tendon_solref_lim", + "tendon_solimp_lim", + "tendon_solref_fri", + "tendon_solimp_fri", + "tendon_lengthspring", + "tendon_armature", + "tendon_actfrclimited", + "tendon_actfrcrange", + ): + expected = np.asarray(getattr(source, field)[sid]) + actual = np.asarray(getattr(imported, field)[iid]) + properties.append( + { + "field": field, + "matches": bool( + np.allclose(expected, actual, atol=atol, rtol=rtol) + ), + "max_abs_error": float( + np.max(np.abs(expected.astype(float) - actual.astype(float))) + ), + "source": expected.tolist(), + "imported": actual.tolist(), + } + ) + rows.append( + { + "name": name, + "source_index": sid, + "imported_index": iid, + "source_address_range": [sa, sa + sn], + "imported_address_range": [ia, ia + inn], + "fixed_joint_wraps_supported": supported, + "source_path": source_path, + "imported_path": imported_path, + "topology_matches": bool(topology_matches), + "properties": properties, + "matches": bool( + topology_matches and all(item["matches"] for item in properties) + ), + } + ) + matches = ( + source.ntendon == imported.ntendon + and len(used) == imported.ntendon + and all(row["matches"] for row in rows) + ) + return { + "status": "passed" if matches else "differences", + "source_count": int(source.ntendon), + "imported_count": int(imported.ntendon), + "matched_count": len(used), + "atol": atol, + "rtol": rtol, + "tendons": rows, + "mismatch_names": [row["name"] for row in rows if not row["matches"]], + } diff --git a/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_dexuni_adapter.py b/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_dexuni_adapter.py new file mode 100644 index 0000000000..17607f3be1 --- /dev/null +++ b/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_dexuni_adapter.py @@ -0,0 +1,230 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- +"""Select DexUni's rigid articulation branch and translate its reset state.""" + +from __future__ import annotations + +import copy +import types +from typing import Any + +import newton +import numpy as np +import torch +import warp as wp +from newton import Model + +__all__ = [ + "import_dexuni_mjcf", + "select_all_articulations", + "refresh_collision_filters", + "DexUniStateAdapter", +] + + +def import_dexuni_mjcf(builder: Any, path: str) -> dict[str, Any]: + """Parse MJCF after DexUni has registered its private solver attributes. + + Newton 1.7's parser registers standalone solver callbacks, which conflict + with DexUni's callbacks. A function-local globals copy selects the private + class while retaining the parser code and flags. Parser-only attributes + absent from the private schema are copied from a fresh public builder. + """ + from newton._src.solvers.mjvbd_v2.mujoco.solver_mujoco import ( + SolverMuJoCo as PrivateMuJoCo, + ) + from newton._src.utils.import_mjcf import parse_mjcf + + reference = newton.ModelBuilder() + newton.solvers.SolverMuJoCo.register_custom_attributes(reference) + additions = [] + for name, attribute in reference.custom_attributes.items(): + if name not in builder.custom_attributes: + builder.add_custom_attribute(copy.deepcopy(attribute)) + additions.append(name) + parser = types.FunctionType( + parse_mjcf.__code__, + {**parse_mjcf.__globals__, "SolverMuJoCo": PrivateMuJoCo}, + parse_mjcf.__name__, + parse_mjcf.__defaults__, + parse_mjcf.__closure__, + ) + parser.__kwdefaults__ = parse_mjcf.__kwdefaults__ + parser( + builder, + path, + ctrl_direct=True, + parse_visuals=False, + parse_sites=True, + collapse_fixed_joints=False, + ) + empty_references = {} + for kind in ("joint", "wrap"): + addresses = builder.custom_attributes[f"mujoco:tendon_{kind}_adr"].values + counts = builder.custom_attributes[f"mujoco:tendon_{kind}_num"].values + changed = 0 + for index, count in enumerate(counts): + if count == 0 and addresses[index] != -1: + # A zero-length range has no referenced row. Keeping address + # zero makes the compact view reject fixed tendons when the + # spatial-wrap domain is empty. + addresses[index] = -1 + changed += 1 + empty_references[kind] = changed + return { + "schema_additions": additions, + "empty_tendon_reference_corrections": empty_references, + "parser_global_state_modified": False, + } + + +def select_all_articulations(backend: Any) -> None: + """Assign every imported articulation, including free objects, to DexUni. + + Call after adding all worlds and before finalizing the DexSim backend. + DexUni's pure_mujoco dispatch then keeps hand-object contacts and tendons + within one private solver. A standalone MJWarp solver is never constructed. + """ + if backend.solver_type != "dexuni": + raise ValueError("Expected a DexUni backend") + count = int(backend.builder.articulation_count) + if count <= 0: + raise ValueError("The manipulation scene contains no articulations") + backend.cfg.solver_cfg.mujoco_articulations = tuple(range(count)) + + +def refresh_collision_filters(solver: Any) -> dict[str, Any]: + """Rebuild collision caches after restoring host MJCF contact masks. + + MuJoCo Warp precomputes eligible pairs and collision-kernel type counts in + ``put_model``. Copy these derived fields while retaining the existing + world-expanded dynamics arrays and data allocations. + """ + model = solver.mj_model + for body_field, geom_field in ( + ("body_contype", "geom_contype"), + ("body_conaffinity", "geom_conaffinity"), + ): + values = getattr(model, body_field) + values[:] = 0 + np.bitwise_or.at(values, model.geom_bodyid, getattr(model, geom_field)) + previous = solver.mjw_model.nxn_geom_pair_filtered.numpy() + with wp.ScopedDevice(solver.model.device): + fresh = solver._mujoco_warp.put_model(model) + updated = fresh.nxn_geom_pair_filtered.numpy() + old_pairs = set(map(tuple, previous.tolist())) + new_pairs = set(map(tuple, updated.tolist())) + added = sorted(new_pairs - old_pairs) + if added: + raise ValueError( + "Source collision-mask restoration adds pairs; existing contact " + f"allocations require a separate capacity check: {added[:8]}" + ) + fields = ( + "geom_contype", + "geom_conaffinity", + "body_contype", + "body_conaffinity", + "nxn_geom_pair", + "nxn_geom_pair_filtered", + "nxn_pairid", + "nxn_pairid_filtered", + "geom_pair_type_count", + ) + for field in fields: + setattr(solver.mjw_model, field, getattr(fresh, field)) + solver.mjw_model.opt.broadphase = fresh.opt.broadphase + return { + "status": "refreshed", + "fields": list(fields) + ["opt.broadphase"], + "eligible_pairs_before": len(old_pairs), + "eligible_pairs_after": len(new_pairs), + "removed_pairs": len(old_pairs - new_pairs), + "added_pairs": len(added), + } + + +class DexUniStateAdapter: + """Scatter private MuJoCo state into the owning DexSim state buffers. + + The compact-to-scene maps come from NewtonBackend's public facade. Physical + stepping remains owned by ``backend.step`` and the outer ``SolverDexUni``. + """ + + def __init__(self, backend: Any) -> None: + outer = backend.solver + if backend.solver_type != "dexuni" or type(outer).__name__ != "SolverDexUni": + raise TypeError("Requested DexUni did not create SolverDexUni") + if outer.features.backend != "pure_mujoco": + raise ValueError( + f"Expected DexUni pure_mujoco dispatch, got {outer.features.backend}" + ) + if outer.features.vbd_solve_enabled or not outer.features.mujoco_solve_enabled: + raise ValueError("Unexpected active DexUni solver branches") + self.backend = backend + self.solver = backend.mujoco_solver + if self.solver is None or not hasattr(self.solver, "mjw_data"): + raise TypeError("DexUni has no active GPU MuJoCo data") + if ".mjvbd_v2.mujoco." not in type(self.solver).__module__: + raise TypeError("DexUni did not select its private MuJoCo implementation") + self.local_state = self.solver.model.state() + parent = backend.model + joint_world = wp.to_torch(parent.joint_world).long() + coord_world = torch.repeat_interleave( + joint_world, torch.diff(wp.to_torch(parent.joint_q_start)).long() + ) + dof_world = torch.repeat_interleave( + joint_world, torch.diff(wp.to_torch(parent.joint_qd_start)).long() + ) + body_world = wp.to_torch(parent.body_world).long() + frequencies = ( + ("joint_q", Model.AttributeFrequency.JOINT_COORD, coord_world), + ("joint_qd", Model.AttributeFrequency.JOINT_DOF, dof_world), + ("body_q", Model.AttributeFrequency.BODY, body_world), + ("body_qd", Model.AttributeFrequency.BODY, body_world), + ) + self.fields = [] + for name, frequency, worlds in frequencies: + mapping = backend.get_mujoco_index_map(frequency) + if mapping is None: + raise ValueError(f"Missing DexUni {name} compact-to-scene map") + indices = wp.to_torch(mapping).long() + local = wp.to_torch(getattr(self.local_state, name)) + if local.shape[0] != indices.numel(): + raise ValueError(f"DexUni {name} state and index map disagree") + self.fields.append((name, indices, worlds[indices])) + + def publish_reset(self, data: Any, env_ids: torch.Tensor) -> None: + """Publish selected MJCF coordinate resets into both scene buffers.""" + selected = [ + (name, indices, torch.isin(worlds, env_ids)) + for name, indices, worlds in self.fields + ] + for parent_state in (self.backend.state_0, self.backend.state_1): + for name, indices, _ in selected: + wp.to_torch(getattr(self.local_state, name)).copy_( + wp.to_torch(getattr(parent_state, name))[indices] + ) + self.solver._update_newton_state( + self.solver.model, + self.local_state, + data, + state_prev=self.local_state, + ) + for name, indices, mask in selected: + wp.to_torch(getattr(parent_state, name))[indices[mask]] = wp.to_torch( + getattr(self.local_state, name) + )[mask] 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..932de53899 --- /dev/null +++ b/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_runtime.py @@ -0,0 +1,697 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Batched manipulation environments stepped by the DexSim Newton backend.""" + +from __future__ import annotations + +import copy +from pathlib import Path +import re +from typing import Any + +import gymnasium as gym +import mujoco +import numpy as np +import torch +import warp as wp + +import dexsim +from embodichain.data import get_data_path +from dexsim.engine.newton_physics import DexUniSolverCfg, MJWarpSolverCfg, NewtonCfg +from dexsim.engine.newton_physics.backend_registry import get_newton_backend + +from ._shadow_task import ShadowState, ShadowTask + +__all__ = ["DexSimManipulationEnv", "ENV_NAMES"] + +ENV_NAMES = { + "M1": "ShadowPenFixedDexSim-v1", + "M2": "ShadowPenContinuousDexSim-v1", + "M3": "LeapCubeFaceTurnDexSim-v1", + "M4": "LeapCubeReorientDexSim-v1", + "M5": "LeapCubeRotateZAxisDexSim-v1", +} + + +def _named_index(model: Any, kind: str, name: str) -> int: + count = getattr(model, "n" + ("jnt" if kind == "joint" else kind)) + candidates = [ + i + for i in range(count) + if getattr(model, kind)(i).name == name + or getattr(model, kind)(i).name.endswith("_" + name) + or ( + kind == "geom" + and re.sub(r"_\d+$", "", getattr(model, kind)(i).name).split("/")[-1] + == name + ) + ] + if len(candidates) != 1: + raise ValueError(f"Cannot resolve {kind} {name!r}: {candidates}") + return candidates[0] + + +def rotate_wxyz(q: torch.Tensor, v: torch.Tensor) -> torch.Tensor: + xyz = q[..., 1:] + t = 2 * torch.cross(xyz, v, dim=-1) + return v + q[..., :1] * t + torch.cross(xyz, t, dim=-1) + + +class DexSimManipulationEnv: + """Expose real DexSim physics through EmbodiChain's vector-env interface. + + The task-local MJCF adapter uses the World-owned backend to retain tendons, + passive joints and direct actuators. Native stepping belongs to DexSim; + MuJoCo forward calls below only update kinematics after explicit reset. + """ + + def __init__( + self, + task_id: str, + num_envs: int = 32, + device: str = "cuda:0", + seed: int = 42, + cuda_graph: bool = True, + solver_kind: str = "mujoco_warp", + diagnostics: bool = True, + solver_iterations: tuple[int, int] | None = None, + ) -> None: + if solver_kind not in ("mujoco_warp", "dexuni"): + raise ValueError(f"Unknown manipulation solver: {solver_kind}") + self.solver_kind = solver_kind + self.diagnostics = diagnostics + device = str(torch.device(device)) + self.task_id = task_id + self.num_envs = num_envs + self.device = torch.device(device) + torch.manual_seed(seed) + if task_id in ("M1", "M2"): + self.task = ShadowTask.from_assets( + Path(get_data_path("ShadowHandPen/shadow_pen.xml")).parent, + num_envs, + variant=task_id, + device=device, + ) + xml = Path(get_data_path("ShadowHandPen/shadow_pen.xml")) + self.ctrl_dt, self.sim_dt = 0.04, 0.002 + self.observation_dim, self.critic_dim, self.action_dim = 87, 87, 18 + else: + from ._leap_tasks import LeapTask + + if task_id == "M3": + xml = Path( + get_data_path("LeapHandCubeFaceTurn/leap_cube_top_layer.xml") + ) + initial = np.load( + get_data_path("LeapHandCubeFaceTurn/LeapCubeFaceTurn-initial.npz") + ) + else: + xml = Path(get_data_path(f"LeapHandCube/{task_id}.xml")) + initial = np.load(get_data_path(f"LeapHandCube/{task_id}-initial.npz")) + source = mujoco.MjModel.from_xml_path(str(xml)) + self.task = LeapTask( + { + "M3": "LeapCubeFaceTurn", + "M4": "LeapCubeReorient", + "M5": "LeapCubeRotateZAxis", + }[task_id], + num_envs, + initial["qpos"], + initial["ctrl"], + source.actuator_ctrlrange[:, 0], + source.actuator_ctrlrange[:, 1], + device=device, + seed=seed, + ) + self.ctrl_dt, self.sim_dt = 0.05, (0.005 if task_id == "M3" else 0.01) + self.observation_dim = {"M3": 65, "M4": 57, "M5": 32}[task_id] + self.critic_dim = {"M3": 65, "M4": 128, "M5": 105}[task_id] + self.action_dim = 16 + self.source = mujoco.MjModel.from_xml_path(str(xml)) + self.source.opt.timestep = self.sim_dt + if task_id in ("M1", "M2", "M3"): + self.source.opt.ccd_iterations = 10 + self.xml = xml + self.substeps = round(self.ctrl_dt / self.sim_dt) + observation_spaces = { + "obs": gym.spaces.Box( + -np.inf, np.inf, (self.observation_dim,), dtype=np.float32 + ) + } + if task_id in ("M4", "M5"): + observation_spaces["critic_obs"] = gym.spaces.Box( + -np.inf, np.inf, (self.critic_dim,), dtype=np.float32 + ) + self.single_observation_space = gym.spaces.Dict(observation_spaces) + self.single_action_space = gym.spaces.Box( + -1, 1, (self.action_dim,), dtype=np.float32 + ) + self.action_space = self.single_action_space + self.observation_space = self.single_observation_space + self.action_format = "tensor" + self.elapsed_steps = torch.zeros(num_envs, dtype=torch.long, device=device) + self._episode_return = torch.zeros(num_envs, device=device) + self._last_observation: dict[str, torch.Tensor] | None = None + self._closed = False + config = dexsim.WorldConfig() + config.open_windows = False + config.backend = dexsim.types.Backend.VULKAN + config.renderer = dexsim.types.Renderer.NORENDER + config.init_ground = False + config.cache_path = str(Path.home() / ".cache/embodichain/dexterous") + solver_options = { + "use_mujoco_contacts": True, + "iterations": int(self.source.opt.iterations), + "ls_iterations": int(self.source.opt.ls_iterations), + "ccd_iterations": int(self.source.opt.ccd_iterations), + "integrator": int(self.source.opt.integrator), + "solver": int(self.source.opt.solver), + "cone": ("pyramidal", "elliptic")[int(self.source.opt.cone)], + "impratio": float(self.source.opt.impratio), + "tolerance": float(self.source.opt.tolerance), + "ls_tolerance": float(self.source.opt.ls_tolerance), + "ccd_tolerance": float(self.source.opt.ccd_tolerance), + "njmax": 512, + "nconmax": 256, + } + self.backend_option_overrides = {} + if solver_iterations is not None or solver_kind == "dexuni": + selected_iterations = solver_iterations or (20, 50) + for name, value in zip( + ("iterations", "ls_iterations"), selected_iterations + ): + self.backend_option_overrides[name] = { + "source": solver_options[name], + "configured": value, + "reason": ( + "Explicit benchmark settings" + if solver_iterations is not None + else "DexUni configuration shared with Unitree validation." + ), + } + solver_options[name] = value + if solver_kind == "dexuni": + solver_cfg = DexUniSolverCfg(mujoco_options=solver_options) + else: + solver_cfg = MJWarpSolverCfg(**solver_options) + config.newton_cfg = NewtonCfg( + device=device, + gravity=self.source.opt.gravity.tolist(), + dt=self.sim_dt, + num_substeps=1, + sync_to_dexsim=False, + solver_cfg=solver_cfg, + ) + config.newton_cfg.use_cuda_graph = cuda_graph + self.world = dexsim.World(config) + self.world.set_manual_update(True) + self.backend = get_newton_backend(self.world) + template = self.backend._create_builder() + template.default_shape_cfg.gap = 0.0 + self.mjcf_import_adapter = {} + if solver_kind == "dexuni": + from ._dexuni_adapter import import_dexuni_mjcf + + self.mjcf_import_adapter = import_dexuni_mjcf(template, str(xml)) + else: + template.add_mjcf( + str(xml), + ctrl_direct=True, + parse_visuals=False, + parse_sites=True, + collapse_fixed_joints=False, + ) + for _ in range(num_envs): + self.backend.builder.add_world(template) + if solver_kind == "dexuni": + from ._dexuni_adapter import select_all_articulations + + select_all_articulations(self.backend) + self.backend.finalize() + self.solver = self.backend.solver + self.mujoco_solver = self.backend.mujoco_solver + self.dexuni_adapter = None + if solver_kind == "dexuni": + from ._dexuni_adapter import DexUniStateAdapter + + self.dexuni_adapter = DexUniStateAdapter(self.backend) + self.imported = self.mujoco_solver.mj_model + self.data = self.mujoco_solver.mjw_data + self.option_import_corrections = { + name: { + "source": float(getattr(self.source.opt, name)), + "exported": float(getattr(self.imported.opt, name)), + } + for name in ("timestep", "disableflags", "enableflags") + if getattr(self.source.opt, name) != getattr(self.imported.opt, name) + } + for name in ("disableflags", "enableflags"): + value = int(getattr(self.source.opt, name)) + setattr(self.imported.opt, name, value) + setattr(self.mujoco_solver.mjw_model.opt, name, value) + self.imported.opt.timestep = self.sim_dt + wp.to_torch(self.mujoco_solver.mjw_model.opt.timestep).fill_(self.sim_dt) + self.backend.invalidate_cuda_graph("source MJCF options restored") + self._make_mapping() + self.reset(seed=seed) + + @property + def unwrapped(self) -> DexSimManipulationEnv: + return self + + def _make_mapping(self) -> None: + source, imported = self.source, self.imported + for name in ("nq", "nv", "nu", "ntendon"): + if getattr(source, name) != getattr(imported, name): + raise ValueError( + f"MJCF {name} changed: {getattr(source, name)} -> {getattr(imported, name)}" + ) + qmap, vmap = [], [] + joint_map = {} + for j in range(source.njnt): + name = source.joint(j).name + if name: + target = _named_index(imported, "joint", name) + else: + body = _named_index( + imported, "body", source.body(int(source.jnt_bodyid[j])).name + ) + candidates = np.flatnonzero(imported.jnt_bodyid == body) + if len(candidates) != 1: + raise ValueError("Unnamed joint cannot be resolved unambiguously") + target = int(candidates[0]) + joint_map[j] = target + width = (7, 4, 1, 1)[int(source.jnt_type[j])] + vwidth = (6, 3, 1, 1)[int(source.jnt_type[j])] + qmap.extend( + range( + int(imported.jnt_qposadr[target]), + int(imported.jnt_qposadr[target]) + width, + ) + ) + vmap.extend( + range( + int(imported.jnt_dofadr[target]), + int(imported.jnt_dofadr[target]) + vwidth, + ) + ) + amap = [] + for a in range(source.nu): + source_joint = int(source.actuator_trnid[a, 0]) + candidates = np.flatnonzero( + imported.actuator_trnid[:, 0] == joint_map[source_joint] + ) + if len(candidates) != 1: + raise ValueError(f"Actuator {a} mapping ambiguous: {candidates}") + amap.append(int(candidates[0])) + self.qmap = torch.tensor(qmap, device=self.device) + self.vmap = torch.tensor(vmap, device=self.device) + self.amap = torch.tensor(amap, device=self.device) + # Native direct-control slots retain source import order, whereas the + # solver may reorder actuators in its compiled MuJoCo model. + self.ctrl = wp.to_torch(self.backend.control.mujoco.ctrl).reshape( + self.num_envs, -1 + ) + self.model_audit = { + "source": str(self.xml), + "mjcf_import_adapter": self.mjcf_import_adapter, + "backend_option_overrides": self.backend_option_overrides, + "solver_selection": { + "requested": self.solver_kind, + "actual_backend": self.backend.solver_type, + "actual_solver_class": ( + f"{type(self.solver).__module__}.{type(self.solver).__name__}" + ), + "mujoco_branch_class": ( + f"{type(self.mujoco_solver).__module__}." + f"{type(self.mujoco_solver).__name__}" + ), + "diagnostics": self.backend.get_solver_diagnostics(), + "standalone_mjwarp_fallback": False, + }, + "option_import_corrections": self.option_import_corrections, + "counts": { + k: int(getattr(source, k)) for k in ("nq", "nv", "nu", "ntendon") + }, + "source_reference_counts": { + k: int(getattr(source, k)) for k in ("nsite", "nsensor") + }, + "imported_reference_counts": { + k: int(getattr(imported, k)) for k in ("nsite", "nsensor") + }, + "observation_state_source": ( + "Named qpos/qvel, body poses, source-local site transforms, " + "and actuator forces; imported sensor channels are not consumed." + ), + "qpos_map": qmap, + "qvel_map": vmap, + "actuator_map": amap, + "gain_max_abs_error": float( + np.max( + np.abs(source.actuator_gainprm - imported.actuator_gainprm[amap]) + ) + ), + "bias_max_abs_error": float( + np.max( + np.abs(source.actuator_biasprm - imported.actuator_biasprm[amap]) + ) + ), + "tendon_range_max_abs_error": ( + float(np.max(np.abs(source.tendon_range - imported.tendon_range))) + if source.ntendon + else 0.0 + ), + "solver_options": { + key: float(getattr(imported.opt, key)) + for key in ( + "timestep", + "iterations", + "ls_iterations", + "ccd_iterations", + "integrator", + "solver", + "cone", + "impratio", + ) + }, + } + if ( + max( + self.model_audit[k] + for k in ( + "gain_max_abs_error", + "bias_max_abs_error", + "tendon_range_max_abs_error", + ) + ) + > 1e-5 + ): + raise ValueError( + f"Imported actuator/tendon properties changed: {self.model_audit}" + ) + self.body_map = { + source.body(i).name: _named_index(imported, "body", source.body(i).name) + for i in range(1, source.nbody) + if source.body(i).name + } + # Newton 1.7 does not export free-joint damping and adds finite force + # limits to originally unlimited joints. Preserve the source arrays in + # both MuJoCo representations before any captured physical step. + corrections = [] + correction_fields = [ + ("dof_damping", vmap), + ("jnt_actfrclimited", [joint_map[i] for i in range(source.njnt)]), + ("jnt_actfrcrange", [joint_map[i] for i in range(source.njnt)]), + ] + if self.solver_kind == "dexuni": + correction_fields.append( + ("jnt_solref", [joint_map[i] for i in range(source.njnt)]) + ) + for field, indices in correction_fields: + expected = getattr(source, field) + actual = getattr(imported, field) + difference = float(np.max(np.abs(expected.astype(float) - actual[indices]))) + if difference: + corrections.append({"field": field, "max_abs_error_before": difference}) + actual[indices] = expected + target = wp.to_torch(getattr(self.mujoco_solver.mjw_model, field)) + values = torch.as_tensor( + expected, device=self.device, dtype=target.dtype + ) + if field == "jnt_actfrclimited": + target[indices] = values + else: + target[:, indices] = values + for geom_id in range(source.ngeom): + name = source.geom(geom_id).name + if not name or not ( + source.geom_contype[geom_id] or source.geom_conaffinity[geom_id] + ): + continue + try: + imported_id = _named_index(imported, "geom", name) + except ValueError: + continue + if ( + abs(source.geom_margin[geom_id] - imported.geom_margin[imported_id]) + > 1e-8 + ): + corrections.append( + { + "field": f"geom[{name}].margin", + "source": float(source.geom_margin[geom_id]), + "exported": float(imported.geom_margin[imported_id]), + } + ) + imported.geom_margin[imported_id] = source.geom_margin[geom_id] + wp.to_torch(self.mujoco_solver.mjw_model.geom_margin)[ + :, imported_id + ] = source.geom_margin[geom_id] + self.model_audit["property_import_corrections"] = corrections + from ._audit_model import audit_model + + audit_source = source + if self.backend_option_overrides: + audit_source = copy.copy(source) + for name, item in self.backend_option_overrides.items(): + setattr(audit_source.opt, name, item["configured"]) + if self.solver_kind == "dexuni": + from ._dexuni_adapter import refresh_collision_filters + + mapping_audit = audit_model( + audit_source, imported, joint_map, amap, self.body_map + ) + for source_id, imported_id in mapping_audit["collision_geom_map"].items(): + for field in ("geom_contype", "geom_conaffinity"): + before = int(getattr(imported, field)[imported_id]) + expected = int(getattr(source, field)[source_id]) + if before != expected: + corrections.append( + { + "field": f"geom[{source.geom(source_id).name}].{field}", + "source": expected, + "exported": before, + } + ) + getattr(imported, field)[imported_id] = expected + self.model_audit["collision_cache_refresh"] = refresh_collision_filters( + self.mujoco_solver + ) + self.backend.invalidate_cuda_graph("source collision filters restored") + self.model_audit["physical_model"] = audit_model( + audit_source, imported, joint_map, amap, self.body_map + ) + self.model_audit["physical_model"]["option_reference"] = ( + "Source model with explicit backend_option_overrides." + if self.backend_option_overrides + else "Source model." + ) + from ._audit_tendons import audit_tendons + + self.model_audit["tendons"] = audit_tendons(source, imported, joint_map) + if self.model_audit["tendons"]["status"] != "passed": + raise ValueError(f"Imported tendons differ: {self.model_audit['tendons']}") + if self.model_audit["physical_model"]["status"] != "passed": + raise ValueError( + f"Imported physical model differs: {self.model_audit['physical_model']['mismatches']}" + ) + if self.task_id == "M3": + labels = self.backend.model.joint_label + joints = [ + i + for i, label in enumerate(labels) + if label == "face_turn" + or label.endswith("/face_turn") + or label.endswith("_face_turn") + ] + if len(joints) != self.num_envs: + raise ValueError(f"Expected one passive face joint per world: {joints}") + starts = wp.to_torch(self.backend.model.joint_qd_start) + self.face_force_indices = starts[ + torch.tensor(joints, device=self.device) + ].long() + self.model_audit["face_force_indices"] = ( + self.face_force_indices.cpu().tolist() + ) + + def _site(self, name: str) -> tuple[torch.Tensor, torch.Tensor]: + source_id = self.source.site(name).id + body_name = self.source.body(int(self.source.site_bodyid[source_id])).name + body_id = self.body_map[body_name] + pos = wp.to_torch(self.data.xpos)[:, body_id] + quat = wp.to_torch(self.data.xquat)[:, body_id] + local = torch.as_tensor( + self.source.site_pos[source_id], dtype=torch.float32, device=self.device + ).expand(self.num_envs, -1) + return pos + rotate_wxyz(quat, local), quat + + def _state(self) -> Any: + qpos = wp.to_torch(self.data.qpos)[:, self.qmap] + qvel = wp.to_torch(self.data.qvel)[:, self.vmap] + if self.task_id in ("M1", "M2"): + palm = wp.to_torch(self.data.xpos)[:, self.body_map["robot0:palm"]] + return ShadowState(qpos, qvel, palm) + state = {"qpos": qpos, "qvel": qvel} + if self.task_id == "M3": + return state + grasp, palm_quat = self._site("grasp_site") + tips = [] + for name in ("th_tip", "if_tip", "mf_tip", "rf_tip"): + tip, _ = self._site(name) + inverse = palm_quat * palm_quat.new_tensor([1, -1, -1, -1]) + tips.append(rotate_wxyz(inverse, tip - grasp)) + cube_quat = qpos[:, 19:23] + state.update( + hand_qpos=qpos[:, :16], + hand_qvel=qvel[:, :16], + actuator_force=wp.to_torch(self.data.actuator_force)[:, self.amap], + cube_pos=qpos[:, 16:19], + cube_quat=cube_quat, + cube_linvel=qvel[:, 16:19], + cube_angvel=rotate_wxyz(cube_quat, qvel[:, 19:22]), + palm_pos=grasp, + fingertip_pos=torch.cat(tips, dim=-1), + ) + return state + + def _observation(self, raw: Any) -> dict[str, torch.Tensor]: + if isinstance(raw, torch.Tensor): + return {"obs": raw} + result = {"obs": raw["state"]} + if self.critic_dim != self.observation_dim: + result["critic_obs"] = raw["privileged_state"] + return result + + def reset( + self, *, seed: int | None = None, options: dict[str, Any] | None = None + ) -> tuple[dict, dict]: + if seed is not None: + torch.manual_seed(seed) + if hasattr(self.task, "generator"): + self.task.generator.manual_seed(seed) + ids = ( + torch.arange(self.num_envs, device=self.device) + if not options + else options.get("reset_ids") + ) + ids = torch.as_tensor(ids, dtype=torch.long, device=self.device) + if self.task_id in ("M1", "M2"): + qpos, qvel, ctrl = self.task.reset(ids) + else: + initial = self.task.reset(ids) + qpos, qvel, ctrl = initial["qpos"], initial["qvel"], initial["ctrl"] + wp.to_torch(self.data.qpos)[ids[:, None], self.qmap[None, :]] = qpos + wp.to_torch(self.data.qvel)[ids[:, None], self.vmap[None, :]] = qvel + self.ctrl[ids] = ctrl + # Transfer explicitly set coordinates to both DexSim-owned buffers. + if self.dexuni_adapter is not None: + self.dexuni_adapter.publish_reset(self.data, ids) + else: + for state in (self.backend.state_0, self.backend.state_1): + self.mujoco_solver._update_newton_state( + self.backend.model, state, self.data, state_prev=state + ) + mask = torch.zeros(self.num_envs + 1, dtype=torch.bool, device=self.device) + mask[ids] = True + self.backend.clear_solver_state(wp.from_torch(mask)) + # Recompute positions/sensors without advancing simulation time. + self.mujoco_solver._mujoco_warp.forward(self.mujoco_solver.mjw_model, self.data) + self.elapsed_steps[ids] = 0 + self._episode_return[ids] = 0 + state = self._state() + if self.task_id in ("M1", "M2"): + fresh = self._observation(self.task.observe(state)) + else: + selected = self._observation(self.task.observe(state, env_ids=ids)) + if self._last_observation is None: + fresh = { + key: value.new_empty((self.num_envs, value.shape[-1])) + for key, value in selected.items() + } + else: + fresh = { + key: value.clone() for key, value in self._last_observation.items() + } + for key, value in selected.items(): + fresh[key][ids] = value + self._last_observation = fresh + return fresh, {} + + def advance_physics(self) -> None: + """Advance one complete control interval with the configured substeps.""" + for _ in range(self.substeps): + if self.task_id == "M3": + # The passive layer detent is updated for each physical substep. + state = self._state() + face_force = self.task.face_detent(state["qpos"][:, -1]) + force = wp.to_torch(self.backend.control.joint_f) + force.zero_() + force[self.face_force_indices] = face_force + self.backend.step() + + def step( + self, actions: torch.Tensor + ) -> tuple[dict, torch.Tensor, torch.Tensor, torch.Tensor, dict]: + if self.diagnostics and not bool(torch.isfinite(actions).all()): + raise FloatingPointError("Non-finite policy action") + actions = actions.clamp(-1.0, 1.0) + if self.task_id in ("M1", "M2"): + targets = self.task.action_targets(actions) + else: + targets = self.task.control_targets(actions) + self.ctrl.copy_(targets) + self.advance_physics() + state = self._state() + if self.diagnostics and not bool( + torch.isfinite( + state.qpos if isinstance(state, ShadowState) else state["qpos"] + ).all() + ): + raise FloatingPointError("Non-finite physical state") + if self.task_id in ("M1", "M2"): + obs, reward, terminated, metrics = self.task.step(state, actions) + self.elapsed_steps += 1 + truncated = self.elapsed_steps >= 500 + else: + obs, reward, terminated, truncated, metrics = self.task.after_step( + state, actions + ) + self.elapsed_steps += 1 + self._episode_return += reward + done = terminated | truncated + result = self._observation(obs) + self._last_observation = result + info = {"metrics": {key: value.clone() for key, value in metrics.items()}} + if bool(done.any()): + ids = torch.where(done)[0] + info["episode"] = { + "r": self._episode_return[ids].clone(), + "l": self.elapsed_steps[ids].clone(), + } + final_observation = {k: v.clone() for k, v in result.items()} + reset_obs, _ = self.reset(options={"reset_ids": ids}) + for key in result: + result[key][ids] = reset_obs[key][ids] + info["final_observation"] = final_observation + info["_final_observation"] = done + self._last_observation = result + return result, reward, terminated.bool(), truncated.bool(), info + + def close(self) -> None: + if not self._closed: + self.world.quit() + self._closed = True 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..2ee0d6a423 --- /dev/null +++ b/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_shadow_task.py @@ -0,0 +1,375 @@ +# ---------------------------------------------------------------------------- +# 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 pathlib import Path +from typing import NamedTuple +import xml.etree.ElementTree as ET + +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, + ) -> ShadowTask: + """Load retained reset arrays and actuator limits without loading physics.""" + 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) + actuator = ET.parse(assets / "shadow_pen.xml").getroot().find("actuator") + bounds = torch.tensor( + [[float(v) for v in item.attrib["ctrlrange"].split()] for item in actuator], + device=device, + dtype=dtype, + ) + return cls( + shadow_config(variant), + num_envs, + q0, + ctrl0, + starts, + bounds[:, 0], + bounds[:, 1], + ) + + @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..07144b8d02 --- /dev/null +++ b/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/leap_cube_face_turn.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 typing import Any +import torch +from embodichain.learning.rl.env import register_learning_env +from ._runtime import DexSimManipulationEnv + +__all__ = ["make_env"] + + +@register_learning_env("LeapCubeFaceTurnDexSim-v1") +def make_env( + *, num_envs: int, device: torch.device | str, **cfg: Any +) -> DexSimManipulationEnv: + """Create the leap cube face turn task with the selected DexSim solver.""" + return DexSimManipulationEnv(task_id="M3", num_envs=num_envs, device=device, **cfg) 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..a7de1689ac --- /dev/null +++ b/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/leap_cube_reorient.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 typing import Any +import torch +from embodichain.learning.rl.env import register_learning_env +from ._runtime import DexSimManipulationEnv + +__all__ = ["make_env"] + + +@register_learning_env("LeapCubeReorientDexSim-v1") +def make_env( + *, num_envs: int, device: torch.device | str, **cfg: Any +) -> DexSimManipulationEnv: + """Create the leap cube reorient task with the selected DexSim solver.""" + return DexSimManipulationEnv(task_id="M4", num_envs=num_envs, device=device, **cfg) 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..da4e0666ed --- /dev/null +++ b/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/leap_cube_rotate_z.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 typing import Any +import torch +from embodichain.learning.rl.env import register_learning_env +from ._runtime import DexSimManipulationEnv + +__all__ = ["make_env"] + + +@register_learning_env("LeapCubeRotateZAxisDexSim-v1") +def make_env( + *, num_envs: int, device: torch.device | str, **cfg: Any +) -> DexSimManipulationEnv: + """Create the leap cube rotate z task with the selected DexSim solver.""" + return DexSimManipulationEnv(task_id="M5", num_envs=num_envs, device=device, **cfg) 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..397f49d3be --- /dev/null +++ b/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/shadow_pen_continuous.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 typing import Any +import torch +from embodichain.learning.rl.env import register_learning_env +from ._runtime import DexSimManipulationEnv + +__all__ = ["make_env"] + + +@register_learning_env("ShadowPenContinuousDexSim-v1") +def make_env( + *, num_envs: int, device: torch.device | str, **cfg: Any +) -> DexSimManipulationEnv: + """Create the shadow pen continuous task with the selected DexSim solver.""" + return DexSimManipulationEnv(task_id="M2", num_envs=num_envs, device=device, **cfg) 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..a81bf002f4 --- /dev/null +++ b/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/shadow_pen_fixed.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 typing import Any +import torch +from embodichain.learning.rl.env import register_learning_env +from ._runtime import DexSimManipulationEnv + +__all__ = ["make_env"] + + +@register_learning_env("ShadowPenFixedDexSim-v1") +def make_env( + *, num_envs: int, device: torch.device | str, **cfg: Any +) -> DexSimManipulationEnv: + """Create the shadow pen fixed task with the selected DexSim solver.""" + return DexSimManipulationEnv(task_id="M1", num_envs=num_envs, device=device, **cfg) 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/gym/envs/tasks/fixtures/g1_isaac_rough.json b/tests/gym/envs/tasks/fixtures/g1_isaac_rough.json new file mode 100644 index 0000000000..7b99f6b478 --- /dev/null +++ b/tests/gym/envs/tasks/fixtures/g1_isaac_rough.json @@ -0,0 +1,4697 @@ +{ + "reference": "Isaac Lab v3.0.0-EA executed functions", + "state": { + "base_lin_vel_b": [ + [ + -1.4557806253433228, + 1.0864568948745728, + 0.05817210674285889 + ], + [ + -0.3569294810295105, + -0.1775071769952774, + 0.4064221978187561 + ], + [ + -0.33068203926086426, + 1.7141629457473755, + 1.1541197299957275 + ], + [ + -0.010942414402961731, + 0.02636963129043579, + -1.2672834396362305 + ] + ], + "base_ang_vel_b": [ + [ + 0.3292386531829834, + 1.6441028118133545, + -2.207279682159424 + ], + [ + -0.12005852162837982, + 0.3629804849624634, + 1.0592864751815796 + ], + [ + -0.060097336769104004, + 0.29801517724990845, + 0.7472468614578247 + ], + [ + -1.6891419887542725, + -0.899388313293457, + -0.36965397000312805 + ] + ], + "projected_gravity_b": [ + [ + 0.6021913290023804, + -0.6925421953201294, + 0.3971787691116333 + ], + [ + 0.21417748928070068, + 0.9763940572738647, + 0.02797865867614746 + ], + [ + 0.3185013234615326, + 0.17123670876026154, + 0.932327389717102 + ], + [ + -0.3802645206451416, + 0.27076369524002075, + 0.8843563795089722 + ] + ], + "base_lin_vel_yaw": [ + [ + -0.6121096611022949, + -0.5909727215766907, + 1.6059709787368774 + ], + [ + -0.3131377100944519, + 0.4113398492336273, + 0.23839201033115387 + ], + [ + -0.7736274003982544, + -1.4774776697158813, + -1.2642230987548828 + ], + [ + -0.46797406673431396, + -0.3962193727493286, + 1.109428882598877 + ] + ], + "base_ang_vel_w": [ + [ + 2.07602596282959, + -0.2684623599052429, + 1.8170301914215088 + ], + [ + -0.22963359951972961, + 1.042650818824768, + -0.35833558440208435 + ], + [ + -0.2057259976863861, + -0.2786875069141388, + -0.7285690903663635 + ], + [ + 1.0689704418182373, + 1.6281509399414062, + -0.07189316302537918 + ] + ], + "command": [ + [ + -0.5404043197631836, + 1.0749247074127197, + 0.509365975856781 + ], + [ + -0.9210910201072693, + 0.24086834490299225, + -0.13246694207191467 + ], + [ + -0.2072896957397461, + 1.3115217685699463, + -0.05750608071684837 + ], + [ + 1.3140007257461548, + 0.15788041055202484, + -0.9369820952415466 + ] + ], + "joint_pos": [ + [ + 1.5056331157684326, + 2.0918118953704834, + -0.07304765284061432, + 1.1237612962722778, + -0.198205828666687, + 0.1139955073595047, + 1.5492374897003174, + -0.014597616158425808, + 2.35783052444458, + 0.47007936239242554, + -0.39112335443496704, + -1.1015937328338623, + -0.7289369106292725, + -0.4832809567451477, + 1.2640043497085571, + -2.392209053039551, + -0.9288906455039978, + 0.5256131291389465, + -0.19249051809310913, + -1.3564655780792236, + 1.4588348865509033, + -0.20993752777576447, + 1.401776671409607, + 1.7631984949111938, + -0.3005925714969635, + 0.808905303478241, + -0.03374389931559563, + 1.619919776916504, + -1.377743124961853, + 0.7529674172401428, + 0.7406900525093079, + 0.177289679646492, + 0.7438516616821289, + -0.7136749029159546, + -0.8477581143379211, + -0.26309698820114136, + 0.04932607337832451 + ], + [ + 0.3320133090019226, + 0.3428820073604584, + -0.18796280026435852, + -0.41633516550064087, + -0.06709006428718567, + -0.04722129553556442, + 0.8697317838668823, + -0.3808351159095764, + -0.704973042011261, + 1.1231826543807983, + 1.6798514127731323, + 0.13716299831867218, + 0.582530677318573, + -0.8504189252853394, + 1.5139645338058472, + 0.7708828449249268, + 0.3680831789970398, + -1.1065921783447266, + -1.0081675052642822, + -0.692912220954895, + 0.6990879774093628, + -0.028723623603582382, + -0.602173388004303, + -0.363231360912323, + -0.33312204480171204, + 0.3564692735671997, + 0.8364291191101074, + 2.3312487602233887, + -1.872986078262329, + -0.565384566783905, + 0.14942148327827454, + -1.291897177696228, + -0.48694589734077454, + -1.4648293256759644, + -0.8810045719146729, + 1.056042194366455, + -1.555394172668457 + ], + [ + -1.6637481451034546, + -1.4028340578079224, + 0.12727148830890656, + -0.5829504728317261, + -1.3949012756347656, + 0.635925829410553, + 0.48540303111076355, + -1.8958559036254883, + 1.5646320581436157, + 1.0953043699264526, + 0.2841986119747162, + -1.2972286939620972, + 0.3363381326198578, + -0.15070541203022003, + -0.011540047824382782, + 1.9533648490905762, + 0.024266785010695457, + 0.8772256374359131, + -0.4529947340488434, + -0.44445371627807617, + -1.2233209609985352, + 1.7572327852249146, + -0.9898506999015808, + 0.6434103846549988, + -0.3203880488872528, + -1.3320015668869019, + -0.15934409201145172, + -0.9075278043746948, + 0.16299092769622803, + -0.6044127345085144, + 1.1671395301818848, + 0.9056613445281982, + 0.9750284552574158, + 0.019435198977589607, + 1.25371515750885, + -0.6955330967903137, + -0.8415408134460449 + ], + [ + -0.530204713344574, + -0.7811362147331238, + -1.0067243576049805, + 0.3849828243255615, + 0.7079736590385437, + 1.9297294616699219, + 2.2639787197113037, + -0.26071739196777344, + 0.37015068531036377, + -0.5081090927124023, + 0.7504653334617615, + -0.639136016368866, + 2.0525288581848145, + 0.667559027671814, + -0.5728611946105957, + -0.9176666140556335, + -2.0228161811828613, + 0.258408784866333, + 1.0313853025436401, + 0.9158302545547485, + -0.35417428612709045, + -0.24451617896556854, + 0.9667870998382568, + 0.916934072971344, + -0.8313652276992798, + -0.028980515897274017, + 0.8581166863441467, + -1.2955471277236938, + 0.8638095259666443, + -0.5798051357269287, + -1.9012408256530762, + -1.0559042692184448, + -1.4149895906448364, + -0.8092951774597168, + 0.03765219822525978, + 0.7535103559494019, + -1.0983672142028809 + ] + ], + "joint_vel": [ + [ + -0.5279741883277893, + 0.8597514629364014, + -0.17805825173854828, + -0.6473056674003601, + -0.4873490035533905, + 0.9520416855812073, + -1.79311203956604, + 0.9569541215896606, + -0.6222836375236511, + 0.6242925524711609, + 0.38798239827156067, + -0.2519591152667999, + 0.7435712814331055, + -0.3246157765388489, + 0.6967641115188599, + -0.25631996989250183, + -1.3544143438339233, + -0.8552826046943665, + 0.2536700963973999, + -1.3728705644607544, + -0.530166506767273, + -2.367126703262329, + 1.2589600086212158, + 0.02631186507642269, + -0.8995487093925476, + -1.3286360502243042, + 0.12793944776058197, + -0.9199894070625305, + 0.08235820382833481, + -2.194843292236328, + -1.5484036207199097, + 0.7937297821044922, + 0.14731340110301971, + 0.04240464046597481, + -0.6879045367240906, + -0.8871468901634216, + -0.47980090975761414 + ], + [ + 0.28267720341682434, + -1.6053545475006104, + 0.12526625394821167, + -0.23374232649803162, + -0.5862246155738831, + 1.0336445569992065, + 0.49877527356147766, + -0.43038249015808105, + -0.8129518628120422, + 0.3756439983844757, + 0.6129004955291748, + 0.04692007228732109, + 0.9859462976455688, + 1.4831371307373047, + 2.486056089401245, + -0.8713778853416443, + 2.4219255447387695, + 0.8619664311408997, + 0.7171047925949097, + -2.2819113731384277, + -0.24522823095321655, + -0.023873133584856987, + -0.0973457619547844, + -1.041334629058838, + 0.3649180233478546, + -0.46169158816337585, + -0.14872486889362335, + -0.23188330233097076, + 0.12479649484157562, + 0.05347807705402374, + 0.5584927797317505, + -1.5811458826065063, + 0.5356708765029907, + 0.21025295555591583, + -0.24703240394592285, + 0.23994095623493195, + 1.1590720415115356 + ], + [ + 1.168839693069458, + -0.9539070725440979, + 0.658633291721344, + 0.5681256651878357, + 0.7912182807922363, + -0.36096110939979553, + 0.6277414560317993, + 0.1642685979604721, + 0.05280255153775215, + -0.34626534581184387, + -0.4431508779525757, + -0.11042255908250809, + -0.06429174542427063, + 1.1978527307510376, + 0.47317540645599365, + -2.292738676071167, + 1.5541141033172607, + -0.5851547122001648, + -0.3537789285182953, + -0.8436524271965027, + -0.44712358713150024, + -1.448716640472412, + 1.552046537399292, + -1.7491686344146729, + 0.919133722782135, + 0.5182786583900452, + -0.4023047089576721, + 0.5836163759231567, + -0.9826108813285828, + -0.7050940990447998, + 1.2669448852539062, + 1.6809521913528442, + -0.1113143041729927, + -0.30720576643943787, + 0.9826673865318298, + -0.6770282983779907, + 0.5214178562164307 + ], + [ + 0.393690288066864, + 0.24256174266338348, + -2.0793709754943848, + 0.07966192066669464, + 1.2994259595870972, + -0.3774186968803406, + 1.9390441179275513, + 0.1396932154893875, + 1.1258900165557861, + 0.04496730491518974, + -1.139866590499878, + -0.2511955499649048, + -0.5875258445739746, + 0.002308705821633339, + -1.0564762353897095, + -0.25632399320602417, + -1.1680738925933838, + -0.16880786418914795, + -0.24299125373363495, + 1.0621012449264526, + 0.5457154512405396, + -0.2439623326063156, + -1.3711411952972412, + -1.1331812143325806, + -0.2143252193927765, + 1.9814000129699707, + -0.961947500705719, + 0.6017144322395325, + -0.21473316848278046, + 1.320539951324463, + 1.0334274768829346, + 0.7553479671478271, + 0.23677706718444824, + 0.3173491656780243, + -0.5547844171524048, + -0.7811285853385925, + 1.7556536197662354 + ] + ], + "joint_acc": [ + [ + 1668.161865234375, + -393.6308898925781, + -2312.38525390625, + -1246.2601318359375, + 159.71267700195312, + 420.9147033691406, + 381.2242126464844, + -544.7760620117188, + -242.781982421875, + -1708.1973876953125, + 372.72576904296875, + 237.90928649902344, + -1330.7008056640625, + -168.1787109375, + 852.7047729492188, + -597.7569580078125, + 1202.3968505859375, + 34.981163024902344, + -860.686279296875, + -825.6607055664062, + 1007.9934692382812, + -935.4822998046875, + -573.8944702148438, + 415.48321533203125, + 163.61143493652344, + 521.2409057617188, + 1364.9368896484375, + 1294.7662353515625, + 172.05364990234375, + -1269.952880859375, + 355.07867431640625, + 132.2051239013672, + -26.438737869262695, + -1893.490478515625, + -377.4305725097656, + 1236.39892578125, + 930.3391723632812 + ], + [ + -188.0395965576172, + 129.8790283203125, + -1576.4200439453125, + -236.45082092285156, + 373.44464111328125, + -51.0729866027832, + 1261.9129638671875, + -886.3538208007812, + -2085.78125, + -351.22894287109375, + 273.7871398925781, + 129.74063110351562, + -1110.380859375, + -262.692138671875, + -882.8880004882812, + 526.4204711914062, + 414.2576599121094, + -531.5410766601562, + -846.2487182617188, + 550.9164428710938, + 1629.3389892578125, + -170.4379425048828, + -1341.866455078125, + -1077.88330078125, + 1401.10498046875, + -686.3238525390625, + 145.0147247314453, + -247.7913818359375, + -913.5870971679688, + 1035.5107421875, + 700.6553955078125, + 262.6425476074219, + 148.6880645751953, + -2147.371337890625, + -720.7433471679688, + 468.95166015625, + 908.7201538085938 + ], + [ + 293.96630859375, + -62.2191162109375, + -352.9985046386719, + 645.46142578125, + 510.98382568359375, + -151.96572875976562, + -121.70757293701172, + -161.01531982421875, + -1437.4180908203125, + 1074.786865234375, + -443.6652526855469, + -490.074462890625, + -596.8087158203125, + -1533.0238037109375, + 584.1063842773438, + -4055.375, + 1158.7008056640625, + 912.3848266601562, + 59.38063049316406, + -15.293424606323242, + 1009.8793334960938, + -1339.5419921875, + -61.18260192871094, + 388.61187744140625, + 1065.2783203125, + -533.8775024414062, + -2285.95947265625, + -1250.846923828125, + -1157.400146484375, + -415.5268859863281, + -1911.385986328125, + -1429.7677001953125, + 840.8233642578125, + -878.7216796875, + -1083.961181640625, + -1765.1134033203125, + -278.2705078125 + ], + [ + -1967.5721435546875, + -777.1732788085938, + -264.4198913574219, + -721.2086791992188, + -707.0787963867188, + 188.36904907226562, + -1370.8006591796875, + 1468.0360107421875, + 197.849365234375, + 420.2668762207031, + -604.5377197265625, + -185.824951171875, + -92.34606170654297, + 23.595867156982422, + -1224.7869873046875, + 360.291259765625, + 965.7421264648438, + -1729.43603515625, + 555.3153686523438, + 552.5769653320312, + -48.220787048339844, + 890.5111083984375, + -674.075927734375, + -1074.4248046875, + 97.20252990722656, + -1809.5001220703125, + 658.4154052734375, + 361.912841796875, + 2121.124755859375, + -940.8626708984375, + -504.9437255859375, + 1016.8355712890625, + 90.6131591796875, + 443.5833435058594, + 412.2043151855469, + -1316.3466796875, + 1739.9268798828125 + ] + ], + "applied_torque": [ + [ + -14.419787406921387, + 48.54924011230469, + -54.83991622924805, + 26.02928924560547, + 22.35517692565918, + 27.480485916137695, + -49.62981033325195, + -2.4085357189178467, + 10.796653747558594, + 10.61724853515625, + 14.973010063171387, + -20.94425392150879, + 6.2938666343688965, + 39.708091735839844, + 24.446033477783203, + -3.485306978225708, + -70.85551452636719, + 42.441864013671875, + -38.19162368774414, + 54.53776550292969, + -38.52880096435547, + 20.979534149169922, + 17.398149490356445, + -56.092044830322266, + 53.53395462036133, + 19.920392990112305, + -40.8914794921875, + 44.15678787231445, + -37.509071350097656, + -1.8634438514709473, + -9.967890739440918, + 3.0223982334136963, + -10.67481517791748, + 2.2599074840545654, + -2.2743537425994873, + 8.38425064086914, + -4.397794723510742 + ], + [ + 6.599841594696045, + -38.21885299682617, + 23.851333618164062, + -36.04075241088867, + 40.549476623535156, + -26.525596618652344, + 18.499557495117188, + 85.853271484375, + -4.581111431121826, + -10.820500373840332, + -8.952424049377441, + -17.172874450683594, + 15.811954498291016, + 4.0161452293396, + 59.99694061279297, + -20.906124114990234, + -30.55010223388672, + -20.30491065979004, + -19.803245544433594, + -7.135894298553467, + 54.86551284790039, + -38.38470458984375, + -22.028562545776367, + 20.400306701660156, + 17.09325408935547, + -31.561738967895508, + 0.6164140701293945, + -18.006847381591797, + 6.462375164031982, + 58.80363082885742, + 18.298526763916016, + -71.24160766601562, + 39.95557403564453, + 59.259944915771484, + -38.69380569458008, + 28.53049659729004, + 27.48499870300293 + ], + [ + -20.03413200378418, + 23.982423782348633, + -13.899916648864746, + 33.40736770629883, + 6.102523326873779, + -9.23681926727295, + 13.737212181091309, + 13.551922798156738, + -21.150386810302734, + 79.3740005493164, + 35.05680465698242, + 18.680601119995117, + 0.32720455527305603, + 17.190914154052734, + -18.40950584411621, + -10.131638526916504, + -0.06177947297692299, + 24.01487922668457, + -5.814537525177002, + 36.41688919067383, + 18.143293380737305, + -21.006946563720703, + 44.05561065673828, + 12.61419677734375, + 31.893657684326172, + -36.02269744873047, + 89.47785949707031, + -61.00448989868164, + -40.48505401611328, + -66.33033752441406, + 4.499334812164307, + 32.170658111572266, + 41.60356903076172, + 30.270051956176758, + -20.365007400512695, + -34.966636657714844, + 11.508540153503418 + ], + [ + 16.702163696289062, + -17.244401931762695, + 6.133879661560059, + -62.83711242675781, + 8.172779083251953, + 14.928387641906738, + -35.22040939331055, + -76.99336242675781, + -3.7889232635498047, + 18.798974990844727, + 26.21240234375, + 34.31686019897461, + 17.863983154296875, + 40.698020935058594, + 11.108121871948242, + -31.979366302490234, + -22.386932373046875, + 42.66255569458008, + 18.51290512084961, + 4.037971019744873, + -8.4000825881958, + -4.237223148345947, + -36.56418228149414, + 54.615657806396484, + -37.73508071899414, + -0.4700183570384979, + -9.351125717163086, + -2.6030120849609375, + 0.45510685443878174, + -33.74147415161133, + 7.973489284515381, + 35.40697479248047, + -67.85062408447266, + -4.384401798248291, + 14.123188972473145, + -29.324581146240234, + 18.412818908691406 + ] + ], + "action": [ + [ + -4.368893146514893, + 0.9160680174827576, + 4.984935283660889, + 0.1185097023844719, + 0.622482419013977, + -3.977832794189453, + -7.159829616546631, + -2.5309689044952393, + 4.721494674682617, + -2.8818554878234863, + -4.431326389312744, + -0.8927403092384338, + 3.247561454772949, + -8.902396202087402, + -1.2800079584121704, + -0.15178997814655304, + 3.9192891120910645, + 4.775058746337891, + -1.6756649017333984, + 3.9211177825927734, + -0.003280824515968561, + 6.563349723815918, + -1.1090047359466553, + 0.2747873067855835, + -4.711472034454346, + 1.2407253980636597, + -1.8140769004821777, + -0.3151421844959259, + -1.636247158050537, + 2.5366387367248535, + 2.516878843307495, + -1.2603262662887573, + -1.2249360084533691, + -0.07307285815477371, + 1.0792949199676514, + 0.6049757599830627, + -1.3881919384002686 + ], + [ + 2.340679407119751, + -3.2012264728546143, + 1.449082612991333, + -0.44263583421707153, + 6.466547012329102, + 4.260446548461914, + -1.1634079217910767, + -0.3325357735157013, + 0.6242257952690125, + 0.10043658316135406, + -3.3272321224212646, + 4.066169261932373, + 2.077775716781616, + -5.893347263336182, + 2.7362892627716064, + 0.7733586430549622, + 1.8175424337387085, + 5.035777568817139, + 1.0537075996398926, + -1.1087294816970825, + 1.8238623142242432, + -1.5047787427902222, + -5.391904830932617, + -4.920398235321045, + 1.9996800422668457, + 2.8552744388580322, + 2.4058778285980225, + 0.7430681586265564, + 7.5287909507751465, + -2.3544540405273438, + -1.5701937675476074, + 3.8395838737487793, + -4.007706642150879, + 3.4519639015197754, + -3.863215684890747, + 1.0905109643936157, + 2.3868038654327393 + ], + [ + -1.9206892251968384, + -0.5940742492675781, + -0.3497787117958069, + 3.7373969554901123, + 8.128355026245117, + 4.392646312713623, + -3.1500251293182373, + 1.746557354927063, + -1.877929925918579, + -6.652039051055908, + 4.322165012359619, + 3.399026393890381, + -5.671651840209961, + -0.2512320280075073, + 7.311364650726318, + -0.06858460605144501, + 5.1759772300720215, + 3.3528430461883545, + -0.6859660744667053, + 3.756129503250122, + 3.4173645973205566, + -8.679512023925781, + 0.5880194902420044, + -0.7168394923210144, + 6.000116348266602, + -2.446148157119751, + -6.562048435211182, + -3.665959119796753, + 1.803282380104065, + 12.5444974899292, + 2.0668933391571045, + -11.293632507324219, + 1.1310479640960693, + -2.0034444332122803, + -9.082246780395508, + 4.693811893463135, + -3.442373037338257 + ], + [ + -4.359130382537842, + 1.4358174800872803, + 1.2535760402679443, + 2.251012086868286, + 4.294274806976318, + -2.0001187324523926, + -0.383171945810318, + 2.659695863723755, + 1.6124097108840942, + -1.9515864849090576, + -2.035461187362671, + 0.10162672400474548, + 3.356719970703125, + -7.295328617095947, + 3.0563695430755615, + -7.556408882141113, + 2.0578768253326416, + -1.689565658569336, + 1.8390171527862549, + -4.7228007316589355, + -6.407613754272461, + 1.15846586227417, + -0.7323806881904602, + 2.3890063762664795, + 5.626026630401611, + -3.9759743213653564, + 7.224872589111328, + -2.195585250854492, + 4.695905685424805, + 0.1920173466205597, + -4.04601526260376, + -1.238318920135498, + 4.002810478210449, + -0.024696817621588707, + -10.090450286865234, + -0.7411140203475952, + 1.8331068754196167 + ] + ], + "previous_action": [ + [ + -1.0108466148376465, + -0.142862007021904, + -3.5306010246276855, + -1.964185118675232, + 0.09453178942203522, + 0.38354989886283875, + 1.1020517349243164, + 1.7958555221557617, + 0.18220387399196625, + 0.3428279161453247, + 0.708647608757019, + -0.7077146172523499, + 0.39291343092918396, + -1.400240421295166, + -0.26445072889328003, + -0.11782901734113693, + -1.8917638063430786, + -0.6669813394546509, + 1.4461886882781982, + -0.3651680648326874, + 0.27947166562080383, + -0.021903466433286667, + -0.9254041314125061, + -0.4791620373725891, + -0.11053915321826935, + -0.18104669451713562, + -0.43702784180641174, + -0.8008014559745789, + -1.1921192407608032, + 0.6687846779823303, + -0.0957140177488327, + 0.1811242550611496, + 0.3682470917701721, + -0.06045543774962425, + -0.21235278248786926, + 2.656416893005371, + -1.2452025413513184 + ], + [ + -1.328525185585022, + -1.0206937789916992, + -0.061315279453992844, + 1.2266062498092651, + 0.8237326741218567, + 1.6854990720748901, + 0.6278128623962402, + -0.9680779576301575, + 0.13542191684246063, + -0.5355046987533569, + -0.5223515629768372, + -0.37166252732276917, + -0.6997039318084717, + -0.49067309498786926, + 0.24551089107990265, + -0.775000810623169, + -1.186268925666809, + 1.0723598003387451, + 0.5600261688232422, + -0.5014552474021912, + 0.16223998367786407, + 0.10682955384254456, + -0.9645101428031921, + -0.36778920888900757, + -1.3676397800445557, + -0.6828271150588989, + -0.6595971584320068, + 0.5313081741333008, + 0.7994405031204224, + 0.5227431058883667, + -1.2183622121810913, + -0.6093205809593201, + 0.8313105702400208, + -0.8710706233978271, + 0.3441033661365509, + 0.40260326862335205, + 0.5778527855873108 + ], + [ + -0.5497952103614807, + -1.04951012134552, + 0.6379414796829224, + 0.04964468255639076, + 0.6542599201202393, + -1.7519702911376953, + -0.5116554498672485, + 0.6801650524139404, + 0.08594992756843567, + 0.19995561242103577, + 0.9948765635490417, + -1.3847026824951172, + -0.10364419221878052, + 0.3080644905567169, + -0.5359504818916321, + 0.6439346671104431, + -0.6799383759498596, + 1.0321851968765259, + 0.8460059762001038, + 0.8201548457145691, + -0.6698185801506042, + 0.3081378936767578, + 1.0963537693023682, + -0.7811451554298401, + -0.7836215496063232, + 0.540791928768158, + 1.011771559715271, + -0.5032227039337158, + 0.32934466004371643, + -0.3124656081199646, + -0.1483641415834427, + -0.10728456825017929, + 0.08037495613098145, + -0.1621134728193283, + -1.295089840888977, + 0.5580396056175232, + -0.37750208377838135 + ], + [ + 0.10396319627761841, + -2.1065852642059326, + 0.6500381231307983, + -1.3376755714416504, + 0.1671949028968811, + 0.22357028722763062, + 0.35604509711265564, + 0.05533676967024803, + -0.785855770111084, + 0.3351445198059082, + -0.08490034192800522, + -0.6312821507453918, + -0.41099265217781067, + -0.8103114366531372, + -1.110275149345398, + -0.5244833827018738, + -0.9151604175567627, + -1.8683534860610962, + -1.3026257753372192, + -0.8013157844543457, + 0.6163517832756042, + -0.4021838903427124, + -1.4598723649978638, + 0.7780170440673828, + 0.7748329043388367, + -0.7759303450584412, + -1.9655919075012207, + -0.47156333923339844, + -1.5894381999969482, + 1.4595211744308472, + 1.0188933610916138, + 0.14971071481704712, + -0.9657685160636902, + -0.19366878271102905, + 1.363334059715271, + -0.28902846574783325, + -0.7591164112091064 + ] + ], + "foot_vel_w": [ + [ + [ + -0.21171289682388306, + -0.7254922389984131, + -0.3174818456172943 + ], + [ + -0.4072316586971283, + 0.6528500318527222, + -0.7806931138038635 + ] + ], + [ + [ + 0.2022387683391571, + 1.0100445747375488, + -0.9875770807266235 + ], + [ + -1.4847157001495361, + -0.7211213707923889, + -0.6197503209114075 + ] + ], + [ + [ + -0.3263791501522064, + 0.6447191834449768, + -0.577362596988678 + ], + [ + -1.0065580606460571, + -0.42130208015441895, + 0.8047337532043457 + ] + ], + [ + [ + -0.5346943736076355, + -1.4255815744400024, + -2.03910231590271 + ], + [ + -1.03001868724823, + -1.543200135231018, + -0.7345817685127258 + ] + ] + ], + "foot_force_history": [ + [ + [ + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.3530494272708893, + -0.22667065262794495, + 0.9696542024612427 + ] + ], + [ + [ + 0.0, + 0.0, + 0.0 + ], + [ + -0.03592069819569588, + -0.4432648718357086, + 0.14850245416164398 + ] + ], + [ + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.18437068164348602, + 2.3312039375305176, + -1.269078016281128 + ] + ] + ], + [ + [ + [ + -0.21128453314304352, + -1.7220808267593384, + 1.0471127033233643 + ], + [ + 0.29737088084220886, + -0.13929983973503113, + -0.013977283611893654 + ] + ], + [ + [ + -1.3237457275390625, + 0.6947073340415955, + 0.18112125992774963 + ], + [ + -1.6194521188735962, + -0.8813993334770203, + -2.2042853832244873 + ] + ], + [ + [ + 0.00311090680770576, + 2.5478298664093018, + -0.616400957107544 + ], + [ + 1.6979860067367554, + -0.9815937876701355, + 0.43721848726272583 + ] + ] + ], + [ + [ + [ + 0.0, + 0.0, + 0.0 + ], + [ + -0.6534702181816101, + 0.7779728770256042, + -0.5271583795547485 + ] + ], + [ + [ + 0.0, + 0.0, + 0.0 + ], + [ + -0.0890507623553276, + -0.7470173835754395, + -0.31302332878112793 + ] + ], + [ + [ + 0.0, + 0.0, + 0.0 + ], + [ + -0.6629698872566223, + -0.7631149291992188, + -0.30985027551651 + ] + ] + ], + [ + [ + [ + -0.01198647078126669, + -1.1322849988937378, + 0.4914545714855194 + ], + [ + -0.5499143004417419, + -0.3918861746788025, + -1.967681884765625 + ] + ], + [ + [ + -0.07355965673923492, + -0.9671895503997803, + -0.8145716786384583 + ], + [ + 0.5471686720848083, + -0.08620382845401764, + 0.565158486366272 + ] + ], + [ + [ + -0.5482525825500488, + -0.5464745759963989, + 0.27679717540740967 + ], + [ + 0.0233794916421175, + -0.19441388547420502, + 0.19257529079914093 + ] + ] + ] + ], + "current_air_time": [ + [ + 0.45275312662124634, + 0.8311971426010132 + ], + [ + 0.9466180205345154, + 0.5241358876228333 + ], + [ + 0.8108326196670532, + 0.1247032880783081 + ], + [ + 0.2205706238746643, + 0.2958891987800598 + ] + ], + "current_contact_time": [ + [ + 0.8795936703681946, + 0.3388788104057312 + ], + [ + 0.019999999552965164, + 0.019999999552965164 + ], + [ + 0.009999999776482582, + 0.0 + ], + [ + 0.5516138076782227, + 0.6729174852371216 + ] + ], + "last_air_time": [ + [ + 0.4153270125389099, + 0.7320988774299622 + ], + [ + 0.6830820441246033, + 0.8455604910850525 + ], + [ + 0.055288076400756836, + 0.9835333824157715 + ], + [ + 0.5538256168365479, + 0.08471375703811646 + ] + ], + "illegal_force_history": [ + [ + [ + [ + 0.0, + 0.0, + 0.0 + ] + ], + [ + [ + 0.0, + 0.0, + 0.0 + ] + ], + [ + [ + 0.0, + 0.0, + 0.0 + ] + ] + ], + [ + [ + [ + -0.45131534337997437, + -1.4162439107894897, + -0.524907648563385 + ] + ], + [ + [ + 0.45356932282447815, + 0.5649322867393494, + -0.3941814601421356 + ] + ], + [ + [ + 1.8396403789520264, + 0.560344398021698, + -0.5622568726539612 + ] + ] + ], + [ + [ + [ + 1.3017932176589966, + 0.33739879727363586, + 1.5420217514038086 + ] + ], + [ + [ + -1.1652177572250366, + -1.9771294593811035, + 0.15924112498760223 + ] + ], + [ + [ + -0.5091078877449036, + 0.27111363410949707, + -0.522592306137085 + ] + ] + ], + [ + [ + [ + 1.3238030672073364, + -0.9065457582473755, + -1.0167999267578125 + ] + ], + [ + [ + 0.7171145081520081, + -0.21839751303195953, + 0.24924913048744202 + ] + ], + [ + [ + 2.582766056060791, + 3.244075298309326, + 0.08227609843015671 + ] + ] + ] + ], + "height_scan": [ + [ + -0.28716176748275757, + -0.4419015645980835, + -1.0200023651123047, + -0.28770166635513306, + -0.6783255934715271, + -0.2890845537185669, + -0.1819729208946228, + -1.004855990409851, + -0.14911001920700073, + -0.5164163112640381, + -0.7400633096694946, + -0.7347542643547058, + -0.7018631100654602, + -0.8625413179397583, + -0.1558164358139038, + -0.167216956615448, + -0.34874844551086426, + -0.8995740413665771, + -0.06789010763168335, + -0.10044735670089722, + -0.8174746632575989, + -0.043960511684417725, + -0.5494846105575562, + -0.18539637327194214, + -0.764228880405426, + -0.03852289915084839, + -0.8604356646537781, + -0.3170236349105835, + -0.685659646987915, + -0.9850010275840759, + -0.10952931642532349, + -0.9684800505638123, + -0.9099814295768738, + -0.9744464159011841, + -0.5389964580535889, + -0.4049417972564697, + -0.10225820541381836, + -0.4741637110710144, + -0.2848247289657593, + -0.12730753421783447, + -0.5466987490653992, + -0.4689098596572876, + -0.43547797203063965, + -0.06664764881134033, + -0.14009946584701538, + -0.5698091387748718, + -0.1875506043434143, + -0.8630085587501526, + -0.5284711122512817, + -0.2081202268600464, + -0.8741657137870789, + -0.21526700258255005, + -0.7706494927406311, + -0.29491281509399414, + -0.149122953414917, + -0.07454103231430054, + -0.4668652415275574, + -0.44386452436447144, + -0.43136876821517944, + -0.1436973214149475, + -1.0063891410827637, + -0.4918827414512634, + -0.719039261341095, + -0.21819210052490234, + -0.7204746603965759, + -0.9743740558624268, + -0.39509516954421997, + -0.4074733257293701, + -0.3168402314186096, + -0.7339693903923035, + -1.0243933200836182, + -0.8311435580253601, + -0.03691112995147705, + -0.3078213930130005, + -0.26423215866088867, + -0.7199183106422424, + -0.21735525131225586, + -0.07431846857070923, + -0.643882155418396, + -0.33414846658706665, + -0.7306238412857056, + -0.23027968406677246, + -0.9862818121910095, + -0.4448670744895935, + -0.23372262716293335, + -0.954705536365509, + -0.9376344084739685, + -0.5205722451210022, + -0.37874358892440796, + -0.19050151109695435, + -0.14629727602005005, + -0.6824342012405396, + -0.10497176647186279, + -0.8963110446929932, + -0.24083244800567627, + -0.9735761284828186, + -0.7543412446975708, + -0.2632828950881958, + -0.08554565906524658, + -0.5910387635231018, + -0.8392587304115295, + -0.7414860725402832, + -0.12770283222198486, + -0.10582554340362549, + -0.7515185475349426, + -0.03629302978515625, + -0.7738940715789795, + -0.5477350354194641, + -0.2340010404586792, + -0.6277101039886475, + -0.7757539749145508, + -0.24597251415252686, + -0.6464226245880127, + -0.13265621662139893, + -0.19900083541870117, + -0.39023828506469727, + -0.5473631024360657, + -0.7507364749908447, + -0.6764105558395386, + -0.7312309741973877, + -0.47062546014785767, + -0.37863850593566895, + -0.04647630453109741, + -0.8432213068008423, + -0.37445610761642456, + -0.1488986611366272, + -0.22894209623336792, + -0.16381454467773438, + -0.4650822877883911, + -0.31299012899398804, + -0.22281551361083984, + -0.045681655406951904, + -0.12639284133911133, + -0.3405425548553467, + -1.0269339084625244, + -0.47167855501174927, + -0.08079349994659424, + -0.7066734433174133, + -0.040200889110565186, + -0.6684422492980957, + -0.5266332030296326, + -0.15009689331054688, + -0.30866575241088867, + -0.30868273973464966, + -0.16131454706192017, + -0.23586338758468628, + -0.8245671987533569, + -0.9666875004768372, + -0.060476064682006836, + -1.0116655826568604, + -0.7532963156700134, + -0.5495728254318237, + -0.19813627004623413, + -0.619171679019928, + -0.9636986255645752, + -0.34169644117355347, + -1.0295491218566895, + -0.5497443675994873, + -0.2530338168144226, + -1.0117013454437256, + -0.5710803270339966, + -0.6607940793037415, + -0.09831494092941284, + -0.5018654465675354, + -0.9933570623397827, + -0.8620007634162903, + -0.648077130317688, + -0.3141677975654602, + -0.6191890835762024, + -0.675189197063446, + -0.20415741205215454, + -0.6640922427177429, + -0.12775558233261108, + -1.0292930603027344, + -0.8042539954185486, + -0.4698062539100647, + -0.19917845726013184, + -0.8262059092521667, + -0.2477855682373047, + -1.0034260749816895, + -0.5516428351402283, + -0.3479820489883423, + -0.20705115795135498, + -0.46222829818725586, + -0.7676801681518555, + -0.937333881855011, + -0.3769838809967041 + ], + [ + -0.954921305179596, + -0.2913939952850342, + -0.1249547004699707, + -0.37924909591674805, + -0.5223706364631653, + -0.7347204089164734, + -0.5991426110267639, + -0.9430311322212219, + -0.7585272789001465, + -0.8234665989875793, + -0.8545740842819214, + -0.14105325937271118, + -0.014199018478393555, + -0.34937143325805664, + -0.004843294620513916, + -0.19558537006378174, + -0.7410959005355835, + -0.876293420791626, + -0.45944130420684814, + -0.645321249961853, + -0.28244680166244507, + -0.13762110471725464, + -0.6961289644241333, + -0.17530977725982666, + -0.1276518702507019, + -0.40358179807662964, + -0.9296593070030212, + -0.22403669357299805, + -0.46550416946411133, + -0.09695237874984741, + -0.7555620074272156, + -0.7586738467216492, + -0.901343584060669, + -0.04748272895812988, + -0.022760987281799316, + -0.18168526887893677, + -0.13777267932891846, + -0.5271425843238831, + -0.21348339319229126, + -0.6210537552833557, + -0.9578269720077515, + -0.7002136707305908, + -0.8739659786224365, + -0.772597074508667, + -0.29854655265808105, + -0.5763288736343384, + -0.6866545081138611, + -0.738849937915802, + -0.01651740074157715, + -0.10252046585083008, + -0.24462497234344482, + -0.09212309122085571, + -0.6821115016937256, + -0.8170308470726013, + -0.3699173927307129, + -0.9347706437110901, + 0.002071201801300049, + -0.878790020942688, + -0.6137945055961609, + -0.1944923996925354, + -0.30779916048049927, + -0.7183806300163269, + -0.1092216968536377, + -0.9390383362770081, + -0.049963414669036865, + -0.8985942006111145, + -0.5156030654907227, + -0.29756617546081543, + -0.8451487421989441, + -0.39132821559906006, + -0.9013601541519165, + -0.8194639682769775, + -0.2985495328903198, + -0.9578280448913574, + -0.5193804502487183, + -0.5354547500610352, + -0.4937899708747864, + -0.551175057888031, + -0.24200361967086792, + -0.7114773988723755, + -0.8307096362113953, + -0.36970198154449463, + -0.06439423561096191, + -0.6370649933815002, + -0.689975917339325, + -0.12258321046829224, + -0.18023985624313354, + -0.19095778465270996, + -0.3796960711479187, + -0.8365139365196228, + -0.5827064514160156, + -0.6776121854782104, + -0.155722975730896, + -0.815798282623291, + -0.8836721181869507, + -0.4118494391441345, + -0.8600543141365051, + -0.5819756984710693, + -0.9878496527671814, + -0.4733116626739502, + -0.39344197511672974, + -0.9271964430809021, + -0.2623245120048523, + -0.9258480668067932, + -0.3514559864997864, + -0.4852069616317749, + -0.4426721930503845, + -0.40432196855545044, + -0.9309852719306946, + -0.0453914999961853, + -0.3760489225387573, + -0.5907788276672363, + -0.2762359380722046, + -0.6073105931282043, + -0.9012067317962646, + -0.9473795890808105, + -0.40634167194366455, + -0.2435922622680664, + -0.17430764436721802, + -0.14575618505477905, + -0.2706841826438904, + -0.8886765837669373, + -0.030385375022888184, + -0.6711327433586121, + -0.25839370489120483, + -0.4670237898826599, + -0.04427814483642578, + -0.2971336841583252, + -0.5705769062042236, + -0.12152796983718872, + -0.25330764055252075, + -0.32702285051345825, + -0.7577423453330994, + -0.7056634426116943, + -0.5181854367256165, + -0.1945406198501587, + -0.598212480545044, + -0.19792360067367554, + -0.3633694648742676, + -0.5090498328208923, + -0.028950512409210205, + -0.682405412197113, + -0.7791985869407654, + -0.4832354784011841, + -0.5657856464385986, + -0.38258951902389526, + -0.41143715381622314, + -0.1167987585067749, + -0.9711939096450806, + -0.38090211153030396, + -0.2797265648841858, + -0.7816302180290222, + -0.9141287803649902, + -0.9264760613441467, + -0.3138616681098938, + -0.4468371272087097, + -0.3800205588340759, + -0.4484589695930481, + -0.06766146421432495, + -0.5285791158676147, + -0.9801487326622009, + -0.5475660562515259, + -0.18531686067581177, + -0.5587353706359863, + -0.7672158479690552, + -0.5399236679077148, + -0.8216560482978821, + -0.06692808866500854, + -0.9802008867263794, + -0.02825319766998291, + -0.8521097302436829, + -0.29705530405044556, + -0.7402810454368591, + -0.9521987438201904, + -0.5989824533462524, + -0.5091099739074707, + -0.9797102212905884, + -0.7307826280593872, + -0.7059122920036316, + -0.3619755506515503, + -0.8406006097793579, + -0.8817713856697083, + -0.5424179434776306, + -0.7115862965583801, + -0.9517701864242554, + -0.7782663702964783, + -0.2805899381637573 + ], + [ + -1.4259082078933716, + -0.7628011107444763, + -1.1494905948638916, + -0.6412063837051392, + -1.26246976852417, + -0.8821176290512085, + -0.9137013554573059, + -0.8261319994926453, + -1.4889938831329346, + -1.151426076889038, + -0.7739695906639099, + -1.1284723281860352, + -0.8209890723228455, + -1.4593219757080078, + -1.3986201286315918, + -1.3377176523208618, + -0.62993985414505, + -1.1792564392089844, + -0.9241922497749329, + -0.7910730242729187, + -1.2805874347686768, + -1.0707993507385254, + -0.9492930173873901, + -1.3837287425994873, + -0.7922476530075073, + -1.3626549243927002, + -1.074632167816162, + -1.4410548210144043, + -1.105318307876587, + -0.9413167834281921, + -1.3076597452163696, + -0.7860186696052551, + -0.5949016809463501, + -0.6691449284553528, + -1.1743073463439941, + -0.8825331926345825, + -1.3958384990692139, + -1.3514487743377686, + -0.5281450748443604, + -1.41390860080719, + -0.7003967761993408, + -0.9328911900520325, + -0.5688399076461792, + -0.9510324001312256, + -1.3506070375442505, + -1.275319218635559, + -0.9986140727996826, + -1.49320387840271, + -1.4843120574951172, + -0.7307277321815491, + -1.0622351169586182, + -1.0467100143432617, + -1.0493956804275513, + -1.3928061723709106, + -0.7706238627433777, + -0.9685169458389282, + -1.1880372762680054, + -0.9757683277130127, + -1.1320688724517822, + -1.1181002855300903, + -1.211276888847351, + -0.8843178153038025, + -1.3307569026947021, + -1.083923578262329, + -0.8621208667755127, + -0.6330054402351379, + -1.401702642440796, + -1.3387494087219238, + -1.425265908241272, + -0.5421697497367859, + -0.8561704158782959, + -0.9407783150672913, + -1.4512741565704346, + -1.4663667678833008, + -0.9251981973648071, + -1.484214186668396, + -1.296557903289795, + -0.6689178347587585, + -0.8467381596565247, + -1.3045446872711182, + -1.0028636455535889, + -0.5476725697517395, + -1.1383848190307617, + -1.492264747619629, + -1.2163026332855225, + -0.5841254591941833, + -1.014596939086914, + -1.2489981651306152, + -0.9467659592628479, + -0.9262081980705261, + -0.5359898805618286, + -1.0335298776626587, + -0.7344545125961304, + -0.6320112347602844, + -1.0315008163452148, + -1.1436221599578857, + -0.6428101658821106, + -0.717728853225708, + -1.097362756729126, + -0.5427185297012329, + -0.5763674974441528, + -0.6342934966087341, + -0.5467061400413513, + -1.447387456893921, + -1.4838192462921143, + -1.186476707458496, + -1.2606291770935059, + -1.2610423564910889, + -1.2567602396011353, + -0.7670435309410095, + -0.5562624335289001, + -1.0019211769104004, + -0.8009345531463623, + -1.248938798904419, + -1.0995445251464844, + -0.7193322777748108, + -0.7792929410934448, + -1.058241367340088, + -0.7708737254142761, + -1.0753350257873535, + -0.5170380473136902, + -0.6725472807884216, + -1.158711314201355, + -0.9907874464988708, + -0.8775873780250549, + -1.182322382926941, + -0.8543204069137573, + -1.4351080656051636, + -0.5689177513122559, + -1.194091796875, + -0.9574165344238281, + -1.3230631351470947, + -0.5407227277755737, + -1.096113681793213, + -0.8761577010154724, + -0.7989087104797363, + -1.1100937128067017, + -1.000990629196167, + -0.6183527708053589, + -1.1465448141098022, + -1.2729742527008057, + -1.1849963665008545, + -0.9400474429130554, + -1.4361584186553955, + -0.8586726784706116, + -1.0775995254516602, + -0.7488973140716553, + -0.5302025675773621, + -0.8484327793121338, + -0.6460759043693542, + -0.9092519283294678, + -1.2286038398742676, + -1.2114779949188232, + -0.9478926658630371, + -0.781777024269104, + -1.2581359148025513, + -0.9231940507888794, + -0.6026317477226257, + -0.7068359851837158, + -0.5500870943069458, + -0.6155065894126892, + -0.8434672355651855, + -0.6625481843948364, + -1.1525171995162964, + -1.0879313945770264, + -0.5161118507385254, + -0.8322681784629822, + -1.306541085243225, + -0.521882951259613, + -0.5572435259819031, + -1.374621033668518, + -1.0994514226913452, + -1.0482306480407715, + -1.40201735496521, + -0.7103193998336792, + -1.2804256677627563, + -0.554164469242096, + -1.2223751544952393, + -0.8806891441345215, + -0.7358636260032654, + -1.1746655702590942, + -1.0769728422164917, + -0.5750457644462585, + -1.044844388961792, + -0.5782709717750549, + -1.0069940090179443, + -1.4719765186309814 + ], + [ + -0.9238142967224121, + -0.0698205828666687, + -0.1403544545173645, + -0.1853806972503662, + -0.7912553548812866, + -0.3743066191673279, + -0.35251379013061523, + -0.6051955223083496, + -0.6003751754760742, + -1.028996467590332, + -1.004363775253296, + -0.4606083631515503, + -0.992021381855011, + -0.7569908499717712, + -0.5070105791091919, + -0.07780998945236206, + -0.9578995704650879, + -0.8302999138832092, + -0.6006336212158203, + -0.6636675596237183, + -0.26259171962738037, + -0.2994014620780945, + -0.49584078788757324, + -0.6343724727630615, + -0.5550675988197327, + -0.5611420273780823, + -0.44774508476257324, + -0.7359809279441833, + -0.08619123697280884, + -0.4696958065032959, + -0.10622358322143555, + -1.0131781101226807, + -0.7854553461074829, + -0.2939700484275818, + -0.9478037357330322, + -0.3207838535308838, + -0.907844066619873, + -0.9980040192604065, + -0.8072037696838379, + -0.4208117723464966, + -0.11277163028717041, + -0.5618969798088074, + -0.7238936424255371, + -0.211897611618042, + -0.49919605255126953, + -0.540176272392273, + -0.4875825047492981, + -0.05083209276199341, + -0.37905454635620117, + -0.348609983921051, + -0.5767907500267029, + -0.6685472726821899, + -0.06904172897338867, + -0.06032043695449829, + -0.6514592170715332, + -0.9603331089019775, + -0.4267512559890747, + -0.07870161533355713, + -0.6747346520423889, + -0.6316162347793579, + -0.493461549282074, + -0.33446455001831055, + -0.5631297826766968, + -0.7761831879615784, + -0.07967740297317505, + -1.0099406242370605, + -0.710037887096405, + -0.37101638317108154, + -0.9681183099746704, + -0.5347010493278503, + -0.9131768941879272, + -0.442663311958313, + -0.4550727605819702, + -0.11293643712997437, + -0.555015504360199, + -0.5921054482460022, + -0.03974407911300659, + -0.7025007605552673, + -1.024024248123169, + -0.7622687220573425, + -0.7169705629348755, + -0.7112247943878174, + -0.5937038064002991, + -0.5658383369445801, + -0.6262092590332031, + -0.7381301522254944, + -0.06292319297790527, + -0.11036300659179688, + -0.4519262909889221, + -0.9524312615394592, + -0.251644492149353, + -0.634813129901886, + -0.10808104276657104, + -0.20290517807006836, + -0.8795364499092102, + -0.7133604288101196, + -0.6017213463783264, + -0.4749462604522705, + -0.9139991402626038, + -0.47803354263305664, + -0.6484487652778625, + -0.601347804069519, + -1.0212500095367432, + -0.7061208486557007, + -0.7504950165748596, + -0.9117038249969482, + -0.9980932474136353, + -0.6756836771965027, + -0.6640557646751404, + -0.30770397186279297, + -0.5957074761390686, + -0.2663015127182007, + -0.4265478849411011, + -0.7591960430145264, + -0.9133948087692261, + -0.6641430854797363, + -0.5102360248565674, + -0.18117982149124146, + -0.43050360679626465, + -0.42123615741729736, + -0.42209959030151367, + -0.8755217790603638, + -0.9732856750488281, + -0.19557172060012817, + -0.42915111780166626, + -0.26035380363464355, + -0.6769423484802246, + -0.8010091781616211, + -0.28690940141677856, + -0.6068267226219177, + -0.9076883792877197, + -0.7012059092521667, + -0.4878358840942383, + -0.8782142400741577, + -0.5469277501106262, + -0.6292080283164978, + -0.06442302465438843, + -0.39036065340042114, + -0.25186532735824585, + -0.39602231979370117, + -0.576009213924408, + -0.9295237064361572, + -0.395857036113739, + -0.08957946300506592, + -0.33785170316696167, + -0.04375237226486206, + -0.14963960647583008, + -0.31550389528274536, + -0.12502753734588623, + -0.7684140801429749, + -0.27367401123046875, + -0.5296682119369507, + -0.5896162390708923, + -0.7512279152870178, + -0.5819600224494934, + -0.4105685353279114, + -0.8416110873222351, + -0.9148604869842529, + -0.43707555532455444, + -1.010685682296753, + -0.5056235790252686, + -0.7135579586029053, + -0.6027063727378845, + -0.39949893951416016, + -0.2314024567604065, + -0.7767473459243774, + -0.3291957378387451, + -0.06120145320892334, + -0.9269648194313049, + -0.8520622253417969, + -0.9365422129631042, + -0.2460879683494568, + -0.9516366124153137, + -0.5487070083618164, + -0.630145251750946, + -0.6992456912994385, + -0.15334832668304443, + -0.22281140089035034, + -0.3819270730018616, + -0.49393123388290405, + -0.2525184750556946, + -0.5560675263404846, + -0.29308879375457764, + -0.859779953956604, + -0.599997341632843, + -0.72066730260849, + -0.726995050907135 + ] + ], + "episode_step": [ + 0, + 20, + 40, + 60 + ], + "root_height": [ + 0.2529202103614807, + 0.46661919355392456, + 0.5359861254692078, + 0.6554553508758545 + ] + }, + "noise": { + "base_lin_vel": [ + [ + 0.04883061721920967, + -0.013821971602737904, + -0.008735167793929577 + ], + [ + -0.031018365174531937, + 0.040441371500492096, + -0.01481848955154419 + ], + [ + -0.014213168993592262, + 0.09062711149454117, + -0.05037219449877739 + ], + [ + 0.03259098529815674, + 0.08449690788984299, + 0.018265271559357643 + ] + ], + "base_ang_vel": [ + [ + -0.12153711169958115, + -0.06397805362939835, + 0.1936502754688263 + ], + [ + 0.09037470817565918, + 0.0034952403511852026, + 0.020504523068666458 + ], + [ + 0.12750859558582306, + -0.011539625935256481, + 0.15800964832305908 + ], + [ + 0.04557690769433975, + 0.13596391677856445, + 0.06388843059539795 + ] + ], + "projected_gravity": [ + [ + 0.017629355192184448, + -0.02608974650502205, + -0.030870115384459496 + ], + [ + -0.009302496910095215, + 0.019067002460360527, + 0.015243589878082275 + ], + [ + -0.013070404529571533, + 0.024919182062149048, + 0.025548148900270462 + ], + [ + -0.0126702431589365, + 0.04979836568236351, + -0.004926800727844238 + ] + ], + "joint_pos": [ + [ + -0.0057575032114982605, + 0.0013853668933734298, + 0.009871982038021088, + 0.005030627362430096, + 0.005901203025132418, + -0.008174210786819458, + -0.008026707917451859, + 0.004386216402053833, + 0.005877886898815632, + -0.009357362985610962, + -0.002096047392114997, + 0.003729259828105569, + -0.003522734623402357, + 0.007057160139083862, + -0.000681234581861645, + 0.0036938809789717197, + -0.0020403824746608734, + 0.001927495002746582, + 0.006183958146721125, + 0.0006244289688766003, + 0.008813641965389252, + -0.0031896650325506926, + -0.007687872275710106, + 0.004311834461987019, + 0.0031393300741910934, + 0.008208028972148895, + -0.008771959692239761, + -0.0005437945947051048, + -0.009363388642668724, + 0.007150211371481419, + -0.003804231761023402, + -0.0016525244573131204, + 0.009646023623645306, + 0.005172127392143011, + -0.008134937845170498, + 0.0015237081097438931, + 0.0016654860228300095 + ], + [ + -0.0070250011049211025, + 0.008180037140846252, + -0.0015627526445314288, + -0.007862006314098835, + -0.0005490505718626082, + 0.009878805838525295, + 0.009438781067728996, + 0.001189577509649098, + 0.0054941559210419655, + -0.002092573558911681, + -0.006113527808338404, + 0.007938407361507416, + -0.0036140417214483023, + -0.00310951704159379, + -0.0070936414413154125, + 0.007110206875950098, + 0.009609895758330822, + -0.007239156868308783, + 0.0022065674420446157, + 0.009812054224312305, + 0.002668023109436035, + 0.0015845977468416095, + 0.009937254711985588, + -0.006574833299964666, + 0.002253255806863308, + -0.009269744157791138, + -0.008170354180037975, + -0.002349680522456765, + 0.004635028541088104, + 0.00797504186630249, + -0.0029285443015396595, + -0.004988042172044516, + -0.009925201535224915, + -0.0014547478640452027, + -0.007709812838584185, + 0.009672446176409721, + -0.006436267867684364 + ], + [ + -0.00582847697660327, + 0.009322091937065125, + -0.008036257699131966, + -0.004748580511659384, + -0.0014337527099996805, + 0.008422642946243286, + -0.00993817113339901, + -0.003982769325375557, + -0.0006971549591980875, + -0.0036841523833572865, + -0.007783230394124985, + 0.0002665829670149833, + 0.004849553108215332, + 0.007614966481924057, + 0.005160970613360405, + 0.002419987926259637, + -0.005628595128655434, + -0.007644395809620619, + -0.008825138211250305, + -0.006654081400483847, + 0.00674707256257534, + -0.0007596480427309871, + 0.0014600479044020176, + -0.007051425985991955, + 0.00920053943991661, + 0.007913350127637386, + 0.00615357980132103, + 0.006131371017545462, + 0.0076631163246929646, + 0.009503038600087166, + 0.008999968878924847, + -0.002048282651230693, + 0.0034172546584159136, + 0.00483757583424449, + -0.00964801013469696, + -0.006213091313838959, + 0.004030534066259861 + ], + [ + -0.0094909043982625, + -0.0027957058046013117, + 0.00891487579792738, + -0.0026885520201176405, + -0.006809473969042301, + -0.009270472452044487, + 0.007180241402238607, + 0.003248116932809353, + 0.0021526420023292303, + -0.0015011620707809925, + -0.006352534051984549, + 0.005831989925354719, + -0.007459655869752169, + -0.004819393157958984, + -0.0031832025852054358, + 0.00876720529049635, + 0.00456439983099699, + -0.008394292555749416, + -0.007530570030212402, + -0.004901149310171604, + -0.00695707555860281, + 0.004014358390122652, + -0.0026265643537044525, + -0.003164188703522086, + 0.009519331157207489, + -0.00016419529856648296, + 0.007517525926232338, + -0.008004449307918549, + 0.0027327954303473234, + -2.755045898084063e-05, + -0.004322327207773924, + -0.007389858830720186, + -0.002289263065904379, + -0.003180756466463208, + -0.008036520332098007, + -0.007352158892899752, + 0.005940380040556192 + ] + ], + "joint_vel": [ + [ + 1.291954755783081, + -0.12978136539459229, + -1.3720617294311523, + -0.5396355986595154, + 0.21575599908828735, + -1.0990208387374878, + -0.24226319789886475, + 1.0541456937789917, + -0.31519049406051636, + 1.2596321105957031, + -0.0023235082626342773, + -0.10287433862686157, + 0.30647099018096924, + -0.3931882381439209, + -0.29570817947387695, + 1.479931354522705, + -1.0126559734344482, + -0.319095253944397, + -0.3643478751182556, + 1.079376220703125, + 0.5055824518203735, + 0.14722323417663574, + -0.4214363694190979, + -0.7030287981033325, + 1.3261168003082275, + -0.4632795453071594, + 0.5863493084907532, + 0.22087472677230835, + 0.4658646583557129, + 0.7025163173675537, + 0.5729398727416992, + -0.6472122073173523, + -0.07871317863464355, + 0.27562934160232544, + 0.4515758156776428, + -0.010535895824432373, + 0.779064416885376 + ], + [ + 0.642591118812561, + 0.6226478219032288, + -0.7088395357131958, + 1.1831715106964111, + -0.37703847885131836, + -1.2027876377105713, + 0.19178080558776855, + -0.08722668886184692, + -0.5969939231872559, + 0.6154507398605347, + -0.16152137517929077, + 1.0399879217147827, + -1.0715959072113037, + -0.9239612817764282, + -0.4761459231376648, + -1.2741475105285645, + 0.35439062118530273, + -0.5172491669654846, + 1.1776759624481201, + 1.0715643167495728, + -1.3331444263458252, + -0.8865401744842529, + 0.7593340873718262, + 0.8651373982429504, + 0.9665860533714294, + -1.2105270624160767, + 0.220251202583313, + 1.231561303138733, + -0.3581063747406006, + -1.0719406604766846, + 1.4191513061523438, + 1.45975661277771, + -0.28395169973373413, + -0.5377694964408875, + 1.2441927194595337, + 0.5622957944869995, + -1.0658917427062988 + ], + [ + 0.673727810382843, + -1.1740869283676147, + -0.052003562450408936, + 1.2892675399780273, + 0.2313660979270935, + 0.9932064414024353, + 1.3333872556686401, + -1.1939518451690674, + -0.6817955374717712, + 0.048616111278533936, + 0.9909465909004211, + 0.3479481339454651, + -1.445811152458191, + 0.23537743091583252, + 1.4869927167892456, + -1.3971707820892334, + -1.4305942058563232, + -0.6248220205307007, + 0.20929992198944092, + 1.4383745193481445, + -0.2615094780921936, + 0.0792306661605835, + -0.5388666987419128, + -1.4612298011779785, + 0.8085447549819946, + 0.6013521552085876, + 1.1867012977600098, + 0.472695529460907, + 1.0868589878082275, + 1.108255386352539, + -0.9932003617286682, + 0.8409976959228516, + -0.21922892332077026, + 0.8188753724098206, + 0.5408661961555481, + 0.7041410207748413, + 1.2410402297973633 + ], + [ + 0.2413002848625183, + 1.4686260223388672, + -0.9628372192382812, + -0.5439913272857666, + -0.2940552234649658, + -0.40880584716796875, + 0.022405385971069336, + -0.6928614377975464, + -0.3720552921295166, + 1.3106389045715332, + -0.8510822653770447, + 1.1379854679107666, + -1.117159366607666, + -1.222306489944458, + 1.3393640518188477, + 0.528426468372345, + 0.05907100439071655, + 1.3263695240020752, + -0.2693510055541992, + -0.7407882213592529, + 0.37777572870254517, + 0.7477294206619263, + 0.02171802520751953, + -0.20283597707748413, + 0.6097682118415833, + 0.8395984768867493, + -1.4558920860290527, + -0.2044631838798523, + 0.6514424085617065, + -1.273893117904663, + -0.2686917185783386, + 1.1251798868179321, + -1.4518529176712036, + 0.9102137088775635, + -0.9907659888267517, + -1.15702223777771, + 0.6048660278320312 + ] + ], + "height_scan": [ + [ + 0.05223618820309639, + 0.07281935214996338, + 0.04870041832327843, + 0.05835052952170372, + 0.052443649619817734, + 0.05397450923919678, + 0.09696508944034576, + -0.07384004443883896, + 0.07450683414936066, + -0.07972053438425064, + -0.05582697317004204, + -0.0025103928055614233, + 0.07548464834690094, + -0.050818659365177155, + 0.06830856949090958, + -0.07037242501974106, + -0.05029522255063057, + 0.05106857046484947, + 0.01951082982122898, + -0.020518077537417412, + -0.08504784107208252, + 0.06689858436584473, + -0.0677390918135643, + 0.0659003034234047, + 0.05153893306851387, + -0.013903319835662842, + -0.0023876905906945467, + -0.09476470202207565, + 0.0212746262550354, + 0.005085790064185858, + -0.04863257333636284, + -0.09049072116613388, + 0.03633619472384453, + 0.02675241231918335, + 0.05675138160586357, + -0.024968409910798073, + -0.09448989480733871, + -0.07529661804437637, + -0.03139544650912285, + 0.08747044950723648, + 0.0036378861404955387, + -0.045174311846494675, + 0.019420409575104713, + 0.05926762893795967, + 0.06458500772714615, + -0.0510452501475811, + 0.07272400707006454, + -0.041569530963897705, + -0.05765208229422569, + -0.056952621787786484, + -0.05372563749551773, + 0.02325906790792942, + 0.074857197701931, + 0.021959329023957253, + -0.03788477182388306, + 0.021688735112547874, + 0.09281517565250397, + -0.05556640774011612, + -0.0017329215770587325, + -0.032182980328798294, + 0.05220526456832886, + -0.03398185968399048, + -0.001009225845336914, + -0.025295555591583252, + -0.09604938328266144, + -0.07017292082309723, + -0.03073829412460327, + -0.03573857620358467, + -0.08984935283660889, + 0.037361741065979004, + -0.07143940776586533, + -0.08415845781564713, + 0.05524646118283272, + -0.05972718074917793, + 0.06948975473642349, + 0.011285520158708096, + -0.09069795906543732, + 0.0958104059100151, + -0.0906735435128212, + -0.09464623034000397, + -0.08838986605405807, + 0.05617351457476616, + -0.0951768159866333, + -0.017627596855163574, + 0.07602667063474655, + -0.02420262061059475, + 0.08525604009628296, + -0.08348560333251953, + 0.05300448089838028, + -0.04079849645495415, + -0.0757252499461174, + 0.05141489580273628, + -0.04664630815386772, + -0.08741772174835205, + 0.05962113291025162, + -0.08534891903400421, + -0.06840262562036514, + -0.060201775282621384, + 0.04182354360818863, + 0.02561667002737522, + -0.0847959890961647, + -0.09846224635839462, + -0.08396317809820175, + 0.023640204221010208, + -0.05097750574350357, + -0.005551946349442005, + 0.06938426941633224, + -0.0732884332537651, + 0.0006749391905032098, + -0.056771207600831985, + 0.012185418978333473, + 0.0973164439201355, + -0.060415517538785934, + 0.07151108235120773, + 0.07361127436161041, + 0.07730904966592789, + 0.007908380590379238, + 0.07696936279535294, + 0.08235805481672287, + -0.08053658157587051, + -0.0328231118619442, + -0.021262360736727715, + 0.056901752948760986, + -0.040937721729278564, + 0.03289289399981499, + 0.07808887213468552, + 0.09658312797546387, + 0.034975696355104446, + 0.019218016415834427, + -0.03373413160443306, + 0.045011308044195175, + -0.04058363661170006, + 0.07068971544504166, + -0.0020122171845287085, + -0.08119992166757584, + 0.08053254336118698, + -0.03653603792190552, + 0.0654241219162941, + -0.018424082547426224, + 0.09486718475818634, + 0.0954732894897461, + 0.06364333629608154, + -0.020247971639037132, + 0.0004792213439941406, + -0.04459511116147041, + 0.0002664923667907715, + 0.09078987687826157, + 0.016690218821167946, + 0.073288694024086, + -0.08597160875797272, + 0.04935495927929878, + -0.08829586952924728, + 0.03480887413024902, + -0.09769415855407715, + -0.01814298704266548, + -0.07210631668567657, + -0.05110807344317436, + 0.09955354779958725, + -0.025356043130159378, + 0.05369465425610542, + -0.03282473236322403, + -0.045098818838596344, + -0.0963265672326088, + -0.014818823896348476, + 0.0970042496919632, + -0.04285046085715294, + -0.022491419687867165, + -0.08302469551563263, + -0.09502489864826202, + 0.09619249403476715, + 0.04927661642432213, + -0.07743270695209503, + 0.07388831675052643, + -0.03430875763297081, + 0.03442783281207085, + -0.004544377326965332, + 0.017544997856020927, + -0.09462057799100876, + -0.03310791403055191, + -0.08079953491687775, + 0.09451492875814438, + 0.034996867179870605, + 0.037820518016815186, + 0.05495729669928551, + -0.06423377990722656, + 0.07771261781454086, + 0.06205332279205322 + ], + [ + 0.04454289749264717, + 0.09633924812078476, + 0.02234947681427002, + -0.03709438070654869, + 0.06537798792123795, + -0.0036954761017113924, + -0.0709725171327591, + -0.0022555708419531584, + -0.019058704376220703, + -0.08295859396457672, + -0.0668288841843605, + 0.06564565002918243, + -0.008014059625566006, + 0.02972257137298584, + -0.06803649663925171, + -0.022064972668886185, + 0.023032331839203835, + 0.09698740392923355, + -0.00355170969851315, + 0.014506769366562366, + -0.09618744999170303, + -0.057544078677892685, + -0.08049031347036362, + 0.06988827139139175, + 0.05659450218081474, + 0.081429623067379, + -0.01583411730825901, + 0.08203321695327759, + 0.022809697315096855, + 0.0025966644752770662, + 0.05739133432507515, + 0.04409891366958618, + 0.0792551040649414, + 0.02670816145837307, + -0.03649036958813667, + 0.03820384666323662, + 0.0018723130924627185, + 0.019527995958924294, + 0.07081802189350128, + 0.06685671955347061, + -0.04732177406549454, + 0.09013142436742783, + 0.08267374336719513, + -0.0453900471329689, + 0.025200629606842995, + -0.06997811794281006, + -0.004295730497688055, + -0.06129920482635498, + 0.0958331748843193, + -0.09351586550474167, + -0.0019816518761217594, + -0.07015091180801392, + -0.0036545991897583008, + -0.098160520195961, + -0.08309948444366455, + 0.08628272265195847, + -0.0010742545127868652, + 0.08101024478673935, + -0.07841720432043076, + -0.04145316034555435, + 0.09812722355127335, + -0.04454462602734566, + -0.055193401873111725, + 0.08143249899148941, + -0.07639435678720474, + 0.04121881723403931, + -0.09146231412887573, + 0.029009629040956497, + 0.005049038212746382, + -0.09455247223377228, + 0.09001157432794571, + -0.05812112241983414, + -0.051116298884153366, + 0.0989212915301323, + -0.09019549936056137, + -0.004682672210037708, + 0.08814387768507004, + -0.07027006149291992, + -0.04986201599240303, + -0.0037648796569556, + -0.0863538309931755, + 0.09390679746866226, + -0.001859474228695035, + -0.08387340605258942, + -0.021552979946136475, + -0.06085775047540665, + 0.08412354439496994, + 0.07760261744260788, + 0.033540308475494385, + -0.007966804318130016, + 0.09766508638858795, + 0.07904251664876938, + 0.036023106426000595, + -0.08556431531906128, + -0.02803283929824829, + -0.07379330694675446, + 0.032509781420230865, + 0.06164292246103287, + 0.06831105053424835, + -0.022572530433535576, + -0.03469933196902275, + 0.05436403676867485, + 0.05001084879040718, + 0.09955954551696777, + -0.06049082428216934, + 0.09357159584760666, + -0.058810651302337646, + 0.026045680046081543, + -0.06182229518890381, + -0.09808714687824249, + 0.07264067232608795, + 0.010136640630662441, + 0.03257089853286743, + -0.032764364033937454, + -0.0168511513620615, + -0.03634246811270714, + -0.050544869154691696, + 0.08632969111204147, + -0.05729971081018448, + -0.004422307014465332, + -0.03534975275397301, + -0.09977515041828156, + 0.05225184187293053, + 0.0987057238817215, + -0.030269814655184746, + -0.08964965492486954, + 0.010916459374129772, + -0.06403247267007828, + -0.021802140399813652, + -0.05780067667365074, + -0.00944598950445652, + 0.0031923174392431974, + -0.08463746309280396, + -0.08637287467718124, + -0.03272193670272827, + 0.0702233538031578, + -0.0173624400049448, + -0.025797570124268532, + -0.027443481609225273, + -0.0668819323182106, + -0.05136158689856529, + -0.0798415094614029, + -0.09227633476257324, + 0.08988626301288605, + -0.063568115234375, + -0.02681502141058445, + 0.02955458126962185, + -0.028867090120911598, + 0.044674646109342575, + 0.08567066490650177, + 0.011871886439621449, + 0.08843716233968735, + -0.03440951183438301, + -0.0064733028411865234, + -0.022554313763976097, + 0.03086276166141033, + -0.0625581294298172, + 0.09783625602722168, + 0.014882815070450306, + -0.08102332800626755, + 0.09377094358205795, + -0.0480550192296505, + 0.039779163897037506, + -0.0028643726836889982, + -0.03406388685107231, + -0.04476151615381241, + -0.08575516939163208, + 0.017647111788392067, + 0.015932811424136162, + 0.09425511211156845, + 0.03151027113199234, + -0.06185638904571533, + 0.01671609841287136, + 0.07810837030410767, + 0.06103941425681114, + 0.08734599500894547, + -0.03332633897662163, + 0.08172085136175156, + 0.023682106286287308, + 0.020249998196959496, + -0.06506023555994034, + -0.03692127391695976, + 0.0109749436378479, + -0.09830556064844131, + 0.04177987575531006, + -0.06498479098081589, + -0.017017556354403496 + ], + [ + -0.060177888721227646, + 0.07695546001195908, + -0.014235473237931728, + 0.056319501250982285, + 0.02831416204571724, + 0.007892119698226452, + -0.006545424461364746, + 0.08087054640054703, + -0.03766915947198868, + 0.07420116662979126, + 0.09232723712921143, + -0.013292670249938965, + -0.05053676292300224, + 0.08112919330596924, + -0.04358324036002159, + 0.040531981736421585, + -0.00338308815844357, + -0.04853551462292671, + 0.019414007663726807, + 0.06695699691772461, + -0.07237537205219269, + -0.09044715017080307, + -0.06357009708881378, + -0.08614201843738556, + -0.09205593913793564, + 0.0293959379196167, + 0.06513507664203644, + -0.09349967539310455, + 0.07413585484027863, + 0.07786517590284348, + -0.009642613120377064, + -0.0392284169793129, + 0.07549523562192917, + 0.059212576597929, + -0.07576089352369308, + -0.0649745985865593, + 0.08366977423429489, + 0.008084154687821865, + -0.02717365138232708, + -0.029798461124300957, + 0.09446582943201065, + -0.012926995754241943, + 0.08698822557926178, + 0.004940473940223455, + -0.012784969992935658, + -0.023058926686644554, + 0.028829706832766533, + 0.058387983590364456, + 0.024434149265289307, + 0.023169292137026787, + -0.05172818899154663, + -0.07996024936437607, + -0.04658503457903862, + 0.08656636625528336, + 0.08940503746271133, + 0.01622825860977173, + 0.08617103099822998, + -0.06762509793043137, + -0.048162173479795456, + -0.004065919201821089, + -0.07386770099401474, + 0.07864071428775787, + -0.03458273410797119, + -0.061582256108522415, + 0.09802073240280151, + -0.009779644198715687, + -0.0743241086602211, + 0.044352032244205475, + -0.040940023958683014, + -0.07911548763513565, + -0.03838908672332764, + 0.044205404818058014, + -0.06801752001047134, + 0.06352364271879196, + 0.015054309740662575, + -0.07199881225824356, + 0.014658212661743164, + -0.08582383394241333, + 0.08640106767416, + -0.09977733343839645, + -0.06289412826299667, + 0.08261722326278687, + -0.050750732421875, + -0.03644021973013878, + 0.046221841126680374, + 0.08906286209821701, + 0.017063140869140625, + -0.06804671138525009, + 0.08603443950414658, + -0.05672280862927437, + 0.0028892995323985815, + -0.036512888967990875, + 0.013175249099731445, + 0.03585414960980415, + 0.08900487422943115, + -0.09577719122171402, + 0.0565030463039875, + -0.06440498679876328, + -0.011400437913835049, + 0.014285111799836159, + 0.09614016860723495, + -0.09369521588087082, + 0.031447745859622955, + -0.09402529150247574, + 0.0787765309214592, + 0.030794739723205566, + -0.026997996494174004, + -0.0543629415333271, + 0.015773450955748558, + 0.06633491814136505, + 0.08232428133487701, + 0.02939009666442871, + -0.08830323070287704, + -0.09033264964818954, + -0.0017514824867248535, + 0.03475460037589073, + -0.04924731329083443, + 0.0434819720685482, + -0.031098080798983574, + 0.0637674331665039, + -0.06308324635028839, + 0.07233884185552597, + 0.07040169090032578, + -0.02829529158771038, + 0.023915303871035576, + 0.011319256387650967, + -0.0036240338813513517, + -0.08577024936676025, + -0.04389088228344917, + -0.013758850283920765, + -0.06473036110401154, + -0.023210465908050537, + -0.09655153006315231, + 0.060141194611787796, + 0.06681670993566513, + 0.027166618034243584, + 0.08695876598358154, + -0.06604985147714615, + 0.024394584819674492, + -0.014177262783050537, + -0.039409566670656204, + 0.003288710257038474, + -0.037664711475372314, + -0.03519705682992935, + 0.011927127838134766, + -0.03361406549811363, + 0.010974431410431862, + -0.06773481518030167, + 0.02630317211151123, + -0.08369409292936325, + 0.083478644490242, + 0.0990869551897049, + 0.017670845612883568, + 0.07239817827939987, + -0.0004896283498965204, + -0.031483449041843414, + 0.04860737547278404, + 0.019183708354830742, + 0.05684603378176689, + -0.033814456313848495, + 0.09340248256921768, + 0.053109217435121536, + -0.09801464527845383, + 0.002619576407596469, + 0.065671406686306, + -0.02876828983426094, + 0.039978016167879105, + 0.007457566447556019, + -0.06877227872610092, + -0.05707237869501114, + 0.03325716406106949, + -0.09724774211645126, + 0.07874005287885666, + 0.06798931211233139, + -0.022149313241243362, + -0.089079849421978, + 0.051343441009521484, + -0.0276468638330698, + 0.02570437267422676, + -0.0693320631980896, + -0.04721296951174736, + -0.057297635823488235, + -0.01642787456512451, + 0.05784428119659424, + -0.011216056533157825, + 0.006887924857437611, + -0.09863735735416412 + ], + [ + 0.06492900848388672, + -0.05018528923392296, + 0.062064457684755325, + -0.07700427621603012, + -0.001814508461393416, + 0.06802570819854736, + 0.09081774950027466, + 0.08420437574386597, + -0.0747893825173378, + 0.04318049177527428, + 0.02780994214117527, + 0.05061762407422066, + 0.04242526367306709, + -0.052467454224824905, + 0.09635405987501144, + 0.09998288005590439, + 0.03605765104293823, + -0.054834987968206406, + -0.018531108275055885, + -0.020158493891358376, + -0.007564544677734375, + 0.015007758513092995, + 0.04752645641565323, + -0.017624545842409134, + -0.06900528818368912, + -0.061820197850465775, + 0.0006124496576376259, + 0.08550550788640976, + -0.07483124732971191, + -0.027611935511231422, + -0.06230641528964043, + -0.08897799253463745, + -0.024844860658049583, + 0.02247229777276516, + -0.02584472857415676, + -0.05675327777862549, + 0.0214140173047781, + -0.0405544638633728, + 0.09345785528421402, + 0.02518298663198948, + 0.09620445221662521, + -0.03274405002593994, + 0.030910074710845947, + 0.004498720169067383, + -0.04450932890176773, + 0.002562320325523615, + -0.08341898024082184, + -0.04559285566210747, + 0.04857748746871948, + -0.09664980322122574, + -0.04028545692563057, + -0.065253846347332, + 0.09295113384723663, + -0.014491403475403786, + -0.013923239894211292, + -0.07172244042158127, + 0.016752421855926514, + 0.0895860567688942, + 0.0095442533493042, + -0.06746995449066162, + 0.058474134653806686, + 0.006632709410041571, + -0.0405343659222126, + -0.013835894875228405, + -0.030789125710725784, + -0.08558028936386108, + 0.017471028491854668, + 0.022224759683012962, + 0.0898694172501564, + -0.03353699669241905, + -0.06690653413534164, + 0.03363626077771187, + 0.0630890280008316, + 0.03878751024603844, + -0.09384393692016602, + 0.03938739374279976, + 0.05119385942816734, + -0.0706164762377739, + -0.007488727569580078, + 0.0720205083489418, + -0.07134796679019928, + 0.021893275901675224, + 0.007097959518432617, + -0.009904086589813232, + -0.07605725526809692, + -0.018921231850981712, + -0.08478587865829468, + -0.0780492052435875, + 0.02330777607858181, + -0.09622656553983688, + -0.07305561751127243, + -0.06052671745419502, + 0.07492294162511826, + 0.03955468162894249, + 0.07775330543518066, + -0.09507244825363159, + -0.079750657081604, + -0.028709782287478447, + 0.08950980007648468, + -0.0020228028297424316, + 0.08796421438455582, + -0.04481114074587822, + 0.031032312661409378, + -0.08870857954025269, + -0.09912276268005371, + 0.06978638470172882, + -0.09709373861551285, + 0.027917003259062767, + 0.091474249958992, + -0.07607757300138474, + 0.0995761975646019, + -0.0551193468272686, + -0.09306540340185165, + -0.038656651973724365, + -0.05885440111160278, + -0.04006587341427803, + -0.08847039192914963, + -0.02173534594476223, + 0.04869749769568443, + -0.0057662129402160645, + 0.06484593451023102, + 0.08721861988306046, + 0.004239571280777454, + 0.049637842923402786, + 0.08774683624505997, + 0.07212910801172256, + 0.08324874937534332, + -0.08991385996341705, + -0.09059523791074753, + -0.02483832836151123, + 0.023088349029421806, + -0.05824993923306465, + -0.07958310842514038, + 0.040257442742586136, + 0.01511386688798666, + 0.009482502937316895, + 0.039411511272192, + -0.03541206195950508, + 0.03691822290420532, + -0.04472539573907852, + -0.08181484788656235, + -0.01677536964416504, + 0.09425091743469238, + 0.07006136327981949, + 0.03818264231085777, + -0.08912372589111328, + 0.054737210273742676, + -0.08210381120443344, + -0.002013945719227195, + -0.0037061572074890137, + -0.007321989629417658, + -0.08071422576904297, + 0.03466961532831192, + 0.02558763138949871, + -0.04179755598306656, + 0.06225713714957237, + 0.03437762334942818, + -0.03198070451617241, + -0.0964009165763855, + -0.033636510372161865, + -0.029756272211670876, + -0.02094721794128418, + 0.09252848476171494, + 0.06269992887973785, + -0.01583796739578247, + -0.07114565372467041, + 0.0399775393307209, + -0.044922519475221634, + -0.09026225656270981, + 0.09108704328536987, + -0.06952458620071411, + -0.08122807741165161, + 0.0780658945441246, + -0.0601351372897625, + 0.004659926984459162, + 0.03154730796813965, + 0.09482917934656143, + 0.05946650728583336, + 0.07316402345895767, + 0.06975121796131134, + 0.04988523945212364, + 0.0017639637226238847, + 0.03666914626955986, + -0.007309675216674805, + -0.05546088144183159, + 0.07015790790319443, + 0.02920314110815525 + ] + ] + }, + "observation": [ + [ + -1.4069499969482422, + 1.0726349353790283, + 0.049436938017606735, + 0.20770153403282166, + 1.5801247358322144, + -2.01362943649292, + 0.6198207139968872, + -0.7186319231987, + 0.36630865931510925, + -0.5404043197631836, + 1.0749247074127197, + 0.509365975856781, + 1.6998757123947144, + 2.0931973457336426, + -0.06317567080259323, + 0.708791971206665, + 0.03769537806510925, + 0.10582129657268524, + 1.7412108182907104, + -0.010211399756371975, + 2.36370849609375, + 0.040722012519836426, + -0.1632193922996521, + -1.0978645086288452, + -0.7324596643447876, + -0.8262237310409546, + 1.1033231019973755, + -2.3885152339935303, + -1.8009309768676758, + 0.5275406241416931, + -0.18630656599998474, + -1.3558411598205566, + 1.4676485061645508, + -0.21312719583511353, + 1.3940887451171875, + 0.7675103545188904, + -0.8174532055854797, + 0.46711334586143494, + 0.11748413741588593, + 1.6193759441375732, + -2.2571065425872803, + 0.7601176500320435, + 0.7368858456611633, + 0.1756371557712555, + 0.7534976601600647, + -0.7085027694702148, + -0.855893075466156, + 0.7384267449378967, + 0.5709915161132812, + 0.7639805674552917, + 0.7299700975418091, + -1.5501199960708618, + -1.1869412660598755, + -0.27159300446510315, + -0.14697915315628052, + -2.0353751182556152, + 2.0110998153686523, + -0.9374741315841675, + 1.8839247226715088, + 0.3856588900089264, + -0.3548334538936615, + 1.0500422716140747, + -0.7178040146827698, + 0.4010559320449829, + 1.2236113548278809, + -2.367070198059082, + -1.1743779182434082, + -0.11067777872085571, + -0.2934943437576294, + -0.024584054946899414, + -2.2199034690856934, + 0.8375236392021179, + -0.6767169237136841, + 0.42656809091567993, + -1.7919156551361084, + 0.7142887711524963, + -0.6991146802902222, + 0.5482228398323059, + -1.4923269748687744, + -0.9754637479782104, + 0.1465175747871399, + 0.06860022246837616, + 0.31803399324417114, + -0.23632872104644775, + -0.897682785987854, + 0.29926350712776184, + -4.368893146514893, + 0.9160680174827576, + 4.984935283660889, + 0.1185097023844719, + 0.622482419013977, + -3.977832794189453, + -7.159829616546631, + -2.5309689044952393, + 4.721494674682617, + -2.8818554878234863, + -4.431326389312744, + -0.8927403092384338, + 3.247561454772949, + -8.902396202087402, + -1.2800079584121704, + -0.15178997814655304, + 3.9192891120910645, + 4.775058746337891, + -1.6756649017333984, + 3.9211177825927734, + -0.003280824515968561, + 6.563349723815918, + -1.1090047359466553, + 0.2747873067855835, + -4.711472034454346, + 1.2407253980636597, + -1.8140769004821777, + -0.3151421844959259, + -1.636247158050537, + 2.5366387367248535, + 2.516878843307495, + -1.2603262662887573, + -1.2249360084533691, + -0.07307285815477371, + 1.0792949199676514, + 0.6049757599830627, + -1.3881919384002686, + -0.23492558300495148, + -0.3690822124481201, + -0.9713019728660583, + -0.22935113310813904, + -0.6258819699287415, + -0.23511004447937012, + -0.08500783145427704, + -1.0, + -0.07460318505764008, + -0.5961368680000305, + -0.7958902716636658, + -0.7372646331787109, + -0.6263784766197205, + -0.9133599996566772, + -0.08750786632299423, + -0.23758938908576965, + -0.3990436792373657, + -0.8485054969787598, + -0.04837927967309952, + -0.12096543610095978, + -0.9025225043296814, + 0.022938072681427002, + -0.6172236800193787, + -0.11949606984853745, + -0.7126899361610413, + -0.05242621898651123, + -0.8628233671188354, + -0.41178834438323975, + -0.6643850207328796, + -0.9799152612686157, + -0.15816189348697662, + -1.0, + -0.8736452460289001, + -0.9476940035820007, + -0.4822450876235962, + -0.42991021275520325, + -0.19674810767173767, + -0.5494603514671326, + -0.31622016429901123, + -0.03983708471059799, + -0.543060839176178, + -0.5140841603279114, + -0.4160575568675995, + -0.007380019873380661, + -0.07551445811986923, + -0.620854377746582, + -0.11482659727334976, + -0.9045780897140503, + -0.5861231684684753, + -0.26507285237312317, + -0.9278913736343384, + -0.19200792908668518, + -0.6957923173904419, + -0.27295348048210144, + -0.18700772523880005, + -0.05285229533910751, + -0.3740500807762146, + -0.49943092465400696, + -0.43310168385505676, + -0.1758802980184555, + -0.9541838765144348, + -0.5258646011352539, + -0.7200484871864319, + -0.2434876561164856, + -0.8165240287780762, + -1.0, + -0.42583346366882324, + -0.4432119131088257, + -0.4066895842552185, + -0.6966076493263245, + -1.0, + -0.915302038192749, + 0.018335331231355667, + -0.3675485849380493, + -0.19474241137504578, + -0.70863276720047, + -0.308053195476532, + 0.021491937339305878, + -0.734555721282959, + -0.4287946820259094, + -0.8190137147903442, + -0.174106165766716, + -1.0, + -0.4624946713447571, + -0.1576959490776062, + -0.9789081811904907, + -0.8523783683776855, + -0.6040578484535217, + -0.3257391154766083, + -0.2313000112771988, + -0.22202253341674805, + -0.6310192942619324, + -0.1516180783510208, + -0.9837287664413452, + -0.18121132254600525, + -1.0, + -0.8227438926696777, + -0.3234846591949463, + -0.04372211545705795, + -0.5654221177101135, + -0.924054741859436, + -0.839948296546936, + -0.2116660177707672, + -0.08218534290790558, + -0.802496075630188, + -0.04184497520327568, + -0.7045097947120667, + -0.6210234761238098, + -0.23332610726356506, + -0.6844813227653503, + -0.7635685801506042, + -0.14865607023239136, + -0.7068381309509277, + -0.06114513427019119, + -0.12538956105709076, + -0.31292924284935, + -0.5394546985626221, + -0.67376708984375, + -0.5940524935722351, + -0.811767578125, + -0.5034485459327698, + -0.3999008536338806, + 0.010425448417663574, + -0.8841590285301208, + -0.34156322479248047, + -0.07080978900194168, + -0.13235896825790405, + -0.12883885204792023, + -0.4458642601966858, + -0.346724271774292, + -0.17780420184135437, + -0.08626529574394226, + -0.05570312589406967, + -0.3425547778606415, + -1.0, + -0.3911460041999817, + -0.11732953786849976, + -0.6412492990493774, + -0.05862497165799141, + -0.5735750794410706, + -0.4311599135398865, + -0.08645355701446533, + -0.32891371846199036, + -0.3082035183906555, + -0.20590965449810028, + -0.2355968952178955, + -0.7337773442268372, + -0.9499973058700562, + 0.012812629342079163, + -1.0, + -0.7039413452148438, + -0.6378687024116516, + -0.1633273959159851, + -0.7168658375740051, + -0.9818416237831116, + -0.41380274295806885, + -1.0, + -0.45019081234931946, + -0.2783898711204529, + -0.9580066800117493, + -0.6039050817489624, + -0.7058929204940796, + -0.19464150071144104, + -0.5166842937469482, + -0.8963528275489807, + -0.9048511981964111, + -0.6705685257911682, + -0.39719247817993164, + -0.7142139673233032, + -0.5789967179298401, + -0.15488079190254211, + -0.7415249347686768, + -0.053867265582084656, + -1.0, + -0.7698261737823486, + -0.47435063123703003, + -0.18163345754146576, + -0.9208264946937561, + -0.280893474817276, + -1.0, + -0.4571278989315033, + -0.3129851818084717, + -0.1692306399345398, + -0.40727099776268005, + -0.831913948059082, + -0.8596212863922119, + -0.3149305582046509 + ], + [ + -0.38794785737991333, + -0.1370657980442047, + 0.3916037082672119, + -0.029683813452720642, + 0.3664757311344147, + 1.0797909498214722, + 0.20487499237060547, + 0.9954610466957092, + 0.043222248554229736, + -0.9210910201072693, + 0.24086834490299225, + -0.13246694207191467, + 0.5249882936477661, + 0.3510620594024658, + -0.1895255595445633, + -0.8441972136497498, + 0.16236089169979095, + -0.03734248876571655, + 1.07917058467865, + -0.37964552640914917, + -0.6994788646697998, + 0.7010900974273682, + 1.9037379026412964, + 0.145101398229599, + 0.578916609287262, + -1.2035285234451294, + 1.3468708992004395, + 0.7779930233955383, + -0.4923069179058075, + -1.1138312816619873, + -1.0059609413146973, + -0.683100163936615, + 0.7017560005187988, + -0.027139026671648026, + -0.5922361612319946, + -1.3698062896728516, + -0.8508687615394592, + -0.002800464630126953, + 0.9882587194442749, + 2.3288991451263428, + -2.7383511066436768, + -0.5574095249176025, + 0.14649294316768646, + -1.2968852519989014, + -0.49687111377716064, + -1.4662840366363525, + -0.8887143731117249, + 2.0657145977020264, + -1.0418304204940796, + 0.925268292427063, + -0.9827067255973816, + -0.5835732817649841, + 0.9494291543960571, + -0.9632630944252014, + -0.16914308071136475, + 0.6905560493469238, + -0.517609179019928, + -1.4099457263946533, + 0.991094708442688, + 0.45137912034988403, + 1.086907982826233, + -0.08564960956573486, + 0.5591758489608765, + 2.0099101066589355, + -2.1455254554748535, + 2.7763161659240723, + 0.34471726417541504, + 1.8947807550430298, + -1.210347056388855, + -1.5783727169036865, + -0.9104133248329163, + 0.6619883179664612, + -0.17619723081588745, + 1.3315041065216064, + -1.672218680381775, + 0.07152633368968964, + 0.9996780157089233, + -0.23330987989902496, + -1.0184625387191772, + 1.9776440858840942, + -0.12138926982879639, + 0.2517191767692566, + -0.3275165557861328, + 0.9971603155136108, + 0.8022367358207703, + 0.09318029880523682, + 2.340679407119751, + -3.2012264728546143, + 1.449082612991333, + -0.44263583421707153, + 6.466547012329102, + 4.260446548461914, + -1.1634079217910767, + -0.3325357735157013, + 0.6242257952690125, + 0.10043658316135406, + -3.3272321224212646, + 4.066169261932373, + 2.077775716781616, + -5.893347263336182, + 2.7362892627716064, + 0.7733586430549622, + 1.8175424337387085, + 5.035777568817139, + 1.0537075996398926, + -1.1087294816970825, + 1.8238623142242432, + -1.5047787427902222, + -5.391904830932617, + -4.920398235321045, + 1.9996800422668457, + 2.8552744388580322, + 2.4058778285980225, + 0.7430681586265564, + 7.5287909507751465, + -2.3544540405273438, + -1.5701937675476074, + 3.8395838737487793, + -4.007706642150879, + 3.4519639015197754, + -3.863215684890747, + 1.0905109643936157, + 2.3868038654327393, + -0.9103783965110779, + -0.19505473971366882, + -0.10260522365570068, + -0.41634348034858704, + -0.45699265599250793, + -0.7384158968925476, + -0.6701151132583618, + -0.9452866911888123, + -0.7775859832763672, + -0.9064251780509949, + -0.9214029908180237, + -0.07540760934352875, + -0.022213079035282135, + -0.3196488618850708, + -0.07287979125976562, + -0.21765033900737762, + -0.7180635929107666, + -0.7793059945106506, + -0.4629930257797241, + -0.6308144927024841, + -0.3786342442035675, + -0.19516518712043762, + -0.7766192555427551, + -0.1054215058684349, + -0.07105736434459686, + -0.32215216755867004, + -0.9454934000968933, + -0.14200347661972046, + -0.4426944851875305, + -0.09435571730136871, + -0.6981706619262695, + -0.714574933052063, + -0.8220884799957275, + -0.020774567499756813, + -0.05925135686993599, + -0.14348141849040985, + -0.1359003633260727, + -0.5076146125793457, + -0.14266537129878998, + -0.5541970133781433, + -1.0, + -0.6100822687149048, + -0.7912922501564026, + -0.8179871439933777, + -0.2733459174633026, + -0.6463069915771484, + -0.6909502148628235, + -0.800149142742157, + 0.07931577414274216, + -0.19603633880615234, + -0.24660661816596985, + -0.16227400302886963, + -0.6857661008834839, + -0.9151913523674011, + -0.45301687717437744, + -0.848487913608551, + 0.0009969472885131836, + -0.7977797985076904, + -0.6922116875648499, + -0.23594555258750916, + -0.20967194437980652, + -0.7629252672195435, + -0.16441509127616882, + -0.8576058149337769, + -0.126357764005661, + -0.8573753833770752, + -0.6070653796195984, + -0.26855653524398804, + -0.8400996923446655, + -0.48588067293167114, + -0.811348557472229, + -0.8775851130485535, + -0.3496658205986023, + -0.8589067459106445, + -0.6095759272575378, + -0.5401374101638794, + -0.40564608573913574, + -0.6214451193809509, + -0.29186564683914185, + -0.7152422666549683, + -0.9170634746551514, + -0.27579519152641296, + -0.06625370681285858, + -0.7209383845329285, + -0.7115288972854614, + -0.1834409534931183, + -0.0961163118481636, + -0.11335516721010208, + -0.3461557626724243, + -0.8444807529449463, + -0.48504137992858887, + -0.5985696911811829, + -0.1196998655796051, + -0.9013625979423523, + -0.911704957485199, + -0.4856427311897278, + -0.8275445103645325, + -0.5203327536582947, + -0.9195386171340942, + -0.4958841800689697, + -0.4281412959098816, + -0.8728324174880981, + -0.2123136669397354, + -0.8262885212898254, + -0.4119468033313751, + -0.39163535833358765, + -0.5014828443527222, + -0.3782762885093689, + -0.9928075671195984, + -0.1434786468744278, + -0.30340826511383057, + -0.5806421637535095, + -0.24366503953933716, + -0.6400749683380127, + -0.9180578589439392, + -0.9837220311164856, + -0.45688652992248535, + -0.15726256370544434, + -0.2316073477268219, + -0.15017849206924438, + -0.3060339391231537, + -0.9884517192840576, + 0.021866466850042343, + -0.5724270343780518, + -0.28866350650787354, + -0.5566734671592712, + -0.033361684530973434, + -0.3611661493778229, + -0.5923790335655212, + -0.17932865023612976, + -0.2627536356449127, + -0.3238305449485779, + -0.8423798084259033, + -0.7920362949371338, + -0.5509073734283447, + -0.12431726604700089, + -0.6155748963356018, + -0.22372117638587952, + -0.3908129334449768, + -0.5759317874908447, + -0.0803121030330658, + -0.7622469067573547, + -0.8714749217033386, + -0.39334923028945923, + -0.6293537616729736, + -0.40940454602241516, + -0.38188257813453674, + -0.14566585421562195, + -0.9265192747116089, + -0.2952314615249634, + -0.2678546905517578, + -0.6931930780410767, + -0.9485383033752441, + -0.9329493641853333, + -0.33641597628593445, + -0.41597437858581543, + -0.44257867336273193, + -0.3506227135658264, + -0.05277865007519722, + -0.6096024513244629, + -0.8863778114318848, + -0.5956210494041443, + -0.14553770422935486, + -0.5615997314453125, + -0.8012797236442566, + -0.584685206413269, + -0.9074112176895142, + -0.04928097873926163, + -0.9642680883407593, + 0.06600191444158554, + -0.8205994367599487, + -0.3589116930961609, + -0.7235649228096008, + -0.8740903735160828, + -0.5379430651664734, + -0.42176398634910583, + -1.0, + -0.6490617990493774, + -0.6822301745414734, + -0.34172555804252625, + -0.90566086769104, + -0.9186926484107971, + -0.5314429998397827, + -0.8098918795585632, + -0.9099903106689453, + -0.8432511687278748, + -0.2976074814796448 + ], + [ + -0.34489521384239197, + 1.8047900199890137, + 1.1037474870681763, + 0.06741125881671906, + 0.2864755392074585, + 0.9052565097808838, + 0.30543091893196106, + 0.19615589082241058, + 0.9578755497932434, + -0.2072896957397461, + 1.3115217685699463, + -0.05750608071684837, + -1.4695765972137451, + -1.3935120105743408, + 0.11923523247241974, + -1.0076990127563477, + -1.1663349866867065, + 0.6443485021591187, + 0.6754648685455322, + -1.8998386859893799, + 1.5639349222183228, + 0.67162024974823, + 0.5064153671264648, + -1.2969621419906616, + 0.3411876857280731, + -0.4930904507637024, + -0.16637907922267914, + 1.955784797668457, + -0.8513618111610413, + 0.8695812225341797, + -0.4618198871612549, + -0.45110780000686646, + -1.2165738344192505, + 1.756473183631897, + -0.9883906245231628, + -0.36364105343818665, + -0.8311875462532043, + -1.6740882396697998, + 0.006809484213590622, + -0.9013964533805847, + -0.6993459463119507, + -0.59490966796875, + 1.1761394739151, + 0.9036130905151367, + 0.9784457087516785, + 0.024272775277495384, + 1.2440671920776367, + 0.2982538044452667, + -0.31751030683517456, + 1.8425674438476562, + -2.1279940605163574, + 0.6066297292709351, + 1.8573932647705078, + 1.0225844383239746, + 0.6322453022003174, + 1.9611287117004395, + -1.029683232307434, + -0.6289929747581482, + -0.29764923453330994, + 0.5477957129478455, + 0.2375255823135376, + -1.5101028680801392, + 1.4332301616668701, + 1.9601681232452393, + -3.6899094581604004, + 0.1235198974609375, + -1.2099766731262207, + -0.14447900652885437, + 0.5947220921516418, + -0.7086330652236938, + -1.3694859743118286, + 1.0131797790527344, + -3.2103984355926514, + 1.7276785373687744, + 1.1196308135986328, + 0.7843965888023376, + 1.056311845779419, + 0.10424810647964478, + 0.40316128730773926, + 0.27374452352523804, + 2.5219497680664062, + -0.3305432200431824, + 0.5116696357727051, + 1.523533582687378, + 0.027112722396850586, + 1.762458086013794, + -1.9206892251968384, + -0.5940742492675781, + -0.3497787117958069, + 3.7373969554901123, + 8.128355026245117, + 4.392646312713623, + -3.1500251293182373, + 1.746557354927063, + -1.877929925918579, + -6.652039051055908, + 4.322165012359619, + 3.399026393890381, + -5.671651840209961, + -0.2512320280075073, + 7.311364650726318, + -0.06858460605144501, + 5.1759772300720215, + 3.3528430461883545, + -0.6859660744667053, + 3.756129503250122, + 3.4173645973205566, + -8.679512023925781, + 0.5880194902420044, + -0.7168394923210144, + 6.000116348266602, + -2.446148157119751, + -6.562048435211182, + -3.665959119796753, + 1.803282380104065, + 12.5444974899292, + 2.0668933391571045, + -11.293632507324219, + 1.1310479640960693, + -2.0034444332122803, + -9.082246780395508, + 4.693811893463135, + -3.442373037338257, + -1.0, + -0.685845673084259, + -1.0, + -0.584886908531189, + -1.0, + -0.8742254972457886, + -0.9202467799186707, + -0.7452614307403564, + -1.0, + -1.0, + -0.6816423535346985, + -1.0, + -0.8715258240699768, + -1.0, + -1.0, + -1.0, + -0.6333229541778564, + -1.0, + -0.904778242111206, + -0.7241160273551941, + -1.0, + -1.0, + -1.0, + -1.0, + -0.8843035697937012, + -1.0, + -1.0, + -1.0, + -1.0, + -0.8634516000747681, + -1.0, + -0.8252471089363098, + -0.5194064378738403, + -0.6099323630332947, + -1.0, + -0.9475077986717224, + -1.0, + -1.0, + -0.5553187131881714, + -1.0, + -0.6059309244155884, + -0.9458181858062744, + -0.4818516969680786, + -0.9460919499397278, + -1.0, + -1.0, + -0.9697843790054321, + -1.0, + -1.0, + -0.7075584530830383, + -1.0, + -1.0, + -1.0, + -1.0, + -0.6812188029289246, + -0.9522886872291565, + -1.0, + -1.0, + -1.0, + -1.0, + -1.0, + -0.8056771159172058, + -1.0, + -1.0, + -0.7641001343727112, + -0.6427850723266602, + -1.0, + -1.0, + -1.0, + -0.6212852597236633, + -0.8945595026016235, + -0.8965728878974915, + -1.0, + -1.0, + -0.9101439118385315, + -1.0, + -1.0, + -0.7547416687011719, + -0.7603371143341064, + -1.0, + -1.0, + -0.46505534648895264, + -1.0, + -1.0, + -1.0, + -0.49506258964538574, + -0.9975337982177734, + -1.0, + -0.8607315421104431, + -0.9829310178756714, + -0.5331006050109863, + -1.0, + -0.7212792634963989, + -0.5961570739746094, + -0.9424959421157837, + -1.0, + -0.5863071084022522, + -0.7821338176727295, + -1.0, + -0.5284334421157837, + -0.4802273213863373, + -0.7279887199401855, + -0.5152583718299866, + -1.0, + -1.0, + -1.0, + -1.0, + -1.0, + -1.0, + -0.7007086277008057, + -0.4739381670951843, + -0.9725310802459717, + -0.8892377614974976, + -1.0, + -1.0, + -0.6845777034759521, + -0.8285402655601501, + -1.0, + -0.8019717931747437, + -1.0, + -0.5801212787628174, + -0.6002084612846375, + -1.0, + -1.0, + -0.8536720871925354, + -1.0, + -0.8579444289207458, + -1.0, + -0.6128086447715759, + -1.0, + -1.0, + -1.0, + -0.6372742652893066, + -1.0, + -0.8093410134315491, + -0.7717421054840088, + -1.0, + -1.0, + -0.5939581990242004, + -1.0, + -1.0, + -1.0, + -0.9777121543884277, + -1.0, + -0.8467455506324768, + -1.0, + -0.7379229068756104, + -0.5979374051094055, + -0.8221296072006226, + -0.7297700047492981, + -0.825773298740387, + -1.0, + -1.0, + -0.8754944801330566, + -0.7822666764259338, + -1.0, + -0.8745867013931274, + -0.583448052406311, + -0.6499899625778198, + -0.5839015245437622, + -0.5221040844917297, + -0.7903580069541931, + -0.7605628371238708, + -1.0, + -1.0, + -0.5448801517486572, + -0.7922901511192322, + -1.0, + -0.5906552076339722, + -0.614315927028656, + -1.0, + -1.0, + -0.9694905877113342, + -1.0, + -0.7324687242507935, + -1.0, + -0.5028210282325745, + -1.0, + -0.8549847602844238, + -0.805195689201355, + -1.0, + -1.0, + -0.5914736390113831, + -0.9870001077651978, + -0.5894870162010193, + -1.0, + -1.0 + ], + [ + 0.021648570895195007, + 0.11086653918027878, + -1.2490181922912598, + -1.643565058708191, + -0.7634243965148926, + -0.3057655394077301, + -0.39293476939201355, + 0.32056206464767456, + 0.8794295787811279, + 1.3140007257461548, + 0.15788041055202484, + -0.9369820952415466, + -0.33969563245773315, + -0.7839319109916687, + -0.9978094696998596, + -0.037705715745687485, + 0.9311642050743103, + 1.9204590320587158, + 2.471158981323242, + -0.2574692666530609, + 0.3723033368587494, + -0.9296101927757263, + 0.9741128087043762, + -0.6333039999008179, + 2.0450692176818848, + 0.31273964047431946, + -0.7360443472862244, + -0.9088994264602661, + -2.888251781463623, + 0.2500144839286804, + 1.0238547325134277, + 0.9109290838241577, + -0.36113137006759644, + -0.24050182104110718, + 0.9641605615615845, + -0.08623011410236359, + -1.3418458700180054, + -0.37914469838142395, + 1.0256342887878418, + -1.3035515546798706, + -0.0034576833713799715, + -0.5798326730728149, + -1.9055631160736084, + -1.0632941722869873, + -1.4172788858413696, + -0.8124759197235107, + 0.029615677893161774, + 1.746158242225647, + -0.5724268555641174, + 0.6349905729293823, + 1.711187720298767, + -3.042208194732666, + -0.46432942152023315, + 1.0053707361221313, + -0.7862245440483093, + 1.9614495038986206, + -0.5531682372093201, + 0.7538347244262695, + 1.355606198310852, + -1.9909489154815674, + 0.8867899179458618, + -1.7046852111816406, + -1.219997763633728, + 0.2828878164291382, + 0.2721024751663208, + -1.1090028285980225, + 1.1575616598129272, + -0.5123422741889954, + 0.3213130235671997, + 0.9234911799430847, + 0.5037670731544495, + -1.3494231700897217, + -1.33601713180542, + 0.39544299244880676, + 2.820998430252075, + -2.417839527130127, + 0.3972512483596802, + 0.4367092251777649, + 0.046646833419799805, + 0.764735758304596, + 1.8805278539657593, + -1.2150758504867554, + 1.2275629043579102, + -1.5455503463745117, + -1.9381508827209473, + 2.3605196475982666, + -4.359130382537842, + 1.4358174800872803, + 1.2535760402679443, + 2.251012086868286, + 4.294274806976318, + -2.0001187324523926, + -0.383171945810318, + 2.659695863723755, + 1.6124097108840942, + -1.9515864849090576, + -2.035461187362671, + 0.10162672400474548, + 3.356719970703125, + -7.295328617095947, + 3.0563695430755615, + -7.556408882141113, + 2.0578768253326416, + -1.689565658569336, + 1.8390171527862549, + -4.7228007316589355, + -6.407613754272461, + 1.15846586227417, + -0.7323806881904602, + 2.3890063762664795, + 5.626026630401611, + -3.9759743213653564, + 7.224872589111328, + -2.195585250854492, + 4.695905685424805, + 0.1920173466205597, + -4.04601526260376, + -1.238318920135498, + 4.002810478210449, + -0.024696817621588707, + -10.090450286865234, + -0.7411140203475952, + 1.8331068754196167, + -0.8588852882385254, + -0.12000587582588196, + -0.07829000055789948, + -0.26238498091697693, + -0.7930698394775391, + -0.3062809109687805, + -0.2616960406303406, + -0.5209911465644836, + -0.6751645803451538, + -0.9858160018920898, + -0.9765538573265076, + -0.40999072790145874, + -0.949596107006073, + -0.809458315372467, + -0.41065651178359985, + 0.022172890603542328, + -0.9218419194221497, + -0.8851348757743835, + -0.6191647052764893, + -0.6838260293006897, + -0.27015626430511475, + -0.28439369797706604, + -0.4483143389225006, + -0.6519970297813416, + -0.6240729093551636, + -0.622962236404419, + -0.4471326470375061, + -0.6504754424095154, + -0.16102248430252075, + -0.49730774760246277, + -0.16853000223636627, + -1.0, + -0.8103002309799194, + -0.2714977562427521, + -0.9736484885215759, + -0.3775371313095093, + -0.886430025100708, + -1.0, + -0.7137458920478821, + -0.39562878012657166, + -0.016567178070545197, + -0.5946410298347473, + -0.6929835677146912, + -0.2073988914489746, + -0.543705403804779, + -0.5376139283180237, + -0.5710014700889587, + -0.09642495214939117, + -0.3304770588874817, + -0.44525977969169617, + -0.6170762181282043, + -0.7338011264801025, + 0.02390940487384796, + -0.07481183856725693, + -0.665382444858551, + -1.0, + -0.4099988341331482, + 0.010884441435337067, + -0.6651903986930847, + -0.6990861892700195, + -0.4349874258041382, + -0.32783183455467224, + -0.6036641597747803, + -0.7900190949440002, + -0.11046652495861053, + -1.0, + -0.6925668716430664, + -0.34879162907600403, + -0.8782488703727722, + -0.5682380199432373, + -0.9800834059715271, + -0.4090270400047302, + -0.3919837474822998, + -0.07414892315864563, + -0.648859441280365, + -0.5527180433273315, + 0.011449780315160751, + -0.7731172442436218, + -1.0, + -0.6902481913566589, + -0.7883185148239136, + -0.6893315315246582, + -0.5866058468818665, + -0.5757424235343933, + -0.7022665143013, + -0.757051408290863, + -0.14770907163619995, + -0.18841221928596497, + -0.42861852049827576, + -1.0, + -0.32470011711120605, + -0.6953398585319519, + -0.03315810114145279, + -0.16335049271583557, + -0.8017831444740295, + -0.8084328770637512, + -0.6814720034599304, + -0.5036560297012329, + -0.8244893550872803, + -0.4800563454627991, + -0.5604845285415649, + -0.6461589336395264, + -0.9902176856994629, + -0.7948294281959534, + -0.8496177792549133, + -0.8419174551963806, + -1.0, + -0.647766649723053, + -0.5725815296173096, + -0.3837815523147583, + -0.4961312711238861, + -0.3214208483695984, + -0.5196132659912109, + -0.7978526949882507, + -0.9722492098808289, + -0.7042089700698853, + -0.5987064242362976, + -0.20291516184806824, + -0.3818061053752899, + -0.4270023703575134, + -0.35725367069244385, + -0.7883031368255615, + -0.9690461158752441, + -0.14593388140201569, + -0.3414042890071869, + -0.1882247030735016, + -0.5936936140060425, + -0.890923023223877, + -0.3775046467781067, + -0.631665050983429, + -0.884600043296814, + -0.7594558596611023, + -0.5674189925193787, + -0.8379567861557007, + -0.5318138599395752, + -0.6197255253791809, + -0.025011513382196426, + -0.4257727265357971, + -0.21494710445404053, + -0.4407477080821991, + -0.6578240394592285, + -0.9462990760803223, + -0.30160611867904663, + -0.01951809972524643, + -0.2996690571308136, + -0.13287609815597534, + -0.0949023962020874, + -0.3976077139377594, + -0.12704148888587952, + -0.7721202373504639, + -0.2809959948062897, + -0.6103824377059937, + -0.5549466013908386, + -0.7256402969360352, + -0.6237576007843018, + -0.3483113944530487, + -0.807233452796936, + -0.9468411803245544, + -0.5334764719009399, + -1.0, + -0.5353798270225525, + -0.7345051765441895, + -0.5101779103279114, + -0.3367990255355835, + -0.24724042415618896, + -0.8478929996490479, + -0.2892181873321533, + -0.10612396895885468, + -1.0, + -0.760975182056427, + -1.0, + -0.3273160457611084, + -0.8735707402229309, + -0.608842134475708, + -0.6254853010177612, + -0.6676983833312988, + -0.058519147336483, + -0.16334488987922668, + -0.3087630569934845, + -0.4241800308227539, + -0.20263323187828064, + -0.5543035864830017, + -0.25641965866088867, + -0.8670896291732788, + -0.6554582118988037, + -0.6505094170570374, + -0.6977919340133667 + ] + ], + "rewards": { + "track_lin_vel_xy_exp": [ + 1.47920191011508e-05, + 0.20297527313232422, + 8.514294717670238e-15, + 8.919614060687309e-07 + ], + "track_ang_vel_z_exp": [ + 0.001070165541023016, + 0.8154081106185913, + 0.16508378088474274, + 0.050110962241888046 + ], + "ang_vel_xy_l2": [ + 2.811472177505493, + 0.14616887271404266, + 0.09242473542690277, + 3.662100076675415 + ], + "dof_torques_l2": [ + 10865.8955078125, + 13946.0693359375, + 11106.2236328125, + 14253.060546875 + ], + "dof_acc_l2": [ + 13256993.0, + 9445130.0, + 3893592.0, + 9315391.0 + ], + "action_rate_l2": [ + 500.3487243652344, + 380.439453125, + 956.3876953125, + 655.1470336914062 + ], + "feet_air_time": [ + 0.0, + 0.0, + 0.009999999776482582, + 0.0 + ], + "flat_orientation_l2": [ + 0.8422490954399109, + 0.9992173314094543, + 0.13076509535312653, + 0.21791407465934753 + ], + "dof_pos_limits": [ + 0.8659737706184387, + 1.020351529121399, + 2.2473158836364746, + 2.237064838409424 + ], + "termination_penalty": [ + 0.0, + 1.0, + 1.0, + 1.0 + ], + "feet_slide": [ + 0.7694483399391174, + 2.6806671619415283, + 1.0911712646484375, + 3.377929210662842 + ], + "joint_deviation_hip": [ + 4.537287712097168, + 1.6166529655456543, + 4.990593433380127, + 2.418728828430176 + ], + "joint_deviation_arms": [ + 11.85978889465332, + 11.5762939453125, + 8.250176429748535, + 8.39795207977295 + ], + "joint_deviation_fingers": [ + 10.732789993286133, + 12.612953186035156, + 11.011817932128906, + 12.498083114624023 + ], + "joint_deviation_torso": [ + 0.7289369106292725, + 0.582530677318573, + 0.3363381326198578, + 2.0525288581848145 + ] + }, + "terminated": [ + false, + true, + true, + true + ] +} diff --git a/tests/gym/envs/tasks/fixtures/terrain_curriculum.json b/tests/gym/envs/tasks/fixtures/terrain_curriculum.json new file mode 100644 index 0000000000..6bcee6dea6 --- /dev/null +++ b/tests/gym/envs/tasks/fixtures/terrain_curriculum.json @@ -0,0 +1,3566 @@ +{ + "origins": [ + [ + [ + -36.0, + -76.0, + 0.4475184679031372 + ], + [ + -36.0, + -68.0, + 0.3967205584049225 + ], + [ + -36.0, + -60.0, + 0.4455190598964691 + ], + [ + -36.0, + -52.0, + 0.44384002685546875 + ], + [ + -36.0, + -44.0, + -0.4050811529159546 + ], + [ + -36.0, + -36.0, + -0.3751884400844574 + ], + [ + -36.0, + -28.0, + -0.43421876430511475 + ], + [ + -36.0, + -20.0, + -0.3770376741886139 + ], + [ + -36.0, + -12.0, + 0.059964701533317566 + ], + [ + -36.0, + -4.0, + 0.05228468030691147 + ], + [ + -36.0, + 4.0, + 0.06362871080636978 + ], + [ + -36.0, + 12.0, + 0.05683664605021477 + ], + [ + -36.0, + 20.0, + 0.10000000149011612 + ], + [ + -36.0, + 28.0, + 0.10000000149011612 + ], + [ + -36.0, + 36.0, + 0.10000000149011612 + ], + [ + -36.0, + 44.0, + 0.10000000149011612 + ], + [ + -36.0, + 52.0, + 0.009999999776482582 + ], + [ + -36.0, + 60.0, + 0.019999999552965164 + ], + [ + -36.0, + 68.0, + -0.07000000029802322 + ], + [ + -36.0, + 76.0, + -0.07500000298023224 + ] + ], + [ + [ + -28.0, + -76.0, + 0.5312986969947815 + ], + [ + -28.0, + -68.0, + 0.592772364616394 + ], + [ + -28.0, + -60.0, + 0.5206702947616577 + ], + [ + -28.0, + -52.0, + 0.597906231880188 + ], + [ + -28.0, + -44.0, + -0.580917477607727 + ], + [ + -28.0, + -36.0, + -0.47692763805389404 + ], + [ + -28.0, + -28.0, + -0.5353581309318542 + ], + [ + -28.0, + -20.0, + -0.527474582195282 + ], + [ + -28.0, + -12.0, + 0.07109580188989639 + ], + [ + -28.0, + -4.0, + 0.07544480264186859 + ], + [ + -28.0, + 4.0, + 0.07549560815095901 + ], + [ + -28.0, + 12.0, + 0.06803545355796814 + ], + [ + -28.0, + 20.0, + 0.10000000149011612 + ], + [ + -28.0, + 28.0, + 0.10000000149011612 + ], + [ + -28.0, + 36.0, + 0.10000000149011612 + ], + [ + -28.0, + 44.0, + 0.10000000149011612 + ], + [ + -28.0, + 52.0, + 0.15000000596046448 + ], + [ + -28.0, + 60.0, + 0.11999999731779099 + ], + [ + -28.0, + 68.0, + -0.15000000596046448 + ], + [ + -28.0, + 76.0, + -0.10000000149011612 + ] + ], + [ + [ + -20.0, + -76.0, + 0.710183322429657 + ], + [ + -20.0, + -68.0, + 0.6831269860267639 + ], + [ + -20.0, + -60.0, + 0.7243079543113708 + ], + [ + -20.0, + -52.0, + 0.6430540084838867 + ], + [ + -20.0, + -44.0, + -0.6902334094047546 + ], + [ + -20.0, + -36.0, + -0.7011524438858032 + ], + [ + -20.0, + -28.0, + -0.6732197403907776 + ], + [ + -20.0, + -20.0, + -0.7095288038253784 + ], + [ + -20.0, + -12.0, + 0.0922103077173233 + ], + [ + -20.0, + -4.0, + 0.08669234067201614 + ], + [ + -20.0, + 4.0, + 0.08398804813623428 + ], + [ + -20.0, + 12.0, + 0.08458934724330902 + ], + [ + -20.0, + 20.0, + 0.10000000149011612 + ], + [ + -20.0, + 28.0, + 0.10000000149011612 + ], + [ + -20.0, + 36.0, + 0.10000000149011612 + ], + [ + -20.0, + 44.0, + 0.10000000149011612 + ], + [ + -20.0, + 52.0, + 0.17499999701976776 + ], + [ + -20.0, + 60.0, + 0.18000000715255737 + ], + [ + -20.0, + 68.0, + -0.20000000298023224 + ], + [ + -20.0, + 76.0, + -0.17000000178813934 + ] + ], + [ + [ + -12.0, + -76.0, + 0.8158683776855469 + ], + [ + -12.0, + -68.0, + 0.831667959690094 + ], + [ + -12.0, + -60.0, + 0.8405332565307617 + ], + [ + -12.0, + -52.0, + 0.7746779322624207 + ], + [ + -12.0, + -44.0, + -0.7673581838607788 + ], + [ + -12.0, + -36.0, + -0.8117712140083313 + ], + [ + -12.0, + -28.0, + -0.8243898749351501 + ], + [ + -12.0, + -20.0, + -0.7574763894081116 + ], + [ + -12.0, + -12.0, + 0.09750459343194962 + ], + [ + -12.0, + -4.0, + 0.10071531683206558 + ], + [ + -12.0, + 4.0, + 0.10953764617443085 + ], + [ + -12.0, + 12.0, + 0.10368829220533371 + ], + [ + -12.0, + 20.0, + 0.10000000149011612 + ], + [ + -12.0, + 28.0, + 0.10000000149011612 + ], + [ + -12.0, + 36.0, + 0.10000000149011612 + ], + [ + -12.0, + 44.0, + 0.10000000149011612 + ], + [ + -12.0, + 52.0, + 0.23999999463558197 + ], + [ + -12.0, + 60.0, + 0.3149999976158142 + ], + [ + -12.0, + 68.0, + -0.26499998569488525 + ], + [ + -12.0, + 76.0, + -0.3050000071525574 + ] + ], + [ + [ + -4.0, + -76.0, + 0.8658663630485535 + ], + [ + -4.0, + -68.0, + 0.9098702073097229 + ], + [ + -4.0, + -60.0, + 0.9520763158798218 + ], + [ + -4.0, + -52.0, + 0.9131640195846558 + ], + [ + -4.0, + -44.0, + -0.9588647484779358 + ], + [ + -4.0, + -36.0, + -0.9428508281707764 + ], + [ + -4.0, + -28.0, + -0.9339745044708252 + ], + [ + -4.0, + -20.0, + -0.8613461256027222 + ], + [ + -4.0, + -12.0, + 0.11034068465232849 + ], + [ + -4.0, + -4.0, + 0.11452268064022064 + ], + [ + -4.0, + 4.0, + 0.12168126553297043 + ], + [ + -4.0, + 12.0, + 0.11265159398317337 + ], + [ + -4.0, + 20.0, + 0.10000000149011612 + ], + [ + -4.0, + 28.0, + 0.10000000149011612 + ], + [ + -4.0, + 36.0, + 0.10000000149011612 + ], + [ + -4.0, + 44.0, + 0.10000000149011612 + ], + [ + -4.0, + 52.0, + 0.36000001430511475 + ], + [ + -4.0, + 60.0, + 0.33000001311302185 + ], + [ + -4.0, + 68.0, + -0.3799999952316284 + ], + [ + -4.0, + 76.0, + -0.33000001311302185 + ] + ], + [ + [ + 4.0, + -76.0, + 1.1029284000396729 + ], + [ + 4.0, + -68.0, + 1.0086320638656616 + ], + [ + 4.0, + -60.0, + 1.0045244693756104 + ], + [ + 4.0, + -52.0, + 1.003873348236084 + ], + [ + 4.0, + -44.0, + -1.0814002752304077 + ], + [ + 4.0, + -36.0, + -1.0783718824386597 + ], + [ + 4.0, + -28.0, + -1.0497510433197021 + ], + [ + 4.0, + -20.0, + -1.0154544115066528 + ], + [ + 4.0, + -12.0, + 0.12635071575641632 + ], + [ + 4.0, + -4.0, + 0.13445423543453217 + ], + [ + 4.0, + 4.0, + 0.1357533484697342 + ], + [ + 4.0, + 12.0, + 0.13784921169281006 + ], + [ + 4.0, + 20.0, + 0.10000000149011612 + ], + [ + 4.0, + 28.0, + 0.10000000149011612 + ], + [ + 4.0, + 36.0, + 0.10000000149011612 + ], + [ + 4.0, + 44.0, + 0.10000000149011612 + ], + [ + 4.0, + 52.0, + 0.4300000071525574 + ], + [ + 4.0, + 60.0, + 0.4000000059604645 + ], + [ + 4.0, + 68.0, + -0.44999998807907104 + ], + [ + 4.0, + 76.0, + -0.41499999165534973 + ] + ], + [ + [ + 12.0, + -76.0, + 1.2019035816192627 + ], + [ + 12.0, + -68.0, + 1.1758776903152466 + ], + [ + 12.0, + -60.0, + 1.164806842803955 + ], + [ + 12.0, + -52.0, + 1.1223701238632202 + ], + [ + 12.0, + -44.0, + -1.1548222303390503 + ], + [ + 12.0, + -36.0, + -1.1638233661651611 + ], + [ + 12.0, + -28.0, + -1.1764601469039917 + ], + [ + 12.0, + -20.0, + -1.1429928541183472 + ], + [ + 12.0, + -12.0, + 0.15083539485931396 + ], + [ + 12.0, + -4.0, + 0.1454271823167801 + ], + [ + 12.0, + 4.0, + 0.14674042165279388 + ], + [ + 12.0, + 12.0, + 0.15137779712677002 + ], + [ + 12.0, + 20.0, + 0.10000000149011612 + ], + [ + 12.0, + 28.0, + 0.10000000149011612 + ], + [ + 12.0, + 36.0, + 0.10000000149011612 + ], + [ + 12.0, + 44.0, + 0.10000000149011612 + ], + [ + 12.0, + 52.0, + 0.5450000166893005 + ], + [ + 12.0, + 60.0, + 0.5149999856948853 + ], + [ + 12.0, + 68.0, + -0.5099999904632568 + ], + [ + 12.0, + 76.0, + -0.5249999761581421 + ] + ], + [ + [ + 20.0, + -76.0, + 1.33104407787323 + ], + [ + 20.0, + -68.0, + 1.2400410175323486 + ], + [ + 20.0, + -60.0, + 1.2375192642211914 + ], + [ + 20.0, + -52.0, + 1.2919387817382812 + ], + [ + 20.0, + -44.0, + -1.268329381942749 + ], + [ + 20.0, + -36.0, + -1.3036613464355469 + ], + [ + 20.0, + -28.0, + -1.2702977657318115 + ], + [ + 20.0, + -20.0, + -1.3154014348983765 + ], + [ + 20.0, + -12.0, + 0.16192816197872162 + ], + [ + 20.0, + -4.0, + 0.15631474554538727 + ], + [ + 20.0, + 4.0, + 0.15908361971378326 + ], + [ + 20.0, + 12.0, + 0.16579194366931915 + ], + [ + 20.0, + 20.0, + 0.10000000149011612 + ], + [ + 20.0, + 28.0, + 0.10000000149011612 + ], + [ + 20.0, + 36.0, + 0.10000000149011612 + ], + [ + 20.0, + 44.0, + 0.10000000149011612 + ], + [ + 20.0, + 52.0, + 0.625 + ], + [ + 20.0, + 60.0, + 0.6349999904632568 + ], + [ + 20.0, + 68.0, + -0.5649999976158142 + ], + [ + 20.0, + 76.0, + -0.6299999952316284 + ] + ], + [ + [ + 28.0, + -76.0, + 1.3741422891616821 + ], + [ + 28.0, + -68.0, + 1.462281584739685 + ], + [ + 28.0, + -60.0, + 1.3774404525756836 + ], + [ + 28.0, + -52.0, + 1.386590600013733 + ], + [ + 28.0, + -44.0, + -1.443994402885437 + ], + [ + 28.0, + -36.0, + -1.3756144046783447 + ], + [ + 28.0, + -28.0, + -1.3618830442428589 + ], + [ + 28.0, + -20.0, + -1.428186058998108 + ], + [ + 28.0, + -12.0, + 0.17241907119750977 + ], + [ + 28.0, + -4.0, + 0.1717700958251953 + ], + [ + 28.0, + 4.0, + 0.17144586145877838 + ], + [ + 28.0, + 12.0, + 0.17648139595985413 + ], + [ + 28.0, + 20.0, + 0.10000000149011612 + ], + [ + 28.0, + 28.0, + 0.10000000149011612 + ], + [ + 28.0, + 36.0, + 0.10000000149011612 + ], + [ + 28.0, + 44.0, + 0.10000000149011612 + ], + [ + 28.0, + 52.0, + 0.6650000214576721 + ], + [ + 28.0, + 60.0, + 0.7099999785423279 + ], + [ + 28.0, + 68.0, + -0.699999988079071 + ], + [ + 28.0, + 76.0, + -0.6499999761581421 + ] + ], + [ + [ + 36.0, + -76.0, + 1.5407485961914062 + ], + [ + 36.0, + -68.0, + 1.5635896921157837 + ], + [ + 36.0, + -60.0, + 1.5700641870498657 + ], + [ + 36.0, + -52.0, + 1.5683965682983398 + ], + [ + 36.0, + -44.0, + -1.5016088485717773 + ], + [ + 36.0, + -36.0, + -1.498430848121643 + ], + [ + 36.0, + -28.0, + -1.5390263795852661 + ], + [ + 36.0, + -20.0, + -1.5827711820602417 + ], + [ + 36.0, + -12.0, + 0.19251567125320435 + ], + [ + 36.0, + -4.0, + 0.1994284689426422 + ], + [ + 36.0, + 4.0, + 0.19853903353214264 + ], + [ + 36.0, + 12.0, + 0.19440963864326477 + ], + [ + 36.0, + 20.0, + 0.10000000149011612 + ], + [ + 36.0, + 28.0, + 0.10000000149011612 + ], + [ + 36.0, + 36.0, + 0.10000000149011612 + ], + [ + 36.0, + 44.0, + 0.10000000149011612 + ], + [ + 36.0, + 52.0, + 0.7950000166893005 + ], + [ + 36.0, + 60.0, + 0.7749999761581421 + ], + [ + 36.0, + 68.0, + -0.7400000095367432 + ], + [ + 36.0, + 76.0, + -0.7450000047683716 + ] + ] + ], + "seed": 42, + "initial": { + "levels": [ + 0, + 5, + 4, + 4, + 0, + 5, + 4, + 2, + 4, + 5, + 4, + 4, + 2, + 0, + 3, + 4, + 5, + 1, + 3, + 4 + ], + "types": [ + 0, + 1, + 2, + 3, + 4, + 5, + 6, + 7, + 8, + 9, + 10, + 11, + 12, + 13, + 14, + 15, + 16, + 17, + 18, + 19 + ], + "env_origins": [ + [ + -36.0, + -76.0, + 0.4475184679031372 + ], + [ + 4.0, + -68.0, + 1.0086320638656616 + ], + [ + -4.0, + -60.0, + 0.9520763158798218 + ], + [ + -4.0, + -52.0, + 0.9131640195846558 + ], + [ + -36.0, + -44.0, + -0.4050811529159546 + ], + [ + 4.0, + -36.0, + -1.0783718824386597 + ], + [ + -4.0, + -28.0, + -0.9339745044708252 + ], + [ + -20.0, + -20.0, + -0.7095288038253784 + ], + [ + -4.0, + -12.0, + 0.11034068465232849 + ], + [ + 4.0, + -4.0, + 0.13445423543453217 + ], + [ + -4.0, + 4.0, + 0.12168126553297043 + ], + [ + -4.0, + 12.0, + 0.11265159398317337 + ], + [ + -20.0, + 20.0, + 0.10000000149011612 + ], + [ + -36.0, + 28.0, + 0.10000000149011612 + ], + [ + -12.0, + 36.0, + 0.10000000149011612 + ], + [ + -4.0, + 44.0, + 0.10000000149011612 + ], + [ + 4.0, + 52.0, + 0.4300000071525574 + ], + [ + -28.0, + 60.0, + 0.11999999731779099 + ], + [ + -12.0, + 68.0, + -0.26499998569488525 + ], + [ + -4.0, + 76.0, + -0.33000001311302185 + ] + ] + }, + "cases": [ + { + "stage": -1, + "ids": [ + 0, + 1, + 2, + 3, + 4, + 5, + 6, + 7, + 8, + 9, + 10, + 11, + 12, + 13, + 14, + 15, + 16, + 17, + 18, + 19 + ], + "positions": [ + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ] + ], + "commands": [ + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.0 + ] + ], + "levels": [ + 1, + 6, + 5, + 5, + 1, + 6, + 5, + 3, + 5, + 6, + 5, + 5, + 3, + 1, + 4, + 5, + 6, + 2, + 4, + 5 + ], + "env_origins": [ + [ + -28.0, + -76.0, + 0.5312986969947815 + ], + [ + 12.0, + -68.0, + 1.1758776903152466 + ], + [ + 4.0, + -60.0, + 1.0045244693756104 + ], + [ + 4.0, + -52.0, + 1.003873348236084 + ], + [ + -28.0, + -44.0, + -0.580917477607727 + ], + [ + 12.0, + -36.0, + -1.1638233661651611 + ], + [ + 4.0, + -28.0, + -1.0497510433197021 + ], + [ + -12.0, + -20.0, + -0.7574763894081116 + ], + [ + 4.0, + -12.0, + 0.12635071575641632 + ], + [ + 12.0, + -4.0, + 0.1454271823167801 + ], + [ + 4.0, + 4.0, + 0.1357533484697342 + ], + [ + 4.0, + 12.0, + 0.13784921169281006 + ], + [ + -12.0, + 20.0, + 0.10000000149011612 + ], + [ + -28.0, + 28.0, + 0.10000000149011612 + ], + [ + -4.0, + 36.0, + 0.10000000149011612 + ], + [ + 4.0, + 44.0, + 0.10000000149011612 + ], + [ + 12.0, + 52.0, + 0.5450000166893005 + ], + [ + -20.0, + 60.0, + 0.18000000715255737 + ], + [ + -4.0, + 68.0, + -0.3799999952316284 + ], + [ + 4.0, + 76.0, + -0.41499999165534973 + ] + ] + }, + { + "stage": 0, + "ids": [ + 0, + 3, + 6, + 9, + 12, + 15, + 18 + ], + "positions": [ + [ + -28.0, + -76.0, + 0.5312986969947815 + ], + [ + 15.999899864196777, + -68.0, + 1.1758776903152466 + ], + [ + 8.0, + -60.0, + 1.0045244693756104 + ], + [ + 8.000100135803223, + -52.0, + 1.003873348236084 + ], + [ + -18.0, + -44.0, + -0.580917477607727 + ], + [ + 12.0, + -36.0, + -1.1638233661651611 + ], + [ + 7.999899864196777, + -28.0, + -1.0497510433197021 + ], + [ + -8.0, + -20.0, + -0.7574763894081116 + ], + [ + 8.000100135803223, + -12.0, + 0.12635071575641632 + ], + [ + 22.0, + -4.0, + 0.1454271823167801 + ], + [ + 4.0, + 4.0, + 0.1357533484697342 + ], + [ + 7.999899864196777, + 12.0, + 0.13784921169281006 + ], + [ + -8.0, + 20.0, + 0.10000000149011612 + ], + [ + -23.999900817871094, + 28.0, + 0.10000000149011612 + ], + [ + 6.0, + 36.0, + 0.10000000149011612 + ], + [ + 4.0, + 44.0, + 0.10000000149011612 + ], + [ + 15.999899864196777, + 52.0, + 0.5450000166893005 + ], + [ + -16.0, + 60.0, + 0.18000000715255737 + ], + [ + 0.00010013580322265625, + 68.0, + -0.3799999952316284 + ], + [ + 14.0, + 76.0, + -0.41499999165534973 + ] + ], + "commands": [ + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ] + ], + "levels": [ + 0, + 6, + 5, + 6, + 1, + 6, + 4, + 3, + 5, + 7, + 5, + 5, + 3, + 1, + 4, + 4, + 6, + 2, + 5, + 5 + ], + "env_origins": [ + [ + -36.0, + -76.0, + 0.4475184679031372 + ], + [ + 12.0, + -68.0, + 1.1758776903152466 + ], + [ + 4.0, + -60.0, + 1.0045244693756104 + ], + [ + 12.0, + -52.0, + 1.1223701238632202 + ], + [ + -28.0, + -44.0, + -0.580917477607727 + ], + [ + 12.0, + -36.0, + -1.1638233661651611 + ], + [ + -4.0, + -28.0, + -0.9339745044708252 + ], + [ + -12.0, + -20.0, + -0.7574763894081116 + ], + [ + 4.0, + -12.0, + 0.12635071575641632 + ], + [ + 20.0, + -4.0, + 0.15631474554538727 + ], + [ + 4.0, + 4.0, + 0.1357533484697342 + ], + [ + 4.0, + 12.0, + 0.13784921169281006 + ], + [ + -12.0, + 20.0, + 0.10000000149011612 + ], + [ + -28.0, + 28.0, + 0.10000000149011612 + ], + [ + -4.0, + 36.0, + 0.10000000149011612 + ], + [ + -4.0, + 44.0, + 0.10000000149011612 + ], + [ + 12.0, + 52.0, + 0.5450000166893005 + ], + [ + -20.0, + 60.0, + 0.18000000715255737 + ], + [ + 4.0, + 68.0, + -0.44999998807907104 + ], + [ + 4.0, + 76.0, + -0.41499999165534973 + ] + ] + }, + { + "stage": 1, + "ids": [ + 1, + 4, + 7, + 10, + 13, + 16, + 19 + ], + "positions": [ + [ + -36.0, + -76.0, + 0.4475184679031372 + ], + [ + 15.999899864196777, + -68.0, + 1.1758776903152466 + ], + [ + 8.0, + -60.0, + 1.0045244693756104 + ], + [ + 16.000099182128906, + -52.0, + 1.1223701238632202 + ], + [ + -18.0, + -44.0, + -0.580917477607727 + ], + [ + 12.0, + -36.0, + -1.1638233661651611 + ], + [ + -9.989738464355469e-05, + -28.0, + -0.9339745044708252 + ], + [ + -8.0, + -20.0, + -0.7574763894081116 + ], + [ + 8.000100135803223, + -12.0, + 0.12635071575641632 + ], + [ + 30.0, + -4.0, + 0.15631474554538727 + ], + [ + 4.0, + 4.0, + 0.1357533484697342 + ], + [ + 7.999899864196777, + 12.0, + 0.13784921169281006 + ], + [ + -8.0, + 20.0, + 0.10000000149011612 + ], + [ + -23.999900817871094, + 28.0, + 0.10000000149011612 + ], + [ + 6.0, + 36.0, + 0.10000000149011612 + ], + [ + -4.0, + 44.0, + 0.10000000149011612 + ], + [ + 15.999899864196777, + 52.0, + 0.5450000166893005 + ], + [ + -16.0, + 60.0, + 0.18000000715255737 + ], + [ + 8.000100135803223, + 68.0, + -0.44999998807907104 + ], + [ + 14.0, + 76.0, + -0.41499999165534973 + ] + ], + "commands": [ + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ] + ], + "levels": [ + 0, + 5, + 5, + 6, + 2, + 6, + 4, + 3, + 5, + 7, + 4, + 5, + 3, + 2, + 4, + 4, + 5, + 2, + 5, + 6 + ], + "env_origins": [ + [ + -36.0, + -76.0, + 0.4475184679031372 + ], + [ + 4.0, + -68.0, + 1.0086320638656616 + ], + [ + 4.0, + -60.0, + 1.0045244693756104 + ], + [ + 12.0, + -52.0, + 1.1223701238632202 + ], + [ + -20.0, + -44.0, + -0.6902334094047546 + ], + [ + 12.0, + -36.0, + -1.1638233661651611 + ], + [ + -4.0, + -28.0, + -0.9339745044708252 + ], + [ + -12.0, + -20.0, + -0.7574763894081116 + ], + [ + 4.0, + -12.0, + 0.12635071575641632 + ], + [ + 20.0, + -4.0, + 0.15631474554538727 + ], + [ + -4.0, + 4.0, + 0.12168126553297043 + ], + [ + 4.0, + 12.0, + 0.13784921169281006 + ], + [ + -12.0, + 20.0, + 0.10000000149011612 + ], + [ + -20.0, + 28.0, + 0.10000000149011612 + ], + [ + -4.0, + 36.0, + 0.10000000149011612 + ], + [ + -4.0, + 44.0, + 0.10000000149011612 + ], + [ + 4.0, + 52.0, + 0.4300000071525574 + ], + [ + -20.0, + 60.0, + 0.18000000715255737 + ], + [ + 4.0, + 68.0, + -0.44999998807907104 + ], + [ + 12.0, + 76.0, + -0.5249999761581421 + ] + ] + }, + { + "stage": 2, + "ids": [ + 2, + 5, + 8, + 11, + 14, + 17 + ], + "positions": [ + [ + -36.0, + -76.0, + 0.4475184679031372 + ], + [ + 7.999899864196777, + -68.0, + 1.0086320638656616 + ], + [ + 8.0, + -60.0, + 1.0045244693756104 + ], + [ + 16.000099182128906, + -52.0, + 1.1223701238632202 + ], + [ + -10.0, + -44.0, + -0.6902334094047546 + ], + [ + 12.0, + -36.0, + -1.1638233661651611 + ], + [ + -9.989738464355469e-05, + -28.0, + -0.9339745044708252 + ], + [ + -8.0, + -20.0, + -0.7574763894081116 + ], + [ + 8.000100135803223, + -12.0, + 0.12635071575641632 + ], + [ + 30.0, + -4.0, + 0.15631474554538727 + ], + [ + -4.0, + 4.0, + 0.12168126553297043 + ], + [ + 7.999899864196777, + 12.0, + 0.13784921169281006 + ], + [ + -8.0, + 20.0, + 0.10000000149011612 + ], + [ + -15.999899864196777, + 28.0, + 0.10000000149011612 + ], + [ + 6.0, + 36.0, + 0.10000000149011612 + ], + [ + -4.0, + 44.0, + 0.10000000149011612 + ], + [ + 7.999899864196777, + 52.0, + 0.4300000071525574 + ], + [ + -16.0, + 60.0, + 0.18000000715255737 + ], + [ + 8.000100135803223, + 68.0, + -0.44999998807907104 + ], + [ + 22.0, + 76.0, + -0.5249999761581421 + ] + ], + "commands": [ + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ] + ], + "levels": [ + 0, + 5, + 5, + 6, + 2, + 5, + 4, + 3, + 6, + 7, + 4, + 4, + 3, + 2, + 5, + 4, + 5, + 2, + 5, + 6 + ], + "env_origins": [ + [ + -36.0, + -76.0, + 0.4475184679031372 + ], + [ + 4.0, + -68.0, + 1.0086320638656616 + ], + [ + 4.0, + -60.0, + 1.0045244693756104 + ], + [ + 12.0, + -52.0, + 1.1223701238632202 + ], + [ + -20.0, + -44.0, + -0.6902334094047546 + ], + [ + 4.0, + -36.0, + -1.0783718824386597 + ], + [ + -4.0, + -28.0, + -0.9339745044708252 + ], + [ + -12.0, + -20.0, + -0.7574763894081116 + ], + [ + 12.0, + -12.0, + 0.15083539485931396 + ], + [ + 20.0, + -4.0, + 0.15631474554538727 + ], + [ + -4.0, + 4.0, + 0.12168126553297043 + ], + [ + -4.0, + 12.0, + 0.11265159398317337 + ], + [ + -12.0, + 20.0, + 0.10000000149011612 + ], + [ + -20.0, + 28.0, + 0.10000000149011612 + ], + [ + 4.0, + 36.0, + 0.10000000149011612 + ], + [ + -4.0, + 44.0, + 0.10000000149011612 + ], + [ + 4.0, + 52.0, + 0.4300000071525574 + ], + [ + -20.0, + 60.0, + 0.18000000715255737 + ], + [ + 4.0, + 68.0, + -0.44999998807907104 + ], + [ + 12.0, + 76.0, + -0.5249999761581421 + ] + ] + }, + { + "stage": 3, + "ids": [ + 0, + 3, + 6, + 9, + 12, + 15, + 18 + ], + "positions": [ + [ + -36.0, + -76.0, + 0.4475184679031372 + ], + [ + 7.999899864196777, + -68.0, + 1.0086320638656616 + ], + [ + 8.0, + -60.0, + 1.0045244693756104 + ], + [ + 16.000099182128906, + -52.0, + 1.1223701238632202 + ], + [ + -10.0, + -44.0, + -0.6902334094047546 + ], + [ + 4.0, + -36.0, + -1.0783718824386597 + ], + [ + -9.989738464355469e-05, + -28.0, + -0.9339745044708252 + ], + [ + -8.0, + -20.0, + -0.7574763894081116 + ], + [ + 16.000099182128906, + -12.0, + 0.15083539485931396 + ], + [ + 30.0, + -4.0, + 0.15631474554538727 + ], + [ + -4.0, + 4.0, + 0.12168126553297043 + ], + [ + -9.989738464355469e-05, + 12.0, + 0.11265159398317337 + ], + [ + -8.0, + 20.0, + 0.10000000149011612 + ], + [ + -15.999899864196777, + 28.0, + 0.10000000149011612 + ], + [ + 14.0, + 36.0, + 0.10000000149011612 + ], + [ + -4.0, + 44.0, + 0.10000000149011612 + ], + [ + 7.999899864196777, + 52.0, + 0.4300000071525574 + ], + [ + -16.0, + 60.0, + 0.18000000715255737 + ], + [ + 8.000100135803223, + 68.0, + -0.44999998807907104 + ], + [ + 22.0, + 76.0, + -0.5249999761581421 + ] + ], + "commands": [ + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ] + ], + "levels": [ + 0, + 5, + 5, + 7, + 2, + 5, + 3, + 3, + 6, + 8, + 4, + 4, + 3, + 2, + 5, + 3, + 5, + 2, + 6, + 6 + ], + "env_origins": [ + [ + -36.0, + -76.0, + 0.4475184679031372 + ], + [ + 4.0, + -68.0, + 1.0086320638656616 + ], + [ + 4.0, + -60.0, + 1.0045244693756104 + ], + [ + 20.0, + -52.0, + 1.2919387817382812 + ], + [ + -20.0, + -44.0, + -0.6902334094047546 + ], + [ + 4.0, + -36.0, + -1.0783718824386597 + ], + [ + -12.0, + -28.0, + -0.8243898749351501 + ], + [ + -12.0, + -20.0, + -0.7574763894081116 + ], + [ + 12.0, + -12.0, + 0.15083539485931396 + ], + [ + 28.0, + -4.0, + 0.1717700958251953 + ], + [ + -4.0, + 4.0, + 0.12168126553297043 + ], + [ + -4.0, + 12.0, + 0.11265159398317337 + ], + [ + -12.0, + 20.0, + 0.10000000149011612 + ], + [ + -20.0, + 28.0, + 0.10000000149011612 + ], + [ + 4.0, + 36.0, + 0.10000000149011612 + ], + [ + -12.0, + 44.0, + 0.10000000149011612 + ], + [ + 4.0, + 52.0, + 0.4300000071525574 + ], + [ + -20.0, + 60.0, + 0.18000000715255737 + ], + [ + 12.0, + 68.0, + -0.5099999904632568 + ], + [ + 12.0, + 76.0, + -0.5249999761581421 + ] + ] + }, + { + "stage": 4, + "ids": [ + 1, + 4, + 7, + 10, + 13, + 16, + 19 + ], + "positions": [ + [ + -26.0, + -76.0, + 0.4475184679031372 + ], + [ + 46.0, + -68.0, + 1.5635896921157837 + ], + [ + 14.0, + -60.0, + 1.0045244693756104 + ], + [ + 30.0, + -52.0, + 1.2919387817382812 + ], + [ + 46.0, + -44.0, + -1.5016088485717773 + ], + [ + 14.0, + -36.0, + -1.0783718824386597 + ], + [ + -2.0, + -28.0, + -0.8243898749351501 + ], + [ + 46.0, + -20.0, + -1.5827711820602417 + ], + [ + 22.0, + -12.0, + 0.15083539485931396 + ], + [ + 38.0, + -4.0, + 0.1717700958251953 + ], + [ + 46.0, + 4.0, + 0.19853903353214264 + ], + [ + 6.0, + 12.0, + 0.11265159398317337 + ], + [ + -2.0, + 20.0, + 0.10000000149011612 + ], + [ + 46.0, + 28.0, + 0.10000000149011612 + ], + [ + 14.0, + 36.0, + 0.10000000149011612 + ], + [ + -2.0, + 44.0, + 0.10000000149011612 + ], + [ + 46.0, + 52.0, + 0.7950000166893005 + ], + [ + -10.0, + 60.0, + 0.18000000715255737 + ], + [ + 22.0, + 68.0, + -0.5099999904632568 + ], + [ + 46.0, + 76.0, + -0.7450000047683716 + ] + ], + "commands": [ + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ] + ], + "levels": [ + 0, + 0, + 5, + 7, + 1, + 5, + 3, + 1, + 6, + 8, + 7, + 4, + 3, + 9, + 5, + 3, + 4, + 2, + 6, + 3 + ], + "env_origins": [ + [ + -36.0, + -76.0, + 0.4475184679031372 + ], + [ + -36.0, + -68.0, + 0.3967205584049225 + ], + [ + 4.0, + -60.0, + 1.0045244693756104 + ], + [ + 20.0, + -52.0, + 1.2919387817382812 + ], + [ + -28.0, + -44.0, + -0.580917477607727 + ], + [ + 4.0, + -36.0, + -1.0783718824386597 + ], + [ + -12.0, + -28.0, + -0.8243898749351501 + ], + [ + -28.0, + -20.0, + -0.527474582195282 + ], + [ + 12.0, + -12.0, + 0.15083539485931396 + ], + [ + 28.0, + -4.0, + 0.1717700958251953 + ], + [ + 20.0, + 4.0, + 0.15908361971378326 + ], + [ + -4.0, + 12.0, + 0.11265159398317337 + ], + [ + -12.0, + 20.0, + 0.10000000149011612 + ], + [ + 36.0, + 28.0, + 0.10000000149011612 + ], + [ + 4.0, + 36.0, + 0.10000000149011612 + ], + [ + -12.0, + 44.0, + 0.10000000149011612 + ], + [ + -4.0, + 52.0, + 0.36000001430511475 + ], + [ + -20.0, + 60.0, + 0.18000000715255737 + ], + [ + 12.0, + 68.0, + -0.5099999904632568 + ], + [ + -12.0, + 76.0, + -0.3050000071525574 + ] + ] + }, + { + "stage": 5, + "ids": [ + 2, + 5, + 8, + 11, + 14, + 17 + ], + "positions": [ + [ + -36.0, + -76.0, + 0.4475184679031372 + ], + [ + -36.0, + -68.0, + 0.3967205584049225 + ], + [ + -36.0, + -60.0, + 0.4455190598964691 + ], + [ + 20.0, + -52.0, + 1.2919387817382812 + ], + [ + -28.0, + -44.0, + -0.580917477607727 + ], + [ + -36.0, + -36.0, + -0.3751884400844574 + ], + [ + -12.0, + -28.0, + -0.8243898749351501 + ], + [ + -28.0, + -20.0, + -0.527474582195282 + ], + [ + -36.0, + -12.0, + 0.059964701533317566 + ], + [ + 28.0, + -4.0, + 0.1717700958251953 + ], + [ + 20.0, + 4.0, + 0.15908361971378326 + ], + [ + -36.0, + 12.0, + 0.05683664605021477 + ], + [ + -12.0, + 20.0, + 0.10000000149011612 + ], + [ + 36.0, + 28.0, + 0.10000000149011612 + ], + [ + -36.0, + 36.0, + 0.10000000149011612 + ], + [ + -12.0, + 44.0, + 0.10000000149011612 + ], + [ + -4.0, + 52.0, + 0.36000001430511475 + ], + [ + -36.0, + 60.0, + 0.019999999552965164 + ], + [ + 12.0, + 68.0, + -0.5099999904632568 + ], + [ + -12.0, + 76.0, + -0.3050000071525574 + ] + ], + "commands": [ + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ], + [ + 0.4000000059604645, + 0.0, + 0.0 + ] + ], + "levels": [ + 0, + 0, + 0, + 7, + 1, + 0, + 3, + 1, + 0, + 8, + 7, + 0, + 3, + 9, + 0, + 3, + 4, + 0, + 6, + 3 + ], + "env_origins": [ + [ + -36.0, + -76.0, + 0.4475184679031372 + ], + [ + -36.0, + -68.0, + 0.3967205584049225 + ], + [ + -36.0, + -60.0, + 0.4455190598964691 + ], + [ + 20.0, + -52.0, + 1.2919387817382812 + ], + [ + -28.0, + -44.0, + -0.580917477607727 + ], + [ + -36.0, + -36.0, + -0.3751884400844574 + ], + [ + -12.0, + -28.0, + -0.8243898749351501 + ], + [ + -28.0, + -20.0, + -0.527474582195282 + ], + [ + -36.0, + -12.0, + 0.059964701533317566 + ], + [ + 28.0, + -4.0, + 0.1717700958251953 + ], + [ + 20.0, + 4.0, + 0.15908361971378326 + ], + [ + -36.0, + 12.0, + 0.05683664605021477 + ], + [ + -12.0, + 20.0, + 0.10000000149011612 + ], + [ + 36.0, + 28.0, + 0.10000000149011612 + ], + [ + -36.0, + 36.0, + 0.10000000149011612 + ], + [ + -12.0, + 44.0, + 0.10000000149011612 + ], + [ + -4.0, + 52.0, + 0.36000001430511475 + ], + [ + -36.0, + 60.0, + 0.019999999552965164 + ], + [ + 12.0, + 68.0, + -0.5099999904632568 + ], + [ + -12.0, + 76.0, + -0.3050000071525574 + ] + ] + } + ] +} diff --git a/tests/gym/envs/tasks/test_isaac_velocity.py b/tests/gym/envs/tasks/test_isaac_velocity.py new file mode 100644 index 0000000000..5c5e04aa3c --- /dev/null +++ b/tests/gym/envs/tasks/test_isaac_velocity.py @@ -0,0 +1,796 @@ +# ---------------------------------------------------------------------------- +# 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 packaged velocity definitions with executed Isaac reference values.""" + +from __future__ import annotations + +import json +from copy import copy +from pathlib import Path +from types import SimpleNamespace + +import numpy as np +import pytest +import torch +from dexsim.spawn import ( + CollisionDesc, + GeometryDesc, + NewtonCollisionDesc, + ObjectDesc, + RenderDesc, +) + +from embodichain.lab.sim.sim_manager import SimulationManager +from embodichain_tasks.locomotion.velocity.isaac.curriculum import ( + ReferenceTerrainCurriculum, +) +from embodichain_tasks.locomotion.velocity.isaac.config import load_config +from embodichain_tasks.locomotion.velocity.isaac.environment import ( + load_reference_env_cfg, +) +from embodichain_tasks.locomotion.velocity.isaac.rsl_rl import ReferenceRslRlEnv +from embodichain_tasks.locomotion.velocity.isaac.runtime import IsaacVelocityEnv +from embodichain_tasks.locomotion.velocity.isaac.terrain import ReferenceHeightScanner +from embodichain_tasks.locomotion.velocity.isaac.mdp import ( + IsaacVelocityState, + action_target, + build_observation, + compute_rewards, + compute_termination, + dc_motor_effort, + update_contact_times, +) + +__all__ = [] + + +@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__(IsaacVelocityEnv) + 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__(IsaacVelocityEnv) + 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 + + 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) + 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() + 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_control_state_read_is_reused_by_reference_managers() -> None: + """Acceleration and task managers consume one live read per physics frame.""" + env = object.__new__(IsaacVelocityEnv) + 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_isaac_rough", 310)]) +@pytest.mark.parametrize("cached", [False, True]) +def test_executed_isaac_reference(task: str, dimension: int, cached: bool) -> None: + """Use golden values exported by the actual Isaac MDP functions.""" + fixture = json.loads( + (Path(__file__).parent / "fixtures" / f"{task}.json").read_text() + ) + state = IsaacVelocityState( + **{ + name: torch.tensor(value, dtype=torch.float32) + for name, value in fixture["state"].items() + } + ) + config = load_config(task) + from embodichain_tasks.locomotion.velocity.isaac.mdp import IsaacVelocityConstants + + constants = ( + IsaacVelocityConstants.from_config(config, state.joint_pos) if cached else None + ) + noise = {name: torch.tensor(value) for name, value in fixture["noise"].items()} + observed = build_observation(config, state, noise=noise, constants=constants) + assert observed.shape == (4, dimension) + torch.testing.assert_close( + observed, torch.tensor(fixture["observation"]), rtol=1e-6, atol=1e-6 + ) + terminated, truncated = compute_termination(config, state) + assert torch.equal(terminated, torch.tensor(fixture["terminated"])) + assert not truncated.any() + reward = compute_rewards(config, state, terminated, constants=constants) + for name, expected in fixture["rewards"].items(): + torch.testing.assert_close( + reward.raw_terms[name], torch.tensor(expected), rtol=1e-6, atol=1e-6 + ) + expected_total = sum( + torch.tensor(value) * config.data["rewards"][name]["weight"] * 0.02 + for name, value in fixture["rewards"].items() + ) + torch.testing.assert_close(reward.total, expected_total, rtol=1e-6, atol=1e-6) + state.episode_step[0] = 1000 + assert compute_termination(config, state)[1].tolist() == [True, False, False, False] + + +@pytest.mark.no_sim +@pytest.mark.parametrize("task", ["g1_isaac_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: + """A shallow scene declaration must retain the modified ground descriptors.""" + 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._spawn_default_plane_descriptor = descriptor + 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(heightfield_resolution=0.1) + sim.configure_ground_plane_collision(newton=collision, geometry=mesh) + 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_rsl_wrapper_preserves_actions_and_timeout_bootstrapping() -> None: + """Failures end episodes while only timeouts bootstrap the value estimate.""" + actions = torch.full((2, 12), 4.0) + received = [] + + def step(value: torch.Tensor, *, policy_observations_only: bool) -> tuple: + assert policy_observations_only + received.append(value) + return ( + {"policy": torch.zeros(2, 48)}, + torch.ones(2), + torch.tensor([True, False]), + torch.tensor([False, True]), + {}, + ) + + inner = SimpleNamespace( + num_envs=2, + velocity_task_config=SimpleNamespace(action_dim=12), + max_episode_steps=1000, + device=torch.device("cpu"), + cfg=None, + step=step, + ) + wrapped = ReferenceRslRlEnv(inner) + _, _, done, extras = wrapped.step(actions) + assert received[0] is actions + assert done.tolist() == [True, True] + assert extras["time_outs"].tolist() == [False, True] + + +@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 = ReferenceHeightScanner( + 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 = ReferenceHeightScanner( + 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__(IsaacVelocityEnv) + 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_matches_executed_isaac_resets() -> None: + """Match source assignments, threshold cases, selected resets and overflow.""" + fixture = json.loads( + (Path(__file__).parent / "fixtures/terrain_curriculum.json").read_text() + ) + origins = torch.tensor(fixture["origins"], dtype=torch.float32) + curriculum = ReferenceTerrainCurriculum( + origins, 20, 5, 8.0, 20.0, torch.Generator().manual_seed(fixture["seed"]) + ) + torch.testing.assert_close( + curriculum.levels, torch.tensor(fixture["initial"]["levels"]), rtol=0, atol=0 + ) + torch.testing.assert_close( + curriculum.types, torch.tensor(fixture["initial"]["types"]), rtol=0, atol=0 + ) + torch.testing.assert_close( + curriculum.env_origins, + torch.tensor(fixture["initial"]["env_origins"]), + rtol=0, + atol=0, + ) + for case in fixture["cases"]: + ids = torch.tensor(case["ids"]) + if case["stage"] in (4, 5): + level = 9 if case["stage"] == 4 else 0 + curriculum.levels[ids] = level + curriculum.env_origins[ids] = origins[level, curriculum.types[ids]] + untouched = torch.ones(20, dtype=torch.bool) + untouched[ids] = False + before_levels = curriculum.levels.clone() + before_origins = curriculum.env_origins.clone() + curriculum.update( + ids, torch.tensor(case["positions"]), torch.tensor(case["commands"]) + ) + torch.testing.assert_close( + curriculum.levels, torch.tensor(case["levels"]), rtol=0, atol=0 + ) + torch.testing.assert_close( + curriculum.env_origins, torch.tensor(case["env_origins"]), rtol=0, atol=0 + ) + assert torch.equal(curriculum.levels[untouched], before_levels[untouched]) + assert torch.equal(curriculum.env_origins[untouched], before_origins[untouched]) + + +@pytest.mark.no_sim +def test_rough_reset_curriculum_uses_terminal_step_command(monkeypatch) -> None: + """A timeout must use the command that generated the terminal reward.""" + origins = torch.zeros((10, 1, 3)) + curriculum = ReferenceTerrainCurriculum( + origins, 1, 5, 8.0, 20.0, torch.Generator().manual_seed(42) + ) + curriculum.levels[:] = 3 + env = object.__new__(IsaacVelocityEnv) + env._device = torch.device("cpu") + env.sim = SimpleNamespace(device=torch.device("cpu")) + env._num_envs = 1 + 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]]) + + class ResetRecorded(Exception): + pass + + def stop_after_curriculum(self, **kwargs): + raise ResetRecorded + + from embodichain_tasks.locomotion.velocity._embodichain import ( + EmbodiChainVelocityEnv, + ) + + monkeypatch.setattr( + EmbodiChainVelocityEnv, "_initialize_episode", stop_after_curriculum + ) + with pytest.raises(ResetRecorded): + env._initialize_episode() + # 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__(IsaacVelocityEnv) + + 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( + IsaacVelocityEnv, + "_apply_reference_interval_events", + lambda self: events.append("push"), + ) + monkeypatch.setattr( + IsaacVelocityEnv, + "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__(IsaacVelocityEnv) + 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( + IsaacVelocityEnv, "_extend_obs", lambda self, obs, **kwargs: observe(self) + ) + monkeypatch.setattr(EmbodiChainVelocityEnv, "step", parent_step) + monkeypatch.setattr(IsaacVelocityEnv, "_advance_command_timer", lambda self: None) + monkeypatch.setattr(IsaacVelocityEnv, "_update_heading_commands", lambda self: None) + monkeypatch.setattr( + IsaacVelocityEnv, "_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_isaac_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__(IsaacVelocityEnv) + env._device = torch.device("cpu") + env._num_envs = 4 + env.sim = SimpleNamespace(device=env._device) + env.cfg = SimpleNamespace(reference_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 = ReferenceHeightScanner( + 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 diff --git a/tests/gym/envs/tasks/test_isaac_velocity_frame.py b/tests/gym/envs/tasks/test_isaac_velocity_frame.py new file mode 100644 index 0000000000..fb10ff7d60 --- /dev/null +++ b/tests/gym/envs/tasks/test_isaac_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.isaac._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) From f036b594e12aa4833af7efe6163ae2b3508bab5c Mon Sep 17 00:00:00 2001 From: acrlw <13927622+acrlw@users.noreply.github.com> Date: Wed, 21 Oct 2026 17:55:42 +0800 Subject: [PATCH 02/24] fix(tasks): preserve G1 contact observations across reset --- .../g1_isaac_rough/agents/ppo.newton.yaml | 2 +- .../locomotion/velocity/isaac/README.md | 5 ++++ .../locomotion/velocity/isaac/runtime.py | 18 +++++++++++++ tests/gym/envs/tasks/test_isaac_velocity.py | 27 +++++++++++++++++++ 4 files changed, 51 insertions(+), 1 deletion(-) diff --git a/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/agents/ppo.newton.yaml b/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/agents/ppo.newton.yaml index 959285b8eb..ecdaeaf458 100644 --- a/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/agents/ppo.newton.yaml +++ b/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/agents/ppo.newton.yaml @@ -1,6 +1,6 @@ trainer: exp_name: g1_rough_ppo_mujoco_warp - gym_config: extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/env.newton.yaml + gym_config: ../env.newton.yaml seed: 42 device: cuda:0 headless: true diff --git a/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/README.md b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/README.md index 615e5950ba..f6895ee7d1 100644 --- a/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/README.md +++ b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/README.md @@ -24,6 +24,11 @@ DexSim selected-link property APIs are required. The package loader supports explicit G1 reference bundles for reproducible terrain and initial-state checks. `ReferenceRslRlEnv` retains the previously validated RSL-RL interface. +The training config resolves its task config relative to the config file, so +the installed configuration also works outside the repository directory. +Contact observations read samples owned by the task's physics-step callback; +reset clears selected rows without querying the invalidated solver snapshot. + The native MuJoCo Warp heightfield collision path remains unsuitable for this terrain because the retained official-runtime reproducer produces nonfinite states. The default deployment therefore selects Newton contacts. DexUni's diff --git a/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/runtime.py b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/runtime.py index dec4140251..f585b7fdfb 100644 --- a/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/runtime.py +++ b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/runtime.py @@ -431,6 +431,24 @@ def step( 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 + ): + 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): diff --git a/tests/gym/envs/tasks/test_isaac_velocity.py b/tests/gym/envs/tasks/test_isaac_velocity.py index 5c5e04aa3c..c89c21903e 100644 --- a/tests/gym/envs/tasks/test_isaac_velocity.py +++ b/tests/gym/envs/tasks/test_isaac_velocity.py @@ -58,6 +58,33 @@ __all__ = [] +@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)), + ) + 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}, + _reference_sensor=contacts, + ) + obs = IsaacVelocityEnv._get_sensor_obs(env) + assert not obs["locomotion_contacts"]["is_valid"].any() + assert obs["camera"].eq(1).all() + 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 490903daa93dd92f0b6089a47e003fa77e549397 Mon Sep 17 00:00:00 2001 From: acrlw <13927622+acrlw@users.noreply.github.com> Date: Wed, 21 Oct 2026 18:52:35 +0800 Subject: [PATCH 03/24] feat(rl): enable G1 DexUni and v2 curriculum resume --- agent_context/topics/rl-learning/training.md | 8 + docs/source/overview/rl/dexterous.md | 11 +- docs/source/overview/rl/locomotion.md | 23 +++ embodichain/learning/rl/checkpoint.py | 25 +++ .../g1_isaac_rough/agents/ppo.dexuni.yaml | 59 ++++++ .../velocity/g1_isaac_rough/catalog.yaml | 2 + .../velocity/g1_isaac_rough/env.dexuni.yaml | 188 ++++++++++++++++++ .../locomotion/velocity/isaac/curriculum.py | 48 +++++ .../locomotion/velocity/isaac/runtime.py | 49 ++++- tests/gym/envs/tasks/test_isaac_velocity.py | 43 ++++ tests/learning/test_rl_resume.py | 88 ++++++++ tests/learning/test_rl_resume_distributed.py | 19 ++ 12 files changed, 558 insertions(+), 5 deletions(-) create mode 100644 extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/agents/ppo.dexuni.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/env.dexuni.yaml 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/overview/rl/dexterous.md b/docs/source/overview/rl/dexterous.md index 1bb4aff559..691004a81e 100644 --- a/docs/source/overview/rl/dexterous.md +++ b/docs/source/overview/rl/dexterous.md @@ -26,8 +26,15 @@ embodichain train-rl --config extensions/embodichain_tasks/configs/tasks/manipul Increase `trainer.iterations` and `trainer.num_envs` for a longer experiment. Policy and optimizer state are saved by the standard trainer. Checkpoint evaluation uses the generated RUN manifest through `embodichain eval-policy`. -The core training CLI does not currently expose an optimizer-resume option; -programmatic continuation must restore policy, optimizer and trainer counters. +Continue training with `embodichain train-rl --config CONFIG --resume CHECKPOINT`. +Set `trainer.iterations` to the cumulative target, for example 15 after an initial +10 updates. Resume restores policy normalization, optimizer, scheduler and update +counters, then starts fresh episodes. The shared schema-v2 loader checks task, +physics, policy and algorithm compatibility. Select a manifest checkpoint with +`--resume latest --run RUN_DIRECTORY`; `--resume-scheduler restart` starts the +configured schedule over the remaining updates while preserving optimizer +momentum. Legacy checkpoints without metadata emit a compatibility warning. +Each resumed invocation writes a new RUN directory. The fixed and continuous pen tasks share their model. Continuous spinning samples from 22 retained initial poses. Cube face turning retains the passive diff --git a/docs/source/overview/rl/locomotion.md b/docs/source/overview/rl/locomotion.md index 0d328ac7c4..b6b11201b3 100644 --- a/docs/source/overview/rl/locomotion.md +++ b/docs/source/overview/rl/locomotion.md @@ -75,3 +75,26 @@ can replace the global ground geometry and collision descriptor. Newton collision configuration also accepts `preserve_mesh_transform` to retain the authored collision mesh pose and scale. These options do not change the default settings of existing tasks. + + +## G1 rough terrain with DexUni + +`g1_isaac_rough/agents/ppo.dexuni.yaml` selects the pure MuJoCo branch of +DexUni with Newton-generated terrain contacts. The paired DexSim build must +support `mujoco_options.use_mujoco_contacts: false` in this branch and export +MuJoCo solver contact forces for contact sensors. The MJWarp deployment remains +`ppo.newton.yaml`; both configurations retain the same terrain and physical +parameters. + +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. 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_isaac_rough/agents/ppo.dexuni.yaml b/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/agents/ppo.dexuni.yaml new file mode 100644 index 0000000000..5f186eca21 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/agents/ppo.dexuni.yaml @@ -0,0 +1,59 @@ +trainer: + exp_name: g1_rough_ppo_dexuni + gym_config: ../env.dexuni.yaml + seed: 42 + device: cuda:0 + headless: true + num_envs: 64 + iterations: 20 + buffer_size: 24 + enable_eval: false + save_frequency_updates: 10 + 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: 384 + 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_isaac_rough/catalog.yaml b/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/catalog.yaml index bc8e151c04..c9031d8736 100644 --- a/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/catalog.yaml +++ b/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/catalog.yaml @@ -9,3 +9,5 @@ default_deployment: newton deployments: newton: config: env.newton.yaml + dexuni: + config: env.dexuni.yaml diff --git a/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/env.dexuni.yaml b/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/env.dexuni.yaml new file mode 100644 index 0000000000..d2716696a4 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/env.dexuni.yaml @@ -0,0 +1,188 @@ +physics: newton +id: UnitreeG1IsaacRoughRL-v1 +max_episode_steps: 1000 +num_envs: 1 +scene_node_capacity: 65535 +headless: true +device: cuda:0 +arena_space: 0.0 +render_cfg: + renderer: no-render +physics_config: + sync_to_renderer: false + gravity: + - 0.0 + - 0.0 + - -9.81 + num_substeps: 2 + physics_dt: 0.005 + solver_cfg: + solver_type: dexuni + mujoco_options: + njmax: 300 + nconmax: 300 + iterations: 100 + ls_iterations: 50 + integrator: implicitfast + separate_worlds: true + tolerance: 1.0e-06 + solver: newton + use_mujoco_contacts: false + cone: pyramidal + impratio: 1.0 + ccd_iterations: 35 + collision_options: + broad_phase: explicit + reduce_contacts: true + max_triangle_pairs: 2500000 +env: + 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: + reference_deterministic_reset: false + reference_randomization: true + reference_origin: + - 0.0 + - 0.0 + - 0.0 + reference_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/embodichain_tasks/locomotion/velocity/isaac/curriculum.py b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/curriculum.py index ddc923eba0..ade4682959 100644 --- a/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/curriculum.py +++ b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/curriculum.py @@ -63,6 +63,54 @@ def __init__( ).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, diff --git a/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/runtime.py b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/runtime.py index f585b7fdfb..08a9e8b8a2 100644 --- a/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/runtime.py +++ b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/runtime.py @@ -458,6 +458,42 @@ def get_obs(self, **kwargs: Any) -> EnvObs: 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 _initialize_episode( self, env_ids: Sequence[int] | None = None, **kwargs: Any ) -> None: @@ -469,9 +505,16 @@ def _initialize_episode( if ids.numel() == 0: return if self._reference_terrain_course is not None: - self._reference_terrain_course.update( - ids, self.robot.body_data.root_pose[:, :3], self.reward_command - ) + resume_reset = getattr(self, "_reference_resume_reset", None) + update_ids = ids if resume_reset is None else ids[~resume_reset[ids]] + if update_ids.numel(): + self._reference_terrain_course.update( + update_ids, + self.robot.body_data.root_pose[:, :3], + self.reward_command, + ) + if resume_reset is not None: + resume_reset[ids] = False previous_deferred = getattr(self, "_reference_events_deferred", False) self._reference_events_deferred = True try: diff --git a/tests/gym/envs/tasks/test_isaac_velocity.py b/tests/gym/envs/tasks/test_isaac_velocity.py index c89c21903e..cc1f58ba29 100644 --- a/tests/gym/envs/tasks/test_isaac_velocity.py +++ b/tests/gym/envs/tasks/test_isaac_velocity.py @@ -821,3 +821,46 @@ def test_height_scanner_graph_replays_changed_poses() -> None: 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 ReferenceTerrainCurriculum( + 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: IsaacVelocityEnv.validate_training_state(env, state) + ) + IsaacVelocityEnv.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/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"], From a185a20c320f955f1cd0d48ece488656568ed247 Mon Sep 17 00:00:00 2001 From: acrlw <13927622+acrlw@users.noreply.github.com> Date: Thu, 22 Oct 2026 09:37:15 +0800 Subject: [PATCH 04/24] fix(rl): retain episode success and update portable task assets --- docs/source/overview/rl/dexterous.md | 15 ++ embodichain/data/assets/rl_assets.py | 4 +- .../manipulation/dexterous/_runtime.py | 6 + .../gym/envs/tasks/test_dexterous_metrics.py | 147 ++++++++++++++++++ 4 files changed, 170 insertions(+), 2 deletions(-) create mode 100644 tests/gym/envs/tasks/test_dexterous_metrics.py diff --git a/docs/source/overview/rl/dexterous.md b/docs/source/overview/rl/dexterous.md index 691004a81e..eb45e7e39a 100644 --- a/docs/source/overview/rl/dexterous.md +++ b/docs/source/overview/rl/dexterous.md @@ -43,6 +43,21 @@ and Z-axis rotation use separate actor and critic observation groups. Selective reset preserves unselected physical rows; completed rows return the next initial observation with terminal reward and done flags. +Evaluation reports `eval/success_rate` for pen spinning, cube face turning and +cube reorientation. An episode succeeds if it reaches at least one task success +event before termination or truncation, including an event on the final step. +Pen spinning requires one complete positive revolution while retaining the pen; +face turning requires holding the target face angle for 20 steps; reorientation +requires reaching the target orientation. The environment retains these events +in `info["success"]` until the corresponding episode resets. Per-step events +remain available in `info["metrics"]["success"]`. + +Continuous Z-axis rotation has no binary success criterion. Its success rate is +unavailable; evaluate `rotation`, `angular_velocity_z`, `retained`, episode length +and return instead. A numeric zero success rate for the other tasks means no +evaluated episode met its success criterion. Short training verifies execution +and checkpoint compatibility, not policy convergence. + The runtime currently contains version-specific MJCF import and compact-state adapters for DexSim/Newton. Model audits check imported masses, inertia, actuators, tendons and collision masks before stepping. These adapters need diff --git a/embodichain/data/assets/rl_assets.py b/embodichain/data/assets/rl_assets.py index e437292ca3..e817019437 100644 --- a/embodichain/data/assets/rl_assets.py +++ b/embodichain/data/assets/rl_assets.py @@ -57,7 +57,7 @@ def __init__(self, data_root: str | None = None) -> None: class UnitreeG1Rough(_RLAsset): """Bundled model inputs for UnitreeG1Rough.""" - archive_md5 = "9353ed3cd8888a2d38df32196a8322a5" + archive_md5 = "7c221d16a0c0f684264a78906acd5a96" class G1RoughTerrains(_RLAsset): @@ -82,7 +82,7 @@ class LeapHandCubeFaceTurn(_RLAsset): class LeapHandCube(_RLAsset): """Bundled model inputs for LeapHandCube.""" - archive_md5 = "1fa070d259d36c5d8b1f45a11187bfa8" + archive_md5 = "05d4c95a9a55de427ff48b1a9ea47089" _ASSET_CATALOG: dict[str, dict[str, str]] = { diff --git a/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_runtime.py b/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_runtime.py index 932de53899..4d8ee2faed 100644 --- a/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_runtime.py +++ b/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_runtime.py @@ -165,6 +165,7 @@ def __init__( self.action_format = "tensor" self.elapsed_steps = torch.zeros(num_envs, dtype=torch.long, device=device) self._episode_return = torch.zeros(num_envs, device=device) + self._episode_success = torch.zeros(num_envs, dtype=torch.bool, device=device) self._last_observation: dict[str, torch.Tensor] | None = None self._closed = False config = dexsim.WorldConfig() @@ -612,6 +613,7 @@ def reset( self.mujoco_solver._mujoco_warp.forward(self.mujoco_solver.mjw_model, self.data) self.elapsed_steps[ids] = 0 self._episode_return[ids] = 0 + self._episode_success[ids] = False state = self._state() if self.task_id in ("M1", "M2"): fresh = self._observation(self.task.observe(state)) @@ -676,6 +678,10 @@ def step( result = self._observation(obs) self._last_observation = result info = {"metrics": {key: value.clone() for key, value in metrics.items()}} + if "success" in metrics: + # Task success is an event; evaluation consumes the completed episode. + self._episode_success |= metrics["success"].bool() + info["success"] = self._episode_success.clone() if bool(done.any()): ids = torch.where(done)[0] info["episode"] = { 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..8382aeccac --- /dev/null +++ b/tests/gym/envs/tasks/test_dexterous_metrics.py @@ -0,0 +1,147 @@ +# ---------------------------------------------------------------------------- +# 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 types import SimpleNamespace +from typing import Any, Callable +from unittest.mock import Mock + +import gymnasium as gym +import pytest +import torch + +from embodichain.learning.rl.evaluation import evaluate_episodes +from embodichain_tasks.manipulation.dexterous import _runtime + + +class _Policy(torch.nn.Module): + obs_dim = 1 + action_dim = 1 + + def __init__(self) -> None: + super().__init__() + self.anchor = torch.nn.Parameter(torch.zeros(())) + + def get_action(self, data: Any, deterministic: bool = False) -> Any: + data["action"] = torch.zeros(data.batch_size[0], 1) + return data + + +@pytest.fixture +def env_factory( + monkeypatch: pytest.MonkeyPatch, +) -> Callable[[str], _runtime.DexSimManipulationEnv]: + """Exercise the real step/reset bridge with deterministic task events.""" + monkeypatch.setattr(_runtime.wp, "to_torch", lambda x: x) + monkeypatch.setattr(_runtime.wp, "from_torch", lambda x: x) + + def make(task_id: str): + env = _runtime.DexSimManipulationEnv.__new__(_runtime.DexSimManipulationEnv) + env.task_id = task_id + env.device = torch.device("cpu") + env.num_envs = 2 + env.diagnostics = False + env.observation_dim = env.critic_dim = 1 + env.single_observation_space = gym.spaces.Dict( + {"obs": gym.spaces.Box(-1.0, 1.0, (1,))} + ) + env.single_action_space = gym.spaces.Box(-1.0, 1.0, (1,)) + env.action_format = "tensor" + env.elapsed_steps = torch.zeros(2, dtype=torch.long) + env._episode_return = torch.zeros(2) + env._episode_success = torch.zeros(2, dtype=torch.bool) + env._last_observation = None + env.ctrl = torch.zeros(2, 1) + env.qmap = env.vmap = torch.tensor([0]) + env.data = SimpleNamespace(qpos=torch.zeros(2, 1), qvel=torch.zeros(2, 1)) + env.dexuni_adapter = Mock() + env.backend = Mock() + env.mujoco_solver = Mock() + env.advance_physics = Mock() + env._state = lambda: None + + def task_reset(ids): + values = torch.zeros(len(ids), 1) + if task_id in ("M1", "M2"): + return values, values, values + return dict(qpos=values, qvel=values, ctrl=values) + + def observe(state, env_ids=None): + return torch.zeros(2 if env_ids is None else len(env_ids), 1) + + def step(state, actions): + # Environment 0 succeeds before termination; environment 1 never does. + 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() + result = (torch.zeros(2, 1), torch.ones(2), done) + if task_id in ("M1", "M2"): + return (*result, metrics) + return (*result, torch.zeros(2, dtype=torch.bool), metrics) + + env.task = SimpleNamespace( + reset=task_reset, + observe=observe, + action_targets=lambda x: x, + control_targets=lambda x: x, + step=step, + after_step=step, + ) + return env + + return make + + +@pytest.mark.parametrize("task_id", ["M1", "M2", "M3", "M4"]) +def test_success_survives_until_autoreset_and_reaches_evaluation( + env_factory: Callable, task_id: str +) -> None: + """An early success counts at episode completion even if its pulse has ended.""" + env = env_factory(task_id) + result = evaluate_episodes(policy=_Policy(), 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.parametrize("task_id", ["M1", "M3"]) +def test_partial_reset_clears_only_selected_success_and_keeps_terminal_info( + env_factory: Callable, task_id: str +) -> None: + env = env_factory(task_id) + env._episode_success[:] = True + env.reset(options={"reset_ids": torch.tensor([0])}) + assert env._episode_success.tolist() == [False, True] + env.elapsed_steps[:] = torch.tensor([2, 0]) + _, _, done, _, info = env.step(torch.zeros(2, 1)) + assert done.tolist() == [True, False] + assert info["success"].tolist() == [False, True] + env.reset() + assert not env._episode_success.any() + assert info["success"].tolist() == [False, True] + + +def test_continuous_rotation_has_metrics_without_invented_binary_success( + env_factory: Callable, +) -> None: + env = env_factory("M5") + result = evaluate_episodes(policy=_Policy(), env=env, num_episodes=2, device="cpu") + assert math.isnan(result["eval/success_rate"]) + assert math.isfinite(result["eval/metrics/rotation"]) From ae083db375f2e91f2cc216886ab7d94a2887f1d9 Mon Sep 17 00:00:00 2001 From: acrlw <13927622+acrlw@users.noreply.github.com> Date: Thu, 22 Oct 2026 10:17:16 +0800 Subject: [PATCH 05/24] perf(rl): disable unused per-step context snapshots for G1 rough --- docs/source/overview/rl/locomotion.md | 6 ++++++ .../locomotion/velocity/g1_isaac_rough/env.dexuni.yaml | 1 + .../locomotion/velocity/g1_isaac_rough/env.newton.yaml | 1 + 3 files changed, 8 insertions(+) diff --git a/docs/source/overview/rl/locomotion.md b/docs/source/overview/rl/locomotion.md index b6b11201b3..9ac86fb05c 100644 --- a/docs/source/overview/rl/locomotion.md +++ b/docs/source/overview/rl/locomotion.md @@ -98,3 +98,9 @@ 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. + +The G1 rough deployments set `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. diff --git a/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/env.dexuni.yaml b/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/env.dexuni.yaml index d2716696a4..204fce5e94 100644 --- a/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/env.dexuni.yaml +++ b/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/env.dexuni.yaml @@ -36,6 +36,7 @@ physics_config: reduce_contacts: true max_triangle_pairs: 2500000 env: + episode_context_in_info: false sim_steps_per_control: 4 control_parts: - locomotion diff --git a/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/env.newton.yaml b/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/env.newton.yaml index 88dc52a341..9b029292f2 100644 --- a/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/env.newton.yaml +++ b/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/env.newton.yaml @@ -36,6 +36,7 @@ physics_config: ccd_iterations: 35 physics_dt: 0.005 env: + episode_context_in_info: false sim_steps_per_control: 4 control_parts: - locomotion From 96044b8155eb3448cbc2064285aa589642467111 Mon Sep 17 00:00:00 2001 From: acrlw <13927622+acrlw@users.noreply.github.com> Date: Thu, 22 Oct 2026 10:26:27 +0800 Subject: [PATCH 06/24] fix(rl): reserve face-turn constraint capacity for large batches --- docs/source/overview/rl/dexterous.md | 6 ++++++ .../embodichain_tasks/manipulation/dexterous/_runtime.py | 4 +++- 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/docs/source/overview/rl/dexterous.md b/docs/source/overview/rl/dexterous.md index eb45e7e39a..560f46200c 100644 --- a/docs/source/overview/rl/dexterous.md +++ b/docs/source/overview/rl/dexterous.md @@ -64,3 +64,9 @@ actuators, tendons and collision masks before stepping. These adapters need engine-owned public interfaces before this task collection can be treated as a backend-independent release. The local validation versions and publication status must accompany a release; short-run checkpoints are not converged policies. + +The face-turn task reserves 2048 constraint rows per world. A 512-row budget +can truncate constraints during large-batch training. Solver iteration limits +remain distinct from capacity limits: source defaults for LEAP tasks use +5 solver iterations and 8 line-search iterations on MJWarp; increase the +iteration budget explicitly when validating convergence. diff --git a/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_runtime.py b/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_runtime.py index 4d8ee2faed..57ca5b7fd4 100644 --- a/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_runtime.py +++ b/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_runtime.py @@ -186,7 +186,9 @@ def __init__( "tolerance": float(self.source.opt.tolerance), "ls_tolerance": float(self.source.opt.ls_tolerance), "ccd_tolerance": float(self.source.opt.ccd_tolerance), - "njmax": 512, + # Face-turn contacts and joint constraints exceed 512 rows in + # large batches. Keep headroom for the 256-contact allocation. + "njmax": 2048 if task_id == "M3" else 512, "nconmax": 256, } self.backend_option_overrides = {} From 105b382d03cda514e2dfb4b77c573c0a7eded243 Mon Sep 17 00:00:00 2001 From: acrlw <13927622+acrlw@users.noreply.github.com> Date: Thu, 22 Oct 2026 12:13:50 +0800 Subject: [PATCH 07/24] perf(tasks): use cubql BVHs for G1 rough terrain queries --- .../tasks/locomotion/velocity/g1_isaac_rough/env.dexuni.yaml | 1 + .../tasks/locomotion/velocity/g1_isaac_rough/env.newton.yaml | 1 + 2 files changed, 2 insertions(+) diff --git a/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/env.dexuni.yaml b/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/env.dexuni.yaml index 204fce5e94..f117cd0d97 100644 --- a/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/env.dexuni.yaml +++ b/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/env.dexuni.yaml @@ -9,6 +9,7 @@ arena_space: 0.0 render_cfg: renderer: no-render physics_config: + mesh_bvh_constructor: cubql sync_to_renderer: false gravity: - 0.0 diff --git a/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/env.newton.yaml b/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/env.newton.yaml index 9b029292f2..18f7fd6608 100644 --- a/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/env.newton.yaml +++ b/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/env.newton.yaml @@ -9,6 +9,7 @@ arena_space: 0.0 render_cfg: renderer: no-render physics_config: + mesh_bvh_constructor: cubql sync_to_renderer: false gravity: - 0.0 From c376eaf59abf8284150d58fe185c1240dde7efa5 Mon Sep 17 00:00:00 2001 From: acrlw <13927622+acrlw@users.noreply.github.com> Date: Thu, 22 Oct 2026 13:28:06 +0800 Subject: [PATCH 08/24] fix: Match the G1 asset archive with its bundled BSD license --- embodichain/data/assets/rl_assets.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/embodichain/data/assets/rl_assets.py b/embodichain/data/assets/rl_assets.py index e817019437..9fd7f7ad44 100644 --- a/embodichain/data/assets/rl_assets.py +++ b/embodichain/data/assets/rl_assets.py @@ -57,7 +57,7 @@ def __init__(self, data_root: str | None = None) -> None: class UnitreeG1Rough(_RLAsset): """Bundled model inputs for UnitreeG1Rough.""" - archive_md5 = "7c221d16a0c0f684264a78906acd5a96" + archive_md5 = "b2ff7af5456458a38f96c03dc4ae39db" class G1RoughTerrains(_RLAsset): From 89612bacfeb934c6020a642176da1111841e5959 Mon Sep 17 00:00:00 2001 From: acrlw <13927622+acrlw@users.noreply.github.com> Date: Fri, 23 Oct 2026 11:32:26 +0800 Subject: [PATCH 09/24] fix(tasks): explicitly enable G1 terrain heightfield conversion --- .../embodichain_tasks/locomotion/velocity/isaac/runtime.py | 1 + tests/gym/envs/tasks/test_isaac_velocity.py | 4 +++- 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/runtime.py b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/runtime.py index 08a9e8b8a2..5adac56591 100644 --- a/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/runtime.py +++ b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/runtime.py @@ -95,6 +95,7 @@ def _prepare_scene(self, **kwargs: Any) -> None: 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 diff --git a/tests/gym/envs/tasks/test_isaac_velocity.py b/tests/gym/envs/tasks/test_isaac_velocity.py index cc1f58ba29..61bce1a421 100644 --- a/tests/gym/envs/tasks/test_isaac_velocity.py +++ b/tests/gym/envs/tasks/test_isaac_velocity.py @@ -337,7 +337,9 @@ def test_ground_replacement_updates_declared_geometry() -> None: 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(heightfield_resolution=0.1) + collision = NewtonCollisionDesc( + convert_to_heightfield=True, heightfield_resolution=0.1 + ) sim.configure_ground_plane_collision(newton=collision, geometry=mesh) assert declared.collisions[0].geometry_type == mesh.geometry_type assert declared.collisions[0].newton is collision From 8b60d52b2176c45348e5234ad22a57e8939abf23 Mon Sep 17 00:00:00 2001 From: acrlw <13927622+acrlw@users.noreply.github.com> Date: Mon, 2 Nov 2026 10:51:03 +0800 Subject: [PATCH 10/24] feat(tasks): load dexterous manipulation scenes from USD --- .../topics/data-assets/data-assets.md | 7 +- docs/source/overview/rl/dexterous.md | 17 +- embodichain/data/assets/rl_assets.py | 6 +- .../manipulation/dexterous/_audit_model.py | 177 ++++++++- .../manipulation/dexterous/_dexuni_adapter.py | 135 +------ .../manipulation/dexterous/_runtime.py | 344 +++++++++--------- tests/data/test_download.py | 24 ++ .../gym/envs/tasks/test_dexterous_metrics.py | 7 +- tests/gym/envs/tasks/test_dexterous_usd.py | 131 +++++++ 9 files changed, 516 insertions(+), 332 deletions(-) create mode 100644 tests/gym/envs/tasks/test_dexterous_usd.py diff --git a/agent_context/topics/data-assets/data-assets.md b/agent_context/topics/data-assets/data-assets.md index 1db8e0f883..a2bf2edda1 100644 --- a/agent_context/topics/data-assets/data-assets.md +++ b/agent_context/topics/data-assets/data-assets.md @@ -256,8 +256,9 @@ 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 CLI -adds four robot entries and the `G1RoughTerrains` scene entry, based on each -class's archive category. The G1 Rough asset does not replace +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/docs/source/overview/rl/dexterous.md b/docs/source/overview/rl/dexterous.md index 560f46200c..7a9d9169a0 100644 --- a/docs/source/overview/rl/dexterous.md +++ b/docs/source/overview/rl/dexterous.md @@ -58,11 +58,18 @@ and return instead. A numeric zero success rate for the other tasks means no evaluated episode met its success criterion. Short training verifies execution and checkpoint compatibility, not policy convergence. -The runtime currently contains version-specific MJCF import and compact-state -adapters for DexSim/Newton. Model audits check imported masses, inertia, -actuators, tendons and collision masks before stepping. These adapters need -engine-owned public interfaces before this task collection can be treated as -a backend-independent release. The local validation versions and publication +SceneBuilder loads the packaged `usd/*.usdc` physics models and replicates them +for each environment. MJCF remains in the asset packages as task metadata and +an independent model-audit reference. Joint targets and passive face torque use +public articulation batches. The assets require DexSim's retained MuJoCo scene +metadata support, including fixed tendons, explicit filters and actuator limits. + +Model audits compare masses, inertia, actuator and tendon parameters, and the +complete effective collision graph against the source MJCF. They read active +device parameters because Newton notifications can leave the host model stale. +Both solver configurations retain source iteration settings unless explicitly +overridden. Reset still uses a version-specific MuJoCo coordinate bridge and, +for DexUni, compact-to-scene maps. Local validation versions and asset publication status must accompany a release; short-run checkpoints are not converged policies. The face-turn task reserves 2048 constraint rows per world. A 512-row budget diff --git a/embodichain/data/assets/rl_assets.py b/embodichain/data/assets/rl_assets.py index 9fd7f7ad44..91985332e1 100644 --- a/embodichain/data/assets/rl_assets.py +++ b/embodichain/data/assets/rl_assets.py @@ -70,19 +70,19 @@ class G1RoughTerrains(_RLAsset): class ShadowHandPen(_RLAsset): """Bundled model inputs for ShadowHandPen.""" - archive_md5 = "3730a16ed282a92d663245fd56e07a96" + archive_md5 = "d5642a9fb20bf49be918455826c35a97" class LeapHandCubeFaceTurn(_RLAsset): """Bundled model inputs for LeapHandCubeFaceTurn.""" - archive_md5 = "e148431937375c1469704f1c0b9b4a15" + archive_md5 = "6394cd7665963f3fdcacbcf170a9af79" class LeapHandCube(_RLAsset): """Bundled model inputs for LeapHandCube.""" - archive_md5 = "05d4c95a9a55de427ff48b1a9ea47089" + archive_md5 = "1bf309614884c706e225d7cd5165b949" _ASSET_CATALOG: dict[str, dict[str, str]] = { diff --git a/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_audit_model.py b/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_audit_model.py index 3c95f9520b..f08213cefd 100644 --- a/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_audit_model.py +++ b/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_audit_model.py @@ -18,6 +18,8 @@ from __future__ import annotations +import copy + from collections.abc import Mapping, Sequence import re from typing import Any @@ -25,7 +27,7 @@ import mujoco import numpy as np -__all__ = ["audit_model"] +__all__ = ["audit_model", "snapshot_execution_model"] def _rotation(quat: np.ndarray) -> np.ndarray: @@ -73,6 +75,92 @@ def has_dynamic_ancestor(body: int) -> bool: return "eligible" +def snapshot_execution_model( + solver: Any, *, derived_cache_report: dict[str, Any] | None = None +) -> mujoco.MjModel: + """Read the active solver parameters into a detached audit model. + + Newton notifications can update device parameters without updating the + host MjModel. This initialization-only readback validates homogeneous + worlds and never changes either live model. + """ + result = copy.copy(solver.mj_model) + remote = getattr(solver, "mjw_model", None) + if remote is None: + return result + for name in dir(result): + local = getattr(result, name) + device_value = getattr(remote, name, None) + if not isinstance(local, np.ndarray) or not hasattr(device_value, "numpy"): + continue + values = device_value.numpy() + if name in { + "actuator_acc0", + "body_invweight0", + "dof_invweight0", + "dof_M0", + "body_subtreemass", + "tendon_invweight0", + }: + # These solver-computed caches are not authored inputs checked below. + # Keep their batch variation visible without treating it as lost USD + # metadata; strict rollout comparison validates its output effects. + if derived_cache_report is not None and values.size: + spread = ( + values - values[0] + if values.ndim == local.ndim + 1 + else np.zeros_like(values) + ) + derived_cache_report[name] = { + "shape": list(values.shape), + "max_world_spread": float(np.max(np.abs(spread))), + } + continue + if values.shape == local.shape: + local[:] = values + elif values.ndim == local.ndim + 1 and values.shape[1:] == local.shape: + if len(values) and not np.array_equal( + values, np.broadcast_to(values[0], values.shape) + ): + raise ValueError(f"Nonuniform execution model field: {name}") + if len(values): + local[:] = values[0] + for name in ( + "gravity", + "timestep", + "integrator", + "solver", + "iterations", + "ls_iterations", + "ccd_iterations", + "cone", + "impratio", + "tolerance", + "ls_tolerance", + "ccd_tolerance", + "enableflags", + "disableflags", + ): + value = getattr(remote.opt, name, None) + if value is None: + continue + if hasattr(value, "numpy"): + values = value.numpy() + shape = np.asarray(getattr(result.opt, name)).shape + if values.shape == shape: + value = values if shape else values.item() + elif values.ndim and values.shape[1:] == shape: + if not np.array_equal(values, np.broadcast_to(values[0], values.shape)): + raise ValueError(f"Nonuniform execution option: {name}") + value = values[0] + else: + raise ValueError( + f"Unexpected execution option shape: {name}: {values.shape}" + ) + setattr(result.opt, name, value) + return result + + def audit_model( source: Any, imported: Any, @@ -80,6 +168,7 @@ def audit_model( actuator_map: Sequence[int], body_map: Mapping[str, int], *, + explicit_collision_filtering: bool = False, atol: float = 2e-6, rtol: float = 2e-5, ) -> dict[str, Any]: @@ -91,6 +180,8 @@ def audit_model( joint_map: Source joint index to imported joint index. actuator_map: Imported actuator indices in source actuator order. body_map: Source body names to imported body indices. + explicit_collision_filtering: Parent exclusions are encoded explicitly; + the complete effective geometry-pair graph is still compared. atol: Absolute numeric tolerance. rtol: Relative numeric tolerance. @@ -116,7 +207,14 @@ def compare( semantic_a.flat[list(ignored_indices)] = 0 semantic_b.flat[list(ignored_indices)] = 0 difference = ( - float(np.max(np.abs(a.astype(float) - b.astype(float)))) + float( + np.max( + np.abs( + np.where(a == b, 0.0, a.astype(float)) + - np.where(a == b, 0.0, b.astype(float)) + ) + ) + ) if same_shape and a.size else 0.0 ) @@ -155,11 +253,20 @@ def compare( "enableflags", "disableflags", ): - compare( - f"option.{field}", getattr(source.opt, field), getattr(imported.opt, field) - ) + expected = getattr(source.opt, field) + if field == "disableflags" and explicit_collision_filtering: + expected = int(expected) | int(mujoco.mjtDisableBit.mjDSBL_FILTERPARENT) + compare(f"option.{field}", expected, getattr(imported.opt, field)) + if field == "disableflags" and explicit_collision_filtering: + checks[-1]["raw_matches"] = int(source.opt.disableflags) == int( + imported.opt.disableflags + ) + checks[-1][ + "equivalence_reason" + ] = "Parent filtering is represented by explicit exclusions; all effective geometry pairs are checked." bmap = {0: 0} + merged_static_bodies = set() for source_id in range(1, source.nbody): name = source.body(source_id).name if name not in body_map: @@ -169,6 +276,20 @@ def compare( continue imported_id = body_map[name] bmap[source_id] = imported_id + if ( + imported_id == 0 + and source.body_weldid[source_id] == 0 + and source.body_mocapid[source_id] < 0 + ): + merged_static_bodies.add(source_id) + checks.append( + { + "field": f"body[{name}].static_world_merge", + "matches": True, + "equivalence_reason": "Static geometry transforms and contacts are checked in the world frame.", + } + ) + continue for field in ("body_mass", "body_ipos", "body_pos"): ignored = ( (0, 1, 2) @@ -211,8 +332,6 @@ def compare( "jnt_axis", "jnt_limited", "jnt_range", - "jnt_actfrclimited", - "jnt_actfrcrange", "jnt_solref", "jnt_solimp", "jnt_stiffness", @@ -224,12 +343,6 @@ def compare( and not imported.jnt_limited[imported_id] ): ignored = (0, 1) - if ( - field == "jnt_actfrcrange" - and not source.jnt_actfrclimited[source_id] - and not imported.jnt_actfrclimited[imported_id] - ): - ignored = (0, 1) compare( f"joint[{name}].{field}", getattr(source, field)[source_id], @@ -237,6 +350,25 @@ def compare( ignored_indices=ignored, reason="Range is inactive because both limit flags are disabled.", ) + source_force_range = ( + source.jnt_actfrcrange[source_id] + if source.jnt_actfrclimited[source_id] + else (-np.inf, np.inf) + ) + imported_force_range = ( + imported.jnt_actfrcrange[imported_id] + if imported.jnt_actfrclimited[imported_id] + else (-np.inf, np.inf) + ) + compare( + f"joint[{name}].effective_actuator_force_range", + source_force_range, + imported_force_range, + ) + checks[-1]["source_limit_flag"] = bool(source.jnt_actfrclimited[source_id]) + checks[-1]["imported_limit_flag"] = bool( + imported.jnt_actfrclimited[imported_id] + ) width = (6, 3, 1, 1)[int(source.jnt_type[source_id])] sa, ia = int(source.jnt_dofadr[source_id]), int( imported.jnt_dofadr[imported_id] @@ -272,8 +404,19 @@ def compare( imported.geom_contype | imported.geom_conaffinity ) used: set[int] = set() + static_reference = mujoco.MjData(source) + mujoco.mj_kinematics(source, static_reference) for sid in source_collision: name = source.geom(int(sid)).name + source_position = source.geom_pos[sid] + source_rotation = _rotation(source.geom_quat[sid]) + source_body = int(source.geom_bodyid[sid]) + if source_body in merged_static_bodies: + body_rotation = static_reference.xmat[source_body].reshape(3, 3) + source_position = ( + static_reference.xpos[source_body] + body_rotation @ source_position + ) + source_rotation = body_rotation @ source_rotation candidates = [ int(i) for i in imported_collision @@ -300,7 +443,7 @@ def compare( for i in candidates if imported.geom_type[i] == source.geom_type[sid] and np.allclose( - imported.geom_pos[i], source.geom_pos[sid], atol=atol, rtol=rtol + imported.geom_pos[i], source_position, atol=atol, rtol=rtol ) ] if len(geometric) != 1: @@ -355,14 +498,14 @@ def compare( ignored, reason = (1, 2), "Sphere collision uses only radius." compare( f"geom[{name or sid}].{field}", - getattr(source, field)[sid], + source_position if field == "geom_pos" else getattr(source, field)[sid], getattr(imported, field)[target], ignored_indices=ignored, reason=reason, ) compare( f"geom[{name or sid}].rotation", - _rotation(source.geom_quat[sid]), + source_rotation, _rotation(imported.geom_quat[target]), ignored_indices=tuple(range(9)) if is_mesh else (), reason="Mesh principal frame may change; body-frame vertices are checked.", @@ -381,7 +524,7 @@ def compare( vb = imported.mesh_vert[ imported_start : imported_start + imported.mesh_vertnum[imported_mesh] ] - va = va @ _rotation(source.geom_quat[sid]).T + source.geom_pos[sid] + va = va @ source_rotation.T + source_position vb = ( vb @ _rotation(imported.geom_quat[target]).T + imported.geom_pos[target] ) diff --git a/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_dexuni_adapter.py b/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_dexuni_adapter.py index 17607f3be1..8583196d49 100644 --- a/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_dexuni_adapter.py +++ b/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_dexuni_adapter.py @@ -17,144 +17,13 @@ from __future__ import annotations -import copy -import types from typing import Any -import newton -import numpy as np import torch import warp as wp from newton import Model -__all__ = [ - "import_dexuni_mjcf", - "select_all_articulations", - "refresh_collision_filters", - "DexUniStateAdapter", -] - - -def import_dexuni_mjcf(builder: Any, path: str) -> dict[str, Any]: - """Parse MJCF after DexUni has registered its private solver attributes. - - Newton 1.7's parser registers standalone solver callbacks, which conflict - with DexUni's callbacks. A function-local globals copy selects the private - class while retaining the parser code and flags. Parser-only attributes - absent from the private schema are copied from a fresh public builder. - """ - from newton._src.solvers.mjvbd_v2.mujoco.solver_mujoco import ( - SolverMuJoCo as PrivateMuJoCo, - ) - from newton._src.utils.import_mjcf import parse_mjcf - - reference = newton.ModelBuilder() - newton.solvers.SolverMuJoCo.register_custom_attributes(reference) - additions = [] - for name, attribute in reference.custom_attributes.items(): - if name not in builder.custom_attributes: - builder.add_custom_attribute(copy.deepcopy(attribute)) - additions.append(name) - parser = types.FunctionType( - parse_mjcf.__code__, - {**parse_mjcf.__globals__, "SolverMuJoCo": PrivateMuJoCo}, - parse_mjcf.__name__, - parse_mjcf.__defaults__, - parse_mjcf.__closure__, - ) - parser.__kwdefaults__ = parse_mjcf.__kwdefaults__ - parser( - builder, - path, - ctrl_direct=True, - parse_visuals=False, - parse_sites=True, - collapse_fixed_joints=False, - ) - empty_references = {} - for kind in ("joint", "wrap"): - addresses = builder.custom_attributes[f"mujoco:tendon_{kind}_adr"].values - counts = builder.custom_attributes[f"mujoco:tendon_{kind}_num"].values - changed = 0 - for index, count in enumerate(counts): - if count == 0 and addresses[index] != -1: - # A zero-length range has no referenced row. Keeping address - # zero makes the compact view reject fixed tendons when the - # spatial-wrap domain is empty. - addresses[index] = -1 - changed += 1 - empty_references[kind] = changed - return { - "schema_additions": additions, - "empty_tendon_reference_corrections": empty_references, - "parser_global_state_modified": False, - } - - -def select_all_articulations(backend: Any) -> None: - """Assign every imported articulation, including free objects, to DexUni. - - Call after adding all worlds and before finalizing the DexSim backend. - DexUni's pure_mujoco dispatch then keeps hand-object contacts and tendons - within one private solver. A standalone MJWarp solver is never constructed. - """ - if backend.solver_type != "dexuni": - raise ValueError("Expected a DexUni backend") - count = int(backend.builder.articulation_count) - if count <= 0: - raise ValueError("The manipulation scene contains no articulations") - backend.cfg.solver_cfg.mujoco_articulations = tuple(range(count)) - - -def refresh_collision_filters(solver: Any) -> dict[str, Any]: - """Rebuild collision caches after restoring host MJCF contact masks. - - MuJoCo Warp precomputes eligible pairs and collision-kernel type counts in - ``put_model``. Copy these derived fields while retaining the existing - world-expanded dynamics arrays and data allocations. - """ - model = solver.mj_model - for body_field, geom_field in ( - ("body_contype", "geom_contype"), - ("body_conaffinity", "geom_conaffinity"), - ): - values = getattr(model, body_field) - values[:] = 0 - np.bitwise_or.at(values, model.geom_bodyid, getattr(model, geom_field)) - previous = solver.mjw_model.nxn_geom_pair_filtered.numpy() - with wp.ScopedDevice(solver.model.device): - fresh = solver._mujoco_warp.put_model(model) - updated = fresh.nxn_geom_pair_filtered.numpy() - old_pairs = set(map(tuple, previous.tolist())) - new_pairs = set(map(tuple, updated.tolist())) - added = sorted(new_pairs - old_pairs) - if added: - raise ValueError( - "Source collision-mask restoration adds pairs; existing contact " - f"allocations require a separate capacity check: {added[:8]}" - ) - fields = ( - "geom_contype", - "geom_conaffinity", - "body_contype", - "body_conaffinity", - "nxn_geom_pair", - "nxn_geom_pair_filtered", - "nxn_pairid", - "nxn_pairid_filtered", - "geom_pair_type_count", - ) - for field in fields: - setattr(solver.mjw_model, field, getattr(fresh, field)) - solver.mjw_model.opt.broadphase = fresh.opt.broadphase - return { - "status": "refreshed", - "fields": list(fields) + ["opt.broadphase"], - "eligible_pairs_before": len(old_pairs), - "eligible_pairs_after": len(new_pairs), - "removed_pairs": len(old_pairs - new_pairs), - "added_pairs": len(added), - } +__all__ = ["DexUniStateAdapter"] class DexUniStateAdapter: @@ -208,7 +77,7 @@ def __init__(self, backend: Any) -> None: self.fields.append((name, indices, worlds[indices])) def publish_reset(self, data: Any, env_ids: torch.Tensor) -> None: - """Publish selected MJCF coordinate resets into both scene buffers.""" + """Publish selected MuJoCo coordinate resets into both scene buffers.""" selected = [ (name, indices, torch.isin(worlds, env_ids)) for name, indices, worlds in self.fields diff --git a/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_runtime.py b/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_runtime.py index 57ca5b7fd4..d4cd328fb8 100644 --- a/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_runtime.py +++ b/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_runtime.py @@ -47,19 +47,24 @@ } -def _named_index(model: Any, kind: str, name: str) -> int: +def _named_index( + model: Any, kind: str, name: str, *, default: int | None = None +) -> int: count = getattr(model, "n" + ("jnt" if kind == "joint" else kind)) candidates = [ i for i in range(count) if getattr(model, kind)(i).name == name or getattr(model, kind)(i).name.endswith("_" + name) + or getattr(model, kind)(i).name.rsplit("/", 1)[-1] == name or ( kind == "geom" and re.sub(r"_\d+$", "", getattr(model, kind)(i).name).split("/")[-1] == name ) ] + if not candidates and default is not None: + return default if len(candidates) != 1: raise ValueError(f"Cannot resolve {kind} {name!r}: {candidates}") return candidates[0] @@ -74,9 +79,9 @@ def rotate_wxyz(q: torch.Tensor, v: torch.Tensor) -> torch.Tensor: class DexSimManipulationEnv: """Expose real DexSim physics through EmbodiChain's vector-env interface. - The task-local MJCF adapter uses the World-owned backend to retain tendons, - passive joints and direct actuators. Native stepping belongs to DexSim; - MuJoCo forward calls below only update kinematics after explicit reset. + SceneBuilder loads the USD physics model. MJCF supplies task metadata and + the import audit reference. DexSim owns simulation stepping; MuJoCo forward + calls below update kinematics after explicit reset. """ def __init__( @@ -146,6 +151,11 @@ def __init__( if task_id in ("M1", "M2", "M3"): self.source.opt.ccd_iterations = 10 self.xml = xml + self.usd = xml.parent / "usd" / f"{xml.stem}.usdc" + if not self.usd.is_file(): + raise FileNotFoundError( + f"The manipulation asset lacks its USD model: {self.usd}" + ) self.substeps = round(self.ctrl_dt / self.sim_dt) observation_spaces = { "obs": gym.spaces.Box( @@ -190,25 +200,26 @@ def __init__( # large batches. Keep headroom for the 256-contact allocation. "njmax": 2048 if task_id == "M3" else 512, "nconmax": 256, + "deterministic": wp.DeterministicMode.RUN_TO_RUN, + "graph_conditional": False, + "update_data_interval": 0, } self.backend_option_overrides = {} - if solver_iterations is not None or solver_kind == "dexuni": - selected_iterations = solver_iterations or (20, 50) + if solver_iterations is not None: + selected_iterations = solver_iterations for name, value in zip( ("iterations", "ls_iterations"), selected_iterations ): self.backend_option_overrides[name] = { "source": solver_options[name], "configured": value, - "reason": ( - "Explicit benchmark settings" - if solver_iterations is not None - else "DexUni configuration shared with Unitree validation." - ), + "reason": "Explicit benchmark settings", } solver_options[name] = value if solver_kind == "dexuni": - solver_cfg = DexUniSolverCfg(mujoco_options=solver_options) + solver_cfg = DexUniSolverCfg( + mujoco_options=solver_options, mujoco_joints="all" + ) else: solver_cfg = MJWarpSolverCfg(**solver_options) config.newton_cfg = NewtonCfg( @@ -223,28 +234,17 @@ def __init__( self.world = dexsim.World(config) self.world.set_manual_update(True) self.backend = get_newton_backend(self.world) - template = self.backend._create_builder() - template.default_shape_cfg.gap = 0.0 - self.mjcf_import_adapter = {} - if solver_kind == "dexuni": - from ._dexuni_adapter import import_dexuni_mjcf - - self.mjcf_import_adapter = import_dexuni_mjcf(template, str(xml)) - else: - template.add_mjcf( - str(xml), - ctrl_direct=True, - parse_visuals=False, - parse_sites=True, - collapse_fixed_joints=False, - ) - for _ in range(num_envs): - self.backend.builder.add_world(template) - if solver_kind == "dexuni": - from ._dexuni_adapter import select_all_articulations - - select_all_articulations(self.backend) - self.backend.finalize() + builder = dexsim.spawn.SceneBuilder(self.world) + builder.add_usd("asset", str(self.usd)) + self._explicit_collision_filtering = bool( + builder.scene_desc.newton + and builder.scene_desc.newton.use_explicit_collision_filtering + ) + self._articulation_names = tuple( + item.name for item in builder.scene_desc.articulations + ) + builder.replicate(num_envs, spacing=(0.0, 0.0, 0.0)) + self.scene = builder.finalize() self.solver = self.backend.solver self.mujoco_solver = self.backend.mujoco_solver self.dexuni_adapter = None @@ -254,22 +254,24 @@ def __init__( self.dexuni_adapter = DexUniStateAdapter(self.backend) self.imported = self.mujoco_solver.mj_model self.data = self.mujoco_solver.mjw_data - self.option_import_corrections = { - name: { - "source": float(getattr(self.source.opt, name)), - "exported": float(getattr(self.imported.opt, name)), - } - for name in ("timestep", "disableflags", "enableflags") - if getattr(self.source.opt, name) != getattr(self.imported.opt, name) - } - for name in ("disableflags", "enableflags"): - value = int(getattr(self.source.opt, name)) - setattr(self.imported.opt, name, value) - setattr(self.mujoco_solver.mjw_model.opt, name, value) + self.option_import_corrections = {} + # World.dt owns the step size; Newton's host MjModel keeps the compiler + # default until explicitly synchronized for the task model audit. self.imported.opt.timestep = self.sim_dt wp.to_torch(self.mujoco_solver.mjw_model.opt.timestep).fill_(self.sim_dt) - self.backend.invalidate_cuda_graph("source MJCF options restored") self._make_mapping() + model = self.backend.model + joint_world = wp.to_torch(model.joint_world).long() + self._reset_field_worlds = { + "joint_q": torch.repeat_interleave( + joint_world, torch.diff(wp.to_torch(model.joint_q_start)).long() + ), + "joint_qd": torch.repeat_interleave( + joint_world, torch.diff(wp.to_torch(model.joint_qd_start)).long() + ), + "body_q": wp.to_torch(model.body_world).long(), + "body_qd": wp.to_torch(model.body_world).long(), + } self.reset(seed=seed) @property @@ -277,7 +279,12 @@ def unwrapped(self) -> DexSimManipulationEnv: return self def _make_mapping(self) -> None: - source, imported = self.source, self.imported + from ._audit_model import snapshot_execution_model + + derived_cache_report = {} + source, imported = self.source, snapshot_execution_model( + self.mujoco_solver, derived_cache_report=derived_cache_report + ) for name in ("nq", "nv", "nu", "ntendon"): if getattr(source, name) != getattr(imported, name): raise ValueError( @@ -287,15 +294,18 @@ def _make_mapping(self) -> None: joint_map = {} for j in range(source.njnt): name = source.joint(j).name - if name: + if name and source.jnt_type[j] != mujoco.mjtJoint.mjJNT_FREE: target = _named_index(imported, "joint", name) else: body = _named_index( imported, "body", source.body(int(source.jnt_bodyid[j])).name ) - candidates = np.flatnonzero(imported.jnt_bodyid == body) + candidates = np.flatnonzero( + (imported.jnt_bodyid == body) + & (imported.jnt_type == source.jnt_type[j]) + ) if len(candidates) != 1: - raise ValueError("Unnamed joint cannot be resolved unambiguously") + raise ValueError("Root joint cannot be resolved unambiguously") target = int(candidates[0]) joint_map[j] = target width = (7, 4, 1, 1)[int(source.jnt_type[j])] @@ -324,14 +334,13 @@ def _make_mapping(self) -> None: self.qmap = torch.tensor(qmap, device=self.device) self.vmap = torch.tensor(vmap, device=self.device) self.amap = torch.tensor(amap, device=self.device) - # Native direct-control slots retain source import order, whereas the - # solver may reorder actuators in its compiled MuJoCo model. - self.ctrl = wp.to_torch(self.backend.control.mujoco.ctrl).reshape( - self.num_envs, -1 - ) + self.ctrl = torch.zeros((self.num_envs, source.nu), device=self.device) + self._bind_controls() self.model_audit = { + "solver_derived_cache_variation": derived_cache_report, "source": str(self.xml), - "mjcf_import_adapter": self.mjcf_import_adapter, + "usd_source": str(self.usd), + "parameter_source": "Active solver device parameters at initialization.", "backend_option_overrides": self.backend_option_overrides, "solver_selection": { "requested": self.solver_kind, @@ -407,64 +416,20 @@ def _make_mapping(self) -> None: f"Imported actuator/tendon properties changed: {self.model_audit}" ) self.body_map = { - source.body(i).name: _named_index(imported, "body", source.body(i).name) + source.body(i).name: _named_index( + imported, + "body", + source.body(i).name, + default=( + 0 + if source.body_weldid[i] == 0 and source.body_mocapid[i] < 0 + else None + ), + ) for i in range(1, source.nbody) if source.body(i).name } - # Newton 1.7 does not export free-joint damping and adds finite force - # limits to originally unlimited joints. Preserve the source arrays in - # both MuJoCo representations before any captured physical step. - corrections = [] - correction_fields = [ - ("dof_damping", vmap), - ("jnt_actfrclimited", [joint_map[i] for i in range(source.njnt)]), - ("jnt_actfrcrange", [joint_map[i] for i in range(source.njnt)]), - ] - if self.solver_kind == "dexuni": - correction_fields.append( - ("jnt_solref", [joint_map[i] for i in range(source.njnt)]) - ) - for field, indices in correction_fields: - expected = getattr(source, field) - actual = getattr(imported, field) - difference = float(np.max(np.abs(expected.astype(float) - actual[indices]))) - if difference: - corrections.append({"field": field, "max_abs_error_before": difference}) - actual[indices] = expected - target = wp.to_torch(getattr(self.mujoco_solver.mjw_model, field)) - values = torch.as_tensor( - expected, device=self.device, dtype=target.dtype - ) - if field == "jnt_actfrclimited": - target[indices] = values - else: - target[:, indices] = values - for geom_id in range(source.ngeom): - name = source.geom(geom_id).name - if not name or not ( - source.geom_contype[geom_id] or source.geom_conaffinity[geom_id] - ): - continue - try: - imported_id = _named_index(imported, "geom", name) - except ValueError: - continue - if ( - abs(source.geom_margin[geom_id] - imported.geom_margin[imported_id]) - > 1e-8 - ): - corrections.append( - { - "field": f"geom[{name}].margin", - "source": float(source.geom_margin[geom_id]), - "exported": float(imported.geom_margin[imported_id]), - } - ) - imported.geom_margin[imported_id] = source.geom_margin[geom_id] - wp.to_torch(self.mujoco_solver.mjw_model.geom_margin)[ - :, imported_id - ] = source.geom_margin[geom_id] - self.model_audit["property_import_corrections"] = corrections + self.model_audit["property_import_corrections"] = [] from ._audit_model import audit_model audit_source = source @@ -472,31 +437,13 @@ def _make_mapping(self) -> None: audit_source = copy.copy(source) for name, item in self.backend_option_overrides.items(): setattr(audit_source.opt, name, item["configured"]) - if self.solver_kind == "dexuni": - from ._dexuni_adapter import refresh_collision_filters - - mapping_audit = audit_model( - audit_source, imported, joint_map, amap, self.body_map - ) - for source_id, imported_id in mapping_audit["collision_geom_map"].items(): - for field in ("geom_contype", "geom_conaffinity"): - before = int(getattr(imported, field)[imported_id]) - expected = int(getattr(source, field)[source_id]) - if before != expected: - corrections.append( - { - "field": f"geom[{source.geom(source_id).name}].{field}", - "source": expected, - "exported": before, - } - ) - getattr(imported, field)[imported_id] = expected - self.model_audit["collision_cache_refresh"] = refresh_collision_filters( - self.mujoco_solver - ) - self.backend.invalidate_cuda_graph("source collision filters restored") self.model_audit["physical_model"] = audit_model( - audit_source, imported, joint_map, amap, self.body_map + audit_source, + imported, + joint_map, + amap, + self.body_map, + explicit_collision_filtering=self._explicit_collision_filtering, ) self.model_audit["physical_model"]["option_reference"] = ( "Source model with explicit backend_option_overrides." @@ -512,24 +459,67 @@ def _make_mapping(self) -> None: raise ValueError( f"Imported physical model differs: {self.model_audit['physical_model']['mismatches']}" ) - if self.task_id == "M3": - labels = self.backend.model.joint_label - joints = [ - i - for i, label in enumerate(labels) - if label == "face_turn" - or label.endswith("/face_turn") - or label.endswith("_face_turn") + + def _bind_controls(self) -> None: + """Bind source actuator order to homogeneous public articulation batches.""" + self._control_batches = [] + assigned = set() + faces = [] + for name in self._articulation_names: + handles = [ + self.scene.get_articulation(f"env_{index}/{name}") + for index in range(self.num_envs) ] - if len(joints) != self.num_envs: - raise ValueError(f"Expected one passive face joint per world: {joints}") - starts = wp.to_torch(self.backend.model.joint_qd_start) - self.face_force_indices = starts[ - torch.tensor(joints, device=self.device) - ].long() - self.model_audit["face_force_indices"] = ( - self.face_force_indices.cpu().tolist() + batch = self.scene.create_articulation_batch(handles) + layouts = batch.joint_layouts_per_articulation[0] + offsets, actuators = [], [] + for layout in layouts: + matches = [ + aid + for aid in range(self.source.nu) + if self.source.joint(int(self.source.actuator_trnid[aid, 0])).name + == layout.name + ] + if matches: + if ( + len(matches) != 1 + or layout.dof_count != 1 + or matches[0] in assigned + ): + raise ValueError( + f"Ambiguous USD actuator mapping: {layout.name}" + ) + assigned.add(matches[0]) + offsets.append(layout.dof_start) + actuators.append(matches[0]) + if layout.name == "face_turn": + if layout.dof_count != 1: + raise ValueError("The passive cube face must have one DOF") + faces.append((batch, layout.dof_start)) + if offsets: + self._control_batches.append( + ( + batch, + torch.tensor(offsets, device=self.device), + torch.tensor(actuators, device=self.device), + torch.zeros( + (self.num_envs, batch.dof_width), device=self.device + ), + ) + ) + if assigned != set(range(self.source.nu)): + raise ValueError( + f"USD actuators are missing: {set(range(self.source.nu)) - assigned}" ) + if self.task_id == "M3": + if len(faces) != 1: + raise ValueError(f"Expected one passive cube face, got {len(faces)}") + self._face_batch, self._face_dof = faces[0] + + def _publish_controls(self) -> None: + for batch, offsets, actuators, commands in self._control_batches: + commands[:, offsets] = self.ctrl[:, actuators] + batch.apply_joint_target_position(commands) def _site(self, name: str) -> tuple[torch.Tensor, torch.Tensor]: source_id = self.source.site(name).id @@ -600,52 +590,67 @@ def reset( wp.to_torch(self.data.qpos)[ids[:, None], self.qmap[None, :]] = qpos wp.to_torch(self.data.qvel)[ids[:, None], self.vmap[None, :]] = qvel self.ctrl[ids] = ctrl + self._publish_controls() # Transfer explicitly set coordinates to both DexSim-owned buffers. if self.dexuni_adapter is not None: self.dexuni_adapter.publish_reset(self.data, ids) else: for state in (self.backend.state_0, self.backend.state_1): + retained = [] + for name, worlds in self._reset_field_worlds.items(): + unselected = ~torch.isin(worlds, ids) + values = wp.to_torch(getattr(state, name)) + retained.append((values, unselected, values[unselected].clone())) self.mujoco_solver._update_newton_state( self.backend.model, state, self.data, state_prev=state ) + for values, unselected, previous in retained: + values[unselected] = previous mask = torch.zeros(self.num_envs + 1, dtype=torch.bool, device=self.device) mask[ids] = True self.backend.clear_solver_state(wp.from_torch(mask)) - # Recompute positions/sensors without advancing simulation time. + # Solver reset clears native controls; restore the selected reset targets + # before computing their initial actuator forces and observations. + wp.to_torch(self.data.ctrl)[ids[:, None], self.amap[None, :]] = ctrl + # Forward updates derived data without advancing time or warm-start state. self.mujoco_solver._mujoco_warp.forward(self.mujoco_solver.mjw_model, self.data) self.elapsed_steps[ids] = 0 self._episode_return[ids] = 0 self._episode_success[ids] = False state = self._state() if self.task_id in ("M1", "M2"): - fresh = self._observation(self.task.observe(state)) + selected = { + key: value[ids] + for key, value in self._observation(self.task.observe(state)).items() + } else: selected = self._observation(self.task.observe(state, env_ids=ids)) - if self._last_observation is None: - fresh = { - key: value.new_empty((self.num_envs, value.shape[-1])) - for key, value in selected.items() - } - else: - fresh = { - key: value.clone() for key, value in self._last_observation.items() - } - for key, value in selected.items(): - fresh[key][ids] = value + if self._last_observation is None: + fresh = { + key: value.new_empty((self.num_envs, value.shape[-1])) + for key, value in selected.items() + } + else: + fresh = { + key: value.clone() for key, value in self._last_observation.items() + } + for key, value in selected.items(): + fresh[key][ids] = value self._last_observation = fresh return fresh, {} def advance_physics(self) -> None: """Advance one complete control interval with the configured substeps.""" + self._publish_controls() for _ in range(self.substeps): if self.task_id == "M3": # The passive layer detent is updated for each physical substep. state = self._state() face_force = self.task.face_detent(state["qpos"][:, -1]) - force = wp.to_torch(self.backend.control.joint_f) - force.zero_() - force[self.face_force_indices] = face_force - self.backend.step() + self._face_batch.apply_joint_force( + face_force[:, None], dof_ids=[self._face_dof] + ) + self.world.update(self.sim_dt) def step( self, actions: torch.Tensor @@ -701,5 +706,6 @@ def step( def close(self) -> None: if not self._closed: + self.scene.close() self.world.quit() self._closed = True 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/tasks/test_dexterous_metrics.py b/tests/gym/envs/tasks/test_dexterous_metrics.py index 8382aeccac..ad4c0c5ef8 100644 --- a/tests/gym/envs/tasks/test_dexterous_metrics.py +++ b/tests/gym/envs/tasks/test_dexterous_metrics.py @@ -67,12 +67,15 @@ def make(task_id: str): env._episode_success = torch.zeros(2, dtype=torch.bool) env._last_observation = None env.ctrl = torch.zeros(2, 1) - env.qmap = env.vmap = torch.tensor([0]) - env.data = SimpleNamespace(qpos=torch.zeros(2, 1), qvel=torch.zeros(2, 1)) + env.qmap = env.vmap = env.amap = torch.tensor([0]) + env.data = SimpleNamespace( + qpos=torch.zeros(2, 1), qvel=torch.zeros(2, 1), ctrl=torch.zeros(2, 1) + ) env.dexuni_adapter = Mock() env.backend = Mock() env.mujoco_solver = Mock() env.advance_physics = Mock() + env._publish_controls = Mock() env._state = lambda: None def task_reset(ids): 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..f395ed3a32 --- /dev/null +++ b/tests/gym/envs/tasks/test_dexterous_usd.py @@ -0,0 +1,131 @@ +# ---------------------------------------------------------------------------- +# 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 copy +from types import SimpleNamespace + +import mujoco +import numpy as np +import pytest + +from embodichain_tasks.manipulation.dexterous._audit_model import ( + audit_model, + snapshot_execution_model, +) + + +def test_static_body_merge_preserves_world_geometry() -> None: + source = mujoco.MjModel.from_xml_string( + '' + ) + target = mujoco.MjModel.from_xml_string( + '' + ) + arguments = ({0: 0}, [], {"ground": 0, "ball": 1}) + assert audit_model(source, target, *arguments)["status"] == "passed" + target.geom_pos[0, 0] += 0.01 + result = audit_model(source, target, *arguments) + assert any(item["field"] == "geom[floor].geom_pos" for item in result["mismatches"]) + + +@pytest.mark.parametrize( + "force_range,passed", [((-np.inf, np.inf), True), ((-1e6, 1e6), False)] +) +def test_unlimited_joint_rejects_any_finite_added_limit(force_range, passed) -> None: + source = mujoco.MjModel.from_xml_string( + '' + ) + target = copy.copy(source) + target.jnt_actfrclimited[0] = True + target.jnt_actfrcrange[0] = force_range + result = audit_model(source, target, {0: 0}, [], {"arm": 1}) + assert (result["status"] == "passed") == passed + + +@pytest.mark.parametrize("exclude,passed", [(True, True), (False, False)]) +def test_parent_filter_normalization_still_checks_every_collision( + exclude, passed +) -> None: + xml = '{}' + source = mujoco.MjModel.from_xml_string(xml.format("")) + target = mujoco.MjModel.from_xml_string( + xml.format( + '' if exclude else "" + ) + ) + target.opt.disableflags |= int(mujoco.mjtDisableBit.mjDSBL_FILTERPARENT) + result = audit_model( + source, + target, + {0: 0, 1: 1}, + [], + {"a": 1, "b": 2}, + explicit_collision_filtering=True, + ) + assert (result["status"] == "passed") == passed + + +def test_execution_snapshot_reads_device_parameters_without_mutation() -> None: + model = mujoco.MjModel.from_xml_string( + '' + ) + values = np.array([[0.25]], dtype=np.float32) + remote = SimpleNamespace( + dof_damping=SimpleNamespace(numpy=lambda: values), opt=SimpleNamespace() + ) + snapshot = snapshot_execution_model( + SimpleNamespace(mj_model=model, mjw_model=remote) + ) + assert snapshot.dof_damping[0] == 0.25 + assert model.dof_damping[0] == 0.0 + values[0, 0] = 0.5 + assert snapshot.dof_damping[0] == 0.25 + + +def test_snapshot_checks_per_world_inputs_without_auditing_derived_acceleration() -> ( + None +): + model = mujoco.MjModel.from_xml_string( + '' + ) + remote = SimpleNamespace( + actuator_acc0=SimpleNamespace(numpy=lambda: np.array([[1.0], [1.0000001]])), + dof_damping=SimpleNamespace(numpy=lambda: np.array([[0.25], [0.25]])), + opt=SimpleNamespace(), + ) + solver = SimpleNamespace(mj_model=model, mjw_model=remote) + report = {} + assert ( + snapshot_execution_model(solver, derived_cache_report=report).dof_damping[0] + == 0.25 + ) + assert report["actuator_acc0"]["max_world_spread"] > 0 + remote.dof_damping = SimpleNamespace(numpy=lambda: np.array([[0.25], [0.5]])) + with pytest.raises(ValueError, match="dof_damping"): + snapshot_execution_model(solver) + + +def test_snapshot_rejects_per_world_timestep_changes() -> None: + model = mujoco.MjModel.from_xml_string("") + remote = SimpleNamespace( + opt=SimpleNamespace( + timestep=SimpleNamespace(numpy=lambda: np.array([0.002, 0.003])) + ) + ) + with pytest.raises(ValueError, match="Nonuniform execution option: timestep"): + snapshot_execution_model(SimpleNamespace(mj_model=model, mjw_model=remote)) From 560c09e977df43f2397a27380e2a15c50992db7b Mon Sep 17 00:00:00 2001 From: acrlw <13927622+acrlw@users.noreply.github.com> Date: Mon, 2 Nov 2026 11:34:20 +0800 Subject: [PATCH 11/24] Use existing MuJoCo ownership selectors for USD manipulation tasks --- embodichain/data/assets/rl_assets.py | 6 +-- .../manipulation/dexterous/_audit_model.py | 2 +- .../manipulation/dexterous/_dexuni_adapter.py | 16 ++++++++ .../manipulation/dexterous/_runtime.py | 12 +++--- tests/gym/envs/tasks/test_dexterous_usd.py | 39 +++++++++++++++++++ 5 files changed, 66 insertions(+), 9 deletions(-) diff --git a/embodichain/data/assets/rl_assets.py b/embodichain/data/assets/rl_assets.py index 91985332e1..4ffad5f1e6 100644 --- a/embodichain/data/assets/rl_assets.py +++ b/embodichain/data/assets/rl_assets.py @@ -70,19 +70,19 @@ class G1RoughTerrains(_RLAsset): class ShadowHandPen(_RLAsset): """Bundled model inputs for ShadowHandPen.""" - archive_md5 = "d5642a9fb20bf49be918455826c35a97" + archive_md5 = "5dc61ae6cfe2f993c2f27eba8103cb89" class LeapHandCubeFaceTurn(_RLAsset): """Bundled model inputs for LeapHandCubeFaceTurn.""" - archive_md5 = "6394cd7665963f3fdcacbcf170a9af79" + archive_md5 = "0dcc05a60e258a305af4e5bb49fc3e2c" class LeapHandCube(_RLAsset): """Bundled model inputs for LeapHandCube.""" - archive_md5 = "1bf309614884c706e225d7cd5165b949" + archive_md5 = "96ecb448c72a8b21f8daba32fa3e4a41" _ASSET_CATALOG: dict[str, dict[str, str]] = { diff --git a/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_audit_model.py b/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_audit_model.py index f08213cefd..574fbcf497 100644 --- a/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_audit_model.py +++ b/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_audit_model.py @@ -104,7 +104,7 @@ def snapshot_execution_model( }: # These solver-computed caches are not authored inputs checked below. # Keep their batch variation visible without treating it as lost USD - # metadata; strict rollout comparison validates its output effects. + # metadata; rollout measurements record its output effects. if derived_cache_report is not None and values.size: spread = ( values - values[0] diff --git a/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_dexuni_adapter.py b/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_dexuni_adapter.py index 8583196d49..d2ec7fceba 100644 --- a/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_dexuni_adapter.py +++ b/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_dexuni_adapter.py @@ -22,10 +22,26 @@ import torch import warp as wp from newton import Model +from dexsim.spawn import SceneDesc +from dexsim.types import ActorType __all__ = ["DexUniStateAdapter"] +def _mujoco_articulation_ids(scene: SceneDesc, num_envs: int) -> tuple[int, ...]: + """Select every rigid articulation in the replicated manipulation scene.""" + # Each non-static standalone body creates its own free or mocap articulation. + standalone = sum( + item.physics is not None + and item.physics.actor_type != ActorType.STATIC + and ( + bool(item.collisions) or bool(getattr(item.physics.newton, "mocap", False)) + ) + for item in scene.mesh_objects + ) + return tuple(range(num_envs * (len(scene.articulations) + standalone))) + + class DexUniStateAdapter: """Scatter private MuJoCo state into the owning DexSim state buffers. diff --git a/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_runtime.py b/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_runtime.py index d4cd328fb8..b0dbdbfd01 100644 --- a/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_runtime.py +++ b/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_runtime.py @@ -200,8 +200,6 @@ def __init__( # large batches. Keep headroom for the 256-contact allocation. "njmax": 2048 if task_id == "M3" else 512, "nconmax": 256, - "deterministic": wp.DeterministicMode.RUN_TO_RUN, - "graph_conditional": False, "update_data_interval": 0, } self.backend_option_overrides = {} @@ -217,9 +215,7 @@ def __init__( } solver_options[name] = value if solver_kind == "dexuni": - solver_cfg = DexUniSolverCfg( - mujoco_options=solver_options, mujoco_joints="all" - ) + solver_cfg = DexUniSolverCfg(mujoco_options=solver_options) else: solver_cfg = MJWarpSolverCfg(**solver_options) config.newton_cfg = NewtonCfg( @@ -236,6 +232,12 @@ def __init__( self.backend = get_newton_backend(self.world) builder = dexsim.spawn.SceneBuilder(self.world) builder.add_usd("asset", str(self.usd)) + if solver_kind == "dexuni": + from ._dexuni_adapter import _mujoco_articulation_ids + + self.backend.cfg.solver_cfg.mujoco_articulations = _mujoco_articulation_ids( + builder.scene_desc, num_envs + ) self._explicit_collision_filtering = bool( builder.scene_desc.newton and builder.scene_desc.newton.use_explicit_collision_filtering diff --git a/tests/gym/envs/tasks/test_dexterous_usd.py b/tests/gym/envs/tasks/test_dexterous_usd.py index f395ed3a32..8031004717 100644 --- a/tests/gym/envs/tasks/test_dexterous_usd.py +++ b/tests/gym/envs/tasks/test_dexterous_usd.py @@ -129,3 +129,42 @@ def test_snapshot_rejects_per_world_timestep_changes() -> None: ) with pytest.raises(ValueError, match="Nonuniform execution option: timestep"): snapshot_execution_model(SimpleNamespace(mj_model=model, mjw_model=remote)) + + +def test_dexuni_ownership_includes_free_bodies_and_replicas() -> None: + from dexsim.spawn import ( + ArticulationDesc, + MeshObjectDesc, + RigidBodyPhysicsDesc, + SceneDesc, + NewtonPhysicsDesc, + CollisionDesc, + ) + from dexsim.types import ActorType + from embodichain_tasks.manipulation.dexterous._dexuni_adapter import ( + _mujoco_articulation_ids, + ) + + scene = SceneDesc( + articulations=[ArticulationDesc(name="hand")], + mesh_objects=[ + MeshObjectDesc( + name="cube", + physics=RigidBodyPhysicsDesc(actor_type=ActorType.DYNAMIC), + collisions=[CollisionDesc()], + ), + MeshObjectDesc( + name="anchor", + physics=RigidBodyPhysicsDesc( + actor_type=ActorType.KINEMATIC, newton=NewtonPhysicsDesc(mocap=True) + ), + ), + MeshObjectDesc( + name="floor", + physics=RigidBodyPhysicsDesc(actor_type=ActorType.STATIC), + collisions=[CollisionDesc()], + ), + ], + ) + assert _mujoco_articulation_ids(scene, 1) == (0, 1, 2) + assert _mujoco_articulation_ids(scene, 4) == tuple(range(12)) From dd310d76dbc635e99d5b348299e6a03f41802c61 Mon Sep 17 00:00:00 2001 From: acrlw <13927622+acrlw@users.noreply.github.com> Date: Tue, 3 Nov 2026 10:15:19 +0800 Subject: [PATCH 12/24] docs: add reproducible manipulation asset and task checks --- .../tasks/manipulation/dexterous/README.md | 96 +++++++++++++++++++ 1 file changed, 96 insertions(+) create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/README.md diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/README.md b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/README.md new file mode 100644 index 0000000000..ae4a33bd95 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/README.md @@ -0,0 +1,96 @@ +# Dexterous manipulation with USD assets + +The five tasks construct their physical scenes from USD. The source MJCF in each +archive supplies task metadata and the independent physical-model audit. Initial +states, meshes, textures, provenance and licenses are included in the same ZIP. + +| Task | Archive | USD | MJCF | +|---|---|---|---| +| Fixed and continuous pen spinning | ShadowHandPen | `usd/shadow_pen.usdc` | `shadow_pen.xml` | +| Cube face turning | LeapHandCubeFaceTurn | `usd/leap_cube_top_layer.usdc` | `leap_cube_top_layer.xml` | +| Cube reorientation | LeapHandCube | `usd/M4.usdc` | `M4.xml` | +| Cube Z-axis rotation | LeapHandCube | `usd/M5.usdc` | `M5.xml` | + +The archives are published under +[robot_assets](https://huggingface.co/datasets/DexForceAI/embodichain_data/tree/e1ebe53462d6f48b5961018e625b7c1304e9e83a/robot_assets) +at revision `e1ebe53462d6f48b5961018e625b7c1304e9e83a`. +The asset descriptors verify the archive content and refresh older cached ZIPs. + +## Download and exercise the task interfaces + +Use Python 3.11 with the project dependencies installed, a CUDA device, and a +compatible development engine build with USD scene metadata, mocap frames and +CUDA graph state preservation. Run the following commands from the repository +root. The `ppo.dexuni.yaml` configurations assign the hand and free rigid bodies +to MuJoCo. Each task runs in a separate process with four environments and eight +control steps, including a selective reset of world 0. + +The expected output contains five `TASK_RESULT` records with `status: passed`. +Checks cover finite observations/rewards, output shapes and dtypes, selective +reset isolation, the solver route, CUDA graph capture and the MJCF model audit. +The script uses `EMBODICHAIN_DATA_ROOT` when set; otherwise it creates a temporary +empty asset cache. It can be stopped with Ctrl-C. + +```bash +#!/usr/bin/env bash +set -euo pipefail +export EMBODICHAIN_DOWNLOAD_PREFIX=https://huggingface.co/datasets/DexForceAI/embodichain_data/resolve/e1ebe53462d6f48b5961018e625b7c1304e9e83a/ +export EMBODICHAIN_DATA_ROOT="${EMBODICHAIN_DATA_ROOT:-$(mktemp -d -t manipulation-assets.XXXXXX)}" +export EMBODICHAIN_SIM_EXIT_PROCESS=0 +for asset in ShadowHandPen LeapHandCubeFaceTurn LeapHandCube; do + python -m embodichain data download --name "$asset" +done +for task in shadow_pen_fixed shadow_pen_continuous leap_cube_face_turn leap_cube_reorient leap_cube_rotate_z; do + python - "$task" <<'PY' +import importlib +import json +import sys +from pathlib import Path +import torch +import yaml +from embodichain.learning.rl.env import build_learning_env + +name = sys.argv[1] +importlib.import_module(f"embodichain_tasks.manipulation.dexterous.{name}") +config = Path("extensions/embodichain_tasks/configs/tasks/manipulation/dexterous") / name / "agents/ppo.dexuni.yaml" +trainer = yaml.safe_load(config.read_text())["trainer"] +spec = trainer["learning_env"] +torch.set_num_threads(4) +env = build_learning_env(spec["name"], num_envs=4, device=trainer["device"], seed=trainer["seed"], **spec["cfg"]) +try: + obs, _ = env.reset(seed=trainer["seed"]) + for step in range(8): + if step == 4: + previous = {key: value[1:].clone() for key, value in obs.items()} + obs, _ = env.reset(options={"reset_ids": torch.tensor([0], device=env.device)}) + assert all(torch.equal(obs[key][1:], value) for key, value in previous.items()) + action = .15 * torch.sin(torch.arange(env.action_dim, device=env.device)[None, :] + torch.arange(4, device=env.device)[:, None] * .13 + step * .3) + obs, reward, terminated, truncated, _ = env.step(action) + assert set(obs) == set(env.single_observation_space.spaces) + for key, value in obs.items(): + assert value.shape == (4, *env.single_observation_space.spaces[key].shape) + assert torch.isfinite(value).all() + assert reward.shape == (4,) and torch.isfinite(reward).all() + assert terminated.shape == truncated.shape == (4,) + assert terminated.dtype == truncated.dtype == torch.bool + assert env.backend.solver.features.backend == "pure_mujoco" + assert env.backend._graph is not None + assert env.model_audit["physical_model"]["status"] == "passed" + print("TASK_RESULT " + json.dumps({"task": name, "status": "passed", "num_envs": 4, "control_steps": 8, "usd": str(env.usd), "mjcf": str(env.xml), "solver": "pure_mujoco", "cuda_graph": True, "selective_reset": "passed", "model_audit": "passed"}), flush=True) +finally: + env.close() +PY +done +``` + +## Training configurations + +Each task's `agents/ppo.dexuni.yaml` selects all-MuJoCo DexUni; +`agents/ppo.mujoco_warp.yaml` selects Newton/MJWarp. For example: + +```bash +python -m embodichain train-rl --config extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/agents/ppo.dexuni.yaml +``` + +The short interface check above exercises loading, reset and stepping. Sustained +training, checkpoint resume and policy evaluation have separate validation results. From 6e7f7441797c96f21fe542b137fc76b241b9bd01 Mon Sep 17 00:00:00 2001 From: acrlw <13927622+acrlw@users.noreply.github.com> Date: Mon, 9 Nov 2026 08:56:39 +0800 Subject: [PATCH 13/24] Preserve terminal curriculum state and isolate task reset seeds --- .../locomotion/velocity/isaac/config.py | 4 ++- .../locomotion/velocity/isaac/runtime.py | 27 +++++++++++-------- .../manipulation/dexterous/_runtime.py | 14 +++++----- .../gym/envs/tasks/test_dexterous_metrics.py | 9 +++++-- tests/gym/envs/tasks/test_isaac_velocity.py | 22 ++++++++++++--- 5 files changed, 53 insertions(+), 23 deletions(-) diff --git a/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/config.py b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/config.py index 4bc702bb05..3f8350893e 100644 --- a/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/config.py +++ b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/config.py @@ -49,7 +49,7 @@ def load_config(task: str) -> IsaacVelocityConfig: robot = data["robot"] dimension = len(robot["joint_names"]) observation_dimension = 12 + 3 * dimension + (187 if dimension == 37 else 0) - return IsaacVelocityConfig( + cfg = IsaacVelocityConfig( data=data, joint_names=tuple(robot["joint_names"]), default_joint_position=tuple(robot["default_joint_position"]), @@ -58,3 +58,5 @@ def load_config(task: str) -> IsaacVelocityConfig: actor_observation_dim=observation_dimension, critic_observation_dim=observation_dimension, ) + cfg.validate() + return cfg diff --git a/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/runtime.py b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/runtime.py index 5adac56591..3eed3e4eb4 100644 --- a/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/runtime.py +++ b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/runtime.py @@ -495,6 +495,22 @@ def load_training_state(self, state: dict[str, Any]) -> None: 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: @@ -505,17 +521,6 @@ def _initialize_episode( ) if ids.numel() == 0: return - if self._reference_terrain_course is not None: - resume_reset = getattr(self, "_reference_resume_reset", None) - update_ids = ids if resume_reset is None else ids[~resume_reset[ids]] - if update_ids.numel(): - self._reference_terrain_course.update( - update_ids, - self.robot.body_data.root_pose[:, :3], - self.reward_command, - ) - if resume_reset is not None: - resume_reset[ids] = False previous_deferred = getattr(self, "_reference_events_deferred", False) self._reference_events_deferred = True try: diff --git a/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_runtime.py b/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_runtime.py index b0dbdbfd01..6e329aee73 100644 --- a/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_runtime.py +++ b/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_runtime.py @@ -89,7 +89,7 @@ def __init__( task_id: str, num_envs: int = 32, device: str = "cuda:0", - seed: int = 42, + seed: int | None = None, cuda_graph: bool = True, solver_kind: str = "mujoco_warp", diagnostics: bool = True, @@ -103,7 +103,7 @@ def __init__( self.task_id = task_id self.num_envs = num_envs self.device = torch.device(device) - torch.manual_seed(seed) + seed = torch.initial_seed() if seed is None else seed if task_id in ("M1", "M2"): self.task = ShadowTask.from_assets( Path(get_data_path("ShadowHandPen/shadow_pen.xml")).parent, @@ -111,6 +111,9 @@ def __init__( variant=task_id, device=device, ) + self._reset_generator = torch.Generator(device=self.device).manual_seed( + seed + ) xml = Path(get_data_path("ShadowHandPen/shadow_pen.xml")) self.ctrl_dt, self.sim_dt = 0.04, 0.002 self.observation_dim, self.critic_dim, self.action_dim = 87, 87, 18 @@ -142,6 +145,7 @@ def __init__( device=device, seed=seed, ) + self._reset_generator = self.task.generator self.ctrl_dt, self.sim_dt = 0.05, (0.005 if task_id == "M3" else 0.01) self.observation_dim = {"M3": 65, "M4": 57, "M5": 32}[task_id] self.critic_dim = {"M3": 65, "M4": 128, "M5": 105}[task_id] @@ -575,9 +579,7 @@ def reset( self, *, seed: int | None = None, options: dict[str, Any] | None = None ) -> tuple[dict, dict]: if seed is not None: - torch.manual_seed(seed) - if hasattr(self.task, "generator"): - self.task.generator.manual_seed(seed) + self._reset_generator.manual_seed(seed) ids = ( torch.arange(self.num_envs, device=self.device) if not options @@ -585,7 +587,7 @@ def reset( ) ids = torch.as_tensor(ids, dtype=torch.long, device=self.device) if self.task_id in ("M1", "M2"): - qpos, qvel, ctrl = self.task.reset(ids) + qpos, qvel, ctrl = self.task.reset(ids, generator=self._reset_generator) else: initial = self.task.reset(ids) qpos, qvel, ctrl = initial["qpos"], initial["qvel"], initial["ctrl"] diff --git a/tests/gym/envs/tasks/test_dexterous_metrics.py b/tests/gym/envs/tasks/test_dexterous_metrics.py index ad4c0c5ef8..beefc343a2 100644 --- a/tests/gym/envs/tasks/test_dexterous_metrics.py +++ b/tests/gym/envs/tasks/test_dexterous_metrics.py @@ -63,6 +63,7 @@ def make(task_id: str): env.single_action_space = gym.spaces.Box(-1.0, 1.0, (1,)) env.action_format = "tensor" env.elapsed_steps = torch.zeros(2, dtype=torch.long) + env._reset_generator = torch.Generator().manual_seed(0) env._episode_return = torch.zeros(2) env._episode_success = torch.zeros(2, dtype=torch.bool) env._last_observation = None @@ -78,9 +79,10 @@ def make(task_id: str): env._publish_controls = Mock() env._state = lambda: None - def task_reset(ids): + def task_reset(ids, *, generator=None): values = torch.zeros(len(ids), 1) if task_id in ("M1", "M2"): + assert generator is env._reset_generator return values, values, values return dict(qpos=values, qvel=values, ctrl=values) @@ -130,7 +132,10 @@ def test_partial_reset_clears_only_selected_success_and_keeps_terminal_info( ) -> None: env = env_factory(task_id) env._episode_success[:] = True - env.reset(options={"reset_ids": torch.tensor([0])}) + 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, 0]) _, _, done, _, info = env.step(torch.zeros(2, 1)) diff --git a/tests/gym/envs/tasks/test_isaac_velocity.py b/tests/gym/envs/tasks/test_isaac_velocity.py index 61bce1a421..d0ab5f205f 100644 --- a/tests/gym/envs/tasks/test_isaac_velocity.py +++ b/tests/gym/envs/tasks/test_isaac_velocity.py @@ -19,6 +19,7 @@ from __future__ import annotations import json +from contextlib import nullcontext from copy import copy from pathlib import Path from types import SimpleNamespace @@ -34,6 +35,7 @@ RenderDesc, ) +from embodichain.lab.gym.envs.episode_context import EpisodeContext from embodichain.lab.sim.sim_manager import SimulationManager from embodichain_tasks.locomotion.velocity.isaac.curriculum import ( ReferenceTerrainCurriculum, @@ -519,7 +521,7 @@ def test_rough_curriculum_matches_executed_isaac_resets() -> None: @pytest.mark.no_sim def test_rough_reset_curriculum_uses_terminal_step_command(monkeypatch) -> None: - """A timeout must use the command that generated the terminal reward.""" + """A timeout must use the terminal pose and its reward command.""" origins = torch.zeros((10, 1, 3)) curriculum = ReferenceTerrainCurriculum( origins, 1, 5, 8.0, 20.0, torch.Generator().manual_seed(42) @@ -529,6 +531,12 @@ def test_rough_reset_curriculum_uses_terminal_step_command(monkeypatch) -> None: 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( @@ -538,21 +546,29 @@ def test_rough_reset_curriculum_uses_terminal_step_command(monkeypatch) -> None: 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, **kwargs): + 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._initialize_episode() + 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 From 69069aeedccc7624863c06d8762d88467c315903 Mon Sep 17 00:00:00 2001 From: acrlw <13927622+acrlw@users.noreply.github.com> Date: Mon, 9 Nov 2026 09:01:55 +0800 Subject: [PATCH 14/24] Place G1 terrain guidance alongside locomotion controls --- docs/source/overview/rl/locomotion.md | 57 +++++++++++++-------------- 1 file changed, 28 insertions(+), 29 deletions(-) diff --git a/docs/source/overview/rl/locomotion.md b/docs/source/overview/rl/locomotion.md index 9ac86fb05c..9bf5c9131d 100644 --- a/docs/source/overview/rl/locomotion.md +++ b/docs/source/overview/rl/locomotion.md @@ -46,6 +46,34 @@ 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_isaac_rough/agents/ppo.dexuni.yaml` selects the pure MuJoCo branch of +DexUni with Newton-generated terrain contacts. The paired DexSim build must +support `mujoco_options.use_mujoco_contacts: false` in this branch and export +MuJoCo solver contact forces for contact sensors. The MJWarp deployment remains +`ppo.newton.yaml`; both configurations retain the same terrain and physical +parameters. + +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. + +The G1 rough deployments set `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. + ## PPO configuration For example, the prepared Go2 training entry is: @@ -75,32 +103,3 @@ can replace the global ground geometry and collision descriptor. Newton collision configuration also accepts `preserve_mesh_transform` to retain the authored collision mesh pose and scale. These options do not change the default settings of existing tasks. - - -## G1 rough terrain with DexUni - -`g1_isaac_rough/agents/ppo.dexuni.yaml` selects the pure MuJoCo branch of -DexUni with Newton-generated terrain contacts. The paired DexSim build must -support `mujoco_options.use_mujoco_contacts: false` in this branch and export -MuJoCo solver contact forces for contact sensors. The MJWarp deployment remains -`ppo.newton.yaml`; both configurations retain the same terrain and physical -parameters. - -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. - -The G1 rough deployments set `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. From 543f2993112b7300c04e73c40ae5f5e19f82531d Mon Sep 17 00:00:00 2001 From: acrlw <13927622+acrlw@users.noreply.github.com> Date: Mon, 9 Nov 2026 11:27:59 +0800 Subject: [PATCH 15/24] test(tasks): model ground redeclaration in the G1 fixture --- tests/gym/envs/tasks/test_isaac_velocity.py | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/tests/gym/envs/tasks/test_isaac_velocity.py b/tests/gym/envs/tasks/test_isaac_velocity.py index d0ab5f205f..a4e1711486 100644 --- a/tests/gym/envs/tasks/test_isaac_velocity.py +++ b/tests/gym/envs/tasks/test_isaac_velocity.py @@ -23,6 +23,7 @@ from copy import copy from pathlib import Path from types import SimpleNamespace +from unittest.mock import Mock import numpy as np import pytest @@ -325,7 +326,7 @@ def test_contact_transition_keeps_elapsed_air_and_contact_time() -> None: @pytest.mark.no_sim def test_ground_replacement_updates_declared_geometry() -> None: - """A shallow scene declaration must retain the modified ground descriptors.""" + """Ground redeclaration must retain the modified render and collision geometry.""" original = GeometryDesc.plane(10.0) descriptor = ObjectDesc( name="ground", @@ -335,6 +336,10 @@ def test_ground_replacement_updates_declared_geometry() -> None: declared = copy(descriptor) sim = object.__new__(SimulationManager) 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), @@ -343,6 +348,8 @@ def test_ground_replacement_updates_declared_geometry() -> None: 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) From 3349c6834e7682e1f0af7ee9ccabefa4b5e90f35 Mon Sep 17 00:00:00 2001 From: acrlw <13927622+acrlw@users.noreply.github.com> Date: Tue, 10 Nov 2026 09:52:59 +0800 Subject: [PATCH 16/24] fix(tasks): preserve flat action clipping and terminal bootstrap rules --- .../locomotion/velocity/_embodichain.py | 3 +- .../locomotion/velocity/isaac/runtime.py | 1 + .../manipulation/dexterous/_runtime.py | 2 +- .../gym/envs/tasks/test_dexterous_metrics.py | 33 ++++++++++++ .../gym/envs/tasks/test_unitree_locomotion.py | 53 +++++++++++++++++++ 5 files changed, 90 insertions(+), 2 deletions(-) diff --git a/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/_embodichain.py b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/_embodichain.py index 604af6aa08..0536d735da 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,7 +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": None, + "clip": self._default_action_clip, } for name, value in mapping.items(): term_cfg.params.setdefault(name, value) diff --git a/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/runtime.py b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/runtime.py index 3eed3e4eb4..0dfac3c749 100644 --- a/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/runtime.py +++ b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/runtime.py @@ -59,6 +59,7 @@ class IsaacVelocityEnv(EmbodiChainVelocityEnv): rays and Newton rasterization for collision. """ + _default_action_clip = None state_type = IsaacVelocityState compute_rewards_fn = staticmethod(compute_rewards) compute_termination_fn = staticmethod(compute_termination) diff --git a/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_runtime.py b/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_runtime.py index 6e329aee73..57956c7485 100644 --- a/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_runtime.py +++ b/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_runtime.py @@ -678,7 +678,7 @@ def step( if self.task_id in ("M1", "M2"): obs, reward, terminated, metrics = self.task.step(state, actions) self.elapsed_steps += 1 - truncated = self.elapsed_steps >= 500 + truncated = (self.elapsed_steps >= 500) & ~terminated else: obs, reward, terminated, truncated, metrics = self.task.after_step( state, actions diff --git a/tests/gym/envs/tasks/test_dexterous_metrics.py b/tests/gym/envs/tasks/test_dexterous_metrics.py index beefc343a2..53a6a668a3 100644 --- a/tests/gym/envs/tasks/test_dexterous_metrics.py +++ b/tests/gym/envs/tasks/test_dexterous_metrics.py @@ -153,3 +153,36 @@ def test_continuous_rotation_has_metrics_without_invented_binary_success( result = evaluate_episodes(policy=_Policy(), env=env, num_episodes=2, device="cpu") assert math.isnan(result["eval/success_rate"]) assert math.isfinite(result["eval/metrics/rotation"]) + + +@pytest.mark.parametrize("task_id", ["M1", "M2"]) +def test_failure_at_time_limit_does_not_bootstrap(env_factory, task_id): + from tensordict import TensorDict + from embodichain.learning.rl.algo.common import compute_gae + + env = env_factory(task_id) + env.elapsed_steps.fill_(499) + env.task.step = lambda state, actions: ( + torch.zeros(2, 1), + torch.ones(2), + torch.tensor([True, False]), + {}, + ) + _, reward, terminated, truncated, _ = env.step(torch.zeros(2, 1)) + assert terminated.tolist() == [True, False] + assert truncated.tolist() == [False, True] + 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 + ), + "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) + torch.testing.assert_close(returns[:, 0], torch.tensor([1.0, 10.0])) diff --git a/tests/gym/envs/tasks/test_unitree_locomotion.py b/tests/gym/envs/tasks/test_unitree_locomotion.py index 81652d9fca..91a37f2ea5 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( + "isaac,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, isaac, 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.isaac.runtime import IsaacVelocityEnv + + @dataclass + class State: + qpos: torch.Tensor + + cls = IsaacVelocityEnv if isaac 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 From c01091d05344ceb8e7fba33bd21c44996876f881 Mon Sep 17 00:00:00 2001 From: acrlw <13927622+acrlw@users.noreply.github.com> Date: Fri, 13 Nov 2026 16:27:30 +0800 Subject: [PATCH 17/24] Align G1 rough locomotion naming with main and remove unused RSL-RL wrapper --- .../topics/rl-learning/rl-learning.md | 2 +- docs/source/api_reference/public_api.rst | 516 +++++++++--------- docs/source/overview/rl/locomotion.md | 6 +- .../velocity/g1_isaac_rough/rsl_rl.json | 65 --- .../agents/ppo.dexuni.yaml | 0 .../agents/ppo.newton.yaml | 0 .../{g1_isaac_rough => g1_rough}/catalog.yaml | 2 +- .../env.dexuni.yaml | 2 +- .../env.newton.yaml | 2 +- .../{g1_isaac_rough => g1_rough}/task.json | 6 +- .../{g1_isaac_rough.py => g1_rough.py} | 16 +- .../locomotion/velocity/isaac/README.md | 36 -- .../locomotion/velocity/isaac/rsl_rl.py | 97 ---- .../locomotion/velocity/rough/README.md | 38 ++ .../velocity/{isaac => rough}/__init__.py | 6 +- .../velocity/{isaac => rough}/_frame.py | 0 .../velocity/{isaac => rough}/config.py | 14 +- .../velocity/{isaac => rough}/curriculum.py | 6 +- .../velocity/{isaac => rough}/environment.py | 52 +- .../velocity/{isaac => rough}/mdp.py | 62 +-- .../velocity/{isaac => rough}/runtime.py | 44 +- .../velocity/{isaac => rough}/terrain.py | 8 +- .../{g1_isaac_rough.json => g1_rough.json} | 2 +- ...aac_velocity.py => test_rough_velocity.py} | 143 ++--- ..._frame.py => test_rough_velocity_frame.py} | 2 +- .../gym/envs/tasks/test_unitree_locomotion.py | 8 +- 26 files changed, 483 insertions(+), 652 deletions(-) delete mode 100644 extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/rsl_rl.json rename extensions/embodichain_tasks/configs/tasks/locomotion/velocity/{g1_isaac_rough => g1_rough}/agents/ppo.dexuni.yaml (100%) rename extensions/embodichain_tasks/configs/tasks/locomotion/velocity/{g1_isaac_rough => g1_rough}/agents/ppo.newton.yaml (100%) rename extensions/embodichain_tasks/configs/tasks/locomotion/velocity/{g1_isaac_rough => g1_rough}/catalog.yaml (92%) rename extensions/embodichain_tasks/configs/tasks/locomotion/velocity/{g1_isaac_rough => g1_rough}/env.dexuni.yaml (99%) rename extensions/embodichain_tasks/configs/tasks/locomotion/velocity/{g1_isaac_rough => g1_rough}/env.newton.yaml (99%) rename extensions/embodichain_tasks/configs/tasks/locomotion/velocity/{g1_isaac_rough => g1_rough}/task.json (99%) rename extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/{g1_isaac_rough.py => g1_rough.py} (74%) delete mode 100644 extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/README.md delete mode 100644 extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/rsl_rl.py create mode 100644 extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/README.md rename extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/{isaac => rough}/__init__.py (83%) rename extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/{isaac => rough}/_frame.py (100%) rename extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/{isaac => rough}/config.py (86%) rename extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/{isaac => rough}/curriculum.py (97%) rename extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/{isaac => rough}/environment.py (76%) rename extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/{isaac => rough}/mdp.py (87%) rename extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/{isaac => rough}/runtime.py (96%) rename extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/{isaac => rough}/terrain.py (97%) rename tests/gym/envs/tasks/fixtures/{g1_isaac_rough.json => g1_rough.json} (99%) rename tests/gym/envs/tasks/{test_isaac_velocity.py => test_rough_velocity.py} (89%) rename tests/gym/envs/tasks/{test_isaac_velocity_frame.py => test_rough_velocity_frame.py} (98%) diff --git a/agent_context/topics/rl-learning/rl-learning.md b/agent_context/topics/rl-learning/rl-learning.md index 852c837798..727886d2b9 100644 --- a/agent_context/topics/rl-learning/rl-learning.md +++ b/agent_context/topics/rl-learning/rl-learning.md @@ -214,7 +214,7 @@ detachment, and reset/terminal issues to the selected environment protocol. ## Rough terrain and dexterous task candidates -`locomotion/velocity/g1_isaac_rough.py` registers the 37-action G1 task. +`locomotion/velocity/g1_rough.py` registers the 37-action G1 task (`UnitreeG1RoughRL-v1`). Its frozen curriculum comes from `G1RoughTerrains`; the robot comes from `UnitreeG1Rough`, independently of the 29-joint G1 flat archive. diff --git a/docs/source/api_reference/public_api.rst b/docs/source/api_reference/public_api.rst index f56012cb6b..8478c726c9 100644 --- a/docs/source/api_reference/public_api.rst +++ b/docs/source/api_reference/public_api.rst @@ -163,36 +163,6 @@ Prepared robot snapshots preserve resource trees and source notices for relocata .. autofunction:: snapshot_urdf_resources -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.data.assets.demo_assets ----------------------------------- @@ -4909,241 +4879,6 @@ reference-state resets and terminal snapshots before automatic row resets. .. automodule:: embodichain_tasks.locomotion.tracking.g1_motion :members: -embodichain_tasks.locomotion.velocity.g1_isaac_rough ----------------------------------------------------- - -The G1 Rough environment registers the 37-action reference task with terrain height observations. - -.. currentmodule:: embodichain_tasks.locomotion.velocity.g1_isaac_rough - -.. autosummary:: - - UnitreeG1IsaacRoughEnv - -.. autoclass:: UnitreeG1IsaacRoughEnv - :members: - - -embodichain_tasks.locomotion.velocity.isaac -------------------------------------------- - -Load reference task configurations from a supplied scene bundle. The loader validates robot joint order, shared initial states and terrain inputs. - -.. currentmodule:: embodichain_tasks.locomotion.velocity.isaac - -.. autosummary:: - - load_reference_env_cfg - -.. autofunction:: load_reference_env_cfg - - -embodichain_tasks.locomotion.velocity.isaac.config --------------------------------------------------- - -Packaged task definitions retain policy joint order, observation dimensions and fixed physics/control periods. - -.. currentmodule:: embodichain_tasks.locomotion.velocity.isaac.config - -.. autosummary:: - - IsaacVelocityConfig - load_config - -.. autoclass:: IsaacVelocityConfig - :members: - -.. autofunction:: load_config - - -embodichain_tasks.locomotion.velocity.isaac.curriculum ------------------------------------------------------- - -Terrain assignment and selected-row curriculum updates use terminal positions and commands before resetting each episode. - -.. currentmodule:: embodichain_tasks.locomotion.velocity.isaac.curriculum - -.. autosummary:: - - ReferenceTerrainCurriculum - -.. autoclass:: ReferenceTerrainCurriculum - :members: - - -embodichain_tasks.locomotion.velocity.isaac.environment -------------------------------------------------------- - -Resolve the reference deployment against exported robot and terrain assets, with optional training randomization and contact generation selection. - -.. currentmodule:: embodichain_tasks.locomotion.velocity.isaac.environment - -.. autosummary:: - - load_reference_env_cfg - -.. autofunction:: load_reference_env_cfg - - -embodichain_tasks.locomotion.velocity.isaac.mdp ------------------------------------------------ - -Pure Torch state, action, observation, reward and termination functions share the reference task definitions. Device constants cache fixed values and reward joint selectors. - -.. currentmodule:: embodichain_tasks.locomotion.velocity.isaac.mdp - -.. autosummary:: - - IsaacVelocityState - IsaacVelocityReward - IsaacVelocityConstants - action_target - observation_terms - build_observation - compute_rewards - compute_termination - dc_motor_effort - update_contact_times - -.. autoclass:: IsaacVelocityState - :members: - -.. autoclass:: IsaacVelocityReward - :members: - -.. autoclass:: IsaacVelocityConstants - :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.isaac.rsl_rl --------------------------------------------------- - -The RSL-RL adapter preserves unbounded policy actions, automatic selected-row resets and timeout bootstrapping. CUDA calls share the existing simulation stream with ordered entry and exit dependencies. - -.. currentmodule:: embodichain_tasks.locomotion.velocity.isaac.rsl_rl - -.. autosummary:: - - ReferenceRslRlEnv - -.. autoclass:: ReferenceRslRlEnv - :members: - - -embodichain_tasks.locomotion.velocity.isaac.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.isaac.runtime - -.. autosummary:: - - IsaacVelocityEnv - -.. autoclass:: IsaacVelocityEnv - :members: - - -embodichain_tasks.locomotion.velocity.isaac.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.isaac.terrain - -.. autosummary:: - - ReferenceHeightScanner - -.. autoclass:: ReferenceHeightScanner - :members: - - -embodichain_tasks.manipulation.dexterous.leap_cube_face_turn ------------------------------------------------------------- - -Create the leap cube face turn vector environment with an explicit DexSim solver and packaged model inputs. - -.. currentmodule:: embodichain_tasks.manipulation.dexterous.leap_cube_face_turn - -.. autosummary:: - - make_env - -.. autofunction:: make_env - - -embodichain_tasks.manipulation.dexterous.leap_cube_reorient ------------------------------------------------------------ - -Create the leap cube reorient vector environment with an explicit DexSim solver and packaged model inputs. - -.. currentmodule:: embodichain_tasks.manipulation.dexterous.leap_cube_reorient - -.. autosummary:: - - make_env - -.. autofunction:: make_env - - -embodichain_tasks.manipulation.dexterous.leap_cube_rotate_z ------------------------------------------------------------ - -Create the leap cube rotate z vector environment with an explicit DexSim solver and packaged model inputs. - -.. currentmodule:: embodichain_tasks.manipulation.dexterous.leap_cube_rotate_z - -.. autosummary:: - - make_env - -.. autofunction:: make_env - - -embodichain_tasks.manipulation.dexterous.shadow_pen_continuous --------------------------------------------------------------- - -Create the shadow pen continuous vector environment with an explicit DexSim solver and packaged model inputs. - -.. currentmodule:: embodichain_tasks.manipulation.dexterous.shadow_pen_continuous - -.. autosummary:: - - make_env - -.. autofunction:: make_env - - -embodichain_tasks.manipulation.dexterous.shadow_pen_fixed ---------------------------------------------------------- - -Create the shadow pen fixed vector environment with an explicit DexSim solver and packaged model inputs. - -.. currentmodule:: embodichain_tasks.manipulation.dexterous.shadow_pen_fixed - -.. autosummary:: - - make_env - -.. autofunction:: make_env - - embodichain_tasks.manipulation.push_cube ---------------------------------------- @@ -6255,6 +5990,257 @@ 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.leap_cube_face_turn +------------------------------------------------------------ + +Create the leap cube face turn vector environment with an explicit DexSim solver and packaged model inputs. + +.. currentmodule:: embodichain_tasks.manipulation.dexterous.leap_cube_face_turn + +.. autosummary:: + + make_env + +.. autofunction:: make_env + + +embodichain_tasks.manipulation.dexterous.leap_cube_reorient +----------------------------------------------------------- + +Create the leap cube reorient vector environment with an explicit DexSim solver and packaged model inputs. + +.. currentmodule:: embodichain_tasks.manipulation.dexterous.leap_cube_reorient + +.. autosummary:: + + make_env + +.. autofunction:: make_env + + +embodichain_tasks.manipulation.dexterous.leap_cube_rotate_z +----------------------------------------------------------- + +Create the leap cube rotate z vector environment with an explicit DexSim solver and packaged model inputs. + +.. currentmodule:: embodichain_tasks.manipulation.dexterous.leap_cube_rotate_z + +.. autosummary:: + + make_env + +.. autofunction:: make_env + + +embodichain_tasks.manipulation.dexterous.shadow_pen_continuous +-------------------------------------------------------------- + +Create the shadow pen continuous vector environment with an explicit DexSim solver and packaged model inputs. + +.. currentmodule:: embodichain_tasks.manipulation.dexterous.shadow_pen_continuous + +.. autosummary:: + + make_env + +.. autofunction:: make_env + + +embodichain_tasks.manipulation.dexterous.shadow_pen_fixed +--------------------------------------------------------- + +Create the shadow pen fixed vector environment with an explicit DexSim solver and packaged model inputs. + +.. currentmodule:: embodichain_tasks.manipulation.dexterous.shadow_pen_fixed + +.. autosummary:: + + make_env + +.. autofunction:: make_env + + Parallel-jaw hand configuration and composition ---------------------------------------------- diff --git a/docs/source/overview/rl/locomotion.md b/docs/source/overview/rl/locomotion.md index 9bf5c9131d..f3ec54f25a 100644 --- a/docs/source/overview/rl/locomotion.md +++ b/docs/source/overview/rl/locomotion.md @@ -48,11 +48,11 @@ Humanoid uses clipped effort actions (`[-0.4, 0.4]`) multiplied by per-joint mot ## G1 rough terrain with DexUni -`g1_isaac_rough/agents/ppo.dexuni.yaml` selects the pure MuJoCo branch of -DexUni with Newton-generated terrain contacts. The paired DexSim build must +`g1_rough/agents/ppo.dexuni.yaml` selects the pure MuJoCo branch of +DexUni with Newton-generated terrain contacts (`UnitreeG1RoughRL-v1`). The paired DexSim build must support `mujoco_options.use_mujoco_contacts: false` in this branch and export MuJoCo solver contact forces for contact sensors. The MJWarp deployment remains -`ppo.newton.yaml`; both configurations retain the same terrain and physical +`g1_rough/agents/ppo.newton.yaml`; both configurations retain the same terrain and physical parameters. The separate `UnitreeG1Rough` and `G1RoughTerrains` asset packages provide the diff --git a/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/rsl_rl.json b/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/rsl_rl.json deleted file mode 100644 index 76eb553363..0000000000 --- a/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/rsl_rl.json +++ /dev/null @@ -1,65 +0,0 @@ -{ - "seed": 42, - "device": "cuda:0", - "num_steps_per_env": 24, - "max_iterations": 5000, - "obs_groups": { - "actor": [ - "policy" - ], - "critic": [ - "policy" - ] - }, - "check_for_nan": true, - "save_interval": 50, - "experiment_name": "g1_rough", - "logger": "tensorboard", - "class_name": "OnPolicyRunner", - "algorithm": { - "class_name": "PPO", - "num_learning_epochs": 5, - "num_mini_batches": 4, - "learning_rate": 0.001, - "schedule": "adaptive", - "gamma": 0.99, - "lam": 0.95, - "entropy_coef": 0.008, - "desired_kl": 0.01, - "max_grad_norm": 1.0, - "optimizer": "adam", - "value_loss_coef": 1.0, - "use_clipped_value_loss": true, - "clip_param": 0.2, - "normalize_advantage_per_mini_batch": false, - "share_cnn_encoders": false, - "rnd_cfg": null, - "symmetry_cfg": null - }, - "actor": { - "class_name": "MLPModel", - "hidden_dims": [ - 512, - 256, - 128 - ], - "activation": "elu", - "obs_normalization": false, - "distribution_cfg": { - "class_name": "GaussianDistribution", - "init_std": 1.0, - "std_type": "scalar" - } - }, - "critic": { - "class_name": "MLPModel", - "hidden_dims": [ - 512, - 256, - 128 - ], - "activation": "elu", - "obs_normalization": false, - "distribution_cfg": null - } -} diff --git a/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/agents/ppo.dexuni.yaml b/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/agents/ppo.dexuni.yaml similarity index 100% rename from extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/agents/ppo.dexuni.yaml rename to extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/agents/ppo.dexuni.yaml diff --git a/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/agents/ppo.newton.yaml b/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/agents/ppo.newton.yaml similarity index 100% rename from extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/agents/ppo.newton.yaml rename to extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/agents/ppo.newton.yaml diff --git a/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/catalog.yaml b/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/catalog.yaml similarity index 92% rename from extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/catalog.yaml rename to extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/catalog.yaml index c9031d8736..9f59a72756 100644 --- a/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/catalog.yaml +++ b/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/catalog.yaml @@ -1,4 +1,4 @@ -task_key: g1_isaac_rough +task_key: g1_rough title: Unitree G1 Rough-Terrain Locomotion summary: G1 locomotion with a frozen terrain curriculum and height observations. tags: diff --git a/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/env.dexuni.yaml b/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/env.dexuni.yaml similarity index 99% rename from extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/env.dexuni.yaml rename to extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/env.dexuni.yaml index f117cd0d97..55f19763e3 100644 --- a/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/env.dexuni.yaml +++ b/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/env.dexuni.yaml @@ -1,5 +1,5 @@ physics: newton -id: UnitreeG1IsaacRoughRL-v1 +id: UnitreeG1RoughRL-v1 max_episode_steps: 1000 num_envs: 1 scene_node_capacity: 65535 diff --git a/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/env.newton.yaml b/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/env.newton.yaml similarity index 99% rename from extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/env.newton.yaml rename to extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/env.newton.yaml index 18f7fd6608..0e0ea4518f 100644 --- a/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/env.newton.yaml +++ b/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/env.newton.yaml @@ -1,5 +1,5 @@ physics: newton -id: UnitreeG1IsaacRoughRL-v1 +id: UnitreeG1RoughRL-v1 max_episode_steps: 1000 num_envs: 1 scene_node_capacity: 65535 diff --git a/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/task.json b/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/task.json similarity index 99% rename from extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/task.json rename to extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/task.json index a23933d249..eeec297f30 100644 --- a/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/task.json +++ b/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/task.json @@ -1,8 +1,6 @@ { - "reference": { - "isaac_lab": "v3.0.0-EA", - "task": "Isaac-Velocity-Rough-G1" - }, + "schema": "unitree-velocity-task", + "official_task": "Unitree-G1-Rough", "robot": { "joint_names": [ "left_hip_pitch_joint", diff --git a/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/g1_isaac_rough.py b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/g1_rough.py similarity index 74% rename from extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/g1_isaac_rough.py rename to extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/g1_rough.py index b2c12ab642..be16dacd10 100644 --- a/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/g1_isaac_rough.py +++ b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/g1_rough.py @@ -14,24 +14,24 @@ # limitations under the License. # ---------------------------------------------------------------------------- -"""Isaac Lab 3.0 EA g1 velocity task on DexSim.""" +"""Unitree G1 rough-terrain velocity task.""" from __future__ import annotations from embodichain.lab.gym.utils.registration import register_env -from .isaac.config import load_config -from .isaac.runtime import IsaacVelocityEnv +from .rough.config import load_config +from .rough.runtime import RoughVelocityEnv -__all__ = ["UnitreeG1IsaacRoughEnv"] +__all__ = ["UnitreeG1RoughEnv"] -_CONFIG = load_config("g1_isaac_rough") +_CONFIG = load_config("g1_rough") @register_env( - "UnitreeG1IsaacRoughRL-v1", max_episode_steps=1000, override=True, supports_rl=True + "UnitreeG1RoughRL-v1", max_episode_steps=1000, override=True, supports_rl=True ) -class UnitreeG1IsaacRoughEnv(IsaacVelocityEnv): - """Run the reference g1 policy interface and rewards.""" +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"]) diff --git a/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/README.md b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/README.md deleted file mode 100644 index f6895ee7d1..0000000000 --- a/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/README.md +++ /dev/null @@ -1,36 +0,0 @@ -# G1 rough-terrain velocity task - -The task controls 37 joints and exposes 310 policy observations, including 187 -terrain height samples. It uses the independent `UnitreeG1Rough` robot archive -and the complete `G1RoughTerrains` curriculum. The existing 29-joint -`UnitreeG1Locomotion` asset remains unchanged. - -The runnable deployment is -`extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/env.newton.yaml`. -It explicitly selects Newton contact generation with the MuJoCo Warp solver, -0.005 s outer physics steps, two solver substeps and four outer steps per -control interval. It retains the reference task's 100 solver iterations and -50 line-search iterations. - -Train through the unified CLI: - -```bash -embodichain train-rl --config extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_isaac_rough/agents/ppo.newton.yaml -``` - -The default smoke configuration uses 64 environments and 20 updates. Increase -these fields for longer training. Archive availability and the matching -DexSim selected-link property APIs are required. The package loader supports -explicit G1 reference bundles for reproducible terrain and initial-state checks. -`ReferenceRslRlEnv` retains the previously validated RSL-RL interface. - -The training config resolves its task config relative to the config file, so -the installed configuration also works outside the repository directory. -Contact observations read samples owned by the task's physics-step callback; -reset clears selected rows without querying the invalidated solver snapshot. - -The native MuJoCo Warp heightfield collision path remains unsuitable for this -terrain because the retained official-runtime reproducer produces nonfinite -states. The default deployment therefore selects Newton contacts. DexUni's -G1 external-contact path still requires the separately retained dispatch -adapter; it is not enabled by this deployment. diff --git a/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/rsl_rl.py b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/rsl_rl.py deleted file mode 100644 index d6ee0b8ea6..0000000000 --- a/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/rsl_rl.py +++ /dev/null @@ -1,97 +0,0 @@ -# ---------------------------------------------------------------------------- -# 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. -# ---------------------------------------------------------------------------- - -"""RSL-RL 5.4 VecEnv interface for the reference task deployments.""" - -from __future__ import annotations - -from contextlib import contextmanager -from typing import TYPE_CHECKING, Any - -import torch -import warp as wp -from tensordict import TensorDict - -if TYPE_CHECKING: - from .runtime import IsaacVelocityEnv - -__all__ = ["ReferenceRslRlEnv"] - - -@contextmanager -def _simulation_stream(device: torch.device): - """Run a policy call on the simulation stream with ordered handoffs.""" - if device.type != "cuda": - yield - return - caller = torch.cuda.current_stream(device) - stream = wp.stream_to_torch(str(device)) - same_stream = caller.cuda_stream == stream.cuda_stream - if not same_stream: - stream.wait_stream(caller) - try: - with torch.cuda.stream(stream): - yield - finally: - if not same_stream: - caller.wait_stream(stream) - - -class ReferenceRslRlEnv: - """Expose reference policy observations and timeout bootstrapping to RSL-RL.""" - - def __init__(self, env: IsaacVelocityEnv) -> None: - self.env = env - self.num_envs = env.num_envs - self.num_actions = env.velocity_task_config.action_dim - self.max_episode_length = env.max_episode_steps - self.device = env.device - self.cfg = env.cfg - - @property - def episode_length_buf(self) -> torch.Tensor: - """Current control-step counters used by the RSL-RL runner.""" - return self.env._elapsed_steps - - @episode_length_buf.setter - def episode_length_buf(self, value: torch.Tensor) -> None: - self.env._elapsed_steps.copy_(value) - self.env._episode_step.copy_(value) - - def get_observations(self) -> TensorDict: - """Return the shared actor/critic reference policy group.""" - with _simulation_stream(self.device): - obs = self.env.get_obs() - return TensorDict({"policy": obs["policy"]}, batch_size=[self.num_envs]) - - def step( - self, actions: torch.Tensor - ) -> tuple[TensorDict, torch.Tensor, torch.Tensor, dict[str, Any]]: - """Forward unbounded actions and keep time limits separate from failures.""" - with _simulation_stream(self.device): - obs, rewards, terminated, truncated, info = self.env.step( - actions, policy_observations_only=True - ) - return ( - TensorDict({"policy": obs["policy"]}, batch_size=[self.num_envs]), - rewards, - terminated | truncated, - {"time_outs": truncated}, - ) - - def close(self) -> None: - """Close the owned simulation environment.""" - self.env.close() diff --git a/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/README.md b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/README.md new file mode 100644 index 0000000000..b5eb769076 --- /dev/null +++ b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/README.md @@ -0,0 +1,38 @@ +# G1 rough-terrain velocity task + +The task controls 37 joints and exposes 310 policy observations, including 187 +terrain height samples. It uses the independent `UnitreeG1Rough` robot archive +and the complete `G1RoughTerrains` curriculum. The existing 29-joint +`UnitreeG1Locomotion` asset remains unchanged. + +Two runnable deployments are packaged under +`extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/`: + +- `env.newton.yaml` (`agents/ppo.newton.yaml`): Newton contact generation with + the MuJoCo Warp solver. +- `env.dexuni.yaml` (`agents/ppo.dexuni.yaml`): Newton contact generation with + the DexUni solver. + +Both deployments use 0.005 s outer physics steps, two solver substeps, four +outer steps per control interval, 100 solver iterations, and 50 line-search +iterations. + +Train through the unified CLI: + +```bash +embodichain train-rl --config extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/agents/ppo.newton.yaml +embodichain train-rl --config extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/agents/ppo.dexuni.yaml +``` + +The default smoke configuration uses 64 environments and 20 updates. Increase +these fields for longer training. Archive availability and the matching DexSim +selected-link property APIs are required. `load_rough_env_cfg` supports explicit +G1 asset bundles for reproducible terrain and initial-state checks. + +The training config resolves its task config relative to the config file, so +the installed configuration also works outside the repository directory. +Contact observations read samples owned by the task's physics-step callback; +reset clears selected rows without querying the invalidated solver snapshot. + +Both deployments select Newton heightfield contacts because native MuJoCo Warp +heightfield collision produces nonfinite states on this terrain. diff --git a/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/__init__.py b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/__init__.py similarity index 83% rename from extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/__init__.py rename to extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/__init__.py index 781fed98bf..73cb831e90 100644 --- a/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/__init__.py +++ b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/__init__.py @@ -14,10 +14,10 @@ # limitations under the License. # ---------------------------------------------------------------------------- -"""Isaac reference velocity task computations and runtime adapter.""" +"""Rough-terrain velocity task computations and runtime adapter.""" from __future__ import annotations -from .environment import load_reference_env_cfg +from .environment import load_rough_env_cfg -__all__ = ["load_reference_env_cfg"] +__all__ = ["load_rough_env_cfg"] diff --git a/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/_frame.py b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/_frame.py similarity index 100% rename from extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/_frame.py rename to extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/_frame.py diff --git a/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/config.py b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/config.py similarity index 86% rename from extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/config.py rename to extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/config.py index 3f8350893e..948084b1bb 100644 --- a/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/config.py +++ b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/config.py @@ -14,7 +14,7 @@ # limitations under the License. # ---------------------------------------------------------------------------- -"""Fixed Isaac Lab 3.0 EA velocity task settings.""" +"""Rough-terrain velocity task settings.""" from __future__ import annotations @@ -24,12 +24,12 @@ from ..contracts._config import load_task_data -__all__ = ["IsaacVelocityConfig", "load_config"] +__all__ = ["RoughVelocityConfig", "load_config"] @configclass -class IsaacVelocityConfig: - """Policy dimensions, timing, model parameters and resolved reference terms.""" +class RoughVelocityConfig: + """Policy dimensions, timing, model parameters and resolved task terms.""" data: dict = MISSING joint_names: tuple[str, ...] = MISSING @@ -43,13 +43,13 @@ class IsaacVelocityConfig: max_episode_steps: int = 1000 -def load_config(task: str) -> IsaacVelocityConfig: - """Load one packaged reference definition, retaining policy joint order.""" +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 = IsaacVelocityConfig( + cfg = RoughVelocityConfig( data=data, joint_names=tuple(robot["joint_names"]), default_joint_position=tuple(robot["default_joint_position"]), diff --git a/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/curriculum.py b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/curriculum.py similarity index 97% rename from extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/curriculum.py rename to extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/curriculum.py index ade4682959..9614a31cad 100644 --- a/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/curriculum.py +++ b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/curriculum.py @@ -14,16 +14,16 @@ # limitations under the License. # ---------------------------------------------------------------------------- -"""Terrain assignment and distance curriculum matching Isaac G1 Rough.""" +"""Terrain assignment and distance curriculum for rough-terrain locomotion.""" from __future__ import annotations import torch -__all__ = ["ReferenceTerrainCurriculum"] +__all__ = ["TerrainCurriculum"] -class ReferenceTerrainCurriculum: +class TerrainCurriculum: """Assign terrain columns and advance rows at episode resets. Args: diff --git a/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/environment.py b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/environment.py similarity index 76% rename from extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/environment.py rename to extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/environment.py index ddb648751e..891b92026c 100644 --- a/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/environment.py +++ b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/environment.py @@ -14,7 +14,7 @@ # limitations under the License. # ---------------------------------------------------------------------------- -"""Build reference environments from explicitly supplied frozen scene assets.""" +"""Build rough-terrain velocity environments from frozen scene assets.""" from __future__ import annotations @@ -30,12 +30,12 @@ from .config import load_config -__all__ = ["load_reference_env_cfg"] +__all__ = ["load_rough_env_cfg"] -def load_reference_env_cfg( +def load_rough_env_cfg( robot: str, - reference_bundle: str | Path, + asset_bundle: str | Path, *, num_envs: int = 1, seed: int = 42, @@ -47,15 +47,15 @@ def load_reference_env_cfg( Args: robot: ``g1``. - reference_bundle: Directory containing the exported robot USDs and, - for G1, terrain.npz and terrain.json with evaluation origins or the - complete reference training terrain and curriculum definition. + 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 reference observation noise, startup - mass/COM events, random reset and interval velocity pushes. + 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`` uses the EA reference setting. + 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. @@ -63,13 +63,13 @@ def load_reference_env_cfg( The resolved EmbodiChain environment configuration. Raises: - ValueError: The robot selector is unknown. - FileNotFoundError: A required reference asset is missing. + ValueError: The robot selector is unknown or bundle metadata is invalid. + FileNotFoundError: A required scene asset is missing. """ - tasks = {"g1": "g1_isaac_rough"} + tasks = {"g1": "g1_rough"} if robot not in tasks: - raise ValueError(f"Unknown reference robot: {robot!r}") - bundle = Path(reference_bundle).expanduser().resolve() + 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) @@ -86,8 +86,8 @@ def load_reference_env_cfg( data["physics_config"]["solver_cfg"]["use_mujoco_contacts"] = use_mujoco_contacts if use_mujoco_contacts: data["physics_config"]["collision_cfg"] = None - # The EA startup material event changes Newton mu to 0.8, while its native - # MJWarp model retains the imported sliding friction of 1.0. + # 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 ) @@ -102,12 +102,12 @@ def load_reference_env_cfg( states = json.loads(Path(initial_state_file).read_text()) task = load_config(tasks[robot]) if ( - states.get("schema") != "isaac_velocity.initial_states@1" + 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 reference robot and policy joint order." + "Initial states must match the target robot and policy joint order." ) rows = states["initial_states"] if not rows: @@ -131,9 +131,7 @@ def load_reference_env_cfg( if np.any(position < limits[:, 0] - 1e-7) or np.any( position > limits[:, 1] + 1e-7 ): - raise ValueError( - "Initial joint positions exceed the reference soft limits." - ) + raise ValueError("Initial joint positions exceed the task soft limits.") extension["reference_initial_states"] = rows for term in data["env"]["observations"].values(): term["params"]["enable_corruption"] = training_randomization @@ -144,9 +142,15 @@ def load_reference_env_cfg( extension["reference_terrain"] = str(terrain) metadata = json.loads((bundle / "terrain.json").read_text()) if training_randomization: - if metadata.get("schema") != "isaac_velocity.rough_training_terrain@1": + 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 the complete reference Rough terrain bundle." + "G1 training requires terrain.json with terrain_origins, " + "max_init_terrain_level, and generator." ) extension["reference_terrain_course"] = metadata else: diff --git a/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/mdp.py b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/mdp.py similarity index 87% rename from extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/mdp.py rename to extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/mdp.py index 451a669d3e..0383ffea26 100644 --- a/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/mdp.py +++ b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/mdp.py @@ -14,7 +14,7 @@ # limitations under the License. # ---------------------------------------------------------------------------- -"""Tensor computations for the Isaac Lab 3.0 EA velocity task definitions.""" +"""Tensor computations for rough-terrain velocity task definitions.""" from __future__ import annotations @@ -24,12 +24,12 @@ import torch if TYPE_CHECKING: - from .config import IsaacVelocityConfig + from .config import RoughVelocityConfig __all__ = [ - "IsaacVelocityState", - "IsaacVelocityReward", - "IsaacVelocityConstants", + "RoughVelocityState", + "RoughVelocityReward", + "RoughVelocityConstants", "action_target", "observation_terms", "build_observation", @@ -41,7 +41,7 @@ @dataclass -class IsaacVelocityState: +class RoughVelocityState: """Measured tensors in policy joint order and XYZW quaternion convention.""" base_lin_vel_b: torch.Tensor @@ -68,7 +68,7 @@ class IsaacVelocityState: @dataclass(frozen=True) -class IsaacVelocityReward: +class RoughVelocityReward: """Reward values before and after weight and control-period multiplication.""" raw_terms: dict[str, torch.Tensor] @@ -77,7 +77,7 @@ class IsaacVelocityReward: @dataclass(frozen=True) -class IsaacVelocityConstants: +class RoughVelocityConstants: """Device tensors for the fixed task configuration.""" default_joint_position: torch.Tensor @@ -88,8 +88,8 @@ class IsaacVelocityConstants: @classmethod def from_config( - cls, config: IsaacVelocityConfig, reference: torch.Tensor - ) -> IsaacVelocityConstants: + 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), @@ -114,12 +114,12 @@ def from_config( def action_target( - config: IsaacVelocityConfig, + config: RoughVelocityConfig, action: torch.Tensor, *, - constants: IsaacVelocityConstants | None = None, + constants: RoughVelocityConstants | None = None, ) -> torch.Tensor: - """Apply the reference default joint offset and scale without clipping.""" + """Apply the task default joint offset and scale without clipping.""" scale = ( action.new_tensor(config.action_scale) if constants is None @@ -134,12 +134,12 @@ def action_target( def observation_terms( - config: IsaacVelocityConfig, - state: IsaacVelocityState, + config: RoughVelocityConfig, + state: RoughVelocityState, *, - constants: IsaacVelocityConstants | None = None, + constants: RoughVelocityConstants | None = None, ) -> dict[str, torch.Tensor]: - """Return reference policy terms in their declared order before noise and clipping.""" + """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 @@ -160,18 +160,18 @@ def observation_terms( def build_observation( - config: IsaacVelocityConfig, - state: IsaacVelocityState, + config: RoughVelocityConfig, + state: RoughVelocityState, *, noise: dict[str, torch.Tensor] | None = None, - constants: IsaacVelocityConstants | None = None, + constants: RoughVelocityConstants | None = None, ) -> torch.Tensor: - """Process term noise, clipping and scaling in reference order. + """Process term noise, clipping and scaling in declared order. Args: - config: Fixed reference task definition. + config: Fixed rough-terrain velocity task definition. state: Measured state tensors. - noise: Optional realized additive noise per term for cross-stack replay. + noise: Optional realized additive noise per term. constants: Optional device tensors for the fixed task configuration. Returns: @@ -195,7 +195,7 @@ def build_observation( def compute_termination( - config: IsaacVelocityConfig, state: IsaacVelocityState + 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 @@ -203,13 +203,13 @@ def compute_termination( def compute_rewards( - config: IsaacVelocityConfig, - state: IsaacVelocityState, + config: RoughVelocityConfig, + state: RoughVelocityState, terminated: torch.Tensor, *, - constants: IsaacVelocityConstants | None = None, -) -> IsaacVelocityReward: - """Evaluate active reference terms, including joint selections and step duration.""" + 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) @@ -287,13 +287,13 @@ def compute_rewards( elif function == "joint_deviation_l1": value = (state.joint_pos - default).abs()[:, joints].sum(-1) else: - raise ValueError(f"Unsupported reference reward function: {function}") + 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 IsaacVelocityReward( + return RoughVelocityReward( raw, weighted, torch.stack(tuple(weighted.values())).sum(0) ) diff --git a/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/runtime.py b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/runtime.py similarity index 96% rename from extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/runtime.py rename to extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/runtime.py index 0dfac3c749..615c0316e9 100644 --- a/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/runtime.py +++ b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/runtime.py @@ -14,7 +14,7 @@ # limitations under the License. # ---------------------------------------------------------------------------- -"""EmbodiChain and DexSim adapter for fixed Isaac velocity definitions.""" +"""EmbodiChain and DexSim runtime adapter for rough-terrain velocity tasks.""" from __future__ import annotations @@ -33,8 +33,8 @@ from .._embodichain import EmbodiChainVelocityEnv from .mdp import ( - IsaacVelocityState, - IsaacVelocityConstants, + RoughVelocityState, + RoughVelocityConstants, build_observation, observation_terms, action_target, @@ -43,24 +43,24 @@ dc_motor_effort, update_contact_times, ) -from .terrain import ReferenceHeightScanner -from .curriculum import ReferenceTerrainCurriculum +from .terrain import HeightScanner +from .curriculum import TerrainCurriculum -__all__ = ["IsaacVelocityEnv"] +__all__ = ["RoughVelocityEnv"] -class IsaacVelocityEnv(EmbodiChainVelocityEnv): - """Run the reference policy interface on shared training or evaluation terrain. +class RoughVelocityEnv(EmbodiChainVelocityEnv): + """Run rough-terrain velocity locomotion on shared training or evaluation terrain. - The benchmark disables startup mass/COM and interval pushes on both stacks. - ``reference_randomization=True`` enables those reference training events. - Training loads the complete reference Rough terrain and curriculum; fixed + Deterministic evaluation disables startup mass/COM randomization and + interval pushes. ``reference_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 - rays and Newton rasterization for collision. + height rays and Newton heightfield rasterization for collision. """ _default_action_clip = None - state_type = IsaacVelocityState + state_type = RoughVelocityState compute_rewards_fn = staticmethod(compute_rewards) compute_termination_fn = staticmethod(compute_termination) @@ -125,7 +125,7 @@ def _init_sim_state(self, **kwargs: Any) -> None: if id(collection) in expected ) config = self.velocity_task_config - self._reference_constants = IsaacVelocityConstants.from_config( + self._reference_constants = RoughVelocityConstants.from_config( config, self.locomotion_action ) self._reference_origin = self.locomotion_action.new_tensor( @@ -185,7 +185,7 @@ def _init_sim_state(self, **kwargs: Any) -> None: from dexsim.engine.newton_physics.backend_registry import get_newton_backend backend = get_newton_backend(self.sim.spawn_result.world) - self._height_scanner = ReferenceHeightScanner( + self._height_scanner = HeightScanner( *self._reference_terrain, self.num_envs, self.device, @@ -213,7 +213,7 @@ def _init_sim_state(self, **kwargs: Any) -> None: device=self.device, dtype=torch.float32, ) - self._reference_terrain_course = ReferenceTerrainCurriculum( + self._reference_terrain_course = TerrainCurriculum( origins, self.num_envs, terrain_course["max_init_terrain_level"], @@ -352,8 +352,8 @@ def _sample_reference_contacts(self) -> None: def _update_sim_state(self, **kwargs: Any) -> None: data = super()._update_sim_state(**kwargs) - # The EA native Go2 actuator path updates articulation data once per - # decimation block. G1 updates it after each individual physics step. + # 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) @@ -613,8 +613,8 @@ def _write_episode_initial_state( """Keep sampled RNG values until the reference reset writes final state.""" self._reference_reset_pose = root_pose.clone() - def get_velocity_locomotion_state(self) -> IsaacVelocityState: - """Fetch policy-ordered reference state and actual torso height rays.""" + 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 @@ -632,7 +632,7 @@ def get_velocity_locomotion_state(self) -> IsaacVelocityState: def _make_reference_state( self, data: dict[str, torch.Tensor] - ) -> IsaacVelocityState: + ) -> 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( @@ -662,7 +662,7 @@ def _make_reference_state( self, "_reference_defer_observations", False ): heights = self._compute_reference_height_scan() - return IsaacVelocityState( + return RoughVelocityState( base_lin_vel_b=linear_b, base_ang_vel_b=angular_b, projected_gravity_b=gravity_b, diff --git a/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/terrain.py b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/terrain.py similarity index 97% rename from extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/terrain.py rename to extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/terrain.py index fe59fd94cc..038341f1c6 100644 --- a/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/isaac/terrain.py +++ b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/terrain.py @@ -14,7 +14,7 @@ # limitations under the License. # ---------------------------------------------------------------------------- -"""Yaw-aligned height rays over reference collision terrain.""" +"""Yaw-aligned height rays over collision terrain.""" from __future__ import annotations @@ -23,7 +23,7 @@ import torch import warp as wp -__all__ = ["ReferenceHeightScanner"] +__all__ = ["HeightScanner"] @wp.kernel(enable_backward=False) @@ -85,8 +85,8 @@ def _scan( height[env, ray] = value -class ReferenceHeightScanner: - """187 terrain rays with the reference torso frame, XY order and offset.""" +class HeightScanner: + """187 terrain rays in the torso yaw frame, XY grid order and height offset.""" def __init__( self, diff --git a/tests/gym/envs/tasks/fixtures/g1_isaac_rough.json b/tests/gym/envs/tasks/fixtures/g1_rough.json similarity index 99% rename from tests/gym/envs/tasks/fixtures/g1_isaac_rough.json rename to tests/gym/envs/tasks/fixtures/g1_rough.json index 7b99f6b478..5aec81ff3b 100644 --- a/tests/gym/envs/tasks/fixtures/g1_isaac_rough.json +++ b/tests/gym/envs/tasks/fixtures/g1_rough.json @@ -1,5 +1,5 @@ { - "reference": "Isaac Lab v3.0.0-EA executed functions", + "task": "g1_rough", "state": { "base_lin_vel_b": [ [ diff --git a/tests/gym/envs/tasks/test_isaac_velocity.py b/tests/gym/envs/tasks/test_rough_velocity.py similarity index 89% rename from tests/gym/envs/tasks/test_isaac_velocity.py rename to tests/gym/envs/tasks/test_rough_velocity.py index a4e1711486..77b45721b7 100644 --- a/tests/gym/envs/tasks/test_isaac_velocity.py +++ b/tests/gym/envs/tasks/test_rough_velocity.py @@ -14,7 +14,7 @@ # limitations under the License. # ---------------------------------------------------------------------------- -"""Compare packaged velocity definitions with executed Isaac reference values.""" +"""Verify packaged rough-terrain velocity definitions, curriculum and runtime.""" from __future__ import annotations @@ -38,18 +38,18 @@ from embodichain.lab.gym.envs.episode_context import EpisodeContext from embodichain.lab.sim.sim_manager import SimulationManager -from embodichain_tasks.locomotion.velocity.isaac.curriculum import ( - ReferenceTerrainCurriculum, +from embodichain_tasks.locomotion.velocity.rough.curriculum import ( + TerrainCurriculum, ) -from embodichain_tasks.locomotion.velocity.isaac.config import load_config -from embodichain_tasks.locomotion.velocity.isaac.environment import ( - load_reference_env_cfg, +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.isaac.rsl_rl import ReferenceRslRlEnv -from embodichain_tasks.locomotion.velocity.isaac.runtime import IsaacVelocityEnv -from embodichain_tasks.locomotion.velocity.isaac.terrain import ReferenceHeightScanner -from embodichain_tasks.locomotion.velocity.isaac.mdp import ( - IsaacVelocityState, +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, @@ -82,7 +82,7 @@ def test_reference_observation_preserves_reset_contact_rows() -> None: sensors={"locomotion_contacts": contacts, "camera": camera}, _reference_sensor=contacts, ) - obs = IsaacVelocityEnv._get_sensor_obs(env) + obs = RoughVelocityEnv._get_sensor_obs(env) assert not obs["locomotion_contacts"]["is_valid"].any() assert obs["camera"].eq(1).all() assert calls == [[3], {"fetch_only": True}] @@ -107,7 +107,7 @@ def test_reference_uses_control_frame_contacts_without_parent_histories() -> Non env._initialize_contact_histories() assert registered == ["feet", "illegal"] registered.clear() - reference = object.__new__(IsaacVelocityEnv) + reference = object.__new__(RoughVelocityEnv) reference.get_sensor = lambda name: sensor reference._initialize_contact_histories() assert registered == [] @@ -123,7 +123,7 @@ def test_joint_acceleration_matches_reference_update_interval( EmbodiChainVelocityEnv, ) - env = object.__new__(IsaacVelocityEnv) + 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) @@ -178,7 +178,7 @@ def update_control_state(self, **kwargs): @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__(IsaacVelocityEnv) + env = object.__new__(RoughVelocityEnv) env._num_envs = 1 env.sim = SimpleNamespace(device=torch.device("cpu")) env.sim_cfg = SimpleNamespace(physics_dt=0.005) @@ -235,24 +235,24 @@ def fetch_state(): @pytest.mark.no_sim -@pytest.mark.parametrize("task, dimension", [("g1_isaac_rough", 310)]) +@pytest.mark.parametrize("task, dimension", [("g1_rough", 310)]) @pytest.mark.parametrize("cached", [False, True]) -def test_executed_isaac_reference(task: str, dimension: int, cached: bool) -> None: - """Use golden values exported by the actual Isaac MDP functions.""" +def test_rough_velocity_mdp_golden_values( + task: str, dimension: int, cached: bool +) -> None: + """Verify observation, reward and termination values against golden fixtures.""" fixture = json.loads( (Path(__file__).parent / "fixtures" / f"{task}.json").read_text() ) - state = IsaacVelocityState( + state = RoughVelocityState( **{ name: torch.tensor(value, dtype=torch.float32) for name, value in fixture["state"].items() } ) config = load_config(task) - from embodichain_tasks.locomotion.velocity.isaac.mdp import IsaacVelocityConstants - constants = ( - IsaacVelocityConstants.from_config(config, state.joint_pos) if cached else None + RoughVelocityConstants.from_config(config, state.joint_pos) if cached else None ) noise = {name: torch.tensor(value) for name, value in fixture["noise"].items()} observed = build_observation(config, state, noise=noise, constants=constants) @@ -278,7 +278,7 @@ def test_executed_isaac_reference(task: str, dimension: int, cached: bool) -> No @pytest.mark.no_sim -@pytest.mark.parametrize("task", ["g1_isaac_rough"]) +@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) @@ -362,35 +362,38 @@ def test_ground_replacement_updates_declared_geometry() -> None: @pytest.mark.no_sim -def test_rsl_wrapper_preserves_actions_and_timeout_bootstrapping() -> None: - """Failures end episodes while only timeouts bootstrap the value estimate.""" - actions = torch.full((2, 12), 4.0) - received = [] - - def step(value: torch.Tensor, *, policy_observations_only: bool) -> tuple: - assert policy_observations_only - received.append(value) - return ( - {"policy": torch.zeros(2, 48)}, - torch.ones(2), - torch.tensor([True, False]), - torch.tensor([False, True]), - {}, +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["reference_deterministic_reset"] is True + assert eval_cfg.extensions["reference_origin"] == [1.0, 2.0, 0.5] - inner = SimpleNamespace( - num_envs=2, - velocity_task_config=SimpleNamespace(action_dim=12), - max_episode_steps=1000, - device=torch.device("cpu"), - cfg=None, - step=step, + train_cfg = load_rough_env_cfg( + "g1", tmp_path, num_envs=4, training_randomization=True ) - wrapped = ReferenceRslRlEnv(inner) - _, _, done, extras = wrapped.step(actions) - assert received[0] is actions - assert done.tolist() == [True, True] - assert extras["time_outs"].tolist() == [False, True] + assert train_cfg.extensions["reference_randomization"] is True + assert "terrain_origins" in train_cfg.extensions["reference_terrain_course"] @pytest.mark.no_sim @@ -399,7 +402,7 @@ def test_height_scanner_keeps_yaw_frame_and_xy_order() -> None: vertices = np.array( [[-5, -5, -1.5], [5, -5, -0.5], [5, 5, 1.5], [-5, 5, 0.5]], dtype=np.float32 ) - scanner = ReferenceHeightScanner( + scanner = HeightScanner( vertices, np.array([[0, 1, 2], [0, 2, 3]], dtype=np.int32), 1, @@ -437,7 +440,7 @@ def test_height_scanner_queries_physics_heightfield_instead_of_source_mesh() -> vertices = np.array( [[-2, -2, 0], [2, -2, 0], [2, 2, 0], [-2, 2, 0]], dtype=np.float32 ) - scanner = ReferenceHeightScanner( + scanner = HeightScanner( vertices, np.array([[0, 1, 2], [0, 2, 3]], dtype=np.int32), 1, @@ -453,7 +456,7 @@ def test_height_scanner_queries_physics_heightfield_instead_of_source_mesh() -> @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__(IsaacVelocityEnv) + env = object.__new__(RoughVelocityEnv) env._num_envs = 1 env._device = torch.device("cpu") env.sim = SimpleNamespace(device=torch.device("cpu")) @@ -482,13 +485,13 @@ def test_contact_impulse_uses_internal_solver_interval() -> None: @pytest.mark.no_sim -def test_rough_curriculum_matches_executed_isaac_resets() -> None: +def test_rough_curriculum_matches_golden_resets() -> None: """Match source assignments, threshold cases, selected resets and overflow.""" fixture = json.loads( (Path(__file__).parent / "fixtures/terrain_curriculum.json").read_text() ) origins = torch.tensor(fixture["origins"], dtype=torch.float32) - curriculum = ReferenceTerrainCurriculum( + curriculum = TerrainCurriculum( origins, 20, 5, 8.0, 20.0, torch.Generator().manual_seed(fixture["seed"]) ) torch.testing.assert_close( @@ -530,11 +533,11 @@ def test_rough_curriculum_matches_executed_isaac_resets() -> None: 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 = ReferenceTerrainCurriculum( + curriculum = TerrainCurriculum( origins, 1, 5, 8.0, 20.0, torch.Generator().manual_seed(42) ) curriculum.levels[:] = 3 - env = object.__new__(IsaacVelocityEnv) + env = object.__new__(RoughVelocityEnv) env._device = torch.device("cpu") env.sim = SimpleNamespace(device=torch.device("cpu")) env._num_envs = 1 @@ -589,7 +592,7 @@ def test_reference_step_orders_events_after_rewards_and_resets(monkeypatch) -> N ) events = [] - env = object.__new__(IsaacVelocityEnv) + env = object.__new__(RoughVelocityEnv) def parent_step(self, action, **kwargs): self._advance_command_timer() @@ -615,12 +618,12 @@ def parent_step(self, action, **kwargs): lambda self: events.append("heading"), ) monkeypatch.setattr( - IsaacVelocityEnv, + RoughVelocityEnv, "_apply_reference_interval_events", lambda self: events.append("push"), ) monkeypatch.setattr( - IsaacVelocityEnv, + RoughVelocityEnv, "get_obs", lambda self, **kwargs: events.append("observation") or {}, ) @@ -639,7 +642,7 @@ def test_training_step_defers_only_unused_observations( EmbodiChainVelocityEnv, ) - env = object.__new__(IsaacVelocityEnv) + env = object.__new__(RoughVelocityEnv) env.cfg = SimpleNamespace(events=SimpleNamespace()) env._num_envs = 2 env.sim = SimpleNamespace(device=torch.device("cpu")) @@ -678,13 +681,13 @@ def parent_step(self, action, **kwargs): monkeypatch.setattr(EmbodiChainVelocityEnv, "get_obs", raw_observe) monkeypatch.setattr( - IsaacVelocityEnv, "_extend_obs", lambda self, obs, **kwargs: observe(self) + RoughVelocityEnv, "_extend_obs", lambda self, obs, **kwargs: observe(self) ) monkeypatch.setattr(EmbodiChainVelocityEnv, "step", parent_step) - monkeypatch.setattr(IsaacVelocityEnv, "_advance_command_timer", lambda self: None) - monkeypatch.setattr(IsaacVelocityEnv, "_update_heading_commands", lambda self: None) + monkeypatch.setattr(RoughVelocityEnv, "_advance_command_timer", lambda self: None) + monkeypatch.setattr(RoughVelocityEnv, "_update_heading_commands", lambda self: None) monkeypatch.setattr( - IsaacVelocityEnv, "_apply_reference_interval_events", lambda self: None + RoughVelocityEnv, "_apply_reference_interval_events", lambda self: None ) obs, *_ = env.step(torch.zeros(2, 12), policy_observations_only=True) @@ -699,7 +702,7 @@ def parent_step(self, action, **kwargs): @pytest.mark.no_sim -@pytest.mark.parametrize("task", ["g1_isaac_rough"]) +@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 @@ -712,7 +715,7 @@ def test_reference_reset_writes_once_and_preserves_sampled_state(monkeypatch, ta rows = torch.tensor([1, 3]) def make_env(legacy: bool): - env = object.__new__(IsaacVelocityEnv) + env = object.__new__(RoughVelocityEnv) env._device = torch.device("cpu") env._num_envs = 4 env.sim = SimpleNamespace(device=env._device) @@ -823,7 +826,7 @@ def test_height_scanner_graph_replays_changed_poses() -> None: builder.add_shape_plane() model = builder.finalize(str(device)) state = model.state() - scanner = ReferenceHeightScanner( + scanner = HeightScanner( vertices, triangles, 2, device, physics_model=model, physics_state=state ) previous = None @@ -853,7 +856,7 @@ def test_curriculum_checkpoint_restores_assignments_and_rejects_other_maps() -> origins = torch.arange(36, dtype=torch.float32).reshape(4, 3, 3) def make(count=6): - return ReferenceTerrainCurriculum( + return TerrainCurriculum( origins.clone(), count, 3, 8.0, 20.0, torch.Generator().manual_seed(4) ) @@ -878,9 +881,9 @@ def make(count=6): _reference_terrain_course=target, num_envs=6, device=torch.device("cpu") ) env.validate_training_state = ( - lambda state: IsaacVelocityEnv.validate_training_state(env, state) + lambda state: RoughVelocityEnv.validate_training_state(env, state) ) - IsaacVelocityEnv.load_training_state(env, {"terrain_course": saved}) + RoughVelocityEnv.load_training_state(env, {"terrain_course": saved}) assert env._reference_resume_reset.all() before = target.state_dict() diff --git a/tests/gym/envs/tasks/test_isaac_velocity_frame.py b/tests/gym/envs/tasks/test_rough_velocity_frame.py similarity index 98% rename from tests/gym/envs/tasks/test_isaac_velocity_frame.py rename to tests/gym/envs/tasks/test_rough_velocity_frame.py index fb10ff7d60..e7a0ad4fb2 100644 --- a/tests/gym/envs/tasks/test_isaac_velocity_frame.py +++ b/tests/gym/envs/tasks/test_rough_velocity_frame.py @@ -22,7 +22,7 @@ import warp as wp from embodichain.utils.math import quat_apply_inverse -from embodichain_tasks.locomotion.velocity.isaac._frame import _reference_frame_vectors +from embodichain_tasks.locomotion.velocity.rough._frame import _reference_frame_vectors __all__: list[str] = [] pytestmark = pytest.mark.no_sim diff --git a/tests/gym/envs/tasks/test_unitree_locomotion.py b/tests/gym/envs/tasks/test_unitree_locomotion.py index 91a37f2ea5..7c44195267 100644 --- a/tests/gym/envs/tasks/test_unitree_locomotion.py +++ b/tests/gym/envs/tasks/test_unitree_locomotion.py @@ -351,7 +351,7 @@ def test_policy_viewer_target_pose_preserves_world_xyzw_and_copies_state(): @pytest.mark.parametrize( - "isaac,override,expected", + "rough,override,expected", [ (False, {}, 0.5), (True, {}, 1.0), @@ -360,7 +360,7 @@ def test_policy_viewer_target_pose_preserves_world_xyzw_and_copies_state(): ], ) def test_environment_action_clipping_defaults_and_overrides( - monkeypatch, isaac, override, expected + monkeypatch, rough, override, expected ): from dataclasses import dataclass from types import SimpleNamespace, MethodType @@ -369,13 +369,13 @@ def test_environment_action_clipping_defaults_and_overrides( from embodichain_tasks.locomotion.velocity._embodichain import ( EmbodiChainVelocityEnv, ) - from embodichain_tasks.locomotion.velocity.isaac.runtime import IsaacVelocityEnv + from embodichain_tasks.locomotion.velocity.rough.runtime import RoughVelocityEnv @dataclass class State: qpos: torch.Tensor - cls = IsaacVelocityEnv if isaac else EmbodiChainVelocityEnv + cls = RoughVelocityEnv if rough else EmbodiChainVelocityEnv host = object.__new__(cls) host.state_type = State host.velocity_task_config = SimpleNamespace( From bd022b99a1fb3919a24104f2059c339cdfa76079 Mon Sep 17 00:00:00 2001 From: acrlw <13927622+acrlw@users.noreply.github.com> Date: Fri, 13 Nov 2026 19:44:14 +0800 Subject: [PATCH 18/24] feat(tasks): refactor dexterous manipulation tasks to USD-native EmbodiedEnv with DexUni default - Replace runtime MJCF loading and _dexuni_adapter with USD-native DexterousManipulationEnv(EmbodiedEnv) loading ShadowHandPen, LeapHandCubeFaceTurn, and LeapHandCube .usdc scenes via SimulationManager.add_usd() - Add SimulationManager.remove_ground_plane() and clear_articulation_dynamics() for groundless and multi-articulation Newton scenes - Standardize Gym IDs (*RL-v1) and catalog deployments (newton -> solver_type: dexuni default; mjwarp -> solver_type: mujoco_warp) across all 5 dexterous tasks - Update public API docs, dexterous RL overview with 4096-env DexUni training/resume/eval baseline, and unit/USD parity tests --- docs/source/api_reference/public_api.rst | 51 +- docs/source/overview/rl/dexterous.md | 110 +- embodichain/lab/sim/sim_manager.py | 52 +- .../tasks/manipulation/dexterous/README.md | 106 +- .../dexterous/leap_cube_face_turn/README.md | 28 + .../{ppo.dexuni.yaml => ppo.mjwarp.yaml} | 42 +- .../{ppo.mujoco_warp.yaml => ppo.newton.yaml} | 42 +- .../leap_cube_face_turn/catalog.yaml | 14 + .../leap_cube_face_turn/env.mjwarp.yaml | 55 + .../leap_cube_face_turn/env.newton.yaml | 59 + .../dexterous/leap_cube_reorient/README.md | 28 + .../leap_cube_reorient/agents/ppo.mjwarp.yaml | 59 + .../leap_cube_reorient/agents/ppo.newton.yaml | 59 + .../dexterous/leap_cube_reorient/catalog.yaml | 14 + .../leap_cube_reorient/env.mjwarp.yaml | 55 + .../leap_cube_reorient/env.newton.yaml | 59 + .../dexterous/leap_cube_rotate_z/README.md | 28 + .../leap_cube_rotate_z/agents/ppo.dexuni.yaml | 53 - .../agents/ppo.mjwarp.yaml} | 44 +- .../agents/ppo.mujoco_warp.yaml | 53 - .../agents/ppo.newton.yaml} | 44 +- .../dexterous/leap_cube_rotate_z/catalog.yaml | 14 + .../leap_cube_rotate_z/env.mjwarp.yaml | 55 + .../leap_cube_rotate_z/env.newton.yaml | 59 + .../dexterous/shadow_pen_continuous/README.md | 28 + .../agents/ppo.dexuni.yaml | 53 - .../agents/ppo.mjwarp.yaml | 59 + .../agents/ppo.mujoco_warp.yaml | 53 - .../agents/ppo.newton.yaml | 59 + .../shadow_pen_continuous/catalog.yaml | 14 + .../shadow_pen_continuous/env.mjwarp.yaml | 55 + .../shadow_pen_continuous/env.newton.yaml | 59 + .../dexterous/shadow_pen_fixed/README.md | 28 + .../shadow_pen_fixed/agents/ppo.dexuni.yaml | 53 - .../shadow_pen_fixed/agents/ppo.mjwarp.yaml | 55 + .../agents/ppo.mujoco_warp.yaml | 53 - .../shadow_pen_fixed/agents/ppo.newton.yaml | 55 + .../dexterous/shadow_pen_fixed/catalog.yaml | 14 + .../shadow_pen_fixed/env.mjwarp.yaml | 55 + .../shadow_pen_fixed/env.newton.yaml | 59 + .../manipulation/dexterous/__init__.py | 15 +- .../manipulation/dexterous/_dexuni_adapter.py | 115 -- .../manipulation/dexterous/_runtime.py | 1393 +++++++++-------- .../manipulation/dexterous/_shadow_task.py | 19 +- .../dexterous/leap_cube_face_turn.py | 23 +- .../dexterous/leap_cube_reorient.py | 23 +- .../dexterous/leap_cube_rotate_z.py | 23 +- .../dexterous/shadow_pen_continuous.py | 23 +- .../dexterous/shadow_pen_fixed.py | 23 +- .../gym/envs/tasks/test_dexterous_metrics.py | 275 +++- tests/gym/envs/tasks/test_dexterous_usd.py | 112 +- 51 files changed, 2489 insertions(+), 1490 deletions(-) create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_face_turn/README.md rename extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_face_turn/agents/{ppo.dexuni.yaml => ppo.mjwarp.yaml} (54%) rename extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_face_turn/agents/{ppo.mujoco_warp.yaml => ppo.newton.yaml} (54%) create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_face_turn/catalog.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_face_turn/env.mjwarp.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_face_turn/env.newton.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_reorient/README.md create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_reorient/agents/ppo.mjwarp.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_reorient/agents/ppo.newton.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_reorient/catalog.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_reorient/env.mjwarp.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_reorient/env.newton.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_rotate_z/README.md delete mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_rotate_z/agents/ppo.dexuni.yaml rename extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/{leap_cube_reorient/agents/ppo.dexuni.yaml => leap_cube_rotate_z/agents/ppo.mjwarp.yaml} (53%) delete mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_rotate_z/agents/ppo.mujoco_warp.yaml rename extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/{leap_cube_reorient/agents/ppo.mujoco_warp.yaml => leap_cube_rotate_z/agents/ppo.newton.yaml} (53%) create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_rotate_z/catalog.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_rotate_z/env.mjwarp.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_rotate_z/env.newton.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/README.md delete mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/agents/ppo.dexuni.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/agents/ppo.mjwarp.yaml delete mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/agents/ppo.mujoco_warp.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/agents/ppo.newton.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/catalog.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/env.mjwarp.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/env.newton.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/README.md delete mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/agents/ppo.dexuni.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/agents/ppo.mjwarp.yaml delete mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/agents/ppo.mujoco_warp.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/agents/ppo.newton.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/catalog.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/env.mjwarp.yaml create mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/env.newton.yaml delete mode 100644 extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_dexuni_adapter.py diff --git a/docs/source/api_reference/public_api.rst b/docs/source/api_reference/public_api.rst index 8478c726c9..b29afdb3f0 100644 --- a/docs/source/api_reference/public_api.rst +++ b/docs/source/api_reference/public_api.rst @@ -6171,74 +6171,95 @@ The scanner evaluates 187 torso-relative terrain rays and replays a fixed-buffer :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 ------------------------------------------------------------ -Create the leap cube face turn vector environment with an explicit DexSim solver and packaged model inputs. +LEAP Hand top-layer articulated cube face-turning environment (``LeapCubeFaceTurnRL-v1``). .. currentmodule:: embodichain_tasks.manipulation.dexterous.leap_cube_face_turn .. autosummary:: - make_env + LeapCubeFaceTurnEnv -.. autofunction:: make_env +.. autoclass:: LeapCubeFaceTurnEnv + :members: embodichain_tasks.manipulation.dexterous.leap_cube_reorient ----------------------------------------------------------- -Create the leap cube reorient vector environment with an explicit DexSim solver and packaged model inputs. +LEAP Hand in-hand cube 3D orientation tracking environment (``LeapCubeReorientRL-v1``). .. currentmodule:: embodichain_tasks.manipulation.dexterous.leap_cube_reorient .. autosummary:: - make_env + LeapCubeReorientEnv -.. autofunction:: make_env +.. autoclass:: LeapCubeReorientEnv + :members: embodichain_tasks.manipulation.dexterous.leap_cube_rotate_z ----------------------------------------------------------- -Create the leap cube rotate z vector environment with an explicit DexSim solver and packaged model inputs. +LEAP Hand continuous cube Z-axis rotation environment (``LeapCubeRotateZAxisRL-v1``). .. currentmodule:: embodichain_tasks.manipulation.dexterous.leap_cube_rotate_z .. autosummary:: - make_env + LeapCubeRotateZAxisEnv -.. autofunction:: make_env +.. autoclass:: LeapCubeRotateZAxisEnv + :members: embodichain_tasks.manipulation.dexterous.shadow_pen_continuous -------------------------------------------------------------- -Create the shadow pen continuous vector environment with an explicit DexSim solver and packaged model inputs. +Shadow Hand continuous multi-pose pen-spinning environment (``ShadowPenContinuousRL-v1``). .. currentmodule:: embodichain_tasks.manipulation.dexterous.shadow_pen_continuous .. autosummary:: - make_env + ShadowPenContinuousEnv -.. autofunction:: make_env +.. autoclass:: ShadowPenContinuousEnv + :members: embodichain_tasks.manipulation.dexterous.shadow_pen_fixed --------------------------------------------------------- -Create the shadow pen fixed vector environment with an explicit DexSim solver and packaged model inputs. +Shadow Hand fixed-grasp pen-spinning environment (``ShadowPenFixedRL-v1``). .. currentmodule:: embodichain_tasks.manipulation.dexterous.shadow_pen_fixed .. autosummary:: - make_env + ShadowPenFixedEnv -.. autofunction:: make_env +.. 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 index 7a9d9169a0..9c44c09ace 100644 --- a/docs/source/overview/rl/dexterous.md +++ b/docs/source/overview/rl/dexterous.md @@ -1,79 +1,51 @@ # Pen and cube manipulation -Five vector tasks use the standard learning-environment interface and PPO trainer. -Each deployment selects its solver explicitly and obtains models and initial -states through the EmbodiChain asset registry. +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` | `ShadowPenFixedDexSim-v1` | 18 | 87 / 87 | `ShadowHandPen` | -| `shadow_pen_continuous` | `ShadowPenContinuousDexSim-v1` | 18 | 87 / 87 | `ShadowHandPen` | -| `leap_cube_face_turn` | `LeapCubeFaceTurnDexSim-v1` | 16 | 65 / 65 | `LeapHandCubeFaceTurn` | -| `leap_cube_reorient` | `LeapCubeReorientDexSim-v1` | 16 | 57 / 128 | `LeapHandCube` | -| `leap_cube_rotate_z` | `LeapCubeRotateZAxisDexSim-v1` | 16 | 32 / 105 | `LeapHandCube` | - -Training configurations are under -`extensions/embodichain_tasks/configs/tasks/manipulation/dexterous//agents/`. -`ppo.mujoco_warp.yaml` and `ppo.dexuni.yaml` select the corresponding solver. -The default configurations run 32 environments for ten updates and save -checkpoints. For example: +| `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` | + +## Deployments and CLI Usage + +Each task directory under `extensions/embodichain_tasks/configs/tasks/manipulation/dexterous//` provides two catalog deployments: + +- `newton` (default): `env.newton.yaml` + `agents/ppo.newton.yaml`, running `solver_type: dexuni` (`use_mujoco_cpu: false`, GPU MuJoCo Warp CUDA with full scene joint ownership). +- `mjwarp`: `env.mjwarp.yaml` + `agents/ppo.mjwarp.yaml`, running `solver_type: mujoco_warp`. ```bash embodichain data download --name ShadowHandPen -embodichain train-rl --config extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/agents/ppo.mujoco_warp.yaml +embodichain show-task embodichain_tasks:shadow_pen_fixed +embodichain run-task --gym_config extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/env.newton.yaml +embodichain train-rl --config extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/agents/ppo.newton.yaml ``` -Increase `trainer.iterations` and `trainer.num_envs` for a longer experiment. -Policy and optimizer state are saved by the standard trainer. Checkpoint -evaluation uses the generated RUN manifest through `embodichain eval-policy`. -Continue training with `embodichain train-rl --config CONFIG --resume CHECKPOINT`. -Set `trainer.iterations` to the cumulative target, for example 15 after an initial -10 updates. Resume restores policy normalization, optimizer, scheduler and update -counters, then starts fresh episodes. The shared schema-v2 loader checks task, -physics, policy and algorithm compatibility. Select a manifest checkpoint with -`--resume latest --run RUN_DIRECTORY`; `--resume-scheduler restart` starts the -configured schedule over the remaining updates while preserving optimizer -momentum. Legacy checkpoints without metadata emit a compatibility warning. -Each resumed invocation writes a new RUN directory. - -The fixed and continuous pen tasks share their model. Continuous spinning -samples from 22 retained initial poses. Cube face turning retains the passive -top-layer joint and updates its detent on every physics substep. Reorientation -and Z-axis rotation use separate actor and critic observation groups. -Selective reset preserves unselected physical rows; completed rows return the -next initial observation with terminal reward and done flags. - -Evaluation reports `eval/success_rate` for pen spinning, cube face turning and -cube reorientation. An episode succeeds if it reaches at least one task success -event before termination or truncation, including an event on the final step. -Pen spinning requires one complete positive revolution while retaining the pen; -face turning requires holding the target face angle for 20 steps; reorientation -requires reaching the target orientation. The environment retains these events -in `info["success"]` until the corresponding episode resets. Per-step events -remain available in `info["metrics"]["success"]`. - -Continuous Z-axis rotation has no binary success criterion. Its success rate is -unavailable; evaluate `rotation`, `angular_velocity_z`, `retained`, episode length -and return instead. A numeric zero success rate for the other tasks means no -evaluated episode met its success criterion. Short training verifies execution -and checkpoint compatibility, not policy convergence. - -SceneBuilder loads the packaged `usd/*.usdc` physics models and replicates them -for each environment. MJCF remains in the asset packages as task metadata and -an independent model-audit reference. Joint targets and passive face torque use -public articulation batches. The assets require DexSim's retained MuJoCo scene -metadata support, including fixed tendons, explicit filters and actuator limits. - -Model audits compare masses, inertia, actuator and tendon parameters, and the -complete effective collision graph against the source MJCF. They read active -device parameters because Newton notifications can leave the host model stale. -Both solver configurations retain source iteration settings unless explicitly -overridden. Reset still uses a version-specific MuJoCo coordinate bridge and, -for DexUni, compact-to-scene maps. Local validation versions and asset publication -status must accompany a release; short-run checkpoints are not converged policies. - -The face-turn task reserves 2048 constraint rows per world. A 512-row budget -can truncate constraints during large-batch training. Solver iteration limits -remain distinct from capacity limits: source defaults for LEAP tasks use -5 solver iterations and 8 line-search iterations on MJWarp; increase the -iteration budget explicitly when validating convergence. +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`. Source MJCF files in the asset archives are used only by offline verification tests (`_audit_model.py`, `_audit_tendons.py`) to verify mass, inertia, joint, actuator, tendon, and collision-graph parity against active GPU solver buffers. + +`LeapCubeFaceTurnRL-v1` reserves `njmax: 2048` constraint rows per world (`512` for the remaining tasks). Both `newton` (`dexuni`) and `mjwarp` (`mujoco_warp`) deployments preserve authored solver settings (`20/50` Newton/line-search iterations on Shadow Hand tasks; `5/8` on LEAP Hand tasks) and set `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 the default `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/embodichain/lab/sim/sim_manager.py b/embodichain/lab/sim/sim_manager.py index b211809a1a..eb42f8ff78 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 "default_plane" in self._spawn_scene 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 @@ -3012,6 +3017,24 @@ 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 + def set_default_global_lighting(self) -> None: """Set default global lighting for the scene. @@ -6922,6 +6945,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/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/README.md b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/README.md index ae4a33bd95..d79fb519d1 100644 --- a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/README.md +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/README.md @@ -1,96 +1,28 @@ -# Dexterous manipulation with USD assets +# Dexterous Manipulation Tasks (USD-Native `EmbodiedEnv`) -The five tasks construct their physical scenes from USD. The source MJCF in each -archive supplies task metadata and the independent physical-model audit. Initial -states, meshes, textures, provenance and licenses are included in the same ZIP. +All five dexterous manipulation RL tasks construct their runtime simulation scenes exclusively from USD (`.usdc`) via `SimulationManager.add_usd()` on `EmbodiedEnv`. Runtime execution has zero dependency on MJCF `.xml` files; source MJCF files included in the asset archives are retained solely for offline physical-model and tendon parity audits. -| Task | Archive | USD | MJCF | -|---|---|---|---| -| Fixed and continuous pen spinning | ShadowHandPen | `usd/shadow_pen.usdc` | `shadow_pen.xml` | -| Cube face turning | LeapHandCubeFaceTurn | `usd/leap_cube_top_layer.usdc` | `leap_cube_top_layer.xml` | -| Cube reorientation | LeapHandCube | `usd/M4.usdc` | `M4.xml` | -| Cube Z-axis rotation | LeapHandCube | `usd/M5.usdc` | `M5.xml` | +| Task key | Gym ID | Archive | Runtime USD | Initial state | +|---|---|---|---|---| +| `shadow_pen_fixed` | `ShadowPenFixedRL-v1` | `ShadowHandPen` | `usd/shadow_pen.usdc` | `ShadowPenSpin-initial.npz` | +| `shadow_pen_continuous` | `ShadowPenContinuousRL-v1` | `ShadowHandPen` | `usd/shadow_pen.usdc` | `ShadowPenSpin-initial.npz` | +| `leap_cube_face_turn` | `LeapCubeFaceTurnRL-v1` | `LeapHandCubeFaceTurn` | `usd/leap_cube_top_layer.usdc` | `LeapCubeFaceTurn-initial.npz` | +| `leap_cube_reorient` | `LeapCubeReorientRL-v1` | `LeapHandCube` | `usd/M4.usdc` | `M4-initial.npz` | +| `leap_cube_rotate_z` | `LeapCubeRotateZAxisRL-v1` | `LeapHandCube` | `usd/M5.usdc` | `M5-initial.npz` | -The archives are published under -[robot_assets](https://huggingface.co/datasets/DexForceAI/embodichain_data/tree/e1ebe53462d6f48b5961018e625b7c1304e9e83a/robot_assets) -at revision `e1ebe53462d6f48b5961018e625b7c1304e9e83a`. -The asset descriptors verify the archive content and refresh older cached ZIPs. +The asset archives are published under [`DexForceAI/embodichain_data/robot_assets`](https://huggingface.co/datasets/DexForceAI/embodichain_data/tree/e1ebe53462d6f48b5961018e625b7c1304e9e83a/robot_assets) and verified by SHA-256 checksums in `embodichain.data.assets.robot_assets`. -## Download and exercise the task interfaces +## Deployments and CLI Usage -Use Python 3.11 with the project dependencies installed, a CUDA device, and a -compatible development engine build with USD scene metadata, mocap frames and -CUDA graph state preservation. Run the following commands from the repository -root. The `ppo.dexuni.yaml` configurations assign the hand and free rigid bodies -to MuJoCo. Each task runs in a separate process with four environments and eight -control steps, including a selective reset of world 0. +Every task ships two deployments in its catalog: -The expected output contains five `TASK_RESULT` records with `status: passed`. -Checks cover finite observations/rewards, output shapes and dtypes, selective -reset isolation, the solver route, CUDA graph capture and the MJCF model audit. -The script uses `EMBODICHAIN_DATA_ROOT` when set; otherwise it creates a temporary -empty asset cache. It can be stopped with Ctrl-C. +- `newton` (default, recommended): `env.newton.yaml` + `agents/ppo.newton.yaml`, using `solver_type: dexuni` (`use_mujoco_cpu: false`, GPU MuJoCo Warp CUDA with full scene joint ownership so tendons, fixed-base hands, and free/hinged objects step inside a single CUDA-graphed solver). +- `mjwarp`: `env.mjwarp.yaml` + `agents/ppo.mjwarp.yaml`, using `solver_type: mujoco_warp`. ```bash -#!/usr/bin/env bash -set -euo pipefail -export EMBODICHAIN_DOWNLOAD_PREFIX=https://huggingface.co/datasets/DexForceAI/embodichain_data/resolve/e1ebe53462d6f48b5961018e625b7c1304e9e83a/ -export EMBODICHAIN_DATA_ROOT="${EMBODICHAIN_DATA_ROOT:-$(mktemp -d -t manipulation-assets.XXXXXX)}" -export EMBODICHAIN_SIM_EXIT_PROCESS=0 -for asset in ShadowHandPen LeapHandCubeFaceTurn LeapHandCube; do - python -m embodichain data download --name "$asset" -done -for task in shadow_pen_fixed shadow_pen_continuous leap_cube_face_turn leap_cube_reorient leap_cube_rotate_z; do - python - "$task" <<'PY' -import importlib -import json -import sys -from pathlib import Path -import torch -import yaml -from embodichain.learning.rl.env import build_learning_env - -name = sys.argv[1] -importlib.import_module(f"embodichain_tasks.manipulation.dexterous.{name}") -config = Path("extensions/embodichain_tasks/configs/tasks/manipulation/dexterous") / name / "agents/ppo.dexuni.yaml" -trainer = yaml.safe_load(config.read_text())["trainer"] -spec = trainer["learning_env"] -torch.set_num_threads(4) -env = build_learning_env(spec["name"], num_envs=4, device=trainer["device"], seed=trainer["seed"], **spec["cfg"]) -try: - obs, _ = env.reset(seed=trainer["seed"]) - for step in range(8): - if step == 4: - previous = {key: value[1:].clone() for key, value in obs.items()} - obs, _ = env.reset(options={"reset_ids": torch.tensor([0], device=env.device)}) - assert all(torch.equal(obs[key][1:], value) for key, value in previous.items()) - action = .15 * torch.sin(torch.arange(env.action_dim, device=env.device)[None, :] + torch.arange(4, device=env.device)[:, None] * .13 + step * .3) - obs, reward, terminated, truncated, _ = env.step(action) - assert set(obs) == set(env.single_observation_space.spaces) - for key, value in obs.items(): - assert value.shape == (4, *env.single_observation_space.spaces[key].shape) - assert torch.isfinite(value).all() - assert reward.shape == (4,) and torch.isfinite(reward).all() - assert terminated.shape == truncated.shape == (4,) - assert terminated.dtype == truncated.dtype == torch.bool - assert env.backend.solver.features.backend == "pure_mujoco" - assert env.backend._graph is not None - assert env.model_audit["physical_model"]["status"] == "passed" - print("TASK_RESULT " + json.dumps({"task": name, "status": "passed", "num_envs": 4, "control_steps": 8, "usd": str(env.usd), "mjcf": str(env.xml), "solver": "pure_mujoco", "cuda_graph": True, "selective_reset": "passed", "model_audit": "passed"}), flush=True) -finally: - env.close() -PY -done +embodichain list-task +embodichain show-task embodichain_tasks:shadow_pen_fixed +embodichain run-task --gym_config extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/env.newton.yaml +embodichain train-rl --config extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/agents/ppo.newton.yaml +embodichain eval-policy runs/ --checkpoint latest --num-envs 64 --episodes 64 --renderer no-render ``` - -## Training configurations - -Each task's `agents/ppo.dexuni.yaml` selects all-MuJoCo DexUni; -`agents/ppo.mujoco_warp.yaml` selects Newton/MJWarp. For example: - -```bash -python -m embodichain train-rl --config extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/agents/ppo.dexuni.yaml -``` - -The short interface check above exercises loading, reset and stepping. Sustained -training, checkpoint resume and policy evaluation have separate validation results. 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..ad64f42143 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_face_turn/README.md @@ -0,0 +1,28 @@ +# 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`. + +| Deployment | Environment | PPO configuration | +|---|---|---| +| `newton` (DexUni, default) | [env.newton.yaml](env.newton.yaml) | [agents/ppo.newton.yaml](agents/ppo.newton.yaml) | +| `mjwarp` (MuJoCo Warp) | [env.mjwarp.yaml](env.mjwarp.yaml) | [agents/ppo.mjwarp.yaml](agents/ppo.mjwarp.yaml) | + +The scene is loaded directly from LeapHandCubeFaceTurn (`usd/leap_cube_top_layer.usdc`) via `SimulationManager.add_usd()`. +`newton` selects `solver_type: dexuni` and dispatches all scene articulations and free bodies to GPU MuJoCo Warp on CUDA (`use_mujoco_cpu: false`, `use_mujoco_contacts: true`); `mjwarp` selects `solver_type: mujoco_warp` directly. + +```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.newton.yaml +embodichain train-rl --config extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_face_turn/agents/ppo.newton.yaml +``` + +## Evaluation + +Evaluate a saved checkpoint with its training run directory: + +```bash +embodichain eval-policy outputs/leap_cube_face_turn_ppo_newton/ --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.dexuni.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_face_turn/agents/ppo.mjwarp.yaml similarity index 54% rename from extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_face_turn/agents/ppo.dexuni.yaml rename to extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_face_turn/agents/ppo.mjwarp.yaml index 703b1ae2ca..e992e21741 100644 --- a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_face_turn/agents/ppo.dexuni.yaml +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_face_turn/agents/ppo.mjwarp.yaml @@ -1,42 +1,42 @@ trainer: - exp_name: leap_cube_face_turn_dexuni - learning_env: - name: LeapCubeFaceTurnDexSim-v1 - cfg: - solver_kind: dexuni - cuda_graph: true + exp_name: leap_cube_face_turn_ppo_mjwarp + gym_config: extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_face_turn/env.mjwarp.yaml seed: 42 device: cuda:0 - num_envs: 32 - iterations: 10 - buffer_size: 24 + headless: true + num_envs: 4096 + iterations: 430 + buffer_size: 40 enable_eval: false - save_freq: 768 + save_frequency_updates: 100 use_wandb: false + wandb_project_name: embodichain-dexterous policy: name: actor_critic actor_obs_normalization: true critic_obs_normalization: true - initial_log_std: -0.693147 + obs_groups: + actor: + - policy + critic: + - policy actor: type: mlp network_cfg: hidden_sizes: + - 512 - 256 - - 256 + - 128 activation: elu critic: type: mlp network_cfg: hidden_sizes: + - 512 - 256 - - 256 + - 128 activation: elu - obs_groups: - actor: - - obs - critic: - - obs + initial_action_std: 0.5 algorithm: name: ppo cfg: @@ -44,10 +44,12 @@ algorithm: name: adam learning_rate: 0.0003 n_epochs: 4 - batch_size: 256 + num_mini_batches: 32 + batch_size: 5120 gamma: 0.99 gae_lambda: 0.95 - clip_coef: 0.2 + 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/agents/ppo.mujoco_warp.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_face_turn/agents/ppo.newton.yaml similarity index 54% rename from extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_face_turn/agents/ppo.mujoco_warp.yaml rename to extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_face_turn/agents/ppo.newton.yaml index 11202c6af7..69bce22722 100644 --- a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_face_turn/agents/ppo.mujoco_warp.yaml +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_face_turn/agents/ppo.newton.yaml @@ -1,42 +1,42 @@ trainer: - exp_name: leap_cube_face_turn_mujoco_warp - learning_env: - name: LeapCubeFaceTurnDexSim-v1 - cfg: - solver_kind: mujoco_warp - cuda_graph: true + exp_name: leap_cube_face_turn_ppo_newton + gym_config: extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_face_turn/env.newton.yaml seed: 42 device: cuda:0 - num_envs: 32 - iterations: 10 - buffer_size: 24 + headless: true + num_envs: 4096 + iterations: 430 + buffer_size: 40 enable_eval: false - save_freq: 768 + save_frequency_updates: 100 use_wandb: false + wandb_project_name: embodichain-dexterous policy: name: actor_critic actor_obs_normalization: true critic_obs_normalization: true - initial_log_std: -0.693147 + obs_groups: + actor: + - policy + critic: + - policy actor: type: mlp network_cfg: hidden_sizes: + - 512 - 256 - - 256 + - 128 activation: elu critic: type: mlp network_cfg: hidden_sizes: + - 512 - 256 - - 256 + - 128 activation: elu - obs_groups: - actor: - - obs - critic: - - obs + initial_action_std: 0.5 algorithm: name: ppo cfg: @@ -44,10 +44,12 @@ algorithm: name: adam learning_rate: 0.0003 n_epochs: 4 - batch_size: 256 + num_mini_batches: 32 + batch_size: 5120 gamma: 0.99 gae_lambda: 0.95 - clip_coef: 0.2 + 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..539efb1e43 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_face_turn/catalog.yaml @@ -0,0 +1,14 @@ +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: newton +deployments: + newton: + config: env.newton.yaml + mjwarp: + config: env.mjwarp.yaml diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_face_turn/env.mjwarp.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_face_turn/env.mjwarp.yaml new file mode 100644 index 0000000000..b6d469d8a9 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_face_turn/env.mjwarp.yaml @@ -0,0 +1,55 @@ +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: mujoco_warp + 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_face_turn/env.newton.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_face_turn/env.newton.yaml new file mode 100644 index 0000000000..1688c0d02f --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_face_turn/env.newton.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..3e8195c9ac --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_reorient/README.md @@ -0,0 +1,28 @@ +# 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`. + +| Deployment | Environment | PPO configuration | +|---|---|---| +| `newton` (DexUni, default) | [env.newton.yaml](env.newton.yaml) | [agents/ppo.newton.yaml](agents/ppo.newton.yaml) | +| `mjwarp` (MuJoCo Warp) | [env.mjwarp.yaml](env.mjwarp.yaml) | [agents/ppo.mjwarp.yaml](agents/ppo.mjwarp.yaml) | + +The scene is loaded directly from LeapHandCube (`usd/M4.usdc`) via `SimulationManager.add_usd()`. +`newton` selects `solver_type: dexuni` and dispatches all scene articulations and free bodies to GPU MuJoCo Warp on CUDA (`use_mujoco_cpu: false`, `use_mujoco_contacts: true`); `mjwarp` selects `solver_type: mujoco_warp` directly. + +```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.newton.yaml +embodichain train-rl --config extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_reorient/agents/ppo.newton.yaml +``` + +## Evaluation + +Evaluate a saved checkpoint with its training run directory: + +```bash +embodichain eval-policy outputs/leap_cube_reorient_ppo_newton/ --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.mjwarp.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_reorient/agents/ppo.mjwarp.yaml new file mode 100644 index 0000000000..3110238c85 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_reorient/agents/ppo.mjwarp.yaml @@ -0,0 +1,59 @@ +trainer: + exp_name: leap_cube_reorient_ppo_mjwarp + gym_config: extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_reorient/env.mjwarp.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/agents/ppo.newton.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_reorient/agents/ppo.newton.yaml new file mode 100644 index 0000000000..3ed19fbb43 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_reorient/agents/ppo.newton.yaml @@ -0,0 +1,59 @@ +trainer: + exp_name: leap_cube_reorient_ppo_newton + gym_config: extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_reorient/env.newton.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..046f680a6c --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_reorient/catalog.yaml @@ -0,0 +1,14 @@ +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: newton +deployments: + newton: + config: env.newton.yaml + mjwarp: + config: env.mjwarp.yaml diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_reorient/env.mjwarp.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_reorient/env.mjwarp.yaml new file mode 100644 index 0000000000..fd70d478c6 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_reorient/env.mjwarp.yaml @@ -0,0 +1,55 @@ +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: mujoco_warp + 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_reorient/env.newton.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_reorient/env.newton.yaml new file mode 100644 index 0000000000..ee3814f23a --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_reorient/env.newton.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..108b315bd0 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_rotate_z/README.md @@ -0,0 +1,28 @@ +# 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`. + +| Deployment | Environment | PPO configuration | +|---|---|---| +| `newton` (DexUni, default) | [env.newton.yaml](env.newton.yaml) | [agents/ppo.newton.yaml](agents/ppo.newton.yaml) | +| `mjwarp` (MuJoCo Warp) | [env.mjwarp.yaml](env.mjwarp.yaml) | [agents/ppo.mjwarp.yaml](agents/ppo.mjwarp.yaml) | + +The scene is loaded directly from LeapHandCube (`usd/M5.usdc`) via `SimulationManager.add_usd()`. +`newton` selects `solver_type: dexuni` and dispatches all scene articulations and free bodies to GPU MuJoCo Warp on CUDA (`use_mujoco_cpu: false`, `use_mujoco_contacts: true`); `mjwarp` selects `solver_type: mujoco_warp` directly. + +```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.newton.yaml +embodichain train-rl --config extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_rotate_z/agents/ppo.newton.yaml +``` + +## Evaluation + +Evaluate a saved checkpoint with its training run directory: + +```bash +embodichain eval-policy outputs/leap_cube_rotate_z_ppo_newton/ --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.dexuni.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_rotate_z/agents/ppo.dexuni.yaml deleted file mode 100644 index 7353f2e49f..0000000000 --- a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_rotate_z/agents/ppo.dexuni.yaml +++ /dev/null @@ -1,53 +0,0 @@ -trainer: - exp_name: leap_cube_rotate_z_dexuni - learning_env: - name: LeapCubeRotateZAxisDexSim-v1 - cfg: - solver_kind: dexuni - cuda_graph: true - seed: 42 - device: cuda:0 - num_envs: 32 - iterations: 10 - buffer_size: 24 - enable_eval: false - save_freq: 768 - use_wandb: false -policy: - name: actor_critic - actor_obs_normalization: true - critic_obs_normalization: true - initial_log_std: -0.693147 - actor: - type: mlp - network_cfg: - hidden_sizes: - - 256 - - 256 - activation: elu - critic: - type: mlp - network_cfg: - hidden_sizes: - - 256 - - 256 - activation: elu - obs_groups: - actor: - - obs - critic: - - critic_obs -algorithm: - name: ppo - cfg: - optimizer: - name: adam - learning_rate: 0.0003 - n_epochs: 4 - batch_size: 256 - gamma: 0.99 - gae_lambda: 0.95 - clip_coef: 0.2 - ent_coef: 0.01 - vf_coef: 0.5 - max_grad_norm: 1.0 diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_reorient/agents/ppo.dexuni.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_rotate_z/agents/ppo.mjwarp.yaml similarity index 53% rename from extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_reorient/agents/ppo.dexuni.yaml rename to extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_rotate_z/agents/ppo.mjwarp.yaml index 35ec3b5f33..26f114984e 100644 --- a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_reorient/agents/ppo.dexuni.yaml +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_rotate_z/agents/ppo.mjwarp.yaml @@ -1,42 +1,42 @@ trainer: - exp_name: leap_cube_reorient_dexuni - learning_env: - name: LeapCubeReorientDexSim-v1 - cfg: - solver_kind: dexuni - cuda_graph: true + exp_name: leap_cube_rotate_z_ppo_mjwarp + gym_config: extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_rotate_z/env.mjwarp.yaml seed: 42 device: cuda:0 - num_envs: 32 - iterations: 10 - buffer_size: 24 + headless: true + num_envs: 4096 + iterations: 650 + buffer_size: 40 enable_eval: false - save_freq: 768 + save_frequency_updates: 100 use_wandb: false + wandb_project_name: embodichain-dexterous policy: name: actor_critic actor_obs_normalization: true critic_obs_normalization: true - initial_log_std: -0.693147 + obs_groups: + actor: + - policy + critic: + - critic actor: type: mlp network_cfg: hidden_sizes: + - 512 - 256 - - 256 + - 128 activation: elu critic: type: mlp network_cfg: hidden_sizes: + - 512 - 256 - - 256 + - 128 activation: elu - obs_groups: - actor: - - obs - critic: - - critic_obs + initial_action_std: 0.5 algorithm: name: ppo cfg: @@ -44,10 +44,12 @@ algorithm: name: adam learning_rate: 0.0003 n_epochs: 4 - batch_size: 256 - gamma: 0.99 + num_mini_batches: 32 + batch_size: 5120 + gamma: 0.97 gae_lambda: 0.95 - clip_coef: 0.2 + 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/agents/ppo.mujoco_warp.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_rotate_z/agents/ppo.mujoco_warp.yaml deleted file mode 100644 index 214e25f61d..0000000000 --- a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_rotate_z/agents/ppo.mujoco_warp.yaml +++ /dev/null @@ -1,53 +0,0 @@ -trainer: - exp_name: leap_cube_rotate_z_mujoco_warp - learning_env: - name: LeapCubeRotateZAxisDexSim-v1 - cfg: - solver_kind: mujoco_warp - cuda_graph: true - seed: 42 - device: cuda:0 - num_envs: 32 - iterations: 10 - buffer_size: 24 - enable_eval: false - save_freq: 768 - use_wandb: false -policy: - name: actor_critic - actor_obs_normalization: true - critic_obs_normalization: true - initial_log_std: -0.693147 - actor: - type: mlp - network_cfg: - hidden_sizes: - - 256 - - 256 - activation: elu - critic: - type: mlp - network_cfg: - hidden_sizes: - - 256 - - 256 - activation: elu - obs_groups: - actor: - - obs - critic: - - critic_obs -algorithm: - name: ppo - cfg: - optimizer: - name: adam - learning_rate: 0.0003 - n_epochs: 4 - batch_size: 256 - gamma: 0.99 - gae_lambda: 0.95 - clip_coef: 0.2 - ent_coef: 0.01 - vf_coef: 0.5 - max_grad_norm: 1.0 diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_reorient/agents/ppo.mujoco_warp.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_rotate_z/agents/ppo.newton.yaml similarity index 53% rename from extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_reorient/agents/ppo.mujoco_warp.yaml rename to extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_rotate_z/agents/ppo.newton.yaml index 0647ed5d18..83cf0133b6 100644 --- a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_reorient/agents/ppo.mujoco_warp.yaml +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_rotate_z/agents/ppo.newton.yaml @@ -1,42 +1,42 @@ trainer: - exp_name: leap_cube_reorient_mujoco_warp - learning_env: - name: LeapCubeReorientDexSim-v1 - cfg: - solver_kind: mujoco_warp - cuda_graph: true + exp_name: leap_cube_rotate_z_ppo_newton + gym_config: extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_rotate_z/env.newton.yaml seed: 42 device: cuda:0 - num_envs: 32 - iterations: 10 - buffer_size: 24 + headless: true + num_envs: 4096 + iterations: 650 + buffer_size: 40 enable_eval: false - save_freq: 768 + save_frequency_updates: 100 use_wandb: false + wandb_project_name: embodichain-dexterous policy: name: actor_critic actor_obs_normalization: true critic_obs_normalization: true - initial_log_std: -0.693147 + obs_groups: + actor: + - policy + critic: + - critic actor: type: mlp network_cfg: hidden_sizes: + - 512 - 256 - - 256 + - 128 activation: elu critic: type: mlp network_cfg: hidden_sizes: + - 512 - 256 - - 256 + - 128 activation: elu - obs_groups: - actor: - - obs - critic: - - critic_obs + initial_action_std: 0.5 algorithm: name: ppo cfg: @@ -44,10 +44,12 @@ algorithm: name: adam learning_rate: 0.0003 n_epochs: 4 - batch_size: 256 - gamma: 0.99 + num_mini_batches: 32 + batch_size: 5120 + gamma: 0.97 gae_lambda: 0.95 - clip_coef: 0.2 + 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..5cbb49bc56 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_rotate_z/catalog.yaml @@ -0,0 +1,14 @@ +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: newton +deployments: + newton: + config: env.newton.yaml + mjwarp: + config: env.mjwarp.yaml diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_rotate_z/env.mjwarp.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_rotate_z/env.mjwarp.yaml new file mode 100644 index 0000000000..2b9df70b46 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_rotate_z/env.mjwarp.yaml @@ -0,0 +1,55 @@ +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: mujoco_warp + 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/env.newton.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_rotate_z/env.newton.yaml new file mode 100644 index 0000000000..c71e08a003 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_rotate_z/env.newton.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..586ceb4b42 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/README.md @@ -0,0 +1,28 @@ +# Shadow Hand Continuous Pen Spin + +Continuously spin a pen through sequential orientation targets with the 24-DOF Shadow Hand. +Gym ID: `ShadowPenContinuousRL-v1`. + +| Deployment | Environment | PPO configuration | +|---|---|---| +| `newton` (DexUni, default) | [env.newton.yaml](env.newton.yaml) | [agents/ppo.newton.yaml](agents/ppo.newton.yaml) | +| `mjwarp` (MuJoCo Warp) | [env.mjwarp.yaml](env.mjwarp.yaml) | [agents/ppo.mjwarp.yaml](agents/ppo.mjwarp.yaml) | + +The scene is loaded directly from ShadowHandPen (`usd/shadow_pen.usdc`) via `SimulationManager.add_usd()`. +`newton` selects `solver_type: dexuni` and dispatches all scene articulations and free bodies to GPU MuJoCo Warp on CUDA (`use_mujoco_cpu: false`, `use_mujoco_contacts: true`); `mjwarp` selects `solver_type: mujoco_warp` directly. + +```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.newton.yaml +embodichain train-rl --config extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/agents/ppo.newton.yaml +``` + +## Evaluation + +Evaluate a saved checkpoint with its training run directory: + +```bash +embodichain eval-policy outputs/shadow_pen_continuous_ppo_newton/ --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.dexuni.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/agents/ppo.dexuni.yaml deleted file mode 100644 index 97a793868b..0000000000 --- a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/agents/ppo.dexuni.yaml +++ /dev/null @@ -1,53 +0,0 @@ -trainer: - exp_name: shadow_pen_continuous_dexuni - learning_env: - name: ShadowPenContinuousDexSim-v1 - cfg: - solver_kind: dexuni - cuda_graph: true - seed: 42 - device: cuda:0 - num_envs: 32 - iterations: 10 - buffer_size: 24 - enable_eval: false - save_freq: 768 - use_wandb: false -policy: - name: actor_critic - actor_obs_normalization: true - critic_obs_normalization: true - initial_log_std: -0.693147 - actor: - type: mlp - network_cfg: - hidden_sizes: - - 256 - - 256 - activation: elu - critic: - type: mlp - network_cfg: - hidden_sizes: - - 256 - - 256 - activation: elu - obs_groups: - actor: - - obs - critic: - - obs -algorithm: - name: ppo - cfg: - optimizer: - name: adam - learning_rate: 0.0003 - n_epochs: 4 - batch_size: 256 - gamma: 0.99 - gae_lambda: 0.95 - clip_coef: 0.2 - ent_coef: 0.01 - vf_coef: 0.5 - max_grad_norm: 1.0 diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/agents/ppo.mjwarp.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/agents/ppo.mjwarp.yaml new file mode 100644 index 0000000000..64e8e63c16 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/agents/ppo.mjwarp.yaml @@ -0,0 +1,59 @@ +trainer: + exp_name: shadow_pen_continuous_ppo_mjwarp + gym_config: extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/env.mjwarp.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/agents/ppo.mujoco_warp.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/agents/ppo.mujoco_warp.yaml deleted file mode 100644 index d5a1b77ccb..0000000000 --- a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/agents/ppo.mujoco_warp.yaml +++ /dev/null @@ -1,53 +0,0 @@ -trainer: - exp_name: shadow_pen_continuous_mujoco_warp - learning_env: - name: ShadowPenContinuousDexSim-v1 - cfg: - solver_kind: mujoco_warp - cuda_graph: true - seed: 42 - device: cuda:0 - num_envs: 32 - iterations: 10 - buffer_size: 24 - enable_eval: false - save_freq: 768 - use_wandb: false -policy: - name: actor_critic - actor_obs_normalization: true - critic_obs_normalization: true - initial_log_std: -0.693147 - actor: - type: mlp - network_cfg: - hidden_sizes: - - 256 - - 256 - activation: elu - critic: - type: mlp - network_cfg: - hidden_sizes: - - 256 - - 256 - activation: elu - obs_groups: - actor: - - obs - critic: - - obs -algorithm: - name: ppo - cfg: - optimizer: - name: adam - learning_rate: 0.0003 - n_epochs: 4 - batch_size: 256 - gamma: 0.99 - gae_lambda: 0.95 - clip_coef: 0.2 - ent_coef: 0.01 - vf_coef: 0.5 - max_grad_norm: 1.0 diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/agents/ppo.newton.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/agents/ppo.newton.yaml new file mode 100644 index 0000000000..e7fb589a22 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/agents/ppo.newton.yaml @@ -0,0 +1,59 @@ +trainer: + exp_name: shadow_pen_continuous_ppo_newton + gym_config: extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/env.newton.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..3ec27fc75f --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/catalog.yaml @@ -0,0 +1,14 @@ +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: newton +deployments: + newton: + config: env.newton.yaml + mjwarp: + config: env.mjwarp.yaml diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/env.mjwarp.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/env.mjwarp.yaml new file mode 100644 index 0000000000..e45d3e1770 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/env.mjwarp.yaml @@ -0,0 +1,55 @@ +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: mujoco_warp + 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_continuous/env.newton.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/env.newton.yaml new file mode 100644 index 0000000000..d90c8a4324 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/env.newton.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..fc5e38bf0e --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/README.md @@ -0,0 +1,28 @@ +# 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`. + +| Deployment | Environment | PPO configuration | +|---|---|---| +| `newton` (DexUni, default) | [env.newton.yaml](env.newton.yaml) | [agents/ppo.newton.yaml](agents/ppo.newton.yaml) | +| `mjwarp` (MuJoCo Warp) | [env.mjwarp.yaml](env.mjwarp.yaml) | [agents/ppo.mjwarp.yaml](agents/ppo.mjwarp.yaml) | + +The scene is loaded directly from ShadowHandPen (`usd/shadow_pen.usdc`) via `SimulationManager.add_usd()`. +`newton` selects `solver_type: dexuni` and dispatches all scene articulations and free bodies to GPU MuJoCo Warp on CUDA (`use_mujoco_cpu: false`, `use_mujoco_contacts: true`); `mjwarp` selects `solver_type: mujoco_warp` directly. + +```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.newton.yaml +embodichain train-rl --config extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/agents/ppo.newton.yaml +``` + +## Evaluation + +Evaluate a saved checkpoint with its training run directory: + +```bash +embodichain eval-policy outputs/shadow_pen_fixed_ppo_newton/ --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.dexuni.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/agents/ppo.dexuni.yaml deleted file mode 100644 index 306a3d2097..0000000000 --- a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/agents/ppo.dexuni.yaml +++ /dev/null @@ -1,53 +0,0 @@ -trainer: - exp_name: shadow_pen_fixed_dexuni - learning_env: - name: ShadowPenFixedDexSim-v1 - cfg: - solver_kind: dexuni - cuda_graph: true - seed: 42 - device: cuda:0 - num_envs: 32 - iterations: 10 - buffer_size: 24 - enable_eval: false - save_freq: 768 - use_wandb: false -policy: - name: actor_critic - actor_obs_normalization: true - critic_obs_normalization: true - initial_log_std: -0.693147 - actor: - type: mlp - network_cfg: - hidden_sizes: - - 256 - - 256 - activation: elu - critic: - type: mlp - network_cfg: - hidden_sizes: - - 256 - - 256 - activation: elu - obs_groups: - actor: - - obs - critic: - - obs -algorithm: - name: ppo - cfg: - optimizer: - name: adam - learning_rate: 0.0003 - n_epochs: 4 - batch_size: 256 - gamma: 0.99 - gae_lambda: 0.95 - clip_coef: 0.2 - ent_coef: 0.01 - vf_coef: 0.5 - max_grad_norm: 1.0 diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/agents/ppo.mjwarp.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/agents/ppo.mjwarp.yaml new file mode 100644 index 0000000000..d2d165ae66 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/agents/ppo.mjwarp.yaml @@ -0,0 +1,55 @@ +trainer: + exp_name: shadow_pen_fixed_ppo_mjwarp + gym_config: extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/env.mjwarp.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/agents/ppo.mujoco_warp.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/agents/ppo.mujoco_warp.yaml deleted file mode 100644 index cd71adda11..0000000000 --- a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/agents/ppo.mujoco_warp.yaml +++ /dev/null @@ -1,53 +0,0 @@ -trainer: - exp_name: shadow_pen_fixed_mujoco_warp - learning_env: - name: ShadowPenFixedDexSim-v1 - cfg: - solver_kind: mujoco_warp - cuda_graph: true - seed: 42 - device: cuda:0 - num_envs: 32 - iterations: 10 - buffer_size: 24 - enable_eval: false - save_freq: 768 - use_wandb: false -policy: - name: actor_critic - actor_obs_normalization: true - critic_obs_normalization: true - initial_log_std: -0.693147 - actor: - type: mlp - network_cfg: - hidden_sizes: - - 256 - - 256 - activation: elu - critic: - type: mlp - network_cfg: - hidden_sizes: - - 256 - - 256 - activation: elu - obs_groups: - actor: - - obs - critic: - - obs -algorithm: - name: ppo - cfg: - optimizer: - name: adam - learning_rate: 0.0003 - n_epochs: 4 - batch_size: 256 - gamma: 0.99 - gae_lambda: 0.95 - clip_coef: 0.2 - ent_coef: 0.01 - vf_coef: 0.5 - max_grad_norm: 1.0 diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/agents/ppo.newton.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/agents/ppo.newton.yaml new file mode 100644 index 0000000000..3eacfdc31d --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/agents/ppo.newton.yaml @@ -0,0 +1,55 @@ +trainer: + exp_name: shadow_pen_fixed_ppo_newton + gym_config: extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/env.newton.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..3dd482046b --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/catalog.yaml @@ -0,0 +1,14 @@ +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: newton +deployments: + newton: + config: env.newton.yaml + mjwarp: + config: env.mjwarp.yaml diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/env.mjwarp.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/env.mjwarp.yaml new file mode 100644 index 0000000000..ca38e382e8 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/env.mjwarp.yaml @@ -0,0 +1,55 @@ +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: mujoco_warp + 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/env.newton.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/env.newton.yaml new file mode 100644 index 0000000000..277fbc2249 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/env.newton.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/manipulation/dexterous/__init__.py b/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/__init__.py index a948cec2c5..d4b0816a0a 100644 --- a/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/__init__.py +++ b/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/__init__.py @@ -13,6 +13,19 @@ # See the License for the specific language governing permissions and # limitations under the License. # ---------------------------------------------------------------------------- + from __future__ import annotations -__all__: list[str] = [] +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/_dexuni_adapter.py b/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_dexuni_adapter.py deleted file mode 100644 index d2ec7fceba..0000000000 --- a/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_dexuni_adapter.py +++ /dev/null @@ -1,115 +0,0 @@ -# ---------------------------------------------------------------------------- -# 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. -# ---------------------------------------------------------------------------- -"""Select DexUni's rigid articulation branch and translate its reset state.""" - -from __future__ import annotations - -from typing import Any - -import torch -import warp as wp -from newton import Model -from dexsim.spawn import SceneDesc -from dexsim.types import ActorType - -__all__ = ["DexUniStateAdapter"] - - -def _mujoco_articulation_ids(scene: SceneDesc, num_envs: int) -> tuple[int, ...]: - """Select every rigid articulation in the replicated manipulation scene.""" - # Each non-static standalone body creates its own free or mocap articulation. - standalone = sum( - item.physics is not None - and item.physics.actor_type != ActorType.STATIC - and ( - bool(item.collisions) or bool(getattr(item.physics.newton, "mocap", False)) - ) - for item in scene.mesh_objects - ) - return tuple(range(num_envs * (len(scene.articulations) + standalone))) - - -class DexUniStateAdapter: - """Scatter private MuJoCo state into the owning DexSim state buffers. - - The compact-to-scene maps come from NewtonBackend's public facade. Physical - stepping remains owned by ``backend.step`` and the outer ``SolverDexUni``. - """ - - def __init__(self, backend: Any) -> None: - outer = backend.solver - if backend.solver_type != "dexuni" or type(outer).__name__ != "SolverDexUni": - raise TypeError("Requested DexUni did not create SolverDexUni") - if outer.features.backend != "pure_mujoco": - raise ValueError( - f"Expected DexUni pure_mujoco dispatch, got {outer.features.backend}" - ) - if outer.features.vbd_solve_enabled or not outer.features.mujoco_solve_enabled: - raise ValueError("Unexpected active DexUni solver branches") - self.backend = backend - self.solver = backend.mujoco_solver - if self.solver is None or not hasattr(self.solver, "mjw_data"): - raise TypeError("DexUni has no active GPU MuJoCo data") - if ".mjvbd_v2.mujoco." not in type(self.solver).__module__: - raise TypeError("DexUni did not select its private MuJoCo implementation") - self.local_state = self.solver.model.state() - parent = backend.model - joint_world = wp.to_torch(parent.joint_world).long() - coord_world = torch.repeat_interleave( - joint_world, torch.diff(wp.to_torch(parent.joint_q_start)).long() - ) - dof_world = torch.repeat_interleave( - joint_world, torch.diff(wp.to_torch(parent.joint_qd_start)).long() - ) - body_world = wp.to_torch(parent.body_world).long() - frequencies = ( - ("joint_q", Model.AttributeFrequency.JOINT_COORD, coord_world), - ("joint_qd", Model.AttributeFrequency.JOINT_DOF, dof_world), - ("body_q", Model.AttributeFrequency.BODY, body_world), - ("body_qd", Model.AttributeFrequency.BODY, body_world), - ) - self.fields = [] - for name, frequency, worlds in frequencies: - mapping = backend.get_mujoco_index_map(frequency) - if mapping is None: - raise ValueError(f"Missing DexUni {name} compact-to-scene map") - indices = wp.to_torch(mapping).long() - local = wp.to_torch(getattr(self.local_state, name)) - if local.shape[0] != indices.numel(): - raise ValueError(f"DexUni {name} state and index map disagree") - self.fields.append((name, indices, worlds[indices])) - - def publish_reset(self, data: Any, env_ids: torch.Tensor) -> None: - """Publish selected MuJoCo coordinate resets into both scene buffers.""" - selected = [ - (name, indices, torch.isin(worlds, env_ids)) - for name, indices, worlds in self.fields - ] - for parent_state in (self.backend.state_0, self.backend.state_1): - for name, indices, _ in selected: - wp.to_torch(getattr(self.local_state, name)).copy_( - wp.to_torch(getattr(parent_state, name))[indices] - ) - self.solver._update_newton_state( - self.solver.model, - self.local_state, - data, - state_prev=self.local_state, - ) - for name, indices, mask in selected: - wp.to_torch(getattr(parent_state, name))[indices[mask]] = wp.to_torch( - getattr(self.local_state, name) - )[mask] diff --git a/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_runtime.py b/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_runtime.py index 57956c7485..aafb52e68d 100644 --- a/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_runtime.py +++ b/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_runtime.py @@ -14,702 +14,825 @@ # limitations under the License. # ---------------------------------------------------------------------------- -"""Batched manipulation environments stepped by the DexSim Newton backend.""" +"""USD-native dexterous manipulation environments built on :class:`EmbodiedEnv`.""" from __future__ import annotations -import copy +from collections.abc import Callable, Sequence +from dataclasses import dataclass +import math from pathlib import Path -import re from typing import Any import gymnasium as gym -import mujoco import numpy as np +from tensordict import TensorDict import torch -import warp as wp -import dexsim +from dexsim.engine.newton_physics.runtime_control import GraphRuntimeControl from embodichain.data import get_data_path -from dexsim.engine.newton_physics import DexUniSolverCfg, MJWarpSolverCfg, NewtonCfg -from dexsim.engine.newton_physics.backend_registry import get_newton_backend - +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__ = ["DexSimManipulationEnv", "ENV_NAMES"] +__all__ = ["DexterousManipulationEnv", "ENV_NAMES", "TASK_SPECS", "rotate_wxyz"] -ENV_NAMES = { - "M1": "ShadowPenFixedDexSim-v1", - "M2": "ShadowPenContinuousDexSim-v1", - "M3": "LeapCubeFaceTurnDexSim-v1", - "M4": "LeapCubeReorientDexSim-v1", - "M5": "LeapCubeRotateZAxisDexSim-v1", +ENV_NAMES: dict[str, str] = { + "M1": "ShadowPenFixedRL-v1", + "M2": "ShadowPenContinuousRL-v1", + "M3": "LeapCubeFaceTurnRL-v1", + "M4": "LeapCubeReorientRL-v1", + "M5": "LeapCubeRotateZAxisRL-v1", } -def _named_index( - model: Any, kind: str, name: str, *, default: int | None = None -) -> int: - count = getattr(model, "n" + ("jnt" if kind == "joint" else kind)) - candidates = [ - i - for i in range(count) - if getattr(model, kind)(i).name == name - or getattr(model, kind)(i).name.endswith("_" + name) - or getattr(model, kind)(i).name.rsplit("/", 1)[-1] == name - or ( - kind == "geom" - and re.sub(r"_\d+$", "", getattr(model, kind)(i).name).split("/")[-1] - == name - ) - ] - if not candidates and default is not None: - return default - if len(candidates) != 1: - raise ValueError(f"Cannot resolve {kind} {name!r}: {candidates}") - return candidates[0] +@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 * torch.cross(xyz, v, dim=-1) + t = 2.0 * torch.cross(xyz, v, dim=-1) return v + q[..., :1] * t + torch.cross(xyz, t, dim=-1) -class DexSimManipulationEnv: - """Expose real DexSim physics through EmbodiChain's vector-env interface. - - SceneBuilder loads the USD physics model. MJCF supplies task metadata and - the import audit reference. DexSim owns simulation stepping; MuJoCo forward - calls below update kinematics after explicit reset. - """ - - def __init__( - self, - task_id: str, - num_envs: int = 32, - device: str = "cuda:0", - seed: int | None = None, - cuda_graph: bool = True, - solver_kind: str = "mujoco_warp", - diagnostics: bool = True, - solver_iterations: tuple[int, int] | None = None, - ) -> None: - if solver_kind not in ("mujoco_warp", "dexuni"): - raise ValueError(f"Unknown manipulation solver: {solver_kind}") - self.solver_kind = solver_kind - self.diagnostics = diagnostics - device = str(torch.device(device)) - self.task_id = task_id - self.num_envs = num_envs - self.device = torch.device(device) - seed = torch.initial_seed() if seed is None else seed - if task_id in ("M1", "M2"): - self.task = ShadowTask.from_assets( - Path(get_data_path("ShadowHandPen/shadow_pen.xml")).parent, - num_envs, - variant=task_id, - device=device, +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._reset_generator = torch.Generator(device=self.device).manual_seed( - seed + 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}" ) - xml = Path(get_data_path("ShadowHandPen/shadow_pen.xml")) - self.ctrl_dt, self.sim_dt = 0.04, 0.002 - self.observation_dim, self.critic_dim, self.action_dim = 87, 87, 18 - else: - from ._leap_tasks import LeapTask + self._usd_path = usd_path - if task_id == "M3": - xml = Path( - get_data_path("LeapHandCubeFaceTurn/leap_cube_top_layer.xml") - ) - initial = np.load( - get_data_path("LeapHandCubeFaceTurn/LeapCubeFaceTurn-initial.npz") - ) - else: - xml = Path(get_data_path(f"LeapHandCube/{task_id}.xml")) - initial = np.load(get_data_path(f"LeapHandCube/{task_id}-initial.npz")) - source = mujoco.MjModel.from_xml_path(str(xml)) - self.task = LeapTask( - { - "M3": "LeapCubeFaceTurn", - "M4": "LeapCubeReorient", - "M5": "LeapCubeRotateZAxis", - }[task_id], - num_envs, - initial["qpos"], - initial["ctrl"], - source.actuator_ctrlrange[:, 0], - source.actuator_ctrlrange[:, 1], - device=device, - seed=seed, + 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._reset_generator = self.task.generator - self.ctrl_dt, self.sim_dt = 0.05, (0.005 if task_id == "M3" else 0.01) - self.observation_dim = {"M3": 65, "M4": 57, "M5": 32}[task_id] - self.critic_dim = {"M3": 65, "M4": 128, "M5": 105}[task_id] - self.action_dim = 16 - self.source = mujoco.MjModel.from_xml_path(str(xml)) - self.source.opt.timestep = self.sim_dt - if task_id in ("M1", "M2", "M3"): - self.source.opt.ccd_iterations = 10 - self.xml = xml - self.usd = xml.parent / "usd" / f"{xml.stem}.usdc" - if not self.usd.is_file(): - raise FileNotFoundError( - f"The manipulation asset lacks its USD model: {self.usd}" + 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.substeps = round(self.ctrl_dt / self.sim_dt) - observation_spaces = { - "obs": gym.spaces.Box( - -np.inf, np.inf, (self.observation_dim,), dtype=np.float32 + self.sim.add_usd( + UsdImportCfg( + name="scene", + source=UsdSourceCfg(file_path=str(usd_path)), + asset_cfgs={_source_path(hand_art): robot_cfg}, ) - } - if task_id in ("M4", "M5"): - observation_spaces["critic_obs"] = gym.spaces.Box( - -np.inf, np.inf, (self.critic_dim,), dtype=np.float32 + ) + 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.single_observation_space = gym.spaces.Dict(observation_spaces) + + 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, 1, (self.action_dim,), dtype=np.float32 + -1.0, 1.0, shape=(self.action_dim,), dtype=np.float32 ) - self.action_space = self.single_action_space - self.observation_space = self.single_observation_space - self.action_format = "tensor" - self.elapsed_steps = torch.zeros(num_envs, dtype=torch.long, device=device) - self._episode_return = torch.zeros(num_envs, device=device) - self._episode_success = torch.zeros(num_envs, dtype=torch.bool, device=device) - self._last_observation: dict[str, torch.Tensor] | None = None - self._closed = False - config = dexsim.WorldConfig() - config.open_windows = False - config.backend = dexsim.types.Backend.VULKAN - config.renderer = dexsim.types.Renderer.NORENDER - config.init_ground = False - config.cache_path = str(Path.home() / ".cache/embodichain/dexterous") - solver_options = { - "use_mujoco_contacts": True, - "iterations": int(self.source.opt.iterations), - "ls_iterations": int(self.source.opt.ls_iterations), - "ccd_iterations": int(self.source.opt.ccd_iterations), - "integrator": int(self.source.opt.integrator), - "solver": int(self.source.opt.solver), - "cone": ("pyramidal", "elliptic")[int(self.source.opt.cone)], - "impratio": float(self.source.opt.impratio), - "tolerance": float(self.source.opt.tolerance), - "ls_tolerance": float(self.source.opt.ls_tolerance), - "ccd_tolerance": float(self.source.opt.ccd_tolerance), - # Face-turn contacts and joint constraints exceed 512 rows in - # large batches. Keep headroom for the 256-contact allocation. - "njmax": 2048 if task_id == "M3" else 512, - "nconmax": 256, - "update_data_interval": 0, - } - self.backend_option_overrides = {} - if solver_iterations is not None: - selected_iterations = solver_iterations - for name, value in zip( - ("iterations", "ls_iterations"), selected_iterations - ): - self.backend_option_overrides[name] = { - "source": solver_options[name], - "configured": value, - "reason": "Explicit benchmark settings", - } - solver_options[name] = value - if solver_kind == "dexuni": - solver_cfg = DexUniSolverCfg(mujoco_options=solver_options) - else: - solver_cfg = MJWarpSolverCfg(**solver_options) - config.newton_cfg = NewtonCfg( - device=device, - gravity=self.source.opt.gravity.tolist(), - dt=self.sim_dt, - num_substeps=1, - sync_to_dexsim=False, - solver_cfg=solver_cfg, + + 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 ) - config.newton_cfg.use_cuda_graph = cuda_graph - self.world = dexsim.World(config) - self.world.set_manual_update(True) - self.backend = get_newton_backend(self.world) - builder = dexsim.spawn.SceneBuilder(self.world) - builder.add_usd("asset", str(self.usd)) - if solver_kind == "dexuni": - from ._dexuni_adapter import _mujoco_articulation_ids - - self.backend.cfg.solver_cfg.mujoco_articulations = _mujoco_articulation_ids( - builder.scene_desc, num_envs - ) - self._explicit_collision_filtering = bool( - builder.scene_desc.newton - and builder.scene_desc.newton.use_explicit_collision_filtering + self._kp = torch.as_tensor( + self._usd_kp, dtype=torch.float32, device=self.device ) - self._articulation_names = tuple( - item.name for item in builder.scene_desc.articulations + self._kd = torch.as_tensor( + self._usd_kd, dtype=torch.float32, device=self.device ) - builder.replicate(num_envs, spacing=(0.0, 0.0, 0.0)) - self.scene = builder.finalize() - self.solver = self.backend.solver - self.mujoco_solver = self.backend.mujoco_solver - self.dexuni_adapter = None - if solver_kind == "dexuni": - from ._dexuni_adapter import DexUniStateAdapter - - self.dexuni_adapter = DexUniStateAdapter(self.backend) - self.imported = self.mujoco_solver.mj_model - self.data = self.mujoco_solver.mjw_data - self.option_import_corrections = {} - # World.dt owns the step size; Newton's host MjModel keeps the compiler - # default until explicitly synchronized for the task model audit. - self.imported.opt.timestep = self.sim_dt - wp.to_torch(self.mujoco_solver.mjw_model.opt.timestep).fill_(self.sim_dt) - self._make_mapping() - model = self.backend.model - joint_world = wp.to_torch(model.joint_world).long() - self._reset_field_worlds = { - "joint_q": torch.repeat_interleave( - joint_world, torch.diff(wp.to_torch(model.joint_q_start)).long() - ), - "joint_qd": torch.repeat_interleave( - joint_world, torch.diff(wp.to_torch(model.joint_qd_start)).long() - ), - "body_q": wp.to_torch(model.body_world).long(), - "body_qd": wp.to_torch(model.body_world).long(), - } - self.reset(seed=seed) - @property - def unwrapped(self) -> DexSimManipulationEnv: - return self - - def _make_mapping(self) -> None: - from ._audit_model import snapshot_execution_model + 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") + 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, + ) - derived_cache_report = {} - source, imported = self.source, snapshot_execution_model( - self.mujoco_solver, derived_cache_report=derived_cache_report - ) - for name in ("nq", "nv", "nu", "ntendon"): - if getattr(source, name) != getattr(imported, name): - raise ValueError( - f"MJCF {name} changed: {getattr(source, name)} -> {getattr(imported, name)}" - ) - qmap, vmap = [], [] - joint_map = {} - for j in range(source.njnt): - name = source.joint(j).name - if name and source.jnt_type[j] != mujoco.mjtJoint.mjJNT_FREE: - target = _named_index(imported, "joint", name) - else: - body = _named_index( - imported, "body", source.body(int(source.jnt_bodyid[j])).name - ) - candidates = np.flatnonzero( - (imported.jnt_bodyid == body) - & (imported.jnt_type == source.jnt_type[j]) - ) - if len(candidates) != 1: - raise ValueError("Root joint cannot be resolved unambiguously") - target = int(candidates[0]) - joint_map[j] = target - width = (7, 4, 1, 1)[int(source.jnt_type[j])] - vwidth = (6, 3, 1, 1)[int(source.jnt_type[j])] - qmap.extend( - range( - int(imported.jnt_qposadr[target]), - int(imported.jnt_qposadr[target]) + width, - ) + 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], ) - vmap.extend( - range( - int(imported.jnt_dofadr[target]), - int(imported.jnt_dofadr[target]) + vwidth, - ) + self._reset_generator = torch.Generator(device=self.device).manual_seed( + seed ) - amap = [] - for a in range(source.nu): - source_joint = int(source.actuator_trnid[a, 0]) - candidates = np.flatnonzero( - imported.actuator_trnid[:, 0] == joint_map[source_joint] + 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, ) - if len(candidates) != 1: - raise ValueError(f"Actuator {a} mapping ambiguous: {candidates}") - amap.append(int(candidates[0])) - self.qmap = torch.tensor(qmap, device=self.device) - self.vmap = torch.tensor(vmap, device=self.device) - self.amap = torch.tensor(amap, device=self.device) - self.ctrl = torch.zeros((self.num_envs, source.nu), device=self.device) - self._bind_controls() - self.model_audit = { - "solver_derived_cache_variation": derived_cache_report, - "source": str(self.xml), - "usd_source": str(self.usd), - "parameter_source": "Active solver device parameters at initialization.", - "backend_option_overrides": self.backend_option_overrides, - "solver_selection": { - "requested": self.solver_kind, - "actual_backend": self.backend.solver_type, - "actual_solver_class": ( - f"{type(self.solver).__module__}.{type(self.solver).__name__}" - ), - "mujoco_branch_class": ( - f"{type(self.mujoco_solver).__module__}." - f"{type(self.mujoco_solver).__name__}" - ), - "diagnostics": self.backend.get_solver_diagnostics(), - "standalone_mjwarp_fallback": False, - }, - "option_import_corrections": self.option_import_corrections, - "counts": { - k: int(getattr(source, k)) for k in ("nq", "nv", "nu", "ntendon") - }, - "source_reference_counts": { - k: int(getattr(source, k)) for k in ("nsite", "nsensor") - }, - "imported_reference_counts": { - k: int(getattr(imported, k)) for k in ("nsite", "nsensor") - }, - "observation_state_source": ( - "Named qpos/qvel, body poses, source-local site transforms, " - "and actuator forces; imported sensor channels are not consumed." - ), - "qpos_map": qmap, - "qvel_map": vmap, - "actuator_map": amap, - "gain_max_abs_error": float( - np.max( - np.abs(source.actuator_gainprm - imported.actuator_gainprm[amap]) - ) - ), - "bias_max_abs_error": float( - np.max( - np.abs(source.actuator_biasprm - imported.actuator_biasprm[amap]) - ) - ), - "tendon_range_max_abs_error": ( - float(np.max(np.abs(source.tendon_range - imported.tendon_range))) - if source.ntendon - else 0.0 - ), - "solver_options": { - key: float(getattr(imported.opt, key)) - for key in ( - "timestep", - "iterations", - "ls_iterations", - "ccd_iterations", - "integrator", - "solver", - "cone", - "impratio", - ) - }, - } - if ( - max( - self.model_audit[k] - for k in ( - "gain_max_abs_error", - "bias_max_abs_error", - "tendon_range_max_abs_error", - ) + 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 ) - > 1e-5 - ): - raise ValueError( - f"Imported actuator/tendon properties changed: {self.model_audit}" + 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 + cube_local_angvel = rotate_wxyz( + _invert_wxyz(cube_quat_wxyz), cube_world_angvel ) - self.body_map = { - source.body(i).name: _named_index( - imported, - "body", - source.body(i).name, - default=( - 0 - if source.body_weldid[i] == 0 and source.body_mocapid[i] < 0 - else None + 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 ), - ) - for i in range(1, source.nbody) - if source.body(i).name - } - self.model_audit["property_import_corrections"] = [] - from ._audit_model import audit_model - - audit_source = source - if self.backend_option_overrides: - audit_source = copy.copy(source) - for name, item in self.backend_option_overrides.items(): - setattr(audit_source.opt, name, item["configured"]) - self.model_audit["physical_model"] = audit_model( - audit_source, - imported, - joint_map, - amap, - self.body_map, - explicit_collision_filtering=self._explicit_collision_filtering, + "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) ) - self.model_audit["physical_model"]["option_reference"] = ( - "Source model with explicit backend_option_overrides." - if self.backend_option_overrides - else "Source model." + 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 ) - from ._audit_tendons import audit_tendons - self.model_audit["tendons"] = audit_tendons(source, imported, joint_map) - if self.model_audit["tendons"]["status"] != "passed": - raise ValueError(f"Imported tendons differ: {self.model_audit['tendons']}") - if self.model_audit["physical_model"]["status"] != "passed": - raise ValueError( - f"Imported physical model differs: {self.model_audit['physical_model']['mismatches']}" - ) + 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 _bind_controls(self) -> None: - """Bind source actuator order to homogeneous public articulation batches.""" - self._control_batches = [] - assigned = set() - faces = [] - for name in self._articulation_names: - handles = [ - self.scene.get_articulation(f"env_{index}/{name}") - for index in range(self.num_envs) - ] - batch = self.scene.create_articulation_batch(handles) - layouts = batch.joint_layouts_per_articulation[0] - offsets, actuators = [], [] - for layout in layouts: - matches = [ - aid - for aid in range(self.source.nu) - if self.source.joint(int(self.source.actuator_trnid[aid, 0])).name - == layout.name - ] - if matches: - if ( - len(matches) != 1 - or layout.dof_count != 1 - or matches[0] in assigned - ): - raise ValueError( - f"Ambiguous USD actuator mapping: {layout.name}" - ) - assigned.add(matches[0]) - offsets.append(layout.dof_start) - actuators.append(matches[0]) - if layout.name == "face_turn": - if layout.dof_count != 1: - raise ValueError("The passive cube face must have one DOF") - faces.append((batch, layout.dof_start)) - if offsets: - self._control_batches.append( - ( - batch, - torch.tensor(offsets, device=self.device), - torch.tensor(actuators, device=self.device), - torch.zeros( - (self.num_envs, batch.dof_width), device=self.device - ), - ) - ) - if assigned != set(range(self.source.nu)): - raise ValueError( - f"USD actuators are missing: {set(range(self.source.nu)) - assigned}" + 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 ) - if self.task_id == "M3": - if len(faces) != 1: - raise ValueError(f"Expected one passive cube face, got {len(faces)}") - self._face_batch, self._face_dof = faces[0] - - def _publish_controls(self) -> None: - for batch, offsets, actuators, commands in self._control_batches: - commands[:, offsets] = self.ctrl[:, actuators] - batch.apply_joint_target_position(commands) - - def _site(self, name: str) -> tuple[torch.Tensor, torch.Tensor]: - source_id = self.source.site(name).id - body_name = self.source.body(int(self.source.site_bodyid[source_id])).name - body_id = self.body_map[body_name] - pos = wp.to_torch(self.data.xpos)[:, body_id] - quat = wp.to_torch(self.data.xquat)[:, body_id] - local = torch.as_tensor( - self.source.site_pos[source_id], dtype=torch.float32, device=self.device - ).expand(self.num_envs, -1) - return pos + rotate_wxyz(quat, local), quat - - def _state(self) -> Any: - qpos = wp.to_torch(self.data.qpos)[:, self.qmap] - qvel = wp.to_torch(self.data.qvel)[:, self.vmap] - if self.task_id in ("M1", "M2"): - palm = wp.to_torch(self.data.xpos)[:, self.body_map["robot0:palm"]] - return ShadowState(qpos, qvel, palm) - state = {"qpos": qpos, "qvel": qvel} - if self.task_id == "M3": - return state - grasp, palm_quat = self._site("grasp_site") - tips = [] - for name in ("th_tip", "if_tip", "mf_tip", "rf_tip"): - tip, _ = self._site(name) - inverse = palm_quat * palm_quat.new_tensor([1, -1, -1, -1]) - tips.append(rotate_wxyz(inverse, tip - grasp)) - cube_quat = qpos[:, 19:23] - state.update( - hand_qpos=qpos[:, :16], - hand_qvel=qvel[:, :16], - actuator_force=wp.to_torch(self.data.actuator_force)[:, self.amap], - cube_pos=qpos[:, 16:19], - cube_quat=cube_quat, - cube_linvel=qvel[:, 16:19], - cube_angvel=rotate_wxyz(cube_quat, qvel[:, 19:22]), - palm_pos=grasp, - fingertip_pos=torch.cat(tips, dim=-1), - ) - return state - - def _observation(self, raw: Any) -> dict[str, torch.Tensor]: - if isinstance(raw, torch.Tensor): - return {"obs": raw} - result = {"obs": raw["state"]} - if self.critic_dim != self.observation_dim: - result["critic_obs"] = raw["privileged_state"] - return result - - def reset( - self, *, seed: int | None = None, options: dict[str, Any] | None = None - ) -> tuple[dict, dict]: - if seed is not None: - self._reset_generator.manual_seed(seed) + 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) - if not options - else options.get("reset_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) ) - ids = torch.as_tensor(ids, dtype=torch.long, device=self.device) - if self.task_id in ("M1", "M2"): - qpos, qvel, ctrl = self.task.reset(ids, generator=self._reset_generator) + 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, qvel, ctrl = initial["qpos"], initial["qvel"], initial["ctrl"] - wp.to_torch(self.data.qpos)[ids[:, None], self.qmap[None, :]] = qpos - wp.to_torch(self.data.qvel)[ids[:, None], self.vmap[None, :]] = qvel - self.ctrl[ids] = ctrl - self._publish_controls() - # Transfer explicitly set coordinates to both DexSim-owned buffers. - if self.dexuni_adapter is not None: - self.dexuni_adapter.publish_reset(self.data, ids) - else: - for state in (self.backend.state_0, self.backend.state_1): - retained = [] - for name, worlds in self._reset_field_worlds.items(): - unselected = ~torch.isin(worlds, ids) - values = wp.to_torch(getattr(state, name)) - retained.append((values, unselected, values[unselected].clone())) - self.mujoco_solver._update_newton_state( - self.backend.model, state, self.data, state_prev=state + 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, ) - for values, unselected, previous in retained: - values[unselected] = previous - mask = torch.zeros(self.num_envs + 1, dtype=torch.bool, device=self.device) - mask[ids] = True - self.backend.clear_solver_state(wp.from_torch(mask)) - # Solver reset clears native controls; restore the selected reset targets - # before computing their initial actuator forces and observations. - wp.to_torch(self.data.ctrl)[ids[:, None], self.amap[None, :]] = ctrl - # Forward updates derived data without advancing time or warm-start state. - self.mujoco_solver._mujoco_warp.forward(self.mujoco_solver.mjw_model, self.data) - self.elapsed_steps[ids] = 0 - self._episode_return[ids] = 0 + 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 - state = self._state() - if self.task_id in ("M1", "M2"): - selected = { - key: value[ids] - for key, value in self._observation(self.task.observe(state)).items() - } - else: - selected = self._observation(self.task.observe(state, env_ids=ids)) - if self._last_observation is None: - fresh = { - key: value.new_empty((self.num_envs, value.shape[-1])) - for key, value in selected.items() - } + 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: - fresh = { - key: value.clone() for key, value in self._last_observation.items() - } - for key, value in selected.items(): - fresh[key][ids] = value - self._last_observation = fresh - return fresh, {} - - def advance_physics(self) -> None: - """Advance one complete control interval with the configured substeps.""" - self._publish_controls() - for _ in range(self.substeps): - if self.task_id == "M3": - # The passive layer detent is updated for each physical substep. - state = self._state() - face_force = self.task.face_detent(state["qpos"][:, -1]) - self._face_batch.apply_joint_force( - face_force[:, None], dof_ids=[self._face_dof] - ) - self.world.update(self.sim_dt) + 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( - self, actions: torch.Tensor - ) -> tuple[dict, torch.Tensor, torch.Tensor, torch.Tensor, dict]: - if self.diagnostics and not bool(torch.isfinite(actions).all()): + 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") - actions = actions.clamp(-1.0, 1.0) - if self.task_id in ("M1", "M2"): - targets = self.task.action_targets(actions) + 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(actions) - self.ctrl.copy_(targets) - self.advance_physics() - state = self._state() - if self.diagnostics and not bool( - torch.isfinite( - state.qpos if isinstance(state, ShadowState) else state["qpos"] - ).all() - ): + 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.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, 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, 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, render_final_step=False) + return + super()._advance_physics() + + 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") - if self.task_id in ("M1", "M2"): - obs, reward, terminated, metrics = self.task.step(state, actions) - self.elapsed_steps += 1 - truncated = (self.elapsed_steps >= 500) & ~terminated + + 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: - obs, reward, terminated, truncated, metrics = self.task.after_step( - state, actions + raw_obs, reward, terminated, _, metrics = self.task.after_step( + state, self._raw_action ) - self.elapsed_steps += 1 - self._episode_return += reward - done = terminated | truncated - result = self._observation(obs) - self._last_observation = result - info = {"metrics": {key: value.clone() for key, value in metrics.items()}} + 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: - # Task success is an event; evaluation consumes the completed episode. self._episode_success |= metrics["success"].bool() - info["success"] = self._episode_success.clone() - if bool(done.any()): - ids = torch.where(done)[0] - info["episode"] = { - "r": self._episode_return[ids].clone(), - "l": self.elapsed_steps[ids].clone(), - } - final_observation = {k: v.clone() for k, v in result.items()} - reset_obs, _ = self.reset(options={"reset_ids": ids}) - for key in result: - result[key][ids] = reset_obs[key][ids] - info["final_observation"] = final_observation - info["_final_observation"] = done - self._last_observation = result - return result, reward, terminated.bool(), truncated.bool(), info - - def close(self) -> None: - if not self._closed: - self.scene.close() - self.world.quit() - self._closed = True + + 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 + 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 index 2ee0d6a423..f423a8aa21 100644 --- a/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_shadow_task.py +++ b/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_shadow_task.py @@ -16,9 +16,9 @@ from __future__ import annotations import math +from collections.abc import Sequence from pathlib import Path from typing import NamedTuple -import xml.etree.ElementTree as ET import torch @@ -143,8 +143,11 @@ def from_assets( 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 and actuator limits without loading physics.""" + """Load retained reset arrays with USD-authored actuator control bounds.""" import numpy as np assets = Path(asset_root) @@ -153,20 +156,16 @@ def from_assets( 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) - actuator = ET.parse(assets / "shadow_pen.xml").getroot().find("actuator") - bounds = torch.tensor( - [[float(v) for v in item.attrib["ctrlrange"].split()] for item in actuator], - 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, - bounds[:, 0], - bounds[:, 1], + lower_tensor, + upper_tensor, ) @staticmethod 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 index 07144b8d02..df999f1686 100644 --- 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 @@ -13,19 +13,20 @@ # See the License for the specific language governing permissions and # limitations under the License. # ---------------------------------------------------------------------------- + from __future__ import annotations -from typing import Any -import torch -from embodichain.learning.rl.env import register_learning_env -from ._runtime import DexSimManipulationEnv +from embodichain.lab.gym.utils.registration import register_env + +from ._runtime import DexterousManipulationEnv + +__all__ = ["LeapCubeFaceTurnEnv"] -__all__ = ["make_env"] +@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.""" -@register_learning_env("LeapCubeFaceTurnDexSim-v1") -def make_env( - *, num_envs: int, device: torch.device | str, **cfg: Any -) -> DexSimManipulationEnv: - """Create the leap cube face turn task with the selected DexSim solver.""" - return DexSimManipulationEnv(task_id="M3", num_envs=num_envs, device=device, **cfg) + 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 index a7de1689ac..0e7d1e91df 100644 --- a/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/leap_cube_reorient.py +++ b/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/leap_cube_reorient.py @@ -13,19 +13,20 @@ # See the License for the specific language governing permissions and # limitations under the License. # ---------------------------------------------------------------------------- + from __future__ import annotations -from typing import Any -import torch -from embodichain.learning.rl.env import register_learning_env -from ._runtime import DexSimManipulationEnv +from embodichain.lab.gym.utils.registration import register_env + +from ._runtime import DexterousManipulationEnv + +__all__ = ["LeapCubeReorientEnv"] -__all__ = ["make_env"] +@register_env( + "LeapCubeReorientRL-v1", max_episode_steps=1000, override=True, supports_rl=True +) +class LeapCubeReorientEnv(DexterousManipulationEnv): + """LEAP Hand in-hand 3D cube reorientation environment.""" -@register_learning_env("LeapCubeReorientDexSim-v1") -def make_env( - *, num_envs: int, device: torch.device | str, **cfg: Any -) -> DexSimManipulationEnv: - """Create the leap cube reorient task with the selected DexSim solver.""" - return DexSimManipulationEnv(task_id="M4", num_envs=num_envs, device=device, **cfg) + 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 index da4e0666ed..ccf8ae403c 100644 --- 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 @@ -13,19 +13,20 @@ # See the License for the specific language governing permissions and # limitations under the License. # ---------------------------------------------------------------------------- + from __future__ import annotations -from typing import Any -import torch -from embodichain.learning.rl.env import register_learning_env -from ._runtime import DexSimManipulationEnv +from embodichain.lab.gym.utils.registration import register_env + +from ._runtime import DexterousManipulationEnv + +__all__ = ["LeapCubeRotateZAxisEnv"] -__all__ = ["make_env"] +@register_env( + "LeapCubeRotateZAxisRL-v1", max_episode_steps=500, override=True, supports_rl=True +) +class LeapCubeRotateZAxisEnv(DexterousManipulationEnv): + """LEAP Hand continuous Z-axis cube rotation environment.""" -@register_learning_env("LeapCubeRotateZAxisDexSim-v1") -def make_env( - *, num_envs: int, device: torch.device | str, **cfg: Any -) -> DexSimManipulationEnv: - """Create the leap cube rotate z task with the selected DexSim solver.""" - return DexSimManipulationEnv(task_id="M5", num_envs=num_envs, device=device, **cfg) + 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 index 397f49d3be..e22ee31cb8 100644 --- a/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/shadow_pen_continuous.py +++ b/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/shadow_pen_continuous.py @@ -13,19 +13,20 @@ # See the License for the specific language governing permissions and # limitations under the License. # ---------------------------------------------------------------------------- + from __future__ import annotations -from typing import Any -import torch -from embodichain.learning.rl.env import register_learning_env -from ._runtime import DexSimManipulationEnv +from embodichain.lab.gym.utils.registration import register_env + +from ._runtime import DexterousManipulationEnv + +__all__ = ["ShadowPenContinuousEnv"] -__all__ = ["make_env"] +@register_env( + "ShadowPenContinuousRL-v1", max_episode_steps=500, override=True, supports_rl=True +) +class ShadowPenContinuousEnv(DexterousManipulationEnv): + """Multi-phase continuous Shadow Hand pen spinning environment.""" -@register_learning_env("ShadowPenContinuousDexSim-v1") -def make_env( - *, num_envs: int, device: torch.device | str, **cfg: Any -) -> DexSimManipulationEnv: - """Create the shadow pen continuous task with the selected DexSim solver.""" - return DexSimManipulationEnv(task_id="M2", num_envs=num_envs, device=device, **cfg) + 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 index a81bf002f4..14ba25b85a 100644 --- a/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/shadow_pen_fixed.py +++ b/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/shadow_pen_fixed.py @@ -13,19 +13,20 @@ # See the License for the specific language governing permissions and # limitations under the License. # ---------------------------------------------------------------------------- + from __future__ import annotations -from typing import Any -import torch -from embodichain.learning.rl.env import register_learning_env -from ._runtime import DexSimManipulationEnv +from embodichain.lab.gym.utils.registration import register_env + +from ._runtime import DexterousManipulationEnv + +__all__ = ["ShadowPenFixedEnv"] -__all__ = ["make_env"] +@register_env( + "ShadowPenFixedRL-v1", max_episode_steps=500, override=True, supports_rl=True +) +class ShadowPenFixedEnv(DexterousManipulationEnv): + """Fixed-grasp Shadow Hand pen rotation environment.""" -@register_learning_env("ShadowPenFixedDexSim-v1") -def make_env( - *, num_envs: int, device: torch.device | str, **cfg: Any -) -> DexSimManipulationEnv: - """Create the shadow pen fixed task with the selected DexSim solver.""" - return DexSimManipulationEnv(task_id="M1", num_envs=num_envs, device=device, **cfg) + task_id = "M1" diff --git a/tests/gym/envs/tasks/test_dexterous_metrics.py b/tests/gym/envs/tasks/test_dexterous_metrics.py index 53a6a668a3..419b3806db 100644 --- a/tests/gym/envs/tasks/test_dexterous_metrics.py +++ b/tests/gym/envs/tasks/test_dexterous_metrics.py @@ -16,120 +16,255 @@ from __future__ import annotations +from contextlib import nullcontext import math from types import SimpleNamespace from typing import Any, Callable from unittest.mock import Mock 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 import _runtime +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) -> None: + 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: - data["action"] = torch.zeros(data.batch_size[0], 1) + del deterministic + data["action"] = torch.zeros(data.batch_size[0], self.action_dim) return data @pytest.fixture -def env_factory( - monkeypatch: pytest.MonkeyPatch, -) -> Callable[[str], _runtime.DexSimManipulationEnv]: - """Exercise the real step/reset bridge with deterministic task events.""" - monkeypatch.setattr(_runtime.wp, "to_torch", lambda x: x) - monkeypatch.setattr(_runtime.wp, "from_torch", lambda x: x) - - def make(task_id: str): - env = _runtime.DexSimManipulationEnv.__new__(_runtime.DexSimManipulationEnv) +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.device = torch.device("cpu") - env.num_envs = 2 - env.diagnostics = False - env.observation_dim = env.critic_dim = 1 + 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( - {"obs": gym.spaces.Box(-1.0, 1.0, (1,))} + { + "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.single_action_space = gym.spaces.Box(-1.0, 1.0, (1,)) - env.action_format = "tensor" - env.elapsed_steps = torch.zeros(2, dtype=torch.long) - env._reset_generator = torch.Generator().manual_seed(0) - env._episode_return = torch.zeros(2) + 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._last_observation = None - env.ctrl = torch.zeros(2, 1) - env.qmap = env.vmap = env.amap = torch.tensor([0]) - env.data = SimpleNamespace( - qpos=torch.zeros(2, 1), qvel=torch.zeros(2, 1), ctrl=torch.zeros(2, 1) + 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.dexuni_adapter = Mock() - env.backend = Mock() - env.mujoco_solver = Mock() - env.advance_physics = Mock() - env._publish_controls = Mock() - env._state = lambda: None + 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): - values = torch.zeros(len(ids), 1) - if task_id in ("M1", "M2"): + n = len(ids) + if env.is_shadow: assert generator is env._reset_generator - return values, values, values - return dict(qpos=values, qvel=values, ctrl=values) + 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): - return torch.zeros(2 if env_ids is None else len(env_ids), 1) + 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(state, actions): - # Environment 0 succeeds before termination; environment 1 never does. - success = (env.elapsed_steps == 0) & torch.tensor([True, False]) - done = env.elapsed_steps >= torch.tensor([2, 3]) - metrics = {"rotation": env.elapsed_steps.float()} + 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() - result = (torch.zeros(2, 1), torch.ones(2), done) - if task_id in ("M1", "M2"): - return (*result, metrics) - return (*result, torch.zeros(2, dtype=torch.bool), metrics) + 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: x, - control_targets=lambda x: x, - step=step, - after_step=step, + 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, task_id: str + env_factory: Callable[[str], DexterousManipulationEnv], task_id: str ) -> None: - """An early success counts at episode completion even if its pulse has ended.""" + """An early success pulse persists across the episode until reset.""" env = env_factory(task_id) - result = evaluate_episodes(policy=_Policy(), env=env, num_episodes=2, device="cpu") + 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, task_id: str + 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() @@ -137,38 +272,48 @@ def test_partial_reset_clears_only_selected_success_and_keeps_terminal_info( 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, 0]) - _, _, done, _, info = env.step(torch.zeros(2, 1)) - assert done.tolist() == [True, False] + 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, + 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=env, num_episodes=2, device="cpu") + 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", ["M1", "M2"]) -def test_failure_at_time_limit_does_not_bootstrap(env_factory, task_id): - from tensordict import TensorDict +def test_failure_at_time_limit_does_not_bootstrap( + env_factory: Callable[[str], DexterousManipulationEnv], task_id: str +) -> None: + """True failure on the final step sets terminated=True and truncated=False.""" from embodichain.learning.rl.algo.common import compute_gae env = env_factory(task_id) - env.elapsed_steps.fill_(499) + env._elapsed_steps.fill_(env.max_episode_steps - 1) env.task.step = lambda state, actions: ( - torch.zeros(2, 1), + torch.zeros(2, env.policy_dim), torch.ones(2), torch.tensor([True, False]), {}, ) - _, reward, terminated, truncated, _ = env.step(torch.zeros(2, 1)) + _, reward, terminated, truncated, _ = env.step(torch.zeros(2, env.action_dim)) assert terminated.tolist() == [True, False] assert truncated.tolist() == [False, True] rollout = TensorDict( diff --git a/tests/gym/envs/tasks/test_dexterous_usd.py b/tests/gym/envs/tasks/test_dexterous_usd.py index 8031004717..1e75cd4834 100644 --- a/tests/gym/envs/tasks/test_dexterous_usd.py +++ b/tests/gym/envs/tasks/test_dexterous_usd.py @@ -131,40 +131,78 @@ def test_snapshot_rejects_per_world_timestep_changes() -> None: snapshot_execution_model(SimpleNamespace(mj_model=model, mjw_model=remote)) -def test_dexuni_ownership_includes_free_bodies_and_replicas() -> None: - from dexsim.spawn import ( - ArticulationDesc, - MeshObjectDesc, - RigidBodyPhysicsDesc, - SceneDesc, - NewtonPhysicsDesc, - CollisionDesc, - ) - from dexsim.types import ActorType - from embodichain_tasks.manipulation.dexterous._dexuni_adapter import ( - _mujoco_articulation_ids, - ) - - scene = SceneDesc( - articulations=[ArticulationDesc(name="hand")], - mesh_objects=[ - MeshObjectDesc( - name="cube", - physics=RigidBodyPhysicsDesc(actor_type=ActorType.DYNAMIC), - collisions=[CollisionDesc()], - ), - MeshObjectDesc( - name="anchor", - physics=RigidBodyPhysicsDesc( - actor_type=ActorType.KINEMATIC, newton=NewtonPhysicsDesc(mocap=True) - ), - ), - MeshObjectDesc( - name="floor", - physics=RigidBodyPhysicsDesc(actor_type=ActorType.STATIC), - collisions=[CollisionDesc()], - ), - ], - ) - assert _mujoco_articulation_ids(scene, 1) == (0, 1, 2) - assert _mujoco_articulation_ids(scene, 4) == tuple(range(12)) +def test_dexuni_scene_joint_ownership_assigns_all_joints(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 caller overrides remain untouched. + 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_runtime_and_catalog_configs_are_usd_native() -> None: + import argparse + import inspect + from pathlib import Path + from embodichain.cli._task_catalog import _catalog_from_args + from embodichain_tasks.manipulation.dexterous import _runtime, _shadow_task + + 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", + "embodichain_tasks:shadow_pen_continuous", + "embodichain_tasks:leap_cube_face_turn", + "embodichain_tasks:leap_cube_reorient", + "embodichain_tasks:leap_cube_rotate_z", + "embodichain_tasks:g1_rough", + ) + for selector in expected: + assert selector in catalog + entry = catalog[selector] + assert entry.default_deployment == "newton" + deps = {d.name: d for d in entry.deployments} + assert set(deps) == {"newton", "mjwarp"} + assert len(deps["newton"].agent_refs) >= 1 + assert len(deps["mjwarp"].agent_refs) >= 1 From a26c00491ba130376373a4f0c878ae0cd66993af Mon Sep 17 00:00:00 2001 From: acrlw <13927622+acrlw@users.noreply.github.com> Date: Fri, 13 Nov 2026 19:44:14 +0800 Subject: [PATCH 19/24] feat(tasks): default g1_rough to DexUni and align RL task catalogs, docs, and tests - Set UnitreeG1RoughRL-v1 default newton deployment to solver_type: dexuni with Newton explicit heightfield contacts and add paired mjwarp deployment - Remove deprecated env.dexuni.yaml, ppo.dexuni.yaml, and legacy rough locomotion README/extension keys - Update locomotion RL overview, rl-learning agent context, g1_rough README, and rough velocity tests --- .../topics/rl-learning/rl-learning.md | 25 +++++----- docs/source/overview/rl/locomotion.md | 22 ++++++--- .../locomotion/velocity/g1_rough/README.md | 29 +++++++++++ .../{ppo.dexuni.yaml => ppo.mjwarp.yaml} | 12 ++--- .../velocity/g1_rough/agents/ppo.newton.yaml | 12 ++--- .../locomotion/velocity/g1_rough/catalog.yaml | 8 ++-- .../{env.dexuni.yaml => env.mjwarp.yaml} | 48 +++++++++---------- .../velocity/g1_rough/env.newton.yaml | 48 +++++++++++-------- .../locomotion/velocity/rough/README.md | 38 --------------- .../locomotion/velocity/rough/environment.py | 25 ++++++---- .../locomotion/velocity/rough/runtime.py | 30 ++++++------ tests/gym/envs/tasks/test_rough_velocity.py | 33 +++++++++++-- 12 files changed, 184 insertions(+), 146 deletions(-) create mode 100644 extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/README.md rename extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/agents/{ppo.dexuni.yaml => ppo.mjwarp.yaml} (80%) rename extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/{env.dexuni.yaml => env.mjwarp.yaml} (83%) delete mode 100644 extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/README.md diff --git a/agent_context/topics/rl-learning/rl-learning.md b/agent_context/topics/rl-learning/rl-learning.md index 727886d2b9..1b2c15d42e 100644 --- a/agent_context/topics/rl-learning/rl-learning.md +++ b/agent_context/topics/rl-learning/rl-learning.md @@ -212,16 +212,19 @@ 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 task candidates +## Rough terrain and dexterous manipulation tasks `locomotion/velocity/g1_rough.py` registers the 37-action G1 task (`UnitreeG1RoughRL-v1`). -Its frozen curriculum comes from `G1RoughTerrains`; the robot comes from -`UnitreeG1Rough`, independently of the 29-joint G1 flat archive. - -`manipulation/dexterous/` contains five task-named learning-env factories and -shared internal physics/MDP adapters. Matching task-first agent configurations -select `mujoco_warp` or `dexuni` explicitly. The declared Dict observation -spaces match returned actor/critic groups. Source MJCF audits run at creation. -Version-specific backend adapters remain local to the task collection pending -engine-owned public import/state interfaces. See the user-facing dexterous -RL page for current training and checkpoint-continuation boundaries. +Its terrain curriculum comes from `G1RoughTerrains` and its robot comes from +`UnitreeG1Rough`, independently of the 29-joint G1 flat archive. Its catalog +defaults to `newton` (`solver_type: dexuni` with Newton heightfield contacts) +alongside `mjwarp` (`solver_type: mujoco_warp`). + +`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 defaults to `newton` (`solver_type: dexuni`, GPU MJWarp CUDA) +and also ships `mjwarp` (`solver_type: mujoco_warp`). Offline physical-model and +tendon parity helpers live in `_audit_model.py` and `_audit_tendons.py`. diff --git a/docs/source/overview/rl/locomotion.md b/docs/source/overview/rl/locomotion.md index f3ec54f25a..e3b79597d6 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 / 325 | | `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 | @@ -48,12 +49,14 @@ Humanoid uses clipped effort actions (`[-0.4, 0.4]`) multiplied by per-joint mot ## G1 rough terrain with DexUni -`g1_rough/agents/ppo.dexuni.yaml` selects the pure MuJoCo branch of -DexUni with Newton-generated terrain contacts (`UnitreeG1RoughRL-v1`). The paired DexSim build must -support `mujoco_options.use_mujoco_contacts: false` in this branch and export -MuJoCo solver contact forces for contact sensors. The MJWarp deployment remains -`g1_rough/agents/ppo.newton.yaml`; both configurations retain the same terrain and physical -parameters. +`g1_rough/env.newton.yaml` (`g1_rough/agents/ppo.newton.yaml`) is the default +deployment for `UnitreeG1RoughRL-v1` and selects `solver_type: dexuni` +(`joint_mode: dynamic`, `contact_mode: auto`, `use_mujoco_cpu: false`, +`use_mujoco_contacts: false`) with Newton explicit heightfield contact generation +(`broad_phase: explicit`, `reduce_contacts: true`, `max_triangle_pairs: 2500000`). +`g1_rough/env.mjwarp.yaml` (`g1_rough/agents/ppo.mjwarp.yaml`) provides the direct +`solver_type: mujoco_warp` deployment under identical terrain, timestep, and +solver-iteration parameters. The separate `UnitreeG1Rough` and `G1RoughTerrains` asset packages provide the 37-joint model and terrain inputs. They do not replace `UnitreeG1Locomotion`. @@ -74,6 +77,13 @@ 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.newton.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/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..cd56355624 --- /dev/null +++ b/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/README.md @@ -0,0 +1,29 @@ +# 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`. + +| Deployment | Environment | PPO configuration | +|---|---|---| +| `newton` (DexUni, recommended) | [env.newton.yaml](env.newton.yaml) | [agents/ppo.newton.yaml](agents/ppo.newton.yaml) | +| `mjwarp` (MuJoCo Warp) | [env.mjwarp.yaml](env.mjwarp.yaml) | [agents/ppo.mjwarp.yaml](agents/ppo.mjwarp.yaml) | + +Each PPO file's `trainer.gym_config` selects its matching deployment. The catalog defaults to `newton`, which runs `DexUni` (`joint_mode: dynamic`, `contact_mode: auto`, `use_mujoco_cpu: false`, `use_mujoco_contacts: false`) with Newton explicit heightfield contact generation (`broad_phase: explicit`, `reduce_contacts: true`, `max_triangle_pairs: 2500000`). Both deployments use 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 consumes 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.newton.yaml +embodichain train-rl --config extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/agents/ppo.newton.yaml +``` + +## Evaluation + +Evaluate a saved checkpoint with its matching training and environment snapshots: + +```bash +embodichain eval-policy runs/ --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.dexuni.yaml b/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/agents/ppo.mjwarp.yaml similarity index 80% rename from extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/agents/ppo.dexuni.yaml rename to extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/agents/ppo.mjwarp.yaml index 5f186eca21..dd412a61ba 100644 --- a/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/agents/ppo.dexuni.yaml +++ b/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/agents/ppo.mjwarp.yaml @@ -1,14 +1,14 @@ trainer: - exp_name: g1_rough_ppo_dexuni - gym_config: ../env.dexuni.yaml + exp_name: unitree_g1_rough_ppo_mjwarp + gym_config: extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/env.mjwarp.yaml seed: 42 device: cuda:0 headless: true - num_envs: 64 - iterations: 20 + num_envs: 4096 + iterations: 3000 buffer_size: 24 enable_eval: false - save_frequency_updates: 10 + save_frequency_updates: 100 use_wandb: false wandb_project_name: embodichain-locomotion policy: @@ -49,7 +49,7 @@ algorithm: maximum_learning_rate: 0.01 n_epochs: 5 num_mini_batches: 4 - batch_size: 384 + batch_size: 24576 gamma: 0.99 gae_lambda: 0.95 clip_coef: 0.2 diff --git a/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/agents/ppo.newton.yaml b/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/agents/ppo.newton.yaml index ecdaeaf458..66c86de419 100644 --- a/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/agents/ppo.newton.yaml +++ b/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/agents/ppo.newton.yaml @@ -1,14 +1,14 @@ trainer: - exp_name: g1_rough_ppo_mujoco_warp - gym_config: ../env.newton.yaml + exp_name: unitree_g1_rough_ppo_newton + gym_config: extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/env.newton.yaml seed: 42 device: cuda:0 headless: true - num_envs: 64 - iterations: 20 + num_envs: 4096 + iterations: 3000 buffer_size: 24 enable_eval: false - save_frequency_updates: 10 + save_frequency_updates: 100 use_wandb: false wandb_project_name: embodichain-locomotion policy: @@ -49,7 +49,7 @@ algorithm: maximum_learning_rate: 0.01 n_epochs: 5 num_mini_batches: 4 - batch_size: 384 + batch_size: 24576 gamma: 0.99 gae_lambda: 0.95 clip_coef: 0.2 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 index 9f59a72756..12fb098f40 100644 --- a/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/catalog.yaml +++ b/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/catalog.yaml @@ -1,13 +1,13 @@ task_key: g1_rough title: Unitree G1 Rough-Terrain Locomotion -summary: G1 locomotion with a frozen terrain curriculum and height observations. +summary: Track planar velocity and yaw-rate commands with the 37-DOF Unitree G1 humanoid across a heightfield terrain curriculum. tags: - locomotion - reinforcement_learning -- benchmark +- humanoid default_deployment: newton deployments: newton: config: env.newton.yaml - dexuni: - config: env.dexuni.yaml + mjwarp: + config: env.mjwarp.yaml diff --git a/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/env.dexuni.yaml b/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/env.mjwarp.yaml similarity index 83% rename from extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/env.dexuni.yaml rename to extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/env.mjwarp.yaml index 55f19763e3..1713a9548a 100644 --- a/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/env.dexuni.yaml +++ b/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/env.mjwarp.yaml @@ -1,8 +1,8 @@ physics: newton id: UnitreeG1RoughRL-v1 max_episode_steps: 1000 -num_envs: 1 -scene_node_capacity: 65535 +num_envs: 8 +scene_node_capacity: 262144 headless: true device: cuda:0 arena_space: 0.0 @@ -17,25 +17,25 @@ physics_config: - -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 - mujoco_options: - njmax: 300 - nconmax: 300 - iterations: 100 - ls_iterations: 50 - integrator: implicitfast - separate_worlds: true - tolerance: 1.0e-06 - solver: newton - use_mujoco_contacts: false - cone: pyramidal - impratio: 1.0 - ccd_iterations: 35 - collision_options: - broad_phase: explicit - reduce_contacts: true - max_triangle_pairs: 2500000 + solver_type: mujoco_warp + njmax: 300 + nconmax: 300 + iterations: 100 + ls_iterations: 50 + integrator: implicitfast + separate_worlds: true + tolerance: 1.0e-06 + solver: newton + use_mujoco_contacts: false + cone: pyramidal + impratio: 1.0 + ccd_iterations: 35 env: episode_context_in_info: false sim_steps_per_control: 4 @@ -68,13 +68,13 @@ env: contract: joint_position.default_offset@1 params: {} extensions: - reference_deterministic_reset: false - reference_randomization: true - reference_origin: + deterministic_reset: false + randomization: true + origin: - 0.0 - 0.0 - 0.0 - reference_terrain: G1RoughTerrains/training/terrain.npz + terrain: G1RoughTerrains/training/terrain.npz robot: uid: robot fpath: UnitreeG1Rough/g1.usda diff --git a/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/env.newton.yaml b/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/env.newton.yaml index 0e0ea4518f..e9c1afe057 100644 --- a/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/env.newton.yaml +++ b/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/env.newton.yaml @@ -1,8 +1,8 @@ physics: newton id: UnitreeG1RoughRL-v1 max_episode_steps: 1000 -num_envs: 1 -scene_node_capacity: 65535 +num_envs: 8 +scene_node_capacity: 262144 headless: true device: cuda:0 arena_space: 0.0 @@ -16,26 +16,34 @@ physics_config: - 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: mujoco_warp - njmax: 300 - nconmax: 300 - iterations: 100 - ls_iterations: 50 - integrator: implicitfast - separate_worlds: true - tolerance: 1.0e-06 - solver: newton - use_mujoco_contacts: false - cone: pyramidal - impratio: 1.0 - ccd_iterations: 35 - physics_dt: 0.005 + 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 @@ -68,13 +76,13 @@ env: contract: joint_position.default_offset@1 params: {} extensions: - reference_deterministic_reset: false - reference_randomization: true - reference_origin: + deterministic_reset: false + randomization: true + origin: - 0.0 - 0.0 - 0.0 - reference_terrain: G1RoughTerrains/training/terrain.npz + terrain: G1RoughTerrains/training/terrain.npz robot: uid: robot fpath: UnitreeG1Rough/g1.usda diff --git a/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/README.md b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/README.md deleted file mode 100644 index b5eb769076..0000000000 --- a/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/README.md +++ /dev/null @@ -1,38 +0,0 @@ -# G1 rough-terrain velocity task - -The task controls 37 joints and exposes 310 policy observations, including 187 -terrain height samples. It uses the independent `UnitreeG1Rough` robot archive -and the complete `G1RoughTerrains` curriculum. The existing 29-joint -`UnitreeG1Locomotion` asset remains unchanged. - -Two runnable deployments are packaged under -`extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/`: - -- `env.newton.yaml` (`agents/ppo.newton.yaml`): Newton contact generation with - the MuJoCo Warp solver. -- `env.dexuni.yaml` (`agents/ppo.dexuni.yaml`): Newton contact generation with - the DexUni solver. - -Both deployments use 0.005 s outer physics steps, two solver substeps, four -outer steps per control interval, 100 solver iterations, and 50 line-search -iterations. - -Train through the unified CLI: - -```bash -embodichain train-rl --config extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/agents/ppo.newton.yaml -embodichain train-rl --config extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/agents/ppo.dexuni.yaml -``` - -The default smoke configuration uses 64 environments and 20 updates. Increase -these fields for longer training. Archive availability and the matching DexSim -selected-link property APIs are required. `load_rough_env_cfg` supports explicit -G1 asset bundles for reproducible terrain and initial-state checks. - -The training config resolves its task config relative to the config file, so -the installed configuration also works outside the repository directory. -Contact observations read samples owned by the task's physics-step callback; -reset clears selected rows without querying the invalidated solver snapshot. - -Both deployments select Newton heightfield contacts because native MuJoCo Warp -heightfield collision produces nonfinite states on this terrain. diff --git a/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/environment.py b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/environment.py index 891b92026c..5d35b5be9c 100644 --- a/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/environment.py +++ b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/environment.py @@ -79,21 +79,26 @@ def load_rough_env_cfg( 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 = data["physics_config"]["solver_cfg"][ - "use_mujoco_contacts" - ] - data["physics_config"]["solver_cfg"]["use_mujoco_contacts"] = use_mujoco_contacts + 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["reference_deterministic_reset"] = not training_randomization - extension["reference_randomization"] = training_randomization + extension["deterministic_reset"] = not training_randomization + extension["randomization"] = training_randomization if initial_state_file is not None: if training_randomization: raise ValueError( @@ -132,14 +137,14 @@ def load_rough_env_cfg( position > limits[:, 1] + 1e-7 ): raise ValueError("Initial joint positions exceed the task soft limits.") - extension["reference_initial_states"] = rows + 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["reference_terrain"] = str(terrain) + extension["terrain"] = str(terrain) metadata = json.loads((bundle / "terrain.json").read_text()) if training_randomization: required_keys = ( @@ -152,9 +157,9 @@ def load_rough_env_cfg( "G1 training requires terrain.json with terrain_origins, " "max_init_terrain_level, and generator." ) - extension["reference_terrain_course"] = metadata + extension["terrain_course"] = metadata else: - extension["reference_origin"] = metadata["env_origins"][0] + 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/runtime.py b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/runtime.py index 615c0316e9..8fe96603de 100644 --- a/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/runtime.py +++ b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/runtime.py @@ -53,7 +53,7 @@ class RoughVelocityEnv(EmbodiChainVelocityEnv): """Run rough-terrain velocity locomotion on shared training or evaluation terrain. Deterministic evaluation disables startup mass/COM randomization and - interval pushes. ``reference_randomization=True`` enables those training + 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. @@ -84,11 +84,11 @@ def _prepare_scene(self, **kwargs: Any) -> None: from embodichain.data import get_data_path import json - path = Path(get_data_path(self.cfg.reference_terrain)) - if getattr(self.cfg, "reference_randomization", False) and not getattr( - self.cfg, "reference_terrain_course", None + path = Path(get_data_path(self.cfg.terrain)) + if getattr(self.cfg, "randomization", False) and not getattr( + self.cfg, "terrain_course", None ): - self.cfg.reference_terrain_course = json.loads( + self.cfg.terrain_course = json.loads( path.with_suffix(".json").read_text() ) terrain = np.load(path) @@ -129,7 +129,7 @@ def _init_sim_state(self, **kwargs: Any) -> None: config, self.locomotion_action ) self._reference_origin = self.locomotion_action.new_tensor( - getattr(self.cfg, "reference_origin", (0.0, 0.0, 0.0)) + getattr(self.cfg, "origin", (0.0, 0.0, 0.0)) ) self.compute_rewards_fn = partial( compute_rewards, constants=self._reference_constants @@ -195,18 +195,18 @@ def _init_sim_state(self, **kwargs: Any) -> None: self._reference_initial_states = ( { name: torch.tensor( - [row[name] for row in self.cfg.reference_initial_states], + [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, "reference_initial_states", None) is not None + 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, "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"], @@ -221,7 +221,7 @@ def _init_sim_state(self, **kwargs: Any) -> None: self.max_episode_steps * self.step_dt, self._locomotion_generator, ) - if getattr(self.cfg, "reference_randomization", False): + if getattr(self.cfg, "randomization", False): self._randomize_startup() self._reference_events_deferred = False @@ -372,8 +372,8 @@ def _update_heading_commands(self) -> None: super()._update_heading_commands() def _apply_reference_interval_events(self) -> None: - """Apply reference velocity pushes after rewards and episode resets.""" - if getattr(self.cfg, "reference_randomization", False): + """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(): @@ -528,7 +528,7 @@ def _initialize_episode( super()._initialize_episode(env_ids=env_ids, **kwargs) finally: self._reference_events_deferred = previous_deferred - deterministic = getattr(self.cfg, "reference_deterministic_reset", True) + deterministic = getattr(self.cfg, "deterministic_reset", True) pose = self._reference_reset_pose self._reference_reset_pose = None if deterministic: @@ -541,9 +541,7 @@ def _initialize_episode( else getattr(self, "_reference_origin", None) ) if origin is None: - origin = pose.new_tensor( - getattr(self.cfg, "reference_origin", (0.0, 0.0, 0.0)) - ) + 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 = ( diff --git a/tests/gym/envs/tasks/test_rough_velocity.py b/tests/gym/envs/tasks/test_rough_velocity.py index 77b45721b7..040a4d8b62 100644 --- a/tests/gym/envs/tasks/test_rough_velocity.py +++ b/tests/gym/envs/tasks/test_rough_velocity.py @@ -361,6 +361,29 @@ def test_ground_replacement_updates_declared_geometry() -> None: 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) + 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 + + 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, @@ -386,14 +409,14 @@ def test_load_rough_env_cfg_resolves_training_and_evaluation_bundles( 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["reference_deterministic_reset"] is True - assert eval_cfg.extensions["reference_origin"] == [1.0, 2.0, 0.5] + 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["reference_randomization"] is True - assert "terrain_origins" in train_cfg.extensions["reference_terrain_course"] + assert train_cfg.extensions["randomization"] is True + assert "terrain_origins" in train_cfg.extensions["terrain_course"] @pytest.mark.no_sim @@ -719,7 +742,7 @@ def make_env(legacy: bool): env._device = torch.device("cpu") env._num_envs = 4 env.sim = SimpleNamespace(device=env._device) - env.cfg = SimpleNamespace(reference_deterministic_reset=False) + 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 From c0d1b55741f2ae9bf0e7fa171d166bdb67032eb1 Mon Sep 17 00:00:00 2001 From: acrlw <13927622+acrlw@users.noreply.github.com> Date: Fri, 13 Nov 2026 20:25:26 +0800 Subject: [PATCH 20/24] fix(sim): preserve default_plane in asset_uids unless remove_ground_plane is called --- embodichain/lab/sim/sim_manager.py | 4 +++- tests/gym/envs/tasks/test_rough_velocity.py | 17 ++++++++++++++++- 2 files changed, 19 insertions(+), 2 deletions(-) diff --git a/embodichain/lab/sim/sim_manager.py b/embodichain/lab/sim/sim_manager.py index eb42f8ff78..63e127e5c3 100644 --- a/embodichain/lab/sim/sim_manager.py +++ b/embodichain/lab/sim/sim_manager.py @@ -1102,7 +1102,7 @@ def asset_uids(self) -> List[str]: uid_list = ( ["default_plane"] if getattr(self.sim_config, "enable_ground_plane", True) - and "default_plane" in self._spawn_scene + and not getattr(self, "_ground_plane_removed", False) else [] ) uid_list.extend(list(self._lights.keys())) @@ -2942,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 @@ -3034,6 +3035,7 @@ def remove_ground_plane(self) -> None: 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. diff --git a/tests/gym/envs/tasks/test_rough_velocity.py b/tests/gym/envs/tasks/test_rough_velocity.py index 040a4d8b62..afda6f2aab 100644 --- a/tests/gym/envs/tasks/test_rough_velocity.py +++ b/tests/gym/envs/tasks/test_rough_velocity.py @@ -37,7 +37,7 @@ ) from embodichain.lab.gym.envs.episode_context import EpisodeContext -from embodichain.lab.sim.sim_manager import SimulationManager +from embodichain.lab.sim.sim_manager import SimulationManager, SimulationManagerCfg from embodichain_tasks.locomotion.velocity.rough.curriculum import ( TerrainCurriculum, ) @@ -335,6 +335,7 @@ def test_ground_replacement_updates_declared_geometry() -> None: ) declared = copy(descriptor) sim = object.__new__(SimulationManager) + sim.sim_config = SimulationManagerCfg() sim._spawn_default_plane_descriptor = descriptor scene = Mock() scene.builder.is_finalized = False @@ -367,6 +368,19 @@ def test_remove_ground_plane_before_prepare() -> None: 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 = () @@ -376,6 +390,7 @@ def test_remove_ground_plane_before_prepare() -> None: 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()) From dcd053dfc960fa4db53baa16485d41cd35c552fe Mon Sep 17 00:00:00 2001 From: acrlw <13927622+acrlw@users.noreply.github.com> Date: Fri, 13 Nov 2026 22:26:29 +0800 Subject: [PATCH 21/24] Unify DexUni task configs and prune offline audit/fixture bloat - Collapse g1_rough and the five dexterous manipulation tasks to a single default deployment (env.yaml + agents/ppo.yaml) running Newton DexUni on GPU MJWarp CUDA, removing 12 duplicate *.newton.yaml and *.mjwarp.yaml files. - Delete unused offline MJCF migration audit helpers (_audit_model.py, _audit_tendons.py) and redundant category README.md. - Replace recorded JSON snapshot fixtures (g1_rough.json, terrain_curriculum.json) with deterministic analytical MDP and curriculum unit tests in test_rough_velocity.py. - Update RL Sphinx documentation, agent_context, and leaf task READMEs to match the single-deployment layout and 37/310/310 G1 rough dimensions. --- .../topics/rl-learning/rl-learning.md | 10 +- docs/source/overview/rl/dexterous.md | 17 +- docs/source/overview/rl/locomotion.md | 17 +- .../locomotion/velocity/g1_rough/README.md | 17 +- .../velocity/g1_rough/agents/ppo.newton.yaml | 59 - .../agents/{ppo.mjwarp.yaml => ppo.yaml} | 4 +- .../locomotion/velocity/g1_rough/catalog.yaml | 11 +- .../velocity/g1_rough/env.mjwarp.yaml | 190 - .../g1_rough/{env.newton.yaml => env.yaml} | 0 .../tasks/manipulation/dexterous/README.md | 28 - .../dexterous/leap_cube_face_turn/README.md | 14 +- .../agents/ppo.newton.yaml | 55 - .../agents/{ppo.mjwarp.yaml => ppo.yaml} | 4 +- .../leap_cube_face_turn/catalog.yaml | 8 +- .../leap_cube_face_turn/env.mjwarp.yaml | 55 - .../{env.newton.yaml => env.yaml} | 0 .../dexterous/leap_cube_reorient/README.md | 14 +- .../leap_cube_reorient/agents/ppo.newton.yaml | 59 - .../agents/{ppo.mjwarp.yaml => ppo.yaml} | 4 +- .../dexterous/leap_cube_reorient/catalog.yaml | 8 +- .../leap_cube_reorient/env.mjwarp.yaml | 55 - .../{env.newton.yaml => env.yaml} | 0 .../dexterous/leap_cube_rotate_z/README.md | 14 +- .../leap_cube_rotate_z/agents/ppo.newton.yaml | 55 - .../agents/{ppo.mjwarp.yaml => ppo.yaml} | 4 +- .../dexterous/leap_cube_rotate_z/catalog.yaml | 8 +- .../leap_cube_rotate_z/env.mjwarp.yaml | 55 - .../{env.newton.yaml => env.yaml} | 0 .../dexterous/shadow_pen_continuous/README.md | 14 +- .../agents/ppo.newton.yaml | 59 - .../agents/{ppo.mjwarp.yaml => ppo.yaml} | 4 +- .../shadow_pen_continuous/catalog.yaml | 8 +- .../shadow_pen_continuous/env.mjwarp.yaml | 55 - .../{env.newton.yaml => env.yaml} | 0 .../dexterous/shadow_pen_fixed/README.md | 14 +- .../shadow_pen_fixed/agents/ppo.newton.yaml | 55 - .../agents/{ppo.mjwarp.yaml => ppo.yaml} | 4 +- .../dexterous/shadow_pen_fixed/catalog.yaml | 8 +- .../shadow_pen_fixed/env.mjwarp.yaml | 55 - .../{env.newton.yaml => env.yaml} | 0 .../locomotion/velocity/rough/environment.py | 2 +- .../manipulation/dexterous/_audit_model.py | 640 --- .../manipulation/dexterous/_audit_tendons.py | 186 - tests/gym/envs/tasks/fixtures/g1_rough.json | 4697 ----------------- .../tasks/fixtures/terrain_curriculum.json | 3566 ------------- tests/gym/envs/tasks/test_dexterous_usd.py | 191 +- tests/gym/envs/tasks/test_rough_velocity.py | 209 +- 47 files changed, 277 insertions(+), 10255 deletions(-) delete mode 100644 extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/agents/ppo.newton.yaml rename extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/agents/{ppo.mjwarp.yaml => ppo.yaml} (93%) delete mode 100644 extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/env.mjwarp.yaml rename extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/{env.newton.yaml => env.yaml} (100%) delete mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/README.md delete mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_face_turn/agents/ppo.newton.yaml rename extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_face_turn/agents/{ppo.mjwarp.yaml => ppo.yaml} (91%) delete mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_face_turn/env.mjwarp.yaml rename extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_face_turn/{env.newton.yaml => env.yaml} (100%) delete mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_reorient/agents/ppo.newton.yaml rename extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_reorient/agents/{ppo.mjwarp.yaml => ppo.yaml} (92%) delete mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_reorient/env.mjwarp.yaml rename extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_reorient/{env.newton.yaml => env.yaml} (100%) delete mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_rotate_z/agents/ppo.newton.yaml rename extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_rotate_z/agents/{ppo.mjwarp.yaml => ppo.yaml} (91%) delete mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_rotate_z/env.mjwarp.yaml rename extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_rotate_z/{env.newton.yaml => env.yaml} (100%) delete mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/agents/ppo.newton.yaml rename extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/agents/{ppo.mjwarp.yaml => ppo.yaml} (91%) delete mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/env.mjwarp.yaml rename extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/{env.newton.yaml => env.yaml} (100%) delete mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/agents/ppo.newton.yaml rename extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/agents/{ppo.mjwarp.yaml => ppo.yaml} (91%) delete mode 100644 extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/env.mjwarp.yaml rename extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/{env.newton.yaml => env.yaml} (100%) delete mode 100644 extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_audit_model.py delete mode 100644 extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_audit_tendons.py delete mode 100644 tests/gym/envs/tasks/fixtures/g1_rough.json delete mode 100644 tests/gym/envs/tasks/fixtures/terrain_curriculum.json diff --git a/agent_context/topics/rl-learning/rl-learning.md b/agent_context/topics/rl-learning/rl-learning.md index 1b2c15d42e..8576d2aacd 100644 --- a/agent_context/topics/rl-learning/rl-learning.md +++ b/agent_context/topics/rl-learning/rl-learning.md @@ -217,14 +217,14 @@ detachment, and reset/terminal issues to the selected environment protocol. `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 -defaults to `newton` (`solver_type: dexuni` with Newton heightfield contacts) -alongside `mjwarp` (`solver_type: mujoco_warp`). +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 defaults to `newton` (`solver_type: dexuni`, GPU MJWarp CUDA) -and also ships `mjwarp` (`solver_type: mujoco_warp`). Offline physical-model and -tendon parity helpers live in `_audit_model.py` and `_audit_tendons.py`. +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/docs/source/overview/rl/dexterous.md b/docs/source/overview/rl/dexterous.md index 9c44c09ace..c9a396fd47 100644 --- a/docs/source/overview/rl/dexterous.md +++ b/docs/source/overview/rl/dexterous.md @@ -10,18 +10,15 @@ Five USD-native dexterous manipulation tasks inherit from `EmbodiedEnv` (`Dexter | `leap_cube_reorient` | `LeapCubeReorientRL-v1` | 16 | 57 / 128 | `LeapHandCube` | | `leap_cube_rotate_z` | `LeapCubeRotateZAxisRL-v1` | 16 | 32 / 105 | `LeapHandCube` | -## Deployments and CLI Usage +## Configuration and CLI Usage -Each task directory under `extensions/embodichain_tasks/configs/tasks/manipulation/dexterous//` provides two catalog deployments: - -- `newton` (default): `env.newton.yaml` + `agents/ppo.newton.yaml`, running `solver_type: dexuni` (`use_mujoco_cpu: false`, GPU MuJoCo Warp CUDA with full scene joint ownership). -- `mjwarp`: `env.mjwarp.yaml` + `agents/ppo.mjwarp.yaml`, running `solver_type: mujoco_warp`. +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.newton.yaml -embodichain train-rl --config extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/agents/ppo.newton.yaml +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`. @@ -34,13 +31,13 @@ Evaluation reports `eval/success_rate` for `ShadowPenFixedRL-v1`, `ShadowPenCont ## 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`. Source MJCF files in the asset archives are used only by offline verification tests (`_audit_model.py`, `_audit_tendons.py`) to verify mass, inertia, joint, actuator, tendon, and collision-graph parity against active GPU solver buffers. +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). Both `newton` (`dexuni`) and `mjwarp` (`mujoco_warp`) deployments preserve authored solver settings (`20/50` Newton/line-search iterations on Shadow Hand tasks; `5/8` on LEAP Hand tasks) and set `arena_space: 0.0` with `separate_worlds: true` so execution-model geometry stays spatially uniform across parallel worlds. +`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 the default `newton` (`solver_type: dexuni`, `4096` parallel environments, single NVIDIA RTX PRO 5000 Blackwell 72 GB GPU): +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`) | |---|---:|---:|---:|---|---| diff --git a/docs/source/overview/rl/locomotion.md b/docs/source/overview/rl/locomotion.md index e3b79597d6..84b370be3f 100644 --- a/docs/source/overview/rl/locomotion.md +++ b/docs/source/overview/rl/locomotion.md @@ -5,7 +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 / 325 | +| `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 | @@ -14,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 @@ -49,14 +49,11 @@ Humanoid uses clipped effort actions (`[-0.4, 0.4]`) multiplied by per-joint mot ## G1 rough terrain with DexUni -`g1_rough/env.newton.yaml` (`g1_rough/agents/ppo.newton.yaml`) is the default -deployment for `UnitreeG1RoughRL-v1` and selects `solver_type: 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`) with Newton explicit heightfield contact generation +`use_mujoco_contacts: false`) and Newton explicit heightfield contact generation (`broad_phase: explicit`, `reduce_contacts: true`, `max_triangle_pairs: 2500000`). -`g1_rough/env.mjwarp.yaml` (`g1_rough/agents/ppo.mjwarp.yaml`) provides the direct -`solver_type: mujoco_warp` deployment under identical terrain, timestep, and -solver-iteration parameters. The separate `UnitreeG1Rough` and `G1RoughTerrains` asset packages provide the 37-joint model and terrain inputs. They do not replace `UnitreeG1Locomotion`. @@ -71,14 +68,14 @@ 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. -The G1 rough deployments set `env.episode_context_in_info: false` to avoid +`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.newton.yaml` (`150 + 10` resumed updates, +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`, 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 index cd56355624..8d85dc87ab 100644 --- a/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/README.md +++ b/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/README.md @@ -3,27 +3,22 @@ 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`. -| Deployment | Environment | PPO configuration | -|---|---|---| -| `newton` (DexUni, recommended) | [env.newton.yaml](env.newton.yaml) | [agents/ppo.newton.yaml](agents/ppo.newton.yaml) | -| `mjwarp` (MuJoCo Warp) | [env.mjwarp.yaml](env.mjwarp.yaml) | [agents/ppo.mjwarp.yaml](agents/ppo.mjwarp.yaml) | +[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. -Each PPO file's `trainer.gym_config` selects its matching deployment. The catalog defaults to `newton`, which runs `DexUni` (`joint_mode: dynamic`, `contact_mode: auto`, `use_mujoco_cpu: false`, `use_mujoco_contacts: false`) with Newton explicit heightfield contact generation (`broad_phase: explicit`, `reduce_contacts: true`, `max_triangle_pairs: 2500000`). Both deployments use 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 consumes the 37-DOF `UnitreeG1Rough` robot archive (`g1.usda`) and the `G1RoughTerrains` curriculum (`training/terrain.npz`). The 29-DOF `UnitreeG1Locomotion` flat-terrain asset remains unchanged. +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.newton.yaml -embodichain train-rl --config extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/agents/ppo.newton.yaml +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 matching training and environment snapshots: +Evaluate a saved checkpoint with its training run directory: ```bash -embodichain eval-policy runs/ --checkpoint latest --num-envs 64 --episodes 64 --renderer no-render +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.newton.yaml b/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/agents/ppo.newton.yaml deleted file mode 100644 index 66c86de419..0000000000 --- a/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/agents/ppo.newton.yaml +++ /dev/null @@ -1,59 +0,0 @@ -trainer: - exp_name: unitree_g1_rough_ppo_newton - gym_config: extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/env.newton.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/agents/ppo.mjwarp.yaml b/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/agents/ppo.yaml similarity index 93% rename from extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/agents/ppo.mjwarp.yaml rename to extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/agents/ppo.yaml index dd412a61ba..5b54ff7488 100644 --- a/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/agents/ppo.mjwarp.yaml +++ b/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/agents/ppo.yaml @@ -1,6 +1,6 @@ trainer: - exp_name: unitree_g1_rough_ppo_mjwarp - gym_config: extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/env.mjwarp.yaml + 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 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 index 12fb098f40..770e5709eb 100644 --- a/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/catalog.yaml +++ b/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/catalog.yaml @@ -1,13 +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. +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: newton +default_deployment: default deployments: - newton: - config: env.newton.yaml - mjwarp: - config: env.mjwarp.yaml + default: + config: env.yaml diff --git a/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/env.mjwarp.yaml b/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/env.mjwarp.yaml deleted file mode 100644 index 1713a9548a..0000000000 --- a/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/env.mjwarp.yaml +++ /dev/null @@ -1,190 +0,0 @@ -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: mujoco_warp - njmax: 300 - nconmax: 300 - iterations: 100 - ls_iterations: 50 - integrator: implicitfast - separate_worlds: true - tolerance: 1.0e-06 - solver: newton - use_mujoco_contacts: false - cone: pyramidal - impratio: 1.0 - ccd_iterations: 35 -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/env.newton.yaml b/extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/env.yaml similarity index 100% rename from extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/env.newton.yaml rename to extensions/embodichain_tasks/configs/tasks/locomotion/velocity/g1_rough/env.yaml diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/README.md b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/README.md deleted file mode 100644 index d79fb519d1..0000000000 --- a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/README.md +++ /dev/null @@ -1,28 +0,0 @@ -# Dexterous Manipulation Tasks (USD-Native `EmbodiedEnv`) - -All five dexterous manipulation RL tasks construct their runtime simulation scenes exclusively from USD (`.usdc`) via `SimulationManager.add_usd()` on `EmbodiedEnv`. Runtime execution has zero dependency on MJCF `.xml` files; source MJCF files included in the asset archives are retained solely for offline physical-model and tendon parity audits. - -| Task key | Gym ID | Archive | Runtime USD | Initial state | -|---|---|---|---|---| -| `shadow_pen_fixed` | `ShadowPenFixedRL-v1` | `ShadowHandPen` | `usd/shadow_pen.usdc` | `ShadowPenSpin-initial.npz` | -| `shadow_pen_continuous` | `ShadowPenContinuousRL-v1` | `ShadowHandPen` | `usd/shadow_pen.usdc` | `ShadowPenSpin-initial.npz` | -| `leap_cube_face_turn` | `LeapCubeFaceTurnRL-v1` | `LeapHandCubeFaceTurn` | `usd/leap_cube_top_layer.usdc` | `LeapCubeFaceTurn-initial.npz` | -| `leap_cube_reorient` | `LeapCubeReorientRL-v1` | `LeapHandCube` | `usd/M4.usdc` | `M4-initial.npz` | -| `leap_cube_rotate_z` | `LeapCubeRotateZAxisRL-v1` | `LeapHandCube` | `usd/M5.usdc` | `M5-initial.npz` | - -The asset archives are published under [`DexForceAI/embodichain_data/robot_assets`](https://huggingface.co/datasets/DexForceAI/embodichain_data/tree/e1ebe53462d6f48b5961018e625b7c1304e9e83a/robot_assets) and verified by SHA-256 checksums in `embodichain.data.assets.robot_assets`. - -## Deployments and CLI Usage - -Every task ships two deployments in its catalog: - -- `newton` (default, recommended): `env.newton.yaml` + `agents/ppo.newton.yaml`, using `solver_type: dexuni` (`use_mujoco_cpu: false`, GPU MuJoCo Warp CUDA with full scene joint ownership so tendons, fixed-base hands, and free/hinged objects step inside a single CUDA-graphed solver). -- `mjwarp`: `env.mjwarp.yaml` + `agents/ppo.mjwarp.yaml`, using `solver_type: mujoco_warp`. - -```bash -embodichain list-task -embodichain show-task embodichain_tasks:shadow_pen_fixed -embodichain run-task --gym_config extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/env.newton.yaml -embodichain train-rl --config extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/agents/ppo.newton.yaml -embodichain eval-policy runs/ --checkpoint latest --num-envs 64 --episodes 64 --renderer no-render -``` 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 index ad64f42143..aee086b69e 100644 --- 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 @@ -3,18 +3,12 @@ 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`. -| Deployment | Environment | PPO configuration | -|---|---|---| -| `newton` (DexUni, default) | [env.newton.yaml](env.newton.yaml) | [agents/ppo.newton.yaml](agents/ppo.newton.yaml) | -| `mjwarp` (MuJoCo Warp) | [env.mjwarp.yaml](env.mjwarp.yaml) | [agents/ppo.mjwarp.yaml](agents/ppo.mjwarp.yaml) | - -The scene is loaded directly from LeapHandCubeFaceTurn (`usd/leap_cube_top_layer.usdc`) via `SimulationManager.add_usd()`. -`newton` selects `solver_type: dexuni` and dispatches all scene articulations and free bodies to GPU MuJoCo Warp on CUDA (`use_mujoco_cpu: false`, `use_mujoco_contacts: true`); `mjwarp` selects `solver_type: mujoco_warp` directly. +[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.newton.yaml -embodichain train-rl --config extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_face_turn/agents/ppo.newton.yaml +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 @@ -22,7 +16,7 @@ embodichain train-rl --config extensions/embodichain_tasks/configs/tasks/manipul Evaluate a saved checkpoint with its training run directory: ```bash -embodichain eval-policy outputs/leap_cube_face_turn_ppo_newton/ --checkpoint latest --episodes 32 --num-envs 32 --renderer no-render +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.newton.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_face_turn/agents/ppo.newton.yaml deleted file mode 100644 index 69bce22722..0000000000 --- a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_face_turn/agents/ppo.newton.yaml +++ /dev/null @@ -1,55 +0,0 @@ -trainer: - exp_name: leap_cube_face_turn_ppo_newton - gym_config: extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_face_turn/env.newton.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/agents/ppo.mjwarp.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_face_turn/agents/ppo.yaml similarity index 91% rename from extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_face_turn/agents/ppo.mjwarp.yaml rename to extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_face_turn/agents/ppo.yaml index e992e21741..042a7a778c 100644 --- a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_face_turn/agents/ppo.mjwarp.yaml +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_face_turn/agents/ppo.yaml @@ -1,6 +1,6 @@ trainer: - exp_name: leap_cube_face_turn_ppo_mjwarp - gym_config: extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_face_turn/env.mjwarp.yaml + 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 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 index 539efb1e43..4277a56390 100644 --- 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 @@ -6,9 +6,7 @@ tags: - manipulation - dexterous - reinforcement_learning -default_deployment: newton +default_deployment: default deployments: - newton: - config: env.newton.yaml - mjwarp: - config: env.mjwarp.yaml + default: + config: env.yaml diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_face_turn/env.mjwarp.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_face_turn/env.mjwarp.yaml deleted file mode 100644 index b6d469d8a9..0000000000 --- a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_face_turn/env.mjwarp.yaml +++ /dev/null @@ -1,55 +0,0 @@ -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: mujoco_warp - 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_face_turn/env.newton.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_face_turn/env.yaml similarity index 100% rename from extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_face_turn/env.newton.yaml rename to extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_face_turn/env.yaml 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 index 3e8195c9ac..af9a62ae19 100644 --- 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 @@ -3,18 +3,12 @@ Reorient a rigid cube in-hand to consecutive 3D target orientations with the 16-DOF LEAP Hand. Gym ID: `LeapCubeReorientRL-v1`. -| Deployment | Environment | PPO configuration | -|---|---|---| -| `newton` (DexUni, default) | [env.newton.yaml](env.newton.yaml) | [agents/ppo.newton.yaml](agents/ppo.newton.yaml) | -| `mjwarp` (MuJoCo Warp) | [env.mjwarp.yaml](env.mjwarp.yaml) | [agents/ppo.mjwarp.yaml](agents/ppo.mjwarp.yaml) | - -The scene is loaded directly from LeapHandCube (`usd/M4.usdc`) via `SimulationManager.add_usd()`. -`newton` selects `solver_type: dexuni` and dispatches all scene articulations and free bodies to GPU MuJoCo Warp on CUDA (`use_mujoco_cpu: false`, `use_mujoco_contacts: true`); `mjwarp` selects `solver_type: mujoco_warp` directly. +[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.newton.yaml -embodichain train-rl --config extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_reorient/agents/ppo.newton.yaml +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 @@ -22,7 +16,7 @@ embodichain train-rl --config extensions/embodichain_tasks/configs/tasks/manipul Evaluate a saved checkpoint with its training run directory: ```bash -embodichain eval-policy outputs/leap_cube_reorient_ppo_newton/ --checkpoint latest --episodes 32 --num-envs 32 --renderer no-render +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.newton.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_reorient/agents/ppo.newton.yaml deleted file mode 100644 index 3ed19fbb43..0000000000 --- a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_reorient/agents/ppo.newton.yaml +++ /dev/null @@ -1,59 +0,0 @@ -trainer: - exp_name: leap_cube_reorient_ppo_newton - gym_config: extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_reorient/env.newton.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/agents/ppo.mjwarp.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_reorient/agents/ppo.yaml similarity index 92% rename from extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_reorient/agents/ppo.mjwarp.yaml rename to extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_reorient/agents/ppo.yaml index 3110238c85..a773ded6eb 100644 --- a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_reorient/agents/ppo.mjwarp.yaml +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_reorient/agents/ppo.yaml @@ -1,6 +1,6 @@ trainer: - exp_name: leap_cube_reorient_ppo_mjwarp - gym_config: extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_reorient/env.mjwarp.yaml + 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 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 index 046f680a6c..ba977c44a7 100644 --- 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 @@ -6,9 +6,7 @@ tags: - manipulation - dexterous - reinforcement_learning -default_deployment: newton +default_deployment: default deployments: - newton: - config: env.newton.yaml - mjwarp: - config: env.mjwarp.yaml + default: + config: env.yaml diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_reorient/env.mjwarp.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_reorient/env.mjwarp.yaml deleted file mode 100644 index fd70d478c6..0000000000 --- a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_reorient/env.mjwarp.yaml +++ /dev/null @@ -1,55 +0,0 @@ -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: mujoco_warp - 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_reorient/env.newton.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_reorient/env.yaml similarity index 100% rename from extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_reorient/env.newton.yaml rename to extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_reorient/env.yaml 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 index 108b315bd0..aec5c75063 100644 --- 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 @@ -3,18 +3,12 @@ Continuously rotate a rigid cube around the world Z-axis in-hand with the 16-DOF LEAP Hand. Gym ID: `LeapCubeRotateZAxisRL-v1`. -| Deployment | Environment | PPO configuration | -|---|---|---| -| `newton` (DexUni, default) | [env.newton.yaml](env.newton.yaml) | [agents/ppo.newton.yaml](agents/ppo.newton.yaml) | -| `mjwarp` (MuJoCo Warp) | [env.mjwarp.yaml](env.mjwarp.yaml) | [agents/ppo.mjwarp.yaml](agents/ppo.mjwarp.yaml) | - -The scene is loaded directly from LeapHandCube (`usd/M5.usdc`) via `SimulationManager.add_usd()`. -`newton` selects `solver_type: dexuni` and dispatches all scene articulations and free bodies to GPU MuJoCo Warp on CUDA (`use_mujoco_cpu: false`, `use_mujoco_contacts: true`); `mjwarp` selects `solver_type: mujoco_warp` directly. +[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.newton.yaml -embodichain train-rl --config extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_rotate_z/agents/ppo.newton.yaml +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 @@ -22,7 +16,7 @@ embodichain train-rl --config extensions/embodichain_tasks/configs/tasks/manipul Evaluate a saved checkpoint with its training run directory: ```bash -embodichain eval-policy outputs/leap_cube_rotate_z_ppo_newton/ --checkpoint latest --episodes 32 --num-envs 32 --renderer no-render +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.newton.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_rotate_z/agents/ppo.newton.yaml deleted file mode 100644 index 83cf0133b6..0000000000 --- a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_rotate_z/agents/ppo.newton.yaml +++ /dev/null @@ -1,55 +0,0 @@ -trainer: - exp_name: leap_cube_rotate_z_ppo_newton - gym_config: extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_rotate_z/env.newton.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/agents/ppo.mjwarp.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_rotate_z/agents/ppo.yaml similarity index 91% rename from extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_rotate_z/agents/ppo.mjwarp.yaml rename to extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_rotate_z/agents/ppo.yaml index 26f114984e..b9443c1dd1 100644 --- a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_rotate_z/agents/ppo.mjwarp.yaml +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_rotate_z/agents/ppo.yaml @@ -1,6 +1,6 @@ trainer: - exp_name: leap_cube_rotate_z_ppo_mjwarp - gym_config: extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_rotate_z/env.mjwarp.yaml + 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 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 index 5cbb49bc56..c5546261fe 100644 --- 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 @@ -6,9 +6,7 @@ tags: - manipulation - dexterous - reinforcement_learning -default_deployment: newton +default_deployment: default deployments: - newton: - config: env.newton.yaml - mjwarp: - config: env.mjwarp.yaml + default: + config: env.yaml diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_rotate_z/env.mjwarp.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_rotate_z/env.mjwarp.yaml deleted file mode 100644 index 2b9df70b46..0000000000 --- a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_rotate_z/env.mjwarp.yaml +++ /dev/null @@ -1,55 +0,0 @@ -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: mujoco_warp - 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/env.newton.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_rotate_z/env.yaml similarity index 100% rename from extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_rotate_z/env.newton.yaml rename to extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/leap_cube_rotate_z/env.yaml 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 index 586ceb4b42..90657284ef 100644 --- 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 @@ -3,18 +3,12 @@ Continuously spin a pen through sequential orientation targets with the 24-DOF Shadow Hand. Gym ID: `ShadowPenContinuousRL-v1`. -| Deployment | Environment | PPO configuration | -|---|---|---| -| `newton` (DexUni, default) | [env.newton.yaml](env.newton.yaml) | [agents/ppo.newton.yaml](agents/ppo.newton.yaml) | -| `mjwarp` (MuJoCo Warp) | [env.mjwarp.yaml](env.mjwarp.yaml) | [agents/ppo.mjwarp.yaml](agents/ppo.mjwarp.yaml) | - -The scene is loaded directly from ShadowHandPen (`usd/shadow_pen.usdc`) via `SimulationManager.add_usd()`. -`newton` selects `solver_type: dexuni` and dispatches all scene articulations and free bodies to GPU MuJoCo Warp on CUDA (`use_mujoco_cpu: false`, `use_mujoco_contacts: true`); `mjwarp` selects `solver_type: mujoco_warp` directly. +[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.newton.yaml -embodichain train-rl --config extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/agents/ppo.newton.yaml +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 @@ -22,7 +16,7 @@ embodichain train-rl --config extensions/embodichain_tasks/configs/tasks/manipul Evaluate a saved checkpoint with its training run directory: ```bash -embodichain eval-policy outputs/shadow_pen_continuous_ppo_newton/ --checkpoint latest --episodes 32 --num-envs 32 --renderer no-render +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.newton.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/agents/ppo.newton.yaml deleted file mode 100644 index e7fb589a22..0000000000 --- a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/agents/ppo.newton.yaml +++ /dev/null @@ -1,59 +0,0 @@ -trainer: - exp_name: shadow_pen_continuous_ppo_newton - gym_config: extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/env.newton.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/agents/ppo.mjwarp.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/agents/ppo.yaml similarity index 91% rename from extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/agents/ppo.mjwarp.yaml rename to extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/agents/ppo.yaml index 64e8e63c16..0ac124562c 100644 --- a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/agents/ppo.mjwarp.yaml +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/agents/ppo.yaml @@ -1,6 +1,6 @@ trainer: - exp_name: shadow_pen_continuous_ppo_mjwarp - gym_config: extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/env.mjwarp.yaml + 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 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 index 3ec27fc75f..25963f3f2a 100644 --- 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 @@ -6,9 +6,7 @@ tags: - manipulation - dexterous - reinforcement_learning -default_deployment: newton +default_deployment: default deployments: - newton: - config: env.newton.yaml - mjwarp: - config: env.mjwarp.yaml + default: + config: env.yaml diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/env.mjwarp.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/env.mjwarp.yaml deleted file mode 100644 index e45d3e1770..0000000000 --- a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/env.mjwarp.yaml +++ /dev/null @@ -1,55 +0,0 @@ -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: mujoco_warp - 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_continuous/env.newton.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/env.yaml similarity index 100% rename from extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/env.newton.yaml rename to extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_continuous/env.yaml 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 index fc5e38bf0e..51e8d77ba1 100644 --- 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 @@ -3,18 +3,12 @@ Spin a pen in-hand to sampled target orientations with the 24-DOF Shadow Hand. Gym ID: `ShadowPenFixedRL-v1`. -| Deployment | Environment | PPO configuration | -|---|---|---| -| `newton` (DexUni, default) | [env.newton.yaml](env.newton.yaml) | [agents/ppo.newton.yaml](agents/ppo.newton.yaml) | -| `mjwarp` (MuJoCo Warp) | [env.mjwarp.yaml](env.mjwarp.yaml) | [agents/ppo.mjwarp.yaml](agents/ppo.mjwarp.yaml) | - -The scene is loaded directly from ShadowHandPen (`usd/shadow_pen.usdc`) via `SimulationManager.add_usd()`. -`newton` selects `solver_type: dexuni` and dispatches all scene articulations and free bodies to GPU MuJoCo Warp on CUDA (`use_mujoco_cpu: false`, `use_mujoco_contacts: true`); `mjwarp` selects `solver_type: mujoco_warp` directly. +[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.newton.yaml -embodichain train-rl --config extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/agents/ppo.newton.yaml +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 @@ -22,7 +16,7 @@ embodichain train-rl --config extensions/embodichain_tasks/configs/tasks/manipul Evaluate a saved checkpoint with its training run directory: ```bash -embodichain eval-policy outputs/shadow_pen_fixed_ppo_newton/ --checkpoint latest --episodes 32 --num-envs 32 --renderer no-render +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.newton.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/agents/ppo.newton.yaml deleted file mode 100644 index 3eacfdc31d..0000000000 --- a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/agents/ppo.newton.yaml +++ /dev/null @@ -1,55 +0,0 @@ -trainer: - exp_name: shadow_pen_fixed_ppo_newton - gym_config: extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/env.newton.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/agents/ppo.mjwarp.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/agents/ppo.yaml similarity index 91% rename from extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/agents/ppo.mjwarp.yaml rename to extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/agents/ppo.yaml index d2d165ae66..d94aebc011 100644 --- a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/agents/ppo.mjwarp.yaml +++ b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/agents/ppo.yaml @@ -1,6 +1,6 @@ trainer: - exp_name: shadow_pen_fixed_ppo_mjwarp - gym_config: extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/env.mjwarp.yaml + 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 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 index 3dd482046b..4ba04cffee 100644 --- 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 @@ -6,9 +6,7 @@ tags: - manipulation - dexterous - reinforcement_learning -default_deployment: newton +default_deployment: default deployments: - newton: - config: env.newton.yaml - mjwarp: - config: env.mjwarp.yaml + default: + config: env.yaml diff --git a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/env.mjwarp.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/env.mjwarp.yaml deleted file mode 100644 index ca38e382e8..0000000000 --- a/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/env.mjwarp.yaml +++ /dev/null @@ -1,55 +0,0 @@ -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: mujoco_warp - 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/env.newton.yaml b/extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/env.yaml similarity index 100% rename from extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/env.newton.yaml rename to extensions/embodichain_tasks/configs/tasks/manipulation/dexterous/shadow_pen_fixed/env.yaml diff --git a/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/environment.py b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/environment.py index 5d35b5be9c..1969837126 100644 --- a/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/environment.py +++ b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/environment.py @@ -74,7 +74,7 @@ def load_rough_env_cfg( if not asset.is_file(): raise FileNotFoundError(asset) resource = files("embodichain_tasks.configs").joinpath( - "tasks", "locomotion", "velocity", tasks[robot], "env.newton.yaml" + "tasks", "locomotion", "velocity", tasks[robot], "env.yaml" ) data = yaml.safe_load(resource.read_text()) data["num_envs"] = num_envs diff --git a/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_audit_model.py b/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_audit_model.py deleted file mode 100644 index 574fbcf497..0000000000 --- a/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_audit_model.py +++ /dev/null @@ -1,640 +0,0 @@ -# ---------------------------------------------------------------------------- -# 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. -# ---------------------------------------------------------------------------- - -"""CPU comparison of source and solver-exported manipulation MJCF models.""" - -from __future__ import annotations - -import copy - -from collections.abc import Mapping, Sequence -import re -from typing import Any - -import mujoco -import numpy as np - -__all__ = ["audit_model", "snapshot_execution_model"] - - -def _rotation(quat: np.ndarray) -> np.ndarray: - matrix = np.empty(9) - mujoco.mju_quat2Mat(matrix, quat) - return matrix.reshape(3, 3) - - -def _contact_reason(model: Any, first: int, second: int) -> str: - """Return the MuJoCo static geom-pair filter, or ``eligible``.""" - if any( - {int(a), int(b)} == {first, second} - for a, b in zip(model.pair_geom1, model.pair_geom2) - ): - return "explicit_pair" - b1, b2 = int(model.geom_bodyid[first]), int(model.geom_bodyid[second]) - - def has_dynamic_ancestor(body: int) -> bool: - while body: - if model.body_dofnum[body]: - return True - body = int(model.body_parentid[body]) - return False - - if not has_dynamic_ancestor(b1) and not has_dynamic_ancestor(b2): - return "static_static" - w1, w2 = int(model.body_weldid[b1]), int(model.body_weldid[b2]) - if w1 == w2: - return "same_weld" - if not ( - int(model.opt.disableflags) & int(mujoco.mjtDisableBit.mjDSBL_FILTERPARENT) - ): - p1 = int(model.body_weldid[model.body_parentid[w1]]) - p2 = int(model.body_weldid[model.body_parentid[w2]]) - if w1 and w2 and (w1 == p2 or w2 == p1): - return "parent_child" - if not ( - int(model.geom_contype[first]) & int(model.geom_conaffinity[second]) - or int(model.geom_contype[second]) & int(model.geom_conaffinity[first]) - ): - return "collision_masks" - lo, hi = sorted((b1, b2)) - if (lo << 16) + hi in model.exclude_signature: - return "explicit_exclude" - return "eligible" - - -def snapshot_execution_model( - solver: Any, *, derived_cache_report: dict[str, Any] | None = None -) -> mujoco.MjModel: - """Read the active solver parameters into a detached audit model. - - Newton notifications can update device parameters without updating the - host MjModel. This initialization-only readback validates homogeneous - worlds and never changes either live model. - """ - result = copy.copy(solver.mj_model) - remote = getattr(solver, "mjw_model", None) - if remote is None: - return result - for name in dir(result): - local = getattr(result, name) - device_value = getattr(remote, name, None) - if not isinstance(local, np.ndarray) or not hasattr(device_value, "numpy"): - continue - values = device_value.numpy() - if name in { - "actuator_acc0", - "body_invweight0", - "dof_invweight0", - "dof_M0", - "body_subtreemass", - "tendon_invweight0", - }: - # These solver-computed caches are not authored inputs checked below. - # Keep their batch variation visible without treating it as lost USD - # metadata; rollout measurements record its output effects. - if derived_cache_report is not None and values.size: - spread = ( - values - values[0] - if values.ndim == local.ndim + 1 - else np.zeros_like(values) - ) - derived_cache_report[name] = { - "shape": list(values.shape), - "max_world_spread": float(np.max(np.abs(spread))), - } - continue - if values.shape == local.shape: - local[:] = values - elif values.ndim == local.ndim + 1 and values.shape[1:] == local.shape: - if len(values) and not np.array_equal( - values, np.broadcast_to(values[0], values.shape) - ): - raise ValueError(f"Nonuniform execution model field: {name}") - if len(values): - local[:] = values[0] - for name in ( - "gravity", - "timestep", - "integrator", - "solver", - "iterations", - "ls_iterations", - "ccd_iterations", - "cone", - "impratio", - "tolerance", - "ls_tolerance", - "ccd_tolerance", - "enableflags", - "disableflags", - ): - value = getattr(remote.opt, name, None) - if value is None: - continue - if hasattr(value, "numpy"): - values = value.numpy() - shape = np.asarray(getattr(result.opt, name)).shape - if values.shape == shape: - value = values if shape else values.item() - elif values.ndim and values.shape[1:] == shape: - if not np.array_equal(values, np.broadcast_to(values[0], values.shape)): - raise ValueError(f"Nonuniform execution option: {name}") - value = values[0] - else: - raise ValueError( - f"Unexpected execution option shape: {name}: {values.shape}" - ) - setattr(result.opt, name, value) - return result - - -def audit_model( - source: Any, - imported: Any, - joint_map: Mapping[int, int], - actuator_map: Sequence[int], - body_map: Mapping[str, int], - *, - explicit_collision_filtering: bool = False, - atol: float = 2e-6, - rtol: float = 2e-5, -) -> dict[str, Any]: - """Compare physical arrays after matching source entities to imported ones. - - Args: - source: Source MuJoCo model with runtime option overrides applied. - imported: MuJoCo model exported by the active Newton solver. - joint_map: Source joint index to imported joint index. - actuator_map: Imported actuator indices in source actuator order. - body_map: Source body names to imported body indices. - explicit_collision_filtering: Parent exclusions are encoded explicitly; - the complete effective geometry-pair graph is still compared. - atol: Absolute numeric tolerance. - rtol: Relative numeric tolerance. - - Returns: - JSON-compatible checks, mismatches, collision mapping and option flags. - This function reports differences without silently changing the model. - """ - checks: list[dict[str, Any]] = [] - - def compare( - name: str, - expected: object, - actual: object, - *, - ignored_indices: Sequence[int] = (), - reason: str | None = None, - ) -> None: - a, b = np.asarray(expected), np.asarray(actual) - same_shape = a.shape == b.shape - raw_matches = bool(same_shape and np.allclose(a, b, atol=atol, rtol=rtol)) - semantic_a, semantic_b = a.copy(), b.copy() - if same_shape and ignored_indices: - semantic_a.flat[list(ignored_indices)] = 0 - semantic_b.flat[list(ignored_indices)] = 0 - difference = ( - float( - np.max( - np.abs( - np.where(a == b, 0.0, a.astype(float)) - - np.where(a == b, 0.0, b.astype(float)) - ) - ) - ) - if same_shape and a.size - else 0.0 - ) - checks.append( - { - "field": name, - "matches": bool( - same_shape - and np.allclose(semantic_a, semantic_b, atol=atol, rtol=rtol) - ), - "raw_matches": raw_matches, - "equivalence_reason": ( - reason if not raw_matches and ignored_indices else None - ), - "max_abs_error": difference if same_shape else None, - "source_shape": list(a.shape), - "imported_shape": list(b.shape), - "source": a.tolist() if a.size <= 9 else None, - "imported": b.tolist() if b.size <= 9 else None, - } - ) - - for field in ( - "gravity", - "timestep", - "integrator", - "solver", - "iterations", - "ls_iterations", - "ccd_iterations", - "cone", - "impratio", - "tolerance", - "ls_tolerance", - "ccd_tolerance", - "enableflags", - "disableflags", - ): - expected = getattr(source.opt, field) - if field == "disableflags" and explicit_collision_filtering: - expected = int(expected) | int(mujoco.mjtDisableBit.mjDSBL_FILTERPARENT) - compare(f"option.{field}", expected, getattr(imported.opt, field)) - if field == "disableflags" and explicit_collision_filtering: - checks[-1]["raw_matches"] = int(source.opt.disableflags) == int( - imported.opt.disableflags - ) - checks[-1][ - "equivalence_reason" - ] = "Parent filtering is represented by explicit exclusions; all effective geometry pairs are checked." - - bmap = {0: 0} - merged_static_bodies = set() - for source_id in range(1, source.nbody): - name = source.body(source_id).name - if name not in body_map: - checks.append( - {"field": f"body[{name or source_id}].mapping", "matches": False} - ) - continue - imported_id = body_map[name] - bmap[source_id] = imported_id - if ( - imported_id == 0 - and source.body_weldid[source_id] == 0 - and source.body_mocapid[source_id] < 0 - ): - merged_static_bodies.add(source_id) - checks.append( - { - "field": f"body[{name}].static_world_merge", - "matches": True, - "equivalence_reason": "Static geometry transforms and contacts are checked in the world frame.", - } - ) - continue - for field in ("body_mass", "body_ipos", "body_pos"): - ignored = ( - (0, 1, 2) - if ( - field == "body_ipos" - and source.body_mass[source_id] == 0 - and imported.body_mass[imported_id] == 0 - ) - else () - ) - compare( - f"body[{name}].{field}", - getattr(source, field)[source_id], - getattr(imported, field)[imported_id], - ignored_indices=ignored, - reason="Zero-mass body center of mass does not enter dynamics.", - ) - compare( - f"body[{name}].rotation", - _rotation(source.body_quat[source_id]), - _rotation(imported.body_quat[imported_id]), - ) - # Principal-axis eigenvectors may change sign/order while the tensor agrees. - ra, rb = _rotation(source.body_iquat[source_id]), _rotation( - imported.body_iquat[imported_id] - ) - compare( - f"body[{name}].inertia_tensor", - ra @ np.diag(source.body_inertia[source_id]) @ ra.T, - rb @ np.diag(imported.body_inertia[imported_id]) @ rb.T, - ) - - dof_map = [] - for source_id in range(source.njnt): - imported_id = joint_map[source_id] - name = source.joint(source_id).name or str(source_id) - for field in ( - "jnt_type", - "jnt_pos", - "jnt_axis", - "jnt_limited", - "jnt_range", - "jnt_solref", - "jnt_solimp", - "jnt_stiffness", - ): - ignored = () - if ( - field == "jnt_range" - and not source.jnt_limited[source_id] - and not imported.jnt_limited[imported_id] - ): - ignored = (0, 1) - compare( - f"joint[{name}].{field}", - getattr(source, field)[source_id], - getattr(imported, field)[imported_id], - ignored_indices=ignored, - reason="Range is inactive because both limit flags are disabled.", - ) - source_force_range = ( - source.jnt_actfrcrange[source_id] - if source.jnt_actfrclimited[source_id] - else (-np.inf, np.inf) - ) - imported_force_range = ( - imported.jnt_actfrcrange[imported_id] - if imported.jnt_actfrclimited[imported_id] - else (-np.inf, np.inf) - ) - compare( - f"joint[{name}].effective_actuator_force_range", - source_force_range, - imported_force_range, - ) - checks[-1]["source_limit_flag"] = bool(source.jnt_actfrclimited[source_id]) - checks[-1]["imported_limit_flag"] = bool( - imported.jnt_actfrclimited[imported_id] - ) - width = (6, 3, 1, 1)[int(source.jnt_type[source_id])] - sa, ia = int(source.jnt_dofadr[source_id]), int( - imported.jnt_dofadr[imported_id] - ) - dof_map.extend(range(ia, ia + width)) - for field in ("dof_damping", "dof_armature", "dof_frictionloss"): - compare( - f"joint[{name}].{field}", - getattr(source, field)[sa : sa + width], - getattr(imported, field)[ia : ia + width], - ) - amap = np.asarray(actuator_map, dtype=int) - for field in ( - "actuator_gainprm", - "actuator_biasprm", - "actuator_gear", - "actuator_ctrlrange", - "actuator_ctrllimited", - "actuator_forcerange", - "actuator_forcelimited", - "actuator_trntype", - "actuator_dyntype", - "actuator_gaintype", - "actuator_biastype", - ): - compare(field, getattr(source, field), getattr(imported, field)[amap]) - - # Verify collision geoms by name first; source unnamed geoms use body-local - # type and position. Visual-only removals are expected from parse_visuals=False. - collision_map: dict[int, int] = {} - source_collision = np.flatnonzero(source.geom_contype | source.geom_conaffinity) - imported_collision = np.flatnonzero( - imported.geom_contype | imported.geom_conaffinity - ) - used: set[int] = set() - static_reference = mujoco.MjData(source) - mujoco.mj_kinematics(source, static_reference) - for sid in source_collision: - name = source.geom(int(sid)).name - source_position = source.geom_pos[sid] - source_rotation = _rotation(source.geom_quat[sid]) - source_body = int(source.geom_bodyid[sid]) - if source_body in merged_static_bodies: - body_rotation = static_reference.xmat[source_body].reshape(3, 3) - source_position = ( - static_reference.xpos[source_body] + body_rotation @ source_position - ) - source_rotation = body_rotation @ source_rotation - candidates = [ - int(i) - for i in imported_collision - if int(i) not in used - and int(imported.geom_bodyid[i]) - == bmap.get(int(source.geom_bodyid[sid]), -1) - ] - named = [ - i - for i in candidates - if name - and ( - imported.geom(i).name == name - or imported.geom(i).name.endswith("_" + name) - or re.sub(r"_\d+$", "", imported.geom(i).name).split("/")[-1] == name - or re.sub(r"_\d+$", "", imported.geom(i).name).endswith("_" + name) - ) - ] - if len(named) == 1: - target = named[0] - else: - geometric = [ - i - for i in candidates - if imported.geom_type[i] == source.geom_type[sid] - and np.allclose( - imported.geom_pos[i], source_position, atol=atol, rtol=rtol - ) - ] - if len(geometric) != 1: - checks.append( - { - "field": f"geom[{name or sid}].mapping", - "matches": False, - "candidates": candidates, - } - ) - continue - target = geometric[0] - used.add(target) - collision_map[int(sid)] = target - is_mesh = int(source.geom_type[sid]) == int(mujoco.mjtGeom.mjGEOM_MESH) - for field in ( - "geom_type", - "geom_pos", - "geom_size", - "geom_friction", - "geom_condim", - "geom_solref", - "geom_solimp", - "geom_margin", - "geom_gap", - "geom_priority", - "geom_solmix", - ): - ignored = () - reason = None - if is_mesh and field in ("geom_pos", "geom_size"): - ignored = (0, 1, 2) - reason = ( - "Mesh principal frame may change; body-frame vertices are checked." - ) - if field == "geom_size": - kind = int(source.geom_type[sid]) - if kind == int(mujoco.mjtGeom.mjGEOM_PLANE): - ignored, reason = ( - 0, - 1, - 2, - ), "Plane size only controls visualization." - elif kind in ( - int(mujoco.mjtGeom.mjGEOM_CAPSULE), - int(mujoco.mjtGeom.mjGEOM_CYLINDER), - ): - ignored, reason = ( - 2, - ), "Capsule/cylinder collision uses radius and half-length." - elif kind == int(mujoco.mjtGeom.mjGEOM_SPHERE): - ignored, reason = (1, 2), "Sphere collision uses only radius." - compare( - f"geom[{name or sid}].{field}", - source_position if field == "geom_pos" else getattr(source, field)[sid], - getattr(imported, field)[target], - ignored_indices=ignored, - reason=reason, - ) - compare( - f"geom[{name or sid}].rotation", - source_rotation, - _rotation(imported.geom_quat[target]), - ignored_indices=tuple(range(9)) if is_mesh else (), - reason="Mesh principal frame may change; body-frame vertices are checked.", - ) - if is_mesh: - source_mesh, imported_mesh = int(source.geom_dataid[sid]), int( - imported.geom_dataid[target] - ) - source_start, imported_start = ( - source.mesh_vertadr[source_mesh], - imported.mesh_vertadr[imported_mesh], - ) - va = source.mesh_vert[ - source_start : source_start + source.mesh_vertnum[source_mesh] - ] - vb = imported.mesh_vert[ - imported_start : imported_start + imported.mesh_vertnum[imported_mesh] - ] - va = va @ source_rotation.T + source_position - vb = ( - vb @ _rotation(imported.geom_quat[target]).T + imported.geom_pos[target] - ) - compare( - f"geom[{name or sid}].body_frame_mesh_bounds", - np.stack((va.min(0), va.max(0))), - np.stack((vb.min(0), vb.max(0))), - ) - # Newton can recenter/reorder the same convex mesh. Compare both - # point sets in the body frame without requiring matching indices. - distances = np.linalg.norm(va[:, None] - vb[None, :], axis=-1) - vertex_error = max( - float(distances.min(0).max()), float(distances.min(1).max()) - ) - checks.append( - { - "field": f"geom[{name or sid}].body_frame_mesh_vertices", - "matches": vertex_error <= atol, - "max_abs_error": vertex_error, - "source_vertex_count": len(va), - "imported_vertex_count": len(vb), - } - ) - compare("collision_geom_count", len(source_collision), len(imported_collision)) - source_excludes = { - tuple( - sorted( - ( - bmap.get(int(signature) >> 16, -1), - bmap.get(int(signature) & 0xFFFF, -1), - ) - ) - ) - for signature in source.exclude_signature - } - imported_excludes = { - tuple(sorted((int(signature) >> 16, int(signature) & 0xFFFF))) - for signature in imported.exclude_signature - } - pair_differences = [] - mapped = sorted(collision_map) - for offset, first in enumerate(mapped): - for second in mapped[offset + 1 :]: - expected = _contact_reason(source, first, second) - actual = _contact_reason( - imported, collision_map[first], collision_map[second] - ) - if (expected in ("eligible", "explicit_pair")) != ( - actual in ("eligible", "explicit_pair") - ): - pair_differences.append( - { - "source_geoms": [ - source.geom(first).name or first, - source.geom(second).name or second, - ], - "source_filter": expected, - "imported_filter": actual, - } - ) - inverse_bmap = {v: k for k, v in bmap.items()} - extra_excludes = [] - for first, second in sorted(imported_excludes - source_excludes): - sa, sb = inverse_bmap.get(first), inverse_bmap.get(second) - reasons: dict[str, int] = {} - if sa is not None and sb is not None: - for ga in np.flatnonzero(source.geom_bodyid == sa): - for gb in np.flatnonzero(source.geom_bodyid == sb): - why = _contact_reason(source, int(ga), int(gb)) - reasons[why] = reasons.get(why, 0) + 1 - extra_excludes.append( - { - "imported_bodies": [first, second], - "source_bodies": [ - source.body(sa).name if sa is not None else None, - source.body(sb).name if sb is not None else None, - ], - "source_geom_pair_filters": reasons, - "redundant": sa is not None - and sb is not None - and not reasons.get("eligible", 0), - } - ) - checks.append( - { - "field": "contact_excludes", - "matches": not pair_differences - and len(collision_map) == len(source_collision), - "raw_matches": source_excludes == imported_excludes, - "missing_source_pairs": sorted(source_excludes - imported_excludes), - "additional_imported_pairs": sorted(imported_excludes - source_excludes), - "additional_imported_pair_details": extra_excludes, - "effective_geom_pair_differences": pair_differences, - "equivalence_reason": "Static geom-pair eligibility includes explicit pairs, welds, parent filtering, masks and excludes.", - } - ) - mismatches = [item for item in checks if not item["matches"]] - return { - "status": "passed" if not mismatches else "differences", - "atol": atol, - "rtol": rtol, - "check_count": len(checks), - "mismatches": mismatches, - "checks": checks, - "equivalent_differences": [ - item - for item in checks - if item["matches"] and not item.get("raw_matches", True) - ], - "collision_geom_map": collision_map, - "source_disableflags": int(source.opt.disableflags), - "imported_disableflags": int(imported.opt.disableflags), - } diff --git a/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_audit_tendons.py b/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_audit_tendons.py deleted file mode 100644 index 48191481d6..0000000000 --- a/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_audit_tendons.py +++ /dev/null @@ -1,186 +0,0 @@ -# ---------------------------------------------------------------------------- -# 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 fixed-tendon topology and physical properties across MJCF export.""" - -from __future__ import annotations - -from collections.abc import Mapping -from typing import Any - -import mujoco -import numpy as np - -__all__ = ["audit_tendons"] - - -def audit_tendons( - source: Any, - imported: Any, - joint_map: Mapping[int, int], - *, - atol: float = 2e-6, - rtol: float = 2e-5, -) -> dict[str, Any]: - """Compare every fixed tendon after mapping joint indices. - - Args: - source: Source MuJoCo model. - imported: MuJoCo model exported by the active solver. - joint_map: Source joint index to imported joint index. - atol: Absolute numeric tolerance. - rtol: Relative numeric tolerance. - - Returns: - JSON-compatible per-tendon address ranges, ordered joint/coefficient - records, physical-property checks, and the overall status. A non-joint - wrap is reported as unsupported instead of being treated as equivalent. - """ - rows: list[dict[str, Any]] = [] - joint_wrap = int(mujoco.mjtWrap.mjWRAP_JOINT) - imported_names = [imported.tendon(i).name for i in range(imported.ntendon)] - used: set[int] = set() - for sid in range(source.ntendon): - name = source.tendon(sid).name - candidates = [ - iid - for iid, actual_name in enumerate(imported_names) - if iid not in used - and ( - actual_name == name - or actual_name.endswith("_" + name) - or actual_name.endswith("/" + name) - ) - ] - if len(candidates) != 1: - rows.append( - { - "name": name, - "matches": False, - "mapping_error": candidates, - } - ) - continue - iid = candidates[0] - used.add(iid) - sa, ia = int(source.tendon_adr[sid]), int(imported.tendon_adr[iid]) - sn, inn = int(source.tendon_num[sid]), int(imported.tendon_num[iid]) - source_path, imported_path = [], [] - supported = True - for model, start, count, destination, remap in ( - (source, sa, sn, source_path, True), - (imported, ia, inn, imported_path, False), - ): - for wid in range(start, start + count): - kind = int(model.wrap_type[wid]) - target = int(model.wrap_objid[wid]) - if kind != joint_wrap: - supported = False - destination.append( - { - "wrap_index": wid, - "wrap_type": kind, - "joint_index": target, - "joint_name": ( - model.joint(target).name if kind == joint_wrap else None - ), - "mapped_joint_index": ( - joint_map.get(target, -1) - if remap and kind == joint_wrap - else target - ), - "coefficient": float(model.wrap_prm[wid]), - } - ) - topology_matches = ( - supported - and sn == inn - and all( - expected["mapped_joint_index"] == actual["mapped_joint_index"] - and expected["wrap_type"] == actual["wrap_type"] - and np.isclose( - expected["coefficient"], - actual["coefficient"], - atol=atol, - rtol=rtol, - ) - for expected, actual in zip(source_path, imported_path) - ) - ) - properties = [] - for field in ( - "tendon_limited", - "tendon_range", - "tendon_stiffness", - "tendon_damping", - "tendon_frictionloss", - "tendon_margin", - "tendon_solref_lim", - "tendon_solimp_lim", - "tendon_solref_fri", - "tendon_solimp_fri", - "tendon_lengthspring", - "tendon_armature", - "tendon_actfrclimited", - "tendon_actfrcrange", - ): - expected = np.asarray(getattr(source, field)[sid]) - actual = np.asarray(getattr(imported, field)[iid]) - properties.append( - { - "field": field, - "matches": bool( - np.allclose(expected, actual, atol=atol, rtol=rtol) - ), - "max_abs_error": float( - np.max(np.abs(expected.astype(float) - actual.astype(float))) - ), - "source": expected.tolist(), - "imported": actual.tolist(), - } - ) - rows.append( - { - "name": name, - "source_index": sid, - "imported_index": iid, - "source_address_range": [sa, sa + sn], - "imported_address_range": [ia, ia + inn], - "fixed_joint_wraps_supported": supported, - "source_path": source_path, - "imported_path": imported_path, - "topology_matches": bool(topology_matches), - "properties": properties, - "matches": bool( - topology_matches and all(item["matches"] for item in properties) - ), - } - ) - matches = ( - source.ntendon == imported.ntendon - and len(used) == imported.ntendon - and all(row["matches"] for row in rows) - ) - return { - "status": "passed" if matches else "differences", - "source_count": int(source.ntendon), - "imported_count": int(imported.ntendon), - "matched_count": len(used), - "atol": atol, - "rtol": rtol, - "tendons": rows, - "mismatch_names": [row["name"] for row in rows if not row["matches"]], - } diff --git a/tests/gym/envs/tasks/fixtures/g1_rough.json b/tests/gym/envs/tasks/fixtures/g1_rough.json deleted file mode 100644 index 5aec81ff3b..0000000000 --- a/tests/gym/envs/tasks/fixtures/g1_rough.json +++ /dev/null @@ -1,4697 +0,0 @@ -{ - "task": "g1_rough", - "state": { - "base_lin_vel_b": [ - [ - -1.4557806253433228, - 1.0864568948745728, - 0.05817210674285889 - ], - [ - -0.3569294810295105, - -0.1775071769952774, - 0.4064221978187561 - ], - [ - -0.33068203926086426, - 1.7141629457473755, - 1.1541197299957275 - ], - [ - -0.010942414402961731, - 0.02636963129043579, - -1.2672834396362305 - ] - ], - "base_ang_vel_b": [ - [ - 0.3292386531829834, - 1.6441028118133545, - -2.207279682159424 - ], - [ - -0.12005852162837982, - 0.3629804849624634, - 1.0592864751815796 - ], - [ - -0.060097336769104004, - 0.29801517724990845, - 0.7472468614578247 - ], - [ - -1.6891419887542725, - -0.899388313293457, - -0.36965397000312805 - ] - ], - "projected_gravity_b": [ - [ - 0.6021913290023804, - -0.6925421953201294, - 0.3971787691116333 - ], - [ - 0.21417748928070068, - 0.9763940572738647, - 0.02797865867614746 - ], - [ - 0.3185013234615326, - 0.17123670876026154, - 0.932327389717102 - ], - [ - -0.3802645206451416, - 0.27076369524002075, - 0.8843563795089722 - ] - ], - "base_lin_vel_yaw": [ - [ - -0.6121096611022949, - -0.5909727215766907, - 1.6059709787368774 - ], - [ - -0.3131377100944519, - 0.4113398492336273, - 0.23839201033115387 - ], - [ - -0.7736274003982544, - -1.4774776697158813, - -1.2642230987548828 - ], - [ - -0.46797406673431396, - -0.3962193727493286, - 1.109428882598877 - ] - ], - "base_ang_vel_w": [ - [ - 2.07602596282959, - -0.2684623599052429, - 1.8170301914215088 - ], - [ - -0.22963359951972961, - 1.042650818824768, - -0.35833558440208435 - ], - [ - -0.2057259976863861, - -0.2786875069141388, - -0.7285690903663635 - ], - [ - 1.0689704418182373, - 1.6281509399414062, - -0.07189316302537918 - ] - ], - "command": [ - [ - -0.5404043197631836, - 1.0749247074127197, - 0.509365975856781 - ], - [ - -0.9210910201072693, - 0.24086834490299225, - -0.13246694207191467 - ], - [ - -0.2072896957397461, - 1.3115217685699463, - -0.05750608071684837 - ], - [ - 1.3140007257461548, - 0.15788041055202484, - -0.9369820952415466 - ] - ], - "joint_pos": [ - [ - 1.5056331157684326, - 2.0918118953704834, - -0.07304765284061432, - 1.1237612962722778, - -0.198205828666687, - 0.1139955073595047, - 1.5492374897003174, - -0.014597616158425808, - 2.35783052444458, - 0.47007936239242554, - -0.39112335443496704, - -1.1015937328338623, - -0.7289369106292725, - -0.4832809567451477, - 1.2640043497085571, - -2.392209053039551, - -0.9288906455039978, - 0.5256131291389465, - -0.19249051809310913, - -1.3564655780792236, - 1.4588348865509033, - -0.20993752777576447, - 1.401776671409607, - 1.7631984949111938, - -0.3005925714969635, - 0.808905303478241, - -0.03374389931559563, - 1.619919776916504, - -1.377743124961853, - 0.7529674172401428, - 0.7406900525093079, - 0.177289679646492, - 0.7438516616821289, - -0.7136749029159546, - -0.8477581143379211, - -0.26309698820114136, - 0.04932607337832451 - ], - [ - 0.3320133090019226, - 0.3428820073604584, - -0.18796280026435852, - -0.41633516550064087, - -0.06709006428718567, - -0.04722129553556442, - 0.8697317838668823, - -0.3808351159095764, - -0.704973042011261, - 1.1231826543807983, - 1.6798514127731323, - 0.13716299831867218, - 0.582530677318573, - -0.8504189252853394, - 1.5139645338058472, - 0.7708828449249268, - 0.3680831789970398, - -1.1065921783447266, - -1.0081675052642822, - -0.692912220954895, - 0.6990879774093628, - -0.028723623603582382, - -0.602173388004303, - -0.363231360912323, - -0.33312204480171204, - 0.3564692735671997, - 0.8364291191101074, - 2.3312487602233887, - -1.872986078262329, - -0.565384566783905, - 0.14942148327827454, - -1.291897177696228, - -0.48694589734077454, - -1.4648293256759644, - -0.8810045719146729, - 1.056042194366455, - -1.555394172668457 - ], - [ - -1.6637481451034546, - -1.4028340578079224, - 0.12727148830890656, - -0.5829504728317261, - -1.3949012756347656, - 0.635925829410553, - 0.48540303111076355, - -1.8958559036254883, - 1.5646320581436157, - 1.0953043699264526, - 0.2841986119747162, - -1.2972286939620972, - 0.3363381326198578, - -0.15070541203022003, - -0.011540047824382782, - 1.9533648490905762, - 0.024266785010695457, - 0.8772256374359131, - -0.4529947340488434, - -0.44445371627807617, - -1.2233209609985352, - 1.7572327852249146, - -0.9898506999015808, - 0.6434103846549988, - -0.3203880488872528, - -1.3320015668869019, - -0.15934409201145172, - -0.9075278043746948, - 0.16299092769622803, - -0.6044127345085144, - 1.1671395301818848, - 0.9056613445281982, - 0.9750284552574158, - 0.019435198977589607, - 1.25371515750885, - -0.6955330967903137, - -0.8415408134460449 - ], - [ - -0.530204713344574, - -0.7811362147331238, - -1.0067243576049805, - 0.3849828243255615, - 0.7079736590385437, - 1.9297294616699219, - 2.2639787197113037, - -0.26071739196777344, - 0.37015068531036377, - -0.5081090927124023, - 0.7504653334617615, - -0.639136016368866, - 2.0525288581848145, - 0.667559027671814, - -0.5728611946105957, - -0.9176666140556335, - -2.0228161811828613, - 0.258408784866333, - 1.0313853025436401, - 0.9158302545547485, - -0.35417428612709045, - -0.24451617896556854, - 0.9667870998382568, - 0.916934072971344, - -0.8313652276992798, - -0.028980515897274017, - 0.8581166863441467, - -1.2955471277236938, - 0.8638095259666443, - -0.5798051357269287, - -1.9012408256530762, - -1.0559042692184448, - -1.4149895906448364, - -0.8092951774597168, - 0.03765219822525978, - 0.7535103559494019, - -1.0983672142028809 - ] - ], - "joint_vel": [ - [ - -0.5279741883277893, - 0.8597514629364014, - -0.17805825173854828, - -0.6473056674003601, - -0.4873490035533905, - 0.9520416855812073, - -1.79311203956604, - 0.9569541215896606, - -0.6222836375236511, - 0.6242925524711609, - 0.38798239827156067, - -0.2519591152667999, - 0.7435712814331055, - -0.3246157765388489, - 0.6967641115188599, - -0.25631996989250183, - -1.3544143438339233, - -0.8552826046943665, - 0.2536700963973999, - -1.3728705644607544, - -0.530166506767273, - -2.367126703262329, - 1.2589600086212158, - 0.02631186507642269, - -0.8995487093925476, - -1.3286360502243042, - 0.12793944776058197, - -0.9199894070625305, - 0.08235820382833481, - -2.194843292236328, - -1.5484036207199097, - 0.7937297821044922, - 0.14731340110301971, - 0.04240464046597481, - -0.6879045367240906, - -0.8871468901634216, - -0.47980090975761414 - ], - [ - 0.28267720341682434, - -1.6053545475006104, - 0.12526625394821167, - -0.23374232649803162, - -0.5862246155738831, - 1.0336445569992065, - 0.49877527356147766, - -0.43038249015808105, - -0.8129518628120422, - 0.3756439983844757, - 0.6129004955291748, - 0.04692007228732109, - 0.9859462976455688, - 1.4831371307373047, - 2.486056089401245, - -0.8713778853416443, - 2.4219255447387695, - 0.8619664311408997, - 0.7171047925949097, - -2.2819113731384277, - -0.24522823095321655, - -0.023873133584856987, - -0.0973457619547844, - -1.041334629058838, - 0.3649180233478546, - -0.46169158816337585, - -0.14872486889362335, - -0.23188330233097076, - 0.12479649484157562, - 0.05347807705402374, - 0.5584927797317505, - -1.5811458826065063, - 0.5356708765029907, - 0.21025295555591583, - -0.24703240394592285, - 0.23994095623493195, - 1.1590720415115356 - ], - [ - 1.168839693069458, - -0.9539070725440979, - 0.658633291721344, - 0.5681256651878357, - 0.7912182807922363, - -0.36096110939979553, - 0.6277414560317993, - 0.1642685979604721, - 0.05280255153775215, - -0.34626534581184387, - -0.4431508779525757, - -0.11042255908250809, - -0.06429174542427063, - 1.1978527307510376, - 0.47317540645599365, - -2.292738676071167, - 1.5541141033172607, - -0.5851547122001648, - -0.3537789285182953, - -0.8436524271965027, - -0.44712358713150024, - -1.448716640472412, - 1.552046537399292, - -1.7491686344146729, - 0.919133722782135, - 0.5182786583900452, - -0.4023047089576721, - 0.5836163759231567, - -0.9826108813285828, - -0.7050940990447998, - 1.2669448852539062, - 1.6809521913528442, - -0.1113143041729927, - -0.30720576643943787, - 0.9826673865318298, - -0.6770282983779907, - 0.5214178562164307 - ], - [ - 0.393690288066864, - 0.24256174266338348, - -2.0793709754943848, - 0.07966192066669464, - 1.2994259595870972, - -0.3774186968803406, - 1.9390441179275513, - 0.1396932154893875, - 1.1258900165557861, - 0.04496730491518974, - -1.139866590499878, - -0.2511955499649048, - -0.5875258445739746, - 0.002308705821633339, - -1.0564762353897095, - -0.25632399320602417, - -1.1680738925933838, - -0.16880786418914795, - -0.24299125373363495, - 1.0621012449264526, - 0.5457154512405396, - -0.2439623326063156, - -1.3711411952972412, - -1.1331812143325806, - -0.2143252193927765, - 1.9814000129699707, - -0.961947500705719, - 0.6017144322395325, - -0.21473316848278046, - 1.320539951324463, - 1.0334274768829346, - 0.7553479671478271, - 0.23677706718444824, - 0.3173491656780243, - -0.5547844171524048, - -0.7811285853385925, - 1.7556536197662354 - ] - ], - "joint_acc": [ - [ - 1668.161865234375, - -393.6308898925781, - -2312.38525390625, - -1246.2601318359375, - 159.71267700195312, - 420.9147033691406, - 381.2242126464844, - -544.7760620117188, - -242.781982421875, - -1708.1973876953125, - 372.72576904296875, - 237.90928649902344, - -1330.7008056640625, - -168.1787109375, - 852.7047729492188, - -597.7569580078125, - 1202.3968505859375, - 34.981163024902344, - -860.686279296875, - -825.6607055664062, - 1007.9934692382812, - -935.4822998046875, - -573.8944702148438, - 415.48321533203125, - 163.61143493652344, - 521.2409057617188, - 1364.9368896484375, - 1294.7662353515625, - 172.05364990234375, - -1269.952880859375, - 355.07867431640625, - 132.2051239013672, - -26.438737869262695, - -1893.490478515625, - -377.4305725097656, - 1236.39892578125, - 930.3391723632812 - ], - [ - -188.0395965576172, - 129.8790283203125, - -1576.4200439453125, - -236.45082092285156, - 373.44464111328125, - -51.0729866027832, - 1261.9129638671875, - -886.3538208007812, - -2085.78125, - -351.22894287109375, - 273.7871398925781, - 129.74063110351562, - -1110.380859375, - -262.692138671875, - -882.8880004882812, - 526.4204711914062, - 414.2576599121094, - -531.5410766601562, - -846.2487182617188, - 550.9164428710938, - 1629.3389892578125, - -170.4379425048828, - -1341.866455078125, - -1077.88330078125, - 1401.10498046875, - -686.3238525390625, - 145.0147247314453, - -247.7913818359375, - -913.5870971679688, - 1035.5107421875, - 700.6553955078125, - 262.6425476074219, - 148.6880645751953, - -2147.371337890625, - -720.7433471679688, - 468.95166015625, - 908.7201538085938 - ], - [ - 293.96630859375, - -62.2191162109375, - -352.9985046386719, - 645.46142578125, - 510.98382568359375, - -151.96572875976562, - -121.70757293701172, - -161.01531982421875, - -1437.4180908203125, - 1074.786865234375, - -443.6652526855469, - -490.074462890625, - -596.8087158203125, - -1533.0238037109375, - 584.1063842773438, - -4055.375, - 1158.7008056640625, - 912.3848266601562, - 59.38063049316406, - -15.293424606323242, - 1009.8793334960938, - -1339.5419921875, - -61.18260192871094, - 388.61187744140625, - 1065.2783203125, - -533.8775024414062, - -2285.95947265625, - -1250.846923828125, - -1157.400146484375, - -415.5268859863281, - -1911.385986328125, - -1429.7677001953125, - 840.8233642578125, - -878.7216796875, - -1083.961181640625, - -1765.1134033203125, - -278.2705078125 - ], - [ - -1967.5721435546875, - -777.1732788085938, - -264.4198913574219, - -721.2086791992188, - -707.0787963867188, - 188.36904907226562, - -1370.8006591796875, - 1468.0360107421875, - 197.849365234375, - 420.2668762207031, - -604.5377197265625, - -185.824951171875, - -92.34606170654297, - 23.595867156982422, - -1224.7869873046875, - 360.291259765625, - 965.7421264648438, - -1729.43603515625, - 555.3153686523438, - 552.5769653320312, - -48.220787048339844, - 890.5111083984375, - -674.075927734375, - -1074.4248046875, - 97.20252990722656, - -1809.5001220703125, - 658.4154052734375, - 361.912841796875, - 2121.124755859375, - -940.8626708984375, - -504.9437255859375, - 1016.8355712890625, - 90.6131591796875, - 443.5833435058594, - 412.2043151855469, - -1316.3466796875, - 1739.9268798828125 - ] - ], - "applied_torque": [ - [ - -14.419787406921387, - 48.54924011230469, - -54.83991622924805, - 26.02928924560547, - 22.35517692565918, - 27.480485916137695, - -49.62981033325195, - -2.4085357189178467, - 10.796653747558594, - 10.61724853515625, - 14.973010063171387, - -20.94425392150879, - 6.2938666343688965, - 39.708091735839844, - 24.446033477783203, - -3.485306978225708, - -70.85551452636719, - 42.441864013671875, - -38.19162368774414, - 54.53776550292969, - -38.52880096435547, - 20.979534149169922, - 17.398149490356445, - -56.092044830322266, - 53.53395462036133, - 19.920392990112305, - -40.8914794921875, - 44.15678787231445, - -37.509071350097656, - -1.8634438514709473, - -9.967890739440918, - 3.0223982334136963, - -10.67481517791748, - 2.2599074840545654, - -2.2743537425994873, - 8.38425064086914, - -4.397794723510742 - ], - [ - 6.599841594696045, - -38.21885299682617, - 23.851333618164062, - -36.04075241088867, - 40.549476623535156, - -26.525596618652344, - 18.499557495117188, - 85.853271484375, - -4.581111431121826, - -10.820500373840332, - -8.952424049377441, - -17.172874450683594, - 15.811954498291016, - 4.0161452293396, - 59.99694061279297, - -20.906124114990234, - -30.55010223388672, - -20.30491065979004, - -19.803245544433594, - -7.135894298553467, - 54.86551284790039, - -38.38470458984375, - -22.028562545776367, - 20.400306701660156, - 17.09325408935547, - -31.561738967895508, - 0.6164140701293945, - -18.006847381591797, - 6.462375164031982, - 58.80363082885742, - 18.298526763916016, - -71.24160766601562, - 39.95557403564453, - 59.259944915771484, - -38.69380569458008, - 28.53049659729004, - 27.48499870300293 - ], - [ - -20.03413200378418, - 23.982423782348633, - -13.899916648864746, - 33.40736770629883, - 6.102523326873779, - -9.23681926727295, - 13.737212181091309, - 13.551922798156738, - -21.150386810302734, - 79.3740005493164, - 35.05680465698242, - 18.680601119995117, - 0.32720455527305603, - 17.190914154052734, - -18.40950584411621, - -10.131638526916504, - -0.06177947297692299, - 24.01487922668457, - -5.814537525177002, - 36.41688919067383, - 18.143293380737305, - -21.006946563720703, - 44.05561065673828, - 12.61419677734375, - 31.893657684326172, - -36.02269744873047, - 89.47785949707031, - -61.00448989868164, - -40.48505401611328, - -66.33033752441406, - 4.499334812164307, - 32.170658111572266, - 41.60356903076172, - 30.270051956176758, - -20.365007400512695, - -34.966636657714844, - 11.508540153503418 - ], - [ - 16.702163696289062, - -17.244401931762695, - 6.133879661560059, - -62.83711242675781, - 8.172779083251953, - 14.928387641906738, - -35.22040939331055, - -76.99336242675781, - -3.7889232635498047, - 18.798974990844727, - 26.21240234375, - 34.31686019897461, - 17.863983154296875, - 40.698020935058594, - 11.108121871948242, - -31.979366302490234, - -22.386932373046875, - 42.66255569458008, - 18.51290512084961, - 4.037971019744873, - -8.4000825881958, - -4.237223148345947, - -36.56418228149414, - 54.615657806396484, - -37.73508071899414, - -0.4700183570384979, - -9.351125717163086, - -2.6030120849609375, - 0.45510685443878174, - -33.74147415161133, - 7.973489284515381, - 35.40697479248047, - -67.85062408447266, - -4.384401798248291, - 14.123188972473145, - -29.324581146240234, - 18.412818908691406 - ] - ], - "action": [ - [ - -4.368893146514893, - 0.9160680174827576, - 4.984935283660889, - 0.1185097023844719, - 0.622482419013977, - -3.977832794189453, - -7.159829616546631, - -2.5309689044952393, - 4.721494674682617, - -2.8818554878234863, - -4.431326389312744, - -0.8927403092384338, - 3.247561454772949, - -8.902396202087402, - -1.2800079584121704, - -0.15178997814655304, - 3.9192891120910645, - 4.775058746337891, - -1.6756649017333984, - 3.9211177825927734, - -0.003280824515968561, - 6.563349723815918, - -1.1090047359466553, - 0.2747873067855835, - -4.711472034454346, - 1.2407253980636597, - -1.8140769004821777, - -0.3151421844959259, - -1.636247158050537, - 2.5366387367248535, - 2.516878843307495, - -1.2603262662887573, - -1.2249360084533691, - -0.07307285815477371, - 1.0792949199676514, - 0.6049757599830627, - -1.3881919384002686 - ], - [ - 2.340679407119751, - -3.2012264728546143, - 1.449082612991333, - -0.44263583421707153, - 6.466547012329102, - 4.260446548461914, - -1.1634079217910767, - -0.3325357735157013, - 0.6242257952690125, - 0.10043658316135406, - -3.3272321224212646, - 4.066169261932373, - 2.077775716781616, - -5.893347263336182, - 2.7362892627716064, - 0.7733586430549622, - 1.8175424337387085, - 5.035777568817139, - 1.0537075996398926, - -1.1087294816970825, - 1.8238623142242432, - -1.5047787427902222, - -5.391904830932617, - -4.920398235321045, - 1.9996800422668457, - 2.8552744388580322, - 2.4058778285980225, - 0.7430681586265564, - 7.5287909507751465, - -2.3544540405273438, - -1.5701937675476074, - 3.8395838737487793, - -4.007706642150879, - 3.4519639015197754, - -3.863215684890747, - 1.0905109643936157, - 2.3868038654327393 - ], - [ - -1.9206892251968384, - -0.5940742492675781, - -0.3497787117958069, - 3.7373969554901123, - 8.128355026245117, - 4.392646312713623, - -3.1500251293182373, - 1.746557354927063, - -1.877929925918579, - -6.652039051055908, - 4.322165012359619, - 3.399026393890381, - -5.671651840209961, - -0.2512320280075073, - 7.311364650726318, - -0.06858460605144501, - 5.1759772300720215, - 3.3528430461883545, - -0.6859660744667053, - 3.756129503250122, - 3.4173645973205566, - -8.679512023925781, - 0.5880194902420044, - -0.7168394923210144, - 6.000116348266602, - -2.446148157119751, - -6.562048435211182, - -3.665959119796753, - 1.803282380104065, - 12.5444974899292, - 2.0668933391571045, - -11.293632507324219, - 1.1310479640960693, - -2.0034444332122803, - -9.082246780395508, - 4.693811893463135, - -3.442373037338257 - ], - [ - -4.359130382537842, - 1.4358174800872803, - 1.2535760402679443, - 2.251012086868286, - 4.294274806976318, - -2.0001187324523926, - -0.383171945810318, - 2.659695863723755, - 1.6124097108840942, - -1.9515864849090576, - -2.035461187362671, - 0.10162672400474548, - 3.356719970703125, - -7.295328617095947, - 3.0563695430755615, - -7.556408882141113, - 2.0578768253326416, - -1.689565658569336, - 1.8390171527862549, - -4.7228007316589355, - -6.407613754272461, - 1.15846586227417, - -0.7323806881904602, - 2.3890063762664795, - 5.626026630401611, - -3.9759743213653564, - 7.224872589111328, - -2.195585250854492, - 4.695905685424805, - 0.1920173466205597, - -4.04601526260376, - -1.238318920135498, - 4.002810478210449, - -0.024696817621588707, - -10.090450286865234, - -0.7411140203475952, - 1.8331068754196167 - ] - ], - "previous_action": [ - [ - -1.0108466148376465, - -0.142862007021904, - -3.5306010246276855, - -1.964185118675232, - 0.09453178942203522, - 0.38354989886283875, - 1.1020517349243164, - 1.7958555221557617, - 0.18220387399196625, - 0.3428279161453247, - 0.708647608757019, - -0.7077146172523499, - 0.39291343092918396, - -1.400240421295166, - -0.26445072889328003, - -0.11782901734113693, - -1.8917638063430786, - -0.6669813394546509, - 1.4461886882781982, - -0.3651680648326874, - 0.27947166562080383, - -0.021903466433286667, - -0.9254041314125061, - -0.4791620373725891, - -0.11053915321826935, - -0.18104669451713562, - -0.43702784180641174, - -0.8008014559745789, - -1.1921192407608032, - 0.6687846779823303, - -0.0957140177488327, - 0.1811242550611496, - 0.3682470917701721, - -0.06045543774962425, - -0.21235278248786926, - 2.656416893005371, - -1.2452025413513184 - ], - [ - -1.328525185585022, - -1.0206937789916992, - -0.061315279453992844, - 1.2266062498092651, - 0.8237326741218567, - 1.6854990720748901, - 0.6278128623962402, - -0.9680779576301575, - 0.13542191684246063, - -0.5355046987533569, - -0.5223515629768372, - -0.37166252732276917, - -0.6997039318084717, - -0.49067309498786926, - 0.24551089107990265, - -0.775000810623169, - -1.186268925666809, - 1.0723598003387451, - 0.5600261688232422, - -0.5014552474021912, - 0.16223998367786407, - 0.10682955384254456, - -0.9645101428031921, - -0.36778920888900757, - -1.3676397800445557, - -0.6828271150588989, - -0.6595971584320068, - 0.5313081741333008, - 0.7994405031204224, - 0.5227431058883667, - -1.2183622121810913, - -0.6093205809593201, - 0.8313105702400208, - -0.8710706233978271, - 0.3441033661365509, - 0.40260326862335205, - 0.5778527855873108 - ], - [ - -0.5497952103614807, - -1.04951012134552, - 0.6379414796829224, - 0.04964468255639076, - 0.6542599201202393, - -1.7519702911376953, - -0.5116554498672485, - 0.6801650524139404, - 0.08594992756843567, - 0.19995561242103577, - 0.9948765635490417, - -1.3847026824951172, - -0.10364419221878052, - 0.3080644905567169, - -0.5359504818916321, - 0.6439346671104431, - -0.6799383759498596, - 1.0321851968765259, - 0.8460059762001038, - 0.8201548457145691, - -0.6698185801506042, - 0.3081378936767578, - 1.0963537693023682, - -0.7811451554298401, - -0.7836215496063232, - 0.540791928768158, - 1.011771559715271, - -0.5032227039337158, - 0.32934466004371643, - -0.3124656081199646, - -0.1483641415834427, - -0.10728456825017929, - 0.08037495613098145, - -0.1621134728193283, - -1.295089840888977, - 0.5580396056175232, - -0.37750208377838135 - ], - [ - 0.10396319627761841, - -2.1065852642059326, - 0.6500381231307983, - -1.3376755714416504, - 0.1671949028968811, - 0.22357028722763062, - 0.35604509711265564, - 0.05533676967024803, - -0.785855770111084, - 0.3351445198059082, - -0.08490034192800522, - -0.6312821507453918, - -0.41099265217781067, - -0.8103114366531372, - -1.110275149345398, - -0.5244833827018738, - -0.9151604175567627, - -1.8683534860610962, - -1.3026257753372192, - -0.8013157844543457, - 0.6163517832756042, - -0.4021838903427124, - -1.4598723649978638, - 0.7780170440673828, - 0.7748329043388367, - -0.7759303450584412, - -1.9655919075012207, - -0.47156333923339844, - -1.5894381999969482, - 1.4595211744308472, - 1.0188933610916138, - 0.14971071481704712, - -0.9657685160636902, - -0.19366878271102905, - 1.363334059715271, - -0.28902846574783325, - -0.7591164112091064 - ] - ], - "foot_vel_w": [ - [ - [ - -0.21171289682388306, - -0.7254922389984131, - -0.3174818456172943 - ], - [ - -0.4072316586971283, - 0.6528500318527222, - -0.7806931138038635 - ] - ], - [ - [ - 0.2022387683391571, - 1.0100445747375488, - -0.9875770807266235 - ], - [ - -1.4847157001495361, - -0.7211213707923889, - -0.6197503209114075 - ] - ], - [ - [ - -0.3263791501522064, - 0.6447191834449768, - -0.577362596988678 - ], - [ - -1.0065580606460571, - -0.42130208015441895, - 0.8047337532043457 - ] - ], - [ - [ - -0.5346943736076355, - -1.4255815744400024, - -2.03910231590271 - ], - [ - -1.03001868724823, - -1.543200135231018, - -0.7345817685127258 - ] - ] - ], - "foot_force_history": [ - [ - [ - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.3530494272708893, - -0.22667065262794495, - 0.9696542024612427 - ] - ], - [ - [ - 0.0, - 0.0, - 0.0 - ], - [ - -0.03592069819569588, - -0.4432648718357086, - 0.14850245416164398 - ] - ], - [ - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.18437068164348602, - 2.3312039375305176, - -1.269078016281128 - ] - ] - ], - [ - [ - [ - -0.21128453314304352, - -1.7220808267593384, - 1.0471127033233643 - ], - [ - 0.29737088084220886, - -0.13929983973503113, - -0.013977283611893654 - ] - ], - [ - [ - -1.3237457275390625, - 0.6947073340415955, - 0.18112125992774963 - ], - [ - -1.6194521188735962, - -0.8813993334770203, - -2.2042853832244873 - ] - ], - [ - [ - 0.00311090680770576, - 2.5478298664093018, - -0.616400957107544 - ], - [ - 1.6979860067367554, - -0.9815937876701355, - 0.43721848726272583 - ] - ] - ], - [ - [ - [ - 0.0, - 0.0, - 0.0 - ], - [ - -0.6534702181816101, - 0.7779728770256042, - -0.5271583795547485 - ] - ], - [ - [ - 0.0, - 0.0, - 0.0 - ], - [ - -0.0890507623553276, - -0.7470173835754395, - -0.31302332878112793 - ] - ], - [ - [ - 0.0, - 0.0, - 0.0 - ], - [ - -0.6629698872566223, - -0.7631149291992188, - -0.30985027551651 - ] - ] - ], - [ - [ - [ - -0.01198647078126669, - -1.1322849988937378, - 0.4914545714855194 - ], - [ - -0.5499143004417419, - -0.3918861746788025, - -1.967681884765625 - ] - ], - [ - [ - -0.07355965673923492, - -0.9671895503997803, - -0.8145716786384583 - ], - [ - 0.5471686720848083, - -0.08620382845401764, - 0.565158486366272 - ] - ], - [ - [ - -0.5482525825500488, - -0.5464745759963989, - 0.27679717540740967 - ], - [ - 0.0233794916421175, - -0.19441388547420502, - 0.19257529079914093 - ] - ] - ] - ], - "current_air_time": [ - [ - 0.45275312662124634, - 0.8311971426010132 - ], - [ - 0.9466180205345154, - 0.5241358876228333 - ], - [ - 0.8108326196670532, - 0.1247032880783081 - ], - [ - 0.2205706238746643, - 0.2958891987800598 - ] - ], - "current_contact_time": [ - [ - 0.8795936703681946, - 0.3388788104057312 - ], - [ - 0.019999999552965164, - 0.019999999552965164 - ], - [ - 0.009999999776482582, - 0.0 - ], - [ - 0.5516138076782227, - 0.6729174852371216 - ] - ], - "last_air_time": [ - [ - 0.4153270125389099, - 0.7320988774299622 - ], - [ - 0.6830820441246033, - 0.8455604910850525 - ], - [ - 0.055288076400756836, - 0.9835333824157715 - ], - [ - 0.5538256168365479, - 0.08471375703811646 - ] - ], - "illegal_force_history": [ - [ - [ - [ - 0.0, - 0.0, - 0.0 - ] - ], - [ - [ - 0.0, - 0.0, - 0.0 - ] - ], - [ - [ - 0.0, - 0.0, - 0.0 - ] - ] - ], - [ - [ - [ - -0.45131534337997437, - -1.4162439107894897, - -0.524907648563385 - ] - ], - [ - [ - 0.45356932282447815, - 0.5649322867393494, - -0.3941814601421356 - ] - ], - [ - [ - 1.8396403789520264, - 0.560344398021698, - -0.5622568726539612 - ] - ] - ], - [ - [ - [ - 1.3017932176589966, - 0.33739879727363586, - 1.5420217514038086 - ] - ], - [ - [ - -1.1652177572250366, - -1.9771294593811035, - 0.15924112498760223 - ] - ], - [ - [ - -0.5091078877449036, - 0.27111363410949707, - -0.522592306137085 - ] - ] - ], - [ - [ - [ - 1.3238030672073364, - -0.9065457582473755, - -1.0167999267578125 - ] - ], - [ - [ - 0.7171145081520081, - -0.21839751303195953, - 0.24924913048744202 - ] - ], - [ - [ - 2.582766056060791, - 3.244075298309326, - 0.08227609843015671 - ] - ] - ] - ], - "height_scan": [ - [ - -0.28716176748275757, - -0.4419015645980835, - -1.0200023651123047, - -0.28770166635513306, - -0.6783255934715271, - -0.2890845537185669, - -0.1819729208946228, - -1.004855990409851, - -0.14911001920700073, - -0.5164163112640381, - -0.7400633096694946, - -0.7347542643547058, - -0.7018631100654602, - -0.8625413179397583, - -0.1558164358139038, - -0.167216956615448, - -0.34874844551086426, - -0.8995740413665771, - -0.06789010763168335, - -0.10044735670089722, - -0.8174746632575989, - -0.043960511684417725, - -0.5494846105575562, - -0.18539637327194214, - -0.764228880405426, - -0.03852289915084839, - -0.8604356646537781, - -0.3170236349105835, - -0.685659646987915, - -0.9850010275840759, - -0.10952931642532349, - -0.9684800505638123, - -0.9099814295768738, - -0.9744464159011841, - -0.5389964580535889, - -0.4049417972564697, - -0.10225820541381836, - -0.4741637110710144, - -0.2848247289657593, - -0.12730753421783447, - -0.5466987490653992, - -0.4689098596572876, - -0.43547797203063965, - -0.06664764881134033, - -0.14009946584701538, - -0.5698091387748718, - -0.1875506043434143, - -0.8630085587501526, - -0.5284711122512817, - -0.2081202268600464, - -0.8741657137870789, - -0.21526700258255005, - -0.7706494927406311, - -0.29491281509399414, - -0.149122953414917, - -0.07454103231430054, - -0.4668652415275574, - -0.44386452436447144, - -0.43136876821517944, - -0.1436973214149475, - -1.0063891410827637, - -0.4918827414512634, - -0.719039261341095, - -0.21819210052490234, - -0.7204746603965759, - -0.9743740558624268, - -0.39509516954421997, - -0.4074733257293701, - -0.3168402314186096, - -0.7339693903923035, - -1.0243933200836182, - -0.8311435580253601, - -0.03691112995147705, - -0.3078213930130005, - -0.26423215866088867, - -0.7199183106422424, - -0.21735525131225586, - -0.07431846857070923, - -0.643882155418396, - -0.33414846658706665, - -0.7306238412857056, - -0.23027968406677246, - -0.9862818121910095, - -0.4448670744895935, - -0.23372262716293335, - -0.954705536365509, - -0.9376344084739685, - -0.5205722451210022, - -0.37874358892440796, - -0.19050151109695435, - -0.14629727602005005, - -0.6824342012405396, - -0.10497176647186279, - -0.8963110446929932, - -0.24083244800567627, - -0.9735761284828186, - -0.7543412446975708, - -0.2632828950881958, - -0.08554565906524658, - -0.5910387635231018, - -0.8392587304115295, - -0.7414860725402832, - -0.12770283222198486, - -0.10582554340362549, - -0.7515185475349426, - -0.03629302978515625, - -0.7738940715789795, - -0.5477350354194641, - -0.2340010404586792, - -0.6277101039886475, - -0.7757539749145508, - -0.24597251415252686, - -0.6464226245880127, - -0.13265621662139893, - -0.19900083541870117, - -0.39023828506469727, - -0.5473631024360657, - -0.7507364749908447, - -0.6764105558395386, - -0.7312309741973877, - -0.47062546014785767, - -0.37863850593566895, - -0.04647630453109741, - -0.8432213068008423, - -0.37445610761642456, - -0.1488986611366272, - -0.22894209623336792, - -0.16381454467773438, - -0.4650822877883911, - -0.31299012899398804, - -0.22281551361083984, - -0.045681655406951904, - -0.12639284133911133, - -0.3405425548553467, - -1.0269339084625244, - -0.47167855501174927, - -0.08079349994659424, - -0.7066734433174133, - -0.040200889110565186, - -0.6684422492980957, - -0.5266332030296326, - -0.15009689331054688, - -0.30866575241088867, - -0.30868273973464966, - -0.16131454706192017, - -0.23586338758468628, - -0.8245671987533569, - -0.9666875004768372, - -0.060476064682006836, - -1.0116655826568604, - -0.7532963156700134, - -0.5495728254318237, - -0.19813627004623413, - -0.619171679019928, - -0.9636986255645752, - -0.34169644117355347, - -1.0295491218566895, - -0.5497443675994873, - -0.2530338168144226, - -1.0117013454437256, - -0.5710803270339966, - -0.6607940793037415, - -0.09831494092941284, - -0.5018654465675354, - -0.9933570623397827, - -0.8620007634162903, - -0.648077130317688, - -0.3141677975654602, - -0.6191890835762024, - -0.675189197063446, - -0.20415741205215454, - -0.6640922427177429, - -0.12775558233261108, - -1.0292930603027344, - -0.8042539954185486, - -0.4698062539100647, - -0.19917845726013184, - -0.8262059092521667, - -0.2477855682373047, - -1.0034260749816895, - -0.5516428351402283, - -0.3479820489883423, - -0.20705115795135498, - -0.46222829818725586, - -0.7676801681518555, - -0.937333881855011, - -0.3769838809967041 - ], - [ - -0.954921305179596, - -0.2913939952850342, - -0.1249547004699707, - -0.37924909591674805, - -0.5223706364631653, - -0.7347204089164734, - -0.5991426110267639, - -0.9430311322212219, - -0.7585272789001465, - -0.8234665989875793, - -0.8545740842819214, - -0.14105325937271118, - -0.014199018478393555, - -0.34937143325805664, - -0.004843294620513916, - -0.19558537006378174, - -0.7410959005355835, - -0.876293420791626, - -0.45944130420684814, - -0.645321249961853, - -0.28244680166244507, - -0.13762110471725464, - -0.6961289644241333, - -0.17530977725982666, - -0.1276518702507019, - -0.40358179807662964, - -0.9296593070030212, - -0.22403669357299805, - -0.46550416946411133, - -0.09695237874984741, - -0.7555620074272156, - -0.7586738467216492, - -0.901343584060669, - -0.04748272895812988, - -0.022760987281799316, - -0.18168526887893677, - -0.13777267932891846, - -0.5271425843238831, - -0.21348339319229126, - -0.6210537552833557, - -0.9578269720077515, - -0.7002136707305908, - -0.8739659786224365, - -0.772597074508667, - -0.29854655265808105, - -0.5763288736343384, - -0.6866545081138611, - -0.738849937915802, - -0.01651740074157715, - -0.10252046585083008, - -0.24462497234344482, - -0.09212309122085571, - -0.6821115016937256, - -0.8170308470726013, - -0.3699173927307129, - -0.9347706437110901, - 0.002071201801300049, - -0.878790020942688, - -0.6137945055961609, - -0.1944923996925354, - -0.30779916048049927, - -0.7183806300163269, - -0.1092216968536377, - -0.9390383362770081, - -0.049963414669036865, - -0.8985942006111145, - -0.5156030654907227, - -0.29756617546081543, - -0.8451487421989441, - -0.39132821559906006, - -0.9013601541519165, - -0.8194639682769775, - -0.2985495328903198, - -0.9578280448913574, - -0.5193804502487183, - -0.5354547500610352, - -0.4937899708747864, - -0.551175057888031, - -0.24200361967086792, - -0.7114773988723755, - -0.8307096362113953, - -0.36970198154449463, - -0.06439423561096191, - -0.6370649933815002, - -0.689975917339325, - -0.12258321046829224, - -0.18023985624313354, - -0.19095778465270996, - -0.3796960711479187, - -0.8365139365196228, - -0.5827064514160156, - -0.6776121854782104, - -0.155722975730896, - -0.815798282623291, - -0.8836721181869507, - -0.4118494391441345, - -0.8600543141365051, - -0.5819756984710693, - -0.9878496527671814, - -0.4733116626739502, - -0.39344197511672974, - -0.9271964430809021, - -0.2623245120048523, - -0.9258480668067932, - -0.3514559864997864, - -0.4852069616317749, - -0.4426721930503845, - -0.40432196855545044, - -0.9309852719306946, - -0.0453914999961853, - -0.3760489225387573, - -0.5907788276672363, - -0.2762359380722046, - -0.6073105931282043, - -0.9012067317962646, - -0.9473795890808105, - -0.40634167194366455, - -0.2435922622680664, - -0.17430764436721802, - -0.14575618505477905, - -0.2706841826438904, - -0.8886765837669373, - -0.030385375022888184, - -0.6711327433586121, - -0.25839370489120483, - -0.4670237898826599, - -0.04427814483642578, - -0.2971336841583252, - -0.5705769062042236, - -0.12152796983718872, - -0.25330764055252075, - -0.32702285051345825, - -0.7577423453330994, - -0.7056634426116943, - -0.5181854367256165, - -0.1945406198501587, - -0.598212480545044, - -0.19792360067367554, - -0.3633694648742676, - -0.5090498328208923, - -0.028950512409210205, - -0.682405412197113, - -0.7791985869407654, - -0.4832354784011841, - -0.5657856464385986, - -0.38258951902389526, - -0.41143715381622314, - -0.1167987585067749, - -0.9711939096450806, - -0.38090211153030396, - -0.2797265648841858, - -0.7816302180290222, - -0.9141287803649902, - -0.9264760613441467, - -0.3138616681098938, - -0.4468371272087097, - -0.3800205588340759, - -0.4484589695930481, - -0.06766146421432495, - -0.5285791158676147, - -0.9801487326622009, - -0.5475660562515259, - -0.18531686067581177, - -0.5587353706359863, - -0.7672158479690552, - -0.5399236679077148, - -0.8216560482978821, - -0.06692808866500854, - -0.9802008867263794, - -0.02825319766998291, - -0.8521097302436829, - -0.29705530405044556, - -0.7402810454368591, - -0.9521987438201904, - -0.5989824533462524, - -0.5091099739074707, - -0.9797102212905884, - -0.7307826280593872, - -0.7059122920036316, - -0.3619755506515503, - -0.8406006097793579, - -0.8817713856697083, - -0.5424179434776306, - -0.7115862965583801, - -0.9517701864242554, - -0.7782663702964783, - -0.2805899381637573 - ], - [ - -1.4259082078933716, - -0.7628011107444763, - -1.1494905948638916, - -0.6412063837051392, - -1.26246976852417, - -0.8821176290512085, - -0.9137013554573059, - -0.8261319994926453, - -1.4889938831329346, - -1.151426076889038, - -0.7739695906639099, - -1.1284723281860352, - -0.8209890723228455, - -1.4593219757080078, - -1.3986201286315918, - -1.3377176523208618, - -0.62993985414505, - -1.1792564392089844, - -0.9241922497749329, - -0.7910730242729187, - -1.2805874347686768, - -1.0707993507385254, - -0.9492930173873901, - -1.3837287425994873, - -0.7922476530075073, - -1.3626549243927002, - -1.074632167816162, - -1.4410548210144043, - -1.105318307876587, - -0.9413167834281921, - -1.3076597452163696, - -0.7860186696052551, - -0.5949016809463501, - -0.6691449284553528, - -1.1743073463439941, - -0.8825331926345825, - -1.3958384990692139, - -1.3514487743377686, - -0.5281450748443604, - -1.41390860080719, - -0.7003967761993408, - -0.9328911900520325, - -0.5688399076461792, - -0.9510324001312256, - -1.3506070375442505, - -1.275319218635559, - -0.9986140727996826, - -1.49320387840271, - -1.4843120574951172, - -0.7307277321815491, - -1.0622351169586182, - -1.0467100143432617, - -1.0493956804275513, - -1.3928061723709106, - -0.7706238627433777, - -0.9685169458389282, - -1.1880372762680054, - -0.9757683277130127, - -1.1320688724517822, - -1.1181002855300903, - -1.211276888847351, - -0.8843178153038025, - -1.3307569026947021, - -1.083923578262329, - -0.8621208667755127, - -0.6330054402351379, - -1.401702642440796, - -1.3387494087219238, - -1.425265908241272, - -0.5421697497367859, - -0.8561704158782959, - -0.9407783150672913, - -1.4512741565704346, - -1.4663667678833008, - -0.9251981973648071, - -1.484214186668396, - -1.296557903289795, - -0.6689178347587585, - -0.8467381596565247, - -1.3045446872711182, - -1.0028636455535889, - -0.5476725697517395, - -1.1383848190307617, - -1.492264747619629, - -1.2163026332855225, - -0.5841254591941833, - -1.014596939086914, - -1.2489981651306152, - -0.9467659592628479, - -0.9262081980705261, - -0.5359898805618286, - -1.0335298776626587, - -0.7344545125961304, - -0.6320112347602844, - -1.0315008163452148, - -1.1436221599578857, - -0.6428101658821106, - -0.717728853225708, - -1.097362756729126, - -0.5427185297012329, - -0.5763674974441528, - -0.6342934966087341, - -0.5467061400413513, - -1.447387456893921, - -1.4838192462921143, - -1.186476707458496, - -1.2606291770935059, - -1.2610423564910889, - -1.2567602396011353, - -0.7670435309410095, - -0.5562624335289001, - -1.0019211769104004, - -0.8009345531463623, - -1.248938798904419, - -1.0995445251464844, - -0.7193322777748108, - -0.7792929410934448, - -1.058241367340088, - -0.7708737254142761, - -1.0753350257873535, - -0.5170380473136902, - -0.6725472807884216, - -1.158711314201355, - -0.9907874464988708, - -0.8775873780250549, - -1.182322382926941, - -0.8543204069137573, - -1.4351080656051636, - -0.5689177513122559, - -1.194091796875, - -0.9574165344238281, - -1.3230631351470947, - -0.5407227277755737, - -1.096113681793213, - -0.8761577010154724, - -0.7989087104797363, - -1.1100937128067017, - -1.000990629196167, - -0.6183527708053589, - -1.1465448141098022, - -1.2729742527008057, - -1.1849963665008545, - -0.9400474429130554, - -1.4361584186553955, - -0.8586726784706116, - -1.0775995254516602, - -0.7488973140716553, - -0.5302025675773621, - -0.8484327793121338, - -0.6460759043693542, - -0.9092519283294678, - -1.2286038398742676, - -1.2114779949188232, - -0.9478926658630371, - -0.781777024269104, - -1.2581359148025513, - -0.9231940507888794, - -0.6026317477226257, - -0.7068359851837158, - -0.5500870943069458, - -0.6155065894126892, - -0.8434672355651855, - -0.6625481843948364, - -1.1525171995162964, - -1.0879313945770264, - -0.5161118507385254, - -0.8322681784629822, - -1.306541085243225, - -0.521882951259613, - -0.5572435259819031, - -1.374621033668518, - -1.0994514226913452, - -1.0482306480407715, - -1.40201735496521, - -0.7103193998336792, - -1.2804256677627563, - -0.554164469242096, - -1.2223751544952393, - -0.8806891441345215, - -0.7358636260032654, - -1.1746655702590942, - -1.0769728422164917, - -0.5750457644462585, - -1.044844388961792, - -0.5782709717750549, - -1.0069940090179443, - -1.4719765186309814 - ], - [ - -0.9238142967224121, - -0.0698205828666687, - -0.1403544545173645, - -0.1853806972503662, - -0.7912553548812866, - -0.3743066191673279, - -0.35251379013061523, - -0.6051955223083496, - -0.6003751754760742, - -1.028996467590332, - -1.004363775253296, - -0.4606083631515503, - -0.992021381855011, - -0.7569908499717712, - -0.5070105791091919, - -0.07780998945236206, - -0.9578995704650879, - -0.8302999138832092, - -0.6006336212158203, - -0.6636675596237183, - -0.26259171962738037, - -0.2994014620780945, - -0.49584078788757324, - -0.6343724727630615, - -0.5550675988197327, - -0.5611420273780823, - -0.44774508476257324, - -0.7359809279441833, - -0.08619123697280884, - -0.4696958065032959, - -0.10622358322143555, - -1.0131781101226807, - -0.7854553461074829, - -0.2939700484275818, - -0.9478037357330322, - -0.3207838535308838, - -0.907844066619873, - -0.9980040192604065, - -0.8072037696838379, - -0.4208117723464966, - -0.11277163028717041, - -0.5618969798088074, - -0.7238936424255371, - -0.211897611618042, - -0.49919605255126953, - -0.540176272392273, - -0.4875825047492981, - -0.05083209276199341, - -0.37905454635620117, - -0.348609983921051, - -0.5767907500267029, - -0.6685472726821899, - -0.06904172897338867, - -0.06032043695449829, - -0.6514592170715332, - -0.9603331089019775, - -0.4267512559890747, - -0.07870161533355713, - -0.6747346520423889, - -0.6316162347793579, - -0.493461549282074, - -0.33446455001831055, - -0.5631297826766968, - -0.7761831879615784, - -0.07967740297317505, - -1.0099406242370605, - -0.710037887096405, - -0.37101638317108154, - -0.9681183099746704, - -0.5347010493278503, - -0.9131768941879272, - -0.442663311958313, - -0.4550727605819702, - -0.11293643712997437, - -0.555015504360199, - -0.5921054482460022, - -0.03974407911300659, - -0.7025007605552673, - -1.024024248123169, - -0.7622687220573425, - -0.7169705629348755, - -0.7112247943878174, - -0.5937038064002991, - -0.5658383369445801, - -0.6262092590332031, - -0.7381301522254944, - -0.06292319297790527, - -0.11036300659179688, - -0.4519262909889221, - -0.9524312615394592, - -0.251644492149353, - -0.634813129901886, - -0.10808104276657104, - -0.20290517807006836, - -0.8795364499092102, - -0.7133604288101196, - -0.6017213463783264, - -0.4749462604522705, - -0.9139991402626038, - -0.47803354263305664, - -0.6484487652778625, - -0.601347804069519, - -1.0212500095367432, - -0.7061208486557007, - -0.7504950165748596, - -0.9117038249969482, - -0.9980932474136353, - -0.6756836771965027, - -0.6640557646751404, - -0.30770397186279297, - -0.5957074761390686, - -0.2663015127182007, - -0.4265478849411011, - -0.7591960430145264, - -0.9133948087692261, - -0.6641430854797363, - -0.5102360248565674, - -0.18117982149124146, - -0.43050360679626465, - -0.42123615741729736, - -0.42209959030151367, - -0.8755217790603638, - -0.9732856750488281, - -0.19557172060012817, - -0.42915111780166626, - -0.26035380363464355, - -0.6769423484802246, - -0.8010091781616211, - -0.28690940141677856, - -0.6068267226219177, - -0.9076883792877197, - -0.7012059092521667, - -0.4878358840942383, - -0.8782142400741577, - -0.5469277501106262, - -0.6292080283164978, - -0.06442302465438843, - -0.39036065340042114, - -0.25186532735824585, - -0.39602231979370117, - -0.576009213924408, - -0.9295237064361572, - -0.395857036113739, - -0.08957946300506592, - -0.33785170316696167, - -0.04375237226486206, - -0.14963960647583008, - -0.31550389528274536, - -0.12502753734588623, - -0.7684140801429749, - -0.27367401123046875, - -0.5296682119369507, - -0.5896162390708923, - -0.7512279152870178, - -0.5819600224494934, - -0.4105685353279114, - -0.8416110873222351, - -0.9148604869842529, - -0.43707555532455444, - -1.010685682296753, - -0.5056235790252686, - -0.7135579586029053, - -0.6027063727378845, - -0.39949893951416016, - -0.2314024567604065, - -0.7767473459243774, - -0.3291957378387451, - -0.06120145320892334, - -0.9269648194313049, - -0.8520622253417969, - -0.9365422129631042, - -0.2460879683494568, - -0.9516366124153137, - -0.5487070083618164, - -0.630145251750946, - -0.6992456912994385, - -0.15334832668304443, - -0.22281140089035034, - -0.3819270730018616, - -0.49393123388290405, - -0.2525184750556946, - -0.5560675263404846, - -0.29308879375457764, - -0.859779953956604, - -0.599997341632843, - -0.72066730260849, - -0.726995050907135 - ] - ], - "episode_step": [ - 0, - 20, - 40, - 60 - ], - "root_height": [ - 0.2529202103614807, - 0.46661919355392456, - 0.5359861254692078, - 0.6554553508758545 - ] - }, - "noise": { - "base_lin_vel": [ - [ - 0.04883061721920967, - -0.013821971602737904, - -0.008735167793929577 - ], - [ - -0.031018365174531937, - 0.040441371500492096, - -0.01481848955154419 - ], - [ - -0.014213168993592262, - 0.09062711149454117, - -0.05037219449877739 - ], - [ - 0.03259098529815674, - 0.08449690788984299, - 0.018265271559357643 - ] - ], - "base_ang_vel": [ - [ - -0.12153711169958115, - -0.06397805362939835, - 0.1936502754688263 - ], - [ - 0.09037470817565918, - 0.0034952403511852026, - 0.020504523068666458 - ], - [ - 0.12750859558582306, - -0.011539625935256481, - 0.15800964832305908 - ], - [ - 0.04557690769433975, - 0.13596391677856445, - 0.06388843059539795 - ] - ], - "projected_gravity": [ - [ - 0.017629355192184448, - -0.02608974650502205, - -0.030870115384459496 - ], - [ - -0.009302496910095215, - 0.019067002460360527, - 0.015243589878082275 - ], - [ - -0.013070404529571533, - 0.024919182062149048, - 0.025548148900270462 - ], - [ - -0.0126702431589365, - 0.04979836568236351, - -0.004926800727844238 - ] - ], - "joint_pos": [ - [ - -0.0057575032114982605, - 0.0013853668933734298, - 0.009871982038021088, - 0.005030627362430096, - 0.005901203025132418, - -0.008174210786819458, - -0.008026707917451859, - 0.004386216402053833, - 0.005877886898815632, - -0.009357362985610962, - -0.002096047392114997, - 0.003729259828105569, - -0.003522734623402357, - 0.007057160139083862, - -0.000681234581861645, - 0.0036938809789717197, - -0.0020403824746608734, - 0.001927495002746582, - 0.006183958146721125, - 0.0006244289688766003, - 0.008813641965389252, - -0.0031896650325506926, - -0.007687872275710106, - 0.004311834461987019, - 0.0031393300741910934, - 0.008208028972148895, - -0.008771959692239761, - -0.0005437945947051048, - -0.009363388642668724, - 0.007150211371481419, - -0.003804231761023402, - -0.0016525244573131204, - 0.009646023623645306, - 0.005172127392143011, - -0.008134937845170498, - 0.0015237081097438931, - 0.0016654860228300095 - ], - [ - -0.0070250011049211025, - 0.008180037140846252, - -0.0015627526445314288, - -0.007862006314098835, - -0.0005490505718626082, - 0.009878805838525295, - 0.009438781067728996, - 0.001189577509649098, - 0.0054941559210419655, - -0.002092573558911681, - -0.006113527808338404, - 0.007938407361507416, - -0.0036140417214483023, - -0.00310951704159379, - -0.0070936414413154125, - 0.007110206875950098, - 0.009609895758330822, - -0.007239156868308783, - 0.0022065674420446157, - 0.009812054224312305, - 0.002668023109436035, - 0.0015845977468416095, - 0.009937254711985588, - -0.006574833299964666, - 0.002253255806863308, - -0.009269744157791138, - -0.008170354180037975, - -0.002349680522456765, - 0.004635028541088104, - 0.00797504186630249, - -0.0029285443015396595, - -0.004988042172044516, - -0.009925201535224915, - -0.0014547478640452027, - -0.007709812838584185, - 0.009672446176409721, - -0.006436267867684364 - ], - [ - -0.00582847697660327, - 0.009322091937065125, - -0.008036257699131966, - -0.004748580511659384, - -0.0014337527099996805, - 0.008422642946243286, - -0.00993817113339901, - -0.003982769325375557, - -0.0006971549591980875, - -0.0036841523833572865, - -0.007783230394124985, - 0.0002665829670149833, - 0.004849553108215332, - 0.007614966481924057, - 0.005160970613360405, - 0.002419987926259637, - -0.005628595128655434, - -0.007644395809620619, - -0.008825138211250305, - -0.006654081400483847, - 0.00674707256257534, - -0.0007596480427309871, - 0.0014600479044020176, - -0.007051425985991955, - 0.00920053943991661, - 0.007913350127637386, - 0.00615357980132103, - 0.006131371017545462, - 0.0076631163246929646, - 0.009503038600087166, - 0.008999968878924847, - -0.002048282651230693, - 0.0034172546584159136, - 0.00483757583424449, - -0.00964801013469696, - -0.006213091313838959, - 0.004030534066259861 - ], - [ - -0.0094909043982625, - -0.0027957058046013117, - 0.00891487579792738, - -0.0026885520201176405, - -0.006809473969042301, - -0.009270472452044487, - 0.007180241402238607, - 0.003248116932809353, - 0.0021526420023292303, - -0.0015011620707809925, - -0.006352534051984549, - 0.005831989925354719, - -0.007459655869752169, - -0.004819393157958984, - -0.0031832025852054358, - 0.00876720529049635, - 0.00456439983099699, - -0.008394292555749416, - -0.007530570030212402, - -0.004901149310171604, - -0.00695707555860281, - 0.004014358390122652, - -0.0026265643537044525, - -0.003164188703522086, - 0.009519331157207489, - -0.00016419529856648296, - 0.007517525926232338, - -0.008004449307918549, - 0.0027327954303473234, - -2.755045898084063e-05, - -0.004322327207773924, - -0.007389858830720186, - -0.002289263065904379, - -0.003180756466463208, - -0.008036520332098007, - -0.007352158892899752, - 0.005940380040556192 - ] - ], - "joint_vel": [ - [ - 1.291954755783081, - -0.12978136539459229, - -1.3720617294311523, - -0.5396355986595154, - 0.21575599908828735, - -1.0990208387374878, - -0.24226319789886475, - 1.0541456937789917, - -0.31519049406051636, - 1.2596321105957031, - -0.0023235082626342773, - -0.10287433862686157, - 0.30647099018096924, - -0.3931882381439209, - -0.29570817947387695, - 1.479931354522705, - -1.0126559734344482, - -0.319095253944397, - -0.3643478751182556, - 1.079376220703125, - 0.5055824518203735, - 0.14722323417663574, - -0.4214363694190979, - -0.7030287981033325, - 1.3261168003082275, - -0.4632795453071594, - 0.5863493084907532, - 0.22087472677230835, - 0.4658646583557129, - 0.7025163173675537, - 0.5729398727416992, - -0.6472122073173523, - -0.07871317863464355, - 0.27562934160232544, - 0.4515758156776428, - -0.010535895824432373, - 0.779064416885376 - ], - [ - 0.642591118812561, - 0.6226478219032288, - -0.7088395357131958, - 1.1831715106964111, - -0.37703847885131836, - -1.2027876377105713, - 0.19178080558776855, - -0.08722668886184692, - -0.5969939231872559, - 0.6154507398605347, - -0.16152137517929077, - 1.0399879217147827, - -1.0715959072113037, - -0.9239612817764282, - -0.4761459231376648, - -1.2741475105285645, - 0.35439062118530273, - -0.5172491669654846, - 1.1776759624481201, - 1.0715643167495728, - -1.3331444263458252, - -0.8865401744842529, - 0.7593340873718262, - 0.8651373982429504, - 0.9665860533714294, - -1.2105270624160767, - 0.220251202583313, - 1.231561303138733, - -0.3581063747406006, - -1.0719406604766846, - 1.4191513061523438, - 1.45975661277771, - -0.28395169973373413, - -0.5377694964408875, - 1.2441927194595337, - 0.5622957944869995, - -1.0658917427062988 - ], - [ - 0.673727810382843, - -1.1740869283676147, - -0.052003562450408936, - 1.2892675399780273, - 0.2313660979270935, - 0.9932064414024353, - 1.3333872556686401, - -1.1939518451690674, - -0.6817955374717712, - 0.048616111278533936, - 0.9909465909004211, - 0.3479481339454651, - -1.445811152458191, - 0.23537743091583252, - 1.4869927167892456, - -1.3971707820892334, - -1.4305942058563232, - -0.6248220205307007, - 0.20929992198944092, - 1.4383745193481445, - -0.2615094780921936, - 0.0792306661605835, - -0.5388666987419128, - -1.4612298011779785, - 0.8085447549819946, - 0.6013521552085876, - 1.1867012977600098, - 0.472695529460907, - 1.0868589878082275, - 1.108255386352539, - -0.9932003617286682, - 0.8409976959228516, - -0.21922892332077026, - 0.8188753724098206, - 0.5408661961555481, - 0.7041410207748413, - 1.2410402297973633 - ], - [ - 0.2413002848625183, - 1.4686260223388672, - -0.9628372192382812, - -0.5439913272857666, - -0.2940552234649658, - -0.40880584716796875, - 0.022405385971069336, - -0.6928614377975464, - -0.3720552921295166, - 1.3106389045715332, - -0.8510822653770447, - 1.1379854679107666, - -1.117159366607666, - -1.222306489944458, - 1.3393640518188477, - 0.528426468372345, - 0.05907100439071655, - 1.3263695240020752, - -0.2693510055541992, - -0.7407882213592529, - 0.37777572870254517, - 0.7477294206619263, - 0.02171802520751953, - -0.20283597707748413, - 0.6097682118415833, - 0.8395984768867493, - -1.4558920860290527, - -0.2044631838798523, - 0.6514424085617065, - -1.273893117904663, - -0.2686917185783386, - 1.1251798868179321, - -1.4518529176712036, - 0.9102137088775635, - -0.9907659888267517, - -1.15702223777771, - 0.6048660278320312 - ] - ], - "height_scan": [ - [ - 0.05223618820309639, - 0.07281935214996338, - 0.04870041832327843, - 0.05835052952170372, - 0.052443649619817734, - 0.05397450923919678, - 0.09696508944034576, - -0.07384004443883896, - 0.07450683414936066, - -0.07972053438425064, - -0.05582697317004204, - -0.0025103928055614233, - 0.07548464834690094, - -0.050818659365177155, - 0.06830856949090958, - -0.07037242501974106, - -0.05029522255063057, - 0.05106857046484947, - 0.01951082982122898, - -0.020518077537417412, - -0.08504784107208252, - 0.06689858436584473, - -0.0677390918135643, - 0.0659003034234047, - 0.05153893306851387, - -0.013903319835662842, - -0.0023876905906945467, - -0.09476470202207565, - 0.0212746262550354, - 0.005085790064185858, - -0.04863257333636284, - -0.09049072116613388, - 0.03633619472384453, - 0.02675241231918335, - 0.05675138160586357, - -0.024968409910798073, - -0.09448989480733871, - -0.07529661804437637, - -0.03139544650912285, - 0.08747044950723648, - 0.0036378861404955387, - -0.045174311846494675, - 0.019420409575104713, - 0.05926762893795967, - 0.06458500772714615, - -0.0510452501475811, - 0.07272400707006454, - -0.041569530963897705, - -0.05765208229422569, - -0.056952621787786484, - -0.05372563749551773, - 0.02325906790792942, - 0.074857197701931, - 0.021959329023957253, - -0.03788477182388306, - 0.021688735112547874, - 0.09281517565250397, - -0.05556640774011612, - -0.0017329215770587325, - -0.032182980328798294, - 0.05220526456832886, - -0.03398185968399048, - -0.001009225845336914, - -0.025295555591583252, - -0.09604938328266144, - -0.07017292082309723, - -0.03073829412460327, - -0.03573857620358467, - -0.08984935283660889, - 0.037361741065979004, - -0.07143940776586533, - -0.08415845781564713, - 0.05524646118283272, - -0.05972718074917793, - 0.06948975473642349, - 0.011285520158708096, - -0.09069795906543732, - 0.0958104059100151, - -0.0906735435128212, - -0.09464623034000397, - -0.08838986605405807, - 0.05617351457476616, - -0.0951768159866333, - -0.017627596855163574, - 0.07602667063474655, - -0.02420262061059475, - 0.08525604009628296, - -0.08348560333251953, - 0.05300448089838028, - -0.04079849645495415, - -0.0757252499461174, - 0.05141489580273628, - -0.04664630815386772, - -0.08741772174835205, - 0.05962113291025162, - -0.08534891903400421, - -0.06840262562036514, - -0.060201775282621384, - 0.04182354360818863, - 0.02561667002737522, - -0.0847959890961647, - -0.09846224635839462, - -0.08396317809820175, - 0.023640204221010208, - -0.05097750574350357, - -0.005551946349442005, - 0.06938426941633224, - -0.0732884332537651, - 0.0006749391905032098, - -0.056771207600831985, - 0.012185418978333473, - 0.0973164439201355, - -0.060415517538785934, - 0.07151108235120773, - 0.07361127436161041, - 0.07730904966592789, - 0.007908380590379238, - 0.07696936279535294, - 0.08235805481672287, - -0.08053658157587051, - -0.0328231118619442, - -0.021262360736727715, - 0.056901752948760986, - -0.040937721729278564, - 0.03289289399981499, - 0.07808887213468552, - 0.09658312797546387, - 0.034975696355104446, - 0.019218016415834427, - -0.03373413160443306, - 0.045011308044195175, - -0.04058363661170006, - 0.07068971544504166, - -0.0020122171845287085, - -0.08119992166757584, - 0.08053254336118698, - -0.03653603792190552, - 0.0654241219162941, - -0.018424082547426224, - 0.09486718475818634, - 0.0954732894897461, - 0.06364333629608154, - -0.020247971639037132, - 0.0004792213439941406, - -0.04459511116147041, - 0.0002664923667907715, - 0.09078987687826157, - 0.016690218821167946, - 0.073288694024086, - -0.08597160875797272, - 0.04935495927929878, - -0.08829586952924728, - 0.03480887413024902, - -0.09769415855407715, - -0.01814298704266548, - -0.07210631668567657, - -0.05110807344317436, - 0.09955354779958725, - -0.025356043130159378, - 0.05369465425610542, - -0.03282473236322403, - -0.045098818838596344, - -0.0963265672326088, - -0.014818823896348476, - 0.0970042496919632, - -0.04285046085715294, - -0.022491419687867165, - -0.08302469551563263, - -0.09502489864826202, - 0.09619249403476715, - 0.04927661642432213, - -0.07743270695209503, - 0.07388831675052643, - -0.03430875763297081, - 0.03442783281207085, - -0.004544377326965332, - 0.017544997856020927, - -0.09462057799100876, - -0.03310791403055191, - -0.08079953491687775, - 0.09451492875814438, - 0.034996867179870605, - 0.037820518016815186, - 0.05495729669928551, - -0.06423377990722656, - 0.07771261781454086, - 0.06205332279205322 - ], - [ - 0.04454289749264717, - 0.09633924812078476, - 0.02234947681427002, - -0.03709438070654869, - 0.06537798792123795, - -0.0036954761017113924, - -0.0709725171327591, - -0.0022555708419531584, - -0.019058704376220703, - -0.08295859396457672, - -0.0668288841843605, - 0.06564565002918243, - -0.008014059625566006, - 0.02972257137298584, - -0.06803649663925171, - -0.022064972668886185, - 0.023032331839203835, - 0.09698740392923355, - -0.00355170969851315, - 0.014506769366562366, - -0.09618744999170303, - -0.057544078677892685, - -0.08049031347036362, - 0.06988827139139175, - 0.05659450218081474, - 0.081429623067379, - -0.01583411730825901, - 0.08203321695327759, - 0.022809697315096855, - 0.0025966644752770662, - 0.05739133432507515, - 0.04409891366958618, - 0.0792551040649414, - 0.02670816145837307, - -0.03649036958813667, - 0.03820384666323662, - 0.0018723130924627185, - 0.019527995958924294, - 0.07081802189350128, - 0.06685671955347061, - -0.04732177406549454, - 0.09013142436742783, - 0.08267374336719513, - -0.0453900471329689, - 0.025200629606842995, - -0.06997811794281006, - -0.004295730497688055, - -0.06129920482635498, - 0.0958331748843193, - -0.09351586550474167, - -0.0019816518761217594, - -0.07015091180801392, - -0.0036545991897583008, - -0.098160520195961, - -0.08309948444366455, - 0.08628272265195847, - -0.0010742545127868652, - 0.08101024478673935, - -0.07841720432043076, - -0.04145316034555435, - 0.09812722355127335, - -0.04454462602734566, - -0.055193401873111725, - 0.08143249899148941, - -0.07639435678720474, - 0.04121881723403931, - -0.09146231412887573, - 0.029009629040956497, - 0.005049038212746382, - -0.09455247223377228, - 0.09001157432794571, - -0.05812112241983414, - -0.051116298884153366, - 0.0989212915301323, - -0.09019549936056137, - -0.004682672210037708, - 0.08814387768507004, - -0.07027006149291992, - -0.04986201599240303, - -0.0037648796569556, - -0.0863538309931755, - 0.09390679746866226, - -0.001859474228695035, - -0.08387340605258942, - -0.021552979946136475, - -0.06085775047540665, - 0.08412354439496994, - 0.07760261744260788, - 0.033540308475494385, - -0.007966804318130016, - 0.09766508638858795, - 0.07904251664876938, - 0.036023106426000595, - -0.08556431531906128, - -0.02803283929824829, - -0.07379330694675446, - 0.032509781420230865, - 0.06164292246103287, - 0.06831105053424835, - -0.022572530433535576, - -0.03469933196902275, - 0.05436403676867485, - 0.05001084879040718, - 0.09955954551696777, - -0.06049082428216934, - 0.09357159584760666, - -0.058810651302337646, - 0.026045680046081543, - -0.06182229518890381, - -0.09808714687824249, - 0.07264067232608795, - 0.010136640630662441, - 0.03257089853286743, - -0.032764364033937454, - -0.0168511513620615, - -0.03634246811270714, - -0.050544869154691696, - 0.08632969111204147, - -0.05729971081018448, - -0.004422307014465332, - -0.03534975275397301, - -0.09977515041828156, - 0.05225184187293053, - 0.0987057238817215, - -0.030269814655184746, - -0.08964965492486954, - 0.010916459374129772, - -0.06403247267007828, - -0.021802140399813652, - -0.05780067667365074, - -0.00944598950445652, - 0.0031923174392431974, - -0.08463746309280396, - -0.08637287467718124, - -0.03272193670272827, - 0.0702233538031578, - -0.0173624400049448, - -0.025797570124268532, - -0.027443481609225273, - -0.0668819323182106, - -0.05136158689856529, - -0.0798415094614029, - -0.09227633476257324, - 0.08988626301288605, - -0.063568115234375, - -0.02681502141058445, - 0.02955458126962185, - -0.028867090120911598, - 0.044674646109342575, - 0.08567066490650177, - 0.011871886439621449, - 0.08843716233968735, - -0.03440951183438301, - -0.0064733028411865234, - -0.022554313763976097, - 0.03086276166141033, - -0.0625581294298172, - 0.09783625602722168, - 0.014882815070450306, - -0.08102332800626755, - 0.09377094358205795, - -0.0480550192296505, - 0.039779163897037506, - -0.0028643726836889982, - -0.03406388685107231, - -0.04476151615381241, - -0.08575516939163208, - 0.017647111788392067, - 0.015932811424136162, - 0.09425511211156845, - 0.03151027113199234, - -0.06185638904571533, - 0.01671609841287136, - 0.07810837030410767, - 0.06103941425681114, - 0.08734599500894547, - -0.03332633897662163, - 0.08172085136175156, - 0.023682106286287308, - 0.020249998196959496, - -0.06506023555994034, - -0.03692127391695976, - 0.0109749436378479, - -0.09830556064844131, - 0.04177987575531006, - -0.06498479098081589, - -0.017017556354403496 - ], - [ - -0.060177888721227646, - 0.07695546001195908, - -0.014235473237931728, - 0.056319501250982285, - 0.02831416204571724, - 0.007892119698226452, - -0.006545424461364746, - 0.08087054640054703, - -0.03766915947198868, - 0.07420116662979126, - 0.09232723712921143, - -0.013292670249938965, - -0.05053676292300224, - 0.08112919330596924, - -0.04358324036002159, - 0.040531981736421585, - -0.00338308815844357, - -0.04853551462292671, - 0.019414007663726807, - 0.06695699691772461, - -0.07237537205219269, - -0.09044715017080307, - -0.06357009708881378, - -0.08614201843738556, - -0.09205593913793564, - 0.0293959379196167, - 0.06513507664203644, - -0.09349967539310455, - 0.07413585484027863, - 0.07786517590284348, - -0.009642613120377064, - -0.0392284169793129, - 0.07549523562192917, - 0.059212576597929, - -0.07576089352369308, - -0.0649745985865593, - 0.08366977423429489, - 0.008084154687821865, - -0.02717365138232708, - -0.029798461124300957, - 0.09446582943201065, - -0.012926995754241943, - 0.08698822557926178, - 0.004940473940223455, - -0.012784969992935658, - -0.023058926686644554, - 0.028829706832766533, - 0.058387983590364456, - 0.024434149265289307, - 0.023169292137026787, - -0.05172818899154663, - -0.07996024936437607, - -0.04658503457903862, - 0.08656636625528336, - 0.08940503746271133, - 0.01622825860977173, - 0.08617103099822998, - -0.06762509793043137, - -0.048162173479795456, - -0.004065919201821089, - -0.07386770099401474, - 0.07864071428775787, - -0.03458273410797119, - -0.061582256108522415, - 0.09802073240280151, - -0.009779644198715687, - -0.0743241086602211, - 0.044352032244205475, - -0.040940023958683014, - -0.07911548763513565, - -0.03838908672332764, - 0.044205404818058014, - -0.06801752001047134, - 0.06352364271879196, - 0.015054309740662575, - -0.07199881225824356, - 0.014658212661743164, - -0.08582383394241333, - 0.08640106767416, - -0.09977733343839645, - -0.06289412826299667, - 0.08261722326278687, - -0.050750732421875, - -0.03644021973013878, - 0.046221841126680374, - 0.08906286209821701, - 0.017063140869140625, - -0.06804671138525009, - 0.08603443950414658, - -0.05672280862927437, - 0.0028892995323985815, - -0.036512888967990875, - 0.013175249099731445, - 0.03585414960980415, - 0.08900487422943115, - -0.09577719122171402, - 0.0565030463039875, - -0.06440498679876328, - -0.011400437913835049, - 0.014285111799836159, - 0.09614016860723495, - -0.09369521588087082, - 0.031447745859622955, - -0.09402529150247574, - 0.0787765309214592, - 0.030794739723205566, - -0.026997996494174004, - -0.0543629415333271, - 0.015773450955748558, - 0.06633491814136505, - 0.08232428133487701, - 0.02939009666442871, - -0.08830323070287704, - -0.09033264964818954, - -0.0017514824867248535, - 0.03475460037589073, - -0.04924731329083443, - 0.0434819720685482, - -0.031098080798983574, - 0.0637674331665039, - -0.06308324635028839, - 0.07233884185552597, - 0.07040169090032578, - -0.02829529158771038, - 0.023915303871035576, - 0.011319256387650967, - -0.0036240338813513517, - -0.08577024936676025, - -0.04389088228344917, - -0.013758850283920765, - -0.06473036110401154, - -0.023210465908050537, - -0.09655153006315231, - 0.060141194611787796, - 0.06681670993566513, - 0.027166618034243584, - 0.08695876598358154, - -0.06604985147714615, - 0.024394584819674492, - -0.014177262783050537, - -0.039409566670656204, - 0.003288710257038474, - -0.037664711475372314, - -0.03519705682992935, - 0.011927127838134766, - -0.03361406549811363, - 0.010974431410431862, - -0.06773481518030167, - 0.02630317211151123, - -0.08369409292936325, - 0.083478644490242, - 0.0990869551897049, - 0.017670845612883568, - 0.07239817827939987, - -0.0004896283498965204, - -0.031483449041843414, - 0.04860737547278404, - 0.019183708354830742, - 0.05684603378176689, - -0.033814456313848495, - 0.09340248256921768, - 0.053109217435121536, - -0.09801464527845383, - 0.002619576407596469, - 0.065671406686306, - -0.02876828983426094, - 0.039978016167879105, - 0.007457566447556019, - -0.06877227872610092, - -0.05707237869501114, - 0.03325716406106949, - -0.09724774211645126, - 0.07874005287885666, - 0.06798931211233139, - -0.022149313241243362, - -0.089079849421978, - 0.051343441009521484, - -0.0276468638330698, - 0.02570437267422676, - -0.0693320631980896, - -0.04721296951174736, - -0.057297635823488235, - -0.01642787456512451, - 0.05784428119659424, - -0.011216056533157825, - 0.006887924857437611, - -0.09863735735416412 - ], - [ - 0.06492900848388672, - -0.05018528923392296, - 0.062064457684755325, - -0.07700427621603012, - -0.001814508461393416, - 0.06802570819854736, - 0.09081774950027466, - 0.08420437574386597, - -0.0747893825173378, - 0.04318049177527428, - 0.02780994214117527, - 0.05061762407422066, - 0.04242526367306709, - -0.052467454224824905, - 0.09635405987501144, - 0.09998288005590439, - 0.03605765104293823, - -0.054834987968206406, - -0.018531108275055885, - -0.020158493891358376, - -0.007564544677734375, - 0.015007758513092995, - 0.04752645641565323, - -0.017624545842409134, - -0.06900528818368912, - -0.061820197850465775, - 0.0006124496576376259, - 0.08550550788640976, - -0.07483124732971191, - -0.027611935511231422, - -0.06230641528964043, - -0.08897799253463745, - -0.024844860658049583, - 0.02247229777276516, - -0.02584472857415676, - -0.05675327777862549, - 0.0214140173047781, - -0.0405544638633728, - 0.09345785528421402, - 0.02518298663198948, - 0.09620445221662521, - -0.03274405002593994, - 0.030910074710845947, - 0.004498720169067383, - -0.04450932890176773, - 0.002562320325523615, - -0.08341898024082184, - -0.04559285566210747, - 0.04857748746871948, - -0.09664980322122574, - -0.04028545692563057, - -0.065253846347332, - 0.09295113384723663, - -0.014491403475403786, - -0.013923239894211292, - -0.07172244042158127, - 0.016752421855926514, - 0.0895860567688942, - 0.0095442533493042, - -0.06746995449066162, - 0.058474134653806686, - 0.006632709410041571, - -0.0405343659222126, - -0.013835894875228405, - -0.030789125710725784, - -0.08558028936386108, - 0.017471028491854668, - 0.022224759683012962, - 0.0898694172501564, - -0.03353699669241905, - -0.06690653413534164, - 0.03363626077771187, - 0.0630890280008316, - 0.03878751024603844, - -0.09384393692016602, - 0.03938739374279976, - 0.05119385942816734, - -0.0706164762377739, - -0.007488727569580078, - 0.0720205083489418, - -0.07134796679019928, - 0.021893275901675224, - 0.007097959518432617, - -0.009904086589813232, - -0.07605725526809692, - -0.018921231850981712, - -0.08478587865829468, - -0.0780492052435875, - 0.02330777607858181, - -0.09622656553983688, - -0.07305561751127243, - -0.06052671745419502, - 0.07492294162511826, - 0.03955468162894249, - 0.07775330543518066, - -0.09507244825363159, - -0.079750657081604, - -0.028709782287478447, - 0.08950980007648468, - -0.0020228028297424316, - 0.08796421438455582, - -0.04481114074587822, - 0.031032312661409378, - -0.08870857954025269, - -0.09912276268005371, - 0.06978638470172882, - -0.09709373861551285, - 0.027917003259062767, - 0.091474249958992, - -0.07607757300138474, - 0.0995761975646019, - -0.0551193468272686, - -0.09306540340185165, - -0.038656651973724365, - -0.05885440111160278, - -0.04006587341427803, - -0.08847039192914963, - -0.02173534594476223, - 0.04869749769568443, - -0.0057662129402160645, - 0.06484593451023102, - 0.08721861988306046, - 0.004239571280777454, - 0.049637842923402786, - 0.08774683624505997, - 0.07212910801172256, - 0.08324874937534332, - -0.08991385996341705, - -0.09059523791074753, - -0.02483832836151123, - 0.023088349029421806, - -0.05824993923306465, - -0.07958310842514038, - 0.040257442742586136, - 0.01511386688798666, - 0.009482502937316895, - 0.039411511272192, - -0.03541206195950508, - 0.03691822290420532, - -0.04472539573907852, - -0.08181484788656235, - -0.01677536964416504, - 0.09425091743469238, - 0.07006136327981949, - 0.03818264231085777, - -0.08912372589111328, - 0.054737210273742676, - -0.08210381120443344, - -0.002013945719227195, - -0.0037061572074890137, - -0.007321989629417658, - -0.08071422576904297, - 0.03466961532831192, - 0.02558763138949871, - -0.04179755598306656, - 0.06225713714957237, - 0.03437762334942818, - -0.03198070451617241, - -0.0964009165763855, - -0.033636510372161865, - -0.029756272211670876, - -0.02094721794128418, - 0.09252848476171494, - 0.06269992887973785, - -0.01583796739578247, - -0.07114565372467041, - 0.0399775393307209, - -0.044922519475221634, - -0.09026225656270981, - 0.09108704328536987, - -0.06952458620071411, - -0.08122807741165161, - 0.0780658945441246, - -0.0601351372897625, - 0.004659926984459162, - 0.03154730796813965, - 0.09482917934656143, - 0.05946650728583336, - 0.07316402345895767, - 0.06975121796131134, - 0.04988523945212364, - 0.0017639637226238847, - 0.03666914626955986, - -0.007309675216674805, - -0.05546088144183159, - 0.07015790790319443, - 0.02920314110815525 - ] - ] - }, - "observation": [ - [ - -1.4069499969482422, - 1.0726349353790283, - 0.049436938017606735, - 0.20770153403282166, - 1.5801247358322144, - -2.01362943649292, - 0.6198207139968872, - -0.7186319231987, - 0.36630865931510925, - -0.5404043197631836, - 1.0749247074127197, - 0.509365975856781, - 1.6998757123947144, - 2.0931973457336426, - -0.06317567080259323, - 0.708791971206665, - 0.03769537806510925, - 0.10582129657268524, - 1.7412108182907104, - -0.010211399756371975, - 2.36370849609375, - 0.040722012519836426, - -0.1632193922996521, - -1.0978645086288452, - -0.7324596643447876, - -0.8262237310409546, - 1.1033231019973755, - -2.3885152339935303, - -1.8009309768676758, - 0.5275406241416931, - -0.18630656599998474, - -1.3558411598205566, - 1.4676485061645508, - -0.21312719583511353, - 1.3940887451171875, - 0.7675103545188904, - -0.8174532055854797, - 0.46711334586143494, - 0.11748413741588593, - 1.6193759441375732, - -2.2571065425872803, - 0.7601176500320435, - 0.7368858456611633, - 0.1756371557712555, - 0.7534976601600647, - -0.7085027694702148, - -0.855893075466156, - 0.7384267449378967, - 0.5709915161132812, - 0.7639805674552917, - 0.7299700975418091, - -1.5501199960708618, - -1.1869412660598755, - -0.27159300446510315, - -0.14697915315628052, - -2.0353751182556152, - 2.0110998153686523, - -0.9374741315841675, - 1.8839247226715088, - 0.3856588900089264, - -0.3548334538936615, - 1.0500422716140747, - -0.7178040146827698, - 0.4010559320449829, - 1.2236113548278809, - -2.367070198059082, - -1.1743779182434082, - -0.11067777872085571, - -0.2934943437576294, - -0.024584054946899414, - -2.2199034690856934, - 0.8375236392021179, - -0.6767169237136841, - 0.42656809091567993, - -1.7919156551361084, - 0.7142887711524963, - -0.6991146802902222, - 0.5482228398323059, - -1.4923269748687744, - -0.9754637479782104, - 0.1465175747871399, - 0.06860022246837616, - 0.31803399324417114, - -0.23632872104644775, - -0.897682785987854, - 0.29926350712776184, - -4.368893146514893, - 0.9160680174827576, - 4.984935283660889, - 0.1185097023844719, - 0.622482419013977, - -3.977832794189453, - -7.159829616546631, - -2.5309689044952393, - 4.721494674682617, - -2.8818554878234863, - -4.431326389312744, - -0.8927403092384338, - 3.247561454772949, - -8.902396202087402, - -1.2800079584121704, - -0.15178997814655304, - 3.9192891120910645, - 4.775058746337891, - -1.6756649017333984, - 3.9211177825927734, - -0.003280824515968561, - 6.563349723815918, - -1.1090047359466553, - 0.2747873067855835, - -4.711472034454346, - 1.2407253980636597, - -1.8140769004821777, - -0.3151421844959259, - -1.636247158050537, - 2.5366387367248535, - 2.516878843307495, - -1.2603262662887573, - -1.2249360084533691, - -0.07307285815477371, - 1.0792949199676514, - 0.6049757599830627, - -1.3881919384002686, - -0.23492558300495148, - -0.3690822124481201, - -0.9713019728660583, - -0.22935113310813904, - -0.6258819699287415, - -0.23511004447937012, - -0.08500783145427704, - -1.0, - -0.07460318505764008, - -0.5961368680000305, - -0.7958902716636658, - -0.7372646331787109, - -0.6263784766197205, - -0.9133599996566772, - -0.08750786632299423, - -0.23758938908576965, - -0.3990436792373657, - -0.8485054969787598, - -0.04837927967309952, - -0.12096543610095978, - -0.9025225043296814, - 0.022938072681427002, - -0.6172236800193787, - -0.11949606984853745, - -0.7126899361610413, - -0.05242621898651123, - -0.8628233671188354, - -0.41178834438323975, - -0.6643850207328796, - -0.9799152612686157, - -0.15816189348697662, - -1.0, - -0.8736452460289001, - -0.9476940035820007, - -0.4822450876235962, - -0.42991021275520325, - -0.19674810767173767, - -0.5494603514671326, - -0.31622016429901123, - -0.03983708471059799, - -0.543060839176178, - -0.5140841603279114, - -0.4160575568675995, - -0.007380019873380661, - -0.07551445811986923, - -0.620854377746582, - -0.11482659727334976, - -0.9045780897140503, - -0.5861231684684753, - -0.26507285237312317, - -0.9278913736343384, - -0.19200792908668518, - -0.6957923173904419, - -0.27295348048210144, - -0.18700772523880005, - -0.05285229533910751, - -0.3740500807762146, - -0.49943092465400696, - -0.43310168385505676, - -0.1758802980184555, - -0.9541838765144348, - -0.5258646011352539, - -0.7200484871864319, - -0.2434876561164856, - -0.8165240287780762, - -1.0, - -0.42583346366882324, - -0.4432119131088257, - -0.4066895842552185, - -0.6966076493263245, - -1.0, - -0.915302038192749, - 0.018335331231355667, - -0.3675485849380493, - -0.19474241137504578, - -0.70863276720047, - -0.308053195476532, - 0.021491937339305878, - -0.734555721282959, - -0.4287946820259094, - -0.8190137147903442, - -0.174106165766716, - -1.0, - -0.4624946713447571, - -0.1576959490776062, - -0.9789081811904907, - -0.8523783683776855, - -0.6040578484535217, - -0.3257391154766083, - -0.2313000112771988, - -0.22202253341674805, - -0.6310192942619324, - -0.1516180783510208, - -0.9837287664413452, - -0.18121132254600525, - -1.0, - -0.8227438926696777, - -0.3234846591949463, - -0.04372211545705795, - -0.5654221177101135, - -0.924054741859436, - -0.839948296546936, - -0.2116660177707672, - -0.08218534290790558, - -0.802496075630188, - -0.04184497520327568, - -0.7045097947120667, - -0.6210234761238098, - -0.23332610726356506, - -0.6844813227653503, - -0.7635685801506042, - -0.14865607023239136, - -0.7068381309509277, - -0.06114513427019119, - -0.12538956105709076, - -0.31292924284935, - -0.5394546985626221, - -0.67376708984375, - -0.5940524935722351, - -0.811767578125, - -0.5034485459327698, - -0.3999008536338806, - 0.010425448417663574, - -0.8841590285301208, - -0.34156322479248047, - -0.07080978900194168, - -0.13235896825790405, - -0.12883885204792023, - -0.4458642601966858, - -0.346724271774292, - -0.17780420184135437, - -0.08626529574394226, - -0.05570312589406967, - -0.3425547778606415, - -1.0, - -0.3911460041999817, - -0.11732953786849976, - -0.6412492990493774, - -0.05862497165799141, - -0.5735750794410706, - -0.4311599135398865, - -0.08645355701446533, - -0.32891371846199036, - -0.3082035183906555, - -0.20590965449810028, - -0.2355968952178955, - -0.7337773442268372, - -0.9499973058700562, - 0.012812629342079163, - -1.0, - -0.7039413452148438, - -0.6378687024116516, - -0.1633273959159851, - -0.7168658375740051, - -0.9818416237831116, - -0.41380274295806885, - -1.0, - -0.45019081234931946, - -0.2783898711204529, - -0.9580066800117493, - -0.6039050817489624, - -0.7058929204940796, - -0.19464150071144104, - -0.5166842937469482, - -0.8963528275489807, - -0.9048511981964111, - -0.6705685257911682, - -0.39719247817993164, - -0.7142139673233032, - -0.5789967179298401, - -0.15488079190254211, - -0.7415249347686768, - -0.053867265582084656, - -1.0, - -0.7698261737823486, - -0.47435063123703003, - -0.18163345754146576, - -0.9208264946937561, - -0.280893474817276, - -1.0, - -0.4571278989315033, - -0.3129851818084717, - -0.1692306399345398, - -0.40727099776268005, - -0.831913948059082, - -0.8596212863922119, - -0.3149305582046509 - ], - [ - -0.38794785737991333, - -0.1370657980442047, - 0.3916037082672119, - -0.029683813452720642, - 0.3664757311344147, - 1.0797909498214722, - 0.20487499237060547, - 0.9954610466957092, - 0.043222248554229736, - -0.9210910201072693, - 0.24086834490299225, - -0.13246694207191467, - 0.5249882936477661, - 0.3510620594024658, - -0.1895255595445633, - -0.8441972136497498, - 0.16236089169979095, - -0.03734248876571655, - 1.07917058467865, - -0.37964552640914917, - -0.6994788646697998, - 0.7010900974273682, - 1.9037379026412964, - 0.145101398229599, - 0.578916609287262, - -1.2035285234451294, - 1.3468708992004395, - 0.7779930233955383, - -0.4923069179058075, - -1.1138312816619873, - -1.0059609413146973, - -0.683100163936615, - 0.7017560005187988, - -0.027139026671648026, - -0.5922361612319946, - -1.3698062896728516, - -0.8508687615394592, - -0.002800464630126953, - 0.9882587194442749, - 2.3288991451263428, - -2.7383511066436768, - -0.5574095249176025, - 0.14649294316768646, - -1.2968852519989014, - -0.49687111377716064, - -1.4662840366363525, - -0.8887143731117249, - 2.0657145977020264, - -1.0418304204940796, - 0.925268292427063, - -0.9827067255973816, - -0.5835732817649841, - 0.9494291543960571, - -0.9632630944252014, - -0.16914308071136475, - 0.6905560493469238, - -0.517609179019928, - -1.4099457263946533, - 0.991094708442688, - 0.45137912034988403, - 1.086907982826233, - -0.08564960956573486, - 0.5591758489608765, - 2.0099101066589355, - -2.1455254554748535, - 2.7763161659240723, - 0.34471726417541504, - 1.8947807550430298, - -1.210347056388855, - -1.5783727169036865, - -0.9104133248329163, - 0.6619883179664612, - -0.17619723081588745, - 1.3315041065216064, - -1.672218680381775, - 0.07152633368968964, - 0.9996780157089233, - -0.23330987989902496, - -1.0184625387191772, - 1.9776440858840942, - -0.12138926982879639, - 0.2517191767692566, - -0.3275165557861328, - 0.9971603155136108, - 0.8022367358207703, - 0.09318029880523682, - 2.340679407119751, - -3.2012264728546143, - 1.449082612991333, - -0.44263583421707153, - 6.466547012329102, - 4.260446548461914, - -1.1634079217910767, - -0.3325357735157013, - 0.6242257952690125, - 0.10043658316135406, - -3.3272321224212646, - 4.066169261932373, - 2.077775716781616, - -5.893347263336182, - 2.7362892627716064, - 0.7733586430549622, - 1.8175424337387085, - 5.035777568817139, - 1.0537075996398926, - -1.1087294816970825, - 1.8238623142242432, - -1.5047787427902222, - -5.391904830932617, - -4.920398235321045, - 1.9996800422668457, - 2.8552744388580322, - 2.4058778285980225, - 0.7430681586265564, - 7.5287909507751465, - -2.3544540405273438, - -1.5701937675476074, - 3.8395838737487793, - -4.007706642150879, - 3.4519639015197754, - -3.863215684890747, - 1.0905109643936157, - 2.3868038654327393, - -0.9103783965110779, - -0.19505473971366882, - -0.10260522365570068, - -0.41634348034858704, - -0.45699265599250793, - -0.7384158968925476, - -0.6701151132583618, - -0.9452866911888123, - -0.7775859832763672, - -0.9064251780509949, - -0.9214029908180237, - -0.07540760934352875, - -0.022213079035282135, - -0.3196488618850708, - -0.07287979125976562, - -0.21765033900737762, - -0.7180635929107666, - -0.7793059945106506, - -0.4629930257797241, - -0.6308144927024841, - -0.3786342442035675, - -0.19516518712043762, - -0.7766192555427551, - -0.1054215058684349, - -0.07105736434459686, - -0.32215216755867004, - -0.9454934000968933, - -0.14200347661972046, - -0.4426944851875305, - -0.09435571730136871, - -0.6981706619262695, - -0.714574933052063, - -0.8220884799957275, - -0.020774567499756813, - -0.05925135686993599, - -0.14348141849040985, - -0.1359003633260727, - -0.5076146125793457, - -0.14266537129878998, - -0.5541970133781433, - -1.0, - -0.6100822687149048, - -0.7912922501564026, - -0.8179871439933777, - -0.2733459174633026, - -0.6463069915771484, - -0.6909502148628235, - -0.800149142742157, - 0.07931577414274216, - -0.19603633880615234, - -0.24660661816596985, - -0.16227400302886963, - -0.6857661008834839, - -0.9151913523674011, - -0.45301687717437744, - -0.848487913608551, - 0.0009969472885131836, - -0.7977797985076904, - -0.6922116875648499, - -0.23594555258750916, - -0.20967194437980652, - -0.7629252672195435, - -0.16441509127616882, - -0.8576058149337769, - -0.126357764005661, - -0.8573753833770752, - -0.6070653796195984, - -0.26855653524398804, - -0.8400996923446655, - -0.48588067293167114, - -0.811348557472229, - -0.8775851130485535, - -0.3496658205986023, - -0.8589067459106445, - -0.6095759272575378, - -0.5401374101638794, - -0.40564608573913574, - -0.6214451193809509, - -0.29186564683914185, - -0.7152422666549683, - -0.9170634746551514, - -0.27579519152641296, - -0.06625370681285858, - -0.7209383845329285, - -0.7115288972854614, - -0.1834409534931183, - -0.0961163118481636, - -0.11335516721010208, - -0.3461557626724243, - -0.8444807529449463, - -0.48504137992858887, - -0.5985696911811829, - -0.1196998655796051, - -0.9013625979423523, - -0.911704957485199, - -0.4856427311897278, - -0.8275445103645325, - -0.5203327536582947, - -0.9195386171340942, - -0.4958841800689697, - -0.4281412959098816, - -0.8728324174880981, - -0.2123136669397354, - -0.8262885212898254, - -0.4119468033313751, - -0.39163535833358765, - -0.5014828443527222, - -0.3782762885093689, - -0.9928075671195984, - -0.1434786468744278, - -0.30340826511383057, - -0.5806421637535095, - -0.24366503953933716, - -0.6400749683380127, - -0.9180578589439392, - -0.9837220311164856, - -0.45688652992248535, - -0.15726256370544434, - -0.2316073477268219, - -0.15017849206924438, - -0.3060339391231537, - -0.9884517192840576, - 0.021866466850042343, - -0.5724270343780518, - -0.28866350650787354, - -0.5566734671592712, - -0.033361684530973434, - -0.3611661493778229, - -0.5923790335655212, - -0.17932865023612976, - -0.2627536356449127, - -0.3238305449485779, - -0.8423798084259033, - -0.7920362949371338, - -0.5509073734283447, - -0.12431726604700089, - -0.6155748963356018, - -0.22372117638587952, - -0.3908129334449768, - -0.5759317874908447, - -0.0803121030330658, - -0.7622469067573547, - -0.8714749217033386, - -0.39334923028945923, - -0.6293537616729736, - -0.40940454602241516, - -0.38188257813453674, - -0.14566585421562195, - -0.9265192747116089, - -0.2952314615249634, - -0.2678546905517578, - -0.6931930780410767, - -0.9485383033752441, - -0.9329493641853333, - -0.33641597628593445, - -0.41597437858581543, - -0.44257867336273193, - -0.3506227135658264, - -0.05277865007519722, - -0.6096024513244629, - -0.8863778114318848, - -0.5956210494041443, - -0.14553770422935486, - -0.5615997314453125, - -0.8012797236442566, - -0.584685206413269, - -0.9074112176895142, - -0.04928097873926163, - -0.9642680883407593, - 0.06600191444158554, - -0.8205994367599487, - -0.3589116930961609, - -0.7235649228096008, - -0.8740903735160828, - -0.5379430651664734, - -0.42176398634910583, - -1.0, - -0.6490617990493774, - -0.6822301745414734, - -0.34172555804252625, - -0.90566086769104, - -0.9186926484107971, - -0.5314429998397827, - -0.8098918795585632, - -0.9099903106689453, - -0.8432511687278748, - -0.2976074814796448 - ], - [ - -0.34489521384239197, - 1.8047900199890137, - 1.1037474870681763, - 0.06741125881671906, - 0.2864755392074585, - 0.9052565097808838, - 0.30543091893196106, - 0.19615589082241058, - 0.9578755497932434, - -0.2072896957397461, - 1.3115217685699463, - -0.05750608071684837, - -1.4695765972137451, - -1.3935120105743408, - 0.11923523247241974, - -1.0076990127563477, - -1.1663349866867065, - 0.6443485021591187, - 0.6754648685455322, - -1.8998386859893799, - 1.5639349222183228, - 0.67162024974823, - 0.5064153671264648, - -1.2969621419906616, - 0.3411876857280731, - -0.4930904507637024, - -0.16637907922267914, - 1.955784797668457, - -0.8513618111610413, - 0.8695812225341797, - -0.4618198871612549, - -0.45110780000686646, - -1.2165738344192505, - 1.756473183631897, - -0.9883906245231628, - -0.36364105343818665, - -0.8311875462532043, - -1.6740882396697998, - 0.006809484213590622, - -0.9013964533805847, - -0.6993459463119507, - -0.59490966796875, - 1.1761394739151, - 0.9036130905151367, - 0.9784457087516785, - 0.024272775277495384, - 1.2440671920776367, - 0.2982538044452667, - -0.31751030683517456, - 1.8425674438476562, - -2.1279940605163574, - 0.6066297292709351, - 1.8573932647705078, - 1.0225844383239746, - 0.6322453022003174, - 1.9611287117004395, - -1.029683232307434, - -0.6289929747581482, - -0.29764923453330994, - 0.5477957129478455, - 0.2375255823135376, - -1.5101028680801392, - 1.4332301616668701, - 1.9601681232452393, - -3.6899094581604004, - 0.1235198974609375, - -1.2099766731262207, - -0.14447900652885437, - 0.5947220921516418, - -0.7086330652236938, - -1.3694859743118286, - 1.0131797790527344, - -3.2103984355926514, - 1.7276785373687744, - 1.1196308135986328, - 0.7843965888023376, - 1.056311845779419, - 0.10424810647964478, - 0.40316128730773926, - 0.27374452352523804, - 2.5219497680664062, - -0.3305432200431824, - 0.5116696357727051, - 1.523533582687378, - 0.027112722396850586, - 1.762458086013794, - -1.9206892251968384, - -0.5940742492675781, - -0.3497787117958069, - 3.7373969554901123, - 8.128355026245117, - 4.392646312713623, - -3.1500251293182373, - 1.746557354927063, - -1.877929925918579, - -6.652039051055908, - 4.322165012359619, - 3.399026393890381, - -5.671651840209961, - -0.2512320280075073, - 7.311364650726318, - -0.06858460605144501, - 5.1759772300720215, - 3.3528430461883545, - -0.6859660744667053, - 3.756129503250122, - 3.4173645973205566, - -8.679512023925781, - 0.5880194902420044, - -0.7168394923210144, - 6.000116348266602, - -2.446148157119751, - -6.562048435211182, - -3.665959119796753, - 1.803282380104065, - 12.5444974899292, - 2.0668933391571045, - -11.293632507324219, - 1.1310479640960693, - -2.0034444332122803, - -9.082246780395508, - 4.693811893463135, - -3.442373037338257, - -1.0, - -0.685845673084259, - -1.0, - -0.584886908531189, - -1.0, - -0.8742254972457886, - -0.9202467799186707, - -0.7452614307403564, - -1.0, - -1.0, - -0.6816423535346985, - -1.0, - -0.8715258240699768, - -1.0, - -1.0, - -1.0, - -0.6333229541778564, - -1.0, - -0.904778242111206, - -0.7241160273551941, - -1.0, - -1.0, - -1.0, - -1.0, - -0.8843035697937012, - -1.0, - -1.0, - -1.0, - -1.0, - -0.8634516000747681, - -1.0, - -0.8252471089363098, - -0.5194064378738403, - -0.6099323630332947, - -1.0, - -0.9475077986717224, - -1.0, - -1.0, - -0.5553187131881714, - -1.0, - -0.6059309244155884, - -0.9458181858062744, - -0.4818516969680786, - -0.9460919499397278, - -1.0, - -1.0, - -0.9697843790054321, - -1.0, - -1.0, - -0.7075584530830383, - -1.0, - -1.0, - -1.0, - -1.0, - -0.6812188029289246, - -0.9522886872291565, - -1.0, - -1.0, - -1.0, - -1.0, - -1.0, - -0.8056771159172058, - -1.0, - -1.0, - -0.7641001343727112, - -0.6427850723266602, - -1.0, - -1.0, - -1.0, - -0.6212852597236633, - -0.8945595026016235, - -0.8965728878974915, - -1.0, - -1.0, - -0.9101439118385315, - -1.0, - -1.0, - -0.7547416687011719, - -0.7603371143341064, - -1.0, - -1.0, - -0.46505534648895264, - -1.0, - -1.0, - -1.0, - -0.49506258964538574, - -0.9975337982177734, - -1.0, - -0.8607315421104431, - -0.9829310178756714, - -0.5331006050109863, - -1.0, - -0.7212792634963989, - -0.5961570739746094, - -0.9424959421157837, - -1.0, - -0.5863071084022522, - -0.7821338176727295, - -1.0, - -0.5284334421157837, - -0.4802273213863373, - -0.7279887199401855, - -0.5152583718299866, - -1.0, - -1.0, - -1.0, - -1.0, - -1.0, - -1.0, - -0.7007086277008057, - -0.4739381670951843, - -0.9725310802459717, - -0.8892377614974976, - -1.0, - -1.0, - -0.6845777034759521, - -0.8285402655601501, - -1.0, - -0.8019717931747437, - -1.0, - -0.5801212787628174, - -0.6002084612846375, - -1.0, - -1.0, - -0.8536720871925354, - -1.0, - -0.8579444289207458, - -1.0, - -0.6128086447715759, - -1.0, - -1.0, - -1.0, - -0.6372742652893066, - -1.0, - -0.8093410134315491, - -0.7717421054840088, - -1.0, - -1.0, - -0.5939581990242004, - -1.0, - -1.0, - -1.0, - -0.9777121543884277, - -1.0, - -0.8467455506324768, - -1.0, - -0.7379229068756104, - -0.5979374051094055, - -0.8221296072006226, - -0.7297700047492981, - -0.825773298740387, - -1.0, - -1.0, - -0.8754944801330566, - -0.7822666764259338, - -1.0, - -0.8745867013931274, - -0.583448052406311, - -0.6499899625778198, - -0.5839015245437622, - -0.5221040844917297, - -0.7903580069541931, - -0.7605628371238708, - -1.0, - -1.0, - -0.5448801517486572, - -0.7922901511192322, - -1.0, - -0.5906552076339722, - -0.614315927028656, - -1.0, - -1.0, - -0.9694905877113342, - -1.0, - -0.7324687242507935, - -1.0, - -0.5028210282325745, - -1.0, - -0.8549847602844238, - -0.805195689201355, - -1.0, - -1.0, - -0.5914736390113831, - -0.9870001077651978, - -0.5894870162010193, - -1.0, - -1.0 - ], - [ - 0.021648570895195007, - 0.11086653918027878, - -1.2490181922912598, - -1.643565058708191, - -0.7634243965148926, - -0.3057655394077301, - -0.39293476939201355, - 0.32056206464767456, - 0.8794295787811279, - 1.3140007257461548, - 0.15788041055202484, - -0.9369820952415466, - -0.33969563245773315, - -0.7839319109916687, - -0.9978094696998596, - -0.037705715745687485, - 0.9311642050743103, - 1.9204590320587158, - 2.471158981323242, - -0.2574692666530609, - 0.3723033368587494, - -0.9296101927757263, - 0.9741128087043762, - -0.6333039999008179, - 2.0450692176818848, - 0.31273964047431946, - -0.7360443472862244, - -0.9088994264602661, - -2.888251781463623, - 0.2500144839286804, - 1.0238547325134277, - 0.9109290838241577, - -0.36113137006759644, - -0.24050182104110718, - 0.9641605615615845, - -0.08623011410236359, - -1.3418458700180054, - -0.37914469838142395, - 1.0256342887878418, - -1.3035515546798706, - -0.0034576833713799715, - -0.5798326730728149, - -1.9055631160736084, - -1.0632941722869873, - -1.4172788858413696, - -0.8124759197235107, - 0.029615677893161774, - 1.746158242225647, - -0.5724268555641174, - 0.6349905729293823, - 1.711187720298767, - -3.042208194732666, - -0.46432942152023315, - 1.0053707361221313, - -0.7862245440483093, - 1.9614495038986206, - -0.5531682372093201, - 0.7538347244262695, - 1.355606198310852, - -1.9909489154815674, - 0.8867899179458618, - -1.7046852111816406, - -1.219997763633728, - 0.2828878164291382, - 0.2721024751663208, - -1.1090028285980225, - 1.1575616598129272, - -0.5123422741889954, - 0.3213130235671997, - 0.9234911799430847, - 0.5037670731544495, - -1.3494231700897217, - -1.33601713180542, - 0.39544299244880676, - 2.820998430252075, - -2.417839527130127, - 0.3972512483596802, - 0.4367092251777649, - 0.046646833419799805, - 0.764735758304596, - 1.8805278539657593, - -1.2150758504867554, - 1.2275629043579102, - -1.5455503463745117, - -1.9381508827209473, - 2.3605196475982666, - -4.359130382537842, - 1.4358174800872803, - 1.2535760402679443, - 2.251012086868286, - 4.294274806976318, - -2.0001187324523926, - -0.383171945810318, - 2.659695863723755, - 1.6124097108840942, - -1.9515864849090576, - -2.035461187362671, - 0.10162672400474548, - 3.356719970703125, - -7.295328617095947, - 3.0563695430755615, - -7.556408882141113, - 2.0578768253326416, - -1.689565658569336, - 1.8390171527862549, - -4.7228007316589355, - -6.407613754272461, - 1.15846586227417, - -0.7323806881904602, - 2.3890063762664795, - 5.626026630401611, - -3.9759743213653564, - 7.224872589111328, - -2.195585250854492, - 4.695905685424805, - 0.1920173466205597, - -4.04601526260376, - -1.238318920135498, - 4.002810478210449, - -0.024696817621588707, - -10.090450286865234, - -0.7411140203475952, - 1.8331068754196167, - -0.8588852882385254, - -0.12000587582588196, - -0.07829000055789948, - -0.26238498091697693, - -0.7930698394775391, - -0.3062809109687805, - -0.2616960406303406, - -0.5209911465644836, - -0.6751645803451538, - -0.9858160018920898, - -0.9765538573265076, - -0.40999072790145874, - -0.949596107006073, - -0.809458315372467, - -0.41065651178359985, - 0.022172890603542328, - -0.9218419194221497, - -0.8851348757743835, - -0.6191647052764893, - -0.6838260293006897, - -0.27015626430511475, - -0.28439369797706604, - -0.4483143389225006, - -0.6519970297813416, - -0.6240729093551636, - -0.622962236404419, - -0.4471326470375061, - -0.6504754424095154, - -0.16102248430252075, - -0.49730774760246277, - -0.16853000223636627, - -1.0, - -0.8103002309799194, - -0.2714977562427521, - -0.9736484885215759, - -0.3775371313095093, - -0.886430025100708, - -1.0, - -0.7137458920478821, - -0.39562878012657166, - -0.016567178070545197, - -0.5946410298347473, - -0.6929835677146912, - -0.2073988914489746, - -0.543705403804779, - -0.5376139283180237, - -0.5710014700889587, - -0.09642495214939117, - -0.3304770588874817, - -0.44525977969169617, - -0.6170762181282043, - -0.7338011264801025, - 0.02390940487384796, - -0.07481183856725693, - -0.665382444858551, - -1.0, - -0.4099988341331482, - 0.010884441435337067, - -0.6651903986930847, - -0.6990861892700195, - -0.4349874258041382, - -0.32783183455467224, - -0.6036641597747803, - -0.7900190949440002, - -0.11046652495861053, - -1.0, - -0.6925668716430664, - -0.34879162907600403, - -0.8782488703727722, - -0.5682380199432373, - -0.9800834059715271, - -0.4090270400047302, - -0.3919837474822998, - -0.07414892315864563, - -0.648859441280365, - -0.5527180433273315, - 0.011449780315160751, - -0.7731172442436218, - -1.0, - -0.6902481913566589, - -0.7883185148239136, - -0.6893315315246582, - -0.5866058468818665, - -0.5757424235343933, - -0.7022665143013, - -0.757051408290863, - -0.14770907163619995, - -0.18841221928596497, - -0.42861852049827576, - -1.0, - -0.32470011711120605, - -0.6953398585319519, - -0.03315810114145279, - -0.16335049271583557, - -0.8017831444740295, - -0.8084328770637512, - -0.6814720034599304, - -0.5036560297012329, - -0.8244893550872803, - -0.4800563454627991, - -0.5604845285415649, - -0.6461589336395264, - -0.9902176856994629, - -0.7948294281959534, - -0.8496177792549133, - -0.8419174551963806, - -1.0, - -0.647766649723053, - -0.5725815296173096, - -0.3837815523147583, - -0.4961312711238861, - -0.3214208483695984, - -0.5196132659912109, - -0.7978526949882507, - -0.9722492098808289, - -0.7042089700698853, - -0.5987064242362976, - -0.20291516184806824, - -0.3818061053752899, - -0.4270023703575134, - -0.35725367069244385, - -0.7883031368255615, - -0.9690461158752441, - -0.14593388140201569, - -0.3414042890071869, - -0.1882247030735016, - -0.5936936140060425, - -0.890923023223877, - -0.3775046467781067, - -0.631665050983429, - -0.884600043296814, - -0.7594558596611023, - -0.5674189925193787, - -0.8379567861557007, - -0.5318138599395752, - -0.6197255253791809, - -0.025011513382196426, - -0.4257727265357971, - -0.21494710445404053, - -0.4407477080821991, - -0.6578240394592285, - -0.9462990760803223, - -0.30160611867904663, - -0.01951809972524643, - -0.2996690571308136, - -0.13287609815597534, - -0.0949023962020874, - -0.3976077139377594, - -0.12704148888587952, - -0.7721202373504639, - -0.2809959948062897, - -0.6103824377059937, - -0.5549466013908386, - -0.7256402969360352, - -0.6237576007843018, - -0.3483113944530487, - -0.807233452796936, - -0.9468411803245544, - -0.5334764719009399, - -1.0, - -0.5353798270225525, - -0.7345051765441895, - -0.5101779103279114, - -0.3367990255355835, - -0.24724042415618896, - -0.8478929996490479, - -0.2892181873321533, - -0.10612396895885468, - -1.0, - -0.760975182056427, - -1.0, - -0.3273160457611084, - -0.8735707402229309, - -0.608842134475708, - -0.6254853010177612, - -0.6676983833312988, - -0.058519147336483, - -0.16334488987922668, - -0.3087630569934845, - -0.4241800308227539, - -0.20263323187828064, - -0.5543035864830017, - -0.25641965866088867, - -0.8670896291732788, - -0.6554582118988037, - -0.6505094170570374, - -0.6977919340133667 - ] - ], - "rewards": { - "track_lin_vel_xy_exp": [ - 1.47920191011508e-05, - 0.20297527313232422, - 8.514294717670238e-15, - 8.919614060687309e-07 - ], - "track_ang_vel_z_exp": [ - 0.001070165541023016, - 0.8154081106185913, - 0.16508378088474274, - 0.050110962241888046 - ], - "ang_vel_xy_l2": [ - 2.811472177505493, - 0.14616887271404266, - 0.09242473542690277, - 3.662100076675415 - ], - "dof_torques_l2": [ - 10865.8955078125, - 13946.0693359375, - 11106.2236328125, - 14253.060546875 - ], - "dof_acc_l2": [ - 13256993.0, - 9445130.0, - 3893592.0, - 9315391.0 - ], - "action_rate_l2": [ - 500.3487243652344, - 380.439453125, - 956.3876953125, - 655.1470336914062 - ], - "feet_air_time": [ - 0.0, - 0.0, - 0.009999999776482582, - 0.0 - ], - "flat_orientation_l2": [ - 0.8422490954399109, - 0.9992173314094543, - 0.13076509535312653, - 0.21791407465934753 - ], - "dof_pos_limits": [ - 0.8659737706184387, - 1.020351529121399, - 2.2473158836364746, - 2.237064838409424 - ], - "termination_penalty": [ - 0.0, - 1.0, - 1.0, - 1.0 - ], - "feet_slide": [ - 0.7694483399391174, - 2.6806671619415283, - 1.0911712646484375, - 3.377929210662842 - ], - "joint_deviation_hip": [ - 4.537287712097168, - 1.6166529655456543, - 4.990593433380127, - 2.418728828430176 - ], - "joint_deviation_arms": [ - 11.85978889465332, - 11.5762939453125, - 8.250176429748535, - 8.39795207977295 - ], - "joint_deviation_fingers": [ - 10.732789993286133, - 12.612953186035156, - 11.011817932128906, - 12.498083114624023 - ], - "joint_deviation_torso": [ - 0.7289369106292725, - 0.582530677318573, - 0.3363381326198578, - 2.0525288581848145 - ] - }, - "terminated": [ - false, - true, - true, - true - ] -} diff --git a/tests/gym/envs/tasks/fixtures/terrain_curriculum.json b/tests/gym/envs/tasks/fixtures/terrain_curriculum.json deleted file mode 100644 index 6bcee6dea6..0000000000 --- a/tests/gym/envs/tasks/fixtures/terrain_curriculum.json +++ /dev/null @@ -1,3566 +0,0 @@ -{ - "origins": [ - [ - [ - -36.0, - -76.0, - 0.4475184679031372 - ], - [ - -36.0, - -68.0, - 0.3967205584049225 - ], - [ - -36.0, - -60.0, - 0.4455190598964691 - ], - [ - -36.0, - -52.0, - 0.44384002685546875 - ], - [ - -36.0, - -44.0, - -0.4050811529159546 - ], - [ - -36.0, - -36.0, - -0.3751884400844574 - ], - [ - -36.0, - -28.0, - -0.43421876430511475 - ], - [ - -36.0, - -20.0, - -0.3770376741886139 - ], - [ - -36.0, - -12.0, - 0.059964701533317566 - ], - [ - -36.0, - -4.0, - 0.05228468030691147 - ], - [ - -36.0, - 4.0, - 0.06362871080636978 - ], - [ - -36.0, - 12.0, - 0.05683664605021477 - ], - [ - -36.0, - 20.0, - 0.10000000149011612 - ], - [ - -36.0, - 28.0, - 0.10000000149011612 - ], - [ - -36.0, - 36.0, - 0.10000000149011612 - ], - [ - -36.0, - 44.0, - 0.10000000149011612 - ], - [ - -36.0, - 52.0, - 0.009999999776482582 - ], - [ - -36.0, - 60.0, - 0.019999999552965164 - ], - [ - -36.0, - 68.0, - -0.07000000029802322 - ], - [ - -36.0, - 76.0, - -0.07500000298023224 - ] - ], - [ - [ - -28.0, - -76.0, - 0.5312986969947815 - ], - [ - -28.0, - -68.0, - 0.592772364616394 - ], - [ - -28.0, - -60.0, - 0.5206702947616577 - ], - [ - -28.0, - -52.0, - 0.597906231880188 - ], - [ - -28.0, - -44.0, - -0.580917477607727 - ], - [ - -28.0, - -36.0, - -0.47692763805389404 - ], - [ - -28.0, - -28.0, - -0.5353581309318542 - ], - [ - -28.0, - -20.0, - -0.527474582195282 - ], - [ - -28.0, - -12.0, - 0.07109580188989639 - ], - [ - -28.0, - -4.0, - 0.07544480264186859 - ], - [ - -28.0, - 4.0, - 0.07549560815095901 - ], - [ - -28.0, - 12.0, - 0.06803545355796814 - ], - [ - -28.0, - 20.0, - 0.10000000149011612 - ], - [ - -28.0, - 28.0, - 0.10000000149011612 - ], - [ - -28.0, - 36.0, - 0.10000000149011612 - ], - [ - -28.0, - 44.0, - 0.10000000149011612 - ], - [ - -28.0, - 52.0, - 0.15000000596046448 - ], - [ - -28.0, - 60.0, - 0.11999999731779099 - ], - [ - -28.0, - 68.0, - -0.15000000596046448 - ], - [ - -28.0, - 76.0, - -0.10000000149011612 - ] - ], - [ - [ - -20.0, - -76.0, - 0.710183322429657 - ], - [ - -20.0, - -68.0, - 0.6831269860267639 - ], - [ - -20.0, - -60.0, - 0.7243079543113708 - ], - [ - -20.0, - -52.0, - 0.6430540084838867 - ], - [ - -20.0, - -44.0, - -0.6902334094047546 - ], - [ - -20.0, - -36.0, - -0.7011524438858032 - ], - [ - -20.0, - -28.0, - -0.6732197403907776 - ], - [ - -20.0, - -20.0, - -0.7095288038253784 - ], - [ - -20.0, - -12.0, - 0.0922103077173233 - ], - [ - -20.0, - -4.0, - 0.08669234067201614 - ], - [ - -20.0, - 4.0, - 0.08398804813623428 - ], - [ - -20.0, - 12.0, - 0.08458934724330902 - ], - [ - -20.0, - 20.0, - 0.10000000149011612 - ], - [ - -20.0, - 28.0, - 0.10000000149011612 - ], - [ - -20.0, - 36.0, - 0.10000000149011612 - ], - [ - -20.0, - 44.0, - 0.10000000149011612 - ], - [ - -20.0, - 52.0, - 0.17499999701976776 - ], - [ - -20.0, - 60.0, - 0.18000000715255737 - ], - [ - -20.0, - 68.0, - -0.20000000298023224 - ], - [ - -20.0, - 76.0, - -0.17000000178813934 - ] - ], - [ - [ - -12.0, - -76.0, - 0.8158683776855469 - ], - [ - -12.0, - -68.0, - 0.831667959690094 - ], - [ - -12.0, - -60.0, - 0.8405332565307617 - ], - [ - -12.0, - -52.0, - 0.7746779322624207 - ], - [ - -12.0, - -44.0, - -0.7673581838607788 - ], - [ - -12.0, - -36.0, - -0.8117712140083313 - ], - [ - -12.0, - -28.0, - -0.8243898749351501 - ], - [ - -12.0, - -20.0, - -0.7574763894081116 - ], - [ - -12.0, - -12.0, - 0.09750459343194962 - ], - [ - -12.0, - -4.0, - 0.10071531683206558 - ], - [ - -12.0, - 4.0, - 0.10953764617443085 - ], - [ - -12.0, - 12.0, - 0.10368829220533371 - ], - [ - -12.0, - 20.0, - 0.10000000149011612 - ], - [ - -12.0, - 28.0, - 0.10000000149011612 - ], - [ - -12.0, - 36.0, - 0.10000000149011612 - ], - [ - -12.0, - 44.0, - 0.10000000149011612 - ], - [ - -12.0, - 52.0, - 0.23999999463558197 - ], - [ - -12.0, - 60.0, - 0.3149999976158142 - ], - [ - -12.0, - 68.0, - -0.26499998569488525 - ], - [ - -12.0, - 76.0, - -0.3050000071525574 - ] - ], - [ - [ - -4.0, - -76.0, - 0.8658663630485535 - ], - [ - -4.0, - -68.0, - 0.9098702073097229 - ], - [ - -4.0, - -60.0, - 0.9520763158798218 - ], - [ - -4.0, - -52.0, - 0.9131640195846558 - ], - [ - -4.0, - -44.0, - -0.9588647484779358 - ], - [ - -4.0, - -36.0, - -0.9428508281707764 - ], - [ - -4.0, - -28.0, - -0.9339745044708252 - ], - [ - -4.0, - -20.0, - -0.8613461256027222 - ], - [ - -4.0, - -12.0, - 0.11034068465232849 - ], - [ - -4.0, - -4.0, - 0.11452268064022064 - ], - [ - -4.0, - 4.0, - 0.12168126553297043 - ], - [ - -4.0, - 12.0, - 0.11265159398317337 - ], - [ - -4.0, - 20.0, - 0.10000000149011612 - ], - [ - -4.0, - 28.0, - 0.10000000149011612 - ], - [ - -4.0, - 36.0, - 0.10000000149011612 - ], - [ - -4.0, - 44.0, - 0.10000000149011612 - ], - [ - -4.0, - 52.0, - 0.36000001430511475 - ], - [ - -4.0, - 60.0, - 0.33000001311302185 - ], - [ - -4.0, - 68.0, - -0.3799999952316284 - ], - [ - -4.0, - 76.0, - -0.33000001311302185 - ] - ], - [ - [ - 4.0, - -76.0, - 1.1029284000396729 - ], - [ - 4.0, - -68.0, - 1.0086320638656616 - ], - [ - 4.0, - -60.0, - 1.0045244693756104 - ], - [ - 4.0, - -52.0, - 1.003873348236084 - ], - [ - 4.0, - -44.0, - -1.0814002752304077 - ], - [ - 4.0, - -36.0, - -1.0783718824386597 - ], - [ - 4.0, - -28.0, - -1.0497510433197021 - ], - [ - 4.0, - -20.0, - -1.0154544115066528 - ], - [ - 4.0, - -12.0, - 0.12635071575641632 - ], - [ - 4.0, - -4.0, - 0.13445423543453217 - ], - [ - 4.0, - 4.0, - 0.1357533484697342 - ], - [ - 4.0, - 12.0, - 0.13784921169281006 - ], - [ - 4.0, - 20.0, - 0.10000000149011612 - ], - [ - 4.0, - 28.0, - 0.10000000149011612 - ], - [ - 4.0, - 36.0, - 0.10000000149011612 - ], - [ - 4.0, - 44.0, - 0.10000000149011612 - ], - [ - 4.0, - 52.0, - 0.4300000071525574 - ], - [ - 4.0, - 60.0, - 0.4000000059604645 - ], - [ - 4.0, - 68.0, - -0.44999998807907104 - ], - [ - 4.0, - 76.0, - -0.41499999165534973 - ] - ], - [ - [ - 12.0, - -76.0, - 1.2019035816192627 - ], - [ - 12.0, - -68.0, - 1.1758776903152466 - ], - [ - 12.0, - -60.0, - 1.164806842803955 - ], - [ - 12.0, - -52.0, - 1.1223701238632202 - ], - [ - 12.0, - -44.0, - -1.1548222303390503 - ], - [ - 12.0, - -36.0, - -1.1638233661651611 - ], - [ - 12.0, - -28.0, - -1.1764601469039917 - ], - [ - 12.0, - -20.0, - -1.1429928541183472 - ], - [ - 12.0, - -12.0, - 0.15083539485931396 - ], - [ - 12.0, - -4.0, - 0.1454271823167801 - ], - [ - 12.0, - 4.0, - 0.14674042165279388 - ], - [ - 12.0, - 12.0, - 0.15137779712677002 - ], - [ - 12.0, - 20.0, - 0.10000000149011612 - ], - [ - 12.0, - 28.0, - 0.10000000149011612 - ], - [ - 12.0, - 36.0, - 0.10000000149011612 - ], - [ - 12.0, - 44.0, - 0.10000000149011612 - ], - [ - 12.0, - 52.0, - 0.5450000166893005 - ], - [ - 12.0, - 60.0, - 0.5149999856948853 - ], - [ - 12.0, - 68.0, - -0.5099999904632568 - ], - [ - 12.0, - 76.0, - -0.5249999761581421 - ] - ], - [ - [ - 20.0, - -76.0, - 1.33104407787323 - ], - [ - 20.0, - -68.0, - 1.2400410175323486 - ], - [ - 20.0, - -60.0, - 1.2375192642211914 - ], - [ - 20.0, - -52.0, - 1.2919387817382812 - ], - [ - 20.0, - -44.0, - -1.268329381942749 - ], - [ - 20.0, - -36.0, - -1.3036613464355469 - ], - [ - 20.0, - -28.0, - -1.2702977657318115 - ], - [ - 20.0, - -20.0, - -1.3154014348983765 - ], - [ - 20.0, - -12.0, - 0.16192816197872162 - ], - [ - 20.0, - -4.0, - 0.15631474554538727 - ], - [ - 20.0, - 4.0, - 0.15908361971378326 - ], - [ - 20.0, - 12.0, - 0.16579194366931915 - ], - [ - 20.0, - 20.0, - 0.10000000149011612 - ], - [ - 20.0, - 28.0, - 0.10000000149011612 - ], - [ - 20.0, - 36.0, - 0.10000000149011612 - ], - [ - 20.0, - 44.0, - 0.10000000149011612 - ], - [ - 20.0, - 52.0, - 0.625 - ], - [ - 20.0, - 60.0, - 0.6349999904632568 - ], - [ - 20.0, - 68.0, - -0.5649999976158142 - ], - [ - 20.0, - 76.0, - -0.6299999952316284 - ] - ], - [ - [ - 28.0, - -76.0, - 1.3741422891616821 - ], - [ - 28.0, - -68.0, - 1.462281584739685 - ], - [ - 28.0, - -60.0, - 1.3774404525756836 - ], - [ - 28.0, - -52.0, - 1.386590600013733 - ], - [ - 28.0, - -44.0, - -1.443994402885437 - ], - [ - 28.0, - -36.0, - -1.3756144046783447 - ], - [ - 28.0, - -28.0, - -1.3618830442428589 - ], - [ - 28.0, - -20.0, - -1.428186058998108 - ], - [ - 28.0, - -12.0, - 0.17241907119750977 - ], - [ - 28.0, - -4.0, - 0.1717700958251953 - ], - [ - 28.0, - 4.0, - 0.17144586145877838 - ], - [ - 28.0, - 12.0, - 0.17648139595985413 - ], - [ - 28.0, - 20.0, - 0.10000000149011612 - ], - [ - 28.0, - 28.0, - 0.10000000149011612 - ], - [ - 28.0, - 36.0, - 0.10000000149011612 - ], - [ - 28.0, - 44.0, - 0.10000000149011612 - ], - [ - 28.0, - 52.0, - 0.6650000214576721 - ], - [ - 28.0, - 60.0, - 0.7099999785423279 - ], - [ - 28.0, - 68.0, - -0.699999988079071 - ], - [ - 28.0, - 76.0, - -0.6499999761581421 - ] - ], - [ - [ - 36.0, - -76.0, - 1.5407485961914062 - ], - [ - 36.0, - -68.0, - 1.5635896921157837 - ], - [ - 36.0, - -60.0, - 1.5700641870498657 - ], - [ - 36.0, - -52.0, - 1.5683965682983398 - ], - [ - 36.0, - -44.0, - -1.5016088485717773 - ], - [ - 36.0, - -36.0, - -1.498430848121643 - ], - [ - 36.0, - -28.0, - -1.5390263795852661 - ], - [ - 36.0, - -20.0, - -1.5827711820602417 - ], - [ - 36.0, - -12.0, - 0.19251567125320435 - ], - [ - 36.0, - -4.0, - 0.1994284689426422 - ], - [ - 36.0, - 4.0, - 0.19853903353214264 - ], - [ - 36.0, - 12.0, - 0.19440963864326477 - ], - [ - 36.0, - 20.0, - 0.10000000149011612 - ], - [ - 36.0, - 28.0, - 0.10000000149011612 - ], - [ - 36.0, - 36.0, - 0.10000000149011612 - ], - [ - 36.0, - 44.0, - 0.10000000149011612 - ], - [ - 36.0, - 52.0, - 0.7950000166893005 - ], - [ - 36.0, - 60.0, - 0.7749999761581421 - ], - [ - 36.0, - 68.0, - -0.7400000095367432 - ], - [ - 36.0, - 76.0, - -0.7450000047683716 - ] - ] - ], - "seed": 42, - "initial": { - "levels": [ - 0, - 5, - 4, - 4, - 0, - 5, - 4, - 2, - 4, - 5, - 4, - 4, - 2, - 0, - 3, - 4, - 5, - 1, - 3, - 4 - ], - "types": [ - 0, - 1, - 2, - 3, - 4, - 5, - 6, - 7, - 8, - 9, - 10, - 11, - 12, - 13, - 14, - 15, - 16, - 17, - 18, - 19 - ], - "env_origins": [ - [ - -36.0, - -76.0, - 0.4475184679031372 - ], - [ - 4.0, - -68.0, - 1.0086320638656616 - ], - [ - -4.0, - -60.0, - 0.9520763158798218 - ], - [ - -4.0, - -52.0, - 0.9131640195846558 - ], - [ - -36.0, - -44.0, - -0.4050811529159546 - ], - [ - 4.0, - -36.0, - -1.0783718824386597 - ], - [ - -4.0, - -28.0, - -0.9339745044708252 - ], - [ - -20.0, - -20.0, - -0.7095288038253784 - ], - [ - -4.0, - -12.0, - 0.11034068465232849 - ], - [ - 4.0, - -4.0, - 0.13445423543453217 - ], - [ - -4.0, - 4.0, - 0.12168126553297043 - ], - [ - -4.0, - 12.0, - 0.11265159398317337 - ], - [ - -20.0, - 20.0, - 0.10000000149011612 - ], - [ - -36.0, - 28.0, - 0.10000000149011612 - ], - [ - -12.0, - 36.0, - 0.10000000149011612 - ], - [ - -4.0, - 44.0, - 0.10000000149011612 - ], - [ - 4.0, - 52.0, - 0.4300000071525574 - ], - [ - -28.0, - 60.0, - 0.11999999731779099 - ], - [ - -12.0, - 68.0, - -0.26499998569488525 - ], - [ - -4.0, - 76.0, - -0.33000001311302185 - ] - ] - }, - "cases": [ - { - "stage": -1, - "ids": [ - 0, - 1, - 2, - 3, - 4, - 5, - 6, - 7, - 8, - 9, - 10, - 11, - 12, - 13, - 14, - 15, - 16, - 17, - 18, - 19 - ], - "positions": [ - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ] - ], - "commands": [ - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ], - [ - 0.0, - 0.0, - 0.0 - ] - ], - "levels": [ - 1, - 6, - 5, - 5, - 1, - 6, - 5, - 3, - 5, - 6, - 5, - 5, - 3, - 1, - 4, - 5, - 6, - 2, - 4, - 5 - ], - "env_origins": [ - [ - -28.0, - -76.0, - 0.5312986969947815 - ], - [ - 12.0, - -68.0, - 1.1758776903152466 - ], - [ - 4.0, - -60.0, - 1.0045244693756104 - ], - [ - 4.0, - -52.0, - 1.003873348236084 - ], - [ - -28.0, - -44.0, - -0.580917477607727 - ], - [ - 12.0, - -36.0, - -1.1638233661651611 - ], - [ - 4.0, - -28.0, - -1.0497510433197021 - ], - [ - -12.0, - -20.0, - -0.7574763894081116 - ], - [ - 4.0, - -12.0, - 0.12635071575641632 - ], - [ - 12.0, - -4.0, - 0.1454271823167801 - ], - [ - 4.0, - 4.0, - 0.1357533484697342 - ], - [ - 4.0, - 12.0, - 0.13784921169281006 - ], - [ - -12.0, - 20.0, - 0.10000000149011612 - ], - [ - -28.0, - 28.0, - 0.10000000149011612 - ], - [ - -4.0, - 36.0, - 0.10000000149011612 - ], - [ - 4.0, - 44.0, - 0.10000000149011612 - ], - [ - 12.0, - 52.0, - 0.5450000166893005 - ], - [ - -20.0, - 60.0, - 0.18000000715255737 - ], - [ - -4.0, - 68.0, - -0.3799999952316284 - ], - [ - 4.0, - 76.0, - -0.41499999165534973 - ] - ] - }, - { - "stage": 0, - "ids": [ - 0, - 3, - 6, - 9, - 12, - 15, - 18 - ], - "positions": [ - [ - -28.0, - -76.0, - 0.5312986969947815 - ], - [ - 15.999899864196777, - -68.0, - 1.1758776903152466 - ], - [ - 8.0, - -60.0, - 1.0045244693756104 - ], - [ - 8.000100135803223, - -52.0, - 1.003873348236084 - ], - [ - -18.0, - -44.0, - -0.580917477607727 - ], - [ - 12.0, - -36.0, - -1.1638233661651611 - ], - [ - 7.999899864196777, - -28.0, - -1.0497510433197021 - ], - [ - -8.0, - -20.0, - -0.7574763894081116 - ], - [ - 8.000100135803223, - -12.0, - 0.12635071575641632 - ], - [ - 22.0, - -4.0, - 0.1454271823167801 - ], - [ - 4.0, - 4.0, - 0.1357533484697342 - ], - [ - 7.999899864196777, - 12.0, - 0.13784921169281006 - ], - [ - -8.0, - 20.0, - 0.10000000149011612 - ], - [ - -23.999900817871094, - 28.0, - 0.10000000149011612 - ], - [ - 6.0, - 36.0, - 0.10000000149011612 - ], - [ - 4.0, - 44.0, - 0.10000000149011612 - ], - [ - 15.999899864196777, - 52.0, - 0.5450000166893005 - ], - [ - -16.0, - 60.0, - 0.18000000715255737 - ], - [ - 0.00010013580322265625, - 68.0, - -0.3799999952316284 - ], - [ - 14.0, - 76.0, - -0.41499999165534973 - ] - ], - "commands": [ - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ] - ], - "levels": [ - 0, - 6, - 5, - 6, - 1, - 6, - 4, - 3, - 5, - 7, - 5, - 5, - 3, - 1, - 4, - 4, - 6, - 2, - 5, - 5 - ], - "env_origins": [ - [ - -36.0, - -76.0, - 0.4475184679031372 - ], - [ - 12.0, - -68.0, - 1.1758776903152466 - ], - [ - 4.0, - -60.0, - 1.0045244693756104 - ], - [ - 12.0, - -52.0, - 1.1223701238632202 - ], - [ - -28.0, - -44.0, - -0.580917477607727 - ], - [ - 12.0, - -36.0, - -1.1638233661651611 - ], - [ - -4.0, - -28.0, - -0.9339745044708252 - ], - [ - -12.0, - -20.0, - -0.7574763894081116 - ], - [ - 4.0, - -12.0, - 0.12635071575641632 - ], - [ - 20.0, - -4.0, - 0.15631474554538727 - ], - [ - 4.0, - 4.0, - 0.1357533484697342 - ], - [ - 4.0, - 12.0, - 0.13784921169281006 - ], - [ - -12.0, - 20.0, - 0.10000000149011612 - ], - [ - -28.0, - 28.0, - 0.10000000149011612 - ], - [ - -4.0, - 36.0, - 0.10000000149011612 - ], - [ - -4.0, - 44.0, - 0.10000000149011612 - ], - [ - 12.0, - 52.0, - 0.5450000166893005 - ], - [ - -20.0, - 60.0, - 0.18000000715255737 - ], - [ - 4.0, - 68.0, - -0.44999998807907104 - ], - [ - 4.0, - 76.0, - -0.41499999165534973 - ] - ] - }, - { - "stage": 1, - "ids": [ - 1, - 4, - 7, - 10, - 13, - 16, - 19 - ], - "positions": [ - [ - -36.0, - -76.0, - 0.4475184679031372 - ], - [ - 15.999899864196777, - -68.0, - 1.1758776903152466 - ], - [ - 8.0, - -60.0, - 1.0045244693756104 - ], - [ - 16.000099182128906, - -52.0, - 1.1223701238632202 - ], - [ - -18.0, - -44.0, - -0.580917477607727 - ], - [ - 12.0, - -36.0, - -1.1638233661651611 - ], - [ - -9.989738464355469e-05, - -28.0, - -0.9339745044708252 - ], - [ - -8.0, - -20.0, - -0.7574763894081116 - ], - [ - 8.000100135803223, - -12.0, - 0.12635071575641632 - ], - [ - 30.0, - -4.0, - 0.15631474554538727 - ], - [ - 4.0, - 4.0, - 0.1357533484697342 - ], - [ - 7.999899864196777, - 12.0, - 0.13784921169281006 - ], - [ - -8.0, - 20.0, - 0.10000000149011612 - ], - [ - -23.999900817871094, - 28.0, - 0.10000000149011612 - ], - [ - 6.0, - 36.0, - 0.10000000149011612 - ], - [ - -4.0, - 44.0, - 0.10000000149011612 - ], - [ - 15.999899864196777, - 52.0, - 0.5450000166893005 - ], - [ - -16.0, - 60.0, - 0.18000000715255737 - ], - [ - 8.000100135803223, - 68.0, - -0.44999998807907104 - ], - [ - 14.0, - 76.0, - -0.41499999165534973 - ] - ], - "commands": [ - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ] - ], - "levels": [ - 0, - 5, - 5, - 6, - 2, - 6, - 4, - 3, - 5, - 7, - 4, - 5, - 3, - 2, - 4, - 4, - 5, - 2, - 5, - 6 - ], - "env_origins": [ - [ - -36.0, - -76.0, - 0.4475184679031372 - ], - [ - 4.0, - -68.0, - 1.0086320638656616 - ], - [ - 4.0, - -60.0, - 1.0045244693756104 - ], - [ - 12.0, - -52.0, - 1.1223701238632202 - ], - [ - -20.0, - -44.0, - -0.6902334094047546 - ], - [ - 12.0, - -36.0, - -1.1638233661651611 - ], - [ - -4.0, - -28.0, - -0.9339745044708252 - ], - [ - -12.0, - -20.0, - -0.7574763894081116 - ], - [ - 4.0, - -12.0, - 0.12635071575641632 - ], - [ - 20.0, - -4.0, - 0.15631474554538727 - ], - [ - -4.0, - 4.0, - 0.12168126553297043 - ], - [ - 4.0, - 12.0, - 0.13784921169281006 - ], - [ - -12.0, - 20.0, - 0.10000000149011612 - ], - [ - -20.0, - 28.0, - 0.10000000149011612 - ], - [ - -4.0, - 36.0, - 0.10000000149011612 - ], - [ - -4.0, - 44.0, - 0.10000000149011612 - ], - [ - 4.0, - 52.0, - 0.4300000071525574 - ], - [ - -20.0, - 60.0, - 0.18000000715255737 - ], - [ - 4.0, - 68.0, - -0.44999998807907104 - ], - [ - 12.0, - 76.0, - -0.5249999761581421 - ] - ] - }, - { - "stage": 2, - "ids": [ - 2, - 5, - 8, - 11, - 14, - 17 - ], - "positions": [ - [ - -36.0, - -76.0, - 0.4475184679031372 - ], - [ - 7.999899864196777, - -68.0, - 1.0086320638656616 - ], - [ - 8.0, - -60.0, - 1.0045244693756104 - ], - [ - 16.000099182128906, - -52.0, - 1.1223701238632202 - ], - [ - -10.0, - -44.0, - -0.6902334094047546 - ], - [ - 12.0, - -36.0, - -1.1638233661651611 - ], - [ - -9.989738464355469e-05, - -28.0, - -0.9339745044708252 - ], - [ - -8.0, - -20.0, - -0.7574763894081116 - ], - [ - 8.000100135803223, - -12.0, - 0.12635071575641632 - ], - [ - 30.0, - -4.0, - 0.15631474554538727 - ], - [ - -4.0, - 4.0, - 0.12168126553297043 - ], - [ - 7.999899864196777, - 12.0, - 0.13784921169281006 - ], - [ - -8.0, - 20.0, - 0.10000000149011612 - ], - [ - -15.999899864196777, - 28.0, - 0.10000000149011612 - ], - [ - 6.0, - 36.0, - 0.10000000149011612 - ], - [ - -4.0, - 44.0, - 0.10000000149011612 - ], - [ - 7.999899864196777, - 52.0, - 0.4300000071525574 - ], - [ - -16.0, - 60.0, - 0.18000000715255737 - ], - [ - 8.000100135803223, - 68.0, - -0.44999998807907104 - ], - [ - 22.0, - 76.0, - -0.5249999761581421 - ] - ], - "commands": [ - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ] - ], - "levels": [ - 0, - 5, - 5, - 6, - 2, - 5, - 4, - 3, - 6, - 7, - 4, - 4, - 3, - 2, - 5, - 4, - 5, - 2, - 5, - 6 - ], - "env_origins": [ - [ - -36.0, - -76.0, - 0.4475184679031372 - ], - [ - 4.0, - -68.0, - 1.0086320638656616 - ], - [ - 4.0, - -60.0, - 1.0045244693756104 - ], - [ - 12.0, - -52.0, - 1.1223701238632202 - ], - [ - -20.0, - -44.0, - -0.6902334094047546 - ], - [ - 4.0, - -36.0, - -1.0783718824386597 - ], - [ - -4.0, - -28.0, - -0.9339745044708252 - ], - [ - -12.0, - -20.0, - -0.7574763894081116 - ], - [ - 12.0, - -12.0, - 0.15083539485931396 - ], - [ - 20.0, - -4.0, - 0.15631474554538727 - ], - [ - -4.0, - 4.0, - 0.12168126553297043 - ], - [ - -4.0, - 12.0, - 0.11265159398317337 - ], - [ - -12.0, - 20.0, - 0.10000000149011612 - ], - [ - -20.0, - 28.0, - 0.10000000149011612 - ], - [ - 4.0, - 36.0, - 0.10000000149011612 - ], - [ - -4.0, - 44.0, - 0.10000000149011612 - ], - [ - 4.0, - 52.0, - 0.4300000071525574 - ], - [ - -20.0, - 60.0, - 0.18000000715255737 - ], - [ - 4.0, - 68.0, - -0.44999998807907104 - ], - [ - 12.0, - 76.0, - -0.5249999761581421 - ] - ] - }, - { - "stage": 3, - "ids": [ - 0, - 3, - 6, - 9, - 12, - 15, - 18 - ], - "positions": [ - [ - -36.0, - -76.0, - 0.4475184679031372 - ], - [ - 7.999899864196777, - -68.0, - 1.0086320638656616 - ], - [ - 8.0, - -60.0, - 1.0045244693756104 - ], - [ - 16.000099182128906, - -52.0, - 1.1223701238632202 - ], - [ - -10.0, - -44.0, - -0.6902334094047546 - ], - [ - 4.0, - -36.0, - -1.0783718824386597 - ], - [ - -9.989738464355469e-05, - -28.0, - -0.9339745044708252 - ], - [ - -8.0, - -20.0, - -0.7574763894081116 - ], - [ - 16.000099182128906, - -12.0, - 0.15083539485931396 - ], - [ - 30.0, - -4.0, - 0.15631474554538727 - ], - [ - -4.0, - 4.0, - 0.12168126553297043 - ], - [ - -9.989738464355469e-05, - 12.0, - 0.11265159398317337 - ], - [ - -8.0, - 20.0, - 0.10000000149011612 - ], - [ - -15.999899864196777, - 28.0, - 0.10000000149011612 - ], - [ - 14.0, - 36.0, - 0.10000000149011612 - ], - [ - -4.0, - 44.0, - 0.10000000149011612 - ], - [ - 7.999899864196777, - 52.0, - 0.4300000071525574 - ], - [ - -16.0, - 60.0, - 0.18000000715255737 - ], - [ - 8.000100135803223, - 68.0, - -0.44999998807907104 - ], - [ - 22.0, - 76.0, - -0.5249999761581421 - ] - ], - "commands": [ - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ] - ], - "levels": [ - 0, - 5, - 5, - 7, - 2, - 5, - 3, - 3, - 6, - 8, - 4, - 4, - 3, - 2, - 5, - 3, - 5, - 2, - 6, - 6 - ], - "env_origins": [ - [ - -36.0, - -76.0, - 0.4475184679031372 - ], - [ - 4.0, - -68.0, - 1.0086320638656616 - ], - [ - 4.0, - -60.0, - 1.0045244693756104 - ], - [ - 20.0, - -52.0, - 1.2919387817382812 - ], - [ - -20.0, - -44.0, - -0.6902334094047546 - ], - [ - 4.0, - -36.0, - -1.0783718824386597 - ], - [ - -12.0, - -28.0, - -0.8243898749351501 - ], - [ - -12.0, - -20.0, - -0.7574763894081116 - ], - [ - 12.0, - -12.0, - 0.15083539485931396 - ], - [ - 28.0, - -4.0, - 0.1717700958251953 - ], - [ - -4.0, - 4.0, - 0.12168126553297043 - ], - [ - -4.0, - 12.0, - 0.11265159398317337 - ], - [ - -12.0, - 20.0, - 0.10000000149011612 - ], - [ - -20.0, - 28.0, - 0.10000000149011612 - ], - [ - 4.0, - 36.0, - 0.10000000149011612 - ], - [ - -12.0, - 44.0, - 0.10000000149011612 - ], - [ - 4.0, - 52.0, - 0.4300000071525574 - ], - [ - -20.0, - 60.0, - 0.18000000715255737 - ], - [ - 12.0, - 68.0, - -0.5099999904632568 - ], - [ - 12.0, - 76.0, - -0.5249999761581421 - ] - ] - }, - { - "stage": 4, - "ids": [ - 1, - 4, - 7, - 10, - 13, - 16, - 19 - ], - "positions": [ - [ - -26.0, - -76.0, - 0.4475184679031372 - ], - [ - 46.0, - -68.0, - 1.5635896921157837 - ], - [ - 14.0, - -60.0, - 1.0045244693756104 - ], - [ - 30.0, - -52.0, - 1.2919387817382812 - ], - [ - 46.0, - -44.0, - -1.5016088485717773 - ], - [ - 14.0, - -36.0, - -1.0783718824386597 - ], - [ - -2.0, - -28.0, - -0.8243898749351501 - ], - [ - 46.0, - -20.0, - -1.5827711820602417 - ], - [ - 22.0, - -12.0, - 0.15083539485931396 - ], - [ - 38.0, - -4.0, - 0.1717700958251953 - ], - [ - 46.0, - 4.0, - 0.19853903353214264 - ], - [ - 6.0, - 12.0, - 0.11265159398317337 - ], - [ - -2.0, - 20.0, - 0.10000000149011612 - ], - [ - 46.0, - 28.0, - 0.10000000149011612 - ], - [ - 14.0, - 36.0, - 0.10000000149011612 - ], - [ - -2.0, - 44.0, - 0.10000000149011612 - ], - [ - 46.0, - 52.0, - 0.7950000166893005 - ], - [ - -10.0, - 60.0, - 0.18000000715255737 - ], - [ - 22.0, - 68.0, - -0.5099999904632568 - ], - [ - 46.0, - 76.0, - -0.7450000047683716 - ] - ], - "commands": [ - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ] - ], - "levels": [ - 0, - 0, - 5, - 7, - 1, - 5, - 3, - 1, - 6, - 8, - 7, - 4, - 3, - 9, - 5, - 3, - 4, - 2, - 6, - 3 - ], - "env_origins": [ - [ - -36.0, - -76.0, - 0.4475184679031372 - ], - [ - -36.0, - -68.0, - 0.3967205584049225 - ], - [ - 4.0, - -60.0, - 1.0045244693756104 - ], - [ - 20.0, - -52.0, - 1.2919387817382812 - ], - [ - -28.0, - -44.0, - -0.580917477607727 - ], - [ - 4.0, - -36.0, - -1.0783718824386597 - ], - [ - -12.0, - -28.0, - -0.8243898749351501 - ], - [ - -28.0, - -20.0, - -0.527474582195282 - ], - [ - 12.0, - -12.0, - 0.15083539485931396 - ], - [ - 28.0, - -4.0, - 0.1717700958251953 - ], - [ - 20.0, - 4.0, - 0.15908361971378326 - ], - [ - -4.0, - 12.0, - 0.11265159398317337 - ], - [ - -12.0, - 20.0, - 0.10000000149011612 - ], - [ - 36.0, - 28.0, - 0.10000000149011612 - ], - [ - 4.0, - 36.0, - 0.10000000149011612 - ], - [ - -12.0, - 44.0, - 0.10000000149011612 - ], - [ - -4.0, - 52.0, - 0.36000001430511475 - ], - [ - -20.0, - 60.0, - 0.18000000715255737 - ], - [ - 12.0, - 68.0, - -0.5099999904632568 - ], - [ - -12.0, - 76.0, - -0.3050000071525574 - ] - ] - }, - { - "stage": 5, - "ids": [ - 2, - 5, - 8, - 11, - 14, - 17 - ], - "positions": [ - [ - -36.0, - -76.0, - 0.4475184679031372 - ], - [ - -36.0, - -68.0, - 0.3967205584049225 - ], - [ - -36.0, - -60.0, - 0.4455190598964691 - ], - [ - 20.0, - -52.0, - 1.2919387817382812 - ], - [ - -28.0, - -44.0, - -0.580917477607727 - ], - [ - -36.0, - -36.0, - -0.3751884400844574 - ], - [ - -12.0, - -28.0, - -0.8243898749351501 - ], - [ - -28.0, - -20.0, - -0.527474582195282 - ], - [ - -36.0, - -12.0, - 0.059964701533317566 - ], - [ - 28.0, - -4.0, - 0.1717700958251953 - ], - [ - 20.0, - 4.0, - 0.15908361971378326 - ], - [ - -36.0, - 12.0, - 0.05683664605021477 - ], - [ - -12.0, - 20.0, - 0.10000000149011612 - ], - [ - 36.0, - 28.0, - 0.10000000149011612 - ], - [ - -36.0, - 36.0, - 0.10000000149011612 - ], - [ - -12.0, - 44.0, - 0.10000000149011612 - ], - [ - -4.0, - 52.0, - 0.36000001430511475 - ], - [ - -36.0, - 60.0, - 0.019999999552965164 - ], - [ - 12.0, - 68.0, - -0.5099999904632568 - ], - [ - -12.0, - 76.0, - -0.3050000071525574 - ] - ], - "commands": [ - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ], - [ - 0.4000000059604645, - 0.0, - 0.0 - ] - ], - "levels": [ - 0, - 0, - 0, - 7, - 1, - 0, - 3, - 1, - 0, - 8, - 7, - 0, - 3, - 9, - 0, - 3, - 4, - 0, - 6, - 3 - ], - "env_origins": [ - [ - -36.0, - -76.0, - 0.4475184679031372 - ], - [ - -36.0, - -68.0, - 0.3967205584049225 - ], - [ - -36.0, - -60.0, - 0.4455190598964691 - ], - [ - 20.0, - -52.0, - 1.2919387817382812 - ], - [ - -28.0, - -44.0, - -0.580917477607727 - ], - [ - -36.0, - -36.0, - -0.3751884400844574 - ], - [ - -12.0, - -28.0, - -0.8243898749351501 - ], - [ - -28.0, - -20.0, - -0.527474582195282 - ], - [ - -36.0, - -12.0, - 0.059964701533317566 - ], - [ - 28.0, - -4.0, - 0.1717700958251953 - ], - [ - 20.0, - 4.0, - 0.15908361971378326 - ], - [ - -36.0, - 12.0, - 0.05683664605021477 - ], - [ - -12.0, - 20.0, - 0.10000000149011612 - ], - [ - 36.0, - 28.0, - 0.10000000149011612 - ], - [ - -36.0, - 36.0, - 0.10000000149011612 - ], - [ - -12.0, - 44.0, - 0.10000000149011612 - ], - [ - -4.0, - 52.0, - 0.36000001430511475 - ], - [ - -36.0, - 60.0, - 0.019999999552965164 - ], - [ - 12.0, - 68.0, - -0.5099999904632568 - ], - [ - -12.0, - 76.0, - -0.3050000071525574 - ] - ] - } - ] -} diff --git a/tests/gym/envs/tasks/test_dexterous_usd.py b/tests/gym/envs/tasks/test_dexterous_usd.py index 1e75cd4834..329cabd0cc 100644 --- a/tests/gym/envs/tasks/test_dexterous_usd.py +++ b/tests/gym/envs/tasks/test_dexterous_usd.py @@ -16,122 +16,23 @@ from __future__ import annotations -import copy +import argparse +import inspect +from pathlib import Path from types import SimpleNamespace -import mujoco -import numpy as np import pytest -from embodichain_tasks.manipulation.dexterous._audit_model import ( - audit_model, - snapshot_execution_model, -) +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_static_body_merge_preserves_world_geometry() -> None: - source = mujoco.MjModel.from_xml_string( - '' - ) - target = mujoco.MjModel.from_xml_string( - '' - ) - arguments = ({0: 0}, [], {"ground": 0, "ball": 1}) - assert audit_model(source, target, *arguments)["status"] == "passed" - target.geom_pos[0, 0] += 0.01 - result = audit_model(source, target, *arguments) - assert any(item["field"] == "geom[floor].geom_pos" for item in result["mismatches"]) - - -@pytest.mark.parametrize( - "force_range,passed", [((-np.inf, np.inf), True), ((-1e6, 1e6), False)] -) -def test_unlimited_joint_rejects_any_finite_added_limit(force_range, passed) -> None: - source = mujoco.MjModel.from_xml_string( - '' - ) - target = copy.copy(source) - target.jnt_actfrclimited[0] = True - target.jnt_actfrcrange[0] = force_range - result = audit_model(source, target, {0: 0}, [], {"arm": 1}) - assert (result["status"] == "passed") == passed - - -@pytest.mark.parametrize("exclude,passed", [(True, True), (False, False)]) -def test_parent_filter_normalization_still_checks_every_collision( - exclude, passed +def test_dexuni_scene_joint_ownership_assigns_all_joints( + monkeypatch: pytest.MonkeyPatch, ) -> None: - xml = '{}' - source = mujoco.MjModel.from_xml_string(xml.format("")) - target = mujoco.MjModel.from_xml_string( - xml.format( - '' if exclude else "" - ) - ) - target.opt.disableflags |= int(mujoco.mjtDisableBit.mjDSBL_FILTERPARENT) - result = audit_model( - source, - target, - {0: 0, 1: 1}, - [], - {"a": 1, "b": 2}, - explicit_collision_filtering=True, - ) - assert (result["status"] == "passed") == passed - - -def test_execution_snapshot_reads_device_parameters_without_mutation() -> None: - model = mujoco.MjModel.from_xml_string( - '' - ) - values = np.array([[0.25]], dtype=np.float32) - remote = SimpleNamespace( - dof_damping=SimpleNamespace(numpy=lambda: values), opt=SimpleNamespace() - ) - snapshot = snapshot_execution_model( - SimpleNamespace(mj_model=model, mjw_model=remote) - ) - assert snapshot.dof_damping[0] == 0.25 - assert model.dof_damping[0] == 0.0 - values[0, 0] = 0.5 - assert snapshot.dof_damping[0] == 0.25 - - -def test_snapshot_checks_per_world_inputs_without_auditing_derived_acceleration() -> ( - None -): - model = mujoco.MjModel.from_xml_string( - '' - ) - remote = SimpleNamespace( - actuator_acc0=SimpleNamespace(numpy=lambda: np.array([[1.0], [1.0000001]])), - dof_damping=SimpleNamespace(numpy=lambda: np.array([[0.25], [0.25]])), - opt=SimpleNamespace(), - ) - solver = SimpleNamespace(mj_model=model, mjw_model=remote) - report = {} - assert ( - snapshot_execution_model(solver, derived_cache_report=report).dof_damping[0] - == 0.25 - ) - assert report["actuator_acc0"]["max_world_spread"] > 0 - remote.dof_damping = SimpleNamespace(numpy=lambda: np.array([[0.25], [0.5]])) - with pytest.raises(ValueError, match="dof_damping"): - snapshot_execution_model(solver) - - -def test_snapshot_rejects_per_world_timestep_changes() -> None: - model = mujoco.MjModel.from_xml_string("") - remote = SimpleNamespace( - opt=SimpleNamespace( - timestep=SimpleNamespace(numpy=lambda: np.array([0.002, 0.003])) - ) - ) - with pytest.raises(ValueError, match="Nonuniform execution option: timestep"): - snapshot_execution_model(SimpleNamespace(mj_model=model, mjw_model=remote)) - - -def test_dexuni_scene_joint_ownership_assigns_all_joints(monkeypatch) -> None: from dexsim.engine.newton_physics import backend_registry from embodichain_tasks.manipulation.dexterous._runtime import ( _DexUniSceneJointOwnership, @@ -153,7 +54,6 @@ def test_dexuni_scene_joint_ownership_assigns_all_joints(monkeypatch) -> None: ) assert solver_cfg.mujoco_joints == tuple(range(12)) - # Explicit caller overrides remain untouched. explicit_cfg = SimpleNamespace( mujoco_articulations=None, mujoco_articulation_paths=("/World/hand",), @@ -166,13 +66,9 @@ def test_dexuni_scene_joint_ownership_assigns_all_joints(monkeypatch) -> None: assert explicit_cfg.mujoco_joints is None -def test_dexterous_runtime_and_catalog_configs_are_usd_native() -> None: - import argparse - import inspect - from pathlib import Path - from embodichain.cli._task_catalog import _catalog_from_args - from embodichain_tasks.manipulation.dexterous import _runtime, _shadow_task - +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 @@ -186,23 +82,58 @@ def test_dexterous_runtime_and_catalog_configs_are_usd_native() -> None: 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", - "embodichain_tasks:shadow_pen_continuous", - "embodichain_tasks:leap_cube_face_turn", - "embodichain_tasks:leap_cube_reorient", - "embodichain_tasks:leap_cube_rotate_z", - "embodichain_tasks:g1_rough", - ) - for selector in expected: + 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 == "newton" + assert entry.default_deployment == "default" deps = {d.name: d for d in entry.deployments} - assert set(deps) == {"newton", "mjwarp"} - assert len(deps["newton"].agent_refs) >= 1 - assert len(deps["mjwarp"].agent_refs) >= 1 + 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 index afda6f2aab..0c5586518b 100644 --- a/tests/gym/envs/tasks/test_rough_velocity.py +++ b/tests/gym/envs/tasks/test_rough_velocity.py @@ -236,45 +236,101 @@ def fetch_state(): @pytest.mark.no_sim @pytest.mark.parametrize("task, dimension", [("g1_rough", 310)]) -@pytest.mark.parametrize("cached", [False, True]) -def test_rough_velocity_mdp_golden_values( - task: str, dimension: int, cached: bool +def test_rough_velocity_mdp_contracts_and_cached_constants( + task: str, dimension: int ) -> None: - """Verify observation, reward and termination values against golden fixtures.""" - fixture = json.loads( - (Path(__file__).parent / "fixtures" / f"{task}.json").read_text() - ) - state = RoughVelocityState( - **{ - name: torch.tensor(value, dtype=torch.float32) - for name, value in fixture["state"].items() - } - ) + """Verify observation clipping/scaling, termination, and cached reward parity.""" config = load_config(task) - constants = ( - RoughVelocityConstants.from_config(config, state.joint_pos) if cached else None - ) - noise = {name: torch.tensor(value) for name, value in fixture["noise"].items()} - observed = build_observation(config, state, noise=noise, constants=constants) - assert observed.shape == (4, dimension) - torch.testing.assert_close( - observed, torch.tensor(fixture["observation"]), rtol=1e-6, atol=1e-6 - ) + 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 torch.equal(terminated, torch.tensor(fixture["terminated"])) - assert not truncated.any() - reward = compute_rewards(config, state, terminated, constants=constants) - for name, expected in fixture["rewards"].items(): + 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( - reward.raw_terms[name], torch.tensor(expected), rtol=1e-6, atol=1e-6 + rew_cached.raw_terms[name], rew_uncached.raw_terms[name], rtol=0, atol=0 ) expected_total = sum( - torch.tensor(value) * config.data["rewards"][name]["weight"] * 0.02 - for name, value in fixture["rewards"].items() + rew_uncached.raw_terms[name] + * config.data["rewards"][name]["weight"] + * config.control_dt + for name in config.data["rewards"] ) - torch.testing.assert_close(reward.total, expected_total, rtol=1e-6, atol=1e-6) - state.episode_step[0] = 1000 - assert compute_termination(config, state)[1].tolist() == [True, False, False, False] + 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 @@ -523,48 +579,59 @@ def test_contact_impulse_uses_internal_solver_interval() -> None: @pytest.mark.no_sim -def test_rough_curriculum_matches_golden_resets() -> None: - """Match source assignments, threshold cases, selected resets and overflow.""" - fixture = json.loads( - (Path(__file__).parent / "fixtures/terrain_curriculum.json").read_text() - ) - origins = torch.tensor(fixture["origins"], dtype=torch.float32) +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, 20, 5, 8.0, 20.0, torch.Generator().manual_seed(fixture["seed"]) - ) - torch.testing.assert_close( - curriculum.levels, torch.tensor(fixture["initial"]["levels"]), rtol=0, atol=0 - ) + 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.types, torch.tensor(fixture["initial"]["types"]), rtol=0, atol=0 + curriculum.env_origins[selected], + origins[curriculum.levels[selected], curriculum.types[selected]], ) - torch.testing.assert_close( - curriculum.env_origins, - torch.tensor(fixture["initial"]["env_origins"]), - rtol=0, - atol=0, - ) - for case in fixture["cases"]: - ids = torch.tensor(case["ids"]) - if case["stage"] in (4, 5): - level = 9 if case["stage"] == 4 else 0 - curriculum.levels[ids] = level - curriculum.env_origins[ids] = origins[level, curriculum.types[ids]] - untouched = torch.ones(20, dtype=torch.bool) - untouched[ids] = False - before_levels = curriculum.levels.clone() - before_origins = curriculum.env_origins.clone() - curriculum.update( - ids, torch.tensor(case["positions"]), torch.tensor(case["commands"]) - ) - torch.testing.assert_close( - curriculum.levels, torch.tensor(case["levels"]), rtol=0, atol=0 - ) - torch.testing.assert_close( - curriculum.env_origins, torch.tensor(case["env_origins"]), rtol=0, atol=0 - ) - assert torch.equal(curriculum.levels[untouched], before_levels[untouched]) - assert torch.equal(curriculum.env_origins[untouched], before_origins[untouched]) @pytest.mark.no_sim From a59f090df3bde4d437cc93f13b914d1b38abf4d2 Mon Sep 17 00:00:00 2001 From: acrlw <13927622+acrlw@users.noreply.github.com> Date: Sat, 14 Nov 2026 19:45:29 +0800 Subject: [PATCH 22/24] fix(tasks): correct rough and dexterous RL state handling --- .../locomotion/velocity/_embodichain.py | 1 + .../locomotion/velocity/rough/runtime.py | 40 +++++--- .../manipulation/dexterous/_runtime.py | 43 ++++++++- .../gym/envs/tasks/test_dexterous_metrics.py | 91 +++++++++++++++++-- tests/gym/envs/tasks/test_rough_velocity.py | 83 ++++++++++++++++- 5 files changed, 233 insertions(+), 25 deletions(-) diff --git a/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/_embodichain.py b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/_embodichain.py index 0536d735da..dcd24dee7f 100644 --- a/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/_embodichain.py +++ b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/_embodichain.py @@ -314,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 diff --git a/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/runtime.py b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/runtime.py index 8fe96603de..7d46a48041 100644 --- a/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/runtime.py +++ b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/runtime.py @@ -37,7 +37,6 @@ RoughVelocityConstants, build_observation, observation_terms, - action_target, compute_rewards, compute_termination, dc_motor_effort, @@ -255,14 +254,8 @@ def _randomize_startup(self) -> None: self.robot.set_com_pose(pose, link_names=[name]) def _step_action(self, action: torch.Tensor) -> torch.Tensor: - """Write unbounded reference targets through the Spawn batch state API.""" - self._position_target.copy_( - action_target( - self.velocity_task_config, - self.locomotion_action, - constants=self._reference_constants, - ) - ) + """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 @@ -272,9 +265,25 @@ def _step_action(self, action: torch.Tensor) -> torch.Tensor: 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 + ] + 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, render_final_step=False) + 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): @@ -284,7 +293,12 @@ def _advance_physics(self) -> None: ): # 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, render_final_step=False) + 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] @@ -307,7 +321,9 @@ def _advance_physics(self) -> None: else: effort = effort.clamp(-self._effort_limit, self._effort_limit) self._applied_effort.copy_(effort) - self.sim.update(self.physics_dt, 1, render_final_step=False) + 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: diff --git a/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_runtime.py b/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_runtime.py index aafb52e68d..ea6ca0e6b7 100644 --- a/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_runtime.py +++ b/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_runtime.py @@ -375,6 +375,9 @@ def _init_sim_state(self, **kwargs: Any) -> None: 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") @@ -532,6 +535,12 @@ def _read_physical_state(self) -> Any: 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 ) @@ -725,6 +734,20 @@ def _step_action(self, action: EnvAction) -> EnvAction: 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 + ] + 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): @@ -733,20 +756,31 @@ def _advance_physics(self) -> None: self._cube_art.set_qf( face_force[:, None], joint_ids=[self._face_joint_id] ) - self.sim.update(self.physics_dt, 1, render_final_step=False) + 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, render_final_step=False) + 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, render_final_step=False) + self.sim.update( + self.physics_dt, 1, after_substep=after_substep, render_final_step=False + ) return - super()._advance_physics() def _update_sim_state(self, **kwargs: Any) -> None: super()._update_sim_state(**kwargs) @@ -835,4 +869,5 @@ def _get_termination_flags( 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/tests/gym/envs/tasks/test_dexterous_metrics.py b/tests/gym/envs/tasks/test_dexterous_metrics.py index 419b3806db..0a59be624f 100644 --- a/tests/gym/envs/tasks/test_dexterous_metrics.py +++ b/tests/gym/envs/tasks/test_dexterous_metrics.py @@ -20,7 +20,7 @@ import math from types import SimpleNamespace from typing import Any, Callable -from unittest.mock import Mock +from unittest.mock import Mock, call import gymnasium as gym import numpy as np @@ -298,14 +298,18 @@ def test_continuous_rotation_has_metrics_without_invented_binary_success( @pytest.mark.no_sim -@pytest.mark.parametrize("task_id", ["M1", "M2"]) -def test_failure_at_time_limit_does_not_bootstrap( - env_factory: Callable[[str], DexterousManipulationEnv], task_id: str +@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: - """True failure on the final step sets terminated=True and truncated=False.""" + """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), @@ -313,9 +317,20 @@ def test_failure_at_time_limit_does_not_bootstrap( 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() == [True, False] - assert truncated.tolist() == [False, True] + 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), @@ -330,4 +345,64 @@ def test_failure_at_time_limit_does_not_bootstrap( batch_size=[2, 2], ) _, returns = compute_gae(rollout, gamma=0.9, gae_lambda=0.95) - torch.testing.assert_close(returns[:, 0], torch.tensor([1.0, 10.0])) + 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) + passive = Mock(requires_substep_update=False) + env.sensors = {"contacts": sensor, "camera": passive} + + 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() + + +@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_rough_velocity.py b/tests/gym/envs/tasks/test_rough_velocity.py index 0c5586518b..85c28f2a93 100644 --- a/tests/gym/envs/tasks/test_rough_velocity.py +++ b/tests/gym/envs/tasks/test_rough_velocity.py @@ -23,7 +23,7 @@ from copy import copy from pathlib import Path from types import SimpleNamespace -from unittest.mock import Mock +from unittest.mock import Mock, call import numpy as np import pytest @@ -37,6 +37,8 @@ ) 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, @@ -61,6 +63,56 @@ __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.""" @@ -142,6 +194,8 @@ 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"] @@ -154,6 +208,8 @@ def update_control_state(self, **kwargs): body_data=SimpleNamespace(fetch_state=fetch_state), set_qf=lambda *a, **kw: None ) env.sim = SimpleNamespace(update=advance) + sensor = Mock(requires_substep_update=True) + env.sensors = {"contacts": sensor} env._position_target = torch.zeros(1, 2) env._kp = torch.ones(2) env._kd = torch.ones(2) @@ -168,6 +224,8 @@ def update_control_state(self, **kwargs): 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() @@ -175,6 +233,29 @@ def update_control_state(self, **kwargs): 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) + env.sensors = {"contacts": sensor} + 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 + 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.""" From f08d8dc02ef7a01956f84c2dff58791777e6da00 Mon Sep 17 00:00:00 2001 From: acrlw <13927622+acrlw@users.noreply.github.com> Date: Sat, 14 Nov 2026 22:01:53 +0800 Subject: [PATCH 23/24] test: isolate asset materialization path override --- tests/gym/envs/task_program/test_asset_materialization.py | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) 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()) From 720a5a0e097f742bac497c251ea68afcaa057c64 Mon Sep 17 00:00:00 2001 From: acrlw <13927622+acrlw@users.noreply.github.com> Date: Thu, 19 Nov 2026 14:07:29 +0800 Subject: [PATCH 24/24] fix(tasks): align sensor sampling and rollout tests with main --- .../locomotion/velocity/rough/runtime.py | 6 ++++- .../manipulation/dexterous/_runtime.py | 5 +++- .../gym/envs/tasks/test_dexterous_metrics.py | 12 ++++++++-- tests/gym/envs/tasks/test_rough_velocity.py | 23 +++++++++++++++---- 4 files changed, 38 insertions(+), 8 deletions(-) diff --git a/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/runtime.py b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/runtime.py index 7d46a48041..f37aa69ddd 100644 --- a/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/runtime.py +++ b/extensions/embodichain_tasks/embodichain_tasks/locomotion/velocity/rough/runtime.py @@ -266,7 +266,10 @@ def _step_action(self, action: torch.Tensor) -> torch.Tensor: 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 + 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() @@ -462,6 +465,7 @@ def _get_sensor_obs(self, **kwargs: Any) -> TensorDict: 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() diff --git a/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_runtime.py b/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_runtime.py index ea6ca0e6b7..973522e770 100644 --- a/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_runtime.py +++ b/extensions/embodichain_tasks/embodichain_tasks/manipulation/dexterous/_runtime.py @@ -738,7 +738,10 @@ def _advance_physics(self) -> None: super()._advance_physics() return sensors = [ - sensor for sensor in self.sensors.values() if sensor.requires_substep_update + 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() diff --git a/tests/gym/envs/tasks/test_dexterous_metrics.py b/tests/gym/envs/tasks/test_dexterous_metrics.py index 0a59be624f..760507027a 100644 --- a/tests/gym/envs/tasks/test_dexterous_metrics.py +++ b/tests/gym/envs/tasks/test_dexterous_metrics.py @@ -338,6 +338,9 @@ def test_time_limit_respects_ignored_terminations( "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 ), @@ -356,9 +359,12 @@ def test_physics_samples_opted_in_sensors( ) -> None: """Custom stepping samples each physics interval, including batched updates.""" env = env_factory(task_id) - sensor = Mock(requires_substep_update=True) + sensor = Mock(requires_substep_update=True, cfg=SimpleNamespace(sample_phase=None)) passive = Mock(requires_substep_update=False) - env.sensors = {"contacts": sensor, "camera": passive} + 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): @@ -374,6 +380,8 @@ def advance(dt, count, *, after_substep=None, **kwargs): ) 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 diff --git a/tests/gym/envs/tasks/test_rough_velocity.py b/tests/gym/envs/tasks/test_rough_velocity.py index 85c28f2a93..27aacb332d 100644 --- a/tests/gym/envs/tasks/test_rough_velocity.py +++ b/tests/gym/envs/tasks/test_rough_velocity.py @@ -126,17 +126,27 @@ def test_reference_observation_preserves_reset_contact_rows() -> None: 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}, + 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}] @@ -208,7 +218,7 @@ def update_control_state(self, **kwargs): body_data=SimpleNamespace(fetch_state=fetch_state), set_qf=lambda *a, **kw: None ) env.sim = SimpleNamespace(update=advance) - sensor = Mock(requires_substep_update=True) + 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) @@ -240,8 +250,11 @@ def test_graph_motor_samples_substep_sensors() -> None: 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) - env.sensors = {"contacts": sensor} + 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): @@ -253,6 +266,8 @@ def advance(dt, count, *, after_substep=None, **kwargs): 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()