From bb29e1b999582391e1157644befb1634d8c15cdb Mon Sep 17 00:00:00 2001 From: maxconway Date: Wed, 16 Sep 2026 15:39:23 -0600 Subject: [PATCH 001/225] Simplify simulator Docker workflows and add ROS integration regressions --- .dockerignore | 4 - .github/workflows/simulator-tests.yml | 69 +++ AGENTS.md | 390 +++++++++++++++++ CLAUDE.md | 78 +++- CL_isaaclab_sim | 2 +- README.md | 489 +++------------------- core_ws/cyclonedds.xml | 2 +- docker/.env.example | 25 +- docker/BUILD.md | 152 ++++--- docker/IsaacDockerfile | 3 +- docker/RobocasaDockerfile | 5 +- docker/RosDockerfile | 57 ++- docker/docker-compose.gui.yml | 21 + docker/docker-compose.stack.yml | 13 + docker/docker-compose.yml | 173 ++++++-- docker/mac/RobocasaDockerfile.arm64 | 2 +- docker/mac/RosDockerfile.slim.arm64 | 2 +- docker/mac/docker-compose.stack.yml | 5 + docker/mac/docker-compose.yml | 99 ++++- docker/mac/scripts/docker_build_mac.sh | 52 +-- docker/mac/scripts/docker_run_mac.sh | 96 +---- docker/mac/scripts/docker_stack_mac.sh | 5 + docker/mac/scripts/launch_robocasa_mac.sh | 7 +- docker/mac/scripts/launch_ros_mac.sh | 20 +- docker/scripts/dds_bridge.py | 57 --- docker/scripts/docker_build.sh | 107 ++--- docker/scripts/docker_common.sh | 113 +++++ docker/scripts/docker_run.sh | 98 +---- docker/scripts/docker_stack.sh | 64 +++ docker/scripts/launch_isaac.sh | 70 ++-- docker/scripts/launch_robocasa.sh | 14 +- docker/scripts/launch_ros.sh | 41 +- docker/scripts/runtime_common.sh | 12 + docker/scripts/wait_for_sim.py | 30 ++ docs/DDS_TUNING.md | 69 +++ docs/DOCKER_TESTING.md | 132 ++++++ docs/MACOS.md | 149 +++++++ docs/RUNNING.md | 160 +++++++ docs/SETUP.md | 89 ++++ h1_robocasa/h12_mujoco.py | 29 +- h1_robocasa/mujoco_ros_bridge.py | 156 ++++--- h1_robocasa/sim_timing.py | 72 ++++ tests/.gitignore | 2 + tests/README.md | 55 +++ tests/docker/test_isaac_domain.py | 42 ++ tests/docker/test_workflows.py | 206 +++++++++ tests/isaaclab/check_livox_ros.py | 24 ++ tests/isaaclab/check_robot_ros.py | 171 ++++++++ tests/isaaclab/inspect_motor_dynamics.py | 9 + tests/isaaclab/inspect_robot_assets.py | 10 + tests/isaaclab/publish_livox_sample.py | 19 + tests/isaaclab/run_in_container.sh | 14 + tests/isaaclab/run_livox_interop.sh | 28 ++ tests/isaaclab/run_robot_interop.sh | 42 ++ tests/isaaclab/run_sensor_transport.sh | 4 + tests/isaaclab/run_smoke.sh | 10 + tests/isaaclab/subscribe_livox_interop.sh | 32 ++ tests/isaaclab/subscribe_robot_interop.sh | 44 ++ tests/isaaclab/test_action_provider.py | 92 ++++ tests/isaaclab/test_control.py | 42 ++ tests/isaaclab/test_dds_motor.py | 92 ++++ tests/isaaclab/test_implicit_action.py | 61 +++ tests/isaaclab/test_livox_idl.py | 16 + tests/isaaclab/test_ros_sensor_bridge.py | 43 ++ tests/isaaclab/test_sensor_transport.py | 91 ++++ tests/isaaclab/test_state.py | 177 ++++++++ tests/requirements-isaac-cpu.txt | 4 + tests/robocasa/README.md | 39 ++ tests/robocasa/run_in_container.sh | 12 + tests/robocasa/test_loop_timing.py | 89 ++++ tests/robocasa/test_sensor_bridge.py | 185 ++++++++ 71 files changed, 3781 insertions(+), 1107 deletions(-) create mode 100644 .github/workflows/simulator-tests.yml create mode 100644 AGENTS.md create mode 100644 docker/docker-compose.gui.yml create mode 100644 docker/docker-compose.stack.yml create mode 100644 docker/mac/docker-compose.stack.yml create mode 100755 docker/mac/scripts/docker_stack_mac.sh delete mode 100644 docker/scripts/dds_bridge.py create mode 100755 docker/scripts/docker_common.sh create mode 100755 docker/scripts/docker_stack.sh create mode 100644 docker/scripts/runtime_common.sh create mode 100644 docker/scripts/wait_for_sim.py create mode 100644 docs/DDS_TUNING.md create mode 100644 docs/DOCKER_TESTING.md create mode 100644 docs/MACOS.md create mode 100644 docs/RUNNING.md create mode 100644 docs/SETUP.md create mode 100644 h1_robocasa/sim_timing.py create mode 100644 tests/.gitignore create mode 100644 tests/README.md create mode 100644 tests/docker/test_isaac_domain.py create mode 100644 tests/docker/test_workflows.py create mode 100644 tests/isaaclab/check_livox_ros.py create mode 100644 tests/isaaclab/check_robot_ros.py create mode 100644 tests/isaaclab/inspect_motor_dynamics.py create mode 100644 tests/isaaclab/inspect_robot_assets.py create mode 100644 tests/isaaclab/publish_livox_sample.py create mode 100755 tests/isaaclab/run_in_container.sh create mode 100755 tests/isaaclab/run_livox_interop.sh create mode 100755 tests/isaaclab/run_robot_interop.sh create mode 100755 tests/isaaclab/run_sensor_transport.sh create mode 100755 tests/isaaclab/run_smoke.sh create mode 100755 tests/isaaclab/subscribe_livox_interop.sh create mode 100755 tests/isaaclab/subscribe_robot_interop.sh create mode 100644 tests/isaaclab/test_action_provider.py create mode 100644 tests/isaaclab/test_control.py create mode 100644 tests/isaaclab/test_dds_motor.py create mode 100644 tests/isaaclab/test_implicit_action.py create mode 100644 tests/isaaclab/test_livox_idl.py create mode 100644 tests/isaaclab/test_ros_sensor_bridge.py create mode 100644 tests/isaaclab/test_sensor_transport.py create mode 100644 tests/isaaclab/test_state.py create mode 100644 tests/requirements-isaac-cpu.txt create mode 100644 tests/robocasa/README.md create mode 100755 tests/robocasa/run_in_container.sh create mode 100644 tests/robocasa/test_loop_timing.py create mode 100644 tests/robocasa/test_sensor_bridge.py diff --git a/.dockerignore b/.dockerignore index a6ea67f..3cd1027 100644 --- a/.dockerignore +++ b/.dockerignore @@ -3,13 +3,9 @@ # on full builds otherwise). Un-deny only files that a Dockerfile COPYs in. # # Whitelisted COPY sources: -# IsaacDockerfile -> CL_isaaclab_sim/requirements.txt # RobocasaDockerfile -> core_ws/src/livox_ros_driver2 (baked-in livox build) # BaseDockerfile -> unitree_sdk2_python (local SDK install) * -!CL_isaaclab_sim -CL_isaaclab_sim/* -!CL_isaaclab_sim/requirements.txt !core_ws core_ws/* !core_ws/src diff --git a/.github/workflows/simulator-tests.yml b/.github/workflows/simulator-tests.yml new file mode 100644 index 0000000..2b87898 --- /dev/null +++ b/.github/workflows/simulator-tests.yml @@ -0,0 +1,69 @@ +name: Simulator regression tests +on: + pull_request: + paths: + - 'CL_isaaclab_sim' + - 'CL_isaaclab_sim/**' + - 'h1_robocasa/**' + - 'docker/**' + - 'tests/**' + - '.github/workflows/simulator-tests.yml' + push: + branches: [main, master] + workflow_dispatch: + +permissions: + contents: read + +jobs: + cpu: + runs-on: ubuntu-22.04 + steps: + - uses: actions/checkout@v4 + - name: Initialize code submodules (no large asset downloads) + run: git submodule update --init CL_isaaclab_sim unitree_sdk2_python + - uses: actions/setup-python@v5 + with: + python-version: '3.11' + - name: Install test dependencies + run: | + sudo apt-get update + sudo apt-get install -y cyclonedds-dev + python -m pip install torch==2.7.0 --index-url https://download.pytorch.org/whl/cpu + CYCLONEDDS_HOME=/usr python -m pip install -r tests/requirements-isaac-cpu.txt + - name: Isaac Lab interface and control regressions + env: + PYTHONPATH: CL_isaaclab_sim:unitree_sdk2_python + run: python -m unittest discover -s tests/isaaclab -p 'test_*.py' -v + - name: RoboCasa loop pacing regressions + env: + PYTHONPATH: h1_robocasa + run: python -m unittest discover -s tests/robocasa -p test_loop_timing.py -v + - name: Linux and Mac Docker workflow regressions + run: python -m unittest discover -s tests/docker -v + + gpu-integration: + # Explicit trusted runs on a prepared worker; CPU job is the portable PR gate. + if: github.event_name == 'workflow_dispatch' + runs-on: [self-hosted, linux, x64, nvidia] + timeout-minutes: 20 + steps: + - uses: actions/checkout@v4 + with: + submodules: recursive + lfs: true + - name: Build simulation and ROS images + run: docker/scripts/docker_build.sh isaac ros + - name: Actual Isaac Lab scene and shutdown + run: ROS_DOMAIN_ID=187 tests/isaaclab/run_smoke.sh + - name: ROS sensor transport + run: ROS_DOMAIN_ID=188 tests/isaaclab/run_sensor_transport.sh + - name: Native ROS Livox interoperability + run: ROS_DOMAIN_ID=189 tests/isaaclab/run_livox_interop.sh + - name: ROS commands drive actual Isaac physics + run: ROS_DOMAIN_ID=191 tests/isaaclab/run_robot_interop.sh + - uses: actions/upload-artifact@v4 + if: always() + with: + name: simulator-test-results + path: tests/results/ diff --git a/AGENTS.md b/AGENTS.md new file mode 100644 index 0000000..b69b5a6 --- /dev/null +++ b/AGENTS.md @@ -0,0 +1,390 @@ +# AGENTS.md — GOLEM developer guide for coding agents + +Everything below was verified against the code on 2026-07-12. Where older docs or +script comments disagree with the code, the code wins (known stale spots are +flagged inline). + +## 1. Project overview + +GOLEM (Generalized Open Layered Embodied Modules) is the software stack for a +Unitree H1-2 humanoid: MuJoCo/RoboCasa and Isaac Lab simulators, a ROS 2 Humble +workspace +(drivers, upper-body IK control, lower-body MPC/RL locomotion, safety layer, +perception model servers, and an LLM-callable skills layer), all running in +separate Docker containers that share one DDS domain. The same `rt/lowcmd` / +`rt/lowstate` Unitree DDS wire format is used against the sims and the real +robot, so code moves between them unchanged. + +## 2. Repo layout + +| Path | What it is | +|---|---| +| `docker/` | x86 setup — the fully-featured, preferred platform (Dockerfiles, `docker-compose.yml`, `scripts/`) | +| `docker/mac/` | self-contained Apple-Silicon (arm64, CPU-only) port — limited: no Isaac, no ML/vision stack, FastDDS instead of CycloneDDS | +| `core_ws/` | the ROS 2 workspace — where the bulk of development happens (see §4) | +| `h1_robocasa/` | RoboCasa/MuJoCo simulator entry point (`h12_mujoco.py`) and its ROS/DDS bridges | +| `CL_isaaclab_sim/` | Isaac Lab simulator (`sim_main.py`, DDS bridges, tasks) | +| `CL_Assets/` | URDF meshes, MuJoCo XML, Isaac USD (Git-LFS) | +| `mujoco_mpc/` | MuJoCo-MPC fork submodule (built standalone, outside colcon — see §8) | +| `unitree_sdk2_python/` | vendored Unitree DDS SDK (Python) | +| `tools/` | standalone debug tools (ROS MCP server for Codex) | +| `docs/` | user-facing docs: `SETUP.md`, `RUNNING.md`, `MACOS.md`, `DDS_TUNING.md`, `NAVIGATION_DEMO.md`, `ROS_MCP_DEBUG.md` | +| `docker/BUILD.md` | deep dive on the image/build system — read it before touching any Dockerfile | + +Most of the tree is **git submodules** (`git submodule update --init --recursive` +is mandatory) and large assets are **Git-LFS** (`git lfs install`). + +## 3. Containers: always use the provided scripts + +Never `docker run` the images by hand — the scripts wire up `docker/.env`, DDS +domain safety checks, bind-mount pre-creation, and stable container names. +They can be invoked from any directory. + +### x86 (preferred platform) + +| Script | Verified arguments | Notes | +|---|---|---| +| `docker/scripts/docker_build.sh [profile ...]` | `isaac`, `robocasa`, `ros` (no args = all three) | `golem_base` is built first automatically for `robocasa`/`ros`; `isaac` is self-contained. Pins the MJPC build to the `mujoco_mpc` submodule SHA (`MJPC_REF`). | +| `docker/scripts/docker_run.sh [cmd...]` | `isaac`, `robocasa`, `ros`; then optionally `bash` (shell instead of default launcher) or any command; a leading `-flag` (e.g. `--headless`) is forwarded to the default launcher | Container names: `golem_ros`, `golem_sim_robocasa`, `golem_sim_isaac`. | + +Paired startup: `docker/scripts/docker_stack.sh up robocasa|isaac [--headless|--gui]`. +Use `restart`, `stop`, or `logs` with the same simulator name. It prepares the +workspaces before physics and gates bringup on a fresh DDS state sample. +`GOLEM_ASSETS_DIR` and `GOLEM_CACHE_DIR` relocate host mounts. Shell exports +win over `docker/.env`. Existing single-service containers require explicit +`--restart`. See `docs/DOCKER_TESTING.md` for both platform smoke tests. + +Default launchers (compose `command`): `docker/scripts/launch_isaac.sh`, +`launch_robocasa.sh`, `launch_ros.sh`. The `ros` launcher colcon-builds +`core_ws` (if stale) and **drops to a shell — it does not launch bringup**. + +### mac (`docker/mac`, limited) + +| Script | Verified arguments | Notes | +|---|---|---| +| `docker/mac/scripts/docker_build_mac.sh [service ...]` | `robocasa`, `ros` (no args = both) | **No `isaac`** — Isaac Sim needs an NVIDIA GPU. arm64 base built from `docker/mac/BaseDockerfile.arm64`. | +| `docker/mac/scripts/docker_run_mac.sh [cmd...]` | `robocasa`, `ros` | Starts ONE service. For paired startup use `docker/mac/scripts/docker_stack_mac.sh up robocasa` (RoboCasa running alone for a few seconds lets the robot collapse — motor command timeout). | + +Mac feature toggles are env vars read by the compose file: `GOLEM_DISPLAY=vnc` +(MuJoCo viewer → noVNC :6080), `GOLEM_RVIZ=vnc` (RViz → :6081), `GOLEM_LOWERBODY=fame|walk|switch`, +`GOLEM_SLAM=1`, `GOLEM_NAV2=1`, `GOLEM_SIM_ODOM=1`, `GOLEM_CAMERAS=0`, +`GOLEM_SPAWN_BACKOFF=`, `GOLEM_CMD_TIMEOUT=`, `GOLEM_ROS_MCP=1`. +GUIs stream over noVNC via `docker/mac/scripts/mac_vnc_tunnel.sh` (no XQuartz). + +### DDS domain safety (both platforms) + +`ROS_DOMAIN_ID=0` is the **real robot's command bus**. The run scripts refuse to +start a sim on it (the x86 `ros` profile asks for interactive confirmation; mac +always rejects). Unset/empty defaults to `1` (the sim domain). Set it in +`docker/.env` (copy from `docker/.env.example`; also holds `GEMINI_API_KEY`, +which the `ros` container needs for the vision servers and `h12_skills`). + +## 4. `core_ws/src` packages + +19 ROS packages in 16 directories. Note two name mismatches: dir `FAST_LIO` → +package `fast_lio`; dir `unitree_ros2` is not itself a package (it nests +`unitree_api`, `unitree_go`, `unitree_hg`, `unitree_ros2_example`). + +| Directory | Role | What it does | Key entry points | +|---|---|---|---| +| `h1_bringup` | **bringup** | Launch-only package that starts the whole robot (sim or real). See §6. | 7 launch files, `config/*.yaml` | +| `h12_ros2_controller` | controller (upper body) | Pinocchio-based arm IK: `/frame_task` + `/dual_arm` action servers, joint state publisher, hand controller | `frame_task_server`, `dual_arm_server`, `joint_state_publisher`, `hand_controller_node` | +| `h12_deploy_mjpc` | controller (lower body, MPC) | MJPC balance/locomotion controller (C++ cores from the `mujoco_mpc` fork, run as separate processes) + RW-EKF base estimator | `mjpc_lowerbody_core`, `controller_launcher.py`, `estimator_node.py` | +| `h12_lowerbody_rl` | controller (lower body, RL) | TorchScript walking + FAME RMA stand/squat policies; switchable stand↔walk controller; consumes `/lowstate` + `/cmd_vel`, emits 12-joint PD setpoints | `walking_node`, `fame_node`, `lowerbody_controller_node` | +| `h12_safety_layer` | safety | Merges upper/lower command channels into `/lowcmd` with limit checks (YAML-configured) | `safety_node` | +| `h12_skills` | **skills** | Action servers for the 12 `/skill/*` atomic skills an LLM can call (frontier exploration lives here). See §7. | `skills` (SkillsNode) | +| `model_server` | model server | ROS service servers wrapping ML models: Gemini VLM, SAM3 segmentation, GraspGenX grasp generation, YOLO detection | `gemini_server`, `sam_server`, `graspgen_server`, `yolo_server` | +| `custom_ros_messages` | interfaces | 18 actions (12 `Skill*`, `FrameTask`, `NamedConfig`, `DualArm`, …), msgs, srvs (`GeminiQuery`, `SamSegment`, `GraspGen`, …) | rosidl only | +| `magpie_control` | driver (gripper/sensors) | Dynamixel gripper node (`/left|right/gripper/*` services, DeliGrasp action), F/T + tactile sensor nodes | `gripper_node`, `ft_sensor_node` | +| `magpie_msgs` | interfaces | Gripper/manipulation interfaces (`GripperState`, `SetGripperPosition/Force`, `DeliGrasp`, …) | rosidl only | +| `estop` | safety (hardware) | Serial e-stop bridge → ROS Bool (real robot only; launched by real bringup, not sim) | `estop_node` | +| `cl_realsense` | perception util | Point-cloud accumulator (tf2 + Open3D) for the RealSense cams; ships the real-camera launch file | `pc_acc`; `launch/h12_rs_cams.launch.py` | +| `FAST_LIO` (`fast_lio`) | SLAM (vendored) | LiDAR-inertial odometry/mapping from the Livox cloud | `fastlio_mapping` | +| `livox_ros_driver2` | driver (vendored) | Livox MID360 lidar driver (builds via its own `build.sh humble`) | `livox_ros_driver2_node` | +| `unitree_ros2/*` | interfaces (vendored) | Unitree IDL packages — `unitree_hg` `LowCmd_`/`LowState_` are the stack's spine | rosidl only | + +Third-party/vendored (do not refactor): `FAST_LIO`, `livox_ros_driver2`, +`unitree_ros2`, plus the top-level `unitree_sdk2_python` and `mujoco_mpc`. + +## 5. Ownership & scoping convention + +Development is **scoped to a single package**. Each package (the ROS 2 +controller, the MJPC deploy package, the lower-body RL package, the skills +package, the model servers, the simulators, …) has its own owner; several are +separate git submodules with their own history. Rules of thumb: + +- Confine a change to the package it belongs to. If a task seems to require + edits across packages, stop and coordinate first. +- Shared infra — `h1_bringup`, `custom_ros_messages`/`magpie_msgs`, + `docker/`, the safety layer, the simulators — affects everyone. Treat + changes there as interface changes: minimal, deliberate, announced. +- Submodule packages must be committed and pushed in the submodule repo, then + the pin bumped here. Never leave a submodule pointing at an unpushed commit. + +## 6. Bringup: how the robot starts + +`h1_bringup` is launch-only. Sim vs real is **separate launch files** (there is +no `sim:=true` arg): + +| Launch file | Scenario | +|---|---| +| `h1_sim_bringup.launch.py` | **x86 sim** — full stack: nav (via include), robot/joint state publishers, `frame_task_server`, `safety_node`, gemini/sam servers, MJPC estimator + lowerbody controller, graspgen + skills (`use_skills`, default true), rviz (`use_rviz`) | +| `h1_sim_bringup_mac.launch.py` | **mac sim** — trimmed: state publishers, `frame_task_server`, `safety_node`; lower body/SLAM/nav2 gated by `GOLEM_*` env vars, not launch args | +| `h1_real_robot_bringup.launch.py` | **real robot, onboard PC** (native, `ROS_DOMAIN_ID=0`) — aggregates the three below-listed real files | +| `h1_real_drivers.launch.py` | real: Livox MID360, RealSense cams, left+right `gripper_node` | +| `h1_real_controller.launch.py` | real: estop, state publishers, staggered `safety_node` + `frame_task_server` | +| `h1_real_desktop_bringup.launch.py` | real: companion x86 desktop — model servers, skills, MJPC controller. Leg control is interlocked behind `start_position_verified:=true` (default **false**) | +| `h1_navigation.launch.py` | shared nav stack (FAST-LIO → pointcloud_to_laserscan → slam_toolbox → nav2); included by sim and real bringups | + +**Wiring a new package into bringup:** add it as `exec_depend` in +`h1_bringup/package.xml`, add a `launch_ros.actions.Node(...)` entry to the +relevant launch file (put node params in `h1_bringup/config/.yaml` — the +top-level YAML key must match the node `name`), and gate optional nodes with +`IfCondition(LaunchConfiguration(...))`. See the `h12_deploy_mjpc` entries in +`h1_sim_bringup.launch.py` as the model. **Mac only:** also add the package to +the `PKGS` list in `docker/mac/scripts/launch_ros_mac.sh` or the slim image +won't build it. + +## 7. Skills (`h12_skills`) — the LLM-callable layer + +Each skill is a mixin class in `h12_skills/skills/` with one +`_exec_()` method; `SkillsNode` registers one action server per entry in +the `SKILL_ACTIONS` table (`base.py`). All types are +`custom_ros_messages/action/Skill*`; every goal has a `timeout` Duration +(0 → 300 s default); every result is `success` + `message`; feedback is +`phase` + `progress`. + +| Action server | Status | +|---|---| +| `/skill/grasp`, `/skill/pick_place`, `/skill/frontier_explore` | **implemented** | +| `/skill/open_door`, `/skill/close_door`, `/skill/open_lid`, `/skill/close_lid`, `/skill/navigate`, `/skill/press`, `/skill/slide_rack`, `/skill/turn_lever`, `/skill/twist_knob` | stubs — goal accepted, then aborted with "not implemented" | + +Skills consume the `frame_task` IK action, the gripper services, and the +`model_server` perception services (`gemini_query`, `sam_segment`, `graspgen` — +this is why the `ros` container needs `GEMINI_API_KEY`). There is currently no +autonomous LLM orchestrator in-repo that calls them; they are invoked over ROS +(by a human, an external agent, or the `tools/ros_mcp_server.py` debug MCP). + +Verified invocation examples (from a shell inside `golem_ros`): + +```bash +ros2 action send_goal /skill/grasp custom_ros_messages/action/SkillGrasp \ + "{target_object: 'vertical fridge handle', arm: 'right', timeout: {sec: 60, nanosec: 0}}" \ + --feedback + +ros2 action send_goal /skill/frontier_explore custom_ros_messages/action/SkillFrontierExplore \ + "{min_frontier_cells: 6, blacklist_radius: 0.6, min_goal_distance: 0.7, goal_timeout: 120.0, timeout: {sec: 1800, nanosec: 0}}" \ + --feedback + +# non-skill but useful: move arms to a named posture first +ros2 action send_goal /named_config custom_ros_messages/action/NamedConfig \ + "{config_name: 'home', duration: {sec: 0, nanosec: 0}}" +``` + +## 8. Do-not-break rules + +### The simulators are shared infra — don't modify them casually + +`CL_isaaclab_sim` and `h1_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. + +### Simulators mimic the real robot's topic surface + +They should publish only what the real robot publishes and consume only what +it consumes. Verified current surface: + +**RoboCasa (`h1_robocasa/h12_mujoco.py`)** — publishes `/clock`, +`/realsense/{head,left_hand,right_hand}/color/image_raw[/compressed]`, +`.../aligned_depth_to_color/image_raw[/compressedDepth]`, `.../color/camera_info`, +`/livox/lidar` (CustomMsg), `/livox/pointcloud`, `/livox/imu`, +`/{left,right}/gripper/state`, and `rt/lowstate` over Unitree DDS; consumes +`rt/lowcmd` and the `/{left,right}/gripper/*` services/DeliGrasp action. +(Trust the bridge code in `mujoco_ros_bridge.py` over any topic list repeated +elsewhere.) + +**Isaac (`CL_isaaclab_sim/sim_main.py`)** — publishes `rt/lowstate` and +optional `rt/inspire/state` over CycloneDDS, `/clock`, three RealSense RGBD +camera streams with camera info, and `/livox/{lidar,pointcloud,imu}`. Consumes +`rt/lowcmd` and optional `rt/inspire/cmd`. The current asset is the Inspire +model; Magpie hardware fidelity depends on the planned CL_Assets pin update. +See `tests/README.md` for saved CPU and cross-container integration tests. + +### Ground truth must NEVER be on by default + +Sims may *optionally* publish ground-truth data (true base/object poses), but +it must be opt-in. Rationale: any node that consumes ground truth "cheats" — +it works in sim and silently fails on the real robot, where no such topic +exists. Keeping it off by default keeps the sim honest and protects +sim-to-real transfer. + +- **RoboCasa complies:** ground-truth `/odom` + `odom→pelvis` TF is gated by + `GOLEM_SIM_ODOM` (default `'0'` = off, `h12_mujoco.py:184`). Only the mac nav + demo opts in. Keep it that way. +- **Isaac:** no full-scene ground-truth publisher or per-step scene JSON conversion. +- **Known deviations (do not make worse):** RoboCasa always publishes privileged task signals + (`/robocasa/success`, `/robocasa/reward`, `/robocasa/task_goal`). Never make + robot-stack code *depend* on any of these, and don't add new ungated + ground-truth publishers. + +### Do NOT bump the `livox_ros_driver2` submodule + +Pinned at **`13eb05e`** ("support Mid-360s Lidar, Ubuntu 24.04 and ROS2 Jazzy"). +Leave it there. A bump to `4a1def9` on 2026-08-05 broke both sim images and was +reverted; the same trap is still armed. + +Why it breaks: the driver and Livox-SDK2 are **separate upstream repos that ship +paired halves of the same feature**, but only the driver is pinned here — both +Dockerfiles install the SDK with an unpinned `git clone --depth 1 …master` +(`RosDockerfile` §2, `RobocasaDockerfile` near the end). That clone sits in a +layer *above* the driver's `COPY`/build, and Docker invalidates downward only, +so bumping the driver rebuilds it against whatever SDK headers your cache +happens to hold. `4a1def9` (Avia2) calls `kLivoxLidarDoubleEchoData` / +`LivoxLidarDoubleEchoRawPoint`, added in SDK commit `08f523c` — against an +older cached SDK it fails with "was not declared in this scope". + +The failure is silent. Upstream's `build.sh` ends in `popd`, so it exits 0 even +when its colcon build fails, and `launch_ros.sh` doesn't check either. The image +builds "successfully" with `/opt/livox_ws/install/livox_ros_driver2/` holding +only `package.*` stubs — no `local_setup.bash`, no Python module — and you find +out at runtime: + +``` +ModuleNotFoundError: No module named 'livox_ros_driver2' # mujoco_ros_bridge.py:36 +``` + +Whether it reproduces depends on how old your Docker cache is, so "it built on +my machine" proves nothing. + +If you genuinely need a newer driver (Avia2/Mid-360s hardware), do it in one +change: add `ARG LIVOX_SDK2_REF=` to **both** Dockerfiles with +a pinned `git fetch --depth 1 origin "$LIVOX_SDK2_REF"`, keep the two SHAs +identical (the images must agree on the `CustomMsg` wire format), and assert the +build worked — `python3 -c 'from livox_ros_driver2.msg import CustomMsg'` — so a +mismatch fails the image build instead of shipping broken. + +### Comments describe the code as it is, not how it got there + +A comment is scoped to what the code does **now**, written for someone who has +never seen an earlier version. Change history belongs in the commit message and +the PR, not in the source — git already has it, and a comment about a past edit +goes stale the moment the next edit lands. + +Don't write: + +- change narration — `# now uses X instead of Y`, `# switched to CycloneDDS`, + `# was 0.5, bumped to 0.8`, `# fixed the TF extrapolation bug` +- dated or attributed notes — `# 2026-07-12 (max): ...`, `# per review` +- status chatter about the edit itself — `# new`, `# updated`, `# temporary`, + `# deprecated, keep for now` +- commented-out code kept "just in case" — delete it; git remembers +- a restatement of the identifier the comment sits above + +Do write what the code can't say for itself: units and frames (`# rad, pelvis +frame`), why a non-obvious constant, ordering or sleep is required, invariants +a caller must uphold, and a link to the upstream issue that forces a workaround. + +Rationale from a past change is welcome when the rationale is still +load-bearing — but phrase it as a present-tense constraint, not as a diff: +`# 3.2.3 matches the MJPC ABI` beats `# downgraded from 3.3.1, MJPC broke`. + +This applies to edits too: when you change behavior, rewrite the nearby comment +and docstring to describe the new behavior instead of appending a note about the +change. A comment that duplicates a list the code owns (topics, package names, +paths, tuning values) is a stale comment waiting to happen — point at the +authoritative source instead of copying it. + +### Other invariants + +- MuJoCo versions are pinned per image (`3.2.3` in `ros` to match the MJPC ABI, + `3.3.1` in `robocasa`) — never bump one without the other side of its pin. +- `numpy<2`, `setuptools==59.6.0`, `wheel<0.44` are deliberately re-clamped in + the Dockerfiles; add new pip deps at the END of a Dockerfile (layer-order + discipline). Details in `docker/BUILD.md`. +- After bumping the `mujoco_mpc` submodule: rebuild the `ros` image at the + matching `MJPC_REF` and wipe stale `core_ws/build/h12_deploy_mjpc`. + +## 9. Running the robot — the standard three-terminal flow + +Everyone runs their package through bringup, which starts the whole robot. + +### x86 + +```bash +# Terminal 1 — simulator (start FIRST so /clock is publishing) +docker/scripts/docker_run.sh robocasa # or: isaac; add --headless if no display + +# Terminal 2 — ROS workspace (auto colcon-builds core_ws, drops to a shell) +docker/scripts/docker_run.sh ros +ros2 launch h1_bringup h1_sim_bringup.launch.py # manual step — the container does NOT launch it +# variants: use_skills:=false use_rviz:=false use_nav:=false + +# Terminal 3 — drive skills: attach to the running ROS container +docker exec -it golem_ros /bin/bash +source /opt/ros/humble/setup.bash +source /home/code/core_ws/install/setup.bash +ros2 action send_goal /skill/grasp custom_ros_messages/action/SkillGrasp \ + "{target_object: 'vertical fridge handle', arm: 'right', timeout: {sec: 60, nanosec: 0}}" --feedback +``` + +### mac + +```bash +# Terminals 1+2 in one: start BOTH services together (bringup auto-launches in ros) +docker/mac/scripts/docker_stack_mac.sh up robocasa +# or two terminals: docker/mac/scripts/docker_run_mac.sh robocasa / ... ros +# (start ros within seconds of robocasa or the robot collapses) + +# Terminal 3 +docker exec -it golem_ros bash # fallback: colima ssh -- docker exec -it golem_ros bash +source /home/code/h12_sim_scripts/robot_cli.sh # rob_pose, rob_grip, rob_stand, rob_explore, ... +``` + +The mac `ros` container auto-runs +`ros2 launch /home/code/core_ws/src/h1_bringup/launch/h1_sim_bringup_mac.launch.py` +(launched by path; only skills built on mac are available — no `h12_skills`, +`model_server`, or nav unless `GOLEM_SLAM/GOLEM_NAV2` are set). + +### Real robot (for awareness — coordinate before touching) + +Onboard PC, native (no docker): `export ROS_DOMAIN_ID=0`, source +`core_ws/install/setup.bash`, then +`ros2 launch h1_bringup h1_real_robot_bringup.launch.py`. Companion desktop: +`ros2 launch h1_bringup h1_real_desktop_bringup.launch.py` +(`start_position_verified:=true` only after physically verifying the robot's +pose — it gates leg commands). + +## 10. Getting-started checklist for a new agent + +1. `git submodule update --init --recursive && git lfs install` (most of the + tree is submodules; builds fail cryptically without this). +2. `cp docker/.env.example docker/.env`; set `GEMINI_API_KEY` and + `ROS_DOMAIN_ID` (any non-zero; 0 is the real robot). +3. Place SAM3 weights at `core_ws/src/model_server/weights/sam3.pt` + (gated HF download — see `docs/SETUP.md`). +4. `docker/scripts/docker_build.sh robocasa ros` (add `isaac` if needed). +5. Run the three-terminal flow in §9; confirm with `ros2 topic hz /lowstate` + and `ros2 action list | grep skill` inside `golem_ros`. +6. Trigger `/skill/grasp` or the frontier explorer (§7). +7. Read first: `docs/RUNNING.md` (run flows), `docker/BUILD.md` (build system), + your target package's source. `README.md` indexes the rest of `docs/`. + +### Common pitfalls + +- **Start RoboCasa before bringup** — ROS nodes latch onto sim `/clock`; and if + you restart the `robocasa` container, restart `golem_ros` too (sim time resets + to 0 → TF extrapolation errors, frozen SLAM map). +- **x86 `ros` container ≠ bringup.** It only builds and gives you a shell. +- Rebuild is incremental across container restarts (build/install are + host-mounted); `colcon build --symlink-install` inside the container forces + it; wipe `container_cache/msgs_ws/` for a clean message rebuild. +- MJPC C++ iteration: `docker exec -it golem_ros /home/code/h12_sim_scripts/rebuild_mjpc.sh` + (`--install` to also refresh assets/proto) — not colcon. +- 9 of the 12 `/skill/*` actions are stubs (§7) — a goal that immediately + aborts with "not implemented" is expected, not a regression. +- Talking to the sim from the host bypasses `docker/.env`; run + `set -a; source docker/.env; set +a` first or topics will be invisible. +- Laggy point clouds/images on x86 are usually kernel UDP buffer drops — see + `docs/DDS_TUNING.md` (sysctls + IRQ pinning). +- Isaac's launcher accepts `--task NAME` and `--headless`; Unitree DDS reads + `ROS_DOMAIN_ID` directly and rejects domain 0. There is no domain relay. diff --git a/CLAUDE.md b/CLAUDE.md index 93ab5c9..c12afd1 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -28,7 +28,7 @@ robot, so code moves between them unchanged. | `mujoco_mpc/` | MuJoCo-MPC fork submodule (built standalone, outside colcon — see §8) | | `unitree_sdk2_python/` | vendored Unitree DDS SDK (Python) | | `tools/` | standalone debug tools (ROS MCP server for Claude Code) | -| `docs/` | `NAVIGATION_DEMO.md` (mac SLAM/nav2/frontier demo), `ROS_MCP_DEBUG.md` | +| `docs/` | user-facing docs: `SETUP.md`, `RUNNING.md`, `MACOS.md`, `DDS_TUNING.md`, `NAVIGATION_DEMO.md`, `ROS_MCP_DEBUG.md` | | `docker/BUILD.md` | deep dive on the image/build system — read it before touching any Dockerfile | Most of the tree is **git submodules** (`git submodule update --init --recursive` @@ -47,6 +47,13 @@ They can be invoked from any directory. | `docker/scripts/docker_build.sh [profile ...]` | `isaac`, `robocasa`, `ros` (no args = all three) | `golem_base` is built first automatically for `robocasa`/`ros`; `isaac` is self-contained. Pins the MJPC build to the `mujoco_mpc` submodule SHA (`MJPC_REF`). | | `docker/scripts/docker_run.sh [cmd...]` | `isaac`, `robocasa`, `ros`; then optionally `bash` (shell instead of default launcher) or any command; a leading `-flag` (e.g. `--headless`) is forwarded to the default launcher | Container names: `golem_ros`, `golem_sim_robocasa`, `golem_sim_isaac`. | +Paired startup: `docker/scripts/docker_stack.sh up robocasa|isaac [--headless|--gui]`. +Use `restart`, `stop`, or `logs` with the same simulator name. It prepares the +workspaces before physics and gates bringup on a fresh DDS state sample. +`GOLEM_ASSETS_DIR` and `GOLEM_CACHE_DIR` relocate host mounts. Shell exports +win over `docker/.env`. Existing single-service containers require explicit +`--restart`. See `docs/DOCKER_TESTING.md` for both platform smoke tests. + Default launchers (compose `command`): `docker/scripts/launch_isaac.sh`, `launch_robocasa.sh`, `launch_ros.sh`. The `ros` launcher colcon-builds `core_ws` (if stale) and **drops to a shell — it does not launch bringup**. @@ -56,7 +63,7 @@ Default launchers (compose `command`): `docker/scripts/launch_isaac.sh`, | Script | Verified arguments | Notes | |---|---|---| | `docker/mac/scripts/docker_build_mac.sh [service ...]` | `robocasa`, `ros` (no args = both) | **No `isaac`** — Isaac Sim needs an NVIDIA GPU. arm64 base built from `docker/mac/BaseDockerfile.arm64`. | -| `docker/mac/scripts/docker_run_mac.sh [cmd...]` | `robocasa`, `ros` | Starts ONE service. For the paired sim prefer `docker compose -f docker/mac/docker-compose.yml up` so both start together (RoboCasa running alone for a few seconds lets the robot collapse — motor command timeout). | +| `docker/mac/scripts/docker_run_mac.sh [cmd...]` | `robocasa`, `ros` | Starts ONE service. For paired startup use `docker/mac/scripts/docker_stack_mac.sh up robocasa` (RoboCasa running alone for a few seconds lets the robot collapse — motor command timeout). | Mac feature toggles are env vars read by the compose file: `GOLEM_DISPLAY=vnc` (MuJoCo viewer → noVNC :6080), `GOLEM_RVIZ=vnc` (RViz → :6081), `GOLEM_LOWERBODY=fame|walk|switch`, @@ -193,13 +200,15 @@ it consumes. Verified current surface: `/livox/lidar` (CustomMsg), `/livox/pointcloud`, `/livox/imu`, `/{left,right}/gripper/state`, and `rt/lowstate` over Unitree DDS; consumes `rt/lowcmd` and the `/{left,right}/gripper/*` services/DeliGrasp action. -(The topic list in `launch_robocasa.sh`'s header comment and the root README is -stale — trust the bridge code in `mujoco_ros_bridge.py`.) +(Trust the bridge code in `mujoco_ros_bridge.py` over any topic list repeated +elsewhere.) **Isaac (`CL_isaaclab_sim/sim_main.py`)** — publishes `rt/lowstate` and -`rt/inspire/state` over DDS plus compressed left-hand camera topics over ROS; -consumes `rt/lowcmd`, `rt/inspire/cmd`, `rt/reset_pose/cmd`. Lidar/IMU ROS -publishers are advertised but currently never fed (publish code commented out). +optional `rt/inspire/state` over CycloneDDS, `/clock`, three RealSense RGBD +camera streams with camera info, and `/livox/{lidar,pointcloud,imu}`. Consumes +`rt/lowcmd` and optional `rt/inspire/cmd`. The current asset is the Inspire +model; Magpie hardware fidelity depends on the planned CL_Assets pin update. +See `tests/README.md` for saved CPU and cross-container integration tests. ### Ground truth must NEVER be on by default @@ -212,10 +221,8 @@ sim-to-real transfer. - **RoboCasa complies:** ground-truth `/odom` + `odom→pelvis` TF is gated by `GOLEM_SIM_ODOM` (default `'0'` = off, `h12_mujoco.py:184`). Only the mac nav demo opts in. Keep it that way. -- **Known deviations (do not make worse):** Isaac publishes full scene ground - truth (`rt/sim_state`, robot + object poses as JSON) unconditionally every - loop — there is no off switch, and `docker/scripts/dds_bridge.py` even relays - it across domains. RoboCasa always publishes privileged task signals +- **Isaac:** no full-scene ground-truth publisher or per-step scene JSON conversion. +- **Known deviations (do not make worse):** RoboCasa always publishes privileged task signals (`/robocasa/success`, `/robocasa/reward`, `/robocasa/task_goal`). Never make robot-stack code *depend* on any of these, and don't add new ungated ground-truth publishers. @@ -256,6 +263,37 @@ identical (the images must agree on the `CustomMsg` wire format), and assert the build worked — `python3 -c 'from livox_ros_driver2.msg import CustomMsg'` — so a mismatch fails the image build instead of shipping broken. +### Comments describe the code as it is, not how it got there + +A comment is scoped to what the code does **now**, written for someone who has +never seen an earlier version. Change history belongs in the commit message and +the PR, not in the source — git already has it, and a comment about a past edit +goes stale the moment the next edit lands. + +Don't write: + +- change narration — `# now uses X instead of Y`, `# switched to CycloneDDS`, + `# was 0.5, bumped to 0.8`, `# fixed the TF extrapolation bug` +- dated or attributed notes — `# 2026-07-12 (max): ...`, `# per review` +- status chatter about the edit itself — `# new`, `# updated`, `# temporary`, + `# deprecated, keep for now` +- commented-out code kept "just in case" — delete it; git remembers +- a restatement of the identifier the comment sits above + +Do write what the code can't say for itself: units and frames (`# rad, pelvis +frame`), why a non-obvious constant, ordering or sleep is required, invariants +a caller must uphold, and a link to the upstream issue that forces a workaround. + +Rationale from a past change is welcome when the rationale is still +load-bearing — but phrase it as a present-tense constraint, not as a diff: +`# 3.2.3 matches the MJPC ABI` beats `# downgraded from 3.3.1, MJPC broke`. + +This applies to edits too: when you change behavior, rewrite the nearby comment +and docstring to describe the new behavior instead of appending a note about the +change. A comment that duplicates a list the code owns (topics, package names, +paths, tuning values) is a stale comment waiting to happen — point at the +authoritative source instead of copying it. + ### Other invariants - MuJoCo versions are pinned per image (`3.2.3` in `ros` to match the MJPC ABI, @@ -293,7 +331,7 @@ ros2 action send_goal /skill/grasp custom_ros_messages/action/SkillGrasp \ ```bash # Terminals 1+2 in one: start BOTH services together (bringup auto-launches in ros) -docker compose -f docker/mac/docker-compose.yml up +docker/mac/scripts/docker_stack_mac.sh up robocasa # or two terminals: docker/mac/scripts/docker_run_mac.sh robocasa / ... ros # (start ros within seconds of robocasa or the robot collapses) @@ -323,13 +361,13 @@ pose — it gates leg commands). 2. `cp docker/.env.example docker/.env`; set `GEMINI_API_KEY` and `ROS_DOMAIN_ID` (any non-zero; 0 is the real robot). 3. Place SAM3 weights at `core_ws/src/model_server/weights/sam3.pt` - (gated HF download — see root `README.md`). + (gated HF download — see `docs/SETUP.md`). 4. `docker/scripts/docker_build.sh robocasa ros` (add `isaac` if needed). 5. Run the three-terminal flow in §9; confirm with `ros2 topic hz /lowstate` and `ros2 action list | grep skill` inside `golem_ros`. 6. Trigger `/skill/grasp` or the frontier explorer (§7). -7. Read first: root `README.md` (run flows, DDS tuning, mac port), - `docker/BUILD.md` (build system), your target package's source. +7. Read first: `docs/RUNNING.md` (run flows), `docker/BUILD.md` (build system), + your target package's source. `README.md` indexes the rest of `docs/`. ### Common pitfalls @@ -347,10 +385,6 @@ pose — it gates leg commands). - Talking to the sim from the host bypasses `docker/.env`; run `set -a; source docker/.env; set +a` first or topics will be invisible. - Laggy point clouds/images on x86 are usually kernel UDP buffer drops — see - "Network / DDS tuning" in the root README (sysctls + IRQ pinning). -- Stale name alert: `docker/mac/scripts/launch_ros_mac.sh` `PKGS` still lists - `h12_lowerbody_controller`; the package is now `h12_lowerbody_rl`. If the mac - lower-body stack won't build/launch, fix that list. -- Isaac's `launch_isaac.sh` hardcodes its task on the `exec` line — the - script's task/`--headless` parsing is dead code; pass flags by invoking - `sim_main.py` directly if you need a different task. + `docs/DDS_TUNING.md` (sysctls + IRQ pinning). +- Isaac's launcher accepts `--task NAME` and `--headless`; Unitree DDS reads + `ROS_DOMAIN_ID` directly and rejects domain 0. There is no domain relay. diff --git a/CL_isaaclab_sim b/CL_isaaclab_sim index deec4de..5d9231f 160000 --- a/CL_isaaclab_sim +++ b/CL_isaaclab_sim @@ -1 +1 @@ -Subproject commit deec4dea876839a2909ed6fa3dabf99b53478b9d +Subproject commit 5d9231f59f9b746d46aac0430662a525fddce90f diff --git a/README.md b/README.md index d3c99c7..217719e 100644 --- a/README.md +++ b/README.md @@ -1,459 +1,74 @@ # GOLEM -GOLEM (Generalized Open Layered Embodied Modules) for the Correll Lab H1 robot: -MuJoCo, ROS 2, and Isaac Sim running in separate containers and sharing a -CycloneDDS ROS domain. +**G**eneralized **O**pen **L**ayered **E**mbodied **M**odules — the software +stack for the Correll Lab's Unitree H1-2 humanoid. -## Migrating from HAMS +Two simulators (MuJoCo/RoboCasa and Isaac Lab) and a ROS 2 Humble workspace run +in separate Docker containers sharing one DDS domain. The workspace holds the +drivers, upper-body IK, lower-body MPC and RL locomotion, a safety layer, +perception model servers, and an LLM-callable skills layer. Sim and real robot +speak the same Unitree `rt/lowcmd` / `rt/lowstate` wire format, so code moves +between them unchanged. -The project was renamed HAMS → GOLEM. This is a clean break — there are no -compatibility fallbacks, so update your local setup: +## Quick start -- **Env vars:** every `HAMS_*` toggle is now `GOLEM_*` (`HAMS_SLAM` → - `GOLEM_SLAM`, and so on). The run scripts abort with an error if they see a - stale `HAMS_*` export, since these toggles default to off and would otherwise - be ignored silently. Note the guard cannot fire if you bypass the scripts with - `docker compose ... up` directly — the stale var is simply dropped. -- **Containers/images:** `hams_base`/`hams_ros`/`hams_sim_*` are now - `golem_base`/`golem_ros`/`golem_sim_*`. Retag rather than rebuild: - `for i in base ros sim_robocasa; do docker tag hams_$i:latest golem_$i:latest; done` -- **Checkouts:** the robot PC's checkout is now `/home/unitree/GOLEM`. Set - `GOLEM_ASSETS_DIR` to override it if you have not renamed it yet. -- **Remote:** `git remote set-url origin git@github.com:correlllab/GOLEM.git`. - -## Layout - -- `docker/` — x86 Dockerfiles, `docker-compose.yml`, and the build/run scripts. -- `docker/mac/` — self-contained Apple-Silicon (arm64/CPU) port: its own - Dockerfiles, `docker-compose.yml`, and `scripts/` (see the macOS section below). -- `core_ws/` — ROS 2 workspace (bringup, IK, perception, safety). Submodules. -- `h1_robocasa/` — RoboCasa/MuJoCo simulator entry point and bridges. -- `CL_Assets/` — URDF, MuJoCo XML, and Isaac USD assets. -- `tools/` — standalone debugging tools (e.g. the ROS MCP server). - -## Prerequisites - -- Docker (with Compose v2) and the NVIDIA Container Toolkit. -- Git LFS (`git lfs install`) — required to fetch the large binary assets - (URDF meshes, MuJoCo XML, Isaac USD) tracked via LFS. -- `git submodule update --init --recursive` to populate `core_ws/src`. -- Copy `docker/.env.example` to `docker/.env` and fill in your `GEMINI_API_KEY` - and `ROS_DOMAIN_ID` (see [Configuration](#configuration)). -- SAM3 weights (`sam3.pt`) — needed by the perception/`sam_server` pipeline. - They are **not** tracked in git; download them from the gated - [`facebook/sam3`](https://huggingface.co/facebook/sam3) repo and place the - file at `core_ws/src/model_server/weights/sam3.pt`: - - ```bash - huggingface-cli login # one-time; accept the facebook/sam3 license - cd core_ws/src/model_server/weights - huggingface-cli download facebook/sam3 --local-dir .cache - mv .cache/sam3.pt sam3.pt # name it exactly sam3.pt - ``` - - See `core_ws/src/model_server/weights/README.md` for details and the - auto-download fallback. - -A few things worth knowing before you run anything: - -- The build/run scripts can be invoked from any directory — they resolve their - own location, so `docker/scripts/docker_run.sh robocasa` works just as well - from `/tmp` as from the repo root. -- `ROS_DOMAIN_ID` is read from `docker/.env` and forwarded into the RoboCasa and - ROS containers. If it is unset or `0`, it is normalized to `1` (domain 0 is - reserved for the real robot). -- Isaac currently overrides this and pins its DDS bridge to channel 1 — see - the Isaac section below. - -## Configuration - -Runtime settings live in `docker/.env` (git-ignored — it holds your API key). -Copy the template once and edit it; you never need to `export` these variables -in your shell: - -```bash -cp docker/.env.example docker/.env -# then edit docker/.env: -# GEMINI_API_KEY=... # https://aistudio.google.com/apikey -# ROS_DOMAIN_ID=1 # any non-zero value; 0 is reserved for the real robot -``` - -Both `docker compose` and `docker/scripts/docker_run.sh` load `docker/.env` -automatically, so every container and every terminal sees the same values. -`ROS_DOMAIN_ID` is passed to all containers; `GEMINI_API_KEY` is passed only to -the `ros` container (the vision pipeline's Gemini backbone and `h12_skills` need -it). Because the run scripts `source` the file, `docker/.env` takes precedence -over any value left exported in your shell. - -## Build the containers - -```bash -docker/scripts/docker_build.sh # all three -docker/scripts/docker_build.sh robocasa ros # subset -docker/scripts/docker_build.sh isaac # isaac only -``` - -The RoboCasa and ROS images both inherit from `golem_base`, which is -built first automatically when either profile is selected. Isaac is -self-contained and does not use the base. - -## Run the RoboCasa container - -```bash -docker/scripts/docker_run.sh robocasa # windowed viewer -docker/scripts/docker_run.sh robocasa --headless # no DISPLAY / SSH / CI -docker/scripts/docker_run.sh robocasa bash # drop to a shell instead - -# to use a custom ROS domain (e.g. several devs on one network), -# set ROS_DOMAIN_ID in docker/.env (see Configuration above) -``` - -Once it's up, RoboCasa publishes `rt/lowstate` over CycloneDDS plus, on the -chosen ROS domain (default `1`): `/clock`; the RealSense topics -`/realsense/{head,left_hand,right_hand}/color/image_raw` and -`.../aligned_depth_to_color/image_raw` (each raw and compressed) with -`.../color/camera_info`; the Livox topics `/livox/lidar` (CustomMsg), -`/livox/pointcloud`, and `/livox/imu`; and `/{left,right}/gripper/state`. - -## Run the ROS container and bringup - -The ROS launcher only builds the workspace and drops to a shell, so bringup -is a manual step. `ROS_DOMAIN_ID` (all containers) and `GEMINI_API_KEY` (the -`ros` container, for the vision pipeline's Gemini backbone and `h12_skills`) -both come from `docker/.env`, so there is nothing to export — just open two -terminals: +x86 Linux with an NVIDIA GPU is the primary platform. Full detail in +[`docs/SETUP.md`](docs/SETUP.md) and [`docs/RUNNING.md`](docs/RUNNING.md). ```bash -# terminal A — start RoboCasa first so /clock is publishing -docker/scripts/docker_run.sh robocasa - -# terminal B — ROS workspace shell (auto-builds core_ws on first run) -docker/scripts/docker_run.sh ros - -# inside the ROS container -ros2 launch h1_bringup h1_sim_bringup.launch.py +git lfs install && git submodule update --init --recursive +cp docker/.env.example docker/.env # set GEMINI_API_KEY and ROS_DOMAIN_ID (never 0) +docker/scripts/docker_build.sh robocasa ros ``` -Bringup starts `joint_state_publisher`, `robot_state_publisher`, the -`frame_task_server` IK solver, the `safety_node`, and `rviz2`. - -## Isaac container — WIP / TODO - -The Isaac profile builds and runs the same way as the others: +Then, in two terminals: ```bash -docker/scripts/docker_build.sh isaac -docker/scripts/docker_run.sh isaac -``` - -The launcher is `docker/scripts/launch_isaac.sh`. Task selection, asset -paths, and the OmniGraph DDS bridge are not yet documented here. Note that -the bridge currently `unset`s `ROS_DOMAIN_ID` and pins itself to channel 1 -regardless of the host setting — bridging to a non-default domain is a -known gap. - -## Network / DDS tuning - -The stack moves bursty, high-bandwidth sensor data (camera images, Livox point -clouds) over CycloneDDS. On a gigabit link the wire has headroom, but the -**default 208 KB kernel UDP receive buffer** overflows on bursts, so the kernel -silently drops datagrams. Lost DDS fragments mean whole point-cloud/image -samples stall or drop — topics go laggy even though the network isn't full. The -tell is `RcvbufErrors` climbing in `/proc/net/snmp`. - -Three settings fix this and **all are required**: - -1. **Host kernel buffers.** The containers run `network_mode: host`, so these - are the host's `net.core` sysctls. CycloneDDS can only *request* a large - socket buffer — the kernel clamps it to `net.core.rmem_max` — so this must be - raised or step 2 has no effect. For the current session: - - ```bash - sudo sysctl -w net.core.rmem_max=2147483647 - sudo sysctl -w net.core.rmem_default=16777216 - sudo sysctl -w net.core.netdev_max_backlog=10000 - ``` - - To persist across reboots, put them in `/etc/sysctl.d/10-cyclonedds.conf`: - - ``` - net.core.rmem_max=2147483647 - net.core.rmem_default=16777216 - net.core.netdev_max_backlog=10000 - ``` +docker/scripts/docker_run.sh robocasa # 1. simulator (start first) - then apply with `sudo sysctl --system`. - -2. **CycloneDDS config.** `core_ws/cyclonedds.xml` requests a 16 MB receive - buffer. The `ros` profile wires it in automatically — `docker-compose.yml` - bind-mounts it to `/home/code/cyclonedds.xml` and sets - `CYCLONEDDS_URI=file:///home/code/cyclonedds.xml`. Edit the file and restart - the container to change the tuning (e.g. to pin the DDS network interface). - -3. **Pin the NIC IRQ off the real-time cores.** `eno1` has a single RX queue, so - *all* inbound DDS traffic is processed in one `NET_RX` softirq on whichever core - holds its IRQ. `irqbalance` parks that IRQ on core 11 — inside the MJPC planner's - `0-11` set — so under a balance run the robot's 500 Hz `/lowstate` stream arrives - at niraj at only ~400 Hz with multi-second stalls (tf fades in rviz, the lowerbody - controller trips its state-stale safe-hold). The wire and the robot are fine; the - loss is this single-core contention. Move the IRQ onto a dedicated core off the RT - set (0–12) and stop irqbalance from moving it back. For the current session: - - ```bash - sudo systemctl stop irqbalance - echo 13 | sudo tee /proc/irq/$(awk -F: '/eno1/{gsub(/ /,"",$1);print $1}' /proc/interrupts)/smp_affinity_list - ``` - - To persist, `sudo systemctl disable --now irqbalance` and run `scripts/pin_net_irq.sh` - at boot (it derives the IRQ, pins it, and prints a ready-made systemd unit). The - launch's CPU map reserves core 13 for exactly this — `OTHER_CPUS` starts at 14 so - no ROS node lands on the network core, and the estimator has core 12. Confirm the - softirq moved (counts should now grow on core 13, not 11): - - ```bash - grep NET_RX /proc/softirqs - ``` - -Verify under load — `RcvbufErrors` should stop climbing: - -```bash -docker exec golem_ros grep '^Udp:' /proc/net/snmp # watch the RcvbufErrors column +docker/scripts/docker_run.sh ros # 2. workspace shell +ros2 launch h1_bringup h1_sim_bringup.launch.py # bringup is a manual step ``` -## Troubleshooting - -- X11 / GUI: run `xhost +local:docker` once per session if rviz, the MuJoCo - viewer, or the slider GUI fail to open. -- Talking to the sim from the host (`ros2 topic list`, standalone `rviz2`) - bypasses the run scripts, so it won't pick up `docker/.env` on its own — - load it into your shell first: `set -a; source docker/.env; set +a`. -- For a clean rebuild of the message workspace, wipe - `container_cache/msgs_ws/` on the host before relaunching. - -## Working example — open the fridge - -End-to-end run of the fridge-opening demo across three terminals. All three -share `ROS_DOMAIN_ID` from `docker/.env` (terminals A and B via the run scripts, -terminal C via the already-running container), so there's nothing to export. +And in a third, ask the robot to do something: ```bash -# terminal A — RoboCasa (start first so /clock is publishing) -./docker/scripts/docker_run.sh robocasa --task OpenFridge --layout 1 --style 1 --seed 42 - -# terminal B — ROS workspace shell, then launch bringup -docker/scripts/docker_run.sh ros -ros2 launch h1_bringup h1_sim_bringup.launch.py - -# terminal C — exec into the running ROS container and run the demo docker exec -it golem_ros bash -source /opt/ros/humble/setup.bash -source /home/code/core_ws/install/setup.bash - -# send the arms to the 'home' named config before grasping -ros2 action send_goal /named_config custom_ros_messages/action/NamedConfig \ - "{config_name: 'home', duration: {sec: 0, nanosec: 0}}" - +source /opt/ros/humble/setup.bash && source /home/code/core_ws/install/setup.bash ros2 action send_goal /skill/grasp custom_ros_messages/action/SkillGrasp \ - "{target_object: 'vertical fridge handle', arm: 'right', timeout: {sec: 60, nanosec: 0}}" \ - --feedback -``` - -## macOS (Apple Silicon) — headless CPU port - -On Apple-Silicon Macs there is a self-contained, CPU-only port that runs **just -ROS 2 + MuJoCo/RoboCasa** — Isaac is dropped and there is no NVIDIA/EGL path, so -MuJoCo renders in software (OSMesa/llvmpipe). It uses its own standalone compose -file, `docker/mac/docker-compose.yml`, and runs the ROS layer on **FastDDS** -instead of CycloneDDS (an arm64 in-process coexistence fix). None of the x86 -instructions above apply — use this section instead. - -### Prerequisites - -- [Colima](https://github.com/abiosoft/colima) + the Docker CLI - (`brew install colima docker docker-compose`). No Docker Desktop, no NVIDIA - toolkit, and no XQuartz (see the GUI note below). -- Git LFS and submodules, exactly as in [Prerequisites](#prerequisites) above. -- No `docker/.env` is needed; the Mac compose reads `ROS_DOMAIN_ID` (optional, - defaults to `1`) plus the `GOLEM_*` feature toggles (`GOLEM_DISPLAY`, - `GOLEM_RVIZ`, `GOLEM_LOWERBODY`, `GOLEM_SLAM`, `GOLEM_NAV2`, …) documented below. -- Start the VM with enough resources — one software-GL viewer alone uses ~5 - cores, and running both the MuJoCo viewer and RViz wants headroom: - - ```bash - colima start --cpu 12 --memory 24 # sized for a 14-core / 48 GB Mac; scale to yours - ``` - -### Build and run - -```bash -# build both arm64 images (robocasa + ros); the base image builds automatically -docker/mac/scripts/docker_build_mac.sh # or: docker_build_mac.sh robocasa | ros - -# headless (no viewer) — the default; brings BOTH services up together -docker compose -f docker/mac/docker-compose.yml up -``` - -`docker/mac/scripts/docker_run_mac.sh [robocasa|ros]` is the Mac counterpart of the -x86 `docker/scripts/docker_run.sh` — it starts one service with a stable name -(`golem_sim_robocasa` / `golem_ros`), a `bash`/`` override, and the same -`GOLEM_DISPLAY`/`GOLEM_RVIZ` env passthrough. Because it runs a single service, use -it in two terminals (start `ros` promptly) or for one-off shells; prefer -`docker compose … up` for the everyday paired sim so RoboCasa never runs alone -(see the collapse warning below). - -The first run is slow (colcon builds the message + controller workspaces into -named volumes); later runs are cached and fast. Both containers use -`network_mode: host` + `ipc: host` so FastDDS's shared-memory transport works -across them. - -> **Always bring both containers up together.** If RoboCasa runs alone for more -> than a few seconds it releases the robot's motors ("Command timeout"), the H1 -> collapses under gravity, and when the ROS controller then connects it reads the -> fallen pose and trips its e-stop. `docker compose … up` (both services) engages -> the controller before the robot can fall. - -### Viewing the GUIs (MuJoCo viewer + RViz) - -MuJoCo's and RViz's OpenGL windows **cannot** be forwarded to XQuartz — -Apple-Silicon XQuartz has broken indirect GLX. Instead each is rendered -in-container with software GL into a virtual display and streamed to your browser -over noVNC. Enable them per-viewer: - -```bash -# MuJoCo viewer on :6080, RViz on :6081 (set either or both) -GOLEM_DISPLAY=vnc GOLEM_RVIZ=vnc docker compose -f docker/mac/docker-compose.yml up -d - -# open the SSH tunnel to both noVNC ports (Colima does not forward container ports) -./docker/mac/scripts/mac_vnc_tunnel.sh # --open also opens the browser; --stop closes the tunnel -``` - -Then open: - -- **MuJoCo viewer** → -- **RViz** (RobotModel + TF) → - -`GOLEM_DISPLAY` (RoboCasa/MuJoCo) and `GOLEM_RVIZ` (ROS/RViz) are independent — set -either or both; the defaults are headless. - -### Driving the robot - -Bringup starts automatically in the `ros` container (`joint_state_publisher`, -`robot_state_publisher`, the `frame_task_server` IK solver, `safety_node`). To -command the H1, source the helper and send joint-space postures or gripper -commands: - -```bash -docker exec -it golem_ros bash # if the host docker CLI is flaky: colima ssh, then docker exec … -source /home/code/h12_sim_scripts/robot_cli.sh - -rob_poses # list postures -rob_pose t_pose # MOVE: home t_pose arms_front arms_overhead elbow_only … (rob_poses lists all) -rob_grip right close # open/close a gripper -rob_home -``` - -Watch the motion in either viewer. (`rob_pose` takes a name and moves the robot; -`rob_poses` only prints the list.) - -### Lower-body controller (standing free) - -By default an elastic-band tether holds the robot upright and only the upper body -is controlled. Set `GOLEM_LOWERBODY=fame` to run the RMA lower-body policy, which -releases the tether and **balances the robot standing unsupported**: - -```bash -GOLEM_DISPLAY=vnc GOLEM_RVIZ=vnc GOLEM_LOWERBODY=fame \ - docker compose -f docker/mac/docker-compose.yml up + "{target_object: 'vertical fridge handle', arm: 'right', timeout: {sec: 60, nanosec: 0}}" --feedback ``` -`GOLEM_LOWERBODY=fame` **stands** (and squats via `/lowerbody/squat_cmd`) but holds -position — it does not locomote. For **stand *and* walk**, use the switchable -controller instead: - -```bash -GOLEM_DISPLAY=vnc GOLEM_RVIZ=vnc GOLEM_LOWERBODY=switch \ - docker compose -f docker/mac/docker-compose.yml up -``` - -It starts band-held idle and auto-switches stand↔walk from the commanded velocity: -`rob_stand` settles it into FAME (stand free) and `rob_go ` drives the -TorchScript walk policy — any non-zero `/cmd_vel` engages walk automatically, so -there is no manual `rob_walk` handover step (`rob_walk` just prints a reminder). The -walk policy **does stay upright** now, handed over from a settled FAME stance — the -earlier "falls a few seconds after the tether releases" was a too-tight motor -watchdog on the slow sim (see gotchas). Launching the raw policy directly -(`GOLEM_LOWERBODY=walk`) still just marches in place; use `switch`. (`torch` is -already in the ros image; building `unitree_hg` needs `rosidl-generator-dds-idl`, -which the image now includes.) - -### Navigation demo (SLAM + nav2 + frontier exploration) +On Apple Silicon there is a CPU-only port that runs ROS 2 + MuJoCo without +Isaac — see [`docs/MACOS.md`](docs/MACOS.md). -The robot can map the kitchen and drive itself around autonomously — 2D SLAM from -the lidar, nav2 planning collision-free paths on a costmap that includes the full -3D cloud, and a frontier explorer sending goals. Full walkthrough: -**[docs/NAVIGATION_DEMO.md](docs/NAVIGATION_DEMO.md)**. In short: - -```bash -GOLEM_DISPLAY=vnc GOLEM_RVIZ=vnc GOLEM_CAMERAS=0 GOLEM_SIM_ODOM=1 \ -GOLEM_LOWERBODY=switch GOLEM_SLAM=1 GOLEM_NAV2=1 GOLEM_SPAWN_BACKOFF=1.5 \ - docker compose -f docker/mac/docker-compose.yml up -d -# then, inside golem_ros: -# source /home/code/h12_sim_scripts/robot_cli.sh -# rob_stand # FAME stand (wait ~15 s sim time for the tether to release) -# rob_explore # walk handover + the /skill/frontier_explore action (autonomous mapping) -``` - -`rob_explore` is a shorthand; send the action directly if you want to tune the -frontier params or watch feedback. Any field left at `0` falls back to the node -default (shown in the comments): - -```bash -ros2 action send_goal /skill/frontier_explore custom_ros_messages/action/SkillFrontierExplore \ - "{min_frontier_cells: 6, blacklist_radius: 0.6, min_goal_distance: 0.7, goal_timeout: 120.0, timeout: {sec: 1800, nanosec: 0}}" \ - --feedback -# min_frontier_cells : ignore frontier clusters smaller than this (0 -> 6) -# blacklist_radius : m; blacklist a failed goal within this radius (0 -> 0.6) -# min_goal_distance : m; skip frontiers closer than this (0 -> 0.7) -# goal_timeout : s; cancel a single nav goal after this (0 -> 120) -# timeout : overall exploration budget (Duration) (0 -> node default) -# Feedback streams phase / progress / frontiers_remaining; result is -# success / message / goals_reached. Ctrl-C cancels the goal. -``` - -`GOLEM_SIM_ODOM=1` publishes the sim's ground-truth base odometry (`/odom` + -`odom→pelvis` TF) that SLAM consumes; it is off by default so the shared sim -bridge stays silent on the real/x86 stack, and the nav demo opts in. - -Watch the map, costmap, and green plan build in RViz (). - -### ROS debugging MCP server (optional) - -`GOLEM_ROS_MCP=1` starts an in-container MCP server that exposes ROS-inspection and -robot-driving tools (`robot_status`, `wait_for`, `costmap_summary`, `drive`, …) to -Claude Code over the same SSH tunnel as the viewers. Setup and tool list: -**[docs/ROS_MCP_DEBUG.md](docs/ROS_MCP_DEBUG.md)**. - -### macOS gotchas +## Layout -- **The host `docker` CLI socket is intermittent** under Colima — `docker …` may - fail with "Cannot connect to the Docker daemon" while the VM and containers are - perfectly healthy. Use `colima ssh -- docker …` as the reliable fallback, or - `colima stop && colima start` to relink the socket (this restarts the containers). -- **Cartesian reaching is disabled.** The `/frame_task` action currently crashes - the controller node, so `rob_reach` is a no-op stub — drive the robot in joint - space with `rob_pose` for now. -- **Two viewers use two displays.** RoboCasa renders on X display `:99` and RViz - on `:100`; they must differ because the containers share one network namespace - (`network_mode: host`). The launchers already handle this. -- **Restart the two containers coherently.** The RoboCasa container owns `/clock`. - Restarting it alone resets sim time to 0 while the ROS side keeps its old clock → - TF extrapolation errors and a frozen `0×0` SLAM map. After restarting/recreating - `robocasa`, restart `golem_ros` too so it resyncs to the fresh clock. -- **`GOLEM_SPAWN_BACKOFF` is baked at container-create.** `docker restart` reuses the - old value; use `docker compose up --force-recreate --no-deps robocasa` (with the - env set) to change it. For nav, `1.5`–`2.0` keeps the robot off the counters. -- **The sim motor watchdog is sim-time based.** The low-level interface zeroes the - motors if no `rt/lowcmd` arrives within `GOLEM_CMD_TIMEOUT` (default `0.5`) seconds - **of sim time** — measured in sim time on purpose, because a wall-clock timeout is - far too tight on a sim running ~0.2× real-time (it used to drop the robot ~0.7 s - after the tether released). Bump `GOLEM_CMD_TIMEOUT` if a heavier scene still trips it. +| Path | What it is | +|---|---| +| `core_ws/` | the ROS 2 workspace — 19 packages, where most development happens | +| `h1_robocasa/` | RoboCasa/MuJoCo simulator entry point and its ROS/DDS bridges | +| `CL_isaaclab_sim/` | Isaac Lab simulator | +| `CL_Assets/` | URDF meshes, MuJoCo XML, Isaac USD (Git-LFS) | +| `docker/` | x86 images, compose file, and the build/run scripts | +| `docker/mac/` | the self-contained Apple-Silicon port | +| `mujoco_mpc/`, `unitree_sdk2_python/` | vendored upstream dependencies | +| `tools/` | standalone debugging tools | + +Most of the tree is git submodules, and large assets are Git-LFS. + +## Documentation + +| Document | Covers | +|---|---| +| [`docs/SETUP.md`](docs/SETUP.md) | prerequisites, `.env`, model weights, building the images | +| [`docs/RUNNING.md`](docs/RUNNING.md) | run flows, bringup, skills, real robot, troubleshooting | +| [`docs/MACOS.md`](docs/MACOS.md) | the Apple-Silicon CPU port and its feature toggles | +| [`docs/NAVIGATION_DEMO.md`](docs/NAVIGATION_DEMO.md) | autonomous SLAM + nav2 frontier exploration | +| [`docs/ROS_MCP_DEBUG.md`](docs/ROS_MCP_DEBUG.md) | the in-container ROS debugging MCP server | +| [`docs/DDS_TUNING.md`](docs/DDS_TUNING.md) | kernel and CycloneDDS tuning for the sensor streams | +| [`docker/BUILD.md`](docker/BUILD.md) | how the images are layered and what is pinned | +| [`CLAUDE.md`](CLAUDE.md) | orientation for coding agents: packages, conventions, invariants | + +Individual packages document themselves — start with the package's own +`README.md` and source. diff --git a/core_ws/cyclonedds.xml b/core_ws/cyclonedds.xml index a6871b8..e5a4e1c 100644 --- a/core_ws/cyclonedds.xml +++ b/core_ws/cyclonedds.xml @@ -14,7 +14,7 @@ NOTE: SocketReceiveBufferSize below is only *requested*; the kernel clamps it to net.core.rmem_max. You MUST raise that on the host too or this has no - effect (see README "Network / DDS tuning"): + effect (see docs/DDS_TUNING.md): sudo sysctl -w net.core.rmem_max=2147483647 --> [cmd…]`: -- Sources `docker/.env` (`GEMINI_API_KEY`, `ROS_DOMAIN_ID`, …). -- Pre-creates host bind sources (`container_cache/msgs_ws`, - `container_cache/mjpc_build`, the nested `mujoco_mpc/build`) so dockerd doesn't - create them root-owned. -- Normalizes `ROS_DOMAIN_ID` (empty→1; `0` rejected for sims, confirmed for `ros`). -- `xhost +local:docker`, stable container names (`golem_ros`, `golem_sim_*`), `--rm`. - -The Apple-Silicon port mirrors these as `docker/mac/scripts/docker_build_mac.sh` and -`docker/mac/scripts/docker_run_mac.sh` (against `docker/mac/docker-compose.yml`): -services `robocasa`/`ros` only (no `isaac`), base built from -`mac/BaseDockerfile.arm64`, no `MJPC_REF`, no `xhost`. See the README macOS section. - -**`docker-compose.yml`** defines three profiles (`isaac`, `robocasa`, `ros`), each -with `runtime: nvidia`, `network_mode: host`, X11 passthrough, and the bind mounts. -Each profile's `command:` is its `launch_*.sh`. - -**`.dockerignore` is deny-all (`*`)** with a tiny whitelist — everything is -bind-mounted at runtime, so only the few files a Dockerfile actually `COPY`s -(`CL_isaaclab_sim/requirements.txt`, `core_ws/src/livox_ros_driver2`, -`unitree_sdk2_python`) are sent to the daemon. This keeps the build context small -(~GB otherwise) and is why `mujoco_mpc` needs no `.dockerignore` entry. - -**Launchers:** -- `launch_ros.sh` — sources ROS, hydrates the MJPC cache (§7), then `colcon build - --symlink-install`s `core_ws` **only if needed** (no `install/`, or any - `package.xml` newer than `install/setup.bash`), sources the overlay, drops to a - shell. `livox_ros_driver2` is built via its own `build.sh` (patched idempotently - to add `--symlink-install`). -- `launch_robocasa.sh` — sources ROS + `/opt/livox_ws`, `colcon build`s the two IDL - packages in `msgs_ws`, picks `MUJOCO_GL`, runs `h12_mujoco.py`. -- `launch_isaac.sh` — runs the **Unitree DDS relay** (`dds_bridge.py`, a - CycloneDDS↔CycloneDDS relay of `rt/lowstate`, `rt/lowcmd`, `rt/inspire/*`, … - between the sim domain and the command domain) in the background, then - `CL_isaaclab_sim/sim_main.py` from the conda interpreter. (The OmniGraph ROS 2 - bridge is a *separate* mechanism, loaded inside Kit by `sim_main` — see §6.) - Isaac task selection is WIP. +**Build** — `docker/scripts/docker_build.sh [--pull] [--no-cache] [service ...]`. +Defaults to all three services. Builds `golem_base` first for ROS/RoboCasa; +propagates the committed MJPC pin into the ROS build. `--pull` refreshes public +upstream images (base and Isaac); child builds use the freshly built local +`golem_base`, rather than attempting to pull that private local tag. + +**Run one service** — `docker/scripts/docker_run.sh SERVICE [--restart] [cmd...]`. +A leading flag invokes that service's launcher. Existing containers are never +removed implicitly; `--restart` explicitly replaces the selected container. + +**Run a pair** — `docker/scripts/docker_stack.sh up|restart|stop|logs SIM`. +The startup command builds the ROS and RoboCasa message workspaces without +starting physics, then starts the simulator and ROS bringup. ROS waits for a +fresh `rt/lowstate` sample on the configured domain. `GOLEM_SIM_TIMEOUT` bounds +that wait; a timeout fails ROS startup and directs users to simulator logs. +`restart` stops both services before recreating them. `stop` preserves caches. + +Both wrapper families source `docker/scripts/docker_common.sh`. They explicitly +load `docker/.env`, preserving exported shell overrides, validate the domain, +check required source paths, and create only cache/mountpoint directories. +`GOLEM_ASSETS_DIR` and `GOLEM_CACHE_DIR` accept absolute paths or paths relative +to the checkout. Required bind sources use `create_host_path: false`. + +Linux Compose is headless by itself. The wrapper selects +`docker-compose.gui.yml` when `GOLEM_DISPLAY=gui`, or automatically when +`DISPLAY` is set. GUI mode requires an Xauthority file; it does not change the +host's X access policy. `--headless` avoids desktop mounts entirely. NVIDIA +runtime and host networking remain in the Linux configuration. + +The Mac wrappers expose the same build/run/stack operations for `robocasa` and +`ros` only. Images use the `:arm64` tag and `linux/arm64` platform. Mac uses +FastDDS, host networking/IPC, and native VM named volumes for colcon build and +install trees. Keep those named volumes: host filesystem shares make symlink +builds slow. VNC is opt-in; there are no NVIDIA or host X11 requirements. + +The ROS image bakes the pinned Unitree C++ SDK at `/opt/unitree_install`. +No SDK clone or SDK-cache mount is needed at startup. The separate MJPC warm +build cache and source mounts remain for controller development. + +`.dockerignore` restricts build context to sources actually copied by the +Dockerfiles: the Unitree Python SDK and Livox driver. Simulation source, model +weights, and assets stay outside the context and arrive through bind mounts. --- @@ -302,8 +297,9 @@ are near-instant; wipe the host dir for a clean rebuild. RoboCasa's pin). - **Layer-order discipline:** heavy/stable layers first; volatile pins appended last. New deps go at the *end* of a Dockerfile so they don't bust cached layers. -- **CUDA wheel split:** `ros`/base use torch **cu130**; Isaac uses **cu128**; - robocasa ends up on a cu121/CPU torch (via `lerobot`). Deliberate, per image. +- **CUDA wheel split:** `ros`/base use torch **cu130**; Isaac inherits NVIDIA's + bundled stack; RoboCasa resolves its own torch through `lerobot`. Inspect + the built images when diagnosing GPU/torch compatibility. --- diff --git a/docker/IsaacDockerfile b/docker/IsaacDockerfile index 2f57d5b..57f35e9 100644 --- a/docker/IsaacDockerfile +++ b/docker/IsaacDockerfile @@ -115,5 +115,6 @@ RUN --mount=type=cache,target=/var/cache/apt \ WORKDIR ${GOLEM_WORKSPACE_PATH} -ENTRYPOINT ["/bin/bash"] +ENTRYPOINT [] +CMD ["/bin/bash"] diff --git a/docker/RobocasaDockerfile b/docker/RobocasaDockerfile index f338001..eb9da96 100644 --- a/docker/RobocasaDockerfile +++ b/docker/RobocasaDockerfile @@ -22,9 +22,8 @@ RUN apt-get update && apt-get install -y --no-install-recommends \ && rm -rf /var/lib/apt/lists/* # MuJoCo Python binding + Pillow for image encoding. -# NOTE: pinned to 3.3.1 to match Robocasa's hard pin (see below). Earlier -# this image used 3.6.0; downgraded so that `pip install robocasa` doesn't -# silently swap the version mid-install. +# Pinned to 3.3.1 to match Robocasa's hard pin (see below); any other version +# is silently swapped out mid-install by `pip install robocasa`. RUN uv pip install --system --no-cache \ "mujoco==3.3.1" \ "numpy>=2.2.6" \ diff --git a/docker/RosDockerfile b/docker/RosDockerfile index e2cabc6..24be8a6 100644 --- a/docker/RosDockerfile +++ b/docker/RosDockerfile @@ -292,11 +292,29 @@ RUN git clone --depth 1 https://github.com/NVlabs/GraspGenX.git /opt/graspgenx \ && pip install --no-cache-dir --no-deps /tmp/gwheel/graspgenx-*.whl \ && rm -rf /tmp/bsetup /tmp/gwheel -# Remove a broken Pillow dist-info ghost (pillow-12.2.0.dist-info holds only a -# REQUESTED file, no METADATA) left in an upstream layer; it crashes pip's -# metadata scan the moment a Pillow-dependent dep is touched. The real, intact -# Pillow 11.0.0 install is untouched. -RUN rm -rf /usr/local/lib/python3.10/dist-packages/pillow-12.2.0.dist-info +# Sweep orphaned .dist-info ghosts: directories holding a stray file but no +# METADATA. pip reports such a ghost's version from the DIRECTORY NAME (so +# `pip freeze` emits a version that is not installed), and hard-crashes with +# "OSError: [Errno 2] ... /METADATA" the moment a dep of that package is touched. +# +# How one is manufactured here (Pillow is the recurring victim): the base image's +# `uv pip install torch torchvision` writes pillow-.dist-info INCLUDING a +# REQUESTED entry in its RECORD. The controller-stack install in section 3 +# re-installs that same pillow for meshcat/open3d -- its +# `--ignore-installed` is a GLOBAL flag, not scoped to `blinker` -- overwriting +# the dist-info with a pip-written RECORD that does NOT list REQUESTED (pillow +# was not explicitly requested). The later `Pillow==11.0.0` pin then uninstalls +# , removing every RECORD-listed file and leaving uv's orphaned REQUESTED +# behind. +# +# Version-agnostic on purpose: a hardcoded dist-info name becomes a silent no-op +# as soon as upstream ships a different Pillow. The real, intact Pillow 11.0.0 +# install is untouched; the assert proves it. +RUN for d in /usr/local/lib/python3.10/dist-packages/*.dist-info; do \ + [ -d "$d" ] || continue; \ + if [ ! -e "$d/METADATA" ]; then echo "removing orphaned dist-info: $d"; rm -rf "$d"; fi; \ + done \ + && python3 -c "import PIL, importlib.metadata as m; r=m.version('pillow'); assert r == PIL.__version__, f'pillow metadata {r} != runtime {PIL.__version__}'; print('pillow ok:', r)" # Freeze the ENTIRE current environment as the additive-install constraints (see # header). grep '==' drops editable / file-url / comment lines pip freeze emits. @@ -447,21 +465,20 @@ RUN grep -viE '^(grpcio|grpcio-tools|grpcio-status|protobuf|mujoco)==' \ # 4. Build the C++ agent_server, install the importable `mujoco_mpc` package, # stage libmujoco, STASH the warm build tree as the incremental-cache seed, -# then REMOVE the source clone. The three source fixes that used to be sed'd -# in here now live as commits on extended_hw_patched, so the seed and the -# runtime-mounted submodule source are byte-identical (a hard requirement for -# the runtime incremental-build cache) and there is NO in-Dockerfile patching. -# The original gotchas still hold and are satisfied by those committed fixes -# plus build-first ordering: +# then REMOVE the source clone. Source fixes live as commits on the fork's +# extended_hw_patched branch, NOT as in-Dockerfile patching, so the seed and +# the runtime-mounted submodule source stay byte-identical (a hard requirement +# for the runtime incremental-build cache). Those commits plus build-first +# ordering are what satisfy these gotchas: # (a) setup.py's build_py.copy_task_assets asserts build/mjpc/tasks exists, # which only CMake (build_ext) produces — so run the CMake build FIRST; # the install's build_ext re-config is then an incremental no-op. Building # just agent_server pulls the copy_resources / copy_model_resources ALL # targets that populate build/mjpc/tasks. -# (b) extended_hw deleted the standalone ui_agent_server executable, so the -# fork's setup.py no longer --targets/copies it (it aborted Ninja). -# (c) the stale h1_2_modified_magpie.xml.patch is replaced by a direct CMake -# copy of the base model in the fork's mjpc/tasks/CMakeLists.txt. +# (b) the fork has no standalone ui_agent_server executable, so its setup.py +# must not --target or copy one (that aborts Ninja). +# (c) the base model is copied directly by the fork's +# mjpc/tasks/CMakeLists.txt, not applied as an XML patch. # (d) copy_task_assets globs are case-insensitive in the fork, so the H1-2 # UPPERCASE .STL meshes reach site-packages. # @@ -557,3 +574,13 @@ RUN apt-get update && apt-get install -y --no-install-recommends \ ros-humble-realsense2-camera \ ros-humble-image-transport-plugins \ && rm -rf /var/lib/apt/lists/* + +# The controller shim links this pinned SDK. Install at build time so launching +# a workspace does not require network access or a per-host SDK cache. +ARG UNITREE_SDK_REF=63c6f53103e2f28e23b807d7399e92338c9d07d3 +RUN git clone https://github.com/unitreerobotics/unitree_sdk2.git /tmp/unitree_sdk2 \ + && git -C /tmp/unitree_sdk2 checkout --quiet "${UNITREE_SDK_REF}" \ + && cmake -S /tmp/unitree_sdk2 -B /tmp/unitree_sdk2/build -DCMAKE_INSTALL_PREFIX=/opt/unitree_install \ + && cmake --install /tmp/unitree_sdk2/build \ + && test -f /opt/unitree_install/lib/libunitree_sdk2.a \ + && rm -rf /tmp/unitree_sdk2 diff --git a/docker/docker-compose.gui.yml b/docker/docker-compose.gui.yml new file mode 100644 index 0000000..cc09f9f --- /dev/null +++ b/docker/docker-compose.gui.yml @@ -0,0 +1,21 @@ +# Linux desktop additions; the host wrapper selects this only for GUI mode. +x-gui: &gui + environment: + DISPLAY: ${DISPLAY:?GUI mode needs DISPLAY} + XAUTHORITY: /root/.Xauthority + volumes: + - type: bind + source: /tmp/.X11-unix + target: /tmp/.X11-unix + bind: + create_host_path: false + - type: bind + source: ${XAUTHORITY:-${HOME}/.Xauthority} + target: /root/.Xauthority + read_only: true + bind: + create_host_path: false +services: + isaac: *gui + robocasa: *gui + ros: *gui diff --git a/docker/docker-compose.stack.yml b/docker/docker-compose.stack.yml new file mode 100644 index 0000000..c7cae7a --- /dev/null +++ b/docker/docker-compose.stack.yml @@ -0,0 +1,13 @@ +# Selected by docker_stack.sh; shell launches keep their existing default. +services: + ros: + environment: + GOLEM_WAIT_FOR_SIM: "1" + GOLEM_SIM_TIMEOUT: ${GOLEM_SIM_TIMEOUT:-180} + command: + - /home/code/h12_sim_scripts/launch_ros.sh + - ros2 + - launch + - h1_bringup + - h1_sim_bringup.launch.py + - use_rviz:=${GOLEM_USE_RVIZ:-false} diff --git a/docker/docker-compose.yml b/docker/docker-compose.yml index c5b362e..23bc699 100644 --- a/docker/docker-compose.yml +++ b/docker/docker-compose.yml @@ -5,7 +5,10 @@ services: build: context: .. dockerfile: docker/IsaacDockerfile + entrypoint: [] image: golem_sim_isaac:latest + platform: linux/amd64 + init: true runtime: nvidia privileged: true shm_size: 16g @@ -13,24 +16,49 @@ services: stdin_open: true tty: true environment: - - DISPLAY=${DISPLAY} - NVIDIA_VISIBLE_DEVICES=all - NVIDIA_DRIVER_CAPABILITIES=all - QT_X11_NO_MITSHM=1 - LIBGL_ALWAYS_INDIRECT=0 - ROS_DOMAIN_ID=${ROS_DOMAIN_ID:-1} volumes: - - /tmp/.X11-unix:/tmp/.X11-unix:rw - - ${XAUTHORITY:-$HOME/.Xauthority}:/root/.Xauthority:ro - - /dev/input:/dev/input:ro - - /dev/dri:/dev/dri:rw - - ../CL_Assets:/home/code/CL_Assets:ro - - ./scripts:/home/code/h12_sim_scripts - - ../CL_isaaclab_sim:/home/code/CL_isaaclab_sim - - ../CL_isaaclab_sim/.isaac_cache/ov:/root/.cache/ov - - ../CL_isaaclab_sim/.isaac_cache/nvidia:/root/.nv - - ../CL_isaaclab_sim/.isaac_cache/local_share_ov:/root/.local/share/ov - - ../CL_isaaclab_sim/.isaac_cache/isaacsim:/root/.isaacsim + - type: bind + source: ${GOLEM_ASSETS_DIR:-../CL_Assets} + target: /home/code/CL_Assets + read_only: true + bind: + create_host_path: false + - type: bind + source: ./scripts + target: /home/code/h12_sim_scripts + read_only: true + bind: + create_host_path: false + - type: bind + source: ../CL_isaaclab_sim + target: /home/code/CL_isaaclab_sim + bind: + create_host_path: false + - type: bind + source: ${GOLEM_CACHE_DIR:-../container_cache}/isaac/ov + target: /root/.cache/ov + bind: + create_host_path: false + - type: bind + source: ${GOLEM_CACHE_DIR:-../container_cache}/isaac/nvidia + target: /root/.nv + bind: + create_host_path: false + - type: bind + source: ${GOLEM_CACHE_DIR:-../container_cache}/isaac/local_share_ov + target: /root/.local/share/ov + bind: + create_host_path: false + - type: bind + source: ${GOLEM_CACHE_DIR:-../container_cache}/isaac/isaacsim + target: /root/.isaacsim + bind: + create_host_path: false command: /home/code/h12_sim_scripts/launch_isaac.sh robocasa: @@ -40,6 +68,8 @@ services: context: .. dockerfile: docker/RobocasaDockerfile image: golem_sim_robocasa:latest + platform: linux/amd64 + init: true runtime: nvidia privileged: true shm_size: 16g @@ -47,7 +77,6 @@ services: stdin_open: true tty: true environment: - - DISPLAY=${DISPLAY} - NVIDIA_VISIBLE_DEVICES=all - NVIDIA_DRIVER_CAPABILITIES=all - QT_X11_NO_MITSHM=1 @@ -57,13 +86,23 @@ services: - MUJOCO_GL=${MUJOCO_GL:-glfw} - ROS_DOMAIN_ID=${ROS_DOMAIN_ID:-1} volumes: - - /tmp/.X11-unix:/tmp/.X11-unix:rw - - ${XAUTHORITY:-$HOME/.Xauthority}:/root/.Xauthority:ro - - /dev/input:/dev/input:ro - - /dev/dri:/dev/dri:rw - - ../h1_robocasa:/home/code/h1_robocasa - - ../CL_Assets:/home/code/CL_Assets:ro - - ./scripts:/home/code/h12_sim_scripts + - type: bind + source: ../h1_robocasa + target: /home/code/h1_robocasa + bind: + create_host_path: false + - type: bind + source: ${GOLEM_ASSETS_DIR:-../CL_Assets} + target: /home/code/CL_Assets + read_only: true + bind: + create_host_path: false + - type: bind + source: ./scripts + target: /home/code/h12_sim_scripts + read_only: true + bind: + create_host_path: false # Only the message packages the magpi sim needs — keeps the rest of # core_ws (model_server weights, FAST_LIO, root-owned host build # dir, etc.) out of this container. The parent rw mount persists @@ -71,9 +110,23 @@ services: # colcon's incremental rebuild on launch is a near-no-op when the # IDL is unchanged; the two inner :ro mounts override their src/ # subdirs so the package sources stay read-only. - - ../container_cache/msgs_ws:/home/code/msgs_ws:rw - - ../core_ws/src/magpie_msgs:/home/code/msgs_ws/src/magpie_msgs:ro - - ../core_ws/src/custom_ros_messages:/home/code/msgs_ws/src/custom_ros_messages:ro + - type: bind + source: ${GOLEM_CACHE_DIR:-../container_cache}/msgs_ws + target: /home/code/msgs_ws + bind: + create_host_path: false + - type: bind + source: ../core_ws/src/magpie_msgs + target: /home/code/msgs_ws/src/magpie_msgs + read_only: true + bind: + create_host_path: false + - type: bind + source: ../core_ws/src/custom_ros_messages + target: /home/code/msgs_ws/src/custom_ros_messages + read_only: true + bind: + create_host_path: false command: /home/code/h12_sim_scripts/launch_robocasa.sh ros: @@ -88,6 +141,8 @@ services: args: - MJPC_REF image: golem_ros:latest + platform: linux/amd64 + init: true runtime: nvidia privileged: true shm_size: 16g @@ -95,7 +150,6 @@ services: stdin_open: true tty: true environment: - - DISPLAY=${DISPLAY} - NVIDIA_VISIBLE_DEVICES=all - NVIDIA_DRIVER_CAPABILITIES=all - QT_X11_NO_MITSHM=1 @@ -104,7 +158,7 @@ services: # sensor traffic (camera images, Livox point clouds) stops overflowing the # socket buffer and dropping DDS fragments. Config file bind-mounted below; # also raise net.core.rmem_max on the host or this has no effect - # (see README "Network / DDS tuning"). + # (see docs/DDS_TUNING.md). - CYCLONEDDS_URI=file:///home/code/cyclonedds.xml # Gemini API key for the vision pipeline's gemini backbone + h12_skills. # Passed through from the host shell: export GEMINI_API_KEY before running. @@ -118,41 +172,69 @@ services: # into via the bind mounts below. graspgen_server requires this explicitly. - GRASPGENX_ASSETS_DIR=/opt/graspgenx/assets volumes: - - /tmp/.X11-unix:/tmp/.X11-unix:rw - - ${XAUTHORITY:-$HOME/.Xauthority}:/root/.Xauthority:ro - - ../core_ws:/home/code/core_ws + - type: bind + source: ../core_ws + target: /home/code/core_ws + bind: + create_host_path: false # CycloneDDS tuning config (see CYCLONEDDS_URI above). core_ws is already # mounted, but bind the file explicitly and read-only so the setting is # obvious and can't be clobbered from inside the container. - - ../core_ws/cyclonedds.xml:/home/code/cyclonedds.xml:ro - - ../CL_Assets:/home/code/CL_Assets:ro + - type: bind + source: ../core_ws/cyclonedds.xml + target: /home/code/cyclonedds.xml + read_only: true + bind: + create_host_path: false + - type: bind + source: ${GOLEM_ASSETS_DIR:-../CL_Assets} + target: /home/code/CL_Assets + read_only: true + bind: + create_host_path: false # Stage the magpie GraspGenX gripper description into the (image-baked) # GraspGenX assets so gripper_name=magpie resolves. Mount the LEAF dirs only — # mounting all of assets/ would shadow the baked proc_grippers/ + sample_data/. # gripper.urdf references ../meshes/magpie/, so the meshes leaf is mounted at # x_grippers/meshes/magpie (one level up from the gripper dir). - - ../CL_Assets/graspgenx/assets/magpie:/opt/graspgenx/assets/x_grippers/magpie:ro - - ../CL_Assets/graspgenx/assets/meshes/magpie:/opt/graspgenx/assets/x_grippers/meshes/magpie:ro - - ./scripts:/home/code/h12_sim_scripts + - type: bind + source: ${GOLEM_ASSETS_DIR:-../CL_Assets}/graspgenx/assets/magpie + target: /opt/graspgenx/assets/x_grippers/magpie + read_only: true + bind: + create_host_path: false + - type: bind + source: ${GOLEM_ASSETS_DIR:-../CL_Assets}/graspgenx/assets/meshes/magpie + target: /opt/graspgenx/assets/x_grippers/meshes/magpie + read_only: true + bind: + create_host_path: false + - type: bind + source: ./scripts + target: /home/code/h12_sim_scripts + read_only: true + bind: + create_host_path: false # --- MuJoCo MPC (MJPC) dev mounts ------------------------------------- # rw SOURCE mount: the ../mujoco_mpc submodule fills the (build-time-removed) # clone path so the C++ agent_server / task XMLs are host-editable. The # importable package lives baked in dist-packages, so `import mujoco_mpc` # works with zero rebuild; keep this checkout pinned at MJPC_REF. - - ../mujoco_mpc:/home/code/mujoco_mpc:rw + - type: bind + source: ../mujoco_mpc + target: /home/code/mujoco_mpc + bind: + create_host_path: false # Persistent CMake build tree, nested INSIDE the source mount at the exact # in-tree path the image seed was built at (/home/code/mujoco_mpc/build), so # CMakeCache absolute paths match and setup.py's build_ext reuses it. Host # bytes live in gitignored container_cache/mjpc_build; launch_ros.sh hydrates # it from /opt/mjpc-build-seed on first launch (see msgs_ws pattern above). - - ../container_cache/mjpc_build:/home/code/mujoco_mpc/build:rw - # unitree_sdk2 C++ SDK install (headers + PREBUILT libunitree_sdk2.a + - # CycloneDDS .so) for the h12_deploy_mjpc controller shim, which compiles - # the fork's deploy core (deploy_common.cc, raw unitree_sdk2 DDS) inside - # colcon. Hydrated by launch_ros.sh on first launch (clone + cmake - # --install, seconds — nothing compiles, the repo ships prebuilt libs); - # persisted here so `run --rm` containers keep it. - - ../container_cache/unitree_install:/opt/unitree_install:rw + - type: bind + source: ${GOLEM_CACHE_DIR:-../container_cache}/mjpc_build + target: /home/code/mujoco_mpc/build + bind: + create_host_path: false # Submodule git metadata (ro): mujoco_mpc/.git is a FILE with # `gitdir: ../.git/modules/mujoco_mpc`, so mounting that dir at the # matching container path makes git work on the mounted source. @@ -160,5 +242,10 @@ services: # un-backdate exactly the files that changed since the image's baked # build seed (otherwise a submodule ahead of MJPC_REF hydrates into a # silently STALE build tree). - - ../.git/modules/mujoco_mpc:/home/code/.git/modules/mujoco_mpc:ro + - type: bind + source: ${GOLEM_MJPC_GIT_DIR:-../.git/modules/mujoco_mpc} + target: /home/code/.git/modules/mujoco_mpc + read_only: true + bind: + create_host_path: false command: /home/code/h12_sim_scripts/launch_ros.sh diff --git a/docker/mac/RobocasaDockerfile.arm64 b/docker/mac/RobocasaDockerfile.arm64 index 8d6c53c..2396172 100644 --- a/docker/mac/RobocasaDockerfile.arm64 +++ b/docker/mac/RobocasaDockerfile.arm64 @@ -7,7 +7,7 @@ # # Build: docker build -t golem_sim_robocasa:latest -f docker/mac/RobocasaDockerfile.arm64 . # ============================== -FROM golem_base:latest +FROM golem_base:arm64 ARG http_proxy ARG https_proxy diff --git a/docker/mac/RosDockerfile.slim.arm64 b/docker/mac/RosDockerfile.slim.arm64 index afd6d14..b67deaf 100644 --- a/docker/mac/RosDockerfile.slim.arm64 +++ b/docker/mac/RosDockerfile.slim.arm64 @@ -10,7 +10,7 @@ # # Build: docker build -t golem_ros:latest -f docker/mac/RosDockerfile.slim.arm64 . # ============================== -FROM golem_base:latest +FROM golem_base:arm64 ARG http_proxy ARG https_proxy diff --git a/docker/mac/docker-compose.stack.yml b/docker/mac/docker-compose.stack.yml new file mode 100644 index 0000000..dd26696 --- /dev/null +++ b/docker/mac/docker-compose.stack.yml @@ -0,0 +1,5 @@ +services: + ros: + environment: + GOLEM_WAIT_FOR_SIM: "1" + GOLEM_SIM_TIMEOUT: ${GOLEM_SIM_TIMEOUT:-180} diff --git a/docker/mac/docker-compose.yml b/docker/mac/docker-compose.yml index 87ca171..8a0e168 100644 --- a/docker/mac/docker-compose.yml +++ b/docker/mac/docker-compose.yml @@ -4,8 +4,8 @@ # /dev/dri | /dev/input | X11 device mounts from docker-compose.yml are simply # absent. Runs headless under Colima. Isaac is intentionally omitted. # -# docker compose -f docker/mac/docker-compose.yml build robocasa -# docker compose -f docker/mac/docker-compose.yml run --rm robocasa +# docker/mac/scripts/docker_build_mac.sh +# docker/mac/scripts/docker_stack_mac.sh up robocasa # ============================== services: robocasa: @@ -13,7 +13,9 @@ services: build: context: ../.. dockerfile: docker/mac/RobocasaDockerfile.arm64 - image: golem_sim_robocasa:latest + image: golem_sim_robocasa:arm64 + platform: linux/arm64 + init: true shm_size: 8g # Share one network namespace so robocasa and ros see each other's DDS # discovery on the same ROS_DOMAIN_ID. @@ -39,6 +41,7 @@ services: # GOLEM_CAMERAS=0 drops the 3 RGBD camera renders (the heaviest per-step CPU # cost) to speed the sim up for locomotion/SLAM. Lidar + odom are unaffected. - GOLEM_CAMERAS=${GOLEM_CAMERAS:-1} + - GOLEM_CMD_TIMEOUT=${GOLEM_CMD_TIMEOUT:-0.5} # GOLEM_SIM_ODOM=1 publishes ground-truth base odometry (odom -> pelvis TF + # /odom) straight from the sim, feeding 2D SLAM without FAST-LIO. Default 0 # (off) so the shared sim bridge stays silent; the nav/SLAM demo sets it to 1 @@ -53,12 +56,46 @@ services: # alongside unitree_sdk2py's (arm64 in-process coexistence fix). - RMW_IMPLEMENTATION=rmw_fastrtps_cpp volumes: - - ../../h1_robocasa:/home/code/h1_robocasa - - ../../CL_Assets:/home/code/CL_Assets:ro - - ./scripts:/home/code/h12_sim_scripts - - ../../container_cache/msgs_ws:/home/code/msgs_ws:rw - - ../../core_ws/src/magpie_msgs:/home/code/msgs_ws/src/magpie_msgs:ro - - ../../core_ws/src/custom_ros_messages:/home/code/msgs_ws/src/custom_ros_messages:ro + - type: bind + source: ../../h1_robocasa + target: /home/code/h1_robocasa + bind: + create_host_path: false + - type: bind + source: ${GOLEM_ASSETS_DIR:-../../CL_Assets} + target: /home/code/CL_Assets + read_only: true + bind: + create_host_path: false + - type: bind + source: ./scripts + target: /home/code/h12_sim_scripts + read_only: true + bind: + create_host_path: false + - type: bind + source: ../scripts + target: /home/code/golem_common + read_only: true + bind: + create_host_path: false + - type: bind + source: ${GOLEM_CACHE_DIR:-../../container_cache}/msgs_ws + target: /home/code/msgs_ws + bind: + create_host_path: false + - type: bind + source: ../../core_ws/src/magpie_msgs + target: /home/code/msgs_ws/src/magpie_msgs + read_only: true + bind: + create_host_path: false + - type: bind + source: ../../core_ws/src/custom_ros_messages + target: /home/code/msgs_ws/src/custom_ros_messages + read_only: true + bind: + create_host_path: false # colcon build/install on the VM's native ext4, NOT the virtiofs mount # (symlink-install onto virtiofs stalls for minutes). Persistent = cached. - msgs_build:/opt/msgs_ws/build @@ -70,7 +107,9 @@ services: build: context: ../.. dockerfile: docker/mac/RosDockerfile.slim.arm64 - image: golem_ros:latest + image: golem_ros:arm64 + platform: linux/arm64 + init: true shm_size: 4g network_mode: host # Shared IPC namespace: required for FastDDS shared-memory transport to the @@ -109,18 +148,48 @@ services: - GOLEM_ROS_MCP=${GOLEM_ROS_MCP:-0} - GOLEM_ROS_MCP_PORT=${GOLEM_ROS_MCP_PORT:-6082} volumes: - - ../../core_ws:/home/code/core_ws - - ../../CL_Assets:/home/code/CL_Assets:ro + - type: bind + source: ../../core_ws + target: /home/code/core_ws + bind: + create_host_path: false + - type: bind + source: ${GOLEM_ASSETS_DIR:-../../CL_Assets} + target: /home/code/CL_Assets + read_only: true + bind: + create_host_path: false # h12_ros2_model/setup.py locates CL_Assets as ../../../CL_Assets relative # to __file__, and colcon runs setup.py from the build-base — so with # build-base at /opt/core_ws/build that resolves to /opt/CL_Assets. Mount # the submodule there too so the build finds meshes/URDFs. (Installed # artifacts are copied into core_ws_install, so runtime is unaffected.) - - ../../CL_Assets:/opt/CL_Assets:ro - - ./scripts:/home/code/h12_sim_scripts + - type: bind + source: ${GOLEM_ASSETS_DIR:-../../CL_Assets} + target: /opt/CL_Assets + read_only: true + bind: + create_host_path: false + - type: bind + source: ./scripts + target: /home/code/h12_sim_scripts + read_only: true + bind: + create_host_path: false + - type: bind + source: ../scripts + target: /home/code/golem_common + read_only: true + bind: + create_host_path: false # The ROS debug MCP server (GOLEM_ROS_MCP=1) lives in the top-level tools/ # dir; mount it read-only so launch_ros_mac.sh can start it. - - ../../tools:/home/code/tools:ro + - type: bind + source: ../../tools + target: /home/code/tools + read_only: true + bind: + create_host_path: false # colcon build/install on the VM's native ext4, NOT the virtiofs mount # (symlink-install onto virtiofs stalls for minutes). Persistent = cached. - core_ws_build:/opt/core_ws/build diff --git a/docker/mac/scripts/docker_build_mac.sh b/docker/mac/scripts/docker_build_mac.sh index ce715dc..0daef1c 100755 --- a/docker/mac/scripts/docker_build_mac.sh +++ b/docker/mac/scripts/docker_build_mac.sh @@ -1,49 +1,5 @@ #!/usr/bin/env bash -# Build the Apple-Silicon (arm64 / CPU-only) Docker images. -# The Mac counterpart of docker/scripts/docker_build.sh. -# -# Usage: docker_build_mac.sh [service ...] -# Services: robocasa, ros -# With no args, both are built. -# Examples: -# docker_build_mac.sh # base + robocasa + ros -# docker_build_mac.sh robocasa # base + robocasa -# docker_build_mac.sh ros # base + ros -# -# Both arm64 images are `FROM golem_base:latest`, so the base is always built -# first -- from docker/mac/BaseDockerfile.arm64 (ubuntu:22.04, CPU-only), NOT the -# x86 docker/BaseDockerfile (nvidia/cuda, won't build without the NVIDIA runtime). -# -# There is no `isaac` here: Isaac Sim needs an NVIDIA GPU, so the arm64 port omits -# it. And unlike the x86 ros build there is no MJPC_REF -- the slim arm64 ros image -# ships no MuJoCo-MPC, so the mujoco_mpc submodule pin is irrelevant to this build. -set -euo pipefail -cd "$(dirname "$0")/../../.." - -COMPOSE=docker/mac/docker-compose.yml -VALID_SERVICES=(robocasa ros) - -if [ "$#" -eq 0 ]; then - SERVICES=("${VALID_SERVICES[@]}") -else - SERVICES=("$@") - for s in "${SERVICES[@]}"; do - ok=0 - for v in "${VALID_SERVICES[@]}"; do - if [ "$s" = "$v" ]; then ok=1; break; fi - done - if [ "$ok" -ne 1 ]; then - echo "error: unknown service '$s' (valid: ${VALID_SERVICES[*]})" >&2 - echo " note: isaac is NVIDIA-only and has no arm64 build." >&2 - exit 2 - fi - done -fi - -# golem_base for arm64. Context is the repo root (matches the compose build -# contexts, which are ../.. relative to docker/mac/). -echo "Building base (docker/mac/BaseDockerfile.arm64)..." -docker build -t golem_base:latest -f docker/mac/BaseDockerfile.arm64 . - -echo "Building services: ${SERVICES[*]}" -docker compose -f "$COMPOSE" build "${SERVICES[@]}" +# Usage: docker_build_mac.sh [--pull] [--no-cache] [robocasa|ros ...] +set -eo pipefail +export GOLEM_PLATFORM=mac +exec "$(dirname "$0")/../../scripts/docker_build.sh" "$@" diff --git a/docker/mac/scripts/docker_run_mac.sh b/docker/mac/scripts/docker_run_mac.sh index 6183acc..37cd1cc 100755 --- a/docker/mac/scripts/docker_run_mac.sh +++ b/docker/mac/scripts/docker_run_mac.sh @@ -1,91 +1,5 @@ -#!/bin/bash -# Apple-Silicon (arm64 / CPU-only) run helper -- the Mac counterpart of -# docker/scripts/docker_run.sh. -# -# Usage: -# ./docker_run_mac.sh [robocasa|ros] -> start the sim / workspace (compose default command) -# ./docker_run_mac.sh [robocasa|ros] bash -> drop to a raw shell instead (no auto colcon build) -# ./docker_run_mac.sh [robocasa|ros] -> run any command inside the container -# -# Display/feature toggles are env vars read by docker/mac/docker-compose.yml -- -# export them ahead of the service name and compose picks them up: -# GOLEM_DISPLAY=vnc ./docker_run_mac.sh robocasa # MuJoCo viewer -> noVNC :6080 -# GOLEM_RVIZ=vnc ./docker_run_mac.sh ros # RViz -> noVNC :6081 -# then open docker/mac/scripts/mac_vnc_tunnel.sh from the Mac (Colima does not -# forward host-network container ports; a tunnel is required). -# -# Differences from the x86 helper: no isaac (NVIDIA-only), no MJPC cache, and no -# `xhost` (macOS has no host X server -- GUIs stream over VNC, not XQuartz). -# -# NOTE: this starts ONE service. For the paired sim you still need both robocasa -# and ros up -- run this in two terminals and start `ros` promptly, or use -# `docker compose -f docker/mac/docker-compose.yml up` to start both at once. -# RoboCasa left running alone for more than a few seconds releases the robot's -# motors ("Command timeout") and the H1 collapses (see README, macOS section). - -SIM=${1:?Usage: ./docker_run_mac.sh [robocasa|ros] [override command/args...]} -shift -cd "$(dirname "$0")/../../.." - -# HAMS -> GOLEM rename guard. The toggles above all default to off, so a stale -# HAMS_* export would be silently dropped by compose interpolation. Fail loudly. -# Note: this only fires via this script -- `docker compose ... up` bypasses it. -legacy=$(env | sed -n 's/^\(HAMS_[A-Z0-9_]*\)=.*/\1/p' | tr '\n' ' ') -if [ -n "$legacy" ]; then - echo "ERROR: legacy HAMS_* env vars set: $legacy" >&2 - echo " The project is now GOLEM; these were renamed to GOLEM_*." >&2 - echo " Update your shell exports / scripts and re-run." >&2 - exit 1 -fi - -COMPOSE=docker/mac/docker-compose.yml - -case "$SIM" in - robocasa|ros) ;; - *) - echo "error: unknown service '$SIM' (valid: robocasa, ros)" >&2 - echo " note: isaac is NVIDIA-only and has no arm64 build." >&2 - exit 2 - ;; -esac - -# The Mac compose needs no docker/.env (it only reads ROS_DOMAIN_ID, defaulted in -# the compose file). Source it anyway if present so any shell overrides reach -# `docker compose` as host-env vars; set -a exports them for interpolation. -if [ -f docker/.env ]; then - set -a - source docker/.env - set +a -fi - -# Pre-create the one host bind source so dockerd doesn't materialise it -# root-owned on first run: the robocasa container's msgs_ws colcon cache. -# Everything else (build/install trees) lives in named volumes on the Colima VM, -# and there is no MJPC cache here (the slim arm64 ros image ships no MuJoCo-MPC). -mkdir -p container_cache/msgs_ws - -# ROS_DOMAIN_ID handling. Domain 0 is the real robot's DDS command bus; the CPU -# sim must never publish onto it. Unset/empty defaults to 1 (the sim domain). -# Unlike the x86 helper there is no "confirm to run on 0" path: there is no -# real-robot workflow from a Mac, so 0 is always rejected. -if [ "${ROS_DOMAIN_ID:-}" = "0" ]; then - echo "ERROR: ROS_DOMAIN_ID=0 is the real robot's DDS domain; the arm64 CPU sim may not run on it." >&2 - echo " Set ROS_DOMAIN_ID to a positive value (e.g. 1) in docker/.env, or unset it to default to 1." >&2 - exit 1 -fi -if [ -z "${ROS_DOMAIN_ID:-}" ]; then - export ROS_DOMAIN_ID=1 -fi - -# If the first arg is a flag (e.g. --headless), forward it to the default mac -# launcher instead of letting docker treat it as the command name. -if [ $# -gt 0 ] && [ "${1#-}" != "$1" ]; then - set -- "/home/code/h12_sim_scripts/launch_${SIM}_mac.sh" "$@" -fi - -# Stable container name per service (matches container_name in the compose file) -# so `docker exec golem_ros ...` works without a generated name. --rm cleans up on -# exit; force-remove a stale one left by a previously killed run. -if [ "$SIM" = "ros" ]; then NAME="golem_ros"; else NAME="golem_sim_${SIM}"; fi -docker rm -f "$NAME" >/dev/null 2>&1 || true -docker compose -f "$COMPOSE" run --rm --remove-orphans --name "$NAME" "$SIM" "$@" +#!/usr/bin/env bash +# Usage: docker_run_mac.sh robocasa|ros [--restart] [command or flags...] +set -eo pipefail +export GOLEM_PLATFORM=mac +exec "$(dirname "$0")/../../scripts/docker_run.sh" "$@" diff --git a/docker/mac/scripts/docker_stack_mac.sh b/docker/mac/scripts/docker_stack_mac.sh new file mode 100755 index 0000000..5ec483e --- /dev/null +++ b/docker/mac/scripts/docker_stack_mac.sh @@ -0,0 +1,5 @@ +#!/usr/bin/env bash +# Usage: docker_stack_mac.sh up|restart|stop|logs robocasa [--headless|--gui] +set -eo pipefail +export GOLEM_PLATFORM=mac +exec "$(dirname "$0")/../../scripts/docker_stack.sh" "$@" diff --git a/docker/mac/scripts/launch_robocasa_mac.sh b/docker/mac/scripts/launch_robocasa_mac.sh index 31fe7e2..213d5ec 100755 --- a/docker/mac/scripts/launch_robocasa_mac.sh +++ b/docker/mac/scripts/launch_robocasa_mac.sh @@ -3,7 +3,7 @@ # Same as launch_robocasa.sh but CPU/software rendering, no NVIDIA EGL. # # Two display modes, selected by the GOLEM_DISPLAY env var (default: headless): -# GOLEM_DISPLAY=headless OSMesa offscreen, no viewer window (original behaviour) +# GOLEM_DISPLAY=headless OSMesa offscreen, no viewer window (the default) # GOLEM_DISPLAY=vnc render the interactive MuJoCo viewer with software GL # (llvmpipe) into an in-container Xvfb and stream it out # over VNC/noVNC. MuJoCo's GL window cannot be forwarded @@ -14,6 +14,8 @@ # # Extra args pass through to h12_mujoco.py. set -e +source "/home/code/golem_common/runtime_common.sh" +validate_sim_domain GOLEM_DISPLAY="${GOLEM_DISPLAY:-headless}" @@ -91,6 +93,7 @@ echo "[launch_robocasa_mac] building $MSGS_WS -> $MSGS_INSTALL" --build-base "$MSGS_BUILD" --install-base "$MSGS_INSTALL" \ --packages-select magpie_msgs custom_ros_messages) source "$MSGS_INSTALL/setup.bash" +[ "${GOLEM_PREPARE_ONLY:-0}" != 1 ] || exit 0 export ROS_DOMAIN_ID="${ROS_DOMAIN_ID:-1}" @@ -113,4 +116,4 @@ fi cd /home/code/h1_robocasa echo "[launch_robocasa_mac] GOLEM_DISPLAY=$GOLEM_DISPLAY MUJOCO_GL=$MUJOCO_GL ROS_DOMAIN_ID=$ROS_DOMAIN_ID args: $*" -python h12_mujoco.py "$@" +python -u h12_mujoco.py "$@" diff --git a/docker/mac/scripts/launch_ros_mac.sh b/docker/mac/scripts/launch_ros_mac.sh index 19b3307..e4739e2 100755 --- a/docker/mac/scripts/launch_ros_mac.sh +++ b/docker/mac/scripts/launch_ros_mac.sh @@ -13,6 +13,8 @@ # Silicon, so we ship pixels. View from the Mac: docker/mac/scripts/mac_vnc_tunnel.sh # then open http://localhost:6081/vnc.html. set -e +source /home/code/golem_common/runtime_common.sh +validate_sim_domain GOLEM_RVIZ="${GOLEM_RVIZ:-0}" # GOLEM_ROS_MCP=1 starts the ROS debugging MCP server (tools/ros_mcp_server.py) @@ -117,11 +119,11 @@ cd "$WS" # unitree_hg + h12_lowerbody_rl provide the optional lower-body stack # (walk/FAME policies). They build cheaply and are only *launched* when # GOLEM_LOWERBODY is set (see the bringup), but building them always keeps -# `ros2 run h12_lowerbody_rl fame_node` available in a shell. unitree_hg -# needs ros-humble-rosidl-generator-dds-idl (added to the image); torch is -# already present. NOTE: FAME balances the robot standing unsupported; the walk -# policy currently does not stay up in the RoboCasa sim (see README). -PKGS="custom_ros_messages magpie_msgs h12_ros2_model h12_ros2_controller h12_safety_layer h1_bringup unitree_hg h12_lowerbody_controller" +# `ros2 run h12_lowerbody_rl fame_node` available in a shell. unitree_hg needs +# ros-humble-rosidl-generator-dds-idl and torch, both in the image. The walk +# policy is stable only when handed over from a settled FAME stance, which is +# what GOLEM_LOWERBODY=switch does (see docs/MACOS.md). +PKGS="custom_ros_messages magpie_msgs h12_ros2_model h12_ros2_controller h12_safety_layer h1_bringup unitree_hg h12_lowerbody_rl" H1_BRINGUP_STUB_DEPS="estop livox_ros_driver2 fast_lio model_server" for _dep in $H1_BRINGUP_STUB_DEPS; do @@ -151,9 +153,11 @@ if [ "$GOLEM_ROS_MCP" = "1" ] || [ "$GOLEM_ROS_MCP" = "vnc" ]; then start_ros_mcp || echo "[launch_ros_mac] MCP server failed to start (continuing without it)" fi -if [ "${1:-}" = "bash" ]; then - echo "[launch_ros_mac] workspace built; dropping to shell (ROS_DOMAIN_ID=$ROS_DOMAIN_ID)" - exec bash +if [ "${GOLEM_WAIT_FOR_SIM:-0}" = 1 ]; then + python3 /home/code/golem_common/wait_for_sim.py +fi +if [ "$#" -gt 0 ]; then + exec "$@" fi echo "[launch_ros_mac] launching minimal bringup (ROS_DOMAIN_ID=$ROS_DOMAIN_ID)" diff --git a/docker/scripts/dds_bridge.py b/docker/scripts/dds_bridge.py deleted file mode 100644 index 2f2688d..0000000 --- a/docker/scripts/dds_bridge.py +++ /dev/null @@ -1,57 +0,0 @@ -from cyclonedds.topic import Topic -from cyclonedds.sub import DataReader, Qos, Listener, Subscriber -from cyclonedds.pub import Publisher, DataWriter -from cyclonedds.domain import Domain, DomainParticipant -from unitree_sdk2py.idl.unitree_go.msg.dds_._MotorStates_ import * -from unitree_sdk2py.idl.unitree_hg.msg.dds_._LowState_ import * -from unitree_sdk2py.idl.unitree_go.msg.dds_._MotorCmds_ import * -from unitree_sdk2py.idl.unitree_hg.msg.dds_._LowCmd_ import * -from unitree_sdk2py.idl.std_msgs.msg.dds_._String_ import String_ -import os -# DOMAIN_ID = int(os.getenv("ROS_DOMAIN_ID")) -# assert DOMAIN_ID > 0, "Please set ROS_DOMAIN_ID environment variable to a positive integer for DDS channel factory initialization, domain id 0 reserved for real robot" -DOMAIN_ID = 0 - - -dp_isaac = DomainParticipant(DOMAIN_ID+1) -dp_cyclone = DomainParticipant(DOMAIN_ID) -qos = Qos() -listener = Listener() -sub_isaac = Subscriber(dp_isaac, qos, listener) -sub_cyclone = Subscriber(dp_cyclone, qos, listener) -pub_isaac = Publisher(dp_isaac, qos, listener) -pub_cyclone = Publisher(dp_cyclone, qos, listener) -bridges = [ - ("lowstate", "rt/lowstate", LowState_, "isaac", "cyclone"), - ("inspire_state", "rt/inspire/state", MotorStates_, "isaac", "cyclone"), - ("sim_state", "rt/sim_state", String_, "isaac", "cyclone"), - ("rewards_state", "rt/rewards_state", String_, "isaac", "cyclone"), - ("inspire_cmd", "rt/inspire/cmd", MotorCmds_, "cyclone", "isaac"), - ("low_cmd", "rt/lowcmd", LowCmd_, "cyclone", "isaac"), - ("reset_pose_cmd", "rt/reset_pose/cmd", String_, "cyclone", "isaac")] -participants = {"isaac": (dp_isaac, sub_isaac, pub_isaac), "cyclone": (dp_cyclone, sub_cyclone, pub_cyclone)} -readers = {} -writers = {} -for name, topic_str, msg_type, read_domain, write_domain in bridges: - read_dp, read_sub, _ = participants[read_domain] - write_dp, _, write_pub = participants[write_domain] - read_topic = Topic(read_dp, topic_str, msg_type, qos, listener) - write_topic = Topic(write_dp, topic_str, msg_type, qos, listener) - readers[name] = DataReader(read_sub, read_topic, qos, listener) - writers[name] = DataWriter(write_pub, write_topic, qos, listener) - - - print(f"\n\n\nBridge set up for topic '{topic_str}' with message type '{msg_type.__name__}' from '{read_domain}' to '{write_domain}' \n\n\n") - -import asyncio -from datetime import datetime -async def main(): - while True: - ts = datetime.now() - for name in readers: - try: - writers[name].write(readers[name].read()[0]) - except Exception as e: - pass - await asyncio.sleep(1) -asyncio.run(main()) diff --git a/docker/scripts/docker_build.sh b/docker/scripts/docker_build.sh index b158b49..68c7409 100755 --- a/docker/scripts/docker_build.sh +++ b/docker/scripts/docker_build.sh @@ -1,67 +1,52 @@ #!/usr/bin/env bash -# Build Docker images. -# -# Usage: docker_build.sh [profile ...] -# Profiles: isaac, robocasa, ros -# With no args, all three are built. -# Examples: -# docker_build.sh # build all -# docker_build.sh isaac # build isaac only -# docker_build.sh robocasa ros # build robocasa + ros -# -# RoboCasa and Ros inherit from golem_base; the base image is built first -# whenever either of those profiles is selected. Isaac is self-contained -# (Sim 5.1 / Lab v2.3.2 / Python 3.11) and does not use golem_base. -set -euo pipefail -cd "$(dirname "$0")/../.." - -# Lock the ros image's MuJoCo-MPC build to the mujoco_mpc submodule's committed -# SHA. RosDockerfile clones mujoco_mpc fresh over HTTPS at build time (no SSH -# keys / no git context inside the build), so it can't read the gitlink itself -# -- we read it here on the host and pass it in as the MJPC_REF build arg (see -# docker-compose.yml ros.build.args). Without this a `git submodule` bump would -# silently ship a stale MJPC. Empty (not a git checkout) -> Dockerfile default. -MJPC_REF="$(git rev-parse HEAD:mujoco_mpc 2>/dev/null || true)" -if [ -n "${MJPC_REF}" ]; then - export MJPC_REF - echo "MJPC_REF (from mujoco_mpc submodule pin): ${MJPC_REF}" -fi - -VALID_PROFILES=(isaac robocasa ros) - -if [ "$#" -eq 0 ]; then - PROFILES=("${VALID_PROFILES[@]}") -else - PROFILES=("$@") - for p in "${PROFILES[@]}"; do - ok=0 - for v in "${VALID_PROFILES[@]}"; do - if [ "$p" = "$v" ]; then ok=1; break; fi - done - if [ "$ok" -ne 1 ]; then - echo "error: unknown profile '$p' (valid: ${VALID_PROFILES[*]})" >&2 - exit 2 - fi - done +# Usage: docker_build.sh [--pull] [--no-cache] [isaac|robocasa|ros ...] +set -eo pipefail +source "$(dirname "$0")/docker_common.sh" +load_config +SERVICES=() +BUILD_FLAGS=() +CHILD_FLAGS=() +for arg in "$@"; do + case "$arg" in + --pull) BUILD_FLAGS+=("$arg");; + --no-cache) BUILD_FLAGS+=("$arg"); CHILD_FLAGS+=("$arg");; + --help|-h) echo 'Usage: docker_build.sh [--pull] [--no-cache] [isaac|robocasa|ros ...]'; exit 0;; + *) validate_service "$arg"; SERVICES+=("$arg");; + esac +done +if [ ${#SERVICES[@]} -eq 0 ]; then + SERVICES=(robocasa ros) + [ "${GOLEM_PLATFORM:-linux}" = mac ] || SERVICES+=(isaac) fi - -needs_base=0 -for p in "${PROFILES[@]}"; do - if [ "$p" = "robocasa" ] || [ "$p" = "ros" ]; then - needs_base=1 - break +NEEDS_BASE=0 +for service in "${SERVICES[@]}"; do + case "$service" in + ros|robocasa) NEEDS_BASE=1; require_file unitree_sdk2_python/setup.py;; + esac + if [ "$service" = robocasa ]; then require_file core_ws/src/livox_ros_driver2/package_ROS2.xml; fi + if [ "$service" = ros ] && [ "${GOLEM_PLATFORM:-linux}" != mac ]; then + export MJPC_REF + MJPC_REF=$(git rev-parse HEAD:mujoco_mpc) || fail 'Cannot resolve the committed mujoco_mpc pin.' + echo "MJPC_REF=$MJPC_REF" fi done - -if [ "$needs_base" -eq 1 ]; then - echo "Building base..." - docker build -t golem_base:latest -f docker/BaseDockerfile . +if [ "$NEEDS_BASE" = 1 ]; then + if [ "${GOLEM_PLATFORM:-linux}" = mac ]; then + docker build --platform linux/arm64 "${BUILD_FLAGS[@]}" -t golem_base:arm64 -f docker/mac/BaseDockerfile.arm64 . + else + docker build --platform linux/amd64 "${BUILD_FLAGS[@]}" -t golem_base:latest -f docker/BaseDockerfile . + fi fi - -PROFILE_ARGS=() -for p in "${PROFILES[@]}"; do - PROFILE_ARGS+=(--profile "$p") +# Child images inherit the local base we just built; --pull there would try +# downloading golem_base from Docker Hub. Isaac has a public upstream base. +CHILDREN=() +for service in "${SERVICES[@]}"; do + if [ "$service" = isaac ]; then + "${COMPOSE[@]}" build "${BUILD_FLAGS[@]}" isaac + else + CHILDREN+=("$service") + fi done - -echo "Building profiles: ${PROFILES[*]}" -docker compose -f docker/docker-compose.yml "${PROFILE_ARGS[@]}" build +if [ "${#CHILDREN[@]}" -gt 0 ]; then + "${COMPOSE[@]}" build "${CHILD_FLAGS[@]}" "${CHILDREN[@]}" +fi diff --git a/docker/scripts/docker_common.sh b/docker/scripts/docker_common.sh new file mode 100755 index 0000000..d1bae03 --- /dev/null +++ b/docker/scripts/docker_common.sh @@ -0,0 +1,113 @@ +#!/usr/bin/env bash +# Shared host-side configuration for Linux and Apple-Silicon wrappers. +GOLEM_ROOT=$(cd "$(dirname "${BASH_SOURCE[0]}")/../.." && pwd) + +fail() { echo "ERROR: $*" >&2; exit 1; } +require_file() { + [ -f "$1" ] || fail "Missing $1. Initialize submodules and fetch Git LFS assets (see docs/SETUP.md)." + local first_line + IFS= read -r first_line < "$1" || true + [ "$first_line" != 'version https://git-lfs.github.com/spec/v1' ] || fail "$1 is a Git LFS pointer; fetch its LFS contents before launching." +} +require_dir() { [ -d "$1" ] || fail "Missing mount directory: $1"; } + +load_config() { + cd "$GOLEM_ROOT" + # Match Compose precedence: explicitly exported shell values win over .env. + local saved_env + saved_env=$(export -p | sed 's/^declare -x /export /') + if [ -f docker/.env ]; then + set -a + source docker/.env + set +a + eval "$saved_env" + fi + [ -z "$(env | sed -n '/^HAMS_[A-Z0-9_]*=/p')" ] || fail 'Rename HAMS_* variables to GOLEM_*.' + export GOLEM_ASSETS_DIR="${GOLEM_ASSETS_DIR:-$GOLEM_ROOT/CL_Assets}" + export GOLEM_CACHE_DIR="${GOLEM_CACHE_DIR:-$GOLEM_ROOT/container_cache}" + # Relative user paths are always relative to the checkout, on both platforms. + case "$GOLEM_ASSETS_DIR" in /*) ;; *) GOLEM_ASSETS_DIR="$GOLEM_ROOT/$GOLEM_ASSETS_DIR";; esac + case "$GOLEM_CACHE_DIR" in /*) ;; *) GOLEM_CACHE_DIR="$GOLEM_ROOT/$GOLEM_CACHE_DIR";; esac + export ROS_DOMAIN_ID="${ROS_DOMAIN_ID:-1}" + case "$ROS_DOMAIN_ID" in ''|*[!0-9]*) fail 'ROS_DOMAIN_ID must be an integer from 0 to 232.';; esac + [ "${#ROS_DOMAIN_ID}" -le 3 ] || fail 'ROS_DOMAIN_ID must be from 0 to 232.' + ROS_DOMAIN_ID=$((10#$ROS_DOMAIN_ID)) + [ "$ROS_DOMAIN_ID" -le 232 ] || fail 'ROS_DOMAIN_ID must be from 0 to 232.' + ENV_FILE="$GOLEM_ROOT/docker/.env" + [ -f "$ENV_FILE" ] || ENV_FILE=/dev/null + if [ "${GOLEM_PLATFORM:-linux}" = mac ]; then + COMPOSE=(docker compose --env-file "$ENV_FILE" -f "$GOLEM_ROOT/docker/mac/docker-compose.yml") + LAUNCH_SUFFIX=_mac + else + COMPOSE=(docker compose --env-file "$ENV_FILE" -f "$GOLEM_ROOT/docker/docker-compose.yml") + LAUNCH_SUFFIX= + fi +} + +validate_service() { + case "$1" in + robocasa|ros) ;; + isaac) [ "${GOLEM_PLATFORM:-linux}" != mac ] || fail 'Isaac requires NVIDIA and is unavailable on Mac.';; + *) fail "Unknown service '$1'; use robocasa, ros, or isaac (Linux only).";; + esac +} + +check_domain() { + if [ "$ROS_DOMAIN_ID" = 0 ]; then + [ "$1" = ros ] && [ "${GOLEM_PLATFORM:-linux}" != mac ] || fail 'Domain 0 is reserved for the real robot.' + local reply + read -r -p 'Domain 0 is the REAL ROBOT bus. Proceed? [y/N] ' reply + case "$reply" in y|Y|yes|YES) ;; *) fail 'Real-robot launch cancelled.';; esac + fi +} + +configure_display() { + [ "${GOLEM_PLATFORM:-linux}" != mac ] || return 0 + local mode=${GOLEM_DISPLAY:-auto} + [ "$mode" != auto ] || { if [ -n "${DISPLAY:-}" ]; then mode=gui; else mode=headless; fi; } + case "$mode" in + headless) export DISPLAY= ;; + gui) + [ -n "${DISPLAY:-}" ] || fail 'GUI mode needs DISPLAY. Use GOLEM_DISPLAY=headless.' + export XAUTHORITY="${XAUTHORITY:-$HOME/.Xauthority}" + require_file "$XAUTHORITY" + require_dir /tmp/.X11-unix + COMPOSE+=(-f "$GOLEM_ROOT/docker/docker-compose.gui.yml") + ;; + *) fail 'Linux GOLEM_DISPLAY must be auto, gui, or headless.';; + esac +} + +prepare_mounts() { + local service=$1 + require_file "$GOLEM_ASSETS_DIR/ros_assets/h1_2_magpie_ros.urdf" + case "$service" in + isaac) + require_file "$GOLEM_ROOT/CL_isaaclab_sim/sim_main.py" + mkdir -p "$GOLEM_CACHE_DIR/isaac/ov" "$GOLEM_CACHE_DIR/isaac/nvidia" \ + "$GOLEM_CACHE_DIR/isaac/local_share_ov" "$GOLEM_CACHE_DIR/isaac/isaacsim" + ;; + robocasa) + require_file "$GOLEM_ROOT/h1_robocasa/h12_mujoco.py" + require_file "$GOLEM_ROOT/core_ws/src/magpie_msgs/package.xml" + require_file "$GOLEM_ROOT/core_ws/src/custom_ros_messages/package.xml" + mkdir -p "$GOLEM_CACHE_DIR/msgs_ws/src/magpie_msgs" "$GOLEM_CACHE_DIR/msgs_ws/src/custom_ros_messages" + ;; + ros) + require_file "$GOLEM_ROOT/core_ws/src/h1_bringup/package.xml" + if [ "${GOLEM_PLATFORM:-linux}" != mac ]; then + require_file "$GOLEM_ROOT/core_ws/cyclonedds.xml" + require_file "$GOLEM_ROOT/mujoco_mpc/CMakeLists.txt" + require_dir "$GOLEM_ASSETS_DIR/graspgenx/assets/magpie" + require_dir "$GOLEM_ASSETS_DIR/graspgenx/assets/meshes/magpie" + export GOLEM_MJPC_GIT_DIR="${GOLEM_MJPC_GIT_DIR:-$(git -C "$GOLEM_ROOT/mujoco_mpc" rev-parse --absolute-git-dir 2>/dev/null || echo "$GOLEM_ROOT/.git/modules/mujoco_mpc")}" + require_dir "$GOLEM_MJPC_GIT_DIR" + mkdir -p "$GOLEM_CACHE_DIR/mjpc_build" "$GOLEM_ROOT/mujoco_mpc/build" + fi + ;; + esac +} + +container_name() { + if [ "$1" = ros ]; then echo golem_ros; else echo "golem_sim_$1"; fi +} diff --git a/docker/scripts/docker_run.sh b/docker/scripts/docker_run.sh index d8e59a8..ffa40d6 100755 --- a/docker/scripts/docker_run.sh +++ b/docker/scripts/docker_run.sh @@ -1,82 +1,24 @@ -#!/bin/bash -# Usage: -# ./docker_run.sh [isaac|robocasa|ros] -> auto-starts the simulation/workspace -# ./docker_run.sh [isaac|robocasa|ros] bash -> drops to a shell instead -# ./docker_run.sh [isaac|robocasa|ros] -> runs any command inside the container -# -# To change the Isaac task or RoboCasa flags, either: -# - Drop to bash and invoke launch_isaac.sh / launch_robocasa.sh with your args, or -# - Invoke the launcher directly: ./docker_run.sh isaac /home/code/h12_sim_scripts/launch_isaac.sh TASKNAME - -SIM=${1:?Usage: ./docker_run.sh [isaac|robocasa|ros] [override command/args...]} +#!/usr/bin/env bash +# Usage: docker_run.sh SERVICE [--restart] [command | launcher flags...] +# GOLEM_DISPLAY=auto|gui|headless selects Linux desktop mounts. +set -eo pipefail +source "$(dirname "$0")/docker_common.sh" +SIM=${1:?Usage: docker_run.sh SERVICE [--restart] [command or flags...]} shift -cd "$(dirname "$0")/../.." - -# HAMS -> GOLEM rename guard. Every feature toggle defaults to off/on without -# erroring, so a stale HAMS_* export would be silently ignored rather than -# failing loudly. Reject it instead of second-guessing what the caller meant. -legacy=$(env | sed -n 's/^\(HAMS_[A-Z0-9_]*\)=.*/\1/p' | tr '\n' ' ') -if [ -n "$legacy" ]; then - echo "ERROR: legacy HAMS_* env vars set: $legacy" >&2 - echo " The project is now GOLEM; these were renamed to GOLEM_*." >&2 - echo " Update your shell exports / scripts and re-run." >&2 - exit 1 +load_config +validate_service "$SIM" +RESTART=0 +if [ "${1:-}" = --restart ]; then RESTART=1; shift; fi +check_domain "$SIM" +for arg in "$@"; do [ "$arg" != --headless ] || export GOLEM_DISPLAY=headless; done +configure_display +prepare_mounts "$SIM" +NAME=$(container_name "$SIM") +if docker inspect "$NAME" >/dev/null 2>&1; then + [ "$RESTART" = 1 ] || fail "$NAME already exists. Use --restart explicitly, or attach with docker exec -it $NAME bash." + docker rm -f "$NAME" fi - -# Load local secrets / overrides (GEMINI_API_KEY, ROS_DOMAIN_ID, ...). compose -# also auto-loads docker/.env, but sourcing here makes the values available to -# this script too (e.g. the ROS_DOMAIN_ID normalization below). set -a exports -# them so the values reach `docker compose` as host-env vars. Note: this -# overwrites any same-named vars already exported in your shell. -if [ -f docker/.env ]; then - set -a - source docker/.env - set +a -fi - -# Pre-create host-side bind sources so dockerd doesn't materialise them -# root-owned on first run. The caches hold msgs_ws build/install/log and the MJPC -# CMake build tree between container restarts (see docker-compose.yml). mjpc's -# SOURCE is a git submodule (never mkdir it); we only pre-create the nested build/ -# mountpoint inside it (gitignored by the submodule's own /build/ rule). -mkdir -p container_cache/msgs_ws container_cache/mjpc_build -[ -d mujoco_mpc ] && mkdir -p mujoco_mpc/build - -# ROS_DOMAIN_ID handling. Domain 0 is the real robot's DDS command bus. -# - Sims (isaac/robocasa) must never run on it: reject an explicit 0 loudly. -# - The ros profile may use it (real robot), but only after confirming the -# interactive prompt below. -# - An unset/empty value defaults to 1, the simulation domain. -if [ "${ROS_DOMAIN_ID:-}" = "0" ]; then - if [ "$SIM" != "ros" ]; then - echo "ERROR: ROS_DOMAIN_ID=0 is the real robot's DDS domain; the '$SIM' sim may not run on it." >&2 - echo " Set ROS_DOMAIN_ID to a positive value (e.g. 1) in docker/.env, or unset it to default to 1." >&2 - exit 1 - fi - echo "WARNING: ROS_DOMAIN_ID=0 -> DDS domain 0 is the REAL ROBOT command bus." >&2 - echo " Nodes will publish/subscribe on the live robot." >&2 - read -r -p "Proceed on DDS domain 0 (real robot)? [y/N] " reply - case "$reply" in - y|Y|yes|YES|Yes) ;; - *) echo "Aborted: refused to run on DDS domain 0." >&2; exit 1 ;; - esac -fi -if [ -z "${ROS_DOMAIN_ID:-}" ]; then - export ROS_DOMAIN_ID=1 -fi - -xhost +local:docker 2>/dev/null || true - -# If the first arg is a flag (e.g. --viewer), forward it to the default launcher -# instead of letting docker treat it as the command name. if [ $# -gt 0 ] && [ "${1#-}" != "$1" ]; then - set -- "/home/code/h12_sim_scripts/launch_${SIM}.sh" "$@" + set -- "/home/code/h12_sim_scripts/launch_${SIM}${LAUNCH_SUFFIX}.sh" "$@" fi - -# Stable container name per profile so `docker exec`/`docker logs` work without -# copy-pasting a generated UUID. The ros profile is the real-robot workspace and -# drops the "sim" infix; the sims (isaac, robocasa) keep it. --rm cleans it up on -# exit; if a previous run was killed without cleanup, force-remove the stale name. -if [ "$SIM" = "ros" ]; then NAME="golem_ros"; else NAME="golem_sim_${SIM}"; fi -docker rm -f "$NAME" >/dev/null 2>&1 || true -docker compose -f docker/docker-compose.yml --profile "$SIM" run --rm --remove-orphans --name "$NAME" "$SIM" "$@" +exec "${COMPOSE[@]}" run --rm --name "$NAME" "$SIM" "$@" diff --git a/docker/scripts/docker_stack.sh b/docker/scripts/docker_stack.sh new file mode 100755 index 0000000..d491a39 --- /dev/null +++ b/docker/scripts/docker_stack.sh @@ -0,0 +1,64 @@ +#!/usr/bin/env bash +# Usage: docker_stack.sh up|restart|stop|logs robocasa|isaac [--headless|--gui] +# Pre-build the ROS workspace before starting physics; wait for DDS before bringup. +set -eo pipefail +source "$(dirname "$0")/docker_common.sh" +ACTION=${1:?Usage: docker_stack.sh up|restart|stop|logs robocasa|isaac [--headless|--gui]} +SIM=${2:?Select robocasa or isaac} +shift 2 +load_config +validate_service "$SIM" +[ "$SIM" != ros ] || fail 'Choose a simulator: robocasa or isaac.' +case "$ACTION" in up|restart|stop|logs) ;; *) fail "Unknown action: $ACTION";; esac +for arg in "$@"; do + case "$arg" in + --headless) export GOLEM_DISPLAY=headless;; + --gui) if [ "${GOLEM_PLATFORM:-linux}" = mac ]; then export GOLEM_DISPLAY=vnc; else export GOLEM_DISPLAY=gui; fi;; + *) fail "Unknown option: $arg";; + esac +done +# Stopping or reading logs must work even if a display or a source disk is gone. +if [ "$ACTION" = stop ]; then + for service in ros "$SIM"; do + name=$(container_name "$service") + if docker inspect "$name" >/dev/null 2>&1; then docker stop "$name"; fi + done + exit 0 +fi +if [ "$ACTION" = logs ]; then exec "${COMPOSE[@]}" logs -f "$SIM" ros; fi +check_domain "$SIM" +configure_display +prepare_mounts "$SIM" +prepare_mounts ros +if [ "$ACTION" = restart ]; then + # Explicit restart also replaces one-off containers created by docker_run.sh. + for service in ros "$SIM"; do + name=$(container_name "$service") + if docker inspect "$name" >/dev/null 2>&1; then docker rm -f "$name"; fi + done +else + for service in "$SIM" ros; do + name=$(container_name "$service") + if [ "$(docker inspect -f '{{.State.Running}}' "$name" 2>/dev/null || true)" = true ]; then + fail "$name is already running. Use the stack restart command to restart both coherently." + fi + done +fi +# Preparation suppresses optional GUI/MCP processes and does not start controllers. +"${COMPOSE[@]}" run --rm -T -e GOLEM_WAIT_FOR_SIM=0 -e GOLEM_RVIZ=0 -e GOLEM_ROS_MCP=0 \ + ros "/home/code/h12_sim_scripts/launch_ros${LAUNCH_SUFFIX}.sh" /bin/true +if [ "$SIM" = robocasa ]; then + "${COMPOSE[@]}" run --rm -T -e GOLEM_PREPARE_ONLY=1 robocasa +fi +if [ "${GOLEM_PLATFORM:-linux}" = mac ]; then + COMPOSE+=(-f "$GOLEM_ROOT/docker/mac/docker-compose.stack.yml") +else + if [ -n "${DISPLAY:-}" ]; then + export GOLEM_USE_RVIZ=${GOLEM_USE_RVIZ:-true} + else + export GOLEM_USE_RVIZ=${GOLEM_USE_RVIZ:-false} + fi + COMPOSE+=(-f "$GOLEM_ROOT/docker/docker-compose.stack.yml") +fi +"${COMPOSE[@]}" up -d --no-build --force-recreate "$SIM" ros +echo "Started $SIM + ros. Use the stack logs command to follow readiness and bringup." diff --git a/docker/scripts/launch_isaac.sh b/docker/scripts/launch_isaac.sh index 5bb82da..5f2bc33 100755 --- a/docker/scripts/launch_isaac.sh +++ b/docker/scripts/launch_isaac.sh @@ -1,19 +1,24 @@ #!/bin/bash -# Usage: ./launch_isaac.sh [--reset-cache] -# Runs the hardcoded task Isaac-PickPlace-Cylinder-H12-27dof-Inspire-Joint. A -# [task_name] arg, --headless, and HEADLESS=1 are parsed below but NOT forwarded -# to the exec (dead code — task and viewport are hardcoded on the exec line); to -# change the task or run headless, invoke sim_main.py directly. +# Usage: ./launch_isaac.sh [task_name] [--headless] [--reset-cache] +# Defaults to task "base" — the only env CL_isaaclab_sim registers at the current +# submodule pin (tasks/h1-2_tasks/stack_rgyblock_h12_27dof_inspire/__init__.py). +# HEADLESS=1 also forces --headless. # # ROS publishing uses Isaac-Sim's bundled isaacsim.ros2.bridge extension -# (loaded by Kit at app startup); there is no /opt/ros/humble in this image -# and rclpy is not installed. +# (loaded by Kit at app startup); use its interpreter for simulation imports. set -e +source "$(dirname "$0")/runtime_common.sh" +validate_sim_domain +export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp +# Python DDS and ROS RMW must load the same native Cyclone library. +export CYCLONEDDS_HOME=/workspace/golem/isaac/cyclonedds/install -export LD_LIBRARY_PATH=/opt/conda/envs/unitree_sim_env/lib/python3.11/site-packages/isaacsim/exts/isaacsim.ros2.bridge/humble/lib/:$LD_LIBRARY_PATH +ISAACLAB_PATH="${ISAACLAB_PATH:-/workspace/golem/isaac/IsaacLab}" +ISAAC_PY="${ISAACLAB_PATH}/_isaac_sim/python.sh" +export LD_LIBRARY_PATH=${CYCLONEDDS_HOME}/lib:/isaac-sim/exts/isaacsim.ros2.bridge/humble/lib:${LD_LIBRARY_PATH:-} -TASK="Isaac-PickPlace-Cylinder-H12-27dof-Inspire-Joint" +TASK="base" HEADLESS_FLAG="" [ "${HEADLESS:-0}" = "1" ] && HEADLESS_FLAG="--headless" # Auto-headless if no display is reachable (SSH without X11, cloud VM, CI). @@ -22,29 +27,42 @@ if [ -z "$HEADLESS_FLAG" ] && [ -z "${DISPLAY:-}" ]; then HEADLESS_FLAG="--headless" fi -for arg in "$@"; do - case "$arg" in - --reset-cache) [ -d "$HOME/.cache/ov/texturecache" ] && rm -rf "$HOME/.cache/ov/texturecache" ;; - --headless) HEADLESS_FLAG="--headless" ;; - --*) ;; # ignore other flags - *) TASK="$arg" ;; +EXTRA_ARGS=() +while [ "$#" -gt 0 ]; do + case "$1" in + --reset-cache) rm -rf "$HOME/.cache/ov/texturecache"; shift ;; + --headless) HEADLESS_FLAG="--headless"; shift ;; + --task) + [ "$#" -ge 2 ] || { echo '--task needs a task name' >&2; exit 2; } + TASK=$2; shift 2 ;; + --) shift; EXTRA_ARGS+=("$@"); break ;; + --*) EXTRA_ARGS+=("$@"); break ;; + *) TASK=$1; shift ;; esac done export PYTHONUNBUFFERED=1 -# Use the conda env's interpreter directly. Compose runs this script -# non-interactively, so ~/.bashrc (which `conda activate`s the env) is not -# sourced — bare `python` would resolve to the base conda Python without -# isaacsim/isaaclab installed. +# Use Isaac Sim's bundled Kit interpreter via IsaacLab's python.sh wrapper. +# Compose runs this script non-interactively, so ~/.bashrc (which defines the +# `python` alias) is not sourced, and python.sh is what sets CARB_APP_PATH / +# EXP_PATH / PYTHONPATH / LD_PRELOAD=libcarb.so — a bare `python3` cannot +# import isaacsim. -unset ROS_DOMAIN_ID # ensure no ROS_DOMAIN_ID is set, so that DDSManager defaults to channel 1 for simulation -/opt/conda/envs/unitree_sim_env/bin/python -u /home/code/h12_sim_scripts/dds_bridge.py < /dev/tty > /dev/tty 2>&1 & disown -exec /opt/conda/envs/unitree_sim_env/bin/python -u \ +# Shim for hardcoded developer paths inside CL_isaaclab_sim: src/python/envs/ +# common/{scene,robots}.py load USDs from /workspace/golem/isaac/mateo_ws/ +# CL_Assets (one dev's personal workspace layout), but compose mounts the assets +# at /home/code/CL_Assets. Symlinked here rather than patched in the submodule — +# the sim packages are owned elsewhere (CLAUDE.md §5/§8). Drop this once the +# asset root is made relative/env-driven upstream. +MATEO_WS=/workspace/golem/isaac/mateo_ws +mkdir -p "$MATEO_WS" +ln -sfn /home/code/CL_Assets "$MATEO_WS/CL_Assets" + +exec "$ISAAC_PY" -u \ /home/code/CL_isaaclab_sim/sim_main.py \ --device cuda \ - --task Isaac-PickPlace-Cylinder-H12-27dof-Inspire-Joint \ + --task "$TASK" \ + $HEADLESS_FLAG \ --enable_inspire_dds \ --enable_cameras \ - --robot_type h1_2 < /dev/tty > /dev/tty 2>&1 - - + --robot_type h1_2 "${EXTRA_ARGS[@]}" diff --git a/docker/scripts/launch_robocasa.sh b/docker/scripts/launch_robocasa.sh index 66246a0..646d102 100755 --- a/docker/scripts/launch_robocasa.sh +++ b/docker/scripts/launch_robocasa.sh @@ -2,13 +2,12 @@ # Usage: ./launch_robocasa.sh [--headless] # Windowed MuJoCo sim by default; pass --headless to disable the passive viewer # (auto-applied when no DISPLAY is reachable, e.g. SSH without X11 / CI). -# Sim publishes: -# - rt/lowstate on CycloneDDS domain 1 (unitree_sdk2py) -# - /realsense/{head,left_hand,right_hand}/color/image_raw[/compressed], -# .../aligned_depth_to_color/image_raw[/compressedDepth], .../color/camera_info, -# /livox/lidar (CustomMsg), /livox/pointcloud, /livox/imu, /clock, and -# /{left,right}/gripper/state on ROS 2 (ROS_DOMAIN_ID=1 unless overridden) +# The sim publishes rt/lowstate over CycloneDDS plus its camera / lidar / clock / +# gripper topics on ROS 2 (ROS_DOMAIN_ID=1 unless overridden). mujoco_ros_bridge.py +# is the authority on that list. set -e +source "$(dirname "$0")/runtime_common.sh" +validate_sim_domain source /opt/ros/humble/setup.bash # livox_ros_driver2 (CustomMsg/CustomPoint) is baked into the robocasa image at @@ -25,6 +24,7 @@ echo "[launch_robocasa] building $MSGS_WS" (cd "$MSGS_WS" && colcon build --symlink-install \ --packages-select magpie_msgs custom_ros_messages) source "$MSGS_WS/install/setup.bash" +[ "${GOLEM_PREPARE_ONLY:-0}" != 1 ] || exit 0 export ROS_DOMAIN_ID="${ROS_DOMAIN_ID:-1}" @@ -49,4 +49,4 @@ cd /home/code/h1_robocasa # -u: docker-compose captures stdout via a pipe, which block-buffers python's # print()s — the "[h12_mujoco] ROS bridges up" readiness line otherwise sits in # the buffer indefinitely on a quiet headless run. -python -u h12_mujoco.py "$@" +exec python -u h12_mujoco.py "$@" diff --git a/docker/scripts/launch_ros.sh b/docker/scripts/launch_ros.sh index 09ab66c..387cd31 100755 --- a/docker/scripts/launch_ros.sh +++ b/docker/scripts/launch_ros.sh @@ -1,9 +1,13 @@ #!/bin/bash -# Build (if needed) and source the core_ws workspace, then drop to bash. +# Build and source core_ws, then execute a command (default: bash). # core_ws is bind-mounted from the host; build/install/log persist there too. set -e source /opt/ros/humble/setup.bash +if [ ! -f /opt/unitree_install/lib/libunitree_sdk2.a ]; then + echo '[launch_ros] missing baked Unitree SDK; rebuild with docker/scripts/docker_build.sh ros' >&2 + exit 1 +fi # --- MuJoCo MPC (MJPC) build-cache hydrate (image seed -> persistent mount) --- # ../container_cache/mjpc_build persists the CMake build tree at the in-tree path @@ -54,31 +58,6 @@ if [ -f "$MJPC_SRC/CMakeLists.txt" ] && [ ! -e "$MJPC_BUILD/CMakeCache.txt" ] \ fi fi -# --- unitree_sdk2 C++ install hydrate (for the h12_deploy_mjpc controller shim) --- -# deploy_common.cc (compiled by colcon from the submodule) links unitree_sdk2's -# C++ ChannelFactory/Publisher/Subscriber. The repo ships PREBUILT -# libunitree_sdk2.a + CycloneDDS .so per arch, so this is a clone + header/lib -# copy — seconds, no compilation. /opt/unitree_install is a persistent host -# mount (container_cache/unitree_install), so this runs exactly once per host. -# SHA pinned to the host's ~/unitree_sdk2 build (63c6f53) so fork and shim link -# the SAME SDK. -UNITREE_PREFIX=/opt/unitree_install -UNITREE_SDK_REF=63c6f53103e2f28e23b807d7399e92338c9d07d3 -if [ ! -f "$UNITREE_PREFIX/lib/libunitree_sdk2.a" ]; then - echo "[launch_ros] hydrating unitree_sdk2 C++ install -> $UNITREE_PREFIX (@$UNITREE_SDK_REF)" - rm -rf /tmp/unitree_sdk2 - if git clone https://github.com/unitreerobotics/unitree_sdk2.git /tmp/unitree_sdk2 \ - && git -C /tmp/unitree_sdk2 checkout --quiet "$UNITREE_SDK_REF" \ - && cmake -S /tmp/unitree_sdk2 -B /tmp/unitree_sdk2/build \ - -DCMAKE_INSTALL_PREFIX="$UNITREE_PREFIX" >/dev/null \ - && cmake --install /tmp/unitree_sdk2/build >/dev/null; then - echo "[launch_ros] unitree_sdk2 installed" - else - echo "[launch_ros] WARNING: unitree_sdk2 hydrate FAILED — h12_deploy_mjpc's controller will be skipped by its CMake guard" - fi - rm -rf /tmp/unitree_sdk2 -fi - # --- MJPC incremental rebuild (ninja no-op scan when already warm) --- # Brings libmjpc/threadpool + the staged task assets + the dist-packages # agent_server current with the mounted submodule BEFORE colcon links @@ -97,7 +76,7 @@ fi # Rebuild only if install/ is missing or any package.xml is newer than its install marker. NEEDS_BUILD=0 -if [ ! -d install ]; then +if [ ! -f install/setup.bash ]; then NEEDS_BUILD=1 elif [ -n "$(find src -name package.xml -newer install/setup.bash 2>/dev/null | head -1)" ]; then NEEDS_BUILD=1 @@ -128,6 +107,10 @@ else echo "[launch_ros] install/ is up to date — skipping build (run 'colcon build --symlink-install' to force)" fi -source install/setup.bash 2>/dev/null || true +source install/setup.bash -exec bash +if [ "${GOLEM_WAIT_FOR_SIM:-0}" = 1 ]; then + python3 /home/code/h12_sim_scripts/wait_for_sim.py +fi +[ "$#" -gt 0 ] || set -- bash +exec "$@" diff --git a/docker/scripts/runtime_common.sh b/docker/scripts/runtime_common.sh new file mode 100644 index 0000000..b4c26c6 --- /dev/null +++ b/docker/scripts/runtime_common.sh @@ -0,0 +1,12 @@ +#!/usr/bin/env bash +# Simulator-side guard also applies when users invoke Compose directly. +validate_sim_domain() { + export ROS_DOMAIN_ID="${ROS_DOMAIN_ID:-1}" + case "$ROS_DOMAIN_ID" in ''|*[!0-9]*) echo 'Invalid simulation ROS_DOMAIN_ID' >&2; return 1;; esac + [ "${#ROS_DOMAIN_ID}" -le 3 ] || return 1 + ROS_DOMAIN_ID=$((10#$ROS_DOMAIN_ID)) + if [ "$ROS_DOMAIN_ID" -lt 1 ] || [ "$ROS_DOMAIN_ID" -gt 232 ]; then + echo 'Simulation ROS_DOMAIN_ID must be 1..232; 0 is reserved for the real robot.' >&2 + return 1 + fi +} diff --git a/docker/scripts/wait_for_sim.py b/docker/scripts/wait_for_sim.py new file mode 100644 index 0000000..c56b304 --- /dev/null +++ b/docker/scripts/wait_for_sim.py @@ -0,0 +1,30 @@ +#!/usr/bin/env python3 +"""Wait for a fresh Unitree lowstate sample before starting ROS bringup.""" +import os +import sys + + +def main(): + domain = int(os.environ.get('ROS_DOMAIN_ID') or '1') + timeout = float(os.environ.get('GOLEM_SIM_TIMEOUT', '180')) + if not 1 <= domain <= 232 or timeout <= 0: + raise ValueError('Expected simulation domain 1..232 and a positive GOLEM_SIM_TIMEOUT') + from unitree_sdk2py.core.channel import ChannelFactoryInitialize, ChannelSubscriber + from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowState_ + + ChannelFactoryInitialize(domain) + subscriber = ChannelSubscriber('rt/lowstate', LowState_) + subscriber.Init() + print(f'[bringup] waiting up to {timeout:g}s for rt/lowstate on domain {domain}', flush=True) + try: + if subscriber.Read(timeout) is None: + print('[bringup] no simulator sample received; inspect simulator logs', file=sys.stderr) + return 1 + finally: + subscriber.Close() + print('[bringup] simulator is publishing', flush=True) + return 0 + + +if __name__ == '__main__': + sys.exit(main()) diff --git a/docs/DDS_TUNING.md b/docs/DDS_TUNING.md new file mode 100644 index 0000000..60c5fa0 --- /dev/null +++ b/docs/DDS_TUNING.md @@ -0,0 +1,69 @@ +# Network / DDS tuning (x86) + +The stack moves bursty, high-bandwidth sensor data (camera images, Livox point +clouds) over CycloneDDS. On a gigabit link the wire has headroom, but the default +208 KB kernel UDP receive buffer overflows on bursts and the kernel silently +drops datagrams. Lost DDS fragments stall or drop whole point-cloud/image +samples, so topics go laggy even though the network is not full. The tell is +`RcvbufErrors` climbing in `/proc/net/snmp`. + +Three settings fix this and **all three are required**. + +## 1. Host kernel buffers + +The containers run `network_mode: host`, so these are the host's `net.core` +sysctls. CycloneDDS can only *request* a large socket buffer — the kernel clamps +it to `net.core.rmem_max` — so without this, step 2 has no effect. + +```bash +sudo sysctl -w net.core.rmem_max=2147483647 +sudo sysctl -w net.core.rmem_default=16777216 +sudo sysctl -w net.core.netdev_max_backlog=10000 +``` + +To persist, put the same three lines in `/etc/sysctl.d/10-cyclonedds.conf` and +run `sudo sysctl --system`. + +## 2. CycloneDDS config + +`core_ws/cyclonedds.xml` requests a 16 MB receive buffer. The `ros` profile wires +it in automatically: `docker-compose.yml` bind-mounts it to +`/home/code/cyclonedds.xml` and sets `CYCLONEDDS_URI` to point at it. Edit the +file and restart the container to change the tuning (for example to pin the DDS +network interface). + +## 3. Pin the NIC IRQ off the real-time cores + +`eno1` has a single RX queue, so *all* inbound DDS traffic is processed in one +`NET_RX` softirq on whichever core holds its IRQ. `irqbalance` parks that IRQ on +core 11 — inside the MJPC planner's `0-11` set — and the contention drops the +robot's 500 Hz `/lowstate` stream to ~400 Hz with multi-second stalls (TF fades +in RViz, the lower-body controller trips its state-stale safe-hold). The wire and +the robot are fine; the loss is single-core contention. + +Move the IRQ to a core outside the RT set (0–12) and stop `irqbalance` putting it +back: + +```bash +sudo systemctl stop irqbalance +echo 13 | sudo tee /proc/irq/$(awk -F: '/eno1/{gsub(/ /,"",$1);print $1}' /proc/interrupts)/smp_affinity_list +``` + +Persist with `sudo systemctl disable --now irqbalance` plus a boot-time unit that +re-applies the affinity. The bringup CPU map reserves core 13 for this — the +estimator takes core 12 and `OTHER_CPUS` starts at 14, so no ROS node lands on +the network core. + +Confirm the softirq moved (counts should grow on core 13, not 11): + +```bash +grep NET_RX /proc/softirqs +``` + +## Verify + +Under load, `RcvbufErrors` should stop climbing: + +```bash +docker exec golem_ros grep '^Udp:' /proc/net/snmp +``` diff --git a/docs/DOCKER_TESTING.md b/docs/DOCKER_TESTING.md new file mode 100644 index 0000000..a409614 --- /dev/null +++ b/docs/DOCKER_TESTING.md @@ -0,0 +1,132 @@ +# Docker smoke tests: Ubuntu NVIDIA and Apple-Silicon Mac + +The host wrappers are the supported entry points. They load `docker/.env`, +validate domains and source mounts, and create cache directories. Commands +below run from the GOLEM checkout. Do not run two simulators on the same DDS +domain at once. Keep domain 0 reserved for the real robot. + +## Before copying to another machine + +The Isaac DDS fix includes an edit in +`CL_isaaclab_sim/src/python/dds/common/dds_master.py`. A superproject commit alone +does not include that file. Commit/push the simulator change in its own repo, +then update GOLEM's submodule pin before testing a fresh clone. No dependency +submodule pins, including Livox, need to change for these Docker fixes. + +Initialize submodules and Git LFS assets as described in [SETUP.md](SETUP.md). +Copy `docker/.env.example` to `docker/.env` if it does not exist. Linux's full +model-server bringup also needs the documented weights and API key. Optional +mount roots (quote paths with spaces): + +```bash +GOLEM_ASSETS_DIR=/absolute/path/to/CL_Assets +GOLEM_CACHE_DIR=/absolute/path/to/container_cache +ROS_DOMAIN_ID=7 +``` + +Set those in `.env` or export them. Shell exports win. On Mac, the host asset +path must be shared into the Colima VM. Build/install caches remain in Docker +named volumes on the VM; `GOLEM_CACHE_DIR` does not relocate those volumes. + +## Ubuntu on Intel/AMD with NVIDIA + +Install Docker Compose v2+ and NVIDIA Container Toolkit first. + +```bash +# Pull public upstream bases and build the two local images. +docker/scripts/docker_build.sh --pull robocasa ros + +# Starts detached; builds ROS/messages before physics, then starts bringup. +docker/scripts/docker_stack.sh up robocasa --headless +docker/scripts/docker_stack.sh logs robocasa +``` + +Look for `[bringup] simulator is publishing`, then normal ROS node startup. +A readiness timeout fails ROS startup; simulator logs remain available. Check +that required controller/model nodes have not exited. In another terminal: + +```bash +docker exec golem_ros bash -lc 'source /home/code/core_ws/install/setup.bash; ros2 node list' +docker exec golem_ros bash -lc 'source /home/code/core_ws/install/setup.bash; ros2 topic hz /clock' +# Ctrl-C ends the topic check. +docker/scripts/docker_stack.sh stop robocasa +``` + +For GUI testing, set `DISPLAY` and `XAUTHORITY` to the existing desktop session, +then use `up robocasa --gui`. No X11 files are required for headless runs. + +Test Isaac separately: + +```bash +docker/scripts/docker_build.sh isaac +docker/scripts/docker_run.sh isaac bash # confirms command override; exit the shell +ROS_DOMAIN_ID=7 docker/scripts/docker_run.sh isaac --task base --headless +``` + +Isaac must log successful DDS initialization and run without `/dev/tty` errors. +From a second terminal, check a fresh DDS sample using the ROS image: + +```bash +ROS_DOMAIN_ID=7 docker/scripts/docker_run.sh ros python3 /home/code/h12_sim_scripts/wait_for_sim.py +``` + +Stop this single-service simulator before trying `docker_stack.sh up isaac +--headless`. Isaac's existing sensor coverage differs from RoboCasa; this +Docker change does not add missing clock/lidar/IMU functionality or establish +full navigation/skills parity. + +## MacBook Air (Apple Silicon): RoboCasa + ROS only + +Use native arm64 Colima with Docker Compose; see [MACOS.md](MACOS.md) for setup. +Start with cameras off and no GUI to reduce CPU/memory pressure: + +```bash +docker/mac/scripts/docker_build_mac.sh --pull +GOLEM_CAMERAS=0 docker/mac/scripts/docker_stack_mac.sh up robocasa + +docker/mac/scripts/docker_stack_mac.sh logs robocasa +docker exec golem_ros bash -lc 'source /opt/core_ws/install/setup.bash; ros2 node list' +docker exec golem_ros bash -lc 'source /opt/core_ws/install/setup.bash; ros2 topic hz /clock' +``` + +Expect the same readiness message, an advancing `/clock`, and the minimal Mac +controller nodes. The VM uses software rendering and FastDDS; there should be +no NVIDIA runtime or host X11 mount requirement. Isaac is intentionally rejected. + +For the optional browser viewer: + +```bash +GOLEM_CAMERAS=0 GOLEM_DISPLAY=vnc docker/mac/scripts/docker_stack_mac.sh restart robocasa +docker/mac/scripts/mac_vnc_tunnel.sh --open +``` + +Then test `GOLEM_RVIZ=vnc` and cameras enabled if resources permit. Finish with: + +```bash +docker/mac/scripts/docker_stack_mac.sh stop robocasa +``` + +## Mount and restart checks (both platforms) + +- Set custom asset/cache paths, including one with spaces. Launch and check + `docker inspect golem_sim_robocasa --format '{{json .Mounts}}'`. +- Temporarily select a nonexistent asset path: the wrapper must fail before + creating a container, with the missing path in the error. +- Set `ROS_DOMAIN_ID=0`: simulation startup must be refused, including direct + Compose launcher use. Restore a positive domain afterward. +- Invoke a single-service wrapper twice: the second invocation must refuse to + replace the first. Explicit `--restart` replaces that selected service. +- Use stack `restart` after a single-service run: it must replace the selected + simulator and ROS together. The next startup should reuse build caches. + +## Automated host checks + +```bash +python3 -m unittest discover -s tests/docker -v +``` + +These exercise wrapper behavior with a recording Docker substitute and render +real Compose configuration without starting containers. They cover mount roots, +missing sources, domain checks, build flags, environment precedence, restart +behavior, and the Mac configuration. They do not establish GPU/GUI runtime, +image build success, controller stability, or performance on a MacBook Air. diff --git a/docs/MACOS.md b/docs/MACOS.md new file mode 100644 index 0000000..8e9733a --- /dev/null +++ b/docs/MACOS.md @@ -0,0 +1,149 @@ +# macOS (Apple Silicon) — headless CPU port + +A self-contained, CPU-only port that runs **ROS 2 + MuJoCo/RoboCasa** on arm64. +Isaac is not available (it needs an NVIDIA GPU), MuJoCo renders in software +(OSMesa/llvmpipe), and the ROS layer runs on **FastDDS** instead of CycloneDDS +(an arm64 in-process coexistence requirement). It uses its own compose file, +`docker/mac/docker-compose.yml` — the x86 instructions in +[`RUNNING.md`](RUNNING.md) do not apply here. + +## Prerequisites + +- [Colima](https://github.com/abiosoft/colima) + the Docker CLI + (`brew install colima docker docker-compose`). No Docker Desktop, no NVIDIA + toolkit, no XQuartz. +- Git LFS and submodules, as in [`SETUP.md`](SETUP.md). +- `docker/.env` is optional. Both wrapper families load it, with exported shell + settings taking precedence. Asset/cache mount roots are configurable there. +- Allocate VM resources according to the Mac's memory. For a 16 GB MacBook Air, + a starting point is `colima start --cpu 4 --memory 6`. Start headless with + cameras disabled; software rendering is CPU-intensive. Larger builds and + simulations may need more RAM. On an 8 GB model, expect tighter limits. + +## Build and run + +```bash +# build both arm64 images (robocasa + ros); the base image builds automatically +docker/mac/scripts/docker_build_mac.sh # or: docker_build_mac.sh robocasa | ros + +# headless (the default); brings BOTH services up together +docker/mac/scripts/docker_stack_mac.sh up robocasa +``` + +The stack wrapper prepares both workspaces before starting physics, then starts +both services and waits for a fresh DDS state sample before ROS bringup. This +avoids leaving the simulator running during a long first controller build. +Follow logs with `docker/mac/scripts/docker_stack_mac.sh logs robocasa`. +Use `restart robocasa` to recreate both services together and `stop robocasa` +to stop them while preserving caches. + +`docker/mac/scripts/docker_run_mac.sh [robocasa|ros]` remains available for +single-service runs and raw command overrides. An existing stable container +name requires explicit `--restart`; no running container is removed implicitly. + +The first startup is slow (colcon builds the message + controller workspaces +into named volumes before either simulation or bringup starts); later runs are cached. +Images use `:arm64` tags, separate from Linux NVIDIA images. Both containers use `network_mode: host` ++ `ipc: host` so FastDDS's shared-memory transport works across them. + +## Feature toggles + +All are environment variables read by `docker/mac/docker-compose.yml`, so set +them on the `up` command line. They are baked in at container-create time — +changing one needs the stack `restart` command to recreate both containers. + +| Variable | Default | Effect | +|---|---|---| +| `GOLEM_DISPLAY` | off | `vnc` → MuJoCo viewer on noVNC port 6080 | +| `GOLEM_RVIZ` | off | `vnc` → RViz on noVNC port 6081 | +| `GOLEM_LOWERBODY` | off (tethered) | `fame` stand-only, `walk` raw policy, `switch` auto stand↔walk | +| `GOLEM_SLAM` | off | `pointcloud_to_laserscan` + `slam_toolbox` → `/map` | +| `GOLEM_NAV2` | off | nav2 stack (implies SLAM) | +| `GOLEM_SIM_ODOM` | off | sim ground-truth `/odom` + `odom→pelvis` TF (SLAM needs it) | +| `GOLEM_CAMERAS` | on | `0` drops the 3 RGBD renders — the heaviest per-step cost | +| `GOLEM_SPAWN_BACKOFF` | 0 | metres to back the robot into open floor at spawn | +| `GOLEM_CMD_TIMEOUT` | 0.5 | sim-seconds before the low-level interface zeroes the motors | +| `GOLEM_ROS_MCP` | off | `1` starts the [ROS debugging MCP server](ROS_MCP_DEBUG.md) | + +## Viewing the GUIs + +MuJoCo's and RViz's OpenGL windows cannot be forwarded to XQuartz +(Apple-Silicon XQuartz has broken indirect GLX). Each is rendered in-container +with software GL into a virtual display and streamed to the browser over noVNC: + +```bash +GOLEM_DISPLAY=vnc GOLEM_RVIZ=vnc docker/mac/scripts/docker_stack_mac.sh up robocasa + +# Colima does not forward container ports — open the SSH tunnel +./docker/mac/scripts/mac_vnc_tunnel.sh # --open opens a browser; --stop closes the tunnel +``` + +- **MuJoCo viewer** → +- **RViz** (RobotModel + TF) → + +RoboCasa renders on X display `:99` and RViz on `:100`; they must differ because +the containers share one network namespace. The launchers handle this. + +## Driving the robot + +Bringup starts automatically in the `ros` container (`joint_state_publisher`, +`robot_state_publisher`, `frame_task_server`, `safety_node`): + +```bash +docker exec -it golem_ros bash # fallback if the host docker CLI is flaky: colima ssh -- docker exec … +source /home/code/h12_sim_scripts/robot_cli.sh + +rob_poses # list postures +rob_pose t_pose # move to one +rob_grip right close # open/close a gripper +rob_home +``` + +## Lower-body control + +By default an elastic-band tether holds the robot upright and only the upper +body is controlled. `GOLEM_LOWERBODY` releases the tether and balances the robot: + +```bash +# stand free (and squat via /lowerbody/squat_cmd), but no locomotion +GOLEM_DISPLAY=vnc GOLEM_LOWERBODY=fame docker/mac/scripts/docker_stack_mac.sh up robocasa + +# stand AND walk — the switchable controller +GOLEM_DISPLAY=vnc GOLEM_LOWERBODY=switch docker/mac/scripts/docker_stack_mac.sh up robocasa +``` + +`switch` starts band-held idle and selects stand↔walk from the commanded +velocity: `rob_stand` settles it into FAME, and `rob_go ` engages +the TorchScript walk policy — any non-zero `/cmd_vel` does, so there is no manual +handover step. The walk policy is stable only when handed over from a settled +FAME stance, which is what `switch` provides; `GOLEM_LOWERBODY=walk` launches the +raw policy directly and only marches in place. + +## Demos + +- **[Navigation demo](NAVIGATION_DEMO.md)** — SLAM + nav2 + autonomous frontier + exploration of the RoboCasa kitchen. +- **[ROS debugging MCP server](ROS_MCP_DEBUG.md)** — `GOLEM_ROS_MCP=1` exposes + ROS inspection and driving tools to Claude Code over the same SSH tunnel. + +## Gotchas + +- **The host `docker` CLI socket is intermittent** under Colima — `docker …` can + fail with "Cannot connect to the Docker daemon" while the VM and containers are + healthy. Use `colima ssh -- docker …`, or `colima stop && colima start` to + relink the socket (this restarts the containers). +- **Cartesian reaching is disabled.** The `/frame_task` action crashes the + controller node here, so `rob_reach` is a no-op stub — drive in joint space + with `rob_pose`. +- **Restart the two containers coherently.** The RoboCasa container owns + `/clock`; restarting it alone resets sim time to 0 while the ROS side keeps its + old clock, giving TF extrapolation errors and a frozen `0×0` SLAM map. Restart + `golem_ros` after `robocasa`. +- **The motor watchdog runs on sim time.** The low-level interface zeroes the + motors if no `rt/lowcmd` arrives within `GOLEM_CMD_TIMEOUT` sim-seconds. It is + sim time on purpose: a wall-clock timeout is far too tight on a sim running + ~0.2× real-time. Raise it if a heavy scene trips it. +- **For nav, `GOLEM_SPAWN_BACKOFF` of `1.5`–`2.0`** keeps the robot clear of the + counters. + +For the MacBook Air smoke test and pass criteria, see [Docker testing](DOCKER_TESTING.md). diff --git a/docs/RUNNING.md b/docs/RUNNING.md new file mode 100644 index 0000000..a3a1b4e --- /dev/null +++ b/docs/RUNNING.md @@ -0,0 +1,160 @@ +# Running the stack (x86) + +Setup first: [`SETUP.md`](SETUP.md). For Apple Silicon see [`MACOS.md`](MACOS.md). + +Always start containers through the provided scripts — they wire up +`docker/.env`, the DDS domain check, bind-mount pre-creation, and stable +container names. + +## Paired startup + +```bash +docker/scripts/docker_stack.sh up robocasa --headless +docker/scripts/docker_stack.sh logs robocasa +docker/scripts/docker_stack.sh restart robocasa --headless +docker/scripts/docker_stack.sh stop robocasa +``` + +Use `isaac` instead of `robocasa` for Isaac. `--gui` selects desktop mounts; +RViz follows GUI mode unless `GOLEM_USE_RVIZ` overrides it. The stack wrapper +prepares workspaces, starts both containers, and gates bringup on a fresh DDS +sample. Caches persist across stop/restart. Do not restart only the simulator +while ROS is using its clock. See [Docker testing](DOCKER_TESTING.md). + +## The three-terminal flow + +```bash +# Terminal 1 — simulator. Start FIRST so /clock is publishing before ROS nodes latch on. +docker/scripts/docker_run.sh robocasa # or: isaac + +# Terminal 2 — ROS workspace. Colcon-builds core_ws if stale, then drops to a shell. +docker/scripts/docker_run.sh ros +ros2 launch h1_bringup h1_sim_bringup.launch.py # bringup is a manual step + +# Terminal 3 — drive the robot from inside the running ROS container +docker exec -it golem_ros bash +source /opt/ros/humble/setup.bash +source /home/code/core_ws/install/setup.bash +``` + +## `docker_run.sh` + +```bash +docker/scripts/docker_run.sh # default launcher +docker/scripts/docker_run.sh robocasa --headless # leading flags go to the launcher +docker/scripts/docker_run.sh robocasa bash # shell instead +docker/scripts/docker_run.sh ros # any command +``` + +Container names are `golem_sim_robocasa`, `golem_sim_isaac`, and `golem_ros`. +The default launchers are `docker/scripts/launch_{robocasa,isaac,ros}.sh`. + +**The single-service `ros` default is a shell.** Paired startup launches +bringup automatically. To run an initialized custom command, invoke +`docker_run.sh ros /home/code/h12_sim_scripts/launch_ros.sh `. +Existing containers require an explicit `--restart` immediately after the +service name; otherwise attach with `docker exec`. + +### RoboCasa + +Publishes `rt/lowstate` over CycloneDDS, and on the chosen ROS domain: `/clock`; +the RealSense topics `/realsense/{head,left_hand,right_hand}/color/image_raw` and +`.../aligned_depth_to_color/image_raw` (raw and compressed) with +`.../color/camera_info`; the Livox topics `/livox/lidar` (CustomMsg), +`/livox/pointcloud`, `/livox/imu`; and `/{left,right}/gripper/state`. It consumes +`rt/lowcmd` and the `/{left,right}/gripper/*` services. + +Scene flags are passed straight through, e.g. +`docker_run.sh robocasa --task OpenFridge --layout 1 --style 1 --seed 42`. + +### Isaac + +Uses the same wrappers, on NVIDIA Linux only. The launcher honors +`ROS_DOMAIN_ID` and communicates directly on that simulation domain; zero is +rejected. Select a task with `docker_run.sh isaac --task base --headless`. +Use `--` before additional simulator options. Logs go to container stdout. + +Isaac's sensor/topic surface still differs from RoboCasa (see `CLAUDE.md`); +starting both containers does not imply full navigation/skills parity. + +## Bringup + +`h1_bringup` is launch-only, and sim versus real are separate launch files (there +is no `sim:=true` argument): + +| Launch file | Scenario | +|---|---| +| `h1_sim_bringup.launch.py` | x86 sim — the full stack | +| `h1_sim_bringup_mac.launch.py` | mac sim — trimmed, toggled by `GOLEM_*` env vars | +| `h1_real_robot_bringup.launch.py` | real robot, onboard PC (aggregates the three below) | +| `h1_real_drivers.launch.py` | real: Livox, RealSense, grippers | +| `h1_real_controller.launch.py` | real: estop, state publishers, safety + IK servers | +| `h1_real_desktop_bringup.launch.py` | real: companion desktop — model servers, skills, MJPC | +| `h1_navigation.launch.py` | shared nav stack, included by the sim and real bringups | + +The x86 sim bringup starts the state publishers, the `frame_task_server` IK +solver, `safety_node`, the Gemini/SAM/GraspGen model servers, the MJPC estimator +and lower-body controller, the skills node, nav, and RViz. Common arguments: + +```bash +ros2 launch h1_bringup h1_sim_bringup.launch.py \ + use_rviz:=false use_nav:=false use_skills:=false lowerbody:=fame +``` + +## Worked example — open the fridge + +```bash +# Terminal 1 +./docker/scripts/docker_run.sh robocasa --task OpenFridge --layout 1 --style 1 --seed 42 + +# Terminal 2 +docker/scripts/docker_run.sh ros +ros2 launch h1_bringup h1_sim_bringup.launch.py + +# Terminal 3 (inside golem_ros, workspace sourced) +ros2 action send_goal /named_config custom_ros_messages/action/NamedConfig \ + "{config_name: 'home', duration: {sec: 0, nanosec: 0}}" + +ros2 action send_goal /skill/grasp custom_ros_messages/action/SkillGrasp \ + "{target_object: 'vertical fridge handle', arm: 'right', timeout: {sec: 60, nanosec: 0}}" \ + --feedback +``` + +`/skill/grasp`, `/skill/pick_place`, and `/skill/frontier_explore` are +implemented; the other nine `/skill/*` action servers accept a goal and then +abort with "not implemented". `core_ws/src/h1_bringup/common_cmds.md` has a +ready-made `send_goal` snippet and field reference for each one. + +## Real robot + +Coordinate before touching. The onboard PC runs natively, without Docker: + +```bash +export ROS_DOMAIN_ID=0 +source core_ws/install/setup.bash +ros2 launch h1_bringup h1_real_robot_bringup.launch.py +``` + +On the companion desktop, `h1_real_desktop_bringup.launch.py` gates leg commands +behind `start_position_verified:=true` (default false) — set it only after +physically verifying the robot's pose. + +## Troubleshooting + +- **Start RoboCasa before bringup.** If you restart the sim container, restart + `golem_ros` too: sim time resets to 0, giving TF extrapolation errors and a + frozen SLAM map. +- **X11 / GUI:** set `XAUTHORITY` to your session's existing authorization file. + Use `GOLEM_DISPLAY=headless` or `--headless` when no X server is available. +- **Talking to the sim from the host** bypasses the run scripts and so misses + `docker/.env` — run `set -a; source docker/.env; set +a` first, or topics stay + invisible. +- **Rebuilds are incremental** across container restarts (build/install are + host-mounted). Force one with `colcon build --symlink-install` inside the + container; for a clean message rebuild, wipe `container_cache/msgs_ws/` on the + host. +- **MJPC C++ iteration** uses `docker exec -it golem_ros + /home/code/h12_sim_scripts/rebuild_mjpc.sh` (`--install` also refreshes assets + and proto), not colcon. +- **Laggy point clouds or images** are usually kernel UDP buffer drops — see + [`DDS_TUNING.md`](DDS_TUNING.md). diff --git a/docs/SETUP.md b/docs/SETUP.md new file mode 100644 index 0000000..9f40a12 --- /dev/null +++ b/docs/SETUP.md @@ -0,0 +1,89 @@ +# Setup (x86 / Linux + NVIDIA) + +One-time setup for the preferred platform. For Apple Silicon see +[`MACOS.md`](MACOS.md). + +## Prerequisites + +- Docker with Compose v2, and the NVIDIA Container Toolkit. +- Git LFS — the URDF meshes, MuJoCo XML, and Isaac USD assets are LFS objects. + +## 1. Clone with submodules and assets + +Most of the tree is git submodules; builds fail cryptically without them. + +```bash +git lfs install +git submodule update --init --recursive +``` + +## 2. Configure `docker/.env` + +Runtime settings live in `docker/.env`, which is git-ignored because it holds +your API key. Copy the template once and edit it — nothing needs to be exported +in your shell: + +```bash +cp docker/.env.example docker/.env +# GEMINI_API_KEY=... # https://aistudio.google.com/apikey +# ROS_DOMAIN_ID=1 # any non-zero value; 0 is reserved for the real robot +``` + +Both `docker compose` and `docker/scripts/docker_run.sh` load the file, so every +container and terminal sees the same values. `ROS_DOMAIN_ID` reaches all +containers; `GEMINI_API_KEY` reaches only the `ros` container, where the vision +servers and `h12_skills` need it. The wrappers load the file and preserve exported shell overrides, matching +Compose precedence. Optional `GOLEM_ASSETS_DIR` and `GOLEM_CACHE_DIR` relocate +host mounts without editing YAML. + +**Domain 0 is the real robot's command bus.** Unset/empty defaults to `1`; +explicit zero is rejected for simulations. Linux ROS-only launches on zero +require confirmation. + +## 3. SAM3 weights + +The `sam_server` segmentation model is not tracked in git. Download it from the +gated [`facebook/sam3`](https://huggingface.co/facebook/sam3) repo: + +```bash +huggingface-cli login # one-time; accept the license +cd core_ws/src/model_server/weights +huggingface-cli download facebook/sam3 --local-dir .cache +mv .cache/sam3.pt sam3.pt # the name must be exactly sam3.pt +``` + +`core_ws/src/model_server/weights/README.md` covers the auto-download fallback. + +## 4. Build the images + +```bash +docker/scripts/docker_build.sh # all three profiles +docker/scripts/docker_build.sh robocasa ros # a subset +docker/scripts/docker_build.sh isaac # isaac only +``` + +`robocasa` and `ros` inherit from `golem_base`, which is built first +automatically whenever either is selected. `isaac` is self-contained. + +The build/run scripts resolve their own location, so they can be invoked from +any directory. + +For how the images are layered, what is pinned and why, see +[`../docker/BUILD.md`](../docker/BUILD.md). + +## Upgrading a checkout from before the HAMS → GOLEM rename + +The rename was a clean break, with no compatibility fallbacks: + +- **Env vars:** every `HAMS_*` toggle is now `GOLEM_*`. The run scripts abort if + they see a stale `HAMS_*` export, because these toggles default to off and + would otherwise be ignored silently. The guard cannot fire if you bypass the + scripts with `docker compose … up`, where the stale variable is simply dropped. +- **Containers/images:** `hams_base`/`hams_ros`/`hams_sim_*` are now + `golem_base`/`golem_ros`/`golem_sim_*`. Retag rather than rebuild: + `for i in base ros sim_robocasa; do docker tag hams_$i:latest golem_$i:latest; done` +- **Checkouts:** the robot PC's checkout is `/home/unitree/GOLEM`. Set + `GOLEM_ASSETS_DIR` to override it if yours is not renamed yet. +- **Remote:** `git remote set-url origin git@github.com:correlllab/GOLEM.git`. + +For paired startup and the two-machine smoke test, see [Docker testing](DOCKER_TESTING.md). diff --git a/h1_robocasa/h12_mujoco.py b/h1_robocasa/h12_mujoco.py index b391158..16f71e8 100644 --- a/h1_robocasa/h12_mujoco.py +++ b/h1_robocasa/h12_mujoco.py @@ -3,7 +3,6 @@ import os import random import threading -import time import mujoco import numpy as np @@ -18,6 +17,7 @@ from mujoco_env import ElasticBand from mujoco_ros_bridge import RosSensorBridge from sim_names import NameResolver +from sim_timing import RealtimePacer, LoopTimings from unitree_interface import SimInterface # Viewer free-camera spawn pose, anchored to the robot so the passive viewer @@ -87,7 +87,7 @@ def _draw_band(handle, anchor, body_pos): scn.ngeom += 1 -def sim_loop(task, viewer=True, layout=None, style=None, seed=None): +def sim_loop(task, viewer=True, layout=None, style=None, seed=None, timing=False): """Launch a RoboCasa task env around the H1-2 and run the shared-MjData loop. Builds the env with robots='H1_2' and steps the env's *own* MjData with our @@ -213,6 +213,7 @@ def sim_loop(task, viewer=True, layout=None, style=None, seed=None): ("gripper0_right_hand_cam", "right_hand", "right_hand_camera_color_optical_frame"), ] if _cameras_on else [] print(f"[h12_mujoco] RGBD cameras {'ON' if _cameras_on else 'OFF (GOLEM_CAMERAS=0)'}") + timings = LoopTimings(enabled=timing) ros_bridge = RosSensorBridge( model, data, cameras=_cameras, @@ -245,6 +246,7 @@ def sim_loop(task, viewer=True, layout=None, style=None, seed=None): else () ), sim_lock=sim_lock, + timings=timings, ) # Gripper bridges (gripper0__ prefixed actuators/sensors). @@ -297,11 +299,13 @@ def sim_loop(task, viewer=True, layout=None, style=None, seed=None): _band_release_t = None _band_prev_on = bool(band.enabled) if band is not None else False + timings.reset(float(data.time)) + pacer = RealtimePacer(model.opt.timestep) try: while True: - start_time = time.time() if viewer and not handle.is_running(): break + physics_start = timings.start() with sim_lock: if band is not None: # Re-write every step; MuJoCo persists xfrc_applied, so we must @@ -316,6 +320,7 @@ def sim_loop(task, viewer=True, layout=None, style=None, seed=None): # /lowcmd, which held a stale-damping torque and toppled FAME. sim_interface.write_ctrl() mujoco.mj_step(model, data) + timings.stop("physics", physics_start) # --- fall logger --- if band is not None and _band_prev_on and not band.enabled: _band_release_t = float(data.time) @@ -332,18 +337,28 @@ def sim_loop(task, viewer=True, layout=None, style=None, seed=None): print(f"[fall-logger] *** ROBOT FELL at sim t={data.time:.2f}s — stood " f"{_since} (uprightness={_upz:.2f}, torso_h={_torso_h:.2f}) ***", flush=True) + task_start = timings.start() env.update_state() # REQUIRED before _check_success - ros_bridge.tick() # /clock + camera + lidar + imu + timings.stop("task_update", task_start) + # tick owns short sensor capture locks; encoding/publication cannot + # block the DDS state reader. No physics step occurs inside tick. + ros_bridge.tick() # /clock + camera + lidar + imu + task_start = timings.start() + with sim_lock: done = measurement.tick() + timings.stop("task_check", task_start) if done: print("[h12_mujoco] task success (debounced).") if viewer: + viewer_start = timings.start() if band is not None and band.enabled: _draw_band(handle, band.point, data.xpos[band_body_id]) else: handle.user_scn.ngeom = 0 # clear overlay when band off handle.sync() - time.sleep(max(0, model.opt.timestep - (time.time() - start_time))) + timings.stop("viewer", viewer_start) + pacer.wait() + timings.maybe_report(float(data.time)) finally: if handle is not None: handle.close() @@ -441,6 +456,8 @@ def _resolve_task(name, seed=None): 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") args = parser.parse_args() task = args.task @@ -451,4 +468,4 @@ def _resolve_task(name, seed=None): task = _resolve_task(task, seed=args.seed) sim_loop(task, viewer=not args.headless, - layout=args.layout, style=args.style, seed=args.seed) + layout=args.layout, style=args.style, seed=args.seed, timing=args.timing) diff --git a/h1_robocasa/mujoco_ros_bridge.py b/h1_robocasa/mujoco_ros_bridge.py index fd07ba2..89ad43e 100644 --- a/h1_robocasa/mujoco_ros_bridge.py +++ b/h1_robocasa/mujoco_ros_bridge.py @@ -26,6 +26,9 @@ import struct from array import array as _array from collections import namedtuple +from contextlib import nullcontext + +from sim_timing import LoopTimings import mujoco import numpy as np @@ -137,12 +140,14 @@ def __init__( lidar_self_prefixes: tuple = (), elastic_band=None, sim_lock=None, + timings=None, ): super().__init__("mujoco_sensors") self.model = model self.data = data self.elastic_band = elastic_band self.sim_lock = sim_lock + self.timings = timings if timings is not None else LoopTimings() self.lidar_frame = lidar_frame @@ -337,9 +342,12 @@ def tick(self) -> None: /clock is published every call. TF and IMU are published at their target rates keyed off sim time. Camera and lidar are rate-throttled the same way but run less frequently (they involve rendering / ray - casting on the main thread). + casting on the main thread). Call without holding sim_lock: each sensor + protects its own MuJoCo reads, then encodes/publishes outside the lock. + Physics must stay on this thread so the state cannot advance mid-tick. """ - sim_t = float(self.data.time) + with self.sim_lock or nullcontext(): + sim_t = float(self.data.time) stamp = _sim_time_to_msg(sim_t) # Always publish /clock so subscriber nodes can track sim time. @@ -376,11 +384,12 @@ def tick(self) -> None: self.get_logger().warn(f"lidar publish failed: {e}") def _publish_imu(self, stamp: TimeMsg) -> None: - sd = self.data.sensordata - # framequat sensor returns (w, x, y, z) world-frame; gyro/accel are site-local. - q = sd[self.imu_quat_adr : self.imu_quat_adr + self.imu_quat_dim] - w = sd[self.imu_gyro_adr : self.imu_gyro_adr + self.imu_gyro_dim] - a = sd[self.imu_acc_adr : self.imu_acc_adr + self.imu_acc_dim] + with self.sim_lock or nullcontext(): + sd = self.data.sensordata + # framequat sensor returns (w, x, y, z) world-frame; gyro/accel are site-local. + q = sd[self.imu_quat_adr : self.imu_quat_adr + self.imu_quat_dim].copy() + w = sd[self.imu_gyro_adr : self.imu_gyro_adr + self.imu_gyro_dim].copy() + a = sd[self.imu_acc_adr : self.imu_acc_adr + self.imu_acc_dim].copy() msg = Imu() msg.header.stamp = stamp @@ -400,11 +409,12 @@ def _publish_imu(self, stamp: TimeMsg) -> None: def _publish_odom(self, stamp: TimeMsg) -> None: # Ground-truth pelvis pose in the sim world (used as the odom frame). # MuJoCo quaternions are (w, x, y, z); ROS wants (x, y, z, w). - pos = self.data.xpos[self.base_body_id] - quat = self.data.xquat[self.base_body_id] - px, py, pz = float(pos[0]), float(pos[1]), float(pos[2]) - qw, qx, qy, qz = (float(quat[0]), float(quat[1]), - float(quat[2]), float(quat[3])) + with self.sim_lock or nullcontext(): + pos = self.data.xpos[self.base_body_id] + quat = self.data.xquat[self.base_body_id] + px, py, pz = float(pos[0]), float(pos[1]), float(pos[2]) + qw, qx, qy, qz = (float(quat[0]), float(quat[1]), + float(quat[2]), float(quat[3])) tf = TransformStamped() tf.header.stamp = stamp @@ -433,35 +443,46 @@ def _publish_odom(self, stamp: TimeMsg) -> None: self.pub_odom.publish(odom) def _publish_camera_frame(self, stamp: TimeMsg) -> None: - if self._renderer is None: - if self.model.vis.global_.offwidth < self.cam_width: - self.model.vis.global_.offwidth = self.cam_width - if self.model.vis.global_.offheight < self.cam_height: - self.model.vis.global_.offheight = self.cam_height - self._renderer = mujoco.Renderer(self.model, height=self.cam_height, width=self.cam_width) - # Group 0: red collision primitives (joint spheres, mesh - # duplicates) — hide. Group 1: H1 body visual meshes — show. - # Group 2: magpie hand visuals + h12 wrist mount — show. - # Group 3: magpie collision meshes + finger pad boxes — hide. - self._scene_opt = mujoco.MjvOption() - self._scene_opt.geomgroup[:] = 0 - self._scene_opt.geomgroup[1] = 1 - self._scene_opt.geomgroup[2] = 1 - # Hide all site markers from camera output. Sites are non-physical - # debug markers (camera/lidar mounts, robosuite's required grip_site - # at the grasp center, etc.); a hand camera looking down the gripper - # axis stares straight at grip_site, whose semi-transparent geom is - # faint in RGB but writes a solid blob into the depth image. - self._scene_opt.sitegroup[:] = 0 + with self.sim_lock or nullcontext(): + if self._renderer is None: + if self.model.vis.global_.offwidth < self.cam_width: + self.model.vis.global_.offwidth = self.cam_width + if self.model.vis.global_.offheight < self.cam_height: + self.model.vis.global_.offheight = self.cam_height + self._renderer = mujoco.Renderer(self.model, height=self.cam_height, width=self.cam_width) + self._rgb_buffer = np.empty((self.cam_height, self.cam_width, 3), dtype=np.uint8) + self._depth_buffer = np.empty((self.cam_height, self.cam_width), dtype=np.float32) + # Group 0: red collision primitives (joint spheres, mesh + # duplicates) — hide. Group 1: H1 body visual meshes — show. + # Group 2: magpie hand visuals + h12 wrist mount — show. + # Group 3: magpie collision meshes + finger pad boxes — hide. + self._scene_opt = mujoco.MjvOption() + self._scene_opt.geomgroup[:] = 0 + self._scene_opt.geomgroup[1] = 1 + self._scene_opt.geomgroup[2] = 1 + # Hide all site markers from camera output. Sites are non-physical + # debug markers (camera/lidar mounts, robosuite's required grip_site + # at the grasp center, etc.); a hand camera looking down the gripper + # axis stares straight at grip_site, whose semi-transparent geom is + # faint in RGB but writes a solid blob into the depth image. + self._scene_opt.sitegroup[:] = 0 r = self._renderer # Render every camera through the shared renderer (one render context, # selected per camera by name). All share resolution/rate; each publishes # to its own /realsense//* topics with its own frame_id. for cam in self.cameras: + render_start = self.timings.start() r.disable_depth_rendering() - r.update_scene(self.data, camera=cam.name, scene_option=self._scene_opt) - rgb_u8 = np.ascontiguousarray(r.render(), dtype=np.uint8) + with self.sim_lock or nullcontext(): + r.update_scene(self.data, camera=cam.cam_id, scene_option=self._scene_opt) + # Both passes consume the same captured MjvScene. Rendering and + # encoding do not access mutable MjData; keep DDS readers unblocked. + rgb_u8 = r.render(out=self._rgb_buffer) + r.enable_depth_rendering() + depth = r.render(out=self._depth_buffer) + self.timings.stop("camera_render", render_start) + publish_start = self.timings.start() rgb_jpeg = io.BytesIO() PILImage.fromarray(rgb_u8, mode="RGB").save(rgb_jpeg, format="JPEG", quality=80) @@ -488,10 +509,6 @@ def _publish_camera_frame(self, stamp: TimeMsg) -> None: rgb_raw.data = _array("B", rgb_u8.tobytes()) cam.pub_rgb_raw.publish(rgb_raw) - r.enable_depth_rendering() - r.update_scene(self.data, camera=cam.name, scene_option=self._scene_opt) - depth = r.render() - # Mask "no hit" pixels (far-plane sentinel + non-finite/<=0) to 0 # before quantizing to mm. RealSense uses 0 as the invalid value # in 16UC1 aligned-depth-to-color, which compressed_depth honors. @@ -529,33 +546,39 @@ def _publish_camera_frame(self, stamp: TimeMsg) -> None: info = cam.info_msg info.header.stamp = stamp cam.pub_info.publish(info) + self.timings.stop("camera_publish", publish_start) def _publish_lidar_scan(self, stamp: TimeMsg) -> None: - origin = np.ascontiguousarray(self.data.xpos[self.lidar_body_id], dtype=np.float64).reshape(3, 1) - rot = np.array(self.data.xmat[self.lidar_body_id], dtype=np.float64).reshape(3, 3) - - # Rotate local ray directions to world frame. - # rot maps local→world (column vecs); for row-vec array: world = local @ rot.T - world_dirs = np.ascontiguousarray(self.lidar_local_dirs @ rot.T, dtype=np.float64) - - # Single batched cast. bodyexclude=torso skips the torso shell at the - # source: the lidar sits inside the torso visual mesh and otherwise - # every ray would self-hit at ~2 cm, forcing a per-ray retry loop - # (~280 ms/scan). Excluding here gives ~80 ms/scan. - dists = self._lidar_dists - geomids = self._lidar_geomids - dists.fill(-1.0) - geomids.fill(-1) - mujoco.mj_multiRay( - self.model, self.data, - origin, world_dirs.reshape(-1, 1), - self.lidar_geomgroup, - 1, # include static geoms - self.lidar_exclude_body_id, # skip torso geoms - geomids, dists, - self.lidar_rays, - self.lidar_max_range, - ) + cast_start = self.timings.start() + with self.sim_lock or nullcontext(): + timebase = int(self.data.time * 1e9) + origin = np.ascontiguousarray(self.data.xpos[self.lidar_body_id], dtype=np.float64).reshape(3, 1) + rot = np.array(self.data.xmat[self.lidar_body_id], dtype=np.float64).reshape(3, 3) + + # Rotate local ray directions to world frame. + # rot maps local→world (column vecs); for row-vec array: world = local @ rot.T + world_dirs = np.ascontiguousarray(self.lidar_local_dirs @ rot.T, dtype=np.float64) + + # Single batched cast. bodyexclude=torso skips the torso shell at the + # source: the lidar sits inside the torso visual mesh and otherwise + # every ray would self-hit at ~2 cm, forcing a per-ray retry loop + # (~280 ms/scan). Excluding here gives ~80 ms/scan. + dists = self._lidar_dists + geomids = self._lidar_geomids + dists.fill(-1.0) + geomids.fill(-1) + mujoco.mj_multiRay( + self.model, self.data, + origin, world_dirs.reshape(-1, 1), + self.lidar_geomgroup, + 1, # include static geoms + self.lidar_exclude_body_id, # skip torso geoms + geomids, dists, + self.lidar_rays, + self.lidar_max_range, + ) + self.timings.stop("lidar_cast", cast_start) + publish_start = self.timings.start() hit_dists = dists.ravel() # Drop self-returns: rays that hit the robot's own body (legs/arms/grippers @@ -575,7 +598,7 @@ def _publish_lidar_scan(self, stamp: TimeMsg) -> None: msg = CustomMsg() msg.header.stamp = stamp msg.header.frame_id = self.lidar_frame - msg.timebase = int(self.data.time * 1e9) + msg.timebase = timebase msg.point_num = int(pts.shape[0]) msg.lidar_id = 0 msg.rsvd = [0, 0, 0] @@ -610,8 +633,11 @@ def _publish_lidar_scan(self, stamp: TimeMsg) -> None: pc2.point_step = PC2_POINT_STEP pc2.row_step = PC2_POINT_STEP * pc2.width pc2.is_dense = True - pc2.data = arr.tobytes() + # A typed byte array avoids rclpy's per-byte Python validation; the + # serialized PointCloud2 payload is identical to the packed NumPy bytes. + pc2.data = _array("B", arr.tobytes()) self.pub_pc2.publish(pc2) + self.timings.stop("lidar_publish", publish_start) def init_ros() -> bool: diff --git a/h1_robocasa/sim_timing.py b/h1_robocasa/sim_timing.py new file mode 100644 index 0000000..74520bf --- /dev/null +++ b/h1_robocasa/sim_timing.py @@ -0,0 +1,72 @@ +"""Wall-clock pacing and optional stage timings for the single-step simulator.""" +import math +import time + + +class RealtimePacer: + """Keep a wall deadline without skipping physics or repaying long pauses.""" + + def __init__(self, timestep, max_lag=0.1, *, clock=time.perf_counter, sleep=time.sleep): + if not math.isfinite(timestep) or timestep <= 0: + raise ValueError('timestep must be finite and positive') + if not math.isfinite(max_lag) or max_lag < 0: + raise ValueError('max_lag must be finite and nonnegative') + self.timestep = timestep + self.max_lag = max_lag + self.clock = clock + self.sleep = sleep + self.deadline = clock() + + def wait(self): + self.deadline += self.timestep + now = self.clock() + delay = self.deadline - now + if delay > 0: + self.sleep(delay) + elif -delay > self.max_lag: + # Renderer initialization/debugger pauses must not create minutes + # of catch-up. Only the wall deadline changes, never simulation time. + self.deadline = now - self.max_lag + + +class LoopTimings: + """Report mean/max stage milliseconds and RTF every few wall seconds.""" + + def __init__(self, enabled=False, interval=5.0, *, clock=time.perf_counter): + 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 = {} + + def reset(self, sim_time): + self.wall_start = self.clock() if self.enabled else 0.0 + self.sim_start = sim_time + self.stages.clear() + + def start(self): + return self.clock() if self.enabled else 0.0 + + def stop(self, name, start): + if self.enabled: + elapsed = self.clock() - start + 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): + if not self.enabled: + return + now = self.clock() + wall = now - self.wall_start + if wall < self.interval: + return + fields = ' '.join( + f'{name}={1000 * total / count:.2f}/{1000 * longest:.2f}ms(n={count})' + for name, (total, count, longest) in self.stages.items() + ) + print(f'[sim-timing] RTF={(sim_time - self.sim_start) / wall:.3f} ' + f'(stage mean/max) {fields}', flush=True) + self.wall_start = now + self.sim_start = sim_time + self.stages.clear() diff --git a/tests/.gitignore b/tests/.gitignore new file mode 100644 index 0000000..b45f559 --- /dev/null +++ b/tests/.gitignore @@ -0,0 +1,2 @@ +results/ +__pycache__/ diff --git a/tests/README.md b/tests/README.md new file mode 100644 index 0000000..1412e31 --- /dev/null +++ b/tests/README.md @@ -0,0 +1,55 @@ +# Simulator regression tests + +All simulator regression tests and repeatable validation commands live here, +outside the simulator submodule. CPU tests run on every relevant pull request +through `.github/workflows/simulator-tests.yml`; select its `cpu` job as a +required branch-protection check to gate merges. + +## CPU tests + +The Isaac suite needs Python 3.11, CPU PyTorch, NumPy, Pillow and CycloneDDS 0.10.2 +plus the checked-out Unitree SDK. It does not start Kit or DDS participants. +See the workflow for the exact dependency setup. + +```bash +PYTHONPATH=CL_isaaclab_sim:unitree_sdk2_python python -m unittest discover -s tests/isaaclab -p 'test_*.py' -v +PYTHONPATH=h1_robocasa python -m unittest discover -s tests/robocasa -p test_loop_timing.py -v +python -m unittest discover -s tests/docker -v +``` + +Using existing Docker images: + +```bash +ROS_DOMAIN_ID=187 tests/isaaclab/run_in_container.sh +ROS_DOMAIN_ID=187 tests/robocasa/run_in_container.sh +``` + +## NVIDIA / ROS integration + +These require the GOLEM Isaac and ROS images, initialized assets including LFS, +Docker NVIDIA runtime, and an NVIDIA GPU for the scene smoke test. Run them on +an isolated GPU worker, not an untrusted pull request with access to a real +robot network. The wrappers reject DDS domain 0 and use disposable containers. +They do not replace the normal simulator containers. + +```bash +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 +``` + +`run_smoke.sh` requires the actual Isaac Lab scene to complete 50 steps and +shut down normally. Its log is `tests/results/isaac-smoke.log`; a timeout fails. +`run_livox_interop.sh` checks that Isaac's CycloneDDS CustomMsg publisher is +decoded by the ROS container's generated Humble Livox message type. + +`run_robot_interop.sh` publishes native ROS LowCmd and Inspire commands, checks +wrist and right-index finger motion in both directions, receives LowState, +Inspire state and all 19 sensor topics, and releases body motors on exit. +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. + +The CPU suite cannot establish physical asset fidelity, real-time performance, +or controller stability. Those need the GPU/ROS integration environment and +robot-specific acceptance tests when the CL_Assets pin changes. diff --git a/tests/docker/test_isaac_domain.py b/tests/docker/test_isaac_domain.py new file mode 100644 index 0000000..993ff4f --- /dev/null +++ b/tests/docker/test_isaac_domain.py @@ -0,0 +1,42 @@ +"""Exercise Isaac's real DDS initialization without requiring Isaac/CycloneDDS.""" +import ast +import contextlib +import io +import os +from pathlib import Path +from types import SimpleNamespace +import unittest +from unittest.mock import patch + + +class IsaacDomain(unittest.TestCase): + @classmethod + def setUpClass(cls): + path = Path(__file__).resolve().parents[2] / 'CL_isaaclab_sim/src/python/dds/common/dds_master.py' + tree = ast.parse(path.read_text()) + manager = next(n for n in tree.body if isinstance(n, ast.ClassDef) and n.name == 'DDSManager') + method = next(n for n in manager.body if isinstance(n, ast.FunctionDef) and n.name == '_init_dds') + cls.code = compile(ast.Module(body=[method], type_ignores=[]), str(path), 'exec') + + def initialize(self, value): + calls = [] + namespace = {'os': os, 'initialize_transport': calls.append} + exec(self.code, namespace) + with patch.dict(os.environ, {'ROS_DOMAIN_ID': value}), contextlib.redirect_stdout(io.StringIO()): + result = namespace['_init_dds'](SimpleNamespace(dds_initialized=False)) + return result, calls + + def test_configured_domain_reaches_sdk(self): + self.assertEqual(self.initialize('7'), (True, [7])) + + def test_empty_domain_defaults_to_simulation(self): + self.assertEqual(self.initialize(''), (True, [1])) + + def test_invalid_domains_abort_initialization(self): + for domain in ('0', '00', '-1', '233', 'bad'): + with self.subTest(domain=domain), self.assertRaises(ValueError): + self.initialize(domain) + + +if __name__ == '__main__': + unittest.main() diff --git a/tests/docker/test_workflows.py b/tests/docker/test_workflows.py new file mode 100644 index 0000000..3cf61b7 --- /dev/null +++ b/tests/docker/test_workflows.py @@ -0,0 +1,206 @@ +"""Host-side Docker workflow regressions; no daemon or GPU required.""" +import json +import os +from pathlib import Path +import shutil +import subprocess +import tempfile +import unittest + +REPO = Path(__file__).resolve().parents[2] + + +class Workflows(unittest.TestCase): + def setUp(self): + self.tmp = tempfile.TemporaryDirectory(prefix='golem test ') + self.addCleanup(self.tmp.cleanup) + 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', 'CL_Assets/ros_assets/h1_2_magpie_ros.urdf', + 'CL_isaaclab_sim/sim_main.py', 'h1_robocasa/h12_mujoco.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/h1_bringup/package.xml', 'mujoco_mpc/CMakeLists.txt'): + p = self.root / name + p.parent.mkdir(parents=True, exist_ok=True) + p.write_text('fixture') + for name in ('CL_Assets/graspgenx/assets/magpie', 'CL_Assets/graspgenx/assets/meshes/magpie', + '.git/modules/mujoco_mpc', 'tools'): + (self.root / name).mkdir(parents=True, exist_ok=True) + self.bin = self.root / 'bin' + self.bin.mkdir() + docker = self.bin / 'docker' + docker.write_text('''#!/usr/bin/env python3 +import json, os, sys +with open(os.environ['CALLS'], 'a') as f: + f.write(json.dumps(sys.argv[1:]) + '\\n') +if sys.argv[1] == 'inspect': + if os.environ.get('EXISTING'): + print('true') + else: + sys.exit(1) +if sys.argv[1] == 'info': + print('aarch64' if os.environ.get('TEST_MAC') else 'x86_64') +if 'config' in sys.argv: + os.execv('/usr/bin/docker', ['docker'] + sys.argv[1:]) +''') + docker.chmod(0o755) + self.env = {k: v for k, v in os.environ.items() if not k.startswith(('GOLEM_', 'ROS_', 'HAMS_'))} + self.env.update(PATH=str(self.bin) + ':' + os.environ['PATH'], CALLS=str(self.root / 'calls'), DISPLAY='') + + def run_script(self, path, *args, **env): + return subprocess.run(['bash', str(self.root / path), *args], cwd='/tmp', + env=dict(self.env, **env), text=True, capture_output=True) + + def calls(self): + p = self.root / 'calls' + return [json.loads(x) for x in p.read_text().splitlines()] if p.exists() else [] + + def test_pull_refreshes_upstream_base_without_pulling_local_child_base(self): + r = self.run_script('docker/scripts/docker_build.sh', '--pull', '--no-cache', 'robocasa') + self.assertEqual(r.returncode, 0, r.stderr) + builds = [c for c in self.calls() if 'build' in c] + self.assertEqual(len(builds), 2) + self.assertIn('--pull', builds[0]) + self.assertNotIn('--pull', builds[1]) + self.assertTrue(all('--no-cache' in c for c in builds)) + + def test_run_never_force_removes_existing_container(self): + r = self.run_script('docker/scripts/docker_run.sh', 'robocasa', '--headless') + self.assertEqual(r.returncode, 0, r.stderr) + self.assertFalse(any(c[:2] == ['rm', '-f'] for c in self.calls())) + + def test_zero_with_leading_zeros_is_rejected(self): + r = self.run_script('docker/scripts/docker_run.sh', 'robocasa', ROS_DOMAIN_ID='00') + self.assertNotEqual(r.returncode, 0) + self.assertFalse(any('run' in c for c in self.calls())) + + def test_missing_source_fails_before_container_creation(self): + (self.root / 'h1_robocasa/h12_mujoco.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.assertFalse(any('run' in c for c in self.calls())) + + def test_isaac_launcher_forwards_options_without_a_terminal(self): + interpreter = self.root / 'isaac lab/_isaac_sim/python.sh' + interpreter.parent.mkdir(parents=True) + interpreter.write_text("#!/usr/bin/env python3\nimport json, os, sys\nprint(json.dumps({'args':sys.argv[1:],'domain':os.environ['ROS_DOMAIN_ID']}))\n") + interpreter.chmod(0o755) + # Isolate the compatibility symlink's host filesystem effects. + for tool in ('mkdir', 'ln'): + stub = self.bin / tool + stub.write_text('#!/bin/sh\nexit 0\n') + stub.chmod(0o755) + r = self.run_script('docker/scripts/launch_isaac.sh', '--task', 'custom', '--', + '--rendering_mode', 'balanced', ROS_DOMAIN_ID='7', + ISAACLAB_PATH=str(interpreter.parent.parent)) + self.assertEqual(r.returncode, 0, r.stderr) + result = json.loads(r.stdout.splitlines()[-1]) + self.assertEqual(result['domain'], '7') + self.assertIn('--headless', result['args']) + self.assertEqual(result['args'][-2:], ['--rendering_mode', 'balanced']) + self.assertEqual(result['args'][result['args'].index('--task') + 1], 'custom') + + def test_lfs_pointer_is_not_accepted_as_an_asset(self): + asset = self.root / 'CL_Assets/ros_assets/h1_2_magpie_ros.urdf' + 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) + self.assertIn('LFS', r.stderr) + self.assertFalse(any('run' in c for c in self.calls())) + + def test_mac_rejects_isaac(self): + r = self.run_script('docker/mac/scripts/docker_run_mac.sh', 'isaac', TEST_MAC='1') + self.assertNotEqual(r.returncode, 0) + self.assertFalse(any('run' in c for c in self.calls())) + + def test_shell_domain_overrides_env_file(self): + (self.root / 'docker/.env').write_text('ROS_DOMAIN_ID=0\n') + r = self.run_script('docker/scripts/docker_run.sh', 'robocasa', ROS_DOMAIN_ID='7') + self.assertEqual(r.returncode, 0, r.stderr) + + def test_headless_compose_has_no_desktop_mounts_and_uses_asset_override(self): + assets = self.root / 'external assets' + assets.mkdir() + r = subprocess.run(['/usr/bin/docker', 'compose', '--env-file', '/dev/null', '-f', + str(self.root / 'docker/docker-compose.yml'), '--profile', 'robocasa', + 'config', '--format', 'json'], env=dict(self.env, GOLEM_ASSETS_DIR=str(assets)), + text=True, capture_output=True) + self.assertEqual(r.returncode, 0, r.stderr) + service = json.loads(r.stdout)['services']['robocasa'] + mounts = {v['target']: v for v in service['volumes']} + self.assertNotIn('/root/.Xauthority', mounts) + self.assertNotIn('/tmp/.X11-unix', mounts) + self.assertEqual(mounts['/home/code/CL_Assets']['source'], str(assets)) + self.assertFalse(mounts['/home/code/CL_Assets'].get('bind', {}).get('create_host_path', False)) + + def test_mac_stack_warms_ros_before_starting_pair(self): + r = self.run_script('docker/mac/scripts/docker_stack_mac.sh', 'up', 'robocasa', TEST_MAC='1') + self.assertEqual(r.returncode, 0, r.stderr) + calls = self.calls() + warm = next(i for i, c in enumerate(calls) if 'run' in c and '/bin/true' in c) + start = next(i for i, c in enumerate(calls) if 'up' in c) + self.assertLess(warm, start) + self.assertIn('ros', calls[start]) + self.assertIn('robocasa', calls[start]) + + def test_existing_container_requires_explicit_restart(self): + r = self.run_script('docker/scripts/docker_run.sh', 'robocasa', EXISTING='1') + self.assertNotEqual(r.returncode, 0) + self.assertFalse(any(c[0] == 'rm' or 'run' in c for c in self.calls())) + + def test_explicit_restart_removes_only_selected_container(self): + r = self.run_script('docker/scripts/docker_run.sh', 'robocasa', '--restart', EXISTING='1') + self.assertEqual(r.returncode, 0, r.stderr) + self.assertIn(['rm', '-f', 'golem_sim_robocasa'], self.calls()) + + def test_asset_and_cache_paths_with_spaces(self): + assets = self.root / 'asset disk' + shutil.copytree(self.root / 'CL_Assets', assets) + cache = self.root / 'cache disk' + r = self.run_script('docker/scripts/docker_run.sh', 'isaac', '--headless', + GOLEM_ASSETS_DIR=str(assets), GOLEM_CACHE_DIR=str(cache)) + self.assertEqual(r.returncode, 0, r.stderr) + self.assertTrue((cache / 'isaac/nvidia').is_dir()) + self.assertFalse((self.root / 'CL_isaaclab_sim/.isaac_cache').exists()) + + def test_runtime_sim_domain_guard_applies_without_wrapper(self): + script = self.root / 'docker/scripts/runtime_common.sh' + for domain in ('0', '00', '-1', '233', 'invalid'): + with self.subTest(domain=domain): + r = subprocess.run(['bash', '-c', 'source "$1"; validate_sim_domain', 'bash', str(script)], + env=dict(self.env, ROS_DOMAIN_ID=domain), capture_output=True) + self.assertNotEqual(r.returncode, 0) + + def test_mac_compose_keeps_native_build_volumes_without_gpu_requirements(self): + r = subprocess.run(['/usr/bin/docker', 'compose', '--env-file', '/dev/null', '-f', + str(self.root / 'docker/mac/docker-compose.yml'), 'config', '--format', 'json'], + env=self.env, text=True, capture_output=True) + self.assertEqual(r.returncode, 0, r.stderr) + services = json.loads(r.stdout)['services'] + self.assertEqual(set(services), {'ros', 'robocasa'}) + for service in services.values(): + self.assertEqual(service['platform'], 'linux/arm64') + self.assertNotIn('runtime', service) + self.assertEqual(service['ipc'], 'host') + self.assertTrue(any(v['type'] == 'volume' and v['target'].endswith('/build') for v in service['volumes'])) + + def test_stack_restart_replaces_single_service_containers(self): + r = self.run_script('docker/scripts/docker_stack.sh', 'restart', 'robocasa', EXISTING='1') + self.assertEqual(r.returncode, 0, r.stderr) + calls = self.calls() + for name in ('golem_ros', 'golem_sim_robocasa'): + self.assertIn(['rm', '-f', name], calls) + first_run = next(i for i, c in enumerate(calls) if 'run' in c) + self.assertTrue(all(i < first_run for i, c in enumerate(calls) if c[:2] == ['rm', '-f'])) + + def test_stack_stop_does_not_require_display_or_mounted_assets(self): + shutil.rmtree(self.root / 'CL_Assets') + r = self.run_script('docker/scripts/docker_stack.sh', 'stop', 'robocasa', GOLEM_DISPLAY='gui') + self.assertEqual(r.returncode, 0, r.stderr) + self.assertFalse(any('run' in c or 'up' in c for c in self.calls())) + + +if __name__ == '__main__': + unittest.main() diff --git a/tests/isaaclab/check_livox_ros.py b/tests/isaaclab/check_livox_ros.py new file mode 100644 index 0000000..3350c60 --- /dev/null +++ b/tests/isaaclab/check_livox_ros.py @@ -0,0 +1,24 @@ +"""ROS-container half of CustomMsg interoperability test; exits nonzero on timeout.""" +import time +import rclpy +from livox_ros_driver2.msg import CustomMsg + +rclpy.init() +node = rclpy.create_node('isaac_livox_interop_check') +received = [] +node.create_subscription(CustomMsg, '/livox/lidar', received.append, 5) +deadline = time.monotonic() + 30 +while not received and time.monotonic() < deadline: + rclpy.spin_once(node, timeout_sec=.25) +try: + assert received, 'no /livox/lidar CustomMsg received' + msg = received[-1] + assert msg.header.frame_id == 'lidar_link' + assert (msg.header.stamp.sec, msg.header.stamp.nanosec) == (7, 123) + assert msg.timebase == 7_000_000_123 + assert msg.point_num == 2 + assert [(p.x, p.y, p.z, p.offset_time, p.line) for p in msg.points] == [(1., 2., 3., 0, 0), (4., 5., 6., 500, 5)] + print('PASS: native ROS decoded CycloneDDS Livox CustomMsg') +finally: + node.destroy_node() + rclpy.shutdown() diff --git a/tests/isaaclab/check_robot_ros.py b/tests/isaaclab/check_robot_ros.py new file mode 100644 index 0000000..3e38287 --- /dev/null +++ b/tests/isaaclab/check_robot_ros.py @@ -0,0 +1,171 @@ +"""Native ROS motor/sensor interoperability check against a live Isaac simulation. + +Invoked by run_robot_interop.sh on an isolated nonzero DDS domain. No DDS +participant is created by the SDK: it only supplies the command CRC encoding. +""" +import math +import os +import time + +import rclpy +from rclpy.qos import qos_profile_sensor_data +from sensor_msgs.msg import Image, CompressedImage, CameraInfo, Imu, PointCloud2 +from rosgraph_msgs.msg import Clock +from livox_ros_driver2.msg import CustomMsg +from unitree_hg.msg import LowCmd, LowState +from unitree_go.msg import MotorCmd, MotorCmds, MotorStates +from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowCmd_ +from unitree_sdk2py.utils.crc import CRC + + +def main(): + domain = int(os.environ.get('ROS_DOMAIN_ID', '0')) + if not 1 <= domain <= 232: + raise RuntimeError('ROS_DOMAIN_ID must be nonzero for simulation testing') + rclpy.init() + node = rclpy.create_node('isaac_robot_interop_check') + publisher = node.create_publisher(LowCmd, '/lowcmd', 10) + hand_publisher = node.create_publisher(MotorCmds, '/inspire/cmd', 10) + crc = CRC() + latest, seen, subscriptions = {}, set(), [] + samples = [] + + def lowstate(msg): + values = [v for motor in msg.motor_state[:27] for v in (motor.q, motor.dq, motor.tau_est)] + quat = list(msg.imu_state.quaternion) + assert all(math.isfinite(x) for x in values + quat), 'nonfinite lowstate' + assert abs(sum(x*x for x in quat) - 1) < .05, 'invalid quaternion norm' + # The base task spawns fixed upright at yaw -90 degrees in WXYZ. + expected = (math.sqrt(.5), 0., 0., -math.sqrt(.5)) + assert abs(sum(a*b for a, b in zip(quat, expected))) > .99, f'expected fixture WXYZ yaw -90 quaternion, got {quat}' + latest['state'] = msg + samples.append((time.monotonic(), float(msg.motor_state[19].q))) + + subscriptions.append(node.create_subscription(LowState, '/lowstate', lowstate, qos_profile_sensor_data)) + + def handstate(msg): + assert len(msg.states) == 12, 'Inspire state must contain 12 motors' + assert all(math.isfinite(m.q) for m in msg.states), 'nonfinite Inspire state' + latest['hand'] = msg + + subscriptions.append(node.create_subscription(MotorStates, '/inspire/state', handstate, qos_profile_sensor_data)) + + def send_hand(positions): + if positions is None: + return + msg = MotorCmds() + for position in positions: + motor = MotorCmd() + motor.mode = 1 + motor.q = float(position) + msg.cmds.append(motor) + hand_publisher.publish(msg) + + def sensor(topic, msg): + if isinstance(msg, Image): + assert msg.width > 0 and msg.height > 0 and len(msg.data) > 0, topic + elif isinstance(msg, CompressedImage): + assert msg.format and len(msg.data) > 0, topic + elif isinstance(msg, CameraInfo): + assert msg.width > 0 and msg.k[0] > 0, topic + elif isinstance(msg, PointCloud2): + assert msg.width > 0 and len(msg.data) > 0, topic + elif isinstance(msg, CustomMsg): + assert msg.point_num > 0 and msg.point_num == len(msg.points), topic + elif isinstance(msg, Imu): + assert all(math.isfinite(v) for v in (msg.linear_acceleration.x, msg.linear_acceleration.y, + msg.linear_acceleration.z, msg.angular_velocity.x, msg.angular_velocity.y, msg.angular_velocity.z)), topic + elif isinstance(msg, Clock): + latest['sim_time'] = msg.clock.sec + msg.clock.nanosec * 1e-9 + seen.add(topic) + + required = {'/clock': Clock, '/livox/lidar': CustomMsg, '/livox/pointcloud': PointCloud2, '/livox/imu': Imu} + for camera in ('head', 'left_hand', 'right_hand'): + root = f'/realsense/{camera}' + required.update({root + '/color/image_raw': Image, + root + '/color/image_raw/compressed': CompressedImage, + root + '/aligned_depth_to_color/image_raw': Image, + root + '/aligned_depth_to_color/image_raw/compressedDepth': CompressedImage, + root + '/color/camera_info': CameraInfo}) + for topic, kind in required.items(): + subscriptions.append(node.create_subscription(kind, topic, lambda msg, t=topic: sensor(t, msg), qos_profile_sensor_data)) + + def send(targets=None): + wire, native = unitree_hg_msg_dds__LowCmd_(), LowCmd() + wire.mode_pr = native.mode_pr = 0 + wire.mode_machine = native.mode_machine = int(latest['state'].mode_machine) if 'state' in latest else 0 + for i, (sdk_motor, ros_motor) in enumerate(zip(wire.motor_cmd, native.motor_cmd)): + if targets is not None and i < 27: + sdk_motor.mode = ros_motor.mode = 1 + sdk_motor.q = ros_motor.q = float(targets[i]) + sdk_motor.kp = ros_motor.kp = 20. + sdk_motor.kd = ros_motor.kd = 2. + native.crc = crc.Crc(wire) + publisher.publish(native) + + deadline = time.monotonic() + float(os.environ.get('ROBOT_INTEROP_TIMEOUT', '300')) + initial = None + initial_hand = None + hand_targets = None + first_hand_actual = None + stage = 0 + start_sim = None + targets = None + next_publish = 0. + next_trace = 0. + try: + print('SUBSCRIBER_READY', flush=True) + while time.monotonic() < deadline: + rclpy.spin_once(node, timeout_sec=.005) + if 'state' not in latest or 'sim_time' not in latest or 'hand' not in latest: + continue + if initial is None: + initial = [float(m.q) for m in latest['state'].motor_state[:27]] + targets = initial.copy() + targets[19] += .1 + initial_hand = [float(m.q) for m in latest['hand'].states] + hand_targets = initial_hand.copy() + hand_targets[3] = .95 + start_sim = latest['sim_time'] + print(f'Initial wrist q={initial[19]:.6f}; target={targets[19]:.6f}; right index normalized q={initial_hand[3]:.6f}, target={hand_targets[3]:.6f}', flush=True) + if latest['sim_time'] >= next_trace: + motor = latest['state'].motor_state[19] + print(f"TRACE sim={latest['sim_time']:.3f} tick={latest['state'].tick} wrist19 q={motor.q:.6f} dq={motor.dq:.6f} tau_est={motor.tau_est:.6f} command(mode=1 q={targets[19]:.6f} dq=0 tau=0 kp=20 kd=2) right_index3 q={latest['hand'].states[3].q:.6f} target={hand_targets[3]:.6f}", flush=True) + next_trace = latest['sim_time'] + .1 + if time.monotonic() >= next_publish: + send(targets) + send_hand(hand_targets) + next_publish = time.monotonic() + .01 + if latest['sim_time'] - start_sim >= 1.0: + actual = float(latest['state'].motor_state[19].q) + assert abs(actual - targets[19]) < .06, f'wrist did not converge: target={targets[19]}, actual={actual}' + hand_actual = float(latest['hand'].states[3].q) + assert abs(hand_actual - hand_targets[3]) < .03, f'right index did not converge: target={hand_targets[3]}, actual={hand_actual}' + if stage == 0: + assert initial_hand[3] - hand_actual > .015, 'Inspire flex command did not produce finger motion' + first_hand_actual = hand_actual + hand_targets[3] = 1.0 + assert actual - initial[19] > .04, 'positive target did not produce positive physical motion' + stage = 1 + targets[19] = initial[19] + start_sim = latest['sim_time'] + print(f'Wrist moved to {actual:.6f}; reversing to {targets[19]:.6f}', flush=True) + else: + assert hand_actual - first_hand_actual > .015, 'Inspire reverse command did not produce finger motion' + missing = set(required) - seen + if not missing: + print(f'PASS: ROS LowCmd and Inspire commands drove wrist and right index in both directions; {len(samples)} LowState samples; all {len(required)} sensor topics received', flush=True) + return + raise AssertionError(f'timeout: stage={stage}, lowstate_samples={len(samples)}, missing_topics={sorted(set(required)-seen)}') + finally: + # Release motors even when an assertion or transport error aborts the test. + for _ in range(10): + send() + send_hand(initial_hand) + time.sleep(.01) + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/tests/isaaclab/inspect_motor_dynamics.py b/tests/isaaclab/inspect_motor_dynamics.py new file mode 100644 index 0000000..5a14a32 --- /dev/null +++ b/tests/isaaclab/inspect_motor_dynamics.py @@ -0,0 +1,9 @@ +"""Read authored wrist mass and drive properties without starting GPU physics.""" +from pxr import Usd +stage = Usd.Stage.Open('/home/code/CL_Assets/isaac_assets/robots/h1_2-26dof-inspire-base-fix-usd/h1_2_26dof_with_inspire_rev_1_0.usd') +for prim in stage.Traverse(): + if any(name in prim.GetName() for name in ('left_wrist', 'L_hand', 'L_base')): + print(str(prim.GetPath()), prim.GetTypeName()) + for attr in prim.GetAttributes(): + if any(k in attr.GetName().lower() for k in ('mass', 'inertia', 'armature', 'stiffness', 'damping', 'maxforce', 'axis')): + print(' ', attr.GetName(), attr.Get()) diff --git a/tests/isaaclab/inspect_robot_assets.py b/tests/isaaclab/inspect_robot_assets.py new file mode 100644 index 0000000..eb65481 --- /dev/null +++ b/tests/isaaclab/inspect_robot_assets.py @@ -0,0 +1,10 @@ +"""Inspect robot USD composition without starting Kit or physics.""" +import glob +from pxr import Usd + +for path in glob.glob('/home/code/CL_Assets/isaac_assets/robots/h1_2_handless/**/*.usd', recursive=True): + stage = Usd.Stage.Open(path) + print(path, flush=True) + print(stage.GetRootLayer().ExportToString()[:2000], flush=True) + print('prims', len(list(stage.Traverse())), flush=True) + print([(str(p.GetPath()), p.GetTypeName()) for p in stage.Traverse() if 'Joint' in p.GetTypeName() or any(s in p.GetName().lower() for s in ('camera', 'lidar', 'mid360'))], flush=True) diff --git a/tests/isaaclab/publish_livox_sample.py b/tests/isaaclab/publish_livox_sample.py new file mode 100644 index 0000000..38e9597 --- /dev/null +++ b/tests/isaaclab/publish_livox_sample.py @@ -0,0 +1,19 @@ +"""Isaac-container half of CustomMsg interoperability test (no GPU).""" +import os +import time +import numpy as np +from src.python.sensors.livox_dds import LivoxPublisher +from src.python.sensors.ros_sensor_bridge import POINT_DTYPE + +domain = int(os.environ['ROS_DOMAIN_ID']) +assert 1 <= domain <= 232 +publisher = LivoxPublisher(domain) +points = np.zeros(2, dtype=POINT_DTYPE) +points['x'], points['y'], points['z'] = [1., 4.], [2., 5.], [3., 6.] +points['timestamp'], points['line'] = [0, 500], [0, 5] +try: + for _ in range(80): + publisher.publish(7, 123, points) + time.sleep(.1) +finally: + publisher.close() diff --git a/tests/isaaclab/run_in_container.sh b/tests/isaaclab/run_in_container.sh new file mode 100755 index 0000000..b21be2b --- /dev/null +++ b/tests/isaaclab/run_in_container.sh @@ -0,0 +1,14 @@ +#!/usr/bin/env bash +# Disposable test container using GOLEM's normal mounts and domain checks. +set -eo pipefail +ROOT=$(cd "$(dirname "$0")/../.." && pwd) +source "$ROOT/docker/scripts/docker_common.sh" +load_config +check_domain isaac +export GOLEM_DISPLAY=headless +configure_display +prepare_mounts isaac +if [ "$#" = 0 ]; then + set -- 'cd /home/code/CL_isaaclab_sim && PYTHONDONTWRITEBYTECODE=1 /isaac-sim/python.sh -m unittest discover -s /home/code/tests/isaaclab -p "test_*.py" -v' +fi +"${COMPOSE[@]}" run --rm -T --no-deps -v "$ROOT/tests:/home/code/tests:ro" --entrypoint bash isaac -lc "$*" diff --git a/tests/isaaclab/run_livox_interop.sh b/tests/isaaclab/run_livox_interop.sh new file mode 100755 index 0000000..3b77b0b --- /dev/null +++ b/tests/isaaclab/run_livox_interop.sh @@ -0,0 +1,28 @@ +#!/usr/bin/env bash +# No GPU simulation: Isaac CycloneDDS writer -> native Humble ROS subscriber. +set -eo pipefail +ROOT=$(cd "$(dirname "$0")/../.." && pwd) +source "$ROOT/docker/scripts/docker_common.sh" +load_config +check_domain isaac +export GOLEM_DISPLAY=headless +configure_display +prepare_mounts isaac +prepare_mounts ros +subscriber="isaac-livox-check-${ROS_DOMAIN_ID}-$$" +cleanup() { docker rm -f "$subscriber" >/dev/null 2>&1 || true; } +trap cleanup EXIT +"${COMPOSE[@]}" run -d --no-deps --name "$subscriber" -v "$ROOT/tests:/home/code/tests:ro" --entrypoint bash ros /home/code/tests/isaaclab/subscribe_livox_interop.sh >/dev/null +for attempt in $(seq 1 120); do + if docker logs "$subscriber" 2>&1 | grep -q 'SUBSCRIBER_READY'; then break; fi + if [ "$(docker inspect --format '{{.State.Running}}' "$subscriber")" != true ]; then + docker logs "$subscriber" + exit 1 + fi + sleep 1 +done +docker logs "$subscriber" 2>&1 | grep -q SUBSCRIBER_READY +"$ROOT/tests/isaaclab/run_in_container.sh" 'cd /home/code/CL_isaaclab_sim; PYTHONPATH=$PWD:$PYTHONPATH /isaac-sim/python.sh /home/code/tests/isaaclab/publish_livox_sample.py' +status=$(docker wait "$subscriber") +docker logs "$subscriber" +test "$status" = 0 diff --git a/tests/isaaclab/run_robot_interop.sh b/tests/isaaclab/run_robot_interop.sh new file mode 100755 index 0000000..23a3e2e --- /dev/null +++ b/tests/isaaclab/run_robot_interop.sh @@ -0,0 +1,42 @@ +#!/usr/bin/env bash +# Live GPU integration: native ROS commands -> Isaac physics -> native ROS state/sensors. +set -eo pipefail +ROOT=$(cd "$(dirname "$0")/../.." && pwd) +export ROS_DOMAIN_ID="${ROS_DOMAIN_ID:-191}" +source "$ROOT/docker/scripts/docker_common.sh" +load_config +check_domain isaac +export GOLEM_DISPLAY=headless +configure_display +prepare_mounts isaac +prepare_mounts ros +subscriber="isaac-robot-check-${ROS_DOMAIN_ID}-$$" +simulator="isaac-robot-sim-${ROS_DOMAIN_ID}-$$" +cleanup() { docker rm -f "$subscriber" "$simulator" >/dev/null 2>&1 || true; } +trap cleanup EXIT +"${COMPOSE[@]}" run -d --no-deps --name "$subscriber" -v "$ROOT/tests:/home/code/tests:ro" --entrypoint bash ros /home/code/tests/isaaclab/subscribe_robot_interop.sh >/dev/null +for attempt in $(seq 1 180); do + if docker logs "$subscriber" 2>&1 | grep -q SUBSCRIBER_READY; then break; fi + if [ "$(docker inspect --format '{{.State.Running}}' "$subscriber")" != true ]; then docker logs "$subscriber"; exit 1; fi + sleep 1 +done +docker logs "$subscriber" 2>&1 | grep -q SUBSCRIBER_READY +"${COMPOSE[@]}" run -d --no-deps --name "$simulator" --entrypoint bash isaac /home/code/h12_sim_scripts/launch_isaac.sh --headless --max_steps 400 >/dev/null +for attempt in $(seq 1 300); do + if [ "$(docker inspect --format '{{.State.Running}}' "$subscriber")" != true ]; then break; fi + if [ "$(docker inspect --format '{{.State.Running}}' "$simulator")" != true ]; then + docker logs "$simulator" + docker logs "$subscriber" + echo 'Simulator ended before ROS interoperability check completed' >&2 + exit 1 + fi + sleep 1 +done +docker logs "$subscriber" +if [ "$(docker inspect --format '{{.State.Running}}' "$subscriber")" = true ]; then + docker logs "$simulator" + echo 'ROS interoperability check exceeded 300 seconds' >&2 + exit 1 +fi +status=$(docker inspect --format '{{.State.ExitCode}}' "$subscriber") +if [ "$status" != 0 ]; then docker logs "$simulator"; exit "$status"; fi diff --git a/tests/isaaclab/run_sensor_transport.sh b/tests/isaaclab/run_sensor_transport.sh new file mode 100755 index 0000000..a4e5653 --- /dev/null +++ b/tests/isaaclab/run_sensor_transport.sh @@ -0,0 +1,4 @@ +#!/usr/bin/env bash +set -eo pipefail +ROOT=$(cd "$(dirname "$0")/../.." && pwd) +exec "$ROOT/tests/isaaclab/run_in_container.sh" 'export LD_LIBRARY_PATH=/workspace/golem/isaac/cyclonedds/install/lib:/isaac-sim/exts/isaacsim.ros2.bridge/humble/lib:$LD_LIBRARY_PATH; export GOLEM_TEST_ROS_TRANSPORT=1; export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp; export CYCLONEDDS_HOME=/workspace/golem/isaac/cyclonedds/install; cd /home/code/CL_isaaclab_sim; /isaac-sim/python.sh -m unittest discover -s /home/code/tests/isaaclab -p test_sensor_transport.py -v' diff --git a/tests/isaaclab/run_smoke.sh b/tests/isaaclab/run_smoke.sh new file mode 100755 index 0000000..0371757 --- /dev/null +++ b/tests/isaaclab/run_smoke.sh @@ -0,0 +1,10 @@ +#!/usr/bin/env bash +# NVIDIA integration: actual Isaac Lab construction, physics, sensors and cleanup. +set -eo pipefail +ROOT=$(cd "$(dirname "$0")/../.." && pwd) +mkdir -p "$ROOT/tests/results" +ROS_DOMAIN_ID=${ROS_DOMAIN_ID:-187} "$ROOT/tests/isaaclab/run_in_container.sh" \ + 'timeout --signal=INT --kill-after=30s 300s /home/code/h12_sim_scripts/launch_isaac.sh --headless --max_steps 50 --enable_profiling --profile_interval 25' \ + > "$ROOT/tests/results/isaac-smoke.log" 2>&1 +# A successful bounded run must reach normal cleanup; timeouts are failures. +grep -q '\[isaac\] completed 50 steps' "$ROOT/tests/results/isaac-smoke.log" diff --git a/tests/isaaclab/subscribe_livox_interop.sh b/tests/isaaclab/subscribe_livox_interop.sh new file mode 100755 index 0000000..1f69a70 --- /dev/null +++ b/tests/isaaclab/subscribe_livox_interop.sh @@ -0,0 +1,32 @@ +#!/usr/bin/env bash +# Build only the pinned interfaces in an ephemeral workspace; never the driver. +set -eo pipefail +source /opt/ros/humble/setup.bash +workspace=$(mktemp -d) +mkdir -p "$workspace/src/livox_ros_driver2/msg" +cp /home/code/core_ws/src/livox_ros_driver2/msg/Custom{Msg,Point}.msg "$workspace/src/livox_ros_driver2/msg/" +cat > "$workspace/src/livox_ros_driver2/package.xml" <<'XML' + + + livox_ros_driver20.0.0Wire test interfaces + GOLEM testMIT + ament_cmakerosidl_default_generators + std_msgsrosidl_default_runtime + rosidl_interface_packages + +XML +cat > "$workspace/src/livox_ros_driver2/CMakeLists.txt" <<'CMAKE' +cmake_minimum_required(VERSION 3.8) +project(livox_ros_driver2) +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(std_msgs REQUIRED) +rosidl_generate_interfaces(${PROJECT_NAME} "msg/CustomMsg.msg" "msg/CustomPoint.msg" DEPENDENCIES std_msgs) +ament_package() +CMAKE +cd "$workspace" +colcon build --packages-select livox_ros_driver2 --cmake-args -DPython3_EXECUTABLE=/usr/bin/python3 +source install/setup.bash +export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp +echo SUBSCRIBER_READY +exec /usr/bin/python3 /home/code/tests/isaaclab/check_livox_ros.py diff --git a/tests/isaaclab/subscribe_robot_interop.sh b/tests/isaaclab/subscribe_robot_interop.sh new file mode 100755 index 0000000..a1f973e --- /dev/null +++ b/tests/isaaclab/subscribe_robot_interop.sh @@ -0,0 +1,44 @@ +#!/usr/bin/env bash +# Native ROS checker; build only message interfaces if the image lacks them. +set -eo pipefail +source /opt/ros/humble/setup.bash +if [ -f /home/code/core_ws/install/setup.bash ]; then source /home/code/core_ws/install/setup.bash; fi +if ! /usr/bin/python3 -c 'from unitree_hg.msg import LowCmd, LowState; from unitree_go.msg import MotorCmds, MotorStates; from livox_ros_driver2.msg import CustomMsg' >/dev/null 2>&1; then + workspace=$(mktemp -d) + for package in unitree_hg unitree_go livox_ros_driver2; do + mkdir -p "$workspace/src/$package/msg" + if [ "$package" = unitree_hg ]; then + msg_root=/home/code/core_ws/src/unitree_ros2/cyclonedds_ws/src/unitree/unitree_hg/msg + for message in LowCmd LowState MotorCmd MotorState IMUState; do cp "$msg_root/$message.msg" "$workspace/src/$package/msg/"; done + elif [ "$package" = unitree_go ]; then + msg_root=/home/code/core_ws/src/unitree_ros2/cyclonedds_ws/src/unitree/unitree_go/msg + for message in MotorCmds MotorStates MotorCmd MotorState; do cp "$msg_root/$message.msg" "$workspace/src/$package/msg/"; done + else + cp /home/code/core_ws/src/livox_ros_driver2/msg/Custom{Msg,Point}.msg "$workspace/src/$package/msg/" + fi + cat > "$workspace/src/$package/package.xml" < +$package0.0.0Pinned wire test interfaces +GOLEM testMIT +ament_cmakerosidl_default_generators +std_msgsrosidl_default_runtime +rosidl_interface_packages +XML + cat > "$workspace/src/$package/CMakeLists.txt" <=10,<12 +cyclonedds==0.10.2 diff --git a/tests/robocasa/README.md b/tests/robocasa/README.md new file mode 100644 index 0000000..e253bd8 --- /dev/null +++ b/tests/robocasa/README.md @@ -0,0 +1,39 @@ +# Loop regression tests and timings + +Run the tests using the existing Linux RoboCasa image, with no rebuild: + +```bash +ROS_DOMAIN_ID=187 tests/robocasa/run_in_container.sh +``` + +The runner uses GOLEM's shared Docker preflight and a disposable container. It +leaves existing simulator containers alone. Tests use real MuJoCo EGL rendering +and ROS message types on a tiny fixed scene, without starting ROS nodes or DDS +participants. They cover pixel/payload equivalence, sensor scheduling, lock +boundaries, and pacing. The image must include the existing Livox messages. + +Pure pacing/timing tests also run without ROS or MuJoCo: + +```bash +PYTHONPATH=h1_robocasa python3 -m unittest discover -s tests/robocasa -p test_loop_timing.py -v +``` + +To measure the actual kitchen loop, add `--timing` to a normal launch: + +```bash +docker/scripts/docker_run.sh robocasa --headless --task OpenFridge --layout 1 --style 1 --seed 42 --timing +``` + +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; `camera_publish` includes image conversion, compression, and publishing. +`lidar_cast` includes scene capture and raycasting; `lidar_publish` includes +point filtering, message construction, and publishing. Physics includes lock +acquisition and PD control. Timings are silent unless requested. + +Compare the same task/layout/style/seed, display mode, subscribers, and hardware. +Discard the first interval for renderer warm-up. These changes keep per-step PD +and task checks, all sensor schedules, message formats, and topic definitions. +The pacer repays at most 100 ms of wall-clock lag without dropping simulation +steps. It does not make an overloaded scene run in real time. diff --git a/tests/robocasa/run_in_container.sh b/tests/robocasa/run_in_container.sh new file mode 100755 index 0000000..4794075 --- /dev/null +++ b/tests/robocasa/run_in_container.sh @@ -0,0 +1,12 @@ +#!/usr/bin/env bash +# Run simulator tests in a disposable container without replacing a user's sim. +set -eo pipefail +ROOT=$(cd "$(dirname "$0")/../.." && pwd) +source "$ROOT/docker/scripts/docker_common.sh" +load_config +check_domain robocasa +export GOLEM_DISPLAY=headless +configure_display +prepare_mounts robocasa +exec "${COMPOSE[@]}" run --rm -T --no-deps -v "$ROOT/tests:/home/code/tests:ro" -e MUJOCO_GL=egl robocasa bash -lc \ + 'source /opt/ros/humble/setup.bash && source /opt/livox_ws/install/setup.bash && cd /home/code/h1_robocasa && python -m unittest discover -s /home/code/tests/robocasa -v "$@"' bash "$@" diff --git a/tests/robocasa/test_loop_timing.py b/tests/robocasa/test_loop_timing.py new file mode 100644 index 0000000..ff00da5 --- /dev/null +++ b/tests/robocasa/test_loop_timing.py @@ -0,0 +1,89 @@ +import io +import unittest +from contextlib import redirect_stdout + +from sim_timing import RealtimePacer, LoopTimings + + +class FakeClock: + def __init__(self): + self.now = 0.0 + self.sleeps = [] + + def __call__(self): + return self.now + + def sleep(self, delay): + self.sleeps.append(delay) + self.now += delay + + +class PacingTests(unittest.TestCase): + def test_fast_steps_track_the_original_deadline(self): + clock = FakeClock() + pacer = RealtimePacer(0.01, clock=clock, sleep=clock.sleep) + for _ in range(10): + clock.now += 0.002 + pacer.wait() + self.assertAlmostEqual(clock.now, 0.1) + + def test_sensor_overrun_is_recovered_without_skipping_steps(self): + clock = FakeClock() + pacer = RealtimePacer(0.01, clock=clock, sleep=clock.sleep) + clock.now = 0.025 + pacer.wait() + pacer.wait() + self.assertEqual(clock.sleeps, []) + pacer.wait() + self.assertAlmostEqual(clock.now, 0.03) + self.assertAlmostEqual(clock.sleeps[0], 0.005) + + def test_long_pause_does_not_accumulate_unbounded_catchup(self): + clock = FakeClock() + pacer = RealtimePacer(0.01, max_lag=0.1, clock=clock, sleep=clock.sleep) + clock.now = 60.0 + pacer.wait() + for _ in range(12): + pacer.wait() + self.assertTrue(clock.sleeps) + self.assertGreater(clock.now, 60.0) + self.assertTrue(all(delay > 0 for delay in clock.sleeps)) + + def test_invalid_timestep_is_rejected(self): + for value in (0, -1, float('nan'), float('inf')): + with self.subTest(value=value), self.assertRaises(ValueError): + RealtimePacer(value) + + def test_timing_is_silent_by_default(self): + 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): + 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): + 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()) + + +if __name__ == '__main__': + unittest.main() diff --git a/tests/robocasa/test_sensor_bridge.py b/tests/robocasa/test_sensor_bridge.py new file mode 100644 index 0000000..2dd139b --- /dev/null +++ b/tests/robocasa/test_sensor_bridge.py @@ -0,0 +1,185 @@ +"""Real MuJoCo rendering and ROS messages; no ROS node or DDS traffic.""" +import io +import threading +import unittest +from types import SimpleNamespace +from unittest.mock import patch + +import mujoco +import numpy as np +from PIL import Image as PILImage +from rclpy.serialization import serialize_message +from sensor_msgs.msg import PointCloud2 + +from mujoco_ros_bridge import ( + RosSensorBridge, _CameraPub, _build_camera_info, _sim_time_to_msg, PC2_DTYPE, +) + +SCENE = ''' + + + + + + + + + + +''' + + +class Recorder: + def __init__(self, lock): + self.lock = lock + self.messages = [] + + def publish(self, msg): + if self.lock.locked(): + raise AssertionError('ROS publication must not hold the simulation lock') + self.messages.append(msg) + + +class SensorTests(unittest.TestCase): + def setUp(self): + self.bridge = object.__new__(RosSensorBridge) + b = self.bridge + b.model = mujoco.MjModel.from_xml_string(SCENE) + b.data = mujoco.MjData(b.model) + mujoco.mj_forward(b.model, b.data) + b.data.time = 1.25 + b.sim_lock = threading.Lock() + b.cam_width = b.cam_height = 32 + b.zfar = float(b.model.vis.map.zfar * b.model.stat.extent) + b._renderer = None + b.cam_period = 1 / 15 + b.lidar_period = .1 + b.imu_period = .005 + b.odom_period = .02 + b.base_body_id = -1 + b._last_cam_sim_t = b._last_lidar_sim_t = b._last_imu_sim_t = b._last_odom_sim_t = 0. + b._camera_specs = [('head', 'head', 'optical')] + pubs = [Recorder(b.sim_lock) for _ in range(5)] + b.cameras = [_CameraPub('head', 'optical', 0, + _build_camera_info(32, 32, 45, 'optical'), *pubs)] + b.pub_clock = Recorder(b.sim_lock) + b.get_logger = lambda: SimpleNamespace(warn=lambda text: self.fail(text)) + # Timing is optional for existing callers of the bridge. + try: + from sim_timing import LoopTimings + b.timings = LoopTimings() + except ImportError: + pass + self.addCleanup(b.shutdown) + + def test_one_scene_update_per_rgb_depth_pair_and_unlocked_render(self): + b = self.bridge + original_update = mujoco.Renderer.update_scene + original_render = mujoco.Renderer.render + updates = [] + + def update(renderer, *args, **kwargs): + self.assertTrue(b.sim_lock.locked()) + updates.append(kwargs['camera']) + return original_update(renderer, *args, **kwargs) + + def render(renderer, *args, **kwargs): + self.assertFalse(b.sim_lock.locked()) + return original_render(renderer, *args, **kwargs) + + with patch.object(mujoco.Renderer, 'update_scene', update), patch.object(mujoco.Renderer, 'render', render): + b._publish_camera_frame(_sim_time_to_msg(b.data.time)) + self.assertEqual(updates, [0]) + + def test_images_match_two_scene_update_reference(self): + b = self.bridge + stamp = _sim_time_to_msg(b.data.time) + b._publish_camera_frame(stamp) + cam = b.cameras[0] + rgb = cam.pub_rgb_raw.messages[0] + depth = cam.pub_depth_raw.messages[0] + # Reference path rebuilds the same scene separately for RGB and depth. + r = b._renderer + r.disable_depth_rendering() + r.update_scene(b.data, camera=cam.name, scene_option=b._scene_opt) + reference_rgb = r.render() + r.enable_depth_rendering() + r.update_scene(b.data, camera=cam.name, scene_option=b._scene_opt) + reference_depth = r.render() + invalid = ~np.isfinite(reference_depth) | (reference_depth <= 0) | (reference_depth >= .999 * b.zfar) + reference_mm = np.clip(reference_depth * 1000, 0, 65535).astype(np.uint16) + reference_mm[invalid] = 0 + self.assertEqual(bytes(rgb.data), reference_rgb.tobytes()) + self.assertEqual(bytes(depth.data), reference_mm.tobytes()) + compressed = cam.pub_depth.messages[0] + decoded = np.asarray(PILImage.open(io.BytesIO(bytes(compressed.data)[12:]))) + np.testing.assert_array_equal(decoded, reference_mm) + expected_jpeg = io.BytesIO() + PILImage.fromarray(reference_rgb, mode='RGB').save(expected_jpeg, format='JPEG', quality=80) + self.assertEqual(bytes(cam.pub_rgb.messages[0].data), expected_jpeg.getvalue()) + for pub in (cam.pub_rgb, cam.pub_depth, cam.pub_rgb_raw, cam.pub_depth_raw, cam.pub_info): + self.assertEqual(pub.messages[0].header.stamp, stamp) + self.assertEqual(pub.messages[0].header.frame_id, 'optical') + self.assertEqual(rgb.encoding, 'rgb8') + self.assertEqual(depth.encoding, '16UC1') + + def test_tick_preserves_sim_time_schedule_and_publication_order(self): + b = self.bridge + events = [] + b.pub_clock.publish = lambda msg: events.append(('clock', msg.clock.sec, msg.clock.nanosec)) + b._publish_imu = lambda stamp: events.append(('imu', stamp.sec, stamp.nanosec)) + b._publish_camera_frame = lambda stamp: events.append(('camera', stamp.sec, stamp.nanosec)) + b._publish_lidar_scan = lambda stamp: events.append(('lidar', stamp.sec, stamp.nanosec)) + b.tick() + self.assertEqual([e[0] for e in events], ['clock', 'imu', 'camera', 'lidar']) + self.assertTrue(all(e[1:] == (1, 250000000) for e in events)) + events.clear() + b.data.time = 1.251 + b.tick() + self.assertEqual([e[0] for e in events], ['clock']) + events.clear() + b.data.time = 1.256 + b.tick() + self.assertEqual([e[0] for e in events], ['clock', 'imu']) + + def test_lidar_capture_locked_but_both_messages_unlocked(self): + b = self.bridge + b.lidar_body_id = 1 + b.lidar_exclude_body_id = 1 + b.lidar_frame = 'lidar' + b.lidar_geomgroup = np.ones((6, 1), dtype=np.uint8) + b.lidar_min_range, b.lidar_max_range = .1, 40. + b.lidar_rays = 2 + b.lidar_local_dirs = np.array([[0., 0., -1.], [0., 0., 1.]]) + b.lidar_lines = np.array([0, 1], dtype=np.uint8) + b.lidar_offset_time_ns = np.array([0, 1000], dtype=np.uint32) + b._lidar_dists = np.empty((2, 1)) + b._lidar_geomids = np.empty((2, 1), dtype=np.int32) + b.geom_is_robot = np.zeros(b.model.ngeom, dtype=bool) + b.pub_lidar = Recorder(b.sim_lock) + b.pub_pc2 = Recorder(b.sim_lock) + original = mujoco.mj_multiRay + + def cast(*args): + self.assertTrue(b.sim_lock.locked()) + return original(*args) + + with patch.object(mujoco, 'mj_multiRay', cast): + b._publish_lidar_scan(_sim_time_to_msg(b.data.time)) + msg = b.pub_lidar.messages[0] + pc2 = b.pub_pc2.messages[0] + self.assertEqual(msg.timebase, 1250000000) + self.assertEqual(msg.point_num, 1) + self.assertAlmostEqual(msg.points[0].z, -1.) + points = np.frombuffer(bytes(pc2.data), dtype=PC2_DTYPE) + self.assertEqual(points.size, 1) + self.assertAlmostEqual(float(points['z'][0]), -1.) + self.assertEqual(pc2.header, msg.header) + reference = PointCloud2() + for field in pc2.get_fields_and_field_types(): + setattr(reference, field, bytes(pc2.data) if field == 'data' else getattr(pc2, field)) + self.assertEqual(serialize_message(pc2), serialize_message(reference)) + + +if __name__ == '__main__': + unittest.main() From 93209ef00e607a66a1780d165948e052bce0645d Mon Sep 17 00:00:00 2001 From: maxconway Date: Wed, 16 Sep 2026 16:08:18 -0600 Subject: [PATCH 002/225] Update supported submodule pins and ship the Magpie Isaac runtime --- .dockerignore | 9 + .github/workflows/simulator-tests.yml | 8 +- AGENTS.md | 5 +- CLAUDE.md | 5 +- CL_Assets | 2 +- CL_isaaclab_sim | 2 +- core_ws/src/h12_ros2_controller | 2 +- core_ws/src/h12_safety_layer | 2 +- docker/BUILD.md | 13 +- docker/IsaacDockerfile | 23 +++ docker/scripts/launch_isaac.sh | 1 - docker/scripts/package_humble_runtime.py | 38 ++++ docs/RUNNING.md | 6 +- tests/README.md | 28 ++- tests/isaaclab/check_magpie_sidecar.py | 29 +++ tests/isaaclab/check_robot_ros.py | 210 ++++++++++++++++++---- tests/isaaclab/inspect_robot_assets.py | 22 ++- tests/isaaclab/probe_magpie_services.py | 58 ++++++ tests/isaaclab/run_magpie_couplings.sh | 14 ++ tests/isaaclab/run_magpie_transport.sh | 11 ++ tests/isaaclab/run_robot_interop.sh | 6 +- tests/isaaclab/run_smoke.sh | 4 + tests/isaaclab/subscribe_robot_interop.sh | 18 +- tests/isaaclab/test_magpie_buffer.py | 41 +++++ tests/isaaclab/test_magpie_control.py | 32 ++++ tests/isaaclab/test_magpie_couplings.py | 50 ++++++ tests/isaaclab/test_robot_variant.py | 29 +++ 27 files changed, 595 insertions(+), 73 deletions(-) create mode 100644 docker/scripts/package_humble_runtime.py create mode 100644 tests/isaaclab/check_magpie_sidecar.py create mode 100644 tests/isaaclab/probe_magpie_services.py create mode 100755 tests/isaaclab/run_magpie_couplings.sh create mode 100755 tests/isaaclab/run_magpie_transport.sh create mode 100644 tests/isaaclab/test_magpie_buffer.py create mode 100644 tests/isaaclab/test_magpie_control.py create mode 100644 tests/isaaclab/test_magpie_couplings.py create mode 100644 tests/isaaclab/test_robot_variant.py diff --git a/.dockerignore b/.dockerignore index 3cd1027..de3f98e 100644 --- a/.dockerignore +++ b/.dockerignore @@ -5,6 +5,7 @@ # Whitelisted COPY sources: # RobocasaDockerfile -> core_ws/src/livox_ros_driver2 (baked-in livox build) # BaseDockerfile -> unitree_sdk2_python (local SDK install) +# IsaacDockerfile -> magpie_msgs + private Humble runtime packager * !core_ws core_ws/* @@ -15,3 +16,11 @@ core_ws/src/livox_ros_driver2/.git !unitree_sdk2_python !unitree_sdk2_python/** unitree_sdk2_python/.git + +!core_ws/src/magpie_msgs +core_ws/src/magpie_msgs/.git +!docker +docker/* +!docker/scripts +docker/scripts/* +!docker/scripts/package_humble_runtime.py diff --git a/.github/workflows/simulator-tests.yml b/.github/workflows/simulator-tests.yml index 2b87898..a83d241 100644 --- a/.github/workflows/simulator-tests.yml +++ b/.github/workflows/simulator-tests.yml @@ -54,6 +54,10 @@ jobs: lfs: true - name: Build simulation and ROS images run: docker/scripts/docker_build.sh isaac ros + - name: Magpie physical linkage schema + run: ROS_DOMAIN_ID=186 tests/isaaclab/run_magpie_couplings.sh + - name: Magpie native ROS services and actions + run: ROS_DOMAIN_ID=190 tests/isaaclab/run_magpie_transport.sh - name: Actual Isaac Lab scene and shutdown run: ROS_DOMAIN_ID=187 tests/isaaclab/run_smoke.sh - name: ROS sensor transport @@ -61,7 +65,9 @@ jobs: - name: Native ROS Livox interoperability run: ROS_DOMAIN_ID=189 tests/isaaclab/run_livox_interop.sh - name: ROS commands drive actual Isaac physics - run: ROS_DOMAIN_ID=191 tests/isaaclab/run_robot_interop.sh + run: ROS_DOMAIN_ID=191 tests/isaaclab/run_robot_interop.sh magpie + - name: Inspire variant regression + run: ROS_DOMAIN_ID=192 tests/isaaclab/run_robot_interop.sh inspire - uses: actions/upload-artifact@v4 if: always() with: diff --git a/AGENTS.md b/AGENTS.md index b69b5a6..460b3b6 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -206,8 +206,9 @@ elsewhere.) **Isaac (`CL_isaaclab_sim/sim_main.py`)** — publishes `rt/lowstate` and optional `rt/inspire/state` over CycloneDDS, `/clock`, three RealSense RGBD camera streams with camera info, and `/livox/{lidar,pointcloud,imu}`. Consumes -`rt/lowcmd` and optional `rt/inspire/cmd`. The current asset is the Inspire -model; Magpie hardware fidelity depends on the planned CL_Assets pin update. +`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. See `tests/README.md` for saved CPU and cross-container integration tests. ### Ground truth must NEVER be on by default diff --git a/CLAUDE.md b/CLAUDE.md index c12afd1..2bf03e2 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -206,8 +206,9 @@ elsewhere.) **Isaac (`CL_isaaclab_sim/sim_main.py`)** — publishes `rt/lowstate` and optional `rt/inspire/state` over CycloneDDS, `/clock`, three RealSense RGBD camera streams with camera info, and `/livox/{lidar,pointcloud,imu}`. Consumes -`rt/lowcmd` and optional `rt/inspire/cmd`. The current asset is the Inspire -model; Magpie hardware fidelity depends on the planned CL_Assets pin update. +`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. See `tests/README.md` for saved CPU and cross-container integration tests. ### Ground truth must NEVER be on by default diff --git a/CL_Assets b/CL_Assets index e65d24c..8572811 160000 --- a/CL_Assets +++ b/CL_Assets @@ -1 +1 @@ -Subproject commit e65d24c0be547e13978a509b2bddc0622cf5ca32 +Subproject commit 85728111e5d14307f77ea8c5f91f2c20981be4ec diff --git a/CL_isaaclab_sim b/CL_isaaclab_sim index 5d9231f..29615de 160000 --- a/CL_isaaclab_sim +++ b/CL_isaaclab_sim @@ -1 +1 @@ -Subproject commit 5d9231f59f9b746d46aac0430662a525fddce90f +Subproject commit 29615de6346a74a32674d4a78fba7148d11beca6 diff --git a/core_ws/src/h12_ros2_controller b/core_ws/src/h12_ros2_controller index 3508df0..ff1025f 160000 --- a/core_ws/src/h12_ros2_controller +++ b/core_ws/src/h12_ros2_controller @@ -1 +1 @@ -Subproject commit 3508df0d79b72952726fb32db629f8f9ed21231b +Subproject commit ff1025f387c848c5d1e8f5683a41dafbc6df9068 diff --git a/core_ws/src/h12_safety_layer b/core_ws/src/h12_safety_layer index d74166e..6bdfc66 160000 --- a/core_ws/src/h12_safety_layer +++ b/core_ws/src/h12_safety_layer @@ -1 +1 @@ -Subproject commit d74166e87dd6ac0d6454026fe04c699671eec947 +Subproject commit 6bdfc66b3329adfdc3f65d3dc3d8787c6ee6e505 diff --git a/docker/BUILD.md b/docker/BUILD.md index 8f52810..0d157ad 100644 --- a/docker/BUILD.md +++ b/docker/BUILD.md @@ -164,7 +164,8 @@ Single-stage, `FROM golem_base`. Provides the MuJoCo + RoboCasa kitchen simulato ## 6. `golem_sim_isaac` (`docker/IsaacDockerfile`) -Single-stage, based on `nvcr.io/nvidia/isaac-sim:5.1.0`. IsaacLab is checked out +The main stage is based on `nvcr.io/nvidia/isaac-sim:5.1.0`. A separate Humble +stage packages the Magpie gripper runtime and message interfaces. IsaacLab is checked out at `b4c321024792976150ca55fddb26fa34480d974e`; its `_isaac_sim` link points to `/isaac-sim`. Run simulator Python through that installation's `python.sh`, which sets Kit's required environment. The image installs CycloneDDS 0.10.x, @@ -176,7 +177,8 @@ to relocate them. Existing `CL_isaaclab_sim/.isaac_cache` contents may be copied there with containers stopped; otherwise caches regenerate on first launch. `launch_isaac.sh` accepts a task name or `--task NAME`, `--headless`, and -`--reset-cache`. Use `--` before additional simulator arguments. Output uses +`--reset-cache`, `--hand_type magpie|inspire`, and `--fix_base` for a bench +fixture. Magpie is the default floating-base robot. Output uses normal container stdout/stderr. The image has no shell entrypoint, so command overrides such as `docker_run.sh isaac bash` work normally. @@ -184,7 +186,12 @@ The simulator's Unitree DDS manager reads `ROS_DOMAIN_ID` (default 1), rejects 0, and communicates directly with the ROS stack on that domain. There is no cross-domain relay. This requires the accompanying `CL_isaaclab_sim` source change; distribute it through the submodule repository before updating the -superproject pin. +superproject pin. The Magpie gripper process uses private Humble Python 3.10 and the +same DDS domain, while the main simulator uses Isaac's bundled interpreter. +Humble's discovery schema matches the ROS container and bundled sensor bridge. +The original `magpie_msgs` source is built into the image; rebuild Isaac when +those interfaces change. The helper runtime leaves Isaac's Python and the +container's libc unchanged. --- diff --git a/docker/IsaacDockerfile b/docker/IsaacDockerfile index 57f35e9..7ed327d 100644 --- a/docker/IsaacDockerfile +++ b/docker/IsaacDockerfile @@ -1,5 +1,20 @@ #dockerfile june 2026, literally almost exactly a combination of isaaclab's dockerfiles, so shoutout them lol +# Build the gripper process against the same ROS distribution as GOLEM. +# Isaac's bundled CPython has a different ABI, so this runtime stays private. +FROM ros:humble-ros-base-jammy AS magpie_humble +RUN apt-get update && apt-get install -y --no-install-recommends \ + python3-colcon-common-extensions ros-humble-rmw-cyclonedds-cpp \ + ros-humble-rosidl-default-generators \ + && rm -rf /var/lib/apt/lists/* +COPY core_ws/src/magpie_msgs /tmp/magpie_msgs +RUN . /opt/ros/humble/setup.sh && cd /tmp && \ + colcon build --base-paths /tmp/magpie_msgs --packages-select magpie_msgs \ + --merge-install --install-base /opt/golem-magpie-humble --cmake-args -DBUILD_TESTING=OFF +COPY docker/scripts/package_humble_runtime.py /tmp/package_humble_runtime.py +RUN . /opt/ros/humble/setup.sh && . /opt/golem-magpie-humble/setup.sh && \ + python3 /tmp/package_humble_runtime.py + FROM nvcr.io/nvidia/isaac-sim:5.1.0 AS builder USER root @@ -118,3 +133,11 @@ WORKDIR ${GOLEM_WORKSPACE_PATH} ENTRYPOINT [] CMD ["/bin/bash"] + +COPY --from=magpie_humble /native-python /opt/golem-humble-python +COPY --from=magpie_humble /opt/ros/humble /opt/ros/humble +COPY --from=magpie_humble /opt/golem-magpie-humble /opt/golem-magpie-humble +RUN PYTHONHOME=/opt/golem-humble-python \ + PYTHONPATH=/opt/golem-humble-python/lib/python3.10/site-packages:/opt/ros/humble/local/lib/python3.10/dist-packages:/opt/ros/humble/lib/python3.10/site-packages:/opt/golem-magpie-humble/local/lib/python3.10/dist-packages:/opt/golem-magpie-humble/lib/python3.10/site-packages \ + LD_LIBRARY_PATH=/opt/golem-humble-python/lib:/opt/ros/humble/lib:/opt/golem-magpie-humble/lib \ + /opt/golem-humble-python/bin/python3.10 -c "from rclpy.action import ActionServer; from magpie_msgs.msg import GripperState; GripperState.__class__.__import_type_support__()" diff --git a/docker/scripts/launch_isaac.sh b/docker/scripts/launch_isaac.sh index 5f2bc33..027780c 100755 --- a/docker/scripts/launch_isaac.sh +++ b/docker/scripts/launch_isaac.sh @@ -63,6 +63,5 @@ exec "$ISAAC_PY" -u \ --device cuda \ --task "$TASK" \ $HEADLESS_FLAG \ - --enable_inspire_dds \ --enable_cameras \ --robot_type h1_2 "${EXTRA_ARGS[@]}" diff --git a/docker/scripts/package_humble_runtime.py b/docker/scripts/package_humble_runtime.py new file mode 100644 index 0000000..9077c4e --- /dev/null +++ b/docker/scripts/package_humble_runtime.py @@ -0,0 +1,38 @@ +"""Package a private Humble/Python runtime without replacing the host's libc.""" +from pathlib import Path +import re +import shutil +import subprocess + +root = Path('/native-python') +(root / 'bin').mkdir(parents=True) +(root / 'lib').mkdir() +shutil.copy2('/usr/bin/python3.10', root / 'bin/python3.10') +(root / 'bin/python3').symlink_to('python3.10') +shutil.copytree('/usr/lib/python3.10', root / 'lib/python3.10') +shutil.copytree('/usr/lib/python3/dist-packages', root / 'lib/python3.10/site-packages', dirs_exist_ok=True) +# The destination has a newer glibc. Keep its loader/libc family together; +# copy other native dependencies privately, including libddsc and Python modules. +glibc = re.compile(r'^(ld-linux.*|lib(c|m|pthread|dl|rt|util|resolv|anl)\.so(?:\..*)?)$') +inputs = [Path('/usr/bin/python3.10')] +for prefix in ('/opt/ros/humble', '/opt/golem-magpie-humble', '/usr/lib/python3.10', '/usr/lib/python3/dist-packages'): + inputs.extend(p for p in Path(prefix).rglob('*.so*') if p.is_file()) +seen = set() +while inputs: + binary = inputs.pop() + if binary in seen: + continue + seen.add(binary) + output = subprocess.run(['ldd', str(binary)], text=True, capture_output=True).stdout + for line in output.splitlines(): + if '=> not found' in line: + raise RuntimeError(f'{binary}: {line.strip()}') + paths = re.findall(r'(/[^\s()]+)', line) + for value in paths: + path = Path(value) + if glibc.match(path.name) or str(path).startswith('/opt/'): + continue + destination = root / 'lib' / path.name + if not destination.exists(): + shutil.copy2(path, destination) + inputs.append(path) diff --git a/docs/RUNNING.md b/docs/RUNNING.md index a3a1b4e..289b13d 100644 --- a/docs/RUNNING.md +++ b/docs/RUNNING.md @@ -72,7 +72,11 @@ Scene flags are passed straight through, e.g. Uses the same wrappers, on NVIDIA Linux only. The launcher honors `ROS_DOMAIN_ID` and communicates directly on that simulation domain; zero is rejected. Select a task with `docker_run.sh isaac --task base --headless`. -Use `--` before additional simulator options. Logs go to container stdout. +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. +The Magpie ROS gripper process starts inside the Isaac container using an +isolated Humble runtime. Its message interfaces are built into the image. Isaac's sensor/topic surface still differs from RoboCasa (see `CLAUDE.md`); starting both containers does not imply full navigation/skills parity. diff --git a/tests/README.md b/tests/README.md index 1412e31..832faca 100644 --- a/tests/README.md +++ b/tests/README.md @@ -44,12 +44,34 @@ shut down normally. Its log is `tests/results/isaac-smoke.log`; a timeout fails. `run_livox_interop.sh` checks that Isaac's CycloneDDS CustomMsg publisher is decoded by the ROS container's generated Humble Livox message type. -`run_robot_interop.sh` publishes native ROS LowCmd and Inspire commands, checks -wrist and right-index finger motion in both directions, receives LowState, -Inspire state and all 19 sensor topics, and releases body motors on exit. +`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. 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. The CPU suite cannot establish physical asset fidelity, real-time performance, or controller stability. Those need the GPU/ROS integration environment and robot-specific acceptance tests when the CL_Assets pin changes. + +## Robot variants + +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. + +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 +from the pinned `magpie_msgs` source. Rebuild Isaac after changing that interface +package. Humble is required for discovery compatibility with the ROS container. Physics and ROS exchange commands/state over a +private local socket; ROS callbacks do not access simulator tensors. + +The gripper aperture calibration and torque-to-force estimate are approximate, +as in RoboCasa. These tests establish control/transport, not calibrated grasp +forces or hardware equivalence under contact. + +```bash +ROS_DOMAIN_ID=191 tests/isaaclab/run_robot_interop.sh magpie +ROS_DOMAIN_ID=192 tests/isaaclab/run_robot_interop.sh inspire +``` diff --git a/tests/isaaclab/check_magpie_sidecar.py b/tests/isaaclab/check_magpie_sidecar.py new file mode 100644 index 0000000..9693fc5 --- /dev/null +++ b/tests/isaaclab/check_magpie_sidecar.py @@ -0,0 +1,29 @@ +"""Actual native ROS services/actions with synthetic physics, without a GPU.""" +from pathlib import Path +import subprocess +import time +from types import SimpleNamespace +from src.python.sensors.magpie_bridge import MagpieBridge +from src.python.sensors.ros_sensor_bridge import RosSensorBridge + +sensors = RosSensorBridge(SimpleNamespace(sensors={})) +bridge = MagpieBridge() +probe = None +try: + launcher = Path('/home/code/CL_isaaclab_sim/src/python/sensors/magpie_launch.sh') + probe = subprocess.Popen(['bash', str(launcher), '--script', '/home/code/tests/isaaclab/probe_magpie_services.py']) + started = time.monotonic() + while probe.poll() is None and time.monotonic() - started < 40: + commands = bridge.get_commands() + states = {side: dict(position=values['aperture'], force=0., finger_positions=[values['aperture']]*2, + is_moving=False, contact_detected=False) for side, values in commands.items()} + bridge.publish(time.monotonic() - started, states) + bridge.acknowledge(commands) + time.sleep(.02) + assert probe.poll() == 0, f'native ROS probe failed/timed out: {probe.poll()}' +finally: + if probe is not None and probe.poll() is None: + probe.terminate() + probe.wait(timeout=5) + bridge.close() + sensors.close() diff --git a/tests/isaaclab/check_robot_ros.py b/tests/isaaclab/check_robot_ros.py index 3e38287..3a17c4e 100644 --- a/tests/isaaclab/check_robot_ros.py +++ b/tests/isaaclab/check_robot_ros.py @@ -13,11 +13,165 @@ from rosgraph_msgs.msg import Clock from livox_ros_driver2.msg import CustomMsg from unitree_hg.msg import LowCmd, LowState -from unitree_go.msg import MotorCmd, MotorCmds, MotorStates from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowCmd_ from unitree_sdk2py.utils.crc import CRC +class InspireCheck: + def __init__(self, node): + from unitree_go.msg import MotorCmd, MotorCmds, MotorStates + self.MotorCmd, self.MotorCmds = MotorCmd, MotorCmds + self.publisher = node.create_publisher(MotorCmds, '/inspire/cmd', 10) + self.state = None + self.initial = None + self.subscription = node.create_subscription(MotorStates, '/inspire/state', self.receive, qos_profile_sensor_data) + + def receive(self, msg): + assert len(msg.states) == 12 and all(math.isfinite(m.q) for m in msg.states), 'invalid Inspire state' + self.state = msg + + def ready(self): + return self.state is not None + + @property + def actual(self): + return float(self.state.states[3].q) + + def start(self): + self.initial = [float(m.q) for m in self.state.states] + self.positions = self.initial.copy() + self.positions[3] = self.target = .95 + + def tick(self): + self.send(self.positions) + + def send(self, positions): + msg = self.MotorCmds() + for position in positions: + motor = self.MotorCmd() + motor.mode, motor.q = 1, float(position) + msg.cmds.append(motor) + self.publisher.publish(msg) + + def verify(self, stage): + assert abs(self.actual - self.target) < .03, f'Inspire target={self.target}, actual={self.actual}' + if stage == 0: + assert self.initial[3] - self.actual > .015, 'Inspire flex did not move' + self.first_actual = self.actual + else: + assert self.actual - self.first_actual > .015, 'Inspire reverse did not move' + + def reverse(self): + self.positions[3] = self.target = 1. + + def action_check(self): + return True + + def restore(self): + if self.initial is not None: + self.send(self.initial) + + +class MagpieCheck: + def __init__(self, node): + from rclpy.action import ActionClient + from magpie_msgs.msg import GripperState + from magpie_msgs.srv import SetGripperPosition, SetGripperForce + from magpie_msgs.action import DeliGrasp + from std_srvs.srv import Trigger + self.Position, self.DeliGrasp = SetGripperPosition, DeliGrasp + self.node = node + self.state, self.initial = {}, None + self.subscriptions, self.clients = [], {} + self.pending = [] + self.goal_future = self.result_future = None + self.state_sequence = 0 + self.result_state_sequence = None + for side in ('left', 'right'): + root = f'/{side}/gripper' + self.subscriptions.append(node.create_subscription(GripperState, root + '/state', lambda msg, s=side: self.receive(s, msg), qos_profile_sensor_data)) + for name, kind in (('set_position', SetGripperPosition), ('set_force', SetGripperForce), ('open', Trigger), ('close', Trigger), ('calibrate', Trigger), ('reset_parameters', Trigger)): + self.clients[(side, name)] = node.create_client(kind, root + '/' + name) + self.action = ActionClient(node, DeliGrasp, '/right/gripper/deligrasp') + self.left_action = ActionClient(node, DeliGrasp, '/left/gripper/deligrasp') + + def receive(self, side, msg): + assert all(math.isfinite(x) for x in [msg.position, msg.force] + list(msg.finger_positions)), 'invalid Magpie state' + self.state[side] = msg + if side == 'right': + self.state_sequence += 1 + + def ready(self): + return {'isaac_magpie_left', 'isaac_magpie_right'} <= set(self.node.get_node_names()) and len(self.state) == 2 and all(c.service_is_ready() for c in self.clients.values()) and self.action.server_is_ready() and self.left_action.server_is_ready() + + @property + def actual(self): + return float(self.state['right'].position) + + def request_position(self, position): + request = self.Position.Request() + request.position, request.speed = float(position), .3 + self.pending.append(self.clients[('right', 'set_position')].call_async(request)) + + def start(self): + self.initial = self.actual + self.target = self.initial - 10. if self.initial > 15. else self.initial + 10. + self.direction = 1. if self.target > self.initial else -1. + self.request_position(self.target) + + def tick(self): + for future in self.pending: + if future.done(): + response = future.result() + assert response.success, f'Magpie set_position rejected: {response.message}' + + def verify(self, stage): + assert self.pending[-1].done(), 'Magpie service response missing' + self.tick() + assert abs(self.actual - self.target) < 3., f'Magpie target={self.target}mm actual={self.actual}mm' + if stage == 0: + assert self.direction * (self.actual - self.initial) > 3., 'Magpie service did not move physical fingers' + self.first_actual = self.actual + else: + assert -self.direction * (self.actual - self.first_actual) > 3., 'Magpie reverse did not move physical fingers' + + def reverse(self): + self.target = self.initial + self.request_position(self.target) + + def action_check(self): + if self.goal_future is None: + goal = self.DeliGrasp.Goal() + self.action_target = max(0., self.initial - 5.) + goal.params.goal_aperture = self.action_target + goal.params.initial_force = 1. + goal.params.additional_closure = 0. + goal.params.additional_force = 0. + goal.params.complete_grasp = False + self.goal_future = self.action.send_goal_async(goal) + return False + if not self.goal_future.done(): + return False + handle = self.goal_future.result() + assert handle.accepted, 'DeliGrasp goal rejected' + if self.result_future is None: + self.result_future = handle.get_result_async() + if not self.result_future.done(): + return False + result = self.result_future.result() + assert result.status == 4 and result.result.success, f'DeliGrasp failed: {result.result.message}' + assert abs(result.result.final_aperture - self.action_target) < 3., 'DeliGrasp result aperture missed target' + # Action results and state topics arrive independently. Require a new + # measured state after the result, bounded by the checker's global deadline. + if self.result_state_sequence is None: + self.result_state_sequence = self.state_sequence + return self.state_sequence > self.result_state_sequence and abs(self.actual - self.action_target) < 3. + + def restore(self): + if self.initial is not None: + self.request_position(self.initial) + + def main(): domain = int(os.environ.get('ROS_DOMAIN_ID', '0')) if not 1 <= domain <= 232: @@ -25,7 +179,9 @@ def main(): rclpy.init() node = rclpy.create_node('isaac_robot_interop_check') publisher = node.create_publisher(LowCmd, '/lowcmd', 10) - hand_publisher = node.create_publisher(MotorCmds, '/inspire/cmd', 10) + hand_type = os.environ.get('ISAAC_HAND_TYPE', 'magpie') + assert hand_type in ('magpie', 'inspire'), hand_type + hand = MagpieCheck(node) if hand_type == 'magpie' else InspireCheck(node) crc = CRC() latest, seen, subscriptions = {}, set(), [] samples = [] @@ -43,24 +199,6 @@ def lowstate(msg): subscriptions.append(node.create_subscription(LowState, '/lowstate', lowstate, qos_profile_sensor_data)) - def handstate(msg): - assert len(msg.states) == 12, 'Inspire state must contain 12 motors' - assert all(math.isfinite(m.q) for m in msg.states), 'nonfinite Inspire state' - latest['hand'] = msg - - subscriptions.append(node.create_subscription(MotorStates, '/inspire/state', handstate, qos_profile_sensor_data)) - - def send_hand(positions): - if positions is None: - return - msg = MotorCmds() - for position in positions: - motor = MotorCmd() - motor.mode = 1 - motor.q = float(position) - msg.cmds.append(motor) - hand_publisher.publish(msg) - def sensor(topic, msg): if isinstance(msg, Image): assert msg.width > 0 and msg.height > 0 and len(msg.data) > 0, topic @@ -105,9 +243,6 @@ def send(targets=None): deadline = time.monotonic() + float(os.environ.get('ROBOT_INTEROP_TIMEOUT', '300')) initial = None - initial_hand = None - hand_targets = None - first_hand_actual = None stage = 0 start_sim = None targets = None @@ -117,51 +252,48 @@ def send(targets=None): print('SUBSCRIBER_READY', flush=True) while time.monotonic() < deadline: rclpy.spin_once(node, timeout_sec=.005) - if 'state' not in latest or 'sim_time' not in latest or 'hand' not in latest: + if 'state' not in latest or 'sim_time' not in latest or not hand.ready(): continue if initial is None: initial = [float(m.q) for m in latest['state'].motor_state[:27]] targets = initial.copy() targets[19] += .1 - initial_hand = [float(m.q) for m in latest['hand'].states] - hand_targets = initial_hand.copy() - hand_targets[3] = .95 + hand.start() start_sim = latest['sim_time'] - print(f'Initial wrist q={initial[19]:.6f}; target={targets[19]:.6f}; right index normalized q={initial_hand[3]:.6f}, target={hand_targets[3]:.6f}', flush=True) + print(f'Initial wrist q={initial[19]:.6f}; target={targets[19]:.6f}; {hand_type} actual={hand.actual:.6f}, target={hand.target:.6f}', flush=True) if latest['sim_time'] >= next_trace: motor = latest['state'].motor_state[19] - print(f"TRACE sim={latest['sim_time']:.3f} tick={latest['state'].tick} wrist19 q={motor.q:.6f} dq={motor.dq:.6f} tau_est={motor.tau_est:.6f} command(mode=1 q={targets[19]:.6f} dq=0 tau=0 kp=20 kd=2) right_index3 q={latest['hand'].states[3].q:.6f} target={hand_targets[3]:.6f}", flush=True) + print(f"TRACE sim={latest['sim_time']:.3f} tick={latest['state'].tick} wrist19 q={motor.q:.6f} dq={motor.dq:.6f} tau_est={motor.tau_est:.6f} command(mode=1 q={targets[19]:.6f} dq=0 tau=0 kp=20 kd=2) {hand_type} actual={hand.actual:.6f} target={hand.target:.6f}", flush=True) next_trace = latest['sim_time'] + .1 if time.monotonic() >= next_publish: send(targets) - send_hand(hand_targets) + hand.tick() next_publish = time.monotonic() + .01 if latest['sim_time'] - start_sim >= 1.0: actual = float(latest['state'].motor_state[19].q) assert abs(actual - targets[19]) < .06, f'wrist did not converge: target={targets[19]}, actual={actual}' - hand_actual = float(latest['hand'].states[3].q) - assert abs(hand_actual - hand_targets[3]) < .03, f'right index did not converge: target={hand_targets[3]}, actual={hand_actual}' + if stage < 2: + hand.verify(stage) if stage == 0: - assert initial_hand[3] - hand_actual > .015, 'Inspire flex command did not produce finger motion' - first_hand_actual = hand_actual - hand_targets[3] = 1.0 + hand.reverse() assert actual - initial[19] > .04, 'positive target did not produce positive physical motion' stage = 1 targets[19] = initial[19] start_sim = latest['sim_time'] print(f'Wrist moved to {actual:.6f}; reversing to {targets[19]:.6f}', flush=True) else: - assert hand_actual - first_hand_actual > .015, 'Inspire reverse command did not produce finger motion' + if stage == 1: + stage = 2 missing = set(required) - seen - if not missing: - print(f'PASS: ROS LowCmd and Inspire commands drove wrist and right index in both directions; {len(samples)} LowState samples; all {len(required)} sensor topics received', flush=True) + if not missing and hand.action_check(): + print(f'PASS: ROS LowCmd and {hand_type} commands drove wrist and gripper in both directions; services/actions verified; {len(samples)} LowState samples; all {len(required)} sensor topics received', flush=True) return raise AssertionError(f'timeout: stage={stage}, lowstate_samples={len(samples)}, missing_topics={sorted(set(required)-seen)}') finally: # Release motors even when an assertion or transport error aborts the test. for _ in range(10): send() - send_hand(initial_hand) + hand.restore() time.sleep(.01) node.destroy_node() rclpy.shutdown() diff --git a/tests/isaaclab/inspect_robot_assets.py b/tests/isaaclab/inspect_robot_assets.py index eb65481..2a53da7 100644 --- a/tests/isaaclab/inspect_robot_assets.py +++ b/tests/isaaclab/inspect_robot_assets.py @@ -1,10 +1,14 @@ -"""Inspect robot USD composition without starting Kit or physics.""" -import glob -from pxr import Usd +"""Inspect composed robot links, joints, limits and sensors without starting Kit.""" +import sys +from isaaclab.app import AppLauncher +app = AppLauncher(headless=True).app +from pxr import Usd, UsdPhysics -for path in glob.glob('/home/code/CL_Assets/isaac_assets/robots/h1_2_handless/**/*.usd', recursive=True): - stage = Usd.Stage.Open(path) - print(path, flush=True) - print(stage.GetRootLayer().ExportToString()[:2000], flush=True) - print('prims', len(list(stage.Traverse())), flush=True) - print([(str(p.GetPath()), p.GetTypeName()) for p in stage.Traverse() if 'Joint' in p.GetTypeName() or any(s in p.GetName().lower() for s in ('camera', 'lidar', 'mid360'))], flush=True) +path = sys.argv[1] if len(sys.argv) > 1 else '/home/code/CL_Assets/isaac_assets/robots/h1_2_magpie/h1_2_magpie.usd' +stage = Usd.Stage.Open(path) +print('ASSET', path, 'default', stage.GetDefaultPrim().GetPath(), flush=True) +for p in stage.Traverse(): + if p.HasAPI(UsdPhysics.ArticulationRootAPI) or p.HasAPI(UsdPhysics.RigidBodyAPI) or 'Joint' in p.GetTypeName(): + print(str(p.GetPath()), p.GetTypeName(), {a.GetName(): a.Get() for a in p.GetAttributes() if any(k in a.GetName() for k in ('Limit','axis','stiffness','damping','mimic'))}, p.GetAppliedSchemas(), flush=True) + +app.close() diff --git a/tests/isaaclab/probe_magpie_services.py b/tests/isaaclab/probe_magpie_services.py new file mode 100644 index 0000000..0d34738 --- /dev/null +++ b/tests/isaaclab/probe_magpie_services.py @@ -0,0 +1,58 @@ +"""Native ROS client assertions against a synthetic-physics Magpie sidecar.""" +import math +import time +import rclpy +from rclpy.action import ActionClient +from magpie_msgs.action import DeliGrasp +from magpie_msgs.msg import GripperState +from magpie_msgs.srv import SetGripperPosition, SetGripperForce + +rclpy.init() +node = rclpy.create_node('test_magpie_services') +states = {} +subscriptions = [node.create_subscription(GripperState, f'/{side}/gripper/state', lambda msg, s=side: states.__setitem__(s, msg), 10) for side in ('left', 'right')] + +def complete(future, timeout=15): + rclpy.spin_until_future_complete(node, future, timeout_sec=timeout) + assert future.done(), 'ROS request timed out' + return future.result() + +def wait_state(target): + deadline = time.monotonic() + 5 + while time.monotonic() < deadline: + rclpy.spin_once(node, timeout_sec=.1) + if 'left' in states and abs(states['left'].position - target) < .1: + return + raise AssertionError(f'measured state did not reach {target}') + +try: + position = node.create_client(SetGripperPosition, '/left/gripper/set_position') + force = node.create_client(SetGripperForce, '/left/gripper/set_force') + assert position.wait_for_service(timeout_sec=5) + wait_state(110.) + assert complete(position.call_async(SetGripperPosition.Request(position=40., speed=.5))).success + wait_state(40.) + assert not complete(position.call_async(SetGripperPosition.Request(position=float('nan'), speed=.5))).success + assert not complete(force.call_async(SetGripperForce.Request(max_force=-1.))).success + wait_state(40.) + action = ActionClient(node, DeliGrasp, '/left/gripper/deligrasp') + assert action.wait_for_server(timeout_sec=5) + goal = DeliGrasp.Goal() + goal.params.goal_aperture = 35. + goal.params.initial_force = 1. + goal.params.complete_grasp = False + handle = complete(action.send_goal_async(goal)) + assert handle.accepted + result = complete(handle.get_result_async()) + assert result.status == 4 and result.result.success and abs(result.result.final_aperture - 35.) < .1 + goal.params.complete_grasp = True + goal.params.additional_closure = 1. + handle = complete(action.send_goal_async(goal)) + assert handle.accepted + result = complete(handle.get_result_async()) + assert result.status == 6 and not result.result.success, 'no-contact grasp must abort' + assert result.result.final_force == 0. + print('PASS: native Magpie service ack, invalid commands, approach action, and no-contact abort') +finally: + node.destroy_node() + rclpy.shutdown() diff --git a/tests/isaaclab/run_magpie_couplings.sh b/tests/isaaclab/run_magpie_couplings.sh new file mode 100755 index 0000000..a622367 --- /dev/null +++ b/tests/isaaclab/run_magpie_couplings.sh @@ -0,0 +1,14 @@ +#!/usr/bin/env bash +# Real USD-schema validation without starting Kit or allocating GPU simulation. +set -eo pipefail +ROOT=$(cd "$(dirname "$0")/../.." && pwd) +exec "$ROOT/tests/isaaclab/run_in_container.sh" ' +set -e +usd_dirs=(/isaac-sim/extscache/omni.usd.libs-*.cp311) +physx_dirs=(/isaac-sim/extscache/omni.usd.schema.physx-*.cp311.*) +usd_schema=${usd_dirs[0]} +physx_schema=${physx_dirs[0]} +export PYTHONPATH="$usd_schema:$physx_schema:${PYTHONPATH:-}" +export LD_LIBRARY_PATH="$usd_schema/bin:$physx_schema/bin:${LD_LIBRARY_PATH:-}" +export PXR_PLUGINPATH_NAME="$physx_schema/plugins/PhysxSchema/resources:${PXR_PLUGINPATH_NAME:-}" +/isaac-sim/python.sh -m unittest discover -s /home/code/tests/isaaclab -p test_magpie_couplings.py -v' diff --git a/tests/isaaclab/run_magpie_transport.sh b/tests/isaaclab/run_magpie_transport.sh new file mode 100755 index 0000000..deb6faa --- /dev/null +++ b/tests/isaaclab/run_magpie_transport.sh @@ -0,0 +1,11 @@ +#!/usr/bin/env bash +# Mixed Isaac bundled-Humble and native-Humble graph regression, without GPU. +set -eo pipefail +ROOT=$(cd "$(dirname "$0")/../.." && pwd) +logfile=$(mktemp) +trap 'rm -f "$logfile"' EXIT +"$ROOT/tests/isaaclab/run_in_container.sh" 'export LD_LIBRARY_PATH=/workspace/golem/isaac/cyclonedds/install/lib:/isaac-sim/exts/isaacsim.ros2.bridge/humble/lib:$LD_LIBRARY_PATH; export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp; export CYCLONEDDS_HOME=/workspace/golem/isaac/cyclonedds/install; cd /home/code/CL_isaaclab_sim; PYTHONPATH=$PWD:$PYTHONPATH /isaac-sim/python.sh /home/code/tests/isaaclab/check_magpie_sidecar.py' 2>&1 | tee "$logfile" +if grep -E -q 'invalid data size|string data is not null-terminated|rcutils_set_error_state|Traceback|Segmentation fault|Fatal Python error|ModuleNotFoundError' "$logfile"; then + echo 'FAIL: mixed ROS runtimes reported a serialization/runtime error' >&2 + exit 1 +fi diff --git a/tests/isaaclab/run_robot_interop.sh b/tests/isaaclab/run_robot_interop.sh index 23a3e2e..0b032c9 100755 --- a/tests/isaaclab/run_robot_interop.sh +++ b/tests/isaaclab/run_robot_interop.sh @@ -2,6 +2,8 @@ # Live GPU integration: native ROS commands -> Isaac physics -> native ROS state/sensors. set -eo pipefail ROOT=$(cd "$(dirname "$0")/../.." && pwd) +hand_type="${1:-${ISAAC_HAND_TYPE:-magpie}}" +case "$hand_type" in magpie|inspire) ;; *) echo 'Expected magpie or inspire' >&2; exit 2 ;; esac export ROS_DOMAIN_ID="${ROS_DOMAIN_ID:-191}" source "$ROOT/docker/scripts/docker_common.sh" load_config @@ -14,14 +16,14 @@ subscriber="isaac-robot-check-${ROS_DOMAIN_ID}-$$" simulator="isaac-robot-sim-${ROS_DOMAIN_ID}-$$" cleanup() { docker rm -f "$subscriber" "$simulator" >/dev/null 2>&1 || true; } trap cleanup EXIT -"${COMPOSE[@]}" run -d --no-deps --name "$subscriber" -v "$ROOT/tests:/home/code/tests:ro" --entrypoint bash ros /home/code/tests/isaaclab/subscribe_robot_interop.sh >/dev/null +"${COMPOSE[@]}" run -d --no-deps --name "$subscriber" -e "ISAAC_HAND_TYPE=$hand_type" -v "$ROOT/tests:/home/code/tests:ro" --entrypoint bash ros /home/code/tests/isaaclab/subscribe_robot_interop.sh >/dev/null for attempt in $(seq 1 180); do if docker logs "$subscriber" 2>&1 | grep -q SUBSCRIBER_READY; then break; fi if [ "$(docker inspect --format '{{.State.Running}}' "$subscriber")" != true ]; then docker logs "$subscriber"; exit 1; fi sleep 1 done docker logs "$subscriber" 2>&1 | grep -q SUBSCRIBER_READY -"${COMPOSE[@]}" run -d --no-deps --name "$simulator" --entrypoint bash isaac /home/code/h12_sim_scripts/launch_isaac.sh --headless --max_steps 400 >/dev/null +"${COMPOSE[@]}" run -d --no-deps --name "$simulator" --entrypoint bash isaac /home/code/h12_sim_scripts/launch_isaac.sh --headless --hand_type "$hand_type" --fix_base --max_steps 800 >/dev/null for attempt in $(seq 1 300); do if [ "$(docker inspect --format '{{.State.Running}}' "$subscriber")" != true ]; then break; fi if [ "$(docker inspect --format '{{.State.Running}}' "$simulator")" != true ]; then diff --git a/tests/isaaclab/run_smoke.sh b/tests/isaaclab/run_smoke.sh index 0371757..4fee10d 100755 --- a/tests/isaaclab/run_smoke.sh +++ b/tests/isaaclab/run_smoke.sh @@ -8,3 +8,7 @@ ROS_DOMAIN_ID=${ROS_DOMAIN_ID:-187} "$ROOT/tests/isaaclab/run_in_container.sh" \ > "$ROOT/tests/results/isaac-smoke.log" 2>&1 # A successful bounded run must reach normal cleanup; timeouts are failures. grep -q '\[isaac\] completed 50 steps' "$ROOT/tests/results/isaac-smoke.log" +if grep -Eq 'invalid data size|string data is not null-terminated|rcutils_set_error_state' "$ROOT/tests/results/isaac-smoke.log"; then + echo 'ROS discovery or deserialization failed; see tests/results/isaac-smoke.log' >&2 + exit 1 +fi diff --git a/tests/isaaclab/subscribe_robot_interop.sh b/tests/isaaclab/subscribe_robot_interop.sh index a1f973e..9cb4d77 100755 --- a/tests/isaaclab/subscribe_robot_interop.sh +++ b/tests/isaaclab/subscribe_robot_interop.sh @@ -3,9 +3,9 @@ set -eo pipefail source /opt/ros/humble/setup.bash if [ -f /home/code/core_ws/install/setup.bash ]; then source /home/code/core_ws/install/setup.bash; fi -if ! /usr/bin/python3 -c 'from unitree_hg.msg import LowCmd, LowState; from unitree_go.msg import MotorCmds, MotorStates; from livox_ros_driver2.msg import CustomMsg' >/dev/null 2>&1; then +if ! /usr/bin/python3 -c 'from unitree_hg.msg import LowCmd, LowState; from unitree_go.msg import MotorCmds, MotorStates; from livox_ros_driver2.msg import CustomMsg; from magpie_msgs.msg import GripperState; from magpie_msgs.srv import SetGripperPosition, SetGripperForce; from magpie_msgs.action import DeliGrasp' >/dev/null 2>&1; then workspace=$(mktemp -d) - for package in unitree_hg unitree_go livox_ros_driver2; do + for package in unitree_hg unitree_go livox_ros_driver2 magpie_msgs; do mkdir -p "$workspace/src/$package/msg" if [ "$package" = unitree_hg ]; then msg_root=/home/code/core_ws/src/unitree_ros2/cyclonedds_ws/src/unitree/unitree_hg/msg @@ -13,6 +13,11 @@ if ! /usr/bin/python3 -c 'from unitree_hg.msg import LowCmd, LowState; from unit elif [ "$package" = unitree_go ]; then msg_root=/home/code/core_ws/src/unitree_ros2/cyclonedds_ws/src/unitree/unitree_go/msg for message in MotorCmds MotorStates MotorCmd MotorState; do cp "$msg_root/$message.msg" "$workspace/src/$package/msg/"; done + elif [ "$package" = magpie_msgs ]; then + mkdir -p "$workspace/src/$package/srv" "$workspace/src/$package/action" + cp /home/code/core_ws/src/magpie_msgs/msg/{GripperState,DeliGraspParams}.msg "$workspace/src/$package/msg/" + cp /home/code/core_ws/src/magpie_msgs/srv/{SetGripperPosition,SetGripperForce}.srv "$workspace/src/$package/srv/" + cp /home/code/core_ws/src/magpie_msgs/action/DeliGrasp.action "$workspace/src/$package/action/" else cp /home/code/core_ws/src/livox_ros_driver2/msg/Custom{Msg,Point}.msg "$workspace/src/$package/msg/" fi @@ -21,7 +26,7 @@ if ! /usr/bin/python3 -c 'from unitree_hg.msg import LowCmd, LowState; from unit $package0.0.0Pinned wire test interfaces GOLEM testMIT ament_cmakerosidl_default_generators -std_msgsrosidl_default_runtime +std_msgsaction_msgsrosidl_default_runtime rosidl_interface_packages XML cat > "$workspace/src/$package/CMakeLists.txt" < Date: Thu, 17 Sep 2026 10:53:27 -0600 Subject: [PATCH 003/225] Add ROS arm and lower-body simulator integration tests --- .github/workflows/simulator-tests.yml | 2 + .gitignore | 3 + tests/stack/README.md | 42 +++++ tests/stack/check_frame_task.py | 224 +++++++++++++++++++++++++ tests/stack/check_lowerbody.py | 203 ++++++++++++++++++++++ tests/stack/launch_ros_stack.sh | 39 +++++ tests/stack/minimal_stack.launch.py | 42 +++++ tests/stack/observe_simulator.py | 89 ++++++++++ tests/stack/run.sh | 73 ++++++++ tests/stack/run_checker.sh | 10 ++ tests/stack/test_frame_task_metrics.py | 42 +++++ tests/stack/test_lowerbody_scoring.py | 70 ++++++++ 12 files changed, 839 insertions(+) create mode 100644 tests/stack/README.md create mode 100644 tests/stack/check_frame_task.py create mode 100644 tests/stack/check_lowerbody.py create mode 100755 tests/stack/launch_ros_stack.sh create mode 100644 tests/stack/minimal_stack.launch.py create mode 100644 tests/stack/observe_simulator.py create mode 100755 tests/stack/run.sh create mode 100755 tests/stack/run_checker.sh create mode 100644 tests/stack/test_frame_task_metrics.py create mode 100644 tests/stack/test_lowerbody_scoring.py diff --git a/.github/workflows/simulator-tests.yml b/.github/workflows/simulator-tests.yml index a83d241..21bce2b 100644 --- a/.github/workflows/simulator-tests.yml +++ b/.github/workflows/simulator-tests.yml @@ -41,6 +41,8 @@ jobs: run: python -m unittest discover -s tests/robocasa -p test_loop_timing.py -v - name: Linux and Mac Docker workflow regressions run: python -m unittest discover -s tests/docker -v + - name: Controller integration acceptance scoring + run: python -m unittest discover -s tests/stack -p 'test_*.py' -v gpu-integration: # Explicit trusted runs on a prepared worker; CPU job is the portable PR gate. diff --git a/.gitignore b/.gitignore index ade1fe6..5f58a28 100644 --- a/.gitignore +++ b/.gitignore @@ -62,6 +62,9 @@ wandb/ # host-side caches for docker container build artefacts /container_cache/ +/core_ws/build_stack_tests/ +/core_ws/install_stack_tests/ +/core_ws/log_stack_tests/ # process crash dumps (e.g. h1_robocasa/core from unitree_sdk2py segfaults) core diff --git a/tests/stack/README.md b/tests/stack/README.md new file mode 100644 index 0000000..bf6acc3 --- /dev/null +++ b/tests/stack/README.md @@ -0,0 +1,42 @@ +# ROS controller integration tests + +Requires the built GOLEM ROS and corresponding simulator images, initialized +submodules and LFS assets, and an Ubuntu/NVIDIA Docker host. Run sequentially +on an isolated simulation domain: + +```bash +ROS_DOMAIN_ID=193 tests/stack/run.sh robocasa arms +ROS_DOMAIN_ID=193 tests/stack/run.sh robocasa locomotion +ROS_DOMAIN_ID=193 tests/stack/run.sh isaac arms +ROS_DOMAIN_ID=193 tests/stack/run.sh isaac locomotion +``` + +Each run builds the minimal actual ROS controller stack, waits for simulator +state before launching controllers, and saves logs, physical telemetry, and +JSON acceptance results under `tests/results/stack-*`. Nonzero exit means +failure, including startup failures and timeouts. Controllers cannot access +the private telemetry mount. The default wall timeout is 900 seconds; override +with `GOLEM_TEST_WALL_TIMEOUT` for slower workers. + +Arms move both wrist frames forward 2 cm and back through `/frame_task`. +Acceptance requires successful actions, measured end-effector progress and +joint motion, and advancing LowState ticks. Isaac uses a fixed base for this +isolated arm test; RoboCasa retains its normal support tether. + +Locomotion requires 5 simulation seconds of unsupported ALMI balance, +3 seconds of forward `/cmd_vel`, and 3 seconds stopping. The normal stack +switches ALMI → walking policy → ALMI; this does not establish that ALMI itself +walks. Tests check physical height, tilt, displacement, both knees, support +release, telemetry continuity, and active policies. No fixed base is allowed. +Isaac currently lacks RoboCasa's startup support tether, which may cause a +real startup/balance failure. Thresholds are in `check_lowerbody.py`. + +The scoring unit tests run in the GitHub CPU merge workflow: + +```bash +python3 -m unittest discover -s tests/stack -p 'test_*.py' -v +``` + +Live acceptance is **not yet validated** for either simulator. These runners +are available for trusted GPU workers; they are not enabled as mandatory live +merge gates until their end-to-end validation is complete. diff --git a/tests/stack/check_frame_task.py b/tests/stack/check_frame_task.py new file mode 100644 index 0000000..d7d2f5c --- /dev/null +++ b/tests/stack/check_frame_task.py @@ -0,0 +1,224 @@ +#!/usr/bin/env python3 +"""Exercise both arms through the real /frame_task server and save measured metrics. + +Contract: FrameTaskServer publishes /{left,right}_ee_pose in pelvis coordinates +(metres, quaternion XYZW). UpperController binds those poses to the wrist yaw +links. Unitree LowState slots 13:20 / 20:27 are the left / right arm motors. +The action server interprets goal.duration as WALL seconds; acceptance below +uses /clock simulation seconds with an independent wall timeout. +""" +import argparse +import copy +import json +import math +import os +from pathlib import Path +import time + + +def arm_metrics(start_pose, target_pose, pose, start_joints, joints): + """Evaluate measured motion; pose = XYZ followed by quaternion XYZW.""" + values = start_pose + target_pose + pose + start_joints + joints + if not all(math.isfinite(v) for v in values): + raise ValueError('Nonfinite arm pose or motor state') + delta = [target_pose[i] - start_pose[i] for i in range(3)] + distance = math.sqrt(sum(v*v for v in delta)) + if distance < 1e-6: + raise ValueError('Motion goal must differ from its measured start') + moved = [pose[i] - start_pose[i] for i in range(3)] + q1, q2 = target_pose[3:], pose[3:] + norm = math.sqrt(sum(v*v for v in q1) * sum(v*v for v in q2)) + if norm < 1e-9: + raise ValueError('Invalid zero quaternion') + dot = min(1., abs(sum(a*b for a, b in zip(q1, q2))) / norm) + return { + 'position_error_m': math.dist(target_pose[:3], pose[:3]), + 'displacement_m': math.sqrt(sum(v*v for v in moved)), + 'directed_progress_m': sum(a*b for a, b in zip(moved, delta)) / distance, + 'orientation_error_rad': 2 * math.acos(dot), + 'max_joint_change_rad': max(abs(a-b) for a, b in zip(joints, start_joints)), + 'target_distance_m': distance, + } + + +def motion_passes(metrics, position_tolerance=.01): + return (metrics['position_error_m'] <= position_tolerance + and metrics['directed_progress_m'] >= .4 * metrics['target_distance_m'] + and metrics['displacement_m'] >= .4 * metrics['target_distance_m'] + and metrics['orientation_error_rad'] <= .15 + and metrics['max_joint_change_rad'] >= .005) + + +def pose_values(pose): + return [pose.position.x, pose.position.y, pose.position.z, + pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w] + + +def main(): + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument('--output', type=Path, required=True) + parser.add_argument('--simulator', choices=('isaac', 'robocasa'), required=True) + parser.add_argument('--start-delay', type=float, default=float(os.environ.get('GOLEM_ARM_START_DELAY', '2')), + help='Simulation seconds after all endpoints become ready') + parser.add_argument('--sim-timeout', type=float, default=15., help='Maximum simulation seconds per action phase') + parser.add_argument('--wall-timeout', type=float, default=float(os.environ.get('GOLEM_TEST_WALL_TIMEOUT', '300'))) + parser.add_argument('--offset', type=float, default=.02, help='Pelvis-frame forward displacement in metres') + parser.add_argument('--position-tolerance', type=float, default=.01) + args = parser.parse_args() + if not 1 <= int(os.environ.get('ROS_DOMAIN_ID', '0')) <= 232: + parser.error('Set ROS_DOMAIN_ID to a nonzero simulation domain') + if not .01 <= args.offset <= .03: + parser.error('--offset must be 0.01..0.03 metres') + if not 0 < args.position_tolerance < args.offset: + parser.error('--position-tolerance must be positive and smaller than --offset') + if args.start_delay < 0 or min(args.sim_timeout, args.wall_timeout) <= 0: + parser.error('Timeouts must be positive and start delay nonnegative') + + import rclpy + from rclpy.action import ActionClient + from rclpy.qos import qos_profile_sensor_data + from geometry_msgs.msg import PoseStamped + from rosgraph_msgs.msg import Clock + from unitree_hg.msg import LowState + from custom_ros_messages.action import FrameTask + + started = time.monotonic() + deadline = started + args.wall_timeout + report = {'test': 'frame_task_both_arms', 'simulator': args.simulator, + 'domain': int(os.environ['ROS_DOMAIN_ID']), 'passed': False, + 'coordinate_frame': 'pelvis', 'frame_names': ['left_wrist_yaw_link', 'right_wrist_yaw_link'], + 'offset_m': args.offset, 'position_tolerance_m': args.position_tolerance, 'phases': []} + latest, counts = {}, {'left': 0, 'right': 0, 'joints': 0} + active_handle = None + rclpy.init() + node = rclpy.create_node('golem_frame_task_physics_check') + client = ActionClient(node, FrameTask, '/frame_task') + subscriptions = [] + + def clock(msg): + now = msg.clock.sec + msg.clock.nanosec * 1e-9 + if 'clock' in latest and now < latest['clock']: + raise AssertionError('Simulation clock reversed during arm test') + latest['clock'] = now + + def ee(side, msg): + if msg.header.frame_id != 'pelvis': + raise AssertionError(f'{side} EE frame must be pelvis, got {msg.header.frame_id}') + if not all(math.isfinite(v) for v in pose_values(msg.pose)): + raise AssertionError(f'{side} EE pose contains nonfinite values') + latest[side] = msg.pose + counts[side] += 1 + + def state(msg): + joints = [float(m.q) for m in msg.motor_state[:27]] + if len(joints) != 27 or not all(math.isfinite(v) for v in joints): + raise AssertionError('LowState must contain 27 finite motor positions') + if latest.get('tick') != int(msg.tick): + latest['tick_changed_at'] = time.monotonic() + latest['joints'] = joints + latest['tick'] = int(msg.tick) + counts['joints'] += 1 + + subscriptions.append(node.create_subscription(Clock, '/clock', clock, qos_profile_sensor_data)) + subscriptions.append(node.create_subscription(LowState, '/lowstate', state, qos_profile_sensor_data)) + for side in ('left', 'right'): + subscriptions.append(node.create_subscription(PoseStamped, f'/{side}_ee_pose', lambda msg, side=side: ee(side, msg), qos_profile_sensor_data)) + + def spin(): + if time.monotonic() >= deadline: + raise TimeoutError(f'Wall timeout after {args.wall_timeout}s; received {counts}') + rclpy.spin_once(node, timeout_sec=.02) + + try: + print('FRAME_TASK_CHECK_READY', flush=True) + while not (client.server_is_ready() and all(k in latest for k in ('left', 'right', 'joints', 'clock'))): + spin() + ready_time = latest['clock'] + while latest['clock'] - ready_time < args.start_delay: + spin() + original = {side: copy.deepcopy(latest[side]) for side in ('left', 'right')} + for phase_name in ('forward', 'return'): + start_poses = {side: pose_values(latest[side]) for side in ('left', 'right')} + start_joints = latest['joints'].copy() + targets = {side: copy.deepcopy(original[side]) for side in ('left', 'right')} + if phase_name == 'forward': + for pose in targets.values(): + pose.position.x += args.offset + goal = FrameTask.Goal() + goal.plan = False + goal.slow_mode = True + goal.frame_names = report['frame_names'] + goal.frame_targets = [targets['left'], targets['right']] + remaining = max(1., deadline - time.monotonic()) + goal.duration.sec = math.ceil(remaining) # Server timeout is wall time. + phase = {'name': phase_name, 'sim_start': latest['clock'], 'action_success': False, + 'starts': start_poses, 'targets': {s: pose_values(p) for s, p in targets.items()}} + report['phases'].append(phase) + feedback_samples = [] + def feedback(message): + feedback_samples.append({'linear_m': list(message.feedback.errors_linear), + 'angular_rad': list(message.feedback.errors_angular)}) + pending = client.send_goal_async(goal, feedback_callback=feedback) + result = None + result_counts = None + result_tick = None + stable_since = None + while True: + spin() + elapsed = latest['clock'] - phase['sim_start'] + if elapsed > args.sim_timeout: + raise TimeoutError(f'{phase_name} exceeded {args.sim_timeout} simulation seconds') + if active_handle is None and pending.done(): + active_handle = pending.result() + if not active_handle.accepted: + raise AssertionError(f'{phase_name} goal rejected') + result = active_handle.get_result_async() + if result is not None and result.done() and result_counts is None: + completed = result.result() + phase['action_status'] = completed.status + phase['action_success'] = bool(completed.result.success) + if completed.status != 4 or not completed.result.success: + raise AssertionError(f'{phase_name} action failed: status={completed.status}, success={completed.result.success}') + result_counts = counts.copy() + result_tick = latest['tick'] + metrics = {} + for side, joint_slice in (('left', slice(13, 20)), ('right', slice(20, 27))): + metrics[side] = arm_metrics(start_poses[side], pose_values(targets[side]), pose_values(latest[side]), + start_joints[joint_slice], latest['joints'][joint_slice]) + phase['metrics'] = metrics + phase['sim_elapsed_s'] = elapsed + phase['feedback_count'] = len(feedback_samples) + phase['last_feedback'] = feedback_samples[-1] if feedback_samples else None + fresh = (result_counts is not None + and latest['tick'] != result_tick + and time.monotonic() - latest.get('tick_changed_at', 0.) < 1. + and all(counts[k] > result_counts[k] for k in counts)) + passed = fresh and all(motion_passes(m, args.position_tolerance) for m in metrics.values()) + stable_since = latest['clock'] if passed and stable_since is None else stable_since if passed else None + if stable_since is not None and latest['clock'] - stable_since >= .2: + phase['passed'] = True + print(json.dumps({'phase': phase_name, 'metrics': metrics}), flush=True) + active_handle = None + break + report['passed'] = True + except Exception as exc: + report['error'] = f'{type(exc).__name__}: {exc}' + finally: + if active_handle is not None and active_handle.accepted: + future = active_handle.cancel_goal_async() + until = time.monotonic() + 2. + while not future.done() and time.monotonic() < until: + rclpy.spin_once(node, timeout_sec=.05) + report['wall_elapsed_s'] = time.monotonic() - started + report['sample_counts'] = counts + report['final_sim_time'] = latest.get('clock') + args.output.parent.mkdir(parents=True, exist_ok=True) + args.output.write_text(json.dumps(report, indent=2, allow_nan=False) + '\n') + print(json.dumps({'passed': report['passed'], 'output': str(args.output), 'error': report.get('error')}), flush=True) + node.destroy_node() + rclpy.shutdown() + return 0 if report['passed'] else 1 + + +if __name__ == '__main__': + raise SystemExit(main()) diff --git a/tests/stack/check_lowerbody.py b/tests/stack/check_lowerbody.py new file mode 100644 index 0000000..6b38124 --- /dev/null +++ b/tests/stack/check_lowerbody.py @@ -0,0 +1,203 @@ +#!/usr/bin/env python3 +"""Exercise ROS lower-body control; score private simulator telemetry offline. + +Telemetry is JSONL with sim_time, position[3], quaternion[WXYZ], +joint_positions[27] (Unitree wire order), and support_active (band OR fixed base). +It is never published to ROS. Run on an isolated simulation domain only. +""" +import argparse +import json +import math +import os +from pathlib import Path +import statistics +import time + + +LIMITS = dict(max_tilt_deg=25., minimum_height=.65, max_height_drop=.20, + balance_drift=.15, minimum_forward=.15, maximum_forward=1., + maximum_lateral=.20, minimum_knee_excursion=.08, + stopped_drift=.08, maximum_sample_gap=.25) + + +def validate_row(row): + for key, size in (('position', 3), ('quaternion', 4), ('joint_positions', 27)): + if len(row[key]) != size or not all(math.isfinite(x) for x in row[key]): + raise AssertionError(f'Invalid telemetry {key}') + if not math.isfinite(row['sim_time']) or row['sim_time'] < 0: + raise AssertionError('Invalid telemetry sim_time') + if type(row.get('support_active')) is not bool: + raise AssertionError('Telemetry must report actual support_active (band OR fixed base)') + norm = math.sqrt(sum(x*x for x in row['quaternion'])) + if not .99 <= norm <= 1.01: + raise AssertionError('Telemetry quaternion must be normalized WXYZ') + + +def tilt_deg(quaternion): + w, x, y, z = quaternion + return math.degrees(math.acos(max(-1., min(1., 1. - 2.*(x*x+y*y))))) + + +def distance(a, b): + return math.hypot(a['position'][0]-b['position'][0], a['position'][1]-b['position'][1]) + + +def score_run(rows, phases, policies, expected_walk_policy='walk'): + """Score a completed 5 s balance, 3 s walk, 3 s stop experiment.""" + selected = {} + for name, duration in (('balance', 5.), ('walk', 3.), ('stop', 3.)): + start, end = phases[name] + assert end - start >= duration - .001, f'{name} phase too short' + samples = [r for r in rows if start <= r['sim_time'] <= end] + assert samples, f'No {name} telemetry' + for row in samples: + validate_row(row) + assert not row['support_active'], f'{name}: external support still active' + stamps = [r['sim_time'] for r in samples] + gaps = [b-a for a, b in zip(stamps, stamps[1:])] + assert all(0 < g <= LIMITS['maximum_sample_gap'] for g in gaps), f'{name}: stale/nonmonotonic telemetry' + assert stamps[0]-start <= .25 and end-stamps[-1] <= .25, f'{name}: incomplete telemetry coverage' + assert max(tilt_deg(r['quaternion']) for r in samples) <= LIMITS['max_tilt_deg'], f'{name}: excessive tilt' + selected[name] = samples + initial = selected['balance'][0] + baseline = statistics.median(r['position'][2] for r in selected['balance'] if r['sim_time'] <= phases['balance'][0]+1.) + all_rows = sum(selected.values(), []) + minimum_height = min(r['position'][2] for r in all_rows) + assert minimum_height >= max(LIMITS['minimum_height'], baseline-LIMITS['max_height_drop']), 'Base collapsed or lost height' + balance_drift = max(distance(initial, r) for r in selected['balance']) + assert balance_drift <= LIMITS['balance_drift'], 'Standing drift exceeds limit' + + def policy_at(stamp): + eligible = [(t, p) for t, p in policies if t <= stamp] + assert eligible, 'Missing active_policy state' + return max(eligible, key=lambda pair: pair[0])[1] + for row in selected['balance']: + assert policy_at(row['sim_time']) == 'almi', 'ALMI not continuously active during balance' + # Handover is allowed for the first second; the last two seconds must run + # the requested locomotion policy. Stop must return to ALMI for its last second. + for phase, policy, settle in (('walk', expected_walk_policy, 1.), ('stop', 'almi', 2.)): + for row in selected[phase]: + if row['sim_time'] >= phases[phase][0]+settle: + assert policy_at(row['sim_time']) == policy, f'{phase}: expected active policy {policy}' + first, last = selected['walk'][0], selected['walk'][-1] + w, x, y, z = first['quaternion'] + yaw = math.atan2(2.*(w*z+x*y), 1.-2.*(y*y+z*z)) + dx, dy = [last['position'][i]-first['position'][i] for i in (0, 1)] + forward = dx*math.cos(yaw)+dy*math.sin(yaw) + lateral = -dx*math.sin(yaw)+dy*math.cos(yaw) + assert LIMITS['minimum_forward'] <= forward <= LIMITS['maximum_forward'], f'Forward displacement {forward:.3f} m outside limits' + assert abs(lateral) <= LIMITS['maximum_lateral'], f'Lateral deviation {lateral:.3f} m exceeds limit' + excursions = [max(r['joint_positions'][i] for r in selected['walk'])-min(r['joint_positions'][i] for r in selected['walk']) for i in (3, 9)] + assert min(excursions) >= LIMITS['minimum_knee_excursion'], 'Both knees must articulate during walking' + stopped = [r for r in selected['stop'] if r['sim_time'] >= phases['stop'][1]-1.] + stop_drift = max(distance(stopped[0], r) for r in stopped) + assert stop_drift <= LIMITS['stopped_drift'], 'Robot did not settle after zero cmd_vel' + return dict(baseline_height=baseline, minimum_height=minimum_height, + balance_drift=balance_drift, forward_displacement=forward, + lateral_displacement=lateral, knee_excursions=excursions, + stopped_drift=stop_drift, limits=LIMITS) + + +def main(): + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument('--telemetry', type=Path, required=True) + parser.add_argument('--output', type=Path, required=True) + parser.add_argument('--expected-walk-policy', choices=('walk', 'almi'), default='walk') + parser.add_argument('--confirm-engage', action='store_true', help='Call confirm_engage on the isolated simulation domain') + parser.add_argument('--wall-timeout', type=float, default=600.) + args = parser.parse_args() + domain = int(os.environ.get('ROS_DOMAIN_ID', '0')) + if not 1 <= domain <= 232: + parser.error('Set explicit isolated ROS_DOMAIN_ID in 1..232 (never hardware domain 0)') + if not math.isfinite(args.wall_timeout) or args.wall_timeout <= 0: + parser.error('wall timeout must be finite and positive') + import rclpy + from rclpy.qos import QoSProfile, DurabilityPolicy, ReliabilityPolicy, qos_profile_sensor_data + from geometry_msgs.msg import Twist + from rosgraph_msgs.msg import Clock + from std_msgs.msg import String + from std_srvs.srv import Trigger + from unitree_hg.msg import LowState + rclpy.init() + node = rclpy.create_node('lowerbody_stack_check') + publisher = node.create_publisher(Twist, '/cmd_vel', 10) + state = dict(clock=None, policy=None, lowstate_at=None, error=None) + policies, rows, phases = [], [], {} + def on_clock(msg): + stamp = msg.clock.sec+msg.clock.nanosec*1e-9 + if state['clock'] is not None and stamp < state['clock']: + state['error'] = 'Simulation clock reset during test' + state['clock'] = stamp + def on_policy(msg): + state['policy'] = msg.data + policies.append((state['clock'] if state['clock'] is not None else 0., msg.data)) + def on_state(msg): + state['lowstate_at'] = time.monotonic() + node.create_subscription(Clock, '/clock', on_clock, 10) + node.create_subscription(String, '/lowerbody/active_policy', on_policy, + QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL, reliability=ReliabilityPolicy.RELIABLE)) + node.create_subscription(LowState, '/lowstate', on_state, qos_profile_sensor_data) + confirm = node.create_client(Trigger, '/lowerbody/confirm_engage') + future = None + file_position, pending = 0, '' + deadline = time.monotonic()+args.wall_timeout + def pump(speed=0.): + nonlocal file_position, pending + assert time.monotonic() < deadline, 'Stack test wall timeout' + cmd = Twist(); cmd.linear.x = speed; publisher.publish(cmd) + rclpy.spin_once(node, timeout_sec=.02) + assert state['error'] is None, state['error'] + if args.telemetry.exists(): + with args.telemetry.open() as stream: + stream.seek(file_position) + pending += stream.read() + file_position = stream.tell() + while '\n' in pending: + line, pending = pending.split('\n', 1) + row = json.loads(line) + validate_row(row) + assert not rows or row['sim_time'] > rows[-1]['sim_time'], 'Telemetry reset/nonmonotonic timestamp' + rows.append(row) + report = dict(success=False, expected_walk_policy=args.expected_walk_policy, phases=phases) + try: + while True: + pump() + if args.confirm_engage and future is None and confirm.service_is_ready(): + future = confirm.call_async(Trigger.Request()) + if future is not None and future.done(): + response = future.result() + assert response is not None and response.success, f'Engage rejected: {response}' + if (state['clock'] is not None and state['policy'] == 'almi' and + state['lowstate_at'] is not None and time.monotonic()-state['lowstate_at'] < 2. and + rows and abs(rows[-1]['sim_time']-state['clock']) < .25 and not rows[-1]['support_active']): + break + for name, duration, speed in (('balance', 5., 0.), ('walk', 3., .15), ('stop', 3., 0.)): + start = state['clock'] + print(f'PHASE {name} sim_time={start:.3f} cmd_vx={speed}', flush=True) + while state['clock']-start < duration: + pump(speed) + assert time.monotonic()-state['lowstate_at'] < 2., 'LowState stopped arriving' + phases[name] = (start, state['clock']) + # Allow the final private telemetry record to flush without advancing a scored phase. + while not rows or rows[-1]['sim_time'] < phases['stop'][1]-.05: + pump() + report.update(score_run(rows, phases, policies, args.expected_walk_policy)) + report['success'] = True + print('LOWERBODY_STACK_PASS', flush=True) + except Exception as exc: + report['error'] = str(exc) + raise + finally: + # /cmd_vel is latched by the controller: explicit zero is mandatory. + for _ in range(5): + publisher.publish(Twist()) + rclpy.spin_once(node, timeout_sec=.02) + report['policies'] = policies + args.output.parent.mkdir(parents=True, exist_ok=True) + args.output.write_text(json.dumps(report, indent=2)+'\n') + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/tests/stack/launch_ros_stack.sh b/tests/stack/launch_ros_stack.sh new file mode 100755 index 0000000..20c223b --- /dev/null +++ b/tests/stack/launch_ros_stack.sh @@ -0,0 +1,39 @@ +#!/usr/bin/env bash +# Runs inside the ROS test container; only the actual controller dependency closure is built. +set -eo pipefail +case "${ROS_DOMAIN_ID:-}" in + ''|*[!0-9]*) echo 'Set an isolated ROS_DOMAIN_ID in 1..232' >&2; exit 2 ;; +esac +if (( 10#$ROS_DOMAIN_ID < 1 || 10#$ROS_DOMAIN_ID > 232 )); then + echo 'ROS_DOMAIN_ID must be in 1..232 for controller tests' >&2 + exit 2 +fi +export TEST_CASE=${TEST_CASE:-arms} +export SIMULATOR=${SIMULATOR:-isaac} +case "$TEST_CASE" in arms|locomotion) ;; *) echo 'TEST_CASE must be arms or locomotion' >&2; exit 2 ;; esac +case "$SIMULATOR" in isaac|robocasa) ;; *) echo 'SIMULATOR must be isaac or robocasa' >&2; exit 2 ;; esac +source /opt/ros/humble/setup.bash +export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp +export PYTHONUNBUFFERED=1 +WS=/home/code/core_ws +# An isolated install avoids inheriting stale interfaces or building MJPC/vision. +# Source locations stay canonical so h12_ros2_model resolves CL_Assets correctly. +# Keep build/ at the standard depth: h12_ros2_model resolves +# assets relative to setup.py even when colcon executes its build copy. +TEST_BUILD_BASE=$WS/build_stack_tests +TEST_INSTALL_BASE=$WS/install_stack_tests +cd "$WS" +colcon --log-base "$WS/log_stack_tests" build \ + --base-paths "$WS/src" \ + --build-base "$TEST_BUILD_BASE" \ + --install-base "$TEST_INSTALL_BASE" \ + --symlink-install \ + --packages-up-to h12_ros2_controller h12_safety_layer h12_lowerbody_rl unitree_hg unitree_go \ + --cmake-args -DBUILD_TESTING=OFF -DPython3_EXECUTABLE=/usr/bin/python3 +source "$TEST_INSTALL_BASE/setup.bash" +echo "STACK_BUILD_READY case=$TEST_CASE simulator=$SIMULATOR domain=$ROS_DOMAIN_ID" +if [ "${1:-}" = --build-only ]; then + exit 0 +fi +python3 /home/code/h12_sim_scripts/wait_for_sim.py +exec ros2 launch /home/code/tests/stack/minimal_stack.launch.py diff --git a/tests/stack/minimal_stack.launch.py b/tests/stack/minimal_stack.launch.py new file mode 100644 index 0000000..e2e54bb --- /dev/null +++ b/tests/stack/minimal_stack.launch.py @@ -0,0 +1,42 @@ +"""Actual upper-body/safety/ALMI stack for isolated simulator integration tests.""" +import os +from pathlib import Path + +from launch import LaunchDescription +from launch.actions import EmitEvent, RegisterEventHandler +from launch.event_handlers import OnProcessExit +from launch.events import Shutdown +from launch_ros.actions import Node + + +def generate_launch_description(): + domain = int(os.environ.get('ROS_DOMAIN_ID', '0')) + case = os.environ.get('TEST_CASE', 'arms') + simulator = os.environ.get('SIMULATOR', 'isaac') + if not 1 <= domain <= 232 or case not in ('arms', 'locomotion') or simulator not in ('isaac', 'robocasa'): + raise RuntimeError('Controller integration tests require an isolated domain and supported test case/simulator') + sim_time = {'use_sim_time': True} + nodes = [ + Node(package='h12_safety_layer', executable='safety_node', name='safety_node', + arguments=['--config', 'sim_safety_split.yaml'], parameters=[sim_time], output='screen'), + Node(package='h12_ros2_controller', executable='frame_task_server', name='frame_task_server', + arguments=['--config', 'sim_safety_split.yaml'], parameters=[sim_time], output='screen'), + Node(package='h12_ros2_controller', executable='joint_state_publisher', name='joint_state_publisher', + parameters=[sim_time], output='screen'), + Node(package='robot_state_publisher', executable='robot_state_publisher', name='robot_state_publisher', + parameters=[sim_time, {'robot_description': Path('/home/code/CL_Assets/ros_assets/h1_2_magpie_ros.urdf').read_text()}], + output='screen'), + ] + if case == 'locomotion': + nodes.append(Node( + package='h12_lowerbody_rl', executable='lowerbody_controller_node', name='lowerbody_controller_node', + parameters=[sim_time, { + 'active_policy': 'almi', + 'engage_wait_for_confirm': False, + 'imu_offset_roll_deg': 0., 'imu_offset_pitch_deg': 0., 'imu_offset_yaw_deg': 0., + }], output='screen')) + # A dead controller invalidates a test; never continue with a partial stack. + guards = [RegisterEventHandler(OnProcessExit(target_action=node, on_exit=[ + EmitEvent(event=Shutdown(reason='A required integration-test ROS node exited')), + ])) for node in nodes] + return LaunchDescription(guards + nodes) diff --git a/tests/stack/observe_simulator.py b/tests/stack/observe_simulator.py new file mode 100644 index 0000000..8053e3c --- /dev/null +++ b/tests/stack/observe_simulator.py @@ -0,0 +1,89 @@ +"""Test-only passive physics recorder. No privileged measurements enter ROS. + +Executed in place of the simulator entry point by run.sh, using the same +launcher, arguments, and environment. The real simulation loop still runs. +""" +import json +import os +from pathlib import Path +import runpy +import sys + + +class Recorder: + def __init__(self, filename): + self.stream = open(filename, 'w', buffering=1) + self.last_time = -1. + + def write(self, stamp, position, quaternion, joints, support): + # 50 Hz physical measurements, independent of rendering and ROS delivery. + if stamp - self.last_time < .019: + return + self.last_time = stamp + self.stream.write(json.dumps(dict(sim_time=float(stamp), + position=list(map(float, position)), quaternion=list(map(float, quaternion)), + joint_positions=list(map(float, joints)), support_active=bool(support))) + '\n') + + +def main(): + simulator = os.environ['SIMULATOR'] + recorder = Recorder(os.environ['GOLEM_TEST_TELEMETRY']) + if simulator == 'isaac': + entry = Path('/home/code/CL_isaaclab_sim/sim_main.py') + sys.path.insert(0, str(entry.parent)) + import gymnasium as gym + make = gym.make + + def observed_make(*args, **kwargs): + wrapper = make(*args, **kwargs) + env = wrapper.unwrapped + step = env.step + from src.python.control.motor_contract import BODY_JOINT_NAMES + robot = env.scene['robot'] + indices = [robot.joint_names.index(name) for name in BODY_JOINT_NAMES] + + def observed_step(*a, **kw): + result = step(*a, **kw) + recorder.write(env._sim_step_counter * env.physics_dt, + robot.data.root_pos_w[0].tolist(), robot.data.root_quat_w[0].tolist(), + robot.data.joint_pos[0, indices].tolist(), robot.is_fixed_base) + return result + + env.step = observed_step + return wrapper + + gym.make = observed_make + elif simulator == 'robocasa': + entry = Path('/home/code/h1_robocasa/h12_mujoco.py') + sys.path.insert(0, str(entry.parent)) + import mujoco + step = mujoco.mj_step + + def observed_step(model, data, *args, **kwargs): + result = step(model, data, *args, **kwargs) + # Only the production loop owns the resolver and band. Ignore + # robosuite's initialization/settling calls (which reset time). + caller = sys._getframe(1) + local = caller.f_locals + if caller.f_code.co_filename == str(entry) and 'band' in local: + resolver = local['resolver'] + body = resolver.body_id('pelvis') + if body < 0: + raise RuntimeError('Test recorder cannot find pelvis body') + band = local['band'] + recorder.write(data.time, data.xpos[body], data.xquat[body], + data.qpos[resolver.motor_qpos], band is not None and band.enabled) + return result + + mujoco.mj_step = observed_step + else: + raise ValueError(f'Unknown simulator: {simulator}') + sys.argv[0] = str(entry) + try: + runpy.run_path(str(entry), run_name='__main__') + finally: + recorder.stream.close() + + +if __name__ == '__main__': + main() diff --git a/tests/stack/run.sh b/tests/stack/run.sh new file mode 100755 index 0000000..dfad490 --- /dev/null +++ b/tests/stack/run.sh @@ -0,0 +1,73 @@ +#!/usr/bin/env bash +# Real ROS controllers -> CycloneDDS -> simulation -> measured acceptance. +set -eo pipefail +ROOT=$(cd "$(dirname "$0")/../.." && pwd) +SIMULATOR=${1:?Usage: run.sh isaac|robocasa arms|locomotion} +TEST_CASE=${2:?Usage: run.sh isaac|robocasa arms|locomotion} +case "$SIMULATOR" in isaac|robocasa) ;; *) exit 2 ;; esac +case "$TEST_CASE" in arms|locomotion) ;; *) exit 2 ;; esac +export ROS_DOMAIN_ID=${ROS_DOMAIN_ID:-193} +export GOLEM_TEST_WALL_TIMEOUT=${GOLEM_TEST_WALL_TIMEOUT:-900} +[[ "$GOLEM_TEST_WALL_TIMEOUT" =~ ^[1-9][0-9]*$ ]] || exit 2 +source "$ROOT/docker/scripts/docker_common.sh" +load_config +check_domain "$SIMULATOR" +export GOLEM_DISPLAY=headless +configure_display +prepare_mounts "$SIMULATOR" +prepare_mounts ros +result="$ROOT/tests/results/stack-$SIMULATOR-$TEST_CASE-$(date +%Y%m%dT%H%M%S)-$$" +mkdir -p "$result/telemetry" "$result/metrics" +# Root in Docker writes the artifacts; allow ordinary host users to inspect and remove them. +chmod 777 "$result/telemetry" "$result/metrics" +prefix="golem-stack-test-${ROS_DOMAIN_ID}-$$" +stack="$prefix-ros"; simulator="$prefix-sim"; checker="$prefix-check" +cleanup() { + local status=$? + trap - EXIT + for pair in "$stack:ros" "$simulator:simulator" "$checker:checker"; do + docker logs "${pair%:*}" >"$result/${pair##*:}.log" 2>&1 || true + docker rm -f "${pair%:*}" >/dev/null 2>&1 || true + done + echo "Stack test artifacts: $result" + exit "$status" +} +trap cleanup EXIT +env_args=(-e "SIMULATOR=$SIMULATOR" -e "TEST_CASE=$TEST_CASE" -e "GOLEM_TEST_WALL_TIMEOUT=$GOLEM_TEST_WALL_TIMEOUT") +tests_mount=(-v "$ROOT/tests/stack:/home/code/tests/stack:ro") +"${COMPOSE[@]}" run -d --no-deps --name "$stack" "${env_args[@]}" "${tests_mount[@]}" --entrypoint bash ros /home/code/tests/stack/launch_ros_stack.sh >/dev/null +for ((attempt=0; attempt<600; attempt++)); do + if docker logs "$stack" 2>&1 | grep STACK_BUILD_READY >/dev/null; then break; fi + [[ $(docker inspect -f '{{.State.Running}}' "$stack") == true ]] || { docker logs "$stack"; exit 1; } + sleep 1 +done +docker logs "$stack" 2>&1 | grep STACK_BUILD_READY >/dev/null || { echo 'ROS test build timed out' >&2; exit 1; } +# Retain every setting in the canonical launcher; replace only the Python entry +# point with the passive recorder. No production source is modified. +python3 - "$ROOT" "$SIMULATOR" "$result/launcher.sh" <<'PY' +from pathlib import Path +import sys +root, sim, output = sys.argv[1:] +source = (Path(root)/f'docker/scripts/launch_{sim}.sh').read_text() +old = '/home/code/CL_isaaclab_sim/sim_main.py' if sim == 'isaac' else 'h12_mujoco.py' +assert source.count(old) == 1, 'Canonical launcher changed; update test instrumentation' +Path(output).write_text(source.replace(old, '/home/code/tests/stack/observe_simulator.py')) +PY +sim_args=(--headless) +if [[ "$SIMULATOR" == isaac ]]; then + sim_args+=(--hand_type magpie) + # Arm kinematics is isolated from balance; locomotion always uses floating base. + [[ "$TEST_CASE" != arms ]] || sim_args+=(--fix_base) +fi +"${COMPOSE[@]}" run -d --no-deps --name "$checker" "${env_args[@]}" "${tests_mount[@]}" -v "$result/telemetry:/telemetry:ro" -v "$result/metrics:/results" --entrypoint bash ros /home/code/tests/stack/run_checker.sh >/dev/null +"${COMPOSE[@]}" run -d --no-deps --name "$simulator" "${env_args[@]}" "${tests_mount[@]}" -e GOLEM_TEST_TELEMETRY=/telemetry/state.jsonl -v "$result/telemetry:/telemetry" -v "$result/launcher.sh:/home/code/h12_sim_scripts/test_launcher.sh:ro" --entrypoint bash "$SIMULATOR" /home/code/h12_sim_scripts/test_launcher.sh "${sim_args[@]}" >/dev/null +for ((attempt=0; attempt&2; exit 1; } + done + sleep 1 +done +docker logs "$checker" +[[ $(docker inspect -f '{{.State.Running}}' "$checker") == false ]] || { echo 'Stack checker timed out' >&2; exit 1; } +exit "$(docker inspect -f '{{.State.ExitCode}}' "$checker")" diff --git a/tests/stack/run_checker.sh b/tests/stack/run_checker.sh new file mode 100755 index 0000000..2fa79fb --- /dev/null +++ b/tests/stack/run_checker.sh @@ -0,0 +1,10 @@ +#!/usr/bin/env bash +set -eo pipefail +source /opt/ros/humble/setup.bash +source /home/code/core_ws/install_stack_tests/setup.bash +export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp PYTHONUNBUFFERED=1 +case "$TEST_CASE" in + arms) exec python3 /home/code/tests/stack/check_frame_task.py --simulator "$SIMULATOR" --output /results/report.json --wall-timeout "$GOLEM_TEST_WALL_TIMEOUT" ;; + locomotion) exec python3 /home/code/tests/stack/check_lowerbody.py --telemetry /telemetry/state.jsonl --output /results/report.json --wall-timeout "$GOLEM_TEST_WALL_TIMEOUT" ;; + *) exit 2 ;; +esac diff --git a/tests/stack/test_frame_task_metrics.py b/tests/stack/test_frame_task_metrics.py new file mode 100644 index 0000000..d81dca7 --- /dev/null +++ b/tests/stack/test_frame_task_metrics.py @@ -0,0 +1,42 @@ +"""Pure scoring regressions: action acknowledgement alone cannot pass physics checks.""" +import importlib.util +from pathlib import Path +import unittest + +spec = importlib.util.spec_from_file_location('frame_check', Path(__file__).with_name('check_frame_task.py')) +module = importlib.util.module_from_spec(spec) +spec.loader.exec_module(module) + +class ArmMetricsTests(unittest.TestCase): + def setUp(self): + self.start = [0., .2, .3, 0., 0., 0., 1.] + self.goal = [.02, .2, .3, 0., 0., 0., 1.] + + def score(self, pose, joints): + return module.arm_metrics(self.start, self.goal, pose, [0.] * 7, joints) + + def test_ack_without_motion_fails(self): + self.assertFalse(module.motion_passes(self.score(self.start, [0.] * 7))) + + def test_pose_without_measured_joint_motion_fails(self): + self.assertFalse(module.motion_passes(self.score(self.goal, [0.] * 7))) + + def test_motion_away_from_target_fails(self): + wrong = self.goal.copy() + wrong[0] = -.02 + self.assertFalse(module.motion_passes(self.score(wrong, [.02] * 7))) + + def test_measured_pose_and_joint_progress_pass(self): + self.assertTrue(module.motion_passes(self.score(self.goal, [.02] * 7))) + + def test_quaternion_sign_is_equivalent(self): + pose = self.goal.copy() + pose[-1] = -1. + self.assertEqual(self.score(pose, [.02] * 7)['orientation_error_rad'], 0.) + + def test_nonfinite_state_is_rejected(self): + with self.assertRaises(ValueError): + self.score(self.goal, [float('nan')] * 7) + +if __name__ == '__main__': + unittest.main() diff --git a/tests/stack/test_lowerbody_scoring.py b/tests/stack/test_lowerbody_scoring.py new file mode 100644 index 0000000..a183cfc --- /dev/null +++ b/tests/stack/test_lowerbody_scoring.py @@ -0,0 +1,70 @@ +"""Synthetic truth cases for lower-body physical acceptance thresholds.""" +import copy +import math +import unittest +from check_lowerbody import score_run + + +def run_data(): + rows=[] + for step in range(551): + t=step*.02 + forward=max(0., min(.45, (t-5.)*.15)) + joints=[0.]*27 + if 5. <= t <= 8.: + joints[3]=.4+.1*math.sin((t-5.)*2*math.pi) + joints[9]=.4-.1*math.sin((t-5.)*2*math.pi) + rows.append(dict(sim_time=t, position=[forward,0.,1.], quaternion=[1.,0.,0.,0.], joint_positions=joints, support_active=False)) + return rows, dict(balance=(0.,5.),walk=(5.,8.),stop=(8.,11.)), [(0.,'almi'),(5.1,'walk'),(8.5,'almi')] + + +class LowerbodyScoringTests(unittest.TestCase): + def test_successful_walk_and_return_to_almi(self): + report=score_run(*run_data()) + self.assertAlmostEqual(report['forward_displacement'],.45) + def test_supported_robot_cannot_pass_balance(self): + rows,phases,policies=run_data();rows[10]['support_active']=True + with self.assertRaisesRegex(AssertionError,'support'): + score_run(rows,phases,policies) + def test_motion_without_bilateral_leg_articulation_fails(self): + rows,phases,policies=run_data() + for row in rows: row['joint_positions']=[0.]*27 + with self.assertRaisesRegex(AssertionError,'knees'): + score_run(rows,phases,policies) + def test_fall_fails_even_with_forward_progress(self): + rows,phases,policies=run_data();rows[320]['position'][2]=.5 + with self.assertRaisesRegex(AssertionError,'collapsed'): + score_run(rows,phases,policies) + def test_excess_tilt_fails(self): + rows,phases,policies=run_data();rows[320]['quaternion']=[math.cos(.4),math.sin(.4),0.,0.] + with self.assertRaisesRegex(AssertionError,'tilt'): + score_run(rows,phases,policies) + def test_stationary_command_echo_is_not_walking(self): + rows,phases,policies=run_data() + for row in rows: row['position'][0]=0. + with self.assertRaisesRegex(AssertionError,'Forward displacement'): + score_run(rows,phases,policies) + def test_must_return_to_almi_after_stop(self): + rows,phases,policies=run_data();policies.pop() + with self.assertRaisesRegex(AssertionError,'expected active policy almi'): + score_run(rows,phases,policies) + def test_direct_almi_locomotion_is_selectable(self): + rows,phases,_=run_data() + score_run(rows,phases,[(0.,'almi')],expected_walk_policy='almi') + def test_missing_telemetry_cannot_hide_fall(self): + rows,phases,policies=run_data();rows=[r for r in rows if not 6. < r['sim_time'] < 7.] + with self.assertRaisesRegex(AssertionError,'stale'): + score_run(rows,phases,policies) + def test_forward_scoring_follows_initial_heading(self): + rows,phases,policies=run_data() + for r in rows: + r['position'][1],r['position'][0]=r['position'][0],0. + r['quaternion']=[math.sqrt(.5),0.,0.,math.sqrt(.5)] + self.assertAlmostEqual(score_run(rows,phases,policies)['forward_displacement'],.45) + def test_missing_support_telemetry_is_error(self): + rows,phases,policies=run_data();del rows[10]['support_active'] + with self.assertRaisesRegex(AssertionError,'support_active'): + score_run(rows,phases,policies) + + +if __name__=='__main__': unittest.main() From 178befef1b05f6f8a1210109e62372e62b0a399d Mon Sep 17 00:00:00 2001 From: maxconway Date: Wed, 23 Sep 2026 11:41:47 -0600 Subject: [PATCH 004/225] Fix real-robot SLAM divergence: upright Livox IMU, one FAST-LIO config The MID360 is mounted inverted. The stock livox_ros_driver2 rotates the points by MID360_config.json's roll-180 extrinsic but publishes the IMU in the raw sensor frame, while mid360.yaml told FAST-LIO the two share a frame. On the real robot FAST-LIO diverged within seconds of walking (recorded bag: 36 km of drift over 146 s; the robot stayed within 2 m). - Add livox_imu_upright (h1_bringup), started only by h1_real_drivers.launch.py: the driver's IMU is remapped to /livox/imu_raw, rotated by the same extrinsic read from MID360_config.json, and republished on /livox/imu. It logs an error at startup if gravity still reads downward. This reproduces the only functional change in the correlllab/fast_lio_nav2_humanoid driver; the livox_ros_driver2 pin stays at 13eb05e. The sims publish an upright IMU directly and do not run it. - mid360.yaml is now the single FAST-LIO config for sim and real: the fast_lio_nav2_humanoid values, except 0.4/0.4 m voxels (0.5 loses registration on the sparse sim lidar; <=0.3 destabilises the real MID360 while walking), scan_line 6 (a line filter only; the real sensor emits lines 0-3, the sims 0-5) and pcd_save_en false. - slam_toolbox_h1.yaml is now the stock mapper_params_online_async values (slam_toolbox 2.6.10) that the reference stack loads. Validated by offline replay: the real bag passed 8/8 runs (0/5 before) including 2 at 1x rate; a RoboCasa walking loop tracks ground truth to 0.22 m RMS. Not yet validated on the robot's own computer or in Isaac. Co-Authored-By: Claude Opus 5.5 (1M context) --- CLAUDE.md | 2 +- core_ws/src/h1_bringup/config/mid360.yaml | 31 +++-- .../h1_bringup/config/slam_toolbox_h1.yaml | 77 +++++++----- .../h1_bringup/livox_imu_upright.py | 119 ++++++++++++++++++ .../launch/h1_real_drivers.launch.py | 14 +++ core_ws/src/h1_bringup/setup.py | 4 +- 6 files changed, 204 insertions(+), 43 deletions(-) create mode 100644 core_ws/src/h1_bringup/h1_bringup/livox_imu_upright.py diff --git a/CLAUDE.md b/CLAUDE.md index 2bf03e2..f1406cc 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -131,7 +131,7 @@ no `sim:=true` arg): | `h1_sim_bringup.launch.py` | **x86 sim** — full stack: nav (via include), robot/joint state publishers, `frame_task_server`, `safety_node`, gemini/sam servers, MJPC estimator + lowerbody controller, graspgen + skills (`use_skills`, default true), rviz (`use_rviz`) | | `h1_sim_bringup_mac.launch.py` | **mac sim** — trimmed: state publishers, `frame_task_server`, `safety_node`; lower body/SLAM/nav2 gated by `GOLEM_*` env vars, not launch args | | `h1_real_robot_bringup.launch.py` | **real robot, onboard PC** (native, `ROS_DOMAIN_ID=0`) — aggregates the three below-listed real files | -| `h1_real_drivers.launch.py` | real: Livox MID360, RealSense cams, left+right `gripper_node` | +| `h1_real_drivers.launch.py` | real: Livox MID360 (+ `livox_imu_upright`, which rotates `/livox/imu_raw` into the point frame — the stock driver rotates only points), RealSense cams, left+right `gripper_node` | | `h1_real_controller.launch.py` | real: estop, state publishers, staggered `safety_node` + `frame_task_server` | | `h1_real_desktop_bringup.launch.py` | real: companion x86 desktop — model servers, skills, MJPC controller. Leg control is interlocked behind `start_position_verified:=true` (default **false**) | | `h1_navigation.launch.py` | shared nav stack (FAST-LIO → pointcloud_to_laserscan → slam_toolbox → nav2); included by sim and real bringups | diff --git a/core_ws/src/h1_bringup/config/mid360.yaml b/core_ws/src/h1_bringup/config/mid360.yaml index ef24b78..5b56089 100644 --- a/core_ws/src/h1_bringup/config/mid360.yaml +++ b/core_ws/src/h1_bringup/config/mid360.yaml @@ -1,13 +1,25 @@ +# FAST-LIO for the H1-2 MID360, shared by the real robot and both simulators. +# Values are the real-verified reference stack's +# (correlllab/fast_lio_nav2_humanoid fast_lio_nav2_integration/config/mid360.yaml) +# except where noted inline. +# +# The identity extrinsic_R requires /livox/imu in the same upright frame as the +# points. The sims publish both from co-located upright sites. On the real robot +# the stock livox_ros_driver2 rotates only the points by MID360_config.json's +# extrinsic (roll 180, the lidar is mounted inverted), so h1_real_drivers.launch.py +# routes the IMU through livox_imu_upright. FAST-LIO seeds camera_init from the +# IMU attitude at startup, so an inverted IMU puts the whole map upside down. /**: ros__parameters: feature_extract_enable: false - point_filter_num: 1 # was 3 — sim LiDAR is already sparse (5 Hz, 180x28 rays); - # decimating 3:1 before scan-to-map registration starves - # FAST-LIO of correspondences and causes odometry drift. + point_filter_num: 3 max_iteration: 3 - filter_size_surf: 0.2 # was 0.5 — finer surf voxels retain more plane features - # per scan; pairs with point_filter_num above. - filter_size_map: 0.2 # was 0.5 — finer map voxels give tighter plane fits. + # 0.4 instead of the reference's 0.5: the sim lidar (180x28 rays) leaves + # too few points at 0.5 and FAST-LIO loses registration, while finer + # voxels (<= 0.3) destabilise the real MID360 while walking. Both + # bounds were measured by replaying real and RoboCasa walking bags. + filter_size_surf: 0.4 + filter_size_map: 0.4 cube_side_length: 1000.0 runtime_pos_log_enable: false map_file_path: "./test.pcd" @@ -21,8 +33,9 @@ preprocess: lidar_type: 1 # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ouster LiDAR, 4 for any other pointcloud input - scan_line: 6 - blind: 0.7 + scan_line: 6 # a filter only (line < scan_line) with feature_extract off; + # the real MID360 emits lines 0-3, the sims 0-5 + blind: 0.5 timestamp_unit: 3 scan_rate: 10 @@ -48,6 +61,6 @@ scan_bodyframe_pub_en: true # true: output the point cloud scans in IMU-body-frame pcd_save: - pcd_save_en: true + pcd_save_en: false # interval -1 buffers every frame in RAM until shutdown interval: -1 # how many LiDAR frames saved in each pcd file; # -1 : all frames will be saved in ONE pcd file, may lead to memory crash when having too much frames. \ No newline at end of file diff --git a/core_ws/src/h1_bringup/config/slam_toolbox_h1.yaml b/core_ws/src/h1_bringup/config/slam_toolbox_h1.yaml index 3f9618b..853c064 100644 --- a/core_ws/src/h1_bringup/config/slam_toolbox_h1.yaml +++ b/core_ws/src/h1_bringup/config/slam_toolbox_h1.yaml @@ -1,7 +1,13 @@ +# slam_toolbox for the H1-2, shared by the real robot and the simulators: the +# stock mapper_params_online_async.yaml (slam_toolbox 2.6.10, Humble) that the +# real-verified reference stack (correlllab/fast_lio_nav2_humanoid) loads, +# copied so a different installed slam_toolbox release cannot change it. +# Launch files override base_frame, odom_frame, scan_topic, odom_topic and +# use_sim_time; the frame/topic values below are fallbacks only. slam_toolbox: ros__parameters: - # Solver + # Plugin params solver_plugin: solver_plugins::CeresSolver ceres_linear_solver: SPARSE_NORMAL_CHOLESKY ceres_preconditioner: SCHUR_JACOBI @@ -9,62 +15,69 @@ slam_toolbox: ceres_dogleg_type: TRADITIONAL_DOGLEG ceres_loss_function: None - # Topics & frames are overridden in the launch file (base_frame, odom_frame, - # scan_topic, odom_topic, use_sim_time). Defaults below are fallbacks only. - odom_frame: camera_init + # ROS Parameters + odom_frame: odom map_frame: map - base_frame: pelvis - scan_topic: /converted_scan + base_frame: base_footprint + scan_topic: /scan use_map_saver: true - mode: mapping + mode: mapping #localization + + # if you'd like to immediately start continuing a map at a given pose + # or at the dock, but they are mutually exclusive, if pose is given + # will use pose + #map_file_name: test_steve + # map_start_pose: [0.0, 0.0, 0.0] + #map_start_at_dock: true - # Tuned for H1: small humanoid steps at ~0.25 m/s, MID-360 derived 2D scans - # at ~10 Hz. The upstream defaults (0.5 m / 0.5 rad) only add scans every - # 2+ seconds at our walking speed; we want more frequent updates. debug_logging: false throttle_scans: 1 - transform_publish_period: 0.05 - map_update_interval: 1.0 + transform_publish_period: 0.02 #if 0 never publishes odometry + map_update_interval: 5.0 resolution: 0.05 - max_laser_range: 20.0 - minimum_time_interval: 0.2 + min_laser_range: 0.0 #for rastering images + max_laser_range: 20.0 #for rastering images + minimum_time_interval: 0.5 transform_timeout: 0.2 - tf_buffer_duration: 30.0 - stack_size_to_use: 40000000 + tf_buffer_duration: 30. + stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps enable_interactive_mode: true - # Mapper params + # General Parameters use_scan_matching: true use_scan_barycenter: true - minimum_travel_distance: 0.10 - minimum_travel_heading: 0.10 + minimum_travel_distance: 0.5 + minimum_travel_heading: 0.5 scan_buffer_size: 10 scan_buffer_maximum_scan_distance: 10.0 - link_match_minimum_response_fine: 0.1 + link_match_minimum_response_fine: 0.1 link_scan_maximum_distance: 1.5 loop_search_maximum_distance: 3.0 - do_loop_closing: true - loop_match_minimum_chain_size: 10 - loop_match_maximum_variance_coarse: 3.0 - loop_match_minimum_response_coarse: 0.35 + do_loop_closing: true + loop_match_minimum_chain_size: 10 + loop_match_maximum_variance_coarse: 3.0 + loop_match_minimum_response_coarse: 0.35 loop_match_minimum_response_fine: 0.45 - # Correlation Parameters + # Correlation Parameters - Correlation Parameters correlation_search_space_dimension: 0.5 correlation_search_space_resolution: 0.01 - correlation_search_space_smear_deviation: 0.1 + correlation_search_space_smear_deviation: 0.1 - # Loop Closure Parameters + # Correlation Parameters - Loop Closure Parameters loop_search_space_dimension: 8.0 loop_search_space_resolution: 0.05 loop_search_space_smear_deviation: 0.03 # Scan Matcher Parameters - distance_variance_penalty: 0.5 - angle_variance_penalty: 1.0 - fine_search_angle_offset: 0.00349 - coarse_search_angle_offset: 0.349 - coarse_angle_resolution: 0.0349 + distance_variance_penalty: 0.5 + angle_variance_penalty: 1.0 + + fine_search_angle_offset: 0.00349 + coarse_search_angle_offset: 0.349 + coarse_angle_resolution: 0.0349 minimum_angle_penalty: 0.9 minimum_distance_penalty: 0.5 use_response_expansion: true + min_pass_through: 2 + occupancy_threshold: 0.1 diff --git a/core_ws/src/h1_bringup/h1_bringup/livox_imu_upright.py b/core_ws/src/h1_bringup/h1_bringup/livox_imu_upright.py new file mode 100644 index 0000000..02106e7 --- /dev/null +++ b/core_ws/src/h1_bringup/h1_bringup/livox_imu_upright.py @@ -0,0 +1,119 @@ +"""Rotate the MID360 IMU into the frame livox_ros_driver2 publishes points in. + +livox_ros_driver2 applies the MID360_config.json ``extrinsic_parameter`` +rotation to the point cloud but publishes the IMU in the raw sensor frame. On +the H1-2 the lidar is mounted inverted (roll 180), so the two streams disagree +by 180 deg of roll. FAST-LIO (mid360.yaml, identity extrinsic_R) needs +them in one frame, and seeds camera_init from the IMU attitude at startup, so +the IMU must arrive upright or the whole map comes out upside down. + +The rotation is read from the same JSON file the driver loads and built with +the driver's convention, R = Rz(yaw) * Ry(pitch) * Rx(roll) +(livox_ros_driver2 src/comm/pub_handler.cpp, SetLidarsExtParam), so points and +IMU stay consistent if the extrinsic is ever changed. + +Subscribes ``imu_in`` (sensor_msgs/Imu), publishes ``imu_out``. +Parameter ``livox_config_path``: path to the driver's MID360_config.json. +""" + +import json +import math + +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import Imu + +GRAVITY_CHECK_SAMPLES = 200 # ~1 s at the MID360's 200 Hz IMU rate +GRAVITY_MIN_Z = 0.5 # g; upright with the torso's ~14 deg lidar pitch reads ~0.98 + + +def driver_rotation(roll_deg, pitch_deg, yaw_deg): + cr, sr = math.cos(math.radians(roll_deg)), math.sin(math.radians(roll_deg)) + cp, sp = math.cos(math.radians(pitch_deg)), math.sin(math.radians(pitch_deg)) + cy, sy = math.cos(math.radians(yaw_deg)), math.sin(math.radians(yaw_deg)) + return ( + (cp * cy, sr * sp * cy - cr * sy, cr * sp * cy + sr * sy), + (cp * sy, sr * sp * sy + cr * cy, cr * sp * sy - sr * cy), + (-sp, sr * cp, cr * cp), + ) + + +def load_rotation(config_path): + with open(config_path) as f: + cfg = json.load(f) + configs = cfg['lidar_configs'] + if len(configs) != 1: + raise ValueError( + f'{config_path}: expected exactly one lidar_configs entry, ' + f'got {len(configs)}') + ext = configs[0]['extrinsic_parameter'] + return driver_rotation(ext['roll'], ext['pitch'], ext['yaw']) + + +def rotate(r, v): + x, y, z = v.x, v.y, v.z + v.x = r[0][0] * x + r[0][1] * y + r[0][2] * z + v.y = r[1][0] * x + r[1][1] * y + r[1][2] * z + v.z = r[2][0] * x + r[2][1] * y + r[2][2] * z + + +class LivoxImuUpright(Node): + + def __init__(self): + super().__init__('livox_imu_upright') + config_path = self.declare_parameter('livox_config_path', '').value + if not config_path: + raise ValueError('livox_config_path parameter is required') + self._r = load_rotation(config_path) + self.get_logger().info( + f'rotating IMU by driver extrinsic from {config_path}: {self._r}') + # Depth matches FAST-LIO's IMU subscription so bursts are buffered, + # not dropped, on the way through. + self._pub = self.create_publisher(Imu, 'imu_out', 200000) + self.create_subscription(Imu, 'imu_in', self._on_imu, 200000) + self._check_acc_z = [] + + def _on_imu(self, msg): + rotate(self._r, msg.angular_velocity) + rotate(self._r, msg.linear_acceleration) + if self._check_acc_z is not None: + self._check_gravity(msg.linear_acceleration) + # The driver never fills orientation (it stays the all-zero default), + # so there is nothing to rotate there. + self._pub.publish(msg) + + def _check_gravity(self, acc): + # FAST-LIO initialises from the first ~1 s of IMU with the robot + # standing, so gravity should read along +z (units of g) once rotated. + # A negative mean means the IMU is still inverted relative to the + # points and FAST-LIO will build an upside-down map. + self._check_acc_z.append(acc.z) + if len(self._check_acc_z) < GRAVITY_CHECK_SAMPLES: + return + mean_z = sum(self._check_acc_z) / len(self._check_acc_z) + self._check_acc_z = None + if mean_z < GRAVITY_MIN_Z: + self.get_logger().error( + f'IMU z acceleration averages {mean_z:.2f} g after rotation; ' + f'expected about +1 g with the robot upright. The IMU and ' + f'point frames disagree, check MID360_config.json ' + f'extrinsic_parameter and mid360.yaml extrinsic_R.') + else: + self.get_logger().info( + f'IMU gravity check passed: z averages {mean_z:.2f} g') + + +def main(): + rclpy.init() + node = LivoxImuUpright() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.try_shutdown() + + +if __name__ == '__main__': + main() diff --git a/core_ws/src/h1_bringup/launch/h1_real_drivers.launch.py b/core_ws/src/h1_bringup/launch/h1_real_drivers.launch.py index 045ff00..c5e6641 100644 --- a/core_ws/src/h1_bringup/launch/h1_real_drivers.launch.py +++ b/core_ws/src/h1_bringup/launch/h1_real_drivers.launch.py @@ -35,6 +35,19 @@ def generate_launch_description(): name='livox_lidar_publisher', output='screen', parameters=livox_mid360_params, + # The driver rotates points by the MID360_config.json extrinsic but + # not the IMU; livox_imu_upright applies the same rotation and owns + # /livox/imu (see mid360.yaml). + remappings=[('/livox/imu', '/livox/imu_raw')], + ) + + livox_imu_upright = Node( + package='h1_bringup', + executable='livox_imu_upright', + name='livox_imu_upright', + output='screen', + parameters=[{'livox_config_path': mid360_config}], + remappings=[('imu_in', '/livox/imu_raw'), ('imu_out', '/livox/imu')], ) # Equivalent of `ros2 launch cl_realsense h12_headcamera.launch.py` — brings @@ -59,6 +72,7 @@ def generate_launch_description(): return LaunchDescription([ livox_mid360, + livox_imu_upright, realsense_cams, grippers, ]) diff --git a/core_ws/src/h1_bringup/setup.py b/core_ws/src/h1_bringup/setup.py index eebf791..8a42bd2 100644 --- a/core_ws/src/h1_bringup/setup.py +++ b/core_ws/src/h1_bringup/setup.py @@ -31,6 +31,8 @@ ], }, entry_points={ - 'console_scripts': [], + 'console_scripts': [ + 'livox_imu_upright = h1_bringup.livox_imu_upright:main', + ], }, ) From 462c479e962248a4dca692e0a99731e515f93d96 Mon Sep 17 00:00:00 2001 From: maxconway Date: Wed, 23 Sep 2026 12:33:04 -0600 Subject: [PATCH 005/225] Lock build inputs, fix core_ws cache correctness, extend stack tests Reproducible builds: - Pin external base images by digest, git clones to exact commits, the uv installer version, and IsaacLab's rl_games/robomimic VCS extras. - Select dated Ubuntu and ROS snapshot archives (docker/locks/apt-sources.sh) with a vendored ROS snapshot key; disable inherited live CUDA APT lists. - Constrain every pip/uv install with docker/locks/*.txt and verify the full RoboCasa asset tree against a recorded digest. - Correct combinations the old images held inconsistently: ROS uses Pinocchio 2.7 / cmeel-boost 1.83 / OpenCV 4.10 on NumPy 1.26 and removes the base image's NumPy-2 cmeel closure first; Isaac keeps its bundled torch 2.7 / psutil 5.9.8 with stable-baselines3 2.8.0, ipython 9.10.1, opencv-python 4.11 and pandas 2.2.3. - Copy a CA bundle from a pinned Jammy image into the bare arm64 base so the HTTPS snapshots work with TLS verification on. Provenance and remaining gaps are in docker/locks/SOURCES.md. core_ws cache: launch_ros.sh always runs incremental colcon and propagates failures, builds a staged copy of livox_ros_driver2 instead of its build.sh (which deletes caches and masks errors), refuses stale installed packages, and stops back-dating MJPC sources. Tests: run_all.sh, native heartbeat-clock regression, prompt lower-body collapse detection, cameras off by default and image IDs in run.json. Bumps h12_safety_layer (heartbeat on steady time) and CL_isaaclab_sim (physical reset on env reset). All four live stack cases still fail; see the stack results for evidence. Co-Authored-By: Claude Opus 5.5 (1M context) --- .dockerignore | 2 + .github/workflows/simulator-tests.yml | 9 +- CL_isaaclab_sim | 2 +- core_ws/src/h12_safety_layer | 2 +- docker/BUILD.md | 8 +- docker/BaseDockerfile | 20 +- docker/IsaacDockerfile | 43 ++-- docker/RobocasaDockerfile | 32 ++- docker/RosDockerfile | 44 ++-- docker/locks/SOURCES.md | 120 ++++++++++ docker/locks/apt-sources.sh | 32 +++ docker/locks/base-amd64-01.txt | 4 + docker/locks/base-amd64-02.txt | 4 + docker/locks/base-amd64-03.txt | 4 + docker/locks/base-amd64.txt | 121 ++++++++++ docker/locks/base-arm64-01.txt | 4 + docker/locks/base-arm64-02.txt | 4 + docker/locks/base-arm64-03.txt | 4 + docker/locks/base-arm64.txt | 102 +++++++++ docker/locks/isaac-amd64-01.txt | 4 + docker/locks/isaac-amd64-02.txt | 4 + docker/locks/isaac-amd64-03.txt | 4 + docker/locks/isaac-amd64-04.txt | 4 + docker/locks/isaac-amd64.txt | 268 ++++++++++++++++++++++ docker/locks/robocasa-amd64-01.txt | 5 + docker/locks/robocasa-amd64-02.txt | 5 + docker/locks/robocasa-amd64-03.txt | 5 + docker/locks/robocasa-amd64-04.txt | 5 + docker/locks/robocasa-amd64.txt | 246 ++++++++++++++++++++ docker/locks/robocasa-arm64-01.txt | 5 + docker/locks/robocasa-arm64-02.txt | 5 + docker/locks/robocasa-arm64-03.txt | 5 + docker/locks/robocasa-arm64-04.txt | 5 + docker/locks/robocasa-arm64.txt | 212 +++++++++++++++++ docker/locks/robocasa-assets.digest | 1 + docker/locks/ros-amd64-01.txt | 5 + docker/locks/ros-amd64-02.txt | 5 + docker/locks/ros-amd64-03.txt | 5 + docker/locks/ros-amd64-04.txt | 5 + docker/locks/ros-amd64-05.txt | 5 + docker/locks/ros-amd64-06.txt | 7 + docker/locks/ros-amd64-07.txt | 6 + docker/locks/ros-amd64-08.txt | 5 + docker/locks/ros-amd64-09.txt | 5 + docker/locks/ros-amd64-10.txt | 5 + docker/locks/ros-amd64-11.txt | 5 + docker/locks/ros-amd64-12.txt | 7 + docker/locks/ros-amd64.txt | 312 ++++++++++++++++++++++++++ docker/locks/ros-arm64-01.txt | 5 + docker/locks/ros-arm64-02.txt | 7 + docker/locks/ros-arm64.txt | 293 ++++++++++++++++++++++++ docker/locks/ros-snapshot.asc | 139 ++++++++++++ docker/mac/BaseDockerfile.arm64 | 24 +- docker/mac/RobocasaDockerfile.arm64 | 32 ++- docker/mac/RosDockerfile.slim.arm64 | 7 +- docker/scripts/launch_ros.sh | 134 ++++++----- tests/docker/test_launch_ros_cache.py | 259 +++++++++++++++++++++ tests/docker/test_source_pins.py | 92 ++++++++ tests/isaaclab/test_physical_reset.py | 34 +++ tests/stack/README.md | 11 +- tests/stack/check_lowerbody.py | 8 + tests/stack/launch_ros_stack.sh | 1 + tests/stack/run.sh | 27 ++- tests/stack/run_all.sh | 13 ++ tests/stack/test_heartbeat_clock.py | 38 ++++ 65 files changed, 2690 insertions(+), 155 deletions(-) create mode 100644 docker/locks/SOURCES.md create mode 100644 docker/locks/apt-sources.sh create mode 100644 docker/locks/base-amd64-01.txt create mode 100644 docker/locks/base-amd64-02.txt create mode 100644 docker/locks/base-amd64-03.txt create mode 100644 docker/locks/base-amd64.txt create mode 100644 docker/locks/base-arm64-01.txt create mode 100644 docker/locks/base-arm64-02.txt create mode 100644 docker/locks/base-arm64-03.txt create mode 100644 docker/locks/base-arm64.txt create mode 100644 docker/locks/isaac-amd64-01.txt create mode 100644 docker/locks/isaac-amd64-02.txt create mode 100644 docker/locks/isaac-amd64-03.txt create mode 100644 docker/locks/isaac-amd64-04.txt create mode 100644 docker/locks/isaac-amd64.txt create mode 100644 docker/locks/robocasa-amd64-01.txt create mode 100644 docker/locks/robocasa-amd64-02.txt create mode 100644 docker/locks/robocasa-amd64-03.txt create mode 100644 docker/locks/robocasa-amd64-04.txt create mode 100644 docker/locks/robocasa-amd64.txt create mode 100644 docker/locks/robocasa-arm64-01.txt create mode 100644 docker/locks/robocasa-arm64-02.txt create mode 100644 docker/locks/robocasa-arm64-03.txt create mode 100644 docker/locks/robocasa-arm64-04.txt create mode 100644 docker/locks/robocasa-arm64.txt create mode 100644 docker/locks/robocasa-assets.digest create mode 100644 docker/locks/ros-amd64-01.txt create mode 100644 docker/locks/ros-amd64-02.txt create mode 100644 docker/locks/ros-amd64-03.txt create mode 100644 docker/locks/ros-amd64-04.txt create mode 100644 docker/locks/ros-amd64-05.txt create mode 100644 docker/locks/ros-amd64-06.txt create mode 100644 docker/locks/ros-amd64-07.txt create mode 100644 docker/locks/ros-amd64-08.txt create mode 100644 docker/locks/ros-amd64-09.txt create mode 100644 docker/locks/ros-amd64-10.txt create mode 100644 docker/locks/ros-amd64-11.txt create mode 100644 docker/locks/ros-amd64-12.txt create mode 100644 docker/locks/ros-amd64.txt create mode 100644 docker/locks/ros-arm64-01.txt create mode 100644 docker/locks/ros-arm64-02.txt create mode 100644 docker/locks/ros-arm64.txt create mode 100644 docker/locks/ros-snapshot.asc create mode 100644 tests/docker/test_launch_ros_cache.py create mode 100644 tests/docker/test_source_pins.py create mode 100644 tests/isaaclab/test_physical_reset.py create mode 100755 tests/stack/run_all.sh create mode 100644 tests/stack/test_heartbeat_clock.py diff --git a/.dockerignore b/.dockerignore index de3f98e..9aa1704 100644 --- a/.dockerignore +++ b/.dockerignore @@ -24,3 +24,5 @@ docker/* !docker/scripts docker/scripts/* !docker/scripts/package_humble_runtime.py +!docker/locks +!docker/locks/** diff --git a/.github/workflows/simulator-tests.yml b/.github/workflows/simulator-tests.yml index 21bce2b..60145b7 100644 --- a/.github/workflows/simulator-tests.yml +++ b/.github/workflows/simulator-tests.yml @@ -6,6 +6,9 @@ on: - 'CL_isaaclab_sim/**' - 'h1_robocasa/**' - 'docker/**' + - 'core_ws/**' + - 'CL_Assets' + - '.gitmodules' - 'tests/**' - '.github/workflows/simulator-tests.yml' push: @@ -48,14 +51,14 @@ jobs: # Explicit trusted runs on a prepared worker; CPU job is the portable PR gate. if: github.event_name == 'workflow_dispatch' runs-on: [self-hosted, linux, x64, nvidia] - timeout-minutes: 20 + timeout-minutes: 90 steps: - uses: actions/checkout@v4 with: submodules: recursive lfs: true - name: Build simulation and ROS images - run: docker/scripts/docker_build.sh isaac ros + run: docker/scripts/docker_build.sh isaac robocasa ros - name: Magpie physical linkage schema run: ROS_DOMAIN_ID=186 tests/isaaclab/run_magpie_couplings.sh - name: Magpie native ROS services and actions @@ -70,6 +73,8 @@ jobs: run: ROS_DOMAIN_ID=191 tests/isaaclab/run_robot_interop.sh magpie - name: Inspire variant regression run: ROS_DOMAIN_ID=192 tests/isaaclab/run_robot_interop.sh inspire + - name: Real frame-task and lower-body control in both simulators + run: tests/stack/run_all.sh - uses: actions/upload-artifact@v4 if: always() with: diff --git a/CL_isaaclab_sim b/CL_isaaclab_sim index 29615de..94b992f 160000 --- a/CL_isaaclab_sim +++ b/CL_isaaclab_sim @@ -1 +1 @@ -Subproject commit 29615de6346a74a32674d4a78fba7148d11beca6 +Subproject commit 94b992fcbb18abc98556079c6953629e63088618 diff --git a/core_ws/src/h12_safety_layer b/core_ws/src/h12_safety_layer index 6bdfc66..5b08888 160000 --- a/core_ws/src/h12_safety_layer +++ b/core_ws/src/h12_safety_layer @@ -1 +1 @@ -Subproject commit 6bdfc66b3329adfdc3f65d3dc3d8787c6ee6e505 +Subproject commit 5b088888fad08ee3ab2d8f8a747c6c321a5f455e diff --git a/docker/BUILD.md b/docker/BUILD.md index 0d157ad..d147838 100644 --- a/docker/BUILD.md +++ b/docker/BUILD.md @@ -217,9 +217,9 @@ mounted source are byte-identical — a requirement for the warm cache below. **At runtime:** `docker-compose` mounts the submodule source over `/home/code/mujoco_mpc` and a persistent build cache (`container_cache/mjpc_build`) at the exact in-tree build path. On first launch -`launch_ros.sh` hydrates the cache from the seed and back-dates the source mtimes, -so the first `docker exec … rebuild_mjpc.sh` is *incremental* (seconds–minutes), -not a cold ~15 min rebuild. `rebuild_mjpc.sh` rebuilds `agent_server` and copies it +`launch_ros.sh` hydrates the cache from the seed and refreshes source timestamps to avoid +reusing stale objects for local edits. The first `docker exec … rebuild_mjpc.sh` revalidates source objects while retaining built dependencies; subsequent launches +use normal incremental builds. `rebuild_mjpc.sh` rebuilds `agent_server` and copies it into `dist-packages` (the path `from mujoco_mpc import agent` auto-spawns). > The submodule gitlink and `MJPC_REF` are pinned to the **same** patched-fork SHA. @@ -281,7 +281,7 @@ bind-mounted source, with output persisted on the host: | Container | Workspace | Built | Cache | |---|---|---|---| -| `ros` | `core_ws` (full) | every start, gated on staleness | host `core_ws/{build,install,log}` | +| `ros` | `core_ws` (full) | every start, incremental every launch | host `core_ws/{build,install,log}` | | `robocasa` | `msgs_ws` (IDL only) | every start (fast no-op) | host `container_cache/msgs_ws` | `--symlink-install` is used throughout so Python nodes and model weights resolve diff --git a/docker/BaseDockerfile b/docker/BaseDockerfile index 0913920..2702fb6 100644 --- a/docker/BaseDockerfile +++ b/docker/BaseDockerfile @@ -7,7 +7,9 @@ # and shared kinematics libs (pin/pink/mink). # Build: docker compose build base # ============================== -FROM nvidia/cuda:12.2.0-devel-ubuntu22.04 +FROM nvidia/cuda:12.2.0-devel-ubuntu22.04@sha256:c4e81887e4aa9f13b1119337323cba89601319ecb282383b879c4ba50510fd17 +COPY docker/locks/base-*.txt docker/locks/apt-sources.sh docker/locks/ros-snapshot.asc /opt/golem-locks/ +RUN sh /opt/golem-locks/apt-sources.sh ENV DEBIAN_FRONTEND=noninteractive ENV TZ=America/Denver @@ -35,14 +37,15 @@ RUN apt-get update && apt-get install -y --no-install-recommends \ libegl1-mesa-dev libosmesa6-dev \ && locale-gen en_US en_US.UTF-8 && update-locale LC_ALL=C.UTF-8 LANG=C.UTF-8 \ && add-apt-repository universe \ + && sh /opt/golem-locks/apt-sources.sh \ && update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-12 100 \ && update-alternatives --install /usr/bin/g++ g++ /usr/bin/g++-12 100 \ && rm -rf /var/lib/apt/lists/* # ROS 2 Humble apt repository. -RUN curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key \ +RUN curl -sSL https://raw.githubusercontent.com/ros/rosdistro/e31287f892ecd01b170d1cbfa298db35abe4a74f/ros.key \ -o /usr/share/keyrings/ros-archive-keyring.gpg \ - && echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu jammy main" \ + && echo "deb [arch=$(dpkg --print-architecture) signed-by=/opt/golem-locks/ros-snapshot.asc] http://snapshots.ros.org/humble/2026-08-07/ubuntu jammy main" \ > /etc/apt/sources.list.d/ros2.list # ROS 2 Humble packages needed by the sim processes (rclpy + sensor/TF publishers). @@ -60,13 +63,14 @@ RUN apt-get update && apt-get install -y --no-install-recommends \ # Install uv for fast, deterministic pip-compatible installs. # UV_INSTALL_DIR overrides the default ~/.local/bin location. -RUN curl -LsSf https://astral.sh/uv/install.sh | env UV_INSTALL_DIR=/usr/local/bin sh \ +RUN curl -LsSf https://astral.sh/uv/0.12.15/install.sh | env UV_INSTALL_DIR=/usr/local/bin sh \ && uv --version # Build CycloneDDS 0.10.x. Pinned because the PyPI `cyclonedds` wheel (transitive # dep of unitree_sdk2_python) references `dds/ddsi/q_radmin.h`, removed post-0.10. # CYCLONEDDS_HOME points the PyPI wheel at this install at pip-build time. -RUN git clone --depth 1 --branch releases/0.10.x https://github.com/eclipse-cyclonedds/cyclonedds /cyclonedds && \ +RUN git clone https://github.com/eclipse-cyclonedds/cyclonedds /cyclonedds && \ + git -C /cyclonedds checkout --quiet 5041f3560c088c99e5088b2b8520b69169621196 && \ cd /cyclonedds && mkdir build install && cd build && \ cmake .. -DCMAKE_INSTALL_PREFIX=../install && \ cmake --build . --target install && \ @@ -78,7 +82,7 @@ ENV CYCLONEDDS_HOME=/cyclonedds/install # the image for development/debugging. COPY unitree_sdk2_python /home/code/unitree_sdk2_python RUN cd /home/code/unitree_sdk2_python && \ - uv pip install --system . && \ + uv pip install -c /opt/golem-locks/base-amd64-01.txt --system . && \ SP=$(python3 -c "import site; print(site.getsitepackages()[0])") && \ cp -r /home/code/unitree_sdk2_python/unitree_sdk2py/* "$SP/unitree_sdk2py/" @@ -86,7 +90,7 @@ RUN cd /home/code/unitree_sdk2_python && \ # Bundled CUDA runtime is self-contained — no host toolkit version dependency. # UV_HTTP_TIMEOUT raised from default 30s because multi-hundred-MB NVIDIA wheels # from pypi.nvidia.com can stall past the default on slow links. -RUN UV_HTTP_TIMEOUT=600 uv pip install --system --no-cache \ +RUN UV_HTTP_TIMEOUT=600 uv pip install -c /opt/golem-locks/base-amd64-02.txt --system --no-cache \ torch torchvision \ --index-url https://download.pytorch.org/whl/cu130 @@ -94,7 +98,7 @@ RUN UV_HTTP_TIMEOUT=600 uv pip install --system --no-cache \ # PyPI `pin` provides the `pinocchio` module. `pink` / `mink` are Pythonic IK # wrappers built on pinocchio. Kept in base so per-sim Dockerfiles don't # duplicate them; ROS container is standalone and installs its own pinocchio. -RUN uv pip install --system --no-cache pin pink mink +RUN uv pip install -c /opt/golem-locks/base-amd64-03.txt --system --no-cache pin pink mink # Source ROS 2 on every interactive shell. RUN echo 'source /opt/ros/humble/setup.bash' >> /root/.bashrc diff --git a/docker/IsaacDockerfile b/docker/IsaacDockerfile index 7ed327d..19f0173 100644 --- a/docker/IsaacDockerfile +++ b/docker/IsaacDockerfile @@ -2,7 +2,9 @@ # Build the gripper process against the same ROS distribution as GOLEM. # Isaac's bundled CPython has a different ABI, so this runtime stays private. -FROM ros:humble-ros-base-jammy AS magpie_humble +FROM ros:humble-ros-base-jammy@sha256:1813d3c85d7f96ff7d3012d865204583255740182db5d0065f8f8cd029a83138 AS magpie_humble +COPY docker/locks/isaac-*.txt docker/locks/apt-sources.sh docker/locks/ros-snapshot.asc /opt/golem-locks/ +RUN sh /opt/golem-locks/apt-sources.sh RUN apt-get update && apt-get install -y --no-install-recommends \ python3-colcon-common-extensions ros-humble-rmw-cyclonedds-cpp \ ros-humble-rosidl-default-generators \ @@ -15,9 +17,11 @@ COPY docker/scripts/package_humble_runtime.py /tmp/package_humble_runtime.py RUN . /opt/ros/humble/setup.sh && . /opt/golem-magpie-humble/setup.sh && \ python3 /tmp/package_humble_runtime.py -FROM nvcr.io/nvidia/isaac-sim:5.1.0 AS builder +FROM nvcr.io/nvidia/isaac-sim:5.1.0@sha256:f3563cb2ba0c18af0b2fb321360dcb73a917b899f879e3213623d6bee484fa54 AS builder +COPY docker/locks/isaac-*.txt docker/locks/apt-sources.sh docker/locks/ros-snapshot.asc /opt/golem-locks/ USER root +RUN sh /opt/golem-locks/apt-sources.sh ENV ACCEPT_EULA=Y ENV OMNI_KIT_ACCEPT_EULA=yes @@ -36,23 +40,29 @@ RUN --mount=type=cache,target=/var/cache/apt \ libglib2.0-0 \ ncurses-term \ wget && \ - apt -y autoremove && apt clean autoclean && \ + apt -y autoremove && apt clean autoclean && \ rm -rf /var/lib/apt/lists/* WORKDIR ${GOLEM_WORKSPACE_PATH} -RUN git clone https://github.com/isaac-sim/IsaacLab.git +RUN git clone https://github.com/isaac-sim/IsaacLab.git && \ + git -C IsaacLab checkout --quiet b4c321024792976150ca55fddb26fa34480d974e WORKDIR ${ISAACLAB_PATH} -RUN git reset --hard b4c321024792976150ca55fddb26fa34480d974e +# IsaacLab's extras otherwise resolve moving VCS refs even at a fixed Lab SHA. +# Reuse commits recorded by pip in the validated existing Isaac image. +RUN grep -Fq 'rl_games.git@python3.11' source/isaaclab_rl/setup.py \ + && sed -i 's|rl_games.git@python3.11|rl_games.git@6b3534f29568158e9e29ec8bf83cc88fce5f0cae|' source/isaaclab_rl/setup.py \ + && grep -Fq 'robomimic.git@v0.4.0' source/isaaclab_mimic/setup.py \ + && sed -i 's|robomimic.git@v0.4.0|robomimic.git@7c66e7a41b5d9dcc905b1a68346bfee1b49b79c9|' source/isaaclab_mimic/setup.py #this verison corresponds to Jun 12 RUN chmod +x ${ISAACLAB_PATH}/isaaclab.sh RUN ln -sf ${ISAACSIM_ROOT_PATH} ${ISAACLAB_PATH}/_isaac_sim -RUN ${ISAACLAB_PATH}/isaaclab.sh -p -m pip install toml +RUN ${ISAACLAB_PATH}/isaaclab.sh -p -m pip install -c /opt/golem-locks/isaac-amd64-01.txt toml RUN --mount=type=cache,target=/var/cache/apt \ ${ISAACLAB_PATH}/isaaclab.sh -p ${ISAACLAB_PATH}/tools/install_deps.py apt ${ISAACLAB_PATH}/source && \ @@ -78,12 +88,13 @@ RUN touch /bin/nvidia-smi && \ touch /var/run/nvidia-persistenced/socket RUN --mount=type=cache,target=${DOCKER_USER_HOME}/.cache/pip \ - ${ISAACLAB_PATH}/isaaclab.sh --install + PIP_CONSTRAINT=/opt/golem-locks/isaac-amd64-02.txt ${ISAACLAB_PATH}/isaaclab.sh --install RUN ${ISAACLAB_PATH}/isaaclab.sh -p -m pip uninstall -y quadprog msal WORKDIR ${GOLEM_WORKSPACE_PATH} -RUN git clone https://github.com/eclipse-cyclonedds/cyclonedds -b releases/0.10.x && \ +RUN git clone https://github.com/eclipse-cyclonedds/cyclonedds && \ + git -C cyclonedds checkout --quiet 5041f3560c088c99e5088b2b8520b69169621196 && \ cd cyclonedds && mkdir build install && cd build && \ cmake .. -DCMAKE_INSTALL_PREFIX=../install && \ cmake --build . --target install @@ -91,13 +102,14 @@ RUN git clone https://github.com/eclipse-cyclonedds/cyclonedds -b releases/0.10. ENV CYCLONEDDS_HOME=${GOLEM_WORKSPACE_PATH}/cyclonedds/install RUN git clone https://github.com/unitreerobotics/unitree_sdk2_python.git && \ - cd unitree_sdk2_python && ${ISAACLAB_PATH}/isaaclab.sh -p -m pip install -e . + git -C unitree_sdk2_python checkout --quiet 65691c8a8bc53b98d3976dba4dbf9d5d20b2e7f5 && \ + cd unitree_sdk2_python && ${ISAACLAB_PATH}/isaaclab.sh -p -m pip install -c /opt/golem-locks/isaac-amd64-03.txt -e . -RUN ${ISAACLAB_PATH}/isaaclab.sh -p -m pip install rerun-sdk==0.20.1 \ +RUN ${ISAACLAB_PATH}/isaaclab.sh -p -m pip install -c /opt/golem-locks/isaac-amd64-04.txt rerun-sdk==0.20.1 \ logging_mp==0.1.5 \ onnxruntime==1.22.1 \ onnx \ - pynput==1.8.1 + pynput==1.8.1 RUN echo "export ISAACLAB_PATH=${ISAACLAB_PATH}" >> ${HOME}/.bashrc && \ echo "export CYCLONEDDS_HOME=${GOLEM_WORKSPACE_PATH}/cyclonedds/install" >> ${HOME}/.bashrc && \ @@ -112,12 +124,13 @@ RUN echo "export ISAACLAB_PATH=${ISAACLAB_PATH}" >> ${HOME}/.bashrc && \ echo "PROMPT_COMMAND='history -a'" >> /root/.bashrc RUN --mount=type=cache,target=/var/cache/apt \ - apt-get update && apt-get install -y --no-install-recommends curl \ + apt-get update && apt-get install -y --no-install-recommends curl \ software-properties-common && \ add-apt-repository universe && \ - curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg && \ - echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu noble main" | tee /etc/apt/sources.list.d/ros2.list > /dev/null && \ - apt-get update && apt-get install -y --no-install-recommends \ + sh /opt/golem-locks/apt-sources.sh && \ + curl -sSL https://raw.githubusercontent.com/ros/rosdistro/e31287f892ecd01b170d1cbfa298db35abe4a74f/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg && \ + echo "deb [arch=$(dpkg --print-architecture) signed-by=/opt/golem-locks/ros-snapshot.asc] http://snapshots.ros.org/jazzy/2026-09-11/ubuntu noble main" | tee /etc/apt/sources.list.d/ros2.list > /dev/null && \ + apt-get update && apt-get install -y --no-install-recommends \ ros-jazzy-ros-base \ ros-jazzy-vision-msgs \ ros-jazzy-rmw-cyclonedds-cpp \ diff --git a/docker/RobocasaDockerfile b/docker/RobocasaDockerfile index eb9da96..f6b4050 100644 --- a/docker/RobocasaDockerfile +++ b/docker/RobocasaDockerfile @@ -5,6 +5,7 @@ # Robocasa (kitchen scenes) is installed from git; pins MuJoCo to 3.3.1. # ============================== FROM golem_base:latest +COPY docker/locks/robocasa-*.txt docker/locks/robocasa-assets.digest /opt/golem-locks/ ARG http_proxy ARG https_proxy @@ -24,7 +25,7 @@ RUN apt-get update && apt-get install -y --no-install-recommends \ # MuJoCo Python binding + Pillow for image encoding. # Pinned to 3.3.1 to match Robocasa's hard pin (see below); any other version # is silently swapped out mid-install by `pip install robocasa`. -RUN uv pip install --system --no-cache \ +RUN uv pip install -c /opt/golem-locks/robocasa-amd64-01.txt --system --no-cache \ "mujoco==3.3.1" \ "numpy>=2.2.6" \ "Pillow>=10.0" @@ -39,11 +40,13 @@ RUN uv pip install --system --no-cache \ # downgrades the cu130 torch in the base image to a CPU/cu121 build for # this image only. Acceptable for the mujoco sim container; the Isaac # container retains cu130 because it builds from base independently. -RUN git clone --depth 1 https://github.com/ARISE-Initiative/robosuite /opt/robosuite && \ - uv pip install --system --no-cache -e /opt/robosuite +RUN git clone https://github.com/ARISE-Initiative/robosuite /opt/robosuite && \ + git -C /opt/robosuite checkout --quiet 5ce6643f3092639d08f7b0f90ed1c6a84f50552c && \ + uv pip install -c /opt/golem-locks/robocasa-amd64-02.txt --system --no-cache -e /opt/robosuite -RUN git clone --depth 1 https://github.com/robocasa/robocasa /opt/robocasa && \ - uv pip install --system --no-cache -e /opt/robocasa +RUN git clone https://github.com/robocasa/robocasa /opt/robocasa && \ + git -C /opt/robocasa checkout --quiet 4f8a2980def75a55dff96b990745b83540425f09 && \ + uv pip install -c /opt/golem-locks/robocasa-amd64-03.txt --system --no-cache -e /opt/robocasa # Robocasa & robosuite both warn on first import about a missing private # macro file. Run their setup scripts once at build time to suppress the @@ -58,11 +61,13 @@ RUN python /opt/robosuite/robosuite/scripts/setup_macros.py && \ # - mimicgen: demonstration-generation environments (git-only). # We don't *use* any of these — installation is purely to silence the # import-time warnings. -RUN git clone --depth 1 https://github.com/ARISE-Initiative/robosuite_models /opt/robosuite_models && \ - uv pip install --system --no-cache -e /opt/robosuite_models && \ - git clone --depth 1 https://github.com/NVlabs/mimicgen /opt/mimicgen && \ - uv pip install --system --no-cache -e /opt/mimicgen && \ - uv pip install --system --no-cache --no-deps "mink==0.0.5" +RUN git clone https://github.com/ARISE-Initiative/robosuite_models /opt/robosuite_models && \ + git -C /opt/robosuite_models checkout --quiet 2d63a6b4319bda772420daf10594ce0de5bd793d && \ + uv pip install -c /opt/golem-locks/robocasa-amd64-04.txt --system --no-cache -e /opt/robosuite_models && \ + git clone https://github.com/NVlabs/mimicgen /opt/mimicgen && \ + git -C /opt/mimicgen checkout --quiet 72bd767c255545f462e7ccfb2731f2e5d4c1d9bb && \ + uv pip install -c /opt/golem-locks/robocasa-amd64-04.txt --system --no-cache -e /opt/mimicgen && \ + uv pip install -c /opt/golem-locks/robocasa-amd64-04.txt --system --no-cache --no-deps "mink==0.0.5" # Robocasa's textures, fixtures, and objects (~10 GB) — required for any # `--scene kitchen` run; KitchenArena fails immediately without them. @@ -77,6 +82,10 @@ RUN git clone --depth 1 https://github.com/ARISE-Initiative/robosuite_models /op # are suppressed because every entry in DOWNLOAD_ASSET_REGISTRY sets # check_folder_exists=False. RUN echo y | python -m robocasa.scripts.download_kitchen_assets --type all +# Verify the complete downloaded tree, including additions/removals. +RUN cd /opt/robocasa/robocasa/models && \ + find assets -type f -print0 | LC_ALL=C sort -z | xargs -0 sha256sum | \ + sha256sum --check /opt/golem-locks/robocasa-assets.digest WORKDIR /home/code/h1_robocasa @@ -107,7 +116,8 @@ RUN mkdir -p /home/code/msgs_ws/src # system library. Appended at the end so iterating on this layer does not bust # the heavy mujoco/robocasa/asset layers above. # ----------------------------------------------------------------------------- -RUN git clone --depth 1 https://github.com/Livox-SDK/Livox-SDK2.git /tmp/Livox-SDK2 && \ +RUN git clone https://github.com/Livox-SDK/Livox-SDK2.git /tmp/Livox-SDK2 && \ + git -C /tmp/Livox-SDK2 checkout --quiet 08f523c930b2f0ba1e98a6afaa8d7476bf479908 && \ cd /tmp/Livox-SDK2 && mkdir build && cd build && \ cmake .. -DCMAKE_POLICY_VERSION_MINIMUM=3.5 && make -j"$(nproc)" && make install && \ ldconfig && \ diff --git a/docker/RosDockerfile b/docker/RosDockerfile index 24be8a6..cdb69ec 100644 --- a/docker/RosDockerfile +++ b/docker/RosDockerfile @@ -17,6 +17,7 @@ # IK stack comes before the volatile vision ML stack for the same reason. # ============================== FROM golem_base:latest +COPY docker/locks/ros-*.txt /opt/golem-locks/ ARG http_proxy ARG https_proxy @@ -78,7 +79,8 @@ RUN apt-get update && apt-get install -y --no-install-recommends \ # the cmake build. CMAKE_POLICY_VERSION_MINIMUM works around upstream's # outdated cmake_minimum_required. # ----------------------------------------------------------------------------- -RUN git clone --depth 1 https://github.com/Livox-SDK/Livox-SDK2.git /tmp/Livox-SDK2 && \ +RUN git clone https://github.com/Livox-SDK/Livox-SDK2.git /tmp/Livox-SDK2 && \ + git -C /tmp/Livox-SDK2 checkout --quiet 08f523c930b2f0ba1e98a6afaa8d7476bf479908 && \ cd /tmp/Livox-SDK2 && mkdir build && cd build && \ cmake .. -DCMAKE_POLICY_VERSION_MINIMUM=3.5 && make -j"$(nproc)" && make install && \ ldconfig && \ @@ -91,7 +93,13 @@ RUN git clone --depth 1 https://github.com/Livox-SDK/Livox-SDK2.git /tmp/Livox-S # `--ignore-installed blinker` works around apt's old python3-blinker that # pip cannot cleanly uninstall. # ----------------------------------------------------------------------------- -RUN pip install --no-cache-dir --ignore-installed blinker \ +# Remove the NumPy-2 kinematics inherited from base, including its cmeel C++ +# libraries, before installing the ROS ABI. The next step runs with pip's global +# --ignore-installed, which writes over packages without uninstalling them. +RUN pip uninstall -y pin libpinocchio coal libcoal eigenpy \ + cmeel cmeel-assimp cmeel-boost cmeel-console-bridge cmeel-octomap \ + cmeel-qhull cmeel-tinyxml2 cmeel-urdfdom cmeel-zlib +RUN pip install -c /opt/golem-locks/ros-amd64-01.txt --no-cache-dir --ignore-installed blinker \ "numpy<2" \ "scikit-learn>=1.4" \ pin-pink \ @@ -111,7 +119,7 @@ RUN pip install --no-cache-dir --ignore-installed blinker \ # because it is not on PyPI. Model weights ship under # model_server/weights and are bind-mounted via core_ws. # ----------------------------------------------------------------------------- -RUN pip install --no-cache-dir \ +RUN pip install -c /opt/golem-locks/ros-amd64-02.txt --no-cache-dir \ "transformers==4.47.1" \ "google-genai==1.30.0" \ "matplotlib==3.9.3" \ @@ -124,22 +132,23 @@ RUN pip install --no-cache-dir \ "pycocotools==2.0.8" \ "ultralytics==8.3.52" -RUN pip install --no-cache-dir \ +RUN pip install -c /opt/golem-locks/ros-amd64-03.txt --no-cache-dir \ "opencv-python-headless==4.10.0.84" \ "iopath==0.1.10" RUN git clone https://github.com/openai/CLIP.git /opt/clip \ && cd /opt/clip && git checkout a1d071733d7111c9c014f024669f959182114e33 -RUN pip install --no-cache-dir --no-deps /opt/clip +RUN pip install -c /opt/golem-locks/ros-amd64-04.txt --no-cache-dir --no-deps /opt/clip # ----------------------------------------------------------------------------- # 5. SAM3 — vendored from git. # SAM3's setup.py produces UNKNOWN-0.0.0; bypass with PYTHONPATH instead. # (hydra-core kept: used for model config resolution.) # ----------------------------------------------------------------------------- -RUN pip install --no-cache-dir "hydra-core>=1.3.2" +RUN pip install -c /opt/golem-locks/ros-amd64-05.txt --no-cache-dir "hydra-core>=1.3.2" -RUN git clone --depth 1 https://github.com/facebookresearch/sam3.git /opt/sam3 +RUN git clone https://github.com/facebookresearch/sam3.git /opt/sam3 && \ + git -C /opt/sam3 checkout --quiet 660a5e9e1b8b4c02c0ad97229b88a09a6e4ff5b7 ENV PYTHONPATH=/opt/sam3:${PYTHONPATH} # ----------------------------------------------------------------------------- @@ -181,7 +190,7 @@ RUN rm -rf /usr/local/lib/python3.10/dist-packages/setuptools \ /usr/local/lib/python3.10/dist-packages/_distutils_hack \ /usr/local/lib/python3.10/dist-packages/wheel \ /usr/local/lib/python3.10/dist-packages/wheel-*.dist-info \ - && pip install --no-cache-dir --no-deps "numpy<2" "setuptools==59.6.0" "wheel<0.44" + && pip install -c /opt/golem-locks/ros-amd64-06.txt --no-cache-dir --no-deps "numpy<2" "setuptools==59.6.0" "wheel<0.44" # ----------------------------------------------------------------------------- # 8. Workspace integration. Base already sources /opt/ros/humble in bashrc; @@ -285,11 +294,12 @@ ENV GRASPGENX_CHECKPOINT_DIR=/opt/graspgenx/ext/graspgenx_checkpoints \ # wheel against it (PYTHONPATH + --no-build-isolation), then install the prebuilt # wheel with the system pip: the build sees PEP 621, but the system's # colcon-critical setuptools 59.6.0 is never touched. Source stays at /opt/graspgenx. -RUN git clone --depth 1 https://github.com/NVlabs/GraspGenX.git /opt/graspgenx \ - && pip install --no-cache-dir --target=/tmp/bsetup "setuptools>=64,<78" wheel setuptools-scm \ +RUN git clone https://github.com/NVlabs/GraspGenX.git /opt/graspgenx && \ + git -C /opt/graspgenx checkout --quiet b9429097728cb1c430dd78b92edf17ba318aad03 \ + && pip install -c /opt/golem-locks/ros-amd64-07.txt --no-cache-dir --target=/tmp/bsetup "setuptools>=64,<78" wheel setuptools-scm \ && SETUPTOOLS_SCM_PRETEND_VERSION=1.0.0 PYTHONPATH=/tmp/bsetup \ pip wheel --no-deps --no-build-isolation -w /tmp/gwheel /opt/graspgenx \ - && pip install --no-cache-dir --no-deps /tmp/gwheel/graspgenx-*.whl \ + && pip install -c /opt/golem-locks/ros-amd64-07.txt --no-cache-dir --no-deps /tmp/gwheel/graspgenx-*.whl \ && rm -rf /tmp/bsetup /tmp/gwheel # Sweep orphaned .dist-info ghosts: directories holding a stray file but no @@ -322,7 +332,7 @@ RUN pip freeze 2>/dev/null | grep '==' > /opt/graspgenx-constraints.txt # Additive GraspGenX deps that install cleanly under the full freeze (validated # in a throwaway container: exit 0, every pinned package unchanged). -RUN PIP_CONSTRAINT=/opt/graspgenx-constraints.txt pip install --no-cache-dir \ +RUN PIP_CONSTRAINT=/opt/graspgenx-constraints.txt pip install -c /opt/golem-locks/ros-amd64-08.txt --no-cache-dir \ "trimesh==4.5.3" \ "yourdfpy==0.0.56" \ torch-geometric \ @@ -345,7 +355,7 @@ RUN PIP_CONSTRAINT=/opt/graspgenx-constraints.txt pip install --no-cache-dir \ # numpy/Pillow/setuptools stay pinned. RUN grep -viE '^huggingface[-_]hub==' /opt/graspgenx-constraints.txt \ > /opt/graspgenx-constraints-nohf.txt \ - && PIP_CONSTRAINT=/opt/graspgenx-constraints-nohf.txt pip install --no-cache-dir \ + && PIP_CONSTRAINT=/opt/graspgenx-constraints-nohf.txt pip install -c /opt/golem-locks/ros-amd64-09.txt --no-cache-dir \ "huggingface-hub==0.25.2" \ "diffusers==0.11.1" @@ -364,7 +374,7 @@ RUN grep -viE '^huggingface[-_]hub==' /opt/graspgenx-constraints.txt \ # tyro, plyfile, ...) are purely additive. RUN grep -viE '^psutil==' /opt/graspgenx-constraints.txt \ > /opt/graspgenx-constraints-viser.txt \ - && PIP_CONSTRAINT=/opt/graspgenx-constraints-viser.txt pip install --no-cache-dir \ + && PIP_CONSTRAINT=/opt/graspgenx-constraints-viser.txt pip install -c /opt/golem-locks/ros-amd64-10.txt --no-cache-dir \ "viser==0.2.23" "psutil==5.9.8" # Asset staging is deferred to integration (install-only scope). We just create @@ -440,7 +450,7 @@ RUN apt-get update && apt-get install -y --no-install-recommends \ # docker_build.sh overrides it with the live `mujoco_mpc` submodule pin # (git rev-parse HEAD:mujoco_mpc), so the baked MJPC can't drift from the gitlink. ARG MJPC_REF=c7fc99f5028d2c09d14060d6d85202a377eefe49 -RUN git clone --branch extended_hw --single-branch \ +RUN git clone \ https://github.com/badinkajink/mujoco_mpc.git /home/code/mujoco_mpc \ && git -C /home/code/mujoco_mpc checkout --quiet "${MJPC_REF}" @@ -460,7 +470,7 @@ RUN git clone --branch extended_hw --single-branch \ # to 82 — the latter is re-clamped back to 59.6.0 in step 6 below. RUN grep -viE '^(grpcio|grpcio-tools|grpcio-status|protobuf|mujoco)==' \ /opt/graspgenx-constraints.txt > /opt/mjpc-constraints.txt \ - && PIP_CONSTRAINT=/opt/mjpc-constraints.txt pip install --no-cache-dir \ + && PIP_CONSTRAINT=/opt/mjpc-constraints.txt pip install -c /opt/golem-locks/ros-amd64-11.txt --no-cache-dir \ grpcio grpcio-tools protobuf "mujoco==3.2.3" # 4. Build the C++ agent_server, install the importable `mujoco_mpc` package, @@ -542,7 +552,7 @@ RUN rm -rf /usr/local/lib/python3.10/dist-packages/setuptools \ /usr/local/lib/python3.10/dist-packages/_distutils_hack \ /usr/local/lib/python3.10/dist-packages/wheel \ /usr/local/lib/python3.10/dist-packages/wheel-*.dist-info \ - && pip install --no-cache-dir --no-deps "numpy<2" "setuptools==59.6.0" "wheel<0.44" \ + && pip install -c /opt/golem-locks/ros-amd64-12.txt --no-cache-dir --no-deps "numpy<2" "setuptools==59.6.0" "wheel<0.44" \ && rm -f /usr/local/lib/python3.10/dist-packages/distutils-precedence.pth # 6. Fail the build loudly if the package isn't importable, the server binary diff --git a/docker/locks/SOURCES.md b/docker/locks/SOURCES.md new file mode 100644 index 0000000..22ac134 --- /dev/null +++ b/docker/locks/SOURCES.md @@ -0,0 +1,120 @@ +# Build input provenance + +Source and external image pins were recorded on 2026-09-17. Updating a pin is +an explicit dependency change that requires rebuilding and validating the +affected images. The build now selects dated Ubuntu and ROS archives, exact source revisions, +external base digests, and per-install Python constraints. Downloaded kitchen +assets are verified against a complete-tree digest. This is dependency-version +reproducibility, not a claim of byte-identical OCI layers: timestamps, compiler +outputs, and isolated Python build backends require separate verification. + +## Existing x86 images used as evidence + +| Image | Local image ID at audit | +| --- | --- | +| golem_base | 85410e75af53 | +| golem_ros | d1cef5abb357 | +| golem_sim_robocasa | 53b0719cdd36 | +| golem_sim_isaac | cabe3c218720 | + +Inspected with `docker compose run --rm --no-deps --entrypoint bash` through the +shared `docker_common.sh` configuration. No simulation was launched. Git SHAs +came from each image's retained `.git` directory; IsaacLab extras came from +`pip freeze` VCS metadata. uv's version was `0.12.15` in the base and its children. +The macOS Dockerfiles reuse the same source revisions; no arm64 built image was +available for validation. + +| Source | Revision | Evidence | +| --- | --- | --- | +| CycloneDDS | 5041f3560c088c99e5088b2b8520b69169621196 | All three x86 child images | +| SAM3 | 660a5e9e1b8b4c02c0ad97229b88a09a6e4ff5b7 | ROS image | +| GraspGenX | b9429097728cb1c430dd78b92edf17ba318aad03 | ROS image | +| robosuite | 5ce6643f3092639d08f7b0f90ed1c6a84f50552c | RoboCasa image | +| robocasa | 4f8a2980def75a55dff96b990745b83540425f09 | RoboCasa image | +| robosuite_models | 2d63a6b4319bda772420daf10594ce0de5bd793d | RoboCasa image | +| mimicgen | 72bd767c255545f462e7ccfb2731f2e5d4c1d9bb | RoboCasa image | +| Unitree Python SDK (Isaac) | 65691c8a8bc53b98d3976dba4dbf9d5d20b2e7f5 | Isaac image | +| rl_games (IsaacLab extra) | 6b3534f29568158e9e29ec8bf83cc88fce5f0cae | Isaac pip VCS metadata | +| robomimic (IsaacLab extra) | 7c66e7a41b5d9dcc905b1a68346bfee1b49b79c9 | Isaac pip VCS metadata | +| Livox-SDK2 | 08f523c930b2f0ba1e98a6afaa8d7476bf479908 | Upstream HEAD; all three public header SHA256s match installed ROS image headers | +| rosdistro signing key | e31287f892ecd01b170d1cbfa298db35abe4a74f | Upstream rosdistro HEAD | + +Livox's build deletes the clone, so matching headers is evidence of interface +compatibility, not proof the SDK binary was built from this exact revision. +The protected `livox_ros_driver2` gitlink remains unchanged at `13eb05e`. +Existing explicit CLIP, IsaacLab, MJPC and Unitree C++ revisions remain unchanged. +The base SDK COPY still uses the repository's Unitree Python submodule checkout. + +CUDA and Isaac base digests come from locally installed images' `RepoDigests`. +Humble ROS and Ubuntu 22.04 digests were resolved from registry manifest indexes; +the latter retains platform selection for arm64. These two registry pins need +fresh build validation. Local `golem_base` tags intentionally refer to the base +built by the repository wrapper, not to a remotely published digest. + +Guard against regressing source pins with: + +```sh +python3 -m unittest discover -s tests/docker -p test_source_pins.py +``` + + +## Package archives and Python transitions + +Ubuntu uses the `20260917T000000Z` snapshot; ROS Humble uses `2026-08-07` and +Jazzy uses `2026-09-11`. `apt-sources.sh` configures those archives and retains +signature verification. The vendored ROS snapshot key fingerprint is +`4B63CF8FDE49746E98FA01DDAD19BAB3CBF125EA`; it differs from the live ROS key. +It was obtained from Ubuntu's keyserver and checked against the ROS snapshot +maintainers' documented fingerprint. Expired Release metadata freshness checks +are disabled for historical snapshots; package signatures are not disabled. + +The Ubuntu snapshot is HTTPS-only (HTTP redirects to HTTPS). The bare arm64 +`ubuntu:22.04` base has no CA bundle, so `mac/BaseDockerfile.arm64` copies +`/etc/ssl/certs/ca-certificates.crt` from the digest-pinned multi-arch +`ros:humble-ros-base-jammy` index before running `apt-sources.sh`. TLS peer +verification stays enabled. The bootstrap was probe-built on amd64 from the same +two indexes; a native arm64 build has not been run. + +`apt-sources.sh` also disables inherited `cuda*`/`nvidia*` APT lists, which +are live and undated. No build step installs from them; CUDA libraries come +from the digest-pinned base images. + +Runtime constraints are passed with `-c` and do not reach pip or uv isolated +build environments, so sdist build backends (setuptools, scikit-build-core, +setuptools-scm, ...) still resolve to current releases. Applying the runtime +locks there would be wrong: the ROS image clamps `setuptools==59.6.0` at runtime +while sdists such as GraspGenX build with `setuptools>=64`. Locking backends +needs separate build-constraint files captured per install step. + +References: [Ubuntu snapshots](https://snapshot.ubuntu.com/), +[ROS snapshot key](https://github.com/osrf/docker_images/blob/master/ros/noetic/ubuntu/focal/ros-core/Dockerfile). + +Each `*-amd64.txt` is the common Python constraint set. Numbered files include +that set and declare intentional per-install transitions (NumPy, Hugging Face, +MuJoCo, and build tools). The arm64 sets reuse portable versions without CUDA +packages; native Mac build/runtime validation remains required. + +A fresh resolver exposed incompatible packages in the old installed ROS image: +NumPy 1.26 coexisted with cmeel-boost 1.90 and OpenCV 5, which require NumPy 2. +The ROS locks now use Pinocchio 2.7 / Boost 1.83 / eigenpy 3.5.1 / hpp-fcl 2.4.4 +and OpenCV 4.10.0.84. They must be validated together after rebuilding; this is +an intentional dependency correction, not a claim those versions were all in +the old image. MuJoCo remains 3.2.3 for ROS and 3.3.1 for RoboCasa. +The ROS image first uninstalls the base image's whole Pinocchio/cmeel closure; +otherwise the `--ignore-installed` reinstall left cmeel-boost 1.83 and 1.90 +(and two urdfdom/assimp versions) installed side by side. + +The old Isaac image was likewise inconsistent (`pip check`): stable-baselines3 +2.9.0 requires torch>=2.8 beside Isaac Sim's bundled torch 2.7.0+cu128; +rl-games 1.6.1 requires psutil<6 beside psutil 7.2.2; opencv-python 5.0 +requires NumPy 2. The Isaac locks keep Isaac Sim's bundled torch, NumPy 1.26 +and psutil 5.9.8, and select stable-baselines3 2.8.0 (newest accepting +torch>=2.3), ipython 9.10.1 (9.13+ requires psutil>=7; 9.11-9.12 require +Python 3.12), opencv-python 4.11.0.86 (matching the bundled headless build), +and pandas 2.2.3 with tzdata 2025.2, which stable-baselines3 2.8 requires. + +`robocasa-assets.digest` hashes the sorted `sha256sum` listing of every file +under `/opt/robocasa/robocasa/models/assets` (123,434 files at capture). This +catches changed contents, additions and removals without committing a large +file manifest. Updating assets requires explicit review and regenerating the +digest from the approved tree, not accepting a mismatch during a normal build. diff --git a/docker/locks/apt-sources.sh b/docker/locks/apt-sources.sh new file mode 100644 index 0000000..f0c25d0 --- /dev/null +++ b/docker/locks/apt-sources.sh @@ -0,0 +1,32 @@ +#!/bin/sh +# Dated, signed package archives; deliberately updated alongside dependency locks. +set -eu +. /etc/os-release +snapshot=https://snapshot.ubuntu.com/ubuntu/20260917T000000Z +# Ubuntu's snapshot includes both amd64 and arm64 archives. +cat > /etc/apt/sources.list < /etc/apt/apt.conf.d/99golem-snapshot +# Replace ROS sources (including modern Deb822 inline signing keys) as a unit. +for source in /etc/apt/sources.list.d/ros*.list /etc/apt/sources.list.d/ros*.sources; do + [ ! -e "$source" ] || mv "$source" "$source.disabled" +done +case "$VERSION_CODENAME" in + jammy) archive=http://snapshots.ros.org/humble/2026-08-07/ubuntu ;; + noble) archive=http://snapshots.ros.org/jazzy/2026-09-11/ubuntu ;; + *) echo "Unsupported Ubuntu release: $VERSION_CODENAME" >&2; exit 1 ;; +esac +printf 'deb [signed-by=/opt/golem-locks/ros-snapshot.asc] %s %s main\n' "$archive" "$VERSION_CODENAME" > /etc/apt/sources.list.d/ros2.list diff --git a/docker/locks/base-amd64-01.txt b/docker/locks/base-amd64-01.txt new file mode 100644 index 0000000..6e75eb8 --- /dev/null +++ b/docker/locks/base-amd64-01.txt @@ -0,0 +1,4 @@ +-c base-amd64.txt +mujoco==3.13.0 +numpy==2.2.6 +psutil==5.9.0 diff --git a/docker/locks/base-amd64-02.txt b/docker/locks/base-amd64-02.txt new file mode 100644 index 0000000..6e75eb8 --- /dev/null +++ b/docker/locks/base-amd64-02.txt @@ -0,0 +1,4 @@ +-c base-amd64.txt +mujoco==3.13.0 +numpy==2.2.6 +psutil==5.9.0 diff --git a/docker/locks/base-amd64-03.txt b/docker/locks/base-amd64-03.txt new file mode 100644 index 0000000..6e75eb8 --- /dev/null +++ b/docker/locks/base-amd64-03.txt @@ -0,0 +1,4 @@ +-c base-amd64.txt +mujoco==3.13.0 +numpy==2.2.6 +psutil==5.9.0 diff --git a/docker/locks/base-amd64.txt b/docker/locks/base-amd64.txt new file mode 100644 index 0000000..ffdb0bf --- /dev/null +++ b/docker/locks/base-amd64.txt @@ -0,0 +1,121 @@ +# Recorded from installed Linux images, 2026-09-17. +# arm64 files reuse portable versions; require native build validation. +absl-py==2.5.0 +argcomplete==1.8.1 +attrs==21.2.0 +black==26.5.1 +blinker==1.4 +catkin-pkg-modules==1.1.1 +click==8.5.0 +cmeel==0.61.0 +cmeel-assimp==6.0.5 +cmeel-boost==1.90.0 +cmeel-console-bridge==1.0.2.3 +cmeel-octomap==1.10.0 +cmeel-qhull==8.0.2.1 +cmeel-tinyxml2==11.0.0 +cmeel-urdfdom==6.0.0 +cmeel-zlib==1.3.2 +coal==3.0.3 +cryptography==3.4.8 +cuda-bindings==13.3.1 +cuda-pathfinder==1.6.0 +cuda-toolkit==13.0.3.0 +cyclonedds==0.10.2 +daqp==0.9.1 +dbus-python==1.2.18 +delegator-py==0.1.1 +distro==1.7.0 +docutils==0.17.1 +eigenpy==3.13.0 +empy==3.3.4 +etils==1.13.0 +filelock==3.32.3 +flake8==4.0.1 +fsspec==2026.7.0 +glfw==2.10.2 +glob2==0.7 +httplib2==0.20.2 +importlib-metadata==4.6.4 +importlib-resources==7.1.0 +iniconfig==1.1.1 +isort==9.0.1 +jeepney==0.7.1 +jinja2==3.1.6 +keyring==23.5.0 +lark==1.1.1 +launchpadlib==1.10.16 +lazr-restfulclient==0.14.4 +lazr-uri==1.0.6 +libcoal==3.0.3 +libpinocchio==4.1.0 +lxml==4.8.0 +markdown-it-py==4.2.0 +markupsafe==3.0.3 +mccabe==0.6.1 +mdurl==0.1.2 +mink==1.3.0 +more-itertools==8.10.0 +mpmath==1.3.0 +mypy-extensions==1.1.0 +netifaces==0.11.0 +networkx==3.4.2 +nvidia-cublas==13.1.1.3 +nvidia-cuda-cupti==13.0.85 +nvidia-cuda-nvrtc==13.0.88 +nvidia-cuda-runtime==13.0.96 +nvidia-cudnn-cu13==9.24.0.43 +nvidia-cufft==12.0.0.61 +nvidia-cufile==1.15.1.6 +nvidia-curand==10.4.0.35 +nvidia-cusolver==12.0.4.66 +nvidia-cusparse==12.6.3.3 +nvidia-cusparselt-cu13==0.8.1 +nvidia-nccl-cu13==2.30.7 +nvidia-nvjitlink==13.3.33 +nvidia-nvshmem-cu13==3.4.5 +nvidia-nvtx==13.0.85 +oauthlib==3.2.0 +opencv-python==5.0.0.93 +packaging==26.3 +pathspec==1.1.1 +pexpect==4.9.0 +pillow==12.3.0 +pin==4.1.0 +pink==0.8.1 +platformdirs==4.11.9 +pluggy==0.13.0 +ptyprocess==0.7.0 +py==1.10.0 +pycodestyle==2.8.0 +pydocstyle==6.1.1 +pyflakes==2.4.0 +pygments==2.21.0 +pygobject==3.42.1 +pyjwt==2.3.0 +pyopengl==3.1.10 +pyparsing==2.4.7 +pytest==6.2.5 +python-apt==2.4.0+ubuntu4.1 +python-dateutil==2.8.1 +pytokens==0.4.1 +pyyaml==5.4.1 +qpsolvers==4.13.0 +rich==15.0.0 +rich-click==1.9.9 +roman==3.3 +rosdistro-modules==1.1.0 +rospkg-modules==1.6.3 +scipy==1.15.3 +secretstorage==3.3.1 +six==1.16.0 +snowballstemmer==2.2.0 +sympy==1.14.0 +toml==0.10.2 +tomli==2.4.1 +torch==2.14.0+cu130 +torchvision==0.29.0+cu130 +triton==3.8.0 +typing-extensions==4.16.0 +wadllib==1.3.6 +zipp==4.1.0 diff --git a/docker/locks/base-arm64-01.txt b/docker/locks/base-arm64-01.txt new file mode 100644 index 0000000..65d375b --- /dev/null +++ b/docker/locks/base-arm64-01.txt @@ -0,0 +1,4 @@ +-c base-arm64.txt +mujoco==3.13.0 +numpy==2.2.6 +psutil==5.9.0 diff --git a/docker/locks/base-arm64-02.txt b/docker/locks/base-arm64-02.txt new file mode 100644 index 0000000..65d375b --- /dev/null +++ b/docker/locks/base-arm64-02.txt @@ -0,0 +1,4 @@ +-c base-arm64.txt +mujoco==3.13.0 +numpy==2.2.6 +psutil==5.9.0 diff --git a/docker/locks/base-arm64-03.txt b/docker/locks/base-arm64-03.txt new file mode 100644 index 0000000..65d375b --- /dev/null +++ b/docker/locks/base-arm64-03.txt @@ -0,0 +1,4 @@ +-c base-arm64.txt +mujoco==3.13.0 +numpy==2.2.6 +psutil==5.9.0 diff --git a/docker/locks/base-arm64.txt b/docker/locks/base-arm64.txt new file mode 100644 index 0000000..65f61c7 --- /dev/null +++ b/docker/locks/base-arm64.txt @@ -0,0 +1,102 @@ +# Recorded from installed Linux images, 2026-09-17. +# arm64 files reuse portable versions; require native build validation. +absl-py==2.5.0 +argcomplete==1.8.1 +attrs==21.2.0 +black==26.5.1 +blinker==1.4 +catkin-pkg-modules==1.1.1 +click==8.5.0 +cmeel==0.61.0 +cmeel-assimp==6.0.5 +cmeel-boost==1.90.0 +cmeel-console-bridge==1.0.2.3 +cmeel-octomap==1.10.0 +cmeel-qhull==8.0.2.1 +cmeel-tinyxml2==11.0.0 +cmeel-urdfdom==6.0.0 +cmeel-zlib==1.3.2 +coal==3.0.3 +cryptography==3.4.8 +cyclonedds==0.10.2 +daqp==0.9.1 +dbus-python==1.2.18 +delegator-py==0.1.1 +distro==1.7.0 +docutils==0.17.1 +eigenpy==3.13.0 +empy==3.3.4 +etils==1.13.0 +filelock==3.32.3 +flake8==4.0.1 +fsspec==2026.7.0 +glfw==2.10.2 +glob2==0.7 +httplib2==0.20.2 +importlib-metadata==4.6.4 +importlib-resources==7.1.0 +iniconfig==1.1.1 +isort==9.0.1 +jeepney==0.7.1 +jinja2==3.1.6 +keyring==23.5.0 +lark==1.1.1 +launchpadlib==1.10.16 +lazr-restfulclient==0.14.4 +lazr-uri==1.0.6 +libcoal==3.0.3 +libpinocchio==4.1.0 +lxml==4.8.0 +markdown-it-py==4.2.0 +markupsafe==3.0.3 +mccabe==0.6.1 +mdurl==0.1.2 +mink==1.3.0 +more-itertools==8.10.0 +mpmath==1.3.0 +mypy-extensions==1.1.0 +netifaces==0.11.0 +networkx==3.4.2 +oauthlib==3.2.0 +opencv-python==5.0.0.93 +packaging==26.3 +pathspec==1.1.1 +pexpect==4.9.0 +pillow==12.3.0 +pin==4.1.0 +pink==0.8.1 +platformdirs==4.11.9 +pluggy==0.13.0 +ptyprocess==0.7.0 +py==1.10.0 +pycodestyle==2.8.0 +pydocstyle==6.1.1 +pyflakes==2.4.0 +pygments==2.21.0 +pygobject==3.42.1 +pyjwt==2.3.0 +pyopengl==3.1.10 +pyparsing==2.4.7 +pytest==6.2.5 +python-apt==2.4.0+ubuntu4.1 +python-dateutil==2.8.1 +pytokens==0.4.1 +pyyaml==5.4.1 +qpsolvers==4.13.0 +rich==15.0.0 +rich-click==1.9.9 +roman==3.3 +rosdistro-modules==1.1.0 +rospkg-modules==1.6.3 +scipy==1.15.3 +secretstorage==3.3.1 +six==1.16.0 +snowballstemmer==2.2.0 +sympy==1.14.0 +toml==0.10.2 +tomli==2.4.1 +torch==2.14.0 +torchvision==0.29.0 +typing-extensions==4.16.0 +wadllib==1.3.6 +zipp==4.1.0 diff --git a/docker/locks/isaac-amd64-01.txt b/docker/locks/isaac-amd64-01.txt new file mode 100644 index 0000000..e29e065 --- /dev/null +++ b/docker/locks/isaac-amd64-01.txt @@ -0,0 +1,4 @@ +-c isaac-amd64.txt +huggingface-hub==0.36.2 +numpy==1.26.4 +psutil==5.9.8 diff --git a/docker/locks/isaac-amd64-02.txt b/docker/locks/isaac-amd64-02.txt new file mode 100644 index 0000000..e29e065 --- /dev/null +++ b/docker/locks/isaac-amd64-02.txt @@ -0,0 +1,4 @@ +-c isaac-amd64.txt +huggingface-hub==0.36.2 +numpy==1.26.4 +psutil==5.9.8 diff --git a/docker/locks/isaac-amd64-03.txt b/docker/locks/isaac-amd64-03.txt new file mode 100644 index 0000000..e29e065 --- /dev/null +++ b/docker/locks/isaac-amd64-03.txt @@ -0,0 +1,4 @@ +-c isaac-amd64.txt +huggingface-hub==0.36.2 +numpy==1.26.4 +psutil==5.9.8 diff --git a/docker/locks/isaac-amd64-04.txt b/docker/locks/isaac-amd64-04.txt new file mode 100644 index 0000000..e29e065 --- /dev/null +++ b/docker/locks/isaac-amd64-04.txt @@ -0,0 +1,4 @@ +-c isaac-amd64.txt +huggingface-hub==0.36.2 +numpy==1.26.4 +psutil==5.9.8 diff --git a/docker/locks/isaac-amd64.txt b/docker/locks/isaac-amd64.txt new file mode 100644 index 0000000..51c3b06 --- /dev/null +++ b/docker/locks/isaac-amd64.txt @@ -0,0 +1,268 @@ +# Recorded from installed Linux images, 2026-09-17. +# arm64 files reuse portable versions; require native build validation. +absl-py==2.5.0 +aioboto3==15.1.0 +aiobotocore==2.24.0 +aiodns==3.1.1 +aiofiles==23.2.1 +aiohappyeyeballs==2.4.4 +aiohttp==3.11.11 +aioitertools==0.11.0 +aiosignal==1.3.2 +annotated-types==0.7.0 +antlr4-python3-runtime==4.9.3 +anyio==4.9.0 +anytree==2.13.0 +asteval==1.0.6 +asttokens==3.0.2 +async-timeout==5.0.1 +attrs==25.1.0 +awscrt==0.23.8 +azure-core==1.28.0 +azure-identity==1.13.0 +azure-storage-blob==12.17.0 +boto3==1.39.11 +botocore==1.39.11 +certifi==2025.7.14 +cffi==1.17.1 +charset-normalizer==3.3.2 +click==8.5.0 +cloudpickle==3.1.2 +cmeel-assimp==5.4.3.1 +cmeel-boost==1.83.0 +cmeel-console-bridge==1.0.2.3 +cmeel-octomap==1.10.0 +cmeel-qhull==8.0.2.1 +cmeel-tinyxml==2.6.2.3 +cmeel-urdfdom==3.1.1.1 +cmeel-zlib==1.3.2 +cmeel==0.61.0 +coloredlogs==15.0.1 +comm==0.2.3 +contourpy==1.3.1 +coverage==7.4.4 +cryptography==44.0.0 +cuda-bindings==13.4.1 +cuda-pathfinder==1.8.1 +cuda-toolkit==13.0.3.0 +cycler==0.11.0 +cyclonedds==0.10.2 +daqp==0.8.5 +decorator==5.3.1 +dex-retargeting==0.4.6 +egl-probe==1.0.2 +eigenpy==3.5.1 +einops==0.8.2 +evdev==2.0.0 +executing==2.2.1 +farama-notifications==0.0.6 +fastapi==0.115.7 +filelock==3.13.1 +flaky==3.8.1 +flatbuffers==25.12.19 +flatdict==4.1.0 +fonttools==4.55.3 +frozenlist==1.5.0 +fsspec==2024.6.1 +gitdb==4.0.12 +gitpython==3.1.62 +googleapis-common-protos==1.75.3 +grpcio==1.84.0 +gunicorn==23.0.0 +gym-notices==0.1.0 +gym==0.23.1 +gymnasium==1.2.1 +h11==0.16.0 +h5py==3.16.0 +hf-xet==1.6.0 +hidapi==0.14.0.post2 +hpp-fcl==2.4.4 +httptools==0.6.1 +humanfriendly==10.0 +hydra-core==1.3.7 +idna-ssl==1.1.0 +idna==3.10 +imageio-ffmpeg==0.6.0 +imageio==2.37.0 +importlib-metadata==9.0.1 +iniconfig==2.3.0 +ipython-pygments-lexers==1.1.1 +ipython==9.10.1 +ipywidgets==8.1.5 +isodate==0.6.1 +jedi==0.20.0 +jinja2==3.1.6 +jmespath==1.0.1 +jsonschema-specifications==2025.9.1 +jsonschema==4.26.0 +junitparser==5.0.3 +jupyterlab-widgets==3.0.17 +kiwisolver==1.4.4 +llvmlite==0.42.0 +logging-mp==0.1.5 +loop-rate-limiters==1.2.0 +lxml==5.4.0 +markdown-it-py==4.2.0 +markdown==3.10.3 +markupsafe==3.0.2 +matplotlib-inline==0.2.2 +matplotlib==3.10.3 +mdurl==0.1.2 +ml-dtypes==0.5.4 +moviepy==2.2.1 +mpmath==1.3.0 +msal-extensions==1.0.0 +msgpack==1.2.2 +multidict==6.1.0 +nest-asyncio==1.5.6 +networkx==3.3 +nlopt==2.7.1 +numba==0.59.1 +nvidia-cublas-cu12==12.8.3.14 +nvidia-cublas==13.1.1.3 +nvidia-cuda-cupti-cu12==12.8.57 +nvidia-cuda-cupti==13.0.85 +nvidia-cuda-nvrtc-cu12==12.8.61 +nvidia-cuda-nvrtc==13.0.88 +nvidia-cuda-runtime-cu12==12.8.57 +nvidia-cuda-runtime==13.0.96 +nvidia-cudnn-cu12==9.7.1.26 +nvidia-cudnn-cu13==9.24.0.43 +nvidia-cufft-cu12==11.3.3.41 +nvidia-cufft==12.0.0.61 +nvidia-cufile-cu12==1.13.0.11 +nvidia-cufile==1.15.1.6 +nvidia-curand-cu12==10.3.9.55 +nvidia-curand==10.4.0.35 +nvidia-cusolver-cu12==11.7.2.55 +nvidia-cusolver==12.0.4.66 +nvidia-cusparse-cu12==12.5.7.53 +nvidia-cusparse==12.6.3.3 +nvidia-cusparselt-cu12==0.6.3 +nvidia-cusparselt-cu13==0.8.1 +nvidia-lula-no-cuda==0.10.1 +nvidia-nccl-cu12==2.26.2 +nvidia-nccl-cu13==2.30.7 +nvidia-nvjitlink-cu12==12.8.61 +nvidia-nvjitlink==13.4.52 +nvidia-nvshmem-cu13==3.4.5 +nvidia-nvtx-cu12==12.8.55 +nvidia-nvtx==13.0.85 +nvidia-srl-base==1.3.0 +nvidia-srl-math==1.0.0 +nvidia-srl-usd-to-urdf==1.0.2 +nvidia-srl-usd==2.0.0 +oauthlib==3.2.2 +omegaconf==2.3.1 +onnx-ir==1.0.0 +onnx==1.22.0 +onnxruntime==1.22.1 +onnxscript==0.7.2 +opencv-python-headless==4.11.0.86 +opencv-python==4.11.0.86 +opentelemetry-api==1.44.0 +opentelemetry-exporter-otlp-proto-common==1.44.0 +opentelemetry-exporter-otlp-proto-http==1.44.0 +opentelemetry-proto==1.44.0 +opentelemetry-sdk==1.44.0 +opentelemetry-semantic-conventions==0.65b0 +orjson==3.12.0 +osqp==0.6.7.post3 +packaging==23.0 +pandas==2.2.3 +parso==0.8.7 +pexpect==4.9.0 +pillow==11.3.0 +pin-pink==3.1.0 +pin==2.7.0 +pint==0.20.1 +platformdirs==4.11.9 +pluggy==1.6.0 +portalocker==2.7.0 +prettytable==3.3.0 +proglog==0.1.12 +prompt-toolkit==3.0.53 +propcache==0.2.1 +protobuf==7.36.1 +ptyprocess==0.7.0 +pure-eval==0.2.4 +pyarrow==25.0.1 +pycares==4.8.0 +pycparser==2.22 +pydantic-core==2.33.2 +pydantic==2.11.7 +pyglet==1.5.31 +pygments==2.21.0 +pynput==1.8.1 +pyparsing==3.0.9 +pyperclip==1.8.0 +pypng==0.20220715.0 +pytest-mock==3.15.1 +pytest==9.1.1 +python-dateutil==2.9.0.post0 +python-dotenv==1.2.3 +python-multipart==0.0.20 +python-xlib==0.33 +pytransform3d==3.16.0 +pytz==2024.1 +pyvers==0.2.3 +pyyaml==6.0.2 +qdldl==0.1.7.post5 +qpsolvers==4.13.0 +qrcode==7.4.2 +ray==2.52.1 +referencing==0.37.0 +regex==2026.9.10 +requests-oauthlib==1.3.1 +requests==2.32.3 +rerun-sdk==0.20.1 +rich-click==1.9.9 +rich==15.0.0 +rpds-py==2026.6.3 +rsl-rl-lib==5.0.1 +rtree==1.3.0 +s3transfer==0.13.1 +safetensors==0.8.0 +scipy==1.15.3 +sentry-sdk==2.29.1 +setproctitle==1.3.7 +six==1.17.0 +skrl==2.1.0 +smmap==5.0.3 +sniffio==1.3.1 +stable-baselines3==2.8.0 +stack-data==0.6.3 +starlette==0.49.1 +sympy==1.13.3 +tensorboard-data-server==0.7.2 +tensorboard==2.21.0 +tensorboardx==2.6.5 +tensordict==0.14.2 +termcolor==3.3.0 +tokenizers==0.22.2 +toml==0.10.2 +tomli==2.4.1 +torch==2.7.0+cu128 +torchvision==0.22.0+cu128 +tornado==6.5.1 +tqdm==4.70.1 +traitlets==5.16.1 +transformers==4.57.6 +trimesh==4.5.1 +triton==3.3.0 +typing-extensions==4.16.0 +typing-inspection==0.4.1 +tzdata==2025.2 +urllib3==2.5.0 +uvicorn==0.29.0 +wandb==0.30.0 +warp-lang==1.17.0 +watchdog==4.0.0 +wcwidth==0.8.3 +websockets==12.0 +werkzeug==3.1.8 +widgetsnbextension==4.0.16 +wrapt==1.16.0 +xxhash==4.0.1 +yarl==1.18.3 +zipp==4.1.0 diff --git a/docker/locks/robocasa-amd64-01.txt b/docker/locks/robocasa-amd64-01.txt new file mode 100644 index 0000000..688eb9b --- /dev/null +++ b/docker/locks/robocasa-amd64-01.txt @@ -0,0 +1,5 @@ +-c robocasa-amd64.txt +huggingface-hub==1.31.0 +mujoco==3.3.1 +numpy==2.2.6 +psutil==7.2.2 diff --git a/docker/locks/robocasa-amd64-02.txt b/docker/locks/robocasa-amd64-02.txt new file mode 100644 index 0000000..7500b45 --- /dev/null +++ b/docker/locks/robocasa-amd64-02.txt @@ -0,0 +1,5 @@ +-c robocasa-amd64.txt +huggingface-hub==1.31.0 +mujoco==3.3.1 +numpy==2.2.5 +psutil==7.2.2 diff --git a/docker/locks/robocasa-amd64-03.txt b/docker/locks/robocasa-amd64-03.txt new file mode 100644 index 0000000..7500b45 --- /dev/null +++ b/docker/locks/robocasa-amd64-03.txt @@ -0,0 +1,5 @@ +-c robocasa-amd64.txt +huggingface-hub==1.31.0 +mujoco==3.3.1 +numpy==2.2.5 +psutil==7.2.2 diff --git a/docker/locks/robocasa-amd64-04.txt b/docker/locks/robocasa-amd64-04.txt new file mode 100644 index 0000000..7500b45 --- /dev/null +++ b/docker/locks/robocasa-amd64-04.txt @@ -0,0 +1,5 @@ +-c robocasa-amd64.txt +huggingface-hub==1.31.0 +mujoco==3.3.1 +numpy==2.2.5 +psutil==7.2.2 diff --git a/docker/locks/robocasa-amd64.txt b/docker/locks/robocasa-amd64.txt new file mode 100644 index 0000000..458effd --- /dev/null +++ b/docker/locks/robocasa-amd64.txt @@ -0,0 +1,246 @@ +# Recorded from installed Linux images, 2026-09-17. +# arm64 files reuse portable versions; require native build validation. +absl-py==2.5.0 +aiohappyeyeballs==2.7.1 +aiohttp==3.14.3 +aiosignal==1.4.0 +annotated-types==0.8.0 +anyio==4.15.1 +argcomplete==1.8.1 +async-timeout==5.0.1 +attrs==26.1.0 +av==17.1.0 +beautifulsoup4==4.15.0 +black==26.5.1 +blinker==1.9.0 +catkin-pkg-modules==1.1.1 +certifi==2026.7.22 +chardet==7.6.0 +charset-normalizer==3.5.1 +click==8.5.0 +cloudpickle==3.1.2 +cmake==4.4.3 +cmeel==0.61.0 +cmeel-assimp==6.0.5 +cmeel-boost==1.90.0 +cmeel-console-bridge==1.0.2.3 +cmeel-octomap==1.10.0 +cmeel-qhull==8.0.2.1 +cmeel-tinyxml2==11.0.0 +cmeel-urdfdom==6.0.0 +cmeel-zlib==1.3.2 +coal==3.0.3 +colcon-argcomplete==0.3.3 +colcon-bash==0.5.0 +colcon-cd==0.2.1 +colcon-cmake==0.2.30 +colcon-common-extensions==0.3.0 +colcon-core==0.21.2 +colcon-defaults==0.2.9 +colcon-devtools==0.3.0 +colcon-library-path==0.2.1 +colcon-metadata==0.3.1 +colcon-notification==0.3.2 +colcon-output==0.2.14 +colcon-package-information==0.4.1 +colcon-package-selection==0.2.10 +colcon-parallel-executor==0.3.0 +colcon-pkg-config==0.1.0 +colcon-powershell==0.5.0 +colcon-python-setup-py==0.2.9 +colcon-recursive-crawl==0.2.3 +colcon-ros==0.5.0 +colcon-test-result==0.3.8 +colcon-zsh==0.5.0 +cryptography==3.4.8 +cuda-bindings==13.3.1 +cuda-pathfinder==1.6.0 +cuda-toolkit==13.0.3.0 +cyclonedds==0.10.2 +daqp==0.9.1 +datasets==3.6.0 +dbus-python==1.2.18 +deepdiff==8.6.2 +delegator-py==0.1.1 +diffusers==0.40.0 +dill==0.3.8 +distlib==0.3.4 +distro==1.7.0 +docutils==0.17.1 +draccus==0.10.0 +eigenpy==3.13.0 +einops==0.8.2 +empy==3.3.4 +etils==1.13.0 +evdev==1.9.3 +exceptiongroup==1.3.1 +farama-notifications==0.0.6 +filelock==3.32.3 +flake8==4.0.1 +flask==3.1.3 +frozenlist==1.8.0 +fsspec==2025.3.0 +gdown==6.3.0 +gitdb==4.0.12 +gitpython==3.1.62 +glfw==2.10.2 +glob2==0.7 +grpcio==1.84.0 +gym==0.26.2 +gym-notices==0.1.0 +gymnasium==0.29.1 +h11==0.16.0 +h5py==3.16.0 +hf-xet==1.6.0 +hidapi==0.15.0 +httpcore==1.0.9 +httplib2==0.20.2 +httpx==0.28.1 +idna==3.19 +imageio==2.37.4 +imageio-ffmpeg==0.6.0 +importlib-metadata==9.0.1 +importlib-resources==7.1.0 +iniconfig==2.3.0 +isort==9.0.1 +itsdangerous==2.2.0 +jeepney==0.7.1 +jinja2==3.1.6 +jsonlines==4.0.0 +keyring==23.5.0 +lark==1.1.1 +launchpadlib==1.10.16 +lazr-restfulclient==0.14.4 +lazr-uri==1.0.6 +lerobot==0.3.3 +libcoal==3.0.3 +libpinocchio==4.1.0 +llvmlite==0.44.0 +lxml==6.1.3 +markdown==3.10.3 +markdown-it-py==4.2.0 +markupsafe==3.0.3 +mccabe==0.6.1 +mdurl==0.1.2 +mergedeep==1.3.4 +mink==0.0.5 +more-itertools==8.10.0 +mpi4py==3.1.3 +mpmath==1.3.0 +multidict==6.8.0 +multiprocess==0.70.16 +mypy-extensions==1.1.0 +netifaces==0.11.0 +networkx==3.4.2 +notify2==0.3 +numba==0.61.2 +nvidia-cublas==13.1.1.3 +nvidia-cublas-cu12==12.6.4.1 +nvidia-cuda-cupti==13.0.85 +nvidia-cuda-cupti-cu12==12.6.80 +nvidia-cuda-nvrtc==13.0.88 +nvidia-cuda-nvrtc-cu12==12.6.77 +nvidia-cuda-runtime==13.0.96 +nvidia-cuda-runtime-cu12==12.6.77 +nvidia-cudnn-cu12==9.5.1.17 +nvidia-cudnn-cu13==9.24.0.43 +nvidia-cufft==12.0.0.61 +nvidia-cufft-cu12==11.3.0.4 +nvidia-cufile==1.15.1.6 +nvidia-cufile-cu12==1.11.1.6 +nvidia-curand==10.4.0.35 +nvidia-curand-cu12==10.3.7.77 +nvidia-cusolver==12.0.4.66 +nvidia-cusolver-cu12==11.7.1.2 +nvidia-cusparse==12.6.3.3 +nvidia-cusparse-cu12==12.5.4.2 +nvidia-cusparselt-cu12==0.6.3 +nvidia-cusparselt-cu13==0.8.1 +nvidia-nccl-cu12==2.26.2 +nvidia-nccl-cu13==2.30.7 +nvidia-nvjitlink==13.3.33 +nvidia-nvjitlink-cu12==12.6.85 +nvidia-nvshmem-cu13==3.4.5 +nvidia-nvtx==13.0.85 +nvidia-nvtx-cu12==12.6.77 +oauthlib==3.2.0 +opencv-python==5.0.0.93 +opencv-python-headless==5.0.0.93 +orderly-set==5.5.0 +packaging==26.3 +pandas==2.3.3 +pathspec==1.1.1 +pexpect==4.9.0 +pillow==12.3.0 +pin==4.1.0 +pink==0.8.1 +platformdirs==4.11.9 +pluggy==1.6.0 +propcache==0.5.4 +protobuf==3.19.6 +ptyprocess==0.7.0 +py==1.10.0 +pyarrow==25.0.1 +pycodestyle==2.8.0 +pydantic==2.13.5 +pydantic-core==2.46.5 +pydocstyle==6.1.1 +pyflakes==2.4.0 +pygame==2.6.1 +pygments==2.21.0 +pygobject==3.42.1 +pyjwt==2.3.0 +pynput==1.8.2 +pyopengl==3.1.10 +pyparsing==2.4.7 +pyserial==3.5 +pysocks==1.7.1 +pytest==9.1.1 +python-apt==2.4.0+ubuntu4.1 +python-dateutil==2.9.0.post0 +python-xlib==0.33 +pytokens==0.4.1 +pytz==2026.3.post1 +pyyaml==6.0.3 +pyyaml-include==1.4.1 +qpsolvers==4.13.0 +quadprog==0.1.13 +regex==2026.9.10 +requests==2.34.2 +rerun-sdk==0.22.1 +rich==15.0.0 +rich-click==1.9.9 +roman==3.3 +rosdistro-modules==1.1.0 +rospkg-modules==1.6.3 +safetensors==0.8.0 +scipy==1.15.3 +secretstorage==3.3.1 +sentry-sdk==2.69.2 +six==1.17.0 +smmap==5.0.3 +snowballstemmer==2.2.0 +soupsieve==2.9.2 +sympy==1.14.0 +tensorboard==2.20.0 +tensorboard-data-server==0.7.2 +termcolor==3.3.0 +tianshou==0.4.10 +toml==0.10.2 +tomli==2.4.1 +torch==2.7.1 +torchcodec==0.5 +torchvision==0.22.1 +tqdm==4.70.1 +triton==3.3.1 +typing-extensions==4.16.0 +typing-inspect==0.9.0 +typing-inspection==0.4.4 +tzdata==2026.4 +urllib3==2.8.0 +wadllib==1.3.6 +wandb==0.25.0 +werkzeug==3.1.8 +xxhash==4.0.1 +yarl==1.25.1 +zipp==4.1.0 diff --git a/docker/locks/robocasa-arm64-01.txt b/docker/locks/robocasa-arm64-01.txt new file mode 100644 index 0000000..5009655 --- /dev/null +++ b/docker/locks/robocasa-arm64-01.txt @@ -0,0 +1,5 @@ +-c robocasa-arm64.txt +huggingface-hub==1.31.0 +mujoco==3.3.1 +numpy==2.2.6 +psutil==7.2.2 diff --git a/docker/locks/robocasa-arm64-02.txt b/docker/locks/robocasa-arm64-02.txt new file mode 100644 index 0000000..09450e2 --- /dev/null +++ b/docker/locks/robocasa-arm64-02.txt @@ -0,0 +1,5 @@ +-c robocasa-arm64.txt +huggingface-hub==1.31.0 +mujoco==3.3.1 +numpy==2.2.5 +psutil==7.2.2 diff --git a/docker/locks/robocasa-arm64-03.txt b/docker/locks/robocasa-arm64-03.txt new file mode 100644 index 0000000..09450e2 --- /dev/null +++ b/docker/locks/robocasa-arm64-03.txt @@ -0,0 +1,5 @@ +-c robocasa-arm64.txt +huggingface-hub==1.31.0 +mujoco==3.3.1 +numpy==2.2.5 +psutil==7.2.2 diff --git a/docker/locks/robocasa-arm64-04.txt b/docker/locks/robocasa-arm64-04.txt new file mode 100644 index 0000000..09450e2 --- /dev/null +++ b/docker/locks/robocasa-arm64-04.txt @@ -0,0 +1,5 @@ +-c robocasa-arm64.txt +huggingface-hub==1.31.0 +mujoco==3.3.1 +numpy==2.2.5 +psutil==7.2.2 diff --git a/docker/locks/robocasa-arm64.txt b/docker/locks/robocasa-arm64.txt new file mode 100644 index 0000000..3f2f8be --- /dev/null +++ b/docker/locks/robocasa-arm64.txt @@ -0,0 +1,212 @@ +# Recorded from installed Linux images, 2026-09-17. +# arm64 files reuse portable versions; require native build validation. +absl-py==2.5.0 +aiohappyeyeballs==2.7.1 +aiohttp==3.14.3 +aiosignal==1.4.0 +annotated-types==0.8.0 +anyio==4.15.1 +argcomplete==1.8.1 +async-timeout==5.0.1 +attrs==26.1.0 +av==17.1.0 +beautifulsoup4==4.15.0 +black==26.5.1 +blinker==1.9.0 +catkin-pkg-modules==1.1.1 +certifi==2026.7.22 +chardet==7.6.0 +charset-normalizer==3.5.1 +click==8.5.0 +cloudpickle==3.1.2 +cmake==4.4.3 +cmeel==0.61.0 +cmeel-assimp==6.0.5 +cmeel-boost==1.90.0 +cmeel-console-bridge==1.0.2.3 +cmeel-octomap==1.10.0 +cmeel-qhull==8.0.2.1 +cmeel-tinyxml2==11.0.0 +cmeel-urdfdom==6.0.0 +cmeel-zlib==1.3.2 +coal==3.0.3 +colcon-argcomplete==0.3.3 +colcon-bash==0.5.0 +colcon-cd==0.2.1 +colcon-cmake==0.2.30 +colcon-common-extensions==0.3.0 +colcon-core==0.21.2 +colcon-defaults==0.2.9 +colcon-devtools==0.3.0 +colcon-library-path==0.2.1 +colcon-metadata==0.3.1 +colcon-notification==0.3.2 +colcon-output==0.2.14 +colcon-package-information==0.4.1 +colcon-package-selection==0.2.10 +colcon-parallel-executor==0.3.0 +colcon-pkg-config==0.1.0 +colcon-powershell==0.5.0 +colcon-python-setup-py==0.2.9 +colcon-recursive-crawl==0.2.3 +colcon-ros==0.5.0 +colcon-test-result==0.3.8 +colcon-zsh==0.5.0 +cryptography==3.4.8 +cyclonedds==0.10.2 +daqp==0.9.1 +datasets==3.6.0 +dbus-python==1.2.18 +deepdiff==8.6.2 +delegator-py==0.1.1 +diffusers==0.40.0 +dill==0.3.8 +distlib==0.3.4 +distro==1.7.0 +docutils==0.17.1 +draccus==0.10.0 +eigenpy==3.13.0 +einops==0.8.2 +empy==3.3.4 +etils==1.13.0 +evdev==1.9.3 +exceptiongroup==1.3.1 +farama-notifications==0.0.6 +filelock==3.32.3 +flake8==4.0.1 +flask==3.1.3 +frozenlist==1.8.0 +fsspec==2025.3.0 +gdown==6.3.0 +gitdb==4.0.12 +gitpython==3.1.62 +glfw==2.10.2 +glob2==0.7 +grpcio==1.84.0 +gym==0.26.2 +gym-notices==0.1.0 +gymnasium==0.29.1 +h11==0.16.0 +h5py==3.16.0 +hf-xet==1.6.0 +hidapi==0.15.0 +httpcore==1.0.9 +httplib2==0.20.2 +httpx==0.28.1 +idna==3.19 +imageio==2.37.4 +imageio-ffmpeg==0.6.0 +importlib-metadata==9.0.1 +importlib-resources==7.1.0 +iniconfig==2.3.0 +isort==9.0.1 +itsdangerous==2.2.0 +jeepney==0.7.1 +jinja2==3.1.6 +jsonlines==4.0.0 +keyring==23.5.0 +lark==1.1.1 +launchpadlib==1.10.16 +lazr-restfulclient==0.14.4 +lazr-uri==1.0.6 +lerobot==0.3.3 +libcoal==3.0.3 +libpinocchio==4.1.0 +llvmlite==0.44.0 +lxml==6.1.3 +markdown==3.10.3 +markdown-it-py==4.2.0 +markupsafe==3.0.3 +mccabe==0.6.1 +mdurl==0.1.2 +mergedeep==1.3.4 +mink==0.0.5 +more-itertools==8.10.0 +mpi4py==3.1.3 +mpmath==1.3.0 +multidict==6.8.0 +multiprocess==0.70.16 +mypy-extensions==1.1.0 +netifaces==0.11.0 +networkx==3.4.2 +notify2==0.3 +numba==0.61.2 +oauthlib==3.2.0 +opencv-python==5.0.0.93 +opencv-python-headless==5.0.0.93 +orderly-set==5.5.0 +packaging==26.3 +pandas==2.3.3 +pathspec==1.1.1 +pexpect==4.9.0 +pillow==12.3.0 +pin==4.1.0 +pink==0.8.1 +platformdirs==4.11.9 +pluggy==1.6.0 +propcache==0.5.4 +protobuf==3.19.6 +ptyprocess==0.7.0 +py==1.10.0 +pyarrow==25.0.1 +pycodestyle==2.8.0 +pydantic==2.13.5 +pydantic-core==2.46.5 +pydocstyle==6.1.1 +pyflakes==2.4.0 +pygame==2.6.1 +pygments==2.21.0 +pygobject==3.42.1 +pyjwt==2.3.0 +pynput==1.8.2 +pyopengl==3.1.10 +pyparsing==2.4.7 +pyserial==3.5 +pysocks==1.7.1 +pytest==9.1.1 +python-apt==2.4.0+ubuntu4.1 +python-dateutil==2.9.0.post0 +python-xlib==0.33 +pytokens==0.4.1 +pytz==2026.3.post1 +pyyaml==6.0.3 +pyyaml-include==1.4.1 +qpsolvers==4.13.0 +quadprog==0.1.13 +regex==2026.9.10 +requests==2.34.2 +rerun-sdk==0.22.1 +rich==15.0.0 +rich-click==1.9.9 +roman==3.3 +rosdistro-modules==1.1.0 +rospkg-modules==1.6.3 +safetensors==0.8.0 +scipy==1.15.3 +secretstorage==3.3.1 +sentry-sdk==2.69.2 +six==1.17.0 +smmap==5.0.3 +snowballstemmer==2.2.0 +soupsieve==2.9.2 +sympy==1.14.0 +tensorboard==2.20.0 +tensorboard-data-server==0.7.2 +termcolor==3.3.0 +tianshou==0.4.10 +toml==0.10.2 +tomli==2.4.1 +torch==2.7.1 +torchvision==0.22.1 +tqdm==4.70.1 +typing-extensions==4.16.0 +typing-inspect==0.9.0 +typing-inspection==0.4.4 +tzdata==2026.4 +urllib3==2.8.0 +wadllib==1.3.6 +wandb==0.25.0 +werkzeug==3.1.8 +xxhash==4.0.1 +yarl==1.25.1 +zipp==4.1.0 diff --git a/docker/locks/robocasa-assets.digest b/docker/locks/robocasa-assets.digest new file mode 100644 index 0000000..7e47e66 --- /dev/null +++ b/docker/locks/robocasa-assets.digest @@ -0,0 +1 @@ +c382a37fa197debe8395fa0c43675b8efbee35e050227295374659dc707326c4 - diff --git a/docker/locks/ros-amd64-01.txt b/docker/locks/ros-amd64-01.txt new file mode 100644 index 0000000..bb761fe --- /dev/null +++ b/docker/locks/ros-amd64-01.txt @@ -0,0 +1,5 @@ +-c ros-amd64.txt +huggingface-hub==0.27.0 +mujoco==3.2.3 +numpy==1.26.4 +psutil==5.9.0 diff --git a/docker/locks/ros-amd64-02.txt b/docker/locks/ros-amd64-02.txt new file mode 100644 index 0000000..bb761fe --- /dev/null +++ b/docker/locks/ros-amd64-02.txt @@ -0,0 +1,5 @@ +-c ros-amd64.txt +huggingface-hub==0.27.0 +mujoco==3.2.3 +numpy==1.26.4 +psutil==5.9.0 diff --git a/docker/locks/ros-amd64-03.txt b/docker/locks/ros-amd64-03.txt new file mode 100644 index 0000000..bb761fe --- /dev/null +++ b/docker/locks/ros-amd64-03.txt @@ -0,0 +1,5 @@ +-c ros-amd64.txt +huggingface-hub==0.27.0 +mujoco==3.2.3 +numpy==1.26.4 +psutil==5.9.0 diff --git a/docker/locks/ros-amd64-04.txt b/docker/locks/ros-amd64-04.txt new file mode 100644 index 0000000..bb761fe --- /dev/null +++ b/docker/locks/ros-amd64-04.txt @@ -0,0 +1,5 @@ +-c ros-amd64.txt +huggingface-hub==0.27.0 +mujoco==3.2.3 +numpy==1.26.4 +psutil==5.9.0 diff --git a/docker/locks/ros-amd64-05.txt b/docker/locks/ros-amd64-05.txt new file mode 100644 index 0000000..bb761fe --- /dev/null +++ b/docker/locks/ros-amd64-05.txt @@ -0,0 +1,5 @@ +-c ros-amd64.txt +huggingface-hub==0.27.0 +mujoco==3.2.3 +numpy==1.26.4 +psutil==5.9.0 diff --git a/docker/locks/ros-amd64-06.txt b/docker/locks/ros-amd64-06.txt new file mode 100644 index 0000000..536f6cd --- /dev/null +++ b/docker/locks/ros-amd64-06.txt @@ -0,0 +1,7 @@ +-c ros-amd64.txt +huggingface-hub==0.27.0 +mujoco==3.2.3 +numpy==1.26.4 +psutil==5.9.0 +setuptools==59.6.0 +wheel==0.43.0 diff --git a/docker/locks/ros-amd64-07.txt b/docker/locks/ros-amd64-07.txt new file mode 100644 index 0000000..4cccf69 --- /dev/null +++ b/docker/locks/ros-amd64-07.txt @@ -0,0 +1,6 @@ +-c ros-amd64.txt +huggingface-hub==0.27.0 +mujoco==3.2.3 +numpy==1.26.4 +psutil==5.9.0 +setuptools==77.0.3 diff --git a/docker/locks/ros-amd64-08.txt b/docker/locks/ros-amd64-08.txt new file mode 100644 index 0000000..bb761fe --- /dev/null +++ b/docker/locks/ros-amd64-08.txt @@ -0,0 +1,5 @@ +-c ros-amd64.txt +huggingface-hub==0.27.0 +mujoco==3.2.3 +numpy==1.26.4 +psutil==5.9.0 diff --git a/docker/locks/ros-amd64-09.txt b/docker/locks/ros-amd64-09.txt new file mode 100644 index 0000000..61ef04e --- /dev/null +++ b/docker/locks/ros-amd64-09.txt @@ -0,0 +1,5 @@ +-c ros-amd64.txt +huggingface-hub==0.25.2 +mujoco==3.2.3 +numpy==1.26.4 +psutil==5.9.0 diff --git a/docker/locks/ros-amd64-10.txt b/docker/locks/ros-amd64-10.txt new file mode 100644 index 0000000..bdf38cf --- /dev/null +++ b/docker/locks/ros-amd64-10.txt @@ -0,0 +1,5 @@ +-c ros-amd64.txt +huggingface-hub==0.25.2 +mujoco==3.2.3 +numpy==1.26.4 +psutil==5.9.8 diff --git a/docker/locks/ros-amd64-11.txt b/docker/locks/ros-amd64-11.txt new file mode 100644 index 0000000..bdf38cf --- /dev/null +++ b/docker/locks/ros-amd64-11.txt @@ -0,0 +1,5 @@ +-c ros-amd64.txt +huggingface-hub==0.25.2 +mujoco==3.2.3 +numpy==1.26.4 +psutil==5.9.8 diff --git a/docker/locks/ros-amd64-12.txt b/docker/locks/ros-amd64-12.txt new file mode 100644 index 0000000..08fdf90 --- /dev/null +++ b/docker/locks/ros-amd64-12.txt @@ -0,0 +1,7 @@ +-c ros-amd64.txt +huggingface-hub==0.25.2 +mujoco==3.2.3 +numpy==1.26.4 +psutil==5.9.8 +setuptools==59.6.0 +wheel==0.43.0 diff --git a/docker/locks/ros-amd64.txt b/docker/locks/ros-amd64.txt new file mode 100644 index 0000000..b482058 --- /dev/null +++ b/docker/locks/ros-amd64.txt @@ -0,0 +1,312 @@ +# ROS NumPy 1.26 ABI: Pinocchio 2.7 / Boost 1.83, explicit compatible closure. +absl-py==2.5.0 +addict==2.4.0 +aiohappyeyeballs==2.7.1 +aiohttp==3.14.3 +aiosignal==1.4.0 +annotated-types==0.8.0 +antlr4-python3-runtime==4.9.3 +anyio==4.15.1 +appdirs==1.4.4 +argcomplete==1.8.1 +asttokens==3.0.2 +async-timeout==5.0.1 +attrs==26.1.0 +beniget==0.4.1 +black==26.5.1 +blinker==1.9.0 +braceexpand==0.1.7 +brotli==1.0.9 +catkin-pkg-modules==1.1.1 +certifi==2026.7.22 +cffi==2.1.1 +chardet==7.6.0 +charset-normalizer==3.5.1 +click==8.5.0 +cloudpickle==3.1.2 +cmeel==0.61.0 +cmeel-assimp==5.4.3.1 +cmeel-boost==1.83.0 +cmeel-console-bridge==1.0.2.3 +cmeel-octomap==1.10.0 +cmeel-qhull==8.0.2.1 +cmeel-tinyxml==2.6.2.3 +cmeel-tinyxml2==11.0.0 +cmeel-urdfdom==3.1.1.1 +cmeel-zlib==1.3.2 +colcon-argcomplete==0.3.3 +colcon-bash==0.5.0 +colcon-cd==0.2.1 +colcon-cmake==0.2.30 +colcon-common-extensions==0.3.0 +colcon-core==0.21.2 +colcon-defaults==0.2.9 +colcon-devtools==0.3.0 +colcon-library-path==0.2.1 +colcon-metadata==0.3.1 +colcon-notification==0.3.2 +colcon-output==0.2.14 +colcon-package-information==0.4.1 +colcon-package-selection==0.2.10 +colcon-parallel-executor==0.3.0 +colcon-pkg-config==0.1.0 +colcon-powershell==0.5.0 +colcon-python-setup-py==0.2.9 +colcon-recursive-crawl==0.2.3 +colcon-ros==0.5.0 +colcon-test-result==0.3.8 +colcon-zsh==0.5.0 +colorlog==6.12.0 +comm==0.2.3 +configargparse==1.7.7 +contourpy==1.3.2 +cryptography==50.0.1 +cuda-bindings==13.3.1 +cuda-pathfinder==1.6.0 +cuda-toolkit==13.0.3.0 +cycler==0.12.1 +cyclonedds==0.10.2 +cython==3.3.0 +daqp==0.9.1 +dash==4.4.1 +dbus-python==1.2.18 +decorator==5.3.1 +delegator-py==0.1.1 +diffusers==0.11.1 +distlib==0.3.4 +distro==1.7.0 +docstring-parser==0.18.0 +docutils==0.17.1 +eigenpy==3.5.1 +einops==0.8.0 +embreex==4.4.0 +empy==3.3.4 +etils==1.13.0 +exceptiongroup==1.3.1 +executing==2.2.1 +fastjsonschema==2.22.2 +filelock==3.32.3 +flake8==4.0.1 +flask==3.1.3 +fonttools==4.65.0 +frozenlist==1.8.0 +fs==2.4.12 +fsspec==2026.7.0 +ftfy==6.1.1 +gast==0.5.2 +glfw==2.10.2 +glob2==0.7 +google-auth==2.58.0 +google-genai==1.30.0 +grpcio==1.84.0 +grpcio-tools==1.84.0 +h11==0.16.0 +h5py==3.16.0 +hpp-fcl==2.4.4 +httpcore==1.0.9 +httplib2==0.20.2 +httpx==0.28.1 +hydra-core==1.3.7 +idna==3.19 +imageio==2.37.4 +importlib-metadata==9.0.1 +importlib-resources==7.1.0 +iniconfig==1.1.1 +iopath==0.1.10 +ipython==8.39.0 +ipywidgets==8.1.9 +isort==9.0.1 +itsdangerous==2.2.0 +janus==2.0.0 +jedi==0.20.0 +jeepney==0.7.1 +jinja2==3.1.6 +joblib==1.6.0 +jsonschema==4.26.0 +jsonschema-specifications==2025.9.1 +jupyter-core==5.9.1 +jupyterlab-widgets==3.0.17 +keyring==23.5.0 +kiwisolver==1.5.1 +lark==1.1.1 +launchpadlib==1.10.16 +lazr-restfulclient==0.14.4 +lazr-uri==1.0.6 +lazy-loader==0.5 +loop-rate-limiters==1.2.0 +lxml==4.8.0 +lz4==3.1.3+dfsg +manifold3d==3.5.3 +mapbox-earcut==2.1.0 +markdown-it-py==4.2.0 +markupsafe==3.0.3 +matplotlib==3.9.3 +matplotlib-inline==0.2.2 +mccabe==0.6.1 +mdurl==0.1.2 +meshcat==0.3.2 +meshcat-shapes==1.0.0 +mink==1.3.0 +more-itertools==8.10.0 +mpi4py==3.1.3 +mpmath==1.3.0 +msgspec==0.21.1 +mujoco-mpc==0.1.0 +multidict==6.8.0 +mypy-extensions==1.1.0 +narwhals==2.26.0 +nbformat==5.11.1 +nest-asyncio==1.6.0 +netifaces==0.11.0 +networkx==3.4.2 +nodeenv==1.10.0 +notify2==0.3 +nvidia-cublas==13.1.1.3 +nvidia-cuda-cupti==13.0.85 +nvidia-cuda-nvrtc==13.0.88 +nvidia-cuda-runtime==13.0.96 +nvidia-cudnn-cu13==9.24.0.43 +nvidia-cufft==12.0.0.61 +nvidia-cufile==1.15.1.6 +nvidia-curand==10.4.0.35 +nvidia-cusolver==12.0.4.66 +nvidia-cusparse==12.6.3.3 +nvidia-cusparselt-cu13==0.8.1 +nvidia-nccl-cu13==2.30.7 +nvidia-nvjitlink==13.3.33 +nvidia-nvshmem-cu13==3.4.5 +nvidia-nvtx==13.0.85 +oauthlib==3.2.0 +omegaconf==2.3.1 +ompl==2.0.1 +open3d==0.19.0 +opencv-python==4.10.0.84 +opencv-python-headless==4.10.0.84 +orjson==3.12.0 +packaging==26.3 +pandas==2.3.3 +parso==0.8.7 +pathspec==1.1.1 +pexpect==4.9.0 +pillow==11.0.0 +pin==2.7.0 +pin-pink==4.4.0 +pink==0.8.1 +platformdirs==4.11.9 +plotly==7.1.0 +pluggy==0.13.0 +ply==3.11 +plyfile==1.1.3 +portalocker==4.3.2 +prompt-toolkit==3.0.53 +propcache==0.5.4 +protobuf==7.36.1 +proxsuite==0.7.3 +ptyprocess==0.7.0 +pure-eval==0.2.4 +py==1.10.0 +py-cpuinfo==9.0.0 +pyasn1==0.6.4 +pyasn1-modules==0.4.2 +pycairo==1.20.1 +pycocotools==2.0.8 +pycodestyle==2.8.0 +pycollada==0.9.3 +pycparser==3.0 +pydantic==2.13.5 +pydantic-core==2.46.5 +pydocstyle==6.1.1 +pydot==1.4.2 +pyflakes==2.4.0 +pygments==2.21.0 +pygobject==3.42.1 +pyjwt==2.3.0 +pyngrok==8.1.2 +pyopengl==3.1.10 +pyparsing==3.3.2 +pyqt5==5.15.6 +pyqt5-sip==12.9.1 +pyquaternion==0.9.9 +pytest==6.2.5 +python-apt==2.4.0+ubuntu4.1 +python-dateutil==2.9.0.post0 +python-fcl==0.7.0.11 +pythran==0.10.0 +pytokens==0.4.1 +pytz==2026.3.post1 +pyvers==0.2.3 +pyyaml==6.0.3 +pyzmq==27.2.0 +qpsolvers==4.13.0 +quadprog==0.1.13 +referencing==0.37.0 +regex==2024.11.6 +requests==2.34.2 +retrying==1.4.2 +rich==15.0.0 +rich-click==1.9.9 +roman==3.3 +rosdep==0.27.0 +rosdep-modules==0.27.0 +rosdistro-modules==1.1.0 +rospkg-modules==1.6.3 +rpds-py==0.30.0 +rtree==1.4.1 +safetensors==0.8.0 +scikit-image==0.25.2 +scikit-learn==1.7.2 +scipy==1.15.3 +seaborn==0.13.2 +secretstorage==3.3.1 +shapely==2.1.2 +sharedarray==3.2.4 +six==1.17.0 +snowballstemmer==2.2.0 +stack-data==0.6.3 +svg-path==7.1 +sympy==1.14.0 +tenacity==9.1.4 +tensorboardx==2.6.5 +tensordict==0.14.2 +threadpoolctl==3.7.0 +tifffile==2025.5.10 +timm==1.0.17 +tokenizers==0.21.4 +toml==0.10.2 +tomli==2.4.1 +torch==2.14.0+cu130 +torch-geometric==2.8.0.post1 +torchvision==0.29.0+cu130 +tornado==6.5.10 +tqdm==4.70.1 +traitlets==5.16.1 +transformers==4.47.1 +transforms3d==0.3.1 +trimesh==4.5.3 +triton==3.8.0 +typeguard==4.6.0 +typing-extensions==4.16.0 +typing-inspection==0.4.4 +tyro==1.0.16 +tzdata==2026.4 +u-msgpack-python==2.8.0 +ufolib2==0.13.1 +ultralytics==8.3.52 +ultralytics-thop==2.1.6 +unicodedata2==14.0.0 +urllib3==2.8.0 +vcstool==0.3.0 +vhacdx==0.0.10 +viser==0.2.23 +wadllib==1.3.6 +wcwidth==0.8.3 +webdataset==1.0.2 +websockets==15.0.1 +werkzeug==3.1.8 +widgetsnbextension==4.0.16 +xatlas==0.0.11 +xxhash==4.0.1 +yapf==0.40.1 +yarl==1.25.1 +yourdfpy==0.0.56 +zipp==4.1.0 diff --git a/docker/locks/ros-arm64-01.txt b/docker/locks/ros-arm64-01.txt new file mode 100644 index 0000000..f9e98c7 --- /dev/null +++ b/docker/locks/ros-arm64-01.txt @@ -0,0 +1,5 @@ +-c ros-arm64.txt +huggingface-hub==0.27.0 +mujoco==3.2.3 +numpy==1.26.4 +psutil==5.9.0 diff --git a/docker/locks/ros-arm64-02.txt b/docker/locks/ros-arm64-02.txt new file mode 100644 index 0000000..ba35178 --- /dev/null +++ b/docker/locks/ros-arm64-02.txt @@ -0,0 +1,7 @@ +-c ros-arm64.txt +huggingface-hub==0.27.0 +mujoco==3.2.3 +numpy==1.26.4 +psutil==5.9.0 +setuptools==59.6.0 +wheel==0.43.0 diff --git a/docker/locks/ros-arm64.txt b/docker/locks/ros-arm64.txt new file mode 100644 index 0000000..9cf8a91 --- /dev/null +++ b/docker/locks/ros-arm64.txt @@ -0,0 +1,293 @@ +# ROS NumPy 1.26 ABI: Pinocchio 2.7 / Boost 1.83, explicit compatible closure. +absl-py==2.5.0 +addict==2.4.0 +aiohappyeyeballs==2.7.1 +aiohttp==3.14.3 +aiosignal==1.4.0 +annotated-types==0.8.0 +antlr4-python3-runtime==4.9.3 +anyio==4.15.1 +appdirs==1.4.4 +argcomplete==1.8.1 +asttokens==3.0.2 +async-timeout==5.0.1 +attrs==26.1.0 +beniget==0.4.1 +black==26.5.1 +blinker==1.9.0 +braceexpand==0.1.7 +brotli==1.0.9 +catkin-pkg-modules==1.1.1 +certifi==2026.7.22 +cffi==2.1.1 +chardet==7.6.0 +charset-normalizer==3.5.1 +click==8.5.0 +cloudpickle==3.1.2 +cmeel==0.61.0 +cmeel-assimp==5.4.3.1 +cmeel-boost==1.83.0 +cmeel-console-bridge==1.0.2.3 +cmeel-octomap==1.10.0 +cmeel-qhull==8.0.2.1 +cmeel-tinyxml==2.6.2.3 +cmeel-tinyxml2==11.0.0 +cmeel-urdfdom==3.1.1.1 +cmeel-zlib==1.3.2 +colcon-argcomplete==0.3.3 +colcon-bash==0.5.0 +colcon-cd==0.2.1 +colcon-cmake==0.2.30 +colcon-common-extensions==0.3.0 +colcon-core==0.21.2 +colcon-defaults==0.2.9 +colcon-devtools==0.3.0 +colcon-library-path==0.2.1 +colcon-metadata==0.3.1 +colcon-notification==0.3.2 +colcon-output==0.2.14 +colcon-package-information==0.4.1 +colcon-package-selection==0.2.10 +colcon-parallel-executor==0.3.0 +colcon-pkg-config==0.1.0 +colcon-powershell==0.5.0 +colcon-python-setup-py==0.2.9 +colcon-recursive-crawl==0.2.3 +colcon-ros==0.5.0 +colcon-test-result==0.3.8 +colcon-zsh==0.5.0 +colorlog==6.12.0 +comm==0.2.3 +configargparse==1.7.7 +contourpy==1.3.2 +cryptography==50.0.1 +cycler==0.12.1 +cyclonedds==0.10.2 +cython==3.3.0 +daqp==0.9.1 +dash==4.4.1 +dbus-python==1.2.18 +decorator==5.3.1 +delegator-py==0.1.1 +diffusers==0.11.1 +distlib==0.3.4 +distro==1.7.0 +docstring-parser==0.18.0 +docutils==0.17.1 +eigenpy==3.5.1 +einops==0.8.0 +embreex==4.4.0 +empy==3.3.4 +etils==1.13.0 +exceptiongroup==1.3.1 +executing==2.2.1 +fastjsonschema==2.22.2 +filelock==3.32.3 +flake8==4.0.1 +flask==3.1.3 +fonttools==4.65.0 +frozenlist==1.8.0 +fs==2.4.12 +fsspec==2026.7.0 +ftfy==6.1.1 +gast==0.5.2 +glfw==2.10.2 +glob2==0.7 +google-auth==2.58.0 +google-genai==1.30.0 +grpcio==1.84.0 +grpcio-tools==1.84.0 +h11==0.16.0 +h5py==3.16.0 +hpp-fcl==2.4.4 +httpcore==1.0.9 +httplib2==0.20.2 +httpx==0.28.1 +hydra-core==1.3.7 +idna==3.19 +imageio==2.37.4 +importlib-metadata==9.0.1 +importlib-resources==7.1.0 +iniconfig==1.1.1 +iopath==0.1.10 +ipython==8.39.0 +ipywidgets==8.1.9 +isort==9.0.1 +itsdangerous==2.2.0 +janus==2.0.0 +jedi==0.20.0 +jeepney==0.7.1 +jinja2==3.1.6 +joblib==1.6.0 +jsonschema==4.26.0 +jsonschema-specifications==2025.9.1 +jupyter-core==5.9.1 +jupyterlab-widgets==3.0.17 +keyring==23.5.0 +kiwisolver==1.5.1 +lark==1.1.1 +launchpadlib==1.10.16 +lazr-restfulclient==0.14.4 +lazr-uri==1.0.6 +lazy-loader==0.5 +loop-rate-limiters==1.2.0 +lxml==4.8.0 +lz4==3.1.3+dfsg +manifold3d==3.5.3 +mapbox-earcut==2.1.0 +markdown-it-py==4.2.0 +markupsafe==3.0.3 +matplotlib==3.9.3 +matplotlib-inline==0.2.2 +mccabe==0.6.1 +mdurl==0.1.2 +meshcat==0.3.2 +meshcat-shapes==1.0.0 +mink==1.3.0 +more-itertools==8.10.0 +mpi4py==3.1.3 +mpmath==1.3.0 +msgspec==0.21.1 +mujoco-mpc==0.1.0 +multidict==6.8.0 +mypy-extensions==1.1.0 +narwhals==2.26.0 +nbformat==5.11.1 +nest-asyncio==1.6.0 +netifaces==0.11.0 +networkx==3.4.2 +nodeenv==1.10.0 +notify2==0.3 +oauthlib==3.2.0 +omegaconf==2.3.1 +ompl==2.0.1 +open3d==0.19.0 +opencv-python==4.10.0.84 +opencv-python-headless==4.10.0.84 +orjson==3.12.0 +packaging==26.3 +pandas==2.3.3 +parso==0.8.7 +pathspec==1.1.1 +pexpect==4.9.0 +pillow==11.0.0 +pin==2.7.0 +pin-pink==4.4.0 +pink==0.8.1 +platformdirs==4.11.9 +plotly==7.1.0 +pluggy==0.13.0 +ply==3.11 +plyfile==1.1.3 +portalocker==4.3.2 +prompt-toolkit==3.0.53 +propcache==0.5.4 +protobuf==7.36.1 +proxsuite==0.7.3 +ptyprocess==0.7.0 +pure-eval==0.2.4 +py==1.10.0 +py-cpuinfo==9.0.0 +pyasn1==0.6.4 +pyasn1-modules==0.4.2 +pycairo==1.20.1 +pycocotools==2.0.8 +pycodestyle==2.8.0 +pycollada==0.9.3 +pycparser==3.0 +pydantic==2.13.5 +pydantic-core==2.46.5 +pydocstyle==6.1.1 +pydot==1.4.2 +pyflakes==2.4.0 +pygments==2.21.0 +pygobject==3.42.1 +pyjwt==2.3.0 +pyngrok==8.1.2 +pyopengl==3.1.10 +pyparsing==3.3.2 +pyqt5==5.15.6 +pyqt5-sip==12.9.1 +pyquaternion==0.9.9 +pytest==6.2.5 +python-apt==2.4.0+ubuntu4.1 +python-dateutil==2.9.0.post0 +python-fcl==0.7.0.11 +pythran==0.10.0 +pytokens==0.4.1 +pytz==2026.3.post1 +pyvers==0.2.3 +pyyaml==6.0.3 +pyzmq==27.2.0 +qpsolvers==4.13.0 +quadprog==0.1.13 +referencing==0.37.0 +regex==2024.11.6 +requests==2.34.2 +retrying==1.4.2 +rich==15.0.0 +rich-click==1.9.9 +roman==3.3 +rosdep==0.27.0 +rosdep-modules==0.27.0 +rosdistro-modules==1.1.0 +rospkg-modules==1.6.3 +rpds-py==0.30.0 +rtree==1.4.1 +safetensors==0.8.0 +scikit-image==0.25.2 +scikit-learn==1.7.2 +scipy==1.15.3 +seaborn==0.13.2 +secretstorage==3.3.1 +shapely==2.1.2 +sharedarray==3.2.4 +six==1.17.0 +snowballstemmer==2.2.0 +stack-data==0.6.3 +svg-path==7.1 +sympy==1.14.0 +tenacity==9.1.4 +tensorboardx==2.6.5 +tensordict==0.14.2 +threadpoolctl==3.7.0 +tifffile==2025.5.10 +timm==1.0.17 +tokenizers==0.21.4 +toml==0.10.2 +tomli==2.4.1 +torch==2.14.0 +torch-geometric==2.8.0.post1 +torchvision==0.29.0 +tornado==6.5.10 +tqdm==4.70.1 +traitlets==5.16.1 +transformers==4.47.1 +transforms3d==0.3.1 +trimesh==4.5.3 +typeguard==4.6.0 +typing-extensions==4.16.0 +typing-inspection==0.4.4 +tyro==1.0.16 +tzdata==2026.4 +u-msgpack-python==2.8.0 +ufolib2==0.13.1 +ultralytics==8.3.52 +ultralytics-thop==2.1.6 +unicodedata2==14.0.0 +urllib3==2.8.0 +vcstool==0.3.0 +vhacdx==0.0.10 +viser==0.2.23 +wadllib==1.3.6 +wcwidth==0.8.3 +webdataset==1.0.2 +websockets==15.0.1 +werkzeug==3.1.8 +widgetsnbextension==4.0.16 +xatlas==0.0.11 +xxhash==4.0.1 +yapf==0.40.1 +yarl==1.25.1 +yourdfpy==0.0.56 +zipp==4.1.0 diff --git a/docker/locks/ros-snapshot.asc b/docker/locks/ros-snapshot.asc new file mode 100644 index 0000000..874df50 --- /dev/null +++ b/docker/locks/ros-snapshot.asc @@ -0,0 +1,139 @@ +-----BEGIN PGP PUBLIC KEY BLOCK----- +Comment: Hostname: +Version: Hockeypuck 2.2 + +xsDNBFvzKm0BDADDLMeQw+6a1vvnXZZrMTwV6YsYnke8CE5Mlji1Z5HHBkXGDo+s +Ovi7EV/GfWm9GN+Dvfc5CWX/x9ig+ud6JMYUa9628RCipkk/gBKNDluTTvcUu6OX +2qWVRls8Gf8rPW4qThHjVw08DJPcpNAIi+tFTRpGj949P7hurjp5/bJ21LJVt7dL +RMSqbd+K+++2WsI0cgcC5zu++6p/Exkrm16Dx2nC0s2qiATnDYM3xMuF+PqGtUM5 +7tYpZmNuvqXs9giYq29N3/1zufJjFnhs80hWdNXMHsfGrInpkROZqhVsTutgKDIR +4o6KocftGAwlDM1tjEs1OXSm+Q5LuS+AFgH2jKjh9AJ2K/0sy+pHIvLtvmG/vEch +M1NW1ge53QrqPjpU7zCrLYqOu78rEkC4ImxVeQt3Dn+9+D+APxM1xuBWJFsiFG+h +Lmh2TYaoZhyBAbIpablnxZR0fuXy6OFiYws4r2uSmKuu0wnYl6OTU0QiBP9+Z1Mw +wthXbnREOzh9x7EAEQEAAc0nUk9TIFNuYXBzaG90IGJ1aWxkZXIgPHJvc2J1aWxk +QHJvcy5vcmc+wsEUBBMBCgA+FiEES2PPj95JdG6Y+gHdrRm6s8vxJeoFAlwaYp0C +GwMFCQlm+4oFCwkIBwIGFQoJCAsCBBYCAwECHgECF4AACgkQrRm6s8vxJeqyzwv9 +GUoCWOB3iPKGmaLxCecTY+akE/XRDLTFYQqXe8Qk52fW5lYiNAfvhoSQDrmHlGuI +ZOsJoNbvIWqU1Oxii5iGBbOfhxOK60Y8PEO3GLa+yAY0PAe8dxKZV1/VLDuNKoBk +8hPOUheGca87FwOE9kINYlo0gsrYymycYuSQxKYZVybB1and6NQxoswYA5JzOsCp +U93hy7Mx9Osn+CH6V9+xrl4MrnXBypH3ceRNulT3kR1meQO1a32jDyHiW9jT2G8N +AaZXBusJXHwMPN1oshzneAdS3NhZlpI39c2S1ZyyHUFu3zExpamG6fw9HT+jX7zI +8MrRcvq/FlNnU4wS1UrqAjZhPzX2mxbxMUQ98c5Ur7d5r4jcfxUittoZahmF5ggw +iGZf3ROPeiDkg/vIs/jeTgt7HNBpRWr7snDlN1IzhgY5FUqFSq6kDaDUG7pl1Hu0 +7mPSyHEH9juS8mDbTwxgJ8xTxaP8b/06J2hv15d/i9dYR3Rgs+iS245gWHHZW6fX +zSdST1MgU3BhbnNob3QgYnVpbGRlciA8cm9zYnVpbGRAcm9zLm9yZz7CwPYEMAEK +ACAWIQRLY8+P3kl0bpj6Ad2tGbqzy/El6gUCXBpi0wIdIAAKCRCtGbqzy/El6qIr +DACdwzH2c11UCUhdHazbacA8DOWNAlWsX/URdeJTd7OeHbe5Gh4vRHFtzKVlSA6T +m6QtwXZI7F47qWX2j9o9BWm2NqAk3X17cBg6AffRtgjonb7VzR8z5vwCVJAErKct +MG1Vz3SU/cPxgNjXvW+lUzeYoil5OKHEkQu5fkdWCTmfVBWgzmwAQ8aC8Os+o30D +Q7Tsc5UP/U67/IakUJsOrPpc1Mu74V3f7A+enexwTdJ86NiHECMOdJvmtMyiSfwX +v48r8bEhgi9c6WXNldHEAFdaiZX7sY0w0ihQOGIEIQn3x+3S3ULoGcCVgL0BbfqV +7sox4ZuLc4qpVp/Vmm1qTe4hjyjN8W5KslxRV+HdnhSftOVIcnj5FyWA2RAC4rkm +kFQUD4y5qoQq3fraQP4Y7Xxe+6eFybCFrSVeiijPKiwpewlSgfhuwGdEqQkbjqdw +WgElPeBcBJ4zzZGibUoAgLiT2oMDt+nTl0k3nKuzGuwtgJsoCTzbYuC83f1HAXbX +RsvCwRQEEwEKAD4CGwMFCwkIBwIGFQoJCAsCBBYCAwECHgECF4AWIQRLY8+P3kl0 +bpj6Ad2tGbqzy/El6gUCaK8tzAUJEAu33wAKCRCtGbqzy/El6uLvC/0VEIA3Cpj8 +2mZ5XZVX0OJ9qGTeLQ/nTgGd5aYG2xXCPAtPWzAAmMJtbCGq/kReEpdp5h3uNZzn +VMJ+o1ZkttgCZ1xYJ36XCrFKfVf4d2Bku/6K+bS47MyA3BeaFC8GaOFETlxhnGac +UOMi0Klfs5snpK7qGuVgk9IW9hUZ3ov3IC8QJGDBIrr98pp91lUmz2P4XduXufHU ++IJfBvjdaWLOYRcqKCOvYOCN+4OOGIs5DdrbxlQkP3oZeMtlCrtudsi4uqwDkNN9 +6KgAYB7xGNawlcJd04wEl2GeyUF01VXjw3JlhOavwlEhyRTeOQT6wNhMJ5CXYKid +1r047pZ5cR6dmTDEd9lz8lEYVFN3pj71B+UNy8xEzKLpP8C3bE0Fw85SO7cL3DFU +0yW8FdyS5ZbXzDQ8gbA4PWl9viHSAeLgS1Nxz3VFvvQHSboIX6nYRx+Ng821gPUG +55Boanv7PEUu08hvnNXF4b1bfohzyYtExa7GD/Jl9HJzoTdRipLrNtfCwRQEEwEK +AD4CGwMFCwkIBwIGFQoJCAsCBBYCAwECHgECF4AWIQRLY8+P3kl0bpj6Ad2tGbqz +y/El6gUCaEG70wUJFbCeZgAKCRCtGbqzy/El6nMoC/9V98HUiQk3MzXuNBXPLeYm +rpDKG4OINBRWgp/x8TTN1Gv7HRwEoVPhhIHeJ9W0rz1s3ANz3RDkpdYx1IR4bE3s +SYOGMZy4UCiifwtMKVJxkUp+rICRMBoGJ4V2lbbFWTaGzTPMhGr1XfLa4s1t3AhW +qTioB+zBIAhgpwAug7W8j1zkkKmHNEthd4dPJ+2+8ZrQKlQ/fCpMKp9dKXDGhpbv +KKUMdAS8fPL4D5RjOr1y02yDvQIMs/+AHeeuHbHgn2a2VLC78zsWemjMAlkIq0qf +L1YXLsX2ULcwk1pkOncRYAcs/mDMdjG5/zpBpOdiQXhLg6icHqm9akdGrzglKUbk +r3RplXa9nPhhwqseUa1moXY/Nv2HtX4qMmrGn+LtyZErJTFjZn9KPT2MB56Vw5bs +X6e/RkhybCL61cLKsr9Lc8KpRB324dC6BMWCzp30KDtmUGI3eyA9wXwiroxsf7va +I9KjxFPBaiN9Hibd8WnFUuIIHBJyly/xkzYHvuHgO/XCwRQEEwEKAD4CGwMFCwkI +BwIGFQoJCAsCBBYCAwECHgECF4AWIQRLY8+P3kl0bpj6Ad2tGbqzy/El6gUCZVuL +mQUJDEj6NAAKCRCtGbqzy/El6o48C/9XmjxCi3eckHqFNM1iC6tgINuSWPHr1K+N +Hx+ZBPbG0hynTgLltvHt98ObEwDq0sE2PSw1y7NWJVEhHDXs2n+wfksUUFChqkQV +BDje5A8ovpHwGp+eXx+lk817ER49ctQjzuyZj8/VMU0yqN9dy89IjOHBAPdfnIKD +CieuVfShzCNRpI3g4Q75cbADjO5Dn6es8IlXw5kmsDGrTQxNhjWa6oPZxpGOX1Z6 +bjjeFvdAzi/PwYXcgQOu2uN59xFAoCouadX6L5IzySeOxGsAyoMJ4AZRxoSLfQgF +tEzS98oNLpldM89H2eZMntfGSrXBIKnEBkiIfjurofj2P9Cx1vQ8NrgzUebFaYUF +MOH8gI91vHw7lwYhpm2sCJkoVoHw5iuNn1ytpcYborsNVpXi1KTRb4jINlkvAyLA +8U9U9jEOQcARaT/cgdFibQD1MolgeLMmTYsgkhgD34++dZ+yhdu5OCHk5SpvplI1 +24DTSGI38fGscgvK5TTQHxKNY3na5mHCwQ4EEwEKADgCGwMFCwkIBwIGFQoJCAsC +BBYCAwECHgECF4AWIQRLY8+P3kl0bpj6Ad2tGbqzy/El6gUCX9u5NwAKCRCtGbqz +y/El6qUpC/45VU8+B1zVW88YcKlsgm+/ZJ7kGUTLVY4QYQDWThwW+x3umghbp06G +7UCLicj0oEdEmapAe0pS6H5IY+sBkwvLFQo0J56jrFg5QNl2QmqTKDUalH9j1P4F +lPgCheNkoYkKGgaYJUVD2NWpyaSa8NLWulx/R0jonGdtvSHfgjRLsnessS+ZNp0H +kyHqmH5A/CjFT7oSESBLbA9U/zMZ6t694+GPdcOZQcqeowGJ0pOzZTXkQ634iriE +ZovL7OGFi9sAx/7t5G95io03x9dX0rqT05aygJoJ3onCH3PnSndVByDwtR/nxHtz +6nefsMkX3D3qt5Uz3aIlh6Jrssj/5BYd0pBl+vPWcYq3J+LQ6rqB3ABCJBCxjRfN +42OULY2zGXJPFypZu0qUiWPcTTcbYLgNpSFZqhUz0hLyfwBrncCq0a4sHjg7Zsq6 +SQ+xS9gfOUR4CgCWRW17CPFuPzwmQvaYpnCK189qZX1F5jObGgqPeLIZu6RnS990 +eS3Ee/8UPfPCwRQEEwEKAD4CGwMFCwkIBwIGFQoJCAsCBBYCAwECHgECF4AWIQRL +Y8+P3kl0bpj6Ad2tGbqzy/El6gUCW/QkdwUJCWb7igAKCRCtGbqzy/El6mVvC/9N +Y/opQ8k+ORx/TBUF370wqHFSIaSTMnM5v0TLELFgUboILhujdij3WEyVMDneLECv +1qr4iRbUqwRqYKvj6FeSSlbBVMQH5ZSgN8statFEJcEOxyvzCGyLJMFu/y+ItE7l +Tzggdikz7kxei21T0bNilHLgvVvULAzIHuX+D0Qi5rzqNoMHkdAlYwvIzGjCDcCb +hIm3NvxqGSZQxgxJGh4P9p8Y/SJBKMK7jCQCFVThDis3Y1wguJebZNw/GwP03n+s +kC166q8u0EPvDdpa5kscGsvlMwhazKPbCq2GHpPz2+1fjPPDg8OngOXrmK92A61/ +oKVceUIEwve4/yAAUVIVSkiqFxBRf4DtdJVsaCoAXZnUR02oVNGIfWqJZ5rVSR75 +gzZc+37QcyQtzrNS0S/8zq9bRivt8ruwkErrT9dje2P3ta+SzZMkspynL/Qx29NU +7n+nHdz9chdvdNjo5IilrXKf5d3QZQ+LooaxFC32ARWckgnmXOuBckzHaF5p+oHO +wM0EW/MqbQEMAK5eG9iC5dMTw+svblPp5KgSzCXgLHJVHi5AIPo4NT/hy7ypKI51 +FcDkKLHsWlDCz//3KWiH04EWyb36+fTGzzJ16BjyNvvHKTcMYBV08s/RhuIYHbvN +0n4ukD8zRbaBgiGE0ipSLlIFtK/RpYlxKLIH2mH5RaXY/01fxJAyqAkgetJxxdKq +NN9RsCdUkgrDS+859R1lyrSsMIUDHr12u1AncpqeekjV3b0yFcQUh39JdDAG0Da/ +2J+WtIlJWoFZBFB9w/Hpxi7lTAP/8Y7IaWid6H1q3vQEZ+Kb7XRDozuqXrTKLjAW +2OWFvJR5BIBK3KdacAGDQ9IwaO+LycbtF+c0v8sm7ygr3ci2632rW1WrN3/yxc+v +P5wVvLbsc0UcNOKpKZevOtT//2FA9T+yCaM2D+guXWO87Objzvxjdwar6vl4ieoY +B6ieVADck0ciUkaSGaumwYr9gI7MDkM2np4Siq9Et3IJzqK9om7rPeYPEgd12Ilq +J1Dg3JdstzzE6wARAQABwsD8BBgBCgAmAhsMFiEES2PPj95JdG6Y+gHdrRm6s8vx +JeoFAmivLjAFCRALuEMACgkQrRm6s8vxJeqnXQv+OScJgZ2Mfnacfjg0o5Bkuxle +EZ7AFhTT4zE+pa/oSaGeAvqLfrTh2vCMGJR9q2932HfLZwnN7a6Ut8DUP0ONgF62 +QWJh0d4HkS3fIdMBDESO285uMUkw4eqBeiUujDND/GoRXh3hjrSWvg0DRtIjFtyH +GB6YA3mpCn+4DlUwMeZgnSdahdjSo0xQLtYeAY/k6b3UHDEuTlOVGbdIx/56XtEQ +6/mTBdpWTiqE3w/odLxFn7Ef90c7w3uXybSwvxaNxgO6w1JnarZgGROWFh3YRiod +/OoGvR5OxI5ZqFwK/DCIAM1K13uPZlJEfL4da970zcJKkt7dZGvjmqWVKrJ6oeit +KUyNRuN8EZ5nML2UuuodLHLa6DAwRhpyaHWYNu34fQb/eb5shIM+zHzX8SQcBm8Q +c/TwnI/rMo2o3ULSaGxXDG6G5/S4842962UbRElopaFsKhpB+9qRVENBM6/OBAEe +Ts7ZPSCrhSfrRnXIhHnmWz1uJnXVvrInpgoWW7T3wsD8BBgBCgAmAhsMFiEES2PP +j95JdG6Y+gHdrRm6s8vxJeoFAmhBu/cFCRWwnooACgkQrRm6s8vxJeq73wwAicJm +2HOd32mMg3MSqfto/Xnu9U/qLqwO0FdXPH48eTavd76FeGB9YQ3bwRbxh+mkZnNF +sxy2OdOEfFVdd71NSNHXf9FRVQioydft9XLUuFo6kNpqnhyT8lib62DSIVMXi0pU +ZJhIUXuZXgyz+zkHOdLWdQ6QiY98BReVmWUGoXrUastp68BTnP9lLANB1OCFCQK3 +TgM5tNsOW6sDXqUzgMOQMdbq0siVn1wBLCOr56OBawtneOMEmqWQ1QIElluOr1wx +YLY2plEkQsrPQpYQKmaVPERfYKw8e8W/h8gaCZxv1P6PdMzdTyfmt0zaB8ZSRVK0 +bn83/JVWpolI3wWTM9pvBBn24DKyC46M2js8cTPtTqoqCh5icvExT3oaJvVVJFR4 +AiTz5mGq9udyaopQ4A6Xvkji7AsrUF0tqkIYCgC1bvOZL0SjbZ7oPJL/D6PuwXdN +ER4CxJ/P0EqdnRufrgHgHUU0vN4oF55xonlVlX2zK2pYvO7DzepZj6R80ZYXwsD8 +BBgBCgAmAhsMFiEES2PPj95JdG6Y+gHdrRm6s8vxJeoFAmVbjAkFCQxI+jQACgkQ +rRm6s8vxJepPqwv/YFBICFpVrmWKCXy5grIatY4Uw8m+EQLMxZewulrNditSFMS7 +QFJ3LkDME5aW4DHXP+9HS0uPEM1w9FonsiRYmOfpmmeJXxGlCPHiPIqDcr02qw0d +0GhWufkIXO5AUHzpImvukJO+FzztNVRsZxD2Roj1gwt7RFyQjVvgk5AI/iNXrxbL +TkfRwzuhWhY/UuAPk6kxYAJUDW2EsU02fsdxO+5EjaNyLnYrcbNeba1p/jFpv4ZJ +lKOHFYRkVKolAoxhkmA8JwGwRaVqlrOuk9USDx2sJcg64rQJZlOjTonGidigAzQ9 +9OE0vAX2CqM8IoGS/0fniX3aaSyy3yTBYOZiObSPzFNQYDZdrXJ9tsIg+ZmgHPQX +5UtMIG+m81zTj36yuV5tITtpziRPDTza0m8pzb2ZegQMEodwH8NQ5dg/7OMcCOZY +UT6M3YGxa/1u/flmzA6nvkXLha7WTuxWCcLEtCCimKmunppl15F7b1ztnjLirA87 +mpnZSd0r4CizfoggwsD2BBgBCgAgAhsMFiEES2PPj95JdG6Y+gHdrRm6s8vxJeoF +Al/buXsACgkQrRm6s8vxJeqSxQwAkB/NGflvZwaf1EXWlDSBuW2onsBJq+gscgv5 +Bwt0Ar1r4ROvIDg67iojNLklA30tjORDQKEWv7QfjFqCUIGKMMeUdgF6Pk3fJ5Ue +93/dw6neEjg7BJCwMjRdX/PQPP8eLlPZyznj0DL22rs19GR1Wd7sWFvn9uzLgJM4 +2a4tDiakvf8eDvLIA1Zw2FtT+bJa9UH5vVLLCcfYfxM36OIxDWz93C1WrNf+W13W +23bnGdryRtbYgW3Q5lM2PDd5YKSmjIrnTuDaJ0yjhcKhNC5lSew5OXa0/UFlkULQ +AQBQ3Q+WcQ/5m57ezuNlQAxQFwL5TjYc+1afDp0F/zzs1iXbiNMG9rJSCbSyQXed +QpUiKWpyULbejfZuvUqaRP2KB+vuTDHCZ/0bSDmYyzXS4ojf7kjIS9mbOwHYbC5s +vM8p0mPFpmHCSPhSJFOa7BAFAqYHWbrLZslK9HYnOmJ8izEakbZM7oHAJRf8r/V+ +c6wsdVB9Y5DyyMTkcmf/zdXWoBSMwsD8BBgBCgAmFiEES2PPj95JdG6Y+gHdrRm6 +s8vxJeoFAlvzKm0CGwwFCQPCZwAACgkQrRm6s8vxJeppCAwAu6gWjzntbdYmacSt +ADE3PsW1nnZKYuqkTyR97XbhhVpzwdDIUFvUBYzZkwap24KYM33CWOXLMGFAAHBY +xlDfJNndjjSwnJM0+ikq2IulxNFupdL6qCy7vrVmTQAzXM+LZ0c3wS8HLF6K3dLs +g9k8Cw85FhoN38XG416VsRewXf/C8q+w/ygwL78f5/voaPuTM2AJ4cL9fhpgTyur +OKpJNaylYDMk/EAyzc6xBommxhguLIPA7OO4JxauDU09/9hkHi017XCzRnmtEgDR +cRQ4Kvg6mhuhi38/2XYiCISG4U3m3VV/HVcf+6LRyIF/jJCPlUnYfItrwPXDgkjz +qyplGorMHpdRqigRctcQmm+QoE1cErrG8qMGwOG3asQ52mxS/N8akl2qaQKP52hm +959GOWjTjXY968ELJehDODqaH339JaXQ6bq29Jj5jkJ5x6WYGjyaRVwEPNfSsSvU +vWv/107oQfTzCkCypUs7M8PoqItnOLT9QZt3BTPttiis+GEz +=AY8Z +-----END PGP PUBLIC KEY BLOCK----- diff --git a/docker/mac/BaseDockerfile.arm64 b/docker/mac/BaseDockerfile.arm64 index 0310ff2..ee4e496 100644 --- a/docker/mac/BaseDockerfile.arm64 +++ b/docker/mac/BaseDockerfile.arm64 @@ -12,7 +12,13 @@ # # Build: docker build -t golem_base:latest -f docker/mac/BaseDockerfile.arm64 . # ============================== -FROM ubuntu:22.04 +# ubuntu:22.04 has no CA bundle, and apt-sources.sh selects HTTPS snapshots. +# Take the bundle from a digest-pinned Jammy image so TLS stays verified. +FROM ros:humble-ros-base-jammy@sha256:1813d3c85d7f96ff7d3012d865204583255740182db5d0065f8f8cd029a83138 AS ca_bundle +FROM ubuntu:22.04@sha256:829f6df217bcbae2b371026e81711d1a787c61b2967ad09d015063663ebafbf7 +COPY --from=ca_bundle /etc/ssl/certs/ca-certificates.crt /etc/ssl/certs/ca-certificates.crt +COPY docker/locks/base-*.txt docker/locks/apt-sources.sh docker/locks/ros-snapshot.asc /opt/golem-locks/ +RUN sh /opt/golem-locks/apt-sources.sh ENV DEBIAN_FRONTEND=noninteractive ENV TZ=America/Denver @@ -38,15 +44,16 @@ RUN apt-get update && apt-get install -y --no-install-recommends \ libegl1-mesa-dev libosmesa6-dev \ && locale-gen en_US en_US.UTF-8 && update-locale LC_ALL=C.UTF-8 LANG=C.UTF-8 \ && add-apt-repository universe \ + && sh /opt/golem-locks/apt-sources.sh \ && update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-12 100 \ && update-alternatives --install /usr/bin/g++ g++ /usr/bin/g++-12 100 \ && rm -rf /var/lib/apt/lists/* # ROS 2 Humble apt repository. `dpkg --print-architecture` resolves to arm64 here, # and packages.ros.org publishes Humble for arm64 (Ubuntu Jammy). -RUN curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key \ +RUN curl -sSL https://raw.githubusercontent.com/ros/rosdistro/e31287f892ecd01b170d1cbfa298db35abe4a74f/ros.key \ -o /usr/share/keyrings/ros-archive-keyring.gpg \ - && echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu jammy main" \ + && echo "deb [arch=$(dpkg --print-architecture) signed-by=/opt/golem-locks/ros-snapshot.asc] http://snapshots.ros.org/humble/2026-08-07/ubuntu jammy main" \ > /etc/apt/sources.list.d/ros2.list # ROS 2 Humble packages needed by the sim processes (rclpy + sensor/TF publishers). @@ -64,13 +71,14 @@ RUN apt-get update && apt-get install -y --no-install-recommends \ # Install uv for fast, deterministic pip-compatible installs. # UV_INSTALL_DIR overrides the default ~/.local/bin location. -RUN curl -LsSf https://astral.sh/uv/install.sh | env UV_INSTALL_DIR=/usr/local/bin sh \ +RUN curl -LsSf https://astral.sh/uv/0.12.15/install.sh | env UV_INSTALL_DIR=/usr/local/bin sh \ && uv --version # Build CycloneDDS 0.10.x. Pinned because the PyPI `cyclonedds` wheel (transitive # dep of unitree_sdk2_python) references `dds/ddsi/q_radmin.h`, removed post-0.10. # CYCLONEDDS_HOME points the PyPI wheel at this install at pip-build time. -RUN git clone --depth 1 --branch releases/0.10.x https://github.com/eclipse-cyclonedds/cyclonedds /cyclonedds && \ +RUN git clone https://github.com/eclipse-cyclonedds/cyclonedds /cyclonedds && \ + git -C /cyclonedds checkout --quiet 5041f3560c088c99e5088b2b8520b69169621196 && \ cd /cyclonedds && mkdir build install && cd build && \ cmake .. -DCMAKE_INSTALL_PREFIX=../install && \ cmake --build . --target install && \ @@ -82,7 +90,7 @@ ENV CYCLONEDDS_HOME=/cyclonedds/install # the image for development/debugging. COPY unitree_sdk2_python /home/code/unitree_sdk2_python RUN cd /home/code/unitree_sdk2_python && \ - uv pip install --system . && \ + uv pip install -c /opt/golem-locks/base-arm64-01.txt --system . && \ SP=$(python3 -c "import site; print(site.getsitepackages()[0])") && \ cp -r /home/code/unitree_sdk2_python/unitree_sdk2py/* "$SP/unitree_sdk2py/" @@ -91,7 +99,7 @@ RUN cd /home/code/unitree_sdk2_python && \ # ~3 GB of dormant nvidia-*-cu13 libs), so pin the explicit CPU index to get a # lean CPU-only build. RoboCasa downgrades torch to a CPU 2.7.1 build anyway. # UV_HTTP_TIMEOUT raised because the torch wheels are large. -RUN UV_HTTP_TIMEOUT=600 uv pip install --system --no-cache \ +RUN UV_HTTP_TIMEOUT=600 uv pip install -c /opt/golem-locks/base-arm64-02.txt --system --no-cache \ torch torchvision \ --index-url https://download.pytorch.org/whl/cpu @@ -99,7 +107,7 @@ RUN UV_HTTP_TIMEOUT=600 uv pip install --system --no-cache \ # PyPI `pin` provides the `pinocchio` module. `pink` / `mink` are Pythonic IK # wrappers built on pinocchio. Kept in base so per-sim Dockerfiles don't # duplicate them; ROS container is standalone and installs its own pinocchio. -RUN uv pip install --system --no-cache pin pink mink +RUN uv pip install -c /opt/golem-locks/base-arm64-03.txt --system --no-cache pin pink mink # Source ROS 2 on every interactive shell. RUN echo 'source /opt/ros/humble/setup.bash' >> /root/.bashrc diff --git a/docker/mac/RobocasaDockerfile.arm64 b/docker/mac/RobocasaDockerfile.arm64 index 2396172..0fa37ed 100644 --- a/docker/mac/RobocasaDockerfile.arm64 +++ b/docker/mac/RobocasaDockerfile.arm64 @@ -8,6 +8,7 @@ # Build: docker build -t golem_sim_robocasa:latest -f docker/mac/RobocasaDockerfile.arm64 . # ============================== FROM golem_base:arm64 +COPY docker/locks/robocasa-*.txt docker/locks/robocasa-assets.digest /opt/golem-locks/ ARG http_proxy ARG https_proxy @@ -27,7 +28,7 @@ RUN apt-get update && apt-get install -y --no-install-recommends \ # MuJoCo Python binding + Pillow for image encoding. Pinned to 3.3.1 to match # Robocasa's hard pin (see below). -RUN uv pip install --system --no-cache \ +RUN uv pip install -c /opt/golem-locks/robocasa-arm64-01.txt --system --no-cache \ "mujoco==3.3.1" \ "numpy>=2.2.6" \ "Pillow>=10.0" @@ -35,26 +36,34 @@ RUN uv pip install --system --no-cache \ # Robocasa kitchen scenes. Both packages are git-only. robosuite is pinned to # master per robocasa.ai. Side effect: lerobot (a Robocasa transitive) pins # torch==2.7.1 (CPU) — fine here. -RUN git clone --depth 1 https://github.com/ARISE-Initiative/robosuite /opt/robosuite && \ - uv pip install --system --no-cache -e /opt/robosuite +RUN git clone https://github.com/ARISE-Initiative/robosuite /opt/robosuite && \ + git -C /opt/robosuite checkout --quiet 5ce6643f3092639d08f7b0f90ed1c6a84f50552c && \ + uv pip install -c /opt/golem-locks/robocasa-arm64-02.txt --system --no-cache -e /opt/robosuite -RUN git clone --depth 1 https://github.com/robocasa/robocasa /opt/robocasa && \ - uv pip install --system --no-cache -e /opt/robocasa +RUN git clone https://github.com/robocasa/robocasa /opt/robocasa && \ + git -C /opt/robocasa checkout --quiet 4f8a2980def75a55dff96b990745b83540425f09 && \ + uv pip install -c /opt/golem-locks/robocasa-arm64-03.txt --system --no-cache -e /opt/robocasa # Robocasa & robosuite setup scripts (create the macros file, silence warnings). RUN python /opt/robosuite/robosuite/scripts/setup_macros.py && \ python /opt/robocasa/robocasa/scripts/setup_macros.py # Optional robosuite/robocasa deps that print "WARNING: not installed" otherwise. -RUN git clone --depth 1 https://github.com/ARISE-Initiative/robosuite_models /opt/robosuite_models && \ - uv pip install --system --no-cache -e /opt/robosuite_models && \ - git clone --depth 1 https://github.com/NVlabs/mimicgen /opt/mimicgen && \ - uv pip install --system --no-cache -e /opt/mimicgen && \ - uv pip install --system --no-cache --no-deps "mink==0.0.5" +RUN git clone https://github.com/ARISE-Initiative/robosuite_models /opt/robosuite_models && \ + git -C /opt/robosuite_models checkout --quiet 2d63a6b4319bda772420daf10594ce0de5bd793d && \ + uv pip install -c /opt/golem-locks/robocasa-arm64-04.txt --system --no-cache -e /opt/robosuite_models && \ + git clone https://github.com/NVlabs/mimicgen /opt/mimicgen && \ + git -C /opt/mimicgen checkout --quiet 72bd767c255545f462e7ccfb2731f2e5d4c1d9bb && \ + uv pip install -c /opt/golem-locks/robocasa-arm64-04.txt --system --no-cache -e /opt/mimicgen && \ + uv pip install -c /opt/golem-locks/robocasa-arm64-04.txt --system --no-cache --no-deps "mink==0.0.5" # Robocasa's textures, fixtures, and objects (~10 GB) — required for any # `--scene kitchen` run. `echo y` answers the interactive confirmation. RUN echo y | python -m robocasa.scripts.download_kitchen_assets --type all +# Verify the complete downloaded tree, including additions/removals. +RUN cd /opt/robocasa/robocasa/models && \ + find assets -type f -print0 | LC_ALL=C sort -z | xargs -0 sha256sum | \ + sha256sum --check /opt/golem-locks/robocasa-assets.digest WORKDIR /home/code/h1_robocasa @@ -77,7 +86,8 @@ RUN mkdir -p /home/code/msgs_ws/src # ----------------------------------------------------------------------------- # Livox-SDK2 — required by livox_ros_driver2's C++ library. # ----------------------------------------------------------------------------- -RUN git clone --depth 1 https://github.com/Livox-SDK/Livox-SDK2.git /tmp/Livox-SDK2 && \ +RUN git clone https://github.com/Livox-SDK/Livox-SDK2.git /tmp/Livox-SDK2 && \ + git -C /tmp/Livox-SDK2 checkout --quiet 08f523c930b2f0ba1e98a6afaa8d7476bf479908 && \ cd /tmp/Livox-SDK2 && mkdir build && cd build && \ cmake .. -DCMAKE_POLICY_VERSION_MINIMUM=3.5 && make -j"$(nproc)" && make install && \ ldconfig && \ diff --git a/docker/mac/RosDockerfile.slim.arm64 b/docker/mac/RosDockerfile.slim.arm64 index b67deaf..70826d1 100644 --- a/docker/mac/RosDockerfile.slim.arm64 +++ b/docker/mac/RosDockerfile.slim.arm64 @@ -11,6 +11,7 @@ # Build: docker build -t golem_ros:latest -f docker/mac/RosDockerfile.slim.arm64 . # ============================== FROM golem_base:arm64 +COPY docker/locks/ros-*.txt /opt/golem-locks/ ARG http_proxy ARG https_proxy @@ -54,7 +55,9 @@ RUN apt-get update && apt-get install -y --no-install-recommends \ # C-extension installs bind to the numpy 1.x ABI (matches apt scipy). OMPL ships # a manylinux2014_aarch64 wheel, so it installs on arm64 with no source build. # ----------------------------------------------------------------------------- -RUN pip install --no-cache-dir --ignore-installed blinker \ +# Remove the NumPy-2 kinematics inherited from base before installing the ROS ABI. +RUN pip uninstall -y pin libpinocchio coal libcoal eigenpy +RUN pip install -c /opt/golem-locks/ros-arm64-01.txt --no-cache-dir --ignore-installed blinker \ "numpy<2" \ "scikit-learn>=1.4" \ pin-pink \ @@ -82,7 +85,7 @@ RUN rm -rf /usr/local/lib/python3.10/dist-packages/setuptools \ /usr/local/lib/python3.10/dist-packages/_distutils_hack \ /usr/local/lib/python3.10/dist-packages/wheel \ /usr/local/lib/python3.10/dist-packages/wheel-*.dist-info \ - && pip install --no-cache-dir --no-deps "numpy<2" "setuptools==59.6.0" "wheel<0.44" \ + && pip install -c /opt/golem-locks/ros-arm64-02.txt --no-cache-dir --no-deps "numpy<2" "setuptools==59.6.0" "wheel<0.44" \ && rm -f /usr/local/lib/python3.10/dist-packages/distutils-precedence.pth # FastDDS RMW for the ROS (rclpy) layer — arm64 coexistence fix (see the diff --git a/docker/scripts/launch_ros.sh b/docker/scripts/launch_ros.sh index 387cd31..20aa661 100755 --- a/docker/scripts/launch_ros.sh +++ b/docker/scripts/launch_ros.sh @@ -9,14 +9,8 @@ if [ ! -f /opt/unitree_install/lib/libunitree_sdk2.a ]; then exit 1 fi -# --- MuJoCo MPC (MJPC) build-cache hydrate (image seed -> persistent mount) --- -# ../container_cache/mjpc_build persists the CMake build tree at the in-tree path -# /home/code/mujoco_mpc/build. On first launch it is empty, so hydrate it from the -# baked seed and back-date the freshly-checked-out submodule source so the first -# in-container rebuild_mjpc.sh is warm (incremental) instead of a ~15-min cold -# rebuild. Guarded on the absence of CMakeCache.txt -> runs exactly once; later -# `run --rm` containers reuse the already-hydrated host dir. No-ops cleanly if the -# submodule isn't checked out (import mujoco_mpc still works from dist-packages). +# Hydrate the persistent MJPC build tree once. Source timestamps cannot prove +# equivalence with the image: local edits may exist even at the same git HEAD. MJPC_SRC=/home/code/mujoco_mpc MJPC_BUILD=/home/code/mujoco_mpc/build MJPC_SEED=/opt/mjpc-build-seed @@ -24,45 +18,27 @@ if [ -f "$MJPC_SRC/CMakeLists.txt" ] && [ ! -e "$MJPC_BUILD/CMakeCache.txt" ] \ && [ -d "$MJPC_SEED" ]; then echo "[launch_ros] hydrating MJPC build cache from seed ($MJPC_SEED -> $MJPC_BUILD)" mkdir -p "$MJPC_BUILD" - cp -a "$MJPC_SEED/." "$MJPC_BUILD/" # preserve mtimes/symlinks/_deps stamps - # The host submodule was checked out AFTER the image build, so its source - # mtimes are NEWER than the seeded objects -> Ninja would recompile everything - # and CMake would reconfigure. Push SOURCE mtimes into the past (NOT the - # multi-GB build tree, whose internal mtime ordering must stay intact). Prune - # the build dir and .git. - echo "[launch_ros] back-dating MJPC source mtimes so the seed stays warm" + cp -a "$MJPC_SEED/." "$MJPC_BUILD/" +fi + +# The marker also migrates persistent caches created by launchers that backdated +# sources. Only successful input invalidation writes it; build failures remain +# retryable because source mtimes still exceed the old objects. +if [ -f "$MJPC_SRC/CMakeLists.txt" ] && [ -e "$MJPC_BUILD/CMakeCache.txt" ] \ + && [ ! -f "$MJPC_BUILD/.golem-source-revalidated-v1" ]; then + # Revalidate every mounted input on first use, including dirty/untracked + # files and sources checked out before the image was built. Keep the seeded + # dependency build tree; subsequent launches use normal incremental builds. find "$MJPC_SRC" \( -path "$MJPC_BUILD" -o -name .git \) -prune -o \ - -exec touch -h -d '2000-01-01T00:00:00' {} + - # The blanket back-date assumes source SHA == seed SHA. When the submodule is - # AHEAD of the image's MJPC_REF (normal between image rebakes), that would - # hydrate a silently STALE build (Ninja sees everything up to date). Fix: re-touch - # exactly the files that differ from the seed ref so Ninja recompiles just the - # delta. Seed ref comes from the baked .mjpc_ref stamp (newer images) or the - # fallback constant (= the MJPC_REF the current image was built with). Needs the - # ../.git/modules/mujoco_mpc ro mount for git to work on the submodule source. - SEED_REF=$(cat "$MJPC_SEED/.mjpc_ref" 2>/dev/null \ - || echo 9f3cb6488aa82efc67893261cee6a1af560e4bc1) - git config --global --get-all safe.directory 2>/dev/null | grep -qx "$MJPC_SRC" \ - || git config --global --add safe.directory "$MJPC_SRC" - if HEAD_REF=$(git -C "$MJPC_SRC" rev-parse HEAD 2>/dev/null); then - if [ "$HEAD_REF" != "$SEED_REF" ]; then - echo "[launch_ros] source ($HEAD_REF) != seed ($SEED_REF) — re-touching the changed files" - git -C "$MJPC_SRC" diff --name-only "$SEED_REF" HEAD 2>/dev/null \ - | (cd "$MJPC_SRC" && xargs -r -d '\n' touch -c -h) \ - || { echo "[launch_ros] WARNING: seed-delta diff failed — re-touching ALL sources (cold-ish rebuild, but correct)"; \ - find "$MJPC_SRC" \( -path "$MJPC_BUILD" -o -name .git \) -prune -o -exec touch -h {} + ; } - fi - else - echo "[launch_ros] WARNING: git unusable on $MJPC_SRC (missing .git/modules mount?) — re-touching ALL sources (cold-ish rebuild, but correct)" - find "$MJPC_SRC" \( -path "$MJPC_BUILD" -o -name .git \) -prune -o -exec touch -h {} + - fi + -exec touch -h {} + + touch "$MJPC_BUILD/.golem-source-revalidated-v1" fi # --- MJPC incremental rebuild (ninja no-op scan when already warm) --- # Brings libmjpc/threadpool + the staged task assets + the dist-packages # agent_server current with the mounted submodule BEFORE colcon links # h12_deploy_mjpc against the build tree (after a hydrate the tree is at the -# image's MJPC_REF; the delta-touch above marked exactly what must recompile). +# image's MJPC_REF; source inputs above require revalidation). if [ -f "$MJPC_SRC/CMakeLists.txt" ] && [ -e "$MJPC_BUILD/CMakeCache.txt" ]; then /home/code/h12_sim_scripts/rebuild_mjpc.sh fi @@ -74,38 +50,58 @@ if [ ! -d src ] || [ -z "$(ls -A src 2>/dev/null)" ]; then echo "[launch_ros] $WS/src is empty — did you forget 'git submodule update --init --recursive'?" fi -# Rebuild only if install/ is missing or any package.xml is newer than its install marker. -NEEDS_BUILD=0 -if [ ! -f install/setup.bash ]; then - NEEDS_BUILD=1 -elif [ -n "$(find src -name package.xml -newer install/setup.bash 2>/dev/null | head -1)" ]; then - NEEDS_BUILD=1 +# Let colcon/CMake check all inputs on every launch: package.xml timestamps do +# not account for C++, headers, interfaces, launch files, or build configuration. +# The upstream Livox build.sh deletes workspace caches and masks colcon errors. +# Select its ROS 2 manifest in a private copy, preserving the mounted submodule. +LIVOX_DIR="$WS/src/livox_ros_driver2" +COLCON_PATHS=(src) +if [ -f "$LIVOX_DIR/package_ROS2.xml" ]; then + STAGE_ROOT="$WS/build/.golem-src" + LIVOX_STAGE="$STAGE_ROOT/livox_ros_driver2" + mkdir -p "$STAGE_ROOT" + touch "$STAGE_ROOT/COLCON_IGNORE" + # Timestamp-preserving checkouts/copies can change content without making + # CMake inputs newer. Compare the small driver tree before refreshing it; + # invalidate only its package outputs when content changes. package.xml is + # selected from package_ROS2.xml below, so the original variant is irrelevant. + if [ -d "$LIVOX_STAGE" ] && \ + ! diff -qr --exclude=.git --exclude=package.xml "$LIVOX_DIR" "$LIVOX_STAGE" >/dev/null; then + rm -rf "$WS/build/livox_ros_driver2" "$WS/install/livox_ros_driver2" + fi + rm -rf "$LIVOX_STAGE" + mkdir -p "$LIVOX_STAGE" + cp -a "$LIVOX_DIR/." "$LIVOX_STAGE/" + cp -p "$LIVOX_STAGE/package_ROS2.xml" "$LIVOX_STAGE/package.xml" + # A CMake cache is tied to its absolute source directory. Discard only the + # driver's old cache/install when migrating from the mounted source path. + LIVOX_CACHE="$WS/build/livox_ros_driver2/CMakeCache.txt" + if [ -f "$LIVOX_CACHE" ] && \ + ! grep -Fxq "CMAKE_HOME_DIRECTORY:INTERNAL=$LIVOX_STAGE" "$LIVOX_CACHE"; then + rm -rf "$WS/build/livox_ros_driver2" "$WS/install/livox_ros_driver2" + fi + COLCON_PATHS=("$LIVOX_STAGE") + for package in "$WS"/src/*; do + [ -d "$package" ] || continue + [ "$package" = "$LIVOX_DIR" ] || COLCON_PATHS+=("$package") + done fi -if [ "$NEEDS_BUILD" = "1" ]; then - # livox_ros_driver2 ships a build.sh that picks the ROS 2 file variants - # (package_ROS2.xml → package.xml, launch_ROS2 → launch) and drives colcon - # from the workspace root. Upstream invokes `colcon build --cmake-args ...` - # without --symlink-install; inject the flag so model_server's - # weights/*.pt are reachable via the install symlink instead of - # needing a manual post-build copy. Idempotent — won't re-patch if already - # present (e.g. after a previous run on the same bind-mounted clone). - LIVOX_DIR="$WS/src/livox_ros_driver2" - if [ -x "$LIVOX_DIR/build.sh" ]; then - if ! grep -q -- '--symlink-install' "$LIVOX_DIR/build.sh"; then - echo "[launch_ros] patching livox_ros_driver2/build.sh to add --symlink-install" - sed -i 's|colcon build --cmake-args|colcon build --symlink-install --cmake-args|' \ - "$LIVOX_DIR/build.sh" - fi - echo "[launch_ros] livox_ros_driver2/build.sh humble" - (cd "$LIVOX_DIR" && ./build.sh humble) - else - echo "[launch_ros] colcon build" - colcon build --symlink-install +# colcon does not remove isolated install prefixes when packages disappear. +# Refuse to source stale code until the user explicitly cleans those caches. +CURRENT_PACKAGES=$(colcon list --names-only --base-paths "${COLCON_PATHS[@]}") +for marker in "$WS"/install/*/share/colcon-core/packages/*; do + [ -f "$marker" ] || continue + package=${marker##*/} + if ! grep -Fxq -- "$package" <<< "$CURRENT_PACKAGES"; then + echo "[launch_ros] stale installed package '$package' has no discovered source; remove its core_ws/build/$package and core_ws/install/$package caches, then retry" >&2 + exit 1 fi -else - echo "[launch_ros] install/ is up to date — skipping build (run 'colcon build --symlink-install' to force)" -fi +done + +echo "[launch_ros] colcon build (incremental)" +colcon build --symlink-install --base-paths "${COLCON_PATHS[@]}" \ + --cmake-args -DROS_EDITION=ROS2 -DDISTRO_ROS=humble source install/setup.bash diff --git a/tests/docker/test_launch_ros_cache.py b/tests/docker/test_launch_ros_cache.py new file mode 100644 index 0000000..6f55308 --- /dev/null +++ b/tests/docker/test_launch_ros_cache.py @@ -0,0 +1,259 @@ +"""Run the launcher with temporary mounts and a fake colcon (no ROS/image needed).""" +import os +import json +from pathlib import Path +import subprocess +import shutil +import tempfile +import unittest + +REPO = Path(__file__).resolve().parents[2] + + +class LaunchCacheTests(unittest.TestCase): + def setUp(self): + self.tmp = tempfile.TemporaryDirectory() + self.addCleanup(self.tmp.cleanup) + self.root = Path(self.tmp.name) + self.ws = self.root / 'home/code/core_ws' + (self.ws / 'src/demo').mkdir(parents=True) + (self.ws / 'src/demo/package.xml').write_text('') + (self.ws / 'install').mkdir() + (self.ws / 'install/setup.bash').write_text(':') + for name in ('opt/ros/humble/setup.bash', 'opt/unitree_install/lib/libunitree_sdk2.a'): + p = self.root / name + p.parent.mkdir(parents=True, exist_ok=True) + p.write_text(':') + self.bin = self.root / 'bin' + self.bin.mkdir() + colcon = self.bin / 'colcon' + colcon.write_text('''#!/usr/bin/env python3 +import json, os, sys +if sys.argv[1] == 'list': + print('demo') + if any('livox_ros_driver2' in arg for arg in sys.argv): + print('livox_ros_driver2') + sys.exit(int(os.environ.get('LIST_EXIT', '0'))) +with open(os.environ['CALLS'], 'a') as f: + f.write(json.dumps(sys.argv[1:]) + '\\n') +sys.exit(int(os.environ.get('BUILD_EXIT', '0'))) +''') + colcon.chmod(0o755) + self.env = dict(os.environ, PATH=str(self.bin) + ':' + os.environ['PATH'], + CALLS=str(self.root / 'calls'), HOME=str(self.root)) + # Relocate fixed container mount paths, keeping the executable logic intact. + source = (REPO / 'docker/scripts/launch_ros.sh').read_text() + self.launcher = self.root / 'launch_ros.sh' + self.launcher.write_text(source.replace('/home/code', str(self.root / 'home/code')) + .replace('/opt/', str(self.root / 'opt') + '/')) + + def run_launch(self, **env): + return subprocess.run(['bash', str(self.launcher), 'touch', str(self.root / 'started')], + env=dict(self.env, **env), text=True, capture_output=True) + + def test_source_changes_trigger_incremental_build_even_with_newer_install_marker(self): + (self.ws / 'src/demo/node.cpp').write_text('changed source') + result = self.run_launch() + self.assertEqual(result.returncode, 0, result.stderr) + self.assertTrue((self.root / 'calls').exists(), 'colcon must scan all inputs every launch') + result = self.run_launch() + self.assertEqual(result.returncode, 0, result.stderr) + self.assertEqual(len((self.root / 'calls').read_text().splitlines()), 2) + + def test_colcon_failure_prevents_command_execution(self): + result = self.run_launch(BUILD_EXIT='23') + self.assertEqual(result.returncode, 23, result.stderr) + self.assertFalse((self.root / 'started').exists()) + + def test_livox_is_staged_without_running_or_mutating_upstream_build_script(self): + driver = self.ws / 'src/livox_ros_driver2' + driver.mkdir() + upstream = '#!/bin/bash\nexit 0\n' + (driver / 'build.sh').write_text(upstream) + (driver / 'build.sh').chmod(0o755) + (driver / 'package_ROS2.xml').write_text('ROS2') + (driver / 'launch_ROS2').mkdir() + (driver / 'launch_ROS2/example.py').write_text('launch') + result = self.run_launch(BUILD_EXIT='19') + self.assertEqual(result.returncode, 19, result.stderr) + self.assertEqual((driver / 'build.sh').read_text(), upstream) + self.assertFalse((driver / 'package.xml').exists()) + self.assertFalse((self.root / 'started').exists()) + args = json.loads((self.root / 'calls').read_text()) + self.assertIn('--symlink-install', args) + self.assertIn('-DDISTRO_ROS=humble', args) + paths = args[args.index('--base-paths') + 1:args.index('--cmake-args')] + self.assertNotIn(str(driver), paths) + staged = next(Path(p) for p in paths if Path(p).name == 'livox_ros_driver2') + self.assertEqual((staged / 'package.xml').read_text(), 'ROS2') + + def test_livox_refresh_removes_deleted_sources_and_preserves_other_builds(self): + driver = self.ws / 'src/livox_ros_driver2' + driver.mkdir() + (driver / 'package_ROS2.xml').write_text('ROS2') + (driver / 'removed.cpp').write_text('old') + other = self.ws / 'build/demo/object.o' + other.parent.mkdir(parents=True) + other.write_text('keep') + result = self.run_launch() + self.assertEqual(result.returncode, 0, result.stderr) + args = json.loads((self.root / 'calls').read_text()) + stage = Path(args[args.index('--base-paths') + 1]) + self.assertTrue((stage / 'removed.cpp').exists()) + (driver / 'removed.cpp').unlink() + (driver / 'added.cpp').write_text('new') + result = self.run_launch() + self.assertEqual(result.returncode, 0, result.stderr) + self.assertFalse((stage / 'removed.cpp').exists()) + self.assertEqual((stage / 'added.cpp').read_text(), 'new') + self.assertEqual(other.read_text(), 'keep') + + def test_livox_cache_moves_to_staged_source_once(self): + driver = self.ws / 'src/livox_ros_driver2' + driver.mkdir() + (driver / 'package_ROS2.xml').write_text('ROS2') + cache = self.ws / 'build/livox_ros_driver2/CMakeCache.txt' + cache.parent.mkdir(parents=True) + cache.write_text(f'CMAKE_HOME_DIRECTORY:INTERNAL={driver}\n') + result = self.run_launch() + self.assertEqual(result.returncode, 0, result.stderr) + self.assertFalse(cache.exists()) + args = json.loads((self.root / 'calls').read_text()) + stage = args[args.index('--base-paths') + 1] + cache.parent.mkdir(parents=True) + cache.write_text(f'CMAKE_HOME_DIRECTORY:INTERNAL={stage}\n') + result = self.run_launch() + self.assertEqual(result.returncode, 0, result.stderr) + self.assertTrue(cache.exists(), 'an already migrated cache must stay incremental') + + def test_livox_same_timestamp_source_edit_invalidates_only_driver_cache(self): + driver = self.ws / 'src/livox_ros_driver2' + driver.mkdir() + (driver / 'package_ROS2.xml').write_text('ROS2') + source = driver / 'node.cpp' + source.write_text('old') + result = self.run_launch() + self.assertEqual(result.returncode, 0, result.stderr) + args = json.loads((self.root / 'calls').read_text()) + stage = args[args.index('--base-paths') + 1] + cache = self.ws / 'build/livox_ros_driver2/CMakeCache.txt' + cache.parent.mkdir(parents=True) + cache.write_text(f'CMAKE_HOME_DIRECTORY:INTERNAL={stage}\n') + unrelated = self.ws / 'build/demo/keep.o' + unrelated.parent.mkdir(parents=True) + unrelated.write_text('keep') + stamp = source.stat() + source.write_text('new') + os.utime(source, ns=(stamp.st_atime_ns, stamp.st_mtime_ns)) + result = self.run_launch() + self.assertEqual(result.returncode, 0, result.stderr) + self.assertFalse(cache.exists(), 'changed driver content must invalidate older objects') + self.assertTrue(unrelated.exists()) + self.assertEqual((Path(stage) / 'node.cpp').read_text(), 'new') + + def test_removed_package_install_prevents_launch(self): + marker = self.ws / 'install/removed/share/colcon-core/packages/removed' + marker.parent.mkdir(parents=True) + marker.write_text('') + result = self.run_launch() + self.assertNotEqual(result.returncode, 0) + self.assertIn('removed', result.stderr) + self.assertFalse((self.root / 'started').exists()) + + def test_package_discovery_failure_prevents_launch(self): + result = self.run_launch(LIST_EXIT='17') + self.assertEqual(result.returncode, 17, result.stderr) + self.assertFalse((self.root / 'started').exists()) + + def test_existing_backdated_mjpc_cache_is_revalidated_once(self): + src = self.root / 'home/code/mujoco_mpc' + build = src / 'build' + build.mkdir(parents=True) + source = src / 'CMakeLists.txt' + source.write_text('project(test)') + os.utime(source, (946684800, 946684800)) + (build / 'CMakeCache.txt').write_text('cache') + obj = build / 'cached.o' + obj.write_text('object') + rebuild = self.root / 'home/code/h12_sim_scripts/rebuild_mjpc.sh' + rebuild.parent.mkdir() + rebuild.write_text('#!/bin/bash\nexit 0\n') + rebuild.chmod(0o755) + result = self.run_launch() + self.assertEqual(result.returncode, 0, result.stderr) + self.assertGreater(source.stat().st_mtime_ns, obj.stat().st_mtime_ns) + stamp = source.stat().st_mtime_ns + result = self.run_launch() + self.assertEqual(result.returncode, 0, result.stderr) + self.assertEqual(source.stat().st_mtime_ns, stamp) + + @unittest.skipUnless(all(shutil.which(x) for x in ('colcon', 'cmake', 'c++')), + 'requires local colcon, cmake, and C++ compiler') + def test_real_build_recompiles_cpp_and_generated_input_and_rejects_removed_package(self): + # Use a tiny standalone CMake package, exercising actual dependency + # checks and installed executables without ROS or the Livox SDK. + (self.bin / 'colcon').unlink() + shutil.rmtree(self.ws / 'src/demo') + driver = self.ws / 'src/livox_ros_driver2' + driver.mkdir() + (driver / 'package_ROS2.xml').write_text( + 'livox_ros_driver20.0.1' + 'fixtureTest' + 'MITcmake') + (driver / 'CMakeLists.txt').write_text( + 'cmake_minimum_required(VERSION 3.10)\nproject(livox_ros_driver2)\n' + 'configure_file(value.msg value.h COPYONLY)\n' + 'add_executable(fixture node.cpp)\n' + 'target_include_directories(fixture PRIVATE ${CMAKE_CURRENT_BINARY_DIR})\n' + 'install(TARGETS fixture DESTINATION bin)\n') + source = driver / 'node.cpp' + source.write_text('#include \n#include "value.h"\nint main(){std::cout << VALUE;}\n') + message = driver / 'value.msg' + message.write_text('#define VALUE 1\n') + binary = self.ws / 'install/livox_ros_driver2/bin/fixture' + for value in (1, 2): + message.write_text(f'#define VALUE {value}\n') + result = self.run_launch() + self.assertEqual(result.returncode, 0, result.stdout + result.stderr) + self.assertEqual(subprocess.check_output([str(binary)], text=True), str(value)) + stamp = source.stat() + source.write_text(source.read_text().replace('<< VALUE', '<< VALUE + 1')) + os.utime(source, ns=(stamp.st_atime_ns, stamp.st_mtime_ns)) + result = self.run_launch() + self.assertEqual(result.returncode, 0, result.stdout + result.stderr) + self.assertEqual(subprocess.check_output([str(binary)], text=True), '3') + shutil.rmtree(driver) + (self.root / 'started').unlink() + result = self.run_launch() + self.assertNotEqual(result.returncode, 0) + self.assertIn('stale installed package', result.stderr) + self.assertFalse((self.root / 'started').exists()) + + def test_dirty_mjpc_source_is_newer_than_hydrated_objects(self): + src = self.root / 'home/code/mujoco_mpc' + seed = self.root / 'opt/mjpc-build-seed' + src.mkdir(parents=True) + seed.mkdir(parents=True) + (src / 'CMakeLists.txt').write_text('project(test)') + dirty = src / 'dirty.cpp' + dirty.write_text('original') + for args in (['init', '-q'], ['add', '.'], ['-c', 'user.name=Test', '-c', 'user.email=t@example.com', 'commit', '-qm', 'seed']): + subprocess.run(['git', '-C', str(src), *args], check=True, capture_output=True) + ref = subprocess.check_output(['git', '-C', str(src), 'rev-parse', 'HEAD'], text=True) + (seed / '.mjpc_ref').write_text(ref) + (seed / 'CMakeCache.txt').write_text('cache') + obj = seed / 'dirty.o' + obj.write_text('old binary') + dirty.write_text('dirty edits') + os.utime(dirty, (946684800, 946684800)) + rebuild = self.root / 'home/code/h12_sim_scripts/rebuild_mjpc.sh' + rebuild.parent.mkdir() + rebuild.write_text('#!/bin/bash\nexit 0\n') + rebuild.chmod(0o755) + result = self.run_launch() + self.assertEqual(result.returncode, 0, result.stderr) + self.assertGreater(dirty.stat().st_mtime_ns, (src / 'build/dirty.o').stat().st_mtime_ns) + + +if __name__ == '__main__': + unittest.main() diff --git a/tests/docker/test_source_pins.py b/tests/docker/test_source_pins.py new file mode 100644 index 0000000..08b0b4d --- /dev/null +++ b/tests/docker/test_source_pins.py @@ -0,0 +1,92 @@ +"""Cheap guards against accidentally reintroducing moving build inputs.""" +from pathlib import Path +import re +import unittest + +ROOT = Path(__file__).resolve().parents[2] +FILES = sorted((ROOT / 'docker').glob('*Dockerfile')) + sorted((ROOT / 'docker/mac').glob('*Dockerfile*')) + +class SourcePinTests(unittest.TestCase): + def test_public_base_images_have_digests(self): + for path in FILES: + for image in re.findall(r'^FROM (\S+)', path.read_text(), re.M): + if not image.startswith('golem_base:'): + self.assertRegex(image, r'@sha256:[0-9a-f]{64}$', str(path)) + + def test_every_git_clone_is_checked_out_at_an_exact_commit(self): + for path in FILES: + text = path.read_text().replace('\\\n', ' ') + args = dict(re.findall(r'^ARG (\w+)=([0-9a-f]{40})$', text, re.M)) + for name, sha in args.items(): + text = text.replace('${' + name + '}', sha) + for run in re.findall(r'^RUN .*', text, re.M): + clones = re.findall(r'git clone\b', run) + if clones: + pins = re.findall(r'git (?:-C \S+ )?(?:checkout(?: --quiet)?|reset --hard) "?[0-9a-f]{40}\b', run) + self.assertEqual(len(clones), len(pins), f'{path}: {run}') + + def test_isaaclab_transitive_vcs_pins_are_retained(self): + text = (ROOT / 'docker/IsaacDockerfile').read_text() + for pin in ( + 'rl_games.git@6b3534f29568158e9e29ec8bf83cc88fce5f0cae', + 'robomimic.git@7c66e7a41b5d9dcc905b1a68346bfee1b49b79c9', + ): + self.assertIn(pin, text) + self.assertLess(text.index('rl_games.git@6b3534f'), text.index('isaaclab.sh --install')) + + def test_isaac_locks_stay_compatible_with_bundled_packages(self): + # Isaac Sim bundles torch 2.7.0 and psutil 5.9.8 (rl-games needs psutil<6). + # stable-baselines3 2.9+ needs torch>=2.8; ipython 9.13+ needs psutil>=7. + locks = ROOT / 'docker/locks' + pins = dict(re.findall(r'^([\w.-]+)==(\S+)$', (locks / 'isaac-amd64.txt').read_text(), re.M)) + self.assertEqual(pins['torch'], '2.7.0+cu128') + self.assertEqual(pins['stable-baselines3'], '2.8.0') + self.assertEqual(pins['ipython'], '9.10.1') + for path in sorted(locks.glob('isaac-amd64-*.txt')): + self.assertIn('\npsutil==5.9.8\n', path.read_text(), str(path)) + + def test_bare_ubuntu_bases_have_a_ca_bundle_before_https_snapshots(self): + for path in FILES: + text = path.read_text() + if re.search(r'^FROM ubuntu:', text, re.M) and 'apt-sources.sh' in text: + copy = text.find('COPY --from=ca_bundle /etc/ssl/certs/ca-certificates.crt') + self.assertNotEqual(copy, -1, str(path)) + self.assertLess(copy, text.index('RUN sh /opt/golem-locks/apt-sources.sh'), str(path)) + + def test_ros_removes_base_kinematics_closure_before_reinstall(self): + # Base pin 4 links cmeel-boost 1.90 (NumPy 2); ROS locks pin 2.7 / boost 1.83. + text = (ROOT / 'docker/RosDockerfile').read_text().replace('\\\n', ' ') + uninstall = re.search(r'^RUN pip uninstall -y (.*)$', text, re.M).group(1).split() + base = (ROOT / 'docker/locks/base-amd64.txt').read_text() + for name in re.findall(r'^((?:cmeel[\w-]*|pin|libpinocchio|coal|libcoal|eigenpy))==', base, re.M): + self.assertIn(name, uninstall) + + def test_downloaded_scripts_do_not_follow_moving_refs(self): + for path in FILES: + text = path.read_text() + self.assertNotIn('https://astral.sh/uv/install.sh', text, str(path)) + self.assertNotIn('ros/rosdistro/master/ros.key', text, str(path)) + +class PythonLockTests(unittest.TestCase): + def test_python_installs_use_committed_constraints(self): + for path in FILES: + text = path.read_text().replace('\\\n', ' ') + for line in text.splitlines(): + if line.startswith('RUN ') and 'pip install' in line: + self.assertEqual(len(re.findall(r'pip install\b', line)), + len(re.findall(r'pip install -c /opt/golem-locks/', line)), str(path)) + for name in re.findall(r'/opt/golem-locks/([\w.-]+)', text): + self.assertTrue((ROOT/'docker/locks'/name).is_file(), name) + + def test_lock_requirements_are_exact(self): + for path in (ROOT/'docker/locks').glob('*.txt'): + for line in path.read_text().splitlines(): + if not line or line.startswith('#'): + continue + if line.startswith('-c '): + self.assertTrue((path.parent/line[3:]).is_file()) + else: + self.assertRegex(line, r'^[a-z0-9.-]+==[^ ;]+$', str(path)) + +if __name__ == '__main__': + unittest.main() diff --git a/tests/isaaclab/test_physical_reset.py b/tests/isaaclab/test_physical_reset.py new file mode 100644 index 0000000..7d12dc2 --- /dev/null +++ b/tests/isaaclab/test_physical_reset.py @@ -0,0 +1,34 @@ +"""Keep physical state restoration wired into the native reset lifecycle.""" +import ast +from pathlib import Path +from types import SimpleNamespace +import unittest + + +ROOT = Path(__file__).resolve().parents[2] +CONFIG = ROOT / 'CL_isaaclab_sim/tasks/h1-2_tasks/stack_rgyblock_h12_27dof_inspire/stack_rgyblock_h12_27dof_inspire_joint_env_cfg.py' + + +class PhysicalResetTests(unittest.TestCase): + def test_reset_restores_physics_before_clearing_imu_history(self): + # Load the actual event configuration without initializing Kit/GPU. + tree = ast.parse(CONFIG.read_text()) + event_class = next(node for node in tree.body + if isinstance(node, ast.ClassDef) and node.name == 'EventCfg') + calls = [] + restore = lambda env, env_ids: calls.append(('physics', env, env_ids)) + reset_imu = lambda env, env_ids: calls.append(('imu', env, env_ids)) + namespace = dict(configclass=lambda cls: cls, EventTermCfg=SimpleNamespace, + base_mdp=SimpleNamespace(reset_scene_to_default=restore), + reset_robot_imu_cache=reset_imu) + exec(compile(ast.Module(body=[event_class], type_ignores=[]), str(CONFIG), 'exec'), namespace) + events = [value for value in vars(namespace['EventCfg']).values() + if isinstance(value, SimpleNamespace) and value.mode == 'reset'] + env, ids = object(), object() + for event in events: + event.func(env, ids, **getattr(event, 'params', {})) + self.assertEqual(calls, [('physics', env, ids), ('imu', env, ids)]) + + +if __name__ == '__main__': + unittest.main() diff --git a/tests/stack/README.md b/tests/stack/README.md index bf6acc3..cc6cb91 100644 --- a/tests/stack/README.md +++ b/tests/stack/README.md @@ -17,6 +17,10 @@ JSON acceptance results under `tests/results/stack-*`. Nonzero exit means failure, including startup failures and timeouts. Controllers cannot access the private telemetry mount. The default wall timeout is 900 seconds; override with `GOLEM_TEST_WALL_TIMEOUT` for slower workers. +Simulator cameras are off by default (`GOLEM_TEST_CAMERAS=0`) because these +controllers use no images and CPU camera rendering slows RoboCasa physics well +below the real-time pace the wall-clock controllers assume. `run.json` records +the camera setting, the git state and the exact image IDs used. Arms move both wrist frames forward 2 cm and back through `/frame_task`. Acceptance requires successful actions, measured end-effector progress and @@ -37,6 +41,7 @@ The scoring unit tests run in the GitHub CPU merge workflow: python3 -m unittest discover -s tests/stack -p 'test_*.py' -v ``` -Live acceptance is **not yet validated** for either simulator. These runners -are available for trusted GPU workers; they are not enabled as mandatory live -merge gates until their end-to-end validation is complete. +Live acceptance runs on trusted NVIDIA workers through the manual GitHub +workflow. `tests/stack/run_all.sh` runs all four cases and returns nonzero if +any fails. Physical failures remain release blockers; CPU scoring tests alone +do not establish simulator/controller stability. diff --git a/tests/stack/check_lowerbody.py b/tests/stack/check_lowerbody.py index 6b38124..f6ef6c8 100644 --- a/tests/stack/check_lowerbody.py +++ b/tests/stack/check_lowerbody.py @@ -158,6 +158,9 @@ def pump(speed=0.): validate_row(row) assert not rows or row['sim_time'] > rows[-1]['sim_time'], 'Telemetry reset/nonmonotonic timestamp' rows.append(row) + if not row['support_active']: + assert row['position'][2] >= LIMITS['minimum_height'], 'Unsupported base collapsed before completing acceptance' + assert tilt_deg(row['quaternion']) <= LIMITS['max_tilt_deg'], 'Unsupported base exceeded tilt limit before completing acceptance' report = dict(success=False, expected_walk_policy=args.expected_walk_policy, phases=phases) try: while True: @@ -193,6 +196,11 @@ def pump(speed=0.): publisher.publish(Twist()) rclpy.spin_once(node, timeout_sec=.02) report['policies'] = policies + report['telemetry_samples'] = len(rows) + if rows: + report['minimum_height_observed'] = min(r['position'][2] for r in rows) + report['maximum_tilt_observed_deg'] = max(tilt_deg(r['quaternion']) for r in rows) + report['final_sim_time'] = rows[-1]['sim_time'] args.output.parent.mkdir(parents=True, exist_ok=True) args.output.write_text(json.dumps(report, indent=2)+'\n') node.destroy_node() diff --git a/tests/stack/launch_ros_stack.sh b/tests/stack/launch_ros_stack.sh index 20c223b..0fb35c3 100755 --- a/tests/stack/launch_ros_stack.sh +++ b/tests/stack/launch_ros_stack.sh @@ -31,6 +31,7 @@ colcon --log-base "$WS/log_stack_tests" build \ --packages-up-to h12_ros2_controller h12_safety_layer h12_lowerbody_rl unitree_hg unitree_go \ --cmake-args -DBUILD_TESTING=OFF -DPython3_EXECUTABLE=/usr/bin/python3 source "$TEST_INSTALL_BASE/setup.bash" +python3 /home/code/tests/stack/test_heartbeat_clock.py echo "STACK_BUILD_READY case=$TEST_CASE simulator=$SIMULATOR domain=$ROS_DOMAIN_ID" if [ "${1:-}" = --build-only ]; then exit 0 diff --git a/tests/stack/run.sh b/tests/stack/run.sh index dfad490..34f401b 100755 --- a/tests/stack/run.sh +++ b/tests/stack/run.sh @@ -8,6 +8,10 @@ case "$SIMULATOR" in isaac|robocasa) ;; *) exit 2 ;; esac case "$TEST_CASE" in arms|locomotion) ;; *) exit 2 ;; esac export ROS_DOMAIN_ID=${ROS_DOMAIN_ID:-193} export GOLEM_TEST_WALL_TIMEOUT=${GOLEM_TEST_WALL_TIMEOUT:-900} +# The arm and lower-body controllers consume no images. CPU-rendered RoboCasa +# cameras slow physics several-fold while these wall-clock controllers keep +# real-time pacing, which the real robot never experiences. +export GOLEM_TEST_CAMERAS=${GOLEM_TEST_CAMERAS:-0} [[ "$GOLEM_TEST_WALL_TIMEOUT" =~ ^[1-9][0-9]*$ ]] || exit 2 source "$ROOT/docker/scripts/docker_common.sh" load_config @@ -29,11 +33,25 @@ cleanup() { docker logs "${pair%:*}" >"$result/${pair##*:}.log" 2>&1 || true docker rm -f "${pair%:*}" >/dev/null 2>&1 || true done + python3 - "$result" "$SIMULATOR" "$TEST_CASE" "$status" <<'PYREPORT' +import json, os, pathlib, subprocess, sys +path, simulator, case, status = sys.argv[1:] +def git(*args): + return subprocess.check_output(['git', *args], text=True).strip() +report = dict(simulator=simulator, case=case, exit_code=int(status), + commit=git('rev-parse', 'HEAD'), + submodules=git('submodule', 'status', '--recursive'), + working_tree=git('status', '--short'), + cameras=os.environ['GOLEM_TEST_CAMERAS'], + images={name: subprocess.check_output(['docker', 'image', 'inspect', '-f', '{{.Id}}', name], text=True).strip() + for name in ('golem_ros:latest', f'golem_sim_{simulator}:latest')}) +pathlib.Path(path, 'run.json').write_text(json.dumps(report, indent=2)+'\n') +PYREPORT echo "Stack test artifacts: $result" exit "$status" } trap cleanup EXIT -env_args=(-e "SIMULATOR=$SIMULATOR" -e "TEST_CASE=$TEST_CASE" -e "GOLEM_TEST_WALL_TIMEOUT=$GOLEM_TEST_WALL_TIMEOUT") +env_args=(-e "SIMULATOR=$SIMULATOR" -e "TEST_CASE=$TEST_CASE" -e "GOLEM_TEST_WALL_TIMEOUT=$GOLEM_TEST_WALL_TIMEOUT" -e "GOLEM_CAMERAS=$GOLEM_TEST_CAMERAS") tests_mount=(-v "$ROOT/tests/stack:/home/code/tests/stack:ro") "${COMPOSE[@]}" run -d --no-deps --name "$stack" "${env_args[@]}" "${tests_mount[@]}" --entrypoint bash ros /home/code/tests/stack/launch_ros_stack.sh >/dev/null for ((attempt=0; attempt<600; attempt++)); do @@ -51,16 +69,19 @@ root, sim, output = sys.argv[1:] source = (Path(root)/f'docker/scripts/launch_{sim}.sh').read_text() old = '/home/code/CL_isaaclab_sim/sim_main.py' if sim == 'isaac' else 'h12_mujoco.py' assert source.count(old) == 1, 'Canonical launcher changed; update test instrumentation' +source = source.replace('"$(dirname "$0")/runtime_common.sh"', '"/home/code/h12_sim_scripts/runtime_common.sh"') Path(output).write_text(source.replace(old, '/home/code/tests/stack/observe_simulator.py')) PY -sim_args=(--headless) +sim_args=(--headless --seed 42) if [[ "$SIMULATOR" == isaac ]]; then sim_args+=(--hand_type magpie) # Arm kinematics is isolated from balance; locomotion always uses floating base. [[ "$TEST_CASE" != arms ]] || sim_args+=(--fix_base) +else + sim_args+=(--task AirDryFruit) fi "${COMPOSE[@]}" run -d --no-deps --name "$checker" "${env_args[@]}" "${tests_mount[@]}" -v "$result/telemetry:/telemetry:ro" -v "$result/metrics:/results" --entrypoint bash ros /home/code/tests/stack/run_checker.sh >/dev/null -"${COMPOSE[@]}" run -d --no-deps --name "$simulator" "${env_args[@]}" "${tests_mount[@]}" -e GOLEM_TEST_TELEMETRY=/telemetry/state.jsonl -v "$result/telemetry:/telemetry" -v "$result/launcher.sh:/home/code/h12_sim_scripts/test_launcher.sh:ro" --entrypoint bash "$SIMULATOR" /home/code/h12_sim_scripts/test_launcher.sh "${sim_args[@]}" >/dev/null +"${COMPOSE[@]}" run -d --no-deps --name "$simulator" "${env_args[@]}" "${tests_mount[@]}" -e GOLEM_TEST_TELEMETRY=/telemetry/state.jsonl -v "$result/telemetry:/telemetry" -v "$result/launcher.sh:/test_launcher.sh:ro" --entrypoint bash "$SIMULATOR" /test_launcher.sh "${sim_args[@]}" >/dev/null for ((attempt=0; attempt Date: Wed, 23 Sep 2026 12:38:40 -0600 Subject: [PATCH 006/225] Rewrite history comments in present tense; add real MID360 frame test; update RViz layout and ignores --- .gitignore | 8 + .../h12_deploy_mjpc/controller_launcher.py | 2 +- .../scripts/lowerbody_controller_node.py | 17 +- core_ws/src/h12_skills/h12_skills/base.py | 4 +- .../h12_skills/skills/frontier_explore.py | 4 +- .../src/h12_skills/h12_skills/skills/grasp.py | 21 +- core_ws/src/h1_bringup/config/mjpc_sim.yaml | 126 ++++----- .../src/h1_bringup/config/nav2_config.yaml | 5 +- .../h1_bringup/launch/h1_grippers.launch.py | 11 +- .../h1_bringup/launch/h1_navigation.launch.py | 35 ++- .../launch/h1_real_controller.launch.py | 9 +- .../launch/h1_sim_bringup.launch.py | 11 +- core_ws/src/h1_bringup/rviz/sim.rviz | 250 +++++++----------- .../h1_bringup/test/test_real_lidar_config.py | 59 +++++ .../model_server/graspgen_server.py | 42 ++- docs/NAVIGATION_DEMO.md | 5 +- h1_robocasa/measurement_bridge.py | 4 +- 17 files changed, 292 insertions(+), 321 deletions(-) create mode 100644 core_ws/src/h1_bringup/test/test_real_lidar_config.py diff --git a/.gitignore b/.gitignore index 5f58a28..1c134f4 100644 --- a/.gitignore +++ b/.gitignore @@ -70,3 +70,11 @@ wandb/ core core.* *.core + +# rosbag recordings (multi-GB; share out of band, not via git) +/bags/ +*.mcap +*.db3 + +# local scratch space +/work/ diff --git a/core_ws/src/h12_deploy_mjpc/h12_deploy_mjpc/controller_launcher.py b/core_ws/src/h12_deploy_mjpc/h12_deploy_mjpc/controller_launcher.py index 644a969..8e1608a 100644 --- a/core_ws/src/h12_deploy_mjpc/h12_deploy_mjpc/controller_launcher.py +++ b/core_ws/src/h12_deploy_mjpc/h12_deploy_mjpc/controller_launcher.py @@ -129,7 +129,7 @@ def main() -> None: # frc_parity is a TRI-STATE INT, not a bool -- pass 1, never true: # -1 = task default -> the `deploy_frc_parity` model numeric, which is # ABSENT from Stabilize_H12_Magpie.xml, so -1 resolves to OFF. - # 0 = force OFF (legacy planner model, byte-identical) -- the A/B arm. + # 0 = force OFF (untightened planner forceranges) -- the A/B arm. # 1 = force ON -> planner forceranges tightened to 0.9 x tau_estop (the # budget the node can actually emit; ankle 75 -> 48.6 Nm). "--frc_parity", str(int(p("frc_parity", -1).value)), diff --git a/core_ws/src/h12_lowerbody_rl/h12_lowerbody_rl/scripts/lowerbody_controller_node.py b/core_ws/src/h12_lowerbody_rl/h12_lowerbody_rl/scripts/lowerbody_controller_node.py index 686867e..37b2120 100644 --- a/core_ws/src/h12_lowerbody_rl/h12_lowerbody_rl/scripts/lowerbody_controller_node.py +++ b/core_ws/src/h12_lowerbody_rl/h12_lowerbody_rl/scripts/lowerbody_controller_node.py @@ -171,13 +171,13 @@ def __init__(self): # Seconds to ramp the pre-pose target from the measured leg pose to the # policy's nominal (cosine blend; see the PREPOSE block above). Starting # at the measured pose means zero initial PD error, so full gains apply - # no step torque. 0 disables the ramp (old step behavior). + # no step torque. 0 disables the ramp (step straight to the nominal). self.declare_parameter("prepose_ramp_s", 2.0) # Hold at the pre-pose crouch until an operator confirms engagement via # the /lowerbody/confirm_engage Trigger service. Default TRUE — on the # real robot the engage is the moment the policy takes authority, so it # must be an explicit operator action. The sim bringup passes false to - # keep unattended runs auto-engaging (the old behavior). While waiting, + # keep unattended runs auto-engaging. While waiting, # the pre-pose timeout never auto-commits. self.declare_parameter("engage_wait_for_confirm", True) self.declare_parameter("disable_elastic_band", True) @@ -269,7 +269,7 @@ def __init__(self): self.create_timer(1.0 / control_hz, self._tick) # Auto-switch sides: the startup policy is the stand side when it can # stand (fame or almi); walk is always the locomotion side. A startup - # policy of "walk" gets fame as its stand side (legacy behavior). + # policy of "walk" gets fame as its stand side. self._stand_policy = startup_policy if startup_policy in STAND_CAPABLE else FAME self.get_logger().info( @@ -389,12 +389,11 @@ def _publish_leg(self, leg: LegCommand) -> None: def _release_band(self, reason: str) -> None: # Called every tick while awaiting release. Only mark the band released # once the sim's toggle actually succeeds — NOT before the service check. - # The old code set _band_released=True up front, so if /elastic_band/toggle - # wasn't discovered yet (a startup race, likelier with the slower non- - # headless MuJoCo viewer) it gave up permanently and the robot hung from - # the band forever: it would even switch to walk on /cmd_vel but never - # move. Now it retries every tick until the service is ready and the call - # returns success. + # /elastic_band/toggle may not be discovered yet (a startup race, likelier + # with the slower non-headless MuJoCo viewer), and marking it released up + # front gives up permanently: the robot hangs from the band forever, still + # switching to walk on /cmd_vel but never moving. Retry every tick until + # the service is ready and the call returns success. if self._band_released or self._band_release_inflight: return if not self._band_cli.service_is_ready(): diff --git a/core_ws/src/h12_skills/h12_skills/base.py b/core_ws/src/h12_skills/h12_skills/base.py index 2ca1a1a..e8d19dd 100644 --- a/core_ws/src/h12_skills/h12_skills/base.py +++ b/core_ws/src/h12_skills/h12_skills/base.py @@ -134,7 +134,7 @@ # taken as "run to completion" — the skill then ends only by finishing, failing, # or being canceled, never on a clock. Deliberate: the long-running skills # (frontier exploration, a visual-servo grasp) have no meaningful upper bound, -# and the old 300 s ceiling silently aborted them mid-work. Send a NON-ZERO +# and a fixed ceiling silently aborts them mid-work. Send a NON-ZERO # goal.timeout whenever a bound is actually wanted. # # Consequences of the infinity, all checked: _Run.deadline becomes inf so @@ -341,7 +341,7 @@ def __init__(self, node_name='h12_skills'): # Live gripper aperture [mm] from the driver's published state (10 Hz), # cached per arm. Unlike _gripper_actual_mm (only refreshed by a position # command), this tracks the aperture after ANY close — including the - # force-based /close close_gripper now uses — so the held-object check + # force-based /close that close_gripper issues — so the held-object check # stays valid. Read via gripper_aperture(). self._gripper_state_mm = {} self.gripper_state_subs = { diff --git a/core_ws/src/h12_skills/h12_skills/skills/frontier_explore.py b/core_ws/src/h12_skills/h12_skills/skills/frontier_explore.py index 505880f..85bddda 100644 --- a/core_ws/src/h12_skills/h12_skills/skills/frontier_explore.py +++ b/core_ws/src/h12_skills/h12_skills/skills/frontier_explore.py @@ -88,11 +88,11 @@ # sit at least the goal tolerance PLUS a margin away, guaranteeing nav2 actually # commands motion; a frontier whose deepest safe cell is nearer than this is # blacklisted instead. MUST exceed _NAV_XY_GOAL_TOL — a smaller value guards -# nothing (this was 0.15 < 0.25, the cause of the instant-success loop). +# nothing and reopens the instant-success loop. _MIN_SAFE_ADVANCE = _NAV_XY_GOAL_TOL + 0.10 # Below this base displacement (m) over one nav attempt we treat the robot as # not having moved at all — used to catch a nav2 "SUCCEEDED" that advanced the -# base nowhere (belt-and-braces if the tolerances above ever drift apart again). +# base nowhere (belt-and-braces if the tolerances above ever drift apart). _STUCK_MOVE = 0.05 # Newly-known map cells during one nav attempt above which the attempt counts as # "made progress" even if nav2 didn't formally reach the goal. With a slow gait diff --git a/core_ws/src/h12_skills/h12_skills/skills/grasp.py b/core_ws/src/h12_skills/h12_skills/skills/grasp.py index 22e18b6..f6d8870 100644 --- a/core_ws/src/h12_skills/h12_skills/skills/grasp.py +++ b/core_ws/src/h12_skills/h12_skills/skills/grasp.py @@ -215,21 +215,20 @@ # Target camera->object depth [m] — a REAL drive target near where the fingers # should sit (~100 mm; the finger contact point is 70.6 mm ahead of the # camera, so the object ends ~29 mm beyond the fingertips). The servo drives -# depth TO this, so it must be a sensible value, not a band-center trick: an -# earlier (0,105) one-sided band centered the setpoint at 52.5 mm and made the -# servo plunge toward it whenever the object started beyond 105 mm, overshooting +# depth TO this, so it must be a sensible value, not a band-center trick: a +# one-sided band such as (0,105) centers the setpoint at 52.5 mm and makes the +# servo plunge toward it whenever the object starts beyond 105 mm, overshooting # into the part and disturbing X/Y. Paired with VISUAL_SERVO_RANGE_TOL_M for the # accepted band (99, 101) mm. VISUAL_SERVO_DEPTH_M = 0.10 -# Per-axis convergence tolerances [m], set from real runs (2026-07-21), not -# theory — these are the values the robust screw picking runs converged with -# (b714d40). The binding limit is the ARM: frame_task settles to ~1.2 mm -# residual, so corrections under ~2 mm don't execute and <4 mm lateral is -# unreachable. +# Per-axis convergence tolerances [m], measured from real screw-picking runs +# rather than derived — these are the values those runs converge with. The +# binding limit is the ARM: frame_task settles to ~1.2 mm residual, so +# corrections under ~2 mm don't execute and <4 mm lateral is unreachable. # X (across the jaws) — what a grasp depends on. BELOW the arm's own ~1.2 mm # resolution, so the loop cannot deliberately place within this band; it -# converges when it happens to land there. b714d40 ran 1 mm; if runs stall -# naming X, loosen it back to that (2 mm is the honest value). +# converges when it happens to land there. If runs stall naming X, loosen +# toward 1-2 mm (2 mm is the honest value against that residual). # Y (along the jaw span) — pre-opened to 106 mm, so far more forgiving; 5 mm # still holds the part near the middle of the grip rather than out at a jaw. # range — decides whether the jaws close around the part or above it. @@ -1114,7 +1113,7 @@ def servo(self, run, gh, arm, obj, targets, idx): label=f'visual servo back-off {retracts}/' f'{VISUAL_SERVO_MAX_RETRACTS}') # After a back-off `after` becomes a CLOCK instant (pre-move - # frames describe the old vantage point outright), and the + # frames describe the previous vantage point outright), and the # detect wait, stall counters and depth prediction all reset — # the hand is somewhere genuinely new. after = _stamp_tuple(self.get_clock().now().to_msg()) diff --git a/core_ws/src/h1_bringup/config/mjpc_sim.yaml b/core_ws/src/h1_bringup/config/mjpc_sim.yaml index 19f33dc..d6ae0fc 100644 --- a/core_ws/src/h1_bringup/config/mjpc_sim.yaml +++ b/core_ws/src/h1_bringup/config/mjpc_sim.yaml @@ -15,11 +15,11 @@ h12_deploy_mjpc_estimator: filter: rw-ekf # PLANT-CLOCK the filter (plant seconds per lowstate tick = RoboCasa's # model.opt.timestep). The wall-clocked default over-integrates by 1/RTF on - # this far-below-realtime sim — measured take 3 (2026-07-03): odometric xy - # drifted ~4x the robot's true motion, feeding the planner an inconsistent - # (pos, vel) pair -> lateral divergence within seconds of policy authority. - # REAL runs 1x, where wall == plant time; mjpc_real.yaml leaves this at 0. - tick_dt: 0.005 # follows --sim-dt 0.005 (plant timestep parity, 2026-07-04) + # this far-below-realtime sim: odometric xy drifts ~4x the robot's true + # motion, feeding the planner an inconsistent (pos, vel) pair that diverges + # laterally within seconds of policy authority. REAL runs at 1x, where wall + # == plant time, so mjpc_real.yaml leaves this at 0. + tick_dt: 0.005 # must match the plant's --sim-dt mjpc_debug_visualizer: ros__parameters: @@ -77,86 +77,58 @@ mjpc_deploy_lowerbody_controller: ros__parameters: task: "Stabilize H12 Magpie" strategy: 6 # 6 = stand (the stabilize task's only real slot) - # ★ RESTORED TO 12 (2026-07-06) = PARITY WITH REAL/TWIN/OWNSIM (all run 12). The - # old drop to 3 "faked" the per-sim-second planner rate for RoboCasa's SLOW RTF - # (0.25: 12 threads = 4x real iters/sim-s -> over-plan wobble; 3 = ~real). BUT - # that band-aid only fixed the PLANNER and REQUIRED staying slow, which LEFT the - # latency compensator (deploy_common.cc:903, wall-clock dlt predicted as sim-sec) - # over-predicting by 1/RTF -> controller<->plant OVERSHOOT/OSCILLATION. Correct - # fix = run like real: 12 threads + RTF as near 1.0 as the box allows (HEADLESS, - # --no-sensors, NO slowmo -> ~0.85). At 0.85: 12thr = ~65 iters/sim-s (~real's 55) - # AND latency over-lead only 1.18x. Do NOT slow down; slowmo breaks BOTH. If 12 - # threads oversubscribe the laptop and TANK RTF (check /clock hz), drop to ~10. + # 12 = parity with real/twin/own-sim, which all run 12. Thread count and RTF + # have to be chosen together, because the CEM planner free-runs on wall time + # (~55 iters/s at 12 threads): at RoboCasa's slow RTF that is ~4x more + # iterations per PLANT-second than twin/real, the elite-variance sampler + # collapses to its 0.01 rad floor on the near-static state, and the stand + # settles into a 5-7 plant-s wobble limit cycle. Cutting threads fixes only + # the planner half and requires staying slow, which leaves the latency + # compensator (deploy_common.cc, wall-clock dt predicted as sim-seconds) + # over-leading by 1/RTF -> controller<->plant overshoot. So run like real + # instead: 12 threads with RTF as near 1.0 as the box allows (HEADLESS, + # --no-sensors, no slowmo -> ~0.85, giving ~65 iters/sim-s against real's + # ~55 and only 1.18x latency over-lead). Do NOT slow the sim down; that + # breaks both halves. If 12 threads oversubscribe the machine and tank RTF + # (check /clock hz), drop to ~10. plan_threads: 12 - # PLANNER-RATE PARITY (2026-07-04, wobble root cause R1): the CEM planner - # free-runs on wall time (~55 iters/s at 12 threads); at RoboCasa's ~0.25x - # RTF that is ~4x more iterations per PLANT-second than twin/real -- the - # elite-variance sampler collapses to its 0.01 rad floor on the near-static - # state and can't see recovery candidates until the tilt re-spreads the - # elites = the measured 5-7 plant-s wobble limit cycle. 3 threads ~= 14 - # iters/s wall ~= the twin's iterations-per-plant-second. RoboCasa BENCH - # yamls only; twin/real chains keep --plan_threads 12. - # BENCHED 2026-07-03 (A/B on the band-drop stand test): 0 collapses into a - # knee-pegged crouch (z 0.64->0.51, legs under-driven -- unlike our twin, - # this plant has NO leg gravcomp; only the two hands); 0.85 holds height - # (z 0.91-0.99) all the way to the drop. Keep 0.85 here. + # Full gravity feed-forward, with the 0.67 kg back_equipment ballast and + # rigid feet (--rigid-feet, default ON in h12_mujoco.py). Rigid feet + # eliminate the topple — foot-slip is its dominant driver — leaving a + # vertical sink as the residual, and 1.0 holds height (z ~0.92 over 40 s) + # where 0.85 sinks to z 0.64: unlike the twin, this plant has no leg + # gravcomp, only the two hands. The remaining 5-12 deg tilt wobble is not a + # free stand; the next parity lever is the contact/integrator mismatch + # (RoboCasa runs impratio=20 / Euler / elliptic against the twin+planner's + # 100 / implicitfast / pyramidal, and the task XML warns Euler diverges on + # biped balance). REAL keeps 0.85 (mjpc_real.yaml). gravity_ff: 1.0 - # gravity_ff A/B, WITH the 0.67 kg back_equipment ballast added AND rigid feet - # (--rigid-feet, default ON in h12_mujoco.py): rigid feet ELIMINATED the topple - # (foot-slip was the dominant driver). Residual then = a vertical SINK; take 12 - # showed gff 1.0 holds height (z~0.92 for 40s) vs gff 0.85 sinking to z 0.64. - # So 1.0 here (full grav-comp) with the feet planted + mass matched. Remaining - # 5-12deg tilt wobble is NOT a stand yet -- next parity lever is the global - # integrator (RoboCasa Euler vs planner/twin implicitfast; the task XML warns - # Euler diverges on biped balance). REAL keeps 0.85 (mjpc_real.yaml). - # gravity_ff A/B, WITH the 0.67 kg back_equipment ballast added (takes 9-10, - # 2026-07-03): neither 0.85 (fwd+lat fold) nor 0 (stronger LATERAL fold + - # knee collapse) stands. The ballast is a correct parity fix but NOT the - # dominant gap -- the fold is a lateral foot-slip, root-caused (opt dump of - # the assembled robosuite model) to the CONTACT/INTEGRATOR mismatch: - # RoboCasa runs impratio=20 / integrator=Euler / cone=elliptic vs the twin+ - # planner's 100 / implicitfast / pyramidal. gravity_ff is a minor amplifier; - # keep 0.85 (matches real) until the contact model is addressed. - # gravity_ff A/B (harness takes, 2026-07-03): 0 = knee-pegged collapse; - # 0.85 = slow policy-commanded knee fold once the blend starts (z 1.03->0.87, - # low knee torque -- the fold is planned, not saturated); 1.0 = height holds - # better (z~0.97) but the tilt fight is WORSE (posture assist 80-100 Nm). - # Neither frees the stand: the divergence starts at blend onset (twin ~5.2s) - # in every take = the documented post-parity twin structural free-stand gap, - # faithfully reproduced on this plant. Keep the validated 0.85. # MUST equal RoboCasa's model.opt.timestep: the sim publishes rt/lowstate # from a RecurrentThread at exactly that interval, and the core's planner - # clock is lowstate tick * twin_dt. - # 0.005 (2026-07-04, T2-vs-T3b): the balance bench now runs the plant at - # the TWIN's dt via --sim-dt 0.005 -- the deploy-chain stand is - # dt-sensitive (twin@0.002 hangs, twin@0.005 stands even at RTF 0.5). + # clock is lowstate tick * twin_dt. The plant is run at the twin's dt + # (--sim-dt 0.005) because the deploy-chain stand is dt-sensitive: it hangs + # at 0.002 and stands at 0.005 even down at RTF 0.5. twin_dt: 0.005 - # ★ TWIN-PARITY A/B RESULT (2026-07-06): tested rt/sportmodestate (GROUND TRUTH, - # plant --truth-sportstate, VERIFIED 191 Hz + controller reading it) -> did NOT - # stop the fall. The LATERAL fold became a SAGITTAL forward->backward divergence - # (plane-floor had already fixed lateral). So the ESTIMATOR was NOT the killer. - # REVERTED to rt/sportmodestate_est = real-parity + the 30 ms LPF that damps the - # raw-truth base velocity the planner over-reacts to. Re-flip to rt/sportmodestate - # (+ plant --truth-sportstate) only to A/B raw truth again. + # REAL-PARITY STATE SOURCE: the RW-EKF estimate on rt/sportmodestate_est is + # the SAME signal path the real robot uses (the bringup already launches the + # estimator). Its filtered velocity gives a quieter stand (tilt 3-5° steady) + # than raw ground truth, whose full-bandwidth base velocity the planner + # over-reacts to; the 30 ms LPF matches real. Ground truth is not the missing + # piece — feeding rt/sportmodestate (plant --truth-sportstate, verified at + # 191 Hz) does not stop the fall, it only turns the lateral fold into a + # sagittal one. The EKF's leg-odometry xy wanders ~0.3 m under sim foot + # micro-slip, which is irrelevant for STANDING (balance = orientation + base + # velocity + CoM). Point this at rt/sportmodestate (+ sim + # --truth-sportstate) only to A/B raw truth. sportstate_topic: rt/sportmodestate_est - # REAL-PARITY STATE SOURCE (2026-07-05): read the RW-EKF estimate on - # rt/sportmodestate_est — the SAME signal path the real robot uses (the - # bringup already launches the estimator). Twin A/B at AngMom 5 showed the - # EKF-filtered velocity gives a QUIETER stand (tilt 3-5° steady vs 5-6° - # then fall) than raw truth: the earlier --truth-sportstate override was a - # self-inflicted parity gap (truth = full-bandwidth velocity the planner - # over-reacts to; the 30 ms LPF matches real). The EKF's leg-odometry xy - # wanders ~0.3 m under sim foot micro-slip, but for STANDING xy position is - # irrelevant (balance = orientation + base velocity + CoM). Set this back to - # rt/sportmodestate (+ sim --truth-sportstate) only to A/B raw truth again. imu_pitch_offset_deg: 0.0 # 1.6 is a REAL-robot IMU mount calibration; sim has none arm_aware: true # legs pre-compensate for measured arm/torso motion # With the FULL sensor set on, each 10 Hz(sim) lidar+camera cycle holds # the sim lock for up to ~1 s of wall on a laptop -- and mj_step + the # lowstate publisher pause WITH it, so sim time itself freezes: nothing - # physical happens during the gap (measured: thresholds 0.05/0.15/0.25/0.5 - # ALL tripped just past their value once per sensor cycle -- each print is - # a lower bound, the freeze outlasted every one of them). The threshold + # physical happens during the gap (thresholds of 0.05/0.15/0.25/0.5 all trip + # just past their own value once per sensor cycle, so each is a lower bound + # on the freeze). The threshold # must sit ABOVE the freeze ceiling or every sensor cycle injects a # mid-freeze damping blip into the command stream. 1.5 rides the freezes # out and still catches a genuinely dead publisher; the bring-up script is @@ -168,8 +140,8 @@ mjpc_deploy_lowerbody_controller: # the twin harness "handoff-timing" doctrine -- a self-supporting robot # leaves the band slack). Keep the launcher's TIMER release OFF: the # /elastic_band/toggle service is a stateful TOGGLE, so firing it after - # the sim already auto-released would turn the band back ON. (Timer - # releases were also benched dishonest: they fire mid-sway and dump the - # robot from whatever posture the tether wound it into.) + # the sim already auto-released turns the band back ON. A timer release is + # also dishonest: it fires mid-sway and dumps the robot from whatever + # posture the tether wound it into. drop_band: false band_drop_after_secs: 25.0 diff --git a/core_ws/src/h1_bringup/config/nav2_config.yaml b/core_ws/src/h1_bringup/config/nav2_config.yaml index ffbdec3..ca87619 100644 --- a/core_ws/src/h1_bringup/config/nav2_config.yaml +++ b/core_ws/src/h1_bringup/config/nav2_config.yaml @@ -23,9 +23,8 @@ controller_server: movement_time_allowance: 10.0 goal_checker: plugin: "nav2_controller::SimpleGoalChecker" - # Real-verified reference values (fast_lio_nav2_humanoid); the earlier - # in-tree 0.10s had drifted tighter than anything a walking humanoid - # can settle inside. + # Real-verified reference values (fast_lio_nav2_humanoid). A walking + # humanoid cannot settle inside anything tighter. xy_goal_tolerance: 0.25 yaw_goal_tolerance: 0.25 stateful: True diff --git a/core_ws/src/h1_bringup/launch/h1_grippers.launch.py b/core_ws/src/h1_bringup/launch/h1_grippers.launch.py index 8a7aec1..0248f60 100644 --- a/core_ws/src/h1_bringup/launch/h1_grippers.launch.py +++ b/core_ws/src/h1_bringup/launch/h1_grippers.launch.py @@ -23,13 +23,12 @@ def generate_launch_description(): output='screen', parameters=[{ 'auto_detect_port': False, - # OpenRB-150 serial ...FF101F14 (was ttyACM0) + # OpenRB-150 serial ...FF101F14 'port': '/dev/serial/by-id/usb-ROBOTIS_OpenRB-150_B0935A515157375037202020FF101F14-if00', 'use_eflesh': False, - # Finger angle limits (degrees), per-gripper calibration. - # Recalibrated 2026-07-21 after gripper repair. min/max are the - # measured servo angles at the open/closed stops; theta_90 - # (parallel-jaw ref) scaled from the prior parallel fraction. + # Finger angle limits (degrees), per-gripper calibration + # (2026-07-21). min/max are the measured servo angles at the + # open/closed stops; theta_90 is the parallel-jaw reference. 'finger1theta_min': 87.98, 'finger1theta_max': 162.4, 'finger1theta_90': 135.71, @@ -47,7 +46,7 @@ def generate_launch_description(): output='screen', parameters=[{ 'auto_detect_port': False, - # OpenRB-150 serial ...FF0F0E1C (replacement board, was ...FF122F35) + # OpenRB-150 serial ...FF0F0E1C 'port': '/dev/serial/by-id/usb-ROBOTIS_OpenRB-150_3EC94E825157375037202020FF0F0E1C-if00', 'use_eflesh': False, # Finger angle limits (degrees), per-gripper calibration. diff --git a/core_ws/src/h1_bringup/launch/h1_navigation.launch.py b/core_ws/src/h1_bringup/launch/h1_navigation.launch.py index 65d09dd..325a180 100644 --- a/core_ws/src/h1_bringup/launch/h1_navigation.launch.py +++ b/core_ws/src/h1_bringup/launch/h1_navigation.launch.py @@ -79,31 +79,28 @@ def generate_launch_description(): parameters=[ {'target_frame': 'pelvis'}, {'use_sim_time': use_sim_time}, - # Height band per the real-verified reference implementation - # (correlllab/fast_lio_nav2_humanoid nav2.launch.py: -0.65..+0.65 - # from pelvis). The previous in-tree -0.90 had drifted from that - # and put the FLOOR inside the band once the ALMI crouch lowered - # the pelvis to ~0.78 m — measured 2026-07-19 as a uniform lethal - # ring at every bearing in the local costmap ("Collision Ahead" - # aborts). The verified -0.65 keeps the floor out in every stance. + # Height band around the pelvis, matching the real-verified + # reference implementation (correlllab/fast_lio_nav2_humanoid + # nav2.launch.py). The lower bound must stay above the floor in + # every stance: the ALMI crouch drops the pelvis to ~0.78 m, so a + # deeper band slices in the floor and the local costmap fills with + # a uniform lethal ring ("Collision Ahead" aborts). {'min_height': -0.65}, - {'max_height': 0.65}, # verified reference value + {'max_height': 0.65}, {'angle_min': -3.14159}, {'angle_max': 3.14159}, {'angle_increment': 0.0087}, # range_min is the 2D horizontal distance from the pelvis origin - # after transforming the cloud to target_frame. 0.6 m covered the - # hanging-arm envelope, but with the IK holding the arms at home - # (and during named-config motions) the hands reach 0.6-0.8 m from - # the pelvis: measured 2026-07-19, ~25% of scan returns landed - # inside 0.8 m concentrated in the forward +/-60 deg cone, marking - # LETHAL cells inside the robot's own footprint and locking nav2 - # into "Collision Ahead" aborts (self-marks also defeat raytrace - # clearing — the arm re-marks the cell every scan). 0.9 m keeps the - # whole arm workspace inside the blind ring; the costmap inflation - # covers the wider near-field gap. + # after transforming the cloud to target_frame, i.e. the blind ring + # that keeps the robot from scanning itself. It must clear the whole + # arm workspace: the hands reach 0.6-0.8 m from the pelvis at the + # home pose and during named-config motions, and returns off the + # arms mark LETHAL cells inside the footprint that raytrace clearing + # cannot clear (the arm re-marks them every scan), locking nav2 into + # "Collision Ahead" aborts. Costmap inflation covers the near-field + # gap this leaves. {'range_min': 0.9}, - {'range_max': 3.0}, # verified reference value + {'range_max': 3.0}, {'use_inf': True}, {'scan_time': 0.0333}, {'transform_tolerance': 0.05}, diff --git a/core_ws/src/h1_bringup/launch/h1_real_controller.launch.py b/core_ws/src/h1_bringup/launch/h1_real_controller.launch.py index 00eb193..abd2fea 100644 --- a/core_ws/src/h1_bringup/launch/h1_real_controller.launch.py +++ b/core_ws/src/h1_bringup/launch/h1_real_controller.launch.py @@ -11,11 +11,10 @@ # is unset), while the controller-side rclpy nodes pick up the env directly. # If the variable is missing, the DDS half and the rclpy half can end up on # different domains and the safety layer will see no commands. -# Absolute path to the checkout on the robot's onboard PC (renamed HAMS -> -# GOLEM). This URDF is read at launch-description build time, so a wrong path -# aborts the entire controller launch -- estop, state publishers, safety layer -# and frame_task all fail to start. GOLEM_ASSETS_DIR overrides it without a code -# edit if the onboard checkout has not been renamed yet. +# Absolute path to the checkout on the robot's onboard PC. This URDF is read at +# launch-description build time, so a wrong path aborts the entire controller +# launch -- estop, state publishers, safety layer and frame_task all fail to +# start. GOLEM_ASSETS_DIR overrides it without a code edit. ASSETS_DIR = os.environ.get('GOLEM_ASSETS_DIR', '/home/unitree/GOLEM/CL_Assets') diff --git a/core_ws/src/h1_bringup/launch/h1_sim_bringup.launch.py b/core_ws/src/h1_bringup/launch/h1_sim_bringup.launch.py index a4c4c03..b5317ee 100644 --- a/core_ws/src/h1_bringup/launch/h1_sim_bringup.launch.py +++ b/core_ws/src/h1_bringup/launch/h1_sim_bringup.launch.py @@ -83,12 +83,11 @@ def generate_launch_description(): # controller (h12_lowerbody_rl lowerbody_controller_node): ALMI stands, # hands off to the walk policy on /cmd_vel, and follows nav2. 'fame' / # 'walk' pin that same RL controller to those policies instead. The RL - # controller launches for any value != 'mjpc'. NOTE: the MJPC nodes - # below are currently COMMENTED OUT, so 'mjpc' launches NO leg - # controller (legs uncontrolled) — the RL controller is the only working - # sim option right now, which is why the default is 'almi'. Only the SIM - # bringup defaults to almi; the real-robot bringup still defaults to mjpc - # behind the start_position_verified interlock. + # controller launches for any value != 'mjpc'. 'mjpc' launches NO leg + # controller here — the MJPC nodes below are disabled, so the legs are + # left uncontrolled — which is why the sim default is 'almi'. The + # real-robot bringup does default to mjpc, behind the + # start_position_verified interlock. DeclareLaunchArgument('lowerbody', default_value='almi'), # almi | almi27 | fame | walk DeclareLaunchArgument('model_logging', default_value='true'), DeclareLaunchArgument('model_visualization', default_value='true'), diff --git a/core_ws/src/h1_bringup/rviz/sim.rviz b/core_ws/src/h1_bringup/rviz/sim.rviz index 352a88a..c69e1e0 100644 --- a/core_ws/src/h1_bringup/rviz/sim.rviz +++ b/core_ws/src/h1_bringup/rviz/sim.rviz @@ -6,8 +6,6 @@ Panels: Expanded: - /Global Options1 - /Status1 - - /TF1 - - /TF1/Frames1 - /HeadDepthImage1/Topic1 - /HeadColorImage1/Topic1 - /LeftDepthImage1/Topic1 @@ -25,7 +23,7 @@ Panels: - /ServoImage1 - /ServoImage1/Topic1 Splitter Ratio: 0.5 - Tree Height: 365 + Tree Height: 1080 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -43,7 +41,7 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: "" + SyncSource: CloudRegisteredBody Visualization Manager: Class: "" Displays: @@ -72,22 +70,14 @@ Visualization Manager: All Enabled: false body: Value: false + camera_color_optical_frame: + Value: true camera_init: Value: false - graspgenx_target_frame: + camera_link: Value: true head_camera_link: Value: false - head_color_frame: - Value: false - head_color_optical_frame: - Value: false - head_depth_frame: - Value: false - head_depth_optical_frame: - Value: false - head_link: - Value: false imu_link: Value: false left_ankle_pitch_link: @@ -100,16 +90,6 @@ Visualization Manager: Value: false left_graspgenx_frame: Value: false - left_hand_color_frame: - Value: false - left_hand_color_optical_frame: - Value: false - left_hand_depth_frame: - Value: false - left_hand_depth_optical_frame: - Value: false - left_hand_link: - Value: false left_hip_pitch_link: Value: false left_hip_roll_link: @@ -156,8 +136,6 @@ Visualization Manager: Value: false pelvis: Value: false - pick_place_target_frame: - Value: true rg_base_bot: Value: false rg_base_top: @@ -186,16 +164,6 @@ Visualization Manager: Value: false right_graspgenx_frame: Value: false - right_hand_color_frame: - Value: false - right_hand_color_optical_frame: - Value: false - right_hand_depth_frame: - Value: false - right_hand_depth_optical_frame: - Value: false - right_hand_link: - Value: false right_hip_pitch_link: Value: false right_hip_roll_link: @@ -224,101 +192,79 @@ Visualization Manager: Show Axes: true Show Names: true Tree: - map: - camera_init: - body: - pelvis: - left_hip_yaw_link: - left_hip_pitch_link: - left_hip_roll_link: - left_knee_link: - left_ankle_pitch_link: - left_ankle_roll_link: - {} - right_hip_yaw_link: - right_hip_pitch_link: - right_hip_roll_link: - right_knee_link: - right_ankle_pitch_link: - right_ankle_roll_link: - {} - torso_link: - head_camera_link: - head_link: - head_color_frame: - head_color_optical_frame: - {} - head_depth_frame: - head_depth_optical_frame: - {} - imu_link: - {} - left_shoulder_pitch_link: - left_shoulder_roll_link: - left_shoulder_yaw_link: - left_elbow_link: - left_wrist_roll_link: - left_wrist_pitch_link: - left_wrist_yaw_link: - left_grasp_frame: - {} - left_graspgenx_frame: - {} - lg_mount: - left_hand_link: - left_hand_color_frame: - left_hand_color_optical_frame: + body: + pelvis: + left_hip_yaw_link: + left_hip_pitch_link: + left_hip_roll_link: + left_knee_link: + left_ankle_pitch_link: + left_ankle_roll_link: + {} + right_hip_yaw_link: + right_hip_pitch_link: + right_hip_roll_link: + right_knee_link: + right_ankle_pitch_link: + right_ankle_roll_link: + {} + torso_link: + head_camera_link: + {} + imu_link: + {} + left_shoulder_pitch_link: + left_shoulder_roll_link: + left_shoulder_yaw_link: + left_elbow_link: + left_wrist_roll_link: + left_wrist_pitch_link: + left_wrist_yaw_link: + left_grasp_frame: + {} + left_graspgenx_frame: + {} + lg_mount: + lg_base_bot: + lg_base_top: + lg_left_crank: + lg_left_finger: + lg_left_rocker: {} - left_hand_depth_frame: - left_hand_depth_optical_frame: + lg_right_crank: + lg_right_finger: + lg_right_rocker: {} - lg_base_bot: - lg_base_top: - lg_left_crank: - lg_left_finger: - lg_left_rocker: - {} - lg_right_crank: - lg_right_finger: - lg_right_rocker: - {} - livox_link: - {} - logo_link: - {} - right_shoulder_pitch_link: - right_shoulder_roll_link: - right_shoulder_yaw_link: - right_elbow_link: - right_wrist_roll_link: - right_wrist_pitch_link: - right_wrist_yaw_link: - rg_mount: - rg_base_bot: - rg_base_top: - rg_left_crank: - rg_left_finger: - rg_left_rocker: - {} - rg_right_crank: - rg_right_finger: - rg_right_rocker: - {} - right_hand_link: - right_hand_color_frame: - right_hand_color_optical_frame: + livox_link: + {} + logo_link: + {} + right_shoulder_pitch_link: + right_shoulder_roll_link: + right_shoulder_yaw_link: + right_elbow_link: + right_wrist_roll_link: + right_wrist_pitch_link: + right_wrist_yaw_link: + rg_mount: + rg_base_bot: + rg_base_top: + rg_left_crank: + rg_left_finger: + rg_left_rocker: {} - right_hand_depth_frame: - right_hand_depth_optical_frame: + rg_right_crank: + rg_right_finger: + rg_right_rocker: {} - right_grasp_frame: - {} - right_graspgenx_frame: - {} - graspgenx_target_frame: - {} - pick_place_target_frame: - {} + right_grasp_frame: + {} + right_graspgenx_frame: + {} + camera_init: + {} + map: + {} Update Interval: 0 Value: true - Alpha: 1 @@ -333,7 +279,7 @@ Visualization Manager: Color: 255; 255; 255 Color Transformer: AxisColor Decay Time: 0 - Enabled: false + Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 0 @@ -354,7 +300,7 @@ Visualization Manager: Value: /livox/pointcloud Use Fixed Frame: true Use rainbow: true - Value: false + Value: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: @@ -401,7 +347,7 @@ Visualization Manager: Color: 255; 255; 255 Color Transformer: Intensity Decay Time: 0 - Enabled: false + Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 0 @@ -422,7 +368,7 @@ Visualization Manager: Value: /cloud_registered_body Use Fixed Frame: true Use rainbow: true - Value: false + Value: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: @@ -506,7 +452,7 @@ Visualization Manager: Value: /realsense/head/aligned_depth_to_color/image_raw Value: false - Class: rviz_default_plugins/Image - Enabled: false + Enabled: true Max Value: 1 Median window: 5 Min Value: 0 @@ -518,7 +464,7 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Best Effort Value: /realsense/head/color/image_raw - Value: false + Value: true - Class: rviz_default_plugins/Image Enabled: false Max Value: 1 @@ -534,7 +480,7 @@ Visualization Manager: Value: /realsense/left_hand/aligned_depth_to_color/image_raw Value: false - Class: rviz_default_plugins/Image - Enabled: false + Enabled: true Max Value: 1 Median window: 5 Min Value: 0 @@ -546,7 +492,7 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Best Effort Value: /realsense/left_hand/color/image_raw - Value: false + Value: true - Class: rviz_default_plugins/Image Enabled: false Max Value: 1 @@ -562,7 +508,7 @@ Visualization Manager: Value: /realsense/right_hand/aligned_depth_to_color/image_raw Value: false - Class: rviz_default_plugins/Image - Enabled: false + Enabled: true Max Value: 1 Median window: 5 Min Value: 0 @@ -574,7 +520,7 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Best Effort Value: /realsense/right_hand/color/image_raw - Value: false + Value: true - Alpha: 1 Class: rviz_default_plugins/RobotModel Collision Enabled: false @@ -868,7 +814,7 @@ Visualization Manager: Class: rviz_default_plugins/Map Color Scheme: map Draw Behind: false - Enabled: false + Enabled: true Name: Map Topic: Depth: 5 @@ -884,7 +830,7 @@ Visualization Manager: Reliability Policy: Reliable Value: /map_updates Use Timestamp: false - Value: false + Value: true - Alpha: 0.699999988079071 Class: rviz_default_plugins/Map Color Scheme: map @@ -1363,7 +1309,7 @@ Visualization Manager: Use rainbow: true Value: false - Class: rviz_default_plugins/Image - Enabled: true + Enabled: false Max Value: 1 Median window: 5 Min Value: 0 @@ -1375,7 +1321,7 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Best Effort Value: /realsense/head/color/image_raw/compressed/annotated - Value: true + Value: false - Class: rviz_default_plugins/Image Enabled: false Max Value: 1 @@ -1405,7 +1351,7 @@ Visualization Manager: Value: /realsense/right_hand/color/image_raw/compressed/annotated Value: false - Class: rviz_default_plugins/Image - Enabled: true + Enabled: false Max Value: 1 Median window: 5 Min Value: 0 @@ -1417,7 +1363,7 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Best Effort Value: /skill/grasp/servo_image - Value: true + Value: false Enabled: true Global Options: Background Color: 48; 48; 48 @@ -1464,7 +1410,7 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 3.1023149490356445 + Distance: 8.231671333312988 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -1479,10 +1425,10 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.6597973704338074 + Pitch: 0.5197973847389221 Target Frame: Value: Orbit (rviz_default_plugins) - Yaw: 1.8074543476104736 + Yaw: 5.440641403198242 Saved: ~ Window Geometry: Displays: @@ -1493,7 +1439,7 @@ Window Geometry: collapsed: false HeadDepthImage: collapsed: false - Height: 1016 + Height: 1371 Hide Left Dock: false Hide Right Dock: true LeftColorImage: @@ -1502,7 +1448,7 @@ Window Geometry: collapsed: false LeftHandAnnotatedImage: collapsed: false - QMainWindow State: 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 + QMainWindow State: 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 RightColorImage: collapsed: false RightDepthImage: @@ -1519,6 +1465,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 925 - X: 995 - Y: 27 + Width: 1687 + X: 66 + Y: 32 diff --git a/core_ws/src/h1_bringup/test/test_real_lidar_config.py b/core_ws/src/h1_bringup/test/test_real_lidar_config.py new file mode 100644 index 0000000..39ee74f --- /dev/null +++ b/core_ws/src/h1_bringup/test/test_real_lidar_config.py @@ -0,0 +1,59 @@ +"""Guards the real-robot MID360 IMU/point frame agreement. + +livox_ros_driver2 rotates only the points by MID360_config.json's extrinsic; +livox_imu_upright must rotate the IMU identically, and mid360.yaml (shared +with the sims) must then treat the two as one frame. If any of the three drifts, FAST-LIO builds an +upside-down or diverging map on the real robot while the sim stays fine. +""" + +import os +import types + +import yaml + +from h1_bringup.livox_imu_upright import load_rotation, rotate + +PKG = os.path.dirname(os.path.dirname(os.path.abspath(__file__))) +CONFIG = os.path.join(PKG, 'config') + + +def _vec(x, y, z): + return types.SimpleNamespace(x=x, y=y, z=z) + + +def test_raw_inverted_gravity_reads_up_after_rotation(): + r = load_rotation(os.path.join(CONFIG, 'MID360_config.json')) + # At rest the inverted MID360 IMU reads about (-0.19, -0.01, -0.98) g. + acc = _vec(-0.19, -0.01, -0.98) + rotate(r, acc) + assert acc.z > 0.9 + assert abs(acc.x + 0.19) < 1e-6 + + +def test_fast_lio_config_expects_upright_imu(): + with open(os.path.join(CONFIG, 'mid360.yaml')) as f: + params = yaml.safe_load(f)['/**']['ros__parameters'] + assert params['common']['imu_topic'] == '/livox/imu' + assert params['mapping']['extrinsic_R'] == [1., 0., 0., 0., 1., 0., 0., 0., 1.] + + +def test_fast_lio_keeps_every_lidar_line(): + # line < scan_line is a hard filter: the real MID360 emits lines 0-3 and + # both sims 0-5, so anything below 6 silently drops sim points. + with open(os.path.join(CONFIG, 'mid360.yaml')) as f: + params = yaml.safe_load(f)['/**']['ros__parameters'] + assert params['preprocess']['scan_line'] >= 6 + + +def test_one_slam_config_set(): + names = sorted(os.listdir(CONFIG)) + assert [n for n in names if n.startswith('mid360')] == ['mid360.yaml'] + assert [n for n in names if n.startswith('slam_toolbox')] == ['slam_toolbox_h1.yaml'] + + +def test_real_drivers_route_imu_through_upright_node(): + path = os.path.join(PKG, 'launch', 'h1_real_drivers.launch.py') + with open(path) as f: + src = f.read() + assert "('/livox/imu', '/livox/imu_raw')" in src + assert "executable='livox_imu_upright'" in src diff --git a/core_ws/src/model_server/model_server/graspgen_server.py b/core_ws/src/model_server/model_server/graspgen_server.py index 132ad9d..2266a96 100644 --- a/core_ws/src/model_server/model_server/graspgen_server.py +++ b/core_ws/src/model_server/model_server/graspgen_server.py @@ -71,13 +71,12 @@ # when the gripper sweep volume comes within this distance of an obstacle, so a big # value (e.g. 5 cm) rejects grasps that merely pass NEAR the scene and can filter # everything; keep it small (near-contact only). Tune up only if grasps clip obstacles. -# Loosened 5mm -> 2mm to ADMIT ROLLED grasps: the scene cloud is sparse (downsampled -# to MAX_SCENE_POINTS), so 5mm is really a clearance band, not penetration — a rolled -# gripper (fingers closing vertically) merely passes ~2-5mm from the support surface -# and was being rejected, collapsing the output to upright (near-yaw-only) grasps. -# Empirically 5mm returned 0 collision-free grasps on 3 of 4 recent calls; 2mm -# restores a full roll spread AND un-breaks those calls. Raise back toward 5mm only -# if grasps start clipping obstacles. +# 2mm is small enough to ADMIT ROLLED grasps: the scene cloud is sparse (downsampled +# to MAX_SCENE_POINTS), so this is really a clearance band, not penetration — a rolled +# gripper (fingers closing vertically) passes only ~2-5mm from the support surface, so +# a 5mm band rejects it, collapses the output to upright (near-yaw-only) grasps, and +# returns 0 collision-free grasps on most calls. Raise toward 5mm only if grasps start +# clipping obstacles. COLLISION_THRESHOLD_M = 0.002 # Scene points within this radius of the object cloud are treated as the object # (not obstacles) and dropped before the collision check. MUST be >= @@ -104,15 +103,14 @@ # approach's z-component (col 2, row 2). Kept when |az| <= sin(APPROACH_MAX_ # PITCH_DEG). At 90 deg this admits every pitch (sin(90)=1), i.e. the gate is # effectively disabled — near-vertical top-down / bottom-up approaches pass. -# (Earlier reach notes put the arm's steepest reachable approach at ~45 deg -# below horizontal, so values above ~45 may admit unreachable grasps.) +# (The arm's steepest reachable approach is ~45 deg below horizontal, so +# values above ~45 may admit unreachable grasps.) # * YAW — heading of +Z off robot-forward +X in the horizontal plane = # atan2(ay, ax) (approach's y- and x-components). Kept when |yaw| <= # APPROACH_MAX_YAW_DEG. A SYMMETRIC forward cone (both sides); at 180 deg # |yaw| <= pi is always true, so every heading passes — including sideways # and behind-the-object approaches. -# The two gates are ANDed. Replaces the old single forward-cone (APPROACH_MAX_ -# ANGLE_DEG) plus per-arm lateral cut with an explicit pitch/yaw reach envelope. +# The two gates are ANDed into an explicit pitch/yaw reach envelope. # NOTE: with pitch=90 and yaw=180 BOTH gates are no-ops, so the approach-direction # filter is effectively disabled — the widest, most varied grasp set is returned # and reachability pruning is left entirely to the grasp skill's IK fallback. Dial @@ -143,14 +141,16 @@ # Candidate planner kwargs (filtered to run_planner_on_object's real signature). # Tuned for MAXIMUM grasp count and variety (paired with the disabled approach -# filter above). Levers, vs. the stock 1024 / 36-yaw / 2-offset / 1cm config: -# * num_grasps / topk_num_grasps 1024 -> 2048: keep twice as many ranked grasps. -# * moe_num_yaws 36 -> 72: sample a yaw every 5 deg (was 10), doubling the -# rotational spread of finger-closing directions at each grasp position. -# * moe_z_offsets_cm (-2,0) -> (-4,-2,0,2): four approach-DEPTH standoffs instead -# of two, so grasps vary along the approach axis as well. -# * moe_obb_position_spacing_cm 1.0 -> 0.5: sample OBB grasp positions on a 5mm -# grid (was 10mm), roughly 4x the spatial candidate density. +# filter above). Each lever, against GraspGenX's stock 1024 / 36-yaw / 2-offset / +# 1cm config: +# * num_grasps / topk_num_grasps 2048 (stock 1024): keep twice as many ranked +# grasps. +# * moe_num_yaws 72 (stock 36): sample a yaw every 5 deg instead of 10, doubling +# the rotational spread of finger-closing directions at each grasp position. +# * moe_z_offsets_cm (-4,-2,0,2) (stock (-2,0)): four approach-DEPTH standoffs +# instead of two, so grasps vary along the approach axis as well. +# * moe_obb_position_spacing_cm 0.5 (stock 1.0): sample OBB grasp positions on a +# 5mm grid instead of 10mm, roughly 4x the spatial candidate density. # grasp_threshold stays -1.0 (no confidence cut). These multiply the pre-topk # candidate pool, so planning is heavier/slower on the GPU — trim any lever back # if latency matters more than breadth. @@ -424,10 +424,6 @@ def _plan_cb(self, request, response): order = np.argsort(-confs) grasps, confs = grasps[order], confs[order] - # print("\n\n\n") - # print(f"{type(grasps)=}") - # print(f"{grasps.shape=}") - # print("\n\n\n") # Approach-direction filter, in `frame` (pelvis: +x forward, +y left, +z up). # Keep grasps whose +Z approach axis (col 2) lies inside the gripper's reach # envelope: pitch (tilt off horizontal) within APPROACH_MAX_PITCH_DEG AND yaw diff --git a/docs/NAVIGATION_DEMO.md b/docs/NAVIGATION_DEMO.md index e589e65..ca87941 100644 --- a/docs/NAVIGATION_DEMO.md +++ b/docs/NAVIGATION_DEMO.md @@ -4,9 +4,8 @@ This walks the H1-2 through a RoboCasa kitchen **autonomously**: it builds a map with SLAM, plans collision-free paths around the furniture with nav2, and drives itself to unexplored frontiers — all on the headless CPU sim, no GPU. -It runs on top of the macOS (Apple-Silicon) port; see the **macOS (Apple -Silicon)** section of the top-level [`README.md`](../README.md) for the one-time -Colima/Docker setup, image builds, and the noVNC tunnel. +It runs on top of the macOS (Apple-Silicon) port; see [`MACOS.md`](MACOS.md) +for the one-time Colima/Docker setup, image builds, and the noVNC tunnel. ## What it demonstrates diff --git a/h1_robocasa/measurement_bridge.py b/h1_robocasa/measurement_bridge.py index 6742a76..e0a5e07 100644 --- a/h1_robocasa/measurement_bridge.py +++ b/h1_robocasa/measurement_bridge.py @@ -27,8 +27,8 @@ def __init__(self, env, debounce_steps=15, elastic_band=None, task_name=""): self.lang = "" self.task_name = task_name - # Optional elastic-band toggle service (same contract as the legacy - # RosSensorBridge: std_srvs/Trigger -> ElasticBand.toggle()). Lets the + # Optional elastic-band toggle service (same contract as + # RosSensorBridge's: std_srvs/Trigger -> ElasticBand.toggle()). Lets the # band be released without the windowed viewer / SPACE key. self.elastic_band = elastic_band if elastic_band is not None: From a122aa4c8c1642e1ebe6754e0d59b025fc3fe2f3 Mon Sep 17 00:00:00 2001 From: maxconway Date: Wed, 23 Sep 2026 17:26:36 -0600 Subject: [PATCH 007/225] RoboCasa at 1x real time with all sensors, clearer spawn, ALMI-only legs Simulator speed (RTF 0.2 -> 1.0 headless, 0.16 -> 1.0 windowed): - Run RoboCasa fixture updates and success checks once per control step, as RoboCasa's env.step does, instead of every physics step. - Serialize Livox CustomMsg scans as one packed buffer (byte-identical to rclpy serialization) instead of 20k Python point objects. - Cast lidar scans and render cameras from MjData snapshots on per-sensor worker threads; schedules, stamps and messages are unchanged, no drops. - Always use EGL for offscreen cameras, creating the context before the passive viewer opens, and sync the viewer from its own MjData on a 60 Hz thread; mouse perturbation forces are forwarded to the simulation. Spawn: back the robot off until every collision geom is GOLEM_SPAWN_MARGIN (default 0.20 m, both platforms) from non-floor geoms in the spawn pose and in the pose the unpowered upper body sags into. Distances use mj_geomDistance with the midphase disabled (inflated box-box contacts and multi-geom midphase culling misreport them); the search never passes an obstacle behind the base. Lower body: add lowerbody_auto_switch (default false) so ALMI stands and walks without handing off to the walk policy; the stack test expects ALMI throughout. docker_run.sh/docker_stack.sh inspect containers by type, since the containers share names with their images. Co-Authored-By: Claude Opus 5.5 (1M context) --- AGENTS.md | 2 + CLAUDE.md | 2 + .../launch/h1_real_desktop_bringup.launch.py | 15 +- .../launch/h1_sim_bringup.launch.py | 22 +- docker/docker-compose.yml | 6 +- docker/mac/docker-compose.yml | 2 + docker/scripts/docker_run.sh | 2 +- docker/scripts/docker_stack.sh | 6 +- docker/scripts/launch_robocasa.sh | 9 +- docs/MACOS.md | 1 + h1_robocasa/h12_mujoco.py | 135 ++++++++-- h1_robocasa/h1_2_robosuite.py | 146 ++++++++--- h1_robocasa/measurement_bridge.py | 13 +- h1_robocasa/mujoco_ros_bridge.py | 237 ++++++++++++------ h1_robocasa/sim_timing.py | 20 +- h1_robocasa/test_spawn_collision.py | 94 +++++++ tests/docker/test_container_checks.py | 20 ++ tests/robocasa/README.md | 18 +- tests/robocasa/test_sensor_bridge.py | 94 ++++++- tests/stack/README.md | 5 +- tests/stack/minimal_stack.launch.py | 1 + tests/stack/run_checker.sh | 2 +- 22 files changed, 671 insertions(+), 181 deletions(-) create mode 100644 tests/docker/test_container_checks.py diff --git a/AGENTS.md b/AGENTS.md index 460b3b6..a5b0b2f 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -69,6 +69,8 @@ Mac feature toggles are env vars read by the compose file: `GOLEM_DISPLAY=vnc` (MuJoCo viewer → noVNC :6080), `GOLEM_RVIZ=vnc` (RViz → :6081), `GOLEM_LOWERBODY=fame|walk|switch`, `GOLEM_SLAM=1`, `GOLEM_NAV2=1`, `GOLEM_SIM_ODOM=1`, `GOLEM_CAMERAS=0`, `GOLEM_SPAWN_BACKOFF=`, `GOLEM_CMD_TIMEOUT=`, `GOLEM_ROS_MCP=1`. +`GOLEM_SPAWN_MARGIN=` (default 0.20) sets the robot's minimum spawn clearance +from non-floor geoms on both platforms. GUIs stream over noVNC via `docker/mac/scripts/mac_vnc_tunnel.sh` (no XQuartz). ### DDS domain safety (both platforms) diff --git a/CLAUDE.md b/CLAUDE.md index f1406cc..dbd7227 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -69,6 +69,8 @@ Mac feature toggles are env vars read by the compose file: `GOLEM_DISPLAY=vnc` (MuJoCo viewer → noVNC :6080), `GOLEM_RVIZ=vnc` (RViz → :6081), `GOLEM_LOWERBODY=fame|walk|switch`, `GOLEM_SLAM=1`, `GOLEM_NAV2=1`, `GOLEM_SIM_ODOM=1`, `GOLEM_CAMERAS=0`, `GOLEM_SPAWN_BACKOFF=`, `GOLEM_CMD_TIMEOUT=`, `GOLEM_ROS_MCP=1`. +`GOLEM_SPAWN_MARGIN=` (default 0.20) sets the robot's minimum spawn clearance +from non-floor geoms on both platforms. GUIs stream over noVNC via `docker/mac/scripts/mac_vnc_tunnel.sh` (no XQuartz). ### DDS domain safety (both platforms) diff --git a/core_ws/src/h1_bringup/launch/h1_real_desktop_bringup.launch.py b/core_ws/src/h1_bringup/launch/h1_real_desktop_bringup.launch.py index 6a0848e..a5cd9b4 100644 --- a/core_ws/src/h1_bringup/launch/h1_real_desktop_bringup.launch.py +++ b/core_ws/src/h1_bringup/launch/h1_real_desktop_bringup.launch.py @@ -74,6 +74,10 @@ def generate_launch_description(): # node additionally holds at the pre-pose crouch until the operator # confirms: ros2 service call /lowerbody/confirm_engage std_srvs/srv/Trigger DeclareLaunchArgument('lowerbody', default_value='almi'), + # auto_switch on the RL controller: false (default) keeps the robot on + # the lowerbody:= policy for standing AND walking (ALMI all the way); + # true hands nonzero /cmd_vel to the walk policy and back. + DeclareLaunchArgument('lowerbody_auto_switch', default_value='false'), DeclareLaunchArgument('use_skills', default_value='true'), DeclareLaunchArgument('model_logging', default_value='true'), DeclareLaunchArgument('model_visualization', default_value='true'), @@ -148,18 +152,19 @@ def generate_launch_description(): condition=IfCondition(LaunchConfiguration('use_skills')), ), - # Switchable lower-body RL controller (almi / fame / walk) — launched - # only when lowerbody:= names an RL policy AND the operator has verified + # Lower-body RL controller (almi / fame / walk) — launched only + # when lowerbody:= names an RL policy AND the operator has verified # the start position; the MJPC controller below is gated off in that - # case. The chosen policy is pinned as active_policy (fame/walk still - # auto-switch between themselves on /cmd_vel; almi never auto-switches — - # it stands AND walks itself). + # case. The chosen policy stays active for standing and walking unless + # lowerbody_auto_switch:=true. Node( package='h12_lowerbody_rl', executable='lowerbody_controller_node', name='lowerbody_controller_node', parameters=[sim_time_param, {'active_policy': LaunchConfiguration('lowerbody'), + 'auto_switch': ParameterValue( + LaunchConfiguration('lowerbody_auto_switch'), value_type=bool), # The elastic band is a SIM fixture; on real there is # no /elastic_band/toggle service, so the release path # only costs a 1 s blocking wait_for_service inside the diff --git a/core_ws/src/h1_bringup/launch/h1_sim_bringup.launch.py b/core_ws/src/h1_bringup/launch/h1_sim_bringup.launch.py index b5317ee..f13b416 100644 --- a/core_ws/src/h1_bringup/launch/h1_sim_bringup.launch.py +++ b/core_ws/src/h1_bringup/launch/h1_sim_bringup.launch.py @@ -79,16 +79,20 @@ def generate_launch_description(): # mp4 (written on shutdown). Reads rt/lowstate + the estimator + rt/mjpc/plan; # commands nothing. use_mjpc_viz:=false for a clean bringup. DeclareLaunchArgument('use_mjpc_viz', default_value='true'), - # Which controller drives the legs. 'almi' (default) = the switchable RL - # controller (h12_lowerbody_rl lowerbody_controller_node): ALMI stands, - # hands off to the walk policy on /cmd_vel, and follows nav2. 'fame' / - # 'walk' pin that same RL controller to those policies instead. The RL + # Which controller drives the legs. 'almi' (default) = the RL controller + # (h12_lowerbody_rl lowerbody_controller_node) running ALMI for standing + # and walking, following /cmd_vel from nav2. 'fame' / 'walk' pin that + # same RL controller to those policies instead. The RL # controller launches for any value != 'mjpc'. 'mjpc' launches NO leg # controller here — the MJPC nodes below are disabled, so the legs are # left uncontrolled — which is why the sim default is 'almi'. The # real-robot bringup does default to mjpc, behind the # start_position_verified interlock. DeclareLaunchArgument('lowerbody', default_value='almi'), # almi | almi27 | fame | walk + # auto_switch on the RL controller: false (default) keeps the robot on + # the lowerbody:= policy for standing AND walking (ALMI all the way); + # true hands nonzero /cmd_vel to the walk policy and back. + DeclareLaunchArgument('lowerbody_auto_switch', default_value='false'), DeclareLaunchArgument('model_logging', default_value='true'), DeclareLaunchArgument('model_visualization', default_value='true'), DeclareLaunchArgument('model_clear_logs', default_value='true'), @@ -164,16 +168,18 @@ def generate_launch_description(): output='screen', ), - # Switchable lower-body RL controller (almi / fame / walk) — launched - # only when lowerbody:= names an RL policy; the MJPC controller below - # is gated off in that case. The chosen policy is pinned as - # active_policy (fame/walk still auto-switch between themselves on + # Lower-body RL controller (almi / fame / walk) — launched only when + # lowerbody:= names an RL policy; the MJPC controller below is gated off + # in that case. The chosen policy stays active throughout unless + # lowerbody_auto_switch:=true. Node( package='h12_lowerbody_rl', executable='lowerbody_controller_node', name='lowerbody_controller_node', parameters=[sim_time_param, {'active_policy': LaunchConfiguration('lowerbody'), # almi/almi27/fame/walk + 'auto_switch': ParameterValue( + LaunchConfiguration('lowerbody_auto_switch'), value_type=bool), # Sim auto-engages after the pre-pose settles (unattended # runs); the operator-confirm arming gate is a REAL-robot # step (see h1_real_desktop_bringup.launch.py). diff --git a/docker/docker-compose.yml b/docker/docker-compose.yml index 23bc699..5a8539f 100644 --- a/docker/docker-compose.yml +++ b/docker/docker-compose.yml @@ -81,10 +81,12 @@ services: - NVIDIA_DRIVER_CAPABILITIES=all - QT_X11_NO_MITSHM=1 - LIBGL_ALWAYS_INDIRECT=0 - # Default to GLFW (native passive viewer). Export MUJOCO_GL=egl on the - # host (or pass --headless to launch_robocasa.sh) for offscreen rendering. + # launch_robocasa.sh exports MUJOCO_GL=egl for offscreen cameras in both + # display modes; the passive viewer opens its own GLFW window. - MUJOCO_GL=${MUJOCO_GL:-glfw} - ROS_DOMAIN_ID=${ROS_DOMAIN_ID:-1} + # Minimum free space (m) between the robot and any non-floor geom at spawn. + - GOLEM_SPAWN_MARGIN=${GOLEM_SPAWN_MARGIN:-0.20} volumes: - type: bind source: ../h1_robocasa diff --git a/docker/mac/docker-compose.yml b/docker/mac/docker-compose.yml index 8a0e168..c2bea40 100644 --- a/docker/mac/docker-compose.yml +++ b/docker/mac/docker-compose.yml @@ -52,6 +52,8 @@ services: # room to plan and keeps FAME from leaning into a nearby counter. NOTE: baked # at container-create -- change it with `up --force-recreate`, not `restart`. - GOLEM_SPAWN_BACKOFF=${GOLEM_SPAWN_BACKOFF:-0} + # Minimum free space (m) between the robot and any non-floor geom at spawn. + - GOLEM_SPAWN_MARGIN=${GOLEM_SPAWN_MARGIN:-0.20} # rclpy runs on FastDDS so it does not load a 2nd CycloneDDS libddsc # alongside unitree_sdk2py's (arm64 in-process coexistence fix). - RMW_IMPLEMENTATION=rmw_fastrtps_cpp diff --git a/docker/scripts/docker_run.sh b/docker/scripts/docker_run.sh index ffa40d6..7fefbd4 100755 --- a/docker/scripts/docker_run.sh +++ b/docker/scripts/docker_run.sh @@ -14,7 +14,7 @@ for arg in "$@"; do [ "$arg" != --headless ] || export GOLEM_DISPLAY=headless; d configure_display prepare_mounts "$SIM" NAME=$(container_name "$SIM") -if docker inspect "$NAME" >/dev/null 2>&1; then +if docker inspect --type container "$NAME" >/dev/null 2>&1; then [ "$RESTART" = 1 ] || fail "$NAME already exists. Use --restart explicitly, or attach with docker exec -it $NAME bash." docker rm -f "$NAME" fi diff --git a/docker/scripts/docker_stack.sh b/docker/scripts/docker_stack.sh index d491a39..7b26c9c 100755 --- a/docker/scripts/docker_stack.sh +++ b/docker/scripts/docker_stack.sh @@ -21,7 +21,7 @@ done if [ "$ACTION" = stop ]; then for service in ros "$SIM"; do name=$(container_name "$service") - if docker inspect "$name" >/dev/null 2>&1; then docker stop "$name"; fi + if docker inspect --type container "$name" >/dev/null 2>&1; then docker stop "$name"; fi done exit 0 fi @@ -34,12 +34,12 @@ if [ "$ACTION" = restart ]; then # Explicit restart also replaces one-off containers created by docker_run.sh. for service in ros "$SIM"; do name=$(container_name "$service") - if docker inspect "$name" >/dev/null 2>&1; then docker rm -f "$name"; fi + if docker inspect --type container "$name" >/dev/null 2>&1; then docker rm -f "$name"; fi done else for service in "$SIM" ros; do name=$(container_name "$service") - if [ "$(docker inspect -f '{{.State.Running}}' "$name" 2>/dev/null || true)" = true ]; then + if [ "$(docker inspect --type container -f '{{.State.Running}}' "$name" 2>/dev/null || true)" = true ]; then fail "$name is already running. Use the stack restart command to restart both coherently." fi done diff --git a/docker/scripts/launch_robocasa.sh b/docker/scripts/launch_robocasa.sh index 646d102..548a36d 100755 --- a/docker/scripts/launch_robocasa.sh +++ b/docker/scripts/launch_robocasa.sh @@ -39,11 +39,10 @@ case " $* " in ;; esac -# MUJOCO_GL: glfw needs an X display, egl is offscreen. -case " $* " in - *" --headless "*) export MUJOCO_GL=egl ;; - *) export MUJOCO_GL=glfw ;; -esac +# MUJOCO_GL selects the offscreen renderer for the RGBD cameras. EGL needs no +# display and can render on the camera worker thread; the passive viewer opens +# its own GLFW window independently of it. +export MUJOCO_GL=egl cd /home/code/h1_robocasa # -u: docker-compose captures stdout via a pipe, which block-buffers python's diff --git a/docs/MACOS.md b/docs/MACOS.md index 8e9733a..4874d8a 100644 --- a/docs/MACOS.md +++ b/docs/MACOS.md @@ -62,6 +62,7 @@ changing one needs the stack `restart` command to recreate both containers. | `GOLEM_SIM_ODOM` | off | sim ground-truth `/odom` + `odom→pelvis` TF (SLAM needs it) | | `GOLEM_CAMERAS` | on | `0` drops the 3 RGBD renders — the heaviest per-step cost | | `GOLEM_SPAWN_BACKOFF` | 0 | metres to back the robot into open floor at spawn | +| `GOLEM_SPAWN_MARGIN` | 0.20 | minimum metres between the robot and any non-floor geom at spawn (also on x86) | | `GOLEM_CMD_TIMEOUT` | 0.5 | sim-seconds before the low-level interface zeroes the motors | | `GOLEM_ROS_MCP` | off | `1` starts the [ROS debugging MCP server](ROS_MCP_DEBUG.md) | diff --git a/h1_robocasa/h12_mujoco.py b/h1_robocasa/h12_mujoco.py index 16f71e8..ee62317 100644 --- a/h1_robocasa/h12_mujoco.py +++ b/h1_robocasa/h12_mujoco.py @@ -3,6 +3,7 @@ import os import random import threading +import time import mujoco import numpy as np @@ -54,6 +55,31 @@ def _initial_motor_qpos(): return qpos +# Motor indices (ROS 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): + """Poses the unpowered upper body settles into before the arm controller + first commands it, as (qpos indices, values) pairs for the spawn clearance + check. Measured in RoboCasa: the torso twists up to ~0.7 rad (either way), + shoulders pitch back ~0.25 rad, 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 + + def _frame_viewer_on_robot(handle, data, body_id): """Set the passive viewer free camera's spawn pose as a function of the robot base body's world pose. lookat tracks the body position; the orbit azimuth is @@ -69,6 +95,21 @@ def _frame_viewer_on_robot(handle, data, body_id): cam.elevation = VIEW_CAM_ELEVATION +# Viewer display rate. A viewer thread syncs its own MjData at this rate so the +# ~11 ms handle.sync() (it waits for the render thread) never blocks physics. +VIEWER_HZ = 60.0 + + +def _copy_display_state(dst, src): + """Copy the state the viewer draws from src (caller holds sim_lock).""" + dst.time = src.time + dst.qpos[:] = src.qpos + dst.qvel[:] = src.qvel + dst.act[:] = src.act + dst.mocap_pos[:] = src.mocap_pos + dst.mocap_quat[:] = src.mocap_quat + + def _draw_band(handle, anchor, body_pos): """Draw the elastic band (anchor sphere + capsule to the torso) in the passive viewer's user scene. Mirrors the legacy MujocoEnv.draw_elastic_band.""" @@ -123,8 +164,10 @@ def sim_loop(task, viewer=True, layout=None, style=None, seed=None, timing=False # perturbs the pose during reset, so restore a baked-in bent-knee stance, # zero all velocities, and re-place the pelvis. Upper-body motors remain at # zero. place_robot_collision_free auto-fits floor clearance AND backs the - # base away (the robot's -x) until no robot geom penetrates a fixture, keeping - # the least-penetrating spot if it can't fully clear. + # base away (the robot's -x) until every robot geom is GOLEM_SPAWN_MARGIN + # (default 0.20 m) from any non-floor geom, in the spawn pose and in the + # sagged pose the unpowered upper body falls into, keeping the widest-gap + # spot if it can't reach that. try: init_qpos = _initial_motor_qpos() data.qpos[resolver.motor_qpos] = init_qpos @@ -133,10 +176,15 @@ def sim_loop(task, viewer=True, layout=None, style=None, seed=None, timing=False # floor (default 0 = at the counter for manipulation). Set ~1.0 for nav2, # which needs the robot's start cell to be free to plan. _spawn_backoff = float(os.environ.get("GOLEM_SPAWN_BACKOFF", "0") or 0) + _spawn_margin = float(os.environ.get("GOLEM_SPAWN_MARGIN", "0.20") or 0) h1_2_robosuite.place_robot_collision_free( env, env.init_robot_base_pos, h1_2_robosuite._euler_to_wxyz(getattr(env, "init_robot_base_ori", None)), extra_backoff=_spawn_backoff, + margin=_spawn_margin, + # Motors stay unpowered until the first command, so the margin must + # also hold once the torso and arms sag. + poses=_limp_upper_body_poses(model, resolver.motor_qpos, init_qpos), ) print("[h12_mujoco] initial stance from baked-in sim defaults") except Exception as e: @@ -270,11 +318,18 @@ def sim_loop(task, viewer=True, layout=None, style=None, seed=None, timing=False "RGBD /realsense/{head,left_hand,right_hand}, livox lidar+imu, " "/{left,right}/gripper/*, /elastic_band/toggle") + # The viewer draws its own MjData, refreshed from `data` by the viewer + # thread below; mouse perturbation forces flow back through viewer_perturb. + viewer_data = mujoco.MjData(model) if viewer else None + if viewer_data is not None: + with sim_lock: + _copy_display_state(viewer_data, data) + mujoco.mj_forward(model, viewer_data) # SPACE in the viewer toggles the band (ElasticBand.key_callback). handle = ( - mujoco.viewer.launch_passive(model, data, key_callback=band.key_callback) + mujoco.viewer.launch_passive(model, viewer_data, key_callback=band.key_callback) if viewer and band is not None - else (mujoco.viewer.launch_passive(model, data) if viewer else None) + else (mujoco.viewer.launch_passive(model, viewer_data) if viewer else None) ) if handle is not None: handle.opt.geomgroup[0] = 0 # hide collision geoms by default @@ -290,7 +345,7 @@ def sim_loop(task, viewer=True, layout=None, style=None, seed=None, timing=False if cam_body_id >= 0: try: with handle.lock(): - _frame_viewer_on_robot(handle, data, cam_body_id) + _frame_viewer_on_robot(handle, viewer_data, cam_body_id) except Exception as e: print(f"[h12_mujoco] viewer camera framing skipped: {e}") # Fall logger: watch the torso's uprightness + height and log once when the @@ -299,6 +354,40 @@ def sim_loop(task, viewer=True, layout=None, style=None, seed=None, timing=False _band_release_t = None _band_prev_on = bool(band.enabled) if band is not None else False + # RoboCasa advances fixture state and checks success once per control step + # (env._post_action at control_freq, 20 Hz by default), not per physics step. + # Several fixtures count calls (toaster heat, scrub contact), and the success + # debounce is in control steps, so matching that cadence keeps task semantics. + task_every = max(1, round(env.control_timestep / model.opt.timestep)) + step_count = 0 + viewer_perturb = np.zeros_like(data.xfrc_applied) + viewer_stop = threading.Event() + + def _viewer_loop(): + period = 1.0 / VIEWER_HZ + while not viewer_stop.is_set() and handle.is_running(): + frame_start = time.perf_counter() + viewer_start = timings.start() + with sim_lock: + _copy_display_state(viewer_data, data) + mujoco.mj_forward(model, viewer_data) + # sync() applies the current mouse perturbation to viewer_data. + viewer_data.xfrc_applied[:] = 0.0 + if band is not None and band.enabled: + _draw_band(handle, band.point, viewer_data.xpos[band_body_id]) + else: + handle.user_scn.ngeom = 0 # clear overlay when band off + handle.sync() + with sim_lock: + viewer_perturb[:] = viewer_data.xfrc_applied + timings.stop("viewer", viewer_start) + viewer_stop.wait(max(0.0, period - (time.perf_counter() - frame_start))) + + viewer_thread = None + if handle is not None: + viewer_thread = threading.Thread(target=_viewer_loop, daemon=True, name="viewer_sync") + viewer_thread.start() + timings.reset(float(data.time)) pacer = RealtimePacer(model.opt.timestep) try: @@ -307,6 +396,9 @@ def sim_loop(task, viewer=True, layout=None, style=None, seed=None, timing=False break physics_start = timings.start() with sim_lock: + if viewer_data is not None: + # Mouse perturbation from the viewer's latest sync (zero when idle). + data.xfrc_applied[:] = viewer_perturb if band is not None: # Re-write every step; MuJoCo persists xfrc_applied, so we must # zero it when the band is disabled or the last force lingers. @@ -337,29 +429,28 @@ def sim_loop(task, viewer=True, layout=None, style=None, seed=None, timing=False print(f"[fall-logger] *** ROBOT FELL at sim t={data.time:.2f}s — stood " f"{_since} (uprightness={_upz:.2f}, torso_h={_torso_h:.2f}) ***", flush=True) - task_start = timings.start() - env.update_state() # REQUIRED before _check_success - timings.stop("task_update", task_start) + step_count += 1 + task_step = step_count % task_every == 0 + if task_step: + task_start = timings.start() + env.update_state() # REQUIRED before _check_success + timings.stop("task_update", task_start) # tick owns short sensor capture locks; encoding/publication cannot # block the DDS state reader. No physics step occurs inside tick. ros_bridge.tick() # /clock + camera + lidar + imu - task_start = timings.start() - with sim_lock: - done = measurement.tick() - timings.stop("task_check", task_start) - if done: - print("[h12_mujoco] task success (debounced).") - if viewer: - viewer_start = timings.start() - if band is not None and band.enabled: - _draw_band(handle, band.point, data.xpos[band_body_id]) - else: - handle.user_scn.ngeom = 0 # clear overlay when band off - handle.sync() - timings.stop("viewer", viewer_start) + 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)) finally: + viewer_stop.set() + if viewer_thread is not None: + viewer_thread.join() if handle is not None: handle.close() executor.shutdown() diff --git a/h1_robocasa/h1_2_robosuite.py b/h1_robocasa/h1_2_robosuite.py index e00f539..c8c917f 100644 --- a/h1_robocasa/h1_2_robosuite.py +++ b/h1_robocasa/h1_2_robosuite.py @@ -292,47 +292,123 @@ def _robot_env_contacts(model, data, prefixes=("robot0_", "gripper0_")): return out -def place_robot_collision_free(env, base_pos, base_quat, step=0.02, max_iters=25, - clearance=0.02, extra_backoff=0.0): - """Place the robot collision-free at spawn by backing it away from whatever it - overlaps. +def _robot_env_gap(model, data, margin, prefixes=("robot0_", "gripper0_"), floor_prefix="floor"): + """Smallest distance (m) from any robot geom to any environment geom other + than the floor, capped at `margin`; negative when they overlap. + + Temporarily inflates the robot geoms' contact margin so one collision pass + finds every robot<->environment pair closer than `margin`, then measures + each pair with mj_geomDistance: contact distances from an inflated margin + are unreliable (box-box can report tens of cm of false penetration). Floors (RoboCasa bodies/geoms named floor*) are skipped: the feet + always stand within a few cm of them. Kinematics must be current (as after + place_robot_clear); contacts are recomputed with the model's own margins + before returning.""" + import mujoco - Starting from the RoboCasa placement (base_pos, base_quat wxyz), place the - robot (place_robot_clear auto-fits floor clearance), then while any robot geom - penetrates an environment geom, step the base backward (the robot's -x, away - from the counter it faces) by `step` and retry, up to `max_iters` steps - (default 25 x 2 cm = 0.5 m). Orientation never changes — pure translation. - - Tracks the least-penetrating position seen (fewest contacts, then least total - depth). If still colliding after max_iters, restores that best position and - warns (best-effort), so the sim still launches. Self-collisions at the zero - pose are not addressed here — translation can't fix them. - - extra_backoff: after clearing the spawn collision, keep backing off up to this - many more metres (stopping before a new collision behind) so the robot stands - in OPEN FLOOR with room in front — needed for nav2, which rejects a goal when - the robot's start cell is occupied. Default 0 keeps the robot at the counter - for manipulation reach.""" - back = _backward_dir(base_quat) + bodies = [mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_BODY, b) or "" for b in model.geom_bodyid] + geoms = [mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_GEOM, g) or "" for g in range(model.ngeom)] + robot = np.array([b.startswith(prefixes) for b in bodies]) + floor = np.array([b.startswith(floor_prefix) or g.startswith(floor_prefix) + for b, g in zip(bodies, geoms)]) + saved = model.geom_margin.copy() + saved_flags = model.opt.disableflags + try: + model.geom_margin[robot] = np.maximum(saved[robot], margin) + # The midphase culls geom pairs inside multi-geom bodies with bounding + # volumes that ignore the inflated margin, dropping nearby pairs. + model.opt.disableflags = saved_flags | mujoco.mjtDisableBit.mjDSBL_MIDPHASE + mujoco.mj_collision(model, data) + pairs = set() + for i in range(data.ncon): + g1, g2 = int(data.contact[i].geom1), int(data.contact[i].geom2) + if robot[g1] != robot[g2] and not (floor[g1] or floor[g2]): + pairs.add((g1, g2)) + fromto = np.zeros(6) + gap = min([margin] + [mujoco.mj_geomDistance(model, data, g1, g2, margin, fromto) + for g1, g2 in pairs]) + finally: + model.geom_margin[:] = saved + model.opt.disableflags = saved_flags + mujoco.mj_collision(model, data) + return gap + + +def _overlaps_behind(model, data, pos, back): + """True if the robot overlaps an environment geom behind the base (on the + `back` side of its xy position), i.e. backing off further would push into it.""" + return any(np.dot(np.asarray(c.pos[:2]) - pos[:2], back[:2]) > 0.0 + for c in _robot_env_contacts(model, data)) + + +def _placed_gap(env, pos, base_quat, clearance, margin, poses, back): + """(gap, blocked) at base `pos` over the current joint state and every pose in + `poses` (each a (qpos indices, values) pair). `blocked` means some pose + overlaps a geom behind the base. Leaves the current joint state placed.""" model = env.sim.model._model data = env.sim.data._data + gap, blocked = margin, False + for idx, values in ((None, None), *poses): + if idx is not None: + spawn = data.qpos[idx].copy() + data.qpos[idx] = values + place_robot_clear(env, pos, base_quat, clearance) + pose_gap = _robot_env_gap(model, data, margin) + if pose_gap < 0.0: + blocked = blocked or _overlaps_behind(model, data, pos, back) + gap = min(gap, pose_gap) + if idx is not None: + data.qpos[idx] = spawn + if poses: + place_robot_clear(env, pos, base_quat, clearance) + return gap, blocked + + +def place_robot_collision_free(env, base_pos, base_quat, step=0.02, max_iters=50, + clearance=0.02, extra_backoff=0.0, margin=0.0, poses=()): + """Place the robot at spawn with at least `margin` metres of free space + between every robot geom and every non-floor environment geom. + + Starting from the RoboCasa placement (base_pos, base_quat wxyz), place the + robot (place_robot_clear auto-fits floor clearance), then while the gap to + the environment is below `margin`, step the base backward (the robot's -x, + away from the counter it faces) by `step` and retry, up to `max_iters` steps + (default 50 x 2 cm = 1.0 m). Orientation never changes — pure translation. + margin=0 only removes overlap (1 mm minimum, so nothing starts touching). + + poses: extra joint configurations, each a (qpos indices, values) pair, that + must also keep the margin at the chosen base position (e.g. the pose the + arms settle into before a controller takes over). The robot is left in its + current joint state. + + Tracks the position with the largest gap seen. If `margin` is not reached + after max_iters, or the robot starts overlapping something behind its base, + restores that best position and warns (best-effort), so + the sim still launches and never ends up on the far side of a fixture. Self-collisions + at the zero pose are not addressed here — translation can't fix them. + + extra_backoff: after reaching the margin, keep backing off up to this many + more metres (stopping before the gap behind drops below `margin`) so the + robot stands in OPEN FLOOR with room in front — needed for nav2, which + rejects a goal when the robot's start cell is occupied.""" + back = _backward_dir(base_quat) + margin = max(margin, 1e-3) # geoms that merely touch still generate contacts pos = np.array(base_pos, dtype=float) - best_pos, best_score = None, None + best_pos, best_gap = None, None for i in range(max_iters + 1): - place_robot_clear(env, pos, base_quat, clearance) # writes pose + z-fit + sim.forward() - contacts = _robot_env_contacts(model, data) - score = (len(contacts), sum(-c.dist for c in contacts)) - if best_score is None or score < best_score: - best_score, best_pos = score, pos.copy() - if not contacts: + gap, blocked = _placed_gap(env, pos, base_quat, clearance, margin, poses, back) + if i and blocked: + break # backed into something behind; never search past it + if best_gap is None or gap > best_gap: + best_gap, best_pos = gap, pos.copy() + if gap >= margin: if i: - print(f"[h1_2_robosuite] moved robot back {i * step:.2f} m to clear spawn collision") + print(f"[h1_2_robosuite] moved robot back {i * step:.2f} m for " + f"{margin:.2f} m spawn clearance") if extra_backoff > 0.0: last_clear, moved = pos.copy(), 0.0 for _ in range(int(extra_backoff / step)): pos[:2] += step * back[:2] - place_robot_clear(env, pos, base_quat, clearance) - if _robot_env_contacts(model, data): + if _placed_gap(env, pos, base_quat, clearance, margin, poses, back)[0] < margin: break last_clear, moved = pos.copy(), moved + step place_robot_clear(env, last_clear, base_quat, clearance) @@ -340,10 +416,10 @@ def place_robot_collision_free(env, base_pos, base_quat, step=0.02, max_iters=25 print(f"[h1_2_robosuite] backed robot {moved:.2f} m further into open floor (nav spawn)") return pos[:2] += step * back[:2] - place_robot_clear(env, best_pos, base_quat, clearance) # restore least-penetrating try - print(f"[h1_2_robosuite] WARNING: spawn still in collision after backing off " - f"{max_iters * step:.2f} m; keeping best position " - f"({best_score[0]} contact(s), {best_score[1]:.3f} m penetration)") + place_robot_clear(env, best_pos, base_quat, clearance) # restore the widest-gap try + print(f"[h1_2_robosuite] WARNING: best spawn clearance {best_gap:.3f} m is below the " + f"requested {margin:.2f} m (searched {i * step:.2f} m back); keeping the " + f"widest-gap position") _PATCHED = False diff --git a/h1_robocasa/measurement_bridge.py b/h1_robocasa/measurement_bridge.py index e0a5e07..0a8a2e8 100644 --- a/h1_robocasa/measurement_bridge.py +++ b/h1_robocasa/measurement_bridge.py @@ -1,17 +1,18 @@ """Publish RoboCasa task measurements (goal / success / reward) onto ROS. -The RoboCasa env shares our MjData (we step it ourselves), so after each -``mj_step`` the loop must call ``env.update_state()`` (refreshes fixture caches: -temperature, timer, door, rack) BEFORE reading ``_check_success``. This bridge -wraps that read and publishes: +The RoboCasa env shares our MjData (we step it ourselves), so once per control +step (``env.control_timestep``, as RoboCasa's own ``env.step`` does) the loop +calls ``env.update_state()`` (refreshes fixture caches: temperature, timer, +door, rack) and then ``tick()``, which reads ``_check_success``. This bridge +wraps that read and publishes, at the control rate: /robocasa/task_name std_msgs/String (the task env name, e.g. "TurnOnToasterOven", latched) /robocasa/task_goal std_msgs/String (the language instruction, latched) /robocasa/success std_msgs/Bool (sustained success, after debounce) /robocasa/reward std_msgs/Float32 (sparse instantaneous = float(success)) -Success is debounced over N consecutive successful steps (RoboCasa teleop uses -~15) so a single transient frame does not count as task completion. +Success is debounced over N consecutive successful control steps (RoboCasa +teleop uses ~15) so a single transient frame does not count as task completion. """ from rclpy.node import Node from std_msgs.msg import Bool, Float32, String diff --git a/h1_robocasa/mujoco_ros_bridge.py b/h1_robocasa/mujoco_ros_bridge.py index 89ad43e..2a844f5 100644 --- a/h1_robocasa/mujoco_ros_bridge.py +++ b/h1_robocasa/mujoco_ros_bridge.py @@ -21,9 +21,11 @@ rate-throttled internally. MuJoCo's EGL renderer context is thread-affine, so rendering must happen on the main thread. """ +import copy import io import os import struct +from concurrent.futures import ThreadPoolExecutor from array import array as _array from collections import namedtuple from contextlib import nullcontext @@ -36,9 +38,10 @@ from builtin_interfaces.msg import Time as TimeMsg from PIL import Image as PILImage from geometry_msgs.msg import TransformStamped -from livox_ros_driver2.msg import CustomMsg, CustomPoint +from livox_ros_driver2.msg import CustomMsg from nav_msgs.msg import Odometry from rclpy.node import Node +from rclpy.serialization import serialize_message from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy, qos_profile_sensor_data from rosgraph_msgs.msg import Clock from sensor_msgs.msg import CameraInfo, CompressedImage, Image, Imu, PointCloud2, PointField @@ -88,6 +91,37 @@ def _sim_time_to_msg(sim_time: float) -> TimeMsg: # One render target + its five RealSense-style publishers. All cameras share the # bridge's renderer/resolution/rate; each carries its own MuJoCo camera name, # header frame_id, intrinsics, and /realsense//* publishers. +# livox_ros_driver2/CustomPoint in CDR: uint32 offset_time, float32 x/y/z, then +# uint8 reflectivity/tag/line. The next element's uint32 is 4-byte aligned, so +# consecutive points are 20 bytes apart and only the last point omits the pad. +LIVOX_POINT_DTYPE = np.dtype([ + ("offset_time", " bytes: + """Serialize a CustomMsg without building one Python object per point. + + rclpy serializes the header and fixed fields of a point-free message; the + empty points sequence is its trailing uint32 length, replaced by the count + and followed by the packed points. Identical to serializing the message. + """ + msg = CustomMsg() + msg.header.stamp = stamp + msg.header.frame_id = frame_id + msg.timebase = int(timebase) + msg.point_num = int(points.shape[0]) + msg.lidar_id = 0 + msg.rsvd = [0, 0, 0] + prefix = serialize_message(msg) + packed = np.zeros(msg.point_num, dtype=LIVOX_POINT_DTYPE) + packed["offset_time"] = offset_time + packed["x"], packed["y"], packed["z"] = points[:, 0], points[:, 1], points[:, 2] + packed["line"] = lines + return prefix[:-4] + struct.pack(" None: + if getattr(self, "_lidar_worker", None) is not None: + self._lidar_worker.shutdown(wait=True) + self._lidar_worker = None + camera_worker = getattr(self, "_camera_worker", None) + if camera_worker is not None: + # Free the GL context on the thread that owns it. + camera_worker.submit(self._close_renderer).result() + camera_worker.shutdown(wait=True) + self._camera_worker = None + else: + self._close_renderer() + + def _close_renderer(self) -> None: if self._renderer is not None: try: self._renderer.close() @@ -341,8 +400,9 @@ def tick(self) -> None: /clock is published every call. TF and IMU are published at their target rates keyed off sim time. Camera and lidar are rate-throttled - the same way but run less frequently (they involve rendering / ray - casting on the main thread). Call without holding sim_lock: each sensor + the same way but run less frequently. Camera frames and lidar scans are + produced from MjData snapshots on worker threads, one job in flight per + sensor (cameras stay on this thread under the windowed GLFW backend). Call without holding sim_lock: each sensor protects its own MuJoCo reads, then encodes/publishes outside the lock. Physics must stay on this thread so the state cannot advance mid-tick. """ @@ -443,29 +503,58 @@ def _publish_odom(self, stamp: TimeMsg) -> None: self.pub_odom.publish(odom) def _publish_camera_frame(self, stamp: TimeMsg) -> None: + # Snapshot MjData so scene capture, both render passes and encoding run + # on the camera worker while physics continues (see _camera_worker). + capture_start = self.timings.start() with self.sim_lock or nullcontext(): - if self._renderer is None: - if self.model.vis.global_.offwidth < self.cam_width: - self.model.vis.global_.offwidth = self.cam_width - if self.model.vis.global_.offheight < self.cam_height: - self.model.vis.global_.offheight = self.cam_height - self._renderer = mujoco.Renderer(self.model, height=self.cam_height, width=self.cam_width) - self._rgb_buffer = np.empty((self.cam_height, self.cam_width, 3), dtype=np.uint8) - self._depth_buffer = np.empty((self.cam_height, self.cam_width), dtype=np.float32) - # Group 0: red collision primitives (joint spheres, mesh - # duplicates) — hide. Group 1: H1 body visual meshes — show. - # Group 2: magpie hand visuals + h12 wrist mount — show. - # Group 3: magpie collision meshes + finger pad boxes — hide. - self._scene_opt = mujoco.MjvOption() - self._scene_opt.geomgroup[:] = 0 - self._scene_opt.geomgroup[1] = 1 - self._scene_opt.geomgroup[2] = 1 - # Hide all site markers from camera output. Sites are non-physical - # debug markers (camera/lidar mounts, robosuite's required grip_site - # at the grasp center, etc.); a hand camera looking down the gripper - # axis stares straight at grip_site, whose semi-transparent geom is - # faint in RGB but writes a solid blob into the depth image. - self._scene_opt.sitegroup[:] = 0 + snapshot = copy.copy(self.data) + self.timings.stop("camera_capture", capture_start) + worker = getattr(self, "_camera_worker", None) + if worker is None: + self._render_and_publish_cameras(stamp, snapshot) + return + # At most one frame set in flight: wait rather than drop, so every + # scheduled frame is published. This re-raises the previous 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 = worker.submit(self._render_and_publish_cameras, stamp, snapshot) + + def _ensure_renderer(self) -> None: + if self._renderer is not None: + return + with self.sim_lock or nullcontext(): + if self.model.vis.global_.offwidth < self.cam_width: + self.model.vis.global_.offwidth = self.cam_width + if self.model.vis.global_.offheight < self.cam_height: + self.model.vis.global_.offheight = self.cam_height + self._renderer = mujoco.Renderer(self.model, height=self.cam_height, width=self.cam_width) + self._rgb_buffer = np.empty((self.cam_height, self.cam_width, 3), dtype=np.uint8) + self._depth_buffer = np.empty((self.cam_height, self.cam_width), dtype=np.float32) + # Group 0: red collision primitives (joint spheres, mesh + # duplicates) — hide. Group 1: H1 body visual meshes — show. + # Group 2: magpie hand visuals + h12 wrist mount — show. + # Group 3: magpie collision meshes + finger pad boxes — hide. + self._scene_opt = mujoco.MjvOption() + self._scene_opt.geomgroup[:] = 0 + self._scene_opt.geomgroup[1] = 1 + self._scene_opt.geomgroup[2] = 1 + # Hide all site markers from camera output. Sites are non-physical + # debug markers (camera/lidar mounts, robosuite's required grip_site + # at the grasp center, etc.); a hand camera looking down the gripper + # axis stares straight at grip_site, whose semi-transparent geom is + # faint in RGB but writes a solid blob into the depth image. + self._scene_opt.sitegroup[:] = 0 + + def _render_and_publish_cameras(self, stamp: TimeMsg, data) -> None: + """Render and publish every camera from an MjData snapshot. + + The renderer's GL context is created, used and freed on the calling + thread only (the camera worker when there is one). + """ + self._ensure_renderer() r = self._renderer # Render every camera through the shared renderer (one render context, @@ -474,10 +563,8 @@ def _publish_camera_frame(self, stamp: TimeMsg) -> None: for cam in self.cameras: render_start = self.timings.start() r.disable_depth_rendering() - with self.sim_lock or nullcontext(): - r.update_scene(self.data, camera=cam.cam_id, scene_option=self._scene_opt) - # Both passes consume the same captured MjvScene. Rendering and - # encoding do not access mutable MjData; keep DDS readers unblocked. + # Both passes consume the same MjvScene, captured from the snapshot. + r.update_scene(data, camera=cam.cam_id, scene_option=self._scene_opt) rgb_u8 = r.render(out=self._rgb_buffer) r.enable_depth_rendering() depth = r.render(out=self._depth_buffer) @@ -549,34 +636,54 @@ def _publish_camera_frame(self, stamp: TimeMsg) -> None: self.timings.stop("camera_publish", publish_start) def _publish_lidar_scan(self, stamp: TimeMsg) -> None: - cast_start = self.timings.start() + # Snapshot MjData (~1 ms for a kitchen) so the ~75 ms raycast can run on + # the lidar worker while physics continues; mj_multiRay releases the GIL. + capture_start = self.timings.start() with self.sim_lock or nullcontext(): - timebase = int(self.data.time * 1e9) - origin = np.ascontiguousarray(self.data.xpos[self.lidar_body_id], dtype=np.float64).reshape(3, 1) - rot = np.array(self.data.xmat[self.lidar_body_id], dtype=np.float64).reshape(3, 3) - - # Rotate local ray directions to world frame. - # rot maps local→world (column vecs); for row-vec array: world = local @ rot.T - world_dirs = np.ascontiguousarray(self.lidar_local_dirs @ rot.T, dtype=np.float64) - - # Single batched cast. bodyexclude=torso skips the torso shell at the - # source: the lidar sits inside the torso visual mesh and otherwise - # every ray would self-hit at ~2 cm, forcing a per-ray retry loop - # (~280 ms/scan). Excluding here gives ~80 ms/scan. - dists = self._lidar_dists - geomids = self._lidar_geomids - dists.fill(-1.0) - geomids.fill(-1) - mujoco.mj_multiRay( - self.model, self.data, - origin, world_dirs.reshape(-1, 1), - self.lidar_geomgroup, - 1, # include static geoms - self.lidar_exclude_body_id, # skip torso geoms - geomids, dists, - self.lidar_rays, - self.lidar_max_range, - ) + snapshot = copy.copy(self.data) + self.timings.stop("lidar_capture", capture_start) + worker = getattr(self, "_lidar_worker", None) + if worker is None: + self._cast_and_publish_lidar(stamp, snapshot) + return + # At most one scan in flight: wait rather than drop, so every scheduled + # scan is published. This re-raises a failure from the previous scan. + wait_start = self.timings.start() + pending, self._lidar_pending = self._lidar_pending, None + if pending is not None: + pending.result() + self.timings.stop("lidar_wait", wait_start) + self._lidar_pending = worker.submit(self._cast_and_publish_lidar, stamp, snapshot) + + def _cast_and_publish_lidar(self, stamp: TimeMsg, data) -> None: + """Cast and publish one scan from an MjData snapshot; never touches self.data.""" + cast_start = self.timings.start() + timebase = int(data.time * 1e9) + origin = np.ascontiguousarray(data.xpos[self.lidar_body_id], dtype=np.float64).reshape(3, 1) + rot = np.array(data.xmat[self.lidar_body_id], dtype=np.float64).reshape(3, 3) + + # Rotate local ray directions to world frame. + # rot maps local→world (column vecs); for row-vec array: world = local @ rot.T + world_dirs = np.ascontiguousarray(self.lidar_local_dirs @ rot.T, dtype=np.float64) + + # Single batched cast. bodyexclude=torso skips the torso shell at the + # source: the lidar sits inside the torso visual mesh and otherwise + # every ray would self-hit at ~2 cm, forcing a per-ray retry loop + # (~280 ms/scan). Excluding here gives ~80 ms/scan. + dists = self._lidar_dists + geomids = self._lidar_geomids + dists.fill(-1.0) + geomids.fill(-1) + mujoco.mj_multiRay( + self.model, data, + origin, world_dirs.reshape(-1, 1), + self.lidar_geomgroup, + 1, # include static geoms + self.lidar_exclude_body_id, # skip torso geoms + geomids, dists, + self.lidar_rays, + self.lidar_max_range, + ) self.timings.stop("lidar_cast", cast_start) publish_start = self.timings.start() hit_dists = dists.ravel() @@ -595,22 +702,8 @@ def _publish_lidar_scan(self, stamp: TimeMsg) -> None: lines = self.lidar_lines[valid] offs = self.lidar_offset_time_ns[valid] - msg = CustomMsg() - msg.header.stamp = stamp - msg.header.frame_id = self.lidar_frame - msg.timebase = timebase - msg.point_num = int(pts.shape[0]) - msg.lidar_id = 0 - msg.rsvd = [0, 0, 0] - msg.points = [ - CustomPoint( - offset_time=int(offs[i]), - x=float(pts[i, 0]), y=float(pts[i, 1]), z=float(pts[i, 2]), - reflectivity=0, tag=0, line=int(lines[i]), - ) - for i in range(msg.point_num) - ] - self.pub_lidar.publish(msg) + self.pub_lidar.publish( + _serialize_livox_scan(stamp, self.lidar_frame, timebase, offs, pts, lines)) # Parallel sensor_msgs/PointCloud2 on /livox/pointcloud — same scan, # same stamp/frame, driver-native field layout. timestamp carries the diff --git a/h1_robocasa/sim_timing.py b/h1_robocasa/sim_timing.py index 74520bf..73e3bf0 100644 --- a/h1_robocasa/sim_timing.py +++ b/h1_robocasa/sim_timing.py @@ -1,5 +1,6 @@ """Wall-clock pacing and optional stage timings for the single-step simulator.""" import math +import threading import time @@ -30,7 +31,10 @@ def wait(self): class LoopTimings: - """Report mean/max stage milliseconds and RTF every few wall seconds.""" + """Report mean/max stage milliseconds and RTF every few wall seconds. + + Sensor worker threads record stages too, so updates and reports are locked. + """ def __init__(self, enabled=False, interval=5.0, *, clock=time.perf_counter): self.enabled = enabled @@ -39,11 +43,13 @@ def __init__(self, enabled=False, interval=5.0, *, clock=time.perf_counter): 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): self.wall_start = self.clock() if self.enabled else 0.0 self.sim_start = sim_time - self.stages.clear() + with self._lock: + self.stages.clear() def start(self): return self.clock() if self.enabled else 0.0 @@ -51,8 +57,9 @@ def start(self): def stop(self, name, start): if self.enabled: elapsed = self.clock() - start - total, count, longest = self.stages.get(name, (0.0, 0, 0.0)) - self.stages[name] = (total + elapsed, count + 1, max(longest, elapsed)) + 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): if not self.enabled: @@ -61,12 +68,13 @@ def maybe_report(self, sim_time): 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 self.stages.items() + for name, (total, count, longest) in stages.items() ) print(f'[sim-timing] RTF={(sim_time - self.sim_start) / wall:.3f} ' f'(stage mean/max) {fields}', flush=True) self.wall_start = now self.sim_start = sim_time - self.stages.clear() diff --git a/h1_robocasa/test_spawn_collision.py b/h1_robocasa/test_spawn_collision.py index 9132157..ff43208 100644 --- a/h1_robocasa/test_spawn_collision.py +++ b/h1_robocasa/test_spawn_collision.py @@ -7,9 +7,12 @@ Run: python3 test_spawn_collision.py """ +import io import math +import re import sys import types +from contextlib import redirect_stdout import mujoco import numpy as np @@ -192,6 +195,97 @@ def test_keeps_best_on_giveup(): assert d.qpos[_free_qadr(m)] < 0.0, "give-up should keep the most-backed-off (least-penetrating) try" +def _gap(m, d, margin): + mujoco.mj_forward(m, d) + return hr._robot_env_gap(m, d, margin) + + +def test_gap_ignores_floor_and_caps_at_margin(): + m, d = _load() + d.qpos[_free_qadr(m)] = -2.0 # far from the counter, hand near nothing + d.qpos[_free_qadr(m) + 2] = 0.12 # hand box 2 cm above the floor plane + assert abs(_gap(m, d, 0.2) - 0.2) < 1e-9, "floor must not count toward spawn clearance" + + +def test_gap_sees_near_pairs_inside_multi_geom_bodies(): + # MuJoCo's midphase would cull the hand<->counter pair once the robot body has + # a second geom; the clearance check must still see it. + two_geoms = SCENE.replace('= 0.2 - 1e-9, "margin spawn must keep 20 cm to the counter" + # Hand front sits 0.3 m ahead of the base; the counter face is at x=0.1. + assert d.qpos[_free_qadr(m)] <= -0.4 + 1e-9 < no_overlap_x + assert d.qpos[_free_qadr(m)] > -0.4 - 0.02 - 1e-9, "must stop at the first spot that meets the margin" + + +def test_margin_zero_keeps_no_overlap_behavior(): + m, d = _load() + env = _FakeEnv(m, d) + hr.place_robot_collision_free(env, np.array([0.0, 0.0, 1.0]), np.array([1.0, 0.0, 0.0, 0.0]), margin=0.0) + assert hr._robot_env_contacts(m, d) == [] + assert -0.22 - 1e-9 <= d.qpos[_free_qadr(m)] < -0.18, "margin 0 only clears the overlap" + + +def test_margin_blocked_behind_keeps_widest_gap(): + blocked = SCENE.replace( + '\n -0.5, "must stay in front of the wall, not search through it" + +ARM_SCENE = SCENE.replace( + '', + '' + '' + '') + + +def test_margin_holds_for_extra_poses_and_keeps_spawn_joints(): + m = mujoco.MjModel.from_xml_string(ARM_SCENE) + d = mujoco.MjData(m) + env = _FakeEnv(m, d) + arm = np.array([int(m.jnt_qposadr[mujoco.mj_name2id(m, mujoco.mjtObj.mjOBJ_JOINT, "robot0_arm_joint")])]) + quat = np.array([1.0, 0.0, 0.0, 0.0]) + hr.place_robot_collision_free(env, np.array([0.0, 0.0, 1.0]), quat, margin=0.2) + upright_x = d.qpos[_free_qadr(m)] + # The forearm swung 90 deg forward reaches 0.4 m ahead of its shoulder. + hr.place_robot_collision_free(env, np.array([0.0, 0.0, 1.0]), quat, margin=0.2, + poses=[(arm, np.array([math.pi / 2]))]) + assert d.qpos[arm[0]] == 0.0, "the robot must be left in its spawn joint state" + assert d.qpos[_free_qadr(m)] < upright_x - 0.05, "the swung pose must need more room" + d.qpos[arm[0]] = math.pi / 2 + assert _gap(m, d, 0.2) >= 0.2 - 1e-9, "margin must hold in the extra pose" + def main(): tests = [v for k, v in sorted(globals().items()) if k.startswith("test_") and callable(v)] failed = 0 diff --git a/tests/docker/test_container_checks.py b/tests/docker/test_container_checks.py new file mode 100644 index 0000000..1c212b3 --- /dev/null +++ b/tests/docker/test_container_checks.py @@ -0,0 +1,20 @@ +"""Container existence checks must not match the identically named images.""" +from pathlib import Path +import re +import unittest + +ROOT = Path(__file__).resolve().parents[2] + + +class ContainerCheckTests(unittest.TestCase): + def test_inspect_by_container_name_is_typed(self): + # golem_ros, golem_sim_robocasa and golem_sim_isaac name both a container + # and an image; untyped `docker inspect` succeeds on the image alone. + for path in sorted((ROOT / 'docker/scripts').glob('*.sh')) + sorted((ROOT / 'docker/mac/scripts').glob('*.sh')): + for line in path.read_text().splitlines(): + if re.search(r'docker inspect\b[^|;]*"\$(?:NAME|name)"', line): + self.assertIn('--type container', line, f'{path.name}: {line.strip()}') + + +if __name__ == '__main__': + unittest.main() diff --git a/tests/robocasa/README.md b/tests/robocasa/README.md index e253bd8..8d55d26 100644 --- a/tests/robocasa/README.md +++ b/tests/robocasa/README.md @@ -33,7 +33,21 @@ point filtering, message construction, and publishing. Physics includes lock acquisition and PD control. Timings are silent unless requested. Compare the same task/layout/style/seed, display mode, subscribers, and hardware. -Discard the first interval for renderer warm-up. These changes keep per-step PD -and task checks, all sensor schedules, message formats, and topic definitions. +Discard the first interval 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`. + +Lidar scans and camera frames are produced from a ~1 ms `MjData` snapshot on +one worker thread per sensor, overlapping physics, with one job in flight per +sensor. A due job waits for the previous one rather than being dropped, so +schedules, stamps, message formats and topics are unchanged. `*_capture` is the +locked snapshot, `*_wait` is time the loop blocked on the previous job. +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` stage), so `handle.sync()` never blocks physics. Mouse perturbation +forces from each sync are applied to the simulation on every step. + The pacer repays at most 100 ms of wall-clock lag without dropping simulation steps. It does not make an overloaded scene run in real time. diff --git a/tests/robocasa/test_sensor_bridge.py b/tests/robocasa/test_sensor_bridge.py index 2dd139b..a875238 100644 --- a/tests/robocasa/test_sensor_bridge.py +++ b/tests/robocasa/test_sensor_bridge.py @@ -8,11 +8,13 @@ import mujoco import numpy as np from PIL import Image as PILImage -from rclpy.serialization import serialize_message +from livox_ros_driver2.msg import CustomMsg, CustomPoint +from rclpy.serialization import deserialize_message, serialize_message from sensor_msgs.msg import PointCloud2 from mujoco_ros_bridge import ( - RosSensorBridge, _CameraPub, _build_camera_info, _sim_time_to_msg, PC2_DTYPE, + RosSensorBridge, _CameraPub, _build_camera_info, _serialize_livox_scan, + _sim_time_to_msg, PC2_DTYPE, ) SCENE = ''' @@ -78,10 +80,12 @@ def test_one_scene_update_per_rgb_depth_pair_and_unlocked_render(self): original_render = mujoco.Renderer.render updates = [] - def update(renderer, *args, **kwargs): - self.assertTrue(b.sim_lock.locked()) + def update(renderer, data, *args, **kwargs): + # Scene capture reads a snapshot, so physics is never blocked. + self.assertFalse(b.sim_lock.locked()) + self.assertIsNot(data, b.data) updates.append(kwargs['camera']) - return original_update(renderer, *args, **kwargs) + return original_update(renderer, data, *args, **kwargs) def render(renderer, *args, **kwargs): self.assertFalse(b.sim_lock.locked()) @@ -123,6 +127,33 @@ def test_images_match_two_scene_update_reference(self): self.assertEqual(rgb.encoding, 'rgb8') self.assertEqual(depth.encoding, '16UC1') + def test_camera_worker_matches_inline_frames_and_owns_renderer(self): + from concurrent.futures import ThreadPoolExecutor + b = self.bridge + stamp = _sim_time_to_msg(b.data.time) + b._publish_camera_frame(stamp) + inline = [bytes(pub.messages[0].data) for pub in (b.cameras[0].pub_rgb_raw, b.cameras[0].pub_depth_raw)] + b._close_renderer() + for pub in b.cameras[0][4:]: + pub.messages.clear() + b._camera_worker, b._camera_pending = ThreadPoolExecutor(max_workers=1), None + threads = [] + original = mujoco.Renderer.render + + def render(renderer, *args, **kwargs): + threads.append(threading.current_thread()) + return original(renderer, *args, **kwargs) + + with patch.object(mujoco.Renderer, 'render', render): + for _ in range(2): + b._publish_camera_frame(stamp) + b.shutdown() + self.assertTrue(threads and all(t is not threading.main_thread() for t in threads)) + self.assertIsNone(b._renderer) + cam = b.cameras[0] + self.assertEqual(len(cam.pub_rgb_raw.messages), 2) + self.assertEqual([bytes(cam.pub_rgb_raw.messages[0].data), bytes(cam.pub_depth_raw.messages[0].data)], inline) + def test_tick_preserves_sim_time_schedule_and_publication_order(self): b = self.bridge events = [] @@ -142,7 +173,7 @@ def test_tick_preserves_sim_time_schedule_and_publication_order(self): b.tick() self.assertEqual([e[0] for e in events], ['clock', 'imu']) - def test_lidar_capture_locked_but_both_messages_unlocked(self): + def _configure_lidar(self): b = self.bridge b.lidar_body_id = 1 b.lidar_exclude_body_id = 1 @@ -158,15 +189,21 @@ def test_lidar_capture_locked_but_both_messages_unlocked(self): b.geom_is_robot = np.zeros(b.model.ngeom, dtype=bool) b.pub_lidar = Recorder(b.sim_lock) b.pub_pc2 = Recorder(b.sim_lock) + + def test_lidar_snapshot_locked_but_cast_and_messages_unlocked(self): + b = self.bridge + self._configure_lidar() original = mujoco.mj_multiRay - def cast(*args): - self.assertTrue(b.sim_lock.locked()) - return original(*args) + def cast(model, data, *args): + # Cast on a snapshot so physics can proceed during the ray cast. + self.assertFalse(b.sim_lock.locked()) + self.assertIsNot(data, b.data) + return original(model, data, *args) with patch.object(mujoco, 'mj_multiRay', cast): b._publish_lidar_scan(_sim_time_to_msg(b.data.time)) - msg = b.pub_lidar.messages[0] + msg = deserialize_message(b.pub_lidar.messages[0], CustomMsg) pc2 = b.pub_pc2.messages[0] self.assertEqual(msg.timebase, 1250000000) self.assertEqual(msg.point_num, 1) @@ -181,5 +218,42 @@ def cast(*args): self.assertEqual(serialize_message(pc2), serialize_message(reference)) + def test_lidar_worker_publishes_every_scan_identically(self): + from concurrent.futures import ThreadPoolExecutor + b = self.bridge + self._configure_lidar() + b._publish_lidar_scan(_sim_time_to_msg(b.data.time)) + inline = list(b.pub_lidar.messages) + b.pub_lidar.messages.clear() + b._lidar_worker, b._lidar_pending = ThreadPoolExecutor(max_workers=1), None + stamps = [_sim_time_to_msg(t) for t in (1.25, 1.35, 1.45)] + for stamp in stamps: + b._publish_lidar_scan(stamp) + b.shutdown() + self.assertEqual(len(b.pub_lidar.messages), len(stamps)) + self.assertEqual(b.pub_lidar.messages[0], inline[0]) + self.assertEqual([deserialize_message(m, CustomMsg).header.stamp for m in b.pub_lidar.messages], stamps) + + def test_packed_livox_scan_matches_rclpy_serialization(self): + rng = np.random.default_rng(7) + stamp = _sim_time_to_msg(3.217) + for count in (0, 1, 2, 20161): + points = rng.normal(size=(count, 3)).astype(np.float32) + offsets = rng.integers(0, 100_000_000, count, dtype=np.uint32) + lines = rng.integers(0, 4, count, dtype=np.uint8) + reference = CustomMsg() + reference.header.stamp = stamp + reference.header.frame_id = 'livox_frame' + reference.timebase = 3217000000 + reference.point_num = count + reference.rsvd = [0, 0, 0] + reference.points = [ + CustomPoint(offset_time=int(offsets[i]), x=float(points[i, 0]), + y=float(points[i, 1]), z=float(points[i, 2]), line=int(lines[i])) + for i in range(count) + ] + packed = _serialize_livox_scan(stamp, 'livox_frame', 3217000000, offsets, points, lines) + self.assertEqual(packed, serialize_message(reference), count) + if __name__ == '__main__': unittest.main() diff --git a/tests/stack/README.md b/tests/stack/README.md index cc6cb91..a32f073 100644 --- a/tests/stack/README.md +++ b/tests/stack/README.md @@ -28,9 +28,8 @@ joint motion, and advancing LowState ticks. Isaac uses a fixed base for this isolated arm test; RoboCasa retains its normal support tether. Locomotion requires 5 simulation seconds of unsupported ALMI balance, -3 seconds of forward `/cmd_vel`, and 3 seconds stopping. The normal stack -switches ALMI → walking policy → ALMI; this does not establish that ALMI itself -walks. Tests check physical height, tilt, displacement, both knees, support +3 seconds of forward `/cmd_vel`, and 3 seconds stopping, with ALMI the active +policy throughout (`auto_switch: False`, as in bringup). Tests check physical height, tilt, displacement, both knees, support release, telemetry continuity, and active policies. No fixed base is allowed. Isaac currently lacks RoboCasa's startup support tether, which may cause a real startup/balance failure. Thresholds are in `check_lowerbody.py`. diff --git a/tests/stack/minimal_stack.launch.py b/tests/stack/minimal_stack.launch.py index e2e54bb..f31f396 100644 --- a/tests/stack/minimal_stack.launch.py +++ b/tests/stack/minimal_stack.launch.py @@ -32,6 +32,7 @@ def generate_launch_description(): package='h12_lowerbody_rl', executable='lowerbody_controller_node', name='lowerbody_controller_node', parameters=[sim_time, { 'active_policy': 'almi', + 'auto_switch': False, 'engage_wait_for_confirm': False, 'imu_offset_roll_deg': 0., 'imu_offset_pitch_deg': 0., 'imu_offset_yaw_deg': 0., }], output='screen')) diff --git a/tests/stack/run_checker.sh b/tests/stack/run_checker.sh index 2fa79fb..fbc2ae4 100755 --- a/tests/stack/run_checker.sh +++ b/tests/stack/run_checker.sh @@ -5,6 +5,6 @@ source /home/code/core_ws/install_stack_tests/setup.bash export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp PYTHONUNBUFFERED=1 case "$TEST_CASE" in arms) exec python3 /home/code/tests/stack/check_frame_task.py --simulator "$SIMULATOR" --output /results/report.json --wall-timeout "$GOLEM_TEST_WALL_TIMEOUT" ;; - locomotion) exec python3 /home/code/tests/stack/check_lowerbody.py --telemetry /telemetry/state.jsonl --output /results/report.json --wall-timeout "$GOLEM_TEST_WALL_TIMEOUT" ;; + locomotion) exec python3 /home/code/tests/stack/check_lowerbody.py --telemetry /telemetry/state.jsonl --output /results/report.json --wall-timeout "$GOLEM_TEST_WALL_TIMEOUT" --expected-walk-policy almi ;; *) exit 2 ;; esac From 189037f9ecb832240a905001c45e3dbd4f76dc47 Mon Sep 17 00:00:00 2001 From: maxconway Date: Wed, 23 Sep 2026 17:28:38 -0600 Subject: [PATCH 008/225] Ignore local Claude Code settings Co-Authored-By: Claude Opus 5.5 (1M context) --- .gitignore | 3 +++ 1 file changed, 3 insertions(+) diff --git a/.gitignore b/.gitignore index 1c134f4..fd84cf6 100644 --- a/.gitignore +++ b/.gitignore @@ -78,3 +78,6 @@ core.* # local scratch space /work/ + +# Claude Code local settings +/.claude/ From c03d76cb6b73f3d302716018695ce46840bcebd3 Mon Sep 17 00:00:00 2001 From: HumanoidDesktop Date: Fri, 2 Oct 2026 14:53:01 -0600 Subject: [PATCH 009/225] Split bringup into building blocks; add h12_slam package h12_slam (new in-tree package) owns the shared nav launch (h1_navigation.launch.py) and the lidar/SLAM/nav configs: MID360_config.json, mid360.yaml, slam_toolbox_h1.yaml, nav2_config*.yaml. h1_bringup top-level launches now only include building blocks: h1_state (jsp + rsp), h1_control ([estop] -> safety -> frame_task, staggered), h1_lowerbody (RL controller + optional MJPC estimator behind start_position_verified), h1_models (model servers + skills). Per-robot lowerbody params move to config/lowerbody_{sim,real}.yaml with identical values. h1_real_controller.launch.py is replaced by h1_state + h1_control. New h1_real_slam.launch.py: drivers + state + navigation. Adds rviz/nav.rviz. Old and new expansions of the sim, real, desktop and safety_grippers bringups produce the same nodes, args, delays and parameter values. In sim, lowerbody:=mjpc now starts no RL node. Commented-out MJPC blocks and the unused use_mjpc_viz arg are removed. After pulling on a symlink-install host, delete build/ and install/ for h1_bringup once to drop dangling links to the moved files. Co-Authored-By: Claude Opus 5.5 (1M context) --- AGENTS.md | 43 +- CLAUDE.md | 43 +- .../h12_skills/skills/frontier_explore.py | 2 +- core_ws/src/h12_slam/LICENSE | 17 + .../config/MID360_config.json | 0 .../config/fastlio_nav2_config.yaml | 0 .../config/mid360.yaml | 0 .../config/nav2_config.yaml | 0 .../config/nav2_config_mac.yaml | 0 .../config/slam_toolbox_h1.yaml | 0 .../config/test_nav2_config.yaml | 0 core_ws/src/h12_slam/h12_slam/__init__.py | 0 .../launch/h1_navigation.launch.py | 5 +- core_ws/src/h12_slam/package.xml | 27 + core_ws/src/h12_slam/resource/h12_slam | 0 core_ws/src/h12_slam/setup.cfg | 4 + core_ws/src/h12_slam/setup.py | 35 + core_ws/src/h12_slam/test/test_copyright.py | 25 + core_ws/src/h12_slam/test/test_flake8.py | 25 + core_ws/src/h12_slam/test/test_pep257.py | 23 + .../src/h1_bringup/config/lowerbody_real.yaml | 19 + .../src/h1_bringup/config/lowerbody_sim.yaml | 12 + .../h1_bringup/launch/h1_control.launch.py | 86 + .../h1_bringup/launch/h1_lowerbody.launch.py | 82 + .../src/h1_bringup/launch/h1_models.launch.py | 105 ++ .../launch/h1_real_controller.launch.py | 75 - .../launch/h1_real_desktop_bringup.launch.py | 248 +-- .../launch/h1_real_drivers.launch.py | 5 +- .../launch/h1_real_robot_bringup.launch.py | 35 +- .../h1_bringup/launch/h1_real_slam.launch.py | 30 + .../launch/h1_safety_grippers.launch.py | 54 +- .../launch/h1_sim_bringup.launch.py | 287 +--- .../launch/h1_sim_bringup_mac.launch.py | 4 +- .../src/h1_bringup/launch/h1_state.launch.py | 49 + core_ws/src/h1_bringup/package.xml | 1 + core_ws/src/h1_bringup/rviz/nav.rviz | 1470 +++++++++++++++++ .../h1_bringup/test/test_real_lidar_config.py | 13 +- docker/mac/scripts/launch_ros_mac.sh | 2 +- docs/RUNNING.md | 18 +- 39 files changed, 2225 insertions(+), 619 deletions(-) create mode 100644 core_ws/src/h12_slam/LICENSE rename core_ws/src/{h1_bringup => h12_slam}/config/MID360_config.json (100%) rename core_ws/src/{h1_bringup => h12_slam}/config/fastlio_nav2_config.yaml (100%) rename core_ws/src/{h1_bringup => h12_slam}/config/mid360.yaml (100%) rename core_ws/src/{h1_bringup => h12_slam}/config/nav2_config.yaml (100%) rename core_ws/src/{h1_bringup => h12_slam}/config/nav2_config_mac.yaml (100%) rename core_ws/src/{h1_bringup => h12_slam}/config/slam_toolbox_h1.yaml (100%) rename core_ws/src/{h1_bringup => h12_slam}/config/test_nav2_config.yaml (100%) create mode 100644 core_ws/src/h12_slam/h12_slam/__init__.py rename core_ws/src/{h1_bringup => h12_slam}/launch/h1_navigation.launch.py (98%) create mode 100644 core_ws/src/h12_slam/package.xml create mode 100644 core_ws/src/h12_slam/resource/h12_slam create mode 100644 core_ws/src/h12_slam/setup.cfg create mode 100644 core_ws/src/h12_slam/setup.py create mode 100644 core_ws/src/h12_slam/test/test_copyright.py create mode 100644 core_ws/src/h12_slam/test/test_flake8.py create mode 100644 core_ws/src/h12_slam/test/test_pep257.py create mode 100644 core_ws/src/h1_bringup/config/lowerbody_real.yaml create mode 100644 core_ws/src/h1_bringup/config/lowerbody_sim.yaml create mode 100644 core_ws/src/h1_bringup/launch/h1_control.launch.py create mode 100644 core_ws/src/h1_bringup/launch/h1_lowerbody.launch.py create mode 100644 core_ws/src/h1_bringup/launch/h1_models.launch.py delete mode 100644 core_ws/src/h1_bringup/launch/h1_real_controller.launch.py create mode 100644 core_ws/src/h1_bringup/launch/h1_real_slam.launch.py create mode 100644 core_ws/src/h1_bringup/launch/h1_state.launch.py create mode 100644 core_ws/src/h1_bringup/rviz/nav.rviz diff --git a/AGENTS.md b/AGENTS.md index a5b0b2f..f5a8754 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -83,16 +83,17 @@ which the `ros` container needs for the vision servers and `h12_skills`). ## 4. `core_ws/src` packages -19 ROS packages in 16 directories. Note two name mismatches: dir `FAST_LIO` → +20 ROS packages in 17 directories. Note two name mismatches: dir `FAST_LIO` → package `fast_lio`; dir `unitree_ros2` is not itself a package (it nests `unitree_api`, `unitree_go`, `unitree_hg`, `unitree_ros2_example`). | Directory | Role | What it does | Key entry points | |---|---|---|---| -| `h1_bringup` | **bringup** | Launch-only package that starts the whole robot (sim or real). See §6. | 7 launch files, `config/*.yaml` | +| `h1_bringup` | **bringup** | Launch-only package that starts the whole robot (sim or real). See §6. | top-level + building-block launch files, `config/*.yaml` | | `h12_ros2_controller` | controller (upper body) | Pinocchio-based arm IK: `/frame_task` + `/dual_arm` action servers, joint state publisher, hand controller | `frame_task_server`, `dual_arm_server`, `joint_state_publisher`, `hand_controller_node` | | `h12_deploy_mjpc` | controller (lower body, MPC) | MJPC balance/locomotion controller (C++ cores from the `mujoco_mpc` fork, run as separate processes) + RW-EKF base estimator | `mjpc_lowerbody_core`, `controller_launcher.py`, `estimator_node.py` | | `h12_lowerbody_rl` | controller (lower body, RL) | TorchScript walking + FAME RMA stand/squat policies; switchable stand↔walk controller; consumes `/lowstate` + `/cmd_vel`, emits 12-joint PD setpoints | `walking_node`, `fame_node`, `lowerbody_controller_node` | +| `h12_slam` | SLAM + navigation | The shared nav launch (FAST-LIO → pointcloud_to_laserscan → slam_toolbox → nav2) and its configs: Livox MID360 driver config (`MID360_config.json`, real robot, read by `h1_real_drivers.launch.py`), FAST-LIO (`mid360.yaml`), slam_toolbox (`slam_toolbox_h1.yaml`), Nav2 (`nav2_config.yaml`; mac bringup: `nav2_config_mac.yaml`) | `launch/h1_navigation.launch.py`, `config/` | | `h12_safety_layer` | safety | Merges upper/lower command channels into `/lowcmd` with limit checks (YAML-configured) | `safety_node` | | `h12_skills` | **skills** | Action servers for the 12 `/skill/*` atomic skills an LLM can call (frontier exploration lives here). See §7. | `skills` (SkillsNode) | | `model_server` | model server | ROS service servers wrapping ML models: Gemini VLM, SAM3 segmentation, GraspGenX grasp generation, YOLO detection | `gemini_server`, `sam_server`, `graspgen_server`, `yolo_server` | @@ -125,25 +126,39 @@ separate git submodules with their own history. Rules of thumb: ## 6. Bringup: how the robot starts -`h1_bringup` is launch-only. Sim vs real is **separate launch files** (there is -no `sim:=true` arg): +`h1_bringup` is launch-only. Sim vs real is **separate top-level launch files** +(there is no `sim:=true` arg); each one only includes the shared building blocks +below and sets `use_sim_time` plus its platform's arguments. -| Launch file | Scenario | +| Top-level launch file | Scenario | Includes | +|---|---|---| +| `h1_sim_bringup.launch.py` | **x86 sim** (`use_sim_time=true`), plus the camera optical-frame static TF and rviz (`use_rviz`) | state, control (sim safety configs, no stagger), lowerbody (`lowerbody_sim.yaml`), models (`use_skills`), navigation (`use_nav`) | +| `h1_sim_bringup_mac.launch.py` | **mac sim** — standalone, includes none of the blocks: state publishers, `frame_task_server`, `safety_node`; lower body/SLAM/nav2 gated by `GOLEM_*` env vars, not launch args | — | +| `h1_real_robot_bringup.launch.py` | **real robot, onboard PC** (native, `ROS_DOMAIN_ID=0`) | real_drivers, navigation, state, control (`use_estop:=true`) | +| `h1_real_desktop_bringup.launch.py` | **real: companion x86 desktop** — hand cameras and rviz. Leg control is interlocked behind `start_position_verified:=true` (default **false**) | models (`use_yolo:=true`), lowerbody (`lowerbody_real.yaml` + MJPC estimator) | +| `h1_safety_grippers.launch.py` | real: actuation-only debug bringup | control (estop + safety, no IK), grippers | +| `h1_real_slam.launch.py` | real: sensors + SLAM/navigation only, on wall time (no estop, safety or IK) | real_drivers, state, navigation | + +| Building block | Starts | |---|---| -| `h1_sim_bringup.launch.py` | **x86 sim** — full stack: nav (via include), robot/joint state publishers, `frame_task_server`, `safety_node`, gemini/sam servers, MJPC estimator + lowerbody controller, graspgen + skills (`use_skills`, default true), rviz (`use_rviz`) | -| `h1_sim_bringup_mac.launch.py` | **mac sim** — trimmed: state publishers, `frame_task_server`, `safety_node`; lower body/SLAM/nav2 gated by `GOLEM_*` env vars, not launch args | -| `h1_real_robot_bringup.launch.py` | **real robot, onboard PC** (native, `ROS_DOMAIN_ID=0`) — aggregates the three below-listed real files | -| `h1_real_drivers.launch.py` | real: Livox MID360, RealSense cams, left+right `gripper_node` | -| `h1_real_controller.launch.py` | real: estop, state publishers, staggered `safety_node` + `frame_task_server` | -| `h1_real_desktop_bringup.launch.py` | real: companion x86 desktop — model servers, skills, MJPC controller. Leg control is interlocked behind `start_position_verified:=true` (default **false**) | -| `h1_navigation.launch.py` | shared nav stack (FAST-LIO → pointcloud_to_laserscan → slam_toolbox → nav2); included by sim and real bringups | +| `h1_state.launch.py` | `joint_state_publisher` + `robot_state_publisher` (`urdf_file` arg) | +| `h1_control.launch.py` | [`estop_node`] → `safety_node` → `frame_task_server`, staggered by `safety_delay` / `frame_task_delay` | +| `h1_lowerbody.launch.py` | RL `lowerbody_controller_node` (`lowerbody:=`, params from `lowerbody_params`) and optional MJPC `estimator_node`, both behind `start_position_verified` | +| `h1_models.launch.py` | gemini + sam servers, [yolo], graspgen + `h12_skills` (`use_skills`) | +| `h1_real_drivers.launch.py` | Livox MID360 (+ `livox_imu_upright`, which rotates `/livox/imu_raw` into the point frame — the stock driver rotates only points), RealSense head camera, `h1_grippers.launch.py` (left+right `gripper_node`) | +| `h12_slam/launch/h1_navigation.launch.py` | shared nav stack (FAST-LIO → pointcloud_to_laserscan → slam_toolbox → nav2) | + +Humble includes are not scoped: an argument passed to one include stays set +for the includes after it, so pass consistent values (all four blocks share +`use_sim_time`). **Wiring a new package into bringup:** add it as `exec_depend` in `h1_bringup/package.xml`, add a `launch_ros.actions.Node(...)` entry to the relevant launch file (put node params in `h1_bringup/config/.yaml` — the top-level YAML key must match the node `name`), and gate optional nodes with -`IfCondition(LaunchConfiguration(...))`. See the `h12_deploy_mjpc` entries in -`h1_sim_bringup.launch.py` as the model. **Mac only:** also add the package to +`IfCondition(LaunchConfiguration(...))`. Add the node to the building block it +belongs to (or a new block included by the top-level files), not to a top-level +file; see `h1_lowerbody.launch.py` as the model. **Mac only:** also add the package to the `PKGS` list in `docker/mac/scripts/launch_ros_mac.sh` or the slim image won't build it. diff --git a/CLAUDE.md b/CLAUDE.md index dbd7227..3546530 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -83,16 +83,17 @@ which the `ros` container needs for the vision servers and `h12_skills`). ## 4. `core_ws/src` packages -19 ROS packages in 16 directories. Note two name mismatches: dir `FAST_LIO` → +20 ROS packages in 17 directories. Note two name mismatches: dir `FAST_LIO` → package `fast_lio`; dir `unitree_ros2` is not itself a package (it nests `unitree_api`, `unitree_go`, `unitree_hg`, `unitree_ros2_example`). | Directory | Role | What it does | Key entry points | |---|---|---|---| -| `h1_bringup` | **bringup** | Launch-only package that starts the whole robot (sim or real). See §6. | 7 launch files, `config/*.yaml` | +| `h1_bringup` | **bringup** | Launch-only package that starts the whole robot (sim or real). See §6. | top-level + building-block launch files, `config/*.yaml` | | `h12_ros2_controller` | controller (upper body) | Pinocchio-based arm IK: `/frame_task` + `/dual_arm` action servers, joint state publisher, hand controller | `frame_task_server`, `dual_arm_server`, `joint_state_publisher`, `hand_controller_node` | | `h12_deploy_mjpc` | controller (lower body, MPC) | MJPC balance/locomotion controller (C++ cores from the `mujoco_mpc` fork, run as separate processes) + RW-EKF base estimator | `mjpc_lowerbody_core`, `controller_launcher.py`, `estimator_node.py` | | `h12_lowerbody_rl` | controller (lower body, RL) | TorchScript walking + FAME RMA stand/squat policies; switchable stand↔walk controller; consumes `/lowstate` + `/cmd_vel`, emits 12-joint PD setpoints | `walking_node`, `fame_node`, `lowerbody_controller_node` | +| `h12_slam` | SLAM + navigation | The shared nav launch (FAST-LIO → pointcloud_to_laserscan → slam_toolbox → nav2) and its configs: Livox MID360 driver config (`MID360_config.json`, real robot, read by `h1_real_drivers.launch.py`), FAST-LIO (`mid360.yaml`), slam_toolbox (`slam_toolbox_h1.yaml`), Nav2 (`nav2_config.yaml`; mac bringup: `nav2_config_mac.yaml`) | `launch/h1_navigation.launch.py`, `config/` | | `h12_safety_layer` | safety | Merges upper/lower command channels into `/lowcmd` with limit checks (YAML-configured) | `safety_node` | | `h12_skills` | **skills** | Action servers for the 12 `/skill/*` atomic skills an LLM can call (frontier exploration lives here). See §7. | `skills` (SkillsNode) | | `model_server` | model server | ROS service servers wrapping ML models: Gemini VLM, SAM3 segmentation, GraspGenX grasp generation, YOLO detection | `gemini_server`, `sam_server`, `graspgen_server`, `yolo_server` | @@ -125,25 +126,39 @@ separate git submodules with their own history. Rules of thumb: ## 6. Bringup: how the robot starts -`h1_bringup` is launch-only. Sim vs real is **separate launch files** (there is -no `sim:=true` arg): +`h1_bringup` is launch-only. Sim vs real is **separate top-level launch files** +(there is no `sim:=true` arg); each one only includes the shared building blocks +below and sets `use_sim_time` plus its platform's arguments. -| Launch file | Scenario | +| Top-level launch file | Scenario | Includes | +|---|---|---| +| `h1_sim_bringup.launch.py` | **x86 sim** (`use_sim_time=true`), plus the camera optical-frame static TF and rviz (`use_rviz`) | state, control (sim safety configs, no stagger), lowerbody (`lowerbody_sim.yaml`), models (`use_skills`), navigation (`use_nav`) | +| `h1_sim_bringup_mac.launch.py` | **mac sim** — standalone, includes none of the blocks: state publishers, `frame_task_server`, `safety_node`; lower body/SLAM/nav2 gated by `GOLEM_*` env vars, not launch args | — | +| `h1_real_robot_bringup.launch.py` | **real robot, onboard PC** (native, `ROS_DOMAIN_ID=0`) | real_drivers, navigation, state, control (`use_estop:=true`) | +| `h1_real_desktop_bringup.launch.py` | **real: companion x86 desktop** — hand cameras and rviz. Leg control is interlocked behind `start_position_verified:=true` (default **false**) | models (`use_yolo:=true`), lowerbody (`lowerbody_real.yaml` + MJPC estimator) | +| `h1_safety_grippers.launch.py` | real: actuation-only debug bringup | control (estop + safety, no IK), grippers | +| `h1_real_slam.launch.py` | real: sensors + SLAM/navigation only, on wall time (no estop, safety or IK) | real_drivers, state, navigation | + +| Building block | Starts | |---|---| -| `h1_sim_bringup.launch.py` | **x86 sim** — full stack: nav (via include), robot/joint state publishers, `frame_task_server`, `safety_node`, gemini/sam servers, MJPC estimator + lowerbody controller, graspgen + skills (`use_skills`, default true), rviz (`use_rviz`) | -| `h1_sim_bringup_mac.launch.py` | **mac sim** — trimmed: state publishers, `frame_task_server`, `safety_node`; lower body/SLAM/nav2 gated by `GOLEM_*` env vars, not launch args | -| `h1_real_robot_bringup.launch.py` | **real robot, onboard PC** (native, `ROS_DOMAIN_ID=0`) — aggregates the three below-listed real files | -| `h1_real_drivers.launch.py` | real: Livox MID360 (+ `livox_imu_upright`, which rotates `/livox/imu_raw` into the point frame — the stock driver rotates only points), RealSense cams, left+right `gripper_node` | -| `h1_real_controller.launch.py` | real: estop, state publishers, staggered `safety_node` + `frame_task_server` | -| `h1_real_desktop_bringup.launch.py` | real: companion x86 desktop — model servers, skills, MJPC controller. Leg control is interlocked behind `start_position_verified:=true` (default **false**) | -| `h1_navigation.launch.py` | shared nav stack (FAST-LIO → pointcloud_to_laserscan → slam_toolbox → nav2); included by sim and real bringups | +| `h1_state.launch.py` | `joint_state_publisher` + `robot_state_publisher` (`urdf_file` arg) | +| `h1_control.launch.py` | [`estop_node`] → `safety_node` → `frame_task_server`, staggered by `safety_delay` / `frame_task_delay` | +| `h1_lowerbody.launch.py` | RL `lowerbody_controller_node` (`lowerbody:=`, params from `lowerbody_params`) and optional MJPC `estimator_node`, both behind `start_position_verified` | +| `h1_models.launch.py` | gemini + sam servers, [yolo], graspgen + `h12_skills` (`use_skills`) | +| `h1_real_drivers.launch.py` | Livox MID360 (+ `livox_imu_upright`, which rotates `/livox/imu_raw` into the point frame — the stock driver rotates only points), RealSense head camera, `h1_grippers.launch.py` (left+right `gripper_node`) | +| `h12_slam/launch/h1_navigation.launch.py` | shared nav stack (FAST-LIO → pointcloud_to_laserscan → slam_toolbox → nav2) | + +Humble includes are not scoped: an argument passed to one include stays set +for the includes after it, so pass consistent values (all four blocks share +`use_sim_time`). **Wiring a new package into bringup:** add it as `exec_depend` in `h1_bringup/package.xml`, add a `launch_ros.actions.Node(...)` entry to the relevant launch file (put node params in `h1_bringup/config/.yaml` — the top-level YAML key must match the node `name`), and gate optional nodes with -`IfCondition(LaunchConfiguration(...))`. See the `h12_deploy_mjpc` entries in -`h1_sim_bringup.launch.py` as the model. **Mac only:** also add the package to +`IfCondition(LaunchConfiguration(...))`. Add the node to the building block it +belongs to (or a new block included by the top-level files), not to a top-level +file; see `h1_lowerbody.launch.py` as the model. **Mac only:** also add the package to the `PKGS` list in `docker/mac/scripts/launch_ros_mac.sh` or the slim image won't build it. diff --git a/core_ws/src/h12_skills/h12_skills/skills/frontier_explore.py b/core_ws/src/h12_skills/h12_skills/skills/frontier_explore.py index 85bddda..cb8d8ac 100644 --- a/core_ws/src/h12_skills/h12_skills/skills/frontier_explore.py +++ b/core_ws/src/h12_skills/h12_skills/skills/frontier_explore.py @@ -77,7 +77,7 @@ _WALL_MARGIN = 0.20 # nav2's controller goal-checker xy tolerance — MUST mirror # controller_server.goal_checker.xy_goal_tolerance in -# h1_bringup/config/nav2_config.yaml. nav2 reports a goal SUCCEEDED the instant +# h12_slam/config/nav2_config.yaml. nav2 reports a goal SUCCEEDED the instant # the base is within this radius of it, commanding no motion. Kept here so the # advance gate below can guarantee every goal we issue sits OUTSIDE it. _NAV_XY_GOAL_TOL = 0.25 diff --git a/core_ws/src/h12_slam/LICENSE b/core_ws/src/h12_slam/LICENSE new file mode 100644 index 0000000..30e8e2e --- /dev/null +++ b/core_ws/src/h12_slam/LICENSE @@ -0,0 +1,17 @@ +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. diff --git a/core_ws/src/h1_bringup/config/MID360_config.json b/core_ws/src/h12_slam/config/MID360_config.json similarity index 100% rename from core_ws/src/h1_bringup/config/MID360_config.json rename to core_ws/src/h12_slam/config/MID360_config.json diff --git a/core_ws/src/h1_bringup/config/fastlio_nav2_config.yaml b/core_ws/src/h12_slam/config/fastlio_nav2_config.yaml similarity index 100% rename from core_ws/src/h1_bringup/config/fastlio_nav2_config.yaml rename to core_ws/src/h12_slam/config/fastlio_nav2_config.yaml diff --git a/core_ws/src/h1_bringup/config/mid360.yaml b/core_ws/src/h12_slam/config/mid360.yaml similarity index 100% rename from core_ws/src/h1_bringup/config/mid360.yaml rename to core_ws/src/h12_slam/config/mid360.yaml diff --git a/core_ws/src/h1_bringup/config/nav2_config.yaml b/core_ws/src/h12_slam/config/nav2_config.yaml similarity index 100% rename from core_ws/src/h1_bringup/config/nav2_config.yaml rename to core_ws/src/h12_slam/config/nav2_config.yaml diff --git a/core_ws/src/h1_bringup/config/nav2_config_mac.yaml b/core_ws/src/h12_slam/config/nav2_config_mac.yaml similarity index 100% rename from core_ws/src/h1_bringup/config/nav2_config_mac.yaml rename to core_ws/src/h12_slam/config/nav2_config_mac.yaml diff --git a/core_ws/src/h1_bringup/config/slam_toolbox_h1.yaml b/core_ws/src/h12_slam/config/slam_toolbox_h1.yaml similarity index 100% rename from core_ws/src/h1_bringup/config/slam_toolbox_h1.yaml rename to core_ws/src/h12_slam/config/slam_toolbox_h1.yaml diff --git a/core_ws/src/h1_bringup/config/test_nav2_config.yaml b/core_ws/src/h12_slam/config/test_nav2_config.yaml similarity index 100% rename from core_ws/src/h1_bringup/config/test_nav2_config.yaml rename to core_ws/src/h12_slam/config/test_nav2_config.yaml diff --git a/core_ws/src/h12_slam/h12_slam/__init__.py b/core_ws/src/h12_slam/h12_slam/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/core_ws/src/h1_bringup/launch/h1_navigation.launch.py b/core_ws/src/h12_slam/launch/h1_navigation.launch.py similarity index 98% rename from core_ws/src/h1_bringup/launch/h1_navigation.launch.py rename to core_ws/src/h12_slam/launch/h1_navigation.launch.py index 325a180..21338b2 100644 --- a/core_ws/src/h1_bringup/launch/h1_navigation.launch.py +++ b/core_ws/src/h12_slam/launch/h1_navigation.launch.py @@ -9,8 +9,7 @@ def generate_launch_description(): - bringup_share = get_package_share_directory('h1_bringup') - config_dir = os.path.join(bringup_share, 'config') + config_dir = os.path.join(get_package_share_directory('h12_slam'), 'config') use_sim_time = LaunchConfiguration('use_sim_time') declare_use_sim_time = DeclareLaunchArgument( @@ -21,7 +20,7 @@ def generate_launch_description(): fast_lio_config_file = LaunchConfiguration('fast_lio_config_file') declare_fast_lio_config_file = DeclareLaunchArgument( 'fast_lio_config_file', default_value='mid360.yaml', - description='Config file for fast_lio (resolved against h1_bringup/config)' + description='Config file for fast_lio (resolved against h12_slam/config)' ) nav2_params_file = LaunchConfiguration('nav2_params_file') diff --git a/core_ws/src/h12_slam/package.xml b/core_ws/src/h12_slam/package.xml new file mode 100644 index 0000000..6eeea53 --- /dev/null +++ b/core_ws/src/h12_slam/package.xml @@ -0,0 +1,27 @@ + + + + h12_slam + 0.0.0 + SLAM and localization for the H1-2. + max + MIT + + launch + launch_ros + fast_lio + tf2_ros + pointcloud_to_laserscan + slam_toolbox + nav2_bringup + nav2_bt_navigator + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/core_ws/src/h12_slam/resource/h12_slam b/core_ws/src/h12_slam/resource/h12_slam new file mode 100644 index 0000000..e69de29 diff --git a/core_ws/src/h12_slam/setup.cfg b/core_ws/src/h12_slam/setup.cfg new file mode 100644 index 0000000..c1dfd03 --- /dev/null +++ b/core_ws/src/h12_slam/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/h12_slam +[install] +install_scripts=$base/lib/h12_slam diff --git a/core_ws/src/h12_slam/setup.py b/core_ws/src/h12_slam/setup.py new file mode 100644 index 0000000..d8efc84 --- /dev/null +++ b/core_ws/src/h12_slam/setup.py @@ -0,0 +1,35 @@ +import os +from glob import glob + +from setuptools import find_packages, setup + +package_name = 'h12_slam' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name, 'launch'), glob('launch/*.py')), + (os.path.join('share', package_name, 'config'), + glob('config/*.yaml') + glob('config/*.json')), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='max', + maintainer_email='maxlconway@gmail.com', + description='SLAM and localization for the H1-2.', + license='MIT', + extras_require={ + 'test': [ + 'pytest', + ], + }, + entry_points={ + 'console_scripts': [ + ], + }, +) diff --git a/core_ws/src/h12_slam/test/test_copyright.py b/core_ws/src/h12_slam/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/core_ws/src/h12_slam/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/core_ws/src/h12_slam/test/test_flake8.py b/core_ws/src/h12_slam/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/core_ws/src/h12_slam/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/core_ws/src/h12_slam/test/test_pep257.py b/core_ws/src/h12_slam/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/core_ws/src/h12_slam/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/core_ws/src/h1_bringup/config/lowerbody_real.yaml b/core_ws/src/h1_bringup/config/lowerbody_real.yaml new file mode 100644 index 0000000..7d3c64e --- /dev/null +++ b/core_ws/src/h1_bringup/config/lowerbody_real.yaml @@ -0,0 +1,19 @@ +# lowerbody_controller_node parameters for the real robot. active_policy and +# auto_switch come from h1_lowerbody.launch.py's launch arguments. The node +# holds at the pre-pose crouch until the operator confirms: +# ros2 service call /lowerbody/confirm_engage std_srvs/srv/Trigger +lowerbody_controller_node: + ros__parameters: + # The elastic band is a sim fixture; on real there is no + # /elastic_band/toggle service, so the release path only costs a 1 s + # blocking wait_for_service inside the engage tick (first policy command + # then computed from 1 s-stale state) plus a redundant second LSTM reset. + # The operator lowers the gantry instead. + disable_elastic_band: false + # Startup seed for the live IMU trim (adjust during a run with + # rqt_reconfigure or ros2 param set). Negative pitch is an anti-lean bias + # (the policy believes it pitches further forward and fights harder); the + # hanging-plumb static calibration would be +0.264. + imu_offset_roll_deg: 0.50 + imu_offset_pitch_deg: -2.5 + imu_offset_yaw_deg: 0.0 diff --git a/core_ws/src/h1_bringup/config/lowerbody_sim.yaml b/core_ws/src/h1_bringup/config/lowerbody_sim.yaml new file mode 100644 index 0000000..fc2c57b --- /dev/null +++ b/core_ws/src/h1_bringup/config/lowerbody_sim.yaml @@ -0,0 +1,12 @@ +# lowerbody_controller_node parameters for the simulators. active_policy and +# auto_switch come from h1_lowerbody.launch.py's launch arguments. +lowerbody_controller_node: + ros__parameters: + # Sim auto-engages after the pre-pose settles (unattended runs); the + # operator-confirm arming gate is a real-robot step. + engage_wait_for_confirm: false + # The sim IMU has no mounting bias to correct (the live trim sliders still + # work for experiments). + imu_offset_roll_deg: 0.0 + imu_offset_pitch_deg: 0.0 + imu_offset_yaw_deg: 0.0 diff --git a/core_ws/src/h1_bringup/launch/h1_control.launch.py b/core_ws/src/h1_bringup/launch/h1_control.launch.py new file mode 100644 index 0000000..0a3b558 --- /dev/null +++ b/core_ws/src/h1_bringup/launch/h1_control.launch.py @@ -0,0 +1,86 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, TimerAction +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterValue + + +# Upper-body command path: [estop] -> safety_node -> frame_task_server. +# +# IMPORTANT: ROS_DOMAIN_ID must be exported in the launching shell. The safety +# layer uses unitree_sdk2py's DDS ChannelSubscriber (which honours +# $ROS_DOMAIN_ID and falls back to the YAML's network.domain_id only if the env +# is unset), while the rclpy nodes pick up the env directly. If the variable is +# missing, the DDS half and the rclpy half can end up on different domains and +# the safety layer will see no commands. + + +def generate_launch_description(): + args = [ + DeclareLaunchArgument( + 'use_sim_time', default_value='false', + description='Use the simulator /clock if true'), + DeclareLaunchArgument( + 'use_estop', default_value='false', + description='Start the serial e-stop bridge (real robot only)'), + DeclareLaunchArgument( + 'use_frame_task', default_value='true', + description='Start the frame_task_server arm IK action server'), + DeclareLaunchArgument( + 'safety_config', default_value='relax_safety_split.yaml', + description='h12_safety_layer config name or path (resolved against its config/ dir)'), + DeclareLaunchArgument( + 'frame_task_config', default_value='safety_split.yaml', + description='h12_ros2_controller config for frame_task_server'), + # The safety layer latches the estop status it reads at startup, so on + # the real robot it waits for the estop node to open its serial port and + # publish, and frame_task_server waits for the safety layer. + DeclareLaunchArgument( + 'safety_delay', default_value='3.0', + description='Seconds (wall clock) before starting safety_node'), + DeclareLaunchArgument( + 'frame_task_delay', default_value='5.0', + description='Seconds (wall clock) before starting frame_task_server'), + ] + + sim_time_param = {'use_sim_time': ParameterValue( + LaunchConfiguration('use_sim_time'), value_type=bool)} + + return LaunchDescription(args + [ + Node( + package='estop', + executable='estop_node', + name='estop_node', + parameters=[sim_time_param], + output='screen', + condition=IfCondition(LaunchConfiguration('use_estop')), + ), + TimerAction( + period=LaunchConfiguration('safety_delay'), + actions=[ + Node( + package='h12_safety_layer', + executable='safety_node', + name='safety_node', + parameters=[sim_time_param], + arguments=['--config', LaunchConfiguration('safety_config')], + output='screen', + ), + ], + ), + TimerAction( + period=LaunchConfiguration('frame_task_delay'), + actions=[ + Node( + package='h12_ros2_controller', + executable='frame_task_server', + name='frame_task_server', + arguments=['--config', LaunchConfiguration('frame_task_config')], + parameters=[sim_time_param], + output='screen', + condition=IfCondition(LaunchConfiguration('use_frame_task')), + ), + ], + ), + ]) diff --git a/core_ws/src/h1_bringup/launch/h1_lowerbody.launch.py b/core_ws/src/h1_bringup/launch/h1_lowerbody.launch.py new file mode 100644 index 0000000..8aa73b3 --- /dev/null +++ b/core_ws/src/h1_bringup/launch/h1_lowerbody.launch.py @@ -0,0 +1,82 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.conditions import IfCondition +from launch.substitutions import AndSubstitution, LaunchConfiguration, PythonExpression +from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterValue + + +# Lower-body leg control: the h12_lowerbody_rl controller and, optionally, the +# h12_deploy_mjpc RW-EKF base estimator. Only one lower-body controller may feed +# /safety/lowcmd_lower_in; this bringup does not launch the MJPC controller, so +# lowerbody:=mjpc leaves the legs uncontrolled. + + +def generate_launch_description(): + config_dir = os.path.join(get_package_share_directory('h1_bringup'), 'config') + + args = [ + DeclareLaunchArgument( + 'use_sim_time', default_value='false', + description='Use the simulator /clock if true'), + # Interlock: nothing here commands the legs until the operator has + # verified the robot is in a safe start position. The sim passes true. + DeclareLaunchArgument( + 'start_position_verified', default_value='false', + description='Start the leg controller and estimator'), + DeclareLaunchArgument( + 'lowerbody', default_value='almi', + description='RL policy: almi | almi27 | fame | walk; mjpc starts no RL node'), + # false keeps the robot on the lowerbody:= policy for standing AND + # walking; true hands nonzero /cmd_vel to the walk policy and back. + DeclareLaunchArgument('lowerbody_auto_switch', default_value='false'), + DeclareLaunchArgument( + 'lowerbody_params', + default_value=os.path.join(config_dir, 'lowerbody_real.yaml'), + description='Parameter file for lowerbody_controller_node'), + DeclareLaunchArgument( + 'use_estimator', default_value='false', + description='Start the MJPC RW-EKF base estimator'), + DeclareLaunchArgument( + 'estimator_params', + default_value=os.path.join(config_dir, 'mjpc_real.yaml'), + description='Parameter file for the MJPC estimator'), + ] + + sim_time_param = {'use_sim_time': ParameterValue( + LaunchConfiguration('use_sim_time'), value_type=bool)} + + return LaunchDescription(args + [ + Node( + package='h12_lowerbody_rl', + executable='lowerbody_controller_node', + name='lowerbody_controller_node', # MUST match the yaml key + parameters=[sim_time_param, + LaunchConfiguration('lowerbody_params'), + {'active_policy': LaunchConfiguration('lowerbody'), + 'auto_switch': ParameterValue( + LaunchConfiguration('lowerbody_auto_switch'), value_type=bool)}], + output='screen', + condition=IfCondition(AndSubstitution( + LaunchConfiguration('start_position_verified'), + PythonExpression( + ["'", LaunchConfiguration('lowerbody'), "' != 'mjpc'"]), + )), + ), + Node( + package='h12_deploy_mjpc', + executable='estimator_node', + name='h12_deploy_mjpc_estimator', # MUST match the yaml key + parameters=[sim_time_param, LaunchConfiguration('estimator_params')], + # 'log' keeps the node's periodic [est] stdout off the console; it + # still goes to the ROS log files. + output='log', + condition=IfCondition(AndSubstitution( + LaunchConfiguration('start_position_verified'), + LaunchConfiguration('use_estimator'), + )), + ), + ]) diff --git a/core_ws/src/h1_bringup/launch/h1_models.launch.py b/core_ws/src/h1_bringup/launch/h1_models.launch.py new file mode 100644 index 0000000..f3413d8 --- /dev/null +++ b/core_ws/src/h1_bringup/launch/h1_models.launch.py @@ -0,0 +1,105 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterValue + + +# Perception model servers (model_server) and the /skill/* action servers. + + +def generate_launch_description(): + args = [ + DeclareLaunchArgument( + 'use_sim_time', default_value='false', + description='Use the simulator /clock if true'), + DeclareLaunchArgument( + 'use_skills', default_value='true', + description='Start graspgen_server and the h12_skills action servers'), + DeclareLaunchArgument( + 'use_yolo', default_value='false', + description='Start the YOLO-World detection publisher'), + # Per-model debug logging + visualization, shared by the graspgen, + # gemini, sam, yolo and skills nodes. clear_logs wipes each model's dir + # on startup so every run begins fresh. Output lands in each package's + # logs//. + DeclareLaunchArgument('model_logging', default_value='true'), + DeclareLaunchArgument('model_visualization', default_value='true'), + DeclareLaunchArgument('model_clear_logs', default_value='true'), + # h12_skills per-run base-sway telemetry (run_telemetry.py): each grasp / + # pick_place run writes logs/runs/// with a 10 Hz + # telemetry.csv + result.json. + DeclareLaunchArgument('record_runs', default_value='true'), + ] + + sim_time_param = {'use_sim_time': ParameterValue( + LaunchConfiguration('use_sim_time'), value_type=bool)} + model_log_params = { + 'enable_logging': ParameterValue( + LaunchConfiguration('model_logging'), value_type=bool), + 'enable_visualization': ParameterValue( + LaunchConfiguration('model_visualization'), value_type=bool), + 'clear_logs': ParameterValue( + LaunchConfiguration('model_clear_logs'), value_type=bool), + } + skills_params = { + 'record_runs': ParameterValue( + LaunchConfiguration('record_runs'), value_type=bool), + } + + return LaunchDescription(args + [ + Node( + package='model_server', + executable='gemini_server', + name='gemini_server', + parameters=[sim_time_param, model_log_params], + output='screen', + ), + Node( + package='model_server', + executable='sam_server', + name='sam_server', + parameters=[sim_time_param, model_log_params], + output='screen', + ), + + # YOLO-World open-vocabulary detection. Subscribes to the head + both + # hand color cameras (its DEFAULT_IMAGE_TOPICS) and publishes a + # DetectionBundle on /detections at 5 Hz per camera, using + # the fine-tuned battery weights (weights/yolo_world_battery_best.pt). + # The vocabulary is read live from the `queries` param, e.g.: + # ros2 param set /yolo_server queries "['Bolt','Nut','Screw']" + Node( + package='model_server', + executable='yolo_server', + name='yolo_server', + parameters=[sim_time_param, model_log_params], + output='screen', + condition=IfCondition(LaunchConfiguration('use_yolo')), + ), + + # GraspGenX grasp planning loads a heavy GPU model, so it is gated with + # the skills that use it; the grasp skill chains + # gemini -> sam -> graspgen -> frame_task. + Node( + package='model_server', + executable='graspgen_server', + name='graspgen_server', + parameters=[sim_time_param, model_log_params], + output='screen', + condition=IfCondition(LaunchConfiguration('use_skills')), + ), + + # On startup the skills node waits ~10 s each (non-fatal) on the vision + # and graspgen services, the grippers, and frame_task, then idles ready + # for goals. + Node( + package='h12_skills', + executable='skills', + name='h12_skills', + parameters=[sim_time_param, model_log_params, skills_params], + output='screen', + condition=IfCondition(LaunchConfiguration('use_skills')), + ), + ]) diff --git a/core_ws/src/h1_bringup/launch/h1_real_controller.launch.py b/core_ws/src/h1_bringup/launch/h1_real_controller.launch.py deleted file mode 100644 index abd2fea..0000000 --- a/core_ws/src/h1_bringup/launch/h1_real_controller.launch.py +++ /dev/null @@ -1,75 +0,0 @@ -import os - -from launch import LaunchDescription -from launch.actions import TimerAction -from launch_ros.actions import Node - - -# IMPORTANT: ROS_DOMAIN_ID must be exported in the launching shell. The safety -# layer uses unitree_sdk2py's DDS ChannelSubscriber (which honours -# $ROS_DOMAIN_ID and falls back to the YAML's network.domain_id only if the env -# is unset), while the controller-side rclpy nodes pick up the env directly. -# If the variable is missing, the DDS half and the rclpy half can end up on -# different domains and the safety layer will see no commands. -# Absolute path to the checkout on the robot's onboard PC. This URDF is read at -# launch-description build time, so a wrong path aborts the entire controller -# launch -- estop, state publishers, safety layer and frame_task all fail to -# start. GOLEM_ASSETS_DIR overrides it without a code edit. -ASSETS_DIR = os.environ.get('GOLEM_ASSETS_DIR', '/home/unitree/GOLEM/CL_Assets') - - -def generate_launch_description(): - with open(os.path.join(ASSETS_DIR, 'ros_assets', 'h1_2_magpie_ros.urdf'), 'r') as urdf_file: - robot_description = urdf_file.read() - - sim_time_param = {'use_sim_time': False} - - return LaunchDescription([ - Node( - package='estop', - executable='estop_node', - name='estop_node', - parameters=[sim_time_param], - output='screen', - ), - Node( - package='h12_ros2_controller', - executable='joint_state_publisher', - name='joint_state_publisher', - parameters=[sim_time_param], - output='screen', - ), - Node( - package='robot_state_publisher', - executable='robot_state_publisher', - name='robot_state_publisher', - parameters=[{'robot_description': robot_description}, sim_time_param], - output='screen', - ), - TimerAction( - period=3.0, - actions=[ - Node( - package='h12_safety_layer', - executable='safety_node', - name='safety_node', - parameters=[sim_time_param], - arguments=['--config', 'relax_safety_split.yaml'], - output='screen', - ), - ], - ), - TimerAction( - period=5.0, - actions=[ - Node( - package='h12_ros2_controller', - executable='frame_task_server', - name='frame_task_server', - arguments=['--config', 'safety_split.yaml'], - parameters=[sim_time_param], - output='screen', - ), - ], - ), - ]) diff --git a/core_ws/src/h1_bringup/launch/h1_real_desktop_bringup.launch.py b/core_ws/src/h1_bringup/launch/h1_real_desktop_bringup.launch.py index a5cd9b4..45cd21d 100644 --- a/core_ws/src/h1_bringup/launch/h1_real_desktop_bringup.launch.py +++ b/core_ws/src/h1_bringup/launch/h1_real_desktop_bringup.launch.py @@ -2,81 +2,42 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import ( - DeclareLaunchArgument, - IncludeLaunchDescription, - TimerAction, -) -from launch.conditions import IfCondition +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import AndSubstitution, LaunchConfiguration, PythonExpression +from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node -from launch_ros.parameter_descriptions import ParameterValue -# IMPORTANT: ROS_DOMAIN_ID must be exported in the launching shell. The safety -# layer uses unitree_sdk2py's DDS ChannelSubscriber (which honours -# $ROS_DOMAIN_ID and falls back to the YAML's network.domain_id only if the env -# is unset), while walking_node / frame_task_server / mujoco_ros_bridge are -# rclpy nodes that pick up the env directly. If the variable is missing, the -# DDS half and the rclpy half can end up on different domains and the safety -# layer will see no commands. -ASSETS_DIR = '/home/code/CL_Assets' +# Companion-desktop bringup: hand cameras, model servers + skills, lower-body +# control, and rviz. The state publishers, safety layer and frame_task_server +# run on the onboard PC (h1_real_robot_bringup.launch.py). ROS_DOMAIN_ID must +# be exported in the launching shell (see h1_control.launch.py). + + +def _include(package, launch_file, launch_arguments=None): + return IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory(package), 'launch', launch_file)), + launch_arguments=(launch_arguments or {}).items(), + ) def generate_launch_description(): - # Companion-desktop bringup: runs the model_server vision services, the - # /skill/* servers, and the MJPC lower-body controller/estimator, then a - # single rviz with sim.rviz. The state publishers and frame_task_server run - # on the onboard PC (h1_real_controller.launch.py), not here. bringup_share = get_package_share_directory('h1_bringup') default_rviz = os.path.join(bringup_share, 'rviz', 'sim.rviz') - # Real robot runs on wall-clock time (no MuJoCo /clock), so use_sim_time is - # False; all nodes below share this parameter. - sim_time_param = {'use_sim_time': False} - - # Per-model debug logging + visualization toggles, shared by the graspgen, - # gemini, sam, and skills nodes. Both on by default; disable with - # `model_logging:=false model_visualization:=false` at launch. clear_logs (on - # by default) wipes each model's dir on startup so every run begins fresh. - # Output lands in each package's logs// (bind-mounted to host). - model_log_params = { - 'enable_logging': ParameterValue( - LaunchConfiguration('model_logging'), value_type=bool), - 'enable_visualization': ParameterValue( - LaunchConfiguration('model_visualization'), value_type=bool), - 'clear_logs': ParameterValue( - LaunchConfiguration('model_clear_logs'), value_type=bool), - } - - # h12_skills-only: per-run base-sway telemetry (run_telemetry.py). Each - # grasp / pick_place run writes its own logs/runs/// with a - # 10 Hz telemetry.csv + result.json (pelvis-from-odometry sway, mirroring - # the fridge study). On by default; disable with `record_runs:=false`. - skills_params = { - 'record_runs': ParameterValue( - LaunchConfiguration('record_runs'), value_type=bool), - } + # Real robot runs on wall-clock time (no simulator /clock). + wall_time = {'use_sim_time': 'false'} return LaunchDescription([ - - # Engage the FAME lower-body standing policy only once the operator has - # verified the robot is in a safe start position - # (start_position_verified:=true). Defaults to false so a bare launch - # never auto-commands the legs on the real robot. + # Leg control engages only once the operator has verified the robot is + # in a safe start position. Defaults to false so a bare launch never + # auto-commands the legs on the real robot. DeclareLaunchArgument('start_position_verified', default_value='false'), - # Which controller drives the legs once start_position_verified:=true. - # 'almi' (default) = the switchable RL controller pinned to the ALMI - # policy; 'fame' | 'walk' pin the other RL policies; 'mjpc' restores - # the MJPC balance controller and gates the RL node off — only one - # lower-body controller may feed /safety/lowcmd_lower_in. Every leg - # path stays behind the start_position_verified interlock, and the RL - # node additionally holds at the pre-pose crouch until the operator - # confirms: ros2 service call /lowerbody/confirm_engage std_srvs/srv/Trigger + # Which RL policy drives the legs once start_position_verified:=true + # (see h1_lowerbody.launch.py); the RL node additionally holds at the + # pre-pose crouch until the operator confirms: + # ros2 service call /lowerbody/confirm_engage std_srvs/srv/Trigger DeclareLaunchArgument('lowerbody', default_value='almi'), - # auto_switch on the RL controller: false (default) keeps the robot on - # the lowerbody:= policy for standing AND walking (ALMI all the way); - # true hands nonzero /cmd_vel to the walk policy and back. DeclareLaunchArgument('lowerbody_auto_switch', default_value='false'), DeclareLaunchArgument('use_skills', default_value='true'), DeclareLaunchArgument('model_logging', default_value='true'), @@ -87,151 +48,34 @@ def generate_launch_description(): # Both hand/wrist RealSense cameras and their camera->link static TFs. # These live on the companion desktop (the wrists' USB runs here), while # the head camera comes up with the onboard driver bringup - # (h1_real_drivers.launch.py). Equivalent of - # `ros2 launch cl_realsense h12_hand_cameras.launch.py`. - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join( - get_package_share_directory('cl_realsense'), - 'launch', 'h12_hand_cameras.launch.py') - ) - ), - - # vision foundation-model services (gemini + sam, served by model_server) - Node( - package='model_server', - executable='gemini_server', - name='gemini_server', - parameters=[sim_time_param, model_log_params], - output='screen', - ), - Node( - package='model_server', - executable='sam_server', - name='sam_server', - parameters=[sim_time_param, model_log_params], - output='screen', - ), - - # yolo_server: YOLO-World open-vocabulary detection publisher. Subscribes - # to the head + both hand color cameras (its DEFAULT_IMAGE_TOPICS) and - # publishes a DetectionBundle on /detections at 5 Hz per cam, - # using the fine-tuned battery weights (weights/yolo_world_battery_best.pt). - # Detects the battery-workcell classes by default (DEFAULT_QUERIES: Bolt, - # BusBar, InteriorScrew, Nut, OrageCove, Screw, ScrewHole). The vocabulary - # is read live from the `queries` param — retune at runtime, e.g.: - # ros2 param set /yolo_server queries "['Bolt','Nut','Screw']" - Node( - package='model_server', - executable='yolo_server', - name='yolo_server', - parameters=[sim_time_param, model_log_params], - output='screen', - ), - - # graspgen_server + h12_skills: the GraspGenX planning service and the - # /skill/* action servers. The grasp skill chains gemini -> sam -> - # graspgen -> frame_task. graspgen_server loads a heavy GPU model, so - # both are gated on use_skills. The skills node waits ~10s each (non- - # fatal) on gemini/sam/graspgen, the grippers, and frame_task — the - # latter two arrive over DDS from the robot/driver bringup. - Node( - package='model_server', - executable='graspgen_server', - name='graspgen_server', - parameters=[sim_time_param, model_log_params], - output='screen', - condition=IfCondition(LaunchConfiguration('use_skills')), - ), - Node( - package='h12_skills', - executable='skills', - name='h12_skills', - parameters=[sim_time_param, model_log_params, skills_params], - output='screen', - condition=IfCondition(LaunchConfiguration('use_skills')), - ), - - # Lower-body RL controller (almi / fame / walk) — launched only - # when lowerbody:= names an RL policy AND the operator has verified - # the start position; the MJPC controller below is gated off in that - # case. The chosen policy stays active for standing and walking unless - # lowerbody_auto_switch:=true. - Node( - package='h12_lowerbody_rl', - executable='lowerbody_controller_node', - name='lowerbody_controller_node', - parameters=[sim_time_param, - {'active_policy': LaunchConfiguration('lowerbody'), - 'auto_switch': ParameterValue( - LaunchConfiguration('lowerbody_auto_switch'), value_type=bool), - # The elastic band is a SIM fixture; on real there is - # no /elastic_band/toggle service, so the release path - # only costs a 1 s blocking wait_for_service inside the - # engage tick (first policy command then computed from - # 1 s-stale state) plus a redundant second LSTM reset. - # The operator lowers the gantry instead. - 'disable_elastic_band': False, - # Startup seed for the LIVE IMU trim (adjust during a - # run with rqt_reconfigure sliders or ros2 param set). - # Negative pitch = anti-lean bias (policy believes it - # pitches further forward and fights harder); the - # hanging-plumb static calibration would be +0.264. - 'imu_offset_roll_deg': 0.50, - 'imu_offset_pitch_deg': -2.5, - 'imu_offset_yaw_deg': 0.0}], - output='screen', - condition=IfCondition(AndSubstitution( - LaunchConfiguration('start_position_verified'), - PythonExpression( - ["'", LaunchConfiguration('lowerbody'), "' != 'mjpc'"]), - )), - ), - - - Node( - package='h12_deploy_mjpc', - executable='estimator_node', - name='h12_deploy_mjpc_estimator', # MUST match the yaml key - parameters=[ - sim_time_param, - os.path.join(get_package_share_directory('h1_bringup'), - 'config', 'mjpc_real.yaml'), - ], - # 'log' (not 'screen') mutes the node's periodic [est] stdout from the - # console — it still goes to the ROS log files. - output='log', - condition=IfCondition(LaunchConfiguration('start_position_verified')), - - ), - - # # --- MJPC lower-body balance controller (spawns mjpc_lowerbody_core) --- - # Node( - # package='h12_deploy_mjpc', - # executable='mjpc_deploy_lowerbody_controller', - # name='mjpc_deploy_lowerbody_controller', # MUST match the yaml key - # parameters=[ - # sim_time_param, - # os.path.join(get_package_share_directory('h1_bringup'), - # 'config', 'mjpc_real.yaml'), - # ], - # # mjpc resolves task model XMLs relative to MJPC_TASKS_DIR; without it - # # the model is null -> mj_makeData segfault on startup. - # additional_env={'MJPC_TASKS_DIR': '/home/code/mujoco_mpc/build/mjpc/tasks'}, - # output='screen', - # condition=IfCondition(AndSubstitution( - # LaunchConfiguration('start_position_verified'), - # PythonExpression( - # ["'", LaunchConfiguration('lowerbody'), "' == 'mjpc'"]), - # )), - # ), + # (h1_real_drivers.launch.py). + _include('cl_realsense', 'h12_hand_cameras.launch.py'), + + _include('h1_bringup', 'h1_models.launch.py', { + **wall_time, + 'use_yolo': 'true', + 'use_skills': LaunchConfiguration('use_skills'), + 'model_logging': LaunchConfiguration('model_logging'), + 'model_visualization': LaunchConfiguration('model_visualization'), + 'model_clear_logs': LaunchConfiguration('model_clear_logs'), + 'record_runs': LaunchConfiguration('record_runs'), + }), + _include('h1_bringup', 'h1_lowerbody.launch.py', { + **wall_time, + 'start_position_verified': LaunchConfiguration('start_position_verified'), + 'lowerbody': LaunchConfiguration('lowerbody'), + 'lowerbody_auto_switch': LaunchConfiguration('lowerbody_auto_switch'), + 'lowerbody_params': os.path.join(bringup_share, 'config', 'lowerbody_real.yaml'), + 'use_estimator': 'true', + 'estimator_params': os.path.join(bringup_share, 'config', 'mjpc_real.yaml'), + }), Node( package='rviz2', executable='rviz2', name='rviz2_sim', - arguments=['-d',default_rviz], - parameters=[sim_time_param], + arguments=['-d', default_rviz], + parameters=[{'use_sim_time': False}], output='screen', ), diff --git a/core_ws/src/h1_bringup/launch/h1_real_drivers.launch.py b/core_ws/src/h1_bringup/launch/h1_real_drivers.launch.py index c5e6641..a53ce9e 100644 --- a/core_ws/src/h1_bringup/launch/h1_real_drivers.launch.py +++ b/core_ws/src/h1_bringup/launch/h1_real_drivers.launch.py @@ -11,11 +11,12 @@ def generate_launch_description(): # Equivalent of `ros2 launch livox_ros_driver2 msg_MID360_launch.py`, # inlined so the MID360 driver comes up as part of this bringup. Params # mirror msg_MID360_launch.py; user_config_path points at the - # humanoid-validated MID360_config.json stored under h1_bringup/config + # humanoid-validated MID360_config.json stored under h12_slam/config # (Unitree onboard net 192.168.123.164 host / 192.168.123.120 lidar, # lidar mounted inverted -> extrinsic roll 180). bringup_share = get_package_share_directory('h1_bringup') - mid360_config = os.path.join(bringup_share, 'config', 'MID360_config.json') + mid360_config = os.path.join( + get_package_share_directory('h12_slam'), 'config', 'MID360_config.json') livox_mid360_params = [ {'xfer_format': 1}, # 0-Pointcloud2(PointXYZRTL), 1-customized pointcloud format diff --git a/core_ws/src/h1_bringup/launch/h1_real_robot_bringup.launch.py b/core_ws/src/h1_bringup/launch/h1_real_robot_bringup.launch.py index c43f13d..5894c62 100644 --- a/core_ws/src/h1_bringup/launch/h1_real_robot_bringup.launch.py +++ b/core_ws/src/h1_bringup/launch/h1_real_robot_bringup.launch.py @@ -6,30 +6,25 @@ from launch.launch_description_sources import PythonLaunchDescriptionSource -def generate_launch_description(): - bringup_share = get_package_share_directory('h1_bringup') +# Real robot, onboard PC. ROS_DOMAIN_ID must be exported in the launching shell +# (0 on the real robot; see h1_control.launch.py). - nav_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(bringup_share, 'launch', 'h1_navigation.launch.py') - ), - launch_arguments={'use_sim_time': 'false'}.items(), - ) - drivers_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(bringup_share, 'launch', 'h1_real_drivers.launch.py') - ), - launch_arguments={'use_sim_time': 'false'}.items(), - ) - controller_launch = IncludeLaunchDescription( +def _include(package, launch_file, launch_arguments): + return IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(bringup_share, 'launch', 'h1_real_controller.launch.py') - ), + os.path.join(get_package_share_directory(package), 'launch', launch_file)), + launch_arguments=launch_arguments.items(), ) + +def generate_launch_description(): + # Real robot runs on wall-clock time (no simulator /clock). + wall_time = {'use_sim_time': 'false'} + return LaunchDescription([ - drivers_launch, - nav_launch, - controller_launch, + _include('h1_bringup', 'h1_real_drivers.launch.py', wall_time), + _include('h12_slam', 'h1_navigation.launch.py', wall_time), + _include('h1_bringup', 'h1_state.launch.py', wall_time), + _include('h1_bringup', 'h1_control.launch.py', {**wall_time, 'use_estop': 'true'}), ]) diff --git a/core_ws/src/h1_bringup/launch/h1_real_slam.launch.py b/core_ws/src/h1_bringup/launch/h1_real_slam.launch.py new file mode 100644 index 0000000..134dd12 --- /dev/null +++ b/core_ws/src/h1_bringup/launch/h1_real_slam.launch.py @@ -0,0 +1,30 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource + + +# Real robot SLAM + navigation only: sensor drivers, state publishers and the +# h12_slam nav stack; no estop, safety layer or IK. ROS_DOMAIN_ID must be +# exported in the launching shell (0 on the real robot). + + +def _include(package, launch_file, launch_arguments): + return IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory(package), 'launch', launch_file)), + launch_arguments=launch_arguments.items(), + ) + + +def generate_launch_description(): + # Real robot runs on wall-clock time (no simulator /clock). + wall_time = {'use_sim_time': 'false'} + + return LaunchDescription([ + _include('h1_bringup', 'h1_real_drivers.launch.py', wall_time), + _include('h1_bringup', 'h1_state.launch.py', wall_time), + _include('h12_slam', 'h1_navigation.launch.py', wall_time), + ]) diff --git a/core_ws/src/h1_bringup/launch/h1_safety_grippers.launch.py b/core_ws/src/h1_bringup/launch/h1_safety_grippers.launch.py index 0f7a67c..104971e 100644 --- a/core_ws/src/h1_bringup/launch/h1_safety_grippers.launch.py +++ b/core_ws/src/h1_bringup/launch/h1_safety_grippers.launch.py @@ -2,10 +2,9 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, TimerAction +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node # Minimal real-robot actuation bringup: estop + safety layer + both grippers. @@ -13,51 +12,31 @@ # have to be alive for a command to reach a motor safely. Use it when working on # gripper/low-level actuation without paying for the full driver stack; the full # onboard bringup (h1_real_robot_bringup.launch.py) is still the normal path. -# -# IMPORTANT: ROS_DOMAIN_ID must be exported in the launching shell (0 on the real -# robot). The safety layer uses unitree_sdk2py's DDS ChannelSubscriber (which -# honours $ROS_DOMAIN_ID and falls back to the YAML's network.domain_id only if -# the env is unset), while estop's rclpy half picks up the env directly. If the -# variable is missing the two halves can land on different domains and the safety -# layer will see no commands. +# ROS_DOMAIN_ID must be exported in the launching shell (0 on the real robot; +# see h1_control.launch.py). def generate_launch_description(): bringup_share = get_package_share_directory('h1_bringup') - # Same config the full real bringup runs with (h1_real_controller.launch.py); - # overridable so a session can tighten/relax limits without editing launch. + # Same default config as the full real bringup; overridable so a session + # can tighten/relax limits without editing launch. safety_config_arg = DeclareLaunchArgument( 'safety_config', default_value='relax_safety_split.yaml', description='h12_safety_layer config name or path (resolved against its config/ dir)', ) - sim_time_param = {'use_sim_time': False} - - estop = Node( - package='estop', - executable='estop_node', - name='estop_node', - parameters=[sim_time_param], - output='screen', - ) - - # Staggered behind estop: the safety layer latches on the estop status it - # reads at startup, so give the estop node time to open its serial port and - # publish real state first (mirrors h1_real_controller.launch.py). - safety = TimerAction( - period=3.0, - actions=[ - Node( - package='h12_safety_layer', - executable='safety_node', - name='safety_node', - parameters=[sim_time_param], - arguments=['--config', LaunchConfiguration('safety_config')], - output='screen', - ), - ], + control = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(bringup_share, 'launch', 'h1_control.launch.py') + ), + launch_arguments={ + 'use_sim_time': 'false', + 'use_estop': 'true', + 'use_frame_task': 'false', + 'safety_config': LaunchConfiguration('safety_config'), + }.items(), ) grippers = IncludeLaunchDescription( @@ -68,7 +47,6 @@ def generate_launch_description(): return LaunchDescription([ safety_config_arg, - estop, - safety, + control, grippers, ]) diff --git a/core_ws/src/h1_bringup/launch/h1_sim_bringup.launch.py b/core_ws/src/h1_bringup/launch/h1_sim_bringup.launch.py index f13b416..1799c0f 100644 --- a/core_ws/src/h1_bringup/launch/h1_sim_bringup.launch.py +++ b/core_ws/src/h1_bringup/launch/h1_sim_bringup.launch.py @@ -2,72 +2,33 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import ( - DeclareLaunchArgument, - IncludeLaunchDescription, - SetEnvironmentVariable, - TimerAction, -) -from launch.conditions import IfCondition, LaunchConfigurationEquals, LaunchConfigurationNotEquals +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import AndSubstitution, LaunchConfiguration, PythonExpression +from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node -from launch_ros.parameter_descriptions import ParameterValue -# IMPORTANT: ROS_DOMAIN_ID must be exported in the launching shell. The safety -# layer uses unitree_sdk2py's DDS ChannelSubscriber (which honours -# $ROS_DOMAIN_ID and falls back to the YAML's network.domain_id only if the env -# is unset), while walking_node / frame_task_server / mujoco_ros_bridge are -# rclpy nodes that pick up the env directly. If the variable is missing, the -# DDS half and the rclpy half can end up on different domains and the safety -# layer will see no commands. +# x86 sim bringup. ROS_DOMAIN_ID must be exported in the launching shell (see +# h1_control.launch.py). ASSETS_DIR = '/home/code/CL_Assets' +def _include(package, launch_file, launch_arguments, condition=None): + return IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory(package), 'launch', launch_file)), + launch_arguments=launch_arguments.items(), + condition=condition, + ) + + def generate_launch_description(): - # The included h12_ros2_controller/full_launch.py starts its own rviz2. We - # can't patch that upstream package, so this bringup inlines its non-rviz - # nodes (plus the model_server vision services) and runs a single rviz with sim.rviz. bringup_share = get_package_share_directory('h1_bringup') default_rviz = os.path.join(bringup_share, 'rviz', 'sim.rviz') - with open(os.path.join(ASSETS_DIR, 'ros_assets', 'h1_2_magpie_ros.urdf'), 'r') as urdf_file: - robot_description = urdf_file.read() - - # MuJoCo publishes /clock with sim time. All nodes should use it so - # TF lookups and sensor timestamps are coherent with the simulation. - sim_time_param = {'use_sim_time': True} - - # Per-model debug logging + visualization toggles, shared by the graspgen, - # gemini, sam, and skills nodes. Both on by default; disable with - # `model_logging:=false model_visualization:=false` at launch. clear_logs (on - # by default) wipes each model's dir on startup so every run begins fresh. - # Output lands in each package's logs// (bind-mounted, persists on host). - model_log_params = { - 'enable_logging': ParameterValue( - LaunchConfiguration('model_logging'), value_type=bool), - 'enable_visualization': ParameterValue( - LaunchConfiguration('model_visualization'), value_type=bool), - 'clear_logs': ParameterValue( - LaunchConfiguration('model_clear_logs'), value_type=bool), - } - - # h12_skills-only: per-run base-sway telemetry (run_telemetry.py). Each - # grasp / pick_place run writes its own logs/runs/// with a - # 10 Hz telemetry.csv + result.json (pelvis-from-odometry sway, mirroring - # the fridge study). On by default; disable with `record_runs:=false`. - skills_params = { - 'record_runs': ParameterValue( - LaunchConfiguration('record_runs'), value_type=bool), - } - - nav_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(bringup_share, 'launch', 'h1_navigation.launch.py') - ), - launch_arguments={'use_sim_time': 'true'}.items(), - condition=IfCondition(LaunchConfiguration('use_nav')), - ) + # MuJoCo publishes /clock with sim time. All nodes use it so TF lookups and + # sensor timestamps are coherent with the simulation. + sim_time = {'use_sim_time': 'true'} return LaunchDescription([ DeclareLaunchArgument('use_rviz', default_value='true'), @@ -75,30 +36,45 @@ def generate_launch_description(): DeclareLaunchArgument('use_nav', default_value='true'), DeclareLaunchArgument('use_skills', default_value='true'), DeclareLaunchArgument('record_runs', default_value='true'), - # Debug-only: renders the measured robot + a blue ghost of the MJPC plan to an - # mp4 (written on shutdown). Reads rt/lowstate + the estimator + rt/mjpc/plan; - # commands nothing. use_mjpc_viz:=false for a clean bringup. - DeclareLaunchArgument('use_mjpc_viz', default_value='true'), - # Which controller drives the legs. 'almi' (default) = the RL controller - # (h12_lowerbody_rl lowerbody_controller_node) running ALMI for standing - # and walking, following /cmd_vel from nav2. 'fame' / 'walk' pin that - # same RL controller to those policies instead. The RL - # controller launches for any value != 'mjpc'. 'mjpc' launches NO leg - # controller here — the MJPC nodes below are disabled, so the legs are - # left uncontrolled — which is why the sim default is 'almi'. The - # real-robot bringup does default to mjpc, behind the - # start_position_verified interlock. + # Which RL policy drives the legs (see h1_lowerbody.launch.py). The sim + # bringup does not launch the MJPC controller, so lowerbody:=mjpc + # leaves the legs uncontrolled. DeclareLaunchArgument('lowerbody', default_value='almi'), # almi | almi27 | fame | walk - # auto_switch on the RL controller: false (default) keeps the robot on - # the lowerbody:= policy for standing AND walking (ALMI all the way); - # true hands nonzero /cmd_vel to the walk policy and back. DeclareLaunchArgument('lowerbody_auto_switch', default_value='false'), DeclareLaunchArgument('model_logging', default_value='true'), DeclareLaunchArgument('model_visualization', default_value='true'), DeclareLaunchArgument('model_clear_logs', default_value='true'), DeclareLaunchArgument('rviz_config', default_value=default_rviz), - nav_launch, + _include('h1_bringup', 'h1_state.launch.py', { + **sim_time, + 'urdf_file': os.path.join(ASSETS_DIR, 'ros_assets', 'h1_2_magpie_ros.urdf'), + }), + _include('h1_bringup', 'h1_control.launch.py', { + **sim_time, + 'safety_config': 'sim_safety_split.yaml', + 'frame_task_config': 'sim_safety_split.yaml', + # No estop in sim, so nothing to wait for. + 'safety_delay': '0.0', + 'frame_task_delay': '0.0', + }), + _include('h1_bringup', 'h1_lowerbody.launch.py', { + **sim_time, + 'start_position_verified': 'true', + 'lowerbody': LaunchConfiguration('lowerbody'), + 'lowerbody_auto_switch': LaunchConfiguration('lowerbody_auto_switch'), + 'lowerbody_params': os.path.join(bringup_share, 'config', 'lowerbody_sim.yaml'), + }), + _include('h1_bringup', 'h1_models.launch.py', { + **sim_time, + 'use_skills': LaunchConfiguration('use_skills'), + 'model_logging': LaunchConfiguration('model_logging'), + 'model_visualization': LaunchConfiguration('model_visualization'), + 'model_clear_logs': LaunchConfiguration('model_clear_logs'), + 'record_runs': LaunchConfiguration('record_runs'), + }), + _include('h12_slam', 'h1_navigation.launch.py', sim_time, + condition=IfCondition(LaunchConfiguration('use_nav'))), # The MuJoCo bridge back-projects depth into 3D using REP-103 optical # convention (+z = forward, +x = right, +y = down) but stamps the @@ -114,176 +90,16 @@ def generate_launch_description(): arguments=['0', '0', '0', '-1.5707963267948966', '0', '-1.5707963267948966', 'camera_link', 'camera_color_optical_frame'], - parameters=[sim_time_param], - output='screen', - ), - - Node( - package="h12_ros2_controller", - executable='joint_state_publisher', - name='joint_state_publisher', - parameters=[sim_time_param], - output='screen' - ), - - # h12_ros2_controller (from full_launch.py, minus rviz) - Node( - package='robot_state_publisher', - executable='robot_state_publisher', - name='robot_state_publisher', - parameters=[{'robot_description': robot_description}, sim_time_param], - output='screen', - ), - Node( - package='h12_ros2_controller', - executable='frame_task_server', - name='frame_task_server', - arguments=['--config', 'sim_safety_split.yaml'], - parameters=[sim_time_param], - output='screen', - ), - - # vision foundation-model services (gemini + sam, served by model_server) - Node( - package='model_server', - executable='gemini_server', - name='gemini_server', - parameters=[sim_time_param, model_log_params], - output='screen', - ), - Node( - package='model_server', - executable='sam_server', - name='sam_server', - parameters=[sim_time_param, model_log_params], + parameters=[{'use_sim_time': True}], output='screen', ), - Node( - package='h12_safety_layer', - executable='safety_node', - name='safety_node', - parameters=[sim_time_param, ], - arguments=['--config', "sim_safety_split.yaml"], - output='screen', - ), - - # Lower-body RL controller (almi / fame / walk) — launched only when - # lowerbody:= names an RL policy; the MJPC controller below is gated off - # in that case. The chosen policy stays active throughout unless - # lowerbody_auto_switch:=true. - Node( - package='h12_lowerbody_rl', - executable='lowerbody_controller_node', - name='lowerbody_controller_node', - parameters=[sim_time_param, - {'active_policy': LaunchConfiguration('lowerbody'), # almi/almi27/fame/walk - 'auto_switch': ParameterValue( - LaunchConfiguration('lowerbody_auto_switch'), value_type=bool), - # Sim auto-engages after the pre-pose settles (unattended - # runs); the operator-confirm arming gate is a REAL-robot - # step (see h1_real_desktop_bringup.launch.py). - 'engage_wait_for_confirm': False, - # Sim's IMU has no mounting bias to correct (the live - # trim sliders still work for experiments). - 'imu_offset_roll_deg': 0.0, - 'imu_offset_pitch_deg': 0.0, - 'imu_offset_yaw_deg': 0.0}], - output='screen', - ), - - # --- MJPC lower-body base estimator (RW-EKF) -> rt/sportmodestate_est --- - # Node( - # package='h12_deploy_mjpc', - # executable='estimator_node', - # name='h12_deploy_mjpc_estimator', # MUST match the yaml key - # parameters=[ - # sim_time_param, - # os.path.join(get_package_share_directory('h1_bringup'), - # 'config', 'mjpc_sim.yaml'), - # ], - # output='screen', - # ), - - # # --- MJPC lower-body balance controller (spawns mjpc_lowerbody_core) --- - # Node( - # package='h12_deploy_mjpc', - # executable='mjpc_deploy_lowerbody_controller', - # name='mjpc_deploy_lowerbody_controller', # MUST match the yaml key - # parameters=[ - # sim_time_param, - # os.path.join(get_package_share_directory('h1_bringup'), - # 'config', 'mjpc_sim.yaml'), - # ], - # # mjpc resolves task model XMLs relative to MJPC_TASKS_DIR; without it - # # the model is null -> mj_makeData segfault on startup. - # additional_env={'MJPC_TASKS_DIR': '/home/code/mujoco_mpc/build/mjpc/tasks'}, - # output='screen', - # condition=LaunchConfigurationEquals('lowerbody', 'mjpc'), - # ), - - # --- MJPC plan debug visualizer (measured robot + blue ghost of the plan) --- - # Subscribes only: rt/lowstate + rt/sportmodestate_est (the estimator above) + - # rt/mjpc/plan (published by the controller's planner thread). Writes an mp4 on - # shutdown. Nothing in the control path depends on it. - # Node( - # package='h12_deploy_mjpc', - # executable='mjpc_debug_visualizer', - # name='mjpc_debug_visualizer', # MUST match the yaml key - # parameters=[ - # sim_time_param, - # os.path.join(get_package_share_directory('h1_bringup'), - # 'config', 'mjpc_sim.yaml'), - # ], - # # MJPC_TASKS_DIR: the viewer loads the SAME staged task XML as the - # # controller, so the plan's qpos rows line up with its model. - # # MUJOCO_GL=osmesa: offscreen software GL -- needs no GPU, and unlike egl - # # it works under both compose and `docker run --gpus all`. - # additional_env={'MJPC_TASKS_DIR': '/home/code/mujoco_mpc/build/mjpc/tasks', - # 'MUJOCO_GL': 'osmesa'}, - # output='screen', - # # Viz reads rt/mjpc/plan, which only exists when the MJPC - # # controller is the active lower body. - # condition=IfCondition(AndSubstitution( - # LaunchConfiguration('use_mjpc_viz'), - # PythonExpression( - # ["'", LaunchConfiguration('lowerbody'), "' == 'mjpc'"]), - # )), - # ), - - - # graspgen_server: GraspGenX 6-DOF grasp-planning service (`graspgen`). - # Heavy GPU model (checkpoints + magpie gripper description), so it's - # gated with the skills that use it; the grasp skill chains - # gemini -> sam -> graspgen -> frame_task. - Node( - package='model_server', - executable='graspgen_server', - name='graspgen_server', - parameters=[sim_time_param, model_log_params], - output='screen', - condition=IfCondition(LaunchConfiguration('use_skills')), - ), - - # h12_skills: serves the /skill/* atomic-skill actions (open_door, - # grasp, pick_place, ...). On startup it waits ~10s each on the vision - # pipeline + graspgen services, the grippers, and the frame_task - # action server (all started above), then idles ready for goals. - Node( - package='h12_skills', - executable='skills', - name='h12_skills', - parameters=[sim_time_param, model_log_params, skills_params], - output='screen', - condition=IfCondition(LaunchConfiguration('use_skills')), - ), - Node( package='rviz2', executable='rviz2', name='rviz2_sim', arguments=['-d', LaunchConfiguration('rviz_config')], - parameters=[sim_time_param], + parameters=[{'use_sim_time': True}], output='screen', condition=IfCondition(LaunchConfiguration('use_rviz')), ), @@ -311,4 +127,3 @@ def generate_launch_description(): # ], # ), ]) - diff --git a/core_ws/src/h1_bringup/launch/h1_sim_bringup_mac.launch.py b/core_ws/src/h1_bringup/launch/h1_sim_bringup_mac.launch.py index a40c46f..f9e5dea 100644 --- a/core_ws/src/h1_bringup/launch/h1_sim_bringup_mac.launch.py +++ b/core_ws/src/h1_bringup/launch/h1_sim_bringup_mac.launch.py @@ -155,7 +155,7 @@ def generate_launch_description(): executable='async_slam_toolbox_node', name='slam_toolbox', parameters=[ - os.path.join(get_package_share_directory('h1_bringup'), + os.path.join(get_package_share_directory('h12_slam'), 'config', 'slam_toolbox_h1.yaml'), {'odom_frame': 'odom', 'scan_topic': '/converted_scan'}, sim_time_param, @@ -177,7 +177,7 @@ def generate_launch_description(): nav2_launch = os.path.join( get_package_share_directory('nav2_bringup'), 'launch', 'navigation_launch.py') nav2_cfg = os.path.join( - get_package_share_directory('h1_bringup'), 'config', 'nav2_config_mac.yaml') + get_package_share_directory('h12_slam'), 'config', 'nav2_config_mac.yaml') nodes.append(IncludeLaunchDescription( PythonLaunchDescriptionSource(nav2_launch), launch_arguments={'params_file': nav2_cfg, 'use_sim_time': 'true'}.items(), diff --git a/core_ws/src/h1_bringup/launch/h1_state.launch.py b/core_ws/src/h1_bringup/launch/h1_state.launch.py new file mode 100644 index 0000000..4e0aeab --- /dev/null +++ b/core_ws/src/h1_bringup/launch/h1_state.launch.py @@ -0,0 +1,49 @@ +import os + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import Command, LaunchConfiguration +from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterValue + + +# Default URDF: the real robot's onboard PC checkout, or GOLEM_ASSETS_DIR when +# set. Inside the containers the assets are at /home/code/CL_Assets, so the sim +# bringup passes urdf_file explicitly. +ASSETS_DIR = os.environ.get('GOLEM_ASSETS_DIR', '/home/unitree/GOLEM/CL_Assets') + + +def generate_launch_description(): + declare_use_sim_time = DeclareLaunchArgument( + 'use_sim_time', default_value='false', + description='Use the simulator /clock if true' + ) + declare_urdf_file = DeclareLaunchArgument( + 'urdf_file', + default_value=os.path.join(ASSETS_DIR, 'ros_assets', 'h1_2_magpie_ros.urdf'), + description='Absolute path to the robot URDF' + ) + + sim_time_param = {'use_sim_time': ParameterValue( + LaunchConfiguration('use_sim_time'), value_type=bool)} + robot_description = ParameterValue( + Command(['cat ', LaunchConfiguration('urdf_file')]), value_type=str) + + return LaunchDescription([ + declare_use_sim_time, + declare_urdf_file, + Node( + package='h12_ros2_controller', + executable='joint_state_publisher', + name='joint_state_publisher', + parameters=[sim_time_param], + output='screen', + ), + Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + parameters=[{'robot_description': robot_description}, sim_time_param], + output='screen', + ), + ]) diff --git a/core_ws/src/h1_bringup/package.xml b/core_ws/src/h1_bringup/package.xml index 0440f9f..bbeb62e 100644 --- a/core_ws/src/h1_bringup/package.xml +++ b/core_ws/src/h1_bringup/package.xml @@ -23,6 +23,7 @@ python3-opencv python3-numpy python3-scipy + h12_slam fast_lio tf2_ros pointcloud_to_laserscan diff --git a/core_ws/src/h1_bringup/rviz/nav.rviz b/core_ws/src/h1_bringup/rviz/nav.rviz new file mode 100644 index 0000000..c69e1e0 --- /dev/null +++ b/core_ws/src/h1_bringup/rviz/nav.rviz @@ -0,0 +1,1470 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /HeadDepthImage1/Topic1 + - /HeadColorImage1/Topic1 + - /LeftDepthImage1/Topic1 + - /LeftColorImage1/Topic1 + - /RightDepthImage1/Topic1 + - /RightColorImage1/Topic1 + - /PoseArray1/Topic1 + - /GraspGenMarkers1/Topic1 + - /HeadPC21/Topic1 + - /LeftHandPC21/Topic1 + - /RightHandPC21/Topic1 + - /HeadAnnotatedImage1/Topic1 + - /LeftHandAnnotatedImage1/Topic1 + - /RightHandAnnotatedImage1/Topic1 + - /ServoImage1 + - /ServoImage1/Topic1 + Splitter Ratio: 0.5 + Tree Height: 1080 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: CloudRegisteredBody +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: false + body: + Value: false + camera_color_optical_frame: + Value: true + camera_init: + Value: false + camera_link: + Value: true + head_camera_link: + Value: false + imu_link: + Value: false + left_ankle_pitch_link: + Value: false + left_ankle_roll_link: + Value: false + left_elbow_link: + Value: false + left_grasp_frame: + Value: false + left_graspgenx_frame: + Value: false + left_hip_pitch_link: + Value: false + left_hip_roll_link: + Value: false + left_hip_yaw_link: + Value: false + left_knee_link: + Value: false + left_shoulder_pitch_link: + Value: false + left_shoulder_roll_link: + Value: false + left_shoulder_yaw_link: + Value: false + left_wrist_pitch_link: + Value: false + left_wrist_roll_link: + Value: false + left_wrist_yaw_link: + Value: false + lg_base_bot: + Value: false + lg_base_top: + Value: false + lg_left_crank: + Value: false + lg_left_finger: + Value: false + lg_left_rocker: + Value: false + lg_mount: + Value: false + lg_right_crank: + Value: false + lg_right_finger: + Value: false + lg_right_rocker: + Value: false + livox_link: + Value: false + logo_link: + Value: false + map: + Value: false + pelvis: + Value: false + rg_base_bot: + Value: false + rg_base_top: + Value: false + rg_left_crank: + Value: false + rg_left_finger: + Value: false + rg_left_rocker: + Value: false + rg_mount: + Value: false + rg_right_crank: + Value: false + rg_right_finger: + Value: false + rg_right_rocker: + Value: false + right_ankle_pitch_link: + Value: false + right_ankle_roll_link: + Value: false + right_elbow_link: + Value: false + right_grasp_frame: + Value: false + right_graspgenx_frame: + Value: false + right_hip_pitch_link: + Value: false + right_hip_roll_link: + Value: false + right_hip_yaw_link: + Value: false + right_knee_link: + Value: false + right_shoulder_pitch_link: + Value: false + right_shoulder_roll_link: + Value: false + right_shoulder_yaw_link: + Value: false + right_wrist_pitch_link: + Value: false + right_wrist_roll_link: + Value: false + right_wrist_yaw_link: + Value: false + torso_link: + Value: false + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + body: + pelvis: + left_hip_yaw_link: + left_hip_pitch_link: + left_hip_roll_link: + left_knee_link: + left_ankle_pitch_link: + left_ankle_roll_link: + {} + right_hip_yaw_link: + right_hip_pitch_link: + right_hip_roll_link: + right_knee_link: + right_ankle_pitch_link: + right_ankle_roll_link: + {} + torso_link: + head_camera_link: + {} + imu_link: + {} + left_shoulder_pitch_link: + left_shoulder_roll_link: + left_shoulder_yaw_link: + left_elbow_link: + left_wrist_roll_link: + left_wrist_pitch_link: + left_wrist_yaw_link: + left_grasp_frame: + {} + left_graspgenx_frame: + {} + lg_mount: + lg_base_bot: + lg_base_top: + lg_left_crank: + lg_left_finger: + lg_left_rocker: + {} + lg_right_crank: + lg_right_finger: + lg_right_rocker: + {} + livox_link: + {} + logo_link: + {} + right_shoulder_pitch_link: + right_shoulder_roll_link: + right_shoulder_yaw_link: + right_elbow_link: + right_wrist_roll_link: + right_wrist_pitch_link: + right_wrist_yaw_link: + rg_mount: + rg_base_bot: + rg_base_top: + rg_left_crank: + rg_left_finger: + rg_left_rocker: + {} + rg_right_crank: + rg_right_finger: + rg_right_rocker: + {} + right_grasp_frame: + {} + right_graspgenx_frame: + {} + camera_init: + {} + map: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 2.2247314453125 + Min Value: -1.073204755783081 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: AxisColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LivoxPointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /livox/pointcloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: CloudRegistered + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /cloud_registered + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: CloudRegisteredBody + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /cloud_registered_body + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: "" + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: CloudEffected + Position Transformer: "" + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /cloud_effected + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: z + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0.5344902276992798 + Min Color: 0; 0; 0 + Min Intensity: -2.0594944953918457 + Name: LaserMap + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /Laser_map + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: rviz_default_plugins/Image + Enabled: false + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: HeadDepthImage + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /realsense/head/aligned_depth_to_color/image_raw + Value: false + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: HeadColorImage + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /realsense/head/color/image_raw + Value: true + - Class: rviz_default_plugins/Image + Enabled: false + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: LeftDepthImage + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /realsense/left_hand/aligned_depth_to_color/image_raw + Value: false + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: LeftColorImage + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /realsense/left_hand/color/image_raw + Value: true + - Class: rviz_default_plugins/Image + Enabled: false + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: RightDepthImage + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /realsense/right_hand/aligned_depth_to_color/image_raw + Value: false + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: RightColorImage + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /realsense/right_hand/color/image_raw + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + head_camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + imu_link: + Alpha: 1 + Show Axes: false + Show Trail: false + left_ankle_pitch_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_ankle_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_elbow_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_grasp_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + left_graspgenx_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + left_hip_pitch_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_hip_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_hip_yaw_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_knee_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_shoulder_pitch_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_shoulder_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_shoulder_yaw_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_wrist_pitch_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_wrist_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_wrist_yaw_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lg_base_bot: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lg_base_top: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lg_left_crank: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lg_left_finger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lg_left_rocker: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lg_mount: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lg_right_crank: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lg_right_finger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lg_right_rocker: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + livox_link: + Alpha: 1 + Show Axes: false + Show Trail: false + logo_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + pelvis: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rg_base_bot: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rg_base_top: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rg_left_crank: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rg_left_finger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rg_left_rocker: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rg_mount: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rg_right_crank: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rg_right_finger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rg_right_rocker: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_ankle_pitch_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_ankle_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_elbow_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_grasp_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + right_graspgenx_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + right_hip_pitch_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_hip_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_hip_yaw_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_knee_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_shoulder_pitch_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_shoulder_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_shoulder_yaw_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_wrist_pitch_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_wrist_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_wrist_yaw_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + torso_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map_updates + Use Timestamp: false + Value: true + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: false + Enabled: false + Name: GlobalCostMap + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/costmap_updates + Use Timestamp: false + Value: false + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: false + Enabled: false + Name: LocalCostMap + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/costmap + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /local_costmap/costmap_updates + Use Timestamp: false + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/LaserScan + Color: 255; 255; 255 + Color Transformer: "" + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: "" + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /converted_scan + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: GlobalPlanPath + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /plan + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 255; 25; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: PathHistory + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /path + Value: true + - Alpha: 1 + Class: rviz_default_plugins/PointStamped + Color: 204; 41; 204 + Enabled: true + History Length: 1 + Name: LookaheadPoint + Radius: 0.20000000298023224 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /lookahead_point + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 255; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: LookaheadCollisionArc + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /lookahead_collision_arc + Value: true + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: false + Name: PublishedFootprint + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /global_costmap/published_footprint + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Tracked Objects PointCloud + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /tracked_objects/pointcloud + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Tracked Objects Markers + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /tracked_objects/markers + Value: false + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz_default_plugins/Pose + Color: 255; 25; 0 + Enabled: false + Head Length: 0.10000000149011612 + Head Radius: 0.10000000149011612 + Name: L_EE_Pose + Shaft Length: 0.10000000149011612 + Shaft Radius: 0.05000000074505806 + Shape: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /left_ee_pose + Value: false + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz_default_plugins/Pose + Color: 255; 25; 0 + Enabled: false + Head Length: 0.10000000149011612 + Head Radius: 0.10000000149011612 + Name: R_EE_Pose + Shaft Length: 0.10000000149011612 + Shaft Radius: 0.05000000074505806 + Shape: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /right_ee_pose + Value: false + - Alpha: 1 + Arrow Length: 0.10000000149011612 + Axes Length: 0.30000001192092896 + Axes Radius: 0.009999999776482582 + Class: rviz_default_plugins/PoseArray + Color: 0; 0; 255 + Enabled: false + Head Length: 0.07000000029802322 + Head Radius: 0.029999999329447746 + Name: PoseArray + Shaft Length: 0.23000000417232513 + Shaft Radius: 0.009999999776482582 + Shape: Arrow (Flat) + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /frame_targets + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: GraspGenMarkers + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /graspgen_markers + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 0.39901435375213623 + Min Value: 0.2905491590499878 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 85; 85; 255 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: GraspClolud + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /grasp_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 0.25 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 0; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Scene cloud + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.0010000000474974513 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /scene_cloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: HeadPC2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.0024999999441206455 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /realsense/head/depth/color/points + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LeftHandPC2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.004999999888241291 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /realsense/left_hand/depth/color/points + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: RightHandPC2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.004999999888241291 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /realsense/right_hand/depth/color/points + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: rviz_default_plugins/Image + Enabled: false + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: HeadAnnotatedImage + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /realsense/head/color/image_raw/compressed/annotated + Value: false + - Class: rviz_default_plugins/Image + Enabled: false + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: LeftHandAnnotatedImage + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /realsense/left_hand/color/image_raw/compressed/annotated + Value: false + - Class: rviz_default_plugins/Image + Enabled: false + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: RightHandAnnotatedImage + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /realsense/right_hand/color/image_raw/compressed/annotated + Value: false + - Class: rviz_default_plugins/Image + Enabled: false + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: ServoImage + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /skill/grasp/servo_image + Value: false + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: pelvis + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 8.231671333312988 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.6177334189414978 + Y: -0.009289286099374294 + Z: 0.1523815095424652 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.5197973847389221 + Target Frame: + Value: Orbit (rviz_default_plugins) + Yaw: 5.440641403198242 + Saved: ~ +Window Geometry: + Displays: + collapsed: true + HeadAnnotatedImage: + collapsed: false + HeadColorImage: + collapsed: false + HeadDepthImage: + collapsed: false + Height: 1371 + Hide Left Dock: false + Hide Right Dock: true + LeftColorImage: + collapsed: false + LeftDepthImage: + collapsed: false + LeftHandAnnotatedImage: + collapsed: false + QMainWindow State: 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 + RightColorImage: + collapsed: false + RightDepthImage: + collapsed: false + RightHandAnnotatedImage: + collapsed: false + Selection: + collapsed: false + ServoImage: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1687 + X: 66 + Y: 32 diff --git a/core_ws/src/h1_bringup/test/test_real_lidar_config.py b/core_ws/src/h1_bringup/test/test_real_lidar_config.py index 39ee74f..264a324 100644 --- a/core_ws/src/h1_bringup/test/test_real_lidar_config.py +++ b/core_ws/src/h1_bringup/test/test_real_lidar_config.py @@ -15,6 +15,7 @@ PKG = os.path.dirname(os.path.dirname(os.path.abspath(__file__))) CONFIG = os.path.join(PKG, 'config') +SLAM_CONFIG = os.path.join(os.path.dirname(PKG), 'h12_slam', 'config') def _vec(x, y, z): @@ -22,7 +23,7 @@ def _vec(x, y, z): def test_raw_inverted_gravity_reads_up_after_rotation(): - r = load_rotation(os.path.join(CONFIG, 'MID360_config.json')) + r = load_rotation(os.path.join(SLAM_CONFIG, 'MID360_config.json')) # At rest the inverted MID360 IMU reads about (-0.19, -0.01, -0.98) g. acc = _vec(-0.19, -0.01, -0.98) rotate(r, acc) @@ -31,7 +32,7 @@ def test_raw_inverted_gravity_reads_up_after_rotation(): def test_fast_lio_config_expects_upright_imu(): - with open(os.path.join(CONFIG, 'mid360.yaml')) as f: + with open(os.path.join(SLAM_CONFIG, 'mid360.yaml')) as f: params = yaml.safe_load(f)['/**']['ros__parameters'] assert params['common']['imu_topic'] == '/livox/imu' assert params['mapping']['extrinsic_R'] == [1., 0., 0., 0., 1., 0., 0., 0., 1.] @@ -40,15 +41,19 @@ def test_fast_lio_config_expects_upright_imu(): def test_fast_lio_keeps_every_lidar_line(): # line < scan_line is a hard filter: the real MID360 emits lines 0-3 and # both sims 0-5, so anything below 6 silently drops sim points. - with open(os.path.join(CONFIG, 'mid360.yaml')) as f: + with open(os.path.join(SLAM_CONFIG, 'mid360.yaml')) as f: params = yaml.safe_load(f)['/**']['ros__parameters'] assert params['preprocess']['scan_line'] >= 6 def test_one_slam_config_set(): - names = sorted(os.listdir(CONFIG)) + names = sorted(os.listdir(SLAM_CONFIG)) assert [n for n in names if n.startswith('mid360')] == ['mid360.yaml'] assert [n for n in names if n.startswith('slam_toolbox')] == ['slam_toolbox_h1.yaml'] + # Bringup loads lidar/SLAM/nav configs from h12_slam; a copy here would silently drift. + leftovers = [n for n in os.listdir(CONFIG) + if n.lower().startswith(('mid360', 'slam_toolbox', 'nav2'))] + assert leftovers == [] def test_real_drivers_route_imu_through_upright_node(): diff --git a/docker/mac/scripts/launch_ros_mac.sh b/docker/mac/scripts/launch_ros_mac.sh index e4739e2..715a6b8 100755 --- a/docker/mac/scripts/launch_ros_mac.sh +++ b/docker/mac/scripts/launch_ros_mac.sh @@ -123,7 +123,7 @@ cd "$WS" # ros-humble-rosidl-generator-dds-idl and torch, both in the image. The walk # policy is stable only when handed over from a settled FAME stance, which is # what GOLEM_LOWERBODY=switch does (see docs/MACOS.md). -PKGS="custom_ros_messages magpie_msgs h12_ros2_model h12_ros2_controller h12_safety_layer h1_bringup unitree_hg h12_lowerbody_rl" +PKGS="custom_ros_messages magpie_msgs h12_ros2_model h12_ros2_controller h12_safety_layer h12_slam h1_bringup unitree_hg h12_lowerbody_rl" H1_BRINGUP_STUB_DEPS="estop livox_ros_driver2 fast_lio model_server" for _dep in $H1_BRINGUP_STUB_DEPS; do diff --git a/docs/RUNNING.md b/docs/RUNNING.md index 289b13d..7fb342d 100644 --- a/docs/RUNNING.md +++ b/docs/RUNNING.md @@ -90,15 +90,19 @@ is no `sim:=true` argument): |---|---| | `h1_sim_bringup.launch.py` | x86 sim — the full stack | | `h1_sim_bringup_mac.launch.py` | mac sim — trimmed, toggled by `GOLEM_*` env vars | -| `h1_real_robot_bringup.launch.py` | real robot, onboard PC (aggregates the three below) | -| `h1_real_drivers.launch.py` | real: Livox, RealSense, grippers | -| `h1_real_controller.launch.py` | real: estop, state publishers, safety + IK servers | -| `h1_real_desktop_bringup.launch.py` | real: companion desktop — model servers, skills, MJPC | -| `h1_navigation.launch.py` | shared nav stack, included by the sim and real bringups | +| `h1_real_robot_bringup.launch.py` | real robot, onboard PC: drivers, state publishers, estop + safety + IK, nav | +| `h1_real_desktop_bringup.launch.py` | real: companion desktop — hand cameras, model servers, skills, lower body | +| `h1_safety_grippers.launch.py` | real: estop + safety layer + grippers only | +| `h1_real_slam.launch.py` | real: drivers, state publishers and SLAM/nav only (no estop, safety or IK) | + +The top-level files above are assembled from building blocks you can also +launch on their own: `h1_state` (state publishers), `h1_control` (estop, +safety, IK), `h1_lowerbody`, `h1_models` (model servers + skills), +`h1_real_drivers`, `h1_grippers`, and `h1_navigation` (in `h12_slam`). The x86 sim bringup starts the state publishers, the `frame_task_server` IK -solver, `safety_node`, the Gemini/SAM/GraspGen model servers, the MJPC estimator -and lower-body controller, the skills node, nav, and RViz. Common arguments: +solver, `safety_node`, the Gemini/SAM/GraspGen model servers, the RL lower-body +controller, the skills node, nav, and RViz. Common arguments: ```bash ros2 launch h1_bringup h1_sim_bringup.launch.py \ From 4703bb067b6a6a2b1389e3ab5ffb3137d9850f86 Mon Sep 17 00:00:00 2001 From: maxconway Date: Tue, 6 Oct 2026 23:40:10 -0600 Subject: [PATCH 010/225] Add locomotion_game_deploy package for the MARL loco-manipulation policy Runs the two-agent policy from Locomanipulation_game (LocoManip-Marl-Flat-IK-Direct-v0) on the H1-2: a legs agent that walks on /cmd_vel and an arms agent that reaches arm targets through IK. Includes the deploy node, controller, kinematics, the marl_ik policy config and controller tests. The exported policy weights are not included; the README describes how to export them. Co-Authored-By: Claude Opus 5.5 (1M context) Claude-Session: https://claude.ai/code/session_012uRSUqhg6wqFCekhR7SYsj --- core_ws/src/locomotion_game_deploy/README.md | 97 +++++ .../locomotion_game_deploy/__init__.py | 0 .../locomotion_game_deploy/controller.py | 277 +++++++++++++ .../locomotion_game_deploy/kinematics.py | 99 +++++ .../marl_deploy_node.py | 372 ++++++++++++++++++ .../locomotion_game_deploy/networks.py | 38 ++ .../locomotion_game_deploy/rotations.py | 102 +++++ .../src/locomotion_game_deploy/package.xml | 28 ++ .../policies/marl_ik/marl_ik.yaml | 81 ++++ .../resource/locomotion_game_deploy | 0 core_ws/src/locomotion_game_deploy/setup.cfg | 4 + core_ws/src/locomotion_game_deploy/setup.py | 36 ++ .../test/test_controller.py | 301 ++++++++++++++ 13 files changed, 1435 insertions(+) create mode 100644 core_ws/src/locomotion_game_deploy/README.md create mode 100644 core_ws/src/locomotion_game_deploy/locomotion_game_deploy/__init__.py create mode 100644 core_ws/src/locomotion_game_deploy/locomotion_game_deploy/controller.py create mode 100644 core_ws/src/locomotion_game_deploy/locomotion_game_deploy/kinematics.py create mode 100644 core_ws/src/locomotion_game_deploy/locomotion_game_deploy/marl_deploy_node.py create mode 100644 core_ws/src/locomotion_game_deploy/locomotion_game_deploy/networks.py create mode 100644 core_ws/src/locomotion_game_deploy/locomotion_game_deploy/rotations.py create mode 100644 core_ws/src/locomotion_game_deploy/package.xml create mode 100644 core_ws/src/locomotion_game_deploy/policies/marl_ik/marl_ik.yaml create mode 100644 core_ws/src/locomotion_game_deploy/resource/locomotion_game_deploy create mode 100644 core_ws/src/locomotion_game_deploy/setup.cfg create mode 100644 core_ws/src/locomotion_game_deploy/setup.py create mode 100644 core_ws/src/locomotion_game_deploy/test/test_controller.py diff --git a/core_ws/src/locomotion_game_deploy/README.md b/core_ws/src/locomotion_game_deploy/README.md new file mode 100644 index 0000000..83b0920 --- /dev/null +++ b/core_ws/src/locomotion_game_deploy/README.md @@ -0,0 +1,97 @@ +# locomotion_game_deploy (LocoMotionGameDeploy) + +Runs the two-agent MARL policy from Locomanipulation_game +(`LocoManip-Marl-Flat-IK-Direct-v0`, branch `marl-direct`) on the H1-2: a +**legs** agent that walks on `/cmd_vel`, and an **arms** agent that reaches +wrist-pose goals through a damped-least-squares IK step plus a learned +residual. One node, `marl_deploy_node`, owns all 27 joints. It publishes +through the split safety layer and obeys `/safety/heartbeat`. + +Modelled on `h12_lowerbody_rl`'s `lowerbody_controller_node`. The node goes +through the same steps (pre-pose ramp, operator confirm, elastic-band +release) and uses the same heartbeat watchdog. + +## Layout + +| File | What it is | +|---|---| +| `locomotion_game_deploy/controller.py` | ROS-free env step: odometry → mode → observations (legs 96, arms 124) → joint targets | +| `locomotion_game_deploy/kinematics.py` | Pinocchio wrist FK, Jacobian and IK step, in the pelvis frame | +| `locomotion_game_deploy/rotations.py` | numpy versions of the isaaclab.utils.math functions training used | +| `locomotion_game_deploy/networks.py` | TorchScript loader, and the `zero` stand-ins for plumbing tests | +| `locomotion_game_deploy/marl_deploy_node.py` | the ROS node: safety channels, heartbeat, pre-pose, band, operator interface | +| `policies/marl_ik/marl_ik.yaml` | deploy config: joint order, defaults, gains, scales, command ranges (each from the training env) | +| `policies/marl_ik/{legs,arms,estimator}.pt`, `export.yaml` | written by the export, below | +| `test/test_controller.py` | unit tests (rotations vs Isaac Lab, Jacobian, IK, observation layout, odometry) | + +## Exporting a checkpoint + +These commands are run in Locomanipulation_game. Exporting needs only torch, no simulator: + +```bash +python scripts/skrl/export_marl.py \ + --checkpoint logs/skrl/locomanip_marl//checkpoints/agent_.pt \ + --template /core_ws/src/locomotion_game_deploy/policies/marl_ik/marl_ik.yaml \ + --out /core_ws/src/locomotion_game_deploy/policies/marl_ik +``` + +It writes: +- `legs.pt` and `arms.pt`: observation scaler, MLP mean, ±10 clamp. +- `estimator.pt`: input norm and MLP. +- `export.yaml`: the run's navigation rest pose and the source files. The node loads it over `marl_ik.yaml`. + +Then check the deploy controller against the training env, step by step: + +```bash +python scripts/skrl/deploy_parity.py --headless \ + --export /core_ws/src/locomotion_game_deploy/policies/marl_ik \ + --deploy_pkg /core_ws/src/locomotion_game_deploy \ + --estimator logs/skrl/locomanip_marl//estimator/estimator_.pt \ + --log_dir logs/skrl/locomanip_marl/ +``` + +## Running in the sim + +Use a domain other than 0; domain 0 is the real robot. + +```bash +export ROS_DOMAIN_ID=42 +docker/scripts/docker_run.sh robocasa --headless # terminal 1 +docker/scripts/docker_run.sh ros # terminal 2, then: +ros2 launch h1_bringup h1_sim_bringup.launch.py lowerbody:=marl_ik use_skills:=false use_nav:=false +``` + +`lowerbody:=marl_ik` gates `frame_task_server`, `lowerbody_controller_node` +and the skills off. Only one publisher may feed each safety channel. + +`marl_policy_mode:=zero` runs without exported networks: the legs hold the +default pose and the arms follow pure IK. That checks the plumbing only. The +robot falls once the band releases. + +Drive it from a third shell (`docker exec -it golem_ros bash`): + +```bash +ros2 topic pub -r 10 /cmd_vel geometry_msgs/msg/Twist "{linear: {x: 0.3}}" # walk (clipped to vx ±0.7, vy ±0.3, wz ±0.5) +ros2 topic pub --once /marl_deploy_node/arm_goal geometry_msgs/msg/PoseArray \ + "{poses: [{position: {x: 0.30, y: 0.25, z: 0.05}, orientation: {w: 1.0}}, + {position: {x: 0.30, y: -0.25, z: 0.05}, orientation: {w: 1.0}}]}" # left, right; pelvis frame +ros2 service call /marl_deploy_node/end_arm_goal std_srvs/srv/Trigger +ros2 topic echo /marl_deploy_node/state # idle, preposing, waiting_confirm, nav, settle_to_arm, arm_goal, settle_to_nav, fault: ... +ros2 topic echo /marl_deploy_node/wrist_error # m, left and right +``` + +**Arm goals.** +- An arm goal stops the robot, waits 0.75 s, then reaches. +- The goal is a `left_wrist_yaw_link` and `right_wrist_yaw_link` pose in the pelvis frame, taken when it arrives. The learned odometry keeps it fixed in the world from then on. +- `/cmd_vel` is ignored during an arm goal; the policy only ever saw a zero command there. +- Ending a goal settles for 1.5 s, then walking resumes. + +## Real robot + +1. Onboard PC: `ros2 launch h1_bringup h1_real_robot_bringup.launch.py use_frame_task:=false` +2. Desktop: `ros2 launch h1_bringup h1_real_desktop_bringup.launch.py lowerbody:=marl_ik start_position_verified:=true use_skills:=false` +3. The node pre-poses all 27 joints, then holds. Engage it with `ros2 service call /marl_deploy_node/confirm_engage std_srvs/srv/Trigger`. + +**Before the first real run:** +- **IMU frame.** Training read the IMU on `torso_link` (`imu_link`, offset (-0.0445, -0.0189, 0.2776), no rotation). The MuJoCo sim's IMU is in the same place. Confirm that the real `LowState.imu_state` comes from that same IMU. +- **Safety-layer gaps.** The safety layer holds the last command on a silent channel: neither input channel has a staleness watchdog in the sim or real configs (`upper_stale_seconds: 0.0`). If this node latches silent, the safety layer keeps sending its last targets until it is stopped. diff --git a/core_ws/src/locomotion_game_deploy/locomotion_game_deploy/__init__.py b/core_ws/src/locomotion_game_deploy/locomotion_game_deploy/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/core_ws/src/locomotion_game_deploy/locomotion_game_deploy/controller.py b/core_ws/src/locomotion_game_deploy/locomotion_game_deploy/controller.py new file mode 100644 index 0000000..f36775f --- /dev/null +++ b/core_ws/src/locomotion_game_deploy/locomotion_game_deploy/controller.py @@ -0,0 +1,277 @@ +"""The two-agent MARL controller, without ROS: robot state in, 27 joint targets out. + +One step() is one env step of LocoManip-Marl-Flat-IK-Direct-v0, in the env's +order (locomanip_marl_env.py _get_observations, then _pre_physics_step): + + 1. pelvis odometry: estimate how the pelvis moved over the last step from + the estimator's history window and the action applied, and move the arm + command by it (ArmTargetsCommand.apply_pelvis_motion); + 2. commands: advance the settle timer, switch mode; + 3. observations: the legs' (96) and the arms' (124); + 4. actions: clamp to +-clip, legs = scale * a + default, arms = q + IK step + + residual_scale * a, both bounded to the hard limits +- target_margin. + +Modes follow the training command schedule: navigation (velocity command +live, arms holding the rest pose) or an arm goal (velocity zero, one wrist +pose per arm fixed in the world), with a zero-velocity settle at each switch. +The operator's arm goal is a pelvis-frame pose taken at request time; the +odometry estimate keeps it on its world point from then on, through the +settle too. +""" + +from dataclasses import dataclass +from enum import Enum + +import numpy as np + +from locomotion_game_deploy.kinematics import ArmKinematics +from locomotion_game_deploy.rotations import ( + compute_pose_error, + projected_gravity, + quat_from_rotvec, + quat_unique, + subtract_frame_transforms, + wxyz_to_xyzw, +) + +NUM_JOINTS = 27 +NUM_LEGS = 12 +TORSO = 12 +ARM_START = 13 +NUM_ARM_JOINTS = 14 + + +@dataclass +class RobotState: + q: np.ndarray # (27,) rad, policy order + dq: np.ndarray # (27,) rad/s + tau: np.ndarray # (27,) Nm, measured + quat: np.ndarray # (4,) IMU orientation, wxyz + gyro: np.ndarray # (3,) rad/s, IMU frame + acc: np.ndarray # (3,) m/s^2, IMU frame, accelerometer convention (+9.81 up at rest) + + +@dataclass +class JointCommand: + q: np.ndarray # (27,) targets + kp: np.ndarray + kd: np.ndarray + + +class Mode(Enum): + NAV = "nav" + SETTLE_TO_ARM = "settle_to_arm" + ARM = "arm_goal" + SETTLE_TO_NAV = "settle_to_nav" + + +class MarlController: + def __init__(self, cfg: dict, kinematics: ArmKinematics, legs, arms, estimator): + """legs / arms / estimator: callables numpy -> numpy (networks.py).""" + self.cfg = cfg + self.kin = kinematics + self.legs, self.arms, self.estimator = legs, arms, estimator + self.dt = float(cfg["control_dt"]) + self.default = np.asarray(cfg["default_joint_pos"], dtype=np.float64) + self.kp = np.asarray(cfg["kp"], dtype=np.float64) + self.kd = np.asarray(cfg["kd"], dtype=np.float64) + self.clip = float(cfg["clip_actions"]) + margin = float(cfg["target_margin"]) + self.lower = kinematics.joint_limits[:, 0] - margin + self.upper = kinematics.joint_limits[:, 1] + margin + self.legs_scale = float(cfg["legs"]["action_scale"]) + self.arms_scale = float(cfg["arms"]["residual_scale"]) + self.ik_damping = float(cfg["arms"]["ik_damping"]) + self.max_ik_step = float(cfg["arms"]["max_ik_step"]) + self.history = int(cfg["estimator"]["history"]) + self.gait_period = float(cfg["gait_period"]) + self.zero_cmd_eps = float(cfg["zero_cmd_eps"]) + ranges = cfg["command_ranges"] + self.cmd_low = np.array([ranges["lin_vel_x"][0], ranges["lin_vel_y"][0], ranges["ang_vel_z"][0]]) + self.cmd_high = np.array([ranges["lin_vel_x"][1], ranges["lin_vel_y"][1], ranges["ang_vel_z"][1]]) + self.settle_to_arm_s = float(cfg["settle_to_arm_s"]) + self.settle_to_nav_s = float(cfg["settle_to_nav_s"]) + self.num_arms = len(cfg["arms"]["ee_frames"]) + if len(self.default) != NUM_JOINTS or len(self.kp) != NUM_JOINTS or len(self.kd) != NUM_JOINTS: + raise ValueError("default_joint_pos, kp and kd need one entry per policy joint (27)") + if cfg.get("rest_pose") is not None: + self.rest_pose = np.asarray(cfg["rest_pose"], dtype=np.float64).reshape(self.num_arms, 7) + else: + self.rest_pose = self._rest_pose_from_fk() + for arm in range(self.num_arms): + self.rest_pose[arm, 3:] = quat_unique(self.rest_pose[arm, 3:]) + self.cmd_vel = np.zeros(3) + self.reset(None) + + # -- lifecycle ------------------------------------------------------------- + def reset(self, state: RobotState | None) -> None: + """Start an episode at the measured state: navigation, standing, empty histories.""" + self.mode = Mode.NAV + self.timer = 0.0 + self.pending_goal: np.ndarray | None = None + self.believed = self.rest_pose.copy() # (arms, 7) pelvis frame, wxyz: the arm command + self.steps = 0 # since reset: the gait clock + self._frames: list[np.ndarray] = [] # estimator history, oldest first + self._prev_action: np.ndarray | None = None + # the action terms' state after a reset: legs see the applied target in action units, arms zero + q_legs = state.q[:NUM_LEGS] if state is not None else self.default[:NUM_LEGS] + self.legs_last = (q_legs - self.default[:NUM_LEGS]) / self.legs_scale + self.arms_last = np.zeros(NUM_ARM_JOINTS) + self.last_obs: dict[str, np.ndarray] = {} + + # -- operator commands ------------------------------------------------------- + def set_velocity_command(self, cmd: np.ndarray) -> None: + """(vx, vy, wz), body frame, clipped to the training ranges. Applies while navigating.""" + self.cmd_vel = np.clip(np.asarray(cmd, dtype=np.float64), self.cmd_low, self.cmd_high) + + def request_arm_goal(self, poses: np.ndarray) -> None: + """poses: (arms, 7) wrist targets (x, y, z, qw, qx, qy, qz) in the pelvis frame, now.""" + goal = np.asarray(poses, dtype=np.float64).reshape(self.num_arms, 7).copy() + for arm in range(self.num_arms): + goal[arm, 3:] = quat_unique(goal[arm, 3:] / np.linalg.norm(goal[arm, 3:])) + if self.mode == Mode.ARM: + self.believed = goal # a goal replacing a goal: no settle, as in training + elif self.settle_to_arm_s > 0.0: + self.pending_goal = goal + self.mode, self.timer = Mode.SETTLE_TO_ARM, self.settle_to_arm_s + else: + self._start_arm_goal(goal) + + def end_arm_goal(self) -> None: + if self.mode == Mode.ARM: + self.believed = self.rest_pose.copy() + if self.settle_to_nav_s > 0.0: + self.mode, self.timer = Mode.SETTLE_TO_NAV, self.settle_to_nav_s + else: + self.mode = Mode.NAV + elif self.mode == Mode.SETTLE_TO_ARM: + self.pending_goal = None + self.mode = Mode.NAV + + # -- the env step ---------------------------------------------------------------- + def step(self, state: RobotState) -> JointCommand: + self._update_odometry(state) + self._update_mode() + self.kin.update(state.q) + + common = self._common_obs(state) + legs_obs = np.concatenate([common, self.legs_last]) + arms_obs = np.concatenate([common, self.arms_last, self._wrist_poses(), self._wrist_errors()]) + self.last_obs = {"legs": legs_obs, "arms": arms_obs} + + legs_action = np.clip(self.legs(legs_obs), -self.clip, self.clip) + arms_action = np.clip(self.arms(arms_obs), -self.clip, self.clip) + + target = self.default.copy() + legs_target = legs_action * self.legs_scale + self.default[:NUM_LEGS] + target[:NUM_LEGS] = self._bound(legs_target, slice(0, NUM_LEGS)) + ik = state.q[ARM_START:].copy() + for arm in range(self.num_arms): + cols = self.kin.arm_cols[arm] - ARM_START + ik[cols] += self.kin.ik_step(arm, self.believed[arm, :3], self.believed[arm, 3:], + self.ik_damping, self.max_ik_step) + target[ARM_START:] = self._bound(ik + arms_action * self.arms_scale, slice(ARM_START, NUM_JOINTS)) + + # what each agent observes of its action next step (mdp.applied_action) + self.legs_last = (target[:NUM_LEGS] - self.default[:NUM_LEGS]) / self.legs_scale + self.arms_last = arms_action + self._prev_action = np.concatenate([legs_action, arms_action]) + self.steps += 1 + return JointCommand(q=target, kp=self.kp.copy(), kd=self.kd.copy()) + + # -- internals ---------------------------------------------------------------------- + def _bound(self, target: np.ndarray, joints: slice) -> np.ndarray: + return np.minimum(np.maximum(target, self.lower[joints]), self.upper[joints]) + + def _start_arm_goal(self, goal: np.ndarray) -> None: + self.believed = goal + self.pending_goal = None + self.mode, self.timer = Mode.ARM, 0.0 + + def _update_mode(self) -> None: + if self.mode not in (Mode.SETTLE_TO_ARM, Mode.SETTLE_TO_NAV): + return + self.timer -= self.dt + if self.timer > 1e-9: + return + if self.mode == Mode.SETTLE_TO_ARM: + self._start_arm_goal(self.pending_goal) + else: + self.mode = Mode.NAV + + def velocity_command(self) -> np.ndarray: + return self.cmd_vel.copy() if self.mode == Mode.NAV else np.zeros(3) + + def _gait_phase(self, cmd: np.ndarray) -> np.ndarray: + if np.linalg.norm(cmd) < self.zero_cmd_eps: + return np.zeros(2) + phase = (self.steps * self.dt % self.gait_period) / self.gait_period + return np.sin(2.0 * np.pi * np.array([phase, (phase + 0.5) % 1.0])) + + def _common_obs(self, state: RobotState) -> np.ndarray: + cmd = self.velocity_command() + ee_targets = np.concatenate([np.concatenate([p[:3], wxyz_to_xyzw(p[3:])]) for p in self.believed]) + return np.concatenate([ + state.acc, + state.gyro, + projected_gravity(state.quat), + cmd, + [1.0 if self.mode == Mode.ARM else 0.0], + ee_targets, + [0.0], # height_drop: an input the actors are always shown as zero + state.q - self.default, + state.dq, + self._gait_phase(cmd), + ]) + + def _wrist_poses(self) -> np.ndarray: + out = [] + for arm in range(self.num_arms): + pos, quat = self.kin.wrist_pose(arm) + out.append(np.concatenate([pos, wxyz_to_xyzw(quat)])) + return np.concatenate(out) + + def _wrist_errors(self) -> np.ndarray: + out = [] + for arm in range(self.num_arms): + pos, quat = self.kin.wrist_pose(arm) + pos_err, rot_err = compute_pose_error(pos, quat, self.believed[arm, :3], self.believed[arm, 3:]) + out.append(np.concatenate([pos_err, rot_err])) + return np.concatenate(out) + + def wrist_errors(self) -> np.ndarray: + """(arms,) position error to the command (m), for logging.""" + return np.linalg.norm(self._wrist_errors().reshape(self.num_arms, 6)[:, :3], axis=1) + + def _update_odometry(self, state: RobotState) -> None: + frame = np.concatenate([ + state.acc, state.gyro, projected_gravity(state.quat), + state.q - self.default, state.dq, state.tau[:NUM_LEGS], + ]) + if not self._frames: + self._frames = [frame] * self.history # the first push fills the window, as CircularBuffer does + else: + self._frames = self._frames[1:] + [frame] + if self._prev_action is None: + return + motion = self.estimator(np.concatenate([self._history_input(), self._prev_action])) + if self.mode not in (Mode.ARM, Mode.SETTLE_TO_ARM): + return + delta_pos = motion[:3] * self.dt + delta_quat = quat_from_rotvec(motion[3:] * self.dt) + # a world-fixed target: T_prev o c_prev = T_new o c_new, so c_new = delta^-1 o c_prev + poses = self.believed if self.mode == Mode.ARM else self.pending_goal + for arm in range(self.num_arms): + pos, quat = subtract_frame_transforms(delta_pos, delta_quat, poses[arm, :3], poses[arm, 3:]) + poses[arm, :3], poses[arm, 3:] = pos, quat_unique(quat) + + def _history_input(self) -> np.ndarray: + """The odometry group's layout: term by term, each term's frames oldest first.""" + frames = np.stack(self._frames) # (history, frame) + sizes = [3, 3, 3, NUM_JOINTS, NUM_JOINTS, NUM_LEGS] + bounds = np.cumsum([0] + sizes) + return np.concatenate([frames[:, lo:hi].reshape(-1) for lo, hi in zip(bounds[:-1], bounds[1:])]) + + def _rest_pose_from_fk(self) -> np.ndarray: + self.kin.update(self.default) + return np.stack([np.concatenate(self.kin.wrist_pose(arm)) for arm in range(self.num_arms)]) diff --git a/core_ws/src/locomotion_game_deploy/locomotion_game_deploy/kinematics.py b/core_ws/src/locomotion_game_deploy/locomotion_game_deploy/kinematics.py new file mode 100644 index 0000000..0aaf649 --- /dev/null +++ b/core_ws/src/locomotion_game_deploy/locomotion_game_deploy/kinematics.py @@ -0,0 +1,99 @@ +"""Wrist forward kinematics and the arms' IK step, in the pelvis frame (Pinocchio). + +Reproduces mdp.IKResidualArmAction in Locomanipulation_game: per arm, one +damped-least-squares step (Isaac Lab's DifferentialIKController, pose command, +absolute, dls) from the measured joint angles toward the commanded wrist pose, +with the wrist Jacobian expressed in the pelvis frame and restricted to the +arm's 7 joints, then each joint's step capped at max_ik_step. + +The training Jacobian came from PhysX, whose linear rows are the velocity of +the link's centre of mass (the URDF origin of the wrist link +alone), while the pose error is the link frame's. The IK step reproduces that +pairing: its linear rows are taken at the COM point, the poses at the frame. + +The model is the H1-2 URDF with a fixed base at the pelvis: every pose and +Jacobian here is relative to the pelvis, as in training (PhysX world-frame +Jacobian rotated into the root frame). +""" + +import xml.etree.ElementTree as ET + +import numpy as np +import pinocchio as pin + +from locomotion_game_deploy.rotations import compute_pose_error, quat_from_matrix, quat_unique + + +class ArmKinematics: + def __init__(self, urdf_path: str, joint_names: list[str], ee_frames: list[str], + arm_joint_names: list[list[str]], pelvis_frame: str = "pelvis"): + """joint_names: the policy's joint order (Unitree motor order); q vectors use it.""" + self.model = pin.buildModelFromUrdf(urdf_path) + self.data = self.model.createData() + self._q_pin = pin.neutral(self.model) + # policy index -> pinocchio q index (every policy joint is a 1-dof revolute joint) + self._q_idx = np.array([self.model.joints[self.model.getJointId(n)].idx_q for n in joint_names]) + self._pelvis_id = self.model.getFrameId(pelvis_frame) + self._ee_ids = [self.model.getFrameId(f) for f in ee_frames] + for name, fid in zip([pelvis_frame] + ee_frames, [self._pelvis_id] + self._ee_ids): + if fid >= len(self.model.frames): + raise ValueError(f"frame {name!r} not in {urdf_path}") + # per arm: columns in the policy's joint order, and in pinocchio's velocity vector + self.arm_cols = [np.array([joint_names.index(j) for j in arm]) for arm in arm_joint_names] + self._v_cols = [np.array([self.model.joints[self.model.getJointId(j)].idx_v for j in arm]) + for arm in arm_joint_names] + # each wrist link's own COM (link frame): where PhysX takes the Jacobian's linear rows + self._jac_points = [_inertial_origin(urdf_path, f) for f in ee_frames] + lower = self.model.lowerPositionLimit[self._q_idx] + upper = self.model.upperPositionLimit[self._q_idx] + self.joint_limits = np.stack([lower, upper], axis=1) # (num joints, 2), policy order + self._jacobians: list[np.ndarray] = [] + self._poses: list[tuple[np.ndarray, np.ndarray]] = [] + + def update(self, q: np.ndarray) -> None: + """Forward kinematics and wrist Jacobians at joint angles q (policy order).""" + self._q_pin[self._q_idx] = q + pin.computeJointJacobians(self.model, self.data, self._q_pin) + pin.updateFramePlacements(self.model, self.data) + o_m_pelvis = self.data.oMf[self._pelvis_id] + r_pelvis_t = o_m_pelvis.rotation.T + self._poses, self._jacobians = [], [] + for arm, fid in enumerate(self._ee_ids): + o_m_ee = self.data.oMf[fid] + pelvis_m_ee = o_m_pelvis.actInv(o_m_ee) + self._poses.append((pelvis_m_ee.translation.copy(), quat_from_matrix(pelvis_m_ee.rotation))) + # frame-origin linear and angular velocity, world axes; moved to the COM point + jac = pin.getFrameJacobian(self.model, self.data, fid, pin.LOCAL_WORLD_ALIGNED)[:, self._v_cols[arm]] + r = o_m_ee.rotation @ self._jac_points[arm] + lin = jac[:3] - _skew(r) @ jac[3:] + self._jacobians.append(np.vstack([r_pelvis_t @ lin, r_pelvis_t @ jac[3:]])) + + def wrist_pose(self, arm: int) -> tuple[np.ndarray, np.ndarray]: + """(position, quaternion wxyz with qw >= 0) of the arm's wrist frame in the pelvis frame.""" + pos, quat = self._poses[arm] + return pos, quat_unique(quat) + + def ik_step(self, arm: int, target_pos: np.ndarray, target_quat: np.ndarray, damping: float, + max_step: float) -> np.ndarray: + """The capped DLS joint step for one arm toward a pelvis-frame wrist pose, in its arm_cols order.""" + pos, quat = self._poses[arm] + pos_err, rot_err = compute_pose_error(pos, quat, target_pos, target_quat) + err = np.concatenate([pos_err, rot_err]) + jac = self._jacobians[arm] + step = jac.T @ np.linalg.solve(jac @ jac.T + damping ** 2 * np.eye(6), err) + return np.clip(step, -max_step, max_step) + + +def _skew(v: np.ndarray) -> np.ndarray: + return np.array([[0.0, -v[2], v[1]], [v[2], 0.0, -v[0]], [-v[1], v[0], 0.0]]) + + +def _inertial_origin(urdf_path: str, link: str) -> np.ndarray: + """The of a URDF link (its own COM, link frame).""" + for element in ET.parse(urdf_path).getroot().iter("link"): + if element.get("name") == link: + origin = element.find("inertial/origin") + if origin is None: + return np.zeros(3) + return np.array([float(v) for v in origin.get("xyz", "0 0 0").split()]) + raise ValueError(f"link {link!r} not in {urdf_path}") diff --git a/core_ws/src/locomotion_game_deploy/locomotion_game_deploy/marl_deploy_node.py b/core_ws/src/locomotion_game_deploy/locomotion_game_deploy/marl_deploy_node.py new file mode 100644 index 0000000..333ba6c --- /dev/null +++ b/core_ws/src/locomotion_game_deploy/locomotion_game_deploy/marl_deploy_node.py @@ -0,0 +1,372 @@ +"""LocoMotionGameDeploy node: the two-agent MARL policy (legs + IK arms) on the H1-2. + +Flow +---- +1. Idle until the first /lowstate. +2. Pre-pose: ramp all 27 joint targets (cosine blend over ``prepose_ramp_s``) + from the measured pose to the policy's default pose at the training gains, + then wait for the joints to settle there. +3. With ``engage_wait_for_confirm`` (default true; the sim launch turns it + off) hold the pre-pose until an operator calls ~/confirm_engage. +4. Engage: the controller starts an episode at the measured state and runs at + ``control_hz`` (50 Hz, the training policy rate). In the sim the elastic + band is released ``band_release_delay_s`` later and the episode restarts + then, so the policy's first free step is its episode's first. + +While engaged the robot navigates on /cmd_vel, or reaches for an arm goal: +a PoseArray on ~/arm_goal (left then right wrist_yaw_link pose, pelvis frame) +stops it, settles and reaches; ~/end_arm_goal settles and returns to walking. +/cmd_vel is ignored during an arm goal: the policy only ever saw zero there. + +This node owns all 27 joints: legs on /safety/lowcmd_lower_in, torso and arms +on /safety/lowcmd_upper_in. frame_task_server and lowerbody_controller_node +must not run beside it (the bringup gates them on lowerbody:=marl_ik). + +Safety +------ +* /safety/heartbeat watchdog, as in h12_lowerbody_rl: armed on the first beat; + a False beat (the safety layer's estop latched) or ``heartbeat_timeout`` of + silence latches this node silent. +* /lowstate older than ``lowstate_timeout``: no policy step and no publish + that tick (stepping on a frozen state would integrate garbage). +* A non-finite observation or target latches the node silent instead of + sending it: the safety layer would latch its estop (all motors limp) on it. +Silent means the safety layer keeps holding the last targets until it is +stopped; it has no staleness watchdog on these channels. + +Interfaces +---------- +sub /lowstate (unitree_hg/LowState) +sub /cmd_vel (geometry_msgs/Twist) +sub /safety/heartbeat (std_msgs/Bool) +sub ~/arm_goal (geometry_msgs/PoseArray) 2 poses, pelvis frame +pub /safety/lowcmd_lower_in (unitree_hg/LowCmd) rows 0-11 +pub /safety/lowcmd_upper_in (unitree_hg/LowCmd) rows 12-26 +pub ~/state (std_msgs/String, latched) +pub ~/wrist_error (std_msgs/Float32MultiArray) m, per arm, while engaged +srv ~/confirm_engage (std_srvs/Trigger) +srv ~/end_arm_goal (std_srvs/Trigger) +""" + +import os +import time + +import numpy as np +import rclpy +import torch +import yaml +from ament_index_python.packages import get_package_share_directory +from geometry_msgs.msg import PoseArray, Twist +from rclpy.node import Node +from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy +from std_msgs.msg import Bool, Float32MultiArray, String +from std_srvs.srv import Trigger +from unitree_hg.msg import LowCmd, LowState + +from locomotion_game_deploy.controller import ( + ARM_START, + NUM_JOINTS, + NUM_LEGS, + JointCommand, + MarlController, + RobotState, +) +from locomotion_game_deploy.kinematics import ArmKinematics +from locomotion_game_deploy.networks import TorchScriptNetwork, ZeroNetwork +from locomotion_game_deploy.rotations import xyzw_to_wxyz + +MOTOR_MODE_PR = 1 +SAFETY_HEARTBEAT_TOPIC = "/safety/heartbeat" +PREPOSE_TOL = 0.15 # rad; max |q - default| over all 27 joints to count as posed +PREPOSE_SETTLE_TICKS = 10 # consecutive in-tolerance ticks (0.2 s at 50 Hz) +PREPOSE_MAX_TICKS = 400 # after the ramp; posed or not, stop waiting (8 s at 50 Hz) + + +def _share(*parts: str) -> str: + return os.path.join(get_package_share_directory("locomotion_game_deploy"), *parts) + + +class MarlDeployNode(Node): + def __init__(self): + super().__init__("marl_deploy_node") + self.declare_parameter("config", _share("policies", "marl_ik", "marl_ik.yaml")) + self.declare_parameter("urdf_path", "/home/code/CL_Assets/ros_assets/h1_2_magpie.urdf") + # "torch": the exported networks next to the config. "zero": stand-ins (legs hold the + # default pose, arms pure IK, no odometry), for plumbing tests without a checkpoint. + self.declare_parameter("policy_mode", "torch") + self.declare_parameter("control_hz", 50.0) + self.declare_parameter("prepose_ramp_s", 2.0) + self.declare_parameter("engage_wait_for_confirm", True) + self.declare_parameter("disable_elastic_band", True) + self.declare_parameter("band_release_delay_s", 1.0) + self.declare_parameter("heartbeat_timeout", 0.5) + self.declare_parameter("lowstate_timeout", 0.1) + + cfg_path = self.get_parameter("config").value + with open(cfg_path) as f: + cfg = yaml.safe_load(f) + cfg_dir = os.path.dirname(os.path.abspath(cfg_path)) + # export_marl.py's per-checkpoint values (rest pose, source files) override the template + export_path = os.path.join(cfg_dir, "export.yaml") + if os.path.isfile(export_path): + with open(export_path) as f: + cfg.update(yaml.safe_load(f) or {}) + self.get_logger().info(f"export: {cfg.get('source')}") + control_hz = float(self.get_parameter("control_hz").value) + if abs(1.0 / control_hz - float(cfg["control_dt"])) > 1e-6: + raise ValueError(f"control_hz {control_hz} doesn't match the policy's control_dt {cfg['control_dt']}") + + torch.set_num_threads(1) + kin = ArmKinematics( + self.get_parameter("urdf_path").value, + joint_names=cfg["joint_names"], + ee_frames=cfg["arms"]["ee_frames"], + arm_joint_names=[cfg["joint_names"][ARM_START:ARM_START + 7], cfg["joint_names"][ARM_START + 7:]], + ) + mode = str(self.get_parameter("policy_mode").value) + if mode == "torch": + legs = TorchScriptNetwork(os.path.join(cfg_dir, cfg["legs"]["policy"]), + cfg["legs"]["num_obs"], cfg["legs"]["num_actions"]) + arms = TorchScriptNetwork(os.path.join(cfg_dir, cfg["arms"]["policy"]), + cfg["arms"]["num_obs"], cfg["arms"]["num_actions"]) + estimator = TorchScriptNetwork(os.path.join(cfg_dir, cfg["estimator"]["path"]), + cfg["estimator"]["num_inputs"], 6) + elif mode == "zero": + self.get_logger().warn("policy_mode=zero: stand-in networks, for plumbing tests only") + legs, arms, estimator = (ZeroNetwork(cfg["legs"]["num_actions"]), + ZeroNetwork(cfg["arms"]["num_actions"]), ZeroNetwork(6)) + else: + raise ValueError(f"policy_mode must be 'torch' or 'zero', not {mode!r}") + self.controller = MarlController(cfg, kin, legs, arms, estimator) + + self._lowstate: LowState | None = None + self._lowstate_time: float | None = None # monotonic receipt time + self._confirm_wait = bool(self.get_parameter("engage_wait_for_confirm").value) + self._confirmed = False + self._ramp_ticks = max(1, int(round(float(self.get_parameter("prepose_ramp_s").value) * control_hz))) + self._prepose_ticks = 0 + self._prepose_pass = 0 + self._prepose_start: np.ndarray | None = None + self._engaged = False + self._band_released = not bool(self.get_parameter("disable_elastic_band").value) + self._band_release_at: float | None = None + self._band_inflight = False + self._lowstate_timeout = float(self.get_parameter("lowstate_timeout").value) + self._hb_timeout = float(self.get_parameter("heartbeat_timeout").value) + self._hb_last: float | None = None + self._dead = False # latched: never publishes again + self._state_name = "" + + lowstate_qos = QoSProfile(reliability=ReliabilityPolicy.BEST_EFFORT, + history=HistoryPolicy.KEEP_LAST, depth=1) + latched = QoSProfile(reliability=ReliabilityPolicy.RELIABLE, durability=DurabilityPolicy.TRANSIENT_LOCAL, + history=HistoryPolicy.KEEP_LAST, depth=1) + self.create_subscription(LowState, "/lowstate", self._on_lowstate, lowstate_qos) + self.create_subscription(Twist, "/cmd_vel", self._on_cmd_vel, 10) + self.create_subscription(Bool, SAFETY_HEARTBEAT_TOPIC, self._on_heartbeat, 10) + self.create_subscription(PoseArray, "~/arm_goal", self._on_arm_goal, 10) + self._lower_pub = self.create_publisher(LowCmd, "/safety/lowcmd_lower_in", 10) + self._upper_pub = self.create_publisher(LowCmd, "/safety/lowcmd_upper_in", 10) + self._state_pub = self.create_publisher(String, "~/state", latched) + self._err_pub = self.create_publisher(Float32MultiArray, "~/wrist_error", 10) + self.create_service(Trigger, "~/confirm_engage", self._on_confirm) + self.create_service(Trigger, "~/end_arm_goal", self._on_end_arm_goal) + self._band_cli = self.create_client(Trigger, "/elastic_band/toggle") + self.create_timer(1.0 / control_hz, self._tick) + self._set_state("idle") + self.get_logger().info( + f"marl_deploy_node ready: {cfg_path} ({mode}), {control_hz:.0f} Hz, " + f"confirm={'required' if self._confirm_wait else 'auto'}") + + # -- callbacks ------------------------------------------------------------------- + def _on_lowstate(self, msg: LowState) -> None: + self._lowstate = msg + self._lowstate_time = time.monotonic() + + def _on_cmd_vel(self, msg: Twist) -> None: + self.controller.set_velocity_command(np.array([msg.linear.x, msg.linear.y, msg.angular.z])) + + def _on_arm_goal(self, msg: PoseArray) -> None: + n = self.controller.num_arms + if len(msg.poses) != n: + self.get_logger().error(f"arm goal needs {n} poses (left, right), got {len(msg.poses)}; ignored") + return + poses = np.array([[p.position.x, p.position.y, p.position.z, + *xyzw_to_wxyz(np.array([p.orientation.x, p.orientation.y, + p.orientation.z, p.orientation.w]))] + for p in msg.poses]) + if not np.isfinite(poses).all() or np.any(np.linalg.norm(poses[:, 3:], axis=1) < 1e-6): + self.get_logger().error("arm goal has a non-finite position or a zero quaternion; ignored") + return + if not self._engaged: + self.get_logger().warn("arm goal before the policy is engaged; ignored") + return + self.controller.request_arm_goal(poses) + self.get_logger().info(f"arm goal: left {poses[0, :3].round(3)}, right {poses[1, :3].round(3)} (pelvis)") + + def _on_heartbeat(self, msg: Bool) -> None: + if self._dead: + return + if not msg.data: + self._latch("safety layer reports estop latched (heartbeat False)") + return + self._hb_last = time.monotonic() + + def _on_confirm(self, _req, resp): + self._confirmed = True + resp.success = True + resp.message = "already engaged" if self._engaged else "confirmed: engaging once the pre-pose settles" + self.get_logger().info(f"operator confirm: {resp.message}") + return resp + + def _on_end_arm_goal(self, _req, resp): + self.controller.end_arm_goal() + resp.success = True + resp.message = f"mode {self.controller.mode.value}" + return resp + + # -- helpers ------------------------------------------------------------------------- + def _latch(self, reason: str) -> None: + self._dead = True + self._set_state(f"fault: {reason}") + self.get_logger().error( + f"LATCHED SILENT: {reason}. No more commands on /safety/lowcmd_*_in; " + "restart this node once the cause is cleared.") + + def _set_state(self, name: str) -> None: + if name != self._state_name: + self._state_name = name + self._state_pub.publish(String(data=name)) + + def _robot_state(self, msg: LowState) -> RobotState: + motors = msg.motor_state + return RobotState( + q=np.array([motors[i].q for i in range(NUM_JOINTS)], dtype=np.float64), + dq=np.array([motors[i].dq for i in range(NUM_JOINTS)], dtype=np.float64), + tau=np.array([motors[i].tau_est for i in range(NUM_JOINTS)], dtype=np.float64), + quat=np.asarray(msg.imu_state.quaternion, dtype=np.float64), + gyro=np.asarray(msg.imu_state.gyroscope, dtype=np.float64), + acc=np.asarray(msg.imu_state.accelerometer, dtype=np.float64), + ) + + def _publish(self, cmd: JointCommand, mode_machine: int) -> None: + if not (np.isfinite(cmd.q).all() and np.isfinite(cmd.kp).all() and np.isfinite(cmd.kd).all()): + self._latch(f"non-finite joint command {cmd.q.round(3).tolist()}") + return + for pub, rows in ((self._lower_pub, range(0, NUM_LEGS)), (self._upper_pub, range(NUM_LEGS, NUM_JOINTS))): + msg = LowCmd() + msg.mode_pr = 0 + msg.mode_machine = mode_machine # the safety layer copies the header of whichever channel spoke last + for i in rows: + m = msg.motor_cmd[i] + m.mode = MOTOR_MODE_PR + m.q = float(cmd.q[i]) + m.dq = 0.0 + m.tau = 0.0 + m.kp = float(cmd.kp[i]) + m.kd = float(cmd.kd[i]) + pub.publish(msg) + + def _release_band(self) -> None: + if self._band_released or self._band_inflight: + return + if not self._band_cli.service_is_ready(): + self.get_logger().warn("waiting for /elastic_band/toggle", throttle_duration_sec=5.0) + return + self._band_inflight = True + self._band_cli.call_async(Trigger.Request()).add_done_callback(self._on_band_toggled) + + def _on_band_toggled(self, fut) -> None: + self._band_inflight = False + result = fut.result() + if result is None: + self.get_logger().warn("elastic band toggle failed; retrying") + return + self._band_released = True + self.get_logger().info(f"elastic band released ({result.message}); episode restarts") + if self._lowstate is not None: + self.controller.reset(self._robot_state(self._lowstate)) + + # -- main loop --------------------------------------------------------------------------- + def _tick(self) -> None: + if self._dead: + return + now = time.monotonic() + if self._hb_last is not None and now - self._hb_last > self._hb_timeout: + self._latch(f"no {SAFETY_HEARTBEAT_TOPIC} for {self._hb_timeout:.1f} s (safety node down?)") + return + if self._lowstate is None: + return + if now - self._lowstate_time > self._lowstate_timeout: + self.get_logger().warn( + f"/lowstate is {now - self._lowstate_time:.2f} s old; not stepping", throttle_duration_sec=1.0) + return + msg = self._lowstate + state = self._robot_state(msg) + if not (np.isfinite(state.q).all() and np.isfinite(state.quat).all()): + self._latch("non-finite /lowstate") + return + + if not self._engaged: + self._prepose(state, msg.mode_machine) + return + + if self._band_release_at is not None and not self._band_released and now >= self._band_release_at: + self._release_band() + + cmd = self.controller.step(state) + for name, obs in self.controller.last_obs.items(): + if not np.isfinite(obs).all(): + self._latch(f"non-finite {name} observation") + return + self._publish(cmd, msg.mode_machine) + self._set_state(self.controller.mode.value) + self._err_pub.publish(Float32MultiArray(data=self.controller.wrist_errors().astype(np.float32).tolist())) + + def _prepose(self, state: RobotState, mode_machine: int) -> None: + ctrl = self.controller + if self._prepose_start is None: + self._prepose_start = state.q.copy() + self._set_state("preposing") + target = ctrl.default.copy() + if self._prepose_ticks < self._ramp_ticks: + frac = self._prepose_ticks / self._ramp_ticks + alpha = 0.5 * (1.0 - np.cos(np.pi * frac)) + target = (1.0 - alpha) * self._prepose_start + alpha * target + self._publish(JointCommand(q=target, kp=ctrl.kp.copy(), kd=ctrl.kd.copy()), mode_machine) + self._prepose_ticks += 1 + err = float(np.max(np.abs(state.q - ctrl.default))) + self._prepose_pass = self._prepose_pass + 1 if err < PREPOSE_TOL else 0 + settled = self._prepose_pass >= PREPOSE_SETTLE_TICKS + timed_out = self._prepose_ticks >= self._ramp_ticks + PREPOSE_MAX_TICKS + if not settled and not timed_out: + return + if self._confirm_wait and not self._confirmed: + self._set_state("waiting_confirm") + self.get_logger().info( + f"pre-posed ({'settled' if settled else 'NOT SETTLED'}, max err {err:.3f} rad); holding. " + "Engage with: ros2 service call /marl_deploy_node/confirm_engage std_srvs/srv/Trigger", + throttle_duration_sec=5.0) + return + self.get_logger().info(f"pre-posed ({'settled' if settled else 'timeout'}, max err {err:.3f} rad): engaging") + ctrl.reset(state) + self._engaged = True + if not self._band_released: + self._band_release_at = time.monotonic() + float(self.get_parameter("band_release_delay_s").value) + + +def main(): + rclpy.init() + node = MarlDeployNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/core_ws/src/locomotion_game_deploy/locomotion_game_deploy/networks.py b/core_ws/src/locomotion_game_deploy/locomotion_game_deploy/networks.py new file mode 100644 index 0000000..e3a77f3 --- /dev/null +++ b/core_ws/src/locomotion_game_deploy/locomotion_game_deploy/networks.py @@ -0,0 +1,38 @@ +"""The exported networks: two actors and the pelvis-motion estimator, as TorchScript. + +scripts/skrl/export_marl.py in Locomanipulation_game writes them. Each actor +file already contains its observation scaler and the +-10 action clamp, so it +maps a raw observation straight to the raw action the env would apply; the +estimator file contains its input normalization. + +ZeroNetwork stands in for a missing export: legs hold the default pose, arms +follow pure IK, and the estimator reports no pelvis motion. It is for +plumbing tests only. +""" + +import numpy as np +import torch + + +class TorchScriptNetwork: + def __init__(self, path: str, num_inputs: int, num_outputs: int): + self.module = torch.jit.load(path, map_location="cpu") + self.module.eval() + with torch.inference_mode(): + out = self.module(torch.zeros(1, num_inputs)) + if out.shape != (1, num_outputs): + raise ValueError(f"{path}: {num_inputs} inputs gave output shape {tuple(out.shape)}, " + f"expected (1, {num_outputs})") + + def __call__(self, x: np.ndarray) -> np.ndarray: + with torch.inference_mode(): + out = self.module(torch.from_numpy(x.astype(np.float32)).unsqueeze(0)) + return out.squeeze(0).numpy().astype(np.float64) + + +class ZeroNetwork: + def __init__(self, num_outputs: int): + self._out = np.zeros(num_outputs) + + def __call__(self, x: np.ndarray) -> np.ndarray: + return self._out.copy() diff --git a/core_ws/src/locomotion_game_deploy/locomotion_game_deploy/rotations.py b/core_ws/src/locomotion_game_deploy/locomotion_game_deploy/rotations.py new file mode 100644 index 0000000..2ae86dd --- /dev/null +++ b/core_ws/src/locomotion_game_deploy/locomotion_game_deploy/rotations.py @@ -0,0 +1,102 @@ +"""Quaternion and frame helpers, numpy, single poses. + +Quaternions are (w, x, y, z), as in Isaac Lab and the Unitree IMU. Each +function mirrors the isaaclab.utils.math function of the same name, which +the training environment used, so the deployed observations and IK step +follow the same conventions. +""" + +import numpy as np + + +def quat_mul(q1: np.ndarray, q2: np.ndarray) -> np.ndarray: + w1, x1, y1, z1 = q1 + w2, x2, y2, z2 = q2 + return np.array([ + w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2, + w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2, + w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2, + w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2, + ]) + + +def quat_conjugate(q: np.ndarray) -> np.ndarray: + return np.array([q[0], -q[1], -q[2], -q[3]]) + + +def quat_unique(q: np.ndarray) -> np.ndarray: + """The same rotation with qw >= 0.""" + return -q if q[0] < 0 else q + + +def quat_apply(q: np.ndarray, v: np.ndarray) -> np.ndarray: + """Rotate v by q.""" + xyz = q[1:] + t = 2.0 * np.cross(xyz, v) + return v + q[0] * t + np.cross(xyz, t) + + +def quat_apply_inverse(q: np.ndarray, v: np.ndarray) -> np.ndarray: + """Rotate v by q^-1: a world vector expressed in the frame q.""" + return quat_apply(quat_conjugate(q), v) + + +def axis_angle_from_quat(q: np.ndarray, eps: float = 1.0e-6) -> np.ndarray: + q = -q if q[0] < 0 else q + mag = np.linalg.norm(q[1:]) + half_angle = np.arctan2(mag, q[0]) + angle = 2.0 * half_angle + ratio = np.sin(half_angle) / angle if abs(angle) > eps else 0.5 - angle * angle / 48.0 + return q[1:] / ratio + + +def quat_from_rotvec(rotvec: np.ndarray, eps: float = 1.0e-12) -> np.ndarray: + """Quaternion for a rotation of |rotvec| rad about rotvec (isaaclab quat_from_angle_axis).""" + angle = np.linalg.norm(rotvec) + axis = rotvec / max(angle, eps) + half = 0.5 * angle + return np.concatenate([[np.cos(half)], axis * np.sin(half)]) + + +def quat_from_matrix(m: np.ndarray) -> np.ndarray: + """Rotation matrix -> unit quaternion, qw >= 0.""" + trace = m[0, 0] + m[1, 1] + m[2, 2] + if trace > 0.0: + s = 2.0 * np.sqrt(trace + 1.0) + q = np.array([0.25 * s, (m[2, 1] - m[1, 2]) / s, (m[0, 2] - m[2, 0]) / s, (m[1, 0] - m[0, 1]) / s]) + elif m[0, 0] > m[1, 1] and m[0, 0] > m[2, 2]: + s = 2.0 * np.sqrt(1.0 + m[0, 0] - m[1, 1] - m[2, 2]) + q = np.array([(m[2, 1] - m[1, 2]) / s, 0.25 * s, (m[0, 1] + m[1, 0]) / s, (m[0, 2] + m[2, 0]) / s]) + elif m[1, 1] > m[2, 2]: + s = 2.0 * np.sqrt(1.0 + m[1, 1] - m[0, 0] - m[2, 2]) + q = np.array([(m[0, 2] - m[2, 0]) / s, (m[0, 1] + m[1, 0]) / s, 0.25 * s, (m[1, 2] + m[2, 1]) / s]) + else: + s = 2.0 * np.sqrt(1.0 + m[2, 2] - m[0, 0] - m[1, 1]) + q = np.array([(m[1, 0] - m[0, 1]) / s, (m[0, 2] + m[2, 0]) / s, (m[1, 2] + m[2, 1]) / s, 0.25 * s]) + q /= np.linalg.norm(q) + return quat_unique(q) + + +def projected_gravity(quat: np.ndarray) -> np.ndarray: + """Unit gravity direction in the body frame: (0, 0, -1) when upright.""" + return quat_apply_inverse(quat, np.array([0.0, 0.0, -1.0])) + + +def subtract_frame_transforms(pos_a, quat_a, pos_b, quat_b): + """Pose b expressed in frame a: T_a^-1 o T_b.""" + quat_a_inv = quat_conjugate(quat_a) + return quat_apply(quat_a_inv, pos_b - pos_a), quat_mul(quat_a_inv, quat_b) + + +def compute_pose_error(pos_cur, quat_cur, pos_des, quat_des): + """(position error, axis-angle error) from the current pose to the desired one, as isaaclab computes them.""" + quat_err = quat_mul(quat_des, quat_conjugate(quat_cur) / np.dot(quat_cur, quat_cur)) + return pos_des - pos_cur, axis_angle_from_quat(quat_err) + + +def wxyz_to_xyzw(q: np.ndarray) -> np.ndarray: + return np.array([q[1], q[2], q[3], q[0]]) + + +def xyzw_to_wxyz(q: np.ndarray) -> np.ndarray: + return np.array([q[3], q[0], q[1], q[2]]) diff --git a/core_ws/src/locomotion_game_deploy/package.xml b/core_ws/src/locomotion_game_deploy/package.xml new file mode 100644 index 0000000..becceeb --- /dev/null +++ b/core_ws/src/locomotion_game_deploy/package.xml @@ -0,0 +1,28 @@ + + + + locomotion_game_deploy + 0.0.0 + LocoMotionGameDeploy: runs the two-agent (legs + IK arms) MARL policy from + Locomanipulation_game on the H1-2 through the split safety layer. + maxconway + TODO: License declaration + + rclpy + geometry_msgs + std_msgs + std_srvs + rcl_interfaces + unitree_hg + ament_index_python + python3-numpy + python3-yaml + pinocchio + + + python3-pytest + + + ament_python + + diff --git a/core_ws/src/locomotion_game_deploy/policies/marl_ik/marl_ik.yaml b/core_ws/src/locomotion_game_deploy/policies/marl_ik/marl_ik.yaml new file mode 100644 index 0000000..dba7438 --- /dev/null +++ b/core_ws/src/locomotion_game_deploy/policies/marl_ik/marl_ik.yaml @@ -0,0 +1,81 @@ +# Deploy config for LocoManip-Marl-Flat-IK-Direct-v0 (Locomanipulation_game, marl-direct). +# Every value mirrors the training env; the source is named on each block. +# Locomanipulation_game scripts/skrl/export_marl.py writes the networks and export.yaml next to +# this file; export.yaml's keys (rest_pose, source) override the ones here. + +control_dt: 0.02 # decimation 4 x sim dt 0.005 (LocoManipMarlEnvCfg) + +# Policy joint order = Unitree motor order 0..26 (assets/h1_2.py ALL_JOINTS_NAMES) +joint_names: [ + left_hip_yaw_joint, left_hip_pitch_joint, left_hip_roll_joint, + left_knee_joint, left_ankle_pitch_joint, left_ankle_roll_joint, + right_hip_yaw_joint, right_hip_pitch_joint, right_hip_roll_joint, + right_knee_joint, right_ankle_pitch_joint, right_ankle_roll_joint, + torso_joint, + left_shoulder_pitch_joint, left_shoulder_roll_joint, left_shoulder_yaw_joint, + left_elbow_joint, left_wrist_roll_joint, left_wrist_pitch_joint, left_wrist_yaw_joint, + right_shoulder_pitch_joint, right_shoulder_roll_joint, right_shoulder_yaw_joint, + right_elbow_joint, right_wrist_roll_joint, right_wrist_pitch_joint, right_wrist_yaw_joint] + +# assets/h1_2.py _INIT_STATE (rad) +default_joint_pos: [ + 0.0, -0.3, 0.0, 0.5, -0.2, 0.0, + 0.0, -0.3, 0.0, 0.5, -0.2, 0.0, + 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + +# assets/h1_2.py _ACTUATORS: stiffness / damping (Nm/rad, Nm s/rad) +kp: [ + 200, 200, 200, 300, 40, 40, + 200, 200, 200, 300, 40, 40, + 300, + 120, 120, 120, 80, 40, 40, 40, + 120, 120, 120, 80, 40, 40, 40] +kd: [ + 2.5, 2.5, 2.5, 4.0, 2.0, 2.0, + 2.5, 2.5, 2.5, 4.0, 2.0, 2.0, + 3.0, + 2.0, 2.0, 2.0, 1.0, 1.0, 1.0, 1.0, + 2.0, 2.0, 2.0, 1.0, 1.0, 1.0, 1.0] + +clip_actions: 10.0 # CLIP_ACTIONS: the env clamps every raw action to +-this +target_margin: 0.4 # rad: joint targets bounded to the URDF hard limits +- this + +legs: + policy: legs.pt + num_obs: 96 + num_actions: 12 + action_scale: 0.25 # target = scale * action + default + +arms: + policy: arms.pt + num_obs: 124 + num_actions: 14 + residual_scale: 0.2 # target = q + IK step + scale * action + ee_frames: [left_wrist_yaw_link, right_wrist_yaw_link] + ik_damping: 0.05 + max_ik_step: 0.1 # rad per joint per policy step + +estimator: + path: estimator.pt + history: 4 # ODOMETRY_HISTORY + num_inputs: 326 # 4 x (3 + 3 + 3 + 27 + 27 + 12) + 26 actions + +# Gait clock (manager_based mdp/rewards.py) +gait_period: 0.8 +zero_cmd_eps: 0.1 + +# Velocity command ranges (legs_r0_env_cfg.py CommandsCfg); /cmd_vel is clipped to them +command_ranges: + lin_vel_x: [-0.7, 0.7] + lin_vel_y: [-0.3, 0.3] + ang_vel_z: [-0.5, 0.5] + +# Zero-velocity settle at a mode switch (LocoManipMarlFlatCurriculumEnvCfg) +settle_to_arm_s: 0.75 +settle_to_nav_s: 1.5 + +# The navigation rest pose: wrist poses (x, y, z, qw, qx, qy, qz), pelvis frame, left then right, +# from the run's arm_target_tables.pt (default_poses). null: forward kinematics at the default pose. +rest_pose: null diff --git a/core_ws/src/locomotion_game_deploy/resource/locomotion_game_deploy b/core_ws/src/locomotion_game_deploy/resource/locomotion_game_deploy new file mode 100644 index 0000000..e69de29 diff --git a/core_ws/src/locomotion_game_deploy/setup.cfg b/core_ws/src/locomotion_game_deploy/setup.cfg new file mode 100644 index 0000000..1d07aae --- /dev/null +++ b/core_ws/src/locomotion_game_deploy/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/locomotion_game_deploy +[install] +install_scripts=$base/lib/locomotion_game_deploy diff --git a/core_ws/src/locomotion_game_deploy/setup.py b/core_ws/src/locomotion_game_deploy/setup.py new file mode 100644 index 0000000..c721ab2 --- /dev/null +++ b/core_ws/src/locomotion_game_deploy/setup.py @@ -0,0 +1,36 @@ +import os +from glob import glob + +from setuptools import find_packages, setup + +package_name = 'locomotion_game_deploy' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + # Exported policies + their deploy config, installed under share//policies//. + (os.path.join('share', package_name, 'policies', 'marl_ik'), + glob('policies/marl_ik/*')), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='maxconway', + maintainer_email='maxlconway@gmail.com', + description='Deploys the Locomanipulation_game MARL (legs + IK arms) policy on the H1-2', + license='TODO: License declaration', + extras_require={ + 'test': [ + 'pytest', + ], + }, + entry_points={ + 'console_scripts': [ + 'marl_deploy_node = locomotion_game_deploy.marl_deploy_node:main', + ], + }, +) diff --git a/core_ws/src/locomotion_game_deploy/test/test_controller.py b/core_ws/src/locomotion_game_deploy/test/test_controller.py new file mode 100644 index 0000000..290b2e1 --- /dev/null +++ b/core_ws/src/locomotion_game_deploy/test/test_controller.py @@ -0,0 +1,301 @@ +"""Unit tests for the ROS-free core: rotations, kinematics, observation layout, odometry. + +Run on a host with pinocchio + torch (e.g. the env_isaaclab conda env): + python -m pytest test/test_controller.py +The Isaac Lab parity tests also need ISAACLAB_MATH, the path to +IsaacLab/source/isaaclab/isaaclab/utils/math.py; they are skipped without it. +""" + +import importlib.util +import os + +import numpy as np +import pytest +import yaml + +from locomotion_game_deploy import rotations as R +from locomotion_game_deploy.controller import MarlController, Mode, RobotState +from locomotion_game_deploy.kinematics import ArmKinematics +from locomotion_game_deploy.networks import ZeroNetwork + +HERE = os.path.dirname(os.path.abspath(__file__)) +CFG_PATH = os.path.join(HERE, "..", "policies", "marl_ik", "marl_ik.yaml") +URDF = os.environ.get( + "H12_URDF", os.path.join(HERE, "..", "..", "..", "..", "CL_Assets", "ros_assets", "h1_2_magpie.urdf")) +RNG = np.random.default_rng(0) + + +def _cfg(): + with open(CFG_PATH) as f: + return yaml.safe_load(f) + + +def _kin(cfg): + names = cfg["joint_names"] + return ArmKinematics(URDF, names, cfg["arms"]["ee_frames"], [names[13:20], names[20:]]) + + +def _random_quat(): + q = RNG.normal(size=4) + return q / np.linalg.norm(q) + + +def _state(cfg, q=None): + q = np.array(cfg["default_joint_pos"]) if q is None else q + return RobotState(q=q, dq=np.zeros(27), tau=np.zeros(27), quat=np.array([1.0, 0, 0, 0]), + gyro=np.zeros(3), acc=np.array([0.0, 0.0, 9.81])) + + +class ConstNetwork: + def __init__(self, out): + self.out = np.asarray(out, dtype=np.float64) + self.inputs = [] + + def __call__(self, x): + self.inputs.append(x.copy()) + return self.out.copy() + + +# -- rotations ----------------------------------------------------------------- + + +def test_matrix_roundtrip(): + for _ in range(50): + q = R.quat_unique(_random_quat()) + w, x, y, z = q + m = np.array([ + [1 - 2 * (y * y + z * z), 2 * (x * y - w * z), 2 * (x * z + w * y)], + [2 * (x * y + w * z), 1 - 2 * (x * x + z * z), 2 * (y * z - w * x)], + [2 * (x * z - w * y), 2 * (y * z + w * x), 1 - 2 * (x * x + y * y)], + ]) + np.testing.assert_allclose(R.quat_from_matrix(m), q, atol=1e-9) + v = RNG.normal(size=3) + np.testing.assert_allclose(R.quat_apply(q, v), m @ v, atol=1e-9) + + +def test_rotvec_roundtrip(): + for _ in range(50): + v = RNG.normal(size=3) * 0.5 + np.testing.assert_allclose(R.axis_angle_from_quat(R.quat_from_rotvec(v)), v, atol=1e-9) + np.testing.assert_allclose(R.quat_from_rotvec(np.zeros(3)), [1, 0, 0, 0]) + + +def _isaaclab_math(): + path = os.environ.get("ISAACLAB_MATH") + if not path: + pytest.skip("ISAACLAB_MATH not set") + spec = importlib.util.spec_from_file_location("isaaclab_math", path) + module = importlib.util.module_from_spec(spec) + spec.loader.exec_module(module) + return module + + +def test_matches_isaaclab(): + import torch + + m = _isaaclab_math() + t = lambda a: torch.tensor(np.asarray(a), dtype=torch.float64).unsqueeze(0) # noqa: E731 + for _ in range(50): + qa, qb = _random_quat(), _random_quat() + pa, pb = RNG.normal(size=3), RNG.normal(size=3) + pos, quat = m.subtract_frame_transforms(t(pa), t(qa), t(pb), t(qb)) + mine = R.subtract_frame_transforms(pa, qa, pb, qb) + np.testing.assert_allclose(mine[0], pos[0].numpy(), atol=1e-9) + np.testing.assert_allclose(mine[1], quat[0].numpy(), atol=1e-9) + perr, rerr = m.compute_pose_error(t(pa), t(qa), t(pb), t(qb), rot_error_type="axis_angle") + mine = R.compute_pose_error(pa, qa, pb, qb) + np.testing.assert_allclose(mine[0], perr[0].numpy(), atol=1e-9) + np.testing.assert_allclose(mine[1], rerr[0].numpy(), atol=1e-9) + g = m.quat_apply_inverse(t(qa), t([0.0, 0.0, -1.0])) + np.testing.assert_allclose(R.projected_gravity(qa), g[0].numpy(), atol=1e-9) + v = RNG.normal(size=3) * 0.3 + qv = m.quat_from_angle_axis(torch.tensor([np.linalg.norm(v)]), t(v)) + np.testing.assert_allclose(R.quat_from_rotvec(v), qv[0].numpy(), atol=1e-9) + + +# -- kinematics ------------------------------------------------------------------- + + +def test_jacobian_matches_finite_differences(): + """Linear rows: the wrist link's COM point (as PhysX reports them); angular rows: the link.""" + cfg = _cfg() + kin = _kin(cfg) + q = np.array(cfg["default_joint_pos"]) + RNG.uniform(-0.3, 0.3, size=27) + kin.update(q) + + def com(arm): + pos, quat = kin.wrist_pose(arm) + return pos + R.quat_apply(quat, kin._jac_points[arm]) + + for arm in range(2): + assert np.linalg.norm(kin._jac_points[arm]) > 0.02 # read from the URDF inertial origin + jac = kin._jacobians[arm] + pos0, quat0 = kin.wrist_pose(arm) + com0 = com(arm) + eps = 1e-6 + for k, col in enumerate(kin.arm_cols[arm]): + dq = q.copy() + dq[col] += eps + kin.update(dq) + pos1, quat1 = kin.wrist_pose(arm) + np.testing.assert_allclose((com(arm) - com0) / eps, jac[:3, k], atol=1e-4) + dtheta = R.axis_angle_from_quat(R.quat_mul(quat1, R.quat_conjugate(quat0))) / eps + np.testing.assert_allclose(dtheta, jac[3:, k], atol=1e-4) + kin.update(q) + + +def test_ik_steps_converge(): + cfg = _cfg() + kin = _kin(cfg) + default = np.array(cfg["default_joint_pos"]) + goal_q = default.copy() + goal_q[13:27] = RNG.uniform(-0.4, 0.4, size=14) + goal_q[16] = goal_q[23] = 0.8 # elbows bent, away from the straight-arm singularity + kin.update(goal_q) + targets = [kin.wrist_pose(arm) for arm in range(2)] + q = default.copy() + for _ in range(60): + kin.update(q) + for arm in range(2): + q[kin.arm_cols[arm]] += kin.ik_step(arm, *targets[arm], damping=0.05, max_step=0.1) + kin.update(q) + for arm in range(2): + pos_err, rot_err = R.compute_pose_error(*kin.wrist_pose(arm), *targets[arm]) + assert np.linalg.norm(pos_err) < 5e-3 + assert np.linalg.norm(rot_err) < 2e-2 + + +def test_ik_step_capped(): + cfg = _cfg() + kin = _kin(cfg) + kin.update(np.array(cfg["default_joint_pos"])) + step = kin.ik_step(0, np.array([0.5, 0.5, 0.5]), np.array([0.0, 1.0, 0.0, 0.0]), 0.05, 0.1) + assert np.all(np.abs(step) <= 0.1 + 1e-12) + + +# -- controller -------------------------------------------------------------------- + + +def _controller(legs=None, arms=None, estimator=None, **overrides): + cfg = _cfg() + cfg.update(overrides) + return MarlController(cfg, _kin(cfg), legs or ZeroNetwork(12), arms or ZeroNetwork(14), + estimator or ZeroNetwork(6)), cfg + + +def test_observation_sizes_and_layout(): + legs, arms = ConstNetwork(np.zeros(12)), ConstNetwork(np.zeros(14)) + ctrl, cfg = _controller(legs=legs, arms=arms) + state = _state(cfg) + ctrl.reset(state) + ctrl.set_velocity_command(np.array([0.5, 0.0, 0.0])) + ctrl.step(state) + lo, ao = legs.inputs[-1], arms.inputs[-1] + assert lo.shape == (96,) and ao.shape == (124,) + np.testing.assert_allclose(lo[:84], ao[:84]) # the shared ProprioCfg terms + np.testing.assert_allclose(lo[0:3], [0, 0, 9.81]) # accelerometer + np.testing.assert_allclose(lo[6:9], [0, 0, -1]) # projected gravity + np.testing.assert_allclose(lo[9:12], [0.5, 0, 0]) # velocity command + assert lo[12] == 0.0 # arm_goal + rest = ctrl.rest_pose + np.testing.assert_allclose(lo[13:16], rest[0, :3]) # left target position + np.testing.assert_allclose(lo[16:20], R.wxyz_to_xyzw(rest[0, 3:])) # xyzw + assert lo[27] == 0.0 # height_drop + np.testing.assert_allclose(lo[28:55], 0.0) # q - default at default + np.testing.assert_allclose(lo[82:84], [0.0, 0.0], atol=1e-12) # t = 0: sin(0), sin(pi) + np.testing.assert_allclose(ao[98:112].reshape(2, 7)[:, :3], rest[:, :3], atol=1e-9) # wrist poses = rest + np.testing.assert_allclose(ao[112:124], 0.0, atol=1e-9) # no error at rest + + +def test_gait_phase_zero_when_standing(): + legs = ConstNetwork(np.zeros(12)) + ctrl, cfg = _controller(legs=legs) + ctrl.reset(_state(cfg)) + ctrl.step(_state(cfg)) + np.testing.assert_allclose(legs.inputs[-1][82:84], 0.0) + + +def test_legs_action_mapping_and_last_action(): + action = np.full(12, 20.0) # beyond the clip + legs = ConstNetwork(action) + ctrl, cfg = _controller(legs=legs) + state = _state(cfg) + ctrl.reset(state) + np.testing.assert_allclose(ctrl.legs_last, 0.0) + cmd = ctrl.step(state) + default = np.array(cfg["default_joint_pos"]) + expected = np.minimum(default[:12] + 10.0 * 0.25, ctrl.upper[:12]) + np.testing.assert_allclose(cmd.q[:12], expected) + np.testing.assert_allclose(ctrl.legs_last, (expected - default[:12]) / 0.25) + assert cmd.q[12] == 0.0 + + +def test_arm_goal_modes_and_settle(): + ctrl, cfg = _controller() + state = _state(cfg) + ctrl.reset(state) + ctrl.set_velocity_command(np.array([0.5, 0.0, 0.0])) + goal = ctrl.rest_pose.copy() + goal[:, 2] -= 0.1 + ctrl.request_arm_goal(goal) + assert ctrl.mode == Mode.SETTLE_TO_ARM + np.testing.assert_allclose(ctrl.velocity_command(), 0.0) + for _ in range(int(round(0.75 / 0.02))): + ctrl.step(state) + assert ctrl.mode == Mode.ARM + np.testing.assert_allclose(ctrl.believed, goal) + ctrl.end_arm_goal() + assert ctrl.mode == Mode.SETTLE_TO_NAV + for _ in range(int(round(1.5 / 0.02))): + ctrl.step(state) + assert ctrl.mode == Mode.NAV + np.testing.assert_allclose(ctrl.velocity_command(), [0.5, 0, 0]) + + +def test_arms_track_goal_with_pure_ik(): + """Zero residual: stepping the joints to the targets the IK gives walks the wrists onto the goal.""" + ctrl, cfg = _controller(settle_to_arm_s=0.0) + q = np.array(cfg["default_joint_pos"]) + ctrl.reset(_state(cfg, q)) + goal = ctrl.rest_pose.copy() + goal[0, :3] += [0.05, 0.05, -0.1] + goal[1, :3] += [0.05, -0.05, -0.1] + ctrl.request_arm_goal(goal) + for _ in range(40): + cmd = ctrl.step(_state(cfg, q)) + q[13:] = cmd.q[13:] # perfect tracking + assert np.all(ctrl.wrist_errors() < 5e-3) + + +def test_odometry_moves_goal_against_pelvis_motion(): + vx = 0.5 # m/s forward, estimated + ctrl, cfg = _controller(estimator=ConstNetwork([vx, 0, 0, 0, 0, 0]), settle_to_arm_s=0.0) + state = _state(cfg) + ctrl.reset(state) + goal = ctrl.rest_pose.copy() + ctrl.request_arm_goal(goal) + n = 10 + for _ in range(n + 1): # the first step has no previous action: no estimate + ctrl.step(state) + np.testing.assert_allclose(ctrl.believed[:, 0], goal[:, 0] - vx * 0.02 * n, atol=1e-9) + np.testing.assert_allclose(ctrl.believed[:, 1:3], goal[:, 1:3], atol=1e-9) + + +def test_estimator_input_layout(): + est = ConstNetwork(np.zeros(6)) + ctrl, cfg = _controller(estimator=est) + states = [] + for k in range(6): + s = _state(cfg) + s.acc = np.array([k, 0.0, 0.0]) + s.tau = np.full(27, float(k)) + states.append(s) + ctrl.reset(states[0]) + for s in states: + ctrl.step(s) + x = est.inputs[-1] # after the 6th frame: window frames 2..5 + assert x.shape == (326,) + np.testing.assert_allclose(x[0:12:3], [2, 3, 4, 5]) # acc x, oldest first, term-major + np.testing.assert_allclose(x[252:300].reshape(4, 12)[:, 0], [2, 3, 4, 5]) # leg torques + first = est.inputs[0] # the 2nd step: window = frame 0 repeated, then frame 1 + np.testing.assert_allclose(first[0:12:3], [0, 0, 0, 1]) From 7c6b84f151c9fa0c99cb6cbaa1c3af810fdc891f Mon Sep 17 00:00:00 2001 From: max Date: Wed, 7 Oct 2026 00:40:06 -0600 Subject: [PATCH 011/225] locomotion_game_deploy: run the Golem2 policy (IK3 odometry and filter, safety-layer bounds) and a game commander The controller takes the deploy features of LocoManip-Marl-Flat-Golem2-Direct-v0 from its yaml (policies/marl_golem), each off by default so marl_ik behaves as before: - odometry "legs": the arm command moves by the IMU attitude (taken back to the pelvis through the torso joint) and the planted feet's kinematics; a foot counts as planted within stance_height of the lowest one, since the robot has no foot force sensor; - arms.residual_cutoff_hz: the 3 Hz one-pole low-pass on the residual, which the arms observe; - target_margin -0.001: targets inside h12_safety_layer's clip; - torque_headroom: targets kept where kp (target - q) - kd qd is within 85% of effort_limits. Checked against the training env step by step (Locomanipulation_game scripts/skrl/deploy_parity.py): observations and networks identical, joint targets within 1.3e-4 rad, leg odometry 2.4 mm/s median error. game_commander_node drives marl_deploy_node with training's schedule: walking segments uniform in the training ranges for 4-8 s and arm goals sampled by the training command (arm_goals__.npz), half each. A lidar obstacle guard and a SLAM geofence keep walking off the furniture; events and a 10 Hz status line go to a JSON-lines log. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01DuoTaYHKcJbW1w79benm8q --- .../locomotion_game_deploy/controller.py | 92 ++++- .../game_commander_node.py | 389 ++++++++++++++++++ .../locomotion_game_deploy/kinematics.py | 18 +- .../marl_deploy_node.py | 8 +- .../src/locomotion_game_deploy/package.xml | 4 + .../policies/marl_golem/.gitignore | 4 + .../policies/marl_golem/marl_golem.yaml | 103 +++++ core_ws/src/locomotion_game_deploy/setup.py | 3 + .../test/test_controller.py | 107 +++++ 9 files changed, 714 insertions(+), 14 deletions(-) create mode 100644 core_ws/src/locomotion_game_deploy/locomotion_game_deploy/game_commander_node.py create mode 100644 core_ws/src/locomotion_game_deploy/policies/marl_golem/.gitignore create mode 100644 core_ws/src/locomotion_game_deploy/policies/marl_golem/marl_golem.yaml diff --git a/core_ws/src/locomotion_game_deploy/locomotion_game_deploy/controller.py b/core_ws/src/locomotion_game_deploy/locomotion_game_deploy/controller.py index f36775f..7edc4e7 100644 --- a/core_ws/src/locomotion_game_deploy/locomotion_game_deploy/controller.py +++ b/core_ws/src/locomotion_game_deploy/locomotion_game_deploy/controller.py @@ -3,13 +3,28 @@ One step() is one env step of LocoManip-Marl-Flat-IK-Direct-v0, in the env's order (locomanip_marl_env.py _get_observations, then _pre_physics_step): - 1. pelvis odometry: estimate how the pelvis moved over the last step from - the estimator's history window and the action applied, and move the arm - command by it (ArmTargetsCommand.apply_pelvis_motion); + 1. pelvis odometry: estimate how the pelvis moved over the last step and + move the arm command by it (ArmTargetsCommand.apply_pelvis_motion); 2. commands: advance the settle timer, switch mode; 3. observations: the legs' (96) and the arms' (124); 4. actions: clamp to +-clip, legs = scale * a + default, arms = q + IK step - + residual_scale * a, both bounded to the hard limits +- target_margin. + + residual_scale * r, both bounded to the hard limits +- target_margin + (negative: inside them). r is the arms' action, low-passed when + arms.residual_cutoff_hz is set (mdp.IKResidualArmAction), and is what the + arms observe as their last action. With torque_headroom set, each target + is then kept where the PD torque it asks for at the measured state, + kp (target - q) - kd qd, is within torque_headroom x effort_limits + (mdp.actions.torque_bounds). + +Odometry is cfg["odometry"]: + learned the pelvis-motion estimator on its history window and the action; + legs rotation from the IMU attitude (taken back to the pelvis through + the torso joint when the IMU sits on the torso), translation from + the planted feet: a foot that stays put moves in the pelvis frame + by the pelvis's motion, p(t-1) - dR p(t) (LocoManipMarlEnv. + _leg_odometry). Training called a foot planted from its contact + force; with no foot force sensor, a foot counts as planted here when + it is within stance_height (m, gravity axis) of the lowest foot. Modes follow the training command schedule: navigation (velocity command live, arms holding the rest pose) or an arm goal (velocity zero, one wrist @@ -28,6 +43,9 @@ from locomotion_game_deploy.rotations import ( compute_pose_error, projected_gravity, + quat_apply, + quat_conjugate, + quat_mul, quat_from_rotvec, quat_unique, subtract_frame_transforms, @@ -92,6 +110,20 @@ def __init__(self, cfg: dict, kinematics: ArmKinematics, legs, arms, estimator): self.settle_to_arm_s = float(cfg["settle_to_arm_s"]) self.settle_to_nav_s = float(cfg["settle_to_nav_s"]) self.num_arms = len(cfg["arms"]["ee_frames"]) + self.torque_headroom = cfg.get("torque_headroom") + if self.torque_headroom is not None: + self.effort_limits = np.asarray(cfg["effort_limits"], dtype=np.float64) + cutoff = cfg["arms"].get("residual_cutoff_hz") + self.filter_alpha = 1.0 if cutoff is None else 1.0 - np.exp(-2.0 * np.pi * float(cutoff) * self.dt) + self.odometry = cfg.get("odometry", "learned") + if self.odometry not in ("learned", "legs"): + raise ValueError(f"odometry must be 'learned' or 'legs', not {self.odometry!r}") + legs_odom = cfg.get("legs_odometry", {}) + self.foot_frames = legs_odom.get("foot_frames", []) + self.imu_frame = legs_odom.get("imu_frame", "pelvis") + self.stance_height = float(legs_odom.get("stance_height", 0.02)) + if self.odometry == "legs" and not self.foot_frames: + raise ValueError("odometry 'legs' needs legs_odometry.foot_frames") if len(self.default) != NUM_JOINTS or len(self.kp) != NUM_JOINTS or len(self.kd) != NUM_JOINTS: raise ValueError("default_joint_pos, kp and kd need one entry per policy joint (27)") if cfg.get("rest_pose") is not None: @@ -117,6 +149,9 @@ def reset(self, state: RobotState | None) -> None: q_legs = state.q[:NUM_LEGS] if state is not None else self.default[:NUM_LEGS] self.legs_last = (q_legs - self.default[:NUM_LEGS]) / self.legs_scale self.arms_last = np.zeros(NUM_ARM_JOINTS) + self.residual = np.zeros(NUM_ARM_JOINTS) # the arms' action after the low-pass + self._prev_pelvis_quat: np.ndarray | None = None + self._prev_feet: np.ndarray | None = None self.last_obs: dict[str, np.ndarray] = {} # -- operator commands ------------------------------------------------------- @@ -150,9 +185,9 @@ def end_arm_goal(self) -> None: # -- the env step ---------------------------------------------------------------- def step(self, state: RobotState) -> JointCommand: + self.kin.update(state.q) self._update_odometry(state) self._update_mode() - self.kin.update(state.q) common = self._common_obs(state) legs_obs = np.concatenate([common, self.legs_last]) @@ -164,17 +199,19 @@ def step(self, state: RobotState) -> JointCommand: target = self.default.copy() legs_target = legs_action * self.legs_scale + self.default[:NUM_LEGS] - target[:NUM_LEGS] = self._bound(legs_target, slice(0, NUM_LEGS)) + target[:NUM_LEGS] = self._torque_bound(self._bound(legs_target, slice(0, NUM_LEGS)), state, slice(0, NUM_LEGS)) ik = state.q[ARM_START:].copy() for arm in range(self.num_arms): cols = self.kin.arm_cols[arm] - ARM_START ik[cols] += self.kin.ik_step(arm, self.believed[arm, :3], self.believed[arm, 3:], self.ik_damping, self.max_ik_step) - target[ARM_START:] = self._bound(ik + arms_action * self.arms_scale, slice(ARM_START, NUM_JOINTS)) + self.residual = self.residual + self.filter_alpha * (arms_action - self.residual) + arms = slice(ARM_START, NUM_JOINTS) + target[arms] = self._torque_bound(self._bound(ik + self.residual * self.arms_scale, arms), state, arms) # what each agent observes of its action next step (mdp.applied_action) self.legs_last = (target[:NUM_LEGS] - self.default[:NUM_LEGS]) / self.legs_scale - self.arms_last = arms_action + self.arms_last = self.residual.copy() self._prev_action = np.concatenate([legs_action, arms_action]) self.steps += 1 return JointCommand(q=target, kp=self.kp.copy(), kd=self.kd.copy()) @@ -183,6 +220,14 @@ def step(self, state: RobotState) -> JointCommand: def _bound(self, target: np.ndarray, joints: slice) -> np.ndarray: return np.minimum(np.maximum(target, self.lower[joints]), self.upper[joints]) + def _torque_bound(self, target: np.ndarray, state: RobotState, joints: slice) -> np.ndarray: + if self.torque_headroom is None: + return target + q, dq = state.q[joints], state.dq[joints] + kp, kd = self.kp[joints], self.kd[joints] + tau = self.torque_headroom * self.effort_limits[joints] + return np.minimum(np.maximum(target, q + (kd * dq - tau) / kp), q + (kd * dq + tau) / kp) + def _start_arm_goal(self, goal: np.ndarray) -> None: self.believed = goal self.pending_goal = None @@ -244,6 +289,11 @@ def wrist_errors(self) -> np.ndarray: return np.linalg.norm(self._wrist_errors().reshape(self.num_arms, 6)[:, :3], axis=1) def _update_odometry(self, state: RobotState) -> None: + if self.odometry == "legs": + motion = self._leg_odometry(state) + if motion is not None: + self._move_goal(*motion) + return frame = np.concatenate([ state.acc, state.gyro, projected_gravity(state.quat), state.q - self.default, state.dq, state.tau[:NUM_LEGS], @@ -255,10 +305,32 @@ def _update_odometry(self, state: RobotState) -> None: if self._prev_action is None: return motion = self.estimator(np.concatenate([self._history_input(), self._prev_action])) + self._move_goal(motion[:3] * self.dt, quat_from_rotvec(motion[3:] * self.dt)) + + def _pelvis_quat(self, state: RobotState) -> np.ndarray: + """The pelvis's world orientation from the IMU's: through the torso joint when the IMU is on the torso.""" + if self.imu_frame == "pelvis": + return state.quat + _, quat_imu_in_pelvis = self.kin.frame_pose(self.imu_frame) + return quat_mul(state.quat, quat_conjugate(quat_imu_in_pelvis)) + + def _leg_odometry(self, state: RobotState) -> tuple[np.ndarray, np.ndarray] | None: + """The pelvis's motion since the last step, in the previous pelvis frame: (delta_pos, delta_quat).""" + quat = self._pelvis_quat(state) + feet = np.stack([self.kin.frame_pose(f)[0] for f in self.foot_frames]) # pelvis frame + prev_quat, prev_feet = self._prev_pelvis_quat, self._prev_feet + self._prev_pelvis_quat, self._prev_feet = quat, feet + if prev_quat is None: + return None + delta_quat = quat_mul(quat_conjugate(prev_quat), quat) + moved = np.stack([prev_feet[i] - quat_apply(delta_quat, feet[i]) for i in range(len(feet))]) + height = np.array([quat_apply(quat, foot)[2] for foot in feet]) # along gravity, from the pelvis + planted = height <= height.min() + self.stance_height + return moved[planted].mean(axis=0), delta_quat + + def _move_goal(self, delta_pos: np.ndarray, delta_quat: np.ndarray) -> None: if self.mode not in (Mode.ARM, Mode.SETTLE_TO_ARM): return - delta_pos = motion[:3] * self.dt - delta_quat = quat_from_rotvec(motion[3:] * self.dt) # a world-fixed target: T_prev o c_prev = T_new o c_new, so c_new = delta^-1 o c_prev poses = self.believed if self.mode == Mode.ARM else self.pending_goal for arm in range(self.num_arms): diff --git a/core_ws/src/locomotion_game_deploy/locomotion_game_deploy/game_commander_node.py b/core_ws/src/locomotion_game_deploy/locomotion_game_deploy/game_commander_node.py new file mode 100644 index 0000000..242cab3 --- /dev/null +++ b/core_ws/src/locomotion_game_deploy/locomotion_game_deploy/game_commander_node.py @@ -0,0 +1,389 @@ +"""Game commander: drives marl_deploy_node with the command schedule the policy was trained on. + +Training (Locomanipulation_game, ArmTargetsCommand) alternates two kinds of event, each drawn on its own: + walking a velocity command (vx, vy, wz) uniform in the training ranges (5% standing still), held for + nav_time_range seconds; + arm goal one wrist pose per arm from the run's target tables, held arm_goal_s seconds; half are standing + goals, half are built in a squat (arm_goals__.npz, exported by Locomanipulation_game + scripts/skrl/export_arm_goals.py); +with arm_goal_prob the chance of an arm goal. marl_deploy_node settles between the two (0.75 s before a reach, +1.5 s before walking) as training does; an arm goal that follows an arm goal replaces it without a settle. + +The training world was an open floor; a kitchen isn't. Two guards keep the schedule from walking the robot into +the furniture, and both only shorten or redirect walking: + obstacles /livox/pointcloud in the pelvis frame: a walking segment ends early when a point between knee and + head height lies within obstacle_distance of the pelvis in the direction of travel, and a new + segment is drawn from the directions that are clear; an arm goal waits for clear_reach in front, + turning in place until it is. + geofence with SLAM running, /Odometry (FAST-LIO, camera_init -> body): outside geofence_radius of the start, + walking segments head back toward it. + +mode "sequence" runs one scripted pass instead: walk 6 s at 0.5 m/s, a standing reach, a crouch reach, walk 6 s. + +Every event and a status line at 10 Hz (state, commands, wrist errors, safety heartbeat, SLAM pose) go to +log_file as JSON lines, for evaluation. + +Interfaces +---------- +sub /marl_deploy_node/state (std_msgs/String) +sub /marl_deploy_node/wrist_error (std_msgs/Float32MultiArray) m, left and right +sub /safety/heartbeat (std_msgs/Bool) +sub /livox/pointcloud (sensor_msgs/PointCloud2) +sub /Odometry (nav_msgs/Odometry) +pub /cmd_vel (geometry_msgs/Twist) +pub /marl_deploy_node/arm_goal (geometry_msgs/PoseArray) +cli /marl_deploy_node/end_arm_goal (std_srvs/Trigger) +""" + +import json +import math +import os +import time + +import numpy as np +import rclpy +from geometry_msgs.msg import Pose, PoseArray, Twist +from nav_msgs.msg import Odometry +from rclpy.node import Node +from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy +from rclpy.time import Time +from sensor_msgs.msg import PointCloud2 +from sensor_msgs_py import point_cloud2 +from std_msgs.msg import Bool, Float32MultiArray, String +from std_srvs.srv import Trigger +from tf2_ros import Buffer, TransformListener + +from locomotion_game_deploy.rotations import quat_apply + +DRIVING_STATES = ("nav", "settle_to_arm", "arm_goal", "settle_to_nav") +REACH_TOL = 0.05 # m: both wrists this close for REACH_HOLD_S counts a goal reached (training: 5 cm, 1 s) +REACH_HOLD_S = 1.0 + + +class GameCommanderNode(Node): + def __init__(self): + super().__init__("game_commander") + self.declare_parameter("mode", "training") # training | sequence + self.declare_parameter("goals_file", "") + self.declare_parameter("seed", 0) + self.declare_parameter("arm_goal_prob", 0.5) + self.declare_parameter("nav_time_range", [4.0, 8.0]) + self.declare_parameter("arm_goal_s", 4.0) + self.declare_parameter("standing_prob", 0.05) + self.declare_parameter("lin_vel_x", [-0.7, 0.7]) + self.declare_parameter("lin_vel_y", [-0.3, 0.3]) + self.declare_parameter("ang_vel_z", [-0.5, 0.5]) + self.declare_parameter("obstacle_distance", 0.6) # m from the pelvis, direction of travel + self.declare_parameter("clear_reach", 0.8) # m in front before an arm goal starts + self.declare_parameter("geofence_radius", 1.5) # m from the start; 0 disables + # the frame the cloud's points are in, when its header names a frame the TF tree lacks + # (RoboCasa stamps "lidar_link"; the sensor sits at the URDF's livox_link) + self.declare_parameter("cloud_frame", "") + self.declare_parameter("duration_s", 0.0) # stop commanding after this (sim time); 0: forever + self.declare_parameter("log_file", "") + + self.rng = np.random.default_rng(int(self.get_parameter("seed").value)) + self.mode = str(self.get_parameter("mode").value) + goals_file = str(self.get_parameter("goals_file").value) + if not goals_file or not os.path.isfile(goals_file): + raise FileNotFoundError(f"goals_file {goals_file!r} not found") + goals = np.load(goals_file) + self.goal_poses, self.goal_low = goals["poses"], goals["low"].astype(bool) + self.goal_crouch = goals["needs_crouch"].astype(bool) + self.ranges = np.array([self.get_parameter(k).value for k in ("lin_vel_x", "lin_vel_y", "ang_vel_z")], dtype=float) + self.log_path = str(self.get_parameter("log_file").value) + self.log = open(self.log_path, "a", buffering=1) if self.log_path else None + + self.deploy_state = "" + self.wrist_error = np.full(2, np.nan) + self.heartbeat: bool | None = None + self.points: np.ndarray | None = None # (N, 3) pelvis frame, obstacle band only + self.cloud_count = {"received": 0, "tf_failed": 0} + self.odom: np.ndarray | None = None # (x, y, yaw) in camera_init + self.odom_start: np.ndarray | None = None + self.cmd = np.zeros(3) + self.event: dict | None = None + self.event_end = 0.0 + self.start_time: float | None = None + self.reach_since: float | None = None + self.reached = False + self.events_done = 0 + self.script: list[dict] = [] + self.finished = False + + latched = QoSProfile(reliability=ReliabilityPolicy.RELIABLE, durability=DurabilityPolicy.TRANSIENT_LOCAL, + history=HistoryPolicy.KEEP_LAST, depth=1) + sensor = QoSProfile(reliability=ReliabilityPolicy.BEST_EFFORT, history=HistoryPolicy.KEEP_LAST, depth=1) + self.create_subscription(String, "/marl_deploy_node/state", self._on_state, latched) + self.create_subscription(Float32MultiArray, "/marl_deploy_node/wrist_error", self._on_wrist_error, 10) + self.create_subscription(Bool, "/safety/heartbeat", self._on_heartbeat, 10) + self.create_subscription(PointCloud2, "/livox/pointcloud", self._on_cloud, sensor) + self.create_subscription(Odometry, "/Odometry", self._on_odom, 10) + self.cmd_pub = self.create_publisher(Twist, "/cmd_vel", 10) + self.goal_pub = self.create_publisher(PoseArray, "/marl_deploy_node/arm_goal", 10) + self.end_cli = self.create_client(Trigger, "/marl_deploy_node/end_arm_goal") + self.tf_buffer = Buffer() + self.tf_listener = TransformListener(self.tf_buffer, self) + self.create_timer(0.05, self._tick) + self.create_timer(0.1, self._status) + self.get_logger().info(f"game_commander: mode {self.mode}, {len(self.goal_poses)} goals from {goals_file}") + + # -- inputs ------------------------------------------------------------------------- + def _on_state(self, msg: String) -> None: + if msg.data != self.deploy_state: + self._write({"type": "deploy_state", "state": msg.data}) + self.deploy_state = msg.data + + def _on_wrist_error(self, msg: Float32MultiArray) -> None: + self.wrist_error = np.asarray(msg.data, dtype=float) + + def _on_heartbeat(self, msg: Bool) -> None: + if self.heartbeat is not False and not msg.data: + self._write({"type": "estop", "detail": "safety heartbeat False"}) + self.heartbeat = bool(msg.data) + + def _on_cloud(self, msg: PointCloud2) -> None: + self.cloud_count["received"] += 1 + try: + frame = str(self.get_parameter("cloud_frame").value) or msg.header.frame_id + tf = self.tf_buffer.lookup_transform("pelvis", frame, Time()) + except Exception as error: # noqa: BLE001 - TF not up yet + self.cloud_count["tf_failed"] += 1 + self.get_logger().warn(f"cloud TF: {error}", throttle_duration_sec=10.0) + return + cloud = point_cloud2.read_points(msg, field_names=("x", "y", "z"), skip_nans=True) + if len(cloud) == 0: + return + if getattr(cloud, "dtype", None) is not None and cloud.dtype.names: + xyz = np.stack([cloud["x"], cloud["y"], cloud["z"]], axis=-1) + else: + xyz = np.asarray(list(cloud), dtype=np.float64).reshape(-1, 3) + r = tf.transform.rotation + t = tf.transform.translation + quat = np.array([r.w, r.x, r.y, r.z]) + rot = _quat_matrix(quat) + pts = xyz.astype(np.float64) @ rot.T + np.array([t.x, t.y, t.z]) + # between knee and head height in the pelvis frame (pelvis ~1 m up), beyond the robot's own body + band = (pts[:, 2] > -0.6) & (pts[:, 2] < 0.8) & (np.hypot(pts[:, 0], pts[:, 1]) > 0.35) + self.points = pts[band] + + def _on_odom(self, msg: Odometry) -> None: + p, q = msg.pose.pose.position, msg.pose.pose.orientation + yaw = math.atan2(2.0 * (q.w * q.z + q.x * q.y), 1.0 - 2.0 * (q.y * q.y + q.z * q.z)) + self.odom = np.array([p.x, p.y, yaw]) + if self.odom_start is None: + self.odom_start = self.odom.copy() + + # -- helpers ------------------------------------------------------------------------- + def _now(self) -> float: + return self.get_clock().now().nanoseconds * 1e-9 + + def _write(self, record: dict) -> None: + record = {"t": round(self._now(), 3), "wall": round(time.time(), 3), **record} + if self.log: + self.log.write(json.dumps(record) + "\n") + if record["type"] not in ("status",): + self.get_logger().info(json.dumps(record)) + + def _clearance(self, direction: float, half_width: float = 0.35) -> float: + """Free distance (m) from the pelvis along a pelvis-frame heading, in a corridor half_width wide.""" + if self.points is None or len(self.points) == 0: + return math.inf + c, s = math.cos(direction), math.sin(direction) + along = self.points[:, 0] * c + self.points[:, 1] * s + across = -self.points[:, 0] * s + self.points[:, 1] * c + ahead = (along > 0.0) & (np.abs(across) < half_width) + return float(along[ahead].min()) if ahead.any() else math.inf + + def _outside_fence(self) -> np.ndarray | None: + """The direction home (pelvis frame, rad) when SLAM puts the robot outside the geofence, else None.""" + radius = float(self.get_parameter("geofence_radius").value) + if radius <= 0.0 or self.odom is None or self.odom_start is None: + return None + d = self.odom_start[:2] - self.odom[:2] + if np.hypot(*d) < radius: + return None + return math.atan2(d[1], d[0]) - self.odom[2] + + def _draw_velocity(self) -> np.ndarray: + home = self._outside_fence() + if home is not None: + return np.array([0.4 * math.cos(home), 0.25 * math.sin(home), 0.0]) + if self.rng.random() < float(self.get_parameter("standing_prob").value): + return np.zeros(3) + for _ in range(20): + cmd = self.rng.uniform(self.ranges[:, 0], self.ranges[:, 1]) + if np.hypot(cmd[0], cmd[1]) < 1e-3 or \ + self._clearance(math.atan2(cmd[1], cmd[0])) > float(self.get_parameter("obstacle_distance").value) + 0.5: + return cmd + return np.array([0.0, 0.0, self.ranges[2, 1] * (1 if self.rng.random() < 0.5 else -1)]) # boxed in: turn + + def _draw_goal(self, kind: str | None = None) -> int: + if kind == "standing": + pool = np.flatnonzero(~self.goal_low & (self.goal_poses[:, :, 0].min(axis=1) > 0.25)) + elif kind == "crouch": + pool = np.flatnonzero(self.goal_low & self.goal_crouch) + else: + pool = np.arange(len(self.goal_poses)) + return int(self.rng.choice(pool)) + + # -- events ----------------------------------------------------------------------------- + def _start_walk(self, cmd: np.ndarray | None = None, duration: float | None = None) -> None: + if self.event is not None and self.event["kind"] == "arm_goal": + self._finish_goal() + self.end_cli.call_async(Trigger.Request()) + lo, hi = self.get_parameter("nav_time_range").value + self.cmd = self._draw_velocity() if cmd is None else np.asarray(cmd, dtype=float) + duration = float(self.rng.uniform(lo, hi)) if duration is None else duration + self.event = {"kind": "walk", "index": self.events_done, "cmd": self.cmd.round(3).tolist(), "duration": round(duration, 2)} + self.event_end = self._now() + duration + self._write({"type": "event", **self.event}) + + def _start_goal(self, index: int, duration: float | None = None) -> None: + if self.event is not None and self.event["kind"] == "arm_goal": + self._finish_goal() + self.cmd = np.zeros(3) + poses = self.goal_poses[index] + msg = PoseArray() + msg.header.frame_id = "pelvis" + for p in poses: + pose = Pose() + pose.position.x, pose.position.y, pose.position.z = (float(v) for v in p[:3]) + pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z = (float(v) for v in p[3:]) + msg.poses.append(pose) + self.goal_pub.publish(msg) + duration = float(self.get_parameter("arm_goal_s").value) if duration is None else duration + # the deploy node settles 0.75 s first when the robot was walking + settle = 0.75 if self.event is None or self.event["kind"] != "arm_goal" else 0.0 + self.event = {"kind": "arm_goal", "index": self.events_done, "goal": index, "low": bool(self.goal_low[index]), + "needs_crouch": bool(self.goal_crouch[index]), "poses": poses.round(4).tolist(), + "duration": duration, "settle": settle, "best_error": math.inf} + self.event_end = self._now() + settle + duration + self.reach_since, self.reached = None, False + self._write({"type": "event", **{k: v for k, v in self.event.items() if k != "best_error"}}) + + def _finish_goal(self) -> None: + ev = self.event + self._write({"type": "goal_result", "index": ev["index"], "goal": ev["goal"], "low": ev["low"], + "needs_crouch": ev["needs_crouch"], "reached": self.reached, + "best_error": None if math.isinf(ev["best_error"]) else round(ev["best_error"], 4)}) + + def _next_event(self) -> None: + self.events_done += 1 + if self.mode == "sequence": + if not self.script: + self.finished = True + self._write({"type": "done", "events": self.events_done}) + if self.event is not None and self.event["kind"] == "arm_goal": + self._finish_goal() + self.end_cli.call_async(Trigger.Request()) + self.event, self.cmd = None, np.zeros(3) + return + step = self.script.pop(0) + if step["kind"] == "walk": + self._start_walk(step["cmd"], step["duration"]) + else: + self._start_goal(self._draw_goal(step["goal"]), step["duration"]) + return + if self.rng.random() < float(self.get_parameter("arm_goal_prob").value): + if self._clearance(0.0, 0.45) < float(self.get_parameter("clear_reach").value): + self._start_walk(np.array([0.0, 0.0, self.ranges[2, 1]]), 2.0) # face open space first + else: + self._start_goal(self._draw_goal()) + else: + self._start_walk() + + # -- loop -------------------------------------------------------------------------------- + def _tick(self) -> None: + now = self._now() + if not self.finished and (self.deploy_state.startswith("fault") or self.heartbeat is False): + # an e-stop or a latched deploy node ends the run: the robot is limp from here + self.finished = True + if self.event is not None and self.event["kind"] == "arm_goal": + self._finish_goal() + self.event, self.cmd = None, np.zeros(3) + self._write({"type": "done", "events": self.events_done, "reason": self.deploy_state or "estop"}) + if self.finished or self.deploy_state not in DRIVING_STATES: + self.cmd_pub.publish(Twist()) + return + if self.start_time is None: + # the obstacle guard needs the cloud in the pelvis frame (TF comes up a few seconds in) + self.waiting_since = now if getattr(self, "waiting_since", None) is None else self.waiting_since + if self.points is None and now - self.waiting_since < 20.0: + self.cmd_pub.publish(Twist()) + return + if self.points is None: + self._write({"type": "warning", "detail": "no obstacle input after 20 s; starting without it"}) + self.start_time = now + if self.mode == "sequence": + self.script = [{"kind": "walk", "cmd": [0.5, 0.0, 0.0], "duration": 6.0}, + {"kind": "goal", "goal": "standing", "duration": 6.0}, + {"kind": "goal", "goal": "crouch", "duration": 6.0}, + {"kind": "walk", "cmd": [0.5, 0.0, 0.0], "duration": 6.0}] + self._write({"type": "start", "mode": self.mode}) + self.events_done = -1 + self._next_event() + duration = float(self.get_parameter("duration_s").value) + if not self.finished and duration > 0.0 and now - self.start_time > duration: + self.finished = True + if self.event is not None and self.event["kind"] == "arm_goal": + self._finish_goal() + self.end_cli.call_async(Trigger.Request()) + self.event, self.cmd = None, np.zeros(3) + self._write({"type": "done", "events": self.events_done}) + if self.finished: + self.cmd_pub.publish(Twist()) + return + + ev = self.event + if ev["kind"] == "walk" and np.hypot(self.cmd[0], self.cmd[1]) > 0.05: + heading = math.atan2(self.cmd[1], self.cmd[0]) + if self._clearance(heading) < float(self.get_parameter("obstacle_distance").value): + self._write({"type": "obstacle_stop", "heading": round(heading, 2)}) + self._next_event() + ev = self.event + if ev is not None and ev["kind"] == "arm_goal" and self.deploy_state == "arm_goal" and np.isfinite(self.wrist_error).all(): + err = float(self.wrist_error.max()) + ev["best_error"] = min(ev["best_error"], err) + if err < REACH_TOL: + self.reach_since = now if self.reach_since is None else self.reach_since + self.reached |= now - self.reach_since >= REACH_HOLD_S + else: + self.reach_since = None + if ev is not None and now >= self.event_end: + self._next_event() + twist = Twist() + twist.linear.x, twist.linear.y, twist.angular.z = (float(v) for v in self.cmd) + self.cmd_pub.publish(twist) + + def _status(self) -> None: + self._write({"type": "status", "state": self.deploy_state, "cmd": self.cmd.round(3).tolist(), + "event": None if self.event is None else self.event["kind"], + "wrist_error": [None if not np.isfinite(e) else round(float(e), 4) for e in self.wrist_error], + "heartbeat": self.heartbeat, + "odom": None if self.odom is None else self.odom.round(3).tolist(), + "front_clearance": round(min(self._clearance(0.0), 9.9), 2), + "cloud": {**self.cloud_count, "band_points": 0 if self.points is None else len(self.points)}}) + + +def _quat_matrix(q: np.ndarray) -> np.ndarray: + return np.stack([quat_apply(q, e) for e in np.eye(3)], axis=1) + + +def main(): + rclpy.init() + node = GameCommanderNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + if node.log: + node.log.close() + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/core_ws/src/locomotion_game_deploy/locomotion_game_deploy/kinematics.py b/core_ws/src/locomotion_game_deploy/locomotion_game_deploy/kinematics.py index 0aaf649..4528c42 100644 --- a/core_ws/src/locomotion_game_deploy/locomotion_game_deploy/kinematics.py +++ b/core_ws/src/locomotion_game_deploy/locomotion_game_deploy/kinematics.py @@ -26,8 +26,10 @@ class ArmKinematics: def __init__(self, urdf_path: str, joint_names: list[str], ee_frames: list[str], - arm_joint_names: list[list[str]], pelvis_frame: str = "pelvis"): - """joint_names: the policy's joint order (Unitree motor order); q vectors use it.""" + arm_joint_names: list[list[str]], pelvis_frame: str = "pelvis", + extra_frames: list[str] | None = None): + """joint_names: the policy's joint order (Unitree motor order); q vectors use it. + extra_frames: more frames whose pelvis-frame pose update() computes (frame_pose), e.g. the feet.""" self.model = pin.buildModelFromUrdf(urdf_path) self.data = self.model.createData() self._q_pin = pin.neutral(self.model) @@ -35,7 +37,10 @@ def __init__(self, urdf_path: str, joint_names: list[str], ee_frames: list[str], self._q_idx = np.array([self.model.joints[self.model.getJointId(n)].idx_q for n in joint_names]) self._pelvis_id = self.model.getFrameId(pelvis_frame) self._ee_ids = [self.model.getFrameId(f) for f in ee_frames] - for name, fid in zip([pelvis_frame] + ee_frames, [self._pelvis_id] + self._ee_ids): + self._extra_ids = {f: self.model.getFrameId(f) for f in (extra_frames or [])} + self._extra_poses: dict[str, tuple[np.ndarray, np.ndarray]] = {} + for name, fid in zip([pelvis_frame] + ee_frames + list(self._extra_ids), + [self._pelvis_id] + self._ee_ids + list(self._extra_ids.values())): if fid >= len(self.model.frames): raise ValueError(f"frame {name!r} not in {urdf_path}") # per arm: columns in the policy's joint order, and in pinocchio's velocity vector @@ -67,12 +72,19 @@ def update(self, q: np.ndarray) -> None: r = o_m_ee.rotation @ self._jac_points[arm] lin = jac[:3] - _skew(r) @ jac[3:] self._jacobians.append(np.vstack([r_pelvis_t @ lin, r_pelvis_t @ jac[3:]])) + for name, fid in self._extra_ids.items(): + pelvis_m_f = o_m_pelvis.actInv(self.data.oMf[fid]) + self._extra_poses[name] = (pelvis_m_f.translation.copy(), quat_from_matrix(pelvis_m_f.rotation)) def wrist_pose(self, arm: int) -> tuple[np.ndarray, np.ndarray]: """(position, quaternion wxyz with qw >= 0) of the arm's wrist frame in the pelvis frame.""" pos, quat = self._poses[arm] return pos, quat_unique(quat) + def frame_pose(self, name: str) -> tuple[np.ndarray, np.ndarray]: + """(position, quaternion wxyz) of an extra frame in the pelvis frame, at the last update().""" + return self._extra_poses[name] + def ik_step(self, arm: int, target_pos: np.ndarray, target_quat: np.ndarray, damping: float, max_step: float) -> np.ndarray: """The capped DLS joint step for one arm toward a pelvis-frame wrist pose, in its arm_cols order.""" diff --git a/core_ws/src/locomotion_game_deploy/locomotion_game_deploy/marl_deploy_node.py b/core_ws/src/locomotion_game_deploy/locomotion_game_deploy/marl_deploy_node.py index 333ba6c..1dabf8e 100644 --- a/core_ws/src/locomotion_game_deploy/locomotion_game_deploy/marl_deploy_node.py +++ b/core_ws/src/locomotion_game_deploy/locomotion_game_deploy/marl_deploy_node.py @@ -27,7 +27,7 @@ * /safety/heartbeat watchdog, as in h12_lowerbody_rl: armed on the first beat; a False beat (the safety layer's estop latched) or ``heartbeat_timeout`` of silence latches this node silent. -* /lowstate older than ``lowstate_timeout``: no policy step and no publish +* /lowstate older than ``lowstate_timeout`` (wall clock): no policy step and no publish that tick (stepping on a frozen state would integrate garbage). * A non-finite observation or target latches the node silent instead of sending it: the safety layer would latch its estop (all motors limp) on it. @@ -117,11 +117,17 @@ def __init__(self): raise ValueError(f"control_hz {control_hz} doesn't match the policy's control_dt {cfg['control_dt']}") torch.set_num_threads(1) + # leg odometry needs the feet, and the IMU's frame when the IMU isn't on the pelvis + legs_odom = cfg.get("legs_odometry", {}) if cfg.get("odometry") == "legs" else {} + extra_frames = list(legs_odom.get("foot_frames", [])) + if legs_odom.get("imu_frame", "pelvis") != "pelvis": + extra_frames.append(legs_odom["imu_frame"]) kin = ArmKinematics( self.get_parameter("urdf_path").value, joint_names=cfg["joint_names"], ee_frames=cfg["arms"]["ee_frames"], arm_joint_names=[cfg["joint_names"][ARM_START:ARM_START + 7], cfg["joint_names"][ARM_START + 7:]], + extra_frames=extra_frames, ) mode = str(self.get_parameter("policy_mode").value) if mode == "torch": diff --git a/core_ws/src/locomotion_game_deploy/package.xml b/core_ws/src/locomotion_game_deploy/package.xml index becceeb..4977ed1 100644 --- a/core_ws/src/locomotion_game_deploy/package.xml +++ b/core_ws/src/locomotion_game_deploy/package.xml @@ -18,6 +18,10 @@ python3-numpy python3-yaml pinocchio + nav_msgs + sensor_msgs + sensor_msgs_py + tf2_ros python3-pytest diff --git a/core_ws/src/locomotion_game_deploy/policies/marl_golem/.gitignore b/core_ws/src/locomotion_game_deploy/policies/marl_golem/.gitignore new file mode 100644 index 0000000..a343a30 --- /dev/null +++ b/core_ws/src/locomotion_game_deploy/policies/marl_golem/.gitignore @@ -0,0 +1,4 @@ +# Written by Locomanipulation_game scripts/skrl/export_marl.py and export_arm_goals.py; see the package README. +*.pt +*.npz +export.yaml diff --git a/core_ws/src/locomotion_game_deploy/policies/marl_golem/marl_golem.yaml b/core_ws/src/locomotion_game_deploy/policies/marl_golem/marl_golem.yaml new file mode 100644 index 0000000..267a219 --- /dev/null +++ b/core_ws/src/locomotion_game_deploy/policies/marl_golem/marl_golem.yaml @@ -0,0 +1,103 @@ +# Deploy config for LocoManip-Marl-Flat-Golem2-Direct-v0 (Locomanipulation_game, marl-direct): the IK task with +# goals spread over the workspace and built in a real squat (IK2), arm commands held by leg kinematics and a +# 3 Hz low-pass on the arms' residual (IK3), and targets clipped as h12_safety_layer clips them, trained with +# its e-stops (90% velocity/torque, 0.02 rad position) ending the episode (Golem), and targets bounded so the +# PD torque they ask for stays within 85% of the effort limit (Golem2). +# Every value mirrors the training env; the source is named on each block. +# Locomanipulation_game scripts/skrl/export_marl.py writes the networks and export.yaml next to +# this file; export.yaml's keys (rest_pose, source) override the ones here. + +control_dt: 0.02 # decimation 4 x sim dt 0.005 (LocoManipMarlEnvCfg) + +# Policy joint order = Unitree motor order 0..26 (assets/h1_2.py ALL_JOINTS_NAMES) +joint_names: [ + left_hip_yaw_joint, left_hip_pitch_joint, left_hip_roll_joint, + left_knee_joint, left_ankle_pitch_joint, left_ankle_roll_joint, + right_hip_yaw_joint, right_hip_pitch_joint, right_hip_roll_joint, + right_knee_joint, right_ankle_pitch_joint, right_ankle_roll_joint, + torso_joint, + left_shoulder_pitch_joint, left_shoulder_roll_joint, left_shoulder_yaw_joint, + left_elbow_joint, left_wrist_roll_joint, left_wrist_pitch_joint, left_wrist_yaw_joint, + right_shoulder_pitch_joint, right_shoulder_roll_joint, right_shoulder_yaw_joint, + right_elbow_joint, right_wrist_roll_joint, right_wrist_pitch_joint, right_wrist_yaw_joint] + +# assets/h1_2.py _INIT_STATE (rad) +default_joint_pos: [ + 0.0, -0.3, 0.0, 0.5, -0.2, 0.0, + 0.0, -0.3, 0.0, 0.5, -0.2, 0.0, + 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + +# assets/h1_2.py _ACTUATORS: stiffness / damping (Nm/rad, Nm s/rad) +kp: [ + 200, 200, 200, 300, 40, 40, + 200, 200, 200, 300, 40, 40, + 300, + 120, 120, 120, 80, 40, 40, 40, + 120, 120, 120, 80, 40, 40, 40] +kd: [ + 2.5, 2.5, 2.5, 4.0, 2.0, 2.0, + 2.5, 2.5, 2.5, 4.0, 2.0, 2.0, + 3.0, + 2.0, 2.0, 2.0, 1.0, 1.0, 1.0, 1.0, + 2.0, 2.0, 2.0, 1.0, 1.0, 1.0, 1.0] + +# assets/h1_2.py _ACTUATORS effort_limit_sim (Nm) = the URDF limits h12_safety_layer e-stops at +effort_limits: [ + 200, 200, 200, 300, 60, 40, + 200, 200, 200, 300, 60, 40, + 200, + 40, 40, 18, 18, 19, 19, 19, + 40, 40, 18, 18, 19, 19, 19] +# Golem2: each target kept where kp (target - q) - kd qd is within this share of effort_limits +torque_headroom: 0.85 + +clip_actions: 10.0 # CLIP_ACTIONS: the env clamps every raw action to +-this +target_margin: -0.001 # rad: joint targets bounded to the URDF hard limits +- this; h12_safety_layer's clip.position_offset + +legs: + policy: legs.pt + num_obs: 96 + num_actions: 12 + action_scale: 0.25 # target = scale * action + default + +arms: + policy: arms.pt + num_obs: 124 + num_actions: 14 + residual_scale: 0.2 # target = q + IK step + scale * action + ee_frames: [left_wrist_yaw_link, right_wrist_yaw_link] + ik_damping: 0.05 + max_ik_step: 0.1 # rad per joint per policy step + residual_cutoff_hz: 3.0 # one-pole low-pass on the residual (IKResidualArmActionCfg.residual_cutoff_hz) + +# Arm commands move by the leg kinematics (LocoManipMarlFlatIK3EnvCfg estimator.odometry "legs") +odometry: legs +legs_odometry: + foot_frames: [left_ankle_roll_link, right_ankle_roll_link] # FOOT_LINK_NAMES + imu_frame: torso_link # training's imu_link: on torso_link, no rotation + stance_height: 0.02 # m: a foot this close to the lowest one (along gravity) counts as planted + +estimator: # trained alongside; unused with odometry legs + path: estimator.pt + history: 4 # ODOMETRY_HISTORY + num_inputs: 326 # 4 x (3 + 3 + 3 + 27 + 27 + 12) + 26 actions + +# Gait clock (manager_based mdp/rewards.py) +gait_period: 0.8 +zero_cmd_eps: 0.1 + +# Velocity command ranges (legs_r0_env_cfg.py CommandsCfg); /cmd_vel is clipped to them +command_ranges: + lin_vel_x: [-0.7, 0.7] + lin_vel_y: [-0.3, 0.3] + ang_vel_z: [-0.5, 0.5] + +# Zero-velocity settle at a mode switch (LocoManipMarlFlatCurriculumEnvCfg) +settle_to_arm_s: 0.75 +settle_to_nav_s: 1.5 + +# The navigation rest pose: wrist poses (x, y, z, qw, qx, qy, qz), pelvis frame, left then right, +# from the run's arm_target_tables.pt (default_poses). null: forward kinematics at the default pose. +rest_pose: null diff --git a/core_ws/src/locomotion_game_deploy/setup.py b/core_ws/src/locomotion_game_deploy/setup.py index c721ab2..d3530ae 100644 --- a/core_ws/src/locomotion_game_deploy/setup.py +++ b/core_ws/src/locomotion_game_deploy/setup.py @@ -16,6 +16,8 @@ # Exported policies + their deploy config, installed under share//policies//. (os.path.join('share', package_name, 'policies', 'marl_ik'), glob('policies/marl_ik/*')), + (os.path.join('share', package_name, 'policies', 'marl_golem'), + glob('policies/marl_golem/*')), ], install_requires=['setuptools'], zip_safe=True, @@ -31,6 +33,7 @@ entry_points={ 'console_scripts': [ 'marl_deploy_node = locomotion_game_deploy.marl_deploy_node:main', + 'game_commander_node = locomotion_game_deploy.game_commander_node:main', ], }, ) diff --git a/core_ws/src/locomotion_game_deploy/test/test_controller.py b/core_ws/src/locomotion_game_deploy/test/test_controller.py index 290b2e1..1a172b3 100644 --- a/core_ws/src/locomotion_game_deploy/test/test_controller.py +++ b/core_ws/src/locomotion_game_deploy/test/test_controller.py @@ -299,3 +299,110 @@ def test_estimator_input_layout(): np.testing.assert_allclose(x[252:300].reshape(4, 12)[:, 0], [2, 3, 4, 5]) # leg torques first = est.inputs[0] # the 2nd step: window = frame 0 repeated, then frame 1 np.testing.assert_allclose(first[0:12:3], [0, 0, 0, 1]) + + +# -- the Golem policy's deploy features ------------------------------------------------ + +GOLEM_CFG_PATH = os.path.join(HERE, "..", "policies", "marl_golem", "marl_golem.yaml") + + +def _golem_controller(legs=None, arms=None, **overrides): + with open(GOLEM_CFG_PATH) as f: + cfg = yaml.safe_load(f) + cfg.update(overrides) + names = cfg["joint_names"] + odom = cfg["legs_odometry"] + kin = ArmKinematics(URDF, names, cfg["arms"]["ee_frames"], [names[13:20], names[20:]], + extra_frames=odom["foot_frames"] + [odom["imu_frame"]]) + return MarlController(cfg, kin, legs or ZeroNetwork(12), arms or ZeroNetwork(14), ZeroNetwork(6)), cfg + + +def test_residual_low_pass_and_observed_action(): + action = np.full(14, 1.0) + arms = ConstNetwork(action) + ctrl, cfg = _golem_controller(arms=arms) + state = _state(cfg) + ctrl.reset(state) + alpha = 1.0 - np.exp(-2.0 * np.pi * 3.0 * 0.02) + ctrl.step(state) + np.testing.assert_allclose(ctrl.residual, alpha * action) + np.testing.assert_allclose(ctrl.arms_last, alpha * action) # the arms observe the filtered residual + ctrl.step(state) + np.testing.assert_allclose(ctrl.residual, (alpha + alpha * (1 - alpha)) * action) + np.testing.assert_allclose(arms.inputs[-1][84:98], alpha * action) + + +def test_golem_targets_inside_the_safety_clip(): + ctrl, cfg = _golem_controller(legs=ConstNetwork(np.full(12, 10.0)), arms=ConstNetwork(np.full(14, 10.0))) + state = _state(cfg) + ctrl.reset(state) + for _ in range(20): + cmd = ctrl.step(state) + limits = ctrl.kin.joint_limits + legs_arms = np.r_[0:12, 13:27] + assert np.all(cmd.q[legs_arms] <= limits[legs_arms, 1] - 0.001 + 1e-12) + assert np.all(cmd.q[legs_arms] >= limits[legs_arms, 0] + 0.001 - 1e-12) + + +def test_leg_odometry_moves_goal_against_pelvis_motion(): + """Feet fixed in the world while the pelvis walks forward: they slide back in the pelvis frame.""" + ctrl, cfg = _golem_controller(settle_to_arm_s=0.0) + state = _state(cfg) + ctrl.reset(state) + ctrl.kin.update(state.q) + feet0 = {f: ctrl.kin.frame_pose(f) for f in ctrl.foot_frames} + shift = {"x": 0.0} + real_update = ctrl.kin.update + + def update(q): + real_update(q) + for f, (pos, quat) in feet0.items(): + ctrl.kin._extra_poses[f] = (pos - np.array([shift["x"], 0.0, 0.0]), quat) + + ctrl.kin.update = update + goal = ctrl.rest_pose.copy() + ctrl.request_arm_goal(goal) + v, n = 0.5, 10 + ctrl.step(state) # the first step only records the feet + for _ in range(n): + shift["x"] += v * 0.02 + ctrl.step(state) + np.testing.assert_allclose(ctrl.believed[:, 0], goal[:, 0] - v * 0.02 * n, atol=1e-9) + np.testing.assert_allclose(ctrl.believed[:, 1:3], goal[:, 1:3], atol=1e-9) + + +def test_leg_odometry_ignores_a_lifted_foot(): + ctrl, cfg = _golem_controller(settle_to_arm_s=0.0) + state = _state(cfg) + ctrl.reset(state) + ctrl.kin.update(state.q) + left, right = ctrl.foot_frames + feet0 = {f: ctrl.kin.frame_pose(f) for f in ctrl.foot_frames} + t = {"k": 0} + real_update = ctrl.kin.update + + def update(q): + real_update(q) + pos, quat = feet0[left] + ctrl.kin._extra_poses[left] = (pos, quat) # planted + pos, quat = feet0[right] + ctrl.kin._extra_poses[right] = (pos + np.array([0.02 * t["k"], 0.0, 0.1]), quat) # swinging, lifted + + ctrl.kin.update = update + goal = ctrl.rest_pose.copy() + ctrl.request_arm_goal(goal) + for k in range(6): + t["k"] = k + ctrl.step(state) + np.testing.assert_allclose(ctrl.believed[:, :3], goal[:, :3], atol=1e-9) # the planted foot didn't move + + +def test_torque_headroom_bounds_the_pd_demand(): + ctrl, cfg = _golem_controller(legs=ConstNetwork(np.full(12, 10.0)), arms=ConstNetwork(np.full(14, -10.0))) + state = _state(cfg) + state.dq = RNG.normal(scale=2.0, size=27) + ctrl.reset(state) + cmd = ctrl.step(state) + joints = np.r_[0:12, 13:27] + demand = ctrl.kp[joints] * (cmd.q[joints] - state.q[joints]) - ctrl.kd[joints] * state.dq[joints] + assert np.all(np.abs(demand) <= 0.85 * ctrl.effort_limits[joints] + 1e-9) From ee4f7578a8936d35da26413224aed6839c32eec3 Mon Sep 17 00:00:00 2001 From: max Date: Wed, 7 Oct 2026 00:40:06 -0600 Subject: [PATCH 012/225] Add locomanipulationgame_bringup.launch.py: the loco-manipulation game in sim Starts the state publishers, the split safety layer with the real robot's preset (relax_safety_split, the e-stops the policy trained against), marl_deploy_node with the policies/marl_golem export, the game commander, and SLAM (FAST-LIO, body->pelvis mount, pointcloud_to_laserscan, slam_toolbox; no nav2, so the commander is the only /cmd_vel publisher). frame_task_server and the lower-body RL node stay off: marl_deploy_node owns both safety channels. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01DuoTaYHKcJbW1w79benm8q --- .../locomanipulationgame_bringup.launch.py | 162 ++++++++++++++++++ core_ws/src/h1_bringup/package.xml | 1 + 2 files changed, 163 insertions(+) create mode 100644 core_ws/src/h1_bringup/launch/locomanipulationgame_bringup.launch.py diff --git a/core_ws/src/h1_bringup/launch/locomanipulationgame_bringup.launch.py b/core_ws/src/h1_bringup/launch/locomanipulationgame_bringup.launch.py new file mode 100644 index 0000000..1f9c378 --- /dev/null +++ b/core_ws/src/h1_bringup/launch/locomanipulationgame_bringup.launch.py @@ -0,0 +1,162 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterValue + +# Loco-manipulation game bringup (x86 sim): the Locomanipulation_game MARL policy +# (locomotion_game_deploy) driving all 27 joints through the split safety layer, +# SLAM, and the game commander that sends it walking and arm-goal commands the +# way training did. ROS_DOMAIN_ID must be exported in the launching shell (see +# h1_control.launch.py); start the simulator first. +# +# marl_deploy_node owns both safety channels, so frame_task_server and the +# lower-body RL node are not started. The safety layer runs the real robot's +# preset (relax_safety_split): the policy was trained against its e-stops, and +# the sim is where to see that it doesn't trip them. Navigation (nav2) is not +# started: the commander is the only /cmd_vel publisher. +ASSETS_DIR = '/home/code/CL_Assets' + + +def _include(package, launch_file, launch_arguments, condition=None): + return IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory(package), 'launch', launch_file)), + launch_arguments=launch_arguments.items(), + condition=condition, + ) + + +def generate_launch_description(): + bringup_share = get_package_share_directory('h1_bringup') + deploy_share = get_package_share_directory('locomotion_game_deploy') + slam_share = get_package_share_directory('h12_slam') + policy_dir = PathJoinSubstitution([deploy_share, 'policies', LaunchConfiguration('policy')]) + use_sim_time = ParameterValue(LaunchConfiguration('use_sim_time'), value_type=bool) + sim_time = {'use_sim_time': LaunchConfiguration('use_sim_time')} + + return LaunchDescription([ + DeclareLaunchArgument('use_sim_time', default_value='true'), + DeclareLaunchArgument('policy', default_value='marl_golem', + description='policies// of locomotion_game_deploy: .yaml plus the export'), + DeclareLaunchArgument('policy_mode', default_value='torch', + description='torch: the exported networks; zero: plumbing stand-ins'), + DeclareLaunchArgument('safety_config', default_value='relax_safety_split.yaml'), + DeclareLaunchArgument('engage_wait_for_confirm', default_value='false', + description='hold the pre-pose until ~/confirm_engage (true on the real robot)'), + DeclareLaunchArgument('use_slam', default_value='true'), + DeclareLaunchArgument('use_commander', default_value='true'), + DeclareLaunchArgument('commander_mode', default_value='training', description='training | sequence'), + DeclareLaunchArgument('goals', default_value='arm_goals_4_10.npz', + description='arm goal samples in the policy directory (spread_drop levels)'), + DeclareLaunchArgument('commander_seed', default_value='0'), + DeclareLaunchArgument('commander_duration_s', default_value='0.0', description='0: run until stopped'), + DeclareLaunchArgument('commander_log', default_value='', description='JSON-lines log of events and status'), + DeclareLaunchArgument('use_rviz', default_value='false'), + DeclareLaunchArgument('rviz_config', default_value=os.path.join(bringup_share, 'rviz', 'nav.rviz')), + + _include('h1_bringup', 'h1_state.launch.py', { + **sim_time, + 'urdf_file': os.path.join(ASSETS_DIR, 'ros_assets', 'h1_2_magpie_ros.urdf'), + }), + _include('h1_bringup', 'h1_control.launch.py', { + **sim_time, + 'use_frame_task': 'false', + 'safety_config': LaunchConfiguration('safety_config'), + # no hardware e-stop node in sim to wait for + 'safety_delay': '0.0', + }), + + Node( + package='locomotion_game_deploy', + executable='marl_deploy_node', + name='marl_deploy_node', + output='screen', + parameters=[{ + 'use_sim_time': use_sim_time, + 'config': PathJoinSubstitution([policy_dir, [LaunchConfiguration('policy'), '.yaml']]), + 'urdf_path': os.path.join(ASSETS_DIR, 'ros_assets', 'h1_2_magpie.urdf'), + 'policy_mode': LaunchConfiguration('policy_mode'), + 'engage_wait_for_confirm': ParameterValue(LaunchConfiguration('engage_wait_for_confirm'), value_type=bool), + 'disable_elastic_band': True, + }], + ), + + Node( + package='locomotion_game_deploy', + executable='game_commander_node', + name='game_commander', + output='screen', + condition=IfCondition(LaunchConfiguration('use_commander')), + parameters=[{ + 'use_sim_time': use_sim_time, + 'mode': LaunchConfiguration('commander_mode'), + 'goals_file': PathJoinSubstitution([policy_dir, LaunchConfiguration('goals')]), + 'seed': ParameterValue(LaunchConfiguration('commander_seed'), value_type=int), + 'duration_s': ParameterValue(LaunchConfiguration('commander_duration_s'), value_type=float), + 'log_file': LaunchConfiguration('commander_log'), + 'cloud_frame': 'livox_link', + }], + ), + + # SLAM: FAST-LIO (/livox/lidar + /livox/imu -> /Odometry, camera_init -> body), its body -> pelvis + # mount, and slam_toolbox's 2-D map (map -> camera_init) from the pelvis-height scan. Same nodes and + # configs as h12_slam/h1_navigation.launch.py, without nav2. + Node( + package='fast_lio', + executable='fastlio_mapping', + parameters=[os.path.join(slam_share, 'config', 'mid360.yaml'), {'use_sim_time': use_sim_time}], + output='screen', + condition=IfCondition(LaunchConfiguration('use_slam')), + ), + Node( + package='tf2_ros', + executable='static_transform_publisher', + name='body_to_pelvis_broadcaster', + arguments=['0.11433', '0.0', '-0.67188', '0.0', '-0.24015730507441985', '0.0', 'body', 'pelvis'], + parameters=[{'use_sim_time': use_sim_time}], + output='screen', + condition=IfCondition(LaunchConfiguration('use_slam')), + ), + Node( + package='pointcloud_to_laserscan', + executable='pointcloud_to_laserscan_node', + name='cloud_registered_to_laserscan', + output='screen', + condition=IfCondition(LaunchConfiguration('use_slam')), + parameters=[{ + 'target_frame': 'pelvis', 'use_sim_time': use_sim_time, + 'min_height': -0.65, 'max_height': 0.65, + 'angle_min': -3.14159, 'angle_max': 3.14159, 'angle_increment': 0.0087, + 'range_min': 0.9, 'range_max': 3.0, 'use_inf': True, + 'scan_time': 0.0333, 'transform_tolerance': 0.05, 'queue_size': 20, + }], + remappings=[('cloud_in', '/cloud_registered_body'), ('scan', '/converted_scan')], + ), + Node( + package='slam_toolbox', + executable='async_slam_toolbox_node', + name='slam_toolbox', + output='screen', + condition=IfCondition(LaunchConfiguration('use_slam')), + parameters=[os.path.join(slam_share, 'config', 'slam_toolbox_h1.yaml'), { + 'base_frame': 'pelvis', 'odom_frame': 'camera_init', 'scan_topic': '/converted_scan', + 'odom_topic': '/Odometry', 'use_sim_time': use_sim_time, + }], + ), + + Node( + package='rviz2', + executable='rviz2', + name='rviz2_locomanipulationgame', + arguments=['-d', LaunchConfiguration('rviz_config')], + parameters=[{'use_sim_time': use_sim_time}], + output='screen', + condition=IfCondition(LaunchConfiguration('use_rviz')), + ), + ]) diff --git a/core_ws/src/h1_bringup/package.xml b/core_ws/src/h1_bringup/package.xml index bbeb62e..ccf9f69 100644 --- a/core_ws/src/h1_bringup/package.xml +++ b/core_ws/src/h1_bringup/package.xml @@ -15,6 +15,7 @@ h12_ros2_controller h12_safety_layer h12_lowerbody_rl + locomotion_game_deploy cl_realsense model_server custom_ros_messages From be35a1fed668fce021dec89b8926536e8c405abf Mon Sep 17 00:00:00 2001 From: max Date: Wed, 7 Oct 2026 00:40:14 -0600 Subject: [PATCH 013/225] tests/locomanipulation_game: run, record, render and score the game in RoboCasa run.sh follows tests/stack/run.sh: disposable ROS and sim containers on a test domain (194), the ROS side running locomanipulationgame_bringup.launch.py after the canonical launch_ros.sh build, the sim running the canonical launch_robocasa.sh with record_simulator.py swapped in as its entry point. The recorder (no ROS output) saves the compiled scene and the state at 30 Hz for render_recording.py, which follows the robot with the room's walls hidden, and 50 Hz pelvis and joint telemetry for analyze_run.py, which scores falls, e-stops, walking tracking, arm goals and joint margins against h12_safety_layer's table. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01DuoTaYHKcJbW1w79benm8q --- tests/locomanipulation_game/analyze_run.py | 180 ++++++++++++++++++ .../locomanipulation_game/record_simulator.py | 83 ++++++++ .../locomanipulation_game/render_recording.py | 109 +++++++++++ tests/locomanipulation_game/run.sh | 86 +++++++++ 4 files changed, 458 insertions(+) create mode 100644 tests/locomanipulation_game/analyze_run.py create mode 100644 tests/locomanipulation_game/record_simulator.py create mode 100644 tests/locomanipulation_game/render_recording.py create mode 100755 tests/locomanipulation_game/run.sh diff --git a/tests/locomanipulation_game/analyze_run.py b/tests/locomanipulation_game/analyze_run.py new file mode 100644 index 0000000..f2deee0 --- /dev/null +++ b/tests/locomanipulation_game/analyze_run.py @@ -0,0 +1,180 @@ +"""Score one loco-manipulation game run (tests/locomanipulation_game/run.sh) from its artifacts. Host python + numpy. + +Reads /ros/commander.jsonl (events, goal results, e-stops, 10 Hz status) and /record/telemetry.jsonl +(the recorder's 50 Hz pelvis pose and joint state). Writes /metrics.json and timeline.png: + engaged sim time from the elastic band's release to the end of the run + falls pelvis below 0.55 m or tilted past 1 rad after the release + e-stops safety heartbeat False (the safety layer's estop latched), deploy faults + walking per walk event (its first 1.5 s excluded: settle and acceleration): commanded vs measured pelvis + velocity in the pelvis frame (xy error, yaw-rate error) and the share of the commanded path covered + arm goals reached (both wrists within 5 cm for 1 s), best wrist error, by standing / low / needs-crouch; + pelvis drop below the run's standing height during low goals + joints closest approach to each URDF position limit, peak speed / URDF velocity limit, share of samples + with the applied torque at the limit, against h12_safety_layer's relax_safety_split table + + python3 analyze_run.py +""" +import json +import math +import sys +from pathlib import Path + +import numpy as np + +sys.path.insert(0, str(Path(__file__).resolve().parents[2] / 'core_ws/src/h12_safety_layer')) +from h12_safety_layer.core.joint_limits import (JOINT_NAMES, URDF_POSITION_LIMITS, URDF_TORQUE_LIMITS, # noqa: E402 + URDF_VELOCITY_LIMITS) + +result = Path(sys.argv[1]) +records = [json.loads(line) for line in open(result / 'ros' / 'commander.jsonl')] if (result / 'ros' / 'commander.jsonl').exists() else [] +tele = [json.loads(line) for line in open(result / 'record' / 'telemetry.jsonl')] +t = np.array([r['sim_time'] for r in tele]) +pos = np.array([r['pelvis_pos'] for r in tele]) +quat = np.array([r['pelvis_quat'] for r in tele]) +q = np.array([r['q'] for r in tele]) +qd = np.array([r['qd'] for r in tele]) +tau = np.array([r['tau'] for r in tele]) +band = np.array([r['band'] for r in tele]) + + +def rotate_inverse(qw, v): + """v (world) into the frame of unit quaternions qw (wxyz), row-wise.""" + w, xyz = qw[:, :1], qw[:, 1:] + xyz = -xyz + t2 = 2.0 * np.cross(xyz, v) + return v + w * t2 + np.cross(xyz, t2) + + +up = rotate_inverse(quat, np.tile([0.0, 0.0, 1.0], (len(quat), 1))) +tilt = np.arccos(np.clip(up[:, 2], -1.0, 1.0)) +yaw = np.arctan2(2 * (quat[:, 0] * quat[:, 3] + quat[:, 1] * quat[:, 2]), 1 - 2 * (quat[:, 2] ** 2 + quat[:, 3] ** 2)) +released = np.flatnonzero(~band) +t0 = t[released[0]] if len(released) else t[-1] +live = t >= t0 +fallen = live & ((pos[:, 2] < 0.55) | (tilt > 1.0)) +first_fall = float(t[np.flatnonzero(fallen)[0]]) if fallen.any() else None +# sim time is the commander's clock too (use_sim_time) +estops = [r for r in records if r['type'] == 'estop'] +faults = [r for r in records if r['type'] == 'deploy_state' and str(r.get('state', '')).startswith('fault')] +end = min([first_fall or math.inf] + [r['t'] for r in estops] + [t[-1]]) +standing_z = float(np.median(pos[live & (t < t0 + 3.0), 2])) if (live & (t < t0 + 3.0)).any() else float('nan') + +# measured pelvis velocity in its own frame, 0.5 s central differences +k = 12 +vel_w = np.zeros_like(pos) +vel_w[k:-k] = (pos[2 * k:] - pos[:-2 * k]) / (t[2 * k:] - t[:-2 * k])[:, None] +vel_b = rotate_inverse(quat, vel_w) +yaw_rate = np.zeros_like(yaw) +yaw_rate[k:-k] = np.unwrap(yaw)[2 * k:] - np.unwrap(yaw)[:-2 * k] +yaw_rate[k:-k] /= (t[2 * k:] - t[:-2 * k]) + +events = [r for r in records if r['type'] == 'event'] +walks, goals = [], [] +for i, ev in enumerate(events): + start = ev['t'] + stop = events[i + 1]['t'] if i + 1 < len(events) else end + stop = min(stop, end) + if stop <= start: + continue + if ev['kind'] == 'walk': + m = (t >= start + 1.5) & (t < stop) & live + if m.sum() < 25: + continue + cmd = np.array(ev['cmd']) + err_xy = np.linalg.norm(vel_b[m, :2] - cmd[:2], axis=1) + commanded = np.linalg.norm(cmd[:2]) * (t[m][-1] - t[m][0]) + moved = np.dot(np.linalg.norm(vel_b[m, :2], axis=1)[:-1], np.diff(t[m])) + along = np.dot(vel_b[m, :2][:-1] @ (cmd[:2] / max(np.linalg.norm(cmd[:2]), 1e-9)), np.diff(t[m])) if commanded > 0 else 0.0 + walks.append({'t': start, 'cmd': ev['cmd'], 'xy_error': float(err_xy.mean()), + 'yaw_rate_error': float(np.abs(yaw_rate[m] - cmd[2]).mean()), + 'path_ratio': float(along / commanded) if commanded > 0.2 else None, 'moved_m': float(moved)}) + else: + res = next((r for r in records if r['type'] == 'goal_result' and r['index'] == ev['index']), None) + m = (t >= start + ev.get('settle', 0.0)) & (t < stop) & live + goals.append({'t': start, 'low': ev['low'], 'needs_crouch': ev['needs_crouch'], + 'reached': None if res is None else res['reached'], 'best_error': None if res is None else res['best_error'], + 'pelvis_drop_max': float(standing_z - pos[m, 2].min()) if m.any() else None, + 'lowest_target_z': float(min(p[2] for p in ev['poses'])), 'completed': res is not None and stop >= start + ev['duration']}) + + +def share(values): + values = [v for v in values if v is not None] + return None if not values else float(np.mean(values)) + + +lo = np.array([lim['low'] for lim in URDF_POSITION_LIMITS]) +hi = np.array([lim['high'] for lim in URDF_POSITION_LIMITS]) +lm = live & (t <= end) +joints = {} +for j, name in enumerate(JOINT_NAMES): + if not lm.any(): + break + joints[name] = {'closest_to_limit_rad': round(float(np.minimum(q[lm, j] - lo[j], hi[j] - q[lm, j]).min()), 4), + 'peak_speed_of_limit': round(float(np.abs(qd[lm, j]).max() / URDF_VELOCITY_LIMITS[j]), 3), + 'torque_at_limit_share': round(float((np.abs(tau[lm, j]) >= 0.999 * URDF_TORQUE_LIMITS[j]).mean()), 4)} +done_goals = [g for g in goals if g['reached'] is not None] +metrics = { + 'result': str(result), + 'engaged_s': round(float(end - t0), 1), + 'band_released_at_s': round(float(t0), 2), + 'fell': first_fall is not None, 'first_fall_s': first_fall, + 'estops': [{k: r[k] for k in ('t', 'detail') if k in r} for r in estops], + 'deploy_faults': [r['state'] for r in faults], + 'standing_pelvis_z': round(standing_z, 3), + 'walking': {'segments': len(walks), 'xy_error_m_s': share([w['xy_error'] for w in walks]), + 'yaw_rate_error': share([w['yaw_rate_error'] for w in walks]), + 'path_ratio': share([w['path_ratio'] for w in walks]), 'distance_m': float(sum(w['moved_m'] for w in walks))}, + 'arm_goals': {'judged': len(done_goals), 'reached': share([float(g['reached']) for g in done_goals]), + 'reached_standing': share([float(g['reached']) for g in done_goals if not g['low']]), + 'reached_low': share([float(g['reached']) for g in done_goals if g['low']]), + 'reached_needs_crouch': share([float(g['reached']) for g in done_goals if g['needs_crouch']]), + 'best_error_m_median': None if not done_goals else float(np.median([g['best_error'] for g in done_goals if g['best_error'] is not None] or [np.nan])), + 'pelvis_drop_low_goals_m': share([g['pelvis_drop_max'] for g in goals if g['low']])}, + 'obstacle_stops': sum(1 for r in records if r['type'] == 'obstacle_stop'), + 'joints_worst': { + 'closest_to_limit': sorted(((v['closest_to_limit_rad'], n) for n, v in joints.items()))[:5], + 'peak_speed_of_limit': sorted(((v['peak_speed_of_limit'], n) for n, v in joints.items()), reverse=True)[:5], + 'torque_at_limit_share': sorted(((v['torque_at_limit_share'], n) for n, v in joints.items()), reverse=True)[:5], + }, + 'goals': goals, 'walks': walks, 'joints': joints, +} +json.dump(metrics, open(result / 'metrics.json', 'w'), indent=1) +summary = {k: v for k, v in metrics.items() if k not in ('goals', 'walks', 'joints')} +print(json.dumps(summary, indent=1)) + +try: + import matplotlib + matplotlib.use('Agg') + import matplotlib.pyplot as plt + status = [r for r in records if r['type'] == 'status'] + st = np.array([r['t'] for r in status]) + werr = np.array([[np.nan if e is None else e for e in (r['wrist_error'] or [None, None])] for r in status], dtype=float) + fig, axes = plt.subplots(3, 1, figsize=(13, 8), sharex=True) + for ax in axes: + for ev in events: + stop = next((e['t'] for e in events if e['t'] > ev['t']), end) + if ev['kind'] == 'arm_goal': + ax.axvspan(ev['t'], stop, color='#ffd9c2' if ev['needs_crouch'] else '#dbe8fa', lw=0) + for r in estops: + ax.axvline(r['t'], color='#d03030', lw=1.5) + if first_fall: + ax.axvline(first_fall, color='#d03030', lw=1.5, ls='--') + axes[0].plot(t, pos[:, 2], color='#2a78d6', lw=1.4) + axes[0].set_ylabel('pelvis height (m)') + if len(st): + axes[1].plot(st, np.nanmax(werr, axis=1) * 100, color='#2a78d6', lw=1.2) + axes[1].axhline(5, color='#999', lw=1) + axes[1].set_ylabel('worse wrist error (cm)') + axes[1].set_ylim(0, 40) + cmd_t = [r['t'] for r in status] + cmd_v = [np.hypot(*r['cmd'][:2]) for r in status] + axes[2].plot(cmd_t, cmd_v, color='#999', lw=1.2, label='commanded') + axes[2].plot(t, np.linalg.norm(vel_b[:, :2], axis=1), color='#2a78d6', lw=1.2, label='measured') + axes[2].set_ylabel('xy speed (m/s)') + axes[2].set_xlabel('sim time (s)') + axes[2].legend(loc='upper right', fontsize=8) + axes[0].set_title('Arm goals shaded (orange: needs a crouch), e-stops red, fall dashed', loc='left', fontsize=10) + fig.tight_layout() + fig.savefig(result / 'timeline.png', dpi=110) +except ImportError: + pass diff --git a/tests/locomanipulation_game/record_simulator.py b/tests/locomanipulation_game/record_simulator.py new file mode 100644 index 0000000..9555959 --- /dev/null +++ b/tests/locomanipulation_game/record_simulator.py @@ -0,0 +1,83 @@ +"""Passive RoboCasa recorder for the loco-manipulation game runs. Nothing it measures enters ROS. + +Executed in place of h12_mujoco.py by run.sh, with the canonical launcher, arguments and environment; the real +simulation loop runs unchanged (the tests/stack/observe_simulator.py pattern). It writes, into $GOLEM_RECORD_DIR: + model.mjb the compiled scene, saved at the first production step (render_recording.py replays it) + qpos.f32 float32 rows [sim_time, qpos...] at 30 Hz of sim time, for the video + telemetry.jsonl 50 Hz: sim time, pelvis position and wxyz quaternion, the 27 motor q, qd and applied + torque, band state +""" +import json +import os +import runpy +import sys +from pathlib import Path + +import numpy as np + +ENTRY = Path('/home/code/h1_robocasa/h12_mujoco.py') +FRAME_DT = 1.0 / 30.0 +TELEMETRY_DT = 0.02 + + +class Recorder: + def __init__(self, directory: Path): + self.dir = directory + self.qpos = open(directory / 'qpos.f32', 'wb', buffering=0) + self.telemetry = open(directory / 'telemetry.jsonl', 'w', buffering=1) + self.next_frame = 0.0 + self.next_telemetry = 0.0 + self.saved_model = False + + def step(self, model, data, resolver, band) -> None: + import mujoco + + if not self.saved_model: + mujoco.mj_saveModel(model, str(self.dir / 'model.mjb'), None) + (self.dir / 'model.json').write_text(json.dumps({'nq': int(model.nq), 'timestep': float(model.opt.timestep)})) + self.saved_model = True + self.next_frame = self.next_telemetry = data.time + if data.time >= self.next_frame: + self.qpos.write(np.concatenate([[data.time], data.qpos]).astype(np.float32).tobytes()) + self.next_frame += FRAME_DT + if data.time >= self.next_telemetry: + pelvis = resolver.body_id('pelvis') + self.telemetry.write(json.dumps({ + 'sim_time': round(float(data.time), 4), + 'pelvis_pos': [round(float(v), 5) for v in data.xpos[pelvis]], + 'pelvis_quat': [round(float(v), 5) for v in data.xquat[pelvis]], + 'q': [round(float(v), 5) for v in data.qpos[resolver.motor_qpos]], + 'qd': [round(float(v), 4) for v in data.qvel[resolver.motor_qvel]], + 'tau': [round(float(v), 3) for v in data.sensordata[resolver.motor_tau]], + 'band': bool(band is not None and band.enabled), + }) + '\n') + self.next_telemetry += TELEMETRY_DT + + +def main(): + recorder = Recorder(Path(os.environ['GOLEM_RECORD_DIR'])) + sys.path.insert(0, str(ENTRY.parent)) + import mujoco + + step = mujoco.mj_step + + def recorded_step(model, data, *args, **kwargs): + result = step(model, data, *args, **kwargs) + # only the production loop holds the resolver and band; robosuite's settling calls reset time + caller = sys._getframe(1) + local = caller.f_locals + if caller.f_code.co_filename == str(ENTRY) and 'band' in local: + recorder.step(model, data, local['resolver'], local['band']) + return result + + mujoco.mj_step = recorded_step + sys.argv[0] = str(ENTRY) + try: + runpy.run_path(str(ENTRY), run_name='__main__') + finally: + recorder.qpos.close() + recorder.telemetry.close() + + +if __name__ == '__main__': + main() diff --git a/tests/locomanipulation_game/render_recording.py b/tests/locomanipulation_game/render_recording.py new file mode 100644 index 0000000..384ef72 --- /dev/null +++ b/tests/locomanipulation_game/render_recording.py @@ -0,0 +1,109 @@ +"""Render a record_simulator.py recording to mp4 with a camera that follows the robot. Runs in the robocasa image. + + python3 render_recording.py [--out video.mp4] [--start 0] [--end inf] [--width 960 --height 540] + [--overlay ] + +The camera follows the pelvis from above and behind, turning with the robot's heading (azimuth is an offset from +it; --fixed_azimuth holds it instead). The room's walls are not drawn, so the camera can sit outside them. With --overlay, the commander's event log +labels each frame with the active event and its command. +""" +import argparse +import json +from pathlib import Path + +import imageio.v2 as imageio +import mujoco +import numpy as np + +parser = argparse.ArgumentParser() +parser.add_argument('record') +parser.add_argument('--out', default=None) +parser.add_argument('--start', type=float, default=0.0, help='sim seconds') +parser.add_argument('--end', type=float, default=float('inf')) +parser.add_argument('--width', type=int, default=960) +parser.add_argument('--height', type=int, default=540) +parser.add_argument('--distance', type=float, default=2.8) +parser.add_argument('--azimuth', type=float, default=25.0, help='deg from the robot heading (0: straight behind)') +parser.add_argument('--fixed_azimuth', action='store_true', help='azimuth in world degrees instead') +parser.add_argument('--elevation', type=float, default=-35.0) +parser.add_argument('--overlay', default=None) +args = parser.parse_args() + +rec = Path(args.record) +model = mujoco.MjModel.from_binary_path(str(rec / 'model.mjb')) +model.vis.global_.offwidth = max(model.vis.global_.offwidth, args.width) +model.vis.global_.offheight = max(model.vis.global_.offheight, args.height) +# the room's walls would block a follow camera: draw them in a hidden geom group (the floor stays) +for i in range(model.ngeom): + body = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_BODY, model.geom_bodyid[i]) or '' + if body.startswith('wall'): + model.geom_group[i] = 4 +data = mujoco.MjData(model) +rows = np.fromfile(rec / 'qpos.f32', dtype=np.float32) +rows = rows[: len(rows) // (model.nq + 1) * (model.nq + 1)].reshape(-1, model.nq + 1) +rows = rows[(rows[:, 0] >= args.start) & (rows[:, 0] <= args.end)] +pelvis = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, 'pelvis') +if pelvis < 0: # robosuite prefixes the robot's bodies (robot0_pelvis) + pelvis = next(i for i in range(model.nbody) if mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_BODY, i).endswith('pelvis')) +print('following body', mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_BODY, pelvis)) + +events = [] +if args.overlay and Path(args.overlay).exists(): + for line in open(args.overlay): + r = json.loads(line) + if r.get('type') in ('event', 'estop', 'obstacle_stop', 'done'): + events.append(r) + +label_font = None +try: + from PIL import Image, ImageDraw, ImageFont + label_font = ImageFont.load_default() +except ImportError: + Image = None + + +def label(t: float) -> str: + current = None + for r in events: + if r['t'] <= t: + current = r + if current is None: + return f't {t:6.1f} s' + if current['type'] == 'event' and current['kind'] == 'walk': + return f"t {t:6.1f} s walk cmd vx {current['cmd'][0]:+.2f} vy {current['cmd'][1]:+.2f} wz {current['cmd'][2]:+.2f}" + if current['type'] == 'event': + kind = 'crouch reach' if current.get('needs_crouch') else ('low reach' if current.get('low') else 'standing reach') + return f't {t:6.1f} s arm goal ({kind})' + return f"t {t:6.1f} s {current['type']}" + + +renderer = mujoco.Renderer(model, args.height, args.width) +camera = mujoco.MjvCamera() +camera.type = mujoco.mjtCamera.mjCAMERA_FREE +camera.distance, camera.azimuth, camera.elevation = args.distance, args.azimuth, args.elevation +out = args.out or str(rec / 'video.mp4') +writer = imageio.get_writer(out, fps=30, codec='libx264', quality=7, macro_block_size=1) +lookat, heading = None, None +for row in rows: + data.qpos[:] = row[1:] + mujoco.mj_forward(model, data) + target = data.xpos[pelvis].copy() + target[2] = max(target[2] - 0.15, 0.5) + lookat = target if lookat is None else 0.9 * lookat + 0.1 * target # smooth the follow + xmat = data.xmat[pelvis].reshape(3, 3) + yaw = np.arctan2(xmat[1, 0], xmat[0, 0]) + heading = yaw if heading is None else heading + 0.05 * np.angle(np.exp(1j * (yaw - heading))) + camera.lookat[:] = lookat + if not args.fixed_azimuth: + camera.azimuth = np.degrees(heading) + args.azimuth + renderer.update_scene(data, camera) + frame = renderer.render() + if Image is not None and events is not None: + img = Image.fromarray(frame) + draw = ImageDraw.Draw(img) + draw.rectangle([0, 0, args.width, 22], fill=(0, 0, 0)) + draw.text((8, 5), label(float(row[0])), fill=(255, 255, 255), font=label_font) + frame = np.asarray(img) + writer.append_data(frame) +writer.close() +print(f'wrote {out}: {len(rows)} frames, sim {rows[0, 0]:.1f}-{rows[-1, 0]:.1f} s') diff --git a/tests/locomanipulation_game/run.sh b/tests/locomanipulation_game/run.sh new file mode 100755 index 0000000..18051a7 --- /dev/null +++ b/tests/locomanipulation_game/run.sh @@ -0,0 +1,86 @@ +#!/usr/bin/env bash +# One loco-manipulation game run in RoboCasa: locomanipulationgame_bringup.launch.py in a disposable ROS +# container, the canonical RoboCasa launcher with record_simulator.py as its entry point in a disposable sim +# container, then a rendered video. Artifacts go to tests/results/locomanipulation_game--