From 8e64dcbe66c2f2a40cb0cc273b8406b72499e3c1 Mon Sep 17 00:00:00 2001 From: henry Date: Wed, 21 Oct 2026 19:12:58 +0800 Subject: [PATCH 01/18] Add G1 visual locomotion training and deployment pipeline --- ...training_a_g1_visual_locomotion_policy.rst | 241 +++++++++ docs/source/policy_deployment/index.rst | 1 + scripts/deploy/g1_deploy.py | 507 ++++++++++++++++++ .../changelog.d/g1-depth-pipeline.fixed.rst | 2 + .../entrypoints/backends/export_rsl_rl.py | 5 +- .../entrypoints/backends/train_rsl_rl.py | 2 +- .../export/leapp_initialized_checkpoints.py | 10 +- .../test/export/test_leapp_export_flow.py | 22 +- source/isaaclab_rl/test/test_entrypoints.py | 10 + .../changelog.d/g1-depth-pipeline.fixed.rst | 2 + .../changelog.d/g1-shared-mdp.changed.rst | 6 + .../core/velocity/config/g1/__init__.py | 21 + .../velocity/config/g1/agents/distillation.py | 102 ++++ .../config/g1/agents/rsl_rl_ppo_cfg.py | 66 ++- .../core/velocity/config/g1/rough_env_cfg.py | 302 ++++++++++- .../core/velocity/mdp/__init__.pyi | 19 +- .../core/velocity/mdp/curriculums.py | 10 +- .../core/velocity/mdp/rewards.py | 108 +++- .../core/velocity/mdp/symmetry/g1.py | 192 +++++++ .../core/velocity/mdp/terminations.py | 14 + .../test/core/test_g1_distillation.py | 241 +++++++++ 21 files changed, 1866 insertions(+), 17 deletions(-) create mode 100644 docs/source/policy_deployment/07_g1_sim2real/training_a_g1_visual_locomotion_policy.rst create mode 100644 scripts/deploy/g1_deploy.py create mode 100644 source/isaaclab_rl/changelog.d/g1-depth-pipeline.fixed.rst create mode 100644 source/isaaclab_tasks/changelog.d/g1-depth-pipeline.fixed.rst create mode 100644 source/isaaclab_tasks/changelog.d/g1-shared-mdp.changed.rst create mode 100644 source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/agents/distillation.py create mode 100644 source/isaaclab_tasks/isaaclab_tasks/core/velocity/mdp/symmetry/g1.py create mode 100644 source/isaaclab_tasks/test/core/test_g1_distillation.py diff --git a/docs/source/policy_deployment/07_g1_sim2real/training_a_g1_visual_locomotion_policy.rst b/docs/source/policy_deployment/07_g1_sim2real/training_a_g1_visual_locomotion_policy.rst new file mode 100644 index 000000000000..648368da41da --- /dev/null +++ b/docs/source/policy_deployment/07_g1_sim2real/training_a_g1_visual_locomotion_policy.rst @@ -0,0 +1,241 @@ +Training a G1 Visual Locomotion Policy for Real Robot Deployment +================================================================ + +.. currentmodule:: isaaclab + +This guide follows one policy from training to a walking robot: a teacher trained with PPO, a +student distilled from it that sees through a chest-mounted depth camera, a LEAPP export, and a +control loop that drives a physical Unitree G1 over DDS. + +:doc:`../05_leapp/deploying_exported_policies_with_leapp` ends at +:class:`~envs.LeappDeploymentEnv`, which runs an exported policy back in simulation. This guide +continues from there onto hardware. The inference stage is unchanged — it is the same +``leapp.InferenceManager`` — and what this guide adds is the robot at both ends of it. + +.. note:: + + The pipeline below is specific to a 29-DoF Unitree G1 with a RealSense D435i on the chest. The + structure generalizes, but every number in it was measured on that robot. + +.. code-block:: text + + 1. train Isaac-Velocity-Rough-G1-Teacher + 2. distill Isaac-Velocity-Rough-G1-Depth-Student + 3. export pipeline .yaml + .onnx + _initial_values.safetensors + 4. deploy scripts/deploy/g1_deploy.py + + +Prerequisites +------------- + +In addition to the ``leapp`` extra required by export, hardware deployment needs +``unitree_sdk2py`` with ``cyclonedds==0.10.2``, a wired connection to the robot's subnet, and a +depth publisher running on the robot's onboard computer. + +.. note:: + + The ``cyclonedds`` pin is load-bearing. A newer CycloneDDS enforces XTypes type consistency at + match time, and the SDK's Python IDL hashes differently from the robot's C++ IDL. Readers then + never match. This passes every simulation test, because both ends share the Python serializer, + and fails only against hardware. + + +Training the Teacher +-------------------- + +The teacher reads a terrain height scan and its own base linear velocity. A real G1 publishes +neither, which is why the second stage exists. + +.. code-block:: bash + + uv run isaaclab train --rl_library rsl_rl \ + --task Isaac-Velocity-Rough-G1-Teacher + +The teacher runs on ``g1_29dof.usd``, not on the ``G1_MINIMAL_CFG`` the stock +``Isaac-Velocity-Rough-G1`` task uses. That asset comes from a superseded URDF whose waist is a +single ``torso_joint`` and whose forearm is ``elbow_pitch``/``elbow_roll``; the hardware has a +three-axis waist, one elbow and three wrist joints, and none of those names exist on the old +model. A reward term whose joint regex matches nothing is silently inert, so the mismatch does not +raise -- it just trains a different robot. ``G1RoughTeacherEnvCfg`` swaps the asset and respells +the joint names of every reward that referred to the old ones. + + +The default configuration uses 4096 environments, 6000 PPO iterations, Newton mesh terrain +with 5–35 cm stairs, per-joint action scales and left-right data augmentation. Simulation torque +limits include 20 N·m at the ankles and are configured separately from hardware limits. +Use ``--seed`` to set the random seed. + + +Distilling the Depth Student +---------------------------- + +.. code-block:: bash + + uv run isaaclab train --rl_library rsl_rl \ + --task Isaac-Velocity-Rough-G1-Depth-Student \ + --checkpoint + +The student runs in the teacher's own environment and drops the privileged observation terms. In +their place it reads five frames of proprioception and three frames of 64x38 depth, rendered from +a camera placed where the hardware's RealSense sits. + +``obs_groups`` is where the two contracts meet: ``student`` reads the deployable groups and +``teacher`` the privileged one. ``rsl_rl`` splits observation groups by rank, sends the rank-4 +depth group through a convolutional encoder, and concatenates the latent with the 1D groups before +the MLP. + + +Distillation starts a new student for 24000 iterations. The teacher observation is noise-free; +the student normalizes five frames per proprioceptive term. A second, reflected camera supplies +mirror augmentation during training only. PPO checkpoints load only the teacher, without restoring +student parameters, optimizer state or iteration. Evaluate checkpoints across the target terrains +to select a policy for deployment. + + +Exporting the Pipeline +---------------------- + +.. code-block:: bash + + uv run --extra leapp isaaclab leapp export --rl_library rsl_rl \ + --task Isaac-Velocity-Rough-G1-Depth-Student \ + physics=newton_mjwarp + +The export declares six inputs and three outputs: + +.. code-block:: text + + inputs robot_joint_pos robot_joint_vel + robot_root_quat_w robot_root_ang_vel_b + base_velocity depth_camera_output_distance_to_image_plane + + outputs joint_pos joint_pos_kp_gains joint_pos_kd_gains + +It also declares fifteen feedback connections. The proprioception history, the depth stack, the +previous action and the observation normalization all live inside the graph, and the +``_initial_values.safetensors`` file holds their reset state. Nothing outside the graph recomputes +any of it. + +The gains are graph outputs, not configuration: the deployment writes the Kp and Kd the policy +asks for on each step. + + +Running on the Robot +-------------------- + +Resolving the Connections Against Hardware +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +:class:`~envs.LeappDeploymentEnv` resolves each ``isaaclab_connection`` in the exported YAML +against a simulated scene. Hardware deployment resolves the same connections against the robot: + +.. list-table:: + :header-rows: 1 + :widths: 44 26 30 + + * - Connection + - Simulation + - Hardware + * - ``state:robot:joint_pos`` + - articulation + - ``rt/lowstate`` + * - ``state:robot:joint_vel`` + - articulation + - ``rt/lowstate`` + * - ``state:robot:root_quat_w`` + - scene root + - IMU + * - ``state:robot:root_ang_vel_b`` + - scene root + - IMU gyroscope + * - ``state:depth_camera:output:distance_to_image_plane`` + - rendered + - UDP depth stream + * - ``command:base_velocity`` + - ``CommandManager`` + - keyboard + * - ``write:robot:set_joint_position_target_index`` + - articulation + - ``rt/lowcmd`` + * - ``write:robot:write_joint_stiffness_to_sim_index`` + - articulation + - ``rt/lowcmd`` Kp + * - ``write:robot:write_joint_damping_to_sim_index`` + - articulation + - ``rt/lowcmd`` Kd + +The Control Loop +^^^^^^^^^^^^^^^^ + +.. literalinclude:: ../../../../scripts/deploy/g1_deploy.py + :language: python + :start-at: while teleop.running: + :end-at: link.wait(CONTROL_DT) + :dedent: 8 + +Two details about the ends of this loop. + +The graph writes **every joint of the articulation**, fingers included, so its 43 outputs are +matched to the 29 motors ``rt/lowcmd`` carries by name, using the ``element_names`` the export +records. The two orders agree for the first 22 joints and then diverge, which is the kind of +mismatch that produces a walking robot with the wrong arm. + +The ramp target is a fixed constant, not something read back from the policy. The operator squares +the robot up in that pose and it has to be the same on every run. + +Conversions +^^^^^^^^^^^ + +Four conversions sit between the robot and the graph. Each is silent when wrong: the arrays keep +their shapes and the robot simply walks badly. + +**Quaternion order.** The IMU publishes ``(w, x, y, z)`` and the graph expects ``(x, y, z, w)``. + +**IMU mounting offset.** Subtracted before the quaternion reaches the graph. + +**Invalid depth.** A real camera returns zero where it got no return. The graph reads zero as a +very close surface, so an unprocessed frame plants obstacles at the robot's feet. Pixels with no +return are filled with the clip range instead. + +**Field of view.** The publisher crops to the trained field of view before downsampling. Resizing +the whole frame compresses the horizontal axis by about 3%, and the array is 64x38 either way, so +nothing downstream can detect it. + +Starting the Camera +^^^^^^^^^^^^^^^^^^^ + +The camera runs on the robot's onboard computer, which carries the RealSense drivers. It crops, +downsamples and publishes 64x38 frames as unsigned 16-bit millimetres over UDP. + +.. code-block:: bash + + ssh unitree@192.168.123.164 \ + '/usr/bin/python3 ~/depth_publisher.py --host ' + +Starting the Policy +^^^^^^^^^^^^^^^^^^^ + +Put the robot in debug mode and support it on a stand before starting. + +.. code-block:: bash + + uv run --extra leapp python scripts/deploy/g1_deploy.py \ + --pipeline \ + --interface --domain_id 0 + +The loop takes control in two steps. Typing ``go`` ramps the robot to the policy's start pose over +three seconds and then holds it. Typing ``go2`` engages the policy three seconds later. ``rt/lowcmd`` +keeps flowing throughout, so there is no watchdog gap between the two. + +.. warning:: + + Keep the robot supported until the policy is engaged. The start pose is held with PD control and + no policy, and it is not statically stable at these gains. + + +Further Reading +--------------- + +- :doc:`../05_leapp/deploying_exported_policies_with_leapp` +- `LEAPP documentation `__ +- :class:`~envs.LeappDeploymentEnv` API reference diff --git a/docs/source/policy_deployment/index.rst b/docs/source/policy_deployment/index.rst index e822836dfd6f..549339e76e16 100644 --- a/docs/source/policy_deployment/index.rst +++ b/docs/source/policy_deployment/index.rst @@ -13,3 +13,4 @@ The guides below cover task-specific configuration and deployment tools. 04_reach/reach_policy 05_leapp/exporting_policies_with_leapp 06_so101_typing/so101_typing_policy + 07_g1_sim2real/training_a_g1_visual_locomotion_policy diff --git a/scripts/deploy/g1_deploy.py b/scripts/deploy/g1_deploy.py new file mode 100644 index 000000000000..60632264086e --- /dev/null +++ b/scripts/deploy/g1_deploy.py @@ -0,0 +1,507 @@ +# 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 + +"""Run a LEAPP-exported policy on a physical Unitree G1. + +Inference is the official ``leapp.InferenceManager``. This script supplies the two ends the graph +cannot reach by itself: the robot's state and camera on the way in, its motors on the way out. No +simulator is launched and Isaac Sim is never imported. + +Needs ``unitree_sdk2py`` with ``cyclonedds==0.10.2``, and a depth publisher on the robot's onboard +computer sending 64x38 frames as unsigned 16-bit millimetres over UDP. +""" + +from __future__ import annotations + +import argparse +import math +import re +import select +import socket +import struct +import sys +import threading +import time + +import numpy as np + +CONTROL_DT = 0.02 +"""Control period [s]. The exported graph carries its history at this rate.""" + +DEPTH_SHAPE = (38, 64) +"""Depth frames the publisher sends and the graph expects, as ``(height, width)``.""" + +DEPTH_MAX_RANGE = 3.0 +"""Range the graph clips at [m], and what a pixel with no return is filled with.""" + +DEPTH_MAX_AGE = 0.2 +"""Refuse to step on a frame older than this [s]. Ten control periods.""" + +FALL_GRAVITY_Z = -0.7 +"""Stop above this projected gravity. Upright is -1.0; this is about 45 degrees over.""" + +MOTOR_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", + "waist_yaw_joint", + "waist_roll_joint", + "waist_pitch_joint", + "left_shoulder_pitch_joint", + "left_shoulder_roll_joint", + "left_shoulder_yaw_joint", + "left_elbow_joint", + "left_wrist_roll_joint", + "left_wrist_pitch_joint", + "left_wrist_yaw_joint", + "right_shoulder_pitch_joint", + "right_shoulder_roll_joint", + "right_shoulder_yaw_joint", + "right_elbow_joint", + "right_wrist_roll_joint", + "right_wrist_pitch_joint", + "right_wrist_yaw_joint", +) +"""The 29 body joints ``rt/lowcmd`` carries, in its own order. + +The exported graph reads and writes every joint of the articulation, fingers included, so its +tensors are matched to these by name rather than by position -- the two orders agree for the first +22 joints and then diverge, which is the kind of mismatch that drives an arm from a hand's target. +""" + +DEFAULT_JOINT_POS = { + ".*_hip_pitch_joint": -0.20, + ".*_knee_joint": 0.42, + ".*_ankle_pitch_joint": -0.23, + ".*_elbow_joint": 0.87, + "left_shoulder_pitch_joint": 0.35, + "left_shoulder_roll_joint": 0.16, + "right_shoulder_pitch_joint": 0.35, + "right_shoulder_roll_joint": -0.16, +} +"""The pose the ramp ends at [rad], by joint-name pattern; joints not listed go to zero. + +This is the task's own initial state, and it is fixed on purpose. The operator squares the robot up +in this pose, so it has to be the same on every run. +""" + +BUTTON_BITS = {"R1": 0, "L1": 1, "start": 2, "select": 3, "R2": 4, "L2": 5, "A": 8, "B": 9} +"""Bits of the 16-bit button mask at offset 2 of ``wireless_remote``, as Unitree's own parser reads +them.""" + +ABORT_COMBOS = (("L2", "B"), ("L1", "A"), ("select",)) +"""Combinations that stop the run. + +**The G1 has no hardware emergency stop.** Unitree's documented stop is a remote combination handled +by the *factory* controller, and once this program owns ``rt/lowcmd`` nothing guarantees that still +applies. So the combination is decoded here, out of every ``rt/lowstate``, and the damping is this +program's own. + +The mapping changed at Motion Control 8.2.0.0: damping is ``L2 + B`` from 8.2.0.0 and ``L1 + A`` +before it, where ``L1 + B`` is instead *zero torque* and drops a hanging robot. All three are +accepted, because guessing the firmware wrong must not be what decides whether the stop works. +""" + + +class OperatorAbort(RuntimeError): + """The operator pressed an abort combination on the remote.""" + + +def abort_pressed(wireless_remote) -> str | None: + """Name the abort combination being held, or ``None``.""" + data = bytes(bytearray(wireless_remote)) + if len(data) < 4 or not any(data): + return None + mask = struct.unpack_from(" None: + self._sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + self._sock.bind(("", port)) + self._sock.settimeout(0.2) + self._lock = threading.Lock() + self._frame: np.ndarray | None = None + self._at = 0.0 + threading.Thread(target=self._receive, daemon=True).start() + + def _receive(self) -> None: + count = DEPTH_SHAPE[0] * DEPTH_SHAPE[1] + while True: + try: + payload = self._sock.recv(1 << 16) + except TimeoutError: + continue + if len(payload) < count * 2: + continue + raw = np.frombuffer(payload, dtype=np.uint16, count=count, offset=len(payload) - count * 2) + frame = raw.reshape(DEPTH_SHAPE).astype(np.float32) / 1000.0 + # A real camera reports no return as zero. The graph reads zero as a surface against the + # lens, so the clip range is what says "nothing within range" instead. + frame = np.where(np.isfinite(frame) & (frame > 0.0), frame, DEPTH_MAX_RANGE) + with self._lock: + self._frame, self._at = np.clip(frame, 0.0, DEPTH_MAX_RANGE), time.monotonic() + + def wait(self, timeout: float = 20.0) -> None: + """Block until the first frame arrives, so a missing camera stops the run before the ramp.""" + deadline = time.monotonic() + timeout + while self.latest() is None and time.monotonic() < deadline: + time.sleep(0.02) + if self.latest() is None: + raise TimeoutError(f"no depth frame in {timeout:.0f}s; is the publisher running?") + + def latest(self) -> np.ndarray | None: + """Newest frame [m], or ``None`` when it is missing or stale.""" + with self._lock: + if self._frame is None or time.monotonic() - self._at > DEPTH_MAX_AGE: + return None + return self._frame + + +class Teleop: + """Forward speed and heading from single keypresses, without waiting for Enter.""" + + HELP = "w/s forward +/- q/e turn left/right space stop x quit" + SPEED_STEP = 0.1 + HEADING_STEP = math.radians(15.0) + + def __init__(self) -> None: + self.vx = 0.0 + self.heading = 0.0 + self.running = True + + def command(self, quat_xyzw: np.ndarray) -> np.ndarray: + """``(vx, vy, yaw_rate)`` [m/s, m/s, rad/s], with the yaw rate steering toward a heading. + + The command's third element is a rate, so the heading target is turned into one here against + the measured orientation. Steering a heading rather than a rate keeps a teleoperated run on + the distribution a fixed command trains on. + """ + key = sys.stdin.read(1) if select.select([sys.stdin], [], [], 0.0)[0] else None + if key == "w": + self.vx = min(1.0, self.vx + self.SPEED_STEP) + elif key == "s": + self.vx = max(0.0, self.vx - self.SPEED_STEP) + elif key == "q": + self.heading += self.HEADING_STEP + elif key == "e": + self.heading -= self.HEADING_STEP + elif key == " ": + self.vx = 0.0 + elif key == "x": + self.running = False + if key: + print(f"[cmd] vx={self.vx:+.2f} m/s heading={math.degrees(self.heading):+.0f} deg") + + x, y, z, w = quat_xyzw + yaw = math.atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z)) + error = (self.heading - yaw + math.pi) % (2.0 * math.pi) - math.pi + return np.array([self.vx, 0.0, np.clip(error, -1.0, 1.0)], dtype=np.float32) + + +class G1Link: + """``rt/lowstate`` in, ``rt/lowcmd`` out, over the Unitree SDK.""" + + def __init__(self, domain_id: int, interface: str) -> None: + from unitree_sdk2py.core.channel import ChannelFactoryInitialize, ChannelPublisher, ChannelSubscriber + from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowCmd_, LowState_ + from unitree_sdk2py.utils.crc import CRC + + ChannelFactoryInitialize(domain_id, interface) + self.state = None + ChannelSubscriber("rt/lowstate", LowState_).Init(self._on_state, 10) + self._publisher = ChannelPublisher("rt/lowcmd", LowCmd_) + self._publisher.Init() + self._command = LowCmd_() + self._crc = CRC() + self._next = time.monotonic() + while self.state is None: + time.sleep(0.02) + + def _on_state(self, message) -> None: + self.state = message + + def read(self) -> dict: + """Joint state and IMU, in the frames the graph expects.""" + state = self.state + motors = state.motor_state + quat = np.asarray(state.imu_state.quaternion[:4], dtype=np.float32) + return { + "joint_pos": np.array([m.q for m in motors[: len(MOTOR_NAMES)]], dtype=np.float32), + "joint_vel": np.array([m.dq for m in motors[: len(MOTOR_NAMES)]], dtype=np.float32), + # The IMU publishes (w, x, y, z); the graph declares (x, y, z, w). + "root_quat_xyzw": quat[[1, 2, 3, 0]], + "ang_vel": np.asarray(state.imu_state.gyroscope[:3], dtype=np.float32), + # Gravity in the pelvis frame; -1.0 upright. + "gravity_z": -float(1.0 - 2.0 * (quat[1] ** 2 + quat[2] ** 2)), + "mode_machine": int(state.mode_machine), + } + + def check_abort(self) -> None: + """Raise if the operator is holding an abort combination. + + Called from every loop that commands the robot -- the ramp, the hold and the policy -- so + the stop works during the motion that most needs it rather than only once walking starts. + """ + combo = abort_pressed(self.state.wireless_remote) if self.state is not None else None + if combo is not None: + raise OperatorAbort(f"operator pressed {combo} on the remote") + + def write(self, joint_pos, kp, kd) -> None: + """Send one ``rt/lowcmd``. The gains are the policy's own outputs, not configuration.""" + for i in range(len(MOTOR_NAMES)): + motor = self._command.motor_cmd[i] + motor.mode, motor.q, motor.dq, motor.tau = 1, float(joint_pos[i]), 0.0, 0.0 + motor.kp, motor.kd = float(kp[i]), float(kd[i]) + self._command.mode_machine = self.state.mode_machine + self._command.crc = self._crc.Crc(self._command) + self._publisher.Write(self._command) + + def damp(self, kd, seconds: float = 1.5) -> None: + """Bleed off energy with no stiffness, then stop commanding. Every exit goes through here.""" + print("[..] damping down") + zero = np.zeros(len(MOTOR_NAMES), dtype=np.float32) + for _ in range(int(seconds / CONTROL_DT)): + self.write(zero, zero, kd) + self.wait(CONTROL_DT) + + def wait(self, period: float) -> None: + """Sleep until the next control instant, without accumulating drift.""" + self._next += period + time.sleep(max(0.0, self._next - time.monotonic())) + + +class GraphPolicy: + """The exported graph, with the robot's arrays converted to and from its layout. + + The conversions are the point of this class, and each is silent when wrong -- the shapes still + match and the robot simply walks badly: + + * the graph reads and writes **all** 43 articulation joints, so the 29 motors are placed by + name and the fingers stay at their default pose and zero velocity; + * depth arrives from the camera rolled 180 degrees against what the graph was trained on; + * depth is a rank-4 ``(1, H, W, 1)`` tensor, not an image-shaped array. + """ + + def __init__(self, pipeline: str, depth_rotation: int) -> None: + import torch + import yaml + from leapp import InferenceManager + + self._torch = torch + self._rotate = depth_rotation == 180 + self.inference = InferenceManager(pipeline) + + with open(pipeline) as stream: + model = next(iter(yaml.safe_load(stream)["models"].values())) + # The export names its own tensors; the connection strings are how Isaac Lab says what each + # one carries, and are what the simulation deployment resolves against a scene. + self._ports = { + io["isaaclab_connection"]: io["name"] + for io in list(model["inputs"]) + list(model["outputs"]) + if "isaaclab_connection" in io + } + names = next(o for o in model["outputs"] if o["name"] == "joint_pos")["element_names"][0] + order = {name: i for i, name in enumerate(names)} + missing = [name for name in MOTOR_NAMES if name not in order] + if missing: + raise KeyError(f"the export does not drive {missing}") + self._motors = np.array([order[name] for name in MOTOR_NAMES], dtype=np.int64) + self._default = np.array( + [next((v for p, v in DEFAULT_JOINT_POS.items() if re.fullmatch(p, n)), 0.0) for n in names], + dtype=np.float32, + ) + self.default_pose = self._default[self._motors] + + # Warm the graph on synthetic inputs, before anything owns rt/lowcmd. This pays the first + # call's cost outside the control loop and leaves self.kp and self.kd holding the gains the + # ramp uses, which are the policy's own rather than numbers repeated here. + for _ in range(3): + self.step( + self.default_pose, + np.zeros(len(MOTOR_NAMES), np.float32), + np.array([0.0, 0.0, 0.0, 1.0], np.float32), + np.zeros(3, np.float32), + np.zeros(3, np.float32), + np.full(DEPTH_SHAPE, DEPTH_MAX_RANGE, np.float32), + ) + self.inference.reset() + + def step(self, joint_pos, joint_vel, quat_xyzw, ang_vel, command, depth): + """One inference step. Returns motor targets, Kp and Kd, each ``(29,)``.""" + q, dq = self._default.copy(), np.zeros_like(self._default) + q[self._motors], dq[self._motors] = joint_pos, joint_vel + frame = np.asarray(depth, dtype=np.float32) + if self._rotate: + frame = frame[::-1, ::-1] + + inputs = { + self._key(connection): self._torch.from_numpy(np.ascontiguousarray(value, np.float32)) + for connection, value in { + "state:robot:joint_pos": q[None], + "state:robot:joint_vel": dq[None], + "state:robot:root_quat_w": np.asarray(quat_xyzw, np.float32)[None], + "state:robot:root_ang_vel_b": np.asarray(ang_vel, np.float32)[None], + "command:base_velocity": np.asarray(command, np.float32)[None], + "state:depth_camera:output:distance_to_image_plane": frame[None, :, :, None], + }.items() + } + with self._torch.inference_mode(): + outputs = self.inference.run_policy(inputs) + + target = self._motors_of(outputs, "write:robot:set_joint_position_target_index") + self.kp = self._motors_of(outputs, "write:robot:write_joint_stiffness_to_sim_index") + self.kd = self._motors_of(outputs, "write:robot:write_joint_damping_to_sim_index") + # The one check worth making here: these three go straight into motor commands, and a NaN + # from a bad joint or IMU reading would be written to the robot as a target. + if not np.isfinite([target, self.kp, self.kd]).all(): + raise ValueError("the graph returned a non-finite target or gain") + return target, self.kp, self.kd + + def _key(self, connection: str) -> str: + """The graph's own name for a tensor, given the Isaac Lab connection it carries.""" + return self._ports[connection] + + def _motors_of(self, outputs: dict, connection: str) -> np.ndarray: + """One of the graph's 43-wide outputs, reduced to the 29 motors in ``rt/lowcmd`` order.""" + return outputs[self._key(connection)].detach().cpu().numpy().reshape(-1)[self._motors] + + +def take_lowcmd(domain_id: int, probe_s: float = 3.0) -> None: + """Refuse to start while something else is writing ``rt/lowcmd``. + + The factory controller publishes at about 1 kHz whenever the robot is not in debug mode, and a + run started against it fights for the motors. Measured on this robot: in debug mode the topic is + silent, so a nonzero count means the remote never took it out of its normal mode. + """ + from unitree_sdk2py.core.channel import ChannelSubscriber + from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowCmd_ + + seen = [0] + subscriber = ChannelSubscriber("rt/lowcmd", LowCmd_) + subscriber.Init(lambda _: seen.__setitem__(0, seen[0] + 1), 10) + time.sleep(probe_s) + subscriber.Close() + if seen[0]: + raise RuntimeError( + f"{seen[0]} rt/lowcmd samples in {probe_s:.0f}s -- something else is commanding the" + " robot. Put it in debug mode with the remote (L2+B, then L2+R2) and try again." + ) + print("[ok] rt/lowcmd is free") + + +def confirm(word: str, prompt: str) -> None: + """Block until the operator types ``word``. Anything else aborts.""" + print(prompt) + if input(f">>> type '{word}' and press Enter: ").strip() != word: + raise SystemExit("[..] aborted by the operator") + + +def bring_up(link: G1Link, pose: np.ndarray, kp, kd, ramp_s: float) -> None: + """Interpolate to the start pose and hold it until the operator engages the policy. + + Two steps rather than one, and ``rt/lowcmd`` keeps flowing through both: the robot is lowered + onto its feet between them, and a gap in the command stream there would drop it. + """ + confirm("go", "\n>>> Robot supported, area clear, e-stop in hand.") + start = link.read()["joint_pos"].copy() + steps = int(ramp_s / CONTROL_DT) + print(f"[..] ramping to the start pose over {ramp_s:.1f}s") + for step in range(steps): + link.check_abort() + link.write(start + (pose - start) * (step + 1) / steps, kp, kd) + link.wait(CONTROL_DT) + + print("\n>>> HOLDING the start pose. Square the robot up and lower it onto its feet.") + print(">>> KEEP IT SUPPORTED: this is PD with no policy, and the pose is not statically stable.") + held = threading.Event() + threading.Thread(target=lambda: (input(">>> type 'go2' and press Enter: "), held.set()), daemon=True).start() + while not held.is_set(): + link.check_abort() + link.write(pose, kp, kd) + link.wait(CONTROL_DT) + + print("[ok] engaging in 3s -- hands clear") + for _ in range(int(3.0 / CONTROL_DT)): + link.check_abort() + link.write(pose, kp, kd) + link.wait(CONTROL_DT) + if link.read()["gravity_z"] > FALL_GRAVITY_Z: + raise RuntimeError("the robot is not upright; not engaging the policy") + + +def main() -> int: + parser = argparse.ArgumentParser(description=__doc__.splitlines()[0]) + parser.add_argument("--pipeline", required=True, help="Exported LEAPP .yaml.") + parser.add_argument("--interface", required=True, help="Network interface on the robot's subnet.") + parser.add_argument("--domain_id", type=int, default=0, help="DDS domain.") + parser.add_argument("--depth_port", type=int, default=5601, help="UDP port the publisher sends to.") + parser.add_argument( + "--depth_rotation", + type=int, + choices=(0, 180), + default=180, + help="Rotate incoming depth before the graph [deg]. The default compensates the export's" + " own 180-degree roll against the camera stream.", + ) + args = parser.parse_args() + + policy = GraphPolicy(args.pipeline, args.depth_rotation) + link = G1Link(args.domain_id, args.interface) + depth = DepthStream(args.depth_port) + depth.wait() + take_lowcmd(args.domain_id) + + print(f"[ok] mode_machine={link.read()['mode_machine']} policy at {1 / CONTROL_DT:.0f} Hz") + print(f"[..] teleop: {Teleop.HELP}") + teleop = Teleop() + bring_up(link, policy.default_pose, policy.kp, policy.kd, ramp_s=3.0) + + try: + while teleop.running: + link.check_abort() + state = link.read() + if state["gravity_z"] > FALL_GRAVITY_Z: + raise RuntimeError(f"fell (projected gravity {state['gravity_z']:+.2f})") + frame = depth.latest() + if frame is None: + raise RuntimeError(f"no depth frame within {DEPTH_MAX_AGE * 1e3:.0f} ms") + joint_pos, kp, kd = policy.step( + state["joint_pos"], + state["joint_vel"], + state["root_quat_xyzw"], + state["ang_vel"], + teleop.command(state["root_quat_xyzw"]), + frame, + ) + link.write(joint_pos, kp, kd) + link.wait(CONTROL_DT) + except (KeyboardInterrupt, OperatorAbort, RuntimeError) as error: + print(f"\n[..] {error or 'interrupted'}") + finally: + link.damp(policy.kd) + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/source/isaaclab_rl/changelog.d/g1-depth-pipeline.fixed.rst b/source/isaaclab_rl/changelog.d/g1-depth-pipeline.fixed.rst new file mode 100644 index 000000000000..be658745771e --- /dev/null +++ b/source/isaaclab_rl/changelog.d/g1-depth-pipeline.fixed.rst @@ -0,0 +1,2 @@ +* Required a checkpoint for custom distillation algorithms before creating the training environment. +* Included student observation groups when exporting distilled policies to LEAPP. diff --git a/source/isaaclab_rl/isaaclab_rl/entrypoints/backends/export_rsl_rl.py b/source/isaaclab_rl/isaaclab_rl/entrypoints/backends/export_rsl_rl.py index baf46a74c11a..db73174b3644 100644 --- a/source/isaaclab_rl/isaaclab_rl/entrypoints/backends/export_rsl_rl.py +++ b/source/isaaclab_rl/isaaclab_rl/entrypoints/backends/export_rsl_rl.py @@ -154,10 +154,11 @@ def export_rsl_rl_agent(args_cli: argparse.Namespace, env_cfg: Any, agent_cfg: R env = gym.make(args_cli.task, cfg=env_cfg, render_mode=None) try: - # only the observation groups consumed by the actor are exported; critic and teacher groups are dropped + # Export only the inference model inputs, excluding privileged and augmentation-only groups. obs_groups_cfg = getattr(agent_cfg, "obs_groups", None) if isinstance(obs_groups_cfg, Mapping): - required_obs_groups = set(obs_groups_cfg.get("actor", ["policy"])) + model_group = "student" if agent_cfg.class_name == "DistillationRunner" else "actor" + required_obs_groups = set(obs_groups_cfg.get(model_group, ["policy"])) else: required_obs_groups = {"policy"} policy_node_name = prepare_export_env(env, args_cli, required_obs_groups=required_obs_groups) diff --git a/source/isaaclab_rl/isaaclab_rl/entrypoints/backends/train_rsl_rl.py b/source/isaaclab_rl/isaaclab_rl/entrypoints/backends/train_rsl_rl.py index 03470fed5b83..8cbb87105b52 100644 --- a/source/isaaclab_rl/isaaclab_rl/entrypoints/backends/train_rsl_rl.py +++ b/source/isaaclab_rl/isaaclab_rl/entrypoints/backends/train_rsl_rl.py @@ -101,7 +101,7 @@ def _resolve_checkpoint(args_cli: argparse.Namespace, agent_cfg: RslRlBaseRunner ) if args_cli.checkpoint: return retrieve_file_path(args_cli.checkpoint) - if agent_cfg.algorithm.class_name == "Distillation": + if agent_cfg.class_name == "DistillationRunner": raise ValueError("Distillation training requires --checkpoint.") return None diff --git a/source/isaaclab_rl/test/export/leapp_initialized_checkpoints.py b/source/isaaclab_rl/test/export/leapp_initialized_checkpoints.py index 7a5d2ed8af24..7f6d66be3041 100644 --- a/source/isaaclab_rl/test/export/leapp_initialized_checkpoints.py +++ b/source/isaaclab_rl/test/export/leapp_initialized_checkpoints.py @@ -70,14 +70,16 @@ def step(self, action): def _create_rsl_rl_checkpoint(env: _CheckpointEnv, cfg, directory: Path) -> Path: """Initialize and save the same runner used by the task, without learning.""" - from rsl_rl.runners import OnPolicyRunner + from rsl_rl.runners import DistillationRunner, OnPolicyRunner cfg.device = "cpu" - runner = OnPolicyRunner(env, to_dict(cfg), log_dir=str(directory), device="cpu") + runner_type = DistillationRunner if cfg.class_name == "DistillationRunner" else OnPolicyRunner + runner = runner_type(env, to_dict(cfg), log_dir=str(directory), device="cpu") # Preserve nontrivial normalization coverage with deterministic synthetic samples. observations = env.get_observations() - runner.alg.actor.update_normalization(observations.apply(lambda value: value + 0.2)) - runner.alg.actor.update_normalization(observations.apply(lambda value: value - 0.4)) + policy = runner.alg.get_policy() + policy.update_normalization(observations.apply(lambda value: value + 0.2)) + policy.update_normalization(observations.apply(lambda value: value - 0.4)) path = directory / "model_0.pt" runner.save(str(path), infos={"initialized_for_leapp_export_test": True}) return path diff --git a/source/isaaclab_rl/test/export/test_leapp_export_flow.py b/source/isaaclab_rl/test/export/test_leapp_export_flow.py index 44acf0cf22ae..ce63a80f400e 100644 --- a/source/isaaclab_rl/test/export/test_leapp_export_flow.py +++ b/source/isaaclab_rl/test/export/test_leapp_export_flow.py @@ -9,6 +9,7 @@ import ast import contextlib +import importlib from pathlib import Path from types import SimpleNamespace @@ -139,6 +140,7 @@ class _DirectTensorEnv(_TensorEnv, DirectRLEnv): ("backend", "task", "checkpoint_task", "direct"), [ pytest.param("rsl_rl", MANAGER_TASK, MANAGER_TASK, False, id="rsl-rl-cnn"), + pytest.param("rsl_rl_distillation", MANAGER_TASK, MANAGER_TASK, False, id="rsl-rl-student"), pytest.param("rl_games", MANAGER_TASK, MANAGER_TASK, False, id="rl-games-lstm"), pytest.param("skrl", MANAGER_TASK, MANAGER_TASK, False, id="skrl"), pytest.param("sb3", MANAGER_TASK, MANAGER_TASK, False, id="sb3-dict-normalization"), @@ -150,14 +152,26 @@ def test_leapp_export_flow(backend: str, task: str, checkpoint_task: str, direct """Load a native checkpoint, export the real backend policy, and replay changed observations.""" from leapp import InferenceManager - from isaaclab_rl.entrypoints.backends import export_rl_games, export_rsl_rl, export_sb3, export_skrl - - modules = {"rsl_rl": export_rsl_rl, "rl_games": export_rl_games, "skrl": export_skrl, "sb3": export_sb3} - module = modules[backend] + distillation = backend == "rsl_rl_distillation" + if distillation: + backend = "rsl_rl" + module = importlib.import_module(f"isaaclab_rl.entrypoints.backends.export_{backend}") torch.set_num_threads(1) torch.manual_seed(42) np.random.seed(42) agent_cfg = load_cfg_from_registry(checkpoint_task, f"{backend}_cfg_entry_point") + if distillation: + from isaaclab_rl.rsl_rl import RslRlDistillationAlgorithmCfg, RslRlDistillationRunnerCfg + + agent_cfg = RslRlDistillationRunnerCfg( + experiment_name="leapp_student", + num_steps_per_env=15, + max_iterations=1, + student=agent_cfg.actor, + teacher=agent_cfg.critic, + algorithm=RslRlDistillationAlgorithmCfg(num_learning_epochs=2, learning_rate=1.0e-3, gradient_length=15), + obs_groups={"student": agent_cfg.obs_groups["actor"], "teacher": ["critic"]}, + ) checkpoint = _CREATORS[backend](_CheckpointEnv(checkpoint_task, backend), agent_cfg, tmp_path) env_cfg = load_cfg_from_registry(checkpoint_task, "env_cfg_entry_point") connected = backend == "rsl_rl" and task == MANAGER_TASK and not direct diff --git a/source/isaaclab_rl/test/test_entrypoints.py b/source/isaaclab_rl/test/test_entrypoints.py index 6c6f2cb5da0a..97cf9848b75a 100644 --- a/source/isaaclab_rl/test/test_entrypoints.py +++ b/source/isaaclab_rl/test/test_entrypoints.py @@ -943,3 +943,13 @@ def make_runner(learning_rate): assert not destination.alg.optimizer.state else: torch.testing.assert_close(destination_state["optimizer_state_dict"], source_state["optimizer_state_dict"]) + + +def test_rsl_custom_distillation_requires_teacher_checkpoint(): + """A custom distillation algorithm must fail before the expensive scene is constructed.""" + from isaaclab_rl.entrypoints.backends.train_rsl_rl import _resolve_checkpoint + + args = SimpleNamespace(checkpoint=None) + cfg = SimpleNamespace(class_name="DistillationRunner", algorithm=SimpleNamespace(class_name="custom:Distillation")) + with pytest.raises(ValueError, match="requires --checkpoint"): + _resolve_checkpoint(args, cfg, "/unused") diff --git a/source/isaaclab_tasks/changelog.d/g1-depth-pipeline.fixed.rst b/source/isaaclab_tasks/changelog.d/g1-depth-pipeline.fixed.rst new file mode 100644 index 000000000000..baf8a5b20919 --- /dev/null +++ b/source/isaaclab_tasks/changelog.d/g1-depth-pipeline.fixed.rst @@ -0,0 +1,2 @@ +* Updated the G1 teacher/depth-student terrain, rewards, action scales, actuator limits and mirror + augmentation, including teacher-only checkpoint loading and per-step normalization. diff --git a/source/isaaclab_tasks/changelog.d/g1-shared-mdp.changed.rst b/source/isaaclab_tasks/changelog.d/g1-shared-mdp.changed.rst new file mode 100644 index 000000000000..9fedeee194e7 --- /dev/null +++ b/source/isaaclab_tasks/changelog.d/g1-shared-mdp.changed.rst @@ -0,0 +1,6 @@ +* Moved G1 rewards and the clearance termination into the shared velocity MDP modules, + scheduling the termination warmup through the curriculum manager. +* Moved G1 symmetry helpers alongside the other velocity-task robot symmetry modules. +* Reused the common depth-image observation with a G1 image-processing modifier, assembling observation + groups directly in the student environment configuration. +* Read G1 sole geometry and reference torques from the asset, reusing configured actuator stiffnesses. diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/__init__.py index 41304bc72775..1c4ee1b64f19 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/__init__.py @@ -36,3 +36,24 @@ "skrl_cfg_entry_point": f"{agents.__name__}:skrl_rough_ppo_cfg.yaml", }, ) + + +gym.register( + id="Isaac-Velocity-Rough-G1-Teacher", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.rough_env_cfg:G1RoughTeacherEnvCfg", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:G1RoughTeacherPPORunnerCfg", + }, +) + +gym.register( + id="Isaac-Velocity-Rough-G1-Depth-Student", + entry_point="isaaclab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.rough_env_cfg:G1RoughDepthStudentEnvCfg", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:G1RoughDepthStudentRunnerCfg", + }, +) diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/agents/distillation.py b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/agents/distillation.py new file mode 100644 index 000000000000..90127b74f10a --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/agents/distillation.py @@ -0,0 +1,102 @@ +# 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 copy import copy + +import torch +from rsl_rl.algorithms import Distillation +from tensordict import TensorDict +from torch import nn + +from isaaclab_tasks.core.velocity.mdp.symmetry.g1 import _action_joint_map, _mirror_group + + +class Mirror: + def __init__(self, env): + self.env = env.unwrapped + self.perm, self.sign = _action_joint_map(self.env) + + @torch.no_grad() + def observations(self, obs): + assert set(obs.keys()) == {"policy", "teacher", "depth", "depth_mirror"} + return TensorDict( + { + "policy": _mirror_group(self.env, "policy", obs["policy"]), + "teacher": _mirror_group(self.env, "teacher", obs["teacher"]), + "depth": obs["depth_mirror"].flip(-1), + "depth_mirror": obs["depth"].flip(-1), + }, + batch_size=obs.batch_size, + ) + + @torch.no_grad() + def actions(self, actions): + return actions[..., self.perm] * self.sign + + +class AugmentedStorage: + def __init__(self, storage, mirror): + self.storage = storage + self.mirror = mirror + + def __getattr__(self, name): + return getattr(self.storage, name) + + def generator(self): + for batch in self.storage.generator(): + out = copy(batch) + out.observations = torch.cat([batch.observations, self.mirror.observations(batch.observations)], dim=0) + out.privileged_actions = torch.cat( + [batch.privileged_actions, self.mirror.actions(batch.privileged_actions)] + ) + out.dones = batch.dones.repeat(2, 1) + yield out + + +class MirrorDistillation(Distillation): + @staticmethod + def construct_algorithm(obs, env, cfg, device): + alg = Distillation.construct_algorithm(obs, env, cfg, device) + assert not alg.student.is_recurrent and not alg.teacher.is_recurrent + alg.mirror = Mirror(env) + alg.storage = AugmentedStorage(alg.storage, alg.mirror) + return alg + + def process_env_step(self, obs, rewards, dones, extras): + mirrored = TensorDict( + {"policy": _mirror_group(self.mirror.env, "policy", obs["policy"])}, batch_size=obs.batch_size + ) + self.student.update_normalization(mirrored) + self.student.update_normalization(obs) + super().process_env_step(obs, rewards, dones, extras) + + def load(self, loaded_dict, load_cfg=None, strict=True): + if "actor_state_dict" in loaded_dict: + load_cfg = {"teacher": True, "student": False, "optimizer": False, "iteration": False} + return super().load(loaded_dict, load_cfg, strict) + + def update(self): + """Update the student with accumulated gradients over rollout steps.""" + self.num_updates += 1 + mean_loss = 0.0 + loss = 0.0 + count = 0 + for _ in range(self.num_learning_epochs): + for batch in self.storage.generator(): + behavior_loss = self.loss_fn(self.student(batch.observations), batch.privileged_actions) + loss = loss + behavior_loss + mean_loss += behavior_loss.item() + count += 1 + if count % self.gradient_length == 0: + self.optimizer.zero_grad() + loss.backward() + if self.is_multi_gpu: + self.reduce_parameters() + if self.max_grad_norm: + nn.utils.clip_grad_norm_(self.student.parameters(), self.max_grad_norm) + self.optimizer.step() + loss = 0.0 + self.storage.clear() + return {"behavior": mean_loss / count} diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/agents/rsl_rl_ppo_cfg.py index 7b890ebdf66c..3aa1bd54aeb4 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/agents/rsl_rl_ppo_cfg.py @@ -5,7 +5,15 @@ from isaaclab.utils import configclass -from isaaclab_rl.rsl_rl import RslRlMLPModelCfg, RslRlOnPolicyRunnerCfg, RslRlPpoAlgorithmCfg +from isaaclab_rl.rsl_rl import ( + RslRlCNNModelCfg, + RslRlDistillationAlgorithmCfg, + RslRlDistillationRunnerCfg, + RslRlMLPModelCfg, + RslRlOnPolicyRunnerCfg, + RslRlPpoAlgorithmCfg, + RslRlSymmetryCfg, +) @configclass @@ -51,3 +59,59 @@ def __post_init__(self): self.experiment_name = "g1_flat" self.actor.hidden_dims = [256, 128, 128] self.critic.hidden_dims = [256, 128, 128] + + +@configclass +class G1RoughTeacherPPORunnerCfg(G1RoughPPORunnerCfg): + seed = 42 + max_iterations = 6000 + save_interval = 250 + experiment_name = "g1_rough_teacher" + + def __post_init__(self): + super().__post_init__() + self.algorithm.symmetry_cfg = RslRlSymmetryCfg( + use_data_augmentation=True, + use_mirror_loss=False, + data_augmentation_func="isaaclab_tasks.core.velocity.mdp.symmetry.g1:compute_symmetric_states", + mirror_loss_coeff=0.0, + ) + + +@configclass +class G1RoughDepthStudentRunnerCfg(RslRlDistillationRunnerCfg): + seed = 42 + num_steps_per_env = 24 + max_iterations = 24000 + save_interval = 250 + experiment_name = "g1_rough_depth_student" + obs_groups = {"student": ["policy", "depth"], "teacher": ["teacher"]} + student = RslRlCNNModelCfg( + cnn_cfg=RslRlCNNModelCfg.CNNCfg( + output_channels=[16, 32, 32], + kernel_size=[5, 3, 3], + stride=[2, 2, 2], + padding="zeros", + activation="relu", + flatten=True, + ), + hidden_dims=[512, 256, 128], + activation="elu", + obs_normalization=True, + distribution_cfg=RslRlCNNModelCfg.GaussianDistributionCfg(init_std=0.1), + ) + teacher = RslRlMLPModelCfg( + hidden_dims=[512, 256, 128], + activation="elu", + obs_normalization=False, + distribution_cfg=RslRlMLPModelCfg.GaussianDistributionCfg(init_std=0.0), + ) + algorithm = RslRlDistillationAlgorithmCfg( + class_name="isaaclab_tasks.core.velocity.config.g1.agents.distillation:MirrorDistillation", + num_learning_epochs=2, + learning_rate=1.0e-3, + gradient_length=15, + max_grad_norm=1.0, + optimizer="adam", + loss_type="mse", + ) diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/rough_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/rough_env_cfg.py index 6c6329d3299b..a15e0393a24c 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/rough_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/rough_env_cfg.py @@ -3,11 +3,37 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""Configuration for the Unitree G1 velocity-tracking environment on rough terrain.""" +"""Configurations for the Unitree G1 velocity-tracking environment on rough terrain. +Beside the standard environment this module holds the two stages of the depth pipeline: a teacher +whose actor reads a terrain height scan and the base linear velocity, and a student that replaces +both with a chest-mounted depth camera and is distilled from the teacher in the teacher's own +environment. +""" + +import math +from copy import deepcopy + +import torch +from isaaclab_newton.sim.schemas import NewtonArticulationCfg +from isaaclab_physx.sim.schemas import PhysxArticulationCfg, PhysxRigidBodyCfg + +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.assets import ArticulationCfg +from isaaclab.managers import CurriculumTermCfg as CurrTerm +from isaaclab.managers import ObservationGroupCfg as ObsGroup +from isaaclab.managers import ObservationTermCfg as ObsTerm from isaaclab.managers import RewardTermCfg as RewTerm from isaaclab.managers import SceneEntityCfg +from isaaclab.managers import TerminationTermCfg as DoneTerm +from isaaclab.sensors import CameraCfg +from isaaclab.sim import PinholeCameraCfg, UsdFileCfg from isaaclab.utils import configclass, replace +from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR, retrieve_file_path +from isaaclab.utils.modifiers import ModifierCfg +from isaaclab.utils.string import resolve_matching_names, resolve_matching_names_values + +from isaaclab_tasks.utils.presets import MultiBackendRendererCfg from isaaclab_assets import G1_MINIMAL_CFG @@ -142,3 +168,277 @@ def __post_init__(self): self.events.base_com = None self.events.base_external_force_torque.params["asset_cfg"].body_names = "torso_link" self.events.reset_robot_joints.params["position_range"] = (1.0, 1.0) + + +DEPTH_WIDTH = 64 + +DEPTH_HEIGHT = 38 + +DEPTH_STACK = 3 + +DEPTH_MAX_RANGE = 3.0 + +DEPTH_OFFSET_POS = (0.0576, 0.0175, 0.4299) + +DEPTH_OFFSET_ROT = (0.9150, 0.0, -0.4035, 0.0) + + +def process_depth_image(data: torch.Tensor) -> torch.Tensor: + """Fill missing depth and rotate the channel-first frames to the G1 camera convention.""" + return torch.nan_to_num(data, nan=DEPTH_MAX_RANGE, posinf=DEPTH_MAX_RANGE).flip((-2, -1)) + + +def get_action_parameters(robot_cfg: ArticulationCfg) -> tuple[dict[str, float], tuple, tuple]: + """Read authored G1 drives and sole geometry before simulation actuator overrides.""" + from pxr import Usd, UsdGeom, UsdPhysics + + stage = Usd.Stage.Open(retrieve_file_path(robot_cfg.spawn.usd_path)) + if stage.GetCompositionErrors(): + raise ValueError(f"Could not compose G1 asset: {stage.GetCompositionErrors()}") + root = stage.GetDefaultPrim() + joints = { + prim.GetName(): UsdPhysics.DriveAPI.Get(prim, "angular").GetMaxForceAttr().Get() + for prim in Usd.PrimRange(root, Usd.TraverseInstanceProxies()) + if prim.IsA(UsdPhysics.RevoluteJoint) + } + action_scale = {} + for actuator in robot_cfg.actuators.values(): + _, names = resolve_matching_names(actuator.joint_names_expr, list(joints)) + if isinstance(actuator.stiffness, dict): + _, names, stiffnesses = resolve_matching_names_values(actuator.stiffness, names) + else: + stiffnesses = [actuator.stiffness] * len(names) + for name, stiffness in zip(names, stiffnesses): + # Use task-specific reference torques for hip roll and finger action scales. + torque = joints[name] + if name.endswith("_hip_roll_joint"): + torque = 88.0 + elif "_hand_" in name: + torque = 2.45 + # Preserve the policy's four-decimal, half-up action mapping. + action_scale[name] = math.floor(0.25 * torque * 10000 / stiffness + 0.5) / 10000 + + body = root.GetChild("left_ankle_roll_link") + plate = UsdGeom.Cube(body.GetChild("foot_plate")) + transform, _ = UsdGeom.XformCache().ComputeRelativeTransform(plate.GetPrim(), body) + sole_size = tuple(plate.GetSizeAttr().Get() * transform.GetRow3(axis).GetLength() for axis in range(3)) + sole_offset = tuple(transform.ExtractTranslation()) + return action_scale, sole_size, sole_offset + + +G1_29DOF_VELOCITY_CFG = ArticulationCfg( + spawn=UsdFileCfg( + usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Robots/Unitree/G1/g1_29dof.usd", + activate_contact_sensors=True, + rigid_props=PhysxRigidBodyCfg( + 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=[ + PhysxArticulationCfg( + enabled_self_collisions=False, solver_position_iteration_count=8, solver_velocity_iteration_count=4 + ), + NewtonArticulationCfg(self_collision_enabled=False), + ], + ), + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.0, 0.0, 0.793), + joint_pos={ + ".*_hip_pitch_joint": -0.20, + ".*_knee_joint": 0.42, + ".*_ankle_pitch_joint": -0.23, + ".*_elbow_joint": 0.87, + "left_shoulder_pitch_joint": 0.35, + "left_shoulder_roll_joint": 0.16, + "right_shoulder_pitch_joint": 0.35, + "right_shoulder_roll_joint": -0.16, + }, + ), + soft_joint_pos_limit_factor=0.9, + actuators={ + "legs": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_.*", ".*_knee_joint", "waist_yaw_joint"], + joint_effort_limit=300.0, + stiffness={ + ".*_hip_pitch_joint": 200.0, + ".*_hip_roll_joint": 150.0, + ".*_hip_yaw_joint": 150.0, + ".*_knee_joint": 200.0, + "waist_yaw_joint": 200.0, + }, + damping=5.0, + armature=0.01, + ), + "feet": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_.*"], joint_effort_limit=20.0, stiffness=20.0, damping=2.0, armature=0.01 + ), + "waist": ImplicitActuatorCfg( + joint_names_expr=["waist_roll_joint", "waist_pitch_joint"], + joint_effort_limit=50.0, + stiffness=200.0, + damping=5.0, + armature=0.01, + ), + "arms": ImplicitActuatorCfg( + joint_names_expr=[".*_shoulder_.*", ".*_elbow_joint", ".*_wrist_.*"], + joint_effort_limit=300.0, + stiffness=40.0, + damping=10.0, + armature=0.01, + ), + "hands": ImplicitActuatorCfg( + joint_names_expr=[".*_hand_.*"], joint_effort_limit=300.0, stiffness=40.0, damping=10.0, armature=0.001 + ), + }, +) + + +@configclass +class G1RoughTeacherEnvCfg(G1RoughEnvCfg): + """G1 teacher on mesh terrain with terrain-height observations.""" + + def __post_init__(self): + super().__post_init__() + self.scene.robot = replace(G1_29DOF_VELOCITY_CFG, prim_path="{ENV_REGEX_NS}/Robot") + self.rewards.joint_deviation_arms.params["asset_cfg"] = SceneEntityCfg( + "robot", + joint_names=[".*_shoulder_.*_joint", ".*_elbow_joint", ".*_wrist_.*_joint"], + ) + self.rewards.joint_deviation_fingers.params["asset_cfg"] = SceneEntityCfg( + "robot", joint_names=[".*_hand_.*_joint"] + ) + self.rewards.joint_deviation_torso.params["asset_cfg"] = SceneEntityCfg( + "robot", joint_names=["waist_yaw_joint", "waist_roll_joint", "waist_pitch_joint"] + ) + + self.seed = 42 + self.scene.num_envs = 4096 + self.sim.use_newton_actuators = False + self.sim.visualizer_cfgs = [] + self.sim.default_visualizer_cfg = None + terrain = self.scene.terrain.terrain_generator + proportions = { + "pyramid_stairs": 0.2, + "pyramid_stairs_inv": 0.2, + "boxes": 0.2, + "random_rough": 0.2, + "hf_pyramid_slope": 0.1, + "hf_pyramid_slope_inv": 0.1, + } + for name, subterrain in terrain.sub_terrains.items(): + subterrain.convert_to_heightfield = False + subterrain.proportion = proportions[name] + for name in ("pyramid_stairs", "pyramid_stairs_inv"): + terrain.sub_terrains[name].step_height_range = (0.05, 0.35) + terrain.sub_terrains[name].step_width = 0.3 + self.actions.joint_pos.scale, sole_size, sole_offset = get_action_parameters(self.scene.robot) + self.rewards.track_lin_vel_xy_exp.weight = 2.0 + self.rewards.ang_vel_xy_l2.weight = -0.05 + self.rewards.feet_air_time.weight = 1.0 + self.rewards.feet_slide.weight = -0.5 + self.rewards.joint_deviation_hip.func = mdp.joint_deviation_l2 + self.rewards.joint_deviation_hip.weight = -1.0 + self.rewards.joint_deviation_hip.params["asset_cfg"].joint_names = [".*_hip_roll_joint", ".*_hip_yaw_joint"] + self.rewards.joint_deviation_torso.func = mdp.joint_deviation_l2 + self.rewards.joint_deviation_torso.weight = -1.0 + self.rewards.pelvis_height = RewTerm( + func=mdp.pelvis_height_deficit_l2, + weight=-10.0, + params={ + "target_height": 0.65, + "asset_cfg": SceneEntityCfg("robot"), + "sensor_cfg": SceneEntityCfg("height_scanner"), + }, + ) + self.rewards.torso_ang_vel = RewTerm( + func=mdp.body_ang_vel_xy_l2, + weight=-0.1, + params={"asset_cfg": SceneEntityCfg("robot", body_names=["torso_link"])}, + ) + self.rewards.root_downward_speed = RewTerm( + func=mdp.root_downward_speed_l2, + weight=-1.0, + params={"speed_limit": 0.5}, + ) + self.rewards.feet_support_edge = RewTerm( + func=mdp.feet_support_edge_distance, + weight=-5.0, + params={ + **deepcopy(self.rewards.feet_slide.params), + "sole_size": sole_size, + "sole_offset": sole_offset, + }, + ) + self.terminations.base_height = DoneTerm( + func=mdp.pelvis_below_terrain_clearance, + params={ + "minimum_height": -math.inf, + "asset_cfg": SceneEntityCfg("robot"), + "sensor_cfg": SceneEntityCfg("height_scanner"), + }, + ) + + self.curriculum.base_height = CurrTerm( + func=mdp.modify_term_cfg, + params={ + "address": "terminations.base_height.params.minimum_height", + "modify_fn": mdp.set_value_after_steps, + "modify_params": {"value": 0.4, "num_steps": 12000}, + }, + ) + + +@configclass +class G1RoughDepthStudentEnvCfg(G1RoughTeacherEnvCfg): + """G1 student with five proprioceptive frames and mirrored depth supervision.""" + + def __post_init__(self): + super().__post_init__() + self.observations.teacher = deepcopy(self.observations.policy) + self.observations.teacher.enable_corruption = False + self.observations.policy.base_lin_vel = None + self.observations.policy.height_scan = None + for name in ("base_ang_vel", "projected_gravity", "velocity_commands", "joint_pos", "joint_vel", "actions"): + term = getattr(self.observations.policy, name) + term.history_length = 5 + term.flatten_history_dim = True + self.observations.depth = ObsGroup(enable_corruption=False, concatenate_terms=True) + self.observations.depth.depth = ObsTerm( + func=mdp.image_depth, + params={ + "sensor_cfg": SceneEntityCfg("depth_camera"), + "data_type": "distance_to_image_plane", + "normalize": False, + "frame_stack": DEPTH_STACK, + "channel_first": True, + }, + modifiers=[ModifierCfg(func=process_depth_image)], + clip=(0.0, DEPTH_MAX_RANGE), + scale=1.0 / DEPTH_MAX_RANGE, + ) + self.observations.depth_mirror = deepcopy(self.observations.depth) + self.observations.depth_mirror.depth.params["sensor_cfg"] = SceneEntityCfg("depth_camera_mirror") + self.scene.depth_camera = CameraCfg( + prim_path="{ENV_REGEX_NS}/Robot/torso_link/depth_camera", + update_period=0.0, + update_latest_camera_pose=True, + renderer_cfg=MultiBackendRendererCfg(), + width=DEPTH_WIDTH, + height=DEPTH_HEIGHT, + data_types=["distance_to_image_plane"], + spawn=PinholeCameraCfg( + focal_length=1.93, + horizontal_aperture=2.0 * 1.93 * math.tan(math.radians(87.0 / 2.0)), + clipping_range=(0.05, 10.0), + ), + offset=CameraCfg.OffsetCfg(pos=DEPTH_OFFSET_POS, rot=DEPTH_OFFSET_ROT, convention="world"), + ) + self.scene.depth_camera_mirror = deepcopy(self.scene.depth_camera) + self.scene.depth_camera_mirror.prim_path = "{ENV_REGEX_NS}/Robot/torso_link/depth_camera_mirror" + x, y, z = DEPTH_OFFSET_POS + self.scene.depth_camera_mirror.offset.pos = (x, -y, z) diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/mdp/__init__.pyi b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/mdp/__init__.pyi index 88a90656769f..ae16ca702721 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/mdp/__init__.pyi +++ b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/mdp/__init__.pyi @@ -4,10 +4,17 @@ # SPDX-License-Identifier: BSD-3-Clause __all__ = [ + "body_ang_vel_xy_l2", "feet_air_time", "feet_air_time_positive_biped", "feet_air_time_variance", "feet_slide", + "feet_support_edge_distance", + "joint_deviation_l2", + "pelvis_below_terrain_clearance", + "pelvis_height_deficit_l2", + "root_downward_speed_l2", + "set_value_after_steps", "stand_still_joint_deviation_l1", "terrain_levels_vel", "terrain_out_of_bounds", @@ -15,16 +22,24 @@ __all__ = [ "track_lin_vel_xy_yaw_frame_exp", ] -from .curriculums import terrain_levels_vel +from .curriculums import set_value_after_steps, terrain_levels_vel from .rewards import ( + body_ang_vel_xy_l2, feet_air_time, feet_air_time_positive_biped, feet_air_time_variance, feet_slide, + feet_support_edge_distance, + joint_deviation_l2, + pelvis_height_deficit_l2, + root_downward_speed_l2, stand_still_joint_deviation_l1, track_ang_vel_z_world_exp, track_lin_vel_xy_yaw_frame_exp, ) -from .terminations import terrain_out_of_bounds +from .terminations import ( + pelvis_below_terrain_clearance, + terrain_out_of_bounds, +) from isaaclab.envs.mdp import * diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/mdp/curriculums.py b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/mdp/curriculums.py index 92b4d4fb2dcd..80108c644da0 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/mdp/curriculums.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/mdp/curriculums.py @@ -8,10 +8,11 @@ from __future__ import annotations from collections.abc import Sequence -from typing import TYPE_CHECKING +from typing import TYPE_CHECKING, Any import torch +from isaaclab.envs.mdp import modify_term_cfg from isaaclab.managers import SceneEntityCfg if TYPE_CHECKING: @@ -50,3 +51,10 @@ def terrain_levels_vel( terrain.update_env_origins(env_ids, move_up, move_down) # return the mean terrain level return torch.mean(terrain.terrain_levels.float()) + + +def set_value_after_steps(env: ManagerBasedRLEnv, env_ids: Sequence[int], data: Any, value: Any, num_steps: int) -> Any: + """Set a term parameter on the first reset at or after ``num_steps`` global environment steps.""" + if env.common_step_counter >= num_steps: + return value + return modify_term_cfg.NO_CHANGE diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/mdp/rewards.py b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/mdp/rewards.py index 83efe1ebe697..4c2b980f763e 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/mdp/rewards.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/mdp/rewards.py @@ -7,13 +7,15 @@ from __future__ import annotations +import math from typing import TYPE_CHECKING import torch from isaaclab.envs import mdp from isaaclab.managers import SceneEntityCfg -from isaaclab.utils.math import quat_apply_inverse, yaw_quat +from isaaclab.utils.math import quat_apply, quat_apply_inverse, yaw_quat +from isaaclab.utils.warp import raycast_mesh if TYPE_CHECKING: from isaaclab.assets import Articulation @@ -152,3 +154,107 @@ 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.linalg.norm(command[:, :2], dim=1) < command_threshold) + + +def pelvis_height_deficit_l2( + env, + target_height: float, + asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), + sensor_cfg: SceneEntityCfg = SceneEntityCfg("height_scanner"), +) -> torch.Tensor: + """Squared shortfall of the root below ``target_height`` above the ground beneath it.""" + asset = env.scene[asset_cfg.name] + sensor = env.scene[sensor_cfg.name] + hits = sensor.data.ray_hits_w.torch[..., 2] + ground = torch.nan_to_num(hits, nan=0.0, posinf=0.0, neginf=0.0).median(dim=1).values + clearance = asset.data.root_pos_w.torch[:, 2] - ground + return torch.clamp(target_height - clearance, min=0.0).square() + + +def joint_deviation_l2(env, asset_cfg: SceneEntityCfg) -> torch.Tensor: + """Squared deviation of the selected joints from their default angles.""" + asset = env.scene[asset_cfg.name] + return torch.sum( + torch.square( + asset.data.joint_pos.torch[:, asset_cfg.joint_ids] + - asset.data.default_joint_pos.torch[:, asset_cfg.joint_ids] + ), + dim=1, + ) + + +def body_ang_vel_xy_l2(env, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot")) -> torch.Tensor: + """Squared world-frame roll/pitch angular velocity of a named body.""" + asset = env.scene[asset_cfg.name] + omega = asset.data.body_ang_vel_w.torch[:, asset_cfg.body_ids, :].squeeze(1) + return torch.sum(torch.square(omega[:, :2]), dim=1) + + +def root_downward_speed_l2( + env, speed_limit: float, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot") +) -> torch.Tensor: + """Penalize world-frame downward root speed exceeding the limit [m/s].""" + velocity_z = env.scene[asset_cfg.name].data.root_lin_vel_w.torch[:, 2] + return torch.clamp(-velocity_z - speed_limit, min=0.0).square() + + +def sole_edge_distances(sole_points, mesh, height_tolerance=0.04, max_distance=0.25): + """Estimate horizontal sole-corner overhang [m] beyond the supporting terrain plane.""" + starts = sole_points.clone() + starts[..., 2] += 0.5 + directions = torch.zeros_like(starts) + directions[..., 2] = -1.0 + hits, _, normals, _ = raycast_mesh(starts, directions, mesh, max_dist=2.0, return_normal=True) + valid = torch.isfinite(hits).all(-1) & torch.isfinite(normals).all(-1) + support = valid & ((hits[..., 2] - sole_points[..., 2]).abs() < height_tolerance) & (normals[..., 2] > 0.7) + support_index = torch.where(support, hits[..., 2], -torch.inf).argmax(-1) + gather = support_index[..., None, None].expand(*support_index.shape, 1, 3) + anchor = torch.nan_to_num(hits.gather(-2, gather).squeeze(-2), posinf=0.0, neginf=0.0) + normal = torch.nan_to_num(normals.gather(-2, gather).squeeze(-2), posinf=0.0, neginf=0.0) + corner_indices = [0, 2, 6, 8] + corners = sole_points[..., corner_indices, :] + ground = hits[..., corner_indices, 2] + plane_z = anchor[..., None, 2] - ((corners[..., :2] - anchor[..., None, :2]) * normal[..., None, :2]).sum( + -1 + ) / normal[..., None, 2].clamp_min(0.7) + outside = (plane_z - ground > height_tolerance) | ~valid[..., corner_indices] + angles = torch.arange(16, device=sole_points.device, dtype=sole_points.dtype) * (2 * math.pi / 16) + horizontal = torch.stack((angles.cos(), angles.sin(), torch.zeros_like(angles)), -1) + ray_start = corners.clone() + ray_start[..., 2] = plane_z - 0.01 + ray_start = ray_start[..., None, :].expand(*corners.shape[:-1], 16, 3).contiguous() + ray_direction = horizontal.expand_as(ray_start).contiguous() + _, distances, _, _ = raycast_mesh( + ray_start.reshape(-1, 16, 3), + ray_direction.reshape(-1, 16, 3), + mesh, + max_dist=max_distance, + return_distance=True, + ) + distance = distances.amin(-1).reshape(*corners.shape[:-1]).clamp_max(max_distance) + return torch.where(support.any(-1)[..., None], torch.where(outside, distance, 0.0), max_distance) + + +def feet_support_edge_distance(env, sensor_cfg, asset_cfg, sole_size, sole_offset) -> torch.Tensor: + """Penalize mean corner overhang [m] for feet carrying more than 20 N.""" + robot = env.scene[asset_cfg.name] + sensor = env.scene[sensor_cfg.name] + positions = robot.data.body_pos_w.torch[:, asset_cfg.body_ids] + quaternions = robot.data.body_quat_w.torch[:, asset_cfg.body_ids] + xy = torch.cartesian_prod( + torch.linspace(-0.5, 0.5, 3, device=env.device), torch.linspace(-0.5, 0.5, 3, device=env.device) + ) + points = torch.cat((xy, torch.full_like(xy[:, :1], -0.5)), -1) + points = points * torch.tensor(sole_size, device=env.device) + torch.tensor(sole_offset, device=env.device) + points = points.expand(*positions.shape[:-1], 9, 3) + points = quat_apply(quaternions[..., None, :].expand(*points.shape[:-1], 4), points) + positions[..., None, :] + scanner = env.scene["height_scanner"] + key = (scanner.cfg.mesh_prim_paths[0], scanner.device) + if key not in scanner.meshes: + from isaaclab.sensors.ray_caster.base_ray_caster import BaseRayCaster + + BaseRayCaster._initialize_warp_meshes(scanner) + mesh = scanner.meshes[key] + distance = sole_edge_distances(points, mesh).mean(-1) + contact = sensor.data.net_forces_w.torch[:, sensor_cfg.body_ids].norm(dim=-1) > 20.0 + return (distance * contact).sum(-1) diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/mdp/symmetry/g1.py b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/mdp/symmetry/g1.py new file mode 100644 index 000000000000..0761671c4464 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/mdp/symmetry/g1.py @@ -0,0 +1,192 @@ +# 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 __future__ import annotations + +from typing import TYPE_CHECKING + +import torch +from tensordict import TensorDict + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv +__all__ = ["compute_symmetric_states"] +_SCAN_ROWS, _SCAN_COLS = (11, 17) +_VECTOR_MIRRORS = { + "base_lin_vel": (1.0, -1.0, 1.0), + "base_ang_vel": (-1.0, 1.0, -1.0), + "projected_gravity": (1.0, -1.0, 1.0), + "velocity_commands": (1.0, -1.0, -1.0), +} +_JOINT_TERMS = ("joint_pos", "joint_vel", "actions") +_PITCH_TOKENS = ("_pitch_", "knee", "elbow") +_FLIP_TOKENS = ("_roll_", "_yaw_") +_EXPLICIT_SIGNS = {"hand_thumb_0_joint": 1.0} + + +def _counterpart(name: str) -> str: + if name.startswith("left_"): + return "right_" + name[len("left_") :] + if name.startswith("right_"): + return "left_" + name[len("right_") :] + return name + + +def _sign_from_name(name: str) -> float | None: + if any(token in name for token in _PITCH_TOKENS): + return 1.0 + if any(token in name for token in _FLIP_TOKENS): + return -1.0 + for suffix, value in _EXPLICIT_SIGNS.items(): + if name.endswith(suffix): + return value + return None + + +def _build_map(env: ManagerBasedRLEnv) -> tuple[torch.Tensor, torch.Tensor]: + robot = env.scene["robot"] + names = list(robot.joint_names) + cached = getattr(env, "_g1_joint_mirror_map", None) + if cached is not None: + return cached + device = env.device + limits = robot.data.joint_pos_limits + limits = (limits.torch if hasattr(limits, "torch") else limits)[0] + default = robot.data.default_joint_pos + default = (default.torch if hasattr(default, "torch") else default)[0] + perm = torch.zeros(len(names), dtype=torch.long, device=device) + sign = torch.ones(len(names), device=device) + for i, name in enumerate(names): + j = names.index(_counterpart(name)) + perm[i] = j + lo_i, hi_i = (float(limits[i, 0]), float(limits[i, 1])) + lo_j, hi_j = (float(limits[j, 0]), float(limits[j, 1])) + same = abs(lo_i - lo_j) < 0.0001 and abs(hi_i - hi_j) < 0.0001 + flipped = abs(lo_i + hi_j) < 0.0001 and abs(hi_i + lo_j) < 0.0001 + by_name = _sign_from_name(name) + if same and (not flipped): + by_limits = 1.0 + elif flipped and (not same): + by_limits = -1.0 + else: + by_limits = None + if by_limits is not None and by_name is not None and (by_limits != by_name): + raise RuntimeError( + f"{name}: mirror sign disagrees between joint limits ({by_limits:+.0f}) and name ({by_name:+.0f})" + ) + resolved = by_limits if by_limits is not None else by_name + if resolved is None: + raise RuntimeError(f"{name}: neither the limits nor the name determine the mirror sign; add it explicitly") + sign[i] = resolved + mirrored_default = sign * default[perm] + if not torch.allclose(mirrored_default, default, atol=1e-05): + worst = int((mirrored_default - default).abs().argmax()) + raise RuntimeError( + f"Mirror changes the default pose of {names[worst]}: " + f"{float(default[worst]):+.4f} -> {float(mirrored_default[worst]):+.4f}" + ) + if not torch.equal(perm[perm], torch.arange(len(names), device=device)): + raise RuntimeError("the joint permutation is not an involution") + env._g1_joint_mirror_map = (perm, sign) + return (perm, sign) + + +def _mirror_joint_block(block: torch.Tensor, perm: torch.Tensor, sign: torch.Tensor) -> torch.Tensor: + return block[:, perm] * sign + + +def _selected_joint_map(env: ManagerBasedRLEnv, joint_ids: list[int] | slice) -> tuple[torch.Tensor, torch.Tensor]: + perm, sign = _build_map(env) + all_ids = tuple(range(perm.numel())) + selected = all_ids[joint_ids] if isinstance(joint_ids, slice) else tuple(joint_ids) + if selected == all_ids: + return (perm, sign) + cache = getattr(env, "_g1_joint_mirror_subsets", None) + if cache is None: + cache = env._g1_joint_mirror_subsets = {} + if selected not in cache: + names = env.scene["robot"].joint_names + selected_names = [names[i] for i in selected] + counterparts = [_counterpart(name) for name in selected_names] + if not selected or len(set(selected)) != len(selected) or any(n not in selected_names for n in counterparts): + raise ValueError("G1 joint selection must be nonempty, unique, and closed under reflection") + local_perm = torch.tensor([selected_names.index(n) for n in counterparts], device=perm.device) + cache[selected] = (local_perm, sign[list(selected)]) + return cache[selected] + + +def _action_joint_map(env: ManagerBasedRLEnv) -> tuple[torch.Tensor, torch.Tensor]: + cached = getattr(env, "_g1_action_mirror_map", None) + if cached is None: + if env.action_manager.active_terms != ["joint_pos"]: + raise ValueError("G1 symmetry expects one joint_pos action term") + cfg = env.action_manager.get_term("joint_pos").cfg + ids, _ = env.scene["robot"].find_joints(cfg.joint_names, preserve_order=cfg.preserve_order) + cached = env._g1_action_mirror_map = _selected_joint_map(env, ids) + return cached + + +def _observation_joint_map(env: ManagerBasedRLEnv, group: str, name: str) -> tuple[torch.Tensor, torch.Tensor]: + cfg = env.observation_manager.cfg + group_cfg = cfg[group] if isinstance(cfg, dict) else getattr(cfg, group) + term_cfg = group_cfg[name] if isinstance(group_cfg, dict) else getattr(group_cfg, name) + asset_cfg = term_cfg.params.get("asset_cfg") + if asset_cfg is None: + return _build_map(env) + if asset_cfg.name != "robot": + raise ValueError("G1 joint observations must refer to robot") + return _selected_joint_map(env, asset_cfg.joint_ids) + + +def _mirror_group(env: ManagerBasedRLEnv, group: str, obs: torch.Tensor) -> torch.Tensor: + out = obs.clone() + offset = 0 + names = env.observation_manager.active_terms[group] + dims = env.observation_manager.group_obs_term_dim[group] + for name, shape in zip(names, dims): + size = 1 + for s in shape: + size *= int(s) + block = obs[:, offset : offset + size] + if name in _VECTOR_MIRRORS: + factor = torch.tensor(_VECTOR_MIRRORS[name], device=obs.device) + out[:, offset : offset + size] = block * factor.repeat(size // 3) + elif name in _JOINT_TERMS: + perm, sign = _action_joint_map(env) if name == "actions" else _observation_joint_map(env, group, name) + if size % perm.numel(): + raise ValueError(f"{group}.{name}: observation size {size} does not match its joint selection") + frames = size // perm.numel() + reshaped = block.reshape(block.shape[0] * frames, perm.numel()) + out[:, offset : offset + size] = _mirror_joint_block(reshaped, perm, sign).reshape(block.shape) + elif name == "height_scan": + if size != _SCAN_ROWS * _SCAN_COLS: + raise ValueError(f"height_scan is {size} values, not the {_SCAN_ROWS}x{_SCAN_COLS} grid") + out[:, offset : offset + size] = block.view(-1, _SCAN_ROWS, _SCAN_COLS).flip(dims=[1]).reshape(block.shape) + else: + raise ValueError(f"no mirror defined for observation term {name!r}; refusing to guess") + offset += size + return out + + +@torch.no_grad() +def compute_symmetric_states( + env: ManagerBasedRLEnv, obs: TensorDict | None = None, actions: torch.Tensor | None = None +): + unwrapped = env.unwrapped + if obs is not None: + batch = obs.batch_size[0] + obs_aug = obs.repeat(2) + for group in obs.keys(): + obs_aug[group][:batch] = obs[group] + obs_aug[group][batch:] = _mirror_group(unwrapped, group, obs[group]) + else: + obs_aug = None + if actions is not None: + perm, sign = _action_joint_map(unwrapped) + actions_aug = torch.cat([actions, _mirror_joint_block(actions, perm, sign)], dim=0) + else: + actions_aug = None + return (obs_aug, actions_aug) diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/mdp/terminations.py b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/mdp/terminations.py index 37559032495a..547b4868fb5d 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/mdp/terminations.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/mdp/terminations.py @@ -48,3 +48,17 @@ def terrain_out_of_bounds( return torch.logical_or(x_out_of_bounds, y_out_of_bounds) else: raise ValueError("Received unsupported terrain type, must be either 'plane' or 'generator'.") + + +def pelvis_below_terrain_clearance( + env, + minimum_height: float, + asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), + sensor_cfg: SceneEntityCfg = SceneEntityCfg("height_scanner"), +) -> torch.Tensor: + """Terminate when the root sits less than ``minimum_height`` above the ground beneath it.""" + asset = env.scene[asset_cfg.name] + sensor = env.scene[sensor_cfg.name] + hits = sensor.data.ray_hits_w.torch[..., 2] + ground = torch.nan_to_num(hits, nan=0.0, posinf=0.0, neginf=0.0).median(dim=1).values + return asset.data.root_pos_w.torch[:, 2] - ground < minimum_height diff --git a/source/isaaclab_tasks/test/core/test_g1_distillation.py b/source/isaaclab_tasks/test/core/test_g1_distillation.py new file mode 100644 index 000000000000..5fdc30d36ae5 --- /dev/null +++ b/source/isaaclab_tasks/test/core/test_g1_distillation.py @@ -0,0 +1,241 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from types import SimpleNamespace + +import pytest +import torch +from tensordict import TensorDict + +from isaaclab.utils import to_dict + +from isaaclab_tasks.core.velocity.config.g1.agents.distillation import MirrorDistillation +from isaaclab_tasks.core.velocity.config.g1.agents.rsl_rl_ppo_cfg import G1RoughDepthStudentRunnerCfg + + +def test_mirror_distillation_load_normalize_and_update(): + """PPO loading starts a new student; each rollout step normalizes both reflections exactly once.""" + torch.manual_seed(42) + robot = SimpleNamespace( + joint_names=["left_shoulder_roll_joint", "right_shoulder_roll_joint"], + data=SimpleNamespace( + joint_pos_limits=torch.tensor([[[-0.5, 1.0], [-1.0, 0.5]]]), + default_joint_pos=torch.tensor([[0.16, -0.16]]), + ), + find_joints=lambda *args, **kwargs: ([0, 1], ["left_shoulder_roll_joint", "right_shoulder_roll_joint"]), + ) + env = SimpleNamespace( + num_envs=2, + num_actions=2, + device="cpu", + scene={"robot": robot}, + action_manager=SimpleNamespace( + active_terms=["joint_pos"], + get_term=lambda name: SimpleNamespace(cfg=SimpleNamespace(joint_names=[".*"], preserve_order=False)), + ), + observation_manager=SimpleNamespace( + active_terms={ + "policy": ["base_ang_vel", "joint_pos", "actions"], + "teacher": ["base_ang_vel", "joint_pos", "actions", "height_scan"], + }, + group_obs_term_dim={"policy": [(15,), (10,), (10,)], "teacher": [(3,), (2,), (2,), (187,)]}, + cfg=SimpleNamespace( + **{group: SimpleNamespace(joint_pos=SimpleNamespace(params={})) for group in ("policy", "teacher")} + ), + ), + ) + env.unwrapped = env + obs = TensorDict( + { + "policy": torch.randn(2, 35), + "teacher": torch.randn(2, 194), + "depth": torch.rand(2, 3, 38, 64), + "depth_mirror": torch.rand(2, 3, 38, 64), + }, + [2], + ) + cfg = to_dict(G1RoughDepthStudentRunnerCfg()) + cfg["multi_gpu"] = None + # Architecture width is irrelevant to loading, reflection and normalization semantics. + cfg["student"]["hidden_dims"] = cfg["teacher"]["hidden_dims"] = [16] + with pytest.warns(UserWarning, match="not divisible"): + alg = MirrorDistillation.construct_algorithm(obs, env, cfg, "cpu") + student_before = {k: v.clone() for k, v in alg.student.state_dict().items()} + teacher_state = {k: v.clone() + 0.01 for k, v in alg.teacher.state_dict().items()} + resumed = alg.load( + {"actor_state_dict": teacher_state, "iter": 5999}, + {"student": True, "teacher": True, "optimizer": False, "iteration": True}, + ) + assert alg.teacher_loaded and not resumed + assert not alg.optimizer.state + for name, value in alg.student.state_dict().items(): + torch.testing.assert_close(value, student_before[name]) + mirror = alg.mirror.observations(obs) + for name, value in alg.mirror.observations(mirror).items(): + torch.testing.assert_close(value, obs[name]) + torch.testing.assert_close( + mirror["policy"][:, :15].reshape(2, 5, 3), obs["policy"][:, :15].reshape(2, 5, 3) * torch.tensor([-1, 1, -1]) + ) + torch.testing.assert_close( + mirror["policy"][:, 15:25].reshape(2, 5, 2), -obs["policy"][:, 15:25].reshape(2, 5, 2).flip(-1) + ) + torch.testing.assert_close(mirror["depth"], obs["depth_mirror"].flip(-1)) + torch.testing.assert_close( + mirror["teacher"][:, 7:].reshape(2, 11, 17), obs["teacher"][:, 7:].reshape(2, 11, 17).flip(1) + ) + expected_samples = [] + for step in range(24): + with torch.no_grad(): + alg.act(obs) + obs = obs.clone() + obs["policy"] += 0.02 + expected_samples.extend([alg.mirror.observations(obs)["policy"], obs["policy"]]) + alg.process_env_step(obs, torch.zeros(2), torch.tensor([step % 3 == 0, False]), {}) + normalizer = alg.student.obs_normalizer + expected = torch.cat(expected_samples) + torch.testing.assert_close(normalizer._mean, expected.mean(0, keepdim=True)) + torch.testing.assert_close(normalizer._var, expected.var(0, unbiased=False, keepdim=True)) + before_update = {k: v.clone() for k, v in normalizer.state_dict().items()} + loss = alg.update()["behavior"] + assert torch.isfinite(torch.tensor(loss)) and loss > 0 + for name, value in normalizer.state_dict().items(): + torch.testing.assert_close(value, before_update[name]) + for name, value in alg.teacher.state_dict().items(): + torch.testing.assert_close(value, teacher_state[name]) + assert any( + not torch.equal(value, student_before[name]) + for name, value in alg.student.state_dict().items() + if name.startswith("mlp.") + ) + + +def test_support_edge_distance_on_a_step(): + """A sole hanging 8 cm over a step is charged only at its two unsupported corners.""" + import trimesh + + from isaaclab.utils.warp import convert_to_warp_mesh + + from isaaclab_tasks.core.velocity.mdp.rewards import sole_edge_distances + + step = trimesh.creation.box(extents=(1.0, 1.0, 1.0)) + step.apply_translation((0.0, 0.0, -0.5)) + mesh = convert_to_warp_mesh(step.vertices, step.faces, device="cpu") + xy = torch.cartesian_prod(torch.tensor([-0.1, 0.0, 0.1]), torch.tensor([-0.04, 0.0, 0.04])) + sole = torch.cat((xy, torch.zeros(9, 1)), dim=-1) + centers = torch.tensor([[0.0, 0.0, 0.0], [0.48, 0.0, 0.0], [2.0, 0.0, 0.0]]) + distances = sole_edge_distances(sole[None] + centers[:, None], mesh) + torch.testing.assert_close( + distances, + torch.tensor([[0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.08, 0.08], [0.25, 0.25, 0.25, 0.25]]), + atol=1e-6, + rtol=0.0, + ) + + +def test_asset_geometry_and_action_parameters(tmp_path): + """Action scales use authored drives and configured gains, with task-specific torque overrides.""" + from pxr import Usd, UsdGeom, UsdPhysics + + from isaaclab.actuators import ImplicitActuatorCfg + from isaaclab.assets import ArticulationCfg + from isaaclab.sim import UsdFileCfg + + from isaaclab_tasks.core.velocity.config.g1.rough_env_cfg import get_action_parameters + + path = tmp_path / "g1.usda" + stage = Usd.Stage.CreateNew(str(path)) + root = UsdGeom.Xform.Define(stage, "/Robot") + stage.SetDefaultPrim(root.GetPrim()) + body = UsdGeom.Xform.Define(stage, "/Robot/left_ankle_roll_link") + body.AddTranslateOp().Set((10.0, 20.0, 30.0)) + plate = UsdGeom.Cube.Define(stage, "/Robot/left_ankle_roll_link/foot_plate") + plate.CreateSizeAttr(2.0) + plate.AddTranslateOp().Set((0.04, 0.005, -0.02)) + plate.AddScaleOp().Set((0.1, 0.03, 0.01)) + torques = { + "left_knee_joint": 139.0, + "left_hip_roll_joint": 139.0, + "left_ankle_pitch_joint": 50.0, + "left_hand_index_0_joint": 1.4, + } + for name, torque in torques.items(): + joint = UsdPhysics.RevoluteJoint.Define(stage, f"/Robot/{name}") + drive = UsdPhysics.DriveAPI.Apply(joint.GetPrim(), "angular") + drive.CreateMaxForceAttr(torque) + drive.CreateStiffnessAttr(999.0) + stage.GetRootLayer().Save() + cfg = ArticulationCfg( + prim_path="/Robot", + spawn=UsdFileCfg(usd_path=str(path)), + actuators={ + "legs": ImplicitActuatorCfg( + joint_names_expr=[".*_knee_joint", ".*_hip_roll_joint"], + stiffness={".*_knee_joint": 200.0, ".*_hip_roll_joint": 150.0}, + damping=5.0, + joint_effort_limit=1.0, + ), + "feet": ImplicitActuatorCfg(joint_names_expr=[".*_ankle_pitch_joint"], stiffness=20.0, damping=2.0), + "hands": ImplicitActuatorCfg(joint_names_expr=[".*_hand_.*"], stiffness=40.0, damping=10.0), + }, + ) + scales, size, offset = get_action_parameters(cfg) + assert scales == { + "left_knee_joint": 0.1738, + "left_hip_roll_joint": 0.1467, + "left_ankle_pitch_joint": 0.625, + "left_hand_index_0_joint": 0.0153, + } + assert size == pytest.approx((0.2, 0.06, 0.02)) + assert offset == pytest.approx((0.04, 0.005, -0.02)) + + UsdPhysics.DriveAPI.Get(stage.GetPrimAtPath("/Robot/left_ankle_pitch_joint"), "angular").GetMaxForceAttr().Set(60.0) + stage.GetRootLayer().Save() + cfg.actuators["feet"].stiffness = 30.0 + updated, _, _ = get_action_parameters(cfg) + assert updated["left_ankle_pitch_joint"] == 0.5 + assert scales["left_ankle_pitch_joint"] == 0.625 + + +def test_depth_history_orientation_and_partial_reset(monkeypatch): + """Deployment keeps oldest-to-newest channels and the calibrated 180-degree image rotation.""" + from isaaclab.managers import ObservationManager + + from isaaclab_tasks.core.velocity.config.g1 import rough_env_cfg + + monkeypatch.setattr( + rough_env_cfg, "get_action_parameters", lambda cfg: (1.0, (0.2, 0.06, 0.02), (0.03, 0.0, -0.02)) + ) + observations = rough_env_cfg.G1RoughDepthStudentEnvCfg().observations + + class CameraScene(dict): + @property + def sensors(self): + return self + + pixels = torch.tensor([[[[0.0], [0.1]], [[0.2], [0.3]]]]).repeat(2, 1, 1, 1) + camera = SimpleNamespace(data=SimpleNamespace(output={"distance_to_image_plane": SimpleNamespace(torch=pixels)})) + env = SimpleNamespace( + num_envs=2, + device="cpu", + sim=SimpleNamespace(is_playing=lambda: True), + scene=CameraScene(depth_camera=camera), + ) + manager = ObservationManager(SimpleNamespace(depth=observations.depth), env) + rotated = torch.tensor([[[[0.3, 0.2], [0.1, 0.0]]]]).repeat(2, 1, 1, 1) + first = manager.compute()["depth"] + torch.testing.assert_close(first, rotated.repeat(1, 3, 1, 1) / 3) + pixels.add_(0.4) + second = manager.compute()["depth"] + torch.testing.assert_close(second, torch.cat([rotated, rotated, rotated + 0.4], dim=1) / 3) + manager.reset([0]) + pixels.add_(0.4) + third = manager.compute()["depth"] + torch.testing.assert_close(third[0:1], (rotated[0:1] + 0.8).repeat(1, 3, 1, 1) / 3) + torch.testing.assert_close(third[1:2], torch.cat([rotated[1:2], rotated[1:2] + 0.4, rotated[1:2] + 0.8], dim=1) / 3) + torch.testing.assert_close(first, rotated.repeat(1, 3, 1, 1) / 3) + manager.reset() + pixels.copy_(torch.tensor([[[[float("nan")], [float("inf")]], [[float("-inf")], [4.0]]]])) + expected = torch.tensor([[[[1.0, 0.0], [1.0, 1.0]]]]).repeat(2, 3, 1, 1) + torch.testing.assert_close(manager.compute()["depth"], expected) From 5f0377b4a7a2e07fde9f5917f79ca5ed4838dc74 Mon Sep 17 00:00:00 2001 From: henry Date: Thu, 22 Oct 2026 09:48:52 +0800 Subject: [PATCH 02/18] Clarify G1 training and deployment guide title --- ...raining_and_deploying_a_g1_visual_locomotion_policy.rst} | 6 +++--- docs/source/policy_deployment/index.rst | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) rename docs/source/policy_deployment/07_g1_sim2real/{training_a_g1_visual_locomotion_policy.rst => training_and_deploying_a_g1_visual_locomotion_policy.rst} (98%) diff --git a/docs/source/policy_deployment/07_g1_sim2real/training_a_g1_visual_locomotion_policy.rst b/docs/source/policy_deployment/07_g1_sim2real/training_and_deploying_a_g1_visual_locomotion_policy.rst similarity index 98% rename from docs/source/policy_deployment/07_g1_sim2real/training_a_g1_visual_locomotion_policy.rst rename to docs/source/policy_deployment/07_g1_sim2real/training_and_deploying_a_g1_visual_locomotion_policy.rst index 648368da41da..e71ba8d4074d 100644 --- a/docs/source/policy_deployment/07_g1_sim2real/training_a_g1_visual_locomotion_policy.rst +++ b/docs/source/policy_deployment/07_g1_sim2real/training_and_deploying_a_g1_visual_locomotion_policy.rst @@ -1,5 +1,5 @@ -Training a G1 Visual Locomotion Policy for Real Robot Deployment -================================================================ +Training and Deploying a G1 Visual Locomotion Policy +==================================================== .. currentmodule:: isaaclab @@ -124,7 +124,7 @@ Running on the Robot -------------------- Resolving the Connections Against Hardware -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ :class:`~envs.LeappDeploymentEnv` resolves each ``isaaclab_connection`` in the exported YAML against a simulated scene. Hardware deployment resolves the same connections against the robot: diff --git a/docs/source/policy_deployment/index.rst b/docs/source/policy_deployment/index.rst index 549339e76e16..d75d018c50b5 100644 --- a/docs/source/policy_deployment/index.rst +++ b/docs/source/policy_deployment/index.rst @@ -13,4 +13,4 @@ The guides below cover task-specific configuration and deployment tools. 04_reach/reach_policy 05_leapp/exporting_policies_with_leapp 06_so101_typing/so101_typing_policy - 07_g1_sim2real/training_a_g1_visual_locomotion_policy + 07_g1_sim2real/training_and_deploying_a_g1_visual_locomotion_policy From 406fa3af4d6f3d0edaa9a9bc191d0ac69a23c307 Mon Sep 17 00:00:00 2001 From: henry Date: Thu, 22 Oct 2026 10:14:13 +0800 Subject: [PATCH 03/18] Show G1 simulation and hardware demonstrations in deployment guide --- ...ng_and_deploying_a_g1_visual_locomotion_policy.rst | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/docs/source/policy_deployment/07_g1_sim2real/training_and_deploying_a_g1_visual_locomotion_policy.rst b/docs/source/policy_deployment/07_g1_sim2real/training_and_deploying_a_g1_visual_locomotion_policy.rst index e71ba8d4074d..302a77cd71ac 100644 --- a/docs/source/policy_deployment/07_g1_sim2real/training_and_deploying_a_g1_visual_locomotion_policy.rst +++ b/docs/source/policy_deployment/07_g1_sim2real/training_and_deploying_a_g1_visual_locomotion_policy.rst @@ -24,6 +24,17 @@ continues from there onto hardware. The inference stage is unchanged — it is t 3. export pipeline .yaml + .onnx + _initial_values.safetensors 4. deploy scripts/deploy/g1_deploy.py +.. raw:: html + + + Prerequisites ------------- From a005bf07eb2065f6daf8c9da4279f4732238f87e Mon Sep 17 00:00:00 2001 From: henry Date: Thu, 22 Oct 2026 10:21:52 +0800 Subject: [PATCH 04/18] Display G1 demonstrations at full content width --- .../training_and_deploying_a_g1_visual_locomotion_policy.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/source/policy_deployment/07_g1_sim2real/training_and_deploying_a_g1_visual_locomotion_policy.rst b/docs/source/policy_deployment/07_g1_sim2real/training_and_deploying_a_g1_visual_locomotion_policy.rst index 302a77cd71ac..4af27424f127 100644 --- a/docs/source/policy_deployment/07_g1_sim2real/training_and_deploying_a_g1_visual_locomotion_policy.rst +++ b/docs/source/policy_deployment/07_g1_sim2real/training_and_deploying_a_g1_visual_locomotion_policy.rst @@ -26,7 +26,7 @@ continues from there onto hardware. The inference stage is unchanged — it is t .. raw:: html -
+
G1 on blocks: PhysX, Newton (MJWarp), and real robot From 35216355dd9998dc92e1cbb2bad11a617f098d35 Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Wed, 21 Oct 2026 19:54:23 -0700 Subject: [PATCH 05/18] Consolidate rough G1 task under teacher configurations --- ...eploying_a_g1_visual_locomotion_policy.rst | 16 ++-- .../g1-teacher-configs.changed.rst | 4 + .../changelog.d/g1-teacher-configs.major | 0 .../core/velocity/config/g1/__init__.py | 15 +--- .../config/g1/agents/rsl_rl_ppo_cfg.py | 44 +++++----- .../config/g1/agents/skrl_rough_ppo_cfg.yaml | 85 ------------------- .../core/velocity/config/g1/flat_env_cfg.py | 26 +++++- .../core/velocity/config/g1/rough_env_cfg.py | 79 +++++++---------- .../test/core/test_velocity_newton_cfg.py | 37 +++++++- 9 files changed, 117 insertions(+), 189 deletions(-) create mode 100644 source/isaaclab_tasks/changelog.d/g1-teacher-configs.changed.rst create mode 100644 source/isaaclab_tasks/changelog.d/g1-teacher-configs.major delete mode 100644 source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/agents/skrl_rough_ppo_cfg.yaml diff --git a/docs/source/policy_deployment/07_g1_sim2real/training_and_deploying_a_g1_visual_locomotion_policy.rst b/docs/source/policy_deployment/07_g1_sim2real/training_and_deploying_a_g1_visual_locomotion_policy.rst index 4af27424f127..b4dd2a0347ba 100644 --- a/docs/source/policy_deployment/07_g1_sim2real/training_and_deploying_a_g1_visual_locomotion_policy.rst +++ b/docs/source/policy_deployment/07_g1_sim2real/training_and_deploying_a_g1_visual_locomotion_policy.rst @@ -19,7 +19,7 @@ continues from there onto hardware. The inference stage is unchanged — it is t .. code-block:: text - 1. train Isaac-Velocity-Rough-G1-Teacher + 1. train Isaac-Velocity-Rough-G1 2. distill Isaac-Velocity-Rough-G1-Depth-Student 3. export pipeline .yaml + .onnx + _initial_values.safetensors 4. deploy scripts/deploy/g1_deploy.py @@ -60,15 +60,11 @@ neither, which is why the second stage exists. .. code-block:: bash uv run isaaclab train --rl_library rsl_rl \ - --task Isaac-Velocity-Rough-G1-Teacher - -The teacher runs on ``g1_29dof.usd``, not on the ``G1_MINIMAL_CFG`` the stock -``Isaac-Velocity-Rough-G1`` task uses. That asset comes from a superseded URDF whose waist is a -single ``torso_joint`` and whose forearm is ``elbow_pitch``/``elbow_roll``; the hardware has a -three-axis waist, one elbow and three wrist joints, and none of those names exist on the old -model. A reward term whose joint regex matches nothing is silently inert, so the mismatch does not -raise -- it just trains a different robot. ``G1RoughTeacherEnvCfg`` swaps the asset and respells -the joint names of every reward that referred to the old ones. + --task Isaac-Velocity-Rough-G1 + +``Isaac-Velocity-Rough-G1`` uses ``G1RoughTeacherEnvCfg`` with ``g1_29dof.usd``. Its rewards +reference that asset's three-axis waist, elbow, wrist and hand joints. The flat G1 task retains +its existing ``G1_MINIMAL_CFG`` asset. The default configuration uses 4096 environments, 6000 PPO iterations, Newton mesh terrain diff --git a/source/isaaclab_tasks/changelog.d/g1-teacher-configs.changed.rst b/source/isaaclab_tasks/changelog.d/g1-teacher-configs.changed.rst new file mode 100644 index 000000000000..fcf05bbec6db --- /dev/null +++ b/source/isaaclab_tasks/changelog.d/g1-teacher-configs.changed.rst @@ -0,0 +1,4 @@ +* **Breaking:** Replaced the legacy rough G1 configuration with the teacher configuration under + ``Isaac-Velocity-Rough-G1``. Changed callers of ``Isaac-Velocity-Rough-G1-Teacher`` to use that task ID; + callers of ``G1RoughEnvCfg`` and ``G1RoughPPORunnerCfg`` must use ``G1RoughTeacherEnvCfg`` and + ``G1RoughTeacherPPORunnerCfg``. Preserved the teacher, depth-student and flat task settings. diff --git a/source/isaaclab_tasks/changelog.d/g1-teacher-configs.major b/source/isaaclab_tasks/changelog.d/g1-teacher-configs.major new file mode 100644 index 000000000000..e69de29bb2d1 diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/__init__.py index 1c4ee1b64f19..80eeee1b2177 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/__init__.py @@ -25,26 +25,15 @@ }, ) +# Keep one rough G1 registration, owned by the teacher configs. gym.register( id="Isaac-Velocity-Rough-G1", entry_point="isaaclab.envs:ManagerBasedRLEnv", disable_env_checker=True, - kwargs={ - "env_cfg_entry_point": f"{__name__}.rough_env_cfg:G1RoughEnvCfg", - "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:G1RoughPPORunnerCfg", - "default_agent": "rsl_rl", - "skrl_cfg_entry_point": f"{agents.__name__}:skrl_rough_ppo_cfg.yaml", - }, -) - - -gym.register( - id="Isaac-Velocity-Rough-G1-Teacher", - entry_point="isaaclab.envs:ManagerBasedRLEnv", - disable_env_checker=True, kwargs={ "env_cfg_entry_point": f"{__name__}.rough_env_cfg:G1RoughTeacherEnvCfg", "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:G1RoughTeacherPPORunnerCfg", + "default_agent": "rsl_rl", }, ) diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/agents/rsl_rl_ppo_cfg.py index 3aa1bd54aeb4..421dcd5fda3e 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/agents/rsl_rl_ppo_cfg.py @@ -15,13 +15,21 @@ RslRlSymmetryCfg, ) +G1_SYMMETRY_CFG = RslRlSymmetryCfg( + use_data_augmentation=True, + use_mirror_loss=False, + data_augmentation_func="isaaclab_tasks.core.velocity.mdp.symmetry.g1:compute_symmetric_states", + mirror_loss_coeff=0.0, +) + @configclass -class G1RoughPPORunnerCfg(RslRlOnPolicyRunnerCfg): +class G1RoughTeacherPPORunnerCfg(RslRlOnPolicyRunnerCfg): + seed = 42 num_steps_per_env = 24 - max_iterations = 5000 - save_interval = 50 - experiment_name = "g1_rough" + max_iterations = 6000 + save_interval = 250 + experiment_name = "g1_rough_teacher" obs_groups = {"actor": ["policy"], "critic": ["policy"]} actor = RslRlMLPModelCfg( hidden_dims=[512, 256, 128], @@ -47,35 +55,21 @@ class G1RoughPPORunnerCfg(RslRlOnPolicyRunnerCfg): lam=0.95, desired_kl=0.01, max_grad_norm=1.0, + symmetry_cfg=G1_SYMMETRY_CFG, ) @configclass -class G1FlatPPORunnerCfg(G1RoughPPORunnerCfg): +class G1FlatPPORunnerCfg(G1RoughTeacherPPORunnerCfg): + max_iterations = 1500 + save_interval = 50 + experiment_name = "g1_flat" + def __post_init__(self): super().__post_init__() - - self.max_iterations = 1500 - self.experiment_name = "g1_flat" self.actor.hidden_dims = [256, 128, 128] self.critic.hidden_dims = [256, 128, 128] - - -@configclass -class G1RoughTeacherPPORunnerCfg(G1RoughPPORunnerCfg): - seed = 42 - max_iterations = 6000 - save_interval = 250 - experiment_name = "g1_rough_teacher" - - def __post_init__(self): - super().__post_init__() - self.algorithm.symmetry_cfg = RslRlSymmetryCfg( - use_data_augmentation=True, - use_mirror_loss=False, - data_augmentation_func="isaaclab_tasks.core.velocity.mdp.symmetry.g1:compute_symmetric_states", - mirror_loss_coeff=0.0, - ) + self.algorithm.symmetry_cfg = None @configclass diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/agents/skrl_rough_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/agents/skrl_rough_ppo_cfg.yaml deleted file mode 100644 index b54682b45cd1..000000000000 --- a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/agents/skrl_rough_ppo_cfg.yaml +++ /dev/null @@ -1,85 +0,0 @@ -# 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 - -seed: 42 - - -# Models are instantiated using skrl's model instantiator utility -# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html -models: - separate: False - policy: # see gaussian_model parameters - class: GaussianMixin - clip_actions: False - clip_log_std: True - min_log_std: -20.0 - max_log_std: 2.0 - initial_log_std: 0.0 - network: - - name: net - input: OBSERVATIONS - layers: [512, 256, 128] - activations: elu - output: ACTIONS - value: # see deterministic_model parameters - class: DeterministicMixin - clip_actions: False - network: - - name: net - input: OBSERVATIONS - layers: [512, 256, 128] - activations: elu - output: ONE - - -# Rollout memory -# https://skrl.readthedocs.io/en/latest/api/memories/random.html -memory: - class: RandomMemory - memory_size: -1 # automatically determined (same as agent:rollouts) - - -# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) -# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html -agent: - class: PPO - rollouts: 24 - learning_epochs: 5 - mini_batches: 4 - discount_factor: 0.99 - lambda: 0.95 - learning_rate: 1.0e-03 - learning_rate_scheduler: KLAdaptiveLR - learning_rate_scheduler_kwargs: - kl_threshold: 0.01 - state_preprocessor: null - state_preprocessor_kwargs: null - value_preprocessor: null - value_preprocessor_kwargs: null - random_timesteps: 0 - learning_starts: 0 - grad_norm_clip: 1.0 - ratio_clip: 0.2 - value_clip: 0.2 - clip_predicted_values: True - entropy_loss_scale: 0.008 - value_loss_scale: 1.0 - kl_threshold: 0.0 - rewards_shaper_scale: 1.0 - time_limit_bootstrap: False - # logging and checkpoint - experiment: - directory: "g1_rough" - experiment_name: "" - write_interval: auto - checkpoint_interval: auto - - -# Sequential trainer -# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html -trainer: - class: SequentialTrainer - timesteps: 72000 - environment_info: log diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/flat_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/flat_env_cfg.py index 45086e8054e2..674d9cb4c7b1 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/flat_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/flat_env_cfg.py @@ -6,15 +6,20 @@ """Configuration for the Unitree G1 velocity-tracking environment on flat terrain.""" from isaaclab.managers import SceneEntityCfg -from isaaclab.utils import configclass +from isaaclab.utils import configclass, replace -from .rough_env_cfg import G1RoughEnvCfg +from isaaclab_assets import G1_MINIMAL_CFG + +from ...velocity_env_cfg import LocomotionVelocityRoughEnvCfg +from .rough_env_cfg import G1Rewards @configclass -class G1FlatEnvCfg(G1RoughEnvCfg): +class G1FlatEnvCfg(LocomotionVelocityRoughEnvCfg): """Configuration for the Unitree G1 velocity-tracking environment on flat terrain.""" + rewards: G1Rewards = G1Rewards() + def __post_init__(self): super().__post_init__() @@ -24,25 +29,40 @@ def __post_init__(self): newton_mjwarp.solver_cfg.nconmax = 10 self.sim.physics.default = newton_mjwarp # scene + self.scene.robot = replace(G1_MINIMAL_CFG, prim_path="{ENV_REGEX_NS}/Robot") self.scene.terrain.terrain_type = "plane" self.scene.terrain.terrain_generator = None self.scene.height_scanner = None # observations self.observations.policy.height_scan = None # commands + self.commands.base_velocity.vel_yaw_success_threshold = 0.8 + self.commands.base_velocity.marker_pos_offset = (0.0, 0.0, 0.75) self.commands.base_velocity.ranges.lin_vel_x = (0.0, 1.0) self.commands.base_velocity.ranges.lin_vel_y = (-0.5, 0.5) self.commands.base_velocity.ranges.ang_vel_z = (-1.0, 1.0) # rewards + self.rewards.undesired_contacts = None + self.rewards.flat_orientation_l2.weight = -1.0 self.rewards.track_ang_vel_z_exp.weight = 1.0 self.rewards.lin_vel_z_l2.weight = -0.2 self.rewards.action_rate_l2.weight = -0.005 self.rewards.dof_acc_l2.weight = -1.0e-7 + self.rewards.dof_acc_l2.params["asset_cfg"] = SceneEntityCfg( + "robot", joint_names=[".*_hip_.*", ".*_knee_joint"] + ) self.rewards.feet_air_time.weight = 0.75 self.rewards.feet_air_time.params["threshold"] = 0.4 self.rewards.dof_torques_l2.weight = -2.0e-6 self.rewards.dof_torques_l2.params["asset_cfg"] = SceneEntityCfg( "robot", joint_names=[".*_hip_.*", ".*_knee_joint"] ) + # terminations + self.terminations.base_contact.params["sensor_cfg"].body_names = "torso_link" + # events + self.events.add_base_mass.params["asset_cfg"].body_names = "torso_link" + self.events.base_com = None + self.events.base_external_force_torque.params["asset_cfg"].body_names = "torso_link" + self.events.reset_robot_joints.params["position_range"] = (1.0, 1.0) # curriculum self.curriculum.terrain_levels = None diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/rough_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/rough_env_cfg.py index a15e0393a24c..e6be582b23e0 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/rough_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/rough_env_cfg.py @@ -5,10 +5,9 @@ """Configurations for the Unitree G1 velocity-tracking environment on rough terrain. -Beside the standard environment this module holds the two stages of the depth pipeline: a teacher -whose actor reads a terrain height scan and the base linear velocity, and a student that replaces -both with a chest-mounted depth camera and is distilled from the teacher in the teacher's own -environment. +This module holds the two stages of the depth pipeline: a teacher whose actor reads a terrain +height scan and the base linear velocity, and a student that replaces both with a chest-mounted +depth camera and is distilled from the teacher in the teacher's own environment. """ import math @@ -35,8 +34,6 @@ from isaaclab_tasks.utils.presets import MultiBackendRendererCfg -from isaaclab_assets import G1_MINIMAL_CFG - from ... import mdp from ...velocity_env_cfg import ( LocomotionVelocityRoughEnvCfg, @@ -128,48 +125,6 @@ class G1Rewards(RewardsCfg): ) -@configclass -class G1RoughEnvCfg(LocomotionVelocityRoughEnvCfg): - """Configuration for the Unitree G1 velocity-tracking environment on rough terrain.""" - - rewards: G1Rewards = G1Rewards() - - def __post_init__(self): - super().__post_init__() - - # physics - self.sim.physics.newton_mjwarp.solver_cfg.njmax = 300 - # scene - self.scene.robot = replace(G1_MINIMAL_CFG, prim_path="{ENV_REGEX_NS}/Robot") - self.scene.height_scanner.prim_path = "{ENV_REGEX_NS}/Robot/torso_link" - # commands - self.commands.base_velocity.vel_yaw_success_threshold = 0.8 - self.commands.base_velocity.marker_pos_offset = (0.0, 0.0, 0.75) - 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) - # rewards - self.rewards.lin_vel_z_l2.weight = 0.0 - self.rewards.undesired_contacts = None - self.rewards.flat_orientation_l2.weight = -1.0 - self.rewards.action_rate_l2.weight = -0.005 - self.rewards.dof_acc_l2.weight = -1.25e-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_.*"] - ) - # terminations - self.terminations.base_contact.params["sensor_cfg"].body_names = "torso_link" - # events - self.events.add_base_mass.params["asset_cfg"].body_names = "torso_link" - self.events.base_com = None - self.events.base_external_force_torque.params["asset_cfg"].body_names = "torso_link" - self.events.reset_robot_joints.params["position_range"] = (1.0, 1.0) - - DEPTH_WIDTH = 64 DEPTH_HEIGHT = 38 @@ -299,12 +254,38 @@ def get_action_parameters(robot_cfg: ArticulationCfg) -> tuple[dict[str, float], @configclass -class G1RoughTeacherEnvCfg(G1RoughEnvCfg): +class G1RoughTeacherEnvCfg(LocomotionVelocityRoughEnvCfg): """G1 teacher on mesh terrain with terrain-height observations.""" + rewards: G1Rewards = G1Rewards() + def __post_init__(self): super().__post_init__() + self.sim.physics.newton_mjwarp.solver_cfg.njmax = 300 self.scene.robot = replace(G1_29DOF_VELOCITY_CFG, prim_path="{ENV_REGEX_NS}/Robot") + self.scene.height_scanner.prim_path = "{ENV_REGEX_NS}/Robot/torso_link" + self.commands.base_velocity.vel_yaw_success_threshold = 0.8 + self.commands.base_velocity.marker_pos_offset = (0.0, 0.0, 0.75) + 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) + self.rewards.lin_vel_z_l2.weight = 0.0 + self.rewards.undesired_contacts = None + self.rewards.flat_orientation_l2.weight = -1.0 + self.rewards.action_rate_l2.weight = -0.005 + self.rewards.dof_acc_l2.weight = -1.25e-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_.*"] + ) + self.terminations.base_contact.params["sensor_cfg"].body_names = "torso_link" + self.events.add_base_mass.params["asset_cfg"].body_names = "torso_link" + self.events.base_com = None + self.events.base_external_force_torque.params["asset_cfg"].body_names = "torso_link" + self.events.reset_robot_joints.params["position_range"] = (1.0, 1.0) self.rewards.joint_deviation_arms.params["asset_cfg"] = SceneEntityCfg( "robot", joint_names=[".*_shoulder_.*_joint", ".*_elbow_joint", ".*_wrist_.*_joint"], diff --git a/source/isaaclab_tasks/test/core/test_velocity_newton_cfg.py b/source/isaaclab_tasks/test/core/test_velocity_newton_cfg.py index 943de458fc61..37ee4fee5e01 100644 --- a/source/isaaclab_tasks/test/core/test_velocity_newton_cfg.py +++ b/source/isaaclab_tasks/test/core/test_velocity_newton_cfg.py @@ -3,15 +3,44 @@ # # SPDX-License-Identifier: BSD-3-Clause +import gymnasium as gym from isaaclab_newton.physics import MJWarpSolverCfg -from isaaclab_tasks.core.velocity.config.g1.rough_env_cfg import G1RoughEnvCfg +from isaaclab_tasks.core.velocity.config.g1 import rough_env_cfg +from isaaclab_tasks.core.velocity.config.g1.agents import rsl_rl_ppo_cfg +from isaaclab_tasks.utils import load_cfg_from_registry +from isaaclab_assets import G1_MINIMAL_CFG -def test_g1_rough_newton_has_sufficient_constraint_capacity(): - env_cfg = G1RoughEnvCfg() - solver_cfg = env_cfg.sim.physics.newton_mjwarp.solver_cfg +def test_g1_rough_teacher_registration_and_newton_capacity(monkeypatch): + """Rough G1 resolves to one teacher task and retains sufficient constraint capacity.""" + monkeypatch.setattr( + rough_env_cfg, "get_action_parameters", lambda cfg: (1.0, (0.2, 0.06, 0.02), (0.03, 0.0, -0.02)) + ) + env_cfg = load_cfg_from_registry("Isaac-Velocity-Rough-G1", "env_cfg_entry_point") + runner_cfg = load_cfg_from_registry("Isaac-Velocity-Rough-G1", "rsl_rl_cfg_entry_point") + assert isinstance(env_cfg, rough_env_cfg.G1RoughTeacherEnvCfg) + assert isinstance(runner_cfg, rsl_rl_ppo_cfg.G1RoughTeacherPPORunnerCfg) + solver_cfg = env_cfg.sim.physics.newton_mjwarp.solver_cfg assert isinstance(solver_cfg, MJWarpSolverCfg) assert solver_cfg.njmax == 300 + assert runner_cfg.algorithm.symmetry_cfg.use_data_augmentation + assert "Isaac-Velocity-Rough-G1-Teacher" not in gym.registry + assert not hasattr(rough_env_cfg, "G1RoughEnvCfg") + assert not hasattr(rsl_rl_ppo_cfg, "G1RoughPPORunnerCfg") + + +def test_g1_flat_keeps_its_robot_and_ppo_settings(): + """The flat task keeps its original asset and PPO settings when rough G1 becomes the teacher.""" + env_cfg = load_cfg_from_registry("Isaac-Velocity-Flat-G1", "env_cfg_entry_point") + runner_cfg = load_cfg_from_registry("Isaac-Velocity-Flat-G1", "rsl_rl_cfg_entry_point") + + assert env_cfg.scene.robot.spawn.usd_path == G1_MINIMAL_CFG.spawn.usd_path + assert env_cfg.scene.terrain.terrain_type == "plane" + assert env_cfg.scene.height_scanner is None + assert runner_cfg.algorithm.symmetry_cfg is None + assert runner_cfg.actor.hidden_dims == runner_cfg.critic.hidden_dims == [256, 128, 128] + assert runner_cfg.max_iterations == 1500 + assert runner_cfg.save_interval == 50 From b3a9f58463d3d72a7dd9e2831133df08e7c77c0e Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Wed, 21 Oct 2026 19:57:23 -0700 Subject: [PATCH 06/18] Remove G1 distillation test file --- .../test/core/test_g1_distillation.py | 241 ------------------ 1 file changed, 241 deletions(-) delete mode 100644 source/isaaclab_tasks/test/core/test_g1_distillation.py diff --git a/source/isaaclab_tasks/test/core/test_g1_distillation.py b/source/isaaclab_tasks/test/core/test_g1_distillation.py deleted file mode 100644 index 5fdc30d36ae5..000000000000 --- a/source/isaaclab_tasks/test/core/test_g1_distillation.py +++ /dev/null @@ -1,241 +0,0 @@ -# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). -# All rights reserved. -# -# SPDX-License-Identifier: BSD-3-Clause - -from types import SimpleNamespace - -import pytest -import torch -from tensordict import TensorDict - -from isaaclab.utils import to_dict - -from isaaclab_tasks.core.velocity.config.g1.agents.distillation import MirrorDistillation -from isaaclab_tasks.core.velocity.config.g1.agents.rsl_rl_ppo_cfg import G1RoughDepthStudentRunnerCfg - - -def test_mirror_distillation_load_normalize_and_update(): - """PPO loading starts a new student; each rollout step normalizes both reflections exactly once.""" - torch.manual_seed(42) - robot = SimpleNamespace( - joint_names=["left_shoulder_roll_joint", "right_shoulder_roll_joint"], - data=SimpleNamespace( - joint_pos_limits=torch.tensor([[[-0.5, 1.0], [-1.0, 0.5]]]), - default_joint_pos=torch.tensor([[0.16, -0.16]]), - ), - find_joints=lambda *args, **kwargs: ([0, 1], ["left_shoulder_roll_joint", "right_shoulder_roll_joint"]), - ) - env = SimpleNamespace( - num_envs=2, - num_actions=2, - device="cpu", - scene={"robot": robot}, - action_manager=SimpleNamespace( - active_terms=["joint_pos"], - get_term=lambda name: SimpleNamespace(cfg=SimpleNamespace(joint_names=[".*"], preserve_order=False)), - ), - observation_manager=SimpleNamespace( - active_terms={ - "policy": ["base_ang_vel", "joint_pos", "actions"], - "teacher": ["base_ang_vel", "joint_pos", "actions", "height_scan"], - }, - group_obs_term_dim={"policy": [(15,), (10,), (10,)], "teacher": [(3,), (2,), (2,), (187,)]}, - cfg=SimpleNamespace( - **{group: SimpleNamespace(joint_pos=SimpleNamespace(params={})) for group in ("policy", "teacher")} - ), - ), - ) - env.unwrapped = env - obs = TensorDict( - { - "policy": torch.randn(2, 35), - "teacher": torch.randn(2, 194), - "depth": torch.rand(2, 3, 38, 64), - "depth_mirror": torch.rand(2, 3, 38, 64), - }, - [2], - ) - cfg = to_dict(G1RoughDepthStudentRunnerCfg()) - cfg["multi_gpu"] = None - # Architecture width is irrelevant to loading, reflection and normalization semantics. - cfg["student"]["hidden_dims"] = cfg["teacher"]["hidden_dims"] = [16] - with pytest.warns(UserWarning, match="not divisible"): - alg = MirrorDistillation.construct_algorithm(obs, env, cfg, "cpu") - student_before = {k: v.clone() for k, v in alg.student.state_dict().items()} - teacher_state = {k: v.clone() + 0.01 for k, v in alg.teacher.state_dict().items()} - resumed = alg.load( - {"actor_state_dict": teacher_state, "iter": 5999}, - {"student": True, "teacher": True, "optimizer": False, "iteration": True}, - ) - assert alg.teacher_loaded and not resumed - assert not alg.optimizer.state - for name, value in alg.student.state_dict().items(): - torch.testing.assert_close(value, student_before[name]) - mirror = alg.mirror.observations(obs) - for name, value in alg.mirror.observations(mirror).items(): - torch.testing.assert_close(value, obs[name]) - torch.testing.assert_close( - mirror["policy"][:, :15].reshape(2, 5, 3), obs["policy"][:, :15].reshape(2, 5, 3) * torch.tensor([-1, 1, -1]) - ) - torch.testing.assert_close( - mirror["policy"][:, 15:25].reshape(2, 5, 2), -obs["policy"][:, 15:25].reshape(2, 5, 2).flip(-1) - ) - torch.testing.assert_close(mirror["depth"], obs["depth_mirror"].flip(-1)) - torch.testing.assert_close( - mirror["teacher"][:, 7:].reshape(2, 11, 17), obs["teacher"][:, 7:].reshape(2, 11, 17).flip(1) - ) - expected_samples = [] - for step in range(24): - with torch.no_grad(): - alg.act(obs) - obs = obs.clone() - obs["policy"] += 0.02 - expected_samples.extend([alg.mirror.observations(obs)["policy"], obs["policy"]]) - alg.process_env_step(obs, torch.zeros(2), torch.tensor([step % 3 == 0, False]), {}) - normalizer = alg.student.obs_normalizer - expected = torch.cat(expected_samples) - torch.testing.assert_close(normalizer._mean, expected.mean(0, keepdim=True)) - torch.testing.assert_close(normalizer._var, expected.var(0, unbiased=False, keepdim=True)) - before_update = {k: v.clone() for k, v in normalizer.state_dict().items()} - loss = alg.update()["behavior"] - assert torch.isfinite(torch.tensor(loss)) and loss > 0 - for name, value in normalizer.state_dict().items(): - torch.testing.assert_close(value, before_update[name]) - for name, value in alg.teacher.state_dict().items(): - torch.testing.assert_close(value, teacher_state[name]) - assert any( - not torch.equal(value, student_before[name]) - for name, value in alg.student.state_dict().items() - if name.startswith("mlp.") - ) - - -def test_support_edge_distance_on_a_step(): - """A sole hanging 8 cm over a step is charged only at its two unsupported corners.""" - import trimesh - - from isaaclab.utils.warp import convert_to_warp_mesh - - from isaaclab_tasks.core.velocity.mdp.rewards import sole_edge_distances - - step = trimesh.creation.box(extents=(1.0, 1.0, 1.0)) - step.apply_translation((0.0, 0.0, -0.5)) - mesh = convert_to_warp_mesh(step.vertices, step.faces, device="cpu") - xy = torch.cartesian_prod(torch.tensor([-0.1, 0.0, 0.1]), torch.tensor([-0.04, 0.0, 0.04])) - sole = torch.cat((xy, torch.zeros(9, 1)), dim=-1) - centers = torch.tensor([[0.0, 0.0, 0.0], [0.48, 0.0, 0.0], [2.0, 0.0, 0.0]]) - distances = sole_edge_distances(sole[None] + centers[:, None], mesh) - torch.testing.assert_close( - distances, - torch.tensor([[0.0, 0.0, 0.0, 0.0], [0.0, 0.0, 0.08, 0.08], [0.25, 0.25, 0.25, 0.25]]), - atol=1e-6, - rtol=0.0, - ) - - -def test_asset_geometry_and_action_parameters(tmp_path): - """Action scales use authored drives and configured gains, with task-specific torque overrides.""" - from pxr import Usd, UsdGeom, UsdPhysics - - from isaaclab.actuators import ImplicitActuatorCfg - from isaaclab.assets import ArticulationCfg - from isaaclab.sim import UsdFileCfg - - from isaaclab_tasks.core.velocity.config.g1.rough_env_cfg import get_action_parameters - - path = tmp_path / "g1.usda" - stage = Usd.Stage.CreateNew(str(path)) - root = UsdGeom.Xform.Define(stage, "/Robot") - stage.SetDefaultPrim(root.GetPrim()) - body = UsdGeom.Xform.Define(stage, "/Robot/left_ankle_roll_link") - body.AddTranslateOp().Set((10.0, 20.0, 30.0)) - plate = UsdGeom.Cube.Define(stage, "/Robot/left_ankle_roll_link/foot_plate") - plate.CreateSizeAttr(2.0) - plate.AddTranslateOp().Set((0.04, 0.005, -0.02)) - plate.AddScaleOp().Set((0.1, 0.03, 0.01)) - torques = { - "left_knee_joint": 139.0, - "left_hip_roll_joint": 139.0, - "left_ankle_pitch_joint": 50.0, - "left_hand_index_0_joint": 1.4, - } - for name, torque in torques.items(): - joint = UsdPhysics.RevoluteJoint.Define(stage, f"/Robot/{name}") - drive = UsdPhysics.DriveAPI.Apply(joint.GetPrim(), "angular") - drive.CreateMaxForceAttr(torque) - drive.CreateStiffnessAttr(999.0) - stage.GetRootLayer().Save() - cfg = ArticulationCfg( - prim_path="/Robot", - spawn=UsdFileCfg(usd_path=str(path)), - actuators={ - "legs": ImplicitActuatorCfg( - joint_names_expr=[".*_knee_joint", ".*_hip_roll_joint"], - stiffness={".*_knee_joint": 200.0, ".*_hip_roll_joint": 150.0}, - damping=5.0, - joint_effort_limit=1.0, - ), - "feet": ImplicitActuatorCfg(joint_names_expr=[".*_ankle_pitch_joint"], stiffness=20.0, damping=2.0), - "hands": ImplicitActuatorCfg(joint_names_expr=[".*_hand_.*"], stiffness=40.0, damping=10.0), - }, - ) - scales, size, offset = get_action_parameters(cfg) - assert scales == { - "left_knee_joint": 0.1738, - "left_hip_roll_joint": 0.1467, - "left_ankle_pitch_joint": 0.625, - "left_hand_index_0_joint": 0.0153, - } - assert size == pytest.approx((0.2, 0.06, 0.02)) - assert offset == pytest.approx((0.04, 0.005, -0.02)) - - UsdPhysics.DriveAPI.Get(stage.GetPrimAtPath("/Robot/left_ankle_pitch_joint"), "angular").GetMaxForceAttr().Set(60.0) - stage.GetRootLayer().Save() - cfg.actuators["feet"].stiffness = 30.0 - updated, _, _ = get_action_parameters(cfg) - assert updated["left_ankle_pitch_joint"] == 0.5 - assert scales["left_ankle_pitch_joint"] == 0.625 - - -def test_depth_history_orientation_and_partial_reset(monkeypatch): - """Deployment keeps oldest-to-newest channels and the calibrated 180-degree image rotation.""" - from isaaclab.managers import ObservationManager - - from isaaclab_tasks.core.velocity.config.g1 import rough_env_cfg - - monkeypatch.setattr( - rough_env_cfg, "get_action_parameters", lambda cfg: (1.0, (0.2, 0.06, 0.02), (0.03, 0.0, -0.02)) - ) - observations = rough_env_cfg.G1RoughDepthStudentEnvCfg().observations - - class CameraScene(dict): - @property - def sensors(self): - return self - - pixels = torch.tensor([[[[0.0], [0.1]], [[0.2], [0.3]]]]).repeat(2, 1, 1, 1) - camera = SimpleNamespace(data=SimpleNamespace(output={"distance_to_image_plane": SimpleNamespace(torch=pixels)})) - env = SimpleNamespace( - num_envs=2, - device="cpu", - sim=SimpleNamespace(is_playing=lambda: True), - scene=CameraScene(depth_camera=camera), - ) - manager = ObservationManager(SimpleNamespace(depth=observations.depth), env) - rotated = torch.tensor([[[[0.3, 0.2], [0.1, 0.0]]]]).repeat(2, 1, 1, 1) - first = manager.compute()["depth"] - torch.testing.assert_close(first, rotated.repeat(1, 3, 1, 1) / 3) - pixels.add_(0.4) - second = manager.compute()["depth"] - torch.testing.assert_close(second, torch.cat([rotated, rotated, rotated + 0.4], dim=1) / 3) - manager.reset([0]) - pixels.add_(0.4) - third = manager.compute()["depth"] - torch.testing.assert_close(third[0:1], (rotated[0:1] + 0.8).repeat(1, 3, 1, 1) / 3) - torch.testing.assert_close(third[1:2], torch.cat([rotated[1:2], rotated[1:2] + 0.4, rotated[1:2] + 0.8], dim=1) / 3) - torch.testing.assert_close(first, rotated.repeat(1, 3, 1, 1) / 3) - manager.reset() - pixels.copy_(torch.tensor([[[[float("nan")], [float("inf")]], [[float("-inf")], [4.0]]]])) - expected = torch.tensor([[[[1.0, 0.0], [1.0, 1.0]]]]).repeat(2, 3, 1, 1) - torch.testing.assert_close(manager.compute()["depth"], expected) From 039b322fcd0ed73fc56031026daa16e7fa01d7ee Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Wed, 21 Oct 2026 20:24:39 -0700 Subject: [PATCH 07/18] Compose G1 task configs and move generic MDP terms to core --- .../source/_static/css/environment-browser.js | 3 +- .../changelog.d/g1-generic-mdp.added.rst | 2 + .../isaaclab/changelog.d/g1-generic-mdp.minor | 0 .../isaaclab/isaaclab/envs/mdp/__init__.pyi | 12 + .../isaaclab/isaaclab/envs/mdp/curriculums.py | 7 + source/isaaclab/isaaclab/envs/mdp/rewards.py | 39 ++ .../isaaclab/envs/mdp/terminations.py | 14 + .../changelog.d/g1-shared-mdp.changed.rst | 8 +- .../core/velocity/config/g1/flat_env_cfg.py | 113 +++- .../core/velocity/config/g1/rough_env_cfg.py | 635 +++++++++++------- .../core/velocity/mdp/__init__.py | 5 +- .../core/velocity/mdp/__init__.pyi | 17 +- .../core/velocity/mdp/curriculums.py | 10 +- .../core/velocity/mdp/rewards.py | 42 -- .../core/velocity/mdp/terminations.py | 14 - .../test/core/test_velocity_newton_cfg.py | 44 ++ 16 files changed, 604 insertions(+), 361 deletions(-) create mode 100644 source/isaaclab/changelog.d/g1-generic-mdp.added.rst create mode 100644 source/isaaclab/changelog.d/g1-generic-mdp.minor diff --git a/docs/source/_static/css/environment-browser.js b/docs/source/_static/css/environment-browser.js index b0cbbe28b1e6..d561662fa224 100644 --- a/docs/source/_static/css/environment-browser.js +++ b/docs/source/_static/css/environment-browser.js @@ -53,7 +53,8 @@ ["Isaac-Velocity-Flat-UnitreeGo2", "rsl_rl,skrl", "isaacsim_physx,newton_kamino,newton_mjwarp,ovphysx", "", "", "environment-previews/go2_flat.jpg", true, {}, {}, "", null, ["||", "isaacsim_physx||", "newton_kamino||", "newton_mjwarp||", "ovphysx||"]], ["Isaac-Velocity-Rough-AnymalD", "rsl_rl,skrl", "isaacsim_physx,newton_kamino,newton_mjwarp,ovphysx", "", "", "tasks/locomotion/anymal_d_rough.jpg", false, {}, {}, "", null, ["||", "isaacsim_physx||", "newton_kamino||", "newton_mjwarp||", "ovphysx||"]], ["Isaac-Velocity-Rough-Cassie", "rsl_rl,skrl", "isaacsim_physx,newton_kamino,newton_mjwarp,ovphysx", "", "", "tasks/cassie_rough.jpg", false, {}, {}, "", null, ["||", "isaacsim_physx||", "newton_kamino||", "newton_mjwarp||", "ovphysx||"]], - ["Isaac-Velocity-Rough-G1", "rsl_rl,skrl", "isaacsim_physx,newton_kamino,newton_mjwarp,ovphysx", "", "", "tasks/locomotion/g1_rough.jpg", false, {}, {}, "", null, ["||", "isaacsim_physx||", "newton_kamino||", "newton_mjwarp||", "ovphysx||"]], + ["Isaac-Velocity-Rough-G1", "rsl_rl", "isaacsim_physx,newton_kamino,newton_mjwarp,ovphysx", "", "", "tasks/locomotion/g1_rough.jpg", false, {}, {}, "", null, ["||", "isaacsim_physx||", "newton_kamino||", "newton_mjwarp||", "ovphysx||"]], + ["Isaac-Velocity-Rough-G1-Depth-Student", "rsl_rl", "isaacsim_physx,newton_kamino,newton_mjwarp,ovphysx", "isaacsim_rtx,newton_renderer,ovrtx", "", "tasks/locomotion/g1_rough.jpg", false, {}, {}, "", null, ["||", "|isaacsim_rtx|", "|newton_renderer|", "|ovrtx|", "isaacsim_physx||", "isaacsim_physx|isaacsim_rtx|", "isaacsim_physx|newton_renderer|", "newton_kamino||", "newton_kamino|isaacsim_rtx|", "newton_kamino|newton_renderer|", "newton_kamino|ovrtx|", "newton_mjwarp||", "newton_mjwarp|isaacsim_rtx|", "newton_mjwarp|newton_renderer|", "newton_mjwarp|ovrtx|", "ovphysx||", "ovphysx|newton_renderer|", "ovphysx|ovrtx|"]], ["Isaac-Velocity-Rough-H1", "rsl_rl,skrl", "isaacsim_physx,newton_kamino,newton_mjwarp,ovphysx", "", "", "tasks/locomotion/h1_rough.jpg", false, {}, {}, "", null, ["||", "isaacsim_physx||", "newton_kamino||", "newton_mjwarp||", "ovphysx||"]], ["Isaac-Velocity-Rough-UnitreeGo2", "rsl_rl,skrl", "isaacsim_physx,newton_kamino,newton_mjwarp,ovphysx", "", "", "tasks/locomotion/go2_rough.jpg", false, {}, {}, "", null, ["||", "isaacsim_physx||", "newton_kamino||", "newton_mjwarp||", "ovphysx||"]], ["IsaacContrib-Assemble-Trocar-G129-Dex3", "rlinf", "", "", "", "tasks/manipulation/g1_assemble_trocar.jpg"], diff --git a/source/isaaclab/changelog.d/g1-generic-mdp.added.rst b/source/isaaclab/changelog.d/g1-generic-mdp.added.rst new file mode 100644 index 000000000000..0e687f210212 --- /dev/null +++ b/source/isaaclab/changelog.d/g1-generic-mdp.added.rst @@ -0,0 +1,2 @@ +* Added shared joint-deviation, body angular-velocity, downward root-speed and terrain-clearance + MDP terms, together with a curriculum callback that updates parameters after a step threshold. diff --git a/source/isaaclab/changelog.d/g1-generic-mdp.minor b/source/isaaclab/changelog.d/g1-generic-mdp.minor new file mode 100644 index 000000000000..e69de29bb2d1 diff --git a/source/isaaclab/isaaclab/envs/mdp/__init__.pyi b/source/isaaclab/isaaclab/envs/mdp/__init__.pyi index 4bcfd3c0d824..92cb2bf21e0e 100644 --- a/source/isaaclab/isaaclab/envs/mdp/__init__.pyi +++ b/source/isaaclab/isaaclab/envs/mdp/__init__.pyi @@ -51,6 +51,7 @@ __all__ = [ "modify_env_param", "modify_reward_weight", "modify_term_cfg", + "set_value_after_steps", "apply_external_force_torque", "push_by_setting_velocity", "randomize_actuator_gains", @@ -122,6 +123,7 @@ __all__ = [ "ang_vel_xy_l2", "applied_torque_limits", "base_height_l2", + "body_ang_vel_xy_l2", "body_lin_acc_l2", "contact_forces", "desired_contacts", @@ -131,6 +133,7 @@ __all__ = [ "is_terminated_term", "joint_acc_l2", "joint_deviation_l1", + "joint_deviation_l2", "joint_pos_limits", "joint_pos_target_l2", "joint_torques_l2", @@ -139,8 +142,10 @@ __all__ = [ "joint_vel_limits", "lin_vel_z_l2", "orientation_command_error", + "pelvis_height_deficit_l2", "position_command_error", "position_command_error_tanh", + "root_downward_speed_l2", "survival_success_rate", "terminated_penalty", "track_ang_vel_z_exp", @@ -155,6 +160,7 @@ __all__ = [ "joint_vel_out_of_limit", "joint_vel_out_of_manual_limit", "pose_command_success", + "pelvis_below_terrain_clearance", "root_height_below_minimum", "time_out", ] @@ -211,6 +217,7 @@ from .curriculums import ( modify_env_param, modify_reward_weight, modify_term_cfg, + set_value_after_steps, ) from .events import ( apply_external_force_torque, @@ -289,6 +296,7 @@ from .rewards import ( ang_vel_xy_l2, applied_torque_limits, base_height_l2, + body_ang_vel_xy_l2, body_lin_acc_l2, contact_forces, desired_contacts, @@ -298,6 +306,7 @@ from .rewards import ( is_terminated_term, joint_acc_l2, joint_deviation_l1, + joint_deviation_l2, joint_pos_limits, joint_pos_target_l2, joint_torques_l2, @@ -306,8 +315,10 @@ from .rewards import ( joint_vel_limits, lin_vel_z_l2, orientation_command_error, + pelvis_height_deficit_l2, position_command_error, position_command_error_tanh, + root_downward_speed_l2, survival_success_rate, terminated_penalty, track_ang_vel_z_exp, @@ -324,6 +335,7 @@ from .terminations import ( joint_vel_out_of_limit, joint_vel_out_of_manual_limit, pose_command_success, + pelvis_below_terrain_clearance, root_height_below_minimum, time_out, ) diff --git a/source/isaaclab/isaaclab/envs/mdp/curriculums.py b/source/isaaclab/isaaclab/envs/mdp/curriculums.py index 31d5d0570d4e..b141f69d8b3e 100644 --- a/source/isaaclab/isaaclab/envs/mdp/curriculums.py +++ b/source/isaaclab/isaaclab/envs/mdp/curriculums.py @@ -291,6 +291,13 @@ def __init__(self, cfg, env): self._address = self._address.replace("s.", "_manager.cfg.", 1) +def set_value_after_steps(env: ManagerBasedRLEnv, env_ids: Sequence[int], data: Any, value: Any, num_steps: int) -> Any: + """Set a term parameter on the first reset at or after ``num_steps`` global environment steps.""" + if env.common_step_counter >= num_steps: + return value + return modify_term_cfg.NO_CHANGE + + class DifficultyScheduler(ManagerTermBase): """Adaptive difficulty scheduler for curriculum learning. diff --git a/source/isaaclab/isaaclab/envs/mdp/rewards.py b/source/isaaclab/isaaclab/envs/mdp/rewards.py index 08431f8949c1..bed6dadf9596 100644 --- a/source/isaaclab/isaaclab/envs/mdp/rewards.py +++ b/source/isaaclab/isaaclab/envs/mdp/rewards.py @@ -105,6 +105,14 @@ def lin_vel_z_l2(env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg = SceneEntity return torch.square(asset.data.root_lin_vel_b.torch[:, 2]) +def root_downward_speed_l2( + env: ManagerBasedRLEnv, speed_limit: float, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot") +) -> torch.Tensor: + """Penalize world-frame downward root speed exceeding the limit [m/s].""" + velocity_z = env.scene[asset_cfg.name].data.root_lin_vel_w.torch[:, 2] + return torch.clamp(-velocity_z - speed_limit, min=0.0).square() + + def ang_vel_xy_l2(env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot")) -> torch.Tensor: """Penalize xy-axis base angular velocity using L2 squared kernel.""" asset: RigidObject = env.scene[asset_cfg.name] @@ -154,6 +162,28 @@ def base_height_l2( return torch.square(asset.data.root_pos_w.torch[:, 2] - adjusted_target_height) +def pelvis_height_deficit_l2( + env: ManagerBasedRLEnv, + target_height: float, + asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), + sensor_cfg: SceneEntityCfg = SceneEntityCfg("height_scanner"), +) -> torch.Tensor: + """Squared shortfall of the root below ``target_height`` above the ground beneath it.""" + asset = env.scene[asset_cfg.name] + sensor = env.scene[sensor_cfg.name] + hits = sensor.data.ray_hits_w.torch[..., 2] + ground = torch.nan_to_num(hits, nan=0.0, posinf=0.0, neginf=0.0).median(dim=1).values + clearance = asset.data.root_pos_w.torch[:, 2] - ground + return torch.clamp(target_height - clearance, min=0.0).square() + + +def body_ang_vel_xy_l2(env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot")) -> torch.Tensor: + """Squared world-frame roll/pitch angular velocity of a named body.""" + asset = env.scene[asset_cfg.name] + omega = asset.data.body_ang_vel_w.torch[:, asset_cfg.body_ids, :].squeeze(1) + return torch.sum(torch.square(omega[:, :2]), dim=1) + + def body_lin_acc_l2(env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot")) -> torch.Tensor: """Penalize the linear acceleration of bodies using L2-kernel.""" asset: Articulation = env.scene[asset_cfg.name] @@ -214,6 +244,15 @@ def joint_deviation_l1(env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg = Scene return torch.sum(torch.abs(angle), dim=1) +def joint_deviation_l2(env: ManagerBasedRLEnv, asset_cfg: SceneEntityCfg) -> torch.Tensor: + """Squared deviation of the selected joints from their default angles.""" + asset: Articulation = env.scene[asset_cfg.name] + angle = ( + asset.data.joint_pos.torch[:, asset_cfg.joint_ids] - asset.data.default_joint_pos.torch[:, asset_cfg.joint_ids] + ) + return torch.sum(torch.square(angle), dim=1) + + def joint_pos_target_l2(env: ManagerBasedRLEnv, target: float, asset_cfg: SceneEntityCfg) -> torch.Tensor: """Penalize joint positions that deviate from a target value using an L2 squared kernel. diff --git a/source/isaaclab/isaaclab/envs/mdp/terminations.py b/source/isaaclab/isaaclab/envs/mdp/terminations.py index 5fb45dd0f007..93d280662502 100644 --- a/source/isaaclab/isaaclab/envs/mdp/terminations.py +++ b/source/isaaclab/isaaclab/envs/mdp/terminations.py @@ -86,6 +86,20 @@ def root_height_below_minimum( return asset.data.root_pos_w.torch[:, 2] < minimum_height +def pelvis_below_terrain_clearance( + env: ManagerBasedRLEnv, + minimum_height: float, + asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), + sensor_cfg: SceneEntityCfg = SceneEntityCfg("height_scanner"), +) -> torch.Tensor: + """Terminate when the root sits less than ``minimum_height`` above the ground beneath it.""" + asset = env.scene[asset_cfg.name] + sensor = env.scene[sensor_cfg.name] + hits = sensor.data.ray_hits_w.torch[..., 2] + ground = torch.nan_to_num(hits, nan=0.0, posinf=0.0, neginf=0.0).median(dim=1).values + return asset.data.root_pos_w.torch[:, 2] - ground < minimum_height + + """ Joint terminations. """ diff --git a/source/isaaclab_tasks/changelog.d/g1-shared-mdp.changed.rst b/source/isaaclab_tasks/changelog.d/g1-shared-mdp.changed.rst index 9fedeee194e7..2fab693ceabc 100644 --- a/source/isaaclab_tasks/changelog.d/g1-shared-mdp.changed.rst +++ b/source/isaaclab_tasks/changelog.d/g1-shared-mdp.changed.rst @@ -1,5 +1,9 @@ -* Moved G1 rewards and the clearance termination into the shared velocity MDP modules, - scheduling the termination warmup through the curriculum manager. +* Placed generic reward, clearance termination and step-scheduled curriculum terms in the core MDP, + keeping foot-support geometry in the velocity MDP modules. +* Declared the G1 teacher's static reward settings in ``G1Rewards`` and preserved the flat task's + reward settings through explicit configuration overrides. +* Composed the G1 environments from scene, event, curriculum, termination and observation configurations, + leaving initialization to supply asset-derived action scales and sole geometry. * Moved G1 symmetry helpers alongside the other velocity-task robot symmetry modules. * Reused the common depth-image observation with a G1 image-processing modifier, assembling observation groups directly in the student environment configuration. diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/flat_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/flat_env_cfg.py index 674d9cb4c7b1..8eec02859f34 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/flat_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/flat_env_cfg.py @@ -5,20 +5,108 @@ """Configuration for the Unitree G1 velocity-tracking environment on flat terrain.""" +from isaaclab.managers import RewardTermCfg as RewTerm from isaaclab.managers import SceneEntityCfg from isaaclab.utils import configclass, replace from isaaclab_assets import G1_MINIMAL_CFG +from ... import mdp from ...velocity_env_cfg import LocomotionVelocityRoughEnvCfg -from .rough_env_cfg import G1Rewards +from .rough_env_cfg import G1EventsCfg, G1Rewards @configclass class G1FlatEnvCfg(LocomotionVelocityRoughEnvCfg): """Configuration for the Unitree G1 velocity-tracking environment on flat terrain.""" - rewards: G1Rewards = G1Rewards() + events: G1EventsCfg = G1EventsCfg() + rewards: G1Rewards = G1Rewards( + track_lin_vel_xy_exp=RewTerm( + func=mdp.track_lin_vel_xy_yaw_frame_exp, + weight=1.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.0, params={"command_name": "base_velocity", "std": 0.5} + ), + lin_vel_z_l2=RewTerm(func=mdp.lin_vel_z_l2, weight=-0.2), + dof_acc_l2=RewTerm( + func=mdp.joint_acc_l2, + weight=-1.0e-7, + params={"asset_cfg": SceneEntityCfg("robot", joint_names=[".*_hip_.*", ".*_knee_joint"])}, + ), + dof_torques_l2=RewTerm( + func=mdp.joint_torques_l2, + weight=-2.0e-6, + params={"asset_cfg": SceneEntityCfg("robot", joint_names=[".*_hip_.*", ".*_knee_joint"])}, + ), + feet_air_time=RewTerm( + func=mdp.feet_air_time_positive_biped, + weight=0.75, + params={ + "command_name": "base_velocity", + "sensor_cfg": SceneEntityCfg("contact_forces", body_names=".*_ankle_roll_link"), + "threshold": 0.4, + }, + ), + feet_slide=RewTerm( + func=mdp.feet_slide, + weight=-0.1, + params={ + "sensor_cfg": SceneEntityCfg("contact_forces", body_names=".*_ankle_roll_link"), + "asset_cfg": SceneEntityCfg("robot", body_names=".*_ankle_roll_link"), + }, + ), + joint_deviation_hip=RewTerm( + func=mdp.joint_deviation_l1, + weight=-0.1, + params={"asset_cfg": SceneEntityCfg("robot", joint_names=[".*_hip_yaw_joint", ".*_hip_roll_joint"])}, + ), + joint_deviation_arms=RewTerm( + func=mdp.joint_deviation_l1, + weight=-0.1, + params={ + "asset_cfg": SceneEntityCfg( + "robot", + joint_names=[ + ".*_shoulder_pitch_joint", + ".*_shoulder_roll_joint", + ".*_shoulder_yaw_joint", + ".*_elbow_pitch_joint", + ".*_elbow_roll_joint", + ], + ) + }, + ), + joint_deviation_fingers=RewTerm( + func=mdp.joint_deviation_l1, + weight=-0.05, + params={ + "asset_cfg": SceneEntityCfg( + "robot", + joint_names=[ + ".*_five_joint", + ".*_three_joint", + ".*_six_joint", + ".*_four_joint", + ".*_zero_joint", + ".*_one_joint", + ".*_two_joint", + ], + ) + }, + ), + joint_deviation_torso=RewTerm( + func=mdp.joint_deviation_l1, + weight=-0.1, + params={"asset_cfg": SceneEntityCfg("robot", joint_names="torso_joint")}, + ), + pelvis_height=None, + torso_ang_vel=None, + root_downward_speed=None, + feet_support_edge=None, + ) def __post_init__(self): super().__post_init__() @@ -41,28 +129,7 @@ def __post_init__(self): self.commands.base_velocity.ranges.lin_vel_x = (0.0, 1.0) self.commands.base_velocity.ranges.lin_vel_y = (-0.5, 0.5) self.commands.base_velocity.ranges.ang_vel_z = (-1.0, 1.0) - # rewards - self.rewards.undesired_contacts = None - self.rewards.flat_orientation_l2.weight = -1.0 - self.rewards.track_ang_vel_z_exp.weight = 1.0 - self.rewards.lin_vel_z_l2.weight = -0.2 - self.rewards.action_rate_l2.weight = -0.005 - self.rewards.dof_acc_l2.weight = -1.0e-7 - self.rewards.dof_acc_l2.params["asset_cfg"] = SceneEntityCfg( - "robot", joint_names=[".*_hip_.*", ".*_knee_joint"] - ) - self.rewards.feet_air_time.weight = 0.75 - self.rewards.feet_air_time.params["threshold"] = 0.4 - self.rewards.dof_torques_l2.weight = -2.0e-6 - self.rewards.dof_torques_l2.params["asset_cfg"] = SceneEntityCfg( - "robot", joint_names=[".*_hip_.*", ".*_knee_joint"] - ) # terminations self.terminations.base_contact.params["sensor_cfg"].body_names = "torso_link" - # events - self.events.add_base_mass.params["asset_cfg"].body_names = "torso_link" - self.events.base_com = None - self.events.base_external_force_torque.params["asset_cfg"].body_names = "torso_link" - self.events.reset_robot_joints.params["position_range"] = (1.0, 1.0) # curriculum self.curriculum.terrain_levels = None diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/rough_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/rough_env_cfg.py index e6be582b23e0..dbc4b12d969a 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/rough_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/rough_env_cfg.py @@ -11,30 +11,34 @@ """ import math -from copy import deepcopy +from dataclasses import MISSING import torch from isaaclab_newton.sim.schemas import NewtonArticulationCfg from isaaclab_physx.sim.schemas import PhysxArticulationCfg, PhysxRigidBodyCfg +import isaaclab.terrains as terrain_gen from isaaclab.actuators import ImplicitActuatorCfg from isaaclab.assets import ArticulationCfg from isaaclab.managers import CurriculumTermCfg as CurrTerm +from isaaclab.managers import EventTermCfg as EventTerm from isaaclab.managers import ObservationGroupCfg as ObsGroup from isaaclab.managers import ObservationTermCfg as ObsTerm from isaaclab.managers import RewardTermCfg as RewTerm from isaaclab.managers import SceneEntityCfg from isaaclab.managers import TerminationTermCfg as DoneTerm -from isaaclab.sensors import CameraCfg -from isaaclab.sim import PinholeCameraCfg, UsdFileCfg -from isaaclab.utils import configclass, replace +from isaaclab.sensors import CameraCfg, RayCasterCfg, patterns +from isaaclab.sim import PinholeCameraCfg, SimulationCfg, UsdFileCfg +from isaaclab.terrains import TerrainGeneratorCfg +from isaaclab.utils import configclass from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR, retrieve_file_path from isaaclab.utils.modifiers import ModifierCfg +from isaaclab.utils.noise import UniformNoiseCfg as Unoise from isaaclab.utils.string import resolve_matching_names, resolve_matching_names_values from isaaclab_tasks.utils.presets import MultiBackendRendererCfg -from ... import mdp +from ... import mdp, velocity_env_cfg from ...velocity_env_cfg import ( LocomotionVelocityRoughEnvCfg, RewardsCfg, @@ -43,12 +47,26 @@ @configclass class G1Rewards(RewardsCfg): - """Reward terms for the MDP.""" + """Reward terms for the G1 teacher and depth student.""" termination_penalty = RewTerm(func=mdp.is_terminated, weight=-200.0) + lin_vel_z_l2 = RewTerm(func=mdp.lin_vel_z_l2, weight=0.0) + undesired_contacts = None + flat_orientation_l2 = RewTerm(func=mdp.flat_orientation_l2, weight=-1.0) + action_rate_l2 = RewTerm(func=mdp.action_rate_l2, weight=-0.005) + dof_acc_l2 = RewTerm( + func=mdp.joint_acc_l2, + weight=-1.25e-7, + params={"asset_cfg": SceneEntityCfg("robot", joint_names=[".*_hip_.*", ".*_knee_joint"])}, + ) + dof_torques_l2 = RewTerm( + func=mdp.joint_torques_l2, + weight=-1.5e-7, + params={"asset_cfg": SceneEntityCfg("robot", joint_names=[".*_hip_.*", ".*_knee_joint", ".*_ankle_.*"])}, + ) track_lin_vel_xy_exp = RewTerm( func=mdp.track_lin_vel_xy_yaw_frame_exp, - weight=1.0, + weight=2.0, params={"command_name": "base_velocity", "std": 0.5}, ) track_ang_vel_z_exp = RewTerm( @@ -56,7 +74,7 @@ class G1Rewards(RewardsCfg): ) feet_air_time = RewTerm( func=mdp.feet_air_time_positive_biped, - weight=0.25, + weight=1.0, params={ "command_name": "base_velocity", "sensor_cfg": SceneEntityCfg("contact_forces", body_names=".*_ankle_roll_link"), @@ -65,7 +83,7 @@ class G1Rewards(RewardsCfg): ) feet_slide = RewTerm( func=mdp.feet_slide, - weight=-0.1, + weight=-0.5, params={ "sensor_cfg": SceneEntityCfg("contact_forces", body_names=".*_ankle_roll_link"), "asset_cfg": SceneEntityCfg("robot", body_names=".*_ankle_roll_link"), @@ -80,9 +98,9 @@ class G1Rewards(RewardsCfg): ) # Penalize deviation from default of the joints that are not essential for locomotion joint_deviation_hip = RewTerm( - func=mdp.joint_deviation_l1, - weight=-0.1, - params={"asset_cfg": SceneEntityCfg("robot", joint_names=[".*_hip_yaw_joint", ".*_hip_roll_joint"])}, + func=mdp.joint_deviation_l2, + weight=-1.0, + params={"asset_cfg": SceneEntityCfg("robot", joint_names=[".*_hip_roll_joint", ".*_hip_yaw_joint"])}, ) joint_deviation_arms = RewTerm( func=mdp.joint_deviation_l1, @@ -90,38 +108,48 @@ class G1Rewards(RewardsCfg): params={ "asset_cfg": SceneEntityCfg( "robot", - joint_names=[ - ".*_shoulder_pitch_joint", - ".*_shoulder_roll_joint", - ".*_shoulder_yaw_joint", - ".*_elbow_pitch_joint", - ".*_elbow_roll_joint", - ], + joint_names=[".*_shoulder_.*_joint", ".*_elbow_joint", ".*_wrist_.*_joint"], ) }, ) joint_deviation_fingers = RewTerm( func=mdp.joint_deviation_l1, weight=-0.05, + params={"asset_cfg": SceneEntityCfg("robot", joint_names=[".*_hand_.*_joint"])}, + ) + joint_deviation_torso = RewTerm( + func=mdp.joint_deviation_l2, + weight=-1.0, params={ "asset_cfg": SceneEntityCfg( - "robot", - joint_names=[ - ".*_five_joint", - ".*_three_joint", - ".*_six_joint", - ".*_four_joint", - ".*_zero_joint", - ".*_one_joint", - ".*_two_joint", - ], + "robot", joint_names=["waist_yaw_joint", "waist_roll_joint", "waist_pitch_joint"] ) }, ) - joint_deviation_torso = RewTerm( - func=mdp.joint_deviation_l1, + pelvis_height = RewTerm( + func=mdp.pelvis_height_deficit_l2, + weight=-10.0, + params={ + "target_height": 0.65, + "asset_cfg": SceneEntityCfg("robot"), + "sensor_cfg": SceneEntityCfg("height_scanner"), + }, + ) + torso_ang_vel = RewTerm( + func=mdp.body_ang_vel_xy_l2, weight=-0.1, - params={"asset_cfg": SceneEntityCfg("robot", joint_names="torso_joint")}, + params={"asset_cfg": SceneEntityCfg("robot", body_names=["torso_link"])}, + ) + root_downward_speed = RewTerm(func=mdp.root_downward_speed_l2, weight=-1.0, params={"speed_limit": 0.5}) + feet_support_edge = RewTerm( + func=mdp.feet_support_edge_distance, + weight=-5.0, + params={ + "sensor_cfg": SceneEntityCfg("contact_forces", body_names=".*_ankle_roll_link"), + "asset_cfg": SceneEntityCfg("robot", body_names=".*_ankle_roll_link"), + "sole_size": MISSING, + "sole_offset": MISSING, + }, ) @@ -181,245 +209,344 @@ def get_action_parameters(robot_cfg: ArticulationCfg) -> tuple[dict[str, float], return action_scale, sole_size, sole_offset -G1_29DOF_VELOCITY_CFG = ArticulationCfg( - spawn=UsdFileCfg( - usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Robots/Unitree/G1/g1_29dof.usd", - activate_contact_sensors=True, - rigid_props=PhysxRigidBodyCfg( - 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=[ - PhysxArticulationCfg( - enabled_self_collisions=False, solver_position_iteration_count=8, solver_velocity_iteration_count=4 - ), - NewtonArticulationCfg(self_collision_enabled=False), - ], - ), - init_state=ArticulationCfg.InitialStateCfg( - pos=(0.0, 0.0, 0.793), - joint_pos={ - ".*_hip_pitch_joint": -0.20, - ".*_knee_joint": 0.42, - ".*_ankle_pitch_joint": -0.23, - ".*_elbow_joint": 0.87, - "left_shoulder_pitch_joint": 0.35, - "left_shoulder_roll_joint": 0.16, - "right_shoulder_pitch_joint": 0.35, - "right_shoulder_roll_joint": -0.16, +@configclass +class G1EventsCfg(velocity_env_cfg.EventsCfg): + """G1 startup and reset events.""" + + add_base_mass = EventTerm( + func=mdp.randomize_rigid_body_mass, + mode="startup", + params={ + "asset_cfg": SceneEntityCfg("robot", body_names="torso_link"), + "mass_distribution_params": (1 / 1.25, 1.25), + "operation": "scale", + "distribution": "log_uniform", }, - ), - soft_joint_pos_limit_factor=0.9, - actuators={ - "legs": ImplicitActuatorCfg( - joint_names_expr=[".*_hip_.*", ".*_knee_joint", "waist_yaw_joint"], - joint_effort_limit=300.0, - stiffness={ - ".*_hip_pitch_joint": 200.0, - ".*_hip_roll_joint": 150.0, - ".*_hip_yaw_joint": 150.0, - ".*_knee_joint": 200.0, - "waist_yaw_joint": 200.0, - }, - damping=5.0, - armature=0.01, - ), - "feet": ImplicitActuatorCfg( - joint_names_expr=[".*_ankle_.*"], joint_effort_limit=20.0, stiffness=20.0, damping=2.0, armature=0.01 - ), - "waist": ImplicitActuatorCfg( - joint_names_expr=["waist_roll_joint", "waist_pitch_joint"], - joint_effort_limit=50.0, - stiffness=200.0, - damping=5.0, - armature=0.01, + ) + base_com = None + base_external_force_torque = EventTerm( + func=mdp.apply_external_force_torque, + mode="reset", + params={ + "asset_cfg": SceneEntityCfg("robot", body_names="torso_link"), + "force_range": (0.0, 0.0), + "torque_range": (-0.0, 0.0), + }, + ) + reset_robot_joints = EventTerm( + func=mdp.reset_joints_by_scale, + mode="reset", + params={"position_range": (1.0, 1.0), "velocity_range": (0.0, 0.0)}, + ) + + +@configclass +class G1CurriculumCfg(velocity_env_cfg.CurriculumCfg): + """G1 terrain progression and root-clearance warmup.""" + + base_height = CurrTerm( + func=mdp.modify_term_cfg, + params={ + "address": "terminations.base_height.params.minimum_height", + "modify_fn": mdp.set_value_after_steps, + "modify_params": {"value": 0.4, "num_steps": 12000}, + }, + ) + + +@configclass +class G1TerminationsCfg(velocity_env_cfg.TerminationsCfg): + """G1 torso-contact and terrain-clearance terminations.""" + + base_contact = DoneTerm( + func=mdp.illegal_contact, + params={"sensor_cfg": SceneEntityCfg("contact_forces", body_names="torso_link"), "threshold": 1.0}, + ) + base_height = DoneTerm( + func=mdp.pelvis_below_terrain_clearance, + params={ + "minimum_height": -math.inf, + "asset_cfg": SceneEntityCfg("robot"), + "sensor_cfg": SceneEntityCfg("height_scanner"), + }, + ) + + +@configclass +class G1SceneCfg(velocity_env_cfg.MySceneCfg): + """G1 robot, mesh terrain and locomotion sensors.""" + + robot = ArticulationCfg( + prim_path="{ENV_REGEX_NS}/Robot", + spawn=UsdFileCfg( + usd_path=f"{ISAACLAB_NUCLEUS_DIR}/Robots/Unitree/G1/g1_29dof.usd", + activate_contact_sensors=True, + rigid_props=PhysxRigidBodyCfg( + 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=[ + PhysxArticulationCfg( + enabled_self_collisions=False, solver_position_iteration_count=8, solver_velocity_iteration_count=4 + ), + NewtonArticulationCfg(self_collision_enabled=False), + ], ), - "arms": ImplicitActuatorCfg( - joint_names_expr=[".*_shoulder_.*", ".*_elbow_joint", ".*_wrist_.*"], - joint_effort_limit=300.0, - stiffness=40.0, - damping=10.0, - armature=0.01, + init_state=ArticulationCfg.InitialStateCfg( + pos=(0.0, 0.0, 0.793), + joint_pos={ + ".*_hip_pitch_joint": -0.20, + ".*_knee_joint": 0.42, + ".*_ankle_pitch_joint": -0.23, + ".*_elbow_joint": 0.87, + "left_shoulder_pitch_joint": 0.35, + "left_shoulder_roll_joint": 0.16, + "right_shoulder_pitch_joint": 0.35, + "right_shoulder_roll_joint": -0.16, + }, ), - "hands": ImplicitActuatorCfg( - joint_names_expr=[".*_hand_.*"], joint_effort_limit=300.0, stiffness=40.0, damping=10.0, armature=0.001 + soft_joint_pos_limit_factor=0.9, + actuators={ + "legs": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_.*", ".*_knee_joint", "waist_yaw_joint"], + joint_effort_limit=300.0, + stiffness={ + ".*_hip_pitch_joint": 200.0, + ".*_hip_roll_joint": 150.0, + ".*_hip_yaw_joint": 150.0, + ".*_knee_joint": 200.0, + "waist_yaw_joint": 200.0, + }, + damping=5.0, + armature=0.01, + ), + "feet": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_.*"], joint_effort_limit=20.0, stiffness=20.0, damping=2.0, armature=0.01 + ), + "waist": ImplicitActuatorCfg( + joint_names_expr=["waist_roll_joint", "waist_pitch_joint"], + joint_effort_limit=50.0, + stiffness=200.0, + damping=5.0, + armature=0.01, + ), + "arms": ImplicitActuatorCfg( + joint_names_expr=[".*_shoulder_.*", ".*_elbow_joint", ".*_wrist_.*"], + joint_effort_limit=300.0, + stiffness=40.0, + damping=10.0, + armature=0.01, + ), + "hands": ImplicitActuatorCfg( + joint_names_expr=[".*_hand_.*"], joint_effort_limit=300.0, stiffness=40.0, damping=10.0, armature=0.001 + ), + }, + ) + + height_scanner = RayCasterCfg( + prim_path="{ENV_REGEX_NS}/Robot/torso_link", + offset=RayCasterCfg.OffsetCfg(pos=(0.0, 0.0, 20.0)), + ray_alignment="yaw", + pattern_cfg=patterns.GridPatternCfg(resolution=0.1, size=[1.6, 1.0]), + debug_vis=False, + mesh_prim_paths=["/World/ground"], + global_world_only=True, + ) + terrain = velocity_env_cfg.MySceneCfg().terrain + terrain.terrain_generator = TerrainGeneratorCfg( + size=(8.0, 8.0), + border_width=20.0, + num_rows=10, + num_cols=20, + horizontal_scale=0.1, + vertical_scale=0.005, + slope_threshold=0.75, + use_cache=False, + sub_terrains={ + "pyramid_stairs": terrain_gen.MeshPyramidStairsTerrainCfg( + proportion=0.2, + step_height_range=(0.05, 0.35), + step_width=0.3, + platform_width=3.0, + border_width=1.0, + holes=False, + convert_to_heightfield=False, + ), + "pyramid_stairs_inv": terrain_gen.MeshInvertedPyramidStairsTerrainCfg( + proportion=0.2, + step_height_range=(0.05, 0.35), + step_width=0.3, + platform_width=3.0, + border_width=1.0, + holes=False, + convert_to_heightfield=False, + ), + "boxes": terrain_gen.MeshRandomGridTerrainCfg( + proportion=0.2, + grid_width=0.45, + grid_height_range=(0.05, 0.2), + platform_width=2.0, + convert_to_heightfield=False, + ), + "random_rough": terrain_gen.HfRandomUniformTerrainCfg( + proportion=0.2, + noise_range=(0.02, 0.10), + noise_step=0.02, + border_width=0.25, + convert_to_heightfield=False, + ), + "hf_pyramid_slope": terrain_gen.HfPyramidSlopedTerrainCfg( + proportion=0.1, + slope_range=(0.0, 0.4), + platform_width=2.0, + border_width=0.25, + convert_to_heightfield=False, + ), + "hf_pyramid_slope_inv": terrain_gen.HfInvertedPyramidSlopedTerrainCfg( + proportion=0.1, + slope_range=(0.0, 0.4), + platform_width=2.0, + border_width=0.25, + convert_to_heightfield=False, + ), + }, + ) + + +@configclass +class G1DepthCameraCfg(CameraCfg): + """Chest-mounted depth camera shared by the student and its reflected view.""" + + update_period = 0.0 + update_latest_camera_pose = True + renderer_cfg = MultiBackendRendererCfg() + width = DEPTH_WIDTH + height = DEPTH_HEIGHT + data_types = ["distance_to_image_plane"] + spawn = PinholeCameraCfg( + focal_length=1.93, + horizontal_aperture=2.0 * 1.93 * math.tan(math.radians(87.0 / 2.0)), + clipping_range=(0.05, 10.0), + ) + offset = CameraCfg.OffsetCfg(pos=DEPTH_OFFSET_POS, rot=DEPTH_OFFSET_ROT, convention="world") + + +@configclass +class G1DepthStudentSceneCfg(G1SceneCfg): + """Teacher scene extended with the student's depth and reflected-depth cameras.""" + + depth_camera = G1DepthCameraCfg(prim_path="{ENV_REGEX_NS}/Robot/torso_link/depth_camera") + depth_camera_mirror = G1DepthCameraCfg( + prim_path="{ENV_REGEX_NS}/Robot/torso_link/depth_camera_mirror", + offset=CameraCfg.OffsetCfg( + pos=(DEPTH_OFFSET_POS[0], -DEPTH_OFFSET_POS[1], DEPTH_OFFSET_POS[2]), + rot=DEPTH_OFFSET_ROT, + convention="world", ), - }, -) + ) + + +@configclass +class G1TeacherPolicyCfg(ObsGroup): + """Teacher observations, also used as noise-free supervision for distillation.""" + + base_lin_vel = ObsTerm(func=mdp.base_lin_vel, noise=Unoise(n_min=-0.1, n_max=0.1)) + base_ang_vel = ObsTerm(func=mdp.base_ang_vel, noise=Unoise(n_min=-0.2, n_max=0.2)) + projected_gravity = ObsTerm(func=mdp.projected_gravity, noise=Unoise(n_min=-0.05, n_max=0.05)) + velocity_commands = ObsTerm(func=mdp.generated_commands, params={"command_name": "base_velocity"}) + joint_pos = ObsTerm(func=mdp.joint_pos_rel, noise=Unoise(n_min=-0.01, n_max=0.01)) + joint_vel = ObsTerm(func=mdp.joint_vel_rel, noise=Unoise(n_min=-1.5, n_max=1.5)) + actions = ObsTerm(func=mdp.last_action) + height_scan = ObsTerm( + func=mdp.height_scan, + params={"sensor_cfg": SceneEntityCfg("height_scanner")}, + noise=Unoise(n_min=-0.1, n_max=0.1), + clip=(-1.0, 1.0), + ) + enable_corruption = True + concatenate_terms = True + + +@configclass +class G1StudentPolicyCfg(G1TeacherPolicyCfg): + """Five proprioceptive frames without privileged velocity or height observations.""" + + base_lin_vel = None + height_scan = None + history_length = 5 + + +@configclass +class G1DepthObservationsCfg(ObsGroup): + """Normalized depth-frame stack in the calibrated G1 camera convention.""" + + depth = ObsTerm( + func=mdp.image_depth, + params={ + "sensor_cfg": SceneEntityCfg("depth_camera"), + "data_type": "distance_to_image_plane", + "normalize": False, + "frame_stack": DEPTH_STACK, + "channel_first": True, + }, + modifiers=[ModifierCfg(func=process_depth_image)], + clip=(0.0, DEPTH_MAX_RANGE), + scale=1.0 / DEPTH_MAX_RANGE, + ) + enable_corruption = False + concatenate_terms = True + + +@configclass +class G1StudentObservationsCfg(velocity_env_cfg.ObservationsCfg): + """Student inference inputs and teacher/reflected-camera supervision groups.""" + + policy: G1StudentPolicyCfg = G1StudentPolicyCfg() + teacher: G1TeacherPolicyCfg = G1TeacherPolicyCfg(enable_corruption=False) + depth: G1DepthObservationsCfg = G1DepthObservationsCfg() + depth_mirror: G1DepthObservationsCfg = G1DepthObservationsCfg() + depth_mirror.depth.params["sensor_cfg"] = SceneEntityCfg("depth_camera_mirror") @configclass class G1RoughTeacherEnvCfg(LocomotionVelocityRoughEnvCfg): - """G1 teacher on mesh terrain with terrain-height observations.""" + """G1 teacher composed from its scene and manager configurations.""" + seed = 42 + scene: G1SceneCfg = G1SceneCfg(num_envs=4096, env_spacing=2.5) rewards: G1Rewards = G1Rewards() + events: G1EventsCfg = G1EventsCfg() + curriculum: G1CurriculumCfg = G1CurriculumCfg() + terminations: G1TerminationsCfg = G1TerminationsCfg() + observations: velocity_env_cfg.ObservationsCfg = velocity_env_cfg.ObservationsCfg(policy=G1TeacherPolicyCfg()) + sim = SimulationCfg( + physics=velocity_env_cfg.RoughPhysicsCfg(), + use_newton_actuators=False, + visualizer_cfgs=[], + default_visualizer_cfg=None, + ) + sim.physics.newton_mjwarp.solver_cfg.njmax = 300 + commands = velocity_env_cfg.CommandsCfg() + commands.base_velocity.vel_yaw_success_threshold = 0.8 + commands.base_velocity.marker_pos_offset = (0.0, 0.0, 0.75) + commands.base_velocity.ranges.lin_vel_x = (0.0, 1.0) + commands.base_velocity.ranges.lin_vel_y = (-0.0, 0.0) + commands.base_velocity.ranges.ang_vel_z = (-1.0, 1.0) def __post_init__(self): super().__post_init__() - self.sim.physics.newton_mjwarp.solver_cfg.njmax = 300 - self.scene.robot = replace(G1_29DOF_VELOCITY_CFG, prim_path="{ENV_REGEX_NS}/Robot") - self.scene.height_scanner.prim_path = "{ENV_REGEX_NS}/Robot/torso_link" - self.commands.base_velocity.vel_yaw_success_threshold = 0.8 - self.commands.base_velocity.marker_pos_offset = (0.0, 0.0, 0.75) - 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) - self.rewards.lin_vel_z_l2.weight = 0.0 - self.rewards.undesired_contacts = None - self.rewards.flat_orientation_l2.weight = -1.0 - self.rewards.action_rate_l2.weight = -0.005 - self.rewards.dof_acc_l2.weight = -1.25e-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_.*"] - ) - self.terminations.base_contact.params["sensor_cfg"].body_names = "torso_link" - self.events.add_base_mass.params["asset_cfg"].body_names = "torso_link" - self.events.base_com = None - self.events.base_external_force_torque.params["asset_cfg"].body_names = "torso_link" - self.events.reset_robot_joints.params["position_range"] = (1.0, 1.0) - self.rewards.joint_deviation_arms.params["asset_cfg"] = SceneEntityCfg( - "robot", - joint_names=[".*_shoulder_.*_joint", ".*_elbow_joint", ".*_wrist_.*_joint"], - ) - self.rewards.joint_deviation_fingers.params["asset_cfg"] = SceneEntityCfg( - "robot", joint_names=[".*_hand_.*_joint"] - ) - self.rewards.joint_deviation_torso.params["asset_cfg"] = SceneEntityCfg( - "robot", joint_names=["waist_yaw_joint", "waist_roll_joint", "waist_pitch_joint"] - ) - - self.seed = 42 - self.scene.num_envs = 4096 - self.sim.use_newton_actuators = False - self.sim.visualizer_cfgs = [] - self.sim.default_visualizer_cfg = None - terrain = self.scene.terrain.terrain_generator - proportions = { - "pyramid_stairs": 0.2, - "pyramid_stairs_inv": 0.2, - "boxes": 0.2, - "random_rough": 0.2, - "hf_pyramid_slope": 0.1, - "hf_pyramid_slope_inv": 0.1, - } - for name, subterrain in terrain.sub_terrains.items(): - subterrain.convert_to_heightfield = False - subterrain.proportion = proportions[name] - for name in ("pyramid_stairs", "pyramid_stairs_inv"): - terrain.sub_terrains[name].step_height_range = (0.05, 0.35) - terrain.sub_terrains[name].step_width = 0.3 self.actions.joint_pos.scale, sole_size, sole_offset = get_action_parameters(self.scene.robot) - self.rewards.track_lin_vel_xy_exp.weight = 2.0 - self.rewards.ang_vel_xy_l2.weight = -0.05 - self.rewards.feet_air_time.weight = 1.0 - self.rewards.feet_slide.weight = -0.5 - self.rewards.joint_deviation_hip.func = mdp.joint_deviation_l2 - self.rewards.joint_deviation_hip.weight = -1.0 - self.rewards.joint_deviation_hip.params["asset_cfg"].joint_names = [".*_hip_roll_joint", ".*_hip_yaw_joint"] - self.rewards.joint_deviation_torso.func = mdp.joint_deviation_l2 - self.rewards.joint_deviation_torso.weight = -1.0 - self.rewards.pelvis_height = RewTerm( - func=mdp.pelvis_height_deficit_l2, - weight=-10.0, - params={ - "target_height": 0.65, - "asset_cfg": SceneEntityCfg("robot"), - "sensor_cfg": SceneEntityCfg("height_scanner"), - }, - ) - self.rewards.torso_ang_vel = RewTerm( - func=mdp.body_ang_vel_xy_l2, - weight=-0.1, - params={"asset_cfg": SceneEntityCfg("robot", body_names=["torso_link"])}, - ) - self.rewards.root_downward_speed = RewTerm( - func=mdp.root_downward_speed_l2, - weight=-1.0, - params={"speed_limit": 0.5}, - ) - self.rewards.feet_support_edge = RewTerm( - func=mdp.feet_support_edge_distance, - weight=-5.0, - params={ - **deepcopy(self.rewards.feet_slide.params), - "sole_size": sole_size, - "sole_offset": sole_offset, - }, - ) - self.terminations.base_height = DoneTerm( - func=mdp.pelvis_below_terrain_clearance, - params={ - "minimum_height": -math.inf, - "asset_cfg": SceneEntityCfg("robot"), - "sensor_cfg": SceneEntityCfg("height_scanner"), - }, - ) - - self.curriculum.base_height = CurrTerm( - func=mdp.modify_term_cfg, - params={ - "address": "terminations.base_height.params.minimum_height", - "modify_fn": mdp.set_value_after_steps, - "modify_params": {"value": 0.4, "num_steps": 12000}, - }, - ) + self.rewards.feet_support_edge.params.update(sole_size=sole_size, sole_offset=sole_offset) @configclass class G1RoughDepthStudentEnvCfg(G1RoughTeacherEnvCfg): """G1 student with five proprioceptive frames and mirrored depth supervision.""" - def __post_init__(self): - super().__post_init__() - self.observations.teacher = deepcopy(self.observations.policy) - self.observations.teacher.enable_corruption = False - self.observations.policy.base_lin_vel = None - self.observations.policy.height_scan = None - for name in ("base_ang_vel", "projected_gravity", "velocity_commands", "joint_pos", "joint_vel", "actions"): - term = getattr(self.observations.policy, name) - term.history_length = 5 - term.flatten_history_dim = True - self.observations.depth = ObsGroup(enable_corruption=False, concatenate_terms=True) - self.observations.depth.depth = ObsTerm( - func=mdp.image_depth, - params={ - "sensor_cfg": SceneEntityCfg("depth_camera"), - "data_type": "distance_to_image_plane", - "normalize": False, - "frame_stack": DEPTH_STACK, - "channel_first": True, - }, - modifiers=[ModifierCfg(func=process_depth_image)], - clip=(0.0, DEPTH_MAX_RANGE), - scale=1.0 / DEPTH_MAX_RANGE, - ) - self.observations.depth_mirror = deepcopy(self.observations.depth) - self.observations.depth_mirror.depth.params["sensor_cfg"] = SceneEntityCfg("depth_camera_mirror") - self.scene.depth_camera = CameraCfg( - prim_path="{ENV_REGEX_NS}/Robot/torso_link/depth_camera", - update_period=0.0, - update_latest_camera_pose=True, - renderer_cfg=MultiBackendRendererCfg(), - width=DEPTH_WIDTH, - height=DEPTH_HEIGHT, - data_types=["distance_to_image_plane"], - spawn=PinholeCameraCfg( - focal_length=1.93, - horizontal_aperture=2.0 * 1.93 * math.tan(math.radians(87.0 / 2.0)), - clipping_range=(0.05, 10.0), - ), - offset=CameraCfg.OffsetCfg(pos=DEPTH_OFFSET_POS, rot=DEPTH_OFFSET_ROT, convention="world"), - ) - self.scene.depth_camera_mirror = deepcopy(self.scene.depth_camera) - self.scene.depth_camera_mirror.prim_path = "{ENV_REGEX_NS}/Robot/torso_link/depth_camera_mirror" - x, y, z = DEPTH_OFFSET_POS - self.scene.depth_camera_mirror.offset.pos = (x, -y, z) + scene: G1DepthStudentSceneCfg = G1DepthStudentSceneCfg(num_envs=4096, env_spacing=2.5) + observations: G1StudentObservationsCfg = G1StudentObservationsCfg() diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/mdp/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/mdp/__init__.py index e5a6ad09e23e..702363cbd397 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/mdp/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/mdp/__init__.py @@ -3,7 +3,10 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""MDP terms specific to the velocity-tracking locomotion environments.""" +"""MDP terms specific to the velocity-tracking locomotion environments. + +Keep generic manager terms in :mod:`isaaclab.envs.mdp`, inherited through the core fallback export. +""" from isaaclab.utils.module import lazy_export diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/mdp/__init__.pyi b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/mdp/__init__.pyi index ae16ca702721..b7704d7e733d 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/mdp/__init__.pyi +++ b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/mdp/__init__.pyi @@ -4,17 +4,11 @@ # SPDX-License-Identifier: BSD-3-Clause __all__ = [ - "body_ang_vel_xy_l2", "feet_air_time", "feet_air_time_positive_biped", "feet_air_time_variance", "feet_slide", "feet_support_edge_distance", - "joint_deviation_l2", - "pelvis_below_terrain_clearance", - "pelvis_height_deficit_l2", - "root_downward_speed_l2", - "set_value_after_steps", "stand_still_joint_deviation_l1", "terrain_levels_vel", "terrain_out_of_bounds", @@ -22,24 +16,17 @@ __all__ = [ "track_lin_vel_xy_yaw_frame_exp", ] -from .curriculums import set_value_after_steps, terrain_levels_vel +from .curriculums import terrain_levels_vel from .rewards import ( - body_ang_vel_xy_l2, feet_air_time, feet_air_time_positive_biped, feet_air_time_variance, feet_slide, feet_support_edge_distance, - joint_deviation_l2, - pelvis_height_deficit_l2, - root_downward_speed_l2, stand_still_joint_deviation_l1, track_ang_vel_z_world_exp, track_lin_vel_xy_yaw_frame_exp, ) -from .terminations import ( - pelvis_below_terrain_clearance, - terrain_out_of_bounds, -) +from .terminations import terrain_out_of_bounds from isaaclab.envs.mdp import * diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/mdp/curriculums.py b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/mdp/curriculums.py index 80108c644da0..92b4d4fb2dcd 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/mdp/curriculums.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/mdp/curriculums.py @@ -8,11 +8,10 @@ from __future__ import annotations from collections.abc import Sequence -from typing import TYPE_CHECKING, Any +from typing import TYPE_CHECKING import torch -from isaaclab.envs.mdp import modify_term_cfg from isaaclab.managers import SceneEntityCfg if TYPE_CHECKING: @@ -51,10 +50,3 @@ def terrain_levels_vel( terrain.update_env_origins(env_ids, move_up, move_down) # return the mean terrain level return torch.mean(terrain.terrain_levels.float()) - - -def set_value_after_steps(env: ManagerBasedRLEnv, env_ids: Sequence[int], data: Any, value: Any, num_steps: int) -> Any: - """Set a term parameter on the first reset at or after ``num_steps`` global environment steps.""" - if env.common_step_counter >= num_steps: - return value - return modify_term_cfg.NO_CHANGE diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/mdp/rewards.py b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/mdp/rewards.py index 4c2b980f763e..90acc4c9afda 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/mdp/rewards.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/mdp/rewards.py @@ -156,48 +156,6 @@ def stand_still_joint_deviation_l1( return mdp.joint_deviation_l1(env, asset_cfg) * (torch.linalg.norm(command[:, :2], dim=1) < command_threshold) -def pelvis_height_deficit_l2( - env, - target_height: float, - asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), - sensor_cfg: SceneEntityCfg = SceneEntityCfg("height_scanner"), -) -> torch.Tensor: - """Squared shortfall of the root below ``target_height`` above the ground beneath it.""" - asset = env.scene[asset_cfg.name] - sensor = env.scene[sensor_cfg.name] - hits = sensor.data.ray_hits_w.torch[..., 2] - ground = torch.nan_to_num(hits, nan=0.0, posinf=0.0, neginf=0.0).median(dim=1).values - clearance = asset.data.root_pos_w.torch[:, 2] - ground - return torch.clamp(target_height - clearance, min=0.0).square() - - -def joint_deviation_l2(env, asset_cfg: SceneEntityCfg) -> torch.Tensor: - """Squared deviation of the selected joints from their default angles.""" - asset = env.scene[asset_cfg.name] - return torch.sum( - torch.square( - asset.data.joint_pos.torch[:, asset_cfg.joint_ids] - - asset.data.default_joint_pos.torch[:, asset_cfg.joint_ids] - ), - dim=1, - ) - - -def body_ang_vel_xy_l2(env, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot")) -> torch.Tensor: - """Squared world-frame roll/pitch angular velocity of a named body.""" - asset = env.scene[asset_cfg.name] - omega = asset.data.body_ang_vel_w.torch[:, asset_cfg.body_ids, :].squeeze(1) - return torch.sum(torch.square(omega[:, :2]), dim=1) - - -def root_downward_speed_l2( - env, speed_limit: float, asset_cfg: SceneEntityCfg = SceneEntityCfg("robot") -) -> torch.Tensor: - """Penalize world-frame downward root speed exceeding the limit [m/s].""" - velocity_z = env.scene[asset_cfg.name].data.root_lin_vel_w.torch[:, 2] - return torch.clamp(-velocity_z - speed_limit, min=0.0).square() - - def sole_edge_distances(sole_points, mesh, height_tolerance=0.04, max_distance=0.25): """Estimate horizontal sole-corner overhang [m] beyond the supporting terrain plane.""" starts = sole_points.clone() diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/mdp/terminations.py b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/mdp/terminations.py index 547b4868fb5d..37559032495a 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/mdp/terminations.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/mdp/terminations.py @@ -48,17 +48,3 @@ def terrain_out_of_bounds( return torch.logical_or(x_out_of_bounds, y_out_of_bounds) else: raise ValueError("Received unsupported terrain type, must be either 'plane' or 'generator'.") - - -def pelvis_below_terrain_clearance( - env, - minimum_height: float, - asset_cfg: SceneEntityCfg = SceneEntityCfg("robot"), - sensor_cfg: SceneEntityCfg = SceneEntityCfg("height_scanner"), -) -> torch.Tensor: - """Terminate when the root sits less than ``minimum_height`` above the ground beneath it.""" - asset = env.scene[asset_cfg.name] - sensor = env.scene[sensor_cfg.name] - hits = sensor.data.ray_hits_w.torch[..., 2] - ground = torch.nan_to_num(hits, nan=0.0, posinf=0.0, neginf=0.0).median(dim=1).values - return asset.data.root_pos_w.torch[:, 2] - ground < minimum_height diff --git a/source/isaaclab_tasks/test/core/test_velocity_newton_cfg.py b/source/isaaclab_tasks/test/core/test_velocity_newton_cfg.py index 37ee4fee5e01..fb1596b53f19 100644 --- a/source/isaaclab_tasks/test/core/test_velocity_newton_cfg.py +++ b/source/isaaclab_tasks/test/core/test_velocity_newton_cfg.py @@ -3,9 +3,15 @@ # # SPDX-License-Identifier: BSD-3-Clause +import ast +from importlib import import_module +from inspect import getsource + import gymnasium as gym from isaaclab_newton.physics import MJWarpSolverCfg +from isaaclab.envs import mdp as core_mdp + from isaaclab_tasks.core.velocity.config.g1 import rough_env_cfg from isaaclab_tasks.core.velocity.config.g1.agents import rsl_rl_ppo_cfg from isaaclab_tasks.utils import load_cfg_from_registry @@ -23,6 +29,11 @@ def test_g1_rough_teacher_registration_and_newton_capacity(monkeypatch): assert isinstance(env_cfg, rough_env_cfg.G1RoughTeacherEnvCfg) assert isinstance(runner_cfg, rsl_rl_ppo_cfg.G1RoughTeacherPPORunnerCfg) + assert isinstance(env_cfg.scene, rough_env_cfg.G1SceneCfg) + assert isinstance(env_cfg.rewards, rough_env_cfg.G1Rewards) + assert isinstance(env_cfg.events, rough_env_cfg.G1EventsCfg) + assert isinstance(env_cfg.curriculum, rough_env_cfg.G1CurriculumCfg) + assert isinstance(env_cfg.terminations, rough_env_cfg.G1TerminationsCfg) solver_cfg = env_cfg.sim.physics.newton_mjwarp.solver_cfg assert isinstance(solver_cfg, MJWarpSolverCfg) assert solver_cfg.njmax == 300 @@ -31,6 +42,39 @@ def test_g1_rough_teacher_registration_and_newton_capacity(monkeypatch): assert not hasattr(rough_env_cfg, "G1RoughEnvCfg") assert not hasattr(rsl_rl_ppo_cfg, "G1RoughPPORunnerCfg") + # Generic manager terms belong to core MDP; task modules must not duplicate or alias them. + for func, module_name in ( + (env_cfg.rewards.joint_deviation_hip.func, "rewards"), + (env_cfg.rewards.pelvis_height.func, "rewards"), + (env_cfg.rewards.torso_ang_vel.func, "rewards"), + (env_cfg.rewards.root_downward_speed.func, "rewards"), + (env_cfg.terminations.base_height.func, "terminations"), + (env_cfg.curriculum.base_height.params["modify_fn"], "curriculums"), + ): + task_module = import_module(f"isaaclab_tasks.core.velocity.mdp.{module_name}") + assert func is getattr(core_mdp, func.__name__) + assert func.__module__ == f"isaaclab.envs.mdp.{module_name}" + assert func.__name__ not in vars(task_module) + + # Scene and manager configs own static settings; initialization only supplies asset-derived values. + for node in ast.walk(ast.parse(getsource(rough_env_cfg.G1RoughTeacherEnvCfg))): + if isinstance(node, ast.Assign): + for target in node.targets: + assert not ast.unparse(target).startswith( + ( + "self.scene.", + "self.rewards.", + "self.events.", + "self.curriculum.", + "self.terminations.", + "self.commands.", + "self.observations.", + "self.sim.", + ) + ) + student_class = ast.parse(getsource(rough_env_cfg.G1RoughDepthStudentEnvCfg)).body[0] + assert not any(isinstance(node, ast.FunctionDef) and node.name == "__post_init__" for node in student_class.body) + def test_g1_flat_keeps_its_robot_and_ppo_settings(): """The flat task keeps its original asset and PPO settings when rough G1 becomes the teacher.""" From 19a7630b514639ee70dc5f8861b6a18db1051273 Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Wed, 21 Oct 2026 20:29:30 -0700 Subject: [PATCH 08/18] Inline G1 student observation group configurations --- .../changelog.d/g1-shared-mdp.changed.rst | 4 +- .../core/velocity/config/g1/rough_env_cfg.py | 90 +++++++------------ .../test/core/test_velocity_newton_cfg.py | 4 + 3 files changed, 39 insertions(+), 59 deletions(-) diff --git a/source/isaaclab_tasks/changelog.d/g1-shared-mdp.changed.rst b/source/isaaclab_tasks/changelog.d/g1-shared-mdp.changed.rst index 2fab693ceabc..08c9a6cec9ab 100644 --- a/source/isaaclab_tasks/changelog.d/g1-shared-mdp.changed.rst +++ b/source/isaaclab_tasks/changelog.d/g1-shared-mdp.changed.rst @@ -5,6 +5,6 @@ * Composed the G1 environments from scene, event, curriculum, termination and observation configurations, leaving initialization to supply asset-derived action scales and sole geometry. * Moved G1 symmetry helpers alongside the other velocity-task robot symmetry modules. -* Reused the common depth-image observation with a G1 image-processing modifier, assembling observation - groups directly in the student environment configuration. +* Reused the common depth-image observation with a G1 image-processing modifier, keeping observation + group definitions inline in the student observations configuration and reusing the shared teacher policy. * Read G1 sole geometry and reference torques from the asset, reusing configured actuator stiffnesses. diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/rough_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/rough_env_cfg.py index dbc4b12d969a..8e1ecf84b6a9 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/rough_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/rough_env_cfg.py @@ -33,7 +33,6 @@ from isaaclab.utils import configclass from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR, retrieve_file_path from isaaclab.utils.modifiers import ModifierCfg -from isaaclab.utils.noise import UniformNoiseCfg as Unoise from isaaclab.utils.string import resolve_matching_names, resolve_matching_names_values from isaaclab_tasks.utils.presets import MultiBackendRendererCfg @@ -451,65 +450,43 @@ class G1DepthStudentSceneCfg(G1SceneCfg): ) -@configclass -class G1TeacherPolicyCfg(ObsGroup): - """Teacher observations, also used as noise-free supervision for distillation.""" - - base_lin_vel = ObsTerm(func=mdp.base_lin_vel, noise=Unoise(n_min=-0.1, n_max=0.1)) - base_ang_vel = ObsTerm(func=mdp.base_ang_vel, noise=Unoise(n_min=-0.2, n_max=0.2)) - projected_gravity = ObsTerm(func=mdp.projected_gravity, noise=Unoise(n_min=-0.05, n_max=0.05)) - velocity_commands = ObsTerm(func=mdp.generated_commands, params={"command_name": "base_velocity"}) - joint_pos = ObsTerm(func=mdp.joint_pos_rel, noise=Unoise(n_min=-0.01, n_max=0.01)) - joint_vel = ObsTerm(func=mdp.joint_vel_rel, noise=Unoise(n_min=-1.5, n_max=1.5)) - actions = ObsTerm(func=mdp.last_action) - height_scan = ObsTerm( - func=mdp.height_scan, - params={"sensor_cfg": SceneEntityCfg("height_scanner")}, - noise=Unoise(n_min=-0.1, n_max=0.1), - clip=(-1.0, 1.0), - ) - enable_corruption = True - concatenate_terms = True - - -@configclass -class G1StudentPolicyCfg(G1TeacherPolicyCfg): - """Five proprioceptive frames without privileged velocity or height observations.""" - - base_lin_vel = None - height_scan = None - history_length = 5 - - -@configclass -class G1DepthObservationsCfg(ObsGroup): - """Normalized depth-frame stack in the calibrated G1 camera convention.""" - - depth = ObsTerm( - func=mdp.image_depth, - params={ - "sensor_cfg": SceneEntityCfg("depth_camera"), - "data_type": "distance_to_image_plane", - "normalize": False, - "frame_stack": DEPTH_STACK, - "channel_first": True, - }, - modifiers=[ModifierCfg(func=process_depth_image)], - clip=(0.0, DEPTH_MAX_RANGE), - scale=1.0 / DEPTH_MAX_RANGE, - ) - enable_corruption = False - concatenate_terms = True - - @configclass class G1StudentObservationsCfg(velocity_env_cfg.ObservationsCfg): """Student inference inputs and teacher/reflected-camera supervision groups.""" - policy: G1StudentPolicyCfg = G1StudentPolicyCfg() - teacher: G1TeacherPolicyCfg = G1TeacherPolicyCfg(enable_corruption=False) - depth: G1DepthObservationsCfg = G1DepthObservationsCfg() - depth_mirror: G1DepthObservationsCfg = G1DepthObservationsCfg() + @configclass + class PolicyCfg(velocity_env_cfg.ObservationsCfg.PolicyCfg): + """Five proprioceptive frames without privileged velocity or height observations.""" + + base_lin_vel = None + height_scan = None + history_length = 5 + + @configclass + class DepthCfg(ObsGroup): + """Normalized depth-frame stack in the calibrated G1 camera convention.""" + + depth = ObsTerm( + func=mdp.image_depth, + params={ + "sensor_cfg": SceneEntityCfg("depth_camera"), + "data_type": "distance_to_image_plane", + "normalize": False, + "frame_stack": DEPTH_STACK, + "channel_first": True, + }, + modifiers=[ModifierCfg(func=process_depth_image)], + clip=(0.0, DEPTH_MAX_RANGE), + scale=1.0 / DEPTH_MAX_RANGE, + ) + enable_corruption = False + concatenate_terms = True + + policy: PolicyCfg = PolicyCfg() + teacher: velocity_env_cfg.ObservationsCfg.PolicyCfg = velocity_env_cfg.ObservationsCfg.PolicyCfg() + teacher.enable_corruption = False + depth: DepthCfg = DepthCfg() + depth_mirror: DepthCfg = DepthCfg() depth_mirror.depth.params["sensor_cfg"] = SceneEntityCfg("depth_camera_mirror") @@ -523,7 +500,6 @@ class G1RoughTeacherEnvCfg(LocomotionVelocityRoughEnvCfg): events: G1EventsCfg = G1EventsCfg() curriculum: G1CurriculumCfg = G1CurriculumCfg() terminations: G1TerminationsCfg = G1TerminationsCfg() - observations: velocity_env_cfg.ObservationsCfg = velocity_env_cfg.ObservationsCfg(policy=G1TeacherPolicyCfg()) sim = SimulationCfg( physics=velocity_env_cfg.RoughPhysicsCfg(), use_newton_actuators=False, diff --git a/source/isaaclab_tasks/test/core/test_velocity_newton_cfg.py b/source/isaaclab_tasks/test/core/test_velocity_newton_cfg.py index fb1596b53f19..4077d0793d5c 100644 --- a/source/isaaclab_tasks/test/core/test_velocity_newton_cfg.py +++ b/source/isaaclab_tasks/test/core/test_velocity_newton_cfg.py @@ -74,6 +74,10 @@ def test_g1_rough_teacher_registration_and_newton_capacity(monkeypatch): ) student_class = ast.parse(getsource(rough_env_cfg.G1RoughDepthStudentEnvCfg)).body[0] assert not any(isinstance(node, ast.FunctionDef) and node.name == "__post_init__" for node in student_class.body) + assert not any( + isinstance(cfg, type) and cfg.__module__ == rough_env_cfg.__name__ and issubclass(cfg, rough_env_cfg.ObsGroup) + for cfg in vars(rough_env_cfg).values() + ) def test_g1_flat_keeps_its_robot_and_ppo_settings(): From bb6fbbdc49db729551723c75f1b82573ea389ffb Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Wed, 21 Oct 2026 20:39:40 -0700 Subject: [PATCH 09/18] Name G1 observation groups by modality --- .../changelog.d/g1-shared-mdp.changed.rst | 7 +- .../velocity/config/g1/agents/distillation.py | 10 ++- .../config/g1/agents/rsl_rl_ppo_cfg.py | 19 ++--- .../core/velocity/config/g1/rough_env_cfg.py | 82 +++++++++++++------ .../test/core/test_velocity_newton_cfg.py | 12 ++- 5 files changed, 86 insertions(+), 44 deletions(-) diff --git a/source/isaaclab_tasks/changelog.d/g1-shared-mdp.changed.rst b/source/isaaclab_tasks/changelog.d/g1-shared-mdp.changed.rst index 08c9a6cec9ab..8989c10d6227 100644 --- a/source/isaaclab_tasks/changelog.d/g1-shared-mdp.changed.rst +++ b/source/isaaclab_tasks/changelog.d/g1-shared-mdp.changed.rst @@ -3,8 +3,11 @@ * Declared the G1 teacher's static reward settings in ``G1Rewards`` and preserved the flat task's reward settings through explicit configuration overrides. * Composed the G1 environments from scene, event, curriculum, termination and observation configurations, - leaving initialization to supply asset-derived action scales and sole geometry. + nesting the depth-camera configuration in its student scene and leaving initialization to supply + asset-derived action scales and sole geometry. * Moved G1 symmetry helpers alongside the other velocity-task robot symmetry modules. * Reused the common depth-image observation with a G1 image-processing modifier, keeping observation - group definitions inline in the student observations configuration and reusing the shared teacher policy. + group definitions inline in the observations configurations. Named groups by their contents: current + proprioception, proprioception history, ray-cast height scans and depth images. +* Declared the teacher's symmetry settings directly in its PPO algorithm configuration. * Read G1 sole geometry and reference torques from the asset, reusing configured actuator stiffnesses. diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/agents/distillation.py b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/agents/distillation.py index 90127b74f10a..481999755abc 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/agents/distillation.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/agents/distillation.py @@ -20,11 +20,12 @@ def __init__(self, env): @torch.no_grad() def observations(self, obs): - assert set(obs.keys()) == {"policy", "teacher", "depth", "depth_mirror"} + assert set(obs.keys()) == {"proprio", "height_scan", "proprio_history", "depth", "depth_mirror"} return TensorDict( { - "policy": _mirror_group(self.env, "policy", obs["policy"]), - "teacher": _mirror_group(self.env, "teacher", obs["teacher"]), + "proprio": _mirror_group(self.env, "proprio", obs["proprio"]), + "height_scan": _mirror_group(self.env, "height_scan", obs["height_scan"]), + "proprio_history": _mirror_group(self.env, "proprio_history", obs["proprio_history"]), "depth": obs["depth_mirror"].flip(-1), "depth_mirror": obs["depth"].flip(-1), }, @@ -66,7 +67,8 @@ def construct_algorithm(obs, env, cfg, device): def process_env_step(self, obs, rewards, dones, extras): mirrored = TensorDict( - {"policy": _mirror_group(self.mirror.env, "policy", obs["policy"])}, batch_size=obs.batch_size + {"proprio_history": _mirror_group(self.mirror.env, "proprio_history", obs["proprio_history"])}, + batch_size=obs.batch_size, ) self.student.update_normalization(mirrored) self.student.update_normalization(obs) diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/agents/rsl_rl_ppo_cfg.py index 421dcd5fda3e..9290f2083c06 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/agents/rsl_rl_ppo_cfg.py @@ -15,13 +15,6 @@ RslRlSymmetryCfg, ) -G1_SYMMETRY_CFG = RslRlSymmetryCfg( - use_data_augmentation=True, - use_mirror_loss=False, - data_augmentation_func="isaaclab_tasks.core.velocity.mdp.symmetry.g1:compute_symmetric_states", - mirror_loss_coeff=0.0, -) - @configclass class G1RoughTeacherPPORunnerCfg(RslRlOnPolicyRunnerCfg): @@ -30,7 +23,7 @@ class G1RoughTeacherPPORunnerCfg(RslRlOnPolicyRunnerCfg): max_iterations = 6000 save_interval = 250 experiment_name = "g1_rough_teacher" - obs_groups = {"actor": ["policy"], "critic": ["policy"]} + obs_groups = {"actor": ["proprio", "height_scan"], "critic": ["proprio", "height_scan"]} actor = RslRlMLPModelCfg( hidden_dims=[512, 256, 128], activation="elu", @@ -55,7 +48,12 @@ class G1RoughTeacherPPORunnerCfg(RslRlOnPolicyRunnerCfg): lam=0.95, desired_kl=0.01, max_grad_norm=1.0, - symmetry_cfg=G1_SYMMETRY_CFG, + symmetry_cfg=RslRlSymmetryCfg( + use_data_augmentation=True, + use_mirror_loss=False, + data_augmentation_func="isaaclab_tasks.core.velocity.mdp.symmetry.g1:compute_symmetric_states", + mirror_loss_coeff=0.0, + ), ) @@ -64,6 +62,7 @@ class G1FlatPPORunnerCfg(G1RoughTeacherPPORunnerCfg): max_iterations = 1500 save_interval = 50 experiment_name = "g1_flat" + obs_groups = {"actor": ["policy"], "critic": ["policy"]} def __post_init__(self): super().__post_init__() @@ -79,7 +78,7 @@ class G1RoughDepthStudentRunnerCfg(RslRlDistillationRunnerCfg): max_iterations = 24000 save_interval = 250 experiment_name = "g1_rough_depth_student" - obs_groups = {"student": ["policy", "depth"], "teacher": ["teacher"]} + obs_groups = {"student": ["proprio_history", "depth"], "teacher": ["proprio", "height_scan"]} student = RslRlCNNModelCfg( cnn_cfg=RslRlCNNModelCfg.CNNCfg( output_channels=[16, 32, 32], diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/rough_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/rough_env_cfg.py index 8e1ecf84b6a9..e68c6a7baa51 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/rough_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/rough_env_cfg.py @@ -33,6 +33,7 @@ from isaaclab.utils import configclass from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR, retrieve_file_path from isaaclab.utils.modifiers import ModifierCfg +from isaaclab.utils.noise import UniformNoiseCfg as Unoise from isaaclab.utils.string import resolve_matching_names, resolve_matching_names_values from isaaclab_tasks.utils.presets import MultiBackendRendererCfg @@ -417,30 +418,29 @@ class G1SceneCfg(velocity_env_cfg.MySceneCfg): ) -@configclass -class G1DepthCameraCfg(CameraCfg): - """Chest-mounted depth camera shared by the student and its reflected view.""" - - update_period = 0.0 - update_latest_camera_pose = True - renderer_cfg = MultiBackendRendererCfg() - width = DEPTH_WIDTH - height = DEPTH_HEIGHT - data_types = ["distance_to_image_plane"] - spawn = PinholeCameraCfg( - focal_length=1.93, - horizontal_aperture=2.0 * 1.93 * math.tan(math.radians(87.0 / 2.0)), - clipping_range=(0.05, 10.0), - ) - offset = CameraCfg.OffsetCfg(pos=DEPTH_OFFSET_POS, rot=DEPTH_OFFSET_ROT, convention="world") - - @configclass class G1DepthStudentSceneCfg(G1SceneCfg): """Teacher scene extended with the student's depth and reflected-depth cameras.""" - depth_camera = G1DepthCameraCfg(prim_path="{ENV_REGEX_NS}/Robot/torso_link/depth_camera") - depth_camera_mirror = G1DepthCameraCfg( + @configclass + class DepthCameraCfg(CameraCfg): + """Chest-mounted depth camera shared by the student and its reflected view.""" + + update_period = 0.0 + update_latest_camera_pose = True + renderer_cfg = MultiBackendRendererCfg() + width = DEPTH_WIDTH + height = DEPTH_HEIGHT + data_types = ["distance_to_image_plane"] + spawn = PinholeCameraCfg( + focal_length=1.93, + horizontal_aperture=2.0 * 1.93 * math.tan(math.radians(87.0 / 2.0)), + clipping_range=(0.05, 10.0), + ) + offset = CameraCfg.OffsetCfg(pos=DEPTH_OFFSET_POS, rot=DEPTH_OFFSET_ROT, convention="world") + + depth_camera = DepthCameraCfg(prim_path="{ENV_REGEX_NS}/Robot/torso_link/depth_camera") + depth_camera_mirror = DepthCameraCfg( prim_path="{ENV_REGEX_NS}/Robot/torso_link/depth_camera_mirror", offset=CameraCfg.OffsetCfg( pos=(DEPTH_OFFSET_POS[0], -DEPTH_OFFSET_POS[1], DEPTH_OFFSET_POS[2]), @@ -451,15 +451,41 @@ class G1DepthStudentSceneCfg(G1SceneCfg): @configclass -class G1StudentObservationsCfg(velocity_env_cfg.ObservationsCfg): - """Student inference inputs and teacher/reflected-camera supervision groups.""" +class G1ObservationsCfg: + """Current proprioception and ray-cast terrain heights.""" + + @configclass + class ProprioCfg(velocity_env_cfg.ObservationsCfg.PolicyCfg): + """Current robot state and velocity commands.""" + + height_scan = None + + @configclass + class HeightScanCfg(ObsGroup): + """Ray-cast terrain heights relative to the robot.""" + + height_scan = ObsTerm( + func=mdp.height_scan, + params={"sensor_cfg": SceneEntityCfg("height_scanner")}, + noise=Unoise(n_min=-0.1, n_max=0.1), + clip=(-1.0, 1.0), + ) + enable_corruption = True + concatenate_terms = True + + proprio: ProprioCfg = ProprioCfg() + height_scan: HeightScanCfg = HeightScanCfg() + + +@configclass +class G1StudentObservationsCfg(G1ObservationsCfg): + """Proprioception history and depth inputs, with noise-free state/height supervision.""" @configclass - class PolicyCfg(velocity_env_cfg.ObservationsCfg.PolicyCfg): + class ProprioHistoryCfg(G1ObservationsCfg.ProprioCfg): """Five proprioceptive frames without privileged velocity or height observations.""" base_lin_vel = None - height_scan = None history_length = 5 @configclass @@ -482,9 +508,10 @@ class DepthCfg(ObsGroup): enable_corruption = False concatenate_terms = True - policy: PolicyCfg = PolicyCfg() - teacher: velocity_env_cfg.ObservationsCfg.PolicyCfg = velocity_env_cfg.ObservationsCfg.PolicyCfg() - teacher.enable_corruption = False + proprio: G1ObservationsCfg.ProprioCfg = G1ObservationsCfg.ProprioCfg() + proprio.enable_corruption = False + height_scan: G1ObservationsCfg.HeightScanCfg = G1ObservationsCfg.HeightScanCfg(enable_corruption=False) + proprio_history: ProprioHistoryCfg = ProprioHistoryCfg() depth: DepthCfg = DepthCfg() depth_mirror: DepthCfg = DepthCfg() depth_mirror.depth.params["sensor_cfg"] = SceneEntityCfg("depth_camera_mirror") @@ -500,6 +527,7 @@ class G1RoughTeacherEnvCfg(LocomotionVelocityRoughEnvCfg): events: G1EventsCfg = G1EventsCfg() curriculum: G1CurriculumCfg = G1CurriculumCfg() terminations: G1TerminationsCfg = G1TerminationsCfg() + observations: G1ObservationsCfg = G1ObservationsCfg() sim = SimulationCfg( physics=velocity_env_cfg.RoughPhysicsCfg(), use_newton_actuators=False, diff --git a/source/isaaclab_tasks/test/core/test_velocity_newton_cfg.py b/source/isaaclab_tasks/test/core/test_velocity_newton_cfg.py index 4077d0793d5c..3a9604ca6313 100644 --- a/source/isaaclab_tasks/test/core/test_velocity_newton_cfg.py +++ b/source/isaaclab_tasks/test/core/test_velocity_newton_cfg.py @@ -41,6 +41,14 @@ def test_g1_rough_teacher_registration_and_newton_capacity(monkeypatch): assert "Isaac-Velocity-Rough-G1-Teacher" not in gym.registry assert not hasattr(rough_env_cfg, "G1RoughEnvCfg") assert not hasattr(rsl_rl_ppo_cfg, "G1RoughPPORunnerCfg") + assert not hasattr(rsl_rl_ppo_cfg, "G1_SYMMETRY_CFG") + student_env_cfg = load_cfg_from_registry("Isaac-Velocity-Rough-G1-Depth-Student", "env_cfg_entry_point") + student_runner_cfg = load_cfg_from_registry("Isaac-Velocity-Rough-G1-Depth-Student", "rsl_rl_cfg_entry_point") + assert student_runner_cfg.obs_groups["teacher"] == runner_cfg.obs_groups["actor"] + for cfg, runner in ((env_cfg, runner_cfg), (student_env_cfg, student_runner_cfg)): + assert not {"policy", "teacher"} & vars(cfg.observations).keys() + for groups in runner.obs_groups.values(): + assert all(isinstance(getattr(cfg.observations, group), rough_env_cfg.ObsGroup) for group in groups) # Generic manager terms belong to core MDP; task modules must not duplicate or alias them. for func, module_name in ( @@ -75,7 +83,9 @@ def test_g1_rough_teacher_registration_and_newton_capacity(monkeypatch): student_class = ast.parse(getsource(rough_env_cfg.G1RoughDepthStudentEnvCfg)).body[0] assert not any(isinstance(node, ast.FunctionDef) and node.name == "__post_init__" for node in student_class.body) assert not any( - isinstance(cfg, type) and cfg.__module__ == rough_env_cfg.__name__ and issubclass(cfg, rough_env_cfg.ObsGroup) + isinstance(cfg, type) + and cfg.__module__ == rough_env_cfg.__name__ + and issubclass(cfg, (rough_env_cfg.ObsGroup, rough_env_cfg.CameraCfg)) for cfg in vars(rough_env_cfg).values() ) From ea343027509ab70ae6732b8f0c5e32cebbb68c96 Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Wed, 21 Oct 2026 20:41:59 -0700 Subject: [PATCH 10/18] Move G1 observation overrides into post-init --- .../isaaclab_tasks/changelog.d/g1-shared-mdp.changed.rst | 5 +++-- .../core/velocity/config/g1/rough_env_cfg.py | 8 +++++--- 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/source/isaaclab_tasks/changelog.d/g1-shared-mdp.changed.rst b/source/isaaclab_tasks/changelog.d/g1-shared-mdp.changed.rst index 8989c10d6227..3239d8fce9b4 100644 --- a/source/isaaclab_tasks/changelog.d/g1-shared-mdp.changed.rst +++ b/source/isaaclab_tasks/changelog.d/g1-shared-mdp.changed.rst @@ -3,11 +3,12 @@ * Declared the G1 teacher's static reward settings in ``G1Rewards`` and preserved the flat task's reward settings through explicit configuration overrides. * Composed the G1 environments from scene, event, curriculum, termination and observation configurations, - nesting the depth-camera configuration in its student scene and leaving initialization to supply + nesting the depth-camera configuration in its student scene and leaving environment initialization to supply asset-derived action scales and sole geometry. * Moved G1 symmetry helpers alongside the other velocity-task robot symmetry modules. * Reused the common depth-image observation with a G1 image-processing modifier, keeping observation group definitions inline in the observations configurations. Named groups by their contents: current - proprioception, proprioception history, ray-cast height scans and depth images. + proprioception, proprioception history, ray-cast height scans and depth images. Apply student supervision + noise and reflected-camera overrides in the observations configuration's post-initialization hook. * Declared the teacher's symmetry settings directly in its PPO algorithm configuration. * Read G1 sole geometry and reference torques from the asset, reusing configured actuator stiffnesses. diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/rough_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/rough_env_cfg.py index e68c6a7baa51..d7e671d0b200 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/rough_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/rough_env_cfg.py @@ -508,13 +508,15 @@ class DepthCfg(ObsGroup): enable_corruption = False concatenate_terms = True - proprio: G1ObservationsCfg.ProprioCfg = G1ObservationsCfg.ProprioCfg() - proprio.enable_corruption = False height_scan: G1ObservationsCfg.HeightScanCfg = G1ObservationsCfg.HeightScanCfg(enable_corruption=False) proprio_history: ProprioHistoryCfg = ProprioHistoryCfg() depth: DepthCfg = DepthCfg() depth_mirror: DepthCfg = DepthCfg() - depth_mirror.depth.params["sensor_cfg"] = SceneEntityCfg("depth_camera_mirror") + + def __post_init__(self): + super().__post_init__() + self.proprio.enable_corruption = False + self.depth_mirror.depth.params["sensor_cfg"] = SceneEntityCfg("depth_camera_mirror") @configclass From fd8e8078bec8f727db7dcb8d8a7e0b125cc2a3ee Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Wed, 21 Oct 2026 20:49:14 -0700 Subject: [PATCH 11/18] Standardize flat G1 observations and extract terrain preset --- .../source/_static/css/environment-browser.js | 2 +- .../changelog.d/g1-flat-skrl.removed.rst | 2 + .../changelog.d/g1-shared-mdp.changed.rst | 3 + .../core/velocity/config/g1/__init__.py | 1 - .../config/g1/agents/rsl_rl_ppo_cfg.py | 2 +- .../config/g1/agents/skrl_flat_ppo_cfg.yaml | 85 ------------------- .../core/velocity/config/g1/flat_env_cfg.py | 5 +- .../core/velocity/config/g1/rough_env_cfg.py | 82 ++---------------- .../core/velocity/config/g1/terrains.py | 68 +++++++++++++++ .../test/core/test_velocity_newton_cfg.py | 5 ++ 10 files changed, 91 insertions(+), 164 deletions(-) create mode 100644 source/isaaclab_tasks/changelog.d/g1-flat-skrl.removed.rst delete mode 100644 source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/agents/skrl_flat_ppo_cfg.yaml create mode 100644 source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/terrains.py diff --git a/docs/source/_static/css/environment-browser.js b/docs/source/_static/css/environment-browser.js index d561662fa224..46aaa2622b01 100644 --- a/docs/source/_static/css/environment-browser.js +++ b/docs/source/_static/css/environment-browser.js @@ -48,7 +48,7 @@ ["Isaac-Shadow-Handover", "rsl_rl", "isaacsim_physx,newton_mjwarp,ovphysx", "", "randomized", "environment-previews/shadow_hand_over.jpg", false, {}, {}, "", null, ["||", "||randomized", "isaacsim_physx||", "isaacsim_physx||randomized", "newton_mjwarp||", "newton_mjwarp||randomized", "ovphysx||", "ovphysx||randomized"]], ["Isaac-Velocity-Flat-AnymalD", "rsl_rl,skrl", "isaacsim_physx,newton_kamino,newton_mjwarp,ovphysx", "", "", "environment-previews/anymal_d_flat.jpg", true, {}, {}, "", null, ["||", "isaacsim_physx||", "newton_kamino||", "newton_mjwarp||", "ovphysx||"]], ["Isaac-Velocity-Flat-Cassie", "rsl_rl,skrl", "isaacsim_physx,newton_kamino,newton_mjwarp,ovphysx", "", "", "environment-previews/cassie_flat.jpg", true, {}, {}, "", null, ["||", "isaacsim_physx||", "newton_kamino||", "newton_mjwarp||", "ovphysx||"]], - ["Isaac-Velocity-Flat-G1", "rsl_rl,skrl", "isaacsim_physx,newton_kamino,newton_mjwarp,ovphysx", "", "", "environment-previews/g1_flat.jpg", true, {}, {}, "", null, ["||", "isaacsim_physx||", "newton_kamino||", "newton_mjwarp||", "ovphysx||"]], + ["Isaac-Velocity-Flat-G1", "rsl_rl", "isaacsim_physx,newton_kamino,newton_mjwarp,ovphysx", "", "", "environment-previews/g1_flat.jpg", true, {}, {}, "", null, ["||", "isaacsim_physx||", "newton_kamino||", "newton_mjwarp||", "ovphysx||"]], ["Isaac-Velocity-Flat-H1", "rsl_rl,skrl", "isaacsim_physx,newton_kamino,newton_mjwarp,ovphysx", "", "", "environment-previews/h1_flat.jpg", true, {}, {}, "", null, ["||", "isaacsim_physx||", "newton_kamino||", "newton_mjwarp||", "ovphysx||"]], ["Isaac-Velocity-Flat-UnitreeGo2", "rsl_rl,skrl", "isaacsim_physx,newton_kamino,newton_mjwarp,ovphysx", "", "", "environment-previews/go2_flat.jpg", true, {}, {}, "", null, ["||", "isaacsim_physx||", "newton_kamino||", "newton_mjwarp||", "ovphysx||"]], ["Isaac-Velocity-Rough-AnymalD", "rsl_rl,skrl", "isaacsim_physx,newton_kamino,newton_mjwarp,ovphysx", "", "", "tasks/locomotion/anymal_d_rough.jpg", false, {}, {}, "", null, ["||", "isaacsim_physx||", "newton_kamino||", "newton_mjwarp||", "ovphysx||"]], diff --git a/source/isaaclab_tasks/changelog.d/g1-flat-skrl.removed.rst b/source/isaaclab_tasks/changelog.d/g1-flat-skrl.removed.rst new file mode 100644 index 000000000000..b9eba8561719 --- /dev/null +++ b/source/isaaclab_tasks/changelog.d/g1-flat-skrl.removed.rst @@ -0,0 +1,2 @@ +* **Breaking:** Removed the SKRL registration and recipe for ``Isaac-Velocity-Flat-G1``. Use + ``--rl_library rsl_rl`` to train, play or export this task. diff --git a/source/isaaclab_tasks/changelog.d/g1-shared-mdp.changed.rst b/source/isaaclab_tasks/changelog.d/g1-shared-mdp.changed.rst index 3239d8fce9b4..d3175d1d6684 100644 --- a/source/isaaclab_tasks/changelog.d/g1-shared-mdp.changed.rst +++ b/source/isaaclab_tasks/changelog.d/g1-shared-mdp.changed.rst @@ -11,4 +11,7 @@ proprioception, proprioception history, ray-cast height scans and depth images. Apply student supervision noise and reflected-camera overrides in the observations configuration's post-initialization hook. * Declared the teacher's symmetry settings directly in its PPO algorithm configuration. +* **Breaking:** Renamed the flat G1 observation group from ``policy`` to ``proprio``. Update observation-group + references and configuration overrides to use ``proprio``. +* Moved the G1 mesh-terrain preset into the G1 terrain configuration module. * Read G1 sole geometry and reference torques from the asset, reusing configured actuator stiffnesses. diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/__init__.py b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/__init__.py index 80eeee1b2177..d68ddf151183 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/__init__.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/__init__.py @@ -21,7 +21,6 @@ "env_cfg_entry_point": f"{__name__}.flat_env_cfg:G1FlatEnvCfg", "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:G1FlatPPORunnerCfg", "default_agent": "rsl_rl", - "skrl_cfg_entry_point": f"{agents.__name__}:skrl_flat_ppo_cfg.yaml", }, ) diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/agents/rsl_rl_ppo_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/agents/rsl_rl_ppo_cfg.py index 9290f2083c06..596647cf3fdd 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/agents/rsl_rl_ppo_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/agents/rsl_rl_ppo_cfg.py @@ -62,7 +62,7 @@ class G1FlatPPORunnerCfg(G1RoughTeacherPPORunnerCfg): max_iterations = 1500 save_interval = 50 experiment_name = "g1_flat" - obs_groups = {"actor": ["policy"], "critic": ["policy"]} + obs_groups = {"actor": ["proprio"], "critic": ["proprio"]} def __post_init__(self): super().__post_init__() diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/agents/skrl_flat_ppo_cfg.yaml b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/agents/skrl_flat_ppo_cfg.yaml deleted file mode 100644 index 711b7190245e..000000000000 --- a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/agents/skrl_flat_ppo_cfg.yaml +++ /dev/null @@ -1,85 +0,0 @@ -# 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 - -seed: 42 - - -# Models are instantiated using skrl's model instantiator utility -# https://skrl.readthedocs.io/en/latest/api/utils/model_instantiators.html -models: - separate: False - policy: # see gaussian_model parameters - class: GaussianMixin - clip_actions: False - clip_log_std: True - min_log_std: -20.0 - max_log_std: 2.0 - initial_log_std: 0.0 - network: - - name: net - input: OBSERVATIONS - layers: [256, 128, 128] - activations: elu - output: ACTIONS - value: # see deterministic_model parameters - class: DeterministicMixin - clip_actions: False - network: - - name: net - input: OBSERVATIONS - layers: [256, 128, 128] - activations: elu - output: ONE - - -# Rollout memory -# https://skrl.readthedocs.io/en/latest/api/memories/random.html -memory: - class: RandomMemory - memory_size: -1 # automatically determined (same as agent:rollouts) - - -# PPO agent configuration (field names are from PPO_DEFAULT_CONFIG) -# https://skrl.readthedocs.io/en/latest/api/agents/ppo.html -agent: - class: PPO - rollouts: 24 - learning_epochs: 5 - mini_batches: 4 - discount_factor: 0.99 - lambda: 0.95 - learning_rate: 1.0e-03 - learning_rate_scheduler: KLAdaptiveLR - learning_rate_scheduler_kwargs: - kl_threshold: 0.01 - state_preprocessor: null - state_preprocessor_kwargs: null - value_preprocessor: null - value_preprocessor_kwargs: null - random_timesteps: 0 - learning_starts: 0 - grad_norm_clip: 1.0 - ratio_clip: 0.2 - value_clip: 0.2 - clip_predicted_values: True - entropy_loss_scale: 0.008 - value_loss_scale: 1.0 - kl_threshold: 0.0 - rewards_shaper_scale: 1.0 - time_limit_bootstrap: False - # logging and checkpoint - experiment: - directory: "g1_flat" - experiment_name: "" - write_interval: auto - checkpoint_interval: auto - - -# Sequential trainer -# https://skrl.readthedocs.io/en/latest/api/trainers/sequential.html -trainer: - class: SequentialTrainer - timesteps: 36000 - environment_info: log diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/flat_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/flat_env_cfg.py index 8eec02859f34..c739d705b36e 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/flat_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/flat_env_cfg.py @@ -13,7 +13,7 @@ from ... import mdp from ...velocity_env_cfg import LocomotionVelocityRoughEnvCfg -from .rough_env_cfg import G1EventsCfg, G1Rewards +from .rough_env_cfg import G1EventsCfg, G1ObservationsCfg, G1Rewards @configclass @@ -21,6 +21,7 @@ class G1FlatEnvCfg(LocomotionVelocityRoughEnvCfg): """Configuration for the Unitree G1 velocity-tracking environment on flat terrain.""" events: G1EventsCfg = G1EventsCfg() + observations: G1ObservationsCfg = G1ObservationsCfg(height_scan=None) rewards: G1Rewards = G1Rewards( track_lin_vel_xy_exp=RewTerm( func=mdp.track_lin_vel_xy_yaw_frame_exp, @@ -121,8 +122,6 @@ def __post_init__(self): self.scene.terrain.terrain_type = "plane" self.scene.terrain.terrain_generator = None self.scene.height_scanner = None - # observations - self.observations.policy.height_scan = None # commands self.commands.base_velocity.vel_yaw_success_threshold = 0.8 self.commands.base_velocity.marker_pos_offset = (0.0, 0.0, 0.75) diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/rough_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/rough_env_cfg.py index d7e671d0b200..5f420584e96d 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/rough_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/rough_env_cfg.py @@ -17,7 +17,6 @@ from isaaclab_newton.sim.schemas import NewtonArticulationCfg from isaaclab_physx.sim.schemas import PhysxArticulationCfg, PhysxRigidBodyCfg -import isaaclab.terrains as terrain_gen from isaaclab.actuators import ImplicitActuatorCfg from isaaclab.assets import ArticulationCfg from isaaclab.managers import CurriculumTermCfg as CurrTerm @@ -29,7 +28,6 @@ from isaaclab.managers import TerminationTermCfg as DoneTerm from isaaclab.sensors import CameraCfg, RayCasterCfg, patterns from isaaclab.sim import PinholeCameraCfg, SimulationCfg, UsdFileCfg -from isaaclab.terrains import TerrainGeneratorCfg from isaaclab.utils import configclass from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR, retrieve_file_path from isaaclab.utils.modifiers import ModifierCfg @@ -43,6 +41,14 @@ LocomotionVelocityRoughEnvCfg, RewardsCfg, ) +from .terrains import G1_ROUGH_TERRAINS_CFG + +DEPTH_WIDTH = 64 +DEPTH_HEIGHT = 38 +DEPTH_STACK = 3 +DEPTH_MAX_RANGE = 3.0 +DEPTH_OFFSET_POS = (0.0576, 0.0175, 0.4299) +DEPTH_OFFSET_ROT = (0.9150, 0.0, -0.4035, 0.0) @configclass @@ -153,19 +159,6 @@ class G1Rewards(RewardsCfg): ) -DEPTH_WIDTH = 64 - -DEPTH_HEIGHT = 38 - -DEPTH_STACK = 3 - -DEPTH_MAX_RANGE = 3.0 - -DEPTH_OFFSET_POS = (0.0576, 0.0175, 0.4299) - -DEPTH_OFFSET_ROT = (0.9150, 0.0, -0.4035, 0.0) - - def process_depth_image(data: torch.Tensor) -> torch.Tensor: """Fill missing depth and rotate the channel-first frames to the G1 camera convention.""" return torch.nan_to_num(data, nan=DEPTH_MAX_RANGE, posinf=DEPTH_MAX_RANGE).flip((-2, -1)) @@ -358,64 +351,7 @@ class G1SceneCfg(velocity_env_cfg.MySceneCfg): global_world_only=True, ) terrain = velocity_env_cfg.MySceneCfg().terrain - terrain.terrain_generator = TerrainGeneratorCfg( - size=(8.0, 8.0), - border_width=20.0, - num_rows=10, - num_cols=20, - horizontal_scale=0.1, - vertical_scale=0.005, - slope_threshold=0.75, - use_cache=False, - sub_terrains={ - "pyramid_stairs": terrain_gen.MeshPyramidStairsTerrainCfg( - proportion=0.2, - step_height_range=(0.05, 0.35), - step_width=0.3, - platform_width=3.0, - border_width=1.0, - holes=False, - convert_to_heightfield=False, - ), - "pyramid_stairs_inv": terrain_gen.MeshInvertedPyramidStairsTerrainCfg( - proportion=0.2, - step_height_range=(0.05, 0.35), - step_width=0.3, - platform_width=3.0, - border_width=1.0, - holes=False, - convert_to_heightfield=False, - ), - "boxes": terrain_gen.MeshRandomGridTerrainCfg( - proportion=0.2, - grid_width=0.45, - grid_height_range=(0.05, 0.2), - platform_width=2.0, - convert_to_heightfield=False, - ), - "random_rough": terrain_gen.HfRandomUniformTerrainCfg( - proportion=0.2, - noise_range=(0.02, 0.10), - noise_step=0.02, - border_width=0.25, - convert_to_heightfield=False, - ), - "hf_pyramid_slope": terrain_gen.HfPyramidSlopedTerrainCfg( - proportion=0.1, - slope_range=(0.0, 0.4), - platform_width=2.0, - border_width=0.25, - convert_to_heightfield=False, - ), - "hf_pyramid_slope_inv": terrain_gen.HfInvertedPyramidSlopedTerrainCfg( - proportion=0.1, - slope_range=(0.0, 0.4), - platform_width=2.0, - border_width=0.25, - convert_to_heightfield=False, - ), - }, - ) + terrain.terrain_generator = G1_ROUGH_TERRAINS_CFG @configclass diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/terrains.py b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/terrains.py new file mode 100644 index 000000000000..8a99b841235a --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/terrains.py @@ -0,0 +1,68 @@ +# Copyright (c) 2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Mesh-terrain preset shared by the G1 teacher and depth student.""" + +import isaaclab.terrains as terrain_gen +from isaaclab.terrains import TerrainGeneratorCfg + +G1_ROUGH_TERRAINS_CFG = TerrainGeneratorCfg( + size=(8.0, 8.0), + border_width=20.0, + num_rows=10, + num_cols=20, + horizontal_scale=0.1, + vertical_scale=0.005, + slope_threshold=0.75, + use_cache=False, + sub_terrains={ + "pyramid_stairs": terrain_gen.MeshPyramidStairsTerrainCfg( + proportion=0.2, + step_height_range=(0.05, 0.35), + step_width=0.3, + platform_width=3.0, + border_width=1.0, + holes=False, + convert_to_heightfield=False, + ), + "pyramid_stairs_inv": terrain_gen.MeshInvertedPyramidStairsTerrainCfg( + proportion=0.2, + step_height_range=(0.05, 0.35), + step_width=0.3, + platform_width=3.0, + border_width=1.0, + holes=False, + convert_to_heightfield=False, + ), + "boxes": terrain_gen.MeshRandomGridTerrainCfg( + proportion=0.2, + grid_width=0.45, + grid_height_range=(0.05, 0.2), + platform_width=2.0, + convert_to_heightfield=False, + ), + "random_rough": terrain_gen.HfRandomUniformTerrainCfg( + proportion=0.2, + noise_range=(0.02, 0.10), + noise_step=0.02, + border_width=0.25, + convert_to_heightfield=False, + ), + "hf_pyramid_slope": terrain_gen.HfPyramidSlopedTerrainCfg( + proportion=0.1, + slope_range=(0.0, 0.4), + platform_width=2.0, + border_width=0.25, + convert_to_heightfield=False, + ), + "hf_pyramid_slope_inv": terrain_gen.HfInvertedPyramidSlopedTerrainCfg( + proportion=0.1, + slope_range=(0.0, 0.4), + platform_width=2.0, + border_width=0.25, + convert_to_heightfield=False, + ), + }, +) diff --git a/source/isaaclab_tasks/test/core/test_velocity_newton_cfg.py b/source/isaaclab_tasks/test/core/test_velocity_newton_cfg.py index 3a9604ca6313..5a1aa8c9de70 100644 --- a/source/isaaclab_tasks/test/core/test_velocity_newton_cfg.py +++ b/source/isaaclab_tasks/test/core/test_velocity_newton_cfg.py @@ -40,6 +40,7 @@ def test_g1_rough_teacher_registration_and_newton_capacity(monkeypatch): assert runner_cfg.algorithm.symmetry_cfg.use_data_augmentation assert "Isaac-Velocity-Rough-G1-Teacher" not in gym.registry assert not hasattr(rough_env_cfg, "G1RoughEnvCfg") + assert not hasattr(rough_env_cfg, "TerrainGeneratorCfg") assert not hasattr(rsl_rl_ppo_cfg, "G1RoughPPORunnerCfg") assert not hasattr(rsl_rl_ppo_cfg, "G1_SYMMETRY_CFG") student_env_cfg = load_cfg_from_registry("Isaac-Velocity-Rough-G1-Depth-Student", "env_cfg_entry_point") @@ -98,6 +99,10 @@ def test_g1_flat_keeps_its_robot_and_ppo_settings(): assert env_cfg.scene.robot.spawn.usd_path == G1_MINIMAL_CFG.spawn.usd_path assert env_cfg.scene.terrain.terrain_type == "plane" assert env_cfg.scene.height_scanner is None + assert not hasattr(env_cfg.observations, "policy") + assert env_cfg.observations.height_scan is None + assert runner_cfg.obs_groups == {"actor": ["proprio"], "critic": ["proprio"]} + assert "skrl_cfg_entry_point" not in gym.spec("Isaac-Velocity-Flat-G1").kwargs assert runner_cfg.algorithm.symmetry_cfg is None assert runner_cfg.actor.hidden_dims == runner_cfg.critic.hidden_dims == [256, 128, 128] assert runner_cfg.max_iterations == 1500 From 7c4abfd7884b043e32740345c3405a592b95cbe4 Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Wed, 21 Oct 2026 20:53:11 -0700 Subject: [PATCH 12/18] Omit default G1 visualizer configuration arguments --- .../core/velocity/config/g1/rough_env_cfg.py | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/rough_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/rough_env_cfg.py index 5f420584e96d..6272d459b5ac 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/rough_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/rough_env_cfg.py @@ -466,12 +466,7 @@ class G1RoughTeacherEnvCfg(LocomotionVelocityRoughEnvCfg): curriculum: G1CurriculumCfg = G1CurriculumCfg() terminations: G1TerminationsCfg = G1TerminationsCfg() observations: G1ObservationsCfg = G1ObservationsCfg() - sim = SimulationCfg( - physics=velocity_env_cfg.RoughPhysicsCfg(), - use_newton_actuators=False, - visualizer_cfgs=[], - default_visualizer_cfg=None, - ) + sim = SimulationCfg(physics=velocity_env_cfg.RoughPhysicsCfg(), use_newton_actuators=False) sim.physics.newton_mjwarp.solver_cfg.njmax = 300 commands = velocity_env_cfg.CommandsCfg() commands.base_velocity.vel_yaw_success_threshold = 0.8 From 2a92ee78408e3debf883154933aa6252218f5616 Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Wed, 21 Oct 2026 20:56:50 -0700 Subject: [PATCH 13/18] Use the shared default actuator path for rough G1 --- source/isaaclab_tasks/changelog.d/g1-shared-mdp.changed.rst | 2 ++ .../isaaclab_tasks/core/velocity/config/g1/rough_env_cfg.py | 2 +- source/isaaclab_tasks/test/core/test_velocity_newton_cfg.py | 1 + 3 files changed, 4 insertions(+), 1 deletion(-) diff --git a/source/isaaclab_tasks/changelog.d/g1-shared-mdp.changed.rst b/source/isaaclab_tasks/changelog.d/g1-shared-mdp.changed.rst index d3175d1d6684..4bd3980d82b7 100644 --- a/source/isaaclab_tasks/changelog.d/g1-shared-mdp.changed.rst +++ b/source/isaaclab_tasks/changelog.d/g1-shared-mdp.changed.rst @@ -14,4 +14,6 @@ * **Breaking:** Renamed the flat G1 observation group from ``policy`` to ``proprio``. Update observation-group references and configuration overrides to use ``proprio``. * Moved the G1 mesh-terrain preset into the G1 terrain configuration module. +* Removed the rough G1 actuator-path override, using the current default shared across physics backends. + The configured implicit actuators retain their solver-owned drives. * Read G1 sole geometry and reference torques from the asset, reusing configured actuator stiffnesses. diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/rough_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/rough_env_cfg.py index 6272d459b5ac..83f0e21f9c0d 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/rough_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/rough_env_cfg.py @@ -466,7 +466,7 @@ class G1RoughTeacherEnvCfg(LocomotionVelocityRoughEnvCfg): curriculum: G1CurriculumCfg = G1CurriculumCfg() terminations: G1TerminationsCfg = G1TerminationsCfg() observations: G1ObservationsCfg = G1ObservationsCfg() - sim = SimulationCfg(physics=velocity_env_cfg.RoughPhysicsCfg(), use_newton_actuators=False) + sim = SimulationCfg(physics=velocity_env_cfg.RoughPhysicsCfg()) sim.physics.newton_mjwarp.solver_cfg.njmax = 300 commands = velocity_env_cfg.CommandsCfg() commands.base_velocity.vel_yaw_success_threshold = 0.8 diff --git a/source/isaaclab_tasks/test/core/test_velocity_newton_cfg.py b/source/isaaclab_tasks/test/core/test_velocity_newton_cfg.py index 5a1aa8c9de70..24f8e8f45c03 100644 --- a/source/isaaclab_tasks/test/core/test_velocity_newton_cfg.py +++ b/source/isaaclab_tasks/test/core/test_velocity_newton_cfg.py @@ -34,6 +34,7 @@ def test_g1_rough_teacher_registration_and_newton_capacity(monkeypatch): assert isinstance(env_cfg.events, rough_env_cfg.G1EventsCfg) assert isinstance(env_cfg.curriculum, rough_env_cfg.G1CurriculumCfg) assert isinstance(env_cfg.terminations, rough_env_cfg.G1TerminationsCfg) + assert env_cfg.sim.use_newton_actuators solver_cfg = env_cfg.sim.physics.newton_mjwarp.solver_cfg assert isinstance(solver_cfg, MJWarpSolverCfg) assert solver_cfg.njmax == 300 From 1714b700174eb59876ea8c9d9b2778321131604b Mon Sep 17 00:00:00 2001 From: henry Date: Thu, 22 Oct 2026 13:15:30 +0800 Subject: [PATCH 14/18] Document G1 deployment environment setup --- ...eploying_a_g1_visual_locomotion_policy.rst | 37 +++++++++++++++---- 1 file changed, 29 insertions(+), 8 deletions(-) diff --git a/docs/source/policy_deployment/07_g1_sim2real/training_and_deploying_a_g1_visual_locomotion_policy.rst b/docs/source/policy_deployment/07_g1_sim2real/training_and_deploying_a_g1_visual_locomotion_policy.rst index b4dd2a0347ba..02cfbc25d767 100644 --- a/docs/source/policy_deployment/07_g1_sim2real/training_and_deploying_a_g1_visual_locomotion_policy.rst +++ b/docs/source/policy_deployment/07_g1_sim2real/training_and_deploying_a_g1_visual_locomotion_policy.rst @@ -39,16 +39,37 @@ continues from there onto hardware. The inference stage is unchanged — it is t Prerequisites ------------- -In addition to the ``leapp`` extra required by export, hardware deployment needs -``unitree_sdk2py`` with ``cyclonedds==0.10.2``, a wired connection to the robot's subnet, and a -depth publisher running on the robot's onboard computer. +Start with the :doc:`Isaac Lab installation `. From the repository +root, install the export and inference dependencies into its Python environment: -.. note:: +.. code-block:: bash + + uv sync --extra leapp + +For hardware deployment on Ubuntu, install CycloneDDS and the Unitree Python SDK in the same +environment. Use version ``0.10.2`` for compatibility with the robot. Python 3.12 requires building +the DDS library first, following the `Unitree SDK installation instructions +`__: + +.. code-block:: bash + + sudo apt-get install -y build-essential cmake git + git clone --depth 1 --branch 0.10.2 https://github.com/eclipse-cyclonedds/cyclonedds.git /tmp/cyclonedds-0.10.2 + export CYCLONEDDS_HOME="$HOME/.local/cyclonedds-0.10.2" + cmake -S /tmp/cyclonedds-0.10.2 -B /tmp/cyclonedds-0.10.2/build -DCMAKE_INSTALL_PREFIX="$CYCLONEDDS_HOME" + cmake --build /tmp/cyclonedds-0.10.2/build --target install --parallel + uv pip install --python .venv/bin/python "cyclonedds==0.10.2" \ + "git+https://github.com/unitreerobotics/unitree_sdk2_python.git" + +Set ``CYCLONEDDS_HOME`` to the same path in each deployment terminal. Check the SDK import before +connecting to the robot: + +.. code-block:: bash + + uv run --extra leapp python -c "from unitree_sdk2py.core.channel import ChannelFactoryInitialize; print('SDK ready')" - The ``cyclonedds`` pin is load-bearing. A newer CycloneDDS enforces XTypes type consistency at - match time, and the SDK's Python IDL hashes differently from the robot's C++ IDL. Readers then - never match. This passes every simulation test, because both ends share the Python serializer, - and fails only against hardware. +Connect the workstation to the robot's subnet over Ethernet. The robot's onboard computer also +needs the RealSense drivers and a depth publisher, as described in `Starting the Camera`_ below. Training the Teacher From 34039a067e27049730c2894b440cbbe5014b37a7 Mon Sep 17 00:00:00 2001 From: henry Date: Thu, 22 Oct 2026 13:45:40 +0800 Subject: [PATCH 15/18] Remove deployment gains explanation --- .../training_and_deploying_a_g1_visual_locomotion_policy.rst | 3 --- 1 file changed, 3 deletions(-) diff --git a/docs/source/policy_deployment/07_g1_sim2real/training_and_deploying_a_g1_visual_locomotion_policy.rst b/docs/source/policy_deployment/07_g1_sim2real/training_and_deploying_a_g1_visual_locomotion_policy.rst index 02cfbc25d767..cf8b583c0e1b 100644 --- a/docs/source/policy_deployment/07_g1_sim2real/training_and_deploying_a_g1_visual_locomotion_policy.rst +++ b/docs/source/policy_deployment/07_g1_sim2real/training_and_deploying_a_g1_visual_locomotion_policy.rst @@ -144,9 +144,6 @@ previous action and the observation normalization all live inside the graph, and ``_initial_values.safetensors`` file holds their reset state. Nothing outside the graph recomputes any of it. -The gains are graph outputs, not configuration: the deployment writes the Kp and Kd the policy -asks for on each step. - Running on the Robot -------------------- From c8ab74e85eb30f8d85f4d2a54ba94e5622110912 Mon Sep 17 00:00:00 2001 From: henry Date: Thu, 22 Oct 2026 13:57:59 +0800 Subject: [PATCH 16/18] Simplify G1 deployment documentation --- ...g_and_deploying_a_g1_visual_locomotion_policy.rst | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/docs/source/policy_deployment/07_g1_sim2real/training_and_deploying_a_g1_visual_locomotion_policy.rst b/docs/source/policy_deployment/07_g1_sim2real/training_and_deploying_a_g1_visual_locomotion_policy.rst index cf8b583c0e1b..ce81df0901b8 100644 --- a/docs/source/policy_deployment/07_g1_sim2real/training_and_deploying_a_g1_visual_locomotion_policy.rst +++ b/docs/source/policy_deployment/07_g1_sim2real/training_and_deploying_a_g1_visual_locomotion_policy.rst @@ -200,19 +200,15 @@ The Control Loop Two details about the ends of this loop. -The graph writes **every joint of the articulation**, fingers included, so its 43 outputs are -matched to the 29 motors ``rt/lowcmd`` carries by name, using the ``element_names`` the export -records. The two orders agree for the first 22 joints and then diverge, which is the kind of -mismatch that produces a walking robot with the wrong arm. +Joint targets are mapped to the 29 body motors by joint name using the exported ``element_names``. -The ramp target is a fixed constant, not something read back from the policy. The operator squares -the robot up in that pose and it has to be the same on every run. +Before enabling the policy, the robot transitions to the predefined starting pose over three seconds +and holds it until the operator confirms. Conversions ^^^^^^^^^^^ -Four conversions sit between the robot and the graph. Each is silent when wrong: the arrays keep -their shapes and the robot simply walks badly. +The following conversions adapt robot sensor data to the graph's input format. **Quaternion order.** The IMU publishes ``(w, x, y, z)`` and the graph expects ``(x, y, z, w)``. From 99eaf1fb7d857ede5e6f64940fba46e45db25a94 Mon Sep 17 00:00:00 2001 From: Henry Hu Date: Fri, 23 Oct 2026 19:41:11 +0800 Subject: [PATCH 17/18] Align G1 hardware deployment with the local control flow --- ...eploying_a_g1_visual_locomotion_policy.rst | 27 +- scripts/deploy/g1_deploy.py | 229 ++++++++++---- tools/test/test_g1_deploy.py | 285 ++++++++++++++++++ 3 files changed, 470 insertions(+), 71 deletions(-) create mode 100644 tools/test/test_g1_deploy.py diff --git a/docs/source/policy_deployment/07_g1_sim2real/training_and_deploying_a_g1_visual_locomotion_policy.rst b/docs/source/policy_deployment/07_g1_sim2real/training_and_deploying_a_g1_visual_locomotion_policy.rst index ce81df0901b8..b196183d0f6b 100644 --- a/docs/source/policy_deployment/07_g1_sim2real/training_and_deploying_a_g1_visual_locomotion_policy.rst +++ b/docs/source/policy_deployment/07_g1_sim2real/training_and_deploying_a_g1_visual_locomotion_policy.rst @@ -196,7 +196,7 @@ The Control Loop :language: python :start-at: while teleop.running: :end-at: link.wait(CONTROL_DT) - :dedent: 8 + :dedent: 12 Two details about the ends of this loop. @@ -212,7 +212,9 @@ The following conversions adapt robot sensor data to the graph's input format. **Quaternion order.** The IMU publishes ``(w, x, y, z)`` and the graph expects ``(x, y, z, w)``. -**IMU mounting offset.** Subtracted before the quaternion reaches the graph. +**IMU mounting offset.** ``--imu_tilt=PITCH,ROLL`` supplies the IMU pitch and roll in degrees +measured with the pelvis level. Both orientation and angular velocity are corrected before reaching +the graph. The default is ``0,0``. **Invalid depth.** A real camera returns zero where it got no return. The graph reads zero as a very close surface, so an unprocessed frame plants obstacles at the robot's feet. Pixels with no @@ -245,8 +247,25 @@ Put the robot in debug mode and support it on a stand before starting. --interface --domain_id 0 The loop takes control in two steps. Typing ``go`` ramps the robot to the policy's start pose over -three seconds and then holds it. Typing ``go2`` engages the policy three seconds later. ``rt/lowcmd`` -keeps flowing throughout, so there is no watchdog gap between the two. +three seconds and then holds it. Type ``go2`` within 300 seconds to engage the policy three seconds +later; other input is ignored. ``rt/lowcmd`` keeps flowing throughout, so there is no watchdog gap +between the two. The robot must be upright after the ramp and before policy engagement. + +The policy starts with zero forward speed and adopts the robot's current heading. Keyboard controls +take effect without Enter: + +.. code-block:: text + + w/s forward speed +/- 0.1 m/s (0 to 1 m/s) + q/e heading target +/- 15 degrees + space zero forward speed + r adopt the current heading + x quit + ? show controls + +The yaw-rate command is half the wrapped heading error, limited to ±1 rad/s. Space leaves the +heading target unchanged. ``Ctrl+C``, ``L2+B``, ``L1+A`` or ``select`` stops the run with damping, +including during the ramp and hold. Exiting the policy restores the terminal settings. .. warning:: diff --git a/scripts/deploy/g1_deploy.py b/scripts/deploy/g1_deploy.py index 60632264086e..bb8c6a2ffc82 100644 --- a/scripts/deploy/g1_deploy.py +++ b/scripts/deploy/g1_deploy.py @@ -17,13 +17,16 @@ import argparse import math +import os import re import select import socket import struct import sys +import termios import threading import time +import tty import numpy as np @@ -42,6 +45,9 @@ FALL_GRAVITY_Z = -0.7 """Stop above this projected gravity. Upright is -1.0; this is about 45 degrees over.""" +UPRIGHT_GRAVITY_Z = -0.9 +"""Require this projected gravity or lower before engaging the policy.""" + MOTOR_NAMES = ( "left_hip_pitch_joint", "left_hip_roll_joint", @@ -178,10 +184,16 @@ def latest(self) -> np.ndarray | None: return self._frame +def heading_from_quat_xyzw(quat_xyzw: np.ndarray) -> float: + """Body heading in the world frame [rad].""" + x, y, z, w = (float(v) for v in quat_xyzw) + return math.atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z)) + + class Teleop: """Forward speed and heading from single keypresses, without waiting for Enter.""" - HELP = "w/s forward +/- q/e turn left/right space stop x quit" + HELP = "w/s forward +/- q/e turn left/right space stop r face current heading x quit ? help" SPEED_STEP = 0.1 HEADING_STEP = math.radians(15.0) @@ -189,6 +201,17 @@ def __init__(self) -> None: self.vx = 0.0 self.heading = 0.0 self.running = True + self._terminal_settings = None + + def __enter__(self) -> Teleop: + if sys.stdin.isatty(): + self._terminal_settings = termios.tcgetattr(sys.stdin.fileno()) + tty.setcbreak(sys.stdin.fileno(), termios.TCSANOW) + return self + + def __exit__(self, *exc) -> None: + if self._terminal_settings is not None: + termios.tcsetattr(sys.stdin.fileno(), termios.TCSANOW, self._terminal_settings) def command(self, quat_xyzw: np.ndarray) -> np.ndarray: """``(vx, vy, yaw_rate)`` [m/s, m/s, rad/s], with the yaw rate steering toward a heading. @@ -197,26 +220,38 @@ def command(self, quat_xyzw: np.ndarray) -> np.ndarray: the measured orientation. Steering a heading rather than a rate keeps a teleoperated run on the distribution a fixed command trains on. """ - key = sys.stdin.read(1) if select.select([sys.stdin], [], [], 0.0)[0] else None - if key == "w": - self.vx = min(1.0, self.vx + self.SPEED_STEP) - elif key == "s": - self.vx = max(0.0, self.vx - self.SPEED_STEP) - elif key == "q": - self.heading += self.HEADING_STEP - elif key == "e": - self.heading -= self.HEADING_STEP - elif key == " ": - self.vx = 0.0 - elif key == "x": - self.running = False - if key: + keys = "" + while select.select([sys.stdin], [], [], 0.0)[0]: + data = os.read(sys.stdin.fileno(), 64) + if not data: + break + keys += data.decode(errors="ignore") + for sequence, key in (("\x1b[A", "w"), ("\x1b[B", "s"), ("\x1b[D", "a"), ("\x1b[C", "d")): + keys = keys.replace(sequence, key) + for key in keys.lower(): + if key == "w": + self.vx = min(1.0, self.vx + self.SPEED_STEP) + elif key == "s": + self.vx = max(0.0, self.vx - self.SPEED_STEP) + elif key == "q": + self.heading += self.HEADING_STEP + elif key == "e": + self.heading -= self.HEADING_STEP + elif key == " ": + self.vx = 0.0 + elif key == "r": + self.heading = heading_from_quat_xyzw(quat_xyzw) + elif key == "x": + self.running = False + elif key == "?": + print(self.HELP) + if keys: print(f"[cmd] vx={self.vx:+.2f} m/s heading={math.degrees(self.heading):+.0f} deg") - x, y, z, w = quat_xyzw - yaw = math.atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z)) - error = (self.heading - yaw + math.pi) % (2.0 * math.pi) - math.pi - return np.array([self.vx, 0.0, np.clip(error, -1.0, 1.0)], dtype=np.float32) + error = self.heading - heading_from_quat_xyzw(quat_xyzw) + wrapped = (error + math.pi) % (2.0 * math.pi) + error = math.pi if wrapped == 0.0 and error > 0.0 else wrapped - math.pi + return np.array([self.vx, 0.0, np.clip(0.5 * error, -1.0, 1.0)], dtype=np.float32) class G1Link: @@ -224,6 +259,7 @@ class G1Link: def __init__(self, domain_id: int, interface: str) -> None: from unitree_sdk2py.core.channel import ChannelFactoryInitialize, ChannelPublisher, ChannelSubscriber + from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowCmd_ from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowCmd_, LowState_ from unitree_sdk2py.utils.crc import CRC @@ -232,9 +268,9 @@ def __init__(self, domain_id: int, interface: str) -> None: ChannelSubscriber("rt/lowstate", LowState_).Init(self._on_state, 10) self._publisher = ChannelPublisher("rt/lowcmd", LowCmd_) self._publisher.Init() - self._command = LowCmd_() + self._command = unitree_hg_msg_dds__LowCmd_() self._crc = CRC() - self._next = time.monotonic() + self._next = None while self.state is None: time.sleep(0.02) @@ -283,12 +319,21 @@ def damp(self, kd, seconds: float = 1.5) -> None: zero = np.zeros(len(MOTOR_NAMES), dtype=np.float32) for _ in range(int(seconds / CONTROL_DT)): self.write(zero, zero, kd) - self.wait(CONTROL_DT) + time.sleep(CONTROL_DT) def wait(self, period: float) -> None: - """Sleep until the next control instant, without accumulating drift.""" + """Keep the control cadence, discarding overdue deadlines after a stall.""" + if self._next is None: + self._next = time.monotonic() self._next += period - time.sleep(max(0.0, self._next - time.monotonic())) + now = time.monotonic() + if now < self._next: + time.sleep(self._next - now) + now = time.monotonic() + if now - self._next < period: + return + if now > self._next: + self._next = now class GraphPolicy: @@ -303,25 +348,28 @@ class GraphPolicy: * depth is a rank-4 ``(1, H, W, 1)`` tensor, not an image-shaped array. """ - def __init__(self, pipeline: str, depth_rotation: int) -> None: + def __init__(self, pipeline: str, depth_rotation: int, imu_tilt: tuple[float, float] = (0.0, 0.0)) -> None: import torch - import yaml from leapp import InferenceManager self._torch = torch self._rotate = depth_rotation == 180 + pitch, roll = (math.radians(angle) / 2.0 for angle in imu_tilt) + cp, sp, cr, sr = math.cos(pitch), math.sin(pitch), math.cos(roll), math.sin(roll) + self._imu_tilt = np.array([cp * cr, cp * sr, sp * cr, -sp * sr], dtype=np.float32) + torch.set_num_threads(1) self.inference = InferenceManager(pipeline) - with open(pipeline) as stream: - model = next(iter(yaml.safe_load(stream)["models"].values())) - # The export names its own tensors; the connection strings are how Isaac Lab says what each - # one carries, and are what the simulation deployment resolves against a scene. - self._ports = { - io["isaaclab_connection"]: io["name"] - for io in list(model["inputs"]) + list(model["outputs"]) - if "isaaclab_connection" in io - } - names = next(o for o in model["outputs"] if o["name"] == "joint_pos")["element_names"][0] + self._ports = {} + for direction, keys in (("inputs", self.inference.inputs), ("outputs", self.inference.outputs)): + for key in keys: + node, name = key.split("/", 1) + spec = next(io for io in self.inference.models[node][direction] if io["name"] == name) + connection = spec.get("isaaclab_connection") + if not connection or connection in self._ports: + raise ValueError(f"missing or duplicate isaaclab_connection for {key}") + self._ports[connection] = (key, spec) + names = self._ports["write:robot:set_joint_position_target_index"][1]["element_names"][0] order = {name: i for i, name in enumerate(names)} missing = [name for name in MOTOR_NAMES if name not in order] if missing: @@ -345,12 +393,27 @@ def __init__(self, pipeline: str, depth_rotation: int) -> None: np.zeros(3, np.float32), np.full(DEPTH_SHAPE, DEPTH_MAX_RANGE, np.float32), ) - self.inference.reset() + with torch.inference_mode(): + self.inference.reset() def step(self, joint_pos, joint_vel, quat_xyzw, ang_vel, command, depth): """One inference step. Returns motor targets, Kp and Kd, each ``(29,)``.""" q, dq = self._default.copy(), np.zeros_like(self._default) q[self._motors], dq[self._motors] = joint_pos, joint_vel + if self._imu_tilt[0] != 1.0: + # R_pelvis_world = R_imu_world * inverse(R_imu_to_pelvis). + quat_xyzw = np.asarray(quat_xyzw, dtype=np.float32) + ang_vel = np.asarray(ang_vel, dtype=np.float32) + w, xyz = quat_xyzw[3], quat_xyzw[:3] + offset_w, offset_xyz = self._imu_tilt[0], -self._imu_tilt[1:] + quat_xyzw = np.concatenate( + ( + w * offset_xyz + offset_w * xyz + np.cross(xyz, offset_xyz), + [w * offset_w - np.dot(xyz, offset_xyz)], + ) + ) + cross = np.cross(self._imu_tilt[1:], ang_vel) + ang_vel = ang_vel + 2.0 * (self._imu_tilt[0] * cross + np.cross(self._imu_tilt[1:], cross)) frame = np.asarray(depth, dtype=np.float32) if self._rotate: frame = frame[::-1, ::-1] @@ -380,7 +443,7 @@ def step(self, joint_pos, joint_vel, quat_xyzw, ang_vel, command, depth): def _key(self, connection: str) -> str: """The graph's own name for a tensor, given the Isaac Lab connection it carries.""" - return self._ports[connection] + return self._ports[connection][0] def _motors_of(self, outputs: dict, connection: str) -> np.ndarray: """One of the graph's 43-wide outputs, reduced to the 29 motors in ``rt/lowcmd`` order.""" @@ -423,30 +486,44 @@ def bring_up(link: G1Link, pose: np.ndarray, kp, kd, ramp_s: float) -> None: Two steps rather than one, and ``rt/lowcmd`` keeps flowing through both: the robot is lowered onto its feet between them, and a gap in the command stream there would drop it. """ - confirm("go", "\n>>> Robot supported, area clear, e-stop in hand.") start = link.read()["joint_pos"].copy() steps = int(ramp_s / CONTROL_DT) print(f"[..] ramping to the start pose over {ramp_s:.1f}s") for step in range(steps): link.check_abort() link.write(start + (pose - start) * (step + 1) / steps, kp, kd) - link.wait(CONTROL_DT) + time.sleep(CONTROL_DT) + if link.read()["gravity_z"] > UPRIGHT_GRAVITY_Z: + raise RuntimeError("the robot is not upright; not engaging the policy") print("\n>>> HOLDING the start pose. Square the robot up and lower it onto its feet.") print(">>> KEEP IT SUPPORTED: this is PD with no policy, and the pose is not statically stable.") - held = threading.Event() - threading.Thread(target=lambda: (input(">>> type 'go2' and press Enter: "), held.set()), daemon=True).start() - while not held.is_set(): + interactive = sys.stdin.isatty() + if interactive: + termios.tcflush(sys.stdin.fileno(), termios.TCIFLUSH) + print(">>> type 'go2' and press Enter within 300s:") + deadline = time.monotonic() + 300.0 + while True: link.check_abort() link.write(pose, kp, kd) - link.wait(CONTROL_DT) + if interactive and select.select([sys.stdin], [], [], 0.0)[0]: + line = sys.stdin.readline() + if not line: + interactive = False + elif line.strip() == "go2": + break + elif line.strip(): + print(f"[..] ignored {line.strip()!r}; type 'go2' to proceed") + if time.monotonic() > deadline: + raise TimeoutError("'go2' not received within 300s of holding") + time.sleep(CONTROL_DT) print("[ok] engaging in 3s -- hands clear") for _ in range(int(3.0 / CONTROL_DT)): link.check_abort() link.write(pose, kp, kd) - link.wait(CONTROL_DT) - if link.read()["gravity_z"] > FALL_GRAVITY_Z: + time.sleep(CONTROL_DT) + if link.read()["gravity_z"] > UPRIGHT_GRAVITY_Z: raise RuntimeError("the robot is not upright; not engaging the policy") @@ -456,6 +533,12 @@ def main() -> int: parser.add_argument("--interface", required=True, help="Network interface on the robot's subnet.") parser.add_argument("--domain_id", type=int, default=0, help="DDS domain.") parser.add_argument("--depth_port", type=int, default=5601, help="UDP port the publisher sends to.") + parser.add_argument( + "--imu_tilt", + default="0,0", + metavar="PITCH,ROLL", + help="IMU pitch and roll [deg] measured with the pelvis level; default 0,0.", + ) parser.add_argument( "--depth_rotation", type=int, @@ -465,8 +548,14 @@ def main() -> int: " own 180-degree roll against the camera stream.", ) args = parser.parse_args() + try: + pitch, roll = (float(value) for value in args.imu_tilt.split(",")) + if not math.isfinite(pitch) or not math.isfinite(roll): + raise ValueError + except ValueError: + parser.error("--imu_tilt requires finite PITCH,ROLL values in degrees") - policy = GraphPolicy(args.pipeline, args.depth_rotation) + policy = GraphPolicy(args.pipeline, args.depth_rotation, (pitch, roll)) link = G1Link(args.domain_id, args.interface) depth = DepthStream(args.depth_port) depth.wait() @@ -475,28 +564,34 @@ def main() -> int: print(f"[ok] mode_machine={link.read()['mode_machine']} policy at {1 / CONTROL_DT:.0f} Hz") print(f"[..] teleop: {Teleop.HELP}") teleop = Teleop() - bring_up(link, policy.default_pose, policy.kp, policy.kd, ramp_s=3.0) + confirm("go", "\n>>> Robot supported, area clear, e-stop in hand.") try: - while teleop.running: - link.check_abort() - state = link.read() - if state["gravity_z"] > FALL_GRAVITY_Z: - raise RuntimeError(f"fell (projected gravity {state['gravity_z']:+.2f})") - frame = depth.latest() - if frame is None: - raise RuntimeError(f"no depth frame within {DEPTH_MAX_AGE * 1e3:.0f} ms") - joint_pos, kp, kd = policy.step( - state["joint_pos"], - state["joint_vel"], - state["root_quat_xyzw"], - state["ang_vel"], - teleop.command(state["root_quat_xyzw"]), - frame, - ) - link.write(joint_pos, kp, kd) - link.wait(CONTROL_DT) - except (KeyboardInterrupt, OperatorAbort, RuntimeError) as error: + bring_up(link, policy.default_pose, policy.kp, policy.kd, ramp_s=3.0) + teleop.heading = heading_from_quat_xyzw(link.read()["root_quat_xyzw"]) + with teleop: + while teleop.running: + link.check_abort() + state = link.read() + if state["gravity_z"] > FALL_GRAVITY_Z: + raise RuntimeError(f"fell (projected gravity {state['gravity_z']:+.2f})") + command = teleop.command(state["root_quat_xyzw"]) + if not teleop.running: + break + frame = depth.latest() + if frame is None: + raise RuntimeError(f"no depth frame within {DEPTH_MAX_AGE * 1e3:.0f} ms") + joint_pos, kp, kd = policy.step( + state["joint_pos"], + state["joint_vel"], + state["root_quat_xyzw"], + state["ang_vel"], + command, + frame, + ) + link.write(joint_pos, kp, kd) + link.wait(CONTROL_DT) + except (KeyboardInterrupt, OperatorAbort, RuntimeError, TimeoutError) as error: print(f"\n[..] {error or 'interrupted'}") finally: link.damp(policy.kd) diff --git a/tools/test/test_g1_deploy.py b/tools/test/test_g1_deploy.py new file mode 100644 index 000000000000..9d85bc61692e --- /dev/null +++ b/tools/test/test_g1_deploy.py @@ -0,0 +1,285 @@ +# 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 + +"""Hardware-free checks of G1 operator controls and control timing.""" + +import contextlib +import hashlib +import importlib.util +import os +import sys +from pathlib import Path +from types import SimpleNamespace + +import numpy as np +import pytest + + +@pytest.fixture +def deploy(): + pytest.importorskip("termios") + path = Path(__file__).parents[2] / "scripts/deploy/g1_deploy.py" + spec = importlib.util.spec_from_file_location("g1_deploy", path) + module = importlib.util.module_from_spec(spec) + spec.loader.exec_module(module) + return module + + +@pytest.fixture +def terminal(monkeypatch): + pty = pytest.importorskip("pty") + master, slave = pty.openpty() + with os.fdopen(slave, "r") as stream: + monkeypatch.setattr(sys, "stdin", stream) + try: + yield master, stream + finally: + os.close(master) + + +def test_teleop_single_keys_heading_and_terminal_restore(deploy, terminal): + import termios + + master, stream = terminal + saved = termios.tcgetattr(stream.fileno()) + quat = np.array([0.0, 0.0, 0.0, 1.0], np.float32) + with pytest.raises(RuntimeError, match="stop"), deploy.Teleop() as teleop: + teleop.heading = 0.6 + os.write(master, b"w") + np.testing.assert_allclose(teleop.command(quat), [0.1, 0.0, 0.3], atol=1e-7) + os.write(master, b" ") + np.testing.assert_allclose(teleop.command(quat), [0.0, 0.0, 0.3], atol=1e-7) + os.write(master, b"r") + np.testing.assert_allclose(teleop.command(quat), [0.0, 0.0, 0.0], atol=1e-7) + raise RuntimeError("stop") + restored = termios.tcgetattr(stream.fileno()) + flags = termios.ICANON | termios.ECHO | termios.ISIG + assert restored[3] & flags == saved[3] & flags + + +def test_control_wait_discards_missed_periods(deploy, monkeypatch): + now = [10.0] + slept = [] + + def sleep(seconds): + slept.append(seconds) + now[0] += seconds + + monkeypatch.setattr(deploy.time, "monotonic", lambda: now[0]) + monkeypatch.setattr(deploy.time, "sleep", sleep) + link = deploy.G1Link.__new__(deploy.G1Link) + link._next = 0.0 + link.wait(0.02) + for _ in range(3): + link.wait(0.02) + np.testing.assert_allclose(slept[-3:], [0.02, 0.02, 0.02], atol=1e-12) + assert now[0] == pytest.approx(10.06) + + +def test_bring_up_waits_for_exact_confirmation_and_keeps_publishing(deploy, terminal, monkeypatch): + master, _ = terminal + now, writes = [10.0], [] + pending = [(13.01, b"wrong\n"), (13.11, b"go2\n")] + + def sleep(seconds): + now[0] += seconds + if pending and now[0] >= pending[0][0]: + os.write(master, pending.pop(0)[1]) + + monkeypatch.setattr(deploy.time, "monotonic", lambda: now[0]) + monkeypatch.setattr(deploy.time, "sleep", sleep) + monkeypatch.setattr(deploy, "confirm", lambda *args: None) + link = SimpleNamespace( + read=lambda: {"joint_pos": np.zeros(29), "gravity_z": -1.0}, + check_abort=lambda: None, + write=lambda target, kp, kd: writes.append((now[0], target.copy())), + wait=lambda period: deploy.G1Link.wait(link, period), + _next=0.0, + ) + deploy.bring_up(link, np.ones(29), np.ones(29), np.ones(29), ramp_s=3.0) + assert not pending + assert now[0] >= 16.11 + np.testing.assert_allclose(np.diff([stamp for stamp, _ in writes[:150]]), 0.02, atol=1e-12) + np.testing.assert_allclose([target[0] for _, target in writes[:150]], np.linspace(1 / 150, 1, 150)) + np.testing.assert_allclose([target for _, target in writes[150:]], 1.0) + assert max(np.diff([stamp for stamp, _ in writes])) <= 0.021 + + +@pytest.fixture +def hardware(deploy, monkeypatch): + state = { + "joint_pos": np.zeros(29), + "joint_vel": np.zeros(29), + "root_quat_xyzw": np.array([0.0, 0.0, np.sqrt(0.5), np.sqrt(0.5)]), + "ang_vel": np.zeros(3), + "gravity_z": -1.0, + "mode_machine": 5, + } + damped, targets = [], [] + link = SimpleNamespace( + read=lambda: state, + check_abort=lambda: None, + wait=lambda period: None, + write=lambda target, kp, kd: targets.append(target.copy()), + damp=lambda kd: damped.append(kd.copy()), + ) + policy = SimpleNamespace(default_pose=np.zeros(29), kp=np.ones(29), kd=np.ones(29)) + monkeypatch.setattr(deploy, "GraphPolicy", lambda *args: policy) + monkeypatch.setattr(deploy, "G1Link", lambda *args: link) + monkeypatch.setattr( + deploy, + "DepthStream", + lambda *args: SimpleNamespace( + wait=lambda: None, + latest=lambda: np.ones((38, 64)), + ), + ) + monkeypatch.setattr(deploy, "take_lowcmd", lambda *args: None) + monkeypatch.setattr(deploy, "confirm", lambda *args: None) + monkeypatch.setattr(sys, "argv", ["g1_deploy.py", "--pipeline", "test.yaml", "--interface", "lo"]) + return policy, damped, targets + + +@pytest.mark.parametrize("failure", ["OperatorAbort", "KeyboardInterrupt", "RuntimeError", "TimeoutError"]) +def test_startup_failures_damp_before_exiting(deploy, hardware, monkeypatch, failure): + policy, damped, _ = hardware + errors = { + "OperatorAbort": deploy.OperatorAbort, + "KeyboardInterrupt": KeyboardInterrupt, + "RuntimeError": RuntimeError, + "TimeoutError": TimeoutError, + } + + def bring_up(*args, **kwargs): + raise errors[failure]("startup stopped") + + monkeypatch.setattr(deploy, "bring_up", bring_up) + with contextlib.suppress(*errors.values()): + deploy.main() + assert len(damped) == 1 + np.testing.assert_array_equal(damped[0], policy.kd) + + +def test_policy_starts_at_measured_heading_and_quit_sends_no_extra_target(deploy, hardware, terminal, monkeypatch): + policy, damped, targets = hardware + master, _ = terminal + commands = [] + + def step(q, dq, quat, gyro, command, depth): + commands.append(command.copy()) + os.write(master, b"x\n") + return np.ones(29), policy.kp, policy.kd + + policy.step = step + monkeypatch.setattr(deploy, "bring_up", lambda *args, **kwargs: None) + assert deploy.main() == 0 + np.testing.assert_allclose(commands, [[0.0, 0.0, 0.0]], atol=1e-7) + assert len(targets) == len(damped) == 1 + + +def test_graph_uses_named_ports_resets_feedback_and_corrects_imu(deploy, tmp_path): + pytest.importorskip("leapp") + import onnx + import torch + import yaml + from onnx import TensorProto, helper, numpy_helper + from safetensors.torch import save_file + + names = list(reversed(deploy.MOTOR_NAMES)) + [f"finger_{i}" for i in range(14)] + n = len(names) + + def port(name, shape, connection=None): + spec = {"name": name, "shape": shape, "dtype": "float32", "type": "tensor"} + if connection: + spec["isaaclab_connection"] = connection + if shape == [1, n]: + spec["element_names"] = [names] + return spec + + inputs = [ + port("q", [1, n], "state:robot:joint_pos"), + port("dq", [1, n], "state:robot:joint_vel"), + port("quat", [1, 4], "state:robot:root_quat_w"), + port("gyro", [1, 3], "state:robot:root_ang_vel_b"), + port("cmd", [1, 3], "command:base_velocity"), + port("depth", [1, 38, 64, 1], "state:depth_camera:output:distance_to_image_plane"), + port("carry", [1, n]), + ] + outputs = [ + port("targets", [1, n], "write:robot:set_joint_position_target_index"), + port("kp", [1, n], "write:robot:write_joint_stiffness_to_sim_index"), + port("kd", [1, n], "write:robot:write_joint_damping_to_sim_index"), + ] + # The target exposes both corrected sensor components and the previous target. + nodes = [ + helper.make_node("Gather", ["gyro", "zero"], ["gyro_x"], axis=1), + helper.make_node("Gather", ["quat", "three"], ["quat_w"], axis=1), + helper.make_node("Mul", ["gyro_x", "two_f"], ["gyro_term"]), + helper.make_node("Mul", ["quat_w", "three_f"], ["quat_term"]), + helper.make_node("Add", ["gyro_term", "quat_term"], ["sensor_term"]), + helper.make_node("Add", ["q", "carry"], ["history_term"]), + helper.make_node("Add", ["history_term", "sensor_term"], ["targets"]), + helper.make_node("Identity", ["gains"], ["kp"]), + helper.make_node("Identity", ["gains"], ["kd"]), + ] + values = { + "zero": np.array([0]), + "three": np.array([3]), + "two_f": np.array(2, np.float32), + "three_f": np.array(3, np.float32), + "gains": np.arange(1, n + 1, dtype=np.float32)[None], + } + graph = helper.make_graph( + nodes, + "sensor_feedback", + [helper.make_tensor_value_info(p["name"], TensorProto.FLOAT, p["shape"]) for p in inputs], + [helper.make_tensor_value_info(p["name"], TensorProto.FLOAT, p["shape"]) for p in outputs], + [numpy_helper.from_array(value, name) for name, value in values.items()], + ) + onnx.save( + helper.make_model(graph, opset_imports=[helper.make_opsetid("", 17)], ir_version=8), tmp_path / "model.onnx" + ) + data = (tmp_path / "model.onnx").read_bytes() + save_file({"custom_node/carry": torch.zeros(1, n)}, str(tmp_path / "initial.safetensors")) + description = { + "models": { + "custom_node": { + "inputs": inputs, + "outputs": outputs, + "parameters": { + "backend": "onnx", + "model_path": "model.onnx", + "md5sum": hashlib.md5(data).hexdigest(), + "sha256sum": hashlib.sha256(data).hexdigest(), + }, + } + }, + "pipeline": { + "inputs": {"custom_node": [p["name"] for p in inputs[:-1]]}, + "outputs": {"custom_node": [p["name"] for p in outputs]}, + "data_flow": {}, + "feedback_flow": {"custom_node/targets": ["custom_node/carry"]}, + "initial_values": "initial.safetensors", + }, + "system information": {"leapp version": "0.7.2"}, + } + pipeline = tmp_path / "pipeline.yaml" + pipeline.write_text(yaml.safe_dump(description)) + policy = deploy.GraphPolicy(str(pipeline), 180, imu_tilt=(90.0, 0.0)) + q = np.linspace(0, 0.28, 29, dtype=np.float32) + sensors = ( + q, + np.zeros(29), + np.array([0, np.sqrt(0.5), 0, np.sqrt(0.5)]), + np.array([0, 0, 1]), + np.zeros(3), + np.ones((38, 64)), + ) + # A level pelvis with this IMU offset has identity orientation and pelvis gyro +X. + for multiplier in (1, 2): + target, kp, kd = policy.step(*sensors) + np.testing.assert_allclose(target, multiplier * (q + 5), atol=2e-6) + np.testing.assert_array_equal(kp, np.arange(29, 0, -1)) + np.testing.assert_array_equal(kd, kp) From d8e2951acdd30744704238c41026dfe641e7e52d Mon Sep 17 00:00:00 2001 From: Henry Hu Date: Fri, 23 Oct 2026 21:08:58 +0800 Subject: [PATCH 18/18] Address G1 review feedback and add the depth publisher --- ...eploying_a_g1_visual_locomotion_policy.rst | 32 +++- scripts/deploy/depth_publisher.py | 178 ++++++++++++++++++ scripts/deploy/g1_deploy.py | 28 ++- .../changelog.d/g1-shared-mdp.changed.rst | 3 +- .../velocity/config/g1/agents/distillation.py | 109 ++++++++--- .../core/velocity/config/g1/rough_env_cfg.py | 42 ++++- .../core/velocity/mdp/symmetry/g1.py | 24 ++- .../test/core/test_velocity_newton_cfg.py | 57 +++++- tools/test/test_depth_publisher.py | 49 +++++ 9 files changed, 468 insertions(+), 54 deletions(-) create mode 100644 scripts/deploy/depth_publisher.py create mode 100644 tools/test/test_depth_publisher.py diff --git a/docs/source/policy_deployment/07_g1_sim2real/training_and_deploying_a_g1_visual_locomotion_policy.rst b/docs/source/policy_deployment/07_g1_sim2real/training_and_deploying_a_g1_visual_locomotion_policy.rst index b196183d0f6b..ad742cc21282 100644 --- a/docs/source/policy_deployment/07_g1_sim2real/training_and_deploying_a_g1_visual_locomotion_policy.rst +++ b/docs/source/policy_deployment/07_g1_sim2real/training_and_deploying_a_g1_visual_locomotion_policy.rst @@ -227,14 +227,42 @@ nothing downstream can detect it. Starting the Camera ^^^^^^^^^^^^^^^^^^^ -The camera runs on the robot's onboard computer, which carries the RealSense drivers. It crops, -downsamples and publishes 64x38 frames as unsigned 16-bit millimetres over UDP. +Run ``scripts/deploy/depth_publisher.py`` on the G1 computer connected to the chest RealSense +camera (PC2). The script is standalone and requires only NumPy and ``pyrealsense2``; Isaac Lab +does not need to be installed on the robot. + +From the Isaac Lab repository root on the workstation, copy the script to PC2. The commands below +use ``192.168.123.164`` as PC2's address; replace it if your robot uses a different address. + +.. code-block:: bash + + scp scripts/deploy/depth_publisher.py unitree@192.168.123.164:~/depth_publisher.py + +Install the Python dependencies on PC2 using the same interpreter that will run the publisher. +The RealSense drivers must be installed and the camera available to the ``unitree`` user. + +.. code-block:: bash + + ssh unitree@192.168.123.164 \ + '/usr/bin/python3 -m pip install --user numpy pyrealsense2' + +Start the publisher from the workstation and leave this SSH session running. Replace +```` with the workstation's address on the robot network, which receives the +depth stream and runs the policy in a second terminal. .. code-block:: bash ssh unitree@192.168.123.164 \ '/usr/bin/python3 ~/depth_publisher.py --host ' +The publisher requests 848x480 depth at 90 Hz, crops using the camera intrinsics toward the +trained 87.0° horizontal and 58.8° vertical field of view, and averages valid pixels into 64x38 +frames. It sends unsigned 16-bit millimetres to UDP port 5601 and reports the publishing rate +every five seconds. Missing returns stay zero; the control loop fills them with the clip range. +If camera startup fails, the script retries and reduces the requested rate to 60 Hz on its final +attempt. ``--port`` must match the policy's ``--depth_port`` if you change the default port. +Press ``Ctrl+C`` to stop the publisher after stopping the policy. + Starting the Policy ^^^^^^^^^^^^^^^^^^^ diff --git a/scripts/deploy/depth_publisher.py b/scripts/deploy/depth_publisher.py new file mode 100644 index 000000000000..7a0761d8bd22 --- /dev/null +++ b/scripts/deploy/depth_publisher.py @@ -0,0 +1,178 @@ +# 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 + +"""Publish the G1 chest RealSense camera to the workstation running g1_deploy.py. + +Run this standalone script on the G1 computer connected to the camera (PC2). It requires +NumPy and pyrealsense2, without Isaac Lab or any other files from this repository. +""" + +from __future__ import annotations + +import argparse +import contextlib +import logging +import math +import socket +import struct +import time +from typing import TYPE_CHECKING + +import numpy as np + +if TYPE_CHECKING: + import pyrealsense2 as rs + +logger = logging.getLogger(__name__) + +DEPTH_SHAPE = (38, 64) +TRAINED_HFOV = 87.0 +TRAINED_VFOV = 58.8 +HEADER = struct.Struct(" tuple[int, int, int, int]: + """Return the centered crop matching the policy's field of view. + + Args: + intrinsics: Native camera dimensions and focal lengths in pixels. + + Returns: + Crop origin and size ``(x, y, width, height)`` in pixels. Axes narrower than + the trained field of view retain their full extent. + """ + width = min(intrinsics.width, round(2 * intrinsics.fx * math.tan(math.radians(TRAINED_HFOV) / 2))) + height = min(intrinsics.height, round(2 * intrinsics.fy * math.tan(math.radians(TRAINED_VFOV) / 2))) + return (intrinsics.width - width) // 2, (intrinsics.height - height) // 2, width, height + + +def downsample_masked(frame_m: np.ndarray) -> np.ndarray: + """Area-average valid depths into the policy's 38-by-64 image. + + Args: + frame_m: Cropped depth image in metres, at least as large as the output. + Non-finite or non-positive pixels indicate missing returns. + + Returns: + Float32 metres; bins with no valid samples contain zero. Image orientation + is unchanged because the deployment control loop handles rotation. + """ + height, width = frame_m.shape + if height < DEPTH_SHAPE[0] or width < DEPTH_SHAPE[1]: + raise ValueError(f"Cropped image {width}x{height} is smaller than the policy's 64x38 input") + valid = np.isfinite(frame_m) & (frame_m > 0.0) + values = np.where(valid, frame_m, 0.0).astype(np.float64) + counts = valid.astype(np.float64) + y_edges = np.arange(DEPTH_SHAPE[0]) * height // DEPTH_SHAPE[0] + x_edges = np.arange(DEPTH_SHAPE[1]) * width // DEPTH_SHAPE[1] + values = np.add.reduceat(np.add.reduceat(values, y_edges, axis=0), x_edges, axis=1) + counts = np.add.reduceat(np.add.reduceat(counts, y_edges, axis=0), x_edges, axis=1) + return np.divide(values, counts, out=np.zeros_like(values), where=counts > 0).astype(np.float32) + + +def pack_frame(sequence: int, frame_m: np.ndarray, capture_time_ns: int) -> bytes: + """Serialize a depth frame for the deployment receiver. + + Args: + sequence: Monotonically increasing frame number, wrapped to uint32 on the wire. + frame_m: Processed depth in metres, with missing returns encoded as zero. + capture_time_ns: Camera capture timestamp in nanoseconds. + + Returns: + Header followed by row-major, little-endian uint16 millimetres. + """ + millimetres = np.clip(np.rint(frame_m * 1000.0), 0, 65535).astype(" tuple[rs.pipeline, rs.pipeline_profile, int]: + """Start streaming, retrying with a camera reset and then a lower frame rate. + + Args: + width: Native stream width in pixels. + height: Native stream height in pixels. + fps: Requested frame rate in Hz. The final attempt uses at most 60 Hz. + + Returns: + Started pipeline, profile, and actual requested frame rate. + + Raises: + RuntimeError: If all three attempts fail to deliver a frame. + """ + import pyrealsense2 as rs + + device = None + last_error = None + for attempt in range(3): + if attempt == 1 and device is not None: + logger.warning("Resetting the camera before retrying") + device.hardware_reset() + time.sleep(5.0) + if attempt == 2 and fps > 60: + logger.warning("Retrying the camera at 60 Hz") + fps = 60 + pipe, cfg = rs.pipeline(), rs.config() + cfg.enable_stream(rs.stream.depth, width, height, rs.format.z16, fps) + try: + profile = pipe.start(cfg) + device = profile.get_device() + pipe.wait_for_frames(timeout_ms=3000) + return pipe, profile, fps + except RuntimeError as error: + last_error = error + logger.warning(f"Camera attempt {attempt + 1}/3 failed: {error}") + with contextlib.suppress(RuntimeError): + pipe.stop() + raise RuntimeError("The camera did not deliver a frame after three attempts") from last_error + + +def main() -> int: + logging.basicConfig(level=logging.INFO, format="%(levelname)s: %(message)s") + parser = argparse.ArgumentParser(description=__doc__.splitlines()[0]) + parser.add_argument("--host", required=True, help="Workstation IP reachable from the G1.") + parser.add_argument("--port", type=int, default=5601, help="Workstation UDP port; matches --depth_port.") + parser.add_argument("--width", type=int, default=848, help="Native depth width in pixels.") + parser.add_argument("--height", type=int, default=480, help="Native depth height in pixels.") + parser.add_argument("--fps", type=int, default=90, help="Native frame rate; default 90 Hz for the 50 Hz policy.") + args = parser.parse_args() + + import pyrealsense2 as rs + + pipe, profile, fps = start_stream(args.width, args.height, args.fps) + try: + scale = profile.get_device().first_depth_sensor().get_depth_scale() + intrinsics = profile.get_stream(rs.stream.depth).as_video_stream_profile().get_intrinsics() + x, y, width, height = crop_to_fov(intrinsics) + hfov = math.degrees(2 * math.atan2(width, 2 * intrinsics.fx)) + vfov = math.degrees(2 * math.atan2(height, 2 * intrinsics.fy)) + logger.info(f"Camera {intrinsics.width}x{intrinsics.height} at {fps} Hz; crop {width}x{height} at ({x}, {y})") + logger.info(f"Cropped field of view {hfov:.2f} x {vfov:.2f} deg; publishing 64x38 to {args.host}:{args.port}") + if abs(hfov - TRAINED_HFOV) > 1.0 or abs(vfov - TRAINED_VFOV) > 1.0: + logger.warning("Cropped field of view differs from the trained camera by more than one degree") + with socket.socket(socket.AF_INET, socket.SOCK_DGRAM) as sock: + sequence, sent, report_time = 0, 0, time.monotonic() + while True: + depth = pipe.wait_for_frames(timeout_ms=2000).get_depth_frame() + if not depth: + continue + raw = np.asanyarray(depth.get_data()) + frame = downsample_masked(raw[y : y + height, x : x + width].astype(np.float32) * scale) + sock.sendto(pack_frame(sequence, frame, int(depth.get_timestamp() * 1e6)), (args.host, args.port)) + sequence += 1 + sent += 1 + now = time.monotonic() + if now - report_time >= 5.0: + logger.info(f"Publishing {sent / (now - report_time):.0f} Hz; {sequence} frames sent") + sent, report_time = 0, now + except KeyboardInterrupt: + logger.info("Interrupted") + finally: + pipe.stop() + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/scripts/deploy/g1_deploy.py b/scripts/deploy/g1_deploy.py index bb8c6a2ffc82..9dfd455910f3 100644 --- a/scripts/deploy/g1_deploy.py +++ b/scripts/deploy/g1_deploy.py @@ -16,6 +16,7 @@ from __future__ import annotations import argparse +import logging import math import os import re @@ -30,6 +31,10 @@ import numpy as np +from isaaclab.app.logging_utils import configure_console_logging + +logger = logging.getLogger("isaaclab.g1_deploy") + CONTROL_DT = 0.02 """Control period [s]. The exported graph carries its history at this rate.""" @@ -246,7 +251,7 @@ def command(self, quat_xyzw: np.ndarray) -> np.ndarray: elif key == "?": print(self.HELP) if keys: - print(f"[cmd] vx={self.vx:+.2f} m/s heading={math.degrees(self.heading):+.0f} deg") + logger.info(f"vx={self.vx:+.2f} m/s heading={math.degrees(self.heading):+.0f} deg") error = self.heading - heading_from_quat_xyzw(quat_xyzw) wrapped = (error + math.pi) % (2.0 * math.pi) @@ -315,7 +320,7 @@ def write(self, joint_pos, kp, kd) -> None: def damp(self, kd, seconds: float = 1.5) -> None: """Bleed off energy with no stiffness, then stop commanding. Every exit goes through here.""" - print("[..] damping down") + logger.info("Damping down") zero = np.zeros(len(MOTOR_NAMES), dtype=np.float32) for _ in range(int(seconds / CONTROL_DT)): self.write(zero, zero, kd) @@ -470,7 +475,7 @@ def take_lowcmd(domain_id: int, probe_s: float = 3.0) -> None: f"{seen[0]} rt/lowcmd samples in {probe_s:.0f}s -- something else is commanding the" " robot. Put it in debug mode with the remote (L2+B, then L2+R2) and try again." ) - print("[ok] rt/lowcmd is free") + logger.info("rt/lowcmd is free") def confirm(word: str, prompt: str) -> None: @@ -488,7 +493,7 @@ def bring_up(link: G1Link, pose: np.ndarray, kp, kd, ramp_s: float) -> None: """ start = link.read()["joint_pos"].copy() steps = int(ramp_s / CONTROL_DT) - print(f"[..] ramping to the start pose over {ramp_s:.1f}s") + logger.info(f"Ramping to the start pose over {ramp_s:.1f}s") for step in range(steps): link.check_abort() link.write(start + (pose - start) * (step + 1) / steps, kp, kd) @@ -513,12 +518,12 @@ def bring_up(link: G1Link, pose: np.ndarray, kp, kd, ramp_s: float) -> None: elif line.strip() == "go2": break elif line.strip(): - print(f"[..] ignored {line.strip()!r}; type 'go2' to proceed") + print(f"Ignored {line.strip()!r}; type 'go2' to proceed") if time.monotonic() > deadline: raise TimeoutError("'go2' not received within 300s of holding") time.sleep(CONTROL_DT) - print("[ok] engaging in 3s -- hands clear") + logger.info("Engaging in 3s -- hands clear") for _ in range(int(3.0 / CONTROL_DT)): link.check_abort() link.write(pose, kp, kd) @@ -528,6 +533,7 @@ def bring_up(link: G1Link, pose: np.ndarray, kp, kd, ramp_s: float) -> None: def main() -> int: + configure_console_logging() parser = argparse.ArgumentParser(description=__doc__.splitlines()[0]) parser.add_argument("--pipeline", required=True, help="Exported LEAPP .yaml.") parser.add_argument("--interface", required=True, help="Network interface on the robot's subnet.") @@ -561,8 +567,8 @@ def main() -> int: depth.wait() take_lowcmd(args.domain_id) - print(f"[ok] mode_machine={link.read()['mode_machine']} policy at {1 / CONTROL_DT:.0f} Hz") - print(f"[..] teleop: {Teleop.HELP}") + logger.info(f"mode_machine={link.read()['mode_machine']} policy at {1 / CONTROL_DT:.0f} Hz") + print(Teleop.HELP) teleop = Teleop() confirm("go", "\n>>> Robot supported, area clear, e-stop in hand.") @@ -591,8 +597,10 @@ def main() -> int: ) link.write(joint_pos, kp, kd) link.wait(CONTROL_DT) - except (KeyboardInterrupt, OperatorAbort, RuntimeError, TimeoutError) as error: - print(f"\n[..] {error or 'interrupted'}") + except (KeyboardInterrupt, OperatorAbort) as error: + logger.info(str(error) or "Interrupted") + except (RuntimeError, TimeoutError) as error: + logger.error(str(error)) finally: link.damp(policy.kd) return 0 diff --git a/source/isaaclab_tasks/changelog.d/g1-shared-mdp.changed.rst b/source/isaaclab_tasks/changelog.d/g1-shared-mdp.changed.rst index 4bd3980d82b7..d6a6de03a366 100644 --- a/source/isaaclab_tasks/changelog.d/g1-shared-mdp.changed.rst +++ b/source/isaaclab_tasks/changelog.d/g1-shared-mdp.changed.rst @@ -16,4 +16,5 @@ * Moved the G1 mesh-terrain preset into the G1 terrain configuration module. * Removed the rough G1 actuator-path override, using the current default shared across physics backends. The configured implicit actuators retain their solver-owned drives. -* Read G1 sole geometry and reference torques from the asset, reusing configured actuator stiffnesses. +* Read G1 sole geometry and reference torques from the asset during environment initialization, + reusing configured actuator stiffnesses without loading USD while constructing configurations. diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/agents/distillation.py b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/agents/distillation.py index 481999755abc..80bd879f0a4c 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/agents/distillation.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/agents/distillation.py @@ -3,7 +3,11 @@ # # SPDX-License-Identifier: BSD-3-Clause -from copy import copy +"""Feed-forward G1 distillation with paired original and mirrored rollout samples.""" + +from __future__ import annotations + +from typing import TYPE_CHECKING import torch from rsl_rl.algorithms import Distillation @@ -12,14 +16,32 @@ from isaaclab_tasks.core.velocity.mdp.symmetry.g1 import _action_joint_map, _mirror_group +if TYPE_CHECKING: + from rsl_rl.env import VecEnv + class Mirror: - def __init__(self, env): + """Reflect G1 proprioception, paired depth cameras, and joint-position actions. + + Args: + env: Wrapped G1 environment supplying joint and observation layouts. + """ + + def __init__(self, env: VecEnv) -> None: self.env = env.unwrapped self.perm, self.sign = _action_joint_map(self.env) @torch.no_grad() - def observations(self, obs): + def observations(self, obs: TensorDict) -> TensorDict: + """Return reflected observations without modifying the input. + + Args: + obs: Batched proprioception, height scan, history, and paired depth images. + + Returns: + The same observation groups and batch shape. Depth images swap camera sides + and flip horizontally; joint and vector terms reflect across the sagittal plane. + """ assert set(obs.keys()) == {"proprio", "height_scan", "proprio_history", "depth", "depth_mirror"} return TensorDict( { @@ -33,39 +55,54 @@ def observations(self, obs): ) @torch.no_grad() - def actions(self, actions): - return actions[..., self.perm] * self.sign + def actions(self, actions: torch.Tensor) -> torch.Tensor: + """Reflect joint-position actions in the environment's action order. + Args: + actions: Actions with the joint dimension last. -class AugmentedStorage: - def __init__(self, storage, mirror): - self.storage = storage - self.mirror = mirror + Returns: + Reflected actions with the same shape, dtype, and device. + """ + return actions[..., self.perm] * self.sign - def __getattr__(self, name): - return getattr(self.storage, name) - def generator(self): - for batch in self.storage.generator(): - out = copy(batch) - out.observations = torch.cat([batch.observations, self.mirror.observations(batch.observations)], dim=0) - out.privileged_actions = torch.cat( - [batch.privileged_actions, self.mirror.actions(batch.privileged_actions)] - ) - out.dones = batch.dones.repeat(2, 1) - yield out +class MirrorDistillation(Distillation): + """Train a feed-forward student on original and sagittally reflected teacher targets. + Each rollout batch is doubled during the update; stored transitions remain unchanged. + Gradients accumulate over ``gradient_length`` batches before each optimizer step. + """ -class MirrorDistillation(Distillation): @staticmethod - def construct_algorithm(obs, env, cfg, device): + def construct_algorithm(obs: TensorDict, env: VecEnv, cfg: dict, device: str) -> MirrorDistillation: + """Construct the RSL-RL algorithm and its environment-specific reflection maps. + + Args: + obs: Initial observations used to determine model input shapes. + env: Wrapped G1 environment. + cfg: RSL-RL runner configuration selecting this algorithm class. + device: Device for models and rollout storage. + + Returns: + Configured distillation algorithm. Both models must be feed-forward. + """ alg = Distillation.construct_algorithm(obs, env, cfg, device) assert not alg.student.is_recurrent and not alg.teacher.is_recurrent alg.mirror = Mirror(env) - alg.storage = AugmentedStorage(alg.storage, alg.mirror) return alg - def process_env_step(self, obs, rewards, dones, extras): + def process_env_step( + self, obs: TensorDict, rewards: torch.Tensor, dones: torch.Tensor, extras: dict[str, torch.Tensor] + ) -> None: + """Record the transition and update normalization with both observation orientations. + + Args: + obs: Observations after the environment step. + rewards: Per-environment rewards. + dones: Per-environment termination flags. + extras: Additional environment information passed to RSL-RL. + """ mirrored = TensorDict( {"proprio_history": _mirror_group(self.mirror.env, "proprio_history", obs["proprio_history"])}, batch_size=obs.batch_size, @@ -74,20 +111,36 @@ def process_env_step(self, obs, rewards, dones, extras): self.student.update_normalization(obs) super().process_env_step(obs, rewards, dones, extras) - def load(self, loaded_dict, load_cfg=None, strict=True): + def load(self, loaded_dict: dict, load_cfg: dict | None = None, strict: bool = True) -> bool: + """Load a distillation checkpoint, or only the teacher from a PPO checkpoint. + + Args: + loaded_dict: Serialized model, optimizer, and iteration state. + load_cfg: RSL-RL component selection, overridden for PPO actor checkpoints. + strict: Whether model state keys must match exactly. + + Returns: + Whether the runner should restore the checkpoint's iteration counter. + """ if "actor_state_dict" in loaded_dict: load_cfg = {"teacher": True, "student": False, "optimizer": False, "iteration": False} return super().load(loaded_dict, load_cfg, strict) - def update(self): - """Update the student with accumulated gradients over rollout steps.""" + def update(self) -> dict[str, float]: + """Optimize paired rollout samples, then clear storage. + + Returns: + Mean behavior-cloning loss across rollout steps and learning epochs. + """ self.num_updates += 1 mean_loss = 0.0 loss = 0.0 count = 0 for _ in range(self.num_learning_epochs): for batch in self.storage.generator(): - behavior_loss = self.loss_fn(self.student(batch.observations), batch.privileged_actions) + observations = torch.cat([batch.observations, self.mirror.observations(batch.observations)], dim=0) + targets = torch.cat([batch.privileged_actions, self.mirror.actions(batch.privileged_actions)]) + behavior_loss = self.loss_fn(self.student(observations), targets) loss = loss + behavior_loss mean_loss += behavior_loss.item() count += 1 diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/rough_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/rough_env_cfg.py index 83f0e21f9c0d..edc22c392c50 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/rough_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/rough_env_cfg.py @@ -10,8 +10,10 @@ depth camera and is distilled from the teacher in the teacher's own environment. """ +from __future__ import annotations + import math -from dataclasses import MISSING +from typing import TYPE_CHECKING import torch from isaaclab_newton.sim.schemas import NewtonArticulationCfg @@ -43,6 +45,9 @@ ) from .terrains import G1_ROUGH_TERRAINS_CFG +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + DEPTH_WIDTH = 64 DEPTH_HEIGHT = 38 DEPTH_STACK = 3 @@ -153,8 +158,9 @@ class G1Rewards(RewardsCfg): params={ "sensor_cfg": SceneEntityCfg("contact_forces", body_names=".*_ankle_roll_link"), "asset_cfg": SceneEntityCfg("robot", body_names=".*_ankle_roll_link"), - "sole_size": MISSING, - "sole_offset": MISSING, + # Filled from the robot asset by the prestartup event before rewards are loaded. + "sole_size": None, + "sole_offset": None, }, ) @@ -202,10 +208,30 @@ def get_action_parameters(robot_cfg: ArticulationCfg) -> tuple[dict[str, float], return action_scale, sole_size, sole_offset +def configure_asset_parameters(env: ManagerBasedRLEnv, env_ids: torch.Tensor | None) -> None: + """Resolve action scales and sole geometry before action and reward managers are built. + + Args: + env: Environment whose robot asset supplies authored drives and sole geometry. + env_ids: Unused; these parameters are shared by all environments. + """ + action_scale, sole_size, sole_offset = get_action_parameters(env.cfg.scene.robot) + if env.cfg.actions.joint_pos.scale is None: + env.cfg.actions.joint_pos.scale = action_scale + reward = env.cfg.rewards.feet_support_edge + if reward is not None: + if reward.params["sole_size"] is None: + reward.params["sole_size"] = sole_size + if reward.params["sole_offset"] is None: + reward.params["sole_offset"] = sole_offset + + @configclass class G1EventsCfg(velocity_env_cfg.EventsCfg): """G1 startup and reset events.""" + asset_parameters: EventTerm | None = None + add_base_mass = EventTerm( func=mdp.randomize_rigid_body_mass, mode="startup", @@ -462,10 +488,13 @@ class G1RoughTeacherEnvCfg(LocomotionVelocityRoughEnvCfg): seed = 42 scene: G1SceneCfg = G1SceneCfg(num_envs=4096, env_spacing=2.5) rewards: G1Rewards = G1Rewards() - events: G1EventsCfg = G1EventsCfg() + events: G1EventsCfg = G1EventsCfg(asset_parameters=EventTerm(func=configure_asset_parameters, mode="prestartup")) curriculum: G1CurriculumCfg = G1CurriculumCfg() terminations: G1TerminationsCfg = G1TerminationsCfg() observations: G1ObservationsCfg = G1ObservationsCfg() + actions: velocity_env_cfg.ActionsCfg = velocity_env_cfg.ActionsCfg() + # Resolve the default from the asset at prestartup, retaining explicit config overrides. + actions.joint_pos.scale = None sim = SimulationCfg(physics=velocity_env_cfg.RoughPhysicsCfg()) sim.physics.newton_mjwarp.solver_cfg.njmax = 300 commands = velocity_env_cfg.CommandsCfg() @@ -475,11 +504,6 @@ class G1RoughTeacherEnvCfg(LocomotionVelocityRoughEnvCfg): commands.base_velocity.ranges.lin_vel_y = (-0.0, 0.0) commands.base_velocity.ranges.ang_vel_z = (-1.0, 1.0) - def __post_init__(self): - super().__post_init__() - self.actions.joint_pos.scale, sole_size, sole_offset = get_action_parameters(self.scene.robot) - self.rewards.feet_support_edge.params.update(sole_size=sole_size, sole_offset=sole_offset) - @configclass class G1RoughDepthStudentEnvCfg(G1RoughTeacherEnvCfg): diff --git a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/mdp/symmetry/g1.py b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/mdp/symmetry/g1.py index 0761671c4464..dcaf42824d09 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/mdp/symmetry/g1.py +++ b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/mdp/symmetry/g1.py @@ -3,6 +3,11 @@ # # SPDX-License-Identifier: BSD-3-Clause +"""Sagittal reflection of G1 joint actions and configured proprioceptive observations. + +Joint names, limits, and the default pose determine the reflection map. Height scans use +the G1 terrain scanner's 11-by-17 grid; history terms retain their frame ordering. +""" from __future__ import annotations @@ -174,7 +179,24 @@ def _mirror_group(env: ManagerBasedRLEnv, group: str, obs: torch.Tensor) -> torc @torch.no_grad() def compute_symmetric_states( env: ManagerBasedRLEnv, obs: TensorDict | None = None, actions: torch.Tensor | None = None -): +) -> tuple[TensorDict | None, torch.Tensor | None]: + """Append sagittally reflected samples to an observation or action batch. + + Args: + env: G1 environment, optionally wrapped, providing joint and observation layouts. + obs: Observation groups shaped ``(batch, features)``, or ``None``. Groups must + contain supported proprioceptive or height-scan terms, rather than depth images. + actions: Joint-position actions shaped ``(batch, joints)``, or ``None``. + + Returns: + Augmented observations and actions with original samples first and reflected samples + second. Each supplied batch doubles in size; omitted inputs remain ``None``. + Inputs are not modified. + + Raises: + ValueError: If an observation layout or action joint selection cannot be reflected. + RuntimeError: If joint limits or the default pose disagree with the reflection map. + """ unwrapped = env.unwrapped if obs is not None: batch = obs.batch_size[0] diff --git a/source/isaaclab_tasks/test/core/test_velocity_newton_cfg.py b/source/isaaclab_tasks/test/core/test_velocity_newton_cfg.py index 24f8e8f45c03..feb6346e26ce 100644 --- a/source/isaaclab_tasks/test/core/test_velocity_newton_cfg.py +++ b/source/isaaclab_tasks/test/core/test_velocity_newton_cfg.py @@ -6,8 +6,10 @@ import ast from importlib import import_module from inspect import getsource +from types import SimpleNamespace import gymnasium as gym +import pytest from isaaclab_newton.physics import MJWarpSolverCfg from isaaclab.envs import mdp as core_mdp @@ -21,9 +23,11 @@ def test_g1_rough_teacher_registration_and_newton_capacity(monkeypatch): """Rough G1 resolves to one teacher task and retains sufficient constraint capacity.""" - monkeypatch.setattr( - rough_env_cfg, "get_action_parameters", lambda cfg: (1.0, (0.2, 0.06, 0.02), (0.03, 0.0, -0.02)) - ) + + def reject_asset_access(cfg): + raise AssertionError("Creating a configuration must not retrieve or parse the robot asset") + + monkeypatch.setattr(rough_env_cfg, "get_action_parameters", reject_asset_access) env_cfg = load_cfg_from_registry("Isaac-Velocity-Rough-G1", "env_cfg_entry_point") runner_cfg = load_cfg_from_registry("Isaac-Velocity-Rough-G1", "rsl_rl_cfg_entry_point") @@ -108,3 +112,50 @@ def test_g1_flat_keeps_its_robot_and_ppo_settings(): assert runner_cfg.actor.hidden_dims == runner_cfg.critic.hidden_dims == [256, 128, 128] assert runner_cfg.max_iterations == 1500 assert runner_cfg.save_interval == 50 + + +def test_g1_prestartup_derives_parameters_from_the_configured_asset(tmp_path): + """Authored drives and geometry supply defaults while explicit task overrides survive startup.""" + from pxr import Gf, Usd, UsdGeom, UsdPhysics + + path = tmp_path / "robot.usda" + stage = Usd.Stage.CreateNew(str(path)) + root = UsdGeom.Xform.Define(stage, "/Robot") + stage.SetDefaultPrim(root.GetPrim()) + for name in ("left_hip_roll_joint", "left_knee_joint", "left_hand_index_0_joint"): + joint = UsdPhysics.RevoluteJoint.Define(stage, f"/Robot/{name}") + UsdPhysics.DriveAPI.Apply(joint.GetPrim(), "angular").CreateMaxForceAttr(120.0) + UsdGeom.Xform.Define(stage, "/Robot/left_ankle_roll_link") + plate = UsdGeom.Cube.Define(stage, "/Robot/left_ankle_roll_link/foot_plate") + plate.CreateSizeAttr(2.0) + transform = Gf.Matrix4d().SetScale(Gf.Vec3d(0.12, 0.03, 0.01)) + transform.SetTranslateOnly(Gf.Vec3d(0.03, 0.0, -0.02)) + plate.AddTransformOp().Set(transform) + stage.GetRootLayer().Save() + + cfg = rough_env_cfg.G1RoughTeacherEnvCfg() + cfg.scene.robot.spawn.usd_path = str(path) + cfg.scene.robot.actuators = { + "joints": rough_env_cfg.ImplicitActuatorCfg( + joint_names_expr=[".*"], + stiffness={"left_hip_roll_joint": 200.0, "left_knee_joint": 100.0, "left_hand_index_0_joint": 40.0}, + damping=0.0, + ) + } + event = cfg.events.asset_parameters + assert event.mode == "prestartup" + env = SimpleNamespace(cfg=cfg) + event.func(env, None, **event.params) + assert cfg.actions.joint_pos.scale == { + "left_hip_roll_joint": 0.11, + "left_knee_joint": 0.3, + "left_hand_index_0_joint": 0.0153, + } + assert cfg.rewards.feet_support_edge.params["sole_size"] == pytest.approx((0.24, 0.06, 0.02)) + assert cfg.rewards.feet_support_edge.params["sole_offset"] == pytest.approx((0.03, 0.0, -0.02)) + + cfg.actions.joint_pos.scale = 0.125 + cfg.rewards.feet_support_edge.params["sole_size"] = (0.3, 0.08, 0.04) + event.func(env, None, **event.params) + assert cfg.actions.joint_pos.scale == 0.125 + assert cfg.rewards.feet_support_edge.params["sole_size"] == (0.3, 0.08, 0.04) diff --git a/tools/test/test_depth_publisher.py b/tools/test/test_depth_publisher.py new file mode 100644 index 000000000000..c0745f5c51c0 --- /dev/null +++ b/tools/test/test_depth_publisher.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 + +"""Hardware-free checks of the standalone G1 depth publisher and its wire format.""" + +import importlib.util +import shutil +import struct +import subprocess +import sys +from pathlib import Path +from types import SimpleNamespace + +import numpy as np + + +def test_copied_publisher_depth_processing_and_wire_format(tmp_path): + """The copied script needs no repository imports and preserves valid depth and image orientation.""" + path = tmp_path / "depth_publisher.py" + shutil.copyfile(Path(__file__).parents[2] / "scripts/deploy/depth_publisher.py", path) + subprocess.run([sys.executable, "-I", str(path), "--help"], cwd=tmp_path, check=True, capture_output=True) + spec = importlib.util.spec_from_file_location("depth_publisher", path) + publisher = importlib.util.module_from_spec(spec) + spec.loader.exec_module(publisher) + + # These intrinsics give a wider horizontal and narrower vertical field of view than training. + intrinsics = SimpleNamespace(width=848, height=480, fx=428.0, fy=427.0) + assert publisher.crop_to_fov(intrinsics) == (18, 0, 812, 480) + + frame = np.full((76, 128), 4.0, dtype=np.float32) + frame[:2, :2] = [[1.0, 3.0], [0.0, np.nan]] + frame[:2, 2:4] = [[0.0, np.inf], [-1.0, np.nan]] + frame[-2:, -2:] = 1.234 + small = publisher.downsample_masked(frame) + expected = np.full((38, 64), 4.0, dtype=np.float32) + expected[0, :2] = [2.0, 0.0] + expected[-1, -1] = 1.234 + np.testing.assert_array_equal(small, expected) + + packet = publisher.pack_frame(2**32 + 7, small, 123456789) + assert len(packet) == 4884 + assert struct.unpack_from("