diff --git a/docs/source/_static/css/environment-browser.js b/docs/source/_static/css/environment-browser.js index edb297ec0bf1..a95671d22e14 100644 --- a/docs/source/_static/css/environment-browser.js +++ b/docs/source/_static/css/environment-browser.js @@ -48,12 +48,13 @@ ["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||"]], ["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/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 new file mode 100644 index 000000000000..ad742cc21282 --- /dev/null +++ b/docs/source/policy_deployment/07_g1_sim2real/training_and_deploying_a_g1_visual_locomotion_policy.rst @@ -0,0 +1,309 @@ +Training and Deploying a G1 Visual Locomotion Policy +==================================================== + +.. 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 + 2. distill Isaac-Velocity-Rough-G1-Depth-Student + 3. export pipeline .yaml + .onnx + _initial_values.safetensors + 4. deploy scripts/deploy/g1_deploy.py + +.. raw:: html + +
+ + G1 on blocks: PhysX, Newton (MJWarp), and real robot + + + G1 on ramps: PhysX, Newton (MJWarp), and real robot + +
+ + +Prerequisites +------------- + +Start with the :doc:`Isaac Lab installation `. From the repository +root, install the export and inference dependencies into its Python environment: + +.. 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')" + +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 +-------------------- + +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 + +``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 +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. + + +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: 12 + +Two details about the ends of this loop. + +Joint targets are mapped to the 29 body motors by joint name using the exported ``element_names``. + +Before enabling the policy, the robot transitions to the predefined starting pose over three seconds +and holds it until the operator confirms. + +Conversions +^^^^^^^^^^^ + +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.** ``--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 +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 +^^^^^^^^^^^^^^^^^^^ + +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 +^^^^^^^^^^^^^^^^^^^ + +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. 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:: + + 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..d75d018c50b5 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_and_deploying_a_g1_visual_locomotion_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 new file mode 100644 index 000000000000..9dfd455910f3 --- /dev/null +++ b/scripts/deploy/g1_deploy.py @@ -0,0 +1,610 @@ +# 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 logging +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 + +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.""" + +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.""" + +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", + "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 + + +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 r face current heading x quit ? help" + 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 + 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. + + 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. + """ + 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: + 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) + 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: + """``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.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 + + 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 = unitree_hg_msg_dds__LowCmd_() + self._crc = CRC() + self._next = None + 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.""" + 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) + time.sleep(CONTROL_DT) + + def wait(self, period: float) -> None: + """Keep the control cadence, discarding overdue deadlines after a stall.""" + if self._next is None: + self._next = time.monotonic() + self._next += period + 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: + """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, imu_tilt: tuple[float, float] = (0.0, 0.0)) -> None: + import torch + 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) + + 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: + 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), + ) + 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] + + 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][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.""" + 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." + ) + logger.info("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. + """ + start = link.read()["joint_pos"].copy() + steps = int(ramp_s / CONTROL_DT) + 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) + 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.") + 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) + 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) + + logger.info("Engaging in 3s -- hands clear") + for _ in range(int(3.0 / CONTROL_DT)): + link.check_abort() + link.write(pose, kp, kd) + time.sleep(CONTROL_DT) + if link.read()["gravity_z"] > UPRIGHT_GRAVITY_Z: + raise RuntimeError("the robot is not upright; not engaging the policy") + + +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.") + 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, + 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() + 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, (pitch, roll)) + link = G1Link(args.domain_id, args.interface) + depth = DepthStream(args.depth_port) + depth.wait() + take_lowcmd(args.domain_id) + + 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.") + + try: + 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) as error: + logger.info(str(error) or "Interrupted") + except (RuntimeError, TimeoutError) as error: + logger.error(str(error)) + finally: + link.damp(policy.kd) + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) 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_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 d104bcc2a383..99b005735cf0 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 @@ -103,7 +103,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 4f1f98ae4c13..4f3d230d4781 100644 --- a/source/isaaclab_rl/test/test_entrypoints.py +++ b/source/isaaclab_rl/test/test_entrypoints.py @@ -941,3 +941,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-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 new file mode 100644 index 000000000000..d6a6de03a366 --- /dev/null +++ b/source/isaaclab_tasks/changelog.d/g1-shared-mdp.changed.rst @@ -0,0 +1,20 @@ +* 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, + 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. 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. +* 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 during environment initialization, + reusing configured actuator stiffnesses without loading USD while constructing configurations. 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 41304bc72775..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,18 +21,27 @@ "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", }, ) +# 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", + "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", - "skrl_cfg_entry_point": f"{agents.__name__}:skrl_rough_ppo_cfg.yaml", + }, +) + +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..80bd879f0a4c --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/config/g1/agents/distillation.py @@ -0,0 +1,157 @@ +# 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 + +"""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 +from tensordict import TensorDict +from torch import nn + +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: + """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: 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( + { + "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), + }, + batch_size=obs.batch_size, + ) + + @torch.no_grad() + 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. + + Returns: + Reflected actions with the same shape, dtype, and device. + """ + return actions[..., self.perm] * self.sign + + +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. + """ + + @staticmethod + 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) + return alg + + 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, + ) + self.student.update_normalization(mirrored) + self.student.update_normalization(obs) + super().process_env_step(obs, rewards, dones, extras) + + 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) -> 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(): + 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 + 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..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 @@ -5,16 +5,25 @@ 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 -class G1RoughPPORunnerCfg(RslRlOnPolicyRunnerCfg): +class G1RoughTeacherPPORunnerCfg(RslRlOnPolicyRunnerCfg): + seed = 42 num_steps_per_env = 24 - max_iterations = 5000 - save_interval = 50 - experiment_name = "g1_rough" - obs_groups = {"actor": ["policy"], "critic": ["policy"]} + max_iterations = 6000 + save_interval = 250 + experiment_name = "g1_rough_teacher" + obs_groups = {"actor": ["proprio", "height_scan"], "critic": ["proprio", "height_scan"]} actor = RslRlMLPModelCfg( hidden_dims=[512, 256, 128], activation="elu", @@ -39,15 +48,63 @@ class G1RoughPPORunnerCfg(RslRlOnPolicyRunnerCfg): lam=0.95, desired_kl=0.01, max_grad_norm=1.0, + 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 G1FlatPPORunnerCfg(G1RoughPPORunnerCfg): +class G1FlatPPORunnerCfg(G1RoughTeacherPPORunnerCfg): + max_iterations = 1500 + save_interval = 50 + experiment_name = "g1_flat" + obs_groups = {"actor": ["proprio"], "critic": ["proprio"]} + 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] + self.algorithm.symmetry_cfg = None + + +@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": ["proprio_history", "depth"], "teacher": ["proprio", "height_scan"]} + 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/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/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..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 @@ -5,16 +5,110 @@ """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 +from isaaclab.utils import configclass, replace -from .rough_env_cfg import G1RoughEnvCfg +from isaaclab_assets import G1_MINIMAL_CFG + +from ... import mdp +from ...velocity_env_cfg import LocomotionVelocityRoughEnvCfg +from .rough_env_cfg import G1EventsCfg, G1ObservationsCfg, G1Rewards @configclass -class G1FlatEnvCfg(G1RoughEnvCfg): +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, + 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__() @@ -24,25 +118,17 @@ 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.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.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" # 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 6c6329d3299b..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 @@ -3,29 +3,81 @@ # # 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. +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. +""" + +from __future__ import annotations + +import math +from typing import TYPE_CHECKING + +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 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.utils import configclass, replace +from isaaclab.managers import TerminationTermCfg as DoneTerm +from isaaclab.sensors import CameraCfg, RayCasterCfg, patterns +from isaaclab.sim import PinholeCameraCfg, SimulationCfg, UsdFileCfg +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_assets import G1_MINIMAL_CFG +from isaaclab_tasks.utils.presets import MultiBackendRendererCfg -from ... import mdp +from ... import mdp, velocity_env_cfg from ...velocity_env_cfg import ( LocomotionVelocityRoughEnvCfg, RewardsCfg, ) +from .terrains import G1_ROUGH_TERRAINS_CFG + +if TYPE_CHECKING: + from isaaclab.envs import ManagerBasedRLEnv + +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 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( @@ -33,7 +85,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"), @@ -42,7 +94,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"), @@ -57,9 +109,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, @@ -67,78 +119,395 @@ 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"), + # Filled from the robot asset by the prestartup event before rewards are loaded. + "sole_size": None, + "sole_offset": None, + }, + ) + + +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 + + +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 G1RoughEnvCfg(LocomotionVelocityRoughEnvCfg): - """Configuration for the Unitree G1 velocity-tracking environment on rough terrain.""" +class G1EventsCfg(velocity_env_cfg.EventsCfg): + """G1 startup and reset events.""" - rewards: G1Rewards = G1Rewards() + asset_parameters: EventTerm | None = None - def __post_init__(self): - super().__post_init__() + 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", + }, + ) + 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)}, + ) - # 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"] + +@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), + ], + ), + 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 + ), + }, + ) + + 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 = G1_ROUGH_TERRAINS_CFG + + +@configclass +class G1DepthStudentSceneCfg(G1SceneCfg): + """Teacher scene extended with the student's depth and reflected-depth cameras.""" + + @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]), + rot=DEPTH_OFFSET_ROT, + convention="world", + ), + ) + + +@configclass +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), ) - 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_.*"] + 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 ProprioHistoryCfg(G1ObservationsCfg.ProprioCfg): + """Five proprioceptive frames without privileged velocity or height observations.""" + + base_lin_vel = 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, ) - # 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) + enable_corruption = False + concatenate_terms = True + + height_scan: G1ObservationsCfg.HeightScanCfg = G1ObservationsCfg.HeightScanCfg(enable_corruption=False) + proprio_history: ProprioHistoryCfg = ProprioHistoryCfg() + depth: DepthCfg = DepthCfg() + depth_mirror: DepthCfg = DepthCfg() + + def __post_init__(self): + super().__post_init__() + self.proprio.enable_corruption = False + self.depth_mirror.depth.params["sensor_cfg"] = SceneEntityCfg("depth_camera_mirror") + + +@configclass +class G1RoughTeacherEnvCfg(LocomotionVelocityRoughEnvCfg): + """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(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() + 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) + + +@configclass +class G1RoughDepthStudentEnvCfg(G1RoughTeacherEnvCfg): + """G1 student with five proprioceptive frames and mirrored depth supervision.""" + + scene: G1DepthStudentSceneCfg = G1DepthStudentSceneCfg(num_envs=4096, env_spacing=2.5) + observations: G1StudentObservationsCfg = G1StudentObservationsCfg() 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/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 88a90656769f..b7704d7e733d 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/core/velocity/mdp/__init__.pyi +++ b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/mdp/__init__.pyi @@ -8,6 +8,7 @@ __all__ = [ "feet_air_time_positive_biped", "feet_air_time_variance", "feet_slide", + "feet_support_edge_distance", "stand_still_joint_deviation_l1", "terrain_levels_vel", "terrain_out_of_bounds", @@ -21,6 +22,7 @@ from .rewards import ( feet_air_time_positive_biped, feet_air_time_variance, feet_slide, + feet_support_edge_distance, stand_still_joint_deviation_l1, track_ang_vel_z_world_exp, track_lin_vel_xy_yaw_frame_exp, 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..90acc4c9afda 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,65 @@ 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 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..dcaf42824d09 --- /dev/null +++ b/source/isaaclab_tasks/isaaclab_tasks/core/velocity/mdp/symmetry/g1.py @@ -0,0 +1,214 @@ +# 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 + +"""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 + +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 +) -> 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] + 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/test/core/test_velocity_newton_cfg.py b/source/isaaclab_tasks/test/core/test_velocity_newton_cfg.py index 943de458fc61..feb6346e26ce 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,159 @@ # # SPDX-License-Identifier: BSD-3-Clause +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_tasks.core.velocity.config.g1.rough_env_cfg import G1RoughEnvCfg +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 -def test_g1_rough_newton_has_sufficient_constraint_capacity(): - env_cfg = G1RoughEnvCfg() +from isaaclab_assets import G1_MINIMAL_CFG - 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.""" + + 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") + + 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) + 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 + 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") + 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 ( + (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) + assert not any( + 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() + ) + + +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 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 + 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("= 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)