diff --git a/CLAUDE.md b/CLAUDE.md index 0eea89d3..17c63519 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -49,7 +49,7 @@ pages are in `docs/g1/` (start at `docs/g1/index.md`). |---|---| | `docker/` | the images and containers (Dockerfiles, `docker-compose.yml`, the G1 overlay `docker-compose.g1.yml`, `scripts/`; the Mac's arm64 images in `docker/mac/` with the overlay `docker-compose.mac.yml`, `docs/MAC.md`) | | `core_ws/` | the ROS 2 workspace — where the bulk of development happens (see §4); the G1's own package is `core_ws/src/dex_3/`, the safety layer, arm controller, leg controller, SLAM/navigation and skills both robots run in `core_ws/src/golem_safety/`, `core_ws/src/golem_upper_ik/`, `core_ws/src/golem_lowerbody_rl/`, `core_ws/src/golem_slam/` and `core_ws/src/golem_skills/` | -| `golem_robocasa/` | RoboCasa/MuJoCo simulator entry point for both robots (`h12_mujoco.py`; `--g1` hands over to the G1 sim package `golem_robocasa/g1/`) and the ROS/DDS bridges | +| `golem_robocasa/` | RoboCasa/MuJoCo simulator for both robots: one entry point and loop (`golem_sim.py`), a robot object each (`h12_robot.py`, `g1/g1_robot.py`) and the ROS/DDS bridges | | `CL_isaaclab_sim/` | Isaac Lab simulator, H1-2 only (`sim_main.py`, DDS bridges, tasks) | | `CL_Assets/` | both robots' URDFs, MJCFs, meshes and self-collision models (the G1's in `CL_Assets/g1/`), Isaac USD (Git-LFS submodule); `golem_ros2_models` installs the models for ROS, and the sims read them directly | | `unitree_sdk2_python/` | vendored Unitree DDS SDK (Python) | @@ -157,9 +157,9 @@ The compose files pass the RoboCasa sims' knobs from the shell or `docker/.env`: (the H1-2's default 0.20, which a task env can set via `spawn_margin`, as `battery_env.py` does; the G1's 0.05). Both robots' spawn searches (`robosuite_common.place_robot_collision_free`) back the robot off, and shift it -sideways where that is not enough, until the clearance holds; the H1-2's must -also hold in its limp sag poses and along `golem_upper_ik`'s start-up arm moves -(`h12_mujoco.STARTUP_ARM_STAGES`, kept equal to its `robot.yaml` by +sideways where that is not enough, until the clearance holds; it must +also hold in each robot's limp sag poses and along `golem_upper_ik`'s start-up arm moves +(`STARTUP_ARM_STAGES` in `h12_robot.py` and `g1/g1_robot.py`, kept equal to each `robot.yaml` by `tests/docker/test_sim_startup_poses.py`), which run while the band still holds it. `GOLEM_CAMERA_SCALE=` (H1-2 default 0.5 of the real 1280x720), `GOLEM_CAMERA_DEPTH=0` and `GOLEM_CAMERA_POINTCLOUD=0` tune the H1-2's RoboCasa cameras. @@ -519,9 +519,11 @@ ros2 action send_goal /named_config custom_ros_messages/action/NamedConfig \ `CL_isaaclab_sim` and `golem_robocasa` should generally not be modified unless necessary, and their **default behaviors must be preserved**. Everyone's sim-vs-real workflow depends on them behaving like the real robot. -`golem_robocasa/h12_mujoco.py` is the one RoboCasa entry point for both robots: it -picks the robot (`--h12`/`--g1`, else `GOLEM_ROBOT`, else `h12`) before its -H1-2 imports and hands the G1 over to `golem_robocasa/g1/g1_mujoco.py`. Both +`golem_robocasa/golem_sim.py` is the one RoboCasa entry point and physics loop for both +robots: it picks the robot (`--h12`/`--g1`, else `GOLEM_ROBOT`, else `h12`) before either +robot's imports and runs that robot's object (`h12_robot.H12Robot`, `g1/g1_robot.G1Robot`: +spawn, band, sensors, DDS interface, locking and what only it serves) through the same loop, +so a change to the loop is made once. Both sims render their cameras and the MID-360's rays through EGL on an NVIDIA GPU: without one (MuJoCo not on EGL, no EGL device, a software renderer) the sim stops at start, once its command line is parsed and before it builds a scene or @@ -530,8 +532,7 @@ it is the Mac images' opt-in `GOLEM_RENDER=cpu` (OSMesa for the cameras, the MID-360 cast exactly on the CPU by `mid360.CpuCaster`; `sim_common.require_renderer`, `mid360.make_caster`). The H1-2 stays the default, and nothing on the G1 path may change its behaviour. The robots share -`sim_common.py`, `measurement_bridge.py`, the pacer in `sim_timing.py` (its -`--timing` report is the H1-2's only) and the `mujoco_sensors` node in +`sim_common.py`, `measurement_bridge.py`, the pacer in `sim_timing.py` and the `mujoco_sensors` node in `mujoco_ros_bridge.py`; each has its own sensor suite (`h12_sensors.py`, `g1/g1_sensors.py`) and Unitree DDS interface (`unitree_interface.py`, `g1/unitree_interface.py`), which share their command @@ -546,7 +547,7 @@ lid) collide with everything as before. They should publish only what the real robot publishes and consume only what it consumes. Verified current surface: -**RoboCasa, H1-2 (`golem_robocasa/h12_mujoco.py`)** — publishes `/clock`, the real +**RoboCasa, H1-2 (`golem_robocasa/golem_sim.py`)** — publishes `/clock`, the real RealSense drivers' topics, frame names and camera TF (`/tf_static` + `/tf`) for `/realsense/{head,left_hand,right_hand}` — colour, depth and aligned depth with their camera_info, and `depth/color/points` — as captured from the robot in @@ -589,7 +590,7 @@ probe clear it. `h12_sensors.py`, the grippers in `magpie_hand_bridge.py`, `/clock` and the opt-in odom in `mujoco_ros_bridge.py`, Unitree DDS in `unitree_interface.py`.) -**RoboCasa, G1 (`h12_mujoco.py --g1`, `golem_robocasa/g1/`)** — publishes `/clock`, +**RoboCasa, G1 (`golem_sim.py --g1`, `golem_robocasa/g1/`)** — publishes `/clock`, the Livox MID-360 as its driver does in CustomMsg mode (`/livox/lidar`, `/livox/imu`, no `/livox/pointcloud`), the head RealSense D435 (`/realsense/head/*` and its `/tf_static`), and over Unitree DDS `rt/lowstate` @@ -600,7 +601,7 @@ motor is off until the first `rt/lowcmd` with a valid CRC, `mode_machine` 5 and `mode_pr` 0, and an elastic band on `torso_link` holds the robot until the toggle (or SPACE in the viewer) releases it; the G1 bringup's leg node calls it. It reads none of HOMIE's files. (Trust `g1/g1_sensors.py` for -the ROS side, `g1/unitree_interface.py` for the DDS side and `g1/g1_mujoco.py` +the ROS side, `g1/unitree_interface.py` for the DDS side and `g1/g1_robot.py` for the band.) Its knobs are `GOLEM_LIDAR`, `GOLEM_LIDAR_POINTS` (points per frame, default the real sensor's 20,000), `GOLEM_LIDAR_HZ`, `GOLEM_CAMERAS`, `GOLEM_CAMERA_HZ`, @@ -609,13 +610,13 @@ frame, default the real sensor's 20,000), `GOLEM_LIDAR_HZ`, `GOLEM_CAMERAS`, `GOLEM_SIM_RATE` (as on the H1-2, above). Their defaults are in the code that reads them: `SensorConfig` in `g1/g1_sensors.py` (sensors), -`unitree_common.py` (the command timeout), `g1/g1_mujoco.py` -(spawn, odom), `measurement_bridge.py` (experiment) and `sim_common.py` +`unitree_common.py` (the command timeout), `g1/g1_robot.py` +(spawn), `golem_sim.py` (odom), `measurement_bridge.py` (experiment) and `sim_common.py` (rate); the compose files pass them (`docker/docker-compose.g1.yml` the G1-only ones). The G1 sim ignores the H1-2-only knobs (`GOLEM_SPAWN_OFFSET`, `GOLEM_HAND_CAMERA_RES`, -`GOLEM_CAMERA_DEPTH`, `GOLEM_CAMERA_POINTCLOUD`). It refuses -`--timing`, and the H1-2 sim refuses `--duration` (`sim_common.py`). +`GOLEM_CAMERA_DEPTH`, `GOLEM_CAMERA_POINTCLOUD`). The sim logs its real-time factor +every 30 s of sim time, with the sensors' counts. The measured surface of each RoboCasa robot is checked in as a golden: `tests/robocasa/surface/{h12,g1}.json`, compared by @@ -629,6 +630,10 @@ three RealSense RGBD camera streams with camera info, and `rt/lowcmd` and optional `rt/inspire/cmd`. The default asset is the floating-base Magpie H1-2; `--hand_type inspire` selects the Inspire fixture. Magpie exposes the real gripper state, services, and DeliGrasp action through a native ROS process inside the Isaac container. +With `--elastic_band` (opt-in, refused with `--fix_base`; without it the robot floats free, +the default) it hangs the robot from the RoboCasa H1-2 sim's elastic band on `torso_link` and +serves `/elastic_band/toggle` (`std_srvs/srv/Trigger`), checked by +`tests/isaaclab/run_elastic_band.sh`. Its H1-2 (the USD) keeps the MID-360 upright, unlike the RoboCasa sim and the real robot's raw frame, so `golem_slam`'s H1-2 configs do not match Isaac's lidar. Its `rt/lowstate` tick counts publishes, not milliseconds, so @@ -647,8 +652,7 @@ exists. Keeping it off by default keeps the sim honest and protects sim-to-real transfer. - **RoboCasa complies, for both robots:** ground-truth `/odom` + `odom→pelvis` - TF is gated by `GOLEM_SIM_ODOM` (default `'0'` = off, read in `h12_mujoco.py` - and `g1/g1_mujoco.py`). No stack opts in: every bringup's navigation runs on FAST-LIO, and the G1 bringup and tests refuse it. Keep it that way. The + TF is gated by `GOLEM_SIM_ODOM` (default `'0'` = off, read in `golem_sim.py`). No stack opts in: every bringup's navigation runs on FAST-LIO, and the G1 bringup and tests refuse it. Keep it that way. The experiment surface (`/sim/ground_truth`, `/sim/reset_objects`) is gated by `GOLEM_SIM_EXPERIMENT` (`measurement_bridge.py`). The closed-loop tests (`tests/g1`, `tests/sim`) run the sim with both off (`GOLEM_SIM_ODOM=0`, @@ -909,7 +913,7 @@ docker/scripts/docker_stack.sh logs robocasa # Or three terminals, to choose the bringup's arguments (the wrappers do not # check the sim's robot here; golem_safety's own check is in §8) -# Terminal 1 — the sim; wait for "[g1_mujoco] ROS bridges up" +# Terminal 1 — the sim; wait for "[golem_sim] ROS bridges up" docker/scripts/docker_run.sh robocasa --g1 --layout 9 --style 9 --seed 42 # Terminal 2 — builds core_ws (both robots' packages), drops to a shell; no # --g1 needed: the ros container runs either robot's bringup diff --git a/CL_isaaclab_sim b/CL_isaaclab_sim index a4cc11bd..78c3f70e 160000 --- a/CL_isaaclab_sim +++ b/CL_isaaclab_sim @@ -1 +1 @@ -Subproject commit a4cc11bd99203b3ba38335ccfc19ab3f8ca8d627 +Subproject commit 78c3f70ee28cb60bc46ab5dfe9d507dfe70b7143 diff --git a/docker/docker-compose.yml b/docker/docker-compose.yml index 56ec6a1a..4f671021 100644 --- a/docker/docker-compose.yml +++ b/docker/docker-compose.yml @@ -104,13 +104,13 @@ services: - QT_X11_NO_MITSHM=1 - ROS_DOMAIN_ID=${ROS_DOMAIN_ID:-1} # H1-2: m of clearance between the robot and any non-floor geom at - # spawn. Empty: the task's spawn_margin, else 0.20 (h12_mujoco.py). + # spawn. Empty: the task's spawn_margin, else 0.20 (golem_sim.py). - GOLEM_SPAWN_MARGIN=${GOLEM_SPAWN_MARGIN:-} # Both robots: m the robot is backed off the spawn counter into open # floor, for walking and navigation. Empty: 0, at the counter. - GOLEM_SPAWN_BACKOFF=${GOLEM_SPAWN_BACKOFF:-} # H1-2: 0 keeps the robot's own links in its lidar scan as occluders. - # Empty: they are filtered out (h12_mujoco.py). + # Empty: they are filtered out (golem_sim.py). - GOLEM_LIDAR_SELF_FILTER=${GOLEM_LIDAR_SELF_FILTER:-} # Both robots: the target real-time factor, sim seconds per wall second # (golem_robocasa/sim_common.py). Empty: 1, real time; 2 twice real time; diff --git a/docker/scripts/docker_common.sh b/docker/scripts/docker_common.sh index 421b8795..67f96100 100755 --- a/docker/scripts/docker_common.sh +++ b/docker/scripts/docker_common.sh @@ -346,7 +346,7 @@ prepare_mounts() { "$GOLEM_CACHE_DIR/isaac/local_share_ov" "$GOLEM_CACHE_DIR/isaac/isaacsim" ;; robocasa) - require_file "$GOLEM_ROOT/golem_robocasa/h12_mujoco.py" + require_file "$GOLEM_ROOT/golem_robocasa/golem_sim.py" require_file "$GOLEM_ROOT/core_ws/src/magpie_msgs/package.xml" require_file "$GOLEM_ROOT/core_ws/src/custom_ros_messages/package.xml" # Mount points of the two read-only package mounts inside msgs_ws. diff --git a/docker/scripts/launch_robocasa.sh b/docker/scripts/launch_robocasa.sh index 735edf00..918b2475 100755 --- a/docker/scripts/launch_robocasa.sh +++ b/docker/scripts/launch_robocasa.sh @@ -1,8 +1,8 @@ #!/bin/bash # The robocasa container's command: build msgs_ws, then run the RoboCasa -# simulator (golem_robocasa/h12_mujoco.py) of the selected robot. +# simulator (golem_robocasa/golem_sim.py) of the selected robot. # -# Usage: launch_robocasa.sh [h12_mujoco.py arguments...] +# Usage: launch_robocasa.sh [golem_sim.py arguments...] # e.g. --headless or --layout 9 --style 9 --seed 42. Without a DISPLAY, # --headless is added (no passive viewer). # Environment: ROS_DOMAIN_ID (1..232, 1 when unset), DISPLAY, @@ -58,4 +58,4 @@ fi cd /home/code/golem_robocasa # -u: Compose reads stdout through a pipe, which would block-buffer print() and # hold back the "ROS bridges up" readiness line on a quiet headless run. -exec python -u h12_mujoco.py "$@" +exec python -u golem_sim.py "$@" diff --git a/docs/RUNNING.md b/docs/RUNNING.md index 1fea5899..43bcb49a 100644 --- a/docs/RUNNING.md +++ b/docs/RUNNING.md @@ -90,7 +90,10 @@ Uses the same wrappers. The launcher honors rejected. Select a task with `docker_run.sh isaac --task base --headless`. Magpie H1-2 is the default asset, with a floating base. Start the ROS lower-body controller to support it. Select the Inspire fixture with `--hand_type inspire`. -Use `--fix_base` only for a fixed-root bench test. Logs go to container stdout. +Use `--fix_base` only for a fixed-root bench test. `--elastic_band` (not with `--fix_base`) +hangs the floating robot from an elastic band on its torso, as the RoboCasa sims do, until +`ros2 service call /elastic_band/toggle std_srvs/srv/Trigger` releases it; a second call +enables it again. Logs go to container stdout. The Magpie ROS gripper process starts inside the Isaac container using an isolated Humble runtime. Its message interfaces are built into the image. diff --git a/docs/g1/control-path.md b/docs/g1/control-path.md index ac23604a..335d553d 100644 --- a/docs/g1/control-path.md +++ b/docs/g1/control-path.md @@ -51,7 +51,7 @@ while `dex3_hands` uses the Unitree SDK. The user-facing interface is ROS 2 too: puts the robot into debug mode before the bringup starts; nothing in this stack does that step or checks it ([First real session](first-session.md#entering-debug-mode-not-yet-written)). -- **In the sim:** `golem_robocasa/g1/` (`g1_mujoco.py`, `unitree_interface.py`) +- **In the sim:** `golem_robocasa/g1/` (`g1_robot.py`, `unitree_interface.py`) plays the robot in debug mode. Every motor is off (zero torque) until the first `rt/lowcmd` with a valid CRC, `mode_machine` 5 and `mode_pr` 0; from then on `rt/lowcmd` drives every body motor. Meanwhile an elastic band on the @@ -59,7 +59,7 @@ while `dex3_hands` uses the Unitree SDK. The user-facing interface is ROS 2 too: through `/elastic_band/toggle` once its policy runs and the arm controller has finished its start-up moves, or `band.max_wait` after the policy started (SPACE in the viewer toggles it by hand). - `g1/unitree_interface.py`'s and `g1_mujoco.py`'s docstrings are the + `g1/unitree_interface.py`'s and `g1_robot.py`'s docstrings are the reference. ## Sim and real compared diff --git a/docs/g1/index.md b/docs/g1/index.md index 9bf6185c..cfd68a91 100644 --- a/docs/g1/index.md +++ b/docs/g1/index.md @@ -7,7 +7,7 @@ and frontier exploration use the shared `golem_slam` and `golem_skills`; its models (Unitree's URDFs, MJCFs and meshes, and a sphere-packed self-collision model) are in `CL_Assets/g1`, installed for ROS by `golem_ros2_models` with the H1-2's, and its RoboCasa kitchen sim is `golem_robocasa/g1/`, started through the same entry point -as the H1-2's (`h12_mujoco.py --g1`). The same bringup runs against the sim or +as the H1-2's (`golem_sim.py --g1`). The same bringup runs against the sim or the real robot, which speaks the same Unitree DDS interface. On the real G1 every node runs in Docker on a laptop wired to the robot. @@ -78,7 +78,7 @@ To send your own arm and hand goals, start the stack in three terminals: ```bash # terminal 1: the sim with its viewer (add --headless for none) docker/scripts/docker_run.sh robocasa --g1 --layout 9 --style 9 --seed 42 -# wait for "[g1_mujoco] ROS bridges up" +# wait for "[golem_sim] ROS bridges up" # terminal 2: builds core_ws (both robots' packages) if needed, then opens a shell docker/scripts/docker_run.sh ros diff --git a/docs/g1/walking.md b/docs/g1/walking.md index a37740a7..316c572f 100644 --- a/docs/g1/walking.md +++ b/docs/g1/walking.md @@ -32,7 +32,7 @@ standing, then it walks 5 s at 0.3 m/s. A passing run ends with ```bash # terminal 1: the sim, the spawn backed 2 m into open floor GOLEM_SPAWN_BACKOFF=2.0 docker/scripts/docker_run.sh robocasa --g1 --layout 9 --style 9 --seed 42 -# wait for "[g1_mujoco] ROS bridges up" +# wait for "[golem_sim] ROS bridges up" # terminal 2: wait for the leg node's "elastic band released" line docker/scripts/docker_run.sh ros diff --git a/golem_robocasa/battery_env.py b/golem_robocasa/battery_env.py index 568295b9..34adf80d 100644 --- a/golem_robocasa/battery_env.py +++ b/golem_robocasa/battery_env.py @@ -1,6 +1,6 @@ """Register the Battery task: the robot at the ARPA battery workcell. -Both sim runners run it (``h12_mujoco.py --task Battery``, ``--g1`` for the +Both sim runners run it (``golem_sim.py --task Battery``, ``--g1`` for the G1). It is a robosuite env, not a RoboCasa kitchen, around the robot the kitchen tasks use (``H1_2`` or ``G1Dex3``), so every robot0_/gripper0_ name the bridges resolve is the kitchen tasks'. The arena is ``CL_Assets/mujoco_assets/battery/ diff --git a/golem_robocasa/blank_env.py b/golem_robocasa/blank_env.py index ab32dcc0..598e9e81 100644 --- a/golem_robocasa/blank_env.py +++ b/golem_robocasa/blank_env.py @@ -1,6 +1,6 @@ """Register the Blank task: the robot alone on a floor, under a light. -Both sim runners run it (``h12_mujoco.py --task Blank``, with or without +Both sim runners run it (``golem_sim.py --task Blank``, with or without ``--g1``). It is a robosuite env, not a RoboCasa kitchen, around the robot the runner registers (``H1_2`` or ``G1Dex3``), so every robot0_/gripper0_ name the bridges resolve is the kitchen tasks'. The arena is robosuite's ``EmptyArena`` diff --git a/golem_robocasa/estop_server.py b/golem_robocasa/estop_server.py index 48e161da..84349c8b 100644 --- a/golem_robocasa/estop_server.py +++ b/golem_robocasa/estop_server.py @@ -42,7 +42,7 @@ class EstopServer: def __init__(self, port: int, tag: str): """Bind the port; raise OSError when another process holds it. - tag prefixes the log lines (e.g. "h12_mujoco"). + tag prefixes the log lines (e.g. "golem_sim"). """ self._tag = tag self._lock = threading.Lock() diff --git a/golem_robocasa/g1/__init__.py b/golem_robocasa/g1/__init__.py index 9b6f4405..b2a7d2b0 100644 --- a/golem_robocasa/g1/__init__.py +++ b/golem_robocasa/g1/__init__.py @@ -1,6 +1,6 @@ """Run the Unitree G1 (29 DoF, Dex3-1 hands) in the RoboCasa sim. -h12_mujoco.py hands --g1 and GOLEM_ROBOT=g1 to g1_mujoco.main. Import this +golem_sim.py runs g1_robot.G1Robot for --g1 and GOLEM_ROBOT=g1. Import this package as ``g1`` with golem_robocasa on sys.path, never with this directory on sys.path: its unitree_interface and sim_names share their names with the H1-2 sim's modules. The modules here import each other relatively and the shared diff --git a/golem_robocasa/g1/g1_mujoco.py b/golem_robocasa/g1/g1_mujoco.py deleted file mode 100644 index 27574059..00000000 --- a/golem_robocasa/g1/g1_mujoco.py +++ /dev/null @@ -1,304 +0,0 @@ -"""Run the G1 RoboCasa MuJoCo sim: h12_mujoco.py hands --g1 and GOLEM_ROBOT=g1 to main().""" -import argparse -import contextlib -import os -import sys -import threading -import time - -import mujoco -import numpy as np -import rclpy.executors - -from mujoco_ros_bridge import RosSensorBridge, init_ros, shutdown_ros - -from . import g1_robosuite # registers G1Dex3 and its gripper -import battery_env # registers the Battery env -import blank_env # registers the Blank env -import robosuite_common -import sim_common -from measurement_bridge import MeasurementBridge -from mujoco_env import ElasticBand, FallLogger, apply_band -from sim_timing import RealtimePacer -from .g1_sensors import G1Sensors, SensorConfig -from .sim_names import BODY_JOINTS, NameResolver -from .unitree_interface import SimInterface -from viewer_sync import close_viewer, open_viewer - -# The fall logger's test: the pelvis tilted more than 60 deg from upright (as -# golem_lowerbody_rl's policies/homie/mujoco_deploy_g1.py tests it), or down on -# the floor. -FALL_UPRIGHTNESS = 0.5 # pelvis z-axis . world up -FALL_PELVIS_HEIGHT = 0.2 # m - -# The elastic band pulls the torso toward a point BAND_ANCHOR_DZ above its -# spawn and holds the robot while no motor drives it, until /elastic_band/toggle -# or SPACE in the viewer releases it. It is twice as stiff as the H1-2's: at -# 1e4 N/m the limp G1 (34.4 kg) rocks fore and aft on its legs, the pelvis -# tilting 16-19 deg for 20 s and more, and the physics can go unstable; at -# 2e4 N/m it settles within 4 s, upright to 0.3 deg, the torso ~1.4 cm below -# the anchor (measured in three RoboCasa kitchens; tests/robocasa/g1/ -# test_g1_band.py checks one). -BAND_BODY = "torso_link" -BAND_ANCHOR_DZ = 0.01 # m -BAND_STIFFNESS = 2e4 # N/m -BAND_DAMPING = 2e3 # N*s/m -# m: the spawn puts the lowest collision geom this far above the floor, so the -# robot hangs from the band with its feet on the floor. -SPAWN_CLEARANCE = 0.001 -# m of free space the spawn keeps from every robot collision geom to any non-floor geom, so -# nothing starts touching and the limp arms have room under the band (their sag is -# unmeasured; GOLEM_SPAWN_MARGIN overrides it). -SPAWN_MARGIN = 0.05 -# s of wall lag the pacer repays at most; past it, it resyncs instead of catching up. -MAX_LAG = 0.1 -# s of sim time between the loop's real-time factor logs. -RTF_LOG_S = 30.0 -# s, Python's GIL switch interval while the sim runs (default 5 ms). The physics -# loop sleeps between steps to keep its pace and needs the GIL back to wake; -# at 5 ms the sensor threads' Python work (the head camera's messages) stalled -# rt/lowstate 5-9 ms, where the real G1 sends it every 2 ms. At 0.5 ms the sim -# with both sensors had no gap over 5 ms in 51 s of sim time. -GIL_SWITCH_S = 0.0005 -# The warnings on which MuJoCo resets the whole scene (robot and kitchen). -UNSTABLE = (mujoco.mjtWarning.mjWARN_BADQPOS, mujoco.mjtWarning.mjWARN_BADQVEL, - mujoco.mjtWarning.mjWARN_BADQACC) - - -def spawn(env, resolver, backoff=0.0, margin=SPAWN_MARGIN): - """Put the robot at rest in g1_robosuite.STANCE, hands open, clear of the fixtures. - - Call after env.reset(), whose zero-action settling folds the limp robot - (and whose initialization noise perturbs every joint). The robot is placed - at RoboCasa's spawn, its lowest collision geom SPAWN_CLEARANCE above the - floor, and backed off (its -x), shifting sideways where that is not enough - (robosuite_common.place_robot_collision_free), until every robot collision - geom is margin (m) from any non-floor geom; backoff (m) then backs it up to - that much further into open floor. The controller's start-up moves the arms - to its home, which is the spawn pose. Zeroes every velocity in the scene. - """ - data = env.sim.data._data - data.qpos[resolver.body.qpos] = [g1_robosuite.STANCE.get(n, 0.0) for n in BODY_JOINTS] - for hand in resolver.hands.values(): - data.qpos[hand.qpos] = 0.0 - data.qvel[:] = 0.0 - g1_robosuite.place_robot_collision_free( - env, env.init_robot_base_pos, - robosuite_common.euler_to_wxyz(getattr(env, "init_robot_base_ori", None)), - clearance=SPAWN_CLEARANCE, extra_backoff=backoff, margin=margin, - ) - - -def make_band(data, resolver): - """Return (band, torso body id): the band on the torso, anchored BAND_ANCHOR_DZ above it now. - - Call it at the spawn. - """ - body = resolver.body_id(BAND_BODY) - if body < 0: - raise KeyError(f"no body {resolver.robot_prefix}{BAND_BODY} for the elastic band") - anchor = data.xpos[body].copy() - anchor[2] += BAND_ANCHOR_DZ - return ElasticBand(anchor, length=0, stiffness=BAND_STIFFNESS, damping=BAND_DAMPING), body - - -def sim_loop(task, viewer=True, layout=None, style=None, seed=None, duration=0.0): - """Run the G1 in task (a RoboCasa kitchen, Battery or Blank) for duration or until stopped. - - The loop steps the env's own MjData (never env.step()) and drives: - - SimInterface : Unitree DDS, as the G1's own computers serve it in - debug mode: rt/lowstate, rt/lowcmd, rt/dex3/{left,right}/*; - every motor off until the first valid rt/lowcmd - - RosSensorBridge : /clock, and G1Sensors: the MID-360 (/livox/lidar, - /livox/imu) and the head D435 (/realsense/head/*), on - unless GOLEM_LIDAR=0 / GOLEM_CAMERAS=0 (SensorConfig), - + ground-truth /odom with GOLEM_SIM_ODOM=1 - - MeasurementBridge: /robocasa/{task_name,task_goal,success,reward} + - /elastic_band/toggle - An elastic band on the torso holds the robot until /elastic_band/toggle or - SPACE in the viewer releases it. The loop paces to GOLEM_SIM_RATE - (sim_common.sim_rate_from_env; default real time). duration is sim seconds - from the first step, counted in physics steps (0: until the viewer closes - or Ctrl-C). The G1 logs its real-time factor itself (no --timing). - """ - - # Before the slow env build: a bad knob fails fast. - sensors = SensorConfig.from_env() - sim_rate = sim_common.sim_rate_from_env() - env = g1_robosuite.make_kitchen_env(task, layout_ids=layout, style_ids=style, seed=seed) - - # robosuite.make() does not reset; reset() builds env.sim, runs the - # placement patch (g1_robosuite) and places the task objects. - env.reset() - # The fixtures' own parts stop colliding with each other and with the static scene; the - # robot and the objects collide with everything as before. - sim_common.exclude_fixture_collisions(env) - - model = env.sim.model._model - data = env.sim.data._data - pfx = env.robots[0].robot_model.naming_prefix - resolver = NameResolver(model, robot_prefix=pfx) - - try: - # GOLEM_SPAWN_BACKOFF (m, default 0: at the counter) backs the robot - # further into open floor; Nav2 needs a free start cell to plan (~1.0). - # GOLEM_SPAWN_MARGIN (m, unset: SPAWN_MARGIN) is the clearance to the fixtures. - _margin = os.environ.get("GOLEM_SPAWN_MARGIN", "").strip() - spawn(env, resolver, backoff=float(os.environ.get("GOLEM_SPAWN_BACKOFF", "0") or 0), - margin=float(_margin) if _margin else SPAWN_MARGIN) - print("[g1_mujoco] initial stance: legs at Unitree's default angles, everything else at 0") - except Exception as e: - print(f"[g1_mujoco] clean reset skipped: {e}") - - # The band holds the robot from the first physics step; every motor is off - # until the first valid rt/lowcmd. - band, band_body = make_band(data, resolver) - band_vel = np.zeros(6) # apply_band's scratch - print(f"[g1_mujoco] elastic band on {BAND_BODY} anchored at {band.point.round(3)}; " - "/elastic_band/toggle or SPACE in the viewer releases it") - - print(f"[g1_mujoco] RoboCasa task {task!r} built around the G1: " - f"nq={model.nq} nu={model.nu}; resolver mapped {len(resolver.body.names)} body " - f"+ {sum(len(h.names) for h in resolver.hands.values())} hand motors") - - # Ground-truth odom (odom -> pelvis on /tf, /odom) from the pelvis, only - # with GOLEM_SIM_ODOM: the real robot has none, and it would compete with - # FAST-LIO's odometry. - _sim_odom_on = sim_common.env_flag('GOLEM_SIM_ODOM', False) - odom_base_body_id = resolver.base_body if _sim_odom_on else -1 - print(f"[g1_mujoco] ground-truth sim odom {'ON (GOLEM_SIM_ODOM=1)' if _sim_odom_on else 'OFF'}") - - sim_interface = SimInterface(model, data, resolver) - # For the closed-loop tests that record the sim from inside its process. - sim_interface.band = band - - sys.setswitchinterval(GIL_SWITCH_S) - init_ros() - - # The physics loop is the only writer of MjData: the sensor threads work on - # copies of the state tick() takes, and the ROS and DDS callbacks only latch - # values the loop reads, so the bridge takes no lock and the loop calls - # tick() between steps. The one other reader is the viewer's sync thread, - # which copies the state under physics_lock (held around the step; a - # null context headless, where nothing else reads). - physics_lock = threading.Lock() if viewer else contextlib.nullcontext() - ros_bridge = RosSensorBridge( - model, data, - lambda node: G1Sensors(node, model, data, prefix=pfx, config=sensors), - base_body_id=odom_base_body_id, - sim_lock=None, - ) - print(f"[g1_mujoco] sensors: {sensors.describe()}") - print(f"[g1_mujoco] {sim_common.describe_sim_rate(sim_rate)}", flush=True) - - measurement = MeasurementBridge(env, elastic_band=band, task_name=task) - print(f"[g1_mujoco] task goal: {measurement.publish_goal()!r}") - - # The executor thread serves the band toggle and the parameter services. - # ros_bridge is not on it: the main loop calls its tick() between steps. The - # thread starts just before the loop's try, whose finally shuts it down. - executor = rclpy.executors.MultiThreadedExecutor() - executor.add_node(measurement) - - # The viewer draws its own MjData, refreshed from data by the viewer thread, - # with the band while it is enabled; its mouse perturbation forces come back - # through view_sync.apply_perturbation(). - handle = view_sync = None - if viewer: - handle, view_sync = open_viewer(model, data, physics_lock, band, band_body, "g1_mujoco", - sim_rate=sim_rate) - # Logs the band's release and the first time the pelvis tips or drops. - fall_logger = FallLogger(resolver.base_body, "pelvis", upright_min=FALL_UPRIGHTNESS, - height_min=FALL_PELVIS_HEIGHT) - # RoboCasa updates its fixtures once per control step (its timers count - # update_state() calls), so the loop updates them and reads the task's - # success at env.control_freq (20 Hz) of sim time, not every physics step. - task_every = max(1, round(1.0 / (env.control_freq * model.opt.timestep))) - steps = 0 - max_steps = round(duration / model.opt.timestep) if duration > 0 else None - done = _success = False - _resets = 0 - pacer = RealtimePacer(model.opt.timestep, sim_rate, max_lag=MAX_LAG) - # The real-time factor log's sim time, wall time and time slept since its last line. - log_sim, log_wall, slept = float(data.time), time.time(), 0.0 - - executor_thread = threading.Thread(target=executor.spin, daemon=True, name="ros_bridge_exec") - executor_thread.start() - print("[g1_mujoco] ROS bridges up: rt/lowstate, rt/lowcmd, " - "rt/dex3/{left,right}/{cmd,state}, /clock, " - + ("/livox/lidar, /livox/imu, " if sensors.lidar else "") - + ("/realsense/head/*, " if sensors.camera else "") + "/robocasa/*, /elastic_band/toggle") - try: - while max_steps is None or steps < max_steps: - if viewer and not handle.is_running(): - break - with physics_lock: - if view_sync is not None: - view_sync.apply_perturbation() - apply_band(model, data, band, band_body, band_vel) - sim_interface.write_ctrl() - _before = float(data.time) - mujoco.mj_step(model, data) - # MuJoCo's auto-reset clears the warning counters and then counts - # the one warning, so they are cleared here to see every reset; - # the reset also puts the sim time back to 0. - if any(data.warning[w].number for w in UNSTABLE) or data.time < _before: - for w in UNSTABLE: - data.warning[w].number = 0 - _resets += 1 - sim_interface.physics_reset() - print(f"[g1_mujoco] *** WARNING: the physics went unstable and MuJoCo reset the " - f"whole scene, robot and kitchen, to its initial state and the sim time to 0 " - f"(reset {_resets} at step {steps}); restart the sim ***", flush=True) - sim_interface.after_step() - fall_logger.step(data, band) - ros_bridge.tick() - steps += 1 - if steps % task_every == 0: - # The fixtures' update writes data, which the viewer's sync thread copies - # under physics_lock. - with physics_lock: - env.update_state() # before _check_success - done = measurement.tick() - if done and not _success: - print("[g1_mujoco] task success (debounced).") - _success = done - slept += pacer.wait() - if data.time < log_sim: # MuJoCo reset the time - log_sim, log_wall, slept = float(data.time), time.time(), 0.0 - elif data.time - log_sim >= RTF_LOG_S: - _wall = time.time() - log_wall - _stats = ros_bridge.stats() - print(f"[g1_mujoco] sim t {data.time:.0f} s: real-time factor {(data.time - log_sim) / _wall:.2f} " - f"over the last {data.time - log_sim:.0f} s, the loop idle {slept / _wall:.0%} of the time" - + (f"; {_stats}" if _stats else ""), flush=True) - log_sim, log_wall, slept = float(data.time), time.time(), 0.0 - finally: - close_viewer(handle, view_sync) - executor.shutdown() - ros_bridge.shutdown() - for node in (measurement, ros_bridge): - node.destroy_node() - sim_interface.shutdown() - shutdown_ros() - - -def main(argv): - """Run the G1 sim with the command-line arguments argv (without the program name).""" - parser = argparse.ArgumentParser(description="Run the G1 RoboCasa MuJoCo sim") - sim_common.add_common_args( - parser, - task_help="RoboCasa env name (e.g. TurnOnToasterOven), Battery for the ARPA " - "battery workcell (CL_Assets/mujoco_assets/battery), or Blank for the robot " - "alone on a floor (no --layout or --style). " - "Omit to launch a random registered kitchen task.", - ) - args = sim_common.parse_args(parser, argv, "g1") - task = sim_common.select_task(parser, args, tag="g1_mujoco", - own_tasks=battery_env.TASKS + blank_env.TASKS, no_kitchen=blank_env.TASKS) - # The camera and the lidar's rays render through EGL on the GPU: stop here - # without one, unless GOLEM_RENDER=cpu renders on the CPU. - sim_common.require_renderer() - - sim_loop(task, viewer=not args.headless, - layout=args.layout, style=args.style, seed=args.seed, duration=args.duration) diff --git a/golem_robocasa/g1/g1_robosuite.py b/golem_robocasa/g1/g1_robosuite.py index ea59b1b0..e061c695 100644 --- a/golem_robocasa/g1/g1_robosuite.py +++ b/golem_robocasa/g1/g1_robosuite.py @@ -323,7 +323,7 @@ def place(env, pos, quat): # of the pelvis at counter-top height: 8 of 12 kitchens tried needed the pelvis # 0.27 m further back than the anchor to clear the counter, so it stands 0.30 m # back. Pelvis z 0.80 keeps the soles ~1.5 cm clear at the stance until -# g1_mujoco.spawn re-places the robot. +# g1_robot.spawn re-places the robot. POS_OFFSET = (0.0, -0.30, 0.80) diff --git a/golem_robocasa/g1/g1_robot.py b/golem_robocasa/g1/g1_robot.py new file mode 100644 index 00000000..646e3419 --- /dev/null +++ b/golem_robocasa/g1/g1_robot.py @@ -0,0 +1,217 @@ +"""Define the G1 for golem_sim.sim_loop: its spawn, band, sensors and Dex3-1 hands' DDS interface.""" +import contextlib +import os +import threading + +import numpy as np + +from . import g1_robosuite # registers G1Dex3 and its gripper +import battery_env # registers the Battery env +import blank_env # registers the Blank env +import robosuite_common +from mujoco_env import ElasticBand, FallLogger +from .g1_sensors import G1Sensors, SensorConfig +from .sim_names import BODY_JOINTS, NameResolver +from .unitree_interface import SimInterface + +# The fall logger's test: the pelvis tilted more than 60 deg from upright (as +# golem_lowerbody_rl's policies/homie/mujoco_deploy_g1.py tests it), or down on +# the floor. +FALL_UPRIGHTNESS = 0.5 # pelvis z-axis . world up +FALL_PELVIS_HEIGHT = 0.2 # m + +# The elastic band pulls the torso toward a point BAND_ANCHOR_DZ above its +# spawn and holds the robot while no motor drives it, until /elastic_band/toggle +# or SPACE in the viewer releases it. It is twice as stiff as the H1-2's: at +# 1e4 N/m the limp G1 (34.4 kg) rocks fore and aft on its legs, the pelvis +# tilting 16-19 deg for 20 s and more, and the physics can go unstable; at +# 2e4 N/m it settles within 4 s, upright to 0.3 deg, the torso ~1.4 cm below +# the anchor (measured in three RoboCasa kitchens; tests/robocasa/g1/ +# test_g1_band.py checks one). +BAND_BODY = "torso_link" +BAND_ANCHOR_DZ = 0.01 # m +BAND_STIFFNESS = 2e4 # N/m +BAND_DAMPING = 2e3 # N*s/m +# m: the spawn puts the lowest collision geom this far above the floor, so the +# robot hangs from the band with its feet on the floor. +SPAWN_CLEARANCE = 0.001 +# m of free space the spawn keeps from every robot collision geom to any non-floor geom, in +# the spawn pose, the limp poses and along the start-up arm moves (GOLEM_SPAWN_MARGIN +# overrides it). +SPAWN_MARGIN = 0.05 +# Slots in sim_names.BODY_JOINTS: 12-14 the waist (yaw, roll, pitch), 15-21 the left arm, +# 22-28 the right, each arm shoulder pitch/roll/yaw, elbow, wrist roll/pitch/yaw. +_WAIST_PITCH, _ARMS = 14, (15, 22) +# The poses (rad) the unpowered arms pass through under the band, as (shoulder pitch, +# shoulder yaw outward, elbow, wrist pitch): where they come to rest, and the far end of +# their first swing. Measured over 30 s limp in the Blank task and in a kitchen (the same +# to the milliradian): the forearms drop from pointing forward, the elbows swinging to +# 1.95 rad and resting at 1.31, the shoulders pitching to -0.37, the wrists to -0.59; the +# waist stays within 0.08 rad. +LIMP_ARM_POSES = ((-0.06, 0.07, 1.31, -0.10), (-0.37, 0.07, 1.95, -0.59)) +LIMP_WAIST_PITCH = (0.08, -0.07) # rad, with each arm pose above +# The arm postures (rad, left arm then right) golem_upper_ik's frame_task_server moves through +# at start, while the band still holds the robot at its spawn: golem_upper_ik/config/g1/ +# robot.yaml's startup_sequence and named_configs, copied because the sim reads no controller +# config (tests/docker/test_sim_startup_poses.py keeps them equal). +STARTUP_ARM_STAGES = ( + ("home", (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)), +) + + +# s, Python's GIL switch interval while the sim runs (default 5 ms). The physics +# loop sleeps between steps to keep its pace and needs the GIL back to wake; +# at 5 ms the sensor threads' Python work (the head camera's messages) stalled +# rt/lowstate 5-9 ms, where the real G1 sends it every 2 ms. At 0.5 ms the sim +# with both sensors had no gap over 5 ms in 51 s of sim time. +GIL_SWITCH_S = 0.0005 + + +def _limp_upper_body_poses(motor_qpos, spawn): + """Return the poses the unpowered upper body sags into, as (qpos indices, values) pairs. + + The spawn clearance must hold in them too, since every motor is off until the first + command (LIMP_ARM_POSES). + """ + poses = [] + for (pitch, yaw, elbow, wrist), waist in zip(LIMP_ARM_POSES, LIMP_WAIST_PITCH): + q = np.array(spawn, dtype=float) + q[_WAIST_PITCH] = waist + for base, side in zip(_ARMS, (1.0, -1.0)): # the right arm's yaw mirrors the left's + q[base + 0] = pitch + q[base + 2] = side * yaw + q[base + 3] = elbow + q[base + 5] = wrist + poses.append((motor_qpos, q)) + return poses + + +def spawn(env, resolver, backoff=0.0, margin=SPAWN_MARGIN): + """Put the robot at rest in g1_robosuite.STANCE, hands open, clear of the fixtures by margin, in the sagged poses too. + + Call after env.reset(), whose zero-action settling folds the limp robot + (and whose initialization noise perturbs every joint). The robot is placed + at RoboCasa's spawn, its lowest collision geom SPAWN_CLEARANCE above the + floor, and backed off (its -x), shifting sideways where that is not enough + (robosuite_common.place_robot_collision_free), until every robot collision + geom is margin (m) from any non-floor geom, in the spawn pose, the poses the + unpowered upper body sags into and along the controller's start-up arm moves + (STARTUP_ARM_STAGES); backoff (m) then backs it up to that much further into + open floor. Zeroes every velocity in the scene. + """ + data = env.sim.data._data + stance = np.array([g1_robosuite.STANCE.get(n, 0.0) for n in BODY_JOINTS], dtype=float) + data.qpos[resolver.body.qpos] = stance + for hand in resolver.hands.values(): + data.qpos[hand.qpos] = 0.0 + data.qvel[:] = 0.0 + g1_robosuite.place_robot_collision_free( + env, env.init_robot_base_pos, + robosuite_common.euler_to_wxyz(getattr(env, "init_robot_base_ori", None)), + clearance=SPAWN_CLEARANCE, extra_backoff=backoff, margin=margin, + poses=(_limp_upper_body_poses(resolver.body.qpos, stance) + + robosuite_common.startup_arm_poses(resolver.body.qpos, stance, STARTUP_ARM_STAGES, + _ARMS[0])), + ) + + +def make_band(data, resolver): + """Return (band, torso body id): the band on the torso, anchored BAND_ANCHOR_DZ above it now. + + Call it at the spawn. + """ + body = resolver.body_id(BAND_BODY) + if body < 0: + raise KeyError(f"no body {resolver.robot_prefix}{BAND_BODY} for the elastic band") + anchor = data.xpos[body].copy() + anchor[2] += BAND_ANCHOR_DZ + return ElasticBand(anchor, length=0, stiffness=BAND_STIFFNESS, damping=BAND_DAMPING), body + + +class G1Robot: + """The G1 with its Dex3-1 hands, in Unitree's debug mode, as golem_sim.SimRobot.""" + + name = "g1" + label = "the G1" + own_tasks = battery_env.TASKS + blank_env.TASKS + no_kitchen = blank_env.TASKS + gil_switch_s = GIL_SWITCH_S + + def __init__(self): + """Read the sensor knobs, so a bad one fails before the slow env build.""" + self.sensors = SensorConfig.from_env() + + def make_env(self, task, layout, style, seed): + """Return the G1's robosuite env for task, not yet reset.""" + return g1_robosuite.make_kitchen_env(task, layout_ids=layout, style_ids=style, seed=seed) + + def make_resolver(self, env): + """Return the G1's sim_names.NameResolver.""" + return NameResolver(env.sim.model._model, robot_prefix=env.robots[0].robot_model.naming_prefix) + + def spawn(self, env, resolver): + """Spawn the G1 (spawn above) with the spawn knobs. + + GOLEM_SPAWN_BACKOFF (m, default 0: at the counter) backs the robot further into open + floor; Nav2 needs a free start cell to plan (~1.0). GOLEM_SPAWN_MARGIN (m, unset: + SPAWN_MARGIN) is the clearance to the fixtures. + """ + margin = os.environ.get("GOLEM_SPAWN_MARGIN", "").strip() + spawn(env, resolver, backoff=float(os.environ.get("GOLEM_SPAWN_BACKOFF", "0") or 0), + margin=float(margin) if margin else SPAWN_MARGIN) + print("[g1_robot] initial stance: legs at Unitree's default angles, everything else at 0") + + def make_band(self, data, resolver): + """Return (band, torso body id): the band on the torso, anchored BAND_ANCHOR_DZ above it now.""" + return make_band(data, resolver) + + def odom_body(self, env, resolver): + """Return the pelvis.""" + return resolver.base_body + + def make_interface(self, model, data, resolver): + """Return the G1's Unitree DDS interface: rt/lowstate, rt/lowcmd and rt/dex3/{left,right}/*.""" + return SimInterface(model, data, resolver) + + def make_lock(self, viewer): + """Return (the viewer's lock or a null context, None). + + The physics loop is the only writer of MjData: the sensor threads work on copies of + the state tick() takes, and the ROS and DDS callbacks only latch values the loop + reads, so the sensors take no lock. The one other reader is the viewer's sync thread, + which copies the state under the physics lock (a null context headless, where nothing + else reads). + """ + return (threading.Lock() if viewer else contextlib.nullcontext()), None + + def make_sensors(self, node, env, model, data, lock): + """Return the G1's sensor suite: the MID-360 and the head D435, as SensorConfig switches them.""" + print(f"[g1_robot] sensors: {self.sensors.describe()}") + return G1Sensors(node, model, data, prefix=env.robots[0].robot_model.naming_prefix, + config=self.sensors) + + def start_extras(self, env, model, data, lock): + """Start nothing: the G1's hands are on its DDS interface, and it has no estop box.""" + return [], () + + def tick_extras(self, sim_t): + """Publish nothing.""" + + def stop_extras(self): + """Stop nothing.""" + + def make_fall_logger(self, resolver, band_body): + """Return the logger of the band's release and the first time the pelvis tips or drops.""" + return FallLogger(resolver.base_body, "pelvis", upright_min=FALL_UPRIGHTNESS, + height_min=FALL_PELVIS_HEIGHT) + + def describe(self, model, resolver): + """Return how many body and hand motors the resolver mapped.""" + return (f"resolver mapped {len(resolver.body.names)} body " + f"+ {sum(len(h.names) for h in resolver.hands.values())} hand motors") + + def surface(self): + """Return the G1 sim's topics, with the sensors that are on.""" + return ("rt/lowstate, rt/lowcmd, rt/dex3/{left,right}/{cmd,state}, /clock" + + (", /livox/lidar, /livox/imu" if self.sensors.lidar else "") + + (", /realsense/head/*" if self.sensors.camera else "")) diff --git a/golem_robocasa/g1/g1_sensors.py b/golem_robocasa/g1/g1_sensors.py index fac9493a..4829af51 100644 --- a/golem_robocasa/g1/g1_sensors.py +++ b/golem_robocasa/g1/g1_sensors.py @@ -58,7 +58,7 @@ renderer's scene (mjv_updateScene, which reads MjData, on the CPU); the thread owns the renderer's GL context (it is thread-affine) and renders that scene, with the GIL released, so the render, far longer than the copy, does - not hold up the physics or rt/lowstate (and g1_mujoco's GIL_SWITCH_S keeps + not hold up the physics or rt/lowstate (and g1_robot's GIL_SWITCH_S keeps the thread's Python work from doing so). It renders under mid360.GL_LOCK, taking turns with the GPU caster (g1_head_camera.py). The scene is handed over, not copied: a frame that comes due while the thread still renders the last is dropped (and diff --git a/golem_robocasa/g1/unitree_interface.py b/golem_robocasa/g1/unitree_interface.py index 7a0b3b2c..8fd08651 100644 --- a/golem_robocasa/g1/unitree_interface.py +++ b/golem_robocasa/g1/unitree_interface.py @@ -12,7 +12,7 @@ frames are dropped. From then on rt/lowcmd drives every body motor, and motor mode 0 turns one off. A hand's motors are off until its first rt/dex3//cmd with finite values on its 7 motors; other hand frames are dropped too. Until a -command drives it the robot hangs from the sim's elastic band (g1_mujoco), as +command drives it the robot hangs from the sim's elastic band (g1_robot), as the H1-2 does in its sim. Every torque is recomputed every physics step from the latched commands and the @@ -131,17 +131,6 @@ def _on_hand_cmd(self, side, msg: HandCmd_): return self._hand_cmd[side] = cmd - def physics_reset(self): - """Restamp the latched rt/lowcmd at the sim time after MuJoCo reset the scene. - - The timeout then still measures the silence since the last frame - instead of waiting for the old sim time to come round again. - """ - now = float(self.data.time) - self.sim_time = now - if self._lowcmd is not None: - self._lowcmd = self._lowcmd._replace(time=now) - def write_ctrl(self): """Write every motor's torque for the next physics step, limited to its actuatorfrcrange.""" d = self.data diff --git a/golem_robocasa/golem_sim.py b/golem_robocasa/golem_sim.py new file mode 100644 index 00000000..e9cdbeba --- /dev/null +++ b/golem_robocasa/golem_sim.py @@ -0,0 +1,297 @@ +"""Run the RoboCasa MuJoCo sim of the H1-2, or of the G1 with --g1 or GOLEM_ROBOT=g1. + +Usage: golem_sim.py [--h12 | --g1] [--headless] [--task NAME] [--layout N] +[--style N] [--seed N]. One loop (sim_loop) runs both robots; what differs between them is +a robot object with one interface (SimRobot below): h12_robot.H12Robot or +g1.g1_robot.G1Robot. The robot is chosen (sim_common.select_robot) and the command line +parsed before either robot's module is imported: the H1-2's needs magpie_msgs and the +H1-2's CL_Assets files, which a G1 container may not have. The renderer is checked next +(sim_common.require_renderer: the sim stops there without a GPU, unless GOLEM_RENDER=cpu), +so --help and a usage error need neither. +""" +import argparse +import sys +import threading +import time +from typing import Protocol + +import sim_common + +# The warnings on which MuJoCo resets the whole scene (robot and kitchen), by mjtWarning name. +UNSTABLE = ("mjWARN_BADQPOS", "mjWARN_BADQVEL", "mjWARN_BADQACC") +# s of sim time between the loop's real-time factor logs. +RTF_LOG_S = 30.0 +TAG = "golem_sim" + + +class SimRobot(Protocol): + """What sim_loop needs of a robot; h12_robot.H12Robot and g1.g1_robot.G1Robot provide it.""" + + name: str # 'h12' or 'g1' + label: str # the robot's name in log lines + gil_switch_s: float | None # s, Python's GIL switch interval while the sim runs; None keeps Python's + + def make_env(self, task, layout, style, seed): + """Return the robot's robosuite env for task, not yet reset.""" + + def make_resolver(self, env): + """Return the robot's name resolver for the reset env's model.""" + + def spawn(self, env, resolver): + """Put the robot at rest at its spawn, clear of the scene; reads the robot's spawn knobs.""" + + def make_band(self, data, resolver): + """Return (ElasticBand, body id) of the band that holds the robot at its spawn.""" + + def odom_body(self, env, resolver): + """Return the id of the body the opt-in ground-truth odom reports.""" + + def make_interface(self, model, data, resolver): + """Return the robot's Unitree DDS interface (write_ctrl, after_step, physics_reset, shutdown).""" + + def make_lock(self, viewer): + """Return (physics lock, sensor lock): the context the loop steps under, and the lock the robot's sensors and extras take (None: none).""" + + def make_sensors(self, node, env, model, data, lock): + """Return the robot's sensor suite (mujoco_ros_bridge.SensorSuite) on node.""" + + def start_extras(self, env, model, data, lock): + """Start what only this robot serves; return (ROS nodes for the executor, viewer key callbacks).""" + + def tick_extras(self, sim_t): + """Publish the extras' state due at sim time sim_t (s).""" + + def stop_extras(self): + """Stop the extras; their nodes are destroyed by the loop afterwards.""" + + def make_fall_logger(self, resolver, band_body): + """Return the robot's mujoco_env.FallLogger.""" + + def describe(self, model, resolver): + """Return the log text naming what the resolver mapped.""" + + def surface(self): + """Return the topics the sim serves, for the 'ROS bridges up' line.""" + + +def _parser() -> argparse.ArgumentParser: + """Return the runner's argument parser.""" + parser = argparse.ArgumentParser(description="Run the RoboCasa MuJoCo sim of the H1-2 or the G1") + sim_common.add_common_args( + parser, + task_help="RoboCasa env name (e.g. TurnOnToasterOven), Battery for the " + "battery workcell (CL_Assets/mujoco_assets/battery), or Blank for " + "the robot alone on a floor (no --layout or --style). " + "Omit to launch a random registered kitchen task.", + ) + return parser + + +def make_robot(name): + """Import and return the robot object for name ('h12' or 'g1'). + + Only the chosen robot's modules are imported. + """ + if name == "g1": + from g1.g1_robot import G1Robot + return G1Robot() + from h12_robot import H12Robot + return H12Robot() + + +def sim_loop(robot, task, viewer=True, layout=None, style=None, seed=None, duration=0.0): + """Run robot (a SimRobot) in task (a RoboCasa kitchen, Battery or Blank) until stopped. + + The loop steps the env's own MjData (never env.step()) and drives: + - the robot's SimInterface : Unitree DDS rt/lowstate and rt/lowcmd (and what else the + robot speaks); every motor off until the first valid rt/lowcmd + - RosSensorBridge : /clock, the robot's sensor suite, and ground-truth /odom + only with GOLEM_SIM_ODOM=1 + - MeasurementBridge : /robocasa/*, /elastic_band/toggle + - the robot's extras : what only it serves (the H1-2's grippers and estop box) + An elastic band on the torso holds the robot until /elastic_band/toggle or SPACE in the + viewer releases it. The loop paces to GOLEM_SIM_RATE (sim_common.sim_rate_from_env; + default real time) and logs its real-time factor every RTF_LOG_S of sim time. duration is + sim seconds from the first step, counted in physics steps (0: until the viewer closes or + Ctrl-C); the tests pass it, the command line does not. + """ + import mujoco + import numpy as np + import rclpy.executors + + from measurement_bridge import MeasurementBridge + from mujoco_env import apply_band + from mujoco_ros_bridge import RosSensorBridge, init_ros, shutdown_ros + from sim_timing import RealtimePacer + from viewer_sync import close_viewer, open_viewer + + # A bad GOLEM_SIM_RATE stops the sim before the kitchen is built. + sim_rate = sim_common.sim_rate_from_env() + env = robot.make_env(task, layout, style, seed) + + # robosuite.make() does not reset; reset() builds env.sim, runs the robot's placement + # patch and places the task objects. + env.reset() + # The fixtures' own parts stop colliding with each other and with the static scene; the + # robot and the objects collide with everything as before. + sim_common.exclude_fixture_collisions(env) + + model = env.sim.model._model + data = env.sim.data._data + resolver = robot.make_resolver(env) + + try: + robot.spawn(env, resolver) + except Exception as e: + print(f"[{TAG}] clean reset skipped: {e}") + + # The band holds the robot from the first physics step; every motor is off until the + # first valid rt/lowcmd. + band, band_body = robot.make_band(data, resolver) + band_vel = np.zeros(6) # apply_band's scratch + print(f"[{TAG}] elastic band on the torso anchored at {band.point.round(3)}; " + "/elastic_band/toggle or SPACE in the viewer releases it") + + print(f"[{TAG}] RoboCasa task {task!r} built around {robot.label}: " + f"nq={model.nq} nu={model.nu}; {robot.describe(model, resolver)}") + + # Ground-truth odom (odom -> pelvis on /tf, /odom), only with GOLEM_SIM_ODOM: the real + # robot has none, and it would compete with FAST-LIO's odometry. + sim_odom = sim_common.env_flag('GOLEM_SIM_ODOM', False) + odom_body = robot.odom_body(env, resolver) if sim_odom else -1 + print(f"[{TAG}] ground-truth sim odom {'ON (GOLEM_SIM_ODOM=1)' if sim_odom else 'OFF'}") + + sim_interface = robot.make_interface(model, data, resolver) + # For the closed-loop tests that record the sim from inside its process. + sim_interface.band = band + + if robot.gil_switch_s is not None: + sys.setswitchinterval(robot.gil_switch_s) + init_ros() + + # physics_lock is held around every write of MjData by this loop; sensor_lock is what + # the robot's sensors and extras take to read or write it from other threads (None when + # they only work on copies the loop hands them). Each robot says which it needs. + physics_lock, sensor_lock = robot.make_lock(viewer) + ros_bridge = RosSensorBridge( + model, data, + lambda node: robot.make_sensors(node, env, model, data, sensor_lock), + base_body_id=odom_body, + sim_lock=sensor_lock, + ) + print(f"[{TAG}] {sim_common.describe_sim_rate(sim_rate)}", flush=True) + + extra_nodes, key_callbacks = robot.start_extras(env, model, data, sensor_lock) + + measurement = MeasurementBridge(env, elastic_band=band, task_name=task) + print(f"[{TAG}] task goal: {measurement.publish_goal()!r}") + + # The executor thread serves the band toggle and the robot's extras. ros_bridge is not + # on it: the loop calls its tick() between steps (a renderer's GL context is + # thread-affine). The thread starts just before the loop's try, whose finally shuts it down. + executor = rclpy.executors.MultiThreadedExecutor() + nodes = [measurement, *extra_nodes] + for node in nodes: + executor.add_node(node) + + # The viewer draws its own MjData, refreshed from data by the viewer thread, with the + # band while it is enabled; its mouse perturbation forces come back through + # view_sync.apply_perturbation(). + handle = view_sync = None + if viewer: + handle, view_sync = open_viewer(model, data, physics_lock, band, band_body, TAG, + sim_rate=sim_rate, key_callbacks=key_callbacks) + # Logs the band's release and the robot's first fall. + fall_logger = robot.make_fall_logger(resolver, band_body) + # RoboCasa updates its fixtures and checks success once per control step (20 Hz) of sim + # time, not per physics step: several fixtures count update_state() calls (toaster heat, + # scrub contact), and the success debounce counts control steps. + task_every = max(1, round(env.control_timestep / model.opt.timestep)) + steps = 0 + max_steps = round(duration / model.opt.timestep) if duration > 0 else None + done = success = False + resets = 0 + unstable = [getattr(mujoco.mjtWarning, name) for name in UNSTABLE] + pacer = RealtimePacer(model.opt.timestep, sim_rate) + # The real-time factor log's sim time, wall time and time slept since its last line. + log_sim, log_wall, slept = float(data.time), time.time(), 0.0 + + executor_thread = threading.Thread(target=executor.spin, daemon=True, name="ros_bridge_exec") + executor_thread.start() + print(f"[{TAG}] ROS bridges up: {robot.surface()}, /robocasa/*, /elastic_band/toggle") + try: + while max_steps is None or steps < max_steps: + if viewer and not handle.is_running(): + break + with physics_lock: + if view_sync is not None: + view_sync.apply_perturbation() + apply_band(model, data, band, band_body, band_vel) + sim_interface.write_ctrl() + before = float(data.time) + mujoco.mj_step(model, data) + # MuJoCo's auto-reset clears the warning counters and then counts the one + # warning, so they are cleared here to see every reset; the reset also puts + # the sim time back to 0. + if any(data.warning[w].number for w in unstable) or data.time < before: + for w in unstable: + data.warning[w].number = 0 + resets += 1 + sim_interface.physics_reset() + print(f"[{TAG}] *** WARNING: the physics went unstable and MuJoCo reset the " + f"whole scene, robot and kitchen, to its initial state and the sim time to 0 " + f"(reset {resets} at step {steps}); restart the sim ***", flush=True) + sim_interface.after_step() + fall_logger.step(data, band) + # tick() takes the sensors' lock only to capture their state, so encoding and + # publishing do not hold up physics. No physics step runs inside it. + ros_bridge.tick() + robot.tick_extras(float(data.time)) + steps += 1 + if steps % task_every == 0: + # The fixtures' update writes data, which the other readers copy under the lock. + with physics_lock: + env.update_state() # before _check_success + done = measurement.tick() + if done and not success: + print(f"[{TAG}] task success (debounced).") + success = done + slept += pacer.wait() + if data.time < log_sim: # MuJoCo reset the time + log_sim, log_wall, slept = float(data.time), time.time(), 0.0 + elif data.time - log_sim >= RTF_LOG_S: + wall = time.time() - log_wall + stats = ros_bridge.stats() + print(f"[{TAG}] sim t {data.time:.0f} s: real-time factor {(data.time - log_sim) / wall:.2f} " + f"over the last {data.time - log_sim:.0f} s, the loop idle {slept / wall:.0%} of the time" + + (f"; {stats}" if stats else ""), flush=True) + log_sim, log_wall, slept = float(data.time), time.time(), 0.0 + finally: + # The extras first: a DeliGrasp settle waits on the sim clock, which no longer + # advances, and must end before the executor's worker thread can exit. + robot.stop_extras() + close_viewer(handle, view_sync) + executor.shutdown() + ros_bridge.shutdown() + for node in (*nodes, ros_bridge): + node.destroy_node() + sim_interface.shutdown() + shutdown_ros() + + +def main(argv): + """Run the sim with the command-line arguments argv (without the program name).""" + name = sim_common.select_robot(argv) + parser = _parser() + args = sim_common.parse_args(parser, argv, name) + # The cameras and the lidar's rays render through EGL on the GPU: stop here without + # one, unless GOLEM_RENDER=cpu renders on the CPU. + sim_common.require_renderer() + robot = make_robot(name) + task = sim_common.select_task(parser, args, tag=TAG, own_tasks=robot.own_tasks, + no_kitchen=robot.no_kitchen) + sim_loop(robot, task, viewer=not args.headless, layout=args.layout, style=args.style, seed=args.seed) + + +if __name__ == "__main__": + main(sys.argv[1:]) diff --git a/golem_robocasa/h12_mujoco.py b/golem_robocasa/h12_mujoco.py deleted file mode 100644 index dc81e617..00000000 --- a/golem_robocasa/h12_mujoco.py +++ /dev/null @@ -1,440 +0,0 @@ -"""Run the RoboCasa MuJoCo sim of the H1-2, or of the G1 with --g1 or GOLEM_ROBOT=g1. - -Usage: h12_mujoco.py [--h12 | --g1] [--headless] [--task NAME] [--layout N] -[--style N] [--seed N] [--timing | --duration S]. The robot is chosen -(sim_common.select_robot) before the H1-2's imports, and --g1 hands the whole -command line to g1.g1_mujoco.main. Each runner parses its command line, then -checks the GPU (sim_common.require_renderer: either sim stops there without one, -unless GOLEM_RENDER=cpu renders on the CPU), so --help and a usage error need no -GPU; the H1-2 does both before its imports. The -H1-2's loop is sim_loop below. -""" -import argparse -import sys - -import sim_common - - -def _parser() -> argparse.ArgumentParser: - """Return the H1-2 runner's argument parser.""" - parser = argparse.ArgumentParser(description="Run the H1-2 RoboCasa MuJoCo sim") - sim_common.add_common_args( - parser, - task_help="RoboCasa env name (e.g. TurnOnToasterOven), Battery for the " - "battery workcell (CL_Assets/mujoco_assets/battery), or Blank for " - "the robot alone on a floor (no --layout or --style). " - "Omit to launch a random registered kitchen task.", - ) - return parser - - -# The G1 is dispatched before the H1-2 imports below, which need magpie_msgs and -# the H1-2's CL_Assets files: the G1 reads only CL_Assets/g1, and its test -# containers do not provide magpie_msgs. The H1-2 parses its command line and -# checks the GPU (its cameras and the lidar's rays render through EGL on it) first. -if __name__ == "__main__": - _robot = sim_common.select_robot(sys.argv[1:]) - if _robot == "g1": - from g1 import g1_mujoco - sys.exit(g1_mujoco.main(sys.argv[1:])) - _PARSER = _parser() - _ARGS = sim_common.parse_args(_PARSER, sys.argv[1:], "h12") - sim_common.require_renderer() - -import os -import threading - -import mujoco -import numpy as np -import rclpy.executors - -from mujoco_ros_bridge import init_ros, shutdown_ros - -import h1_2_robosuite # registers H1_2 and the MAGPIE grippers -import mid360 -import realsense_profile -import robosuite_common -import battery_env # registers the Battery env -import blank_env # registers the Blank env -from h12_sensors import H12Sensors -from magpie_hand_bridge import MagpieHandBridge -from measurement_bridge import MeasurementBridge -from mujoco_env import ElasticBand, FallLogger, apply_band -from mujoco_ros_bridge import RosSensorBridge -from estop_server import start_estop_server -from sim_names import NameResolver -from sim_timing import RealtimePacer, LoopTimings -from unitree_interface import SimInterface -from viewer_sync import close_viewer, open_viewer - -NUM_MOTORS = 27 -# The spawn pose (rad) in sim_names.ROS_MOTOR_ORDER: bent-knee legs, then the -# torso and both arms at 0. -INIT_LEG_POS = np.array([ - 0.0, -0.16, 0.0, 0.36, -0.2, 0.0, - 0.0, -0.16, 0.0, 0.36, -0.2, 0.0, -], dtype=float) -INIT_ARM_POS = np.array([ - 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -], dtype=float) - - -def _initial_motor_qpos(): - """Return the 27-motor spawn pose (rad), without reading any controller config.""" - qpos = np.concatenate([INIT_LEG_POS, INIT_ARM_POS]) - if qpos.shape != (NUM_MOTORS,): - raise ValueError(f"initial motor pose must contain {NUM_MOTORS} entries") - return qpos - - -# Slots in ROS_MOTOR_ORDER: 12 torso, 13-19 left arm, 20-26 right arm, each arm -# shoulder pitch/roll/yaw, elbow, wrist roll/pitch/yaw. -_TORSO, _ARMS = 12, (13, 20) - - -def _limp_upper_body_poses(model, motor_qpos, spawn): - """Return the poses the unpowered upper body sags into, as (qpos indices, values) pairs. - - The spawn clearance must hold in them too, since every motor is off until - the first command. Measured in RoboCasa: the torso twists up to ~0.7 rad - either way, the shoulders pitch back ~0.25 rad, the elbows bend ~0.45 rad - and the wrists drop onto their pitch stops, swinging the grippers forward. - """ - poses = [] - for twist in (0.75, -0.75): - q = np.array(spawn, dtype=float) - q[_TORSO] = twist - for base in _ARMS: - q[base + 0] = 0.25 # shoulder pitch (rad, + is backward) - q[base + 2] = -0.2 # shoulder yaw - q[base + 3] = 0.45 # elbow - wrist_pitch = int(np.flatnonzero(model.jnt_qposadr == motor_qpos[base + 5])[0]) - q[base + 5] = model.jnt_range[wrist_pitch, 0] - poses.append((motor_qpos, q)) - return poses - - -# The arm postures (rad, left arm then right, each shoulder pitch/roll/yaw, elbow, wrist -# roll/pitch/yaw) golem_upper_ik's frame_task_server moves through at start, while the band -# still holds the robot at its spawn: golem_upper_ik/config/h12/robot.yaml's -# startup_sequence and named_configs, copied because the sim reads no controller config -# (tests/docker/test_sim_startup_poses.py keeps them equal). -STARTUP_ARM_STAGES = ( - ("init_1", (0.0, 0.35, 0.0, 1.3, 0.0, 0.0, 0.0, 0.0, -0.35, 0.0, 1.3, 0.0, 0.0, 0.0)), - ("init_2", (0.7, 0.35, 0.0, 1.3, 0.0, 0.0, 0.0, 0.7, -0.35, 0.0, 1.3, 0.0, 0.0, 0.0)), - ("init_3", (0.7, 0.35, 0.0, 0.0, 0.0, 0.0, 0.0, 0.7, -0.35, 0.0, 0.0, 0.0, 0.0, 0.0)), - ("home", (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)), -) -# Points checked along each start-up move, its end included: the arms sweep between stages. -STARTUP_STEPS = 4 - - -def _startup_arm_poses(motor_qpos, spawn): - """Return the poses of the start-up arm moves from the spawn pose, as (qpos indices, values) pairs.""" - poses = [] - prev = np.asarray(spawn, dtype=float) - for _, arms in STARTUP_ARM_STAGES: - target = prev.copy() - target[_ARMS[0]:_ARMS[0] + 14] = arms - for a in np.linspace(0.0, 1.0, STARTUP_STEPS + 1)[1:]: - poses.append((motor_qpos, (1.0 - a) * prev + a * target)) - prev = target - return poses - - -def spawn(env, resolver, backoff=0.0, margin=None, offset=(0.0, 0.0)): - """Put the robot at rest in its spawn pose, clear of the fixtures by margin, in the sagged poses too. - - Call after env.reset(), whose zero-action settling folds the limp robot. - The robot is placed at RoboCasa's spawn shifted by offset ((dx, dy) m, - world), its lowest geom 2 cm above the floor, and backed off (its -x) until - every robot geom is margin (m; None: the env's spawn_margin, else 0.20) - from any non-floor geom, in the spawn pose, the poses the unpowered upper - body sags into and along the controller's start-up arm moves - (STARTUP_ARM_STAGES); where backing straight off does not reach it, the - search shifts sideways too (robosuite_common.place_robot_collision_free). - backoff (m) then backs it up to that much further into open floor. Zeroes - every velocity in the scene. - """ - data = env.sim.data._data - init_qpos = _initial_motor_qpos() - data.qpos[resolver.motor_qpos] = init_qpos - data.qvel[:] = 0.0 - pos = list(env.init_robot_base_pos) - pos[0] += offset[0] - pos[1] += offset[1] - h1_2_robosuite.place_robot_collision_free( - env, pos, robosuite_common.euler_to_wxyz(getattr(env, "init_robot_base_ori", None)), - extra_backoff=backoff, - margin=getattr(env, "spawn_margin", 0.20) if margin is None else margin, - poses=(_limp_upper_body_poses(env.sim.model._model, resolver.motor_qpos, init_qpos) - + _startup_arm_poses(resolver.motor_qpos, init_qpos)), - ) - - -def sim_loop(task, viewer=True, layout=None, style=None, seed=None, timing=False): - """Run the H1-2 in task (a RoboCasa kitchen, Battery or Blank) until the viewer closes or Ctrl-C. - - The loop steps the env's own MjData (never env.step()) and drives: - - SimInterface : Unitree DDS rt/lowstate + rt/lowcmd (27 body motors) - - RosSensorBridge : /clock, opt-in odom, and H12Sensors: RGBD cameras, - MID-360 lidar + IMU - - MagpieHandBridge : /{left,right}/gripper/* - - MeasurementBridge: /robocasa/*, /elastic_band/toggle - An elastic band holds the robot until it is released. The loop paces to - GOLEM_SIM_RATE (sim_common.sim_rate_from_env; default real time). timing - enables the --timing stage report. - """ - - # A bad GOLEM_SIM_RATE stops the sim before the kitchen is built. - sim_rate = sim_common.sim_rate_from_env() - - env = h1_2_robosuite.make_kitchen_env(task, layout_ids=layout, style_ids=style, seed=seed) - - # robosuite.make() does not reset; reset() builds env.sim, runs the - # placement patch (h1_2_robosuite) and places the task objects. - env.reset() - # The fixtures' own parts stop colliding with each other and with the static scene; the - # robot and the objects collide with everything as before. - sim_common.exclude_fixture_collisions(env) - - model = env.sim.model._model - data = env.sim.data._data - resolver = NameResolver(model) - - try: - # GOLEM_SPAWN_BACKOFF (m, default 0: at the counter) backs the robot - # further into open floor; Nav2 needs a free start cell to plan (~1.0). - # GOLEM_SPAWN_MARGIN (m, unset: the env's spawn_margin, else 0.20) is - # the clearance to the fixtures. GOLEM_SPAWN_OFFSET="dx,dy" (m, world) - # shifts the task's spawn before placement; unset keeps the task's own. - _spawn_margin = os.environ.get("GOLEM_SPAWN_MARGIN", "").strip() - _offset = os.environ.get("GOLEM_SPAWN_OFFSET", "").strip() - _dx, _dy = (float(v) for v in _offset.split(",")) if _offset else (0.0, 0.0) - if _offset: - print(f"[h12_mujoco] spawn offset ({_dx:+.3f}, {_dy:+.3f}) m") - spawn(env, resolver, backoff=float(os.environ.get("GOLEM_SPAWN_BACKOFF", "0") or 0), - margin=float(_spawn_margin) if _spawn_margin else None, offset=(_dx, _dy)) - print("[h12_mujoco] initial stance from baked-in sim defaults") - except Exception as e: - print(f"[h12_mujoco] clean reset skipped: {e}") - - # The elastic band on the torso holds the robot until SPACE or - # /elastic_band/toggle releases it. - band = None - band_body_id = -1 - band_vel = np.zeros(6) # apply_band's scratch - try: - band_body_id = resolver.body_id("torso_link") - anchor = data.xpos[band_body_id].copy() - # 4 cm below the torso's spawn (m): the feet carry part of the weight. - anchor[2] -= 0.04 - # Stiffness 1e4 N/m holds the limp robot near upright through the ~20 s - # the leg policy waits under the band for the arms' start-up (static - # tilt ~ gravity torque / stiffness; at 1e3 it pitched forward ~60 deg). - # Damping 2e3 N*s/m settles a sideways shove well within a second. - band = ElasticBand(anchor, length=0, stiffness=1e4, damping=2e3) - print(f"[h12_mujoco] elastic band on torso anchored at {anchor.round(3)}") - except Exception as e: - print(f"[h12_mujoco] elastic band setup skipped: {e}") - - print(f"[h12_mujoco] RoboCasa task {task!r} built around H1_2: " - f"nq={model.nq} nu={model.nu}; resolver mapped {len(resolver.motor_ctrl)} motors") - - # Every MjData access off the main loop's thread (sensors, grippers, - # viewer) holds sim_lock, which the main loop holds around mj_step. - sim_lock = threading.Lock() - pfx = env.robots[0].robot_model.naming_prefix - - # Ground-truth odom (odom -> pelvis on /tf, /odom) from the pelvis free - # joint's body, only with GOLEM_SIM_ODOM: the real robot has none, and it - # would compete with FAST-LIO's odometry. - odom_base_body_id = -1 - _sim_odom_on = sim_common.env_flag('GOLEM_SIM_ODOM', False) - if _sim_odom_on: - try: - _fj = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, - f"{pfx}{robosuite_common.FREE_JOINT_NAME}") - odom_base_body_id = int(model.jnt_bodyid[_fj]) - except Exception as e: - print(f"[h12_mujoco] base odom body resolve skipped: {e}") - print(f"[h12_mujoco] ground-truth sim odom {'ON (GOLEM_SIM_ODOM=1)' if _sim_odom_on else 'OFF'}") - - # The gripper actuators are the hand bridges' and disjoint from the 27 body motors. - sim_interface = SimInterface(model, data, resolver=resolver) - # For the closed-loop tests that record the sim from inside its process. - sim_interface.band = band - - init_ros() - - # The cameras publish what the real RealSense drivers do - # (realsense_profile.yaml). Rendering is the heaviest per-step CPU cost: - # GOLEM_CAMERAS=0 drops every camera. GOLEM_CAMERA_SCALE renders the head - # camera at that fraction of its real 1280x720 (same projection, scaled - # camera_info). The hand cameras keep their gripper-MJCF fovy at - # GOLEM_HAND_CAMERA_RES=x (default 256x256; the D405's colour stream - # is 640x480). GOLEM_CAMERA_DEPTH=0 drops depth/image_rect_raw and - # GOLEM_CAMERA_POINTCLOUD=0 drops depth/color/points; with both off the - # depth render is skipped. - _cameras_on = sim_common.env_flag('GOLEM_CAMERAS', True) - _camera_scale = float(os.environ.get('GOLEM_CAMERA_SCALE', '').strip() or 0.5) - _profile = realsense_profile.load_profile() - _camera_names = [ - ("head", f"{pfx}{h1_2_robosuite.HEAD_CAMERA}"), - ("left_hand", "gripper0_left_hand_cam"), - ("right_hand", "gripper0_right_hand_cam"), - ] if _cameras_on else [] - _hand_res = tuple(int(v) for v in - (os.environ.get('GOLEM_HAND_CAMERA_RES', '').strip() or '256x256').lower().split('x')) - _cameras = realsense_profile.camera_specs(model, _camera_names, _profile, _camera_scale, _hand_res) - # GOLEM_LIDAR_POINTS: points per 10 Hz frame, default the real sensor's 20,000. - _lidar_points = mid360.points_from_env() - print(f"[h12_mujoco] MID-360 lidar: {_lidar_points} points a frame") - _depth_on = sim_common.env_flag('GOLEM_CAMERA_DEPTH', True) - _cloud_on = sim_common.env_flag('GOLEM_CAMERA_POINTCLOUD', True) - print(f"[h12_mujoco] RGBD cameras {'ON' if _cameras_on else 'OFF (GOLEM_CAMERAS=0)'}" - + (f": head {_cameras[0].color.width}x{_cameras[0].color.height} " - f"(GOLEM_CAMERA_SCALE={_camera_scale:g}), hands {_hand_res[0]}x{_hand_res[1]}, " - f"depth stream {'on' if _depth_on else 'off'}, " - f"point cloud {'on' if _cloud_on else 'off'}" if _cameras else "")) - timings = LoopTimings(enabled=timing) - ros_bridge = RosSensorBridge( - model, data, - lambda node: H12Sensors( - node, model, data, - cameras=_cameras, - camera_tf=realsense_profile.tf_edges(_profile, [ns for ns, _ in _camera_names]), - camera_tf_rate_hz=float(_profile["tf_rate_hz"]), - depth_stream=_depth_on, - pointcloud=_cloud_on, - # The real MID-360's IMU rate. FAST-LIO propagates the pose with the - # IMU between scans, and at 100 Hz the gait's accelerations alias - # into drift. - imu_rate_hz=200.0, - lidar_body=f"{pfx}livox_link", - lidar_exclude_body=f"{pfx}torso_link", - imu_quat_sensor=f"{pfx}livox_imu_quat", - imu_gyro_sensor=f"{pfx}livox_imu_gyro", - imu_acc_sensor=f"{pfx}livox_imu_acc", - # Drop the robot's own limbs from the scan: their returns map an - # obstacle under the robot and Nav2 refuses the start. - # GOLEM_LIDAR_SELF_FILTER=0 keeps them as returns. - lidar_self_prefixes=( - (pfx, "gripper0_") if sim_common.env_flag("GOLEM_LIDAR_SELF_FILTER", True) else () - ), - lidar_points=_lidar_points, - sim_lock=sim_lock, - timings=timings, - ), - base_body_id=odom_base_body_id, - sim_lock=sim_lock, - ) - - print(f"[h12_mujoco] {sim_common.describe_sim_rate(sim_rate)}", flush=True) - - hand_right = MagpieHandBridge(model, data, side="right", sim_lock=sim_lock, - name_prefix="gripper0_right_") - hand_left = MagpieHandBridge(model, data, side="left", sim_lock=sim_lock, - name_prefix="gripper0_left_") - - measurement = MeasurementBridge(env, elastic_band=band, task_name=task) - print(f"[h12_mujoco] task goal: {measurement.publish_goal()!r}") - - # The executor thread serves the grippers and the band toggle. ros_bridge is - # not on it: the main loop calls its tick(), whose renderer GL context is - # thread-affine. The thread starts just before the loop's try, whose finally - # shuts it down. - executor = rclpy.executors.MultiThreadedExecutor() - for node in (measurement, hand_right, hand_left): - executor.add_node(node) - - # The viewer draws its own MjData, refreshed from data by the viewer thread, - # with the band while it is enabled; its mouse perturbation forces come back - # through view_sync.apply_perturbation(). - # The hardware estop box estop_node reads (estop_server); SAFETY: golem_safety - # latches on its ENTER toggle, and on its silence once the sim stops. - estop = start_estop_server("h12_mujoco") - - handle = view_sync = None - if viewer: - handle, view_sync = open_viewer(model, data, sim_lock, band, band_body_id, "h12_mujoco", - timings=timings, sim_rate=sim_rate, - key_callbacks=(estop.key_callback,)) - # Logs the band's release and the first time the torso tips or drops. - fall_logger = FallLogger(band_body_id, "torso", upright_min=0.5, height_min=0.5) - - # RoboCasa updates fixtures and checks success once per control step - # (20 Hz), not per physics step: several fixtures count calls (toaster - # heat, scrub contact), and the success debounce counts control steps. - task_every = max(1, round(env.control_timestep / model.opt.timestep)) - step_count = 0 - - timings.reset(float(data.time)) - pacer = RealtimePacer(model.opt.timestep, sim_rate) - executor_thread = threading.Thread(target=executor.spin, daemon=True, name="ros_bridge_exec") - executor_thread.start() - print("[h12_mujoco] ROS bridges up: rt/lowstate+rt/lowcmd, /clock, " - "RGBD /realsense/{head,left_hand,right_hand}, livox lidar+imu, " - "/{left,right}/gripper/*, /elastic_band/toggle") - try: - while True: - if viewer and not handle.is_running(): - break - physics_start = timings.start() - with sim_lock: - if view_sync is not None: - view_sync.apply_perturbation() - if band is not None: - apply_band(model, data, band, band_body_id, band_vel) - sim_interface.write_ctrl() - mujoco.mj_step(model, data) - timings.stop("physics", physics_start) - lowstate_start = timings.start() - sim_interface.after_step() - timings.stop("lowstate", lowstate_start) - fall_logger.step(data, band) - step_count += 1 - task_step = step_count % task_every == 0 - if task_step: - task_start = timings.start() - env.update_state() # before _check_success - timings.stop("task_update", task_start) - # tick() takes sim_lock only to capture sensor state, so encoding and - # publishing do not hold up physics. No physics step runs inside it. - ros_bridge.tick() - # The grippers' state publishes at its rate of sim time. - sim_t = float(data.time) - hand_right.tick(sim_t) - hand_left.tick(sim_t) - if task_step: - task_start = timings.start() - with sim_lock: - done = measurement.tick() - timings.stop("task_check", task_start) - if done: - print("[h12_mujoco] task success (debounced).") - pacer.wait() - timings.maybe_report(float(data.time), ros_bridge.stats) - finally: - # A DeliGrasp settle waits on the sim clock, which no longer advances: end it first, - # whatever the viewer's shutdown does, so its goal aborts and the executor's worker - # thread can exit. - hand_right.stop() - hand_left.stop() - close_viewer(handle, view_sync) - estop.shutdown() - executor.shutdown() - ros_bridge.shutdown() - for node in (measurement, hand_right, hand_left, ros_bridge): - node.destroy_node() - sim_interface.shutdown() - shutdown_ros() - - -if __name__ == "__main__": - _TASK = sim_common.select_task(_PARSER, _ARGS, tag="h12_mujoco", - own_tasks=battery_env.TASKS + blank_env.TASKS, no_kitchen=blank_env.TASKS) - sim_loop(_TASK, viewer=not _ARGS.headless, - layout=_ARGS.layout, style=_ARGS.style, seed=_ARGS.seed, timing=_ARGS.timing) diff --git a/golem_robocasa/h12_robot.py b/golem_robocasa/h12_robot.py new file mode 100644 index 00000000..564b8d6a --- /dev/null +++ b/golem_robocasa/h12_robot.py @@ -0,0 +1,291 @@ +"""Define the H1-2 for golem_sim.sim_loop: its spawn, band, sensors, grippers and estop box.""" +import os +import threading + +import mujoco +import numpy as np + +import h1_2_robosuite # registers H1_2 and the MAGPIE grippers +import mid360 +import realsense_profile +import robosuite_common +import sim_common +import battery_env # registers the Battery env +import blank_env # registers the Blank env +from h12_sensors import H12Sensors +from magpie_hand_bridge import MagpieHandBridge +from mujoco_env import ElasticBand, FallLogger +from estop_server import start_estop_server +from sim_names import NameResolver +from unitree_interface import SimInterface + +NUM_MOTORS = 27 +# The spawn pose (rad) in sim_names.ROS_MOTOR_ORDER: bent-knee legs, then the +# torso and both arms at 0. +INIT_LEG_POS = np.array([ + 0.0, -0.16, 0.0, 0.36, -0.2, 0.0, + 0.0, -0.16, 0.0, 0.36, -0.2, 0.0, +], dtype=float) +INIT_ARM_POS = np.array([ + 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, +], dtype=float) + + +def _initial_motor_qpos(): + """Return the 27-motor spawn pose (rad), without reading any controller config.""" + qpos = np.concatenate([INIT_LEG_POS, INIT_ARM_POS]) + if qpos.shape != (NUM_MOTORS,): + raise ValueError(f"initial motor pose must contain {NUM_MOTORS} entries") + return qpos + + +# Slots in ROS_MOTOR_ORDER: 12 torso, 13-19 left arm, 20-26 right arm, each arm +# shoulder pitch/roll/yaw, elbow, wrist roll/pitch/yaw. +_TORSO, _ARMS = 12, (13, 20) + + +def _limp_upper_body_poses(model, motor_qpos, spawn): + """Return the poses the unpowered upper body sags into, as (qpos indices, values) pairs. + + The spawn clearance must hold in them too, since every motor is off until + the first command. Measured in RoboCasa: the torso twists up to ~0.7 rad + either way, the shoulders pitch back ~0.25 rad, the elbows bend ~0.45 rad + and the wrists drop onto their pitch stops, swinging the grippers forward. + """ + poses = [] + for twist in (0.75, -0.75): + q = np.array(spawn, dtype=float) + q[_TORSO] = twist + for base in _ARMS: + q[base + 0] = 0.25 # shoulder pitch (rad, + is backward) + q[base + 2] = -0.2 # shoulder yaw + q[base + 3] = 0.45 # elbow + wrist_pitch = int(np.flatnonzero(model.jnt_qposadr == motor_qpos[base + 5])[0]) + q[base + 5] = model.jnt_range[wrist_pitch, 0] + poses.append((motor_qpos, q)) + return poses + + +# The arm postures (rad, left arm then right, each shoulder pitch/roll/yaw, elbow, wrist +# roll/pitch/yaw) golem_upper_ik's frame_task_server moves through at start, while the band +# still holds the robot at its spawn: golem_upper_ik/config/h12/robot.yaml's +# startup_sequence and named_configs, copied because the sim reads no controller config +# (tests/docker/test_sim_startup_poses.py keeps them equal). +STARTUP_ARM_STAGES = ( + ("init_1", (0.0, 0.35, 0.0, 1.3, 0.0, 0.0, 0.0, 0.0, -0.35, 0.0, 1.3, 0.0, 0.0, 0.0)), + ("init_2", (0.7, 0.35, 0.0, 1.3, 0.0, 0.0, 0.0, 0.7, -0.35, 0.0, 1.3, 0.0, 0.0, 0.0)), + ("init_3", (0.7, 0.35, 0.0, 0.0, 0.0, 0.0, 0.0, 0.7, -0.35, 0.0, 0.0, 0.0, 0.0, 0.0)), + ("home", (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)), +) + +def spawn(env, resolver, backoff=0.0, margin=None, offset=(0.0, 0.0)): + """Put the robot at rest in its spawn pose, clear of the fixtures by margin, in the sagged poses too. + + Call after env.reset(), whose zero-action settling folds the limp robot. + The robot is placed at RoboCasa's spawn shifted by offset ((dx, dy) m, + world), its lowest geom 2 cm above the floor, and backed off (its -x) until + every robot geom is margin (m; None: the env's spawn_margin, else 0.20) + from any non-floor geom, in the spawn pose, the poses the unpowered upper + body sags into and along the controller's start-up arm moves + (STARTUP_ARM_STAGES); where backing straight off does not reach it, the + search shifts sideways too (robosuite_common.place_robot_collision_free). + backoff (m) then backs it up to that much further into open floor. Zeroes + every velocity in the scene. + """ + data = env.sim.data._data + init_qpos = _initial_motor_qpos() + data.qpos[resolver.motor_qpos] = init_qpos + data.qvel[:] = 0.0 + pos = list(env.init_robot_base_pos) + pos[0] += offset[0] + pos[1] += offset[1] + h1_2_robosuite.place_robot_collision_free( + env, pos, robosuite_common.euler_to_wxyz(getattr(env, "init_robot_base_ori", None)), + extra_backoff=backoff, + margin=getattr(env, "spawn_margin", 0.20) if margin is None else margin, + poses=(_limp_upper_body_poses(env.sim.model._model, resolver.motor_qpos, init_qpos) + + robosuite_common.startup_arm_poses(resolver.motor_qpos, init_qpos, STARTUP_ARM_STAGES, + _ARMS[0])), + ) + + +# The elastic band on the torso holds the robot until SPACE or /elastic_band/toggle releases +# it. Its anchor is 4 cm below the torso's spawn: the feet carry part of the weight. +# Stiffness 1e4 N/m holds the limp robot near upright through the ~20 s the leg policy waits +# under the band for the arms' start-up (static tilt ~ gravity torque / stiffness; at 1e3 it +# pitched forward ~60 deg). Damping 2e3 N*s/m settles a sideways shove well within a second. +BAND_BODY = "torso_link" +BAND_ANCHOR_DZ = -0.04 # m +BAND_STIFFNESS = 1e4 # N/m +BAND_DAMPING = 2e3 # N*s/m + + +def make_band(data, resolver): + """Return (band, torso body id): the band on the torso, anchored BAND_ANCHOR_DZ from it now. + + Call it at the spawn. Raises KeyError when the model has no torso_link. + """ + body = resolver.body_id(BAND_BODY) + if body < 0: + raise KeyError(f"no body {BAND_BODY} for the elastic band") + anchor = data.xpos[body].copy() + anchor[2] += BAND_ANCHOR_DZ + return ElasticBand(anchor, length=0, stiffness=BAND_STIFFNESS, damping=BAND_DAMPING), body + + +class H12Robot: + """The H1-2 with its Magpie grippers, as golem_sim.SimRobot.""" + + name = "h12" + label = "the H1-2" + own_tasks = battery_env.TASKS + blank_env.TASKS + no_kitchen = blank_env.TASKS + gil_switch_s = None + + def __init__(self): + """Start with no extras running.""" + self._hands = () + self._estop = None + + def make_env(self, task, layout, style, seed): + """Return the H1-2's robosuite env for task, not yet reset.""" + return h1_2_robosuite.make_kitchen_env(task, layout_ids=layout, style_ids=style, seed=seed) + + def make_resolver(self, env): + """Return the H1-2's sim_names.NameResolver.""" + return NameResolver(env.sim.model._model) + + def spawn(self, env, resolver): + """Spawn the H1-2 (spawn above) with the spawn knobs. + + GOLEM_SPAWN_BACKOFF (m, default 0: at the counter) backs the robot further into open + floor; Nav2 needs a free start cell to plan (~1.0). GOLEM_SPAWN_MARGIN (m, unset: the + env's spawn_margin, else 0.20) is the clearance to the fixtures. + GOLEM_SPAWN_OFFSET="dx,dy" (m, world) shifts the task's spawn before placement; unset + keeps the task's own. + """ + margin = os.environ.get("GOLEM_SPAWN_MARGIN", "").strip() + offset = os.environ.get("GOLEM_SPAWN_OFFSET", "").strip() + dx, dy = (float(v) for v in offset.split(",")) if offset else (0.0, 0.0) + if offset: + print(f"[h12_robot] spawn offset ({dx:+.3f}, {dy:+.3f}) m") + spawn(env, resolver, backoff=float(os.environ.get("GOLEM_SPAWN_BACKOFF", "0") or 0), + margin=float(margin) if margin else None, offset=(dx, dy)) + print("[h12_robot] initial stance from baked-in sim defaults") + + def make_band(self, data, resolver): + """Return (band, torso body id): the band on the torso, anchored BAND_ANCHOR_DZ from it now.""" + return make_band(data, resolver) + + def odom_body(self, env, resolver): + """Return the body of the pelvis free joint.""" + model = env.sim.model._model + pfx = env.robots[0].robot_model.naming_prefix + joint = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, f"{pfx}{robosuite_common.FREE_JOINT_NAME}") + return int(model.jnt_bodyid[joint]) + + def make_interface(self, model, data, resolver): + """Return the H1-2's Unitree DDS interface; the gripper actuators are the hand bridges'.""" + return SimInterface(model, data, resolver=resolver) + + def make_lock(self, viewer): + """Return one lock for both: every MjData access off the loop's thread (sensors, grippers, viewer) holds it.""" + lock = threading.Lock() + return lock, lock + + def make_sensors(self, node, env, model, data, lock): + """Return the H1-2's sensor suite: the three RealSense cameras and the MID-360. + + The cameras publish what the real RealSense drivers do (realsense_profile.yaml). + Rendering is the heaviest per-step CPU cost: GOLEM_CAMERAS=0 drops every camera. + GOLEM_CAMERA_SCALE renders the head camera at that fraction of its real 1280x720 (same + projection, scaled camera_info). The hand cameras keep their gripper-MJCF fovy at + GOLEM_HAND_CAMERA_RES=x (default 256x256; the D405's colour stream is 640x480). + GOLEM_CAMERA_DEPTH=0 drops depth/image_rect_raw and GOLEM_CAMERA_POINTCLOUD=0 drops + depth/color/points; with both off the depth render is skipped. + """ + pfx = env.robots[0].robot_model.naming_prefix + cameras_on = sim_common.env_flag('GOLEM_CAMERAS', True) + camera_scale = float(os.environ.get('GOLEM_CAMERA_SCALE', '').strip() or 0.5) + profile = realsense_profile.load_profile() + camera_names = [ + ("head", f"{pfx}{h1_2_robosuite.HEAD_CAMERA}"), + ("left_hand", "gripper0_left_hand_cam"), + ("right_hand", "gripper0_right_hand_cam"), + ] if cameras_on else [] + hand_res = tuple(int(v) for v in + (os.environ.get('GOLEM_HAND_CAMERA_RES', '').strip() or '256x256').lower().split('x')) + cameras = realsense_profile.camera_specs(model, camera_names, profile, camera_scale, hand_res) + # GOLEM_LIDAR_POINTS: points per 10 Hz frame, default the real sensor's 20,000. + lidar_points = mid360.points_from_env() + print(f"[h12_robot] MID-360 lidar: {lidar_points} points a frame") + depth_on = sim_common.env_flag('GOLEM_CAMERA_DEPTH', True) + cloud_on = sim_common.env_flag('GOLEM_CAMERA_POINTCLOUD', True) + print(f"[h12_robot] RGBD cameras {'ON' if cameras_on else 'OFF (GOLEM_CAMERAS=0)'}" + + (f": head {cameras[0].color.width}x{cameras[0].color.height} " + f"(GOLEM_CAMERA_SCALE={camera_scale:g}), hands {hand_res[0]}x{hand_res[1]}, " + f"depth stream {'on' if depth_on else 'off'}, " + f"point cloud {'on' if cloud_on else 'off'}" if cameras else "")) + return H12Sensors( + node, model, data, + cameras=cameras, + camera_tf=realsense_profile.tf_edges(profile, [ns for ns, _ in camera_names]), + camera_tf_rate_hz=float(profile["tf_rate_hz"]), + depth_stream=depth_on, + pointcloud=cloud_on, + # The real MID-360's IMU rate. FAST-LIO propagates the pose with the IMU between + # scans, and at 100 Hz the gait's accelerations alias into drift. + imu_rate_hz=200.0, + lidar_body=f"{pfx}livox_link", + lidar_exclude_body=f"{pfx}torso_link", + imu_quat_sensor=f"{pfx}livox_imu_quat", + imu_gyro_sensor=f"{pfx}livox_imu_gyro", + imu_acc_sensor=f"{pfx}livox_imu_acc", + # Drop the robot's own limbs from the scan: their returns map an obstacle under + # the robot and Nav2 refuses the start. GOLEM_LIDAR_SELF_FILTER=0 keeps them. + lidar_self_prefixes=( + (pfx, "gripper0_") if sim_common.env_flag("GOLEM_LIDAR_SELF_FILTER", True) else () + ), + lidar_points=lidar_points, + sim_lock=lock, + ) + + def start_extras(self, env, model, data, lock): + """Start the Magpie gripper bridges and the estop box; return (the gripper nodes, the estop's key callback). + + SAFETY: the estop box is the one golem_bringup's estop_node reads (estop_server); + golem_safety latches on its ENTER toggle, and on its silence once the sim stops. + """ + self._hands = ( + MagpieHandBridge(model, data, side="right", sim_lock=lock, name_prefix="gripper0_right_"), + MagpieHandBridge(model, data, side="left", sim_lock=lock, name_prefix="gripper0_left_"), + ) + self._estop = start_estop_server("golem_sim") + return list(self._hands), (self._estop.key_callback,) + + def tick_extras(self, sim_t): + """Publish the grippers' state at its rate of sim time.""" + for hand in self._hands: + hand.tick(sim_t) + + def stop_extras(self): + """End the grippers' DeliGrasp settles, which wait on the sim clock, and close the estop box.""" + for hand in self._hands: + hand.stop() + if self._estop is not None: + self._estop.shutdown() + + def make_fall_logger(self, resolver, band_body): + """Return the logger of the band's release and the first time the torso tips or drops.""" + return FallLogger(band_body, "torso", upright_min=0.5, height_min=0.5) + + def describe(self, model, resolver): + """Return how many motors the resolver mapped.""" + return f"resolver mapped {len(resolver.motor_ctrl)} motors" + + def surface(self): + """Return the H1-2 sim's topics.""" + return ("rt/lowstate+rt/lowcmd, /clock, RGBD /realsense/{head,left_hand,right_hand}, " + "livox lidar+imu, /{left,right}/gripper/*") diff --git a/golem_robocasa/h12_sensors.py b/golem_robocasa/h12_sensors.py index 96d46a28..dc6fdddb 100644 --- a/golem_robocasa/h12_sensors.py +++ b/golem_robocasa/h12_sensors.py @@ -49,7 +49,7 @@ time, and packs and publishes each frame it completes. The caster casts on its own copy of the model and releases the GIL while it renders, so the casts run beside the physics. The ring holds one frame of casts: when the thread falls -that far behind, tick() waits for it (the lidar_wait stage), so a frame trails +that far behind, tick() waits for it, so a frame trails the sim by at most one frame period and the real-time factor drops instead. State time. tick() runs after mj_step, which leaves the kinematics of the @@ -64,7 +64,6 @@ from collections import namedtuple from contextlib import nullcontext -from sim_timing import LoopTimings import mujoco import numpy as np @@ -192,7 +191,7 @@ class _LidarScans(mid360.ScanRing): """Cast the MID-360's scans on the mid360_scan thread (a mid360.ScanRing) and publish both lidar topics.""" def __init__(self, model, body_id, exclude_body_id, robot_geoms, frame_id, - publish_lidar, publish_cloud, timings=None, sim_lock=None, + publish_lidar, publish_cloud, sim_lock=None, points_per_frame=None): """Build the caster GOLEM_RENDER picks (mid360.make_caster) and start the thread. @@ -215,7 +214,7 @@ def __init__(self, model, body_id, exclude_body_id, robot_geoms, frame_id, else points_per_frame * mid360.FRAME_RATE_HZ) super().__init__(mid360.Mid360Scanner(caster, points_per_second=rate), model, lambda data: (data.xpos[body], data.xmat[body].reshape(3, 3)), - self._publish_frame, caster.batch_s, sim_lock, timings, _log) + self._publish_frame, caster.batch_s, sim_lock, _log) def _publish_frame(self, frame): """Publish a mid360.LidarFrame on /livox/lidar and /livox/pointcloud, stamped at its first point.""" @@ -313,7 +312,6 @@ def __init__( lidar_self_prefixes: tuple = (), lidar_points=None, sim_lock=None, - timings=None, ): """Build the sensors; raise RuntimeError for a body, camera or sensor missing from model. @@ -331,7 +329,6 @@ def __init__( self.model = model self.data = data self.sim_lock = sim_lock - self.timings = timings if timings is not None else LoopTimings() self._camera_specs = list(cameras) self.depth_stream = bool(depth_stream) @@ -398,7 +395,7 @@ def __init__( self._lidar = _LidarScans(model, self.lidar_body_id, self.lidar_exclude_body_id, self.geom_is_robot, LIVOX_FRAME, self.pub_lidar.publish, self.pub_pc2.publish, - self.timings, sim_lock, lidar_points) + sim_lock, lidar_points) # The cameras render on a worker thread, through EGL, while physics runs. self._camera_worker = ThreadPoolExecutor(max_workers=1, thread_name_prefix="camera") self._camera_pending = None @@ -551,16 +548,12 @@ def _publish_imu(self, stamp: TimeMsg) -> None: def _publish_camera_frame(self, stamp: TimeMsg) -> None: """Snapshot MjData and submit rendering and publishing every camera from it to the camera worker.""" - capture_start = self.timings.start() snapshot = self._capture_camera_snapshot() - self.timings.stop("camera_capture", capture_start) # One frame set in flight at most: wait rather than drop, so every # scheduled frame is published. result() re-raises the last one's failure. - wait_start = self.timings.start() pending, self._camera_pending = self._camera_pending, None if pending is not None: pending.result() - self.timings.stop("camera_wait", wait_start) self._camera_pending = self._camera_worker.submit(self._render_and_publish_cameras, stamp, snapshot) def _capture_camera_snapshot(self): @@ -676,7 +669,6 @@ def _render_and_publish_cameras(self, stamp: TimeMsg, data) -> None: self._ensure_renderer() for cam in self.cameras: spec, pubs = cam.spec, cam.pubs - render_start = self.timings.start() size = (spec.color.width, spec.color.height) r, (rgb_buf, depth_buf) = self._renderer[size], self._buffers[size] r.disable_depth_rendering() @@ -686,11 +678,9 @@ def _render_and_publish_cameras(self, stamp: TimeMsg, data) -> None: r.enable_depth_rendering() aligned = r.render(out=depth_buf) aligned_mm = self._depth_mm(aligned) - self.timings.stop("camera_render", render_start) depth_mm = None if self.depth_stream or self.pointcloud: - depth_start = self.timings.start() size = (spec.depth_image.width, spec.depth_image.height) rd, (_, ddepth_buf) = self._renderer[size], self._buffers[size] rd.enable_depth_rendering() @@ -698,9 +688,7 @@ def _render_and_publish_cameras(self, stamp: TimeMsg, data) -> None: with mid360.GL_LOCK: depth = rd.render(out=ddepth_buf) depth_mm = self._depth_mm(depth) - self.timings.stop("camera_depth_render", depth_start) - publish_start = self.timings.start() pubs["color"].publish(self._image_msg(stamp, spec.color_frame, rgb_u8, "rgb8", 3)) if pubs["color_jpeg"].get_subscription_count() > 0: jpeg = io.BytesIO() @@ -723,10 +711,8 @@ def _render_and_publish_cameras(self, stamp: TimeMsg, data) -> None: if pubs["depth_png"].get_subscription_count() > 0: pubs["depth_png"].publish(self._compressed_depth_msg(stamp, spec.depth_frame, depth_mm)) self._publish_info(pubs["depth_info"], cam.depth_info, stamp) - self.timings.stop("camera_publish", publish_start) if self.pointcloud: - cloud_start = self.timings.start() points = realsense_profile.colored_cloud( depth_mm.astype(np.float32) / 1000.0, spec.depth_image, rgb_u8, spec.color, spec.T_color_depth) @@ -741,4 +727,3 @@ def _render_and_publish_cameras(self, stamp: TimeMsg, data) -> None: msg.is_dense = True msg.data = _array("B", points.tobytes()) pubs["points"].publish(msg) - self.timings.stop("camera_cloud", cloud_start) diff --git a/golem_robocasa/h1_2_robosuite.py b/golem_robocasa/h1_2_robosuite.py index a258b20f..9874c783 100644 --- a/golem_robocasa/h1_2_robosuite.py +++ b/golem_robocasa/h1_2_robosuite.py @@ -37,7 +37,7 @@ # The robot MJCF's head camera, whose optical frame is the URDF head_camera_link. HEAD_CAMERA = "head_cam" -# robosuite's init_qpos of the 27 actuated joints; h12_mujoco puts the robot in +# robosuite's init_qpos of the 27 actuated joints; h12_robot puts the robot in # its own spawn pose after the reset. _INIT_QPOS = np.zeros(27) @@ -198,7 +198,7 @@ def place(env, pos, quat): # RoboCasa's placement offset (m): the pelvis z. The soles are 1.041 m below -# the pelvis at init_qpos, so 1.05 keeps them ~1 cm clear until h12_mujoco +# the pelvis at init_qpos, so 1.05 keeps them ~1 cm clear until h12_robot # re-places the robot. POS_OFFSET = (0.0, 0.0, 1.05) diff --git a/golem_robocasa/magpie_hand_bridge.py b/golem_robocasa/magpie_hand_bridge.py index 11756a08..488da6fb 100644 --- a/golem_robocasa/magpie_hand_bridge.py +++ b/golem_robocasa/magpie_hand_bridge.py @@ -1,6 +1,6 @@ """Serve the H1-2 sim's MAGPIE grippers on the real magpie_control driver's ROS surface. -One MagpieHandBridge serves one gripper. h12_mujoco.py spins one per side on +One MagpieHandBridge serves one gripper. golem_sim.py spins one per side on its background MultiThreadedExecutor, and its physics loop calls tick() after every step; every MjData or MjModel access holds the sim lock, since mj_step runs on the main thread. The state rate and the DeliGrasp settles count sim diff --git a/golem_robocasa/mid360.py b/golem_robocasa/mid360.py index 19330cc2..4a32c3cb 100644 --- a/golem_robocasa/mid360.py +++ b/golem_robocasa/mid360.py @@ -612,19 +612,6 @@ def update(self, snapshot, t): return frame -class _NoTimings: - """Stand in for sim_timing.LoopTimings when no stage is timed.""" - - @staticmethod - def start(): - """Return a dummy start mark.""" - return 0.0 - - @staticmethod - def stop(name, start): - """Record nothing.""" - - class ScanRing: """Cast a scanner's rays on a thread of its own, fed with pose snapshots from the physics thread. @@ -636,18 +623,15 @@ class ScanRing: GIL, so the casts run beside the physics; other runtime edits of the model do not reach it. Backpressure. The ring holds the whole casts that fit in one frame period (at least one), the - one being cast included. When it is full, tick() waits for the thread (counted in waits, - timed as the lidar_wait stage): a published frame trails the sim by at most one frame period + one being cast included. When it is full, tick() waits for the thread (counted in waits): a published frame trails the sim by at most one frame period (one interval when the interval is longer) and no ray is dropped; the sim's real-time factor drops instead. Threads. tick() runs on the physics thread between steps; it holds lock, when given, only for the copy, and the live model's alpha is read under it. publish runs on the scan thread. - timings (sim_timing.LoopTimings' start() and stop()) times the stages lidar_capture (the - copy), lidar_wait, lidar_cast (one scanner update) and lidar_publish. """ - def __init__(self, scanner, model, pose, publish, interval, lock=None, timings=None, log=_log): + def __init__(self, scanner, model, pose, publish, interval, lock=None, log=_log): """Start the scan thread for scanner, whose caster casts on a copy of model (the live one). pose(data) returns the sensor's (origin (3,) m, rotation (3, 3) sensor -> world) in data; @@ -663,7 +647,6 @@ def __init__(self, scanner, model, pose, publish, interval, lock=None, timings=N self._pose = pose self._publish = publish self._lock = lock - self.timings = timings if timings is not None else _NoTimings() self._log = log self.data = mujoco.MjData(model) # the thread's: only its geom poses are read n = self.slots @@ -713,7 +696,6 @@ def submit(self, data, t): if not self._thread.is_alive(): raise RuntimeError("the lidar's scan thread has stopped") from None self.waits += 1 - wait_start = self.timings.start() slot = None while slot is None: try: @@ -721,8 +703,6 @@ def submit(self, data, t): except queue.Empty: if not self._thread.is_alive(): raise RuntimeError("the lidar's scan thread has stopped") from None - self.timings.stop("lidar_wait", wait_start) - capture_start = self.timings.start() lock = self._lock if lock is not None: lock.acquire() @@ -737,7 +717,6 @@ def submit(self, data, t): finally: if lock is not None: lock.release() - self.timings.stop("lidar_capture", capture_start) self._work.put((slot, t)) def _run(self): @@ -747,7 +726,6 @@ def _run(self): counts the cast in progress. """ d = self.data - timings = self.timings while (item := self._work.get()) is not None: slot, t = item try: @@ -755,16 +733,12 @@ def _run(self): np.copyto(d.geom_xmat, self._geom_xmat[slot]) snapshot = Snapshot(self._origin[slot], self._rotation[slot], d, self._geom_alpha[slot], self._mat_alpha[slot]) - cast_start = timings.start() frame = self.scanner.update(snapshot, t) - timings.stop("lidar_cast", cast_start) if frame is not None: now = self.now if now is not None: self.max_lag = max(self.max_lag, now - t) - publish_start = timings.start() self._publish(frame) - timings.stop("lidar_publish", publish_start) self.frames += 1 except Exception as e: # keeps the sim running; logged once per kind if type(e) not in self._errors: diff --git a/golem_robocasa/mujoco_env.py b/golem_robocasa/mujoco_env.py index 8768dd0f..b351039f 100644 --- a/golem_robocasa/mujoco_env.py +++ b/golem_robocasa/mujoco_env.py @@ -1,7 +1,7 @@ """Model the elastic band both sim runners hang the robot from until a policy drives it, and log a fall. The band is a spring-damper pulling a body (the torso) toward a fixed anchor -above its spawn. h12_mujoco.sim_loop and g1.g1_mujoco.sim_loop apply it every +above its spawn. golem_sim.sim_loop applies it every physics step while it is enabled (apply_band); SPACE in the viewer or /elastic_band/toggle (MeasurementBridge) releases it. FallLogger logs the band's release and the robot's first fall, which the closed-loop tests read. diff --git a/golem_robocasa/robosuite_common.py b/golem_robocasa/robosuite_common.py index 396c6cec..b994ad92 100644 --- a/golem_robocasa/robosuite_common.py +++ b/golem_robocasa/robosuite_common.py @@ -222,6 +222,29 @@ def left_dir(base_quat): LATERAL_OFFSETS = (0.0, 0.1, -0.1, 0.2, -0.2, 0.3, -0.3, 0.4, -0.4, 0.5, -0.5) +# Points checked along each start-up arm move, its end included: the arms sweep between stages. +STARTUP_STEPS = 4 + + +def startup_arm_poses(motor_qpos, spawn, stages, first_arm_slot, steps=STARTUP_STEPS): + """Return the poses of the controller's start-up arm moves from the spawn pose, for place_robot_collision_free. + + spawn is the body motors' spawn pose (rad) and motor_qpos their qpos indices. stages is + ((name, 14 arm angles rad), ...), golem_upper_ik's startup_sequence in order; the 14 arm + motors start at slot first_arm_slot. Each move is sampled at steps points, its end + included. Returns (qpos indices, values) pairs. + """ + poses = [] + prev = np.asarray(spawn, dtype=float) + for _, arms in stages: + target = prev.copy() + target[first_arm_slot:first_arm_slot + 14] = arms + for a in np.linspace(0.0, 1.0, steps + 1)[1:]: + poses.append((motor_qpos, (1.0 - a) * prev + a * target)) + prev = target + return poses + + def place_robot_collision_free(env, base_pos, base_quat, place, step=0.02, max_iters=50, margin=0.0, poses=(), extra_backoff=0.0, lateral_offsets=LATERAL_OFFSETS, tag="robosuite_common"): diff --git a/golem_robocasa/sim_common.py b/golem_robocasa/sim_common.py index 68a9ca17..dacce3fa 100644 --- a/golem_robocasa/sim_common.py +++ b/golem_robocasa/sim_common.py @@ -1,7 +1,7 @@ """Select the robot, check the renderer, parse the command line and the sims' knobs, resolve tasks and frame the viewer for both sims. It imports only the standard library at module level (mujoco and robocasa -inside the functions that need them): h12_mujoco.py picks the robot and checks +inside the functions that need them): golem_sim.py picks the robot and checks the GPU with it before it imports either robot's modules. """ import argparse @@ -15,7 +15,6 @@ ROBOT_LABELS = {"h12": "the H1-2", "g1": "the G1"} # The option each robot's loop does not implement; parse_args refuses it. -_UNSUPPORTED = {"h12": "--duration", "g1": "--timing"} # The passive viewer's opening camera: straight down onto the robot wherever # RoboCasa and the spawn placement put it, rotated with the robot's yaw so the @@ -181,11 +180,6 @@ def add_common_args(parser: argparse.ArgumentParser, *, parser.add_argument("--layout", type=int, default=None, help="RoboCasa kitchen layout id") parser.add_argument("--style", type=int, default=None, help="RoboCasa kitchen style id") parser.add_argument("--seed", type=int, default=None, help="episode seed") - parser.add_argument("--timing", action="store_true", - help="Log real-time factor and stage mean/max timings every 5 seconds") - parser.add_argument("--duration", type=float, default=0.0, - help="sim seconds to run from the first step, then exit " - "(default 0: run until closed; G1 only)") add_robot_args(parser) @@ -193,16 +187,13 @@ def parse_args(parser: argparse.ArgumentParser, argv: Sequence[str], robot: str) """Parse argv for the runner of robot ('h12' or 'g1'). Exits with a usage error (2) for a robot flag that reads as the other robot - (h12_mujoco.py dispatches whole --h12/--g1 tokens, so only an abbreviation - such as --g gets here) and for the option in _UNSUPPORTED for this robot. + (golem_sim.py picks the robot from whole --h12/--g1 tokens, so only an abbreviation + such as --g gets here). """ args = parser.parse_args(argv) if args.robot not in (None, robot): parser.error(f"the robot flag is abbreviated and reads as --{args.robot} here, " f"but this runs {ROBOT_LABELS[robot]}; spell the robot flag out: --{args.robot}") - flag = _UNSUPPORTED[robot] - if getattr(args, flag[2:]): - parser.error(f"{flag} is not supported for {ROBOT_LABELS[robot]} yet") return args diff --git a/golem_robocasa/sim_timing.py b/golem_robocasa/sim_timing.py index a41322b3..ab942d61 100644 --- a/golem_robocasa/sim_timing.py +++ b/golem_robocasa/sim_timing.py @@ -1,6 +1,5 @@ -"""Pace both RoboCasa sims' loops to a target real-time factor; report the H1-2's per-stage timings (--timing).""" +"""Pace the RoboCasa sim loop to a target real-time factor.""" import math -import threading import time @@ -44,64 +43,3 @@ def wait(self): # Only the wall deadline moves; sim time is never skipped. self.deadline = now - self.max_lag return 0.0 - - -class LoopTimings: - """Collect per-stage wall times and print mean/max ms and the real-time factor. - - Disabled (the default) it does nothing. The sensor worker threads record - stages too, so updates and reports take a lock. - """ - - def __init__(self, enabled=False, interval=5.0, *, clock=time.perf_counter): - """Report every interval wall seconds when enabled.""" - self.enabled = enabled - self.interval = interval - self.clock = clock - self.wall_start = clock() if enabled else 0.0 - self.sim_start = 0.0 - self.stages = {} - self._lock = threading.Lock() - - def reset(self, sim_time): - """Start a new report interval at sim_time (s) and drop the stages so far.""" - self.wall_start = self.clock() if self.enabled else 0.0 - self.sim_start = sim_time - with self._lock: - self.stages.clear() - - def start(self): - """Return the start mark of a stage, to pass to stop().""" - return self.clock() if self.enabled else 0.0 - - def stop(self, name, start): - """Record the wall time since start under the stage name.""" - if self.enabled: - elapsed = self.clock() - start - with self._lock: - total, count, longest = self.stages.get(name, (0.0, 0, 0.0)) - self.stages[name] = (total + elapsed, count + 1, max(longest, elapsed)) - - def maybe_report(self, sim_time, stats=None): - """Print the report and start a new interval once interval wall seconds have passed. - - stats, when given, is called at each report and what it returns ("" for nothing) is - appended to the line (the sensors' counts). - """ - if not self.enabled: - return - now = self.clock() - wall = now - self.wall_start - if wall < self.interval: - return - with self._lock: - stages, self.stages = self.stages, {} - fields = ' '.join( - f'{name}={1000 * total / count:.2f}/{1000 * longest:.2f}ms(n={count})' - for name, (total, count, longest) in stages.items() - ) - extra = stats() if stats is not None else '' - print(f'[sim-timing] RTF={(sim_time - self.sim_start) / wall:.3f} ' - f'(stage mean/max) {fields}' + (f'; {extra}' if extra else ''), flush=True) - self.wall_start = now - self.sim_start = sim_time diff --git a/golem_robocasa/unitree_common.py b/golem_robocasa/unitree_common.py index 39e3aec9..78f3c8da 100644 --- a/golem_robocasa/unitree_common.py +++ b/golem_robocasa/unitree_common.py @@ -443,6 +443,17 @@ def _on_lowcmd(self, msg): self.commanded = True log(f't={cmd.time:.3f}s: first valid rt/lowcmd; it drives all {self._motors} body motors') + def physics_reset(self): + """Restamp the latched rt/lowcmd at the sim time after MuJoCo reset the scene. + + The timeout then still measures the silence since the last frame + instead of waiting for the old sim time to come round again. + """ + now = float(self.data.time) # the subclass's MjData + self.sim_time = now + if self._lowcmd is not None: + self._lowcmd = self._lowcmd._replace(time=now) + def _lowcmd_torques(self, q, dq, now): """Return the latched command's torques at q, dq, or zeros once it is older than the timeout.""" cmd = self._lowcmd diff --git a/golem_robocasa/unitree_interface.py b/golem_robocasa/unitree_interface.py index f5bd44d5..533acb4a 100644 --- a/golem_robocasa/unitree_interface.py +++ b/golem_robocasa/unitree_interface.py @@ -7,7 +7,7 @@ CRC, mode_machine 6, mode_pr 0 and finite values on the 27 body motors; other frames are dropped. From then on rt/lowcmd drives the body: motor mode 1 turns a motor's PD on, any other mode turns it off. Until a command drives it the -robot hangs from the sim's elastic band (h12_mujoco). +robot hangs from the sim's elastic band (h12_robot). Every torque is recomputed every physics step from the latched command and the current state (write_ctrl), as the motors' ~1 kHz servo loop does: a torque diff --git a/golem_robocasa/viewer_sync.py b/golem_robocasa/viewer_sync.py index 07fa925a..c40d32e5 100644 --- a/golem_robocasa/viewer_sync.py +++ b/golem_robocasa/viewer_sync.py @@ -83,7 +83,7 @@ def draw(handle, viewer_data): return draw -def open_viewer(model, data, lock, band, body, tag, timings=None, sim_rate=None, key_callbacks=()): +def open_viewer(model, data, lock, band, body, tag, sim_rate=None, key_callbacks=()): """Launch the passive viewer on its own copy of data, framed on body, and start its sync thread. lock is the physics loop's lock (ViewerSync). band, the sim's ElasticBand @@ -120,7 +120,7 @@ def on_key(keycode): print(f"[{tag}] viewer camera framing skipped: {e}") view_sync = ViewerSync(model, data, handle, lock, viewer_data, draw=band_overlay(band, body) if band is not None else None, - timings=timings, sim_rate=sim_rate) + sim_rate=sim_rate) view_sync.start() return handle, view_sync @@ -137,15 +137,14 @@ class ViewerSync: """Refresh the viewer's MjData from the simulation's and sync the viewer at hz.""" def __init__(self, model, data, handle, lock, viewer_data, draw=None, hz=VIEWER_HZ, - timings=None, sim_rate=None): + sim_rate=None): """Set up the sync of data into viewer_data, drawn by the passive viewer handle. lock is the threading.Lock the physics loop holds around mj_step and after_step (and around apply_perturbation); the copy takes it too. viewer_data is the MjData the viewer was launched with. draw(handle, viewer_data), if given, runs once per frame before the sync to put the - overlay in handle.user_scn; without it the overlay is cleared. timings, - a sim_timing.LoopTimings, records each frame's wall time as stage "viewer". + overlay in handle.user_scn; without it the overlay is cleared. Each frame also feeds data.time to an RtfMeter, whose real-time factor, with sim_rate (the target, if given), is set as the viewer's text every RTF_TEXT_PERIOD_S. """ @@ -156,7 +155,6 @@ def __init__(self, model, data, handle, lock, viewer_data, draw=None, hz=VIEWER_ self._viewer_data = viewer_data self._draw = draw self._period = 1.0 / hz - self._timings = timings self._perturb = np.zeros_like(data.xfrc_applied) self._sim_rate = sim_rate self._rtf = RtfMeter() @@ -197,7 +195,6 @@ def _run(self): def _frame(self): """Copy the state, show the real-time factor, draw the overlay, sync and keep the mouse force.""" viewer_data = self._viewer_data - mark = self._timings.start() if self._timings is not None else 0.0 with self._lock: copy_display_state(viewer_data, self._data) now = time.perf_counter() @@ -216,5 +213,3 @@ def _frame(self): self._handle.sync() with self._lock: self._perturb[:] = viewer_data.xfrc_applied - if self._timings is not None: - self._timings.stop("viewer", mark) diff --git a/tests/README.md b/tests/README.md index b17f344b..18a17aa3 100644 --- a/tests/README.md +++ b/tests/README.md @@ -58,6 +58,7 @@ ROS_DOMAIN_ID=187 tests/isaaclab/run_smoke.sh ROS_DOMAIN_ID=188 tests/isaaclab/run_livox_interop.sh ROS_DOMAIN_ID=189 tests/isaaclab/run_sensor_transport.sh ROS_DOMAIN_ID=191 tests/isaaclab/run_robot_interop.sh +tests/isaaclab/run_elastic_band.sh ``` `run_smoke.sh` requires the actual Isaac Lab scene to complete 50 steps and @@ -68,6 +69,11 @@ decoded by the ROS container's generated Humble Livox message type. `run_robot_interop.sh` publishes native ROS LowCmd and checks wrist motion, Magpie gripper services/actions (or Inspire commands when selected), measured hand state and all 19 sensor topics. It releases body motors on exit. +`run_elastic_band.sh` runs the sim with `--elastic_band` and no controller for 50 s of sim +time (domain 185 unless `ROS_DOMAIN_ID` is set): the limp robot must hang within 0.10 m of the +band's anchor for 40 s, collapse when `/elastic_band/toggle` releases it, and be pulled back to the +anchor from where it lay when a second call enables the band. Its log is +`tests/results/isaac-elastic-band.log`. The workflow has an explicit manual GPU job for a prepared self-hosted runner labelled `nvidia`; it is not run automatically on untrusted pull requests. @@ -80,7 +86,9 @@ robot-specific acceptance tests when the CL_Assets pin changes. Isaac defaults to the floating-base Magpie H1-2 in `CL_Assets`. Use `docker/scripts/docker_run.sh isaac --hand_type inspire --headless` for the Inspire fixture. `--fix_base` fixes the Magpie root for isolated motor tests; -normal launches leave it free and require a lower-body controller. +normal launches leave it free and require a lower-body controller. `--elastic_band` (not with +`--fix_base`) hangs the free robot from the RoboCasa sims' elastic band on `torso_link` until +`/elastic_band/toggle` releases it. Magpie exposes `/{left,right}/gripper/state`, the gripper services and DeliGrasp through a native ROS process in the Isaac container. Its isolated Humble/Python runtime and messages are built into the Isaac image diff --git a/tests/docker/test_sim_startup_poses.py b/tests/docker/test_sim_startup_poses.py index e4565683..93cc1b64 100644 --- a/tests/docker/test_sim_startup_poses.py +++ b/tests/docker/test_sim_startup_poses.py @@ -1,4 +1,4 @@ -"""Check the H1-2 sim's copy of the controller's start-up arm moves against golem_upper_ik's config.""" +"""Check each robot's sim copy of the controller's start-up arm moves against golem_upper_ik's config.""" import ast from pathlib import Path import unittest @@ -6,28 +6,35 @@ import yaml ROOT = Path(__file__).resolve().parents[2] -SIM = ROOT / 'golem_robocasa' / 'h12_mujoco.py' -ROBOT = ROOT / 'core_ws' / 'src' / 'golem_upper_ik' / 'config' / 'h12' / 'robot.yaml' - - -def sim_stages(): - """Return h12_mujoco.STARTUP_ARM_STAGES, read from the source without importing the sim.""" - for node in ast.parse(SIM.read_text()).body: +# Each robot's sim module and its controller's robot.yaml. +ROBOTS = { + 'h12': (ROOT / 'golem_robocasa' / 'h12_robot.py', + ROOT / 'core_ws' / 'src' / 'golem_upper_ik' / 'config' / 'h12' / 'robot.yaml'), + 'g1': (ROOT / 'golem_robocasa' / 'g1' / 'g1_robot.py', + ROOT / 'core_ws' / 'src' / 'golem_upper_ik' / 'config' / 'g1' / 'robot.yaml'), +} + + +def sim_stages(sim): + """Return the module's STARTUP_ARM_STAGES, read from the source without importing the sim.""" + for node in ast.parse(sim.read_text()).body: if isinstance(node, ast.Assign) and any( getattr(t, 'id', None) == 'STARTUP_ARM_STAGES' for t in node.targets): return [(name, list(arms)) for name, arms in ast.literal_eval(node.value)] - raise AssertionError(f'{SIM} has no STARTUP_ARM_STAGES') + raise AssertionError(f'{sim} has no STARTUP_ARM_STAGES') class StartupPoseTests(unittest.TestCase): """The spawn search clears the arm moves the controller really makes.""" def test_stages_are_the_controllers_startup_sequence(self): - """STARTUP_ARM_STAGES is robot.yaml's startup_sequence with its named_configs' values.""" - robot = yaml.safe_load(ROBOT.read_text()) - expected = [(name, [float(q) for q in robot['named_configs'][name]]) - for name in robot['startup_sequence']] - self.assertEqual([(n, [float(q) for q in a]) for n, a in sim_stages()], expected) + """Each robot's STARTUP_ARM_STAGES is its robot.yaml's startup_sequence with its named_configs' values.""" + for name, (sim, config) in ROBOTS.items(): + with self.subTest(robot=name): + robot = yaml.safe_load(config.read_text()) + expected = [(stage, [float(q) for q in robot['named_configs'][stage]]) + for stage in robot['startup_sequence']] + self.assertEqual([(n, [float(q) for q in a]) for n, a in sim_stages(sim)], expected) if __name__ == '__main__': diff --git a/tests/docker/test_workflows.py b/tests/docker/test_workflows.py index 218463a0..7b1e6ea4 100644 --- a/tests/docker/test_workflows.py +++ b/tests/docker/test_workflows.py @@ -25,7 +25,7 @@ def setUp(self): self.root = Path(self.tmp.name) shutil.copytree(REPO / 'docker', self.root / 'docker', ignore=shutil.ignore_patterns('.env', '__pycache__')) for name in ('unitree_sdk2_python/setup.py', *self.H12_MODEL, *self.G1_MODEL, - 'CL_isaaclab_sim/sim_main.py', 'golem_robocasa/h12_mujoco.py', + 'CL_isaaclab_sim/sim_main.py', 'golem_robocasa/golem_sim.py', 'core_ws/src/magpie_msgs/package.xml', 'core_ws/src/custom_ros_messages/package.xml', 'core_ws/src/livox_ros_driver2/package_ROS2.xml', 'core_ws/cyclonedds.xml', 'core_ws/src/golem_bringup/package.xml'): @@ -93,10 +93,10 @@ def test_zero_with_leading_zeros_is_rejected(self): def test_missing_source_fails_before_container_creation(self): """A missing sim source fails, naming the file, before any container is created.""" - (self.root / 'golem_robocasa/h12_mujoco.py').unlink() + (self.root / 'golem_robocasa/golem_sim.py').unlink() r = self.run_script('docker/scripts/docker_run.sh', 'robocasa') self.assertNotEqual(r.returncode, 0) - self.assertIn('h12_mujoco.py', r.stderr) + self.assertIn('golem_sim.py', r.stderr) self.assertFalse(any('run' in c for c in self.calls())) def test_isaac_launcher_forwards_options_without_a_terminal(self): @@ -122,7 +122,7 @@ def test_isaac_launcher_forwards_options_without_a_terminal(self): def test_lfs_pointer_is_not_accepted_as_an_asset(self): """A Git LFS pointer in place of a required file is refused, naming LFS.""" - asset = self.root / 'golem_robocasa/h12_mujoco.py' + asset = self.root / 'golem_robocasa/golem_sim.py' asset.write_text('version https://git-lfs.github.com/spec/v1\noid sha256:123\nsize 10\n') r = self.run_script('docker/scripts/docker_run.sh', 'robocasa') self.assertNotEqual(r.returncode, 0) diff --git a/tests/g1/README.md b/tests/g1/README.md index f6409b78..89b30f96 100644 --- a/tests/g1/README.md +++ b/tests/g1/README.md @@ -77,7 +77,7 @@ robocasa ros`); they never build them. so the checkout's own `core_ws/build` and `core_ws/install` are left alone. The first run builds the whole workspace; later runs rebuild what changed. `CORE_WS=` builds and uses that workspace in place instead. The tests share that build, so one runs at a time. -- **Sim.** The sim role of `test_sim_*.py` runs `golem_robocasa/h12_mujoco.py --g1` through +- **Sim.** The sim role of `test_sim_*.py` runs `golem_robocasa/golem_sim.py --g1` through `runpy`, with a recorder patched onto `g1.unitree_interface.SimInterface`. The recorder reads the robot's state inside the sim process, so no ground truth is published. - **Knobs.** The sim's knobs come from the shell or the scripts, never from `docker/.env`. diff --git a/tests/g1/test_sim_bringup.py b/tests/g1/test_sim_bringup.py index 30e8d598..e6f7a325 100644 --- a/tests/g1/test_sim_bringup.py +++ b/tests/g1/test_sim_bringup.py @@ -2,7 +2,7 @@ test_sim_bringup.sh runs them; see it for the usage. - sim in the robocasa container: h12_mujoco.py --g1 as __main__ (its own + sim in the robocasa container: golem_sim.py --g1 as __main__ (its own CLI), recording the robot's state from the sim after every 10th physics step (50 Hz of sim time; GOLEM_TEST_RECORD_EVERY=1 for every step) to /sim.npz, until /stop appears. @@ -112,7 +112,7 @@ OPTICAL_IN_LINK_XYZW = (-0.5, 0.5, -0.5, 0.5) # rpy (-pi/2, 0, -pi/2), REP 103's optical frame # the feet whose world positions the sim records (the G1's ankle roll links) FEET = ("left_ankle_roll_link", "right_ankle_roll_link") -# the sim's log line once the band is off (golem_robocasa/g1/g1_mujoco.py's fall logger) +# the sim's log line once the band is off (golem_robocasa/g1/g1_robot.py's fall logger) BAND_RELEASED_LINE = "elastic band released at t=" # the leg node (golem_lowerbody_rl): its ROS logger, and its log lines: its run config, # the policy's start, each call of the band's toggle with its reason, and the toggle's @@ -335,7 +335,7 @@ def write_sensor_config(out): def run_recorded_sim(out, robot, sim_args, recorder, tag, on_init=None): - """Run h12_mujoco.py for robot ('h12' or 'g1') as __main__ with recorder.step after every SimInterface.after_step. + """Run golem_sim.py for robot ('h12' or 'g1') as __main__ with recorder.step after every SimInterface.after_step. on_init(iface), if given, runs once the SimInterface is made: the robot is at the spawn, in the sim's stance, before the first step and the bridges. SIGTERM, like Ctrl-C, ends the sim, @@ -367,7 +367,7 @@ def interrupt(signum, frame): if on_init is not None: interface.__init__ = hooked_init signal.signal(signal.SIGTERM, interrupt) - script = os.path.join(SIM, "h12_mujoco.py") + script = os.path.join(SIM, "golem_sim.py") sys.argv = [script, f"--{robot}"] + sim_args try: runpy.run_path(script, run_name="__main__") @@ -378,7 +378,7 @@ def interrupt(signum, frame): def run_sim(out, sim_args): - """Run h12_mujoco.py --g1 with the recorder; write sensors.json, for the drive, first.""" + """Run golem_sim.py --g1 with the recorder; write sensors.json, for the drive, first.""" write_sensor_config(out) run_recorded_sim(out, "g1", sim_args, Recorder(out), "test_sim_bringup") @@ -1393,7 +1393,7 @@ def main(): parser.add_argument("role", choices=("sim", "drive", "report")) parser.add_argument("--out", required=True, help="the shared output directory") parser.add_argument("--walk", action="store_true", help="drive: the walk test") - parser.epilog = "sim: the G1 sim's arguments (h12_mujoco.py --g1) follow a --" + parser.epilog = "sim: the G1 sim's arguments (golem_sim.py --g1) follow a --" argv = sys.argv[1:] sim_args = argv[argv.index("--") + 1:] if "--" in argv else [] args = parser.parse_args(argv[:len(argv) - len(sim_args) - ("--" in argv)]) diff --git a/tests/g1/test_sim_bringup.sh b/tests/g1/test_sim_bringup.sh index 8b287be9..df274098 100755 --- a/tests/g1/test_sim_bringup.sh +++ b/tests/g1/test_sim_bringup.sh @@ -4,7 +4,7 @@ # tests/g1/test_sim_bringup.sh # the arm test # tests/g1/test_sim_bringup.sh --walk # the walk test # -# Builds core_ws for the G1, starts the sim (h12_mujoco.py --g1) headless in the +# Builds core_ws for the G1, starts the sim (golem_sim.py --g1) headless in the # layout 9 / style 9 / seed 42 kitchen, then, in its network namespace, a ros # container that runs 'ros2 launch golem_bringup g1_sim.launch.py # use_rviz:=false' and the test's steps (test_sim_bringup.py drive). It then stops diff --git a/tests/g1/test_sim_explore.py b/tests/g1/test_sim_explore.py index 5b65f280..46050d0f 100644 --- a/tests/g1/test_sim_explore.py +++ b/tests/g1/test_sim_explore.py @@ -4,7 +4,7 @@ RoboCasa kitchen explored, or its time runs out. test_sim_explore.sh runs the roles and has the usage. Built on test_sim_nav.py. - sim test_sim_nav.py's sim role as it is: h12_mujoco.py --g1, recording the robot's + sim test_sim_nav.py's sim role as it is: golem_sim.py --g1, recording the robot's state (/sim.npz) and any robot-environment contact (/contacts.json), and at the spawn the kitchen's occupancy from the sim's own geometry (/scene.npz), among it the floor the robot can reach: the cells of a 5 cm @@ -357,7 +357,7 @@ def main(): parser = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter) parser.add_argument("role", choices=("sim", "drive", "report")) parser.add_argument("--out", required=True, help="the shared output directory") - parser.epilog = "sim: the G1 sim's arguments (h12_mujoco.py --g1) follow a --" + parser.epilog = "sim: the G1 sim's arguments (golem_sim.py --g1) follow a --" argv = sys.argv[1:] sim_args = argv[argv.index("--") + 1:] if "--" in argv else [] args = parser.parse_args(argv[:len(argv) - len(sim_args) - ("--" in argv)]) diff --git a/tests/g1/test_sim_explore.sh b/tests/g1/test_sim_explore.sh index 5f9a8812..2e3ea332 100755 --- a/tests/g1/test_sim_explore.sh +++ b/tests/g1/test_sim_explore.sh @@ -5,7 +5,7 @@ # # tests/g1/test_sim_explore.sh # FAST_LIO on the sim's MID-360 # -# Builds core_ws for the G1 and starts the sim (h12_mujoco.py --g1) headless, +# Builds core_ws for the G1 and starts the sim (golem_sim.py --g1) headless, # with its sensors on, in the layout 9 / style 9 / seed 42 kitchen, the spawn # backed EXPLORE_BACKOFF m into open floor, as test_sim_nav.sh does. In its # network namespace a ros container then runs 'ros2 launch golem_bringup diff --git a/tests/g1/test_sim_nav.py b/tests/g1/test_sim_nav.py index 29ba855b..2a774c14 100644 --- a/tests/g1/test_sim_nav.py +++ b/tests/g1/test_sim_nav.py @@ -3,7 +3,7 @@ Nav2 walks the G1 to a goal in the RoboCasa kitchen. test_sim_nav.sh runs the roles and has the usage. - sim in the robocasa container: h12_mujoco.py --g1 as __main__, recording + sim in the robocasa container: golem_sim.py --g1 as __main__, recording the robot's state as test_sim_bringup.py's sim role does (/sim.npz), and also, after every physics step, any penetrating contact between a robot geom and anything but the floor (/contacts.json). At startup, @@ -383,7 +383,7 @@ def save(self): def run_sim(out, sim_args): - """Run h12_mujoco.py --g1 with the recorder, the scene map and the goal; write sensors.json, for the drive, first.""" + """Run golem_sim.py --g1 with the recorder, the scene map and the goal; write sensors.json, for the drive, first.""" bringup_test.write_sensor_config(out) bringup_test.run_recorded_sim( out, "g1", sim_args, NavRecorder(out), "test_sim_nav", @@ -1135,7 +1135,7 @@ def contacts(self): self.check("no contact", not c["contact_steps"], f"{c['steps']} physics steps: {text}") def fall_log(self): - """Check the sim's fall logger (golem_robocasa/g1/g1_mujoco.py), which looks at every physics step, logged no fall.""" + """Check the sim's fall logger (golem_robocasa/g1/g1_robot.py), which looks at every physics step, logged no fall.""" path = os.path.join(self.out, "sim.log") if not os.path.exists(path): self.check("no fall logged", False, "no sim.log") @@ -1308,7 +1308,7 @@ def main(): parser.add_argument("--out", required=True, help="the shared output directory") parser.add_argument("--keep-lidar", action="store_true", help="drive: keep the /livox/lidar frames from the goal on (lidar_frames.npz)") - parser.epilog = "sim: the G1 sim's arguments (h12_mujoco.py --g1) follow a --" + parser.epilog = "sim: the G1 sim's arguments (golem_sim.py --g1) follow a --" argv = sys.argv[1:] sim_args = argv[argv.index("--") + 1:] if "--" in argv else [] args = parser.parse_args(argv[:len(argv) - len(sim_args) - ("--" in argv)]) diff --git a/tests/g1/test_sim_nav.sh b/tests/g1/test_sim_nav.sh index ab20c286..bf0a17b9 100755 --- a/tests/g1/test_sim_nav.sh +++ b/tests/g1/test_sim_nav.sh @@ -5,7 +5,7 @@ # tests/g1/test_sim_nav.sh # FAST_LIO on the sim's MID-360 # NAV_GOAL=0.62,-5.08,180 tests/g1/test_sim_nav.sh # the corridor behind the counter peninsula # -# Builds core_ws for the G1 and starts the sim (h12_mujoco.py --g1) headless, +# Builds core_ws for the G1 and starts the sim (golem_sim.py --g1) headless, # with its sensors on, in the layout 9 / style 9 / seed 42 kitchen, the spawn # backed NAV_BACKOFF m into open floor. In its network namespace a ros container # then runs 'ros2 launch golem_bringup g1_sim.launch.py use_rviz:=false diff --git a/tests/isaaclab/check_elastic_band.py b/tests/isaaclab/check_elastic_band.py new file mode 100644 index 00000000..a2d43b72 --- /dev/null +++ b/tests/isaaclab/check_elastic_band.py @@ -0,0 +1,150 @@ +"""Run the Isaac sim with --elastic_band and check the band holds, releases and re-holds the robot (GPU). + +run_elastic_band.sh runs this in place of sim_main.py. It wraps the env's step to record the +torso, and a thread calls /elastic_band/toggle over ROS, from a context of its own, at two +sim times. Nothing drives the motors, so the robot hangs limp on the band, collapses when it +is released, and is pulled back to the anchor, from wherever it lay, when it is re-enabled: +a force applied in the torso's frame instead of the world's would not bring it back. +Each check prints PASS or FAIL and the last line PASSED or FAILED, which run_elastic_band.sh reads: +Kit's shutdown sets the exit status. +""" +import math +from pathlib import Path +import runpy +import sys +import threading +import time + +ENTRY = Path('/home/code/CL_isaaclab_sim/sim_main.py') +RELEASE_T = 40.0 # s of sim time: the first toggle, which releases the band; the leg node holds under it up to 34 s +REHOLD_T = 44.0 # s of sim time: the second toggle, which enables it again +END_T = 50.0 # s of sim time the run lasts +STEP_DT = 0.01 # s, the env step sim_main.py runs at (--max_steps counts these) +HOLD_RADIUS = 0.10 # m, how far from the anchor the held torso may hang +HOLD_TILT = 20.0 # deg, the held torso's largest tilt +FALLEN_DROP = 0.30 # m, how far below the anchor the released torso must fall + + +def tilt_deg(quaternion): + """Return the angle (deg) between a body's z axis and world up, from its WXYZ quaternion.""" + w, x, y, z = quaternion + return math.degrees(math.acos(max(-1., min(1., 1. - 2. * (x * x + y * y))))) + + +class Run: + """Hold what the wrapped env records and what the toggle thread hears.""" + + def __init__(self): + """Start with no samples and no replies.""" + self.rows = [] # (sim time s, torso position m world, tilt deg, band enabled) + self.anchor = None # m, world: the band's anchor + self.sim_time = 0. + self.replies = [] # the Trigger replies' (success, message), in call order + self.error = None + + def wrap(self, make): + """Return gym.make wrapped to record the torso after every env step.""" + def observed_make(*args, **kwargs): + """Make the Isaac env and wrap its step.""" + wrapper = make(*args, **kwargs) + env = wrapper.unwrapped + step = env.step + robot = env.scene['robot'] + torso = robot.data.body_names.index('torso_link') + + def observed_step(*a, **kw): + """Step the env and record the torso and the band's state.""" + result = step(*a, **kw) + band = env.elastic_band + self.anchor = band.anchor[0].tolist() + self.sim_time = env._sim_step_counter * env.physics_dt + self.rows.append((self.sim_time, robot.data.body_pos_w[0, torso].tolist(), + tilt_deg(robot.data.body_quat_w[0, torso].tolist()), band.enabled)) + # Report on the last step: Kit's shutdown ends the process once sim_main.py's loop exits. + if len(self.rows) == round(END_T / STEP_DT): + self.report() + return result + + env.step = observed_step + return wrapper + return observed_make + + def toggle_at(self, times): + """Call /elastic_band/toggle once sim time passes each of times; record the replies.""" + try: + # Isaac's bundled ROS Python, as the sim's own bridge finds it. + from src.python.sensors.ros_sensor_bridge import bootstrap_ros + rclpy = bootstrap_ros() + from rclpy.context import Context + from rclpy.executors import SingleThreadedExecutor + from std_srvs.srv import Trigger + context = Context() + rclpy.init(context=context) + node = rclpy.create_node('elastic_band_check', context=context) + executor = SingleThreadedExecutor(context=context) + executor.add_node(node) + client = node.create_client(Trigger, '/elastic_band/toggle') + for when in times: + while self.sim_time < when: + time.sleep(0.01) + if not client.wait_for_service(timeout_sec=10.0): + raise RuntimeError('/elastic_band/toggle is not served') + future = client.call_async(Trigger.Request()) + executor.spin_until_future_complete(future, timeout_sec=10.0) + if not future.done(): + raise RuntimeError('/elastic_band/toggle did not answer within 10 s') + self.replies.append((future.result().success, future.result().message)) + except Exception as error: # reported by the checks + self.error = error + + def distance(self, row): + """Return how far (m) a row's torso is from the anchor.""" + return math.dist(row[1], self.anchor) + + def checks(self): + """Return [(passed, text)] for the run.""" + held = [r for r in self.rows if 3.0 <= r[0] < RELEASE_T] + fallen = [r for r in self.rows if REHOLD_T - 0.5 <= r[0] < REHOLD_T] + reheld = [r for r in self.rows if r[0] >= END_T - 1.0] + results = [(self.error is None and self.replies == [(True, 'disabled'), (True, 'enabled')], + f"/elastic_band/toggle answered {self.replies} (expected disabled, then enabled); error {self.error}")] + if not (held and fallen and reheld): + return results + [(False, f'the run recorded {len(self.rows)} samples, too few for every phase')] + results.append((max(map(self.distance, held)) <= HOLD_RADIUS and max(r[2] for r in held) <= HOLD_TILT, + f'held: torso at most {max(map(self.distance, held)):.3f} m from the anchor (limit {HOLD_RADIUS}), ' + f'tilt at most {max(r[2] for r in held):.1f} deg (limit {HOLD_TILT})')) + drop = self.anchor[2] - max(r[1][2] for r in fallen) + results.append((drop >= FALLEN_DROP and not any(r[3] for r in fallen), + f'released: torso {drop:.3f} m below the anchor before the second toggle (at least {FALLEN_DROP}), ' + f'tilt {fallen[-1][2]:.1f} deg')) + results.append((max(map(self.distance, reheld)) <= HOLD_RADIUS and all(r[3] for r in reheld), + f're-held from the fallen pose: torso at most {max(map(self.distance, reheld)):.3f} m from ' + f'the anchor in the last second (limit {HOLD_RADIUS}), tilt {reheld[-1][2]:.1f} deg')) + return results + + + def report(self): + """Print the torso every 0.5 s, each check as PASS or FAIL, then PASSED or FAILED with the count.""" + for row in self.rows[::50]: + print(f'[check] t={row[0]:5.2f} s torso {[round(v, 3) for v in row[1]]} m, ' + f'{self.distance(row):.3f} m from the anchor, tilt {row[2]:5.1f} deg, band {row[3]}', flush=True) + results = self.checks() + for passed, text in results: + print(f"{'PASS' if passed else 'FAIL'} {text}", flush=True) + print(f"{'PASSED' if all(p for p, _ in results) else 'FAILED'}: " + f"{sum(p for p, _ in results)}/{len(results)} checks", flush=True) + + +def main(): + """Run sim_main.py with the band and toggle it twice; the last step prints the checks.""" + run = Run() + sys.path.insert(0, str(ENTRY.parent)) + import gymnasium as gym + gym.make = run.wrap(gym.make) + threading.Thread(target=run.toggle_at, args=((RELEASE_T, REHOLD_T),), daemon=True).start() + sys.argv = [str(ENTRY), '--elastic_band', '--max_steps', str(round(END_T / STEP_DT))] + sys.argv[1:] + runpy.run_path(str(ENTRY), run_name='__main__') + + +if __name__ == '__main__': + main() diff --git a/tests/isaaclab/run_elastic_band.sh b/tests/isaaclab/run_elastic_band.sh new file mode 100755 index 00000000..963c8abf --- /dev/null +++ b/tests/isaaclab/run_elastic_band.sh @@ -0,0 +1,26 @@ +#!/usr/bin/env bash +# Check the Isaac sim's elastic band holds, releases and re-holds the floating H1-2 (GPU). +# +# check_elastic_band.py runs in place of sim_main.py, under a copy of launch_isaac.sh, with +# --elastic_band for 50 s of sim time, and calls /elastic_band/toggle twice over ROS. +# Usage: tests/isaaclab/run_elastic_band.sh +# Environment: ROS_DOMAIN_ID (default 185; check_domain refuses 0), docker/.env and the shell. +# Writes tests/results/isaac-elastic-band.log. +# Exit: 0 when every check passes; nonzero otherwise, including a 900 s timeout. +set -eo pipefail +ROOT=$(cd "$(dirname "$0")/../.." && pwd) +mkdir -p "$ROOT/tests/results" +LOG="$ROOT/tests/results/isaac-elastic-band.log" +# The launcher's environment (the shared CycloneDDS library, Isaac's Python) with the check as +# its entry point; SIGINT at 900 s, then SIGKILL 30 s later, bound a run that hangs. +status=0 +ROS_DOMAIN_ID=${ROS_DOMAIN_ID:-185} "$ROOT/tests/isaaclab/run_in_container.sh" \ + 'sed -e "s#/home/code/CL_isaaclab_sim/sim_main.py#/home/code/tests/isaaclab/check_elastic_band.py#" \ + -e "s#\"\$(dirname \"\$0\")/runtime_common.sh\"#/home/code/h12_sim_scripts/runtime_common.sh#" \ + /home/code/h12_sim_scripts/launch_isaac.sh > /tmp/launch_check.sh && + timeout --signal=INT --kill-after=30s 900s bash /tmp/launch_check.sh --headless' \ + > "$LOG" 2>&1 || status=$? +# The checks' lines, then the verdict: only a run that reaches its last step prints PASSED. +[ "$status" = 0 ] || echo "run_elastic_band: the sim exited with status $status" >&2 +grep -E '^(PASS|FAIL|PASSED|FAILED)' "$LOG" || true +grep -q '^PASSED: ' "$LOG" diff --git a/tests/isaaclab/test_elastic_band.py b/tests/isaaclab/test_elastic_band.py new file mode 100644 index 00000000..61b0c3f9 --- /dev/null +++ b/tests/isaaclab/test_elastic_band.py @@ -0,0 +1,114 @@ +"""Check the Isaac sim's elastic band: its force, its toggle and what it stores on the robot, without Kit.""" +from pathlib import Path +import sys +from types import SimpleNamespace +from unittest.mock import Mock +import unittest +import torch +sys.path.insert(0, str(Path(__file__).resolve().parents[2] / 'CL_isaaclab_sim')) +from src.python.control.elastic_band import ( + BAND_DAMPING, BAND_DROP, BAND_STIFFNESS, ElasticBand, band_force, world_to_body) + + +def stand_in_robot(position=(0., 0., 1.), velocity=(0., 0., 0.)): + """Return a stand-in articulation with a pelvis and a torso_link at position, moving at velocity.""" + data = SimpleNamespace(body_names=['pelvis', 'torso_link'], + body_pos_w=torch.tensor([[[0., 0., 0.], list(position)]]), + body_quat_w=torch.tensor([[[1., 0., 0., 0.], [1., 0., 0., 0.]]]), + body_lin_vel_w=torch.tensor([[[0., 0., 0.], list(velocity)]])) + return Mock(data=data, device='cpu') + + +def stored_force(robot): + """Return the force of the robot's last set_forces_and_torques call, (num_envs, bodies, 3).""" + return robot.permanent_wrench_composer.set_forces_and_torques.call_args.kwargs['forces'] + + +class BandForceTests(unittest.TestCase): + """Run band_force on single bodies.""" + + def test_spring_pulls_toward_the_anchor(self): + """A body 0.1 m below the anchor is pulled up by stiffness times 0.1 m.""" + force = band_force(torch.tensor([[0., 0., .9]]), torch.zeros(1, 3), torch.tensor([[0., 0., 1.]]), 1e4, 0.) + torch.testing.assert_close(force, torch.tensor([[0., 0., 1000.]])) + + def test_damping_acts_on_the_whole_velocity(self): + """A body at the anchor moving sideways is damped against its velocity.""" + force = band_force(torch.tensor([[0., 0., 1.]]), torch.tensor([[.5, 0., 0.]]), torch.tensor([[0., 0., 1.]]), 1e4, 2e3) + torch.testing.assert_close(force, torch.tensor([[-1000., 0., 0.]])) + + def test_matches_the_robocasa_band(self): + """The force equals golem_robocasa's ElasticBand.evaluate_force at a displaced, moving body.""" + x, v, anchor = torch.tensor([[.03, -.02, .9]]), torch.tensor([[.1, .2, -.3]]), torch.tensor([[0., 0., 1.]]) + offset = x - anchor + expected = -BAND_STIFFNESS * offset.norm() * offset / offset.norm() - BAND_DAMPING * v + torch.testing.assert_close(band_force(x, v, anchor, BAND_STIFFNESS, BAND_DAMPING), expected) + + +class ElasticBandTests(unittest.TestCase): + """Run ElasticBand against a stand-in articulation.""" + + def test_anchor_is_below_the_torso_spawn(self): + """The anchor is the torso's position at creation, lowered by the drop.""" + band = ElasticBand(stand_in_robot()) + torch.testing.assert_close(band.anchor, torch.tensor([[0., 0., 1. - BAND_DROP]])) + self.assertTrue(band.enabled) + + def test_a_robot_without_the_body_is_refused(self): + """A missing band body raises ValueError naming it.""" + with self.assertRaisesRegex(ValueError, 'no_such_link'): + ElasticBand(stand_in_robot(), body='no_such_link') + + def test_apply_stores_the_pull_on_the_torso(self): + """While enabled, apply() stores the band's pull on the torso alone, as a body-frame force.""" + robot = stand_in_robot() + band = ElasticBand(robot) + robot.data.body_pos_w[0, 1, 2] = .9 + band.apply() + call = robot.permanent_wrench_composer.set_forces_and_torques.call_args.kwargs + torch.testing.assert_close(call['forces'], torch.tensor([[[0., 0., BAND_STIFFNESS * (.1 - BAND_DROP)]]])) + torch.testing.assert_close(call['torques'], torch.zeros(1, 1, 3)) + self.assertEqual(call['body_ids'].tolist(), [1]) + self.assertFalse(call['is_global']) + + def test_the_pull_follows_a_turned_torso(self): + """With the torso rolled 90 deg about x, the upward pull is stored along the body's +y.""" + robot = stand_in_robot() + band = ElasticBand(robot) + robot.data.body_pos_w[0, 1, 2] = .9 + robot.data.body_quat_w[0, 1] = torch.tensor([2 ** -.5, 2 ** -.5, 0., 0.]) + band.apply() + torch.testing.assert_close(stored_force(robot), torch.tensor([[[0., BAND_STIFFNESS * (.1 - BAND_DROP), 0.]]]), + atol=1e-3, rtol=1e-5) + + def test_world_to_body_inverts_the_orientation(self): + """A body yawed 90 deg sees the world's +x along its own -y.""" + yaw = torch.tensor([[2 ** -.5, 0., 0., 2 ** -.5]]) + torch.testing.assert_close(world_to_body(yaw, torch.tensor([[1., 0., 0.]])), torch.tensor([[0., -1., 0.]]), + atol=1e-6, rtol=0) + + def test_release_zeroes_the_stored_force_once(self): + """After a release, the next apply() stores zero and the ones after it write nothing.""" + robot = stand_in_robot() + band = ElasticBand(robot) + band.apply() + self.assertFalse(band.toggle()) + band.apply() + torch.testing.assert_close(stored_force(robot), torch.zeros(1, 1, 3)) + band.apply() + self.assertEqual(robot.permanent_wrench_composer.set_forces_and_torques.call_count, 2) + + def test_toggle_back_on_pulls_again(self): + """A second toggle enables the band, and apply() stores its pull again.""" + robot = stand_in_robot() + band = ElasticBand(robot) + robot.data.body_pos_w[0, 1, 2] = .9 + band.toggle() + band.apply() + self.assertTrue(band.toggle()) + band.apply() + self.assertGreater(float(stored_force(robot)[0, 0, 2]), 0.) + + +if __name__ == '__main__': + unittest.main() diff --git a/tests/isaaclab/test_implicit_action.py b/tests/isaaclab/test_implicit_action.py index b27cbe7d..595e7a8f 100644 --- a/tests/isaaclab/test_implicit_action.py +++ b/tests/isaaclab/test_implicit_action.py @@ -33,6 +33,7 @@ def test_targets_and_gains_reach_solver_without_explicit_pd_torque(self): """Targets, gains and feed-forward torque reach the drives with no PD torque added, unchanged gains are not rewritten, and mode 0 zeroes them.""" action = object.__new__(module.UnitreeMotorAction) action.num_envs = 1 + action._env = SimpleNamespace() action._body_ids = list(range(27)) action._hand_ids = list(range(27,39)) action._coupled_ids = list(range(39,51)) diff --git a/tests/robocasa/README.md b/tests/robocasa/README.md index fc19619b..ae4bc888 100644 --- a/tests/robocasa/README.md +++ b/tests/robocasa/README.md @@ -51,42 +51,30 @@ PYTHONPATH=golem_robocasa python3 -m unittest discover -s tests/robocasa -p test factor); its gripper tests, which check that the grippers' rates count sim time, need rclpy and are skipped outside the container. -To measure the actual kitchen loop, add `--timing` to a normal launch: +To measure the actual kitchen loop, read the sim's own log of a normal launch: ```bash -docker/scripts/docker_run.sh robocasa --headless --task OpenFridge --layout 1 --style 1 --seed 42 --timing +docker/scripts/docker_run.sh robocasa --headless --task OpenFridge --layout 1 --style 1 --seed 42 ``` -Every five wall-clock seconds, `[sim-timing]` reports simulation/wall-clock time -(RTF: 1 means real time) and mean/max milliseconds per stage call, with call -counts. `camera_render` includes scene capture and both render passes per -camera (with the wait for `mid360.GL_LOCK`, which the cameras and the lidar's -GPU caster take turns on); `camera_depth_render` the depth stream's render; -`camera_publish` image conversion, compression, and publishing; `camera_cloud` -the point cloud. `lidar_capture` is the physics thread's copy of the poses one -lidar cast reads, under the sim lock; `lidar_wait` the physics thread waiting -for a free slot when the lidar's thread is a frame behind; `lidar_cast` one -cast of the rays due since the last (every caster's `batch_s` of sim time, on -the lidar's thread); `lidar_publish` packing and publishing a frame on both -lidar topics. The line ends with the lidar's counts: frames, the casts that -waited, and how far behind the sim a frame was at most. Physics includes lock -acquisition and PD control; `lowstate` is the `rt/lowstate` sample built, -signed and published after every physics step. Timings are silent unless requested. +Every 30 s of sim time it logs `real-time factor ... over the last 30 s`, the share of +wall time the loop slept, and the sensors' counts: lidar frames, the casts that made the +physics wait and how far behind the sim a frame was at most, and on the G1 the head +camera's frames published and dropped. `GOLEM_SIM_RATE=max` gives the unpaced ceiling. Both sims build that sample with numpy (`LowStateWriter` in `unitree_common.py`); `test_lowstate_writer.py` checks its bytes against `unitree_sdk2py`'s serializer and CRC, and that they take at most a third of the SDK path's time, printing both paths' microseconds per sample. Compare the same task/layout/style/seed, display mode, subscribers, and hardware. -Discard the first interval for renderer warm-up. PD torque is recomputed every +Discard the first line for renderer warm-up. PD torque is recomputed every physics step. RoboCasa fixture updates and success checks run once per control step (`env.control_timestep`, 20 Hz), matching RoboCasa's own `env.step`. Camera frames are rendered from a partial `MjData` snapshot on a worker thread, overlapping physics, with one job in flight. A due job waits for the -previous one rather than being dropped, so every scheduled frame is published. -`camera_capture` is the locked snapshot, `camera_wait` is time the loop blocked -on the previous job. The MID-360's casts run on their own thread from a ring of +previous one rather than being dropped, so every scheduled frame is published +(the H1-2's cameras; the G1's head camera drops a frame due while it renders). The MID-360's casts run on their own thread from a ring of pose snapshots (`mid360.ScanRing`): the ring holds one frame of casts, and when the thread falls that far behind the loop waits for it, so the real-time factor drops instead of the lidar falling more than a frame behind. @@ -94,7 +82,7 @@ The launcher selects EGL for offscreen camera rendering in both display modes; the camera context is created before the passive viewer opens, because a new EGL context cannot be made current once the GLFW window exists. The viewer draws its own `MjData`, refreshed and synced at 60 Hz on a separate thread -(`viewer_sync.py`, shared by both sims; the H1-2's `viewer` stage), so +(`viewer_sync.py`, shared by both robots), so `handle.sync()` never blocks physics. Mouse perturbation forces from each sync are applied to the simulation on every step. The thread also shows the measured real-time factor (sim time over the last second of wall time), the diff --git a/tests/robocasa/g1/test_g1_band.py b/tests/robocasa/g1/test_g1_band.py index 84eb0bb6..bb2c11cb 100644 --- a/tests/robocasa/g1/test_g1_band.py +++ b/tests/robocasa/g1/test_g1_band.py @@ -2,7 +2,7 @@ The kitchen is tests/robocasa/surface/run.sh's: layout 9, style 9, seed 42. The env is built headless with g1_robosuite.make_kitchen_env, the robot spawned and the band built by -g1_mujoco's own spawn() and make_band(), and the physics stepped as sim_loop steps it: the +g1_robot's own spawn() and make_band(), and the physics stepped as sim_loop steps it: the interface's motor torques (SimInterface with no rt/lowcmd, so every motor off), the band's force (mujoco_env.apply_band), mj_step. Run it in the robocasa image with golem_robocasa on PYTHONPATH; tests/robocasa/g1/run_in_container.sh runs it with no network on ROS_DOMAIN_ID 97, because @@ -23,7 +23,7 @@ import mujoco_env import robosuite_common import sim_common -from g1 import g1_mujoco, g1_robosuite +from g1 import g1_robot, g1_robosuite from g1.sim_names import NameResolver from g1.unitree_interface import SimInterface @@ -46,7 +46,7 @@ def _sim(): env.reset() model, data = env.sim.model._model, env.sim.data._data resolver = NameResolver(model, robot_prefix=env.robots[0].robot_model.naming_prefix) - g1_mujoco.spawn(env, resolver) + g1_robot.spawn(env, resolver) iface = SimInterface(model, data, resolver) spawn = (data.qpos.copy(), data.qvel.copy(), float(data.time)) return env, model, data, resolver, iface, spawn @@ -60,7 +60,7 @@ def _from_spawn(): data.xfrc_applied[:] = 0.0 mujoco.mj_forward(model, data) iface.sim_time = t - band, body = g1_mujoco.make_band(data, resolver) + band, body = g1_robot.make_band(data, resolver) return model, data, resolver, iface, band, body @@ -89,7 +89,7 @@ def test_spawn_clear_of_floor_and_kitchen(): contacts = robosuite_common.robot_env_contacts(model, data) assert not contacts, f"{len(contacts)} robot<->kitchen contacts at spawn" assert np.allclose(band.point - data.xpos[body], [0.0, 0.0, 0.01]) - # Twice the H1-2 band's stiffness: at its 1e4 N/m the limp G1 keeps rocking (g1_mujoco.py). + # Twice the H1-2 band's stiffness: at its 1e4 N/m the limp G1 keeps rocking (g1_robot.py). assert (band.stiffness, band.damping, band.length) == (2e4, 2e3, 0) assert band.enabled diff --git a/tests/robocasa/g1/test_g1_sim_rate.py b/tests/robocasa/g1/test_g1_sim_rate.py index ff7a53c1..3799a026 100644 --- a/tests/robocasa/g1/test_g1_sim_rate.py +++ b/tests/robocasa/g1/test_g1_sim_rate.py @@ -1,6 +1,6 @@ """Check that the G1 sim's loop paces to GOLEM_SIM_RATE through the shared pacer (sim_timing.RealtimePacer). -Each test runs g1_mujoco.sim_loop headless in the Blank task, sensors off, in a subprocess +Each test runs golem_sim.sim_loop on the G1 headless in the Blank task, sensors off, in a subprocess (it starts ROS and Unitree DDS), with the pacer replaced by a subclass that records how it was built and counts its waits. Run it in the robocasa image with golem_robocasa on PYTHONPATH; tests/robocasa/g1/run_in_container.sh runs it with no network on ROS_DOMAIN_ID 97. @@ -19,7 +19,8 @@ LOOP = textwrap.dedent(""" import json import sim_timing - from g1 import g1_mujoco + import golem_sim + from g1 import g1_robot pacers = [] @@ -32,8 +33,8 @@ def wait(self): pacers[-1]["waits"] += 1 return super().wait() - g1_mujoco.RealtimePacer = RecordedPacer - g1_mujoco.sim_loop("Blank", viewer=False, duration=%r) + sim_timing.RealtimePacer = RecordedPacer + golem_sim.sim_loop(g1_robot.G1Robot(), "Blank", viewer=False, duration=%r) print("PACERS " + json.dumps(pacers)) """ % DURATION) @@ -59,7 +60,7 @@ def test_rate_two_paces_every_step_at_twice_real_time(): (pacer,) = _pacers(r) assert pacer["rate"] == 2.0, pacer assert pacer["waits"] == round(DURATION / pacer["timestep"]), pacer - assert "[g1_mujoco] sim rate: 2x real time (GOLEM_SIM_RATE)" in r.stdout + assert "[golem_sim] sim rate: 2x real time (GOLEM_SIM_RATE)" in r.stdout def test_default_is_real_time(): @@ -67,7 +68,7 @@ def test_default_is_real_time(): r = _run("") (pacer,) = _pacers(r) assert pacer["rate"] == 1.0, pacer - assert "[g1_mujoco] sim rate: 1x real time (GOLEM_SIM_RATE)" in r.stdout + assert "[golem_sim] sim rate: 1x real time (GOLEM_SIM_RATE)" in r.stdout def test_max_is_unpaced(): @@ -75,7 +76,7 @@ def test_max_is_unpaced(): r = _run("max") (pacer,) = _pacers(r) assert pacer["rate"] == float("inf"), pacer - assert "[g1_mujoco] sim rate: unpaced" in r.stdout + assert "[golem_sim] sim rate: unpaced" in r.stdout def test_bad_rate_stops_before_the_scene_is_built(): diff --git a/tests/robocasa/spawn_scene.py b/tests/robocasa/spawn_scene.py index 682c396e..865b2be7 100644 --- a/tests/robocasa/spawn_scene.py +++ b/tests/robocasa/spawn_scene.py @@ -6,7 +6,7 @@ robosuite module, with stand-ins for the robosuite modules it imports where robosuite is not installed (the host), so the placement functions run on numpy and mujoco alone. Used by test_robosuite_common.py, test_h12_spawn_collision.py and g1/test_g1_spawn_collision.py. -import_h12_mujoco imports the H1-2's runner without its gripper bridge, for the tests that spawn +import_h12_robot imports the H1-2's runner without its gripper bridge, for the tests that spawn the H1-2 in a real env (test_blank_env.py, test_fixture_collisions.py, test_mid360_gpu.py). """ import importlib @@ -142,8 +142,8 @@ def mod(name): mod("robosuite.robots").register_robot_class = lambda *a, **k: (lambda cls: cls) -def import_h12_mujoco(): - """Import and return h12_mujoco, with a stand-in gripper bridge unless a test registered its own. +def import_h12_robot(): + """Import and return h12_robot, with a stand-in gripper bridge unless a test registered its own. The real bridge needs magpie_msgs, which the test containers' shell does not source; the spawn and the band do not use it. @@ -152,7 +152,7 @@ def import_h12_mujoco(): stub = types.ModuleType("magpie_hand_bridge") stub.MagpieHandBridge = None sys.modules["magpie_hand_bridge"] = stub - return importlib.import_module("h12_mujoco") + return importlib.import_module("h12_robot") def import_robot_module(name): diff --git a/tests/robocasa/test_blank_env.py b/tests/robocasa/test_blank_env.py index 1819b135..31c3582c 100644 --- a/tests/robocasa/test_blank_env.py +++ b/tests/robocasa/test_blank_env.py @@ -2,7 +2,7 @@ The env builds, holds only the floor and the robot, spawns the robot at the origin facing +x, and the elastic band holds the limp robot up. Each robot is spawned and banded as its runner -does it: the G1 with g1_mujoco's own spawn() and make_band(); the H1-2 with h12_mujoco's spawn() +does it: the G1 with g1_robot's own spawn() and make_band(); the H1-2 with h12_robot's spawn() and its band's values, which sit inline in its sim loop. No motor is driven (ctrl 0), as in both sims before the first rt/lowcmd. @@ -15,7 +15,7 @@ import mujoco import numpy as np -from spawn_scene import import_h12_mujoco +from spawn_scene import import_h12_robot STEPS = 500 # physics steps with the band on # m the pelvis may sink below its spawn height while hanging: the feet drop onto the floor from @@ -25,7 +25,7 @@ # m: the most the lowest robot geom may stand above the floor at spawn (the runners' # clearances are 0.02 for the H1-2 and 0.001 for the G1). SPAWN_CLEARANCE_MAX = 0.03 -# The H1-2's band as h12_mujoco's sim loop builds it: anchor offset from the torso (m), +# The H1-2's band as h12_robot's sim loop builds it: anchor offset from the torso (m), # stiffness (N/m) and damping (N*s/m). H12_BAND = dict(dz=-0.04, stiffness=1e4, damping=2e3) # Bodies robosuite's Arena adds to every scene: mocap end-effector targets for its GUI, with @@ -44,12 +44,12 @@ def _h12(): import robosuite_common from mujoco_env import ElasticBand from sim_names import NameResolver - h12_mujoco = import_h12_mujoco() + h12_robot = import_h12_robot() env = h1_2_robosuite.make_kitchen_env("Blank") env.reset() model, data = env.sim.model._model, env.sim.data._data resolver = NameResolver(model) - h12_mujoco.spawn(env, resolver) + h12_robot.spawn(env, resolver) torso = resolver.body_id("torso_link") anchor = data.xpos[torso].copy() anchor[2] += H12_BAND["dz"] @@ -65,14 +65,14 @@ def _g1(): Returns (env, model, data, pelvis body, band, band body, lowest robot geom z). """ - from g1 import g1_mujoco, g1_robosuite + from g1 import g1_robot, g1_robosuite from g1.sim_names import NameResolver env = g1_robosuite.make_kitchen_env("Blank") env.reset() model, data = env.sim.model._model, env.sim.data._data resolver = NameResolver(model, robot_prefix=env.robots[0].robot_model.naming_prefix) - g1_mujoco.spawn(env, resolver) - band, body = g1_mujoco.make_band(data, resolver) + g1_robot.spawn(env, resolver) + band, body = g1_robot.make_band(data, resolver) return env, model, data, resolver.base_body, band, body, \ g1_robosuite.lowest_robot_point_z(env) diff --git a/tests/robocasa/test_fixture_collisions.py b/tests/robocasa/test_fixture_collisions.py index 08ffe410..25ab1d85 100644 --- a/tests/robocasa/test_fixture_collisions.py +++ b/tests/robocasa/test_fixture_collisions.py @@ -12,7 +12,7 @@ import numpy as np import sim_common -from spawn_scene import import_h12_mujoco +from spawn_scene import import_h12_robot LAYOUT, STYLE, SEED = 9, 9, 42 # the surface probe's kitchen PAIR_STEPS = 100 # physics steps over which the broadphase pairs are recorded @@ -25,26 +25,26 @@ def _h12_kitchen(task): - """Return the H1-2's env for task, spawned by h12_mujoco.spawn().""" - h12_mujoco = import_h12_mujoco() + """Return the H1-2's env for task, spawned by h12_robot.spawn().""" + h12_robot = import_h12_robot() import h1_2_robosuite from sim_names import NameResolver env = h1_2_robosuite.make_kitchen_env(task, layout_ids=LAYOUT, style_ids=STYLE, seed=SEED) env.reset() model, data = env.sim.model._model, env.sim.data._data - h12_mujoco.spawn(env, NameResolver(model)) + h12_robot.spawn(env, NameResolver(model)) mujoco.mj_forward(model, data) return env def _g1_kitchen(task): - """Return the G1's env for task, spawned by g1_mujoco.spawn().""" - from g1 import g1_mujoco, g1_robosuite + """Return the G1's env for task, spawned by g1_robot.spawn().""" + from g1 import g1_robot, g1_robosuite from g1.sim_names import NameResolver env = g1_robosuite.make_kitchen_env(task, layout_ids=LAYOUT, style_ids=STYLE, seed=SEED) env.reset() model, data = env.sim.model._model, env.sim.data._data - g1_mujoco.spawn(env, NameResolver(model, robot_prefix=env.robots[0].robot_model.naming_prefix)) + g1_robot.spawn(env, NameResolver(model, robot_prefix=env.robots[0].robot_model.naming_prefix)) mujoco.mj_forward(model, data) return env @@ -208,7 +208,7 @@ def test_objects_still_rest_in_fixtures(self): An object must still touch the fixture it was placed in; a loose fixture (on a free joint) must still touch a geom of another fixture. Neither may sink. """ - from g1 import g1_mujoco + from g1 import g1_robot from g1.sim_names import NameResolver from mujoco_env import apply_band for task in RESTING_TASKS: @@ -216,7 +216,7 @@ def test_objects_still_rest_in_fixtures(self): env = _g1_kitchen(task) model, data = env.sim.model._model, env.sim.data._data geoms = _Geoms(env) - band, band_body = g1_mujoco.make_band( + band, band_body = g1_robot.make_band( data, NameResolver(model, robot_prefix=env.robots[0].robot_model.naming_prefix)) vel = np.zeros(6) self.assertGreater(sim_common.exclude_fixture_collisions(env), 0) diff --git a/tests/robocasa/test_gpu_required.py b/tests/robocasa/test_gpu_required.py index 59320a7c..904f822e 100644 --- a/tests/robocasa/test_gpu_required.py +++ b/tests/robocasa/test_gpu_required.py @@ -5,7 +5,7 @@ sim_common.egl_renderer() is the probe and sim_common.require_gpu() the stop. The probe is made to fail three ways: a failing probe passed in, MuJoCo not on EGL (MUJOCO_GL=osmesa) and Mesa's -software EGL in place of NVIDIA's, the last two in h12_mujoco.py run as a subprocess for each +software EGL in place of NVIDIA's, the last two in golem_sim.py run as a subprocess for each robot; with the probe failing, --help and a usage error still answer. Run it in the robocasa image, on its GPU: ROS_DOMAIN_ID=231 tests/robocasa/run_in_container.sh -p test_gpu_required.py @@ -17,7 +17,7 @@ import sim_common -SIM = os.path.join(os.path.dirname(os.path.abspath(sim_common.__file__)), "h12_mujoco.py") +SIM = os.path.join(os.path.dirname(os.path.abspath(sim_common.__file__)), "golem_sim.py") # glvnd's vendor file for Mesa's EGL: with only it listed, EGL renders on Mesa's software # rasteriser even where NVIDIA's EGL is installed. MESA_EGL_VENDOR = "/usr/share/glvnd/egl_vendor.d/50_mesa.json" @@ -94,7 +94,7 @@ class SimStartTests(unittest.TestCase): """Run each robot's sim with the probe made to fail and check it stops at start.""" def start(self, robot, *args, **env): - """Run h12_mujoco.py for robot with args (default: headless in the Blank task) and env added. + """Run golem_sim.py for robot with args (default: headless in the Blank task) and env added. Returns the CompletedProcess. """ diff --git a/tests/robocasa/test_h12_lidar_mount.py b/tests/robocasa/test_h12_lidar_mount.py index 8b917edd..4752585c 100644 --- a/tests/robocasa/test_h12_lidar_mount.py +++ b/tests/robocasa/test_h12_lidar_mount.py @@ -22,7 +22,6 @@ import h12_sensors import mid360 import sim_common -from sim_timing import LoopTimings ASSETS = os.path.join(os.path.dirname(os.path.abspath(__file__)), '..', '..', 'CL_Assets') URDF = os.path.join(ASSETS, 'ros_assets', 'h1_2_magpie.urdf') @@ -135,7 +134,7 @@ def test_h12_frame_has_20000_rays_on_4_lines(self): robot = model.body_rootid[model.geom_bodyid] == model.body_rootid[torso] published = [] scans = h12_sensors._LidarScans(model, body('livox_link'), torso, robot, 'livox_frame', - published.append, lambda msg: None, LoopTimings()) + published.append, lambda msg: None) frames = [] update = scans.scanner.update diff --git a/tests/robocasa/test_loop_timing.py b/tests/robocasa/test_loop_timing.py index b9807850..b2dde4b9 100644 --- a/tests/robocasa/test_loop_timing.py +++ b/tests/robocasa/test_loop_timing.py @@ -1,9 +1,7 @@ -"""Check the H1-2 sim loop's real-time pacer and its --timing report (sim_timing.py), without ROS or MuJoCo.""" -import io +"""Check the sim loop's real-time pacer (sim_timing.py), without ROS or MuJoCo.""" import unittest -from contextlib import redirect_stdout -from sim_timing import RealtimePacer, LoopTimings +from sim_timing import RealtimePacer class FakeClock: @@ -25,7 +23,7 @@ def sleep(self, delay): class PacingTests(unittest.TestCase): - """Check RealtimePacer and LoopTimings against a fake clock.""" + """Check RealtimePacer against a fake clock.""" def test_fast_steps_track_the_original_deadline(self): """Steps faster than real time are slowed to one 10 ms step per 10 ms.""" @@ -66,58 +64,6 @@ def test_invalid_timestep_is_rejected(self): with self.subTest(value=value), self.assertRaises(ValueError): RealtimePacer(value) - def test_timing_is_silent_by_default(self): - """Timings print nothing unless enabled.""" - timing = LoopTimings() - with redirect_stdout(io.StringIO()) as output: - timing.stop('physics', timing.start()) - timing.maybe_report(1.0) - self.assertEqual(output.getvalue(), '') - - def test_timing_baseline_excludes_startup_and_existing_sim_time(self): - """The real-time factor counts from the reset, not from start-up or sim time before it.""" - clock = FakeClock() - timing = LoopTimings(enabled=True, interval=1, clock=clock) - clock.now = 10.0 - timing.reset(5.0) - clock.now = 11.0 - with redirect_stdout(io.StringIO()) as output: - timing.maybe_report(5.5) - self.assertIn('RTF=0.500', output.getvalue()) - - def test_timing_reports_sim_wall_ratio_and_stage_cost(self): - """The report gives the real-time factor and each stage's cost in ms.""" - clock = FakeClock() - timing = LoopTimings(enabled=True, interval=1, clock=clock) - start = timing.start() - clock.now = 0.02 - timing.stop('physics', start) - clock.now = 1.0 - with redirect_stdout(io.StringIO()) as output: - timing.maybe_report(0.5) - self.assertIn('RTF=0.500', output.getvalue()) - self.assertIn('physics', output.getvalue()) - self.assertIn('20.00', output.getvalue()) - - def test_timing_report_appends_the_sensor_stats(self): - """stats() is called only when a report is due, and its text ends the line.""" - clock = FakeClock() - timing = LoopTimings(enabled=True, interval=1, clock=clock) - calls = [] - - def stats(): - """Return the sensors' counts and record the call.""" - calls.append(clock.now) - return '9 lidar frames' - - with redirect_stdout(io.StringIO()) as output: - clock.now = 0.5 - timing.maybe_report(0.5, stats) - clock.now = 1.0 - timing.maybe_report(1.0, stats) - self.assertEqual(calls, [1.0]) - self.assertTrue(output.getvalue().rstrip().endswith('; 9 lidar frames'), output.getvalue()) - if __name__ == '__main__': unittest.main() diff --git a/tests/robocasa/test_mid360.py b/tests/robocasa/test_mid360.py index afa5975c..fa681a34 100644 --- a/tests/robocasa/test_mid360.py +++ b/tests/robocasa/test_mid360.py @@ -14,7 +14,6 @@ import mid360 from exact_rays import ExactCaster -from sim_timing import LoopTimings FRAME_POINTS = 20_000 # POINTS_PER_SECOND / FRAME_RATE_HZ STEP_S = 0.01 # s of sim time between update() calls (ExactCaster.batch_s) @@ -256,7 +255,7 @@ def test_scanner_restarts_after_a_reset_inside_the_first_frame(self): class ScanRingTests(unittest.TestCase): """Check ScanRing's depth, backpressure and frame lag.""" - def ring(self, cast_s=0.0, interval=STEP_S, timings=None): + def ring(self, cast_s=0.0, interval=STEP_S): """Return a started ScanRing in a 3 m cube room whose casts take at least cast_s wall s. Frames go to self.frames, each with the ring's sim time when it was published. @@ -276,8 +275,7 @@ def slow(*args, **kwargs): self.frames = [] ring = mid360.ScanRing(mid360.Mid360Scanner(caster), model, lambda d: (np.zeros(3), UPSIDE_DOWN), - lambda frame: self.frames.append((frame, ring.now)), interval, - timings=timings) + lambda frame: self.frames.append((frame, ring.now)), interval) self.addCleanup(ring.close) self.data = data return ring @@ -294,10 +292,9 @@ def test_a_slow_caster_makes_tick_wait_and_no_frame_lags_a_frame_period(self): Casts of 25 ms every 10 ms of sim time: the physics loop below, which would run far faster than real time, is held to the casts' pace. Every frame still fires all 20,000 rays (each hits the room), and at its publication the sim is at most one frame period - past the frame's end. Each wait is timed as the lidar_wait stage. + past the frame's end. """ - timings = LoopTimings(enabled=True) - ring = self.ring(cast_s=0.025, timings=timings) + ring = self.ring(cast_s=0.025) start = time.perf_counter() for k in range(301): # 0.6 s of sim time in 2 ms steps ring.tick(self.data, k * 0.002) @@ -315,8 +312,6 @@ def test_a_slow_caster_makes_tick_wait_and_no_frame_lags_a_frame_period(self): self.assertGreater(ring.max_lag, 0.05) self.assertLessEqual(ring.max_lag, 0.1 + 1e-9) self.assertIn(f"6 lidar frames ({ring.waits} of 61 casts waited", ring.stats()) - self.assertEqual(timings.stages["lidar_wait"][1], ring.waits) - self.assertEqual(timings.stages["lidar_capture"][1], 61) def test_a_fast_caster_never_waits(self): """tick() does not wait, nor count a wait, while the casts keep up with the loop.""" diff --git a/tests/robocasa/test_mid360_gpu.py b/tests/robocasa/test_mid360_gpu.py index e3f984cd..15ce5bc6 100644 --- a/tests/robocasa/test_mid360_gpu.py +++ b/tests/robocasa/test_mid360_gpu.py @@ -18,7 +18,7 @@ import mid360 import sim_common from exact_rays import ExactCaster -from spawn_scene import import_h12_mujoco +from spawn_scene import import_h12_robot LAYOUT, STYLE, SEED = 9, 9, 42 # the surface probe's kitchen FRAME_POINTS = 20_000 # one 10 Hz frame of the pattern @@ -79,15 +79,15 @@ def _rotation(axis, degrees): @functools.lru_cache(maxsize=None) def _g1(): - """Return the G1's Kitchen, spawned by g1_mujoco.spawn() at RoboCasa's spawn.""" - from g1 import g1_lidar, g1_mujoco, g1_robosuite + """Return the G1's Kitchen, spawned by g1_robot.spawn() at RoboCasa's spawn.""" + from g1 import g1_lidar, g1_robot, g1_robosuite from g1.sim_names import NameResolver env = g1_robosuite.make_kitchen_env(sim_common.random_task(seed=SEED), layout_ids=LAYOUT, style_ids=STYLE, seed=SEED) env.reset() model, data = env.sim.model._model, env.sim.data._data prefix = env.robots[0].robot_model.naming_prefix - g1_mujoco.spawn(env, NameResolver(model, robot_prefix=prefix)) + g1_robot.spawn(env, NameResolver(model, robot_prefix=prefix)) mujoco.mj_forward(model, data) site = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, prefix + g1_lidar.SITE) return Kitchen(model, data, int(model.site_bodyid[site]), g1_lidar.GEOM_GROUPS, @@ -96,8 +96,8 @@ def _g1(): @functools.lru_cache(maxsize=None) def _h12(): - """Return the H1-2's Kitchen, spawned by h12_mujoco.spawn() at RoboCasa's spawn.""" - h12_mujoco = import_h12_mujoco() + """Return the H1-2's Kitchen, spawned by h12_robot.spawn() at RoboCasa's spawn.""" + h12_robot = import_h12_robot() import h1_2_robosuite from sim_names import NameResolver env = h1_2_robosuite.make_kitchen_env(sim_common.random_task(seed=SEED), layout_ids=LAYOUT, @@ -105,7 +105,7 @@ def _h12(): env.reset() model, data = env.sim.model._model, env.sim.data._data resolver = NameResolver(model) - h12_mujoco.spawn(env, resolver) + h12_robot.spawn(env, resolver) mujoco.mj_forward(model, data) lidar = resolver.body_id("livox_link") return Kitchen(model, data, resolver.body_id("torso_link"), H12_GROUPS, diff --git a/tests/robocasa/test_robot_select.py b/tests/robocasa/test_robot_select.py index c434a37e..1af37d9b 100644 --- a/tests/robocasa/test_robot_select.py +++ b/tests/robocasa/test_robot_select.py @@ -89,14 +89,14 @@ def test_defaults(self): """The common options' defaults.""" args = self.parser.parse_args([]) self.assertEqual(vars(args), dict(robot=None, headless=False, task=None, layout=None, - style=None, seed=None, timing=False, duration=0.0)) + style=None, seed=None)) def test_values(self): """Every common option parses to its value and type.""" args = self.parser.parse_args(['--g1', '--headless', '--task', 'OpenFridge', '--layout', '9', - '--style', '8', '--seed', '42', '--timing', '--duration', '2.5']) + '--style', '8', '--seed', '42']) self.assertEqual(vars(args), dict(robot='g1', headless=True, task='OpenFridge', layout=9, - style=8, seed=42, timing=True, duration=2.5)) + style=8, seed=42)) def test_robot_flags_are_the_shared_pair(self): """The common options include the mutually exclusive robot flags.""" @@ -116,7 +116,7 @@ class ParseArgsTests(unittest.TestCase): def setUp(self): """Make a parser with the common options.""" - self.parser = argparse.ArgumentParser(prog='h12_mujoco.py') + self.parser = argparse.ArgumentParser(prog='golem_sim.py') add_common_args(self.parser) def parse_error(self, argv, robot): @@ -153,20 +153,12 @@ def test_abbreviated_flag_that_names_this_robot_is_accepted(self): self.assertEqual(parse_args(self.parser, ['--h1'], 'h12').robot, 'h12') self.assertEqual(parse_args(self.parser, ['--g'], 'g1').robot, 'g1') - def test_h12_has_no_duration_yet(self): - """The H1-2 runner refuses a nonzero --duration; the G1 runner takes it.""" - code, err = self.parse_error(['--duration', '5'], 'h12') - self.assertEqual(code, 2) - self.assertIn('--duration is not supported for the H1-2 yet', err) - self.assertEqual(parse_args(self.parser, ['--duration', '5'], 'g1').duration, 5.0) - self.assertEqual(parse_args(self.parser, ['--duration', '0'], 'h12').duration, 0.0) - - def test_g1_has_no_timing_yet(self): - """The G1 runner refuses --timing; the H1-2 runner takes it.""" - code, err = self.parse_error(['--timing'], 'g1') - self.assertEqual(code, 2) - self.assertIn('--timing is not supported for the G1 yet', err) - self.assertTrue(parse_args(self.parser, ['--timing'], 'h12').timing) + def test_no_timing_or_duration_option(self): + """Neither robot's runner takes --timing or --duration.""" + for robot in ('h12', 'g1'): + for flag in (['--timing'], ['--duration', '5']): + code, _ = self.parse_error(flag, robot) + self.assertEqual(code, 2) def test_other_options_pass_through(self): """Other options parse unchanged.""" diff --git a/tests/robocasa/test_sensor_bridge.py b/tests/robocasa/test_sensor_bridge.py index 6a20d21b..9f0f7351 100644 --- a/tests/robocasa/test_sensor_bridge.py +++ b/tests/robocasa/test_sensor_bridge.py @@ -28,7 +28,6 @@ self_filter_geoms, PC2_DTYPE, _COMPRESSED_DEPTH_HEADER, _JPEG_QUALITY, ) from mujoco_ros_bridge import RosSensorBridge, _sim_time_to_msg -from sim_timing import LoopTimings SCENE = ''' @@ -230,7 +229,6 @@ def setUp(self): _camera_info(spec.depth_info, 'depth_optical'), {k: Recorder(b.sim_lock) for k in keys})] b.get_logger = lambda: SimpleNamespace(warn=lambda text: self.fail(text)) - b.timings = LoopTimings() b._camera_worker, b._camera_pending = ThreadPoolExecutor(max_workers=1), None self.addCleanup(b.shutdown) @@ -504,7 +502,7 @@ def scans(self, robot_geoms=None): self.cloud, self.custom = Recorder(self.lock), Recorder(self.lock) scans = _LidarScans(self.model, self.lidar, self.torso, self.robot if robot_geoms is None else robot_geoms, 'livox_frame', - self.custom.publish, self.cloud.publish, LoopTimings(), self.lock) + self.custom.publish, self.cloud.publish, self.lock) self.addCleanup(scans.close) return scans diff --git a/tests/robocasa/test_sim_common_tasks.py b/tests/robocasa/test_sim_common_tasks.py index a2a667a4..4670577e 100644 --- a/tests/robocasa/test_sim_common_tasks.py +++ b/tests/robocasa/test_sim_common_tasks.py @@ -17,8 +17,8 @@ # The --task hooks passed to resolve_task, one per runner, each claiming the Battery and Blank tasks. OWN_TASKS = battery_env.TASKS + blank_env.TASKS -H12_TASKS = {'tag': 'h12_mujoco', 'robot_task': lambda name: sim_common.canonical_task(name, OWN_TASKS)} -G1_TASKS = {'tag': 'g1_mujoco', 'robot_task': lambda name: sim_common.canonical_task(name, OWN_TASKS)} +H12_TASKS = {'tag': 'golem_sim', 'robot_task': lambda name: sim_common.canonical_task(name, OWN_TASKS)} +G1_TASKS = {'tag': 'golem_sim', 'robot_task': lambda name: sim_common.canonical_task(name, OWN_TASKS)} def _resolve(*args, **kwargs): @@ -38,33 +38,33 @@ class ResolveTaskTests(unittest.TestCase): def test_abstract_base_picks_the_same_subclass_as_before(self): """A seeded abstract task base resolves to a fixed concrete subclass and says so.""" - task, out = _resolve('OpenDoor', seed=1, tag='h12_mujoco') + task, out = _resolve('OpenDoor', seed=1, tag='golem_sim') self.assertEqual(task, 'OpenCabinet') self.assertEqual( - out, "[h12_mujoco] 'OpenDoor' is an abstract task base; randomly selected " + out, "[golem_sim] 'OpenDoor' is an abstract task base; randomly selected " "concrete subclass 'OpenCabinet' (from 3 options)\n") - task, _ = _resolve('ManipulateDoor', seed=1, tag='h12_mujoco') + task, _ = _resolve('ManipulateDoor', seed=1, tag='golem_sim') self.assertEqual(task, 'CloseFridge') def test_log_prefix_is_the_robots_tag(self): """resolve_task's message starts with the runner's tag.""" - _, out = _resolve('OpenDoor', seed=1, tag='g1_mujoco') - self.assertTrue(out.startswith("[g1_mujoco] 'OpenDoor' is an abstract task base"), out) + _, out = _resolve('OpenDoor', seed=1, tag='golem_sim') + self.assertTrue(out.startswith("[golem_sim] 'OpenDoor' is an abstract task base"), out) def test_concrete_task_is_returned_unchanged(self): """A concrete task resolves to itself, silently.""" - task, out = _resolve('OpenFridge', tag='g1_mujoco') + task, out = _resolve('OpenFridge', tag='golem_sim') self.assertEqual((task, out), ('OpenFridge', '')) def test_unknown_task_exits_with_the_tag(self): """An unknown task exits with a message under the runner's tag.""" with self.assertRaises(SystemExit) as caught: - sim_common.resolve_task('NoSuchTask', tag='g1_mujoco') - self.assertEqual(caught.exception.code, "[g1_mujoco] unknown RoboCasa task 'NoSuchTask'") + sim_common.resolve_task('NoSuchTask', tag='golem_sim') + self.assertEqual(caught.exception.code, "[golem_sim] unknown RoboCasa task 'NoSuchTask'") def test_robot_hook_claims_its_task_first(self): """A runner's task hook resolves its own task before RoboCasa's.""" - task, out = _resolve('battery', tag='h12_mujoco', + task, out = _resolve('battery', tag='golem_sim', robot_task=lambda name: sim_common.canonical_task(name, battery_env.TASKS)) self.assertEqual((task, out), ('Battery', '')) @@ -86,7 +86,7 @@ class RefuseKitchenArgsTests(unittest.TestCase): def _parser(self): """Return a parser with the common options.""" - parser = argparse.ArgumentParser(prog='h12_mujoco.py') + parser = argparse.ArgumentParser(prog='golem_sim.py') sim_common.add_common_args(parser) return parser @@ -122,18 +122,18 @@ class SelectTaskTests(unittest.TestCase): def _select(self, argv): """Return (task, what select_task printed) for argv, with the runners' own tasks.""" - parser = argparse.ArgumentParser(prog='h12_mujoco.py') + parser = argparse.ArgumentParser(prog='golem_sim.py') sim_common.add_common_args(parser) out = io.StringIO() with contextlib.redirect_stdout(out): - task = sim_common.select_task(parser, parser.parse_args(argv), tag='g1_mujoco', own_tasks=OWN_TASKS, + task = sim_common.select_task(parser, parser.parse_args(argv), tag='golem_sim', own_tasks=OWN_TASKS, no_kitchen=blank_env.TASKS) return task, out.getvalue() def test_no_task_picks_a_seeded_kitchen_and_says_so(self): """Without --task the seeded random pick is chosen and logged under the tag.""" self.assertEqual(self._select(['--seed', '42']), - ('CloseStandMixerHead', "[g1_mujoco] no --task given; randomly selected " + ('CloseStandMixerHead', "[golem_sim] no --task given; randomly selected " "'CloseStandMixerHead'\n")) def test_own_task_in_any_case_and_its_kitchen_refusal(self): diff --git a/tests/robocasa/test_sim_rate.py b/tests/robocasa/test_sim_rate.py index 428eea74..9a9b669e 100644 --- a/tests/robocasa/test_sim_rate.py +++ b/tests/robocasa/test_sim_rate.py @@ -311,7 +311,8 @@ def stop(self): stub = types.ModuleType("magpie_hand_bridge") stub.MagpieHandBridge = RecordedHand sys.modules["magpie_hand_bridge"] = stub - import h12_mujoco + import golem_sim + import h12_robot pacers = [] @@ -326,9 +327,9 @@ def wait(self): raise KeyboardInterrupt return super().wait() - h12_mujoco.RealtimePacer = RecordedPacer + sim_timing.RealtimePacer = RecordedPacer try: - h12_mujoco.sim_loop("Blank", viewer=False) + golem_sim.sim_loop(h12_robot.H12Robot(), "Blank", viewer=False) except KeyboardInterrupt: pass print("RESULT " + json.dumps({"pacers": pacers, "hands": [ @@ -357,7 +358,7 @@ def test_rate_reaches_the_pacer_and_both_hands_tick_every_step(self): (pacer,) = result["pacers"] self.assertEqual(pacer["rate"], 2.0) self.assertEqual(pacer["waits"], H12_LOOP_STEPS) - self.assertIn("[h12_mujoco] sim rate: 2x real time (GOLEM_SIM_RATE)", r.stdout) + self.assertIn("[golem_sim] sim rate: 2x real time (GOLEM_SIM_RATE)", r.stdout) self.assertEqual(sorted(h["side"] for h in result["hands"]), ["left", "right"]) for hand in result["hands"]: self.assertEqual(len(hand["ticks"]), H12_LOOP_STEPS, hand["side"]) diff --git a/tests/sim/test_lmg_explore.py b/tests/sim/test_lmg_explore.py index fa0a3795..aa4e6f3e 100644 --- a/tests/sim/test_lmg_explore.py +++ b/tests/sim/test_lmg_explore.py @@ -91,7 +91,7 @@ def on_init(out, iface): def run_sim(out, sim_args): - """Run h12_mujoco.py --h12 with the recorder, the scene map and the saved model.""" + """Run golem_sim.py --h12 with the recorder, the scene map and the saved model.""" if os.environ.get("GOLEM_ROBOT", "").strip().lower() != ROBOT: raise SystemExit(f"GOLEM_ROBOT must be {ROBOT}") bringup_test.run_recorded_sim(out, ROBOT, sim_args, Recorder(out), "test_lmg_explore", @@ -445,7 +445,7 @@ def main(): parser = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter) parser.add_argument("role", choices=("sim", "drive", "report", "video")) parser.add_argument("--out", required=True, help="the shared output directory") - parser.epilog = "sim: h12_mujoco.py's arguments follow a --" + parser.epilog = "sim: golem_sim.py's arguments follow a --" argv = sys.argv[1:] sim_args = argv[argv.index("--") + 1:] if "--" in argv else [] args = parser.parse_args(argv[:len(argv) - len(sim_args) - ("--" in argv)]) diff --git a/tests/sim/test_lmg_explore.sh b/tests/sim/test_lmg_explore.sh index 31e7a8b8..9cef52d3 100755 --- a/tests/sim/test_lmg_explore.sh +++ b/tests/sim/test_lmg_explore.sh @@ -8,7 +8,7 @@ # GOLEM_TEST_VIDEO=1 tests/sim/test_lmg_explore.sh # and the video # tests/sim/test_lmg_explore.sh --video OUT # re-render OUT/explore.mp4 of a finished run # -# Builds core_ws and starts the sim (h12_mujoco.py --h12) headless, with its sensors on, in the +# Builds core_ws and starts the sim (golem_sim.py --h12) headless, with its sensors on, in the # layout 9 / style 9 / seed 42 kitchen, the spawn backed EXPLORE_BACKOFF m into open floor, as # test_sim_stack.sh does. In its network namespace a ros container then runs # 'ros2 launch golem_bringup lmg_sim.launch.py start_position_verified:=true use_rviz:=false diff --git a/tests/sim/test_sim_stack.py b/tests/sim/test_sim_stack.py index db2dcf57..69416aa3 100644 --- a/tests/sim/test_sim_stack.py +++ b/tests/sim/test_sim_stack.py @@ -4,7 +4,7 @@ while /named_config moves its arms, and SLAM stays healthy throughout. test_sim_stack.sh runs the roles and has the usage. - sim in the robocasa container: h12_mujoco.py as __main__ for GOLEM_ROBOT's + sim in the robocasa container: golem_sim.py as __main__ for GOLEM_ROBOT's robot (--h12 or --g1), recording from the sim's own state: after every physics step the elastic band's state and any penetrating contact between a robot geom and anything but the floor, and every test_sim_bringup.RECORD_EVERY steps the @@ -186,7 +186,7 @@ def save(self): def run_sim(out, sim_args): - """Run h12_mujoco.py for GOLEM_ROBOT's robot with the recorder, the scene map and the goal.""" + """Run golem_sim.py for GOLEM_ROBOT's robot with the recorder, the scene map and the goal.""" robot = robot_from_env() bringup_test.run_recorded_sim( out, robot, sim_args, Recorder(out), "test_sim_stack", @@ -684,7 +684,7 @@ def main(): parser = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter) parser.add_argument("role", choices=("sim", "drive", "report")) parser.add_argument("--out", required=True, help="the shared output directory") - parser.epilog = "sim: h12_mujoco.py's arguments follow a --; the robot is GOLEM_ROBOT's" + parser.epilog = "sim: golem_sim.py's arguments follow a --; the robot is GOLEM_ROBOT's" argv = sys.argv[1:] sim_args = argv[argv.index("--") + 1:] if "--" in argv else [] args = parser.parse_args(argv[:len(argv) - len(sim_args) - ("--" in argv)]) diff --git a/tests/sim/test_sim_stack.sh b/tests/sim/test_sim_stack.sh index 56a93dfa..2c8f1b9b 100755 --- a/tests/sim/test_sim_stack.sh +++ b/tests/sim/test_sim_stack.sh @@ -7,7 +7,7 @@ # tests/sim/test_sim_stack.sh --g1 # GOLEM_TEST_VIDEO=1 tests/sim/test_sim_stack.sh --h12 # and OUT/stack.mp4 # -# Builds core_ws and starts the robot's sim (h12_mujoco.py --h12 or --g1) +# Builds core_ws and starts the robot's sim (golem_sim.py --h12 or --g1) # headless, with its sensors on, in the layout 9 / style 9 / seed 42 kitchen, # the spawn backed 2.0 m into open floor. In its network namespace a ros # container then runs the robot's sim bringup with navigation diff --git a/tests/stack/README.md b/tests/stack/README.md index 90a5cd79..663e677b 100644 --- a/tests/stack/README.md +++ b/tests/stack/README.md @@ -37,8 +37,8 @@ release, telemetry continuity, and active policies. No fixed base is allowed. `isaac locomotion` cannot pass: `run.sh` refuses it and `run_all.sh` skips it. Isaac's `rt/lowstate` tick counts publishes, not milliseconds of the node's clock, so the leg node's readiness check never passes and the policy never starts (the -node logs that the tick is off the node's clock), and Isaac serves no -`/elastic_band/toggle`. Thresholds are in `check_lowerbody.py`. +node logs that the tick is off the node's clock). Isaac's elastic band and its +`/elastic_band/toggle` are opt-in (`--elastic_band`), and this test does not start it. Thresholds are in `check_lowerbody.py`. The scoring unit tests run in the GitHub CPU merge workflow: diff --git a/tests/stack/observe_simulator.py b/tests/stack/observe_simulator.py index c7ecb128..6915adea 100644 --- a/tests/stack/observe_simulator.py +++ b/tests/stack/observe_simulator.py @@ -65,15 +65,15 @@ def observed_step(*a, **kw): gym.make = observed_make elif simulator == 'robocasa': - entry = Path('/home/code/golem_robocasa/h12_mujoco.py') + entry = Path('/home/code/golem_robocasa/golem_sim.py') sys.path.insert(0, str(entry.parent)) import mujoco step = mujoco.mj_step def observed_step(model, data, *args, **kwargs): - """Step MuJoCo and, for steps of h12_mujoco.py's main loop, record the pelvis and joints.""" + """Step MuJoCo and, for steps of golem_sim.py's loop, record the pelvis and joints.""" result = step(model, data, *args, **kwargs) - # Only h12_mujoco.py's main loop has `resolver` and `band` locals; robosuite's + # Only golem_sim.py's loop has `resolver` and `band` locals; robosuite's # initialisation and settling steps, which reset time, are not recorded. caller = sys._getframe(1) local = caller.f_locals diff --git a/tests/stack/run.sh b/tests/stack/run.sh index 5c5806c4..c2e5b4f7 100755 --- a/tests/stack/run.sh +++ b/tests/stack/run.sh @@ -104,7 +104,7 @@ from pathlib import Path import sys root, sim, output = sys.argv[1:] source = (Path(root)/f'docker/scripts/launch_{sim}.sh').read_text() -entry = '/home/code/CL_isaaclab_sim/sim_main.py' if sim == 'isaac' else 'h12_mujoco.py' +entry = '/home/code/CL_isaaclab_sim/sim_main.py' if sim == 'isaac' else 'golem_sim.py' old = entry if sim == 'isaac' else f'exec python -u {entry}' assert source.count(old) == 1, 'Canonical launcher changed; update test instrumentation' # The copy is mounted at /test_launcher.sh, away from runtime_common.sh, so that path is made absolute. diff --git a/tests/stack/run_all.sh b/tests/stack/run_all.sh index ef7db6a7..397a5f64 100755 --- a/tests/stack/run_all.sh +++ b/tests/stack/run_all.sh @@ -1,7 +1,8 @@ #!/usr/bin/env bash # Run every stack case that can pass: arms and locomotion against RoboCasa, then arms against # Isaac. Isaac's locomotion is skipped: its rt/lowstate tick counts publishes, not ms of the -# node's clock, so the leg node never starts there, and Isaac serves no /elastic_band/toggle. +# node's clock, so the leg node never starts there, and the test's Isaac sim runs without the opt-in band +# (--elastic_band). # # Usage: tests/stack/run_all.sh # Environment: whatever run.sh reads (ROS_DOMAIN_ID, GOLEM_TEST_WALL_TIMEOUT, GOLEM_TEST_CAMERAS).