diff --git a/README.md b/README.md index 4f4c8d70c..0ebb4a068 100644 --- a/README.md +++ b/README.md @@ -14,24 +14,25 @@ **Notice:** This repository is a fork of the main Isaac Lab repo built on top of upstream commit [84d0ff05](https://github.com/isaac-sim/IsaacLab/tree/84d0ff05a6). Modifications for heterogeneous training are layered directly on top of the core framework, maintaining a clean Git history. -**Notice:** This project is different from [IsaacLab-HARL](https://github.com/DIRECTLab/IsaacLab-HARL) which focuses on heterogeneous _multi-agent_ learning in Isaac Lab. In this project, we focus on heterogeneous _single-robot_ learning, where each environment contains a single robot but the robots across environments are different. This allows us to train morphology-agnostic policies across multiple quadrupedal robots simultaneously. +**Notice:** This project is different from [IsaacLab-HARL](https://github.com/DIRECTLab/IsaacLab-HARL) which focuses on heterogeneous _multi-agent_ learning in Isaac Lab. In this project, we focus on heterogeneous _single-robot_ learning, where each environment contains a single robot but the robots across environments are different. This allows us to train morphology-agnostic policies across multiple quadrupedal and humanoid/bipedal robots simultaneously. **Isaac Lab** is a GPU-accelerated, open-source framework designed to unify and simplify robotics research workflows, such as reinforcement learning, imitation learning, and motion planning. Built on NVIDIA Isaac Sim, it combines fast and accurate physics and sensor simulation, making it an ideal choice for sim-to-real transfer in robotics. ## 🤖 Heterogeneous Multi-Robot Training -This fork introduces **Hetero-IsaacLab**, a specialized architecture for training morphology-agnostic locomotion policies across multiple heterogeneous **quadrupedal** robotic environments simultaneously. +This fork introduces **Hetero-IsaacLab**, a specialized architecture for training morphology-agnostic locomotion policies across multiple heterogeneous **quadrupedal** and **humanoid/bipedal** robotic environments simultaneously. Most physics simulators and RL frameworks assume homogeneity, making it difficult to train universal controllers. This repository bridges that gap, providing concrete advantages: -* **Morphology-Agnostic Feature Learning:** The policy is forced to learn fundamental locomotion principles that transcend specific hardware rather than memorizing robot-specific quirks. -* **Efficient Multi-Platform Deployment:** Training 8 robot types heterogeneously uses the same compute as training 1 robot type, eliminating the need to maintain separate codebases and models. -* **Better Exploration:** Different morphologies explore different regions of the state-action space naturally (e.g., lighter robots discover high-speed gaits, heavier ones excel at stability). +* **Morphology-Agnostic Feature Learning:** The policy is forced to learn fundamental locomotion principles that transcend specific hardware morphologies rather than memorizing robot-specific quirks. +* **Efficient Multi-Platform Deployment:** Training 8 quadruped or 5 humanoid robot types heterogeneously uses the same compute as training 1 robot type, eliminating the need to maintain separate codebases and models. +* **Better Exploration:** Different morphologies explore different regions of the state-action space naturally (e.g., varying leg lengths, weights, and joint actuation limits discover robust, versatile gaits). ### Key Architecture Features -* **Heterogeneous Configuration System:** Custom config classes with dynamic environment assignment and reward filtering. -* **Observation & Action Unification:** Enforces an "ANYmal Joint-Major" format, mapping diverse joint orders (e.g., Spot, Unitree) to a standard policy format. -* **Index Mapping System:** Efficient conversion between global environment IDs and robot-local indices. -* **Comprehensive Domain Randomization:** Handles extreme morphological quirks with flexible reset randomizations (mass, CoM, friction) and interval randomization for external disturbances. +* **Dual-Paradigm Heterogeneous Environments:** Supports both Manager-Based environments and high-throughput Direct RL workflows (`HeteroQuadrupedEnv` and `HeteroHumanoidEnv`). +* **Quadruped Joint Unification:** Enforces an "ANYmal Joint-Major" format, mapping diverse quadruped joint orders (e.g., Spot, Unitree, ANYmal) to a standard policy format. +* **Humanoid Semantic Joint Alignment & Action Masking:** A canonical 50-DOF semantic joint alignment system combined with `MaskedMLPModel` to seamlessly handle morphological variations across humanoids (from 12-DOF biped legs to 30+ DOF full-body humanoids with arms and hands) without gradient pollution or entropy distortions. +* **Index Mapping System:** Efficient zero-copy conversion between global environment IDs and robot-local indices. +* **Comprehensive Domain Randomization:** Handles extreme morphological quirks with per-robot overrides for mass, center of mass, ground friction, velocity command ranges, and external disturbance pushes. ### Training Results and Insights For a comprehensive breakdown of these experimental results, including the detailed methodology and the complete set of performance plots, the full report is available in [this WandB report](https://wandb.ai/modanesh/Hetero-Isaac/reports/Hetero-IsaacLab-Experiments--VmlldzoxNjQ4NTMzNw). @@ -54,6 +55,7 @@ cd Hetero-IsaacLab ### Basic Training +#### 1. Heterogeneous Quadrupeds To train on a specific subset of quadrupeds, pass the `--quadrupeds` flag from `anymal_d,anymal_c,anymal_b,unitree_a1,unitree_go1,unitree_go2,unitree_b2,spot`: ```bash @@ -63,13 +65,32 @@ To train on a specific subset of quadrupeds, pass the `--quadrupeds` flag from ` --quadrupeds anymal_d,anymal_c,anymal_b,unitree_a1,unitree_go1,unitree_go2,unitree_b2,spot ``` -To train on heterogeneous **humanoid/biped** robots (`cassie,digit,g1,h1`), pass the `--humanoids` flag: +#### 2. Heterogeneous Humanoids & Bipeds +To train on heterogeneous **humanoid/biped** robots (`cassie,digit,g1,h1,h2`), pass the `--humanoids` flag: ```bash -# Train on all 4 humanoid robots with 4096 environments +# Train on all 5 humanoid robots with 4096 environments ./isaaclab.sh -p scripts/reinforcement_learning/rsl_rl/train.py \ --task=Isaac-Velocity-Flat-HeteroHumanoid-v0 \ - --humanoids cassie,digit,g1,h1 + --humanoids cassie,digit,g1,h1,h2 +``` + +#### 3. Single-Robot Locomotion (Unitree H2 Humanoid) +You can also train on the standalone Unitree H2 full-body humanoid (31-DOF) via the Manager-Based RL workflow: + +```bash +# Download official Unitree H2 USD assets (if needed) +python scripts/tools/download_h2_assets.py + +# Train Unitree H2 on flat terrain +./isaaclab.sh -p scripts/reinforcement_learning/rsl_rl/train.py \ + --task=Isaac-Velocity-Flat-H2-v0 \ + --num_envs=4096 + +# Play and visualize trained H2 policy +./isaaclab.sh -p scripts/reinforcement_learning/rsl_rl/play.py \ + --task=Isaac-Velocity-Flat-H2-Play-v0 \ + --num_envs=16 ``` diff --git a/docs/source/_static/all_robots_unified.gif b/docs/source/_static/all_robots_unified.gif index 92ab0b7e8..da644db5d 100644 --- a/docs/source/_static/all_robots_unified.gif +++ b/docs/source/_static/all_robots_unified.gif @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:502d04115ee537542976356ca1d74cb2f533d00c277d86d9658e7684379b2869 -size 8835546 +oid sha256:f074513f8d7d819cbc4f0e91e76354d68a7d7f6df809631289ae0bf7778d282c +size 23299834 diff --git a/scripts/demos/visualize_all_robots.py b/scripts/demos/visualize_all_robots.py index 2c4e29ca9..6df43a9f7 100644 --- a/scripts/demos/visualize_all_robots.py +++ b/scripts/demos/visualize_all_robots.py @@ -1,11 +1,15 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + import argparse -import sys -import torch import os import subprocess + from isaaclab.app import AppLauncher -parser = argparse.ArgumentParser(description="Visualize all 12 robots in one scene.") +parser = argparse.ArgumentParser(description="Visualize all 13 robots in one scene.") AppLauncher.add_app_launcher_args(parser) args_cli = parser.parse_args() @@ -13,20 +17,29 @@ simulation_app = app_launcher.app import isaaclab.sim as sim_utils -from isaaclab.scene import InteractiveScene, InteractiveSceneCfg from isaaclab.assets import AssetBaseCfg +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg from isaaclab.sensors import CameraCfg from isaaclab.utils import configclass +from isaaclab_assets.robots.agility import DIGIT_V4_CFG + # Quadruped configs -from isaaclab_assets.robots.anymal import ANYMAL_D_CFG, ANYMAL_C_CFG, ANYMAL_B_CFG -from isaaclab_assets.robots.unitree import UNITREE_A1_CFG, UNITREE_GO1_CFG, UNITREE_GO2_CFG, UNITREE_B2_CFG -from isaaclab_assets.robots.spot import SPOT_CFG +from isaaclab_assets.robots.anymal import ANYMAL_B_CFG, ANYMAL_C_CFG, ANYMAL_D_CFG # Humanoid configs from isaaclab_assets.robots.cassie import CASSIE_CFG -from isaaclab_assets.robots.agility import DIGIT_V4_CFG -from isaaclab_assets.robots.unitree import G1_MINIMAL_CFG, H1_MINIMAL_CFG +from isaaclab_assets.robots.spot import SPOT_CFG +from isaaclab_assets.robots.unitree import ( + G1_MINIMAL_CFG, + H1_MINIMAL_CFG, + H2_MINIMAL_CFG, + UNITREE_A1_CFG, + UNITREE_B2_CFG, + UNITREE_GO1_CFG, + UNITREE_GO2_CFG, +) + @configclass class UniversalSceneCfg(InteractiveSceneCfg): @@ -38,7 +51,7 @@ class UniversalSceneCfg(InteractiveSceneCfg): prim_path="/World/ground", spawn=sim_utils.GroundPlaneCfg(), ) - + # Light light = AssetBaseCfg( prim_path="/World/skyLight", @@ -51,7 +64,7 @@ class UniversalSceneCfg(InteractiveSceneCfg): # Global camera to capture the grid camera = CameraCfg( prim_path="/World/camera", - update_period=0.02, # 50 fps + update_period=0.02, # 50 fps height=720, width=1280, data_types=["rgb"], @@ -60,13 +73,13 @@ class UniversalSceneCfg(InteractiveSceneCfg): ), # Position the camera at (18.0, 18.0, 12.0) looking at (2.0, 3.0, 0.0) offset=CameraCfg.OffsetCfg( - pos=(18.0, 18.0, 12.0), - rot=(0.3420, 0.2027, 0.4679, 0.7893), # w, x, y, z - convention="opengl" + pos=(18.0, 18.0, 12.0), + rot=(0.3420, 0.2027, 0.4679, 0.7893), # w, x, y, z + convention="opengl", ), ) - # 12 Robots + # 13 Robots (8 Quadrupeds + 5 Humanoids) anymal_d = ANYMAL_D_CFG.replace(prim_path="/World/envs/env_.*/anymal_d") anymal_c = ANYMAL_C_CFG.replace(prim_path="/World/envs/env_.*/anymal_c") anymal_b = ANYMAL_B_CFG.replace(prim_path="/World/envs/env_.*/anymal_b") @@ -75,41 +88,45 @@ class UniversalSceneCfg(InteractiveSceneCfg): unitree_go2 = UNITREE_GO2_CFG.replace(prim_path="/World/envs/env_.*/unitree_go2") unitree_b2 = UNITREE_B2_CFG.replace(prim_path="/World/envs/env_.*/unitree_b2") spot = SPOT_CFG.replace(prim_path="/World/envs/env_.*/spot") - + cassie = CASSIE_CFG.replace(prim_path="/World/envs/env_.*/cassie") digit = DIGIT_V4_CFG.replace(prim_path="/World/envs/env_.*/digit") g1 = G1_MINIMAL_CFG.replace(prim_path="/World/envs/env_.*/g1") h1 = H1_MINIMAL_CFG.replace(prim_path="/World/envs/env_.*/h1") + h2 = H2_MINIMAL_CFG.replace(prim_path="/World/envs/env_.*/h2") + def main(): scene_cfg = UniversalSceneCfg() - - # The configs are set up to be placed in a grid in env_0 and then cloned. - # Grid: 4 columns, 3 rows - # x goes 0, 2, 4 - # y goes 0, 2, 4, 6 + configs = [ - scene_cfg.anymal_d, scene_cfg.anymal_c, scene_cfg.anymal_b, scene_cfg.unitree_a1, - scene_cfg.unitree_go1, scene_cfg.unitree_go2, scene_cfg.unitree_b2, scene_cfg.spot, - scene_cfg.cassie, scene_cfg.digit, scene_cfg.g1, scene_cfg.h1 + scene_cfg.anymal_d, + scene_cfg.anymal_c, + scene_cfg.anymal_b, + scene_cfg.unitree_a1, + scene_cfg.unitree_go1, + scene_cfg.unitree_go2, + scene_cfg.unitree_b2, + scene_cfg.spot, + scene_cfg.cassie, + scene_cfg.digit, + scene_cfg.g1, + scene_cfg.h1, + scene_cfg.h2, ] - + # We will orient them all to face -X so they look at the camera - # Quaternion for 180 degree yaw (Z axis) - import math - yaw_180 = (0.0, 0.0, 0.0, 1.0) # w,x,y,z for 180 yaw is actually (0, 0, 0, 1) or (0, 0, 1, 0) - # wait w=cos(90)=0, z=sin(90)=1 -> (0, 0, 0, 1) - + for i, cfg in enumerate(configs): row = i // 4 col = i % 4 cfg.init_state.pos = (row * 2.0, col * 2.0, 1.0) cfg.init_state.rot = (0.0, 0.0, 0.0, 1.0) - + # Reset simulation first sim = sim_utils.SimulationContext(sim_utils.SimulationCfg(dt=0.005)) sim.set_camera_view([10, 10, 5], [3, 3, 0]) - + scene = InteractiveScene(scene_cfg) sim.reset() scene.update(0.0) @@ -126,48 +143,53 @@ def main(): # Step physics (zero actions applied naturally) sim.step() scene.update(sim.cfg.dt) - + # Capture camera if step % 4 == 0: # capture every 4 physics steps (dt=0.005 -> 0.02 render step) # image is (H, W, 4) from isaaclab camera img_tensor = scene.sensors["camera"].data.output["rgb"][0].cpu().numpy() - + # Convert RGBA to RGB if needed if img_tensor.shape[2] == 4: img_tensor = img_tensor[:, :, :3] - + # IsaacLab images are usually uint8 already if img_tensor.dtype != np.uint8: img_tensor = (img_tensor * 255).astype(np.uint8) - + frames.append(img_tensor) - + print(f"[INFO] Captured {len(frames)} frames. Generating GIF with ffmpeg...") - + # Write frames to tmp dir for ffmpeg for i, f in enumerate(frames): # Convert RGB to BGR for cv2 imwrite f_bgr = cv2.cvtColor(f, cv2.COLOR_RGB2BGR) cv2.imwrite(f"tmp_frames/frame_{i:04d}.png", f_bgr) - + # Generate GIF out_path = "docs/source/_static/all_robots_unified.gif" cmd = [ "ffmpeg", - "-framerate", "50", - "-i", "tmp_frames/frame_%04d.png", - "-filter_complex", "split[s0][s1];[s0]palettegen=max_colors=256[p];[s1][p]paletteuse=dither=bayer", + "-framerate", + "50", + "-i", + "tmp_frames/frame_%04d.png", + "-filter_complex", + "split[s0][s1];[s0]palettegen=max_colors=256[p];[s1][p]paletteuse=dither=bayer", "-y", - out_path + out_path, ] subprocess.run(cmd, check=True) - + # Cleanup for i in range(len(frames)): os.remove(f"tmp_frames/frame_{i:04d}.png") os.rmdir("tmp_frames") - + print(f"[SUCCESS] Unified GIF saved to {out_path}") + simulation_app.close() + if __name__ == "__main__": main() diff --git a/scripts/reinforcement_learning/rsl_rl/cli_args.py b/scripts/reinforcement_learning/rsl_rl/cli_args.py index 39fd2ead7..5a5c37c20 100644 --- a/scripts/reinforcement_learning/rsl_rl/cli_args.py +++ b/scripts/reinforcement_learning/rsl_rl/cli_args.py @@ -50,7 +50,7 @@ def add_rsl_rl_args(parser: argparse.ArgumentParser): "--humanoids", type=str, default=None, - help="Comma-separated list of humanoids to use, e.g., 'cassie,digit,g1,h1'.", + help="Comma-separated list of humanoids to use, e.g., 'cassie,digit,g1,h1,h2'.", ) diff --git a/scripts/tools/download_h2_assets.py b/scripts/tools/download_h2_assets.py new file mode 100644 index 000000000..719f6a8b1 --- /dev/null +++ b/scripts/tools/download_h2_assets.py @@ -0,0 +1,161 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +# Copyright (c) 2022-2026, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Utility script to download official Unitree H2 USD assets into the local Isaac Lab asset cache.""" + +import argparse +import os +import shutil +import subprocess +import urllib.request + +REPO_ID = "unitreerobotics/unitree_model" +HF_BASE_URL = f"https://huggingface.co/datasets/{REPO_ID}/resolve/main/H2" + +# Files composing the H2 USD asset +H2_FILES = [ + "H2_dae.usd", + "configuration/H2_dae_base.usd", + "configuration/H2_dae_physics.usd", + "configuration/H2_dae_robot.usd", + "configuration/H2_dae_sensor.usd", +] + + +def get_default_target_dir() -> str: + """Get default asset target directory inside isaaclab_assets/data/Robots/Unitree/H2.""" + script_dir = os.path.dirname(os.path.abspath(__file__)) + repo_root = os.path.abspath(os.path.join(script_dir, "../../")) + return os.path.join(repo_root, "source/isaaclab_assets/data/Robots/Unitree/H2") + + +def download_via_huggingface_hub(target_dir: str) -> bool: + """Attempt download using huggingface_hub Python package.""" + try: + from huggingface_hub import hf_hub_download + + print("[INFO] Attempting download using huggingface_hub...") + for rel_path in H2_FILES: + dest = os.path.join(target_dir, rel_path) + os.makedirs(os.path.dirname(dest), exist_ok=True) + hf_path = f"H2/{rel_path}" + print(f"[INFO] Downloading {hf_path} -> {dest}") + downloaded = hf_hub_download( + repo_id=REPO_ID, + filename=hf_path, + repo_type="dataset", + local_dir_use_symlinks=False, + ) + shutil.copy2(downloaded, dest) + return True + except Exception as e: + print(f"[WARNING] huggingface_hub download failed: {e}") + return False + + +def download_via_git_sparse_checkout(target_dir: str) -> bool: + """Attempt download using git sparse-checkout.""" + try: + print("[INFO] Attempting download using git clone (sparse-checkout)...") + temp_dir = os.path.join(target_dir, "_temp_git_clone") + if os.path.exists(temp_dir): + shutil.rmtree(temp_dir) + + clone_cmd = [ + "git", + "clone", + "--depth=1", + "--filter=blob:none", + "--sparse", + f"https://huggingface.co/datasets/{REPO_ID}", + temp_dir, + ] + subprocess.run(clone_cmd, check=True) + subprocess.run(["git", "-C", temp_dir, "sparse-checkout", "set", "H2"], check=True) + subprocess.run(["git", "-C", temp_dir, "lfs", "pull"], check=False) + + src_h2 = os.path.join(temp_dir, "H2") + if os.path.exists(src_h2): + for item in os.listdir(src_h2): + s = os.path.join(src_h2, item) + d = os.path.join(target_dir, item) + if os.path.isdir(s): + shutil.copytree(s, d, dirs_exist_ok=True) + else: + shutil.copy2(s, d) + shutil.rmtree(temp_dir) + return True + except Exception as e: + print(f"[WARNING] Git sparse-checkout failed: {e}") + temp_dir = os.path.join(target_dir, "_temp_git_clone") + if os.path.exists(temp_dir): + shutil.rmtree(temp_dir, ignore_errors=True) + return False + + +def download_via_urllib(target_dir: str) -> bool: + """Fallback direct HTTP download using urllib.""" + print("[INFO] Attempting direct download via urllib...") + try: + for rel_path in H2_FILES: + dest = os.path.join(target_dir, rel_path) + os.makedirs(os.path.dirname(dest), exist_ok=True) + url = f"{HF_BASE_URL}/{rel_path}" + print(f"[INFO] Downloading {url} -> {dest} ...") + req = urllib.request.Request(url, headers={"User-Agent": "IsaacLab-H2-Downloader"}) + with urllib.request.urlopen(req) as resp, open(dest, "wb") as out_file: + shutil.copyfileobj(resp, out_file) + return True + except Exception as e: + print(f"[ERROR] Direct urllib download failed: {e}") + return False + + +def ensure_h2_assets(target_dir: str | None = None) -> str: + """Ensure H2 USD assets are present in the target directory, downloading if missing.""" + if target_dir is None: + target_dir = get_default_target_dir() + + main_usd = os.path.join(target_dir, "H2_dae.usd") + base_usd = os.path.join(target_dir, "configuration/H2_dae_base.usd") + + if os.path.exists(main_usd) and os.path.exists(base_usd) and os.path.getsize(base_usd) > 1000000: + print(f"[INFO] H2 assets already present at: {target_dir}") + return main_usd + + os.makedirs(target_dir, exist_ok=True) + print(f"[INFO] Downloading Unitree H2 assets into: {target_dir}") + + success = ( + download_via_huggingface_hub(target_dir) + or download_via_git_sparse_checkout(target_dir) + or download_via_urllib(target_dir) + ) + + if not success or not os.path.exists(main_usd): + raise RuntimeError( + f"Failed to download Unitree H2 assets into '{target_dir}'. " + f"Please download manually from https://huggingface.co/datasets/{REPO_ID}." + ) + + print(f"[INFO] Successfully downloaded H2 assets to: {target_dir}") + return main_usd + + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description="Download Unitree H2 USD assets.") + parser.add_argument( + "--target_dir", + type=str, + default=None, + help="Target directory for downloaded assets (default: source/isaaclab_assets/data/Robots/Unitree/H2).", + ) + args = parser.parse_args() + ensure_h2_assets(args.target_dir) diff --git a/source/isaaclab_assets/isaaclab_assets/robots/unitree.py b/source/isaaclab_assets/isaaclab_assets/robots/unitree.py index b5a78993a..9f4e5ef2c 100644 --- a/source/isaaclab_assets/isaaclab_assets/robots/unitree.py +++ b/source/isaaclab_assets/isaaclab_assets/robots/unitree.py @@ -12,6 +12,8 @@ * :obj:`UNITREE_GO2_CFG`: Unitree Go2 robot with DC motor model for the legs * :obj:`H1_CFG`: H1 humanoid robot * :obj:`H1_MINIMAL_CFG`: H1 humanoid robot with minimal collision bodies +* :obj:`H2_CFG`: H2 humanoid robot (31-DOF) +* :obj:`H2_MINIMAL_CFG`: H2 humanoid robot with minimal collision bodies * :obj:`G1_CFG`: G1 humanoid robot * :obj:`G1_MINIMAL_CFG`: G1 humanoid robot with minimal collision bodies * :obj:`G1_29DOF_CFG`: G1 humanoid robot configured for locomanipulation tasks @@ -20,6 +22,9 @@ Reference: https://github.com/unitreerobotics/unitree_ros """ +import math +import os + import isaaclab.sim as sim_utils from isaaclab.actuators import ActuatorNetMLPCfg, DCMotorCfg, ImplicitActuatorCfg from isaaclab.assets.articulation import ArticulationCfg @@ -312,6 +317,200 @@ """ +_H2_LOCAL_USD = os.path.abspath(os.path.join(os.path.dirname(__file__), "../../data/Robots/Unitree/H2/H2_dae.usd")) +H2_USD_PATH = _H2_LOCAL_USD if os.path.exists(_H2_LOCAL_USD) else f"{ISAACLAB_NUCLEUS_DIR}/Robots/Unitree/H2/h2.usd" + + +# Unitree H2 Actuator Motor Constants (derived from natural frequency 10Hz, damping ratio 2.0) +_H2_ARMATURE_5020 = 0.003609725 +_H2_ARMATURE_7520_14 = 0.010177520 +_H2_ARMATURE_7520_22 = 0.025101925 +_H2_ARMATURE_4010 = 0.00425 + +_H2_NATURAL_FREQ = 10 * 2.0 * math.pi +_H2_DAMPING_RATIO = 2.0 + +_H2_STIFFNESS_5020 = _H2_ARMATURE_5020 * _H2_NATURAL_FREQ**2 +_H2_STIFFNESS_7520_14 = _H2_ARMATURE_7520_14 * _H2_NATURAL_FREQ**2 +_H2_STIFFNESS_7520_22 = _H2_ARMATURE_7520_22 * _H2_NATURAL_FREQ**2 +_H2_STIFFNESS_4010 = _H2_ARMATURE_4010 * _H2_NATURAL_FREQ**2 + +_H2_DAMPING_5020 = 2.0 * _H2_DAMPING_RATIO * _H2_ARMATURE_5020 * _H2_NATURAL_FREQ +_H2_DAMPING_7520_14 = 2.0 * _H2_DAMPING_RATIO * _H2_ARMATURE_7520_14 * _H2_NATURAL_FREQ +_H2_DAMPING_7520_22 = 2.0 * _H2_DAMPING_RATIO * _H2_ARMATURE_7520_22 * _H2_NATURAL_FREQ +_H2_DAMPING_4010 = 2.0 * _H2_DAMPING_RATIO * _H2_ARMATURE_4010 * _H2_NATURAL_FREQ + + +H2_CFG = ArticulationCfg( + spawn=sim_utils.UsdFileCfg( + usd_path=H2_USD_PATH, + activate_contact_sensors=True, + rigid_props=sim_utils.RigidBodyPropertiesCfg( + disable_gravity=False, + retain_accelerations=False, + linear_damping=0.0, + angular_damping=0.0, + max_linear_velocity=1000.0, + max_angular_velocity=1000.0, + max_depenetration_velocity=1.0, + ), + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, solver_position_iteration_count=8, solver_velocity_iteration_count=4 + ), + ), + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.0, 0.0, 1.04), + joint_pos={ + ".*_hip_pitch_joint": -0.312, + ".*_knee_joint": 0.669, + ".*_ankle_pitch_joint": -0.363, + "waist_pitch_joint": 0.0, + "waist_roll_joint": 0.0, + "waist_yaw_joint": 0.0, + ".*_elbow_joint": 0.6, + ".*_shoulder_pitch_joint": 0.0, + ".*_shoulder_roll_joint": 0.0, + ".*_shoulder_yaw_joint": 0.0, + }, + joint_vel={".*": 0.0}, + ), + soft_joint_pos_limit_factor=0.9, + actuators={ + "legs": ImplicitActuatorCfg( + joint_names_expr=[ + ".*_hip_yaw_joint", + ".*_hip_roll_joint", + ".*_hip_pitch_joint", + ".*_knee_joint", + ], + effort_limit_sim={ + ".*_hip_yaw_joint": 264.0, + ".*_hip_roll_joint": 417.0, + ".*_hip_pitch_joint": 417.0, + ".*_knee_joint": 417.0, + }, + velocity_limit_sim={ + ".*_hip_yaw_joint": 32.0, + ".*_hip_roll_joint": 20.0, + ".*_hip_pitch_joint": 20.0, + ".*_knee_joint": 20.0, + }, + stiffness={ + ".*_hip_pitch_joint": _H2_STIFFNESS_7520_22, + ".*_hip_roll_joint": _H2_STIFFNESS_7520_22, + ".*_hip_yaw_joint": _H2_STIFFNESS_7520_14, + ".*_knee_joint": _H2_STIFFNESS_7520_22, + }, + damping={ + ".*_hip_pitch_joint": _H2_DAMPING_7520_22, + ".*_hip_roll_joint": _H2_DAMPING_7520_22, + ".*_hip_yaw_joint": _H2_DAMPING_7520_14, + ".*_knee_joint": _H2_DAMPING_7520_22, + }, + armature={ + ".*_hip_pitch_joint": _H2_ARMATURE_7520_22, + ".*_hip_roll_joint": _H2_ARMATURE_7520_22, + ".*_hip_yaw_joint": _H2_ARMATURE_7520_14, + ".*_knee_joint": _H2_ARMATURE_7520_22, + }, + ), + "feet": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_pitch_joint", ".*_ankle_roll_joint"], + effort_limit_sim=150.0, + velocity_limit_sim=37.0, + stiffness=2.0 * _H2_STIFFNESS_5020, + damping=2.0 * _H2_DAMPING_5020, + armature=2.0 * _H2_ARMATURE_5020, + ), + "waist": ImplicitActuatorCfg( + joint_names_expr=["waist_roll_joint", "waist_pitch_joint"], + effort_limit_sim=200.0, + velocity_limit_sim=37.0, + stiffness=200.0, + damping=5.0, + armature=0.01, + ), + "waist_yaw": ImplicitActuatorCfg( + joint_names_expr=["waist_yaw_joint"], + effort_limit_sim=264.0, + velocity_limit_sim=32.0, + stiffness=_H2_STIFFNESS_7520_14, + damping=_H2_DAMPING_7520_14, + armature=_H2_ARMATURE_7520_14, + ), + "head": ImplicitActuatorCfg( + joint_names_expr=["head_pitch_joint", "head_yaw_joint"], + effort_limit_sim=150.0, + velocity_limit_sim=37.0, + stiffness=2.0 * _H2_STIFFNESS_5020, + damping=2.0 * _H2_DAMPING_5020, + armature=2.0 * _H2_ARMATURE_5020, + ), + "arms": ImplicitActuatorCfg( + joint_names_expr=[ + ".*_shoulder_pitch_joint", + ".*_shoulder_roll_joint", + ".*_shoulder_yaw_joint", + ".*_elbow_joint", + ".*_wrist_roll_joint", + ".*_wrist_pitch_joint", + ".*_wrist_yaw_joint", + ], + effort_limit_sim={ + ".*_shoulder_pitch_joint": 75.0, + ".*_shoulder_roll_joint": 75.0, + ".*_shoulder_yaw_joint": 75.0, + ".*_elbow_joint": 75.0, + ".*_wrist_roll_joint": 75.0, + ".*_wrist_pitch_joint": 15.0, + ".*_wrist_yaw_joint": 15.0, + }, + velocity_limit_sim={ + ".*_shoulder_pitch_joint": 37.0, + ".*_shoulder_roll_joint": 37.0, + ".*_shoulder_yaw_joint": 37.0, + ".*_elbow_joint": 37.0, + ".*_wrist_roll_joint": 37.0, + ".*_wrist_pitch_joint": 22.0, + ".*_wrist_yaw_joint": 22.0, + }, + stiffness={ + ".*_shoulder_pitch_joint": _H2_STIFFNESS_5020, + ".*_shoulder_roll_joint": _H2_STIFFNESS_5020, + ".*_shoulder_yaw_joint": _H2_STIFFNESS_5020, + ".*_elbow_joint": _H2_STIFFNESS_5020, + ".*_wrist_roll_joint": _H2_STIFFNESS_5020, + ".*_wrist_pitch_joint": _H2_STIFFNESS_4010, + ".*_wrist_yaw_joint": _H2_STIFFNESS_4010, + }, + damping={ + ".*_shoulder_pitch_joint": _H2_DAMPING_5020, + ".*_shoulder_roll_joint": _H2_DAMPING_5020, + ".*_shoulder_yaw_joint": _H2_DAMPING_5020, + ".*_elbow_joint": _H2_DAMPING_5020, + ".*_wrist_roll_joint": _H2_DAMPING_5020, + ".*_wrist_pitch_joint": _H2_DAMPING_4010, + ".*_wrist_yaw_joint": _H2_DAMPING_4010, + }, + armature={ + ".*_shoulder_pitch_joint": _H2_ARMATURE_5020, + ".*_shoulder_roll_joint": _H2_ARMATURE_5020, + ".*_shoulder_yaw_joint": _H2_ARMATURE_5020, + ".*_elbow_joint": _H2_ARMATURE_5020, + ".*_wrist_roll_joint": _H2_ARMATURE_5020, + ".*_wrist_pitch_joint": _H2_ARMATURE_4010, + ".*_wrist_yaw_joint": _H2_ARMATURE_4010, + }, + ), + }, +) +"""Configuration for the Unitree H2 Humanoid robot (31-DOF).""" + + +H2_MINIMAL_CFG = H2_CFG.copy() +"""Configuration for the Unitree H2 Humanoid robot with minimal collision bodies.""" + + G1_CFG = ArticulationCfg( spawn=sim_utils.UsdFileCfg( usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Robots/Unitree/G1/g1.usd", diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/hetero_humanoid/hetero_humanoid_env.py b/source/isaaclab_tasks/isaaclab_tasks/direct/hetero_humanoid/hetero_humanoid_env.py index 415a75ca9..9a0b331fc 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/hetero_humanoid/hetero_humanoid_env.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/hetero_humanoid/hetero_humanoid_env.py @@ -6,7 +6,6 @@ from __future__ import annotations import math -from typing import Dict, Tuple import torch @@ -26,7 +25,7 @@ class HeterogeneousHumanoidVelocityEnv(DirectRLEnv): cfg: HeterogeneousHumanoidVelocityEnvCfg def __init__(self, cfg: HeterogeneousHumanoidVelocityEnvCfg, render_mode: str | None = None, **kwargs): - self.all_humanoids = ["cassie", "digit", "g1", "h1"] + self.all_humanoids = ["cassie", "digit", "g1", "h1", "h2"] self.humanoids_list = getattr(cfg, "humanoids", self.all_humanoids) print(f"[INFO] Instantiating environment with humanoids: {self.humanoids_list}") @@ -48,10 +47,10 @@ def __init__(self, cfg: HeterogeneousHumanoidVelocityEnvCfg, render_mode: str | self.scene.filter_collisions() - self.robots: Dict[str, Articulation] = dict() - self.robot_sensors: Dict[str, ContactSensor] = dict() - self.robot_scanners: Dict[str, RayCaster] = dict() - self.robot_env_ids: Dict[str, torch.Tensor] = dict() + self.robots: dict[str, Articulation] = dict() + self.robot_sensors: dict[str, ContactSensor] = dict() + self.robot_scanners: dict[str, RayCaster] = dict() + self.robot_env_ids: dict[str, torch.Tensor] = dict() for robot_name in self.humanoids_list: self.robots[robot_name] = self.scene[robot_name] @@ -89,10 +88,29 @@ def __init__(self, cfg: HeterogeneousHumanoidVelocityEnvCfg, render_mode: str | self.reward_manager = RewardManager(self.cfg.rewards, self) + # Precompute illegal contact body IDs for each humanoid + self._illegal_contact_body_ids = {} + for robot_name in self.humanoids_list: + sensor = self.robot_sensors[robot_name] + contact_bodies = "torso" + if robot_name in ROBOT_CONFIGS: + cfg_r = ROBOT_CONFIGS[robot_name] + if cfg_r.illegal_contact_bodies is not None: + contact_bodies = cfg_r.illegal_contact_bodies + else: + contact_bodies = cfg_r.base_link + try: + body_ids, _ = sensor.find_bodies(contact_bodies) + except ValueError: + body_ids = [0] + self._illegal_contact_body_ids[robot_name] = body_ids + print(f"[INFO - {robot_name.upper()}] Resolved {len(body_ids)} illegal contact bodies: {contact_bodies}") + self.termination_results = { "time_out": torch.zeros(self.num_envs, device=self.device, dtype=torch.bool), "base_contact": torch.zeros(self.num_envs, device=self.device, dtype=torch.bool), "base_orientation": torch.zeros(self.num_envs, device=self.device, dtype=torch.bool), + "root_height": torch.zeros(self.num_envs, device=self.device, dtype=torch.bool), } if self.sim.has_gui(): @@ -158,7 +176,7 @@ def _setup_joint_mappings(self): # Standard bird leg uses identity. Humanoid needs pitch/knee flipped. # We also check for 'ankle' to catch H1's ankle, which doesn't have 'pitch' in its name. jname = joint_names[physical_joints[i]] - if "pitch" in jname or "knee" in jname or "ankle" in jname: + if ("pitch" in jname or "knee" in jname or "ankle" in jname) and "roll" not in jname: signs[i] = -1.0 self.joint_signs[robot_name] = signs @@ -368,7 +386,7 @@ def _get_rewards(self) -> torch.Tensor: """Compute rewards using the reward manager.""" return self.reward_manager.compute(dt=self.step_dt) - def _get_dones(self) -> Tuple[torch.Tensor, torch.Tensor]: + def _get_dones(self) -> tuple[torch.Tensor, torch.Tensor]: """Check termination conditions for each robot type.""" time_out = self.episode_length_buf >= self.max_episode_length self.termination_results["time_out"][:] = time_out @@ -377,16 +395,9 @@ def _get_dones(self) -> Tuple[torch.Tensor, torch.Tensor]: env_ids = self.robot_env_ids[robot_name] net_contact_forces = sensor.data.net_forces_w_history - base_link_name = "torso" - if robot_name in ROBOT_CONFIGS: - base_link_name = ROBOT_CONFIGS[robot_name].base_link - - try: - base_link, _ = sensor.find_bodies(base_link_name) - except ValueError: - base_link = [0] # fallback + body_ids = self._illegal_contact_body_ids.get(robot_name, [0]) terminated_robot = torch.any( - torch.max(torch.norm(net_contact_forces[:, :, base_link], dim=-1), dim=1)[0] + torch.max(torch.norm(net_contact_forces[:, :, body_ids], dim=-1), dim=1)[0] > self.cfg.contact_threshold, dim=1, ) @@ -407,7 +418,18 @@ def _get_dones(self) -> Tuple[torch.Tensor, torch.Tensor]: self.termination_results["base_orientation"][:] = base_orientation - dones = base_contact | base_orientation + root_height = torch.zeros(self.num_envs, dtype=torch.bool, device=self.device) + for robot_name, robot in self.robots.items(): + if robot_name in ROBOT_CONFIGS: + min_h = ROBOT_CONFIGS[robot_name].min_root_height + if min_h is not None: + env_ids = self.robot_env_ids[robot_name] + terminated_robot = robot.data.root_pos_w[:, 2] < min_h + root_height[env_ids] = terminated_robot + + self.termination_results["root_height"][:] = root_height + + dones = base_contact | base_orientation | root_height return dones, time_out def _reset_idx(self, env_ids: torch.Tensor | None): diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/hetero_humanoid/hetero_humanoid_rewards.py b/source/isaaclab_tasks/isaaclab_tasks/direct/hetero_humanoid/hetero_humanoid_rewards.py index 827fd4754..52d60ad3c 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/hetero_humanoid/hetero_humanoid_rewards.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/hetero_humanoid/hetero_humanoid_rewards.py @@ -8,6 +8,7 @@ from isaaclab.assets import Articulation, RigidObject from isaaclab.managers import SceneEntityCfg from isaaclab.sensors import ContactSensor +from isaaclab.utils.math import quat_apply_inverse, yaw_quat def get_robot_env_ids(env, cfg: SceneEntityCfg): @@ -24,9 +25,14 @@ def get_robot_env_ids(env, cfg: SceneEntityCfg): def _resolve_joint_ids(asset, cfg: SceneEntityCfg): """Resolve joint IDs from config, defaulting to all if None.""" - if cfg.joint_ids is None: - return slice(None) - return cfg.joint_ids + if cfg.joint_ids is not None and not isinstance(cfg.joint_ids, slice): + return cfg.joint_ids + if cfg.joint_names is not None: + ids, _ = asset.find_joints(cfg.joint_names) + return ids + if cfg.joint_ids is not None: + return cfg.joint_ids + return slice(None) def _resolve_body_ids(asset, cfg: SceneEntityCfg): @@ -39,11 +45,10 @@ def _resolve_body_ids(asset, cfg: SceneEntityCfg): return slice(None) -from isaaclab.utils.math import quat_apply_inverse, yaw_quat - - def track_lin_vel_xy_exp(env, std: float, command_name: str, asset_cfg: SceneEntityCfg) -> torch.Tensor: - """Reward tracking of linear velocity commands (xy axes) in the gravity aligned robot frame using exponential kernel.""" + """Reward tracking of linear velocity commands (xy axes) in the gravity aligned + robot frame using exponential kernel. + """ asset: RigidObject = env.scene[asset_cfg.name] robot_env_ids = get_robot_env_ids(env, asset_cfg) reward = torch.zeros(env.num_envs, device=env.device) @@ -95,19 +100,50 @@ def feet_air_time_biped(env, sensor_cfg: SceneEntityCfg, command_name: str, thre feet_ids = _resolve_body_ids(contact_sensor, sensor_cfg) robot_env_ids = get_robot_env_ids(env, sensor_cfg) - air_time = contact_sensor.data.current_air_time[:, feet_ids] - contact_time = contact_sensor.data.current_contact_time[:, feet_ids] + air_time = contact_sensor.data.current_air_time[robot_env_ids][:, feet_ids] + contact_time = contact_sensor.data.current_contact_time[robot_env_ids][:, feet_ids] in_contact = contact_time > 0.0 in_mode_time = torch.where(in_contact, contact_time, air_time) single_stance = torch.sum(in_contact.int(), dim=1) == 1 - reward = torch.zeros(env.num_envs, device=env.device) robot_reward = torch.min(torch.where(single_stance.unsqueeze(-1), in_mode_time, 0.0), dim=1)[0] robot_reward = torch.clamp(robot_reward, max=threshold) is_moving = torch.norm(env._commands[robot_env_ids, :2], dim=1) > 0.1 - robot_reward *= is_moving + robot_reward = robot_reward * is_moving + + reward = torch.zeros(env.num_envs, device=env.device) + reward[robot_env_ids] = robot_reward + return reward + +def feet_air_time_symmetry_biped( + env, sensor_cfg: SceneEntityCfg, command_name: str, yaw_std: float = 0.25 +) -> torch.Tensor: + """Penalize asymmetry in feet air time for bipeds during straight walking. + + This function penalizes differences in air time between the left and right foot upon landing, + preventing asymmetric strides (limping). It is smoothly gated by turning commands so that + differential strides needed for sharp turns are not penalized. + """ + contact_sensor: ContactSensor = env.scene[sensor_cfg.name] + feet_ids = _resolve_body_ids(contact_sensor, sensor_cfg) + robot_env_ids = get_robot_env_ids(env, sensor_cfg) + + commands = env._commands[robot_env_ids] + yaw_cmd = commands[:, 2] + straight_gating = torch.exp(-torch.square(yaw_cmd) / (2 * (yaw_std**2))) + is_moving = torch.norm(commands[:, :2], dim=1) > 0.1 + + first_contact = contact_sensor.compute_first_contact(env.step_dt)[robot_env_ids][:, feet_ids] + any_first_contact = torch.any(first_contact, dim=1) + + last_air_time = contact_sensor.data.last_air_time[robot_env_ids][:, feet_ids] + both_feet_stepped = (last_air_time[:, 0] > 0.0) & (last_air_time[:, 1] > 0.0) + air_diff = torch.abs(last_air_time[:, 0] - last_air_time[:, 1]) + + robot_reward = air_diff * any_first_contact * both_feet_stepped * straight_gating * is_moving + reward = torch.zeros(env.num_envs, device=env.device) reward[robot_env_ids] = robot_reward return reward @@ -127,8 +163,9 @@ def feet_slide(env, sensor_cfg: SceneEntityCfg, asset_cfg: SceneEntityCfg) -> to robot_env_ids = get_robot_env_ids(env, sensor_cfg) reward = torch.zeros(env.num_envs, device=env.device) + net_forces = contact_sensor.data.net_forces_w_history[robot_env_ids] in_contact = ( - torch.max(torch.norm(contact_sensor.data.net_forces_w_history[:, :, sensor_feet_ids], dim=-1), dim=1)[0] > 1.0 + torch.max(torch.norm(net_forces[:, :, sensor_feet_ids], dim=-1), dim=1)[0] > 1.0 ) feet_vel = torch.norm(asset.data.body_lin_vel_w[:, asset_feet_ids, :2], dim=-1) @@ -225,7 +262,7 @@ def no_fly(env, sensor_cfg: SceneEntityCfg) -> torch.Tensor: feet_ids = _resolve_body_ids(contact_sensor, sensor_cfg) robot_env_ids = get_robot_env_ids(env, sensor_cfg) - in_contact = contact_sensor.data.net_forces_w[:, feet_ids, 2] > 1.0 + in_contact = contact_sensor.data.net_forces_w[robot_env_ids][:, feet_ids, 2] > 1.0 has_contact = torch.any(in_contact, dim=-1) reward = torch.zeros(env.num_envs, device=env.device) @@ -265,7 +302,7 @@ def undesired_contacts(env, threshold: float, sensor_cfg: SceneEntityCfg) -> tor robot_env_ids = get_robot_env_ids(env, sensor_cfg) body_ids = _resolve_body_ids(contact_sensor, sensor_cfg) - net_contact_forces = contact_sensor.data.net_forces_w_history + net_contact_forces = contact_sensor.data.net_forces_w_history[robot_env_ids] is_contact = torch.max(torch.norm(net_contact_forces[:, :, body_ids], dim=-1), dim=1)[0] > threshold reward = torch.zeros(env.num_envs, device=env.device) @@ -279,7 +316,7 @@ def desired_contacts(env, sensor_cfg: SceneEntityCfg, threshold: float = 1.0) -> robot_env_ids = get_robot_env_ids(env, sensor_cfg) body_ids = _resolve_body_ids(contact_sensor, sensor_cfg) - contacts = contact_sensor.data.net_forces_w_history[:, :, body_ids, :].norm(dim=-1).max(dim=1)[0] > threshold + contacts = contact_sensor.data.net_forces_w_history[robot_env_ids][:, :, body_ids, :].norm(dim=-1).max(dim=1)[0] > threshold zero_contact = (~contacts).all(dim=1) reward = torch.zeros(env.num_envs, device=env.device) diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/hetero_humanoid/hetero_humanoid_velocity_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/direct/hetero_humanoid/hetero_humanoid_velocity_env_cfg.py index 3fbde40c1..f1932099c 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/hetero_humanoid/hetero_humanoid_velocity_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/hetero_humanoid/hetero_humanoid_velocity_env_cfg.py @@ -3,8 +3,9 @@ # # SPDX-License-Identifier: BSD-3-Clause +from __future__ import annotations + import math -from typing import List import gymnasium as gym @@ -27,7 +28,7 @@ # Humanoid Robot Assets from isaaclab_assets.robots.cassie import CASSIE_CFG -from isaaclab_assets.robots.unitree import G1_MINIMAL_CFG, H1_MINIMAL_CFG +from isaaclab_assets.robots.unitree import G1_MINIMAL_CFG, H1_MINIMAL_CFG, H2_MINIMAL_CFG from .robot_configs import ROBOT_CONFIGS @@ -40,7 +41,7 @@ class HeterogeneousRobotCfg(ArticulationCfg): """Configuration for a robot asset in a heterogeneous scene.""" - env_ids: List[int] | None = None + env_ids: list[int] | None = None """List of environment IDs this robot is present in.""" @@ -48,7 +49,7 @@ class HeterogeneousRobotCfg(ArticulationCfg): class HeterogeneousSensorCfg(ContactSensorCfg): """Configuration for a sensor asset in a heterogeneous scene.""" - env_ids: List[int] | None = None + env_ids: list[int] | None = None """List of environment IDs this sensor is present in.""" @@ -56,7 +57,7 @@ class HeterogeneousSensorCfg(ContactSensorCfg): class HeterogeneousRayCasterCfg(RayCasterCfg): """Configuration for a raycaster asset in a heterogeneous scene.""" - env_ids: List[int] | None = None + env_ids: list[int] | None = None """List of environment IDs this sensor is present in.""" @@ -115,6 +116,9 @@ class HeterogeneousHumanoidSceneCfg(InteractiveSceneCfg): h1: HeterogeneousRobotCfg = H1_MINIMAL_CFG.replace(prim_path="{ENV_REGEX_NS}/h1") h1.spawn.activate_contact_sensors = True + h2: HeterogeneousRobotCfg = H2_MINIMAL_CFG.replace(prim_path="{ENV_REGEX_NS}/h2") + h2.spawn.activate_contact_sensors = True + # Contact Sensors cassie_contacts: HeterogeneousSensorCfg = ContactSensorCfg( prim_path="{ENV_REGEX_NS}/cassie/.*", history_length=3, track_air_time=True @@ -128,6 +132,9 @@ class HeterogeneousHumanoidSceneCfg(InteractiveSceneCfg): h1_contacts: HeterogeneousSensorCfg = ContactSensorCfg( prim_path="{ENV_REGEX_NS}/h1/.*", history_length=3, track_air_time=True ) + h2_contacts: HeterogeneousSensorCfg = ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/h2/.*", history_length=3, track_air_time=True + ) sky_light = AssetBaseCfg( prim_path="/World/skyLight", @@ -169,6 +176,7 @@ class HeterogeneousHumanoidRoughSceneCfg(HeterogeneousHumanoidSceneCfg): digit_scanner = get_raycaster_cfg("{ENV_REGEX_NS}/digit/torso_base") g1_scanner = get_raycaster_cfg("{ENV_REGEX_NS}/g1/torso_link") h1_scanner = get_raycaster_cfg("{ENV_REGEX_NS}/h1/torso_link") + h2_scanner = get_raycaster_cfg("{ENV_REGEX_NS}/h2/pelvis") ## @@ -231,6 +239,15 @@ class RewardsCfg: "threshold": 0.3, }, ) + gait_symmetry_cassie = RewTerm( + func=custom_mdp.feet_air_time_symmetry_biped, + weight=-10.0, + params={ + "sensor_cfg": SceneEntityCfg("cassie_contacts", body_names=".*toe"), + "command_name": "base_velocity", + "yaw_std": 0.25, + }, + ) flat_orientation_l2_cassie = RewTerm( func=custom_mdp.flat_orientation_l2, weight=-2.5, params={"asset_cfg": SceneEntityCfg("cassie")} ) @@ -296,11 +313,20 @@ class RewardsCfg: ) feet_air_time_digit = RewTerm( func=custom_mdp.feet_air_time_biped, - weight=0.25, + weight=0.75, + params={ + "sensor_cfg": SceneEntityCfg("digit_contacts", body_names=".*_leg_toe_roll"), + "command_name": "base_velocity", + "threshold": 0.4, + }, + ) + gait_symmetry_digit = RewTerm( + func=custom_mdp.feet_air_time_symmetry_biped, + weight=-2.5, params={ "sensor_cfg": SceneEntityCfg("digit_contacts", body_names=".*_leg_toe_roll"), "command_name": "base_velocity", - "threshold": 0.8, + "yaw_std": 0.25, }, ) flat_orientation_l2_digit = RewTerm( @@ -395,11 +421,20 @@ class RewardsCfg: ) feet_air_time_g1 = RewTerm( func=custom_mdp.feet_air_time_biped, - weight=0.75, + weight=0.35, params={ "sensor_cfg": SceneEntityCfg("g1_contacts", body_names=".*_ankle_roll_link"), "command_name": "base_velocity", - "threshold": 0.4, + "threshold": 0.28, + }, + ) + gait_symmetry_g1 = RewTerm( + func=custom_mdp.feet_air_time_symmetry_biped, + weight=-10.0, + params={ + "sensor_cfg": SceneEntityCfg("g1_contacts", body_names=".*_ankle_roll_link"), + "command_name": "base_velocity", + "yaw_std": 0.25, }, ) flat_orientation_l2_g1 = RewTerm( @@ -494,6 +529,15 @@ class RewardsCfg: "threshold": 0.4, }, ) + gait_symmetry_h1 = RewTerm( + func=custom_mdp.feet_air_time_symmetry_biped, + weight=-10.0, + params={ + "sensor_cfg": SceneEntityCfg("h1_contacts", body_names=".*ankle_link"), + "command_name": "base_velocity", + "yaw_std": 0.25, + }, + ) flat_orientation_l2_h1 = RewTerm( func=custom_mdp.flat_orientation_l2, weight=-1.0, params={"asset_cfg": SceneEntityCfg("h1")} ) @@ -527,12 +571,141 @@ class RewardsCfg: func=custom_mdp.joint_deviation_l1, weight=-0.1, params={"asset_cfg": SceneEntityCfg("h1", joint_names="torso")} ) + # --- H2 REWARDS --- + track_lin_vel_xy_exp_h2 = RewTerm( + func=custom_mdp.track_lin_vel_xy_exp, + weight=2.0, + params={"command_name": "base_velocity", "std": math.sqrt(0.25), "asset_cfg": SceneEntityCfg("h2")}, + ) + track_ang_vel_z_exp_h2 = RewTerm( + func=custom_mdp.track_ang_vel_z_exp, + weight=1.5, + params={"command_name": "base_velocity", "std": math.sqrt(0.25), "asset_cfg": SceneEntityCfg("h2")}, + ) + ang_vel_xy_l2_h2 = RewTerm(func=custom_mdp.ang_vel_xy_l2, weight=-0.2, params={"asset_cfg": SceneEntityCfg("h2")}) + lin_vel_z_l2_h2 = RewTerm(func=custom_mdp.lin_vel_z_l2, weight=-2.0, params={"asset_cfg": SceneEntityCfg("h2")}) + dof_torques_l2_h2 = RewTerm( + func=custom_mdp.joint_torques_l2, + weight=-1.5e-7, + params={"asset_cfg": SceneEntityCfg("h2", joint_names=[".*_hip_.*", ".*_knee_joint", ".*_ankle_.*"])}, + ) + dof_acc_l2_h2 = RewTerm( + func=custom_mdp.joint_acc_l2, + weight=-2.5e-7, + params={"asset_cfg": SceneEntityCfg("h2", joint_names=[".*_hip_.*", ".*_knee_joint"])}, + ) + action_rate_l2_h2 = RewTerm( + func=custom_mdp.action_rate_l2, weight=-0.003, params={"asset_cfg": SceneEntityCfg("h2")} + ) + feet_air_time_h2 = RewTerm( + func=custom_mdp.feet_air_time_biped, + weight=0.75, + params={ + "sensor_cfg": SceneEntityCfg("h2_contacts", body_names=".*_ankle_pitch_link"), + "command_name": "base_velocity", + "threshold": 0.22, + }, + ) + gait_symmetry_h2 = RewTerm( + func=custom_mdp.feet_air_time_symmetry_biped, + weight=-5.0, + params={ + "sensor_cfg": SceneEntityCfg("h2_contacts", body_names=".*_ankle_pitch_link"), + "command_name": "base_velocity", + "yaw_std": 0.25, + }, + ) + no_fly_h2 = RewTerm( + func=custom_mdp.no_fly, + weight=-1.0, + params={"sensor_cfg": SceneEntityCfg("h2_contacts", body_names=".*_ankle_pitch_link")}, + ) + flat_orientation_l2_h2 = RewTerm( + func=custom_mdp.flat_orientation_l2, weight=-2.5, params={"asset_cfg": SceneEntityCfg("h2")} + ) + feet_slide_h2 = RewTerm( + func=custom_mdp.feet_slide, + weight=-0.25, + params={ + "sensor_cfg": SceneEntityCfg("h2_contacts", body_names=".*_ankle_pitch_link"), + "asset_cfg": SceneEntityCfg("h2", body_names=".*_ankle_pitch_link"), + }, + ) + dof_pos_limits_h2 = RewTerm( + func=custom_mdp.joint_pos_limits, + weight=-1.0, + params={"asset_cfg": SceneEntityCfg("h2", joint_names=[".*_ankle_.*", ".*_knee_joint", "waist_pitch_joint"])}, + ) + termination_penalty_h2 = RewTerm( + func=custom_mdp.is_terminated, weight=-200.0, params={"asset_cfg": SceneEntityCfg("h2")} + ) + joint_deviation_hip_roll_h2 = RewTerm( + func=custom_mdp.joint_deviation_l1, + weight=-1.5, + params={"asset_cfg": SceneEntityCfg("h2", joint_names=".*_hip_roll_joint")}, + ) + joint_deviation_hip_yaw_h2 = RewTerm( + func=custom_mdp.joint_deviation_l1, + weight=-0.25, + params={"asset_cfg": SceneEntityCfg("h2", joint_names=".*_hip_yaw_joint")}, + ) + joint_deviation_waist_roll_h2 = RewTerm( + func=custom_mdp.joint_deviation_l1, + weight=-1.0, + params={"asset_cfg": SceneEntityCfg("h2", joint_names="waist_roll_joint")}, + ) + # Penalize waist pitch to prevent laying back or pitching forward + joint_deviation_waist_pitch_h2 = RewTerm( + func=custom_mdp.joint_deviation_l1, + weight=-2.0, + params={"asset_cfg": SceneEntityCfg("h2", joint_names="waist_pitch_joint")}, + ) + joint_deviation_waist_yaw_h2 = RewTerm( + func=custom_mdp.joint_deviation_l1, + weight=-0.2, + params={"asset_cfg": SceneEntityCfg("h2", joint_names="waist_yaw_joint")}, + ) + joint_deviation_shoulder_roll_h2 = RewTerm( + func=custom_mdp.joint_deviation_l1, + weight=-0.2, + params={"asset_cfg": SceneEntityCfg("h2", joint_names=".*_shoulder_roll_joint")}, + ) + # Allow natural sagittal arm swing during locomotion + joint_deviation_shoulder_pitch_h2 = RewTerm( + func=custom_mdp.joint_deviation_l1, + weight=-0.01, + params={"asset_cfg": SceneEntityCfg("h2", joint_names=".*_shoulder_pitch_joint")}, + ) + joint_deviation_arms_h2 = RewTerm( + func=custom_mdp.joint_deviation_l1, + weight=-0.1, + params={ + "asset_cfg": SceneEntityCfg( + "h2", + joint_names=[ + ".*_shoulder_yaw_joint", + ".*_elbow_joint", + ], + ) + }, + ) + joint_deviation_wrists_h2 = RewTerm( + func=custom_mdp.joint_deviation_l1, + weight=-0.1, + params={"asset_cfg": SceneEntityCfg("h2", joint_names=".*_wrist_.*")}, + ) + joint_deviation_head_h2 = RewTerm( + func=custom_mdp.joint_deviation_l1, + weight=-0.2, + params={"asset_cfg": SceneEntityCfg("h2", joint_names="head_.*")}, + ) + @configclass class HeterogeneousHumanoidVelocityEnvCfg(DirectRLEnvCfg): """Base Configuration for heterogeneous-humanoid velocity-tracking environment.""" - humanoids: list[str] = ["cassie", "digit", "g1", "h1"] + humanoids: list[str] = ["cassie", "digit", "g1", "h1", "h2"] episode_length_s = 20.0 decimation = 4 @@ -551,6 +724,7 @@ class HeterogeneousHumanoidVelocityEnvCfg(DirectRLEnvCfg): action_scale_digit: float = 0.5 action_scale_g1: float = 0.25 action_scale_h1: float = 0.5 + action_scale_h2: float = 0.25 action_space = gym.spaces.Box(low=-float("inf"), high=float("inf"), shape=(num_actions,)) @@ -587,6 +761,7 @@ class HeterogeneousHumanoidVelocityEnvCfg(DirectRLEnvCfg): reset_base_vel_range_digit: tuple = (-0.5, 0.5) reset_base_vel_range_g1: tuple = (0.0, 0.0) reset_base_vel_range_h1: tuple = (0.0, 0.0) + reset_base_vel_range_h2: tuple = (0.0, 0.0) reset_joint_pos_mode_cassie: str = "scale" reset_joint_pos_range_cassie: tuple = (1.0, 1.0) @@ -600,6 +775,9 @@ class HeterogeneousHumanoidVelocityEnvCfg(DirectRLEnvCfg): reset_joint_pos_mode_h1: str = "scale" reset_joint_pos_range_h1: tuple = (1.0, 1.0) + reset_joint_pos_mode_h2: str = "scale" + reset_joint_pos_range_h2: tuple = (1.0, 1.0) + resampling_time_range: tuple = (10.0, 10.0) standing_probability: float = 0.1 heading_control_stiffness: float = 0.5 @@ -662,6 +840,18 @@ class HeterogeneousHumanoidVelocityEnvCfg(DirectRLEnvCfg): "heading": (-math.pi, math.pi), } + # -- H2 + resampling_time_range_h2: tuple = (10.0, 10.0) + standing_probability_h2: float = 0.02 + randomize_base_mass_h2: bool = False + push_robots_h2: bool = False + command_ranges_h2: dict = { + "lin_vel_x": (0.0, 1.0), + "lin_vel_y": (0.0, 0.0), + "ang_vel_z": (-1.0, 1.0), + "heading": (-math.pi, math.pi), + } + rewards: RewardsCfg = RewardsCfg() diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/hetero_humanoid/robot_configs.py b/source/isaaclab_tasks/isaaclab_tasks/direct/hetero_humanoid/robot_configs.py index fe6397c83..6fde3ecdd 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/hetero_humanoid/robot_configs.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/hetero_humanoid/robot_configs.py @@ -10,8 +10,9 @@ appended after the canonical legs and discovered automatically at runtime. """ +from __future__ import annotations + from dataclasses import dataclass, field -from typing import Dict, List @dataclass @@ -25,7 +26,7 @@ class RobotJointConfig: # 15-19: Left Arm (Shoulder Pitch, Roll, Yaw, Elbow Pitch, Roll) # 20-24: Right Arm (Shoulder Pitch, Roll, Yaw, Elbow Pitch, Roll) # 25+: Unmapped joints (Fingers, etc) will be appended dynamically - canonical_joints: List[str | None] = field(default_factory=list) + canonical_joints: list[str | None] = field(default_factory=list) # --- Kinematic type --- # "bird" = Cassie/Digit (no sign flip needed) @@ -35,6 +36,8 @@ class RobotJointConfig: # --- Termination --- base_link: str = "torso" orientation_limit: float | None = None # radians; None = no orientation termination + illegal_contact_bodies: list[str] | str | None = None + min_root_height: float | None = None # meters; None = no root height termination # --- Action scale override (if different from default) --- action_scale: float = 0.5 @@ -44,7 +47,7 @@ class RobotJointConfig: # Per-Robot Configurations # ============================================================================= -ROBOT_CONFIGS: Dict[str, RobotJointConfig] = { +ROBOT_CONFIGS: dict[str, RobotJointConfig] = { "cassie": RobotJointConfig( canonical_joints=[ # 0-11: Legs @@ -116,7 +119,7 @@ class RobotJointConfig: ], kinematic_type="bird", base_link="torso_base", - orientation_limit=0.7, + orientation_limit=None, action_scale=0.5, ), "g1": RobotJointConfig( @@ -209,4 +212,53 @@ class RobotJointConfig: orientation_limit=None, action_scale=0.5, ), + "h2": RobotJointConfig( + canonical_joints=[ + # 0-11: Legs + "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", + # 12-14: Torso / Waist + "waist_yaw_joint", + "waist_roll_joint", + "waist_pitch_joint", + # 15-19: Left Arm + "left_shoulder_pitch_joint", + "left_shoulder_roll_joint", + "left_shoulder_yaw_joint", + "left_elbow_joint", + None, + # 20-24: Right Arm + "right_shoulder_pitch_joint", + "right_shoulder_roll_joint", + "right_shoulder_yaw_joint", + "right_elbow_joint", + None, + # 25-30: Wrists + "left_wrist_roll_joint", + "left_wrist_pitch_joint", + "left_wrist_yaw_joint", + "right_wrist_roll_joint", + "right_wrist_pitch_joint", + "right_wrist_yaw_joint", + # 31-32: Head + "head_yaw_joint", + "head_pitch_joint", + ], + kinematic_type="humanoid", + base_link="pelvis", + orientation_limit=None, + illegal_contact_bodies="torso_link", + min_root_height=None, + action_scale=0.25, + ), } diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/h2/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/h2/__init__.py new file mode 100644 index 000000000..4793748ed --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/h2/__init__.py @@ -0,0 +1,55 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import gymnasium as gym + +from . import agents + +## +# Register Gym environments. +## + +gym.register( + id="Isaac-Velocity-Rough-H2-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.rough_env_cfg:H2RoughEnvCfg", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:H2RoughPPORunnerCfg", + }, +) + + +gym.register( + id="Isaac-Velocity-Rough-H2-Play-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.rough_env_cfg:H2RoughEnvCfg_PLAY", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:H2RoughPPORunnerCfg", + }, +) + + +gym.register( + id="Isaac-Velocity-Flat-H2-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.flat_env_cfg:H2FlatEnvCfg", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:H2FlatPPORunnerCfg", + }, +) + + +gym.register( + id="Isaac-Velocity-Flat-H2-Play-v0", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.flat_env_cfg:H2FlatEnvCfg_PLAY", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:H2FlatPPORunnerCfg", + }, +) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/h2/agents/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/h2/agents/__init__.py new file mode 100644 index 000000000..460a30569 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/h2/agents/__init__.py @@ -0,0 +1,4 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/h2/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/h2/agents/rsl_rl_ppo_cfg.py new file mode 100644 index 000000000..2e3e5ea92 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/h2/agents/rsl_rl_ppo_cfg.py @@ -0,0 +1,49 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.utils import configclass + +from isaaclab_rl.rsl_rl import RslRlOnPolicyRunnerCfg, RslRlPpoActorCriticCfg, RslRlPpoAlgorithmCfg + + +@configclass +class H2RoughPPORunnerCfg(RslRlOnPolicyRunnerCfg): + num_steps_per_env = 24 + max_iterations = 3000 + save_interval = 50 + experiment_name = "h2_rough" + policy = RslRlPpoActorCriticCfg( + init_noise_std=1.0, + actor_obs_normalization=False, + critic_obs_normalization=False, + actor_hidden_dims=[512, 256, 128], + critic_hidden_dims=[512, 256, 128], + activation="elu", + ) + algorithm = RslRlPpoAlgorithmCfg( + value_loss_coef=1.0, + use_clipped_value_loss=True, + clip_param=0.2, + entropy_coef=0.01, + num_learning_epochs=5, + num_mini_batches=4, + learning_rate=1.0e-3, + schedule="adaptive", + gamma=0.99, + lam=0.95, + desired_kl=0.01, + max_grad_norm=1.0, + ) + + +@configclass +class H2FlatPPORunnerCfg(H2RoughPPORunnerCfg): + def __post_init__(self): + super().__post_init__() + + self.max_iterations = 1000 + self.experiment_name = "h2_flat" + self.policy.actor_hidden_dims = [256, 128, 128] + self.policy.critic_hidden_dims = [256, 128, 128] diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/h2/flat_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/h2/flat_env_cfg.py new file mode 100644 index 000000000..1b61cd46f --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/h2/flat_env_cfg.py @@ -0,0 +1,41 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from isaaclab.utils import configclass + +from .rough_env_cfg import H2RoughEnvCfg + + +@configclass +class H2FlatEnvCfg(H2RoughEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + # change terrain to flat + self.scene.terrain.terrain_type = "plane" + self.scene.terrain.terrain_generator = None + # no height scan + self.scene.height_scanner = None + self.observations.policy.height_scan = None + # no terrain curriculum + self.curriculum.terrain_levels = None + self.rewards.feet_air_time.weight = 1.5 + self.rewards.feet_air_time.params["threshold"] = 0.32 + + +class H2FlatEnvCfg_PLAY(H2FlatEnvCfg): + def __post_init__(self) -> None: + # post init of parent + super().__post_init__() + + # make a smaller scene for play + self.scene.num_envs = 50 + self.scene.env_spacing = 2.5 + # disable randomization for play + self.observations.policy.enable_corruption = False + # remove random pushing + self.events.base_external_force_torque = None + self.events.push_robot = None diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/h2/rough_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/h2/rough_env_cfg.py new file mode 100644 index 000000000..448736d5b --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/h2/rough_env_cfg.py @@ -0,0 +1,239 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + + +from isaaclab.managers import RewardTermCfg as RewTerm +from isaaclab.managers import SceneEntityCfg +from isaaclab.managers import TerminationTermCfg as DoneTerm +from isaaclab.utils import configclass + +import isaaclab_tasks.manager_based.locomotion.velocity.mdp as mdp +from isaaclab_tasks.manager_based.locomotion.velocity.velocity_env_cfg import ( + LocomotionVelocityRoughEnvCfg, + RewardsCfg, + TerminationsCfg, +) + +## +# Pre-defined configs +## +from isaaclab_assets import H2_MINIMAL_CFG # isort: skip + + +@configclass +class H2Rewards(RewardsCfg): + """Reward terms for the MDP.""" + + termination_penalty = RewTerm(func=mdp.is_terminated, weight=-200.0) + track_lin_vel_xy_exp = RewTerm( + func=mdp.track_lin_vel_xy_yaw_frame_exp, + weight=2.0, + params={"command_name": "base_velocity", "std": 0.5}, + ) + track_ang_vel_z_exp = RewTerm( + func=mdp.track_ang_vel_z_world_exp, weight=1.5, params={"command_name": "base_velocity", "std": 0.5} + ) + feet_air_time = RewTerm( + func=mdp.feet_air_time_positive_biped, + weight=1.5, + params={ + "command_name": "base_velocity", + "sensor_cfg": SceneEntityCfg("contact_forces", body_names=".*_ankle_pitch_link"), + "threshold": 0.32, + }, + ) + gait_symmetry = RewTerm( + func=mdp.feet_air_time_symmetry_biped, + weight=-15.0, + params={ + "command_name": "base_velocity", + "sensor_cfg": SceneEntityCfg("contact_forces", body_names=".*_ankle_pitch_link"), + "yaw_std": 0.25, + }, + ) + feet_slide = RewTerm( + func=mdp.feet_slide, + weight=-0.25, + params={ + "sensor_cfg": SceneEntityCfg("contact_forces", body_names=".*_ankle_pitch_link"), + "asset_cfg": SceneEntityCfg("robot", body_names=".*_ankle_pitch_link"), + }, + ) + + no_fly = RewTerm( + func=mdp.no_fly, + weight=-1.0, + params={"sensor_cfg": SceneEntityCfg("contact_forces", body_names=".*_ankle_pitch_link")}, + ) + + # Penalize ankle joint limits + dof_pos_limits = RewTerm( + func=mdp.joint_pos_limits, + weight=-1.0, + params={"asset_cfg": SceneEntityCfg("robot", joint_names=[".*_ankle_pitch_joint", ".*_ankle_roll_joint"])}, + ) + # Penalize deviation from default of the joints that are not essential for locomotion + joint_deviation_hip = RewTerm( + func=mdp.joint_deviation_l1, + weight=-0.25, + params={"asset_cfg": SceneEntityCfg("robot", joint_names=[".*_hip_yaw_joint", ".*_hip_roll_joint"])}, + ) + # Strongly penalize lateral waist roll to prevent upper body swinging left/right + joint_deviation_waist_roll = RewTerm( + func=mdp.joint_deviation_l1, + weight=-1.5, + params={"asset_cfg": SceneEntityCfg("robot", joint_names="waist_roll_joint")}, + ) + # Penalize waist pitch to prevent laying back or pitching forward + joint_deviation_waist_pitch = RewTerm( + func=mdp.joint_deviation_l1, + weight=-1.0, + params={"asset_cfg": SceneEntityCfg("robot", joint_names="waist_pitch_joint")}, + ) + joint_deviation_waist_yaw = RewTerm( + func=mdp.joint_deviation_l1, + weight=-0.2, + params={"asset_cfg": SceneEntityCfg("robot", joint_names="waist_yaw_joint")}, + ) + # Penalize arms flaring out sideways (shoulder roll) + joint_deviation_shoulder_roll = RewTerm( + func=mdp.joint_deviation_l1, + weight=-0.2, + params={"asset_cfg": SceneEntityCfg("robot", joint_names=".*_shoulder_roll_joint")}, + ) + # Light penalty on shoulder pitch to allow natural sagittal arm swing during locomotion + joint_deviation_shoulder_pitch = RewTerm( + func=mdp.joint_deviation_l1, + weight=-0.01, + params={"asset_cfg": SceneEntityCfg("robot", joint_names=".*_shoulder_pitch_joint")}, + ) + joint_deviation_arms = RewTerm( + func=mdp.joint_deviation_l1, + weight=-0.1, + params={ + "asset_cfg": SceneEntityCfg( + "robot", + joint_names=[ + ".*_shoulder_yaw_joint", + ".*_elbow_joint", + ], + ) + }, + ) + joint_deviation_wrists = RewTerm( + func=mdp.joint_deviation_l1, + weight=-0.1, + params={"asset_cfg": SceneEntityCfg("robot", joint_names=".*_wrist_.*")}, + ) + joint_deviation_head = RewTerm( + func=mdp.joint_deviation_l1, + weight=-0.2, + params={"asset_cfg": SceneEntityCfg("robot", joint_names="head_.*")}, + ) + + +@configclass +class H2TerminationsCfg(TerminationsCfg): + """Termination terms for the MDP.""" + + base_orientation = DoneTerm( + func=mdp.bad_orientation_safe, + params={"limit_angle": 0.7}, + ) + root_height = DoneTerm( + func=mdp.root_height_below_minimum, + params={"minimum_height": 0.65}, + ) + + +@configclass +class H2RoughEnvCfg(LocomotionVelocityRoughEnvCfg): + rewards: H2Rewards = H2Rewards() + terminations: H2TerminationsCfg = H2TerminationsCfg() + + def __post_init__(self): + # post init of parent + super().__post_init__() + # Scene + self.scene.robot = H2_MINIMAL_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + if self.scene.height_scanner: + self.scene.height_scanner.prim_path = "{ENV_REGEX_NS}/Robot/pelvis" + + # Randomization + self.events.push_robot = None + self.events.add_base_mass = None + self.events.reset_robot_joints.params["position_range"] = (1.0, 1.0) + self.events.base_external_force_torque.params["asset_cfg"].body_names = [".*pelvis"] + self.events.reset_base.params = { + "pose_range": {"x": (-0.5, 0.5), "y": (-0.5, 0.5), "yaw": (-3.14, 3.14)}, + "velocity_range": { + "x": (0.0, 0.0), + "y": (0.0, 0.0), + "z": (0.0, 0.0), + "roll": (0.0, 0.0), + "pitch": (0.0, 0.0), + "yaw": (0.0, 0.0), + }, + } + self.events.base_com = None + + # Rewards + self.rewards.lin_vel_z_l2.weight = -2.0 + self.rewards.ang_vel_xy_l2.weight = -0.2 + self.rewards.undesired_contacts = None + self.rewards.flat_orientation_l2.weight = -1.5 + self.rewards.action_rate_l2.weight = -0.005 + self.rewards.dof_acc_l2.weight = -2.5e-7 + self.rewards.dof_acc_l2.params["asset_cfg"] = SceneEntityCfg( + "robot", joint_names=[".*_hip_.*", ".*_knee_joint"] + ) + self.rewards.dof_torques_l2.weight = -1.5e-7 + self.rewards.dof_torques_l2.params["asset_cfg"] = SceneEntityCfg( + "robot", joint_names=[".*_hip_.*", ".*_knee_joint", ".*_ankle_.*"] + ) + + # Commands + self.commands.base_velocity.ranges.lin_vel_x = (0.0, 1.0) + self.commands.base_velocity.ranges.lin_vel_y = (0.0, 0.0) + self.commands.base_velocity.ranges.ang_vel_z = (-1.0, 1.0) + + # Terminations + self.terminations.base_contact.params["sensor_cfg"].body_names = [ + ".*pelvis", + ".*torso.*", + ".*_knee_.*", + ".*_elbow_.*", + ".*_hand_.*", + ".*waist.*", + ] + + +@configclass +class H2RoughEnvCfg_PLAY(H2RoughEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + # make a smaller scene for play + self.scene.num_envs = 50 + self.scene.env_spacing = 2.5 + self.episode_length_s = 40.0 + # spawn the robot randomly in the grid (instead of their terrain levels) + self.scene.terrain.max_init_terrain_level = None + # reduce the number of terrains to save memory + if self.scene.terrain.terrain_generator is not None: + self.scene.terrain.terrain_generator.num_rows = 5 + self.scene.terrain.terrain_generator.num_cols = 5 + self.scene.terrain.terrain_generator.curriculum = False + + self.commands.base_velocity.ranges.lin_vel_x = (1.0, 1.0) + self.commands.base_velocity.ranges.lin_vel_y = (0.0, 0.0) + self.commands.base_velocity.ranges.ang_vel_z = (-1.0, 1.0) + self.commands.base_velocity.ranges.heading = (0.0, 0.0) + # disable randomization for play + self.observations.policy.enable_corruption = False + # remove random pushing + self.events.base_external_force_torque = None + self.events.push_robot = None diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/mdp/rewards.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/mdp/rewards.py index f804aa688..29e76dd34 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/mdp/rewards.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/mdp/rewards.py @@ -68,6 +68,33 @@ def feet_air_time_positive_biped(env, command_name: str, threshold: float, senso return reward +def feet_air_time_symmetry_biped( + env: ManagerBasedRLEnv, command_name: str, sensor_cfg: SceneEntityCfg, yaw_std: float = 0.25 +) -> torch.Tensor: + """Penalize asymmetry in feet air time for bipeds during straight walking. + + This function penalizes differences in air time between the left and right foot upon landing, + preventing asymmetric strides (limping). It is smoothly gated by turning commands so that + differential strides needed for sharp turns are not penalized. + """ + contact_sensor: ContactSensor = env.scene.sensors[sensor_cfg.name] + commands = env.command_manager.get_command(command_name) + yaw_cmd = commands[:, 2] + straight_gating = torch.exp(-torch.square(yaw_cmd) / (2 * (yaw_std**2))) + is_moving = torch.norm(commands[:, :2], dim=1) > 0.1 + + feet_ids = sensor_cfg.body_ids + first_contact = contact_sensor.compute_first_contact(env.step_dt)[:, feet_ids] + any_first_contact = torch.any(first_contact, dim=1) + + last_air_time = contact_sensor.data.last_air_time[:, feet_ids] + both_feet_stepped = (last_air_time[:, 0] > 0.0) & (last_air_time[:, 1] > 0.0) + air_diff = torch.abs(last_air_time[:, 0] - last_air_time[:, 1]) + + penalty = air_diff * any_first_contact * both_feet_stepped * straight_gating * is_moving + return penalty + + def feet_slide(env, sensor_cfg: SceneEntityCfg, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot")) -> torch.Tensor: """Penalize feet sliding. @@ -117,3 +144,11 @@ def stand_still_joint_deviation_l1( command = env.command_manager.get_command(command_name) # Penalize motion when command is nearly zero. return mdp.joint_deviation_l1(env, asset_cfg) * (torch.norm(command[:, :2], dim=1) < command_threshold) + + +def no_fly(env: ManagerBasedRLEnv, sensor_cfg: SceneEntityCfg) -> torch.Tensor: + """Penalize when all specified feet are off the ground at the same time (flight phase).""" + contact_sensor: ContactSensor = env.scene.sensors[sensor_cfg.name] + contacts = contact_sensor.data.net_forces_w_history[:, :, sensor_cfg.body_ids, :].norm(dim=-1).max(dim=1)[0] > 1.0 + has_contact = torch.any(contacts, dim=1) + return (~has_contact).float() diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/mdp/terminations.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/mdp/terminations.py index 6c037d01e..386347a36 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/mdp/terminations.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/mdp/terminations.py @@ -11,6 +11,7 @@ from __future__ import annotations +import math from typing import TYPE_CHECKING import torch @@ -22,6 +23,19 @@ from isaaclab.envs import ManagerBasedRLEnv +def bad_orientation_safe( + env: ManagerBasedRLEnv, limit_angle: float, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot") +) -> torch.Tensor: + """Terminate when the asset's orientation is too far from upright. + + Uses cosine comparison directly to prevent NaN floating-point issues in torch.acos. + When upright, -projected_gravity_b[:, 2] == 1.0. When tilted > limit_angle, it drops < cos(limit_angle). + """ + asset: RigidObject = env.scene[asset_cfg.name] + cos_limit = math.cos(limit_angle) + return -asset.data.projected_gravity_b[:, 2] < cos_limit + + def terrain_out_of_bounds( env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), distance_buffer: float = 3.0 ) -> torch.Tensor: