diff --git a/.dockerignore b/.dockerignore index a6ea67f..9aa1704 100644 --- a/.dockerignore +++ b/.dockerignore @@ -3,13 +3,10 @@ # 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) +# IsaacDockerfile -> magpie_msgs + private Humble runtime packager * -!CL_isaaclab_sim -CL_isaaclab_sim/* -!CL_isaaclab_sim/requirements.txt !core_ws core_ws/* !core_ws/src @@ -19,3 +16,13 @@ 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 +!docker/locks +!docker/locks/** diff --git a/.github/workflows/docs.yml b/.github/workflows/docs.yml new file mode 100644 index 0000000..d74591b --- /dev/null +++ b/.github/workflows/docs.yml @@ -0,0 +1,48 @@ +name: Docs + +on: + push: + branches: [main] + paths: + - 'docs/**' + - 'mkdocs.yml' + - '.github/workflows/docs.yml' + workflow_dispatch: + +permissions: + contents: read + pages: write + id-token: write + +concurrency: + group: pages + cancel-in-progress: false + +jobs: + build: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v7 + - uses: actions/setup-python@v7 + with: + python-version: "3.12" + cache: pip + cache-dependency-path: docs/requirements-docs.txt + - name: Install docs requirements + run: pip install -r docs/requirements-docs.txt + - name: Build site + run: mkdocs build --strict + - uses: actions/configure-pages@v6 + - uses: actions/upload-pages-artifact@v5 + with: + path: site + + deploy: + needs: build + runs-on: ubuntu-latest + environment: + name: github-pages + url: ${{ steps.deployment.outputs.page_url }} + steps: + - id: deployment + uses: actions/deploy-pages@v5 diff --git a/.github/workflows/simulator-tests.yml b/.github/workflows/simulator-tests.yml new file mode 100644 index 0000000..ade9c52 --- /dev/null +++ b/.github/workflows/simulator-tests.yml @@ -0,0 +1,91 @@ +name: Simulator regression tests +on: + pull_request: + paths: + - 'CL_isaaclab_sim' + - 'CL_isaaclab_sim/**' + - 'golem_robocasa/**' + - 'docker/**' + - 'core_ws/**' + - 'CL_Assets' + - '.gitmodules' + - '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) + # .gitmodules uses SSH URLs; the runner has no SSH key, and both are public + run: | + git config --global url."https://github.com/".insteadOf git@github.com: + git submodule update --init CL_isaaclab_sim unitree_sdk2_python + - uses: actions/setup-python@v5 + with: + # cyclonedds 0.10.2 ships wheels (with its C library) up to 3.10, the + # images' Python; on 3.11 pip builds it against jammy's 0.8.2 and fails + python-version: '3.10' + - name: Install test dependencies + run: | + # torch's own index lacks some of its dependencies' build tools + python -m pip install torch==2.7.0 --index-url https://download.pytorch.org/whl/cpu \ + --extra-index-url https://pypi.org/simple + 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: golem_robocasa + run: python -m unittest discover -s tests/robocasa -p test_loop_timing.py -v + - name: RoboCasa robot selection + env: + PYTHONPATH: golem_robocasa + run: python -m unittest discover -s tests/robocasa -p test_robot_select.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. + if: github.event_name == 'workflow_dispatch' + runs-on: [self-hosted, linux, x64, nvidia] + 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 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 + 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 + 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 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: + name: simulator-test-results + path: tests/results/ diff --git a/.gitignore b/.gitignore index ade1fe6..9c7d964 100644 --- a/.gitignore +++ b/.gitignore @@ -62,8 +62,26 @@ 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) +# process crash dumps (e.g. golem_robocasa/core from unitree_sdk2py segfaults) core +!core/ core.* *.core + +# rosbag recordings (multi-GB; share out of band, not via git) +/bags/ +*.mcap +*.db3 + +# local scratch space +/work/ + +# Claude Code local settings +/.claude/ + +# MkDocs site +/site/ diff --git a/.gitmodules b/.gitmodules index 063bf12..660a7c1 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,18 +1,6 @@ [submodule "CL_isaaclab_sim"] path = CL_isaaclab_sim url = git@github.com:correlllab/CL_isaaclab_sim.git -[submodule "core_ws/src/h12_ros2_controller"] - path = core_ws/src/h12_ros2_controller - url = git@github.com:correlllab/h12_ros2_controller.git - branch = main -[submodule "core_ws/src/h12_ros2_model"] - path = core_ws/src/h12_ros2_model - url = git@github.com:correlllab/h12_ros2_model.git - branch = unified_sim -[submodule "core_ws/src/h12_safety_layer"] - path = core_ws/src/h12_safety_layer - url = git@github.com:correlllab/h12_safety_layer.git - branch = main [submodule "core_ws/src/custom_ros_messages"] path = core_ws/src/custom_ros_messages url = git@github.com:correlllab/custom_ros_messages.git diff --git a/AGENTS.md b/AGENTS.md new file mode 100644 index 0000000..789a255 --- /dev/null +++ b/AGENTS.md @@ -0,0 +1,830 @@ +# AGENTS.md — GOLEM developer guide for coding agents + +The safety layer and the arm controller of both robots (`golem_safety`, +`golem_upper_ik`), the bringup package (`golem_bringup`) and the start-ups it +runs, the models (`golem_ros2_models`, `CL_Assets/g1`), the robot selection, the G1 text (its packages, sim, start-up and +invariants), the `mode_machine` identity check, the package tables and the +stale-install pitfall were checked against the code on 2026-10-28; the +DDS-domain rules and the H1-2 claims beside them on 2026-10-26; the rest was +verified 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 two +Unitree humanoids, the H1-2 and the G1 (29 DoF, two Dex3-1 hands). The H1-2 has +MuJoCo/RoboCasa and Isaac Lab simulators and 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 G1 has its own +packages in the same workspace (`g1_homie`, HOMIE's walking policy; `dex_3`, +the Dex3-1 hands) and a RoboCasa sim (no Isaac). Both robots' models +are in `CL_Assets`, installed for ROS by one package, `golem_ros2_models`. +Both robots run one safety layer, `golem_safety`, and one arm IK controller, +`golem_upper_ik`, each with a profile per robot, and start from one bringup +package, `golem_bringup`. Each robot speaks its own Unitree DDS interface +(`rt/lowcmd` / `rt/lowstate`, and on the G1 also `rt/dex3/*`) identically +against its sim and the real robot, so code moves between them unchanged. The +G1 runs in Unitree's debug mode: Unitree's own controller is released, and the +stack drives every motor on `rt/lowcmd`, as on the H1-2. + +The H1-2 is the default everywhere. `GOLEM_ROBOT=g1` or `--g1` on a simulator or +stack wrapper selects the G1 where the attached robot matters: the sim and +`docker_stack.sh`'s bringup (§3). Every `ros` container builds and runs both +robots' packages (`docker_run.sh ros` takes no `--g1` or `--h12` flag). The G1 +pages are in `docs/g1/` (start at `docs/g1/index.md`). + +## 2. Repo layout + +| Path | What it is | +|---|---| +| `docker/` | x86 setup — the fully-featured, preferred platform (Dockerfiles, `docker-compose.yml`, the G1 overlay `docker-compose.g1.yml`, `scripts/`) | +| `docker/mac/` | self-contained Apple-Silicon (arm64, CPU-only) port — limited: H1-2 only, no Isaac, no ML/vision stack, FastDDS instead of CycloneDDS | +| `core_ws/` | the ROS 2 workspace — where the bulk of development happens (see §4); the G1's packages are `core_ws/src/g1_homie/` and `core_ws/src/dex_3/`, the safety layer, arm controller, SLAM/navigation and skills both robots run in `core_ws/src/golem_safety/`, `core_ws/src/golem_upper_ik/`, `core_ws/src/golem_slam/` and `core_ws/src/golem_skills/` | +| `golem_robocasa/` | RoboCasa/MuJoCo simulator entry point for both robots (`h12_mujoco.py`; `--g1` hands over to the G1 sim package `golem_robocasa/g1/`) and the ROS/DDS bridges | +| `CL_isaaclab_sim/` | Isaac Lab simulator, H1-2 only (`sim_main.py`, DDS bridges, tasks) | +| `CL_Assets/` | both robots' URDFs, MJCFs, meshes and self-collision models (the G1's in `CL_Assets/g1/`), Isaac USD (Git-LFS submodule); `golem_ros2_models` installs the models for ROS, and the sims read them directly | +| `mujoco_mpc/` | MuJoCo-MPC fork submodule (built standalone, outside colcon — see §8) | +| `unitree_sdk2_python/` | vendored Unitree DDS SDK (Python) | +| `tests/` | simulator regression and acceptance tests (`tests/README.md`); the G1 acceptance tests are in `tests/g1/` | +| `tools/` | standalone debug tools (ROS MCP server for Codex; `read_lowstate.py`, which prints the real G1's `rt/lowstate`) | +| `docs/` | user-facing docs: `SETUP.md`, `RUNNING.md`, `MACOS.md`, `DDS_TUNING.md`, `DOCKER_TESTING.md`, `NAVIGATION_DEMO.md`, `ROS_MCP_DEBUG.md`, and the G1's in `docs/g1/` | +| `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, robot selection 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`). The images serve both robots. | +| `docker/scripts/docker_run.sh [--restart] [cmd...]` | `isaac`, `robocasa`, `ros`; `--restart` right after the profile replaces an existing container; then optionally `bash` (shell instead of default launcher) or any command; a leading `-flag` (e.g. `--headless`) is forwarded to the default launcher; on simulator profiles (`robocasa`, `isaac`), `--g1` / `--h12` anywhere in the arguments select the robot (`ros` takes no robot flag) | Container names: `golem_ros`, `golem_sim_robocasa`, `golem_sim_isaac`, for either robot. Simulator profiles print `robot: h12` or `robot: g1` before the domain check. | + +Paired startup: `docker/scripts/docker_stack.sh up robocasa|isaac [--headless|--gui] [--g1|--h12]`. +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 from a +simulator of the selected robot (`wait_for_sim.py`). The bringup it starts is +`golem_bringup h12_sim.launch.py`, or `golem_bringup +g1_sim.launch.py` for the G1 (with only `use_rviz` set). +`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` (incrementally, on every start) and **drops to a shell — it does not +launch bringup**. + +### Robot selection (x86 only) + +`docker/scripts/docker_common.sh` owns this. On simulator and stack wrappers, +the robot is `--g1`/`--h12` on the wrapper, else `GOLEM_ROBOT` from the shell, +else from `docker/.env`, else `h12`. The selection means "which robot is +attached" and matters only where that is inherent: which robot the RoboCasa sim +runs, and which bringup `docker_stack.sh` starts and which sim +`wait_for_sim.py` expects. Every `ros` container serves both robots (`docker_run.sh ros` +takes no `--g1`/`--h12` flag and uses `core_ws/cyclonedds.xml` directly): the +base compose file bind-mounts `core_ws/src/g1_homie/policy` (at +`/home/code/g1_homie`, HOMIE's policy) and `tools/` (at `/home/code/tools`, for +`read_lowstate.py`) read-only into the `ros` service (the sim reads none of +HOMIE's files), and `CL_Assets/g1` into both services. For `g1` the simulator +and stack wrappers: + +- add `docker/docker-compose.g1.yml` to the `robocasa` service: it sets + `GOLEM_ROBOT=g1` and passes the G1 sim knobs (§8); +- check in `prepare_mounts` that the G1 sim's `CL_Assets/g1` files (its MJCF, + URDF and meshes) exist and are not LFS pointers, before any container starts + (every `ros` container, either robot, gets the same check of the + `CL_Assets/g1` files `golem_ros2_models` installs and of the policy files); +- refuse the Mac wrappers and the Isaac simulator. + +`launch_ros.sh` builds every package in `core_ws`, the G1's included, +whatever `GOLEM_ROBOT` says, so one `ros` container runs either robot's +bringup. `golem_ros2_models` installs both robots' models from `CL_Assets`, and +`golem_upper_ik` loads the G1's only when a node runs with `robot:=g1`. Both +robots use the same images and container names, so the wrappers run one robot +at a time on a host. + +### 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). | + +The Mac wrappers run the H1-2 only; they refuse `--g1` and `GOLEM_ROBOT=g1`. +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|almi`, +`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; a task env can set its own via +`spawn_margin`, as `battery_env.py` does) sets the H1-2's minimum spawn +clearance from non-floor geoms on both platforms. `GOLEM_CAMERA_SCALE=` +(H1-2 default 0.5 of the real 1280x720), `GOLEM_CAMERA_DEPTH=0` and +`GOLEM_CAMERA_POINTCLOUD=0` tune the H1-2's RoboCasa cameras on both platforms. +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 robots' 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 `golem_skills`, and +`GOLEM_ROBOT`). + +The real G1 is commanded from the laptop wired to it, through +`docker/scripts/docker_run.sh ros` on domain 0 (the H1-2 runs natively on +its onboard PC). Both robots use the default CycloneDDS configuration +(`core_ws/cyclonedds.xml`) as is. + +**One robot stack per DDS domain.** The two stacks share node, action and DDS +topic names, with identical `unitree_hg` types. Each robot's sims report its +real robot's `rt/lowstate` `mode_machine`: 6 for the H1-2, 5 for the G1. +`wait_for_sim.py` refuses a simulator of the other robot by that value, but it +runs only in `docker_stack.sh` and the mac stack (`GOLEM_WAIT_FOR_SIM=1`) and in +`tests/stack`. `docker_run.sh ros` followed by a manual `ros2 launch` is not +guarded by the wrappers. `golem_safety` checks `rt/lowstate`'s value too (§8), +but that does not replace one stack per domain: the stacks share the +`rt/safety/*` input topics and the node and action names, `golem_safety` does +not check which robot an input command was written for, and `golem_upper_ik` +does not check `rt/lowstate`'s `mode_machine`. + +## 4. `core_ws/src` packages + +23 ROS packages in 20 top-level directories: 21 (the H1-2's, the shared ones +and the six shared `golem_*` packages) and the 2 G1 packages +(below). 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 | +|---|---|---|---| +| `golem_safety` | **safety (both robots)** | C++. The only writer of `/lowcmd` (§8): streams zero gains from boot, engages each `/safety/*` input on its own first valid command and relays it, clips it, checks every `/lowstate` (robot identity, estop limits) and latches an estop until restart. A robot profile (`robots/.yaml`, the joint table both `golem_*` packages read) and configs per robot (`config/h12/`, `config/g1/`) | `safety_node` (`robot`, `config`, `sim`, `use_estop` parameters); `launch/safety.launch.py` | +| `golem_upper_ik` | **controller (upper body, both robots)** | Pink IK on the 14 arm joints with the torso (H1-2) or waist (G1) held; OMPL planning and a self-collision barrier from each robot's sphere model and SRDF (the G1's spheres were packed on the rubber-hand G1: none on the Dex3-1 fingers; its torso spheres stand inside the torso's flanks, so `home` keeps the H1-2's 20 mm `d_min`; its `arms_down`, where the real hands rest on the hips, is in self-collision); the follow gate; the safety/controller config pairing (`golem_upper_ik.pairing`) the bringups check | `frame_task_server` (`/frame_task`, `/named_config`), `joint_state_publisher`; `launch/upper_ik.launch.py` | +| `golem_bringup` | **bringup (both robots)** | Launch-only package that starts the whole H1-2 (`launch/h12/`) or G1 (`launch/g1/`), sim or real. See §6. | top-level + building-block launch files, `config/h12/*.yaml` | +| `golem_ros2_models` | **description (both robots)** | Installs both robots' models from `CL_Assets`: the H1-2 with its Magpie grippers and the G1 with its Dex3-1 hands, each a Pinocchio URDF, a ROS copy with `package://golem_ros2_models/...` URIs, the meshes, and a sphere-packed self-collision model with its SRDF. No nodes and no joint table (that is `golem_safety`'s robot profile) | `golem_ros2_models.paths`; `launch/display.launch.py` (`robot:=h12|g1`) | +| `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 and ALMI stand/walk policies; controller runs one pinned policy; consumes `/lowstate` + `/cmd_vel`, emits 12-joint PD setpoints | `walking_node`, `fame_node`, `lowerbody_controller_node` | +| `lmg_deploy` | controller (whole body, MARL) | The loco-manipulation game's two-agent (legs + IK arms) MARL policy, run by the `lmg_*` bringups | `marl_deploy_node` | +| `golem_slam` | SLAM + navigation (both robots) | Shared navigation for both robots (FAST-LIO deskewed cloud → self filter → floor alignment/OctoMap → slam_toolbox → Nav2 MPPI), the G1's dynamic waist transform (`g1_body_to_pelvis`), and its configs: Livox MID360 driver config (`MID360_config.json`, real robot, read by `h12_real_drivers.launch.py`), FAST-LIO (`mid360.yaml`, `g1_fastlio*.yaml`), slam_toolbox (`slam_toolbox_h1.yaml`, `slam_toolbox_g1.yaml`), Nav2 (`nav2_config*.yaml`) | `launch/h12_navigation.launch.py`, `launch/g1_navigation.launch.py`, `g1_body_to_pelvis`, `config/` | +| `golem_skills` | **skills (both robots)** | 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 (both robots) | 19 actions (13 `Skill*`, `FrameTask`, `NamedConfig`, `DualArm`, …), msgs, srvs (`GeminiQuery`, `SamSegment`, `GraspGen`, `SetHandPose`, …) | 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 (used by `golem_slam`) | `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 | + +### The G1 packages + +Built in every `ros` container, for either robot (§3). Their interfaces are in +`custom_ros_messages`; each package has a README with the +details, and `docs/g1/packages.md` indexes them. + +| Directory | Role | What it does | Key entry points | +|---|---|---|---| +| `g1_homie` | controller (lower body) | HOMIE's walking policy (onnxruntime) on the 12 leg joints, driven by `/cmd_vel` through `golem_safety`'s split mode. Follows the H1-2's leg node: waits for `/safety/heartbeat`, pre-poses the legs, on the real robot holds until `/lowerbody/confirm_engage`, and in the sim releases the elastic band | `locomotion_node` | +| `dex_3` | driver (hands) | Dex3-1 hands on `rt/dex3//*`: `//hand/{set_pose,open,close,release}`; bypasses the safety layer | `dex3_hands`; `launch/hands.launch.py` | + +Third-party/vendored (do not refactor): `FAST_LIO`, `livox_ros_driver2`, +`unitree_ros2`, plus the top-level `unitree_sdk2_python` and `mujoco_mpc`, +Unitree's G1 models in `CL_Assets/g1` (kept byte-for-byte upstream; its +`README.md`), and HOMIE's policy in +`core_ws/src/g1_homie/policy/` (non-commercial licence, §8). + +## 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, the G1 packages, …) 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 — `golem_bringup`, `custom_ros_messages`/`magpie_msgs` + (both robots use `custom_ros_messages`), `docker/`, `golem_safety` and + `golem_upper_ik` (both robots run them), 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 + +### The H1-2 (`golem_bringup`, `launch/h12/`) + +`golem_bringup` is launch-only. Sim vs real is **separate top-level launch files** +(no top-level file takes a `sim:=` argument); each one only includes the shared +building blocks below and sets `use_sim_time` plus its platform's arguments, +including `h12_control.launch.py`'s `sim`, which has no default. + +| Top-level launch file | Scenario | Includes | +|---|---|---| +| `h12_sim.launch.py` | **x86 sim** (`use_sim_time=true`), plus the camera optical-frame static TF and rviz (`use_rviz`) | state, control (`sim:=true`, `sim_safety_split` for both `safety_node` and `frame_task_server`, no stagger), lowerbody (`lowerbody_sim.yaml`), models (`use_skills`), navigation (`use_nav`) | +| `h12_sim_mac.launch.py` | **mac sim** — standalone, includes none of the blocks: `robot_state_publisher` and `golem_upper_ik`'s `joint_state_publisher` and `frame_task_server`, `golem_safety`'s `safety_node` (`sim: true`, both on `sim_safety_split`); lower body/SLAM/nav2 gated by `GOLEM_*` env vars, not launch args | — | +| `h12_real_robot.launch.py` | **real robot, onboard PC** (native, `ROS_DOMAIN_ID=0`) | real_drivers, navigation, state, control (`sim:=false`, `use_estop:=true`, the defaults `relax_safety_split` and `safety_split`), lowerbody (`lowerbody_real.yaml`, no estimator). Leg control is interlocked behind `start_position_verified:=true` (default **false**) | +| `h12_real_desktop.launch.py` | **real: companion x86 desktop** — rviz, plus the hand cameras with `use_hand_cameras:=true` (default **false**) | models (`use_yolo:=true`) | +| `h12_safety_grippers.launch.py` | real: actuation-only debug bringup | control (`sim:=false`, estop + safety, no IK), grippers | +| `lmg_sim.launch.py` | **x86 sim**, loco-manipulation game: the `lmg_deploy` MARL policy (Explicit500) owns all 27 joints, no operator confirm. Refuses an unset or 0 `ROS_DOMAIN_ID` | state, control (`sim:=true`, no IK, no stagger), locomanipulationgame, navigation (`use_slam`) | +| `lmg_real.launch.py` | **real, onboard PC**, loco-manipulation game, run *instead of* `h12_real_robot`. Policy behind `start_position_verified:=true` + `/marl_deploy_node/confirm_engage` | real_drivers, state, control (`sim:=false`, `use_estop:=true`, no IK), locomanipulationgame, navigation (`use_slam`) | + +| Building block | Starts | +|---|---| +| `h12_state.launch.py` | `golem_upper_ik`'s `joint_state_publisher` (`robot: h12`, the 27 body joints from `/lowstate`) + `robot_state_publisher` (`urdf_file` arg) | +| `h12_control.launch.py` | [`estop_node`] → `golem_safety`'s `safety_node` (`robot: h12`, `safety_config`, `sim`, `use_estop`) → `golem_upper_ik`'s `frame_task_server` (`robot: h12`, `frame_task_config`; `use_frame_task`), staggered by `safety_delay` (3 s) / `frame_task_delay` (5 s). `sim` has no default: a launch without it fails before any node starts. With `use_frame_task` it refuses a `frame_task_config` that does not run with `safety_config` (`golem_upper_ik.pairing`) before starting anything | +| `h12_lowerbody.launch.py` | RL `lowerbody_controller_node` (`lowerbody:=`, params from `lowerbody_params`) and optional MJPC `estimator_node`, both behind `start_position_verified` | +| `h12_models.launch.py` | gemini + sam servers, [yolo], graspgen + `golem_skills` (`use_skills`) | +| `h12_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, `h12_grippers.launch.py` (left+right `gripper_node`) | +| `h12_locomanipulationgame.launch.py` | `marl_deploy_node` (behind `start_position_verified`; holds arm goals in the world by the SLAM `/Odometry`) | +| `golem_slam/launch/h12_navigation.launch.py` | shared nav stack (FAST-LIO → pointcloud_to_laserscan → slam_toolbox → nav2) | + +`golem_slam` also ships one top-level file of its own, `h12_real_slam.launch.py` +(`ros2 launch golem_slam h12_real_slam.launch.py`): real robot sensors + SLAM/navigation +only, on wall time — real_drivers, state, navigation; no estop, safety or IK, so +no joint is commanded. Record its runs with `ros2 run golem_slam record_slam_bag --name `. + +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`). + +**The H1-2's start-up** (`h12_control.launch.py`; `golem_safety`'s and +`golem_upper_ik`'s READMEs have the details): + +- `safety_node` streams zero-gain `/lowcmd` from boot. It engages each input on + that input's first valid command once `/lowstate` reports `mode_machine` 6, + and, with `use_estop:=true`, once a first hardware estop sample has arrived; + until then it logs `engage_refused` with the reason, at most once a second. +- **`/safety/heartbeat` means relaying can begin.** `safety_node` publishes no + beat until those two conditions hold; then it beats true at 10 Hz, and false + once its estop latches (false from the start if it latched first). + `h12_lowerbody_rl` pre-poses the legs only from the first beat, and + `lmg_deploy` and `golem_skills` read it too. +- **`frame_task_server` needs `golem_safety` relaying.** It waits up to + `controller.state_timeout` (5 s) for a fresh `/lowstate`, then holds the + measured pose until it sees its commands on `/lowcmd` (the follow gate), for + up to `controller.startup_timeout` (30 s). If `golem_safety` has not relayed + them by then, it sends one zero-gain command and exits 1 with an error that + lists the causes: no `/lowstate` reporting `mode_machine` 6 (an H1-2 sim + started before the sims reported 6), no first estop sample with + `use_estop:=true`, a safety config that does not pair with its controller + config, or an estop latched at this start-up (the limit checks run from + boot, so a joint already past `limits.estop` latches it) or in an earlier + run. The other nodes keep running, without `/frame_task` and + `/named_config`. +- **`use_estop`.** `use_estop:=true` (the real bringups and + `h12_safety_grippers`) starts `estop_node` and has `safety_node` read + `/estop/triggered` and `/estop/plugged_in`: no input engages before the first + sample, a sample that says triggered or unplugged latches the estop until + restart, and so does no new sample for `estop.timeout_seconds` (0.5 s) once + the first has arrived. With `use_estop:=false` (the sim bringups) those topics + are ignored. +- **The real robot's estop start-up.** `estop_node` publishes nothing until its + first attempt to connect ends; that attempt probes every serial port it finds + (up to about 3.25 s for each port that opens, so another serial device such as + a gripper board lengthens it). When an attempt fails, it publishes unplugged + and triggered until a retry, once a second, connects. **A `safety_node` that + is running while `estop_node` reports a failed connection latches until + restart**: when the first attempt fails and no retry has connected by + `safety_delay` (3 s), or the first attempt outlasts `safety_delay` and then + fails. A first attempt that outlasts `safety_delay` and then connects only + delays the start: no input engages and no heartbeat goes out until its first + sample, so the legs pre-pose and the arms start late. `safety_delay` must + cover the time `estop_node` takes to connect (the comment in + `h12_control.launch.py`). + +**Wiring a new package into bringup:** add it as `exec_depend` in +`golem_bringup/package.xml`, add a `launch_ros.actions.Node(...)` entry to the +relevant launch file (put node params in `golem_bringup/config/h12/.yaml` — the +top-level YAML key must match the node `name`), and gate optional nodes with +`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 `h12_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; a package the Mac does not build goes into `BRINGUP_STUB_DEPS` +there instead, or the Mac build of `golem_bringup` fails. For the G1 the node +goes into `launch/g1/g1_core.launch.py`, with the same `exec_depend`. + +### The G1 (`golem_bringup`, `launch/g1/`) + +Two top-level files include one shared core, `g1_core.launch.py`, with `sim` +and `use_sim_time` set for their platform. The docstring of each file is its +reference. + +| Launch file | Scenario | Starts | +|---|---|---| +| `g1_sim.launch.py` | **G1 sim** (sim time; `sim_split` for `golem_safety` and the controller, `g1_homie`'s `sim.yaml`). Always starts HOMIE's walking node. `use_nav` (default true) adds shared navigation (`golem_slam`, `odom_source:=fastlio` only), `use_skills:=true` starts `golem_skills` (including `/skill/frontier_explore`) | `g1_core` | +| `g1_real_robot.launch.py` | **real G1** in Unitree's debug mode, every node in the laptop's `golem_ros` container on domain 0 (`split` and `g1_split`, `g1_homie`'s `g1.yaml`). The walking node only with `start_position_verified:=true` (default **false**): without it the leg motors stay off and the arms run. `use_nav` (default true) adds the MID-360 driver and `golem_slam`; no skills yet | `g1_core` | +| `g1_core.launch.py` | shared core: `robot_state_publisher`, `golem_upper_ik`'s `joint_state_publisher` (`robot: g1`), `golem_safety`'s `safety_node` (`robot: g1`, `safety_config`, `sim`, `use_estop: false`: the G1 has no estop node), then `golem_upper_ik`'s `frame_task_server` (`robot: g1`, `controller_config`) after `controller_delay` (2 s), then `g1_homie`'s `locomotion_node` (`locomotion_config`) after `locomotion_delay` (2 s more: always in the sim, with `start_position_verified` on the real robot); [`dex3_hands`] (`use_hands`), [rviz] (`use_rviz`), [`golem_slam`] (`use_nav`), [`golem_skills`] (`use_skills`) | — | + +The core refuses to start on a bad combination; `startup_refusal` in +`g1_core.launch.py` and `pairing_refusal` in +`core_ws/src/golem_upper_ik/golem_upper_ik/pairing.py` are the rules: + +- **Config pairing.** The safety config must relay the upper-body channel + (`split` or `sim_split`; `full` is refused), and each runs with exactly one + controller config (`G1_SAFETY_CONTROLLER_CONFIGS` in `pairing.py`: + `g1_split`, `sim_split`), which `controller_config` defaults to. The real + robot refuses every `sim_*` config and the walking node's `sim` config, and + the sim refuses the real ones (`split`, `g1_split`, the walking node's `g1`). +- **The leg interlock.** On the real G1 the walking node starts only with + `start_position_verified:=true`, set only after checking the robot is in a + safe start pose; the argument refuses nothing. The bringup writes `rt/lowcmd` from the + moment `safety_node` starts, so the operator puts the G1 into Unitree's debug + mode, on a gantry, before launching, a manual step that nothing in this stack + performs or checks (its procedure is not written yet: + `docs/g1/first-session.md`). The gantry carries the robot while the legs are + off, and the split estop turns every motor off. +- **Navigation.** `use_nav` (default true on both G1 bringups, as on the + H1-2's) adds `golem_slam`; on the real G1 it also starts the MID-360 driver + (`launch/g1/g1_real_drivers.launch.py`, `golem_slam/config/g1_MID360_config.json`) + and FAST-LIO on `g1_fastlio.yaml`. Not yet run on the real G1. + +**The G1's start-up** (`g1_core.launch.py`'s docstring; `golem_safety`'s, +`golem_upper_ik`'s and `g1_homie`'s READMEs have the details): + +- `safety_node` streams zero-gain `/lowcmd` (every motor off) from boot, checks + every `/lowstate` against the estop limits from the first sample, and engages + each input on that input's own first valid command once `/lowstate` reports + `mode_machine` 5, as on the H1-2; until then it logs `engage_refused`. Its + heartbeat is the H1-2's: none until `/lowstate` reports 5, then true at + 10 Hz, false once latched. +- `frame_task_server` holds the measured pose until its follow gate sees + `golem_safety` relay its commands, which happens on its first command, with + or without the walking node; then it moves the arms to `home` and the waist + to its hold. It stops with an error if `/lowstate` is not fresh within + `controller.state_timeout` (5 s), if nothing is relayed within + `controller.startup_timeout` (30 s), or if the moves take longer than + `controller.startup_move_timeout` (30 s). The leg motors stay off until the + walking node commands them. +- `locomotion_node` sends nothing until the first true `/safety/heartbeat` and + an upright `rt/lowstate`, as the H1-2's leg node. Then it moves the legs to + HOMIE's default pose over 2.0 s and holds it until 1 s of steady, on-time + `rt/lowstate` (its tick within `startup.tick_rate` of the wall clock) and the + arm controller's commands. On the real robot (`g1.yaml`) it also holds until + `ros2 service call /lowerbody/confirm_engage std_srvs/srv/Trigger` (the H1-2 + leg node's service); in the sim (`sim.yaml`) the policy then starts, under + the sim's elastic band, and the node releases the band through + `/elastic_band/toggle` once `/left_ee_pose` has come (`frame_task_server` + publishes it after its start-up moves), or `band.max_wait` (30 s) after the + policy started. A false heartbeat, or none for 0.5 s after the first, stops + it for good in every phase. +- After any estop, restart the whole bringup. + +## 7. Skills (`golem_skills`) — the LLM-callable layer + +Each skill is a mixin class in `golem_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" | + +`custom_ros_messages` also defines `SkillRetrieveMemory`; no node in this +repo serves it yet. On the G1, `golem_skills` starts when `use_skills:=true` +is passed to `g1_sim.launch.py` (with `wait_for_services: false` so it starts +immediately without waiting for H1-2 vision/gripper services), serving +`/skill/frontier_explore`. + +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 `golem_robocasa` should generally not be modified unless +necessary, and their **default behaviors must be preserved**. Everyone's +sim-vs-real workflow depends on them behaving like the real robot. +`golem_robocasa/h12_mujoco.py` is the one RoboCasa entry point for both robots: it +picks the robot (`--h12`/`--g1`, else `GOLEM_ROBOT`, else `h12`) before its +H1-2 imports and hands the G1 over to `golem_robocasa/g1/g1_mujoco.py`. The H1-2 +stays the default, and nothing on the G1 path may change its behaviour. The robots share +`sim_common.py`, `measurement_bridge.py` and the `mujoco_sensors` node in +`mujoco_ros_bridge.py` (`sim_timing.py` is the H1-2's only); each has its own sensor suite +(`h12_sensors.py`, `g1/g1_sensors.py`) and Unitree DDS interface +(`unitree_interface.py`, `g1/unitree_interface.py`). + +### 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-2 (`golem_robocasa/h12_mujoco.py`)** — publishes `/clock`, the real +RealSense drivers' topics, frame names and camera TF (`/tf_static` + `/tf`) for +`/realsense/{head,left_hand,right_hand}` — colour, depth and aligned depth with +their camera_info, and `depth/color/points` — as captured from the robot in +`golem_robocasa/realsense_profile.yaml`, `/livox/lidar` (CustomMsg), `/livox/pointcloud`, `/livox/imu`, +`/{left,right}/gripper/state`, and `rt/lowstate` over Unitree DDS (with the +real robot's `mode_machine`, 6); consumes `rt/lowcmd` and the +`/{left,right}/gripper/*` services/DeliGrasp action. +(Trust the code over any topic list repeated elsewhere: the sensor suite in +`h12_sensors.py`, the grippers in `magpie_hand_bridge.py`, `/clock` and the +opt-in odom in `mujoco_ros_bridge.py`, Unitree DDS in `unitree_interface.py`.) + +**RoboCasa, G1 (`h12_mujoco.py --g1`, `golem_robocasa/g1/`)** — publishes `/clock`, +the Livox MID-360 as its driver does in CustomMsg mode (`/livox/lidar`, +`/livox/imu`, no `/livox/pointcloud`), the head RealSense D435 +(`/realsense/head/*` and its `/tf_static`), and over Unitree DDS `rt/lowstate` +and `rt/dex3/{left,right}/state`, and serves `/elastic_band/toggle` +(`std_srvs/srv/Trigger`, as the H1-2 sim does); consumes `rt/lowcmd` and +`rt/dex3/{left,right}/cmd`. It plays the robot in Unitree's debug mode: every +motor is off until the first `rt/lowcmd` with a valid CRC, `mode_machine` 5 +and `mode_pr` 0, and an elastic band on `torso_link` holds the robot until the +toggle (or SPACE in the viewer) releases it; the G1 bringup's walking node +calls it. It reads none of HOMIE's files. (Trust `g1/g1_sensors.py` for +the ROS side, `g1/unitree_interface.py` for the DDS side and `g1/g1_mujoco.py` +for the band.) Its knobs are `GOLEM_LIDAR`, `GOLEM_LIDAR_POINTS`, +`GOLEM_LIDAR_HZ`, `GOLEM_CAMERAS`, `GOLEM_CAMERA_HZ`, `GOLEM_CAMERA_SCALE` (G1 +default 0.5), `GOLEM_CMD_TIMEOUT`, `GOLEM_SPAWN_BACKOFF`, `GOLEM_SIM_ODOM`, +`GOLEM_SIM_EXPERIMENT`. Their defaults are in the code +that reads them: `SensorConfig` in `g1/g1_sensors.py` (sensors), +`g1/unitree_interface.py` (the command timeout), `g1/g1_mujoco.py` +(spawn, odom) and `measurement_bridge.py` (experiment); the compose files pass them +(`docker/docker-compose.g1.yml` the G1-only ones). The G1 sim ignores the H1-2-only +knobs (`GOLEM_SPAWN_MARGIN`, `GOLEM_SPAWN_OFFSET`, `GOLEM_HAND_CAMERA_RES`, +`GOLEM_CAMERA_DEPTH`, `GOLEM_CAMERA_POINTCLOUD`, the tether ones). It refuses +`--timing`, and the H1-2 sim refuses `--duration` (`sim_common.py`). + +The measured surface of each RoboCasa robot is checked in as a golden: +`tests/robocasa/surface/{h12,g1}.json`, compared by +`tests/robocasa/surface/run.sh h12|g1` (see its README). A change that alters +what the sim publishes re-records the golden in the same commit. + +**Isaac (`CL_isaaclab_sim/sim_main.py`, H1-2 only)** — publishes `rt/lowstate` +(`mode_machine` 6) 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 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 + +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, for both robots:** ground-truth `/odom` + `odom→pelvis` + TF is gated by `GOLEM_SIM_ODOM` (default `'0'` = off, read in `h12_mujoco.py` + and `g1/g1_mujoco.py`). Only the mac nav demo opts in; the G1 bringup and tests refuse it. Keep it that way. The + experiment surface (`/sim/ground_truth`, `/sim/reset_objects`) is gated by + `GOLEM_SIM_EXPERIMENT` (`measurement_bridge.py`). The G1 acceptance tests + record the robot's state inside the sim process instead of publishing it. +- **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`), for both + robots. 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". +**Stale, not re-verified:** both Dockerfiles check out Livox-SDK2 at `08f523c` +(`RosDockerfile` §2, `RobocasaDockerfile`); the unpinned `master` clone +described in this paragraph is gone. The pin rule above stands. + +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' # h12_sensors.py, or g1/g1_sensors.py for the G1 +``` + +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 + +- **`golem_safety` is the only writer of the body command topic, on both + robots.** Its `safety_node` is the only publisher of `rt/lowcmd` (ROS + `/lowcmd`), which drives every body motor (the G1 runs in Unitree's debug + mode). + Controllers publish only to its `rt/safety/*` inputs: `golem_upper_ik` and + `g1_homie` refuse a command topic outside them. The exception is + `dex_3`, which writes `rt/dex3//cmd` directly, with no safety layer. +- **Quiet inputs are held and reported, not estopped.** Once an input has + engaged, an input with no new command for its `stale.*_seconds` keeps its + last command relayed (`input_quiet`, then `input_resumed`), and a quiet + `rt/lowstate` leaves the relay running (`state_quiet`, `state_resumed`). This + includes the G1's split mode: a walking-node fault silences + `locomotion_node`, and `golem_safety` keeps relaying its last leg command, + which does not balance the robot, instead of turning every motor off. +- **The estop latch** is one way until restart. It latches on a command with a + non-finite value, a second live writer on an input (a new writer is accepted + after `single_writer.handover_seconds`, 0.5 s, of quiet), a measured joint + past `limits.estop` or not finite (checked from boot, on both robots), another + robot's `mode_machine` once an input has engaged, the transport-lag check + (`control.state_lag_ms`, on domain 0; off in every G1 config), and with + `use_estop` a hardware estop that is triggered, unplugged or silent. What it + writes comes from the config's `estop_response`: + - H1-2 (all 8 configs): 50 zero-gain commands (mode 0, every value 0), then + no `rt/lowcmd` until restart. + - G1 (all 3 configs): the estop command, written every cycle until restart + (`mode: zero`, `after: keep_writing`: every motor off, so the robot drops; + support it). +- **Robot identity.** `golem_safety` checks every `rt/lowstate` against its + robot profile's `mode_machine`, 6 for the H1-2 and 5 for the G1: no input + engages without it, another value latches the estop once engaged, and every + command it writes carries the profile's `mode_machine`. Each robot's sims + report the real value, and `wait_for_sim.py` checks the same values (§3). +- **The controller's exit is one last command.** On SIGINT or SIGTERM, or a + start-up that fails after its first command, `golem_upper_ik`'s + `frame_task_server` sends its upper-body slots one command with mode 0 and + zero kp, kd and tau on the IK joints, then stops publishing. The held joints + get the same, except on the G1 (the profile's + `shutdown_keeps_held_joints_on_lowcmd`). On the H1-2 `golem_safety` relays + it, so the torso and arms go limp and stay so (the quiet input keeps its last + command relayed). On the G1, in split mode (the only one its bringups run), + it relays it too: the waist keeps its hold angle, gains and torque and only + the arms go limp, while the walking node keeps the legs. No estop latches. +- **`golem_safety`'s own exit** (SIGINT, SIGTERM, SIGHUP): it stops publishing + at once (an estop burst in progress is finished first, and with + `keep_writing` the estop command is written once if none has been since the + latch). Once `rt/lowcmd` falls silent, the sims turn every body motor off + after `GOLEM_CMD_TIMEOUT` (0.5 s of sim time); what the real robots do then + has not been measured. +- **One robot stack per DDS domain** (§3). Never run the H1-2's and the G1's + stacks on one domain. +- **HOMIE's policy is non-commercial.** `core_ws/src/g1_homie/policy/` + holds OpenHomie's pretrained policy, CC BY-NC 4.0 by its `LICENSE` (its + README says CC BY-NC-SA 4.0): treat it as CC BY-NC-SA 4.0, unlike the MIT + G1 packages. Keep its `LICENSE` and `README.md` beside the files, and keep + `homie_policy.py` inside `policy/`. +- 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), each through a lock step (`-c /opt/golem-locks/.txt`, + enforced by `tests/docker/test_source_pins.py`). 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/deploy_mjpc`. + +## 9. Running the robot — the standard three-terminal flow + +Everyone runs their package through bringup, which starts the whole robot. + +### x86, the H1-2 + +```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 golem_bringup h12_sim.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 +``` + +### x86, the G1 + +```bash +# Paired: the sim plus g1_sim.launch.py (the arms and HOMIE), gated on the sim's robot +docker/scripts/docker_stack.sh up robocasa --g1 --headless +docker/scripts/docker_stack.sh logs robocasa + +# Or three terminals, to choose the bringup's arguments (the wrappers do not +# check the sim's robot here; golem_safety's own check is in §8) +# Terminal 1 — the sim; wait for "[g1_mujoco] ROS bridges up" +docker/scripts/docker_run.sh robocasa --g1 --layout 9 --style 9 --seed 42 +# Terminal 2 — builds core_ws (both robots' packages), drops to a shell; no +# --g1 needed: the ros container runs either robot's bringup +docker/scripts/docker_run.sh ros +ros2 launch golem_bringup g1_sim.launch.py # wait for the walking node's "elastic band released" +# variants: use_rviz:=false use_hands:=false use_nav:=false use_skills:=true + +# Terminal 3 +docker exec -it golem_ros bash +source /opt/ros/humble/setup.bash && source /home/code/core_ws/install/setup.bash +ros2 action send_goal /named_config custom_ros_messages/action/NamedConfig \ + "{config_name: 't_pose', plan: false, duration: {sec: 0, nanosec: 0}}" +``` + +Walking and navigation need the spawn backed into open floor +(`GOLEM_SPAWN_BACKOFF=2.0` before terminal 1's command); `docs/g1/walking.md`, +`navigation.md` and `exploration.md` have those flows. The closed-loop checks +are `tests/g1/test_sim_bringup.sh [--walk]`, `tests/g1/test_sim_nav.sh` and +`tests/g1/test_sim_explore.sh` (`tests/g1/README.md`). + +### 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/golem_bringup/launch/h12/h12_sim_mac.launch.py` +(launched by path; only skills built on mac are available — no `golem_skills`, +`model_server`, or nav unless `GOLEM_SLAM/GOLEM_NAV2` are set). + +### Real robot (for awareness — coordinate before touching) + +H1-2: onboard PC, native (no docker): build the whole workspace, as +`launch_ros.sh` does (the G1's packages included: `golem_bringup` +exec_depends on them). The build +includes `golem_ros2_models`, which installs both robots' models from the +checkout's `CL_Assets` (its LFS meshes pulled): the H1-2's launches read the +robot description from `golem_ros2_models`, not from `CL_Assets` directly. +`export ROS_DOMAIN_ID=0`, source +`core_ws/install/setup.bash`, then +`ros2 launch golem_bringup h12_real_robot.launch.py` +(`start_position_verified:=true` only after physically verifying the robot's +pose — it gates leg commands; the policy then holds its pre-pose until +`/lowerbody/confirm_engage`). Companion desktop: +`ros2 launch golem_bringup h12_real_desktop.launch.py`. The arms start only +once `golem_safety` relays `frame_task_server`'s commands, which needs the estop +box connected: a `safety_node` that starts while `estop_node` reports a failed +connection latches until restart, and `frame_task_server` exits after +`controller.startup_timeout` (30 s) when nothing is relayed (§6). + +G1: in Unitree's debug mode, on a gantry; the procedure for entering debug +mode on our G1 is not written yet, and nobody runs a session until it is +(`docs/g1/first-session.md`). The laptop wired to the robot, in Docker: +`ROS_DOMAIN_ID=0` in `docker/.env`, `docker/scripts/docker_run.sh ros`, then +`ros2 launch golem_bringup g1_real_robot.launch.py use_hands:=false use_rviz:=false` +— the arms; the leg motors stay off. `start_position_verified:=true` adds +HOMIE's walking node, which pre-poses the legs and holds them until +`/lowerbody/confirm_engage`. Nothing has run on the real G1 yet; follow +`docs/g1/first-session.md` step by step. + +## 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/`; the + G1 starts at `docs/g1/index.md`. + +### 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). +- **Restart the sim containers after pulling.** The sims run the checkout's + `golem_robocasa` and `CL_isaaclab_sim` from bind mounts, so a running sim keeps + the code it started with. An H1-2 sim started before the H1-2 sims reported + `mode_machine` 6 is refused: `wait_for_sim.py` stops `docker_stack.sh`, and + `golem_safety` never engages (`engage_refused`: `/lowstate mode_machine is 0, + expected 6`), so `frame_task_server` exits after 30 s. For Isaac, run + `git submodule update` first: the H1-2 Isaac sim reports 6 from the current + `CL_isaaclab_sim` pin. +- **Delete the stale installs of removed packages once after pulling.** colcon + never removes a package that is gone from `core_ws/src`, so a workspace built + before `golem_bringup` replaced `h12_bringup` and `g1_bringup` (and the four + unused packages `h12_safety_layer`, `h12_ros2_controller`, `g1_safety_layer` + and `g1_control` were deleted), or before `golem_ros2_models` replaced + `h12_ros2_model` and `g1_description`, or before `dex_3` and `g1_homie` + replaced `g1_dex3` and `g1_locomotion`, keeps their build and install + directories. + From the repo root, on every host that built the workspace (`sudo` for files + a container left root-owned): + `rm -rf {core_ws/build,core_ws/install,core_ws/build_stack_tests,core_ws/install_stack_tests,container_cache/g1_tests/build,container_cache/g1_tests/install}/{h12_bringup,g1_bringup,h12_safety_layer,h12_ros2_controller,g1_safety_layer,g1_control,h12_ros2_model,g1_description,g1_dex3,g1_locomotion,h12_slam,h12_deploy_mjpc,h12_skills,g1_navigation}`. + Until then `launch_ros.sh` (`docker_run.sh ros`, `docker_stack.sh`, + `tests/g1`) exits before it builds, with `stale installed package '' has + no discovered source`, and a native workspace (the H1-2's onboard PC) keeps + answering to the old names. A `--symlink-install` workspace also keeps links + to deleted G1 configs that fail the next `golem_upper_ik` build; the change + report (`docs/superpowers/specs/2026-10-28-golem-bringup-and-g1-debug-mode/change-report.md`) + has that cleanup and the Mac's. +- **x86 `ros` container ≠ bringup.** It only builds and gives you a shell. +- **Check the `robot:` line on simulator and stack runs.** `docker_run.sh robocasa|isaac` + and `docker_stack.sh up` print the robot they selected; a `GOLEM_ROBOT` left + in the shell or `docker/.env` switches simulator robots silently otherwise. + Stop one robot's containers before starting the other's. +- 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. + +### Navigation/package migration (2026-10-29) + +`h12_slam`, `h12_deploy_mjpc` and `h12_skills` directories and Python modules +are now `golem_slam`, `deploy_mjpc` and `golem_skills`, and `g1_navigation` was +removed (`g1_body_to_pelvis` moved into `golem_slam` and `/skill/frontier_explore` +unified in `golem_skills`). Clean their old build/install prefixes as documented +in `docs/RUNNING.md`. G1 bringup uses the shared SLAM pipeline with G1 profiles +(scan matching/loop closure, OctoMap, MPPI). The G1 launch requires FAST-LIO and +refuses real mode and sim_gt. H1-2 defaults are preserved. G1 floor alignment +uses a 0.5 m minimum floor depth, and its dynamic base transform is refreshed per +odometry sample. Do not infer real G1 validation from simulated navigation checks. diff --git a/CLAUDE.md b/CLAUDE.md index 93ab5c9..38c030d 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -1,34 +1,54 @@ # CLAUDE.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). +The safety layer and the arm controller of both robots (`golem_safety`, +`golem_upper_ik`), the bringup package (`golem_bringup`) and the start-ups it +runs, the models (`golem_ros2_models`, `CL_Assets/g1`), the robot selection, the G1 text (its packages, sim, start-up and +invariants), the `mode_machine` identity check, the package tables and the +stale-install pitfall were checked against the code on 2026-10-28; the +DDS-domain rules and the H1-2 claims beside them on 2026-10-26; the rest was +verified 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 +GOLEM (Generalized Open Layered Embodied Modules) is the software stack for two +Unitree humanoids, the H1-2 and the G1 (29 DoF, two Dex3-1 hands). The H1-2 has +MuJoCo/RoboCasa and Isaac Lab simulators and 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. +separate Docker containers that share one DDS domain. The G1 has its own +packages in the same workspace (`g1_homie`, HOMIE's walking policy; `dex_3`, +the Dex3-1 hands) and a RoboCasa sim (no Isaac). Both robots' models +are in `CL_Assets`, installed for ROS by one package, `golem_ros2_models`. +Both robots run one safety layer, `golem_safety`, and one arm IK controller, +`golem_upper_ik`, each with a profile per robot, and start from one bringup +package, `golem_bringup`. Each robot speaks its own Unitree DDS interface +(`rt/lowcmd` / `rt/lowstate`, and on the G1 also `rt/dex3/*`) identically +against its sim and the real robot, so code moves between them unchanged. The +G1 runs in Unitree's debug mode: Unitree's own controller is released, and the +stack drives every motor on `rt/lowcmd`, as on the H1-2. + +The H1-2 is the default everywhere. `GOLEM_ROBOT=g1` or `--g1` on a simulator or +stack wrapper selects the G1 where the attached robot matters: the sim and +`docker_stack.sh`'s bringup (§3). Every `ros` container builds and runs both +robots' packages (`docker_run.sh ros` takes no `--g1` or `--h12` flag). The G1 +pages are in `docs/g1/` (start at `docs/g1/index.md`). ## 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) | +| `docker/` | x86 setup — the fully-featured, preferred platform (Dockerfiles, `docker-compose.yml`, the G1 overlay `docker-compose.g1.yml`, `scripts/`) | +| `docker/mac/` | self-contained Apple-Silicon (arm64, CPU-only) port — limited: H1-2 only, no Isaac, no ML/vision stack, FastDDS instead of CycloneDDS | +| `core_ws/` | the ROS 2 workspace — where the bulk of development happens (see §4); the G1's packages are `core_ws/src/g1_homie/` and `core_ws/src/dex_3/`, the safety layer, arm controller, SLAM/navigation and skills both robots run in `core_ws/src/golem_safety/`, `core_ws/src/golem_upper_ik/`, `core_ws/src/golem_slam/` and `core_ws/src/golem_skills/` | +| `golem_robocasa/` | RoboCasa/MuJoCo simulator entry point for both robots (`h12_mujoco.py`; `--g1` hands over to the G1 sim package `golem_robocasa/g1/`) and the ROS/DDS bridges | +| `CL_isaaclab_sim/` | Isaac Lab simulator, H1-2 only (`sim_main.py`, DDS bridges, tasks) | +| `CL_Assets/` | both robots' URDFs, MJCFs, meshes and self-collision models (the G1's in `CL_Assets/g1/`), Isaac USD (Git-LFS submodule); `golem_ros2_models` installs the models for ROS, and the sims read them directly | | `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` | +| `tests/` | simulator regression and acceptance tests (`tests/README.md`); the G1 acceptance tests are in `tests/g1/` | +| `tools/` | standalone debug tools (ROS MCP server for Claude Code; `read_lowstate.py`, which prints the real G1's `rt/lowstate`) | +| `docs/` | user-facing docs: `SETUP.md`, `RUNNING.md`, `MACOS.md`, `DDS_TUNING.md`, `DOCKER_TESTING.md`, `NAVIGATION_DEMO.md`, `ROS_MCP_DEBUG.md`, and the G1's in `docs/g1/` | | `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` @@ -37,110 +57,333 @@ 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. +domain safety checks, bind-mount pre-creation, robot selection 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`. | +| `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`). The images serve both robots. | +| `docker/scripts/docker_run.sh [--restart] [cmd...]` | `isaac`, `robocasa`, `ros`; `--restart` right after the profile replaces an existing container; then optionally `bash` (shell instead of default launcher) or any command; a leading `-flag` (e.g. `--headless`) is forwarded to the default launcher; on simulator profiles (`robocasa`, `isaac`), `--g1` / `--h12` anywhere in the arguments select the robot (`ros` takes no robot flag) | Container names: `golem_ros`, `golem_sim_robocasa`, `golem_sim_isaac`, for either robot. Simulator profiles print `robot: h12` or `robot: g1` before the domain check. | + +Paired startup: `docker/scripts/docker_stack.sh up robocasa|isaac [--headless|--gui] [--g1|--h12]`. +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 from a +simulator of the selected robot (`wait_for_sim.py`). The bringup it starts is +`golem_bringup h12_sim.launch.py`, or `golem_bringup +g1_sim.launch.py` for the G1 (with only `use_rviz` set). +`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**. +`core_ws` (incrementally, on every start) and **drops to a shell — it does not +launch bringup**. + +### Robot selection (x86 only) + +`docker/scripts/docker_common.sh` owns this. On simulator and stack wrappers, +the robot is `--g1`/`--h12` on the wrapper, else `GOLEM_ROBOT` from the shell, +else from `docker/.env`, else `h12`. The selection means "which robot is +attached" and matters only where that is inherent: which robot the RoboCasa sim +runs, and which bringup `docker_stack.sh` starts and which sim +`wait_for_sim.py` expects. Every `ros` container serves both robots (`docker_run.sh ros` +takes no `--g1`/`--h12` flag and uses `core_ws/cyclonedds.xml` directly): the +base compose file bind-mounts `core_ws/src/g1_homie/policy` (at +`/home/code/g1_homie`, HOMIE's policy) and `tools/` (at `/home/code/tools`, for +`read_lowstate.py`) read-only into the `ros` service (the sim reads none of +HOMIE's files), and `CL_Assets/g1` into both services. For `g1` the simulator +and stack wrappers: + +- add `docker/docker-compose.g1.yml` to the `robocasa` service: it sets + `GOLEM_ROBOT=g1` and passes the G1 sim knobs (§8); +- check in `prepare_mounts` that the G1 sim's `CL_Assets/g1` files (its MJCF, + URDF and meshes) exist and are not LFS pointers, before any container starts + (every `ros` container, either robot, gets the same check of the + `CL_Assets/g1` files `golem_ros2_models` installs and of the policy files); +- refuse the Mac wrappers and the Isaac simulator. + +`launch_ros.sh` builds every package in `core_ws`, the G1's included, +whatever `GOLEM_ROBOT` says, so one `ros` container runs either robot's +bringup. `golem_ros2_models` installs both robots' models from `CL_Assets`, and +`golem_upper_ik` loads the G1's only when a node runs with `robot:=g1`. Both +robots use the same images and container names, so the wrappers run one robot +at a time on a host. ### 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 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). | +The Mac wrappers run the H1-2 only; they refuse `--g1` and `GOLEM_ROBOT=g1`. 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`, +(MuJoCo viewer → noVNC :6080), `GOLEM_RVIZ=vnc` (RViz → :6081), `GOLEM_LOWERBODY=fame|walk|almi`, `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; a task env can set its own via +`spawn_margin`, as `battery_env.py` does) sets the H1-2's minimum spawn +clearance from non-floor geoms on both platforms. `GOLEM_CAMERA_SCALE=` +(H1-2 default 0.5 of the real 1280x720), `GOLEM_CAMERA_DEPTH=0` and +`GOLEM_CAMERA_POINTCLOUD=0` tune the H1-2's RoboCasa cameras on both platforms. 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 +`ROS_DOMAIN_ID=0` is the **real robots' 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`). +which the `ros` container needs for the vision servers and `golem_skills`, and +`GOLEM_ROBOT`). + +The real G1 is commanded from the laptop wired to it, through +`docker/scripts/docker_run.sh ros` on domain 0 (the H1-2 runs natively on +its onboard PC). Both robots use the default CycloneDDS configuration +(`core_ws/cyclonedds.xml`) as is. + +**One robot stack per DDS domain.** The two stacks share node, action and DDS +topic names, with identical `unitree_hg` types. Each robot's sims report its +real robot's `rt/lowstate` `mode_machine`: 6 for the H1-2, 5 for the G1. +`wait_for_sim.py` refuses a simulator of the other robot by that value, but it +runs only in `docker_stack.sh` and the mac stack (`GOLEM_WAIT_FOR_SIM=1`) and in +`tests/stack`. `docker_run.sh ros` followed by a manual `ros2 launch` is not +guarded by the wrappers. `golem_safety` checks `rt/lowstate`'s value too (§8), +but that does not replace one stack per domain: the stacks share the +`rt/safety/*` input topics and the node and action names, `golem_safety` does +not check which robot an input command was written for, and `golem_upper_ik` +does not check `rt/lowstate`'s `mode_machine`. ## 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`). +23 ROS packages in 20 top-level directories: 21 (the H1-2's, the shared ones +and the six shared `golem_*` packages) and the 2 G1 packages +(below). 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) | +| `golem_safety` | **safety (both robots)** | C++. The only writer of `/lowcmd` (§8): streams zero gains from boot, engages each `/safety/*` input on its own first valid command and relays it, clips it, checks every `/lowstate` (robot identity, estop limits) and latches an estop until restart. A robot profile (`robots/.yaml`, the joint table both `golem_*` packages read) and configs per robot (`config/h12/`, `config/g1/`) | `safety_node` (`robot`, `config`, `sim`, `use_estop` parameters); `launch/safety.launch.py` | +| `golem_upper_ik` | **controller (upper body, both robots)** | Pink IK on the 14 arm joints with the torso (H1-2) or waist (G1) held; OMPL planning and a self-collision barrier from each robot's sphere model and SRDF (the G1's spheres were packed on the rubber-hand G1: none on the Dex3-1 fingers; its torso spheres stand inside the torso's flanks, so `home` keeps the H1-2's 20 mm `d_min`; its `arms_down`, where the real hands rest on the hips, is in self-collision); the follow gate; the safety/controller config pairing (`golem_upper_ik.pairing`) the bringups check | `frame_task_server` (`/frame_task`, `/named_config`), `joint_state_publisher`; `launch/upper_ik.launch.py` | +| `golem_bringup` | **bringup (both robots)** | Launch-only package that starts the whole H1-2 (`launch/h12/`) or G1 (`launch/g1/`), sim or real. See §6. | top-level + building-block launch files, `config/h12/*.yaml` | +| `golem_ros2_models` | **description (both robots)** | Installs both robots' models from `CL_Assets`: the H1-2 with its Magpie grippers and the G1 with its Dex3-1 hands, each a Pinocchio URDF, a ROS copy with `package://golem_ros2_models/...` URIs, the meshes, and a sphere-packed self-collision model with its SRDF. No nodes and no joint table (that is `golem_safety`'s robot profile) | `golem_ros2_models.paths`; `launch/display.launch.py` (`robot:=h12|g1`) | +| `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 and ALMI stand/walk policies; controller runs one pinned policy; consumes `/lowstate` + `/cmd_vel`, emits 12-joint PD setpoints | `walking_node`, `fame_node`, `lowerbody_controller_node` | +| `lmg_deploy` | controller (whole body, MARL) | The loco-manipulation game's two-agent (legs + IK arms) MARL policy, run by the `lmg_*` bringups | `marl_deploy_node` | +| `golem_slam` | SLAM + navigation (both robots) | Shared navigation for both robots (FAST-LIO deskewed cloud → self filter → floor alignment/OctoMap → slam_toolbox → Nav2 MPPI), the G1's dynamic waist transform (`g1_body_to_pelvis`), and its configs: Livox MID360 driver config (`MID360_config.json`, real robot, read by `h12_real_drivers.launch.py`), FAST-LIO (`mid360.yaml`, `g1_fastlio*.yaml`), slam_toolbox (`slam_toolbox_h1.yaml`, `slam_toolbox_g1.yaml`), Nav2 (`nav2_config*.yaml`) | `launch/h12_navigation.launch.py`, `launch/g1_navigation.launch.py`, `g1_body_to_pelvis`, `config/` | +| `golem_skills` | **skills (both robots)** | 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 | +| `custom_ros_messages` | interfaces (both robots) | 19 actions (13 `Skill*`, `FrameTask`, `NamedConfig`, `DualArm`, …), msgs, srvs (`GeminiQuery`, `SamSegment`, `GraspGen`, `SetHandPose`, …) | 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` | +| `FAST_LIO` (`fast_lio`) | SLAM (vendored) | LiDAR-inertial odometry/mapping from the Livox cloud (used by `golem_slam`) | `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 | +### The G1 packages + +Built in every `ros` container, for either robot (§3). Their interfaces are in +`custom_ros_messages`; each package has a README with the +details, and `docs/g1/packages.md` indexes them. + +| Directory | Role | What it does | Key entry points | +|---|---|---|---| +| `g1_homie` | controller (lower body) | HOMIE's walking policy (onnxruntime) on the 12 leg joints, driven by `/cmd_vel` through `golem_safety`'s split mode. Follows the H1-2's leg node: waits for `/safety/heartbeat`, pre-poses the legs, on the real robot holds until `/lowerbody/confirm_engage`, and in the sim releases the elastic band | `locomotion_node` | +| `dex_3` | driver (hands) | Dex3-1 hands on `rt/dex3//*`: `//hand/{set_pose,open,close,release}`; bypasses the safety layer | `dex3_hands`; `launch/hands.launch.py` | + Third-party/vendored (do not refactor): `FAST_LIO`, `livox_ros_driver2`, -`unitree_ros2`, plus the top-level `unitree_sdk2_python` and `mujoco_mpc`. +`unitree_ros2`, plus the top-level `unitree_sdk2_python` and `mujoco_mpc`, +Unitree's G1 models in `CL_Assets/g1` (kept byte-for-byte upstream; its +`README.md`), and HOMIE's policy in +`core_ws/src/g1_homie/policy/` (non-commercial licence, §8). ## 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: +package, the model servers, the simulators, the G1 packages, …) 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. +- Shared infra — `golem_bringup`, `custom_ros_messages`/`magpie_msgs` + (both robots use `custom_ros_messages`), `docker/`, `golem_safety` and + `golem_upper_ik` (both robots run them), 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): +### The H1-2 (`golem_bringup`, `launch/h12/`) + +`golem_bringup` is launch-only. Sim vs real is **separate top-level launch files** +(no top-level file takes a `sim:=` argument); each one only includes the shared +building blocks below and sets `use_sim_time` plus its platform's arguments, +including `h12_control.launch.py`'s `sim`, which has no default. -| Launch file | Scenario | +| Top-level launch file | Scenario | Includes | +|---|---|---| +| `h12_sim.launch.py` | **x86 sim** (`use_sim_time=true`), plus the camera optical-frame static TF and rviz (`use_rviz`) | state, control (`sim:=true`, `sim_safety_split` for both `safety_node` and `frame_task_server`, no stagger), lowerbody (`lowerbody_sim.yaml`), models (`use_skills`), navigation (`use_nav`) | +| `h12_sim_mac.launch.py` | **mac sim** — standalone, includes none of the blocks: `robot_state_publisher` and `golem_upper_ik`'s `joint_state_publisher` and `frame_task_server`, `golem_safety`'s `safety_node` (`sim: true`, both on `sim_safety_split`); lower body/SLAM/nav2 gated by `GOLEM_*` env vars, not launch args | — | +| `h12_real_robot.launch.py` | **real robot, onboard PC** (native, `ROS_DOMAIN_ID=0`) | real_drivers, navigation, state, control (`sim:=false`, `use_estop:=true`, the defaults `relax_safety_split` and `safety_split`), lowerbody (`lowerbody_real.yaml`, no estimator). Leg control is interlocked behind `start_position_verified:=true` (default **false**) | +| `h12_real_desktop.launch.py` | **real: companion x86 desktop** — rviz, plus the hand cameras with `use_hand_cameras:=true` (default **false**) | models (`use_yolo:=true`) | +| `h12_safety_grippers.launch.py` | real: actuation-only debug bringup | control (`sim:=false`, estop + safety, no IK), grippers | +| `lmg_sim.launch.py` | **x86 sim**, loco-manipulation game: the `lmg_deploy` MARL policy (Explicit500) owns all 27 joints, no operator confirm. Refuses an unset or 0 `ROS_DOMAIN_ID` | state, control (`sim:=true`, no IK, no stagger), locomanipulationgame, navigation (`use_slam`) | +| `lmg_real.launch.py` | **real, onboard PC**, loco-manipulation game, run *instead of* `h12_real_robot`. Policy behind `start_position_verified:=true` + `/marl_deploy_node/confirm_engage` | real_drivers, state, control (`sim:=false`, `use_estop:=true`, no IK), locomanipulationgame, navigation (`use_slam`) | + +| 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 | +| `h12_state.launch.py` | `golem_upper_ik`'s `joint_state_publisher` (`robot: h12`, the 27 body joints from `/lowstate`) + `robot_state_publisher` (`urdf_file` arg) | +| `h12_control.launch.py` | [`estop_node`] → `golem_safety`'s `safety_node` (`robot: h12`, `safety_config`, `sim`, `use_estop`) → `golem_upper_ik`'s `frame_task_server` (`robot: h12`, `frame_task_config`; `use_frame_task`), staggered by `safety_delay` (3 s) / `frame_task_delay` (5 s). `sim` has no default: a launch without it fails before any node starts. With `use_frame_task` it refuses a `frame_task_config` that does not run with `safety_config` (`golem_upper_ik.pairing`) before starting anything | +| `h12_lowerbody.launch.py` | RL `lowerbody_controller_node` (`lowerbody:=`, params from `lowerbody_params`) and optional MJPC `estimator_node`, both behind `start_position_verified` | +| `h12_models.launch.py` | gemini + sam servers, [yolo], graspgen + `golem_skills` (`use_skills`) | +| `h12_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, `h12_grippers.launch.py` (left+right `gripper_node`) | +| `h12_locomanipulationgame.launch.py` | `marl_deploy_node` (behind `start_position_verified`; holds arm goals in the world by the SLAM `/Odometry`) | +| `golem_slam/launch/h12_navigation.launch.py` | shared nav stack (FAST-LIO → pointcloud_to_laserscan → slam_toolbox → nav2) | + +`golem_slam` also ships one top-level file of its own, `h12_real_slam.launch.py` +(`ros2 launch golem_slam h12_real_slam.launch.py`): real robot sensors + SLAM/navigation +only, on wall time — real_drivers, state, navigation; no estop, safety or IK, so +no joint is commanded. Record its runs with `ros2 run golem_slam record_slam_bag --name `. + +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`). + +**The H1-2's start-up** (`h12_control.launch.py`; `golem_safety`'s and +`golem_upper_ik`'s READMEs have the details): + +- `safety_node` streams zero-gain `/lowcmd` from boot. It engages each input on + that input's first valid command once `/lowstate` reports `mode_machine` 6, + and, with `use_estop:=true`, once a first hardware estop sample has arrived; + until then it logs `engage_refused` with the reason, at most once a second. +- **`/safety/heartbeat` means relaying can begin.** `safety_node` publishes no + beat until those two conditions hold; then it beats true at 10 Hz, and false + once its estop latches (false from the start if it latched first). + `h12_lowerbody_rl` pre-poses the legs only from the first beat, and + `lmg_deploy` and `golem_skills` read it too. +- **`frame_task_server` needs `golem_safety` relaying.** It waits up to + `controller.state_timeout` (5 s) for a fresh `/lowstate`, then holds the + measured pose until it sees its commands on `/lowcmd` (the follow gate), for + up to `controller.startup_timeout` (30 s). If `golem_safety` has not relayed + them by then, it sends one zero-gain command and exits 1 with an error that + lists the causes: no `/lowstate` reporting `mode_machine` 6 (an H1-2 sim + started before the sims reported 6), no first estop sample with + `use_estop:=true`, a safety config that does not pair with its controller + config, or an estop latched at this start-up (the limit checks run from + boot, so a joint already past `limits.estop` latches it) or in an earlier + run. The other nodes keep running, without `/frame_task` and + `/named_config`. +- **`use_estop`.** `use_estop:=true` (the real bringups and + `h12_safety_grippers`) starts `estop_node` and has `safety_node` read + `/estop/triggered` and `/estop/plugged_in`: no input engages before the first + sample, a sample that says triggered or unplugged latches the estop until + restart, and so does no new sample for `estop.timeout_seconds` (0.5 s) once + the first has arrived. With `use_estop:=false` (the sim bringups) those topics + are ignored. +- **The real robot's estop start-up.** `estop_node` publishes nothing until its + first attempt to connect ends; that attempt probes every serial port it finds + (up to about 3.25 s for each port that opens, so another serial device such as + a gripper board lengthens it). When an attempt fails, it publishes unplugged + and triggered until a retry, once a second, connects. **A `safety_node` that + is running while `estop_node` reports a failed connection latches until + restart**: when the first attempt fails and no retry has connected by + `safety_delay` (3 s), or the first attempt outlasts `safety_delay` and then + fails. A first attempt that outlasts `safety_delay` and then connects only + delays the start: no input engages and no heartbeat goes out until its first + sample, so the legs pre-pose and the arms start late. `safety_delay` must + cover the time `estop_node` takes to connect (the comment in + `h12_control.launch.py`). **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 +`golem_bringup/package.xml`, add a `launch_ros.actions.Node(...)` entry to the +relevant launch file (put node params in `golem_bringup/config/h12/.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 `h12_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. +won't build it; a package the Mac does not build goes into `BRINGUP_STUB_DEPS` +there instead, or the Mac build of `golem_bringup` fails. For the G1 the node +goes into `launch/g1/g1_core.launch.py`, with the same `exec_depend`. -## 7. Skills (`h12_skills`) — the LLM-callable layer +### The G1 (`golem_bringup`, `launch/g1/`) -Each skill is a mixin class in `h12_skills/skills/` with one +Two top-level files include one shared core, `g1_core.launch.py`, with `sim` +and `use_sim_time` set for their platform. The docstring of each file is its +reference. + +| Launch file | Scenario | Starts | +|---|---|---| +| `g1_sim.launch.py` | **G1 sim** (sim time; `sim_split` for `golem_safety` and the controller, `g1_homie`'s `sim.yaml`). Always starts HOMIE's walking node. `use_nav` (default true) adds shared navigation (`golem_slam`, `odom_source:=fastlio` only), `use_skills:=true` starts `golem_skills` (including `/skill/frontier_explore`) | `g1_core` | +| `g1_real_robot.launch.py` | **real G1** in Unitree's debug mode, every node in the laptop's `golem_ros` container on domain 0 (`split` and `g1_split`, `g1_homie`'s `g1.yaml`). The walking node only with `start_position_verified:=true` (default **false**): without it the leg motors stay off and the arms run. `use_nav` (default true) adds the MID-360 driver and `golem_slam`; no skills yet | `g1_core` | +| `g1_core.launch.py` | shared core: `robot_state_publisher`, `golem_upper_ik`'s `joint_state_publisher` (`robot: g1`), `golem_safety`'s `safety_node` (`robot: g1`, `safety_config`, `sim`, `use_estop: false`: the G1 has no estop node), then `golem_upper_ik`'s `frame_task_server` (`robot: g1`, `controller_config`) after `controller_delay` (2 s), then `g1_homie`'s `locomotion_node` (`locomotion_config`) after `locomotion_delay` (2 s more: always in the sim, with `start_position_verified` on the real robot); [`dex3_hands`] (`use_hands`), [rviz] (`use_rviz`), [`golem_slam`] (`use_nav`), [`golem_skills`] (`use_skills`) | — | + +The core refuses to start on a bad combination; `startup_refusal` in +`g1_core.launch.py` and `pairing_refusal` in +`core_ws/src/golem_upper_ik/golem_upper_ik/pairing.py` are the rules: + +- **Config pairing.** The safety config must relay the upper-body channel + (`split` or `sim_split`; `full` is refused), and each runs with exactly one + controller config (`G1_SAFETY_CONTROLLER_CONFIGS` in `pairing.py`: + `g1_split`, `sim_split`), which `controller_config` defaults to. The real + robot refuses every `sim_*` config and the walking node's `sim` config, and + the sim refuses the real ones (`split`, `g1_split`, the walking node's `g1`). +- **The leg interlock.** On the real G1 the walking node starts only with + `start_position_verified:=true`, set only after checking the robot is in a + safe start pose; the argument refuses nothing. The bringup writes `rt/lowcmd` from the + moment `safety_node` starts, so the operator puts the G1 into Unitree's debug + mode, on a gantry, before launching, a manual step that nothing in this stack + performs or checks (its procedure is not written yet: + `docs/g1/first-session.md`). The gantry carries the robot while the legs are + off, and the split estop turns every motor off. +- **Navigation.** `use_nav` (default true on both G1 bringups, as on the + H1-2's) adds `golem_slam`; on the real G1 it also starts the MID-360 driver + (`launch/g1/g1_real_drivers.launch.py`, `golem_slam/config/g1_MID360_config.json`) + and FAST-LIO on `g1_fastlio.yaml`. Not yet run on the real G1. + +**The G1's start-up** (`g1_core.launch.py`'s docstring; `golem_safety`'s, +`golem_upper_ik`'s and `g1_homie`'s READMEs have the details): + +- `safety_node` streams zero-gain `/lowcmd` (every motor off) from boot, checks + every `/lowstate` against the estop limits from the first sample, and engages + each input on that input's own first valid command once `/lowstate` reports + `mode_machine` 5, as on the H1-2; until then it logs `engage_refused`. Its + heartbeat is the H1-2's: none until `/lowstate` reports 5, then true at + 10 Hz, false once latched. +- `frame_task_server` holds the measured pose until its follow gate sees + `golem_safety` relay its commands, which happens on its first command, with + or without the walking node; then it moves the arms to `home` and the waist + to its hold. It stops with an error if `/lowstate` is not fresh within + `controller.state_timeout` (5 s), if nothing is relayed within + `controller.startup_timeout` (30 s), or if the moves take longer than + `controller.startup_move_timeout` (30 s). The leg motors stay off until the + walking node commands them. +- `locomotion_node` sends nothing until the first true `/safety/heartbeat` and + an upright `rt/lowstate`, as the H1-2's leg node. Then it moves the legs to + HOMIE's default pose over 2.0 s and holds it until 1 s of steady, on-time + `rt/lowstate` (its tick within `startup.tick_rate` of the wall clock) and the + arm controller's commands. On the real robot (`g1.yaml`) it also holds until + `ros2 service call /lowerbody/confirm_engage std_srvs/srv/Trigger` (the H1-2 + leg node's service); in the sim (`sim.yaml`) the policy then starts, under + the sim's elastic band, and the node releases the band through + `/elastic_band/toggle` once `/left_ee_pose` has come (`frame_task_server` + publishes it after its start-up moves), or `band.max_wait` (30 s) after the + policy started. A false heartbeat, or none for 0.5 s after the first, stops + it for good in every phase. +- After any estop, restart the whole bringup. + +## 7. Skills (`golem_skills`) — the LLM-callable layer + +Each skill is a mixin class in `golem_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 @@ -152,6 +395,12 @@ the `SKILL_ACTIONS` table (`base.py`). All types are | `/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" | +`custom_ros_messages` also defines `SkillRetrieveMemory`; no node in this +repo serves it yet. On the G1, `golem_skills` starts when `use_skills:=true` +is passed to `g1_sim.launch.py` (with `wait_for_services: false` so it starts +immediately without waiting for H1-2 vision/gripper services), serving +`/skill/frontier_explore`. + 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 @@ -178,28 +427,72 @@ ros2 action send_goal /named_config custom_ros_messages/action/NamedConfig \ ### The simulators are shared infra — don't modify them casually -`CL_isaaclab_sim` and `h1_robocasa` should generally not be modified unless +`CL_isaaclab_sim` and `golem_robocasa` should generally not be modified unless necessary, and their **default behaviors must be preserved**. Everyone's sim-vs-real workflow depends on them behaving like the real robot. +`golem_robocasa/h12_mujoco.py` is the one RoboCasa entry point for both robots: it +picks the robot (`--h12`/`--g1`, else `GOLEM_ROBOT`, else `h12`) before its +H1-2 imports and hands the G1 over to `golem_robocasa/g1/g1_mujoco.py`. The H1-2 +stays the default, and nothing on the G1 path may change its behaviour. The robots share +`sim_common.py`, `measurement_bridge.py` and the `mujoco_sensors` node in +`mujoco_ros_bridge.py` (`sim_timing.py` is the H1-2's only); each has its own sensor suite +(`h12_sensors.py`, `g1/g1_sensors.py`) and Unitree DDS interface +(`unitree_interface.py`, `g1/unitree_interface.py`). ### 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. -(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`.) - -**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). +**RoboCasa, H1-2 (`golem_robocasa/h12_mujoco.py`)** — publishes `/clock`, the real +RealSense drivers' topics, frame names and camera TF (`/tf_static` + `/tf`) for +`/realsense/{head,left_hand,right_hand}` — colour, depth and aligned depth with +their camera_info, and `depth/color/points` — as captured from the robot in +`golem_robocasa/realsense_profile.yaml`, `/livox/lidar` (CustomMsg), `/livox/pointcloud`, `/livox/imu`, +`/{left,right}/gripper/state`, and `rt/lowstate` over Unitree DDS (with the +real robot's `mode_machine`, 6); consumes `rt/lowcmd` and the +`/{left,right}/gripper/*` services/DeliGrasp action. +(Trust the code over any topic list repeated elsewhere: the sensor suite in +`h12_sensors.py`, the grippers in `magpie_hand_bridge.py`, `/clock` and the +opt-in odom in `mujoco_ros_bridge.py`, Unitree DDS in `unitree_interface.py`.) + +**RoboCasa, G1 (`h12_mujoco.py --g1`, `golem_robocasa/g1/`)** — publishes `/clock`, +the Livox MID-360 as its driver does in CustomMsg mode (`/livox/lidar`, +`/livox/imu`, no `/livox/pointcloud`), the head RealSense D435 +(`/realsense/head/*` and its `/tf_static`), and over Unitree DDS `rt/lowstate` +and `rt/dex3/{left,right}/state`, and serves `/elastic_band/toggle` +(`std_srvs/srv/Trigger`, as the H1-2 sim does); consumes `rt/lowcmd` and +`rt/dex3/{left,right}/cmd`. It plays the robot in Unitree's debug mode: every +motor is off until the first `rt/lowcmd` with a valid CRC, `mode_machine` 5 +and `mode_pr` 0, and an elastic band on `torso_link` holds the robot until the +toggle (or SPACE in the viewer) releases it; the G1 bringup's walking node +calls it. It reads none of HOMIE's files. (Trust `g1/g1_sensors.py` for +the ROS side, `g1/unitree_interface.py` for the DDS side and `g1/g1_mujoco.py` +for the band.) Its knobs are `GOLEM_LIDAR`, `GOLEM_LIDAR_POINTS`, +`GOLEM_LIDAR_HZ`, `GOLEM_CAMERAS`, `GOLEM_CAMERA_HZ`, `GOLEM_CAMERA_SCALE` (G1 +default 0.5), `GOLEM_CMD_TIMEOUT`, `GOLEM_SPAWN_BACKOFF`, `GOLEM_SIM_ODOM`, +`GOLEM_SIM_EXPERIMENT`. Their defaults are in the code +that reads them: `SensorConfig` in `g1/g1_sensors.py` (sensors), +`g1/unitree_interface.py` (the command timeout), `g1/g1_mujoco.py` +(spawn, odom) and `measurement_bridge.py` (experiment); the compose files pass them +(`docker/docker-compose.g1.yml` the G1-only ones). The G1 sim ignores the H1-2-only +knobs (`GOLEM_SPAWN_MARGIN`, `GOLEM_SPAWN_OFFSET`, `GOLEM_HAND_CAMERA_RES`, +`GOLEM_CAMERA_DEPTH`, `GOLEM_CAMERA_POINTCLOUD`, the tether ones). It refuses +`--timing`, and the H1-2 sim refuses `--duration` (`sim_common.py`). + +The measured surface of each RoboCasa robot is checked in as a golden: +`tests/robocasa/surface/{h12,g1}.json`, compared by +`tests/robocasa/surface/run.sh h12|g1` (see its README). A change that alters +what the sim publishes re-records the golden in the same commit. + +**Isaac (`CL_isaaclab_sim/sim_main.py`, H1-2 only)** — publishes `rt/lowstate` +(`mode_machine` 6) 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 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 @@ -209,16 +502,17 @@ 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. -- **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 - (`/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. +- **RoboCasa complies, for both robots:** ground-truth `/odom` + `odom→pelvis` + TF is gated by `GOLEM_SIM_ODOM` (default `'0'` = off, read in `h12_mujoco.py` + and `g1/g1_mujoco.py`). Only the mac nav demo opts in; the G1 bringup and tests refuse it. Keep it that way. The + experiment surface (`/sim/ground_truth`, `/sim/reset_objects`) is gated by + `GOLEM_SIM_EXPERIMENT` (`measurement_bridge.py`). The G1 acceptance tests + record the robot's state inside the sim process instead of publishing it. +- **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`), for both + robots. 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 @@ -235,6 +529,9 @@ 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". +**Stale, not re-verified:** both Dockerfiles check out Livox-SDK2 at `08f523c` +(`RosDockerfile` §2, `RobocasaDockerfile`); the unpinned `master` clone +described in this paragraph is gone. The pin rule above stands. 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 @@ -243,7 +540,7 @@ only `package.*` stubs — no `local_setup.bash`, no Python module — and you f out at runtime: ``` -ModuleNotFoundError: No module named 'livox_ros_driver2' # mujoco_ros_bridge.py:36 +ModuleNotFoundError: No module named 'livox_ros_driver2' # h12_sensors.py, or g1/g1_sensors.py for the G1 ``` Whether it reproduces depends on how old your Docker cache is, so "it built on @@ -256,21 +553,108 @@ 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 +- **`golem_safety` is the only writer of the body command topic, on both + robots.** Its `safety_node` is the only publisher of `rt/lowcmd` (ROS + `/lowcmd`), which drives every body motor (the G1 runs in Unitree's debug + mode). + Controllers publish only to its `rt/safety/*` inputs: `golem_upper_ik` and + `g1_homie` refuse a command topic outside them. The exception is + `dex_3`, which writes `rt/dex3//cmd` directly, with no safety layer. +- **Quiet inputs are held and reported, not estopped.** Once an input has + engaged, an input with no new command for its `stale.*_seconds` keeps its + last command relayed (`input_quiet`, then `input_resumed`), and a quiet + `rt/lowstate` leaves the relay running (`state_quiet`, `state_resumed`). This + includes the G1's split mode: a walking-node fault silences + `locomotion_node`, and `golem_safety` keeps relaying its last leg command, + which does not balance the robot, instead of turning every motor off. +- **The estop latch** is one way until restart. It latches on a command with a + non-finite value, a second live writer on an input (a new writer is accepted + after `single_writer.handover_seconds`, 0.5 s, of quiet), a measured joint + past `limits.estop` or not finite (checked from boot, on both robots), another + robot's `mode_machine` once an input has engaged, the transport-lag check + (`control.state_lag_ms`, on domain 0; off in every G1 config), and with + `use_estop` a hardware estop that is triggered, unplugged or silent. What it + writes comes from the config's `estop_response`: + - H1-2 (all 8 configs): 50 zero-gain commands (mode 0, every value 0), then + no `rt/lowcmd` until restart. + - G1 (all 3 configs): the estop command, written every cycle until restart + (`mode: zero`, `after: keep_writing`: every motor off, so the robot drops; + support it). +- **Robot identity.** `golem_safety` checks every `rt/lowstate` against its + robot profile's `mode_machine`, 6 for the H1-2 and 5 for the G1: no input + engages without it, another value latches the estop once engaged, and every + command it writes carries the profile's `mode_machine`. Each robot's sims + report the real value, and `wait_for_sim.py` checks the same values (§3). +- **The controller's exit is one last command.** On SIGINT or SIGTERM, or a + start-up that fails after its first command, `golem_upper_ik`'s + `frame_task_server` sends its upper-body slots one command with mode 0 and + zero kp, kd and tau on the IK joints, then stops publishing. The held joints + get the same, except on the G1 (the profile's + `shutdown_keeps_held_joints_on_lowcmd`). On the H1-2 `golem_safety` relays + it, so the torso and arms go limp and stay so (the quiet input keeps its last + command relayed). On the G1, in split mode (the only one its bringups run), + it relays it too: the waist keeps its hold angle, gains and torque and only + the arms go limp, while the walking node keeps the legs. No estop latches. +- **`golem_safety`'s own exit** (SIGINT, SIGTERM, SIGHUP): it stops publishing + at once (an estop burst in progress is finished first, and with + `keep_writing` the estop command is written once if none has been since the + latch). Once `rt/lowcmd` falls silent, the sims turn every body motor off + after `GOLEM_CMD_TIMEOUT` (0.5 s of sim time); what the real robots do then + has not been measured. +- **One robot stack per DDS domain** (§3). Never run the H1-2's and the G1's + stacks on one domain. +- **HOMIE's policy is non-commercial.** `core_ws/src/g1_homie/policy/` + holds OpenHomie's pretrained policy, CC BY-NC 4.0 by its `LICENSE` (its + README says CC BY-NC-SA 4.0): treat it as CC BY-NC-SA 4.0, unlike the MIT + G1 packages. Keep its `LICENSE` and `README.md` beside the files, and keep + `homie_policy.py` inside `policy/`. - 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`. + discipline), each through a lock step (`-c /opt/golem-locks/.txt`, + enforced by `tests/docker/test_source_pins.py`). 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`. + matching `MJPC_REF` and wipe stale `core_ws/build/deploy_mjpc`. ## 9. Running the robot — the standard three-terminal flow Everyone runs their package through bringup, which starts the whole robot. -### x86 +### x86, the H1-2 ```bash # Terminal 1 — simulator (start FIRST so /clock is publishing) @@ -278,7 +662,7 @@ docker/scripts/docker_run.sh robocasa # or: isaac; add --headless if # 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 +ros2 launch golem_bringup h12_sim.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 @@ -289,11 +673,41 @@ ros2 action send_goal /skill/grasp custom_ros_messages/action/SkillGrasp \ "{target_object: 'vertical fridge handle', arm: 'right', timeout: {sec: 60, nanosec: 0}}" --feedback ``` +### x86, the G1 + +```bash +# Paired: the sim plus g1_sim.launch.py (the arms and HOMIE), gated on the sim's robot +docker/scripts/docker_stack.sh up robocasa --g1 --headless +docker/scripts/docker_stack.sh logs robocasa + +# Or three terminals, to choose the bringup's arguments (the wrappers do not +# check the sim's robot here; golem_safety's own check is in §8) +# Terminal 1 — the sim; wait for "[g1_mujoco] ROS bridges up" +docker/scripts/docker_run.sh robocasa --g1 --layout 9 --style 9 --seed 42 +# Terminal 2 — builds core_ws (both robots' packages), drops to a shell; no +# --g1 needed: the ros container runs either robot's bringup +docker/scripts/docker_run.sh ros +ros2 launch golem_bringup g1_sim.launch.py # wait for the walking node's "elastic band released" +# variants: use_rviz:=false use_hands:=false use_nav:=false use_skills:=true + +# Terminal 3 +docker exec -it golem_ros bash +source /opt/ros/humble/setup.bash && source /home/code/core_ws/install/setup.bash +ros2 action send_goal /named_config custom_ros_messages/action/NamedConfig \ + "{config_name: 't_pose', plan: false, duration: {sec: 0, nanosec: 0}}" +``` + +Walking and navigation need the spawn backed into open floor +(`GOLEM_SPAWN_BACKOFF=2.0` before terminal 1's command); `docs/g1/walking.md`, +`navigation.md` and `exploration.md` have those flows. The closed-loop checks +are `tests/g1/test_sim_bringup.sh [--walk]`, `tests/g1/test_sim_nav.sh` and +`tests/g1/test_sim_explore.sh` (`tests/g1/README.md`). + ### mac ```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) @@ -303,18 +717,39 @@ source /home/code/h12_sim_scripts/robot_cli.sh # rob_pose, rob_grip, rob_stand ``` 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`, +`ros2 launch /home/code/core_ws/src/golem_bringup/launch/h12/h12_sim_mac.launch.py` +(launched by path; only skills built on mac are available — no `golem_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 +H1-2: onboard PC, native (no docker): build the whole workspace, as +`launch_ros.sh` does (the G1's packages included: `golem_bringup` +exec_depends on them). The build +includes `golem_ros2_models`, which installs both robots' models from the +checkout's `CL_Assets` (its LFS meshes pulled): the H1-2's launches read the +robot description from `golem_ros2_models`, not from `CL_Assets` directly. +`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` +`ros2 launch golem_bringup h12_real_robot.launch.py` (`start_position_verified:=true` only after physically verifying the robot's -pose — it gates leg commands). +pose — it gates leg commands; the policy then holds its pre-pose until +`/lowerbody/confirm_engage`). Companion desktop: +`ros2 launch golem_bringup h12_real_desktop.launch.py`. The arms start only +once `golem_safety` relays `frame_task_server`'s commands, which needs the estop +box connected: a `safety_node` that starts while `estop_node` reports a failed +connection latches until restart, and `frame_task_server` exits after +`controller.startup_timeout` (30 s) when nothing is relayed (§6). + +G1: in Unitree's debug mode, on a gantry; the procedure for entering debug +mode on our G1 is not written yet, and nobody runs a session until it is +(`docs/g1/first-session.md`). The laptop wired to the robot, in Docker: +`ROS_DOMAIN_ID=0` in `docker/.env`, `docker/scripts/docker_run.sh ros`, then +`ros2 launch golem_bringup g1_real_robot.launch.py use_hands:=false use_rviz:=false` +— the arms; the leg motors stay off. `start_position_verified:=true` adds +HOMIE's walking node, which pre-poses the legs and holds them until +`/lowerbody/confirm_engage`. Nothing has run on the real G1 yet; follow +`docs/g1/first-session.md` step by step. ## 10. Getting-started checklist for a new agent @@ -323,20 +758,51 @@ 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/`; the + G1 starts at `docs/g1/index.md`. ### 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). +- **Restart the sim containers after pulling.** The sims run the checkout's + `golem_robocasa` and `CL_isaaclab_sim` from bind mounts, so a running sim keeps + the code it started with. An H1-2 sim started before the H1-2 sims reported + `mode_machine` 6 is refused: `wait_for_sim.py` stops `docker_stack.sh`, and + `golem_safety` never engages (`engage_refused`: `/lowstate mode_machine is 0, + expected 6`), so `frame_task_server` exits after 30 s. For Isaac, run + `git submodule update` first: the H1-2 Isaac sim reports 6 from the current + `CL_isaaclab_sim` pin. +- **Delete the stale installs of removed packages once after pulling.** colcon + never removes a package that is gone from `core_ws/src`, so a workspace built + before `golem_bringup` replaced `h12_bringup` and `g1_bringup` (and the four + unused packages `h12_safety_layer`, `h12_ros2_controller`, `g1_safety_layer` + and `g1_control` were deleted), or before `golem_ros2_models` replaced + `h12_ros2_model` and `g1_description`, or before `dex_3` and `g1_homie` + replaced `g1_dex3` and `g1_locomotion`, keeps their build and install + directories. + From the repo root, on every host that built the workspace (`sudo` for files + a container left root-owned): + `rm -rf {core_ws/build,core_ws/install,core_ws/build_stack_tests,core_ws/install_stack_tests,container_cache/g1_tests/build,container_cache/g1_tests/install}/{h12_bringup,g1_bringup,h12_safety_layer,h12_ros2_controller,g1_safety_layer,g1_control,h12_ros2_model,g1_description,g1_dex3,g1_locomotion,h12_slam,h12_deploy_mjpc,h12_skills,g1_navigation}`. + Until then `launch_ros.sh` (`docker_run.sh ros`, `docker_stack.sh`, + `tests/g1`) exits before it builds, with `stale installed package '' has + no discovered source`, and a native workspace (the H1-2's onboard PC) keeps + answering to the old names. A `--symlink-install` workspace also keeps links + to deleted G1 configs that fail the next `golem_upper_ik` build; the change + report (`docs/superpowers/specs/2026-10-28-golem-bringup-and-g1-debug-mode/change-report.md`) + has that cleanup and the Mac's. - **x86 `ros` container ≠ bringup.** It only builds and gives you a shell. +- **Check the `robot:` line on simulator and stack runs.** `docker_run.sh robocasa|isaac` + and `docker_stack.sh up` print the robot they selected; a `GOLEM_ROBOT` left + in the shell or `docker/.env` switches simulator robots silently otherwise. + Stop one robot's containers before starting the other's. - 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. @@ -347,10 +813,18 @@ 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. + +### Navigation/package migration (2026-10-29) + +`h12_slam`, `h12_deploy_mjpc` and `h12_skills` directories and Python modules +are now `golem_slam`, `deploy_mjpc` and `golem_skills`, and `g1_navigation` was +removed (`g1_body_to_pelvis` moved into `golem_slam` and `/skill/frontier_explore` +unified in `golem_skills`). Clean their old build/install prefixes as documented +in `docs/RUNNING.md`. G1 bringup uses the shared SLAM pipeline with G1 profiles +(scan matching/loop closure, OctoMap, MPPI). The G1 launch requires FAST-LIO and +refuses real mode and sim_gt. H1-2 defaults are preserved. G1 floor alignment +uses a 0.5 m minimum floor depth, and its dynamic base transform is refreshed per +odometry sample. Do not infer real G1 validation from simulated navigation checks. diff --git a/CL_Assets b/CL_Assets index e65d24c..963c93f 160000 --- a/CL_Assets +++ b/CL_Assets @@ -1 +1 @@ -Subproject commit e65d24c0be547e13978a509b2bddc0622cf5ca32 +Subproject commit 963c93f8611a193eeb344b41b34c44dcf201e0ec diff --git a/CL_isaaclab_sim b/CL_isaaclab_sim index deec4de..7395998 160000 --- a/CL_isaaclab_sim +++ b/CL_isaaclab_sim @@ -1 +1 @@ -Subproject commit deec4dea876839a2909ed6fa3dabf99b53478b9d +Subproject commit 739599845d16af6cbf2be3744c4dfb3a33bb1e65 diff --git a/README.md b/README.md index d3c99c7..0267a54 100644 --- a/README.md +++ b/README.md @@ -1,459 +1,111 @@ # 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 humanoids: the H1-2 and, beside it, the G1 +(see [The G1](#the-g1)). -## 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 +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 -docker/scripts/docker_build.sh # all three -docker/scripts/docker_build.sh robocasa ros # subset -docker/scripts/docker_build.sh isaac # isaac only +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 ``` -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 +Then, in two terminals: ```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 +docker/scripts/docker_run.sh robocasa # 1. simulator (start first) -# to use a custom ROS domain (e.g. several devs on one network), -# set ROS_DOMAIN_ID in docker/.env (see Configuration above) +docker/scripts/docker_run.sh ros # 2. workspace shell +ros2 launch golem_bringup h12_sim.launch.py # bringup is a manual step ``` -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: +And in a third, ask the robot to do something: ```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 -``` - -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: - -```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 - ``` - - 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 -``` - -## 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. - -```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 + "{target_object: 'vertical fridge handle', arm: 'right', timeout: {sec: 60, nanosec: 0}}" --feedback ``` -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**: +On Apple Silicon there is a CPU-only port that runs ROS 2 + MuJoCo without +Isaac (H1-2 only) — see [`docs/MACOS.md`](docs/MACOS.md). -```bash -GOLEM_DISPLAY=vnc GOLEM_RVIZ=vnc GOLEM_LOWERBODY=fame \ - docker compose -f docker/mac/docker-compose.yml up -``` +## The G1 -`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: +GOLEM also runs the Unitree G1 (29 DoF, two Dex3-1 hands): its own packages +(`core_ws/src/g1_homie`, HOMIE's walking policy; `core_ws/src/dex_3`, the +Dex3-1 hands), the +safety layer, arm IK, models, bringup, SLAM/navigation and skills packages it shares with the H1-2 +(`core_ws/src/golem_safety`, `core_ws/src/golem_upper_ik`, +`core_ws/src/golem_ros2_models`, `core_ws/src/golem_bringup`, +`core_ws/src/golem_slam`, `core_ws/src/golem_skills`), its models in +`CL_Assets/g1`, and a RoboCasa sim in +`golem_robocasa/g1/`. The H1-2 stays the default; `--g1` on a wrapper, or +`GOLEM_ROBOT=g1`, selects the G1 (x86 Linux, RoboCasa only). The images are +the same, and every `ros` container builds and runs both robots' packages. ```bash -GOLEM_DISPLAY=vnc GOLEM_RVIZ=vnc GOLEM_LOWERBODY=switch \ - docker compose -f docker/mac/docker-compose.yml up +tests/g1/test_sim_bringup.sh # the G1 acceptance test (arms); see tests/g1/README.md +docker/scripts/docker_stack.sh up robocasa --g1 # the G1 sim and its bringup (the arms and HOMIE) ``` -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 the real G1 the whole stack runs in Docker on a laptop wired to the robot, +with the robot in Unitree's debug mode. Nothing has run on it yet; the first +session follows [`docs/g1/first-session.md`](docs/g1/first-session.md). Start +at [`docs/g1/index.md`](docs/g1/index.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: +**Licence note.** HOMIE's walking policy in +`core_ws/src/g1_homie/policy/` (from +[OpenHomie](https://github.com/InternRobotics/OpenHomie)) is for +non-commercial use only. Its `LICENSE` is CC BY-NC 4.0, but OpenHomie's README +says CC BY-NC-SA 4.0, which also requires changed versions to be shared under +the same licence: treat it as CC BY-NC-SA 4.0. The G1 packages around it +(`g1_homie`, `dex_3`) are MIT. See the policy's `README.md`. -```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 — 23 packages, the G1's in `core_ws/src/{g1_homie,dex_3}`; where most development happens | +| `golem_robocasa/` | RoboCasa/MuJoCo simulator entry point for both robots and its ROS/DDS bridges; the G1 sim in `golem_robocasa/g1/` | +| `CL_isaaclab_sim/` | Isaac Lab simulator (H1-2) | +| `CL_Assets/` | both robots' URDFs, MJCFs, meshes and self-collision models (the G1's in `CL_Assets/g1/`), Isaac USD (Git-LFS) | +| `docker/` | x86 images, compose files (with the G1 overlay), and the build/run scripts | +| `docker/mac/` | the self-contained Apple-Silicon port | +| `mujoco_mpc/`, `unitree_sdk2_python/` | vendored upstream dependencies | +| `tests/` | simulator regression and acceptance tests | +| `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 | +| [`docs/DOCKER_TESTING.md`](docs/DOCKER_TESTING.md) | the Docker smoke tests on Ubuntu NVIDIA and Apple-Silicon Mac | +| [`docs/g1/`](docs/g1/index.md) | the G1: sim flows, safety layer, DDS, walking, navigation, exploration, the first real session | +| [`tests/README.md`](tests/README.md) | the simulator regression and acceptance tests | +| [`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..a09ff94 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 --> None: rclpy.init() node = Node("mjpc_deploy_lowerbody_controller") - _libdir = os.path.join(get_package_prefix("h12_deploy_mjpc"), - "lib", "h12_deploy_mjpc") + _libdir = os.path.join(get_package_prefix("deploy_mjpc"), + "lib", "deploy_mjpc") p = node.declare_parameter # controller_exe selects which compiled core to run (mjpc_lowerbody_core = @@ -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_deploy_mjpc/h12_deploy_mjpc/debug_visualizer_node.py b/core_ws/src/deploy_mjpc/deploy_mjpc/debug_visualizer_node.py similarity index 100% rename from core_ws/src/h12_deploy_mjpc/h12_deploy_mjpc/debug_visualizer_node.py rename to core_ws/src/deploy_mjpc/deploy_mjpc/debug_visualizer_node.py diff --git a/core_ws/src/h12_deploy_mjpc/h12_deploy_mjpc/estimator_node.py b/core_ws/src/deploy_mjpc/deploy_mjpc/estimator_node.py similarity index 99% rename from core_ws/src/h12_deploy_mjpc/h12_deploy_mjpc/estimator_node.py rename to core_ws/src/deploy_mjpc/deploy_mjpc/estimator_node.py index 217d130..f6c5ec8 100644 --- a/core_ws/src/h12_deploy_mjpc/h12_deploy_mjpc/estimator_node.py +++ b/core_ws/src/deploy_mjpc/deploy_mjpc/estimator_node.py @@ -44,7 +44,7 @@ def main() -> None: rclpy.init() - node = Node("h12_deploy_mjpc_estimator") + node = Node("deploy_mjpc_estimator") p = node.declare_parameter script = p("estimator_script", _DEFAULT_SCRIPT).value diff --git a/core_ws/src/h12_deploy_mjpc/package.xml b/core_ws/src/deploy_mjpc/package.xml similarity index 98% rename from core_ws/src/h12_deploy_mjpc/package.xml rename to core_ws/src/deploy_mjpc/package.xml index 6963bee..64df94d 100644 --- a/core_ws/src/h12_deploy_mjpc/package.xml +++ b/core_ws/src/deploy_mjpc/package.xml @@ -1,7 +1,7 @@ - h12_deploy_mjpc + deploy_mjpc 0.0.0 MJPC lower-body balance controller for the Unitree H1-2: the fork's own CLI controller (badinkajink/mujoco_mpc@extended_hw diff --git a/core_ws/src/h12_deploy_mjpc/resource/h12_deploy_mjpc b/core_ws/src/deploy_mjpc/resource/deploy_mjpc similarity index 100% rename from core_ws/src/h12_deploy_mjpc/resource/h12_deploy_mjpc rename to core_ws/src/deploy_mjpc/resource/deploy_mjpc diff --git a/core_ws/src/dex_3/.gitignore b/core_ws/src/dex_3/.gitignore new file mode 100644 index 0000000..1bc127a --- /dev/null +++ b/core_ws/src/dex_3/.gitignore @@ -0,0 +1,4 @@ +# Build artifacts +*.egg-info/ +__pycache__/ +*.pyc diff --git a/core_ws/src/dex_3/README.md b/core_ws/src/dex_3/README.md new file mode 100644 index 0000000..1b08d54 --- /dev/null +++ b/core_ws/src/dex_3/README.md @@ -0,0 +1,111 @@ +# dex_3 + +ROS 2 driver for the G1's two Dex3-1 hands. It talks to the hands directly over +Unitree DDS (`rt/dex3/{left,right}/cmd` and `state`, `unitree_hg` `HandCmd_` / +`HandState_`); the hands are not on the `rt/lowcmd` path or the safety layer. + +## ROS interface + +Per hand, `` is `left` or `right`: + +- `//hand/set_pose` (`custom_ros_messages/srv/SetHandPose`): 7 joint targets + in `HandCmd_` order (thumb_0, thumb_1, thumb_2, middle_0, middle_1, index_0, + index_1), rad, reached by linear interpolation over `duration` seconds, or + longer: no joint moves faster than `max_speed`. So `duration: 0` (the + srv's "go directly") is ramped too, e.g. 0.75 s for 1.5 rad at the default + 2 rad/s. A negative or non-finite `duration` is refused. Targets are + clipped to the URDF limits this package keeps + (`HAND_JOINTS` in `dex_3/dex3.py`). +- `//hand/open`, `//hand/close` (`std_srvs/Trigger`): named poses, + reached over `pose_duration` (default 1 s; the default `max_speed` never + stretches them, from any pose inside the limits). +- `//hand/release` (`std_srvs/Trigger`): sends Unitree's stop (timeout bit, + zero gains; the fingers go limp) and stops commanding that hand. +- `//hand/pressure` (`std_msgs/Float32MultiArray`, sensors x 12). +- `/joint_states`: the finger joints, so `robot_state_publisher` can draw the + hands; the body joints come from `golem_upper_ik`'s `joint_state_publisher`. + +```bash +ros2 launch dex_3 hands.launch.py # options: on_shutdown:=silent max_speed:=1.0 +ros2 service call /left/hand/close std_srvs/srv/Trigger +ros2 service call /right/hand/set_pose custom_ros_messages/srv/SetHandPose \ + "{positions: [0.0, -0.3, -0.6, 0.6, 0.6, 0.6, 0.6], duration: 1.0}" +``` + +## Parameters + +Besides `sides`, `kp`, `kd`, `rate_hz`, `pose_duration`, `joint_state_hz` and +`network_interface` (empty: `G1_DDS_INTERFACE`): + +- `domain_id` (default -1, unset): the Unitree DDS domain is `ROS_DOMAIN_ID`'s + (0 is the real robot, 1 the sim). With `ROS_DOMAIN_ID` unset or empty, this + parameter must give it; with neither, the node refuses to start. There is + no fallback to domain 0. The node also refuses a `domain_id` that disagrees + with `ROS_DOMAIN_ID`, and a Unitree domain other than the one its own ROS + side runs on. With `ROS_DOMAIN_ID` unset, ROS 2 runs on its default domain 0, + so in practice set `ROS_DOMAIN_ID`: without it only `domain_id:=0` starts. + Write it in plain decimal: rcl reads a leading 0 as octal (`0141` is domain + 97), and the node refuses the mismatch. The startup log names the domain + and interface. +- `on_shutdown` (default `release`): what a hand this node was commanding gets + when the node exits. + - `release`: Unitree's stop (timeout bit, zero gains), so the fingers go + limp and drop what they hold. The default: once the driver is gone, + nothing keeps the grip under control, so the hand is left in a known + state. + - `silent`: no stop; the driver just stops publishing, leaving the hand + with its last command. What the Dex3 then does is unknown: the Unitree + code we have read (the Dex3 examples in `unitree_ros2` and + `unitree_sdk2`, `xr_teleoperate`'s Dex3 controller) documents no command + timeout. The only hint is that both Dex3 examples re-send their grip only + once a second, so a command presumably lasts at least that long. Do not + rely on `silent` holding an object until it has been tried on our hands. + - `on_shutdown` is applied on SIGINT, SIGTERM and SIGHUP (e.g. the + terminal it runs in closing), however many arrive (Ctrl-C in a terminal + reaches the node twice: from the terminal, and from `ros2 launch` passing + it on), and the node then exits 0. SIGKILL and a crash end the node + without it, so they always behave like `silent`. +- `max_speed` (default 2.0 rad/s): cap on any finger joint's commanded speed, + for every move including the first. A first-session choice, below every + URDF velocity limit (12 rad/s, thumb_0 3.14 rad/s). + +## Behavior + +- A hand is left alone until its first command: nothing is published to it, so + another program driving it is not disturbed. The first command starts from + the hand's measured pose. It needs `rt/dex3//state` from the last + 0.5 s with finite joint positions, or it is refused. +- While commanding, `HandCmd_` streams at `rate_hz` (default 100) holding the + current target. Timing uses the monotonic clock, so a wall-clock step (NTP) + cannot cut a move short. On shutdown, see `on_shutdown`. +- Gains start at Unitree's Dex3 example values, kp 1.5 and kd 0.1 (launch args + `kp`, `kd`). The hand's real gains are not in any model; tune on hardware. + Negative or non-finite gains are refused. +- The motor `mode` byte follows Unitree's example: motor index in bits 0-3, + status 1 in bits 4-6, timeout bit 7. + +## Poses + +Closing directions were checked with forward kinematics on the with-hand URDF. +The hands are mirrored: the left thumb closes toward its upper limits and the +left fingers toward their lower limits, the right hand the other way. + +- `open`: all zeros, fingers straight. +- `close`: fingers at 70% of their closing range, thumb_2 at 60%, thumb_1 at 50%, + thumb_0 at 0; the thumb ends about 1 cm from the finger tips. + +## Before using it on the robot + +- Nothing else may be publishing `rt/dex3/{left,right}/cmd`; two publishers + fight over the hands. +- Whether `thumb_0` mirrors between hands is still unknown on our unit. + +## Tests + +```bash +PYTHONPATH=.:$PYTHONPATH python3 -m pytest test +``` + +The DDS tests run on domain 97 inside one process against a fake hand; they never +touch the robot. `test_hand_node.py` needs rclpy from the sourced workspace (the +`:$PYTHONPATH` above); without it, it is skipped. diff --git a/core_ws/src/dex_3/dex_3/__init__.py b/core_ws/src/dex_3/dex_3/__init__.py new file mode 100644 index 0000000..1f8d370 --- /dev/null +++ b/core_ws/src/dex_3/dex_3/__init__.py @@ -0,0 +1 @@ +'''Dex3-1 hand driver for the Unitree G1''' diff --git a/core_ws/src/dex_3/dex_3/dex3.py b/core_ws/src/dex_3/dex_3/dex3.py new file mode 100644 index 0000000..387154d --- /dev/null +++ b/core_ws/src/dex_3/dex_3/dex3.py @@ -0,0 +1,165 @@ +''' +Dex3-1 hand logic with no ROS or DDS: joint limits, named poses, motor mode, +interpolation, the driver's DDS domain and shutdown choices +''' + +import math + +import numpy as np + +SIDES = ('left', 'right') +# motors per hand: the HandCmd_/HandState_ motor entries the driver writes and reads +NUM_HAND_MOTORS = 7 +# each hand's joints in HandCmd_/HandState_ motor order: (name, lower, upper), rad. +# The limits are the Dex3-1's in Unitree's G1 URDF +# (CL_Assets/g1/g1_description/g1_29dof_with_hand_rev_1_0.urdf). Six of the seven are +# mirrored between the hands; thumb_0's range is symmetric, so it is the same on both +HAND_JOINTS = { + 'left': ( + ('left_hand_thumb_0_joint', -1.04719755, 1.04719755), + ('left_hand_thumb_1_joint', -0.61086523, 1.04719755), + ('left_hand_thumb_2_joint', 0.0, 1.74532925), + ('left_hand_middle_0_joint', -1.57079632, 0.0), + ('left_hand_middle_1_joint', -1.74532925, 0.0), + ('left_hand_index_0_joint', -1.57079632, 0.0), + ('left_hand_index_1_joint', -1.74532925, 0.0), + ), + 'right': ( + ('right_hand_thumb_0_joint', -1.04719755, 1.04719755), + ('right_hand_thumb_1_joint', -1.04719755, 0.61086523), + ('right_hand_thumb_2_joint', -1.74532925, 0.0), + ('right_hand_middle_0_joint', 0.0, 1.57079632), + ('right_hand_middle_1_joint', 0.0, 1.74532925), + ('right_hand_index_0_joint', 0.0, 1.57079632), + ('right_hand_index_1_joint', 0.0, 1.74532925), + ), +} +JOINT_NAMES = {side: [name for name, _, _ in HAND_JOINTS[side]] for side in SIDES} +LOWER = {side: np.array([lower for _, lower, _ in HAND_JOINTS[side]]) for side in SIDES} +UPPER = {side: np.array([upper for _, _, upper in HAND_JOINTS[side]]) for side in SIDES} + +# gains from Unitree's Dex3 grip example +# (unitree_ros2/example/src/src/g1/dex3/g1_dex3_example.cpp); the hand's real +# PD gains are not in any model, so these start deliberately soft +DEFAULT_KP = 1.5 +DEFAULT_KD = 0.1 + +# cap on any finger joint's commanded speed, rad/s: a move, including a 0 s +# "go directly", takes at least its largest joint distance over this. A +# first-session choice, not a measured limit: below every URDF velocity limit +# (12 rad/s, thumb_0 3.14), and fast enough that open and close over the +# default 1 s are never stretched (from any pose inside the limits, no joint +# is more than 1.75 rad from either) +DEFAULT_MAX_SPEED = 2.0 + +# HandCmd_ motor mode byte, as in Unitree's example: bits 0-3 motor index, +# bits 4-6 status (1 = enabled), bit 7 timeout. Commands use timeout 0; the +# example's stop sends timeout 1 with zero gains, which releases the finger +MODE_STATUS_ENABLED = 0x01 + +# closing direction per joint (thumb_0, thumb_1, thumb_2, middle_0, middle_1, +# index_0, index_1), checked with forward kinematics on the with-hand URDF: +# the left hand closes toward the upper limit on the thumb and the lower limit +# on the fingers; the right hand is mirrored +CLOSE_TOWARD_UPPER = { + 'left': np.array([False, True, True, False, False, False, False]), + 'right': np.array([False, False, False, True, True, True, True]), +} +# fraction of each joint's closing range used by the `close` pose; thumb_0 +# (thumb rotation) stays at 0. Brings the thumb ~1 cm from the finger tips +CLOSE_FRACTION = np.array([0.0, 0.5, 0.6, 0.7, 0.7, 0.7, 0.7]) + + +def motor_mode(index: int, timeout: bool = False) -> int: + '''HandCmd_ mode byte for motor `index`''' + if not 0 <= index < NUM_HAND_MOTORS: + raise ValueError(f'motor index must be in [0, {NUM_HAND_MOTORS}), got {index}') + return (index & 0x0F) | ((MODE_STATUS_ENABLED & 0x07) << 4) | ((1 if timeout else 0) << 7) + + +def named_pose(side: str, name: str) -> np.ndarray: + '''Joint targets for `open` (all zeros, fingers straight) or `close` (power grip)''' + if name == 'open': + return np.zeros(NUM_HAND_MOTORS) + if name == 'close': + end = np.where(CLOSE_TOWARD_UPPER[side], UPPER[side], LOWER[side]) + return CLOSE_FRACTION * end + raise ValueError(f'unknown hand pose {name!r} (open, close)') + + +def validate_positions(side: str, positions) -> np.ndarray: + '''Return positions as a 7-vector clipped to the URDF limits; raise on bad input''' + if side not in SIDES: + raise ValueError(f'side must be one of {SIDES}, got {side!r}') + q = np.asarray(positions, dtype=np.float64).reshape(-1) + if q.shape != (NUM_HAND_MOTORS,): + raise ValueError(f'expected {NUM_HAND_MOTORS} joint positions, got {q.size}') + if not np.all(np.isfinite(q)): + raise ValueError('joint positions must be finite') + return np.clip(q, LOWER[side], UPPER[side]) + + +def move_duration(q_start, q_goal, duration, max_speed: float) -> float: + ''' + Seconds to interpolate from q_start to q_goal: `duration`, stretched so no + joint moves faster than `max_speed` rad/s. Raise on a negative or + non-finite duration (a NaN one would command NaN positions) + ''' + duration = float(duration) + if not math.isfinite(duration) or duration < 0.0: + raise ValueError(f'duration must be finite and >= 0 s, got {duration}') + distance = float(np.max(np.abs(np.asarray(q_goal, dtype=np.float64) + - np.asarray(q_start, dtype=np.float64)))) + return max(duration, distance / max_speed) + + +class PoseInterpolator: + '''Linear joint-space interpolation from one pose to another over a duration''' + + def __init__(self, q_start, q_goal=None, duration=0.0, t_start=0.0): + self.q_start = np.asarray(q_start, dtype=np.float64) + self.q_goal = self.q_start.copy() if q_goal is None else np.asarray(q_goal, dtype=np.float64) + self.duration = max(float(duration), 0.0) + self.t_start = float(t_start) + + def value(self, now: float) -> np.ndarray: + if self.duration <= 0.0: + return self.q_goal.copy() + alpha = min(max((now - self.t_start) / self.duration, 0.0), 1.0) + return (1.0 - alpha) * self.q_start + alpha * self.q_goal + + def done(self, now: float) -> bool: + return now - self.t_start >= self.duration + + +# dex3_hands' on_shutdown parameter: what a hand the node was commanding gets on +# exit (Unitree's stop, so the fingers go limp; or no message at all) +SHUTDOWN_ACTIONS = ('release', 'silent') + + +def dds_domain(ros_domain_id, domain_id_parameter, ros_domain=None): + ''' + Unitree DDS domain from ROS_DOMAIN_ID (unset or empty: None), else from the + domain_id parameter (< 0: not set). There is no default: domain 0 is the + real robot, so a missing domain is an error, and so is a mismatch. With + ros_domain, the domain the node's own ROS side runs on, the result must + also equal it: with ROS_DOMAIN_ID unset ROS runs on domain 0, and rcl reads + a leading 0 as octal ('010' is 8) + ''' + from_env = None + if ros_domain_id: + if not ros_domain_id.isdigit(): + raise ValueError(f'ROS_DOMAIN_ID must be an integer >= 0, got {ros_domain_id!r}') + from_env = int(ros_domain_id) + parameter = int(domain_id_parameter) + from_parameter = parameter if parameter >= 0 else None + if from_env is None and from_parameter is None: + raise ValueError('no DDS domain: set ROS_DOMAIN_ID (0 is the real robot, 1 the sim) ' + 'or the domain_id parameter') + if from_env is not None and from_parameter is not None and from_env != from_parameter: + raise ValueError(f'ROS_DOMAIN_ID={from_env} and domain_id:={from_parameter} disagree') + domain = from_env if from_env is not None else from_parameter + if ros_domain is not None and int(ros_domain) != domain: + raise ValueError(f'the node runs ROS on domain {ros_domain} but would run Unitree DDS on ' + f'domain {domain}: set ROS_DOMAIN_ID (decimal) to one domain for both') + return domain diff --git a/core_ws/src/dex_3/dex_3/driver.py b/core_ws/src/dex_3/dex_3/driver.py new file mode 100644 index 0000000..249b745 --- /dev/null +++ b/core_ws/src/dex_3/dex_3/driver.py @@ -0,0 +1,179 @@ +'''DDS driver for one Dex3-1 hand (rt/dex3//cmd and state), with no ROS''' + +import math +import time +import threading + +import numpy as np + +from unitree_sdk2py.idl.unitree_hg.msg.dds_ import HandCmd_, HandState_ +from unitree_sdk2py.core.channel import ChannelPublisher, ChannelSubscriber +from unitree_sdk2py.idl.default import unitree_hg_msg_dds__HandCmd_ as HandCmdDefault + +from dex_3.dex3 import ( + DEFAULT_KD, + DEFAULT_KP, + DEFAULT_MAX_SPEED, + NUM_HAND_MOTORS, + PoseInterpolator, + motor_mode, + move_duration, + validate_positions, +) + +# stop commands sent on release (timeout bit set, zero gains), like the stop in +# Unitree's Dex3 example; then the driver goes silent +RELEASE_MESSAGES = 10 +# how long shutdown() waits for those stop commands to go out (it returns as +# soon as they have; 10 take 0.1 s at 100 Hz, longer on a starved CPU) +RELEASE_TIMEOUT_S = 1.0 +# a first command needs hand state newer than this +MAX_STATE_AGE_S = 0.5 + + +def _check_value(name, value, positive=False): + '''value as a float; raise unless finite and >= 0 (> 0 if positive)''' + value = float(value) + bound = '> 0' if positive else '>= 0' + if not math.isfinite(value) or value < 0.0 or (positive and value == 0.0): + raise ValueError(f'{name} must be finite and {bound}, got {value}') + return value + + +class Dex3Hand: + ''' + One Dex3-1 hand. Silent until the first command, so a hand driven by another + program (or by nobody) is left alone; after that it streams HandCmd_ at + rate_hz, holding the current target. No joint is commanded faster than + max_speed rad/s. release() sends Unitree's stop and goes silent again. + Timing uses time.monotonic(), so a wall-clock step cannot skip a move + ''' + + def __init__(self, side, kp=DEFAULT_KP, kd=DEFAULT_KD, rate_hz=100.0, + cmd_topic=None, state_topic=None, max_speed=DEFAULT_MAX_SPEED): + self.side = side + self.kp = _check_value('kp', kp) + self.kd = _check_value('kd', kd) + self.max_speed = _check_value('max_speed', max_speed, positive=True) + self._dt = 1.0 / _check_value('rate_hz', rate_hz, positive=True) + self._lock = threading.Lock() + self._interp = None # None: not commanding + self._release_left = 0 + self._state = None + self._cmd = HandCmdDefault() + self._pub = ChannelPublisher(cmd_topic or f'rt/dex3/{side}/cmd', HandCmd_) + self._pub.Init() + self._sub = ChannelSubscriber(state_topic or f'rt/dex3/{side}/state', HandState_) + self._sub.Init(self._on_state, 10) + self._running = True + self._thread = threading.Thread(target=self._loop, name=f'dex3_{side}', daemon=True) + self._thread.start() + + def _on_state(self, msg: HandState_) -> None: + if len(msg.motor_state) < NUM_HAND_MOTORS: + return + motors = msg.motor_state[:NUM_HAND_MOTORS] + state = { + 'q': np.array([m.q for m in motors], dtype=np.float64), + 'dq': np.array([m.dq for m in motors], dtype=np.float64), + 'tau': np.array([m.tau_est for m in motors], dtype=np.float64), + 'pressure': np.array([list(p.pressure) for p in msg.press_sensor_state], + dtype=np.float32).reshape(-1, 12), + 'time': time.monotonic(), + } + with self._lock: + self._state = state + + @property + def state(self): + '''Latest hand state (q, dq, tau, pressure, time from time.monotonic()) or None''' + with self._lock: + return None if self._state is None else dict(self._state) + + @property + def commanding(self) -> bool: + with self._lock: + return self._interp is not None + + def set_pose(self, positions, duration: float = 0.0) -> np.ndarray: + ''' + Move to `positions` (7 rad, clipped to the URDF limits) over `duration` + s, or longer if a joint would otherwise move faster than max_speed + ''' + goal = validate_positions(self.side, positions) + now = time.monotonic() + with self._lock: + if self._interp is not None: + start = self._interp.value(now) + else: + # first command: start from where the hand is + if self._state is None or now - self._state['time'] > MAX_STATE_AGE_S: + raise RuntimeError( + f'no recent {self.side} hand state on rt/dex3/{self.side}/state') + start = self._state['q'].copy() + if not np.all(np.isfinite(start)): + raise RuntimeError( + f'{self.side} hand state has non-finite joint positions: {start}') + seconds = move_duration(start, goal, duration, self.max_speed) + self._interp = PoseInterpolator(start, goal, seconds, now) + self._release_left = 0 + return goal + + def release(self) -> bool: + ''' + Send Unitree's stop (fingers go limp) and stop commanding. Does nothing, + and returns False, if this driver is not commanding the hand + ''' + with self._lock: + if self._interp is None: + return False + self._interp = None + self._release_left = RELEASE_MESSAGES + return True + + def _fill(self, q, kp, kd, timeout): + for i in range(NUM_HAND_MOTORS): + motor = self._cmd.motor_cmd[i] + motor.mode = motor_mode(i, timeout=timeout) + motor.q = float(q[i]) + motor.dq = 0.0 + motor.tau = 0.0 + motor.kp = float(kp) + motor.kd = float(kd) + + def _loop(self): + while self._running: + start_time = time.monotonic() + send = False + with self._lock: + if self._interp is not None: + self._fill(self._interp.value(start_time), self.kp, self.kd, timeout=False) + send = True + elif self._release_left > 0: + self._fill(np.zeros(NUM_HAND_MOTORS), 0.0, 0.0, timeout=True) + self._release_left -= 1 + send = True + if send: + self._pub.Write(self._cmd) + time.sleep(max(0.0, self._dt - (time.monotonic() - start_time))) + + def shutdown(self, release: bool = True) -> None: + ''' + Stop the driver. With release (the default), a hand this driver was + commanding first gets Unitree's stop, so its fingers go limp. With + release=False the driver just stops publishing, and the hand is left + with the last command it got (what the Dex3 then does is not documented + by Unitree). Stop messages already queued by release() are sent either way + ''' + if release: + self.release() + deadline = time.monotonic() + RELEASE_TIMEOUT_S + while time.monotonic() < deadline: + with self._lock: + if self._release_left == 0: + break + time.sleep(self._dt) + self._running = False + self._thread.join(timeout=1.0) + self._pub.Close() + self._sub.Close() diff --git a/core_ws/src/dex_3/dex_3/hand_node.py b/core_ws/src/dex_3/dex_3/hand_node.py new file mode 100644 index 0000000..1845dd9 --- /dev/null +++ b/core_ws/src/dex_3/dex_3/hand_node.py @@ -0,0 +1,188 @@ +'''ROS 2 node driving both Dex3-1 hands over rt/dex3/*''' + +import os +import signal +import threading +from functools import partial + +import rclpy +from rclpy.node import Node +from rclpy.signals import SignalHandlerOptions +from std_srvs.srv import Trigger +from sensor_msgs.msg import JointState +from std_msgs.msg import Float32MultiArray, MultiArrayDimension +from unitree_sdk2py.core.channel import ChannelFactoryInitialize + +from custom_ros_messages.srv import SetHandPose + +from dex_3.driver import Dex3Hand +from dex_3.dex3 import ( + DEFAULT_KD, + DEFAULT_KP, + DEFAULT_MAX_SPEED, + JOINT_NAMES, + SHUTDOWN_ACTIONS, + SIDES, + dds_domain, + named_pose, +) + + +class Dex3HandNode(Node): + ''' + Per hand in {left, right}: + - services //hand/set_pose (SetHandPose), //hand/open, + //hand/close and //hand/release (std_srvs/Trigger) + - topic //hand/pressure (Float32MultiArray, sensors x 12) + and /joint_states with the finger joints, for robot_state_publisher. + A hand is only commanded after its first service call. On shutdown a hand + this node was commanding is released (on_shutdown 'release', the fingers go + limp) or simply left with its last command ('silent') + ''' + + def __init__(self): + super().__init__('dex3_hands') + self.declare_parameter('sides', list(SIDES)) + self.declare_parameter('kp', DEFAULT_KP) + self.declare_parameter('kd', DEFAULT_KD) + self.declare_parameter('rate_hz', 100.0) + self.declare_parameter('pose_duration', 1.0) # s, for open and close + self.declare_parameter('joint_state_hz', 50.0) + self.declare_parameter('network_interface', '') + self.declare_parameter('domain_id', -1) # used when ROS_DOMAIN_ID is unset; < 0: unset + self.declare_parameter('on_shutdown', 'release') # release | silent + self.declare_parameter('max_speed', DEFAULT_MAX_SPEED) # rad/s, any finger joint + + sides = [s for s in self.get_parameter('sides').value] + bad = [s for s in sides if s not in SIDES] + if bad: + raise ValueError(f'unknown hand sides {bad}; use {list(SIDES)}') + self._pose_duration = float(self.get_parameter('pose_duration').value) + self._on_shutdown = str(self.get_parameter('on_shutdown').value) + if self._on_shutdown not in SHUTDOWN_ACTIONS: + raise ValueError(f'on_shutdown must be one of {list(SHUTDOWN_ACTIONS)}, ' + f'got {self._on_shutdown!r}') + + # Unitree DDS on the same domain as ROS (the real robot is domain 0), + # never a default one: refused unless it is also the domain this node's + # ROS side runs on. The SDK ignores CYCLONEDDS_URI, so the interface + # comes from the network_interface parameter or G1_DDS_INTERFACE + # (e.g. enp130s0) + domain = dds_domain(os.environ.get('ROS_DOMAIN_ID'), + self.get_parameter('domain_id').value, + ros_domain=self.context.get_domain_id()) + interface = (self.get_parameter('network_interface').value + or os.environ.get('G1_DDS_INTERFACE', '')) + if interface: + ChannelFactoryInitialize(domain, interface) + else: + ChannelFactoryInitialize(domain) + + kp = float(self.get_parameter('kp').value) + kd = float(self.get_parameter('kd').value) + rate_hz = float(self.get_parameter('rate_hz').value) + max_speed = float(self.get_parameter('max_speed').value) + self.hands = {side: Dex3Hand(side, kp=kp, kd=kd, rate_hz=rate_hz, max_speed=max_speed) + for side in sides} + + self._pressure_pubs = {} + for side in sides: + prefix = f'/{side}/hand' + self.create_service(SetHandPose, f'{prefix}/set_pose', partial(self._set_pose, side)) + for name in ('open', 'close'): + self.create_service(Trigger, f'{prefix}/{name}', partial(self._named_pose, side, name)) + self.create_service(Trigger, f'{prefix}/release', partial(self._release, side)) + self._pressure_pubs[side] = self.create_publisher( + Float32MultiArray, f'{prefix}/pressure', 10) + self._joint_state_pub = self.create_publisher(JointState, '/joint_states', 10) + self.create_timer(1.0 / float(self.get_parameter('joint_state_hz').value), + self._publish_state) + self.get_logger().info( + f'Dex3 hands {sides} ready on DDS domain {domain}, interface ' + f'{interface or "auto"} (kp={kp}, kd={kd}, max_speed={max_speed} rad/s); ' + f'silent until the first command; on_shutdown={self._on_shutdown}') + + def _set_pose(self, side, request, response): + try: + goal = self.hands[side].set_pose(request.positions, request.duration) + except (ValueError, RuntimeError) as err: + response.success = False + response.message = str(err) + return response + response.success = True + response.message = f'{side} hand moving to {goal.round(3).tolist()}' + return response + + def _named_pose(self, side, name, request, response): + try: + self.hands[side].set_pose(named_pose(side, name), self._pose_duration) + except (ValueError, RuntimeError) as err: + response.success = False + response.message = str(err) + return response + response.success = True + response.message = f'{side} hand: {name}' + return response + + def _release(self, side, request, response): + released = self.hands[side].release() + response.success = True + response.message = (f'{side} hand released' if released + else f'{side} hand was not being commanded') + return response + + def _publish_state(self): + msg = JointState() + msg.header.stamp = self.get_clock().now().to_msg() + for side, hand in self.hands.items(): + state = hand.state + if state is None: + continue + msg.name.extend(JOINT_NAMES[side]) + msg.position.extend(state['q'].tolist()) + msg.velocity.extend(state['dq'].tolist()) + msg.effort.extend(state['tau'].tolist()) + pressure = Float32MultiArray() + rows, cols = state['pressure'].shape + pressure.layout.dim = [ + MultiArrayDimension(label='sensor', size=rows, stride=rows * cols), + MultiArrayDimension(label='reading', size=cols, stride=cols), + ] + pressure.data = state['pressure'].reshape(-1).tolist() + self._pressure_pubs[side].publish(pressure) + if msg.name: + self._joint_state_pub.publish(msg) + + def destroy_node(self): + for hand in self.hands.values(): + hand.shutdown(release=self._on_shutdown == 'release') + return super().destroy_node() + + +# signals that end the node through destroy_node(), so on_shutdown applies: +# Ctrl-C, ros2 launch shutting down (SIGINT, then SIGTERM) and a closed terminal +# (SIGHUP, whose default would end the process at once). SIGKILL cannot be caught +EXIT_SIGNALS = (signal.SIGINT, signal.SIGTERM, signal.SIGHUP) + + +def main(args=None): + # an exit signal only requests the exit, however many arrive. rclpy's own + # handlers stay off: Ctrl-C in a terminal reaches the node twice (from the + # terminal, and from ros2 launch passing it on), and a second + # KeyboardInterrupt would cut the release short + exit_requested = threading.Event() + for signum in EXIT_SIGNALS: + signal.signal(signum, lambda *_: exit_requested.set()) + rclpy.init(args=args, signal_handler_options=SignalHandlerOptions.NO) + node = Dex3HandNode() + try: + while not exit_requested.is_set(): + rclpy.spin_once(node, timeout_sec=0.1) + finally: + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/core_ws/src/dex_3/launch/hands.launch.py b/core_ws/src/dex_3/launch/hands.launch.py new file mode 100644 index 0000000..1fc4122 --- /dev/null +++ b/core_ws/src/dex_3/launch/hands.launch.py @@ -0,0 +1,49 @@ +'''Start the Dex3-1 hand driver. + + ros2 launch dex_3 hands.launch.py # both hands + ros2 launch dex_3 hands.launch.py sides:="[left]" # one hand + ros2 launch dex_3 hands.launch.py kp:=1.0 kd:=0.1 + ros2 launch dex_3 hands.launch.py on_shutdown:=silent # don't release on exit + +The DDS domain is ROS_DOMAIN_ID's (0 is the real robot). With ROS_DOMAIN_ID +unset, domain_id must be given and match the domain ROS itself then runs on +(0), or the driver refuses to start: set ROS_DOMAIN_ID instead. +''' + +from typing import List + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterValue + + +def generate_launch_description(): + return LaunchDescription([ + DeclareLaunchArgument('sides', default_value='[left, right]'), + DeclareLaunchArgument('kp', default_value='1.5'), + DeclareLaunchArgument('kd', default_value='0.1'), + DeclareLaunchArgument('max_speed', default_value='2.0', + description='rad/s cap on any finger joint'), + DeclareLaunchArgument('on_shutdown', default_value='release', + description='release (fingers go limp) or silent (stop publishing)'), + DeclareLaunchArgument('domain_id', default_value='-1', + description='DDS domain if ROS_DOMAIN_ID is unset (-1: none)'), + DeclareLaunchArgument('use_sim_time', default_value='false'), + Node( + package='dex_3', + executable='dex3_hands', + name='dex3_hands', + output='screen', + parameters=[{ + 'sides': ParameterValue(LaunchConfiguration('sides'), value_type=List[str]), + 'kp': ParameterValue(LaunchConfiguration('kp'), value_type=float), + 'kd': ParameterValue(LaunchConfiguration('kd'), value_type=float), + 'max_speed': ParameterValue(LaunchConfiguration('max_speed'), value_type=float), + 'on_shutdown': ParameterValue(LaunchConfiguration('on_shutdown'), value_type=str), + 'domain_id': ParameterValue(LaunchConfiguration('domain_id'), value_type=int), + 'use_sim_time': ParameterValue(LaunchConfiguration('use_sim_time'), value_type=bool), + }], + ), + ]) diff --git a/core_ws/src/dex_3/package.xml b/core_ws/src/dex_3/package.xml new file mode 100644 index 0000000..30a403f --- /dev/null +++ b/core_ws/src/dex_3/package.xml @@ -0,0 +1,23 @@ + + + + dex_3 + 0.0.0 + Dex3-1 hand driver for the Unitree G1 (rt/dex3/* over DDS) + Niraj Pudasaini + MIT + + rclpy + sensor_msgs + std_msgs + std_srvs + custom_ros_messages + launch + launch_ros + + python3-pytest + + + ament_python + + diff --git a/core_ws/src/h12_skills/h12_skills/__init__.py b/core_ws/src/dex_3/resource/dex_3 similarity index 100% rename from core_ws/src/h12_skills/h12_skills/__init__.py rename to core_ws/src/dex_3/resource/dex_3 diff --git a/core_ws/src/dex_3/setup.cfg b/core_ws/src/dex_3/setup.cfg new file mode 100644 index 0000000..93cccc5 --- /dev/null +++ b/core_ws/src/dex_3/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/dex_3 +[install] +install_scripts=$base/lib/dex_3 diff --git a/core_ws/src/dex_3/setup.py b/core_ws/src/dex_3/setup.py new file mode 100644 index 0000000..b50ee15 --- /dev/null +++ b/core_ws/src/dex_3/setup.py @@ -0,0 +1,32 @@ +from glob import glob + +from setuptools import find_packages, setup + +package_name = 'dex_3' + +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']), + ('share/' + package_name + '/launch', glob('launch/*.py')), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='Niraj Pudasaini', + maintainer_email='npudasai@nd.edu', + description='Dex3-1 hand driver for the Unitree G1 (rt/dex3/* over DDS)', + license='MIT', + extras_require={ + 'test': [ + 'pytest', + ], + }, + entry_points={ + 'console_scripts': [ + 'dex3_hands = dex_3.hand_node:main', + ], + }, +) diff --git a/core_ws/src/dex_3/test/test_dex3.py b/core_ws/src/dex_3/test/test_dex3.py new file mode 100644 index 0000000..8ef2325 --- /dev/null +++ b/core_ws/src/dex_3/test/test_dex3.py @@ -0,0 +1,131 @@ +import numpy as np +import pytest + +from dex_3.dex3 import ( + DEFAULT_MAX_SPEED, + HAND_JOINTS, + JOINT_NAMES, + LOWER, + NUM_HAND_MOTORS, + SIDES, + UPPER, + PoseInterpolator, + dds_domain, + motor_mode, + move_duration, + named_pose, + validate_positions, +) + + +@pytest.mark.parametrize('side', SIDES) +def test_hand_table_is_in_handcmd_order(side): + order = ['thumb_0', 'thumb_1', 'thumb_2', 'middle_0', 'middle_1', 'index_0', 'index_1'] + assert len(HAND_JOINTS[side]) == NUM_HAND_MOTORS == 7 + assert JOINT_NAMES[side] == [f'{side}_hand_{joint}_joint' for joint in order] + assert np.all(LOWER[side] < UPPER[side]) + + +def test_hands_mirror_except_thumb_0(): + for left, right in zip(HAND_JOINTS['left'], HAND_JOINTS['right']): + if left[0].endswith('thumb_0_joint'): + assert left[1:] == right[1:] + else: + assert right[1:] == (-left[2], -left[1]), left[0] + + +def test_motor_mode_matches_unitrees_example_encoding(): + # id in bits 0-3, status 1 in bits 4-6, timeout in bit 7 + assert motor_mode(0) == 0x10 + assert motor_mode(6) == 0x16 + assert motor_mode(3, timeout=True) == 0x93 + with pytest.raises(ValueError): + motor_mode(7) + + +@pytest.mark.parametrize('side', ['left', 'right']) +def test_named_poses_are_inside_the_limits(side): + for name in ('open', 'close'): + q = named_pose(side, name) + assert q.shape == (7,) + assert np.all(q >= LOWER[side]) and np.all(q <= UPPER[side]), name + assert np.all(named_pose(side, 'open') == 0.0) + + +def test_close_curls_the_fingers_the_right_way_and_mirrors(): + left, right = named_pose('left', 'close'), named_pose('right', 'close') + assert np.all(left[3:] < 0) and np.all(right[3:] > 0) # fingers + assert left[1] > 0 and left[2] > 0 # thumb toward the fingers + assert np.allclose(right, -left) # mirrored hands (thumb_0 stays 0) + + +def test_validate_clips_and_rejects_bad_input(): + q = validate_positions('left', [5.0, 0, 0, 0, 0, 0, -5.0]) + assert q[0] == UPPER['left'][0] and q[6] == LOWER['left'][6] + with pytest.raises(ValueError, match='7 joint positions'): + validate_positions('left', [0.0] * 6) + with pytest.raises(ValueError, match='finite'): + validate_positions('right', [0, 0, 0, np.nan, 0, 0, 0]) + with pytest.raises(ValueError, match='side'): + validate_positions('middle', [0.0] * 7) + + +def test_interpolator(): + interp = PoseInterpolator(np.zeros(7), np.ones(7), duration=2.0, t_start=10.0) + assert np.allclose(interp.value(10.0), 0.0) + assert np.allclose(interp.value(11.0), 0.5) + assert np.allclose(interp.value(20.0), 1.0) and interp.done(12.0) + assert np.allclose(PoseInterpolator(np.zeros(7), np.ones(7)).value(0.0), 1.0) + + +def test_move_duration_caps_the_joint_speed(): + start, goal = np.zeros(7), np.array([0.0, 0.0, 0.0, 1.0, 0.0, 0.0, -1.5]) + assert move_duration(start, goal, 0.0, 2.0) == pytest.approx(0.75) # "go directly", capped + assert move_duration(start, goal, 0.5, 2.0) == pytest.approx(0.75) + assert move_duration(start, goal, 2.0, 2.0) == pytest.approx(2.0) # slower requests stand + assert move_duration(start, start, 0.0, 2.0) == 0.0 + + +def test_move_duration_rejects_bad_durations(): + # a NaN duration would make PoseInterpolator command NaN positions + assert np.all(np.isnan(PoseInterpolator(np.zeros(7), np.ones(7), float('nan')).value(1.0))) + for bad in (float('nan'), float('inf'), -0.1): + with pytest.raises(ValueError, match='duration'): + move_duration(np.zeros(7), np.ones(7), bad, 2.0) + + +@pytest.mark.parametrize('side', ['left', 'right']) +def test_default_speed_cap_never_stretches_open_and_close(side): + # each joint is farthest from its goal at one of its limits, so starting + # with every joint at its lower, then its upper, limit covers the worst case + for name in ('open', 'close'): + for start in (LOWER[side], UPPER[side]): + assert move_duration(start, named_pose(side, name), 1.0, DEFAULT_MAX_SPEED) == 1.0 + + +def test_dds_domain_comes_from_ros_domain_id_or_the_parameter(): + assert dds_domain('1', -1) == 1 + assert dds_domain('0', -1) == 0 # the real robot, only when set explicitly + assert dds_domain(None, 97) == 97 + assert dds_domain('', 97) == 97 # empty is unset, as for ROS + assert dds_domain('97', 97) == 97 + for unset in (None, ''): + with pytest.raises(ValueError, match='no DDS domain'): + dds_domain(unset, -1) + with pytest.raises(ValueError, match='disagree'): + dds_domain('1', 0) + for bad in ('-1', 'one', ' 1', '0x0', '1.0'): + with pytest.raises(ValueError, match='ROS_DOMAIN_ID must be'): + dds_domain(bad, -1) + + +def test_dds_domain_must_be_the_nodes_ros_domain(): + assert dds_domain('1', -1, ros_domain=1) == 1 + assert dds_domain(None, 97, ros_domain=97) == 97 + # ROS_DOMAIN_ID unset: ROS runs on domain 0, so domain_id:=1 would split them + with pytest.raises(ValueError, + match='runs ROS on domain 0 but would run Unitree DDS on domain 1'): + dds_domain(None, 1, ros_domain=0) + # rcl reads '010' as octal 8 + with pytest.raises(ValueError, match='runs ROS on domain 8'): + dds_domain('010', -1, ros_domain=8) diff --git a/core_ws/src/dex_3/test/test_driver_dds.py b/core_ws/src/dex_3/test/test_driver_dds.py new file mode 100644 index 0000000..ba8961b --- /dev/null +++ b/core_ws/src/dex_3/test/test_driver_dds.py @@ -0,0 +1,285 @@ +'''Dex3Hand against a fake hand over DDS inside one process (no robot, no network)''' + +import gc +import time +from types import SimpleNamespace + +import numpy as np +import pytest + +from unitree_sdk2py.idl.unitree_hg.msg.dds_ import HandCmd_, HandState_ +from unitree_sdk2py.idl.default import unitree_hg_msg_dds__HandState_ as HandStateDefault +from unitree_sdk2py.core.channel import ( + ChannelFactoryInitialize, + ChannelPublisher, + ChannelSubscriber, +) + +from dex_3 import driver +from dex_3.driver import RELEASE_MESSAGES, Dex3Hand +from dex_3.dex3 import DEFAULT_KD, DEFAULT_KP, DEFAULT_MAX_SPEED, UPPER, motor_mode, named_pose + +TEST_DOMAIN = 97 # never 0 (the real robot) or 1 (the sim) + +# the fake hands' state publishers and command recorders of the current test +_ENTITIES = [] + + +@pytest.fixture(scope='module', autouse=True) +def dds(): + ChannelFactoryInitialize(TEST_DOMAIN) + + +@pytest.fixture(autouse=True) +def close_entities(): + ''' + Close each test's DDS entities in the main thread. Left to the garbage + collector (a pytest.raises keeps them in a reference cycle), they can be + deleted from inside a DDS listener thread, which crashes the process + ''' + yield + while _ENTITIES: + _ENTITIES.pop().Close() + gc.collect() + + +def _state_publisher(side, q): + pub = ChannelPublisher(f'rt/dex3/{side}/state', HandState_) + pub.Init() + _ENTITIES.append(pub) + msg = HandStateDefault() + for i, value in enumerate(q): + msg.motor_state[i].q = float(value) + return pub, msg + + +class _Recorder: + '''rt/dex3//cmd messages with their arrival times (time.monotonic())''' + + def __init__(self, side): + self.messages = [] + self.times = [] + sub = ChannelSubscriber(f'rt/dex3/{side}/cmd', HandCmd_) + sub.Init(self._on_cmd, 50) + _ENTITIES.append(sub) + + def _on_cmd(self, msg): + self.times.append(time.monotonic()) + self.messages.append(msg) + + def __len__(self): + return len(self.messages) + + def since(self, n): + '''Positions (k x 7) and arrival times of messages n onward''' + k = len(self.messages) + return _positions(self.messages[n:k]), np.array(self.times[n:k]) + + +def _publish_state(pub, msg, n=20): + for _ in range(n): + pub.Write(msg) + time.sleep(0.01) + + +def _positions(messages): + return np.array([[m.motor_cmd[i].q for i in range(7)] for m in messages]) + + +def _is_stop(message): + return all(m.mode == motor_mode(i, timeout=True) and m.kp == 0.0 and m.kd == 0.0 + for i, m in enumerate(message.motor_cmd[:7])) + + +def _stops_after(messages): + ''' + The stop commands in messages, checking nothing else follows the first: + a hold command already on its way can still arrive just before them + ''' + first = next((i for i, m in enumerate(messages) if _is_stop(m)), len(messages)) + assert all(_is_stop(m) for m in messages[first:]), 'a command after the stop' + return messages[first:] + + +def _wait_until(condition, timeout=2.0): + deadline = time.monotonic() + timeout + while not condition() and time.monotonic() < deadline: + time.sleep(0.01) + + +def _assert_within_speed_cap(qs, times, q_from, t_from, max_speed=DEFAULT_MAX_SPEED): + ''' + No command is farther from q_from than max_speed allows since t_from (taken + just before the set_pose call). A message arrives after it was computed, so + a loaded CPU cannot fail this; a jump fails it at once + ''' + ahead = np.abs(qs - q_from).max(axis=1) - max_speed * (times - t_from) + assert ahead.max() < 1e-3, f'{ahead.max():.3f} rad ahead of the {max_speed} rad/s cap' + + +def test_needs_state_before_the_first_command(): + hand = Dex3Hand('right') + try: + with pytest.raises(RuntimeError, match='no recent right hand state'): + hand.set_pose(named_pose('right', 'close')) + finally: + hand.shutdown() + + +def test_silent_then_smooth_move_hold_and_release(): + start = np.array([0.1, 0.0, 0.2, -0.3, 0.0, 0.0, 0.0]) + state_pub, state = _state_publisher('left', start) + received = _Recorder('left') + hand = Dex3Hand('left', rate_hz=200.0) + try: + _publish_state(state_pub, state) + assert hand.state is not None and np.allclose(hand.state['q'], start, atol=1e-6) + assert len(received) == 0, 'must stay silent until the first command' + assert hand.release() is False # nothing to release: still silent + time.sleep(0.1) + assert len(received) == 0 + + goal = named_pose('left', 'close') + t_cmd = time.monotonic() + hand.set_pose(goal, duration=0.5) + time.sleep(0.8) + qs, times = received.since(0) + # starts from the measured pose and moves no faster than the cap + _assert_within_speed_cap(qs, times, start, t_cmd) + assert np.allclose(qs[-1], goal, atol=1e-6), 'ends at the goal' + last = received.messages[-1] + assert [m.mode for m in last.motor_cmd] == [motor_mode(i) for i in range(7)] + assert all(abs(m.kp - DEFAULT_KP) < 1e-6 and abs(m.kd - DEFAULT_KD) < 1e-6 + for m in last.motor_cmd) + + # out-of-range targets are clipped to the URDF limits, and a 0 s move + # still goes no faster than max_speed (1.22 rad here: about 0.61 s) + n = len(received) + t_cmd = time.monotonic() + hand.set_pose([9.0] * 7, duration=0.0) + time.sleep(0.8) + qs, times = received.since(n) + _assert_within_speed_cap(qs, times, goal, t_cmd) + assert np.allclose(qs[-1], UPPER['left']) + + # release: Unitree's stop (timeout bit, zero gains), then silence + n = len(received) + assert hand.release() is True + _wait_until(lambda: len(_stops_after(received.messages[n:])) >= RELEASE_MESSAGES) + count = len(received) + assert len(_stops_after(received.messages[n:])) == RELEASE_MESSAGES + time.sleep(0.1) + assert len(received) == count, 'silent after the stop' + finally: + hand.shutdown() + + +def test_zero_duration_first_command_is_capped_by_max_speed(): + # the hand far from the goal: a "go directly" first command must not jump + start = np.array([0.0, 0.0, 0.0, 0.2, 0.0, 0.0, 0.0]) + goal = np.array([0.0, 0.0, 0.0, 1.5, 0.0, 0.0, 0.0]) # right middle_0, 1.3 rad away + expected = 1.3 / DEFAULT_MAX_SPEED # 0.65 s + state_pub, state = _state_publisher('right', start) + received = _Recorder('right') + hand = Dex3Hand('right', rate_hz=200.0) + try: + _publish_state(state_pub, state) + t_cmd = time.monotonic() + hand.set_pose(goal, duration=0.0) + time.sleep(expected + 0.3) + qs, times = received.since(0) + _assert_within_speed_cap(qs, times, start, t_cmd) + assert np.allclose(qs[-1], goal, atol=1e-6) + first_at_goal = times[np.argmax(np.abs(qs[:, 3] - 1.5) < 1e-6)] + assert first_at_goal - t_cmd > expected, 'reached the goal no sooner than the cap allows' + finally: + hand.shutdown() + + +def test_refuses_non_finite_state_and_bad_durations(): + state_pub, state = _state_publisher('left', [0.0, np.nan, 0, 0, 0, 0, 0]) + received = _Recorder('left') + hand = Dex3Hand('left', rate_hz=200.0) + try: + _publish_state(state_pub, state) + with pytest.raises(RuntimeError, match='non-finite'): + hand.set_pose(named_pose('left', 'open'), duration=1.0) + assert not hand.commanding + + state.motor_state[1].q = 0.0 + _publish_state(state_pub, state) + for bad in (float('nan'), float('inf'), -1.0): + with pytest.raises(ValueError, match='duration'): + hand.set_pose(named_pose('left', 'close'), duration=bad) + assert not hand.commanding + time.sleep(0.1) + assert len(received) == 0, 'refused commands publish nothing' + finally: + hand.shutdown() + + +@pytest.mark.parametrize('kwargs', [ + {'kp': -1.0}, {'kd': float('nan')}, {'max_speed': 0.0}, {'max_speed': float('inf')}, + {'rate_hz': 0.0}, +]) +def test_rejects_bad_settings(kwargs): + with pytest.raises(ValueError, match='must be finite'): + Dex3Hand('left', **kwargs) + + +def test_a_wall_clock_step_does_not_skip_a_move(monkeypatch): + # e.g. NTP stepping the clock forward during a move: the driver times + # with time.monotonic(), so the move still takes its full duration. 1 s + # for 1.22 rad is below the speed cap, so only a skipped move can fail this + offset = [0.0] + monkeypatch.setattr(driver, 'time', SimpleNamespace( + time=lambda: time.time() + offset[0], monotonic=time.monotonic, sleep=time.sleep)) + start = np.zeros(7) + state_pub, state = _state_publisher('left', start) + received = _Recorder('left') + hand = Dex3Hand('left', rate_hz=200.0) + try: + _publish_state(state_pub, state) + goal = named_pose('left', 'close') + t_cmd = time.monotonic() + hand.set_pose(goal, duration=1.0) + time.sleep(0.1) + offset[0] = 100.0 + time.sleep(1.2) + qs, times = received.since(0) + _assert_within_speed_cap(qs, times, start, t_cmd, max_speed=1.23) + assert np.allclose(qs[-1], goal, atol=1e-6) + finally: + hand.shutdown() + + +@pytest.mark.parametrize('release', [True, False]) +def test_shutdown_releases_or_just_goes_silent(release): + state_pub, state = _state_publisher('right', np.zeros(7)) + received = _Recorder('right') + hand = Dex3Hand('right', rate_hz=200.0) + try: + _publish_state(state_pub, state) + hand.set_pose(named_pose('right', 'close'), duration=0.2) # 0.61 s at max_speed + time.sleep(0.9) + n = len(received) + assert n > 0 and not any(_is_stop(m) for m in received.messages) + except BaseException: + hand.shutdown() + raise + hand.shutdown(release=release) + # the recorder hands messages over on its own thread, so give it a moment + _wait_until(lambda: len(_stops_after(received.messages[n:])) >= RELEASE_MESSAGES, + timeout=1.0 if release else 0.2) + after = received.messages[n:] + if release: + # Unitree's stop, so the fingers go limp + assert len(_stops_after(after)) == RELEASE_MESSAGES + else: + # no stop: the last command the hand got is the held close pose + assert not any(_is_stop(m) for m in after) + assert np.allclose(_positions(received.messages[-1:])[0], named_pose('right', 'close'), + atol=1e-6) + count = len(received) + time.sleep(0.1) + assert len(received) == count, 'silent after shutdown' diff --git a/core_ws/src/dex_3/test/test_hand_node.py b/core_ws/src/dex_3/test/test_hand_node.py new file mode 100644 index 0000000..74b4c50 --- /dev/null +++ b/core_ws/src/dex_3/test/test_hand_node.py @@ -0,0 +1,159 @@ +''' +Dex3HandNode's DDS domain and shutdown choice, with the hands and DDS faked. +Needs rclpy: run with the sourced workspace on PYTHONPATH (see README) +''' + +import os +import signal +import threading +import time + +import pytest + +rclpy = pytest.importorskip('rclpy') + +from dex_3 import hand_node # noqa: E402 +from dex_3.dex3 import DEFAULT_MAX_SPEED # noqa: E402 +from dex_3.hand_node import Dex3HandNode # noqa: E402 + +ROS_TEST_DOMAIN = 97 # never 0 (the real robot) or 1 (the sim) + + +class FakeHand: + '''Stands in for Dex3Hand: records how it was made and shut down''' + instances = [] + state = None # no hand state yet, as before a hand's first rt/dex3 state + + def __init__(self, side, **kwargs): + self.side = side + self.kwargs = kwargs + self.shutdowns = [] + FakeHand.instances.append(self) + + def shutdown(self, release=True): + self.shutdowns.append(release) + + +@pytest.fixture +def channel_inits(monkeypatch): + '''ChannelFactoryInitialize calls; no DDS is started''' + calls = [] + monkeypatch.setattr(hand_node, 'ChannelFactoryInitialize', lambda *args: calls.append(args)) + monkeypatch.setattr(hand_node, 'Dex3Hand', FakeHand) + FakeHand.instances = [] + monkeypatch.delenv('ROS_DOMAIN_ID', raising=False) + monkeypatch.delenv('G1_DDS_INTERFACE', raising=False) + yield calls + if rclpy.ok(): + rclpy.shutdown() + + +def _make_node(*parameters): + args = ['--ros-args'] + [arg for p in parameters for arg in ('-p', p)] + # the node's own ROS side always runs on the test domain + rclpy.init(args=args, domain_id=ROS_TEST_DOMAIN) + return Dex3HandNode() + + +def test_refuses_to_start_without_a_domain(channel_inits): + with pytest.raises(ValueError, match='no DDS domain'): + _make_node() + assert channel_inits == [] and FakeHand.instances == [] + + +def test_domain_from_the_parameter(channel_inits): + _make_node(f'domain_id:={ROS_TEST_DOMAIN}') + assert channel_inits == [(ROS_TEST_DOMAIN,)] + assert [hand.side for hand in FakeHand.instances] == ['left', 'right'] + assert all(hand.kwargs['max_speed'] == DEFAULT_MAX_SPEED for hand in FakeHand.instances) + + +def test_domain_and_interface_from_the_environment(channel_inits, monkeypatch): + monkeypatch.setenv('ROS_DOMAIN_ID', str(ROS_TEST_DOMAIN)) + monkeypatch.setenv('G1_DDS_INTERFACE', 'lo') + _make_node() + assert channel_inits == [(ROS_TEST_DOMAIN, 'lo')] + + +def test_refuses_a_parameter_that_disagrees_with_ros_domain_id(channel_inits, monkeypatch): + monkeypatch.setenv('ROS_DOMAIN_ID', str(ROS_TEST_DOMAIN)) + with pytest.raises(ValueError, match='disagree'): + _make_node('domain_id:=1') + assert channel_inits == [] + + +def test_refuses_a_parameter_that_is_not_the_nodes_ros_domain(channel_inits): + # as under ros2 launch with ROS_DOMAIN_ID unset and domain_id:=1, where + # ROS itself runs on domain 0: ROS and Unitree DDS would split + with pytest.raises(ValueError, match=f'runs ROS on domain {ROS_TEST_DOMAIN}'): + _make_node(f'domain_id:={ROS_TEST_DOMAIN + 1}') + assert channel_inits == [] and FakeHand.instances == [] + + +def test_refuses_an_octal_ros_domain_id(channel_inits, monkeypatch): + # rcl reads ROS_DOMAIN_ID with a leading 0 as octal: 0141 is domain 97 + monkeypatch.setenv('ROS_DOMAIN_ID', '0141') + rclpy.init(args=[]) + with pytest.raises(ValueError, match=f'runs ROS on domain {ROS_TEST_DOMAIN} but would ' + 'run Unitree DDS on domain 141'): + Dex3HandNode() + assert channel_inits == [] and FakeHand.instances == [] + + +@pytest.mark.parametrize('parameters, release', [ + ((), True), # default: release, the fingers go limp + (('on_shutdown:=release',), True), + (('on_shutdown:=silent',), False), +]) +def test_on_shutdown(channel_inits, parameters, release): + node = _make_node(f'domain_id:={ROS_TEST_DOMAIN}', *parameters) + node.destroy_node() + assert [hand.shutdowns for hand in FakeHand.instances] == [[release], [release]] + + +def test_rejects_an_unknown_on_shutdown(channel_inits): + with pytest.raises(ValueError, match='on_shutdown'): + _make_node(f'domain_id:={ROS_TEST_DOMAIN}', 'on_shutdown:=hold') + assert channel_inits == [] and FakeHand.instances == [] + + +@pytest.mark.parametrize('first', hand_node.EXIT_SIGNALS, ids=lambda s: signal.Signals(s).name) +def test_main_releases_once_through_repeated_exit_signals(channel_inits, monkeypatch, first): + # Ctrl-C in a terminal reaches the node twice: from the terminal, and from + # ros2 launch passing it on, which may follow with SIGTERM. No exit signal, + # first or repeated, may cut the release short + monkeypatch.setenv('ROS_DOMAIN_ID', str(ROS_TEST_DOMAIN)) + handlers = {signum: signal.getsignal(signum) for signum in hand_node.EXIT_SIGNALS} + unhandled = [] + + def send(signum): + if signal.getsignal(signum) is signal.SIG_DFL: + # main() does not handle it: sent, it would end the test process + unhandled.append(signal.Signals(signum).name) + signum = signal.SIGINT + os.kill(os.getpid(), signum) + + def slow_shutdown(self, release=True): + for signum in hand_node.EXIT_SIGNALS: # more signals, mid-release + send(signum) + time.sleep(0.3) + self.shutdowns.append(release) + + def first_signal(): + while not FakeHand.instances: + time.sleep(0.01) + time.sleep(0.3) + send(first) + + monkeypatch.setattr(FakeHand, 'shutdown', slow_shutdown) + threading.Thread(target=first_signal, daemon=True).start() + try: + hand_node.main(['--ros-args', '-p', 'sides:=[left]']) + except KeyboardInterrupt: + pytest.fail('a second signal interrupted the shutdown') + finally: + for signum, handler in handlers.items(): + if handler is not None: + signal.signal(signum, handler) + assert unhandled == [] + assert [hand.shutdowns for hand in FakeHand.instances] == [[True]] diff --git a/core_ws/src/g1_homie/.gitignore b/core_ws/src/g1_homie/.gitignore new file mode 100644 index 0000000..1bc127a --- /dev/null +++ b/core_ws/src/g1_homie/.gitignore @@ -0,0 +1,4 @@ +# Build artifacts +*.egg-info/ +__pycache__/ +*.pyc diff --git a/core_ws/src/h1_bringup/LICENSE b/core_ws/src/g1_homie/LICENSE similarity index 100% rename from core_ws/src/h1_bringup/LICENSE rename to core_ws/src/g1_homie/LICENSE diff --git a/core_ws/src/g1_homie/README.md b/core_ws/src/g1_homie/README.md new file mode 100644 index 0000000..3d02424 --- /dev/null +++ b/core_ws/src/g1_homie/README.md @@ -0,0 +1,380 @@ +# g1_homie + +ROS 2 node that runs HOMIE's walking policy on the Unitree G1's legs. The legs' +commands go to `golem_safety`'s split mode, which merges them with the arm +controller's waist and arms and alone writes `rt/lowcmd`. It needs +`unitree_sdk2py` and its cyclonedds, onnxruntime, numpy and PyYAML, which the +ros image has. + +## License + +This package's code (`g1_homie/`, `config/`, `test/`) is MIT. The policy +it runs is not: `deploy.onnx`, `g1.yaml` and `homie_policy.py` in `policy/` +are HOMIE's, for non-commercial use only (treat them as CC BY-NC-SA 4.0; see +`policy/README.md` and `policy/LICENSE` for the license and the credits). +The node copies nothing from `policy/`: it loads it in place from the +`policy_dir` parameter, by default `/home/code/g1_homie`, where the +containers mount `policy/`. + +## ROS interface + +- `/cmd_vel` (`geometry_msgs/Twist`): `linear.x`, `linear.y` (m/s) and + `angular.z` (rad/s) in the base frame. A command older than + `commands.timeout` (0.5 s) means zero velocity. +- `/locomotion/height` (`std_msgs/Float32`): the pelvis height target, m, + from 0.24 to 0.74. It starts at 0.74, standing. +- `/safety/heartbeat` (`std_msgs/Bool`): `golem_safety`'s. The node starts + only after the first true beat, and a false beat, or none for + `heartbeat.timeout` after the first, stops it for good (Behavior). +- `/lowerbody/confirm_engage` (`std_srvs/Trigger`): the operator's go-ahead + on the real robot, the H1-2's leg node's service. Accepted only while the + legs hold the default pose; refused at any other time, naming the phase. +- With `band.release` (`sim.yaml`): `/left_ee_pose` + (`geometry_msgs/PoseStamped`), which `frame_task_server` publishes once its + start-up moves are done, and a client of the sim's `/elastic_band/toggle` + (`std_srvs/Trigger`). `g1.yaml` has neither. +- Parameter `policy_dir`: the policy's directory (this package's `policy/`). +- `--config`: `g1.yaml` (real robot), `sim.yaml` (sim), or a path. +- `use_sim_time` is always false, whatever the launch passes. The node times + everything by `rt/lowstate`'s tick and the monotonic clock; sim time would + only add a `/clock` subscription (500 Hz in the sim) to its Python. + +```bash +ros2 run g1_homie locomotion_node --config sim.yaml +ros2 service call /lowerbody/confirm_engage std_srvs/srv/Trigger # g1.yaml, while holding +ros2 topic pub -r 10 /cmd_vel geometry_msgs/msg/Twist "{linear: {x: 0.3}}" +ros2 topic pub --once /locomotion/height std_msgs/msg/Float32 "{data: 0.7}" +``` + +The G1 bringups (`golem_bringup`'s `launch/g1/`) start it after the arm controller: +`g1_sim.launch.py` always, with `sim.yaml`, and `g1_real_robot.launch.py` only with +`start_position_verified:=true`, with `g1.yaml`; see their launch files. + +## DDS interface + +- Reads `rt/lowstate`, only its newest sample. The reader is a plain DDS + reader that keeps one sample (keep-last 1, best effort) and has no + listener: a backlog is dropped inside DDS, and only the samples the node + uses are decoded. The tick paces the policy; the DDS source timestamp + starts the latency. The IMU (`imu_state.quaternion`, w first, and + `imu_state.gyroscope`) is taken as the pelvis IMU, as in HOMIE's own + deployment. The positions and velocities of the policy's 27 joints (legs, + waist yaw, arms) are looked up by name in the package's own table of the + G1's body slots (`BODY_JOINT_NAMES` in `locomotion.py`): slots 0-12 and + 15-28. +- Reads `rt/safety/lowcmd_upper_in` the same way, and only while waiting to + start, to know the arm controller is running. It never writes there. +- Writes `rt/safety/lowcmd_lower_in`: slots 0-11 with mode 1, the policy's + target q, dq = tau = 0 and the training gains (hips kp 100 kd 2, knees + 150/4, ankles 40/2). Every other slot is zero; split mode ignores them. +- Like the other G1 nodes, `ROS_DOMAIN_ID` overrides `network.domain_id` and + `G1_DDS_INTERFACE` (e.g. `enp130s0`) overrides `network.interface`. + +## Behavior + +The node follows the H1-2's leg node (`h12_lowerbody_rl`'s +`lowerbody_controller_node`): idle until `golem_safety` beats, a pre-pose, +on the real robot the operator's confirm, the policy, and in the sim the +band's release. + +1. **Waiting.** Silent until `golem_safety` has sent a first true + `/safety/heartbeat` (it beats once it can relay: `/lowstate` reports + `mode_machine` 5) and a sample is finite, with a unit IMU quaternion, + and upright (gravity z in the pelvis frame below -0.9, a tilt under + about 25 deg): as the H1-2's leg node, which pre-poses on the first beat + and state, the legs are not left limp while the arm controller moves the + waist and arms. Until the node's first command `golem_safety` streams + zero gains to the legs (they hang limp, in the sim from the band). The + reason for waiting is logged when it changes, each kind at most every 5 s. +2. **Heartbeat watchdog.** From the first beat on, in every phase, a false + beat (`golem_safety`'s estop has latched; it beats false from then on) + or no beat for `heartbeat.timeout` (0.5 s; it beats at 10 Hz) stops all + output for good and logs why. The worker checks the beat's age on every + wake, before it takes a sample. +3. **Pre-pose.** The legs move in a straight line from their measured + positions to the policy's default pose over `startup.ramp_seconds` + (2.0 s in both configs, the H1-2's `prepose_ramp_s`), one target per + policy step, at the training gains. +4. **Hold.** The node then holds the default pose, one target per policy + step with every fault active except the latency fault (the pose is a PD + hold), until the policy may start: for 1 s of `rt/lowstate`'s tick + every sample is steady (no gap over `faults.max_state_gap`) and on time + (taken within half of `faults.max_latency` of its write, so that the + policy does not run straight into the latency fault; the wall-clock gap + between samples is not held to that, since a sim below real time spaces + them out while each is still answered at once), the tick kept to the wall clock over that second within + `startup.tick_rate`, and the arm controller has commanded in the last + 0.5 s. The 1 s covers ten of the kernel's 100 ms CPU-quota periods, so a + laptop that cannot keep up keeps the legs in the pre-pose instead of + faulting the policy. The tick check catches a `tick_seconds` that does + not match the robot's tick, which would run the policy at the wrong + rate. In the sim (`sim.yaml`) the policy starts as soon as that holds. + With `startup.wait_for_confirm` (`g1.yaml`) the node also waits for + `ros2 service call /lowerbody/confirm_engage std_srvs/srv/Trigger`, + which it logs; a call before the timing holds is refused with the + reason, as is a call in any other phase, naming the phase. The policy + starts on the next policy step. +5. **Walking.** The policy starts with an empty history, at zero velocity + and 0.74 m: a `/cmd_vel` or `/locomotion/height` that came before it + started is dropped. In the sim the band still holds the robot. The + policy steps on a grid of `rt/lowstate`'s tick 20 ms apart, from the + pre-pose's first step. A step is due at the first sample at or past its + grid tick; the next one is due at the first grid tick at least half a + period after that sample. So the policy follows the robot's clock (the + sim's, in the sim) at 50 Hz on average, however many samples the node + sees, and never steps twice within 10 ms. It always works from the + newest sample: when the node falls behind, the samples it missed are + dropped, not worked through. Each target is checked for latency and + published at once, and the safety layer holds it at 500 Hz. Commands are + clipped to the trained ranges, then to the `commands` limits, and move + toward the request by at most `commands.accel` (velocities) and + `commands.height_rate` (height). +6. **Band release (sim).** With `band.release` (`sim.yaml`), a 10 Hz timer + calls the sim's `/elastic_band/toggle` once the policy runs and a + `/left_ee_pose` has come (`frame_task_server` publishes it only after + its start-up moves), or `band.max_wait` (30 s) after the policy started + without one. The service flips the band, so `band.py`'s `BandRelease` + keeps at most one call in flight, with no timeout (a late response can + never meet a second call), sends a failed call again, and sends nothing + after one has succeeded. A call that never answers leaves the band on. + Nothing is called once the node has faulted. When the call has taken the + band off (the service answers `disabled`), the policy's history and last + action restart empty, as the H1-2's leg node resets its policy at the + release: its first steps on the free robot are not computed from the + steps the band held. The command, the height and the step grid carry on. + If it answers `enabled`, the band was already off (SPACE in the viewer) + and now holds the robot again: the node warns and calls nothing more. + The release waits for `frame_task_server`, whose start-up moves (about + 1.9 s in the sim) start on the same `rt/lowstate` sample as the 2 s + pre-pose, so the policy runs under the band for only a few steps. When + it runs there longer, it crouches toward its 0.74 m height while the + band holds the pelvis at 0.78 m, with the feet off the floor, and + stumbles at the release: forced to 1.5 s (`ramp_seconds: 0.5`), the + robot tilted 23-28 deg and stepped 26-45 cm, with or without the + history reset (the history is 6 steps, 0.12 s). + The real robot has no band (`g1.yaml`: `band.release: false`), and the + node has no client then. +7. **Faults.** Once the legs are commanded (pre-pose, hold or walking), any + of these stops all output for good and logs why: + - a gap over `faults.max_state_gap` between the `rt/lowstate` samples + the node takes, or none for that long (a sample whose tick did not + advance does not count); + - a tick that goes back; + - a value that is not finite, or an IMU quaternion whose norm is not 1; + - a fall: gravity z above -0.5 (tilted over 60 deg); + - a policy step that fails or gives targets that are not finite; + - a command later than `faults.max_latency`, measured just before it is + published, from the DDS write of the `rt/lowstate` sample it answers. + The robot's clock is not the laptop's, so the node takes the fastest + delivery of the last ~2 s as the offset between them. The latency then + counts any slower delivery, the time the sample waited for the node, + and the policy step. + + And in every phase, the heartbeat watchdog (2). + + The node never sends a zero-gain or damping command itself. On every + fault above but a non-finite joint value, `golem_safety` holds a quiet + input: it keeps relaying the node's last leg command, which does not + balance the robot, and after `stale.lower_seconds` (0.2 s in + `split.yaml`) reports the quiet input (`input_quiet`). It does not estop + on it: the command stands until `golem_safety` estops for another reason + (a fall that drives a joint of `rt/lowstate` past `split.yaml`'s + `limits.estop`, say) or the bringup stops. A non-finite joint value is + the exception: `golem_safety` checks q, dq and tau_est of every body + joint in each `rt/lowstate` it takes, latches its estop on a non-finite + one, and `split.yaml`'s `estop_response` (`mode: zero`, `after: + keep_writing`) turns every motor off and keeps writing that command. It + does not check the IMU, so a non-finite gyro or quaternion is held like + the other faults. A heartbeat fault means `golem_safety` has latched its + estop (and writes its estop command) or stopped; restart the bringup. + Only a restart clears a fault. Shutdown (Ctrl-C) also just stops + publishing. +8. **Timing.** The pre-pose's log line gives the tick's rate against the + wall clock while waiting (1.00 on the robot). Every 10 s, after a + publish, the node logs the latency of its commands (median and max) and + how far apart its policy steps were in ticks. + +## Real time + +HOMIE stood in the sim with a constant 30 ms of extra delay and fell at +60 ms (the sim's stage-3a probes). The node used to fall in the sim +walk test when the ros container was capped at 1.5 CPUs. An instrumented +copy of that node showed where its delay came from: + +- `rt/lowstate` reached its handler up to 558 ms late (90th percentile + 244 ms, against at most 2.4 ms at full CPU). Once a container has used + its CPU quota for a 100 ms period, every thread in it waits for the next + one, the DDS receive threads too, and onnxruntime's default thread pool + spent much of that quota spinning. +- It then worked through the queued samples in order (up to 81 at once), + stepping on every 10th, so its targets went out in bursts (median 9 ms + apart, up to 184 ms). The safety layer's 500 Hz relay forwards only the + newest of a burst. +- Its own work was small: 0.55 ms of CPU per policy step and write. + +So the node keeps its own work small, acts only on the newest sample, and +measures what it cannot control: + +- onnxruntime runs with one intra-op and one inter-op thread, no spinning and + nothing pinned: HomiePolicy's own session (`homie_policy.policy_session`). + onnxruntime's default session has a thread pool as wide as the machine, + pinned one thread per CPU, and its threads + can spin between runs: 1.3-2.2 cores at 50 Hz in four benchmarks in the + ros image on the 24-core laptop, and two threads at 100% in the node + above. One thread takes a median 0.27-0.29 ms a step, 1.4-1.6% of a core + (0.89 ms and 4.5% once, on a loaded host), with identical actions + (`test_locomotion.py` checks it). +- `rt/lowstate` and the arm controller's input (500 Hz too) are plain DDS + readers that keep the newest sample, decoded only when the node takes + it. The SDK's subscribers decode every sample in the DDS receive thread, + under the GIL: about 0.5 ms each, 26% of a core for `rt/lowstate`. + Taking and decoding the newest sample costs 0.25-0.27 ms, 13% of a core + while the node keeps up with every sample. The arm controller's input is + read only while waiting. +- No `/clock` subscription (see `use_sim_time` above). +- The publish path is the policy step, the CRC and one write; the only + logging on it is the timing line every 10 s. + +In the walk test at full CPU (`tests/g1/test_sim_bringup.sh --walk`, +five runs at a host load of 5-10) the node used 0.20-0.24 cores, and its +commands left a median 0.7-0.9 ms (at most 1.9-7.0 ms) after their +`rt/lowstate` sample. The old node used 2.39 cores in a run the same hour +(two onnxruntime threads at 100%), 0.56 in another, and 2.43 in the probe +above. A traced copy of the node, timed against the sim's own record, put +its latency estimate within 0.6 ms of the true delay. + +What the node cannot fix is the CPU the rest of its container needs, 2.2-2.6 +cores in those runs (the safety layer 0.8-1.0, the arm controller 0.7-0.9, +`joint_state_publisher` 0.4-0.5, the hands 0.2-0.3). Capped below that with +`docker run --cpus`, the container is throttled in 98-100% of its 100 ms CFS +periods, and every thread in it, the safety layer's and the DDS receive +threads too, then waits for the next period: + +- `rt/lowstate` reached the node 41-54 ms late at 1.5 CPUs and 64-73 ms + late at 1.0 CPUs, so the node did not start. In four runs (two at each + cap) it was still waiting, logging why, when the test gave up 209-227 s + of sim time in. In earlier runs of this code it never started at 1.0 + CPUs (6 of 6), and at 1.5 CPUs it started in 2 of 5 (not counting one + with the sim at a real-time factor of 0.06), then faulted within 2 s on a + command 27.1 or 59.1 ms late. +- Capped at 1.5 CPUs while walking (three runs), it faulted on a command + 25.5-35.9 ms late, 0.57-0.87 s after the cap, with the robot still up. + The safety layer of those runs then held its last target until its 1 s + leg watchdog (`sim_split.yaml`) estopped, and the robot dropped when the + estop turned every motor off. `golem_safety` holds the last target and + reports the quiet input instead (Behavior, Faults). +- The old node fell in every capped run (five here, three in stage 3a). + +How the container gets its CPU matters as much as how much (this is the +docker setup's choice, not the node's). The same 1.5 CPUs as a 10 ms CFS +period (`--cpu-period 10000 --cpu-quota 15000`) walked in 3 of 6 runs; two +more stood but tilted to 27 deg or sank to 0.699 m, and one fell. Two pinned +cores with no quota (`--cpuset-cpus`) walked in 2 of 2. In the traced 10 ms +runs the node's side stayed fast (a median 1.3-1.4 ms from the state to its +write, at most 18.5-22.0 ms), but the safety layer's relay, starved in the +same container, took a median 25-32 ms (at most 137-171 ms) to get a command +into the sim and overwrote 17-18% of them. Of the sim's 157 holds over 40 ms, +155 came after the node had already written a newer command. The node's +latency fault cannot see that part. + +## Configs + +| Key | `g1.yaml` (real) | `sim.yaml` (sim) | +|---|---|---| +| `network.domain_id` | 0 | 1 | +| `imu_frame` | `pelvis`, the only one accepted | `pelvis` | +| `tick_seconds` | 0.001, assumed (see below) | 0.001: the sim counts ms of sim time | +| `startup.ramp_seconds` | 2.0, the H1-2's pre-pose | 2.0 | +| `startup.wait_for_confirm` | true: hold until `/lowerbody/confirm_engage` | false | +| `startup.tick_rate` | [0.9, 1.1]: the robot keeps real time | [0.25, 1.25]: the sim's real-time factor | +| `heartbeat.timeout` | 0.5 s, above the 0.1 s beat period | same | +| `band.release` | false: no band | true: call `/elastic_band/toggle` | +| `band.max_wait` | 30.0 s (unused) | 30.0 s from the policy's start | +| `commands.vx`, `vy`, `yaw` | [-0.3, 0.5], [-0.3, 0.3], [-0.6, 0.6] | the trained ranges: [-0.8, 1.2], [-0.5, 0.5], [-0.8, 0.8] | +| `commands.accel` | 0.5 m/s², 0.5 m/s², 1.0 rad/s² | same | +| `commands.timeout` | 0.5 s | same | +| `commands.height_rate` | 0.1 m/s | same | +| `faults.max_state_gap` | 0.05 s | 0.5 s, the sim can run slower than real time | +| `faults.max_latency` | 0.025 s | same | + +- Every key is required and an unknown key is an error, so a typo cannot fall + back to a default. +- `topics.low_cmd` must be a `golem_safety` input (`rt/safety/...`), and + differ from `topics.upper_cmd`. +- The command limits must include 0 and lie inside the trained ranges + (`cmd_range` in `policy/g1.yaml`). +- `tick_seconds` must divide the 20 ms policy period, and + `startup.tick_rate` must include 1. +- `heartbeat.timeout` must be longer than `golem_safety`'s 0.1 s beat period. +- `faults.max_latency` is well under the constant delay HOMIE stood with in + the sim (30 ms; it fell at 60 ms, the sim's stage-3a probes). +- The limits and rates are starting values, not yet tuned on our G1. + +## Before using it on the robot + +- Only one program may command `rt/lowcmd`. The robot runs in Unitree's + debug mode, with Unitree's controller released, from before the bringup + starts: `golem_safety` writes every motor from its first moment. Entering + debug mode is a manual operator step that nothing in this stack does + (`docs/g1/first-session.md`). +- Support the robot (a gantry): `split.yaml`'s estop zeroes every motor. +- After the pre-pose the node holds the legs at the default pose until you + call `ros2 service call /lowerbody/confirm_engage std_srvs/srv/Trigger` + (Behavior, Confirm); check the stance before you do. +- A fault silences the node (Behavior, Faults), and `golem_safety` then + holds the legs on the node's last command instead of turning every motor + off: the robot stands on a frozen target that does not balance it. A + non-finite joint value in `rt/lowstate`, a fall that drives a joint past + `split.yaml`'s `limits.estop`, or a `rt/lowstate` from another robot + (`mode_machine` not 5) makes `golem_safety` estop (every motor off). +- The frame of `rt/lowstate`'s IMU on our unit is not verified yet; the node + takes it as the pelvis IMU, and accepts only `imu_frame: pelvis`. +- The unit of `rt/lowstate`'s tick on our unit is not verified yet; the node + assumes ms (`tick_seconds: 0.001`), so the tick must grow by 2 per 500 Hz + sample. A wrong unit would change the policy's rate, so the node waits + (logging the rate) unless the tick runs at 0.9-1.1 s per s of wall clock. + +## Files + +- `config/`: `g1.yaml` and `sim.yaml`. +- `policy/`: HOMIE's pretrained policy and its MuJoCo script, with their own + README and non-commercial LICENSE. +- `g1_homie/` + - `config.py` loads and checks a config, and starts Unitree DDS from it. + - `locomotion.py` holds the logic with no ROS or DDS: loading the policy, + the joint slots, the observed state, the command filter, and the + startup and fault handling (`LocomotionController`). + - `band.py` decides when the sim's band is released (`BandRelease`), with + no ROS. + - `driver.py` runs the controller on Unitree DDS (`LocomotionDriver`). + - `locomotion_node.py` is the ROS 2 node. +- `test/`: unit tests of the logic, DDS tests, and tests of the node. + +## Tests + +```bash +PYTEST_DISABLE_PLUGIN_AUTOLOAD=1 PYTHONPATH=.:$PYTHONPATH python3 -m pytest test +``` + +- They need the workspace built and sourced (`golem_safety`). They load + the policy from this package's `policy/`. +- The DDS tests run on domain 97: a fake G1, a fake arm controller and the + node in the test's process (its controller and driver, and its + `/safety/heartbeat` subscription), and `golem_safety`'s installed + `safety_node` (robot `g1`, a copy of its `split.yaml`, its heartbeat + remapped to a topic of the test's own) as a subprocess. Its heartbeat + gates the start; the heartbeat tests stop it or make it latch. They never + touch the robot. The test's process is busy enough that `rt/lowstate` is + sometimes tens of ms late, so they relax `faults.max_state_gap` and + `faults.max_latency` to 0.25 s and `startup.tick_rate` to [0.5, 2.0], + and `golem_safety`'s quiet-`rt/lowstate` report (`stale.state_seconds`) + to 0.25 s; the unit tests check the real values. Their 50 Hz checks still + need the host to keep up: at a load average of 11-19 on the 24-core laptop + the fake G1 itself fell behind once (565 samples in 1 s) and one failed; + the repeats passed. +- The fault tests (a stale or fallen state, a non-finite gyro, a late command) check + that the node faults and sends nothing more, and that `golem_safety` keeps + relaying its last leg command, reports the quiet leg input once and does + not estop. The stale-`rt/lowstate` test keeps `split.yaml`'s own + `stale.state_seconds`, and `golem_safety` reports the quiet `rt/lowstate` + and its return too. diff --git a/core_ws/src/g1_homie/config/g1.yaml b/core_ws/src/g1_homie/config/g1.yaml new file mode 100644 index 0000000..e14d33f --- /dev/null +++ b/core_ws/src/g1_homie/config/g1.yaml @@ -0,0 +1,71 @@ +# Real G1. HOMIE's walking policy drives the legs through golem_safety's +# split mode (its split.yaml), which merges them with the arm controller's +# waist + arms and alone writes rt/lowcmd. The node never writes rt/lowcmd. +# Command limits and rates are starting values, not yet tuned on our G1. + +topics: + low_state: rt/lowstate + # the legs' commands: must be a golem_safety input (rt/safety/...) + low_cmd: rt/safety/lowcmd_lower_in + # read only: the policy starts once the arm controller is commanding here + upper_cmd: rt/safety/lowcmd_upper_in + +network: + domain_id: 0 + interface: '' + +# frame of the rt/lowstate IMU. HOMIE was trained on the pelvis IMU and reads +# rt/lowstate's IMU as it; only pelvis is accepted until our unit's is verified +imu_frame: pelvis + +# rt/lowstate's tick, s per count: the policy steps every 20 ms of tick, so it +# follows the robot's clock however many samples the node sees. Assumed ms, as +# in the sim (2 per 500 Hz sample); not yet verified on our G1, so +# startup.tick_rate checks it before the legs move +tick_seconds: 0.001 + +startup: + # s to move the legs from where they are to the policy's default pose (the + # pre-pose), as the H1-2's leg node does + ramp_seconds: 2.0 + # hold the default pose until the operator calls /lowerbody/confirm_engage + # (std_srvs/srv/Trigger), then start the policy + wait_for_confirm: true + # s of rt/lowstate tick per s of wall clock the policy start accepts, measured + # while the legs hold the pre-pose: the robot's tick keeps to real time, and a + # tick_seconds that does not match its unit would change the policy's rate + tick_rate: [0.9, 1.1] + +heartbeat: + # s without golem_safety's /safety/heartbeat (10 Hz) after the first beat + # that stops the node for good, as a false beat (its estop latched) does + timeout: 0.5 + +band: + # the real robot has no elastic band: /elastic_band/toggle is never called + release: false + # s after the policy started at which the band would be released without + # /left_ee_pose (unused while release is false) + max_wait: 30.0 + +commands: + # deployment limits, inside the trained ranges (vx -0.8..1.2, vy -0.5..0.5, + # yaw -0.8..0.8): vx, vy in m/s, yaw in rad/s + vx: [-0.3, 0.5] + vy: [-0.3, 0.3] + yaw: [-0.6, 0.6] + # largest change per second: m/s^2, m/s^2, rad/s^2 + accel: [0.5, 0.5, 1.0] + # /cmd_vel older than this (s) means zero velocity + timeout: 0.5 + # largest change of the height target per second (m/s) + height_rate: 0.1 + +faults: + # longest gap (s, wall clock) allowed between rt/lowstate samples once the + # node commands the legs + max_state_gap: 0.05 + # longest a leg command may come after the rt/lowstate sample it answers + # (s, wall clock, from the robot's DDS write to the publish): well under the + # constant delay HOMIE stood with in the sim (30 ms; it fell at 60 ms) + max_latency: 0.025 diff --git a/core_ws/src/g1_homie/config/sim.yaml b/core_ws/src/g1_homie/config/sim.yaml new file mode 100644 index 0000000..726a251 --- /dev/null +++ b/core_ws/src/g1_homie/config/sim.yaml @@ -0,0 +1,74 @@ +# Simulated G1, DDS domain 1. HOMIE's walking policy drives the legs through +# golem_safety's split mode (its sim_split.yaml), which merges them with the +# arm controller's waist + arms and alone writes rt/lowcmd. +# The command limits are the trained ranges, and the rt/lowstate gap is relaxed +# because the sim can run slower than real time. + +topics: + low_state: rt/lowstate + # the legs' commands: must be a golem_safety input (rt/safety/...) + low_cmd: rt/safety/lowcmd_lower_in + # read only: the policy starts once the arm controller is commanding here + upper_cmd: rt/safety/lowcmd_upper_in + +network: + domain_id: 1 + interface: '' + +# frame of the rt/lowstate IMU. HOMIE was trained on the pelvis IMU and reads +# rt/lowstate's IMU as it; only pelvis is accepted until our unit's is verified +imu_frame: pelvis + +# rt/lowstate's tick, s per count: the policy steps every 20 ms of tick, so it +# follows the robot's clock however many samples the node sees. The sim counts +# ms of sim time +tick_seconds: 0.001 + +startup: + # s to move the legs from where they are to the policy's default pose (the + # pre-pose), as the H1-2's leg node does: the band holds the robot meanwhile + ramp_seconds: 2.0 + # hold the default pose until /lowerbody/confirm_engage is called: not in the + # sim, where the policy starts once the timing below holds + wait_for_confirm: false + # s of rt/lowstate tick per s of wall clock the policy start accepts, measured + # while the legs hold the pre-pose: the sim's real-time factor, which drops + # well below 1 on a loaded laptop (0.2 with golem_slam running) and catches up + # by at most 0.1 s. The policy steps on the tick, so a slow sim does not + # change its rate; the floor only catches a stalled one + tick_rate: [0.1, 1.25] + +heartbeat: + # s without golem_safety's /safety/heartbeat (10 Hz) after the first beat + # that stops the node for good, as a false beat (its estop latched) does + timeout: 0.5 + +band: + # call the sim's /elastic_band/toggle once the policy runs and frame_task_server + # is up (its first /left_ee_pose); the toggle flips the band, so it is called + # until one call succeeds, and never again + release: true + # s after the policy started at which the band is released without /left_ee_pose + max_wait: 30.0 + +commands: + # the trained ranges: vx, vy in m/s, yaw in rad/s + vx: [-0.8, 1.2] + vy: [-0.5, 0.5] + yaw: [-0.8, 0.8] + # largest change per second: m/s^2, m/s^2, rad/s^2 + accel: [0.5, 0.5, 1.0] + # /cmd_vel older than this (s) means zero velocity + timeout: 0.5 + # largest change of the height target per second (m/s) + height_rate: 0.1 + +faults: + # longest gap (s, wall clock) allowed between rt/lowstate samples once the + # node commands the legs; larger than g1.yaml's 0.05 s because the sim can + # run slower than real time + max_state_gap: 0.5 + # longest a leg command may come after the rt/lowstate sample it answers + # (s, wall clock, from the robot's DDS write to the publish): well under the + # constant delay HOMIE stood with in the sim (30 ms; it fell at 60 ms) + max_latency: 0.025 diff --git a/core_ws/src/g1_homie/g1_homie/__init__.py b/core_ws/src/g1_homie/g1_homie/__init__.py new file mode 100644 index 0000000..21ca4d6 --- /dev/null +++ b/core_ws/src/g1_homie/g1_homie/__init__.py @@ -0,0 +1 @@ +'''HOMIE's walking policy on the Unitree G1's legs''' diff --git a/core_ws/src/g1_homie/g1_homie/band.py b/core_ws/src/g1_homie/g1_homie/band.py new file mode 100644 index 0000000..70cf055 --- /dev/null +++ b/core_ws/src/g1_homie/g1_homie/band.py @@ -0,0 +1,55 @@ +''' +When the walking node releases the sim's elastic band, with no ROS: the policy must be +running, and frame_task_server up or band.max_wait passed since the policy started +''' + + +class BandRelease: + ''' + The sim's /elastic_band/toggle flips the band, so it is called until one call + succeeds and never again after that: at most one call in flight, a call stays in + flight until its result (there is no timeout, after which a late response could + meet a second call), and a failed one is sent again. Disabled (the real robot, + which has no band), it never calls. + + The caller calls poll() often (the node at 10 Hz); when it returns a reason, the + caller sends the call at once and reports how it ended with result(). Not + thread-safe: the caller serializes the calls. Times are one monotonic clock, in s + ''' + + def __init__(self, enabled: bool, max_wait: float): + self.enabled = enabled + self.max_wait = max_wait + self.released = False + # a call is due but the service is not there yet (for the caller's log) + self.service_missing = False + self._in_flight = False + + def poll(self, now: float, walking_since: float | None, frame_task_ready: bool, + service_ready: bool) -> str | None: + ''' + The reason to call /elastic_band/toggle now, and the call marked in flight; None + when no call is due. walking_since is when the policy started (None while it + does not run), frame_task_ready whether /left_ee_pose has been seen + ''' + if not self.enabled or self.released or self._in_flight or walking_since is None: + return None + if frame_task_ready: + reason = 'the policy runs and frame_task_server is up (/left_ee_pose)' + elif now - walking_since >= self.max_wait: + reason = (f'the policy has run for {self.max_wait:.0f} s, the maximum wait ' + '(band.max_wait), with no /left_ee_pose from frame_task_server') + else: + return None + self.service_missing = not service_ready + if not service_ready: + return None + self._in_flight = True + return reason + + def result(self, success: bool) -> None: + '''How the call poll() asked for ended: success, or failed and to be sent again''' + if not self._in_flight: + return + self._in_flight = False + self.released = success diff --git a/core_ws/src/g1_homie/g1_homie/config.py b/core_ws/src/g1_homie/g1_homie/config.py new file mode 100644 index 0000000..6279f1a --- /dev/null +++ b/core_ws/src/g1_homie/g1_homie/config.py @@ -0,0 +1,200 @@ +'''Load and check the walking node's YAML config, and start DDS from it''' + +import math +import os +from pathlib import Path + +import yaml + +from g1_homie.locomotion import HEARTBEAT_PERIOD, POLICY_DT + +# every key is required; an unknown one is an error, so a typo cannot fall +# back to a default +SECTIONS = { + 'topics': ('low_state', 'low_cmd', 'upper_cmd'), + 'network': ('domain_id', 'interface'), + 'startup': ('ramp_seconds', 'wait_for_confirm', 'tick_rate'), + 'heartbeat': ('timeout',), + 'band': ('release', 'max_wait'), + 'commands': ('vx', 'vy', 'yaw', 'accel', 'timeout', 'height_rate'), + 'faults': ('max_state_gap', 'max_latency'), +} +TOP_LEVEL = tuple(SECTIONS) + ('imu_frame', 'tick_seconds') +COMMANDS = ('vx', 'vy', 'yaw') +# HOMIE was trained on the pelvis IMU and deployed with rt/lowstate's IMU as it +IMU_FRAMES = ('pelvis',) + + +def _check_keys(mapping, keys, where): + if not isinstance(mapping, dict): + raise ValueError(f'{where} must be a mapping') + missing = [k for k in keys if k not in mapping] + unknown = [k for k in mapping if k not in keys] + if missing or unknown: + raise ValueError(f'{where}: missing keys {missing}, unknown keys {unknown}') + + +def _positive(value, name): + if isinstance(value, bool) or not isinstance(value, (int, float)): + raise ValueError(f'{name} must be a number, got {value!r}') + if not (math.isfinite(value) and value > 0.0): + raise ValueError(f'{name} must be positive and finite, got {value}') + return float(value) + + +def _flag(value, name): + if not isinstance(value, bool): + raise ValueError(f'{name} must be true or false, got {value!r}') + return value + + +def _numbers(value, count, name): + if (not isinstance(value, list) or len(value) != count or + any(isinstance(v, bool) or not isinstance(v, (int, float)) for v in value)): + raise ValueError(f'{name} must be a list of {count} numbers, got {value!r}') + if not all(math.isfinite(v) for v in value): + raise ValueError(f'{name} must be finite, got {value}') + return [float(v) for v in value] + + +def check_config(raw, cmd_range): + ''' + The config as plain values, checked; raises ValueError. cmd_range is the + policy's trained [low, high] for vx, vy and yaw (cmd_range in the policy's + g1.yaml), which the deployment limits must stay inside + ''' + _check_keys(raw, TOP_LEVEL, 'config') + for section, keys in SECTIONS.items(): + _check_keys(raw[section], keys, section) + + topics = {key: str(value) for key, value in raw['topics'].items()} + if not topics['low_cmd'].startswith('rt/safety/'): + # rt/lowcmd is only ever written by golem_safety + raise ValueError('topics.low_cmd must be a golem_safety input (rt/safety/...), ' + f'got {topics["low_cmd"]}') + if topics['low_cmd'] == topics['upper_cmd']: + raise ValueError('topics.low_cmd and topics.upper_cmd must differ: upper_cmd is only read') + + if raw['imu_frame'] not in IMU_FRAMES: + raise ValueError( + f'imu_frame must be one of {list(IMU_FRAMES)}, got {raw["imu_frame"]!r}: HOMIE was ' + "trained on the pelvis IMU, and a torso IMU's orientation would have to be " + 'carried through the waist joints, which the node does not do') + + commands = raw['commands'] + limits = {} + for name, (low_trained, high_trained) in zip(COMMANDS, cmd_range): + low, high = _numbers(commands[name], 2, f'commands.{name}') + # zero must stay reachable: the node starts at zero velocity and falls + # back to it when /cmd_vel goes quiet + if not low_trained <= low <= 0.0 <= high <= high_trained: + raise ValueError( + f'commands.{name} {[low, high]} must include 0 and lie inside the trained range ' + f'{[float(low_trained), float(high_trained)]}') + limits[name] = (low, high) + accel = _numbers(commands['accel'], len(COMMANDS), 'commands.accel') + for name, value in zip(COMMANDS, accel): + _positive(value, f'commands.accel ({name})') + + tick_seconds = _positive(raw['tick_seconds'], 'tick_seconds') + ticks = POLICY_DT / tick_seconds + if not (ticks >= 1.0 and abs(ticks - round(ticks)) < 1e-6): + raise ValueError(f'tick_seconds must divide the {POLICY_DT} s policy period, ' + f'got {tick_seconds}') + tick_rate = _numbers(raw['startup']['tick_rate'], 2, 'startup.tick_rate') + # a tick that keeps to the wall clock must always be accepted + if not 0.0 < tick_rate[0] <= 1.0 <= tick_rate[1]: + raise ValueError(f'startup.tick_rate {tick_rate} must be [low, high] with ' + '0 < low <= 1 <= high') + + heartbeat_timeout = _positive(raw['heartbeat']['timeout'], 'heartbeat.timeout') + # a timeout within one beat period would stop the node between two beats + if heartbeat_timeout <= HEARTBEAT_PERIOD: + raise ValueError(f"heartbeat.timeout must be longer than golem_safety's beat period " + f'({HEARTBEAT_PERIOD} s), got {heartbeat_timeout}') + + domain_id = raw['network']['domain_id'] + if isinstance(domain_id, bool) or not isinstance(domain_id, int) or domain_id < 0: + raise ValueError(f'network.domain_id must be a nonnegative integer, got {domain_id!r}') + interface = raw['network']['interface'] + + return { + 'topics': topics, + 'network': { + 'domain_id': domain_id, + 'interface': None if interface in (None, '') else str(interface), + }, + 'imu_frame': raw['imu_frame'], + 'tick_seconds': tick_seconds, + 'startup': { + 'ramp_seconds': _positive(raw['startup']['ramp_seconds'], 'startup.ramp_seconds'), + 'wait_for_confirm': _flag(raw['startup']['wait_for_confirm'], + 'startup.wait_for_confirm'), + 'tick_rate': tuple(tick_rate), + }, + 'heartbeat': {'timeout': heartbeat_timeout}, + 'band': { + 'release': _flag(raw['band']['release'], 'band.release'), + 'max_wait': _positive(raw['band']['max_wait'], 'band.max_wait'), + }, + 'commands': { + **limits, + 'accel': accel, + 'timeout': _positive(commands['timeout'], 'commands.timeout'), + 'height_rate': _positive(commands['height_rate'], 'commands.height_rate'), + }, + 'faults': { + key: _positive(raw['faults'][key], f'faults.{key}') for key in SECTIONS['faults'] + }, + } + + +def load_config(path, cmd_range): + '''Load the YAML config at path and check it (see check_config)''' + with open(path, 'r', encoding='utf-8') as f: + raw = yaml.safe_load(f) + try: + return check_config(raw, cmd_range) + except ValueError as e: + raise ValueError(f'{path}: {e}') from None + + +def resolve_config_path(config_name): + '''A bare config name (g1.yaml) from the installed config/, or a path as given''' + path = Path(config_name) + if not path.suffix: + path = path.with_suffix('.yaml') + if path.is_absolute(): + return path + from ament_index_python.packages import get_package_share_directory + share = Path(get_package_share_directory('g1_homie')) + if path.parts[0] == 'config': + return share / path + return share / 'config' / path + + +def init_channel_factory(config): + ''' + Start Unitree DDS on the config's domain and interface, and return the + domain; ValueError for a config written for another domain when that + domain is 0, the real robot + ''' + from unitree_sdk2py.core.channel import ChannelFactoryInitialize + + # ROS_DOMAIN_ID overrides network.domain_id, as for the other G1 nodes. + # G1_DDS_INTERFACE (e.g. enp130s0, the laptop's wired port to the robot) + # overrides network.interface. The Unitree SDK ignores CYCLONEDDS_URI and + # otherwise auto-picks an interface, which may be WiFi + env_domain = os.environ.get('ROS_DOMAIN_ID') + domain_id = int(env_domain) if env_domain is not None else config['network']['domain_id'] + if domain_id == 0 and config['network']['domain_id'] != 0: + # DDS domain 0 is the real robot: only a config written for it drives it + raise ValueError( + f'ROS_DOMAIN_ID=0 is the real robot, but this config is for DDS domain ' + f'{config["network"]["domain_id"]} (sim.yaml is for the sim); use g1.yaml') + interface = os.environ.get('G1_DDS_INTERFACE') or config['network']['interface'] + if interface: + ChannelFactoryInitialize(domain_id, interface) + else: + ChannelFactoryInitialize(domain_id) + return domain_id diff --git a/core_ws/src/g1_homie/g1_homie/driver.py b/core_ws/src/g1_homie/g1_homie/driver.py new file mode 100644 index 0000000..fcdc676 --- /dev/null +++ b/core_ws/src/g1_homie/g1_homie/driver.py @@ -0,0 +1,188 @@ +'''DDS side of the walking node, with no ROS: rt/lowstate in, leg commands out''' + +import threading +import time + +from cyclonedds.core import InstanceState, ReadCondition, SampleState, ViewState, WaitSet +from cyclonedds.domain import DomainParticipant +from cyclonedds.internal import InvalidSample +from cyclonedds.qos import Policy, Qos +from cyclonedds.sub import DataReader +from cyclonedds.topic import Topic +from cyclonedds.util import duration +from unitree_sdk2py.core.channel import ChannelPublisher +from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowCmd_ as LowCmdDefault +from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowCmd_, LowState_ +from unitree_sdk2py.utils.crc import CRC + +from g1_homie.locomotion import FAULTED, WALKING, SourceClock, read_state + +# LowCmd_.mode_pr: MODE_PR drives the ankles' pitch/roll joints, which the policy +# commands; 1 (MODE_AB) would drive the two ankle motors directly +MODE_PR = 0 + +# how long the worker waits for rt/lowstate before it checks the heartbeat and +# rt/lowstate for silence again (s) +WATCHDOG_PERIOD = 0.01 +# the readers keep only the newest sample: a backlog is dropped inside DDS, and +# nothing is decoded until the worker takes it +NEWEST_ONLY = Qos(Policy.History.KeepLast(1), Policy.Reliability.BestEffort) +ANY_SAMPLE = SampleState.Any | ViewState.Any | InstanceState.Any + + +class LocomotionDriver: + ''' + Runs a LocomotionController on Unitree DDS. A worker thread takes the + newest rt/lowstate sample whenever one arrives and gives it to the + controller with its tick; samples that arrived while it was busy are gone. + On every wake, a sample or not, it first checks golem_safety's heartbeat + and rt/lowstate for silence (LocomotionController.check_stale): from the + first wake past either's timeout, nothing more is sent. The heartbeat + itself, the operator's confirm, the commands and the band's release come + from the caller's threads (the ROS node). Each leg target the controller + returns is checked for latency (LocomotionController.latency_ok, from the + sample's DDS write, see SourceClock) and written at once to + topics['low_cmd'] (golem_safety's lower-body input, which holds it at + 500 Hz): slots 0-11 with mode 1, dq = tau = 0 and the training gains, + every other slot zero. + topics['upper_cmd'] is only read until the policy starts, to know the arm + controller is running. Nothing is written on a fault or on shutdown. + + The SDK's subscribers decode every sample in the DDS receive thread, under + the GIL, so a node that falls behind would first have to decode its + backlog; the readers here are plain DDS readers on a participant of the + node's own (domain_id, after ChannelFactoryInitialize). The publisher is + the SDK's + ''' + + def __init__(self, controller, topics, domain_id): + self._controller = controller + # serializes every call into the controller, and its writes + self._lock = threading.Lock() + self._closed = False + self._source_clock = SourceClock() + + self._cmd = LowCmdDefault() + # golem_safety stamps the G1 profile's mode_pr and mode_machine (0 and 5) + # again before anything reaches the robot + self._cmd.mode_pr = MODE_PR + for i, slot in enumerate(controller.leg_slots): + motor = self._cmd.motor_cmd[slot] + motor.mode = 1 + motor.kp = float(controller.kp[i]) + motor.kd = float(controller.kd[i]) + self._crc = CRC() + + self._pub = ChannelPublisher(topics['low_cmd'], LowCmd_) + self._pub.Init() + self._participant = DomainParticipant(domain_id) + self._topics = (Topic(self._participant, topics['low_state'], LowState_), + Topic(self._participant, topics['upper_cmd'], LowCmd_)) + self._state_reader = DataReader(self._participant, self._topics[0], NEWEST_ONLY) + self._upper_reader = DataReader(self._participant, self._topics[1], NEWEST_ONLY) + self._state_ready = ReadCondition(self._state_reader, ANY_SAMPLE) + self._upper_ready = ReadCondition(self._upper_reader, ANY_SAMPLE) + self._waitset = WaitSet(self._participant) + self._waitset.attach(self._state_ready) + self._waitset.attach(self._upper_ready) + self._worker = threading.Thread(target=self._run, name='locomotion', daemon=True) + self._worker.start() + + @property + def phase(self): + with self._lock: + return self._controller.phase + + @property + def fault(self): + with self._lock: + return self._controller.fault + + @property + def command(self): + '''The (vx, vy, yaw) and height the policy got last''' + with self._lock: + return self._controller.command + + @property + def walking_since(self): + '''When the policy started (time.monotonic()), while it runs; None otherwise''' + with self._lock: + return self._controller.walking_since + + def on_heartbeat(self, beat): + '''A /safety/heartbeat from golem_safety, received now''' + with self._lock: + self._controller.on_heartbeat(beat, time.monotonic()) + + def confirm(self): + '''The operator's go-ahead; (accepted, message), see LocomotionController.confirm''' + with self._lock: + return self._controller.confirm() + + def on_band_released(self): + '''The sim's elastic band has come off; see LocomotionController.on_band_released''' + with self._lock: + self._controller.on_band_released() + + def set_cmd_vel(self, vx, vy, yaw): + with self._lock: + self._controller.set_cmd_vel(vx, vy, yaw, time.monotonic()) + + def set_height(self, height): + with self._lock: + self._controller.set_height(height) + + def _run(self): + '''Take the newest samples as they arrive, and check for silence on every wake''' + while True: + # the GIL is released while waiting + self._waitset.wait(duration(seconds=WATCHDOG_PERIOD)) + with self._lock: + if self._closed: + return + try: + self._step() + except Exception as e: # noqa: BLE001 - never leave the legs on a broken node + self._controller.latch(f'unexpected error: {e!r}') + + def _step(self): + now = time.monotonic() + self._controller.check_stale(now) + if self._upper_reader is not None: + if any(not isinstance(m, InvalidSample) for m in self._upper_reader.take(N=1)): + self._controller.on_upper_cmd(now) + if self._controller.phase in (WALKING, FAULTED): + # only the policy's start needs the arm controller: stop waking on it + self._waitset.detach(self._upper_ready) + self._upper_ready = self._upper_reader = None + samples = self._state_reader.take(N=1) + if not samples or isinstance(samples[0], InvalidSample): + return + msg = samples[0] + source = msg.sample_info.source_timestamp + written = self._source_clock.written(source * 1e-9, now) if source > 0 else now + state = read_state(msg, self._controller.slots) + targets = self._controller.on_lowstate(state, msg.tick, now, now - written) + if targets is None or self._closed: + return + for i, slot in enumerate(self._controller.leg_slots): + self._cmd.motor_cmd[slot].q = float(targets[i]) + self._cmd.crc = self._crc.Crc(self._cmd) + sent = time.monotonic() + if self._controller.latency_ok(sent - written): + self._pub.Write(self._cmd) + # after the write, so the timing log never delays a command + self._controller.log_timing(sent) + + def shutdown(self): + '''Stop without writing anything more: golem_safety owns the estop''' + with self._lock: + self._closed = True + self._worker.join(timeout=1.0) + self._pub.Close() + # dropping the last references deletes the DDS entities; the participant + # last, which also deletes anything left under it + self._waitset = self._state_ready = self._upper_ready = None + self._state_reader = self._upper_reader = self._topics = None + self._participant = None diff --git a/core_ws/src/g1_homie/g1_homie/locomotion.py b/core_ws/src/g1_homie/g1_homie/locomotion.py new file mode 100644 index 0000000..7e804e7 --- /dev/null +++ b/core_ws/src/g1_homie/g1_homie/locomotion.py @@ -0,0 +1,630 @@ +''' +The walking node's logic with no ROS or DDS: loading the policy, the joint +slots, the observed state, the velocity and height commands, and the startup +(golem_safety's heartbeat, the pre-pose, the operator's confirm) and fault +handling +''' + +import collections +import importlib.util +import os +from typing import NamedTuple + +import numpy as np + +# the G1's body joints in rt/lowstate / rt/lowcmd slot order (Unitree's G1JointIndex) +BODY_JOINT_NAMES = ( + 'left_hip_pitch_joint', 'left_hip_roll_joint', 'left_hip_yaw_joint', + 'left_knee_joint', 'left_ankle_pitch_joint', 'left_ankle_roll_joint', + 'right_hip_pitch_joint', 'right_hip_roll_joint', 'right_hip_yaw_joint', + 'right_knee_joint', 'right_ankle_pitch_joint', 'right_ankle_roll_joint', + 'waist_yaw_joint', 'waist_roll_joint', 'waist_pitch_joint', + 'left_shoulder_pitch_joint', 'left_shoulder_roll_joint', 'left_shoulder_yaw_joint', + 'left_elbow_joint', 'left_wrist_roll_joint', 'left_wrist_pitch_joint', + 'left_wrist_yaw_joint', + 'right_shoulder_pitch_joint', 'right_shoulder_roll_joint', 'right_shoulder_yaw_joint', + 'right_elbow_joint', 'right_wrist_roll_joint', 'right_wrist_pitch_joint', + 'right_wrist_yaw_joint', +) +SLOT_BY_NAME = {name: slot for slot, name in enumerate(BODY_JOINT_NAMES)} + +# HOMIE was trained at 50 Hz. The node steps on a 20 ms grid of rt/lowstate's +# tick, so it follows the robot's clock (the sim's, in the sim) rather than the +# laptop's, however many samples it sees +POLICY_HZ = 50.0 +POLICY_DT = 1.0 / POLICY_HZ +# rt/lowstate's tick is a uint32 that wraps +TICK_MODULO = 2 ** 32 +# rt/lowstate slots of the policy's 27 joints: legs 0-11, waist yaw 12 and the +# arms 15-28. Waist roll and pitch (13, 14) are not observed +POLICY_SLOTS = tuple(range(0, 13)) + tuple(range(15, 29)) +# gravity's z in the pelvis frame is -1 upright. Starting needs it below -0.9 +# (tilted less than about 25 deg); above -0.5 (more than 60 deg) is a fall +UPRIGHT_GRAVITY_Z = -0.9 +FALLEN_GRAVITY_Z = -0.5 +# how far the IMU quaternion's norm may be from 1 +QUAT_NORM_TOLERANCE = 0.05 +# the arm controller counts as running while its last command is this recent (s) +UPPER_FRESH_SECONDS = 0.5 +# the start conditions must hold this long, in rt/lowstate's tick (s): ten of +# the kernel's 100 ms CPU-quota periods, so a container that is being +# throttled shows it before the legs move +STEADY_SECONDS = 1.0 +# SourceClock keeps the fastest delivery over the last WINDOW_BUCKETS buckets +# of BUCKET_SECONDS each (s): about 2 s +BUCKET_SECONDS = 0.5 +WINDOW_BUCKETS = 4 +# how often the timing (latency, policy step spacing) is logged, s +STATS_SECONDS = 10.0 +# a reason to wait is logged when it changes, each at most this often (s) +WAIT_LOG_SECONDS = 5.0 +# golem_safety's /safety/heartbeat period (s); heartbeat.timeout must be longer +HEARTBEAT_PERIOD = 0.1 + +# phases: silent until the start conditions hold, then a ramp of the legs to +# the default pose, a hold there until the operator confirms (with +# startup.wait_for_confirm), then the policy; a fault is silent for good +WAITING = 'waiting' +RAMPING = 'ramping' +HOLDING = 'holding' +WALKING = 'walking' +FAULTED = 'faulted' + + +def load_policy(policy_dir): + ''' + HomiePolicy from the policy's homie_policy.py, g1.yaml and deploy.onnx in + policy_dir (this package's policy/), used in place (nothing is copied) + ''' + module_path = os.path.join(policy_dir, 'homie_policy.py') + if not os.path.isfile(module_path): + raise FileNotFoundError( + f'{module_path} not found: policy_dir must be the policy directory (g1_homie/policy)') + spec = importlib.util.spec_from_file_location('homie_policy', module_path) + module = importlib.util.module_from_spec(spec) + spec.loader.exec_module(module) + # HomiePolicy's session (homie_policy.policy_session) runs on the calling + # thread: one intra-op and one inter-op thread, no spinning, nothing pinned. + # onnxruntime's default pool (as wide as the machine, pinned, spinning) + # burned about two cores at 50 Hz, and its pinned threads could be starved + # by another process's on the same CPUs, which hung the node's startup + policy = module.HomiePolicy(os.path.join(policy_dir, 'g1.yaml')) + # onnxruntime's first run is slow: take it now rather than in the first + # walking step, then start again from an empty history + policy.step(np.zeros(3), policy.config['height_cmd'], np.zeros(3), [1.0, 0.0, 0.0, 0.0], + policy.default_q, np.zeros(len(policy.policy_joints))) + policy.reset() + return policy + + +def policy_slots(joint_names): + '''The rt/lowstate motor slot of each policy joint, looked up by name in BODY_JOINT_NAMES''' + unknown = [name for name in joint_names if name not in SLOT_BY_NAME] + if unknown: + raise ValueError(f'policy joints that are not G1 body joints: {unknown}') + slots = tuple(SLOT_BY_NAME[name] for name in joint_names) + if slots != POLICY_SLOTS: + raise ValueError(f'policy joints map to slots {list(slots)}, expected 0-12 and 15-28') + return slots + + +class State(NamedTuple): + '''What the policy observes of one rt/lowstate sample''' + quat_wxyz: np.ndarray # pelvis orientation, w first: the rt/lowstate IMU + gyro: np.ndarray # pelvis angular velocity in the pelvis frame, rad/s + q: np.ndarray # positions of the policy joints, rad + dq: np.ndarray # velocities of the policy joints, rad/s + + +def read_state(msg, slots): + '''State from a unitree_hg LowState_; slots from policy_slots()''' + motors = msg.motor_state + return State( + quat_wxyz=np.array(msg.imu_state.quaternion, dtype=np.float64), + gyro=np.array(msg.imu_state.gyroscope, dtype=np.float64), + q=np.array([motors[i].q for i in slots], dtype=np.float64), + dq=np.array([motors[i].dq for i in slots], dtype=np.float64), + ) + + +def tick_delta(tick, previous): + '''tick - previous for rt/lowstate's uint32 tick, across its wrap; negative if it went back''' + delta = (int(tick) - int(previous)) % TICK_MODULO + return delta - TICK_MODULO if delta >= TICK_MODULO // 2 else delta + + +class SourceClock: + ''' + When each rt/lowstate sample was written, on the node's clock, from its DDS + source timestamp (the writer's clock, s). The two clocks differ by an + unknown offset but run at the same rate, real time, in the sim too (its + writes carry the host's clock, not sim time). So this keeps the smallest + `received - source` of the last ~2 s, the fastest delivery, and puts each + sample's write at its source timestamp plus that offset. A latency + measured from there counts any delivery delay beyond the fastest one, the + time the sample waited for the node and the node's own work + ''' + + def __init__(self): + # [start, smallest offset] per bucket, newest last + self._buckets = collections.deque(maxlen=WINDOW_BUCKETS) + + def written(self, source, received): + '''The node-clock time a sample with this source timestamp was written; received is now''' + offset = received - source + if self._buckets and received - self._buckets[-1][0] < BUCKET_SECONDS: + self._buckets[-1][1] = min(self._buckets[-1][1], offset) + else: + self._buckets.append([received, offset]) + return source + min(bucket[1] for bucket in self._buckets) + + +def gravity_z(quat_wxyz): + '''z of gravity in the pelvis frame, -1 upright (homie_policy's get_gravity_orientation)''' + w, x, y, z = quat_wxyz + return -(w * w - x * x - y * y + z * z) + + +class CommandFilter: + ''' + The velocity and height commands the policy gets: clipped to the trained + ranges, then to the deployment limits, and moved toward the request by at + most the configured rates. Starts at zero velocity and the standing height + ''' + + def __init__(self, policy, commands): + self._policy = policy + self._low = np.array([commands[name][0] for name in ('vx', 'vy', 'yaw')]) + self._high = np.array([commands[name][1] for name in ('vx', 'vy', 'yaw')]) + self._max_step = np.array(commands['accel']) * POLICY_DT + self._max_height_step = commands['height_rate'] * POLICY_DT + self.cmd = np.zeros(3) + self.height = float(policy.config['height_cmd']) + + def update(self, cmd, height): + '''One policy step toward cmd (vx, vy, yaw) and height; returns the new (cmd, height)''' + cmd, height = self._policy.clip_command(cmd, height) + cmd = np.clip(np.asarray(cmd, dtype=np.float64), self._low, self._high) + self.cmd = self.cmd + np.clip(cmd - self.cmd, -self._max_step, self._max_step) + step = min(max(height - self.height, -self._max_height_step), self._max_height_step) + self.height += step + return self.cmd.copy(), self.height + + +class LocomotionController: + ''' + HOMIE on the legs, fed the newest rt/lowstate sample with its tick. Not + thread-safe: the caller serializes the calls. The ramp and the policy step + on a grid of ticks one policy period (20 ms of robot time) apart, from the + first ramp step: a step is due at the first sample at or past its grid + tick, and the next one at the first grid tick at least half a period after + that sample. Samples the caller drops never change the rate: 50 Hz of + robot time on average, steps never closer than 10 ms. + + - waiting: silent until golem_safety's first true heartbeat (on_heartbeat) + and a finite, upright rt/lowstate, as the H1-2's leg node waits only for + the heartbeat and the state + - ramping: the legs move from their measured positions to the policy's + default pose over startup.ramp_seconds + - holding: the default pose every policy step, every fault checked, until + the policy may start: rt/lowstate has been steady and on time (no sample + older than half faults.max_latency when taken, no gap over + faults.max_state_gap) for + STEADY_SECONDS, its tick has kept to the wall clock within + startup.tick_rate over that time (a wrong tick_seconds would change the + policy's rate), the arm controller is commanding, and, with + startup.wait_for_confirm, the operator has confirmed (confirm) + - walking: the policy runs, starting at zero velocity and the standing + height, with an empty history; in the sim its history empties again + when the elastic band comes off (on_band_released) + - faulted: once the legs are commanded, a stale, non-finite or fallen + state, a tick that goes back, a bad IMU quaternion, a policy step that + fails, or a policy command later than faults.max_latency (latency_ok) stops all + output for good. golem_safety then keeps relaying the last leg command, + which does not balance the robot, and reports the quiet input; it does + not estop on it. It estops on what it checks in rt/lowstate itself: a + non-finite joint value, a joint past limits.estop or another robot's + mode_machine. The node never sends a zero-gain or damping command + itself. In every phase, a false heartbeat (golem_safety's estop has + latched), or none for heartbeat.timeout once the first has come + (check_stale), stops all output for good too + + log(level, message) reports phase changes, why it is waiting, faults and, + every STATS_SECONDS (log_timing), the timing; level is 'info', 'warn' or + 'error'. The `now` arguments are one monotonic clock, in s + ''' + + def __init__(self, policy, config, log): + self.policy = policy + self.slots = policy_slots(policy.policy_joints) + self.leg_slots = self.slots[:len(policy.leg_joints)] + self.kp, self.kd = policy.gains(policy.leg_joints) + self._default = np.asarray(policy.default_angles, dtype=np.float64) + self._ramp_steps = max(1, int(round(config['startup']['ramp_seconds'] * POLICY_HZ))) + self._wait_for_confirm = config['startup']['wait_for_confirm'] + self._heartbeat_timeout = config['heartbeat']['timeout'] + self._tick_seconds = config['tick_seconds'] + # ticks per policy step (20 for a tick of 1 ms) + self._period_ticks = int(round(POLICY_DT / self._tick_seconds)) + self._steady_ticks = int(round(STEADY_SECONDS / self._tick_seconds)) + self._tick_rate = config['startup']['tick_rate'] + self._max_gap = config['faults']['max_state_gap'] + self._max_latency = config['faults']['max_latency'] + self._cmd_timeout = config['commands']['timeout'] + self._upper_topic = config['topics']['upper_cmd'] + self._filter = CommandFilter(policy, config['commands']) + self._log = log + + self.phase = WAITING + self.fault = None + self._last_tick = None # the newest tick taken + self._last_sample = None # when it was taken + # tick from which every sample was usable, while waiting, and when it was taken + self._good_since = self._good_wall = None + self._due_tick = None # grid tick of the next ramp or policy step + self._step_tick = None # tick of the last one + self._wait_reason = None + self._wait_logged = {} # when each kind of reason to wait was last logged + # for the timing log: latencies (s) and policy step spacing (ticks) + self._latencies = [] + self._step_gaps = [] + self._stats_start = None + self._upper_time = None + self._cmd_vel = np.zeros(3) + self._cmd_vel_time = None + self._height = self._filter.height + self._ramp_start = None + self._ramp_step = 0 + self._beat_time = None # when the last true heartbeat came + self._walking_since = None + self._ready_reason = 'rt/lowstate has not been steady yet' # None once the policy may start + + @property + def command(self): + '''The (vx, vy, yaw) and height the policy got last''' + return self._filter.cmd.copy(), self._filter.height + + @property + def walking_since(self): + '''When the policy started (s, the `now` clock), while it runs; None otherwise''' + return self._walking_since if self.phase == WALKING else None + + def on_heartbeat(self, beat, now): + ''' + golem_safety's /safety/heartbeat at now (s): true once it can relay the + commands, false once its estop has latched. The node starts only after + the first true beat; a false beat stops it for good whenever it comes + ''' + if self.phase == FAULTED: + return + if not beat: + self._latch_heartbeat( + "golem_safety's /safety/heartbeat is false: its estop has latched") + return + self._beat_time = now + + def confirm(self): + ''' + The operator's go-ahead (/lowerbody/confirm_engage): the policy starts on + the next policy step. Accepted only while holding the default pose; + returns (accepted, message) + ''' + if self.phase != HOLDING: + return False, (f'refused: the node is {self.phase}; the policy starts on a confirm ' + f'only while the legs hold the default pose ({HOLDING})') + if self._ready_reason is not None: + return False, f'refused: the policy cannot start yet: {self._ready_reason}' + self._start_policy(self._last_sample) + return True, 'confirmed: the policy starts on the next policy step' + + def on_band_released(self): + ''' + The sim's elastic band has come off. While the policy runs, its history + and last action restart empty, as at its start, so its first steps on + the free robot are not computed from the steps the band held, as the + H1-2's leg node does; the command, the height and the step grid carry + on. Ignored in every other phase + ''' + if self.phase != WALKING: + return + self.policy.reset() + self._log('info', 'elastic band off: the policy carries on from an empty history') + + def on_upper_cmd(self, now): + '''The arm controller commanded (a sample on topics.upper_cmd) at now (s)''' + self._upper_time = now + + def set_cmd_vel(self, vx, vy, yaw, now): + '''A /cmd_vel request in the base frame (m/s, m/s, rad/s), received at now (s)''' + cmd = np.array([vx, vy, yaw], dtype=np.float64) + if not np.all(np.isfinite(cmd)): + self._log('warn', f'/cmd_vel is not finite ({cmd.tolist()}): stopping') + cmd = np.zeros(3) + self._cmd_vel, self._cmd_vel_time = cmd, now + + def set_height(self, height): + '''A /locomotion/height request, the pelvis height target (m)''' + if not np.isfinite(height): + self._log('warn', f'ignoring a /locomotion/height that is not finite ({height})') + return + self._height = float(height) + + def check_stale(self, now): + ''' + Latch a fault if golem_safety's heartbeat has stopped (none for + heartbeat.timeout since the last, once the first has come), or + rt/lowstate has gone quiet while the legs are commanded + ''' + if self.phase == FAULTED: + return + if self._beat_time is not None and now - self._beat_time > self._heartbeat_timeout: + self._latch_heartbeat( + f'no /safety/heartbeat for {(now - self._beat_time) * 1000:.0f} ms ' + f'(> {self._heartbeat_timeout * 1000:.0f} ms): golem_safety has stopped or ' + 'cannot keep up') + return + if self.phase == WAITING: + return + age = now - self._last_sample + if age > self._max_gap: + self._latch(f'rt/lowstate stale: no sample for {age * 1000:.0f} ms ' + f'(> {self._max_gap * 1000:.0f} ms)') + + def latch(self, reason): + '''Stop commanding the legs for good''' + if self.phase != FAULTED: + self._latch(reason) + + def on_lowstate(self, state, tick, now, age=0.0): + ''' + Take an rt/lowstate sample with its tick, taken at now (s), age s after + it was written (SourceClock); returns the 12 leg position targets to + publish, or None to stay silent. A sample whose tick did not advance + is ignored + ''' + delta = None if self._last_tick is None else tick_delta(tick, self._last_tick) + if delta == 0: + return None + gap = None if self._last_sample is None else now - self._last_sample + went_back = delta is not None and delta < 0 + previous_tick = self._last_tick + self._last_sample, self._last_tick = now, tick + if self.phase == FAULTED: + return None + if self.phase == WAITING: + return self._wait(state, tick, now, gap, age, previous_tick if went_back else None) + + # the legs are commanded: every problem is a fault + if went_back: + return self._latch(f'rt/lowstate tick went back from {previous_tick} to {tick}') + if gap is not None and gap > self._max_gap: + return self._latch(f'rt/lowstate stale: {gap * 1000:.0f} ms between samples ' + f'(> {self._max_gap * 1000:.0f} ms)') + problem = self._state_problem(state) + if problem is not None: + return self._latch(problem) + gz = gravity_z(state.quat_wxyz) + if gz > FALLEN_GRAVITY_Z: + return self._latch( + f'fell: gravity z {gz:.2f} in the pelvis frame (> {FALLEN_GRAVITY_Z})') + if self.phase != WALKING: + self._track_ready(tick, now, gap, age) + if tick_delta(tick, self._due_tick) < 0: + return None + self._step_at(tick) + if self.phase == RAMPING: + return self._ramp(now) + if self.phase == HOLDING: + if not self._wait_for_confirm and self._ready_reason is None: + self._start_policy(now) + return self._walk(state, now) + return self._default.copy() + return self._walk(state, now) + + def _step_at(self, tick): + '''A ramp or policy step on this tick: set the grid tick of the next one''' + period = self._period_ticks + if self._due_tick is None: + # the first step sets the grid + self._due_tick, late = tick, 0 + else: + late = tick_delta(tick, self._due_tick) + self._step_gaps.append(tick_delta(tick, self._step_tick)) + # the first grid tick at least half a period after this one: the + # smallest k >= 1 with k * period >= late + period / 2 + periods = (2 * late + 3 * period - 1) // (2 * period) + self._due_tick = (self._due_tick + periods * period) % TICK_MODULO + self._step_tick = tick + + def latency_ok(self, latency): + ''' + Check a command just before it is published: latency (s) is the time + from the rt/lowstate sample it answers (when it was written, + SourceClock) to now. False, and a latched fault, if it is over + faults.max_latency while the policy runs; the pre-pose is a PD hold, + which a late command does not upset + ''' + if self.phase == FAULTED: + return False + if latency > self._max_latency and self.phase == WALKING: + self._latch(f'command late: {latency * 1000:.1f} ms after its rt/lowstate sample ' + f'(> {self._max_latency * 1000:.0f} ms)') + return False + self._latencies.append(latency) + return True + + def log_timing(self, now): + ''' + Log the timing (latency_ok's latencies, the policy steps' spacing) once + every STATS_SECONDS; the caller calls it after a publish, not before + ''' + if self._stats_start is None: + self._stats_start = now + return + if now - self._stats_start < STATS_SECONDS or not self._latencies: + return + latency = np.array(self._latencies) * 1000 + timing = (f'timing over the last {now - self._stats_start:.0f} s: {latency.size} ' + f'commands, latency median {np.median(latency):.1f} ms, ' + f'max {latency.max():.1f} ms (limit {self._max_latency * 1000:.0f} ms)') + if self._step_gaps: + gaps = np.array(self._step_gaps) * self._tick_seconds * 1000 + timing += (f'; steps {gaps.min():.0f}-{gaps.max():.0f} ms of rt/lowstate time apart, ' + f'mean {gaps.mean():.1f} ms') + self._log('info', timing) + self._latencies, self._step_gaps, self._stats_start = [], [], now + + def _state_problem(self, state): + '''Why a state cannot be used (values not finite, a bad IMU quaternion), or None''' + for name, values in zip(State._fields, state): + bad = np.flatnonzero(~np.isfinite(values)) + if bad.size: + # joint names for q and dq, indices for the IMU + where = bad.tolist() + if name in ('q', 'dq'): + where = [BODY_JOINT_NAMES[self.slots[i]] for i in bad] + return f'rt/lowstate {name} is not finite (at {where})' + norm = float(np.linalg.norm(state.quat_wxyz)) + if abs(norm - 1.0) > QUAT_NORM_TOLERANCE: + return f'the rt/lowstate IMU quaternion is not a unit quaternion (norm {norm:.3f})' + return None + + def _wait(self, state, tick, now, gap, age, went_back_from): + '''Start the ramp on the first finite, upright sample after the first true heartbeat''' + reason = self._state_problem(state) + if reason is None: + gz = gravity_z(state.quat_wxyz) + if gz >= UPRIGHT_GRAVITY_Z: + reason = (f'the robot is not upright (gravity z {gz:.2f} in the pelvis frame, ' + f'needs < {UPRIGHT_GRAVITY_Z})') + if reason is None and self._beat_time is None: + reason = 'no /safety/heartbeat yet (golem_safety beats once it can relay)' + if reason is not None: + self._log_wait(reason, now) + return None + self.phase = RAMPING + self._step_at(tick) + self._ramp_start = state.q[:len(self.leg_slots)].copy() + self._ramp_step = 0 + self._track_ready(tick, now, gap, age) + self._log('info', f'moving the legs from where they are to the default pose over ' + f'{self._ramp_steps / POLICY_HZ:.1f} s') + return self._ramp(now) + + def _track_ready(self, tick, now, gap, age): + ''' + While the legs pre-pose and hold: set _ready_reason to why the policy + may not start yet, or None once the timing has held for STEADY_SECONDS + of ticks and the arm controller is commanding + ''' + reason = None + if gap is not None and gap > self._max_gap: + reason = (f'rt/lowstate is not steady ({gap * 1000:.0f} ms between samples, ' + f'max {self._max_gap * 1000:.0f} ms)') + # a command answering a sample this late would be late too: the laptop + # is not keeping up. Half faults.max_latency, so that the policy does + # not run straight into the latency fault. The wall-clock gap between + # samples is not checked here: a sim below real time spaces them out + # (9 ms at 0.2x for 2 ms of tick) while each is still answered at once + if reason is None and age > self._max_latency / 2: + reason = (f'rt/lowstate reaches the node late ({age * 1000:.0f} ms, ' + f'max {self._max_latency * 500:.1f} ms to start)') + if reason is not None: + self._good_since = None + elif self._good_since is None: + self._good_since, self._good_wall = tick, now + upper_age = None if self._upper_time is None else now - self._upper_time + if reason is None and (upper_age is None or upper_age > UPPER_FRESH_SECONDS): + reason = f'no commands from the arm controller on {self._upper_topic}' + if reason is None and tick_delta(tick, self._good_since) < self._steady_ticks: + # the steady window is still filling: nothing to report + self._ready_reason = 'rt/lowstate has not been steady for long enough yet' + return + if reason is None: + # s of tick per s of wall clock over the steady samples + rate = (tick_delta(tick, self._good_since) * self._tick_seconds / + max(now - self._good_wall, 1e-9)) + low, high = self._tick_rate + if not low <= rate <= high: + reason = (f"rt/lowstate's tick is off the wall clock ({rate:.2f} s of tick per s, " + f'needs {low:.2f}-{high:.2f}; is tick_seconds right?)') + self._good_since = None + if reason is not None: + self._log_wait(reason, now, 'the policy waits') + elif self._ready_reason is not None: + self._log('info', f'the policy may start (rt/lowstate\'s tick ran at {rate:.2f} s ' + f'per s of wall clock)') + self._ready_reason = reason + + def _log_wait(self, reason, now, prefix='waiting to start'): + ''' + Log a reason to wait when its kind (the text before any ' (') changes, + so a steady one is logged once, and each kind at most every + WAIT_LOG_SECONDS, so two that alternate do not flood the log + ''' + kind = reason.split(' (')[0] + last = self._wait_logged.get(kind) + if kind != self._wait_reason and (last is None or now - last >= WAIT_LOG_SECONDS): + self._wait_logged[kind] = now + self._log('info', f'{prefix}: {reason}') + self._wait_reason = kind + + def _ramp(self, now): + '''The next target on the line from the measured legs to the default pose''' + alpha = self._ramp_step / self._ramp_steps + targets = self._ramp_start + alpha * (self._default - self._ramp_start) + self._ramp_step += 1 + if self._ramp_step > self._ramp_steps: + # the default pose was just sent: the hold or the policy takes over on + # the next step + self.phase = HOLDING + if self._wait_for_confirm: + self._log('info', 'legs at the default pose: holding it until the operator ' + 'confirms (ros2 service call /lowerbody/confirm_engage ' + 'std_srvs/srv/Trigger)') + else: + self._log('info', 'legs at the default pose: holding it until the policy may start') + return targets + + def _start_policy(self, now): + ''' + The policy takes the legs on the next step, with an empty history, at + zero velocity and the standing height: a /cmd_vel or height request that + came before is dropped + ''' + self.phase = WALKING + self._walking_since = now + self.policy.reset() + self._cmd_vel, self._cmd_vel_time = np.zeros(3), None + self._height = self._filter.height + self._log('info', f'walking: policy started at zero velocity and ' + f'{self._filter.height:.2f} m height') + + def _walk(self, state, now): + '''One policy step on the filtered command''' + fresh = self._cmd_vel_time is not None and now - self._cmd_vel_time <= self._cmd_timeout + cmd, height = self._filter.update(self._cmd_vel if fresh else np.zeros(3), self._height) + try: + targets = self.policy.step(cmd, height, state.gyro, state.quat_wxyz, state.q, state.dq) + except Exception as e: # noqa: BLE001 - any policy failure stops the legs + return self._latch(f'policy step failed: {e}') + targets = np.asarray(targets, dtype=np.float64) + if targets.shape != (len(self.leg_slots),) or not np.all(np.isfinite(targets)): + return self._latch(f'policy output is not 12 finite targets: {targets.tolist()}') + return targets + + def _latch(self, reason, after=None): + '''Stop for good; after says what happens next (default: golem_safety holds the legs)''' + self.phase = FAULTED + self.fault = reason + if after is None: + after = ('Stopped commanding the legs; golem_safety keeps relaying their last ' + 'command, which does not balance the robot (it estops instead when ' + 'rt/lowstate has a non-finite joint value, a joint past limits.estop or ' + "another robot's mode_machine). Restart the node to clear") + self._log('error', f'fault: {reason}. {after}') + return None + + def _latch_heartbeat(self, reason): + '''Stop for good: golem_safety has latched its estop, or stopped''' + self._latch(reason, 'Stopped commanding the legs for good. Restart the bringup to clear') diff --git a/core_ws/src/g1_homie/g1_homie/locomotion_node.py b/core_ws/src/g1_homie/g1_homie/locomotion_node.py new file mode 100644 index 0000000..c8f96f3 --- /dev/null +++ b/core_ws/src/g1_homie/g1_homie/locomotion_node.py @@ -0,0 +1,187 @@ +'''ROS 2 node running HOMIE's walking policy on the G1's legs''' + +import argparse +import time + +import rclpy +from rclpy.node import Node +from rclpy.parameter import Parameter +from std_msgs.msg import Bool, Float32 +from std_srvs.srv import Trigger +from geometry_msgs.msg import PoseStamped, Twist + +from g1_homie.band import BandRelease +from g1_homie.config import init_channel_factory, load_config, resolve_config_path +from g1_homie.driver import LocomotionDriver +from g1_homie.locomotion import LocomotionController, load_policy + +# where the ros container mounts this package's policy/ (deploy.onnx, g1.yaml, homie_policy.py) +DEFAULT_POLICY_DIR = '/home/code/g1_homie' +# how often the band release is checked (s) +BAND_PERIOD = 0.1 +# /elastic_band/toggle's message when the call left the band on (golem_robocasa's +# MeasurementBridge answers "enabled" or "disabled") +BAND_ON = 'enabled' + + +class LocomotionNode(Node): + ''' + - /cmd_vel (geometry_msgs/Twist): linear.x, linear.y (m/s) and angular.z + (rad/s) in the base frame; older than commands.timeout means stop + - /locomotion/height (std_msgs/Float32): the pelvis height target, m + (standing, 0.74, until one arrives) + - /safety/heartbeat (std_msgs/Bool): golem_safety's. The node starts after + the first true beat; a false beat, or none for heartbeat.timeout once + the first has come, stops it for good + - /lowerbody/confirm_engage (std_srvs/Trigger): the operator's go-ahead + while the legs hold the default pose (startup.wait_for_confirm); refused + at any other time + - with band.release (the sim): /left_ee_pose (geometry_msgs/PoseStamped), + which frame_task_server publishes once its start-up moves are done, and + a client of the sim's /elastic_band/toggle (std_srvs/Trigger), called + from a 10 Hz timer as BandRelease says: once the policy runs and a + /left_ee_pose has come, or band.max_wait after the policy started. When + the call takes the band off, the policy's history restarts + (LocomotionController.on_band_released) + The legs go to golem_safety's lower-body input over Unitree DDS; see + LocomotionController for the startup and the faults. + + use_sim_time is always false, whatever the launch passes: the node times + everything by rt/lowstate's tick and the monotonic clock, and sim time + would only add a /clock subscription (500 Hz in the sim) in Python + ''' + + def __init__(self, config_name='g1.yaml'): + no_sim_time = Parameter('use_sim_time', Parameter.Type.BOOL, False) + super().__init__('locomotion_node', parameter_overrides=[no_sim_time]) + self.declare_parameter('policy_dir', DEFAULT_POLICY_DIR) + policy_dir = self.get_parameter('policy_dir').value + policy = load_policy(policy_dir) + config_path = resolve_config_path(config_name) + config = load_config(config_path, policy.config['cmd_range']) + + startup = config['startup'] + self.get_logger().info( + f'Loaded {config_path} and the policy in {policy_dir}. Silent until golem_safety ' + f'beats on /safety/heartbeat and {config["topics"]["low_state"]} is upright; then the ' + f'legs move to the default pose over {startup["ramp_seconds"]:.1f} s and hold it ' + f'until the timing is steady and the arm controller commands on ' + f'{config["topics"]["upper_cmd"]}' + + (' and /lowerbody/confirm_engage is called,' if startup['wait_for_confirm'] else ',') + + ' and the policy starts' + + ('; the elastic band is released once the policy runs and /left_ee_pose has come' + if config['band']['release'] else '')) + + domain_id = init_channel_factory(config) + controller = LocomotionController(policy, config, log=self._log) + self.create_subscription(Twist, '/cmd_vel', self._on_cmd_vel, 10) + self.create_subscription(Float32, '/locomotion/height', self._on_height, 10) + self.create_subscription(Bool, '/safety/heartbeat', self._on_heartbeat, 10) + self.create_service(Trigger, '/lowerbody/confirm_engage', self._on_confirm) + self._driver = LocomotionDriver(controller, config['topics'], domain_id) + + self._band = BandRelease(config['band']['release'], config['band']['max_wait']) + self._frame_task_ready = False + self._pose_sub = self._band_client = self._band_timer = None + if self._band.enabled: + self._pose_sub = self.create_subscription( + PoseStamped, '/left_ee_pose', self._on_ee_pose, 10) + self._band_client = self.create_client(Trigger, '/elastic_band/toggle') + self._band_timer = self.create_timer(BAND_PERIOD, self._on_band_timer) + + def _log(self, level, message): + # one call per severity: rclpy refuses a change of severity at one call site + if level == 'error': + self.get_logger().error(message) + elif level == 'warn': + self.get_logger().warning(message) + else: + self.get_logger().info(message) + + def _on_cmd_vel(self, msg: Twist): + self._driver.set_cmd_vel(msg.linear.x, msg.linear.y, msg.angular.z) + + def _on_height(self, msg: Float32): + self._driver.set_height(msg.data) + + def _on_heartbeat(self, msg: Bool): + self._driver.on_heartbeat(msg.data) + + def _on_confirm(self, request, response): + response.success, response.message = self._driver.confirm() + self._log('info' if response.success else 'warn', + f'/lowerbody/confirm_engage: {response.message}') + return response + + def _on_ee_pose(self, msg: PoseStamped): + self._frame_task_ready = True + + def _on_band_timer(self): + if self._frame_task_ready and self._pose_sub is not None: + # one pose is all the release needs: stop taking frame_task_server's 100 Hz + self.destroy_subscription(self._pose_sub) + self._pose_sub = None + missing = self._band.service_missing + reason = self._band.poll(time.monotonic(), self._driver.walking_since, + self._frame_task_ready, self._band_client.service_is_ready()) + if self._band.service_missing and not missing: + self._log('warn', 'the elastic band is due for release, but /elastic_band/toggle ' + 'is not there yet: waiting for it') + if reason is None: + return + self._log('info', f'releasing the elastic band: {reason}') + self._band_client.call_async(Trigger.Request()).add_done_callback(self._on_band_toggled) + + def _on_band_toggled(self, future): + try: + response = future.result() + failure = 'no response' if response is None else response.message + except Exception as e: # noqa: BLE001 - a failed call is sent again + response, failure = None, repr(e) + success = response is not None and response.success + self._band.result(success) + if not success: + self._log('warn', f'/elastic_band/toggle failed ({failure}): calling it again') + return + self._band_timer.cancel() + if self._pose_sub is not None: + self.destroy_subscription(self._pose_sub) + self._pose_sub = None + if response.message == BAND_ON: + # the band was already off (SPACE in the viewer), and the toggle hung the + # robot from it again. Another call would take it off again, under a + # robot someone may have hung up on purpose, so none is sent + self._log('warn', '/elastic_band/toggle answered "enabled": the elastic band was ' + 'already off and is now ON, and the robot hangs from it again. ' + 'The node does not call the toggle again') + return + self._log('info', f'elastic band released (/elastic_band/toggle: {response.message})') + self._driver.on_band_released() + + def destroy_node(self): + # stop publishing; golem_safety keeps relaying the last leg command and + # reports the quiet input + self._driver.shutdown() + return super().destroy_node() + + +def main(args=None): + parser = argparse.ArgumentParser(description="HOMIE's walking policy on the G1's legs") + parser.add_argument('--config', type=str, default='g1.yaml', + help='config name under config/ (g1.yaml or sim.yaml), or a path') + parsed_args, ros_args = parser.parse_known_args(args) + + rclpy.init(args=ros_args) + node = LocomotionNode(config_name=parsed_args.config) + 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/g1_homie/package.xml b/core_ws/src/g1_homie/package.xml new file mode 100644 index 0000000..29898a0 --- /dev/null +++ b/core_ws/src/g1_homie/package.xml @@ -0,0 +1,23 @@ + + + + g1_homie + 0.0.0 + HOMIE's walking policy on the Unitree G1's legs, through golem_safety's split mode + Niraj Pudasaini + MIT + + rclpy + geometry_msgs + std_msgs + std_srvs + golem_safety + python3-numpy + python3-yaml + + python3-pytest + + + ament_python + + diff --git a/core_ws/src/g1_homie/policy/LICENSE b/core_ws/src/g1_homie/policy/LICENSE new file mode 100644 index 0000000..f821082 --- /dev/null +++ b/core_ws/src/g1_homie/policy/LICENSE @@ -0,0 +1,81 @@ +This work is licensed under the Creative Commons Attribution-NonCommercial 4.0 International License (CC BY-NC 4.0). +You may not use the material for commercial purposes. Below is a copy of this license: + +By exercising the Licensed Rights (defined below), You accept and agree to be bound by the terms and conditions of this Creative Commons Attribution-NonCommercial 4.0 International Public License ("Public License"). To the extent this Public License may be interpreted as a contract, You are granted the Licensed Rights in consideration of Your acceptance of these terms and conditions, and the Licensor grants You such rights in consideration of benefits the Licensor receives from making the Licensed Material available under these terms and conditions. + +Section 1 – Definitions. +Adapted Material means material subject to Copyright and Similar Rights that is derived from or based upon the Licensed Material and in which the Licensed Material is translated, altered, arranged, transformed, or otherwise modified in a manner requiring permission under the Copyright and Similar Rights held by the Licensor. For purposes of this Public License, where the Licensed Material is a musical work, performance, or sound recording, Adapted Material is always produced where the Licensed Material is synched in timed relation with a moving image. +Adapter's License means the license You apply to Your Copyright and Similar Rights in Your contributions to Adapted Material in accordance with the terms and conditions of this Public License. +Copyright and Similar Rights means copyright and/or similar rights closely related to copyright including, without limitation, performance, broadcast, sound recording, and Sui Generis Database Rights, without regard to how the rights are labeled or categorized. For purposes of this Public License, the rights specified in Section 2(b)(1)-(2) are not Copyright and Similar Rights. +Effective Technological Measures means those measures that, in the absence of proper authority, may not be circumvented under laws fulfilling obligations under Article 11 of the WIPO Copyright Treaty adopted on December 20, 1996, and/or similar international agreements. +Exceptions and Limitations means fair use, fair dealing, and/or any other exception or limitation to Copyright and Similar Rights that applies to Your use of the Licensed Material. +Licensed Material means the artistic or literary work, database, or other material to which the Licensor applied this Public License. +Licensed Rights means the rights granted to You subject to the terms and conditions of this Public License, which are limited to all Copyright and Similar Rights that apply to Your use of the Licensed Material and that the Licensor has authority to license. +Licensor means the individual(s) or entity(ies) granting rights under this Public License. +NonCommercial means not primarily intended for or directed towards commercial advantage or monetary compensation. For purposes of this Public License, the exchange of the Licensed Material for other material subject to Copyright and Similar Rights by digital file-sharing or similar means is NonCommercial provided there is no payment of monetary compensation in connection with the exchange. +Share means to provide material to the public by any means or process that requires permission under the Licensed Rights, such as reproduction, public display, public performance, distribution, dissemination, communication, or importation, and to make material available to the public including in ways that members of the public may access the material from a place and at a time individually chosen by them. +Sui Generis Database Rights means rights other than copyright resulting from Directive 96/9/EC of the European Parliament and of the Council of 11 March 1996 on the legal protection of databases, as amended and/or succeeded, as well as other essentially equivalent rights anywhere in the world. +You means the individual or entity exercising the Licensed Rights under this Public License. Your has a corresponding meaning. +Section 2 – Scope. +License grant . +Subject to the terms and conditions of this Public License, the Licensor hereby grants You a worldwide, royalty-free, non-sublicensable, non-exclusive, irrevocable license to exercise the Licensed Rights in the Licensed Material to: +reproduce and Share the Licensed Material, in whole or in part, for NonCommercial purposes only; and +produce, reproduce, and Share Adapted Material for NonCommercial purposes only. +Exceptions and Limitations . For the avoidance of doubt, where Exceptions and Limitations apply to Your use, this Public License does not apply, and You do not need to comply with its terms and conditions. +Term . The term of this Public License is specified in Section 6(a) . +Media and formats; technical modifications allowed . The Licensor authorizes You to exercise the Licensed Rights in all media and formats whether now known or hereafter created, and to make technical modifications necessary to do so. The Licensor waives and/or agrees not to assert any right or authority to forbid You from making technical modifications necessary to exercise the Licensed Rights, including technical modifications necessary to circumvent Effective Technological Measures. For purposes of this Public License, simply making modifications authorized by this Section 2(a)(4) never produces Adapted Material. +Downstream recipients . +Offer from the Licensor – Licensed Material . Every recipient of the Licensed Material automatically receives an offer from the Licensor to exercise the Licensed Rights under the terms and conditions of this Public License. +No downstream restrictions . You may not offer or impose any additional or different terms or conditions on, or apply any Effective Technological Measures to, the Licensed Material if doing so restricts exercise of the Licensed Rights by any recipient of the Licensed Material. +No endorsement . Nothing in this Public License constitutes or may be construed as permission to assert or imply that You are, or that Your use of the Licensed Material is, connected with, or sponsored, endorsed, or granted official status by, the Licensor or others designated to receive attribution as provided in Section 3(a)(1)(A)(i) . +Other rights . +Moral rights, such as the right of integrity, are not licensed under this Public License, nor are publicity, privacy, and/or other similar personality rights; however, to the extent possible, the Licensor waives and/or agrees not to assert any such rights held by the Licensor to the limited extent necessary to allow You to exercise the Licensed Rights, but not otherwise. +Patent and trademark rights are not licensed under this Public License. +To the extent possible, the Licensor waives any right to collect royalties from You for the exercise of the Licensed Rights, whether directly or through a collecting society under any voluntary or waivable statutory or compulsory licensing scheme. In all other cases the Licensor expressly reserves any right to collect such royalties, including when the Licensed Material is used other than for NonCommercial purposes. +Section 3 – License Conditions. +Your exercise of the Licensed Rights is expressly made subject to the following conditions. + +Attribution . +If You Share the Licensed Material (including in modified form), You must: + +retain the following if it is supplied by the Licensor with the Licensed Material: +identification of the creator(s) of the Licensed Material and any others designated to receive attribution, in any reasonable manner requested by the Licensor (including by pseudonym if designated); +a copyright notice; +a notice that refers to this Public License; +a notice that refers to the disclaimer of warranties; +a URI or hyperlink to the Licensed Material to the extent reasonably practicable; +indicate if You modified the Licensed Material and retain an indication of any previous modifications; and +indicate the Licensed Material is licensed under this Public License, and include the text of, or the URI or hyperlink to, this Public License. +You may satisfy the conditions in Section 3(a)(1) in any reasonable manner based on the medium, means, and context in which You Share the Licensed Material. For example, it may be reasonable to satisfy the conditions by providing a URI or hyperlink to a resource that includes the required information. +If requested by the Licensor, You must remove any of the information required by Section 3(a)(1)(A) to the extent reasonably practicable. +If You Share Adapted Material You produce, the Adapter's License You apply must not prevent recipients of the Adapted Material from complying with this Public License. +Section 4 – Sui Generis Database Rights. +Where the Licensed Rights include Sui Generis Database Rights that apply to Your use of the Licensed Material: + +for the avoidance of doubt, Section 2(a)(1) grants You the right to extract, reuse, reproduce, and Share all or a substantial portion of the contents of the database for NonCommercial purposes only; +if You include all or a substantial portion of the database contents in a database in which You have Sui Generis Database Rights, then the database in which You have Sui Generis Database Rights (but not its individual contents) is Adapted Material; and +You must comply with the conditions in Section 3(a) if You Share all or a substantial portion of the contents of the database. +For the avoidance of doubt, this Section 4 supplements and does not replace Your obligations under this Public License where the Licensed Rights include other Copyright and Similar Rights. + +Section 5 – Disclaimer of Warranties and Limitation of Liability. +Unless otherwise separately undertaken by the Licensor, to the extent possible, the Licensor offers the Licensed Material as-is and as-available, and makes no representations or warranties of any kind concerning the Licensed Material, whether express, implied, statutory, or other. This includes, without limitation, warranties of title, merchantability, fitness for a particular purpose, non-infringement, absence of latent or other defects, accuracy, or the presence or absence of errors, whether or not known or discoverable. Where disclaimers of warranties are not allowed in full or in part, this disclaimer may not apply to You. +To the extent possible, in no event will the Licensor be liable to You on any legal theory (including, without limitation, negligence) or otherwise for any direct, special, indirect, incidental, consequential, punitive, exemplary, or other losses, costs, expenses, or damages arising out of this Public License or use of the Licensed Material, even if the Licensor has been advised of the possibility of such losses, costs, expenses, or damages. Where a limitation of liability is not allowed in full or in part, this limitation may not apply to You. +The disclaimer of warranties and limitation of liability provided above shall be interpreted in a manner that, to the extent possible, most closely approximates an absolute disclaimer and waiver of all liability. +Section 6 – Term and Termination. +This Public License applies for the term of the Copyright and Similar Rights licensed here. However, if You fail to comply with this Public License, then Your rights under this Public License terminate automatically. +Where Your right to use the Licensed Material has terminated under Section 6(a), it reinstates: + +automatically as of the date the violation is cured, provided it is cured within 30 days of Your discovery of the violation; or +upon express reinstatement by the Licensor. +For the avoidance of doubt, this Section 6(b) does not affect any right the Licensor may have to seek remedies for Your violations of this Public License. + +For the avoidance of doubt, the Licensor may also offer the Licensed Material under separate terms or conditions or stop distributing the Licensed Material at any time; however, doing so will not terminate this Public License. +Sections 1 , 5 , 6 , 7 , and 8 survive termination of this Public License. +Section 7 – Other Terms and Conditions. +The Licensor shall not be bound by any additional or different terms or conditions communicated by You unless expressly agreed. +Any arrangements, understandings, or agreements regarding the Licensed Material not stated herein are separate from and independent of the terms and conditions of this Public License. +Section 8 – Interpretation. +For the avoidance of doubt, this Public License does not, and shall not be interpreted to, reduce, limit, restrict, or impose conditions on any use of the Licensed Material that could lawfully be made without permission under this Public License. +To the extent possible, if any provision of this Public License is deemed unenforceable, it shall be automatically reformed to the minimum extent necessary to make it enforceable. If the provision cannot be reformed, it shall be severed from this Public License without affecting the enforceability of the remaining terms and conditions. +No term or condition of this Public License will be waived and no failure to comply consented to unless expressly agreed to by the Licensor. +Nothing in this Public License constitutes or may be interpreted as a limitation upon, or waiver of, any privileges and immunities that apply to the Licensor or You, including from the legal processes of any jurisdiction or authority. \ No newline at end of file diff --git a/core_ws/src/g1_homie/policy/README.md b/core_ws/src/g1_homie/policy/README.md new file mode 100644 index 0000000..c98fc44 --- /dev/null +++ b/core_ws/src/g1_homie/policy/README.md @@ -0,0 +1,180 @@ +# g1_homie/policy: HOMIE's walking policy + +The walking policy for GOLEM's Unitree G1. It holds HOMIE's pretrained +locomotion policy and a MuJoCo script that runs it in simulation (sim2sim). +The `g1_homie` package around it runs it on the G1's legs +(`locomotion_node`); the G1 sim reads none of its files. The `ros` container +mounts this folder at `/home/code/g1_homie`. + +The policy and the script come from [OpenHomie](https://github.com/InternRobotics/OpenHomie); +see [Credits](#credits). We kept only its MuJoCo deployment. The training code +(HomieRL), the real-robot deployment (HomieDeploy) and the teleoperation +hardware (HomieHardware) were removed. The robot model is the one in the +repo's `CL_Assets/g1`, so this folder has no model files of its own. + +## Status + +- **Walks in MuJoCo.** Tracking over 10 s, measured after the first 3 s: + + | Command | Result | + |---|---| + | stand | stays at 0.744 m | + | forward 0.5 m/s | 0.46 m/s | + | forward 1.0 m/s | 0.98 m/s | + | backward 0.5 m/s | 0.48 m/s | + | sideways 0.3 m/s | 0.23 m/s, drifting 0.17 rad/s in yaw | + | turn 0.5 rad/s | 0.46 rad/s | + | squat to 0.40 m | pelvis at 0.34 m | + +- **Not on the real robot yet.** It will run on the laptop and send the legs' + commands through `golem_safety`'s `split` mode. + +## Contents + +| Path | What it is | +|---|---| +| `deploy.onnx` | HOMIE's pretrained G1 policy (the example checkpoint from HomieDeploy) | +| `homie_policy.py` | The policy's observation, history and action, shared by everything that runs it | +| `mujoco_deploy_g1.py` | MuJoCo sim2sim loop: PD control at 500 Hz, policy at 50 Hz | +| `g1.yaml` | Model and policy paths, joints, PD gains, observation scales, commands and their trained ranges | +| `test_homie_policy.py` | Tests for `homie_policy.py`; they need no simulator | +| `LICENSE` | OpenHomie's license | + +## Run it + +It uses the `g1` conda env, the Python 3.10 env with the Unitree SDK. The first +time, install the extra packages: + +```bash +conda activate g1 +pip install onnxruntime mujoco==3.2.3 pyyaml +``` + +Then, from this folder: + +```bash +python mujoco_deploy_g1.py # viewer, standing +python mujoco_deploy_g1.py --cmd 0.5 0 0 # walk forward at 0.5 m/s +python mujoco_deploy_g1.py --cmd 0 0 0.5 --height 0.7 # turn at 0.5 rad/s, crouched a little +python mujoco_deploy_g1.py --headless --duration 10 --cmd 0.5 0 0 +``` + +- `--cmd VX VY YAW` sets the velocity command in m/s and rad/s. Training covered + vx from -0.8 to 1.2, vy from -0.5 to 0.5, and yaw from -0.8 to 0.8. +- `--height` sets the pelvis height target, from 0.24 m to 0.74 m. The default, + 0.74 m, is standing. Turning keeps its speed down to 0.70 m and slows below + that. For a 0.5 rad/s turn it measured 0.46 rad/s at 0.74 m, 0.49 at 0.70 m, + 0.35 at 0.65 m and 0.01 at 0.60 m (over 10 s, after the first 3 s). +- `--headless` prints the pelvis position once a second, and stops if the robot + falls. + +## The policy + +- **Output:** 12 leg joint targets, applied as `default_angles + 0.25 * action`. +- **Input:** the last 6 steps of a 76-value observation (456 values). Each step + holds: + - the velocity command; + - the height command; + - the pelvis angular velocity; + - gravity in the pelvis frame; + - the positions and velocities of 27 joints; + - the previous action. +- **The 27 joints:** the G1's 29 body joints minus waist roll and waist pitch. + That is 12 legs, waist yaw, and 14 arms. They are listed in `g1.yaml`. +- **IMU:** the policy was trained on the pelvis IMU (`imu_in_pelvis`). That IMU + has no rotation relative to the pelvis, so the script reads the pelvis state. + +## The policy module + +`homie_policy.py` holds the policy's observation, history and action code, so +every program that runs the policy shares one copy. It needs only numpy, +onnxruntime and pyyaml, and works with numpy 1.26 and 2.x. + +```python +from homie_policy import HomiePolicy + +policy = HomiePolicy() # loads g1.yaml and deploy.onnx +kp, kd = policy.gains(joint_names) # PD gains, matched by substring +cmd, height = policy.clip_command(cmd, height) # to the trained ranges +targets = policy.step(cmd, height, gyro, quat_wxyz, q, dq) # at 50 Hz +policy.reset() # before starting again +``` + +- `q` and `dq` are the positions and velocities of `policy.policy_joints`, in + that order. `gyro` is the pelvis angular velocity in the pelvis frame. + `quat_wxyz` is the pelvis orientation with w first, the order of MuJoCo's + `qpos` and Unitree's `imu_state.quaternion`. +- `targets` are position targets for `policy.leg_joints`, to track with the + gains from `policy.gains()`. +- `reset()` zeros the observation history and the last action. +- `step()` raises ValueError if the observation has the wrong size or a value + that is not finite, such as a NaN in `dq`. It checks before it changes the + history or the last action, so later steps are unaffected. +- The onnxruntime session (`policy_session()`) runs each step on the calling + thread: one intra-op and one inter-op thread, no spinning, nothing pinned + to a CPU. onnxruntime's default instead pins a pool of threads as wide as + the machine, the same CPUs in every process, and two of them spin at 100% + between 50 Hz steps. With two processes running the policy that way on one + host, the second one's threads on those two CPUs got no CPU time and it + never finished loading. The actions are identical. +- `clip_command()` clips to `cmd_range` and `height_range` in `g1.yaml`, and + rejects values that are not finite. `mujoco_deploy_g1.py` does not clip, so + the sim can still try commands outside the trained ranges. +- Run the tests with `python test_homie_policy.py`, or with pytest. + +## Changes from HOMIE's MuJoCo script + +All values now come from HOMIE's training config (HomieRL's +`g1_29dof_config.py` and `legged_robot_config.py`). + +- **Scales:** + - Angular velocity: 0.5 instead of 0.25. + - Yaw command: 0.5 instead of 0.25. + + With HOMIE's values the robot fell when walking backward and did not turn. +- **Height command:** 0.74 m (standing) instead of 0.34 m, which is a deep squat. +- **Model:** Unitree's unmodified G1 in `CL_Assets/g1` (`g1_29dof_with_hand_rev_1_0`, + the variant our robot reports). HOMIE used its own copy, which differed in two + ways: + - It locked waist roll, waist pitch and the hands. Here those joints are + actuated and held at 0 with the training gains. + - It gave every joint a small damping, armature and friction loss. `g1.yaml` + sets the same values in code. +- **Gains:** the waist and arms use the training gains, instead of the single + kp 100 / kd 0.5 of HOMIE's script. +- **Joints:** looked up by name instead of by position in the model. +- **Policy:** loaded with onnxruntime. TorchScript and `legged_gym` are no longer + needed. + +## Credits + +- **HOMIE** by Qingwei Ben, Feiyu Jia, Jia Zeng, Junting Dong, Dahua Lin and + Jiangmiao Pang (Shanghai AI Laboratory and The Chinese University of Hong + Kong). The policy, the MuJoCo script and its config come from + [OpenHomie](https://github.com/InternRobotics/OpenHomie). + [Paper](https://arxiv.org/abs/2502.13013), + [project page](https://homietele.github.io/). + + ```bibtex + @article{ben2025homie, + title={HOMIE: Humanoid Loco-Manipulation with Isomorphic Exoskeleton Cockpit}, + author={Ben, Qingwei and Jia, Feiyu and Zeng, Jia and Dong, Junting and Lin, Dahua and Pang, Jiangmiao}, + journal={arXiv preprint arXiv:2502.13013}, + year={2025} + } + ``` + +- The MuJoCo script builds on Unitree's + [unitree_rl_gym](https://github.com/unitreerobotics/unitree_rl_gym). +- The G1 model is Unitree's, from + [unitree_ros](https://github.com/unitreerobotics/unitree_ros) (see + `CL_Assets/g1/README.md`). + +## License + +The code is for non-commercial use only. OpenHomie's `LICENSE` file is CC BY-NC +4.0, but its README says CC BY-NC-SA 4.0, which also requires changed versions +to be shared under the same license. Treat it as the stricter CC BY-NC-SA 4.0. +This license is different from the MIT license of GOLEM's G1 packages +(`dex_3`, `g1_homie`), including the `g1_homie` code around +this folder. diff --git a/core_ws/src/g1_homie/policy/deploy.onnx b/core_ws/src/g1_homie/policy/deploy.onnx new file mode 100644 index 0000000..363b462 Binary files /dev/null and b/core_ws/src/g1_homie/policy/deploy.onnx differ diff --git a/core_ws/src/g1_homie/policy/g1.yaml b/core_ws/src/g1_homie/policy/g1.yaml new file mode 100644 index 0000000..9079f8a --- /dev/null +++ b/core_ws/src/g1_homie/policy/g1.yaml @@ -0,0 +1,61 @@ +# HOMIE's G1 walking policy in MuJoCo. Values follow HOMIE's training config +# (HomieRL g1_29dof_config.py and legged_robot_config.py) unless noted. +# Paths are relative to this file. + +policy_path: deploy.onnx +xml_path: ../CL_Assets/g1/g1_description/g1_29dof_with_hand_rev_1_0.xml + +# Simulation parameters +simulation_duration: 60.0 +simulation_dt: 0.002 +control_decimation: 10 # policy at 50 Hz, as in training (dt 0.005 x decimation 4) + +# The 27 joints the policy observes, in its order: the 29 body joints minus +# waist roll and waist pitch. The first num_actions (the legs) are its outputs. +policy_joints: [ + left_hip_pitch_joint, left_hip_roll_joint, left_hip_yaw_joint, + left_knee_joint, left_ankle_pitch_joint, left_ankle_roll_joint, + right_hip_pitch_joint, right_hip_roll_joint, right_hip_yaw_joint, + right_knee_joint, right_ankle_pitch_joint, right_ankle_roll_joint, + waist_yaw_joint, + 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, +] +num_actions: 12 +obs_history_len: 6 + +# Default joint angles for legs; every other joint is held at 0 +default_angles: [-0.1, 0.0, 0.0, 0.3, -0.2, 0.0, + -0.1, 0.0, 0.0, 0.3, -0.2, 0.0] + +# PD gains, matched to joint names by substring as in training. Waist roll, +# waist pitch and the hands were locked in HOMIE's sim; here they are actuated +# and held at 0 (the hands with the config's otherwise unused hand gains). +stiffness: {hip_yaw: 100, hip_roll: 100, hip_pitch: 100, knee: 150, ankle: 40, + waist: 300, shoulder: 200, elbow: 100, wrist: 20, hand: 10} +damping: {hip_yaw: 2, hip_roll: 2, hip_pitch: 2, knee: 4, ankle: 2, + waist: 5, shoulder: 4, elbow: 1, wrist: 0.5, hand: 2} + +# HOMIE's MJCF gives every joint these; Unitree's model in CL_Assets/g1 has none +joint_damping: 0.001 +joint_armature: 0.01 +joint_frictionloss: 0.1 + +# Scaling factors +ang_vel_scale: 0.5 # HOMIE's MuJoCo script had 0.25 +dof_pos_scale: 1.0 +dof_vel_scale: 0.05 +action_scale: 0.25 +cmd_scale: [2.0, 2.0, 0.5] # HOMIE's deploy scripts had 0.25 for yaw +clip_actions: 100.0 + +# Initial commands: vx, vy (m/s), yaw rate (rad/s) +cmd_init: [0.0, 0.0, 0.0] +# Pelvis height target (m); 0.74 is standing. HOMIE's MuJoCo config had 0.34, +# a deep squat. +height_cmd: 0.74 +# The ranges HOMIE was trained on, as [min, max]; clip_command() clips to them +cmd_range: [[-0.8, 1.2], [-0.5, 0.5], [-0.8, 0.8]] # vx, vy, yaw +height_range: [0.24, 0.74] diff --git a/core_ws/src/g1_homie/policy/homie_policy.py b/core_ws/src/g1_homie/policy/homie_policy.py new file mode 100644 index 0000000..d2666f3 --- /dev/null +++ b/core_ws/src/g1_homie/policy/homie_policy.py @@ -0,0 +1,190 @@ +"""HOMIE's G1 walking policy: its observation, history and action. + + from homie_policy import HomiePolicy + + policy = HomiePolicy() # g1.yaml next to this file + kp, kd = policy.gains(joint_names) # PD gains for any joints + cmd, height = policy.clip_command(cmd, height) # to the trained ranges + targets = policy.step(cmd, height, gyro, quat_wxyz, q, dq) # at 50 Hz + +q and dq are the positions and velocities of policy.policy_joints, in that +order; the targets are for policy.leg_joints. Quaternions are w, x, y, z. It +needs only numpy, onnxruntime and pyyaml, so simulators and ROS nodes can +share it. +""" +import collections +import os + +import numpy as np +import onnxruntime as ort +import yaml + +HERE = os.path.dirname(os.path.abspath(__file__)) + + +def load_config(config_path): + """Load the YAML config; paths in it are relative to the file. + + In the containers this directory is /home/code/g1_homie, beside + /home/code/CL_Assets; in the repo it is core_ws/src/g1_homie/policy, + so a path that is not there is looked up from the repo's root as well. + """ + with open(config_path, 'r') as f: + config = yaml.safe_load(f) + + base = os.path.dirname(os.path.abspath(config_path)) + for path_key in ['policy_path', 'xml_path']: + path = os.path.normpath(os.path.join(base, config[path_key])) + in_repo = os.path.normpath(os.path.join(base, '..', '..', '..', '..', config[path_key])) + config[path_key] = path if os.path.exists(path) or not os.path.exists(in_repo) else in_repo + + array_keys = ['default_angles', 'cmd_scale', 'cmd_init'] + for key in array_keys: + config[key] = np.array(config[key], dtype=np.float32) + + return config + + +def quat_rotate_inverse(q, v): + """Rotate vector v by the inverse of quaternion q""" + w = q[..., 0] + x = q[..., 1] + y = q[..., 2] + z = q[..., 3] + + q_conj = np.array([w, -x, -y, -z]) + + return np.array([ + v[0] * (q_conj[0]**2 + q_conj[1]**2 - q_conj[2]**2 - q_conj[3]**2) + + v[1] * 2 * (q_conj[1] * q_conj[2] - q_conj[0] * q_conj[3]) + + v[2] * 2 * (q_conj[1] * q_conj[3] + q_conj[0] * q_conj[2]), + + v[0] * 2 * (q_conj[1] * q_conj[2] + q_conj[0] * q_conj[3]) + + v[1] * (q_conj[0]**2 - q_conj[1]**2 + q_conj[2]**2 - q_conj[3]**2) + + v[2] * 2 * (q_conj[2] * q_conj[3] - q_conj[0] * q_conj[1]), + + v[0] * 2 * (q_conj[1] * q_conj[3] - q_conj[0] * q_conj[2]) + + v[1] * 2 * (q_conj[2] * q_conj[3] + q_conj[0] * q_conj[1]) + + v[2] * (q_conj[0]**2 - q_conj[1]**2 - q_conj[2]**2 + q_conj[3]**2) + ]) + + +def get_gravity_orientation(quat): + """Get gravity vector in body frame""" + gravity_vec = np.array([0.0, 0.0, -1.0]) + return quat_rotate_inverse(quat, gravity_vec) + + +def policy_session(policy_path): + """The policy's onnxruntime session: one thread, no spinning, no pinning + + onnxruntime's default session makes a thread pool as wide as the machine + (23 threads on a 24-core laptop), pins each thread to one CPU, the same + CPUs in every process, and lets them spin between runs. At 50 Hz two of + them each kept a CPU 100% busy (CPUs 1 and 8 on the laptop) in every + process that ran the policy. With two processes running it on one host, + the second one's pool threads on those two CPUs got no CPU time for + minutes, and it never finished loading. The policy is a + small MLP: on the calling thread a step takes about 0.2 ms, with the same + actions. With one intra-op thread there is no pool, so nothing is pinned + or spins; the spinning entries also cover a pool if a count changes. + """ + options = ort.SessionOptions() + options.intra_op_num_threads = 1 + options.inter_op_num_threads = 1 + options.execution_mode = ort.ExecutionMode.ORT_SEQUENTIAL + options.add_session_config_entry('session.intra_op.allow_spinning', '0') + options.add_session_config_entry('session.inter_op.allow_spinning', '0') + return ort.InferenceSession(policy_path, sess_options=options, providers=['CPUExecutionProvider']) + + +def joint_gains(name, gains): + """Gain for a joint, matched by substring as in legged_gym""" + for key, value in gains.items(): + if key in name: + return value + raise KeyError(f'no gain matches joint {name}') + + +class HomiePolicy: + """The ONNX policy with its observation history and last action""" + + def __init__(self, config_path=os.path.join(HERE, 'g1.yaml')): + self.config = config = load_config(config_path) + self.policy_joints = config['policy_joints'] + self.leg_joints = self.policy_joints[:config['num_actions']] + self.default_angles = config['default_angles'] + # default positions of the policy joints: the legs' default angles, 0 for the rest + self.default_q = np.zeros(len(self.policy_joints), dtype=np.float32) + self.default_q[:len(self.leg_joints)] = self.default_angles + + self.session = policy_session(config['policy_path']) + policy_input = self.session.get_inputs()[0] + self.input_name = policy_input.name + # cmd, height, angular velocity, gravity, joint positions and velocities, last action + self.num_obs = 3 + 1 + 3 + 3 + 2 * len(self.policy_joints) + len(self.leg_joints) + num_inputs = self.num_obs * config['obs_history_len'] + if policy_input.shape[-1] != num_inputs: + raise ValueError(f'policy takes {policy_input.shape[-1]} inputs, config gives {num_inputs}') + self.reset() + + def gains(self, names): + """kp and kd for each joint name, matched by substring as in training""" + kp = np.array([joint_gains(n, self.config['stiffness']) for n in names], dtype=np.float64) + kd = np.array([joint_gains(n, self.config['damping']) for n in names], dtype=np.float64) + return kp, kd + + def clip_command(self, cmd, height): + """The command and height target clipped to the trained ranges; ValueError if not finite""" + cmd = np.asarray(cmd, dtype=np.float32) + if not (np.all(np.isfinite(cmd)) and np.isfinite(height)): + raise ValueError(f'command is not finite: {cmd}, height {height}') + low, high = np.array(self.config['cmd_range'], dtype=np.float32).T + low_height, high_height = self.config['height_range'] + return np.clip(cmd, low, high), min(max(float(height), low_height), high_height) + + def reset(self): + """Zero the observation history and the last action""" + history_len = self.config['obs_history_len'] + self.action = np.zeros(len(self.leg_joints), dtype=np.float32) + self.history = collections.deque( + [np.zeros(self.num_obs, dtype=np.float32)] * history_len, maxlen=history_len) + + def observation(self, cmd, height, gyro, quat_wxyz, q, dq): + """One step of the policy's observation, as in HOMIE's training env""" + # the state in float64, as MuJoCo gives it, whatever dtype the caller passes; + # the command in float32, like cmd_scale + config = self.config + return np.concatenate([ + np.asarray(cmd, dtype=np.float32) * config['cmd_scale'], + [height], + np.asarray(gyro, dtype=np.float64) * config['ang_vel_scale'], + get_gravity_orientation(np.asarray(quat_wxyz, dtype=np.float64)), + (np.asarray(q, dtype=np.float64) - self.default_q) * config['dof_pos_scale'], + np.asarray(dq, dtype=np.float64) * config['dof_vel_scale'], + self.action, + ]).astype(np.float32) + + def step(self, cmd, height, gyro, quat_wxyz, q, dq): + """Run the policy on the current state; returns the leg position targets + + cmd is vx, vy (m/s) and the yaw rate (rad/s), height the pelvis height + target (m), gyro the pelvis angular velocity in the pelvis frame (rad/s), + quat_wxyz the pelvis orientation, and q, dq the positions and velocities + of policy_joints. Call it at 50 Hz, the rate the policy was trained at. + If the observation has the wrong size or a value that is not finite, it + raises ValueError and leaves the history and the last action unchanged. + """ + # check before changing any state: one NaN kept in the history would + # reach every later output through the last action + current = self.observation(cmd, height, gyro, quat_wxyz, q, dq) + if current.shape != (self.num_obs,): + raise ValueError(f'observation has {current.size} values, expected {self.num_obs}') + not_finite = np.flatnonzero(~np.isfinite(current)).tolist() + if not_finite: + raise ValueError(f'observation is not finite at {not_finite}') + self.history.append(current) + obs = np.concatenate(self.history)[None, :] + action = self.session.run(None, {self.input_name: obs})[0][0] + self.action = np.clip(action, -self.config['clip_actions'], self.config['clip_actions']) + return self.action * self.config['action_scale'] + self.default_angles diff --git a/core_ws/src/g1_homie/policy/mujoco_deploy_g1.py b/core_ws/src/g1_homie/policy/mujoco_deploy_g1.py new file mode 100644 index 0000000..16e8c13 --- /dev/null +++ b/core_ws/src/g1_homie/policy/mujoco_deploy_g1.py @@ -0,0 +1,127 @@ +"""HOMIE's G1 walking policy in MuJoCo (sim2sim). + + python mujoco_deploy_g1.py # viewer, standing + python mujoco_deploy_g1.py --cmd 0.5 0 0 # walk forward at 0.5 m/s + python mujoco_deploy_g1.py --headless --duration 10 --cmd 0.5 0 0 + +Adapted from OpenHomie's MujocoDeploy/mujoco_deploy_g1.py. It runs the ONNX +policy on Unitree's G1 model in CL_Assets/g1; joints are looked up by name. +""" +import argparse +import os +import time + +import mujoco +import mujoco.viewer +import numpy as np + +from homie_policy import HomiePolicy, get_gravity_orientation + +HERE = os.path.dirname(os.path.abspath(__file__)) + + +def pd_control(target_q, q, kp, target_dq, dq, kd): + """Calculates torques from position commands""" + return (target_q - q) * kp + (target_dq - dq) * kd + + +class Robot: + """Name-based indices into the MuJoCo model""" + + def __init__(self, m, policy): + if m.jnt_type[0] != mujoco.mjtJoint.mjJNT_FREE or m.jnt_qposadr[0] != 0: + raise ValueError('expected the floating base as the first joint') + + # every actuator is a motor on one hinge joint + joint_ids = m.actuator_trnid[:, 0] + self.names = [mujoco.mj_id2name(m, mujoco.mjtObj.mjOBJ_JOINT, j) for j in joint_ids] + self.qadr = m.jnt_qposadr[joint_ids] + self.vadr = m.jnt_dofadr[joint_ids] + self.kp, self.kd = policy.gains(self.names) + + # policy joints as actuator indices + index = {n: i for i, n in enumerate(self.names)} + missing = [n for n in policy.policy_joints if n not in index] + if missing: + raise KeyError(f'policy joints not actuated in the model: {missing}') + self.policy = np.array([index[n] for n in policy.policy_joints]) + self.legs = self.policy[:len(policy.leg_joints)] + + +def main(): + parser = argparse.ArgumentParser(description=__doc__, formatter_class=argparse.RawDescriptionHelpFormatter) + parser.add_argument('--config', default=os.path.join(HERE, 'g1.yaml')) + parser.add_argument('--cmd', type=float, nargs=3, metavar=('VX', 'VY', 'YAW'), + help='velocity command (default: cmd_init)') + parser.add_argument('--height', type=float, help='pelvis height command (default: height_cmd)') + parser.add_argument('--duration', type=float, help='seconds (default: simulation_duration)') + parser.add_argument('--headless', action='store_true', help='no viewer; run as fast as possible') + args = parser.parse_args() + + # Load configuration and policy + policy = HomiePolicy(args.config) + config = policy.config + cmd = np.array(args.cmd, dtype=np.float32) if args.cmd else config['cmd_init'].copy() + height_cmd = config['height_cmd'] if args.height is None else args.height + duration = config['simulation_duration'] if args.duration is None else args.duration + + # Load robot model + m = mujoco.MjModel.from_xml_path(config['xml_path']) + d = mujoco.MjData(m) + m.opt.timestep = config['simulation_dt'] + m.dof_damping[:] = config['joint_damping'] + m.dof_armature[:] = config['joint_armature'] + m.dof_frictionloss[:] = config['joint_frictionloss'] + robot = Robot(m, policy) + + # Initialize variables + target_q = np.zeros(m.nu) + target_q[robot.legs] = policy.default_angles + + def step(): + """One physics step; the policy runs every control_decimation steps""" + d.ctrl[:] = pd_control(target_q, d.qpos[robot.qadr], robot.kp, + 0.0, d.qvel[robot.vadr], robot.kd) + mujoco.mj_step(m, d) + + if counter % config['control_decimation'] == 0: + # imu_in_pelvis has no rotation relative to the pelvis, the floating base + target_q[robot.legs] = policy.step(cmd, height_cmd, d.qvel[3:6], d.qpos[3:7], + d.qpos[robot.qadr[robot.policy]], + d.qvel[robot.vadr[robot.policy]]) + + print(f'cmd vx {cmd[0]:.2f} vy {cmd[1]:.2f} yaw {cmd[2]:.2f}, height {height_cmd:.2f}, {duration:.0f} s') + counter = 0 + if args.headless: + report_every = int(round(1.0 / m.opt.timestep)) + while d.time < duration: + counter += 1 + step() + if counter % report_every == 0: + x, y, z = d.qpos[0:3] + print(f't {d.time:5.1f} pelvis x {x:6.2f} y {y:6.2f} z {z:5.3f}') + # fallen: the pelvis tilted more than 60 deg from upright + if get_gravity_orientation(d.qpos[3:7])[2] > -0.5: + print('fell') + return 1 + return 0 + + with mujoco.viewer.launch_passive(m, d) as viewer: + start = time.time() + while viewer.is_running() and time.time() - start < duration: + step_start = time.time() + counter += 1 + step() + + # Sync viewer + viewer.sync() + + # Time keeping + time_until_next_step = m.opt.timestep - (time.time() - step_start) + if time_until_next_step > 0: + time.sleep(time_until_next_step) + return 0 + + +if __name__ == '__main__': + raise SystemExit(main()) diff --git a/core_ws/src/g1_homie/policy/test_homie_policy.py b/core_ws/src/g1_homie/policy/test_homie_policy.py new file mode 100644 index 0000000..8cd91f5 --- /dev/null +++ b/core_ws/src/g1_homie/policy/test_homie_policy.py @@ -0,0 +1,201 @@ +"""Tests for homie_policy.py; no simulator needed. + + python test_homie_policy.py + python -m pytest test_homie_policy.py +""" +import math +import os +import tempfile + +import numpy as np +import yaml + +from homie_policy import HERE, HomiePolicy + +# the legs' default angles in g1.yaml +DEFAULT_ANGLES = [-0.1, 0.0, 0.0, 0.3, -0.2, 0.0] * 2 +STAND = dict(cmd=[0.0, 0.0, 0.0], height=0.74, gyro=[0.0, 0.0, 0.0], quat_wxyz=[1.0, 0.0, 0.0, 0.0], + q=DEFAULT_ANGLES + [0.0] * 15, dq=[0.0] * 27) + + +class FakeSession: + """Stands in for the ONNX session: records each input and returns the given actions""" + + def __init__(self, actions): + self.actions = list(actions) + self.inputs = [] + + def run(self, output_names, feed): + (x,) = feed.values() + self.inputs.append(x.copy()) + return [np.array([self.actions.pop(0)], dtype=np.float32)] + + +def raises(error, f, *args, **kwargs): + """The error f(*args, **kwargs) raises, or None""" + try: + f(*args, **kwargs) + except error as e: + return e + return None + + +def test_observation_layout(): + policy = HomiePolicy() + i = np.arange(1, 28) + q = np.concatenate([DEFAULT_ANGLES, np.zeros(15)]) + 0.01 * i + # pitched 30 deg nose down: gravity is (sin 30, 0, -cos 30) in the pelvis frame + quat = [math.cos(math.radians(15)), 0.0, math.sin(math.radians(15)), 0.0] + obs = policy.observation([0.5, -0.25, 0.4], 0.6, [0.2, -0.4, 1.0], quat, q, 1.0 * i) + expected = np.concatenate([ + [1.0, -0.5, 0.2], # cmd * (2, 2, 0.5) + [0.6], # height, unscaled + [0.1, -0.2, 0.5], # angular velocity * 0.5 + [0.5, 0.0, -math.sqrt(3) / 2], # gravity + 0.01 * i, # (q - default) * 1.0; waist and arms default to 0 + 0.05 * i, # dq * 0.05 + np.zeros(12), # last action: none yet + ]) + assert obs.dtype == np.float32 and obs.shape == (76,) + np.testing.assert_allclose(obs, expected, rtol=0, atol=1e-6) + + +def test_history_order(): + policy = HomiePolicy() + policy.session = FakeSession([np.zeros(12)] * 9) + for k in range(1, 9): + policy.step(**dict(STAND, height=0.1 * k)) + inputs = policy.session.inputs + assert inputs[0].shape == (1, 456) and inputs[0].dtype == np.float32 + heights = [x.reshape(6, 76)[:, 3] for x in inputs] + # oldest first, zeros before the first step + assert not inputs[0][0, :5 * 76].any() + np.testing.assert_allclose(heights[0], [0, 0, 0, 0, 0, 0.1], atol=1e-6) + np.testing.assert_allclose(heights[2], [0, 0, 0, 0.1, 0.2, 0.3], atol=1e-6) + np.testing.assert_allclose(heights[7], [0.3, 0.4, 0.5, 0.6, 0.7, 0.8], atol=1e-6) + + # reset() starts over from zeros + policy.reset() + policy.step(**dict(STAND, height=0.5)) + assert not inputs[8][0, :5 * 76].any() + np.testing.assert_allclose(inputs[8].reshape(6, 76)[:, 3], [0, 0, 0, 0, 0, 0.5], atol=1e-6) + + +def test_action_to_target(): + policy = HomiePolicy() + action = [150.0, -150.0, 1.0, -1.0, 0.4, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.0] + clipped = [100.0, -100.0, 1.0, -1.0, 0.4, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.0] # clip_actions 100 + policy.session = FakeSession([action, action, action]) + targets = policy.step(**STAND) + # the targets are for the leg joints: default_angles + 0.25 * the clipped action + assert policy.leg_joints == policy.policy_joints[:12] and targets.shape == (12,) + expected = np.array(DEFAULT_ANGLES) + 0.25 * np.array(clipped) + np.testing.assert_allclose(targets, expected, rtol=0, atol=1e-6) + + # the clipped action is the next step's last action, until reset() + policy.step(**STAND) + np.testing.assert_allclose(policy.session.inputs[1][0, -12:], clipped, atol=1e-6) + policy.reset() + policy.step(**STAND) + assert not policy.session.inputs[2][0, -12:].any() + + +def test_policy_runs(): + policy = HomiePolicy() + targets = policy.step(**STAND) + assert targets.shape == (12,) and targets.dtype == np.float32 and np.isfinite(targets).all() + + +def test_bad_state_rejected(): + # the real network: a NaN let into the history would reach every later + # output through the last action + policy, reference = HomiePolicy(), HomiePolicy() + walk = dict(STAND, cmd=[0.5, 0.0, 0.0]) + for _ in range(3): + policy.step(**walk) + reference.step(**walk) + history, action = np.concatenate(policy.history), policy.action.copy() + cases = [ + (dict(walk, dq=[math.nan] + [0.0] * 26), 'observation is not finite at [37]'), + (dict(walk, height=math.inf), 'observation is not finite at [3]'), + (dict(walk, gyro=[0.0] * 4), 'observation has 77 values, expected 76'), + ] + for state, message in cases: + error = raises(ValueError, policy.step, **state) + assert str(error) == message, error + # the history and the last action are unchanged + np.testing.assert_array_equal(np.concatenate(policy.history), history) + np.testing.assert_array_equal(policy.action, action) + + # later steps, past the 6 in the history, are the same as if the bad states never came + for _ in range(8): + np.testing.assert_array_equal(policy.step(**walk), reference.step(**walk)) + + +def test_input_size_check(): + with open(os.path.join(HERE, 'g1.yaml')) as f: + config = yaml.safe_load(f) + config['policy_path'] = os.path.join(HERE, 'deploy.onnx') + # 5 steps of 76, or 6 steps of 74 with a joint missing + changes = [({'obs_history_len': 5}, 380), ({'policy_joints': config['policy_joints'][:-1]}, 444)] + for change, given in changes: + with tempfile.TemporaryDirectory() as tmp: + path = os.path.join(tmp, 'g1.yaml') + with open(path, 'w') as f: + yaml.safe_dump(dict(config, **change), f) + error = raises(ValueError, HomiePolicy, path) + assert str(error) == f'policy takes 456 inputs, config gives {given}', error + + +def test_session_one_thread(): + # no thread pool, so no thread pinned to a CPU or spinning (policy_session), + # and the same actions as onnxruntime's default session + import onnxruntime as ort + tasks = '/proc/self/task' + before = set(os.listdir(tasks)) if os.path.isdir(tasks) else None + policy = HomiePolicy() + if before is not None: + assert set(os.listdir(tasks)) <= before, 'the session started threads' + options = policy.session.get_session_options() + assert (options.intra_op_num_threads, options.inter_op_num_threads) == (1, 1) + assert options.execution_mode == ort.ExecutionMode.ORT_SEQUENTIAL + for key in ('session.intra_op.allow_spinning', 'session.inter_op.allow_spinning'): + assert options.get_session_config_entry(key) == '0', key + default = ort.InferenceSession(policy.config['policy_path'], providers=['CPUExecutionProvider']) + size = policy.session.get_inputs()[0].shape[-1] + for x in np.random.default_rng(0).normal(0.0, 1.0, (300, 1, size)).astype(np.float32): + np.testing.assert_array_equal(policy.session.run(None, {policy.input_name: x})[0], + default.run(None, {policy.input_name: x})[0]) + + +def test_clip_command(): + policy = HomiePolicy() + cases = [ + ([0.5, -0.2, 0.3], 0.5, [0.5, -0.2, 0.3], 0.5), # in range: unchanged + ([2.0, 1.0, 1.5], 0.9, [1.2, 0.5, 0.8], 0.74), # above + ([-2.0, -1.0, -1.5], 0.1, [-0.8, -0.5, -0.8], 0.24), # below + ] + for cmd, height, clipped, clipped_height in cases: + cmd, height = policy.clip_command(cmd, height) + np.testing.assert_allclose(cmd, clipped, rtol=0, atol=1e-6) + assert height == clipped_height + assert raises(ValueError, policy.clip_command, [math.nan, 0.0, 0.0], 0.74) + assert raises(ValueError, policy.clip_command, [0.0, 0.0, 0.0], math.inf) + + +def test_gains(): + policy = HomiePolicy() + kp, kd = policy.gains(['left_hip_roll_joint', 'right_knee_joint', 'left_ankle_pitch_joint', + 'waist_pitch_joint', 'right_shoulder_yaw_joint', 'left_elbow_joint', + 'right_wrist_roll_joint', 'left_hand_thumb_0_joint']) + np.testing.assert_array_equal(kp, [100, 150, 40, 300, 200, 100, 20, 10]) + np.testing.assert_array_equal(kd, [2, 4, 2, 5, 4, 1, 0.5, 2]) + assert raises(KeyError, policy.gains, ['head_joint']) + + +if __name__ == '__main__': + tests = [test for name, test in list(globals().items()) if name.startswith('test_')] + for test in tests: + test() + print('ok', test.__name__) + print(f'{len(tests)} passed') diff --git a/core_ws/src/h12_skills/resource/h12_skills b/core_ws/src/g1_homie/resource/g1_homie similarity index 100% rename from core_ws/src/h12_skills/resource/h12_skills rename to core_ws/src/g1_homie/resource/g1_homie diff --git a/core_ws/src/g1_homie/setup.cfg b/core_ws/src/g1_homie/setup.cfg new file mode 100644 index 0000000..6428ddd --- /dev/null +++ b/core_ws/src/g1_homie/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/g1_homie +[install] +install_scripts=$base/lib/g1_homie diff --git a/core_ws/src/g1_homie/setup.py b/core_ws/src/g1_homie/setup.py new file mode 100644 index 0000000..d19798e --- /dev/null +++ b/core_ws/src/g1_homie/setup.py @@ -0,0 +1,33 @@ +from glob import glob + +from setuptools import find_packages, setup + +package_name = 'g1_homie' + +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']), + ('share/' + package_name + '/config', glob('config/*.yaml')), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='Niraj Pudasaini', + maintainer_email='npudasai@nd.edu', + description=("HOMIE's walking policy on the Unitree G1's legs, " + "through golem_safety's split mode"), + license='MIT', + extras_require={ + 'test': [ + 'pytest', + ], + }, + entry_points={ + 'console_scripts': [ + 'locomotion_node = g1_homie.locomotion_node:main', + ], + }, +) diff --git a/core_ws/src/g1_homie/test/conftest.py b/core_ws/src/g1_homie/test/conftest.py new file mode 100644 index 0000000..ec5e210 --- /dev/null +++ b/core_ws/src/g1_homie/test/conftest.py @@ -0,0 +1,10 @@ +'''pytest setup shared by the tests''' + +import gc + +# the DDS tests close every channel they open, but a test that fails while +# setting up may not. When the cyclic garbage collector reclaims an unclosed +# Unitree channel, cyclonedds-python frees its listener but keeps its DDS +# entity, and the next channel on that topic can crash the process; with the +# collector off, an unclosed channel stays alive until the session ends +gc.disable() diff --git a/core_ws/src/g1_homie/test/test_band.py b/core_ws/src/g1_homie/test/test_band.py new file mode 100644 index 0000000..9a23a47 --- /dev/null +++ b/core_ws/src/g1_homie/test/test_band.py @@ -0,0 +1,77 @@ +''' +The sim's band release with no ROS: when /elastic_band/toggle is called, and that the +toggle (which flips the band) is never sent twice +''' + +from g1_homie.band import BandRelease + +MAX_WAIT = 30.0 +START = 100.0 # when the policy started (walking_since), s + + +def test_band_release_waits_for_walking(): + band = BandRelease(enabled=True, max_wait=MAX_WAIT) + # before the policy runs, nothing, even with frame_task_server up and long past max_wait + for now in (START, START + 2 * MAX_WAIT): + assert band.poll(now, None, frame_task_ready=True, service_ready=True) is None + reason = band.poll(START + 0.1, START, frame_task_ready=True, service_ready=True) + assert reason is not None and '/left_ee_pose' in reason + assert not band.released + + +def test_band_release_waits_for_frame_task(): + band = BandRelease(enabled=True, max_wait=MAX_WAIT) + for now in (START, START + 1.0, START + MAX_WAIT - 0.01): + assert band.poll(now, START, frame_task_ready=False, service_ready=True) is None + reason = band.poll(START + MAX_WAIT + 0.01, START, frame_task_ready=False, service_ready=True) + assert reason is not None and 'maximum wait' in reason and '30 s' in reason + + +def test_band_release_waits_for_the_service(): + band = BandRelease(enabled=True, max_wait=MAX_WAIT) + assert band.poll(START + 1.0, START, frame_task_ready=True, service_ready=False) is None + assert band.service_missing + assert band.poll(START + 1.1, START, frame_task_ready=True, service_ready=True) is not None + + +def test_band_release_one_call_in_flight(): + band = BandRelease(enabled=True, max_wait=MAX_WAIT) + assert band.poll(START + 1.0, START, frame_task_ready=True, service_ready=True) is not None + # no response yet: never a second call, however long it takes + for now in (START + 1.1, START + 5.0, START + 10 * MAX_WAIT): + assert band.poll(now, START, frame_task_ready=True, service_ready=True) is None + assert not band.released + band.result(True) + assert band.released + + +def test_band_release_retries_after_failure(): + band = BandRelease(enabled=True, max_wait=MAX_WAIT) + assert band.poll(START + 1.0, START, frame_task_ready=True, service_ready=True) is not None + band.result(False) + assert not band.released + assert band.poll(START + 1.1, START, frame_task_ready=True, service_ready=True) is not None + band.result(True) + assert band.released + + +def test_band_release_never_after_success(): + band = BandRelease(enabled=True, max_wait=MAX_WAIT) + assert band.poll(START + 1.0, START, frame_task_ready=True, service_ready=True) is not None + band.result(True) + # a late or repeated response changes nothing, and nothing is ever sent again + band.result(False) + band.result(True) + assert band.released + for now in (START + 1.1, START + 2 * MAX_WAIT): + for ready in (True, False): + assert band.poll(now, START, frame_task_ready=ready, service_ready=True) is None + assert band.released + + +def test_band_release_disabled_never_calls(): + band = BandRelease(enabled=False, max_wait=MAX_WAIT) + for now in (START, START + 1.0, START + 2 * MAX_WAIT): + for walking_since in (None, START): + assert band.poll(now, walking_since, frame_task_ready=True, service_ready=True) is None + assert not band.released and not band.service_missing diff --git a/core_ws/src/g1_homie/test/test_locomotion.py b/core_ws/src/g1_homie/test/test_locomotion.py new file mode 100644 index 0000000..c384ac1 --- /dev/null +++ b/core_ws/src/g1_homie/test/test_locomotion.py @@ -0,0 +1,1008 @@ +''' +The walking node's logic with no DDS or ROS: config, slots, state, commands, +startup (the heartbeat, the pre-pose, the confirm hold), the tick cadence, the +latency and faults +''' + +import importlib.util +import math +import re +from pathlib import Path + +import numpy as np +import pytest +import yaml + +from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowState_ as LowStateDefault + +import unitree_sdk2py.core.channel +from g1_homie.config import check_config, init_channel_factory, load_config +from g1_homie.locomotion import ( + FAULTED, + HOLDING, + POLICY_DT, + POLICY_SLOTS, + RAMPING, + STATS_SECONDS, + STEADY_SECONDS, + TICK_MODULO, + WAITING, + WALKING, + CommandFilter, + LocomotionController, + SourceClock, + State, + gravity_z, + load_policy, + policy_slots, + read_state, + tick_delta, +) + +# the package's policy/ (the containers also mount it at /home/code/g1_homie) +POLICY_DIR = Path(__file__).resolve().parents[1] / 'policy' +CONFIG_DIR = Path(__file__).resolve().parents[1] / 'config' +SAMPLE_DT = 0.002 # rt/lowstate at 500 Hz +TICK_S = 0.001 # its tick counts ms +# rt/lowstate samples per policy step, and to start, when every one arrives +SAMPLES_PER_STEP = 10 +SAMPLES_TO_START = round(STEADY_SECONDS / SAMPLE_DT) +BEAT_DT = 0.1 # golem_safety's /safety/heartbeat at 10 Hz + + +def _raw_config(name='g1.yaml'): + with open(CONFIG_DIR / name) as f: + return yaml.safe_load(f) + + +def _tilted(degrees): + '''Pelvis quaternion (w, x, y, z) rolled by `degrees` about x''' + half = math.radians(degrees) / 2 + return (math.cos(half), math.sin(half), 0.0, 0.0) + + +class FakeSession: + '''Stands in for the ONNX session: records each input, returns zero actions, runs `hook`''' + + def __init__(self, hook=None): + self.inputs = [] + self.hook = hook + + def run(self, output_names, feed): + (x,) = feed.values() + self.inputs.append(x.copy()) + if self.hook is not None: + return [np.array([self.hook()], dtype=np.float32)] + return [np.zeros((1, 12), dtype=np.float32)] + + +class Clock: + '''A fake monotonic clock''' + + def __init__(self): + self.t = 100.0 + + +@pytest.fixture(scope='module') +def trained(): + '''The policy's trained vx, vy and yaw ranges''' + return load_policy(str(POLICY_DIR)).config['cmd_range'] + + +@pytest.fixture +def policy(): + return load_policy(str(POLICY_DIR)) + + +def _standing(policy, legs=None, quat=(1.0, 0.0, 0.0, 0.0), gyro=(0.0, 0.0, 0.0)): + '''The State of a still robot, legs at `legs` (default: the default pose), the rest at 0''' + q = np.zeros(len(POLICY_SLOTS)) + q[:12] = policy.default_angles if legs is None else legs + return State(np.array(quat, dtype=np.float64), np.array(gyro, dtype=np.float64), + q, np.zeros(len(POLICY_SLOTS))) + + +class Rig: + ''' + A LocomotionController on a fake clock, fed samples 2 ms (2 ticks) apart, + and published as the driver does: only if latency_ok, the latency counted + from the sample's arrival (no delivery delay). golem_safety's heartbeat + beats true every BEAT_DT while `beating`. The config is the shipped `config` + with ramp_seconds and wait_for_confirm replaced unless they are None: by + default a 0.1 s ramp straight into the policy + ''' + + def __init__(self, policy, trained, ramp_seconds=0.1, tick=1000, config='g1.yaml', + wait_for_confirm=False): + raw = _raw_config(config) + if ramp_seconds is not None: + raw['startup']['ramp_seconds'] = ramp_seconds + if wait_for_confirm is not None: + raw['startup']['wait_for_confirm'] = wait_for_confirm + self.config = check_config(raw, trained) + self.policy = policy + self.session = policy.session = FakeSession() + self.clock = Clock() + self.tick = tick + self.log = [] + self.steps = [] # the ticks of the published commands + self.beating = True + self.last_beat = None # when the last beat was given + self.controller = LocomotionController( + policy, self.config, log=lambda level, message: self.log.append((level, message))) + + def feed(self, state, n, upper=True, dt=SAMPLE_DT, ticks=None, age=0.0): + ''' + n samples dt s apart, their ticks `ticks` apart (dt in ms by default), + each taken age s after it was written; returns what was published for + each (None: silent) + ''' + out = [] + for _ in range(n): + self.clock.t += dt + step = round(dt / TICK_S) if ticks is None else ticks + self.tick = (self.tick + step) % TICK_MODULO + if self.beating and (self.last_beat is None or + self.clock.t - self.last_beat >= BEAT_DT - 1e-9): + self.controller.on_heartbeat(True, self.clock.t) + self.last_beat = self.clock.t + if upper: + self.controller.on_upper_cmd(self.clock.t) + arrival = self.clock.t + targets = self.controller.on_lowstate(state, self.tick, arrival, age) + if targets is not None and not self.controller.latency_ok(self.clock.t - arrival): + targets = None + if targets is not None: + self.steps.append(self.tick) + self.controller.log_timing(self.clock.t) + out.append(targets) + return out + + def walk(self, state): + '''Run the startup (a 0.1 s ramp) until the policy has stepped once''' + for _ in range(2 * SAMPLES_TO_START): + self.feed(state, 1) + if self.session.inputs: + return + raise AssertionError(f'never walked: {self.controller.phase}, {self.log}') + + def until(self, state, phase, limit=4 * SAMPLES_TO_START): + '''Feed samples one at a time until the controller is in `phase`; returns the outputs''' + out = [] + while self.controller.phase != phase: + assert len(out) < limit, f'never {phase}: {self.controller.phase}, {self.log}' + out += self.feed(state, 1) + return out + + +def _messages(log, level=None): + return [m for lvl, m in log if level is None or lvl == level] + + +def test_configs(trained): + real = load_config(CONFIG_DIR / 'g1.yaml', trained) + assert real['topics'] == {'low_state': 'rt/lowstate', 'low_cmd': 'rt/safety/lowcmd_lower_in', + 'upper_cmd': 'rt/safety/lowcmd_upper_in'} + assert real['network'] == {'domain_id': 0, 'interface': None} + # the real robot holds the default pose until the operator confirms, and has no band + assert real['startup'] == {'ramp_seconds': 2.0, 'wait_for_confirm': True, + 'tick_rate': (0.9, 1.1)} + assert real['heartbeat'] == {'timeout': 0.5} + assert real['band'] == {'release': False, 'max_wait': 30.0} + limits = [real['commands'][k] for k in ('vx', 'vy', 'yaw')] + assert limits == [(-0.3, 0.5), (-0.3, 0.3), (-0.6, 0.6)] + assert real['commands']['timeout'] == 0.5 + assert real['faults'] == {'max_state_gap': 0.05, 'max_latency': 0.025} + assert real['tick_seconds'] == 0.001 + + sim = load_config(CONFIG_DIR / 'sim.yaml', trained) + assert sim['network']['domain_id'] == 1 + # the same 2 s pre-pose, no confirm, and the band released once the policy runs; the + # sim's real-time factor may drop well below 1 + assert sim['startup'] == {'ramp_seconds': 2.0, 'wait_for_confirm': False, + 'tick_rate': (0.1, 1.25)} + assert sim['heartbeat'] == {'timeout': 0.5} + assert sim['band'] == {'release': True, 'max_wait': 30.0} + # the sim's tick is ms of sim time, and its latency limit the robot's + assert sim['tick_seconds'] == 0.001 and sim['faults']['max_latency'] == 0.025 + # the sim uses the trained ranges + assert [list(sim['commands'][k]) for k in ('vx', 'vy', 'yaw')] == trained + + # each of these keys is required in both files, and a config without one is refused, + # naming it + for name in ('g1.yaml', 'sim.yaml'): + for section, key in (('startup', 'ramp_seconds'), ('startup', 'wait_for_confirm'), + ('heartbeat', 'timeout'), ('band', 'release'), ('band', 'max_wait')): + raw = _raw_config(name) + del raw[section][key] + message = f"{section}: missing keys ['{key}'], unknown keys []" + with pytest.raises(ValueError, match=re.escape(message)): + check_config(raw, trained) + for section in ('heartbeat', 'band'): + raw = _raw_config(name) + del raw[section] + message = f"config: missing keys ['{section}'], unknown keys []" + with pytest.raises(ValueError, match=re.escape(message)): + check_config(raw, trained) + + +@pytest.mark.parametrize('section, key, value, message', [ + ('topics', 'low_cmd', 'rt/lowcmd', + 'topics.low_cmd must be a golem_safety input (rt/safety/...), got rt/lowcmd'), + ('topics', 'low_cmd', 'rt/safety/lowcmd_upper_in', + 'topics.low_cmd and topics.upper_cmd must differ'), + (None, 'imu_frame', 'torso', "imu_frame must be one of ['pelvis'], got 'torso'"), + ('commands', 'vx', [-0.3, 1.5], + 'commands.vx [-0.3, 1.5] must include 0 and lie inside the trained range [-0.8, 1.2]'), + ('commands', 'yaw', [-0.9, 0.6], 'commands.yaw [-0.9, 0.6] must include 0'), + ('commands', 'vy', [0.1, 0.3], 'commands.vy [0.1, 0.3] must include 0'), + ('commands', 'vx', [0.5], 'commands.vx must be a list of 2 numbers'), + ('commands', 'accel', [0.5, 0.0, 1.0], 'commands.accel (vy) must be positive'), + ('commands', 'timeout', True, 'commands.timeout must be a number'), + ('startup', 'ramp_seconds', 0.0, 'startup.ramp_seconds must be positive'), + ('startup', 'wait_for_confirm', 'yes', + "startup.wait_for_confirm must be true or false, got 'yes'"), + ('heartbeat', 'timeout', 0, 'heartbeat.timeout must be positive'), + ('heartbeat', 'timeout', 0.1, + "heartbeat.timeout must be longer than golem_safety's beat period (0.1 s), got 0.1"), + ('band', 'release', 1, 'band.release must be true or false, got 1'), + ('band', 'max_wait', -1.0, 'band.max_wait must be positive'), + ('startup', 'tick_rate', [0.9, 0.95], + 'startup.tick_rate [0.9, 0.95] must be [low, high] with 0 < low <= 1 <= high'), + ('startup', 'tick_rate', [0.0, 1.1], 'must be [low, high] with 0 < low <= 1 <= high'), + ('startup', 'tick_rate', 1.0, 'startup.tick_rate must be a list of 2 numbers'), + ('faults', 'max_state_gap', math.inf, 'faults.max_state_gap must be positive and finite'), + ('faults', 'max_latency', 0, 'faults.max_latency must be positive'), + (None, 'tick_seconds', 0.003, 'tick_seconds must divide the 0.02 s policy period, got 0.003'), + (None, 'tick_seconds', 0.04, 'tick_seconds must divide the 0.02 s policy period, got 0.04'), + (None, 'tick_seconds', '1ms', "tick_seconds must be a number, got '1ms'"), + ('network', 'domain_id', -1, 'network.domain_id must be a nonnegative integer'), +]) +def test_config_rejects_bad_values(trained, section, key, value, message): + raw = _raw_config() + (raw if section is None else raw[section])[key] = value + with pytest.raises(ValueError, match=re.escape(message)): + check_config(raw, trained) + + +def test_config_rejects_missing_and_unknown_keys(trained): + raw = _raw_config() + del raw['faults']['max_state_gap'] + message = "faults: missing keys ['max_state_gap'], unknown keys []" + with pytest.raises(ValueError, match=re.escape(message)): + check_config(raw, trained) + raw = _raw_config() + raw['startup']['ramp_second'] = 1.0 # a typo must not fall back to a default + with pytest.raises(ValueError, match=re.escape("unknown keys ['ramp_second']")): + check_config(raw, trained) + raw = _raw_config() + raw['estop'] = {} + message = "config: missing keys [], unknown keys ['estop']" + with pytest.raises(ValueError, match=re.escape(message)): + check_config(raw, trained) + + +def test_dds_domain_and_interface(trained, monkeypatch): + # the SDK call is faked: nothing here may start DDS, least of all on domain 0 + calls = [] + monkeypatch.setattr(unitree_sdk2py.core.channel, 'ChannelFactoryInitialize', + lambda *args: calls.append(args)) + real = load_config(CONFIG_DIR / 'g1.yaml', trained) + sim = load_config(CONFIG_DIR / 'sim.yaml', trained) + monkeypatch.delenv('ROS_DOMAIN_ID', raising=False) + monkeypatch.delenv('G1_DDS_INTERFACE', raising=False) + init_channel_factory(sim) + monkeypatch.setenv('ROS_DOMAIN_ID', '97') + monkeypatch.setenv('G1_DDS_INTERFACE', 'lo') + init_channel_factory(real) + assert calls == [(1,), (97, 'lo')] + # the sim's config never drives the real robot + monkeypatch.setenv('ROS_DOMAIN_ID', '0') + message = ('ROS_DOMAIN_ID=0 is the real robot, but this config is for DDS domain 1 ' + '(sim.yaml is for the sim); use g1.yaml') + with pytest.raises(ValueError, match=re.escape(message)): + init_channel_factory(sim) + assert calls == [(1,), (97, 'lo')] + + +def test_policy_joints_map_to_their_slots(policy): + assert policy_slots(policy.policy_joints) == tuple(range(0, 13)) + tuple(range(15, 29)) + joints = list(policy.policy_joints) + with pytest.raises(ValueError, match='not G1 body joints'): + policy_slots(joints[:-1] + ['head_joint']) + swapped = joints[:] + swapped[0], swapped[1] = swapped[1], swapped[0] + with pytest.raises(ValueError, match='expected 0-12 and 15-28'): + policy_slots(swapped) + with pytest.raises(ValueError, match='expected 0-12 and 15-28'): + policy_slots(joints[:12] + ['waist_roll_joint'] + joints[13:]) + + +def test_observation_from_lowstate_matches_homie_policy(policy): + msg = LowStateDefault() + # a different value in every slot, so a mix-up shows + for i in range(len(msg.motor_state)): + msg.motor_state[i].q = 0.01 * i + 0.001 + msg.motor_state[i].dq = -0.1 * i - 0.05 + quat = (math.cos(0.2), 0.6 * math.sin(0.2), 0.8 * math.sin(0.2), 0.0) + gyro = (0.3, -0.2, 0.1) + msg.imu_state.quaternion = list(quat) + msg.imu_state.gyroscope = list(gyro) + msg.imu_state.rpy = [9.0, 9.0, 9.0] # not observed + state = read_state(msg, policy_slots(policy.policy_joints)) + + # the same state written down by slot: legs 0-11, waist yaw 12, arms 15-28 + slots = list(range(0, 13)) + list(range(15, 29)) + q = [0.01 * i + 0.001 for i in slots] + dq = [-0.1 * i - 0.05 for i in slots] + cmd, height = [0.4, -0.1, 0.3], 0.7 + np.testing.assert_array_equal( + policy.observation(cmd, height, state.gyro, state.quat_wxyz, state.q, state.dq), + policy.observation(cmd, height, gyro, quat, q, dq)) + + +def test_gravity_z_matches_homie_policy(): + spec = importlib.util.spec_from_file_location('homie_policy', POLICY_DIR / 'homie_policy.py') + homie = importlib.util.module_from_spec(spec) + spec.loader.exec_module(homie) + rng = np.random.default_rng(0) + for quat in rng.normal(size=(20, 4)): + quat /= np.linalg.norm(quat) + assert gravity_z(quat) == pytest.approx(homie.get_gravity_orientation(quat)[2], abs=1e-12) + assert gravity_z(_tilted(0.0)) == -1.0 + assert gravity_z(_tilted(60.0)) == pytest.approx(-0.5) + + +def test_command_filter_clips_then_rate_limits(policy, trained): + commands = load_config(CONFIG_DIR / 'g1.yaml', trained)['commands'] + f = CommandFilter(policy, commands) + assert f.cmd.tolist() == [0.0, 0.0, 0.0] and f.height == pytest.approx(0.74) + + # one step moves by at most accel * dt, and the height by height_rate * dt + cmd, height = f.update([0.4, -0.2, 0.5], 0.5) + np.testing.assert_allclose(cmd, [0.5 * POLICY_DT, -0.5 * POLICY_DT, 1.0 * POLICY_DT]) + assert height == pytest.approx(0.74 - 0.1 * POLICY_DT) + + # beyond the trained ranges: clipped to them, then to the deployment limits + for _ in range(300): + cmd, height = f.update([2.0, -2.0, 2.0], 0.1) + np.testing.assert_allclose(cmd, [0.5, -0.3, 0.6]) + assert height == pytest.approx(0.24) # the trained height range + for _ in range(300): + cmd, height = f.update([-2.0, 2.0, -2.0], 2.0) + np.testing.assert_allclose(cmd, [-0.3, 0.3, -0.6]) + assert height == pytest.approx(0.74) + with pytest.raises(ValueError, match='not finite'): + f.update([math.nan, 0.0, 0.0], 0.74) + + +def test_waits_for_an_upright_finite_state(policy, trained): + rig = Rig(policy, trained) + legs = np.asarray(policy.default_angles) + 0.2 + # tilted 40 deg, then not finite: waiting, not a fault + assert rig.feed(_standing(policy, legs, quat=_tilted(40.0)), 50) == [None] * 50 + bad = _standing(policy, legs, gyro=(math.nan, 0.0, 0.0)) + assert rig.feed(bad, 50) == [None] * 50 + assert rig.controller.phase == WAITING and rig.controller.fault is None + assert _messages(rig.log) == [ + 'waiting to start: the robot is not upright ' + '(gravity z -0.77 in the pelvis frame, needs < -0.9)', + 'waiting to start: rt/lowstate gyro is not finite (at [0])', + ] + # the first good sample starts the ramp from the measured legs, as the H1-2's leg + # node does: with no arm controller and whatever the timing + out = rig.feed(_standing(policy, legs), 1, upper=False, age=0.03) + np.testing.assert_allclose(out[0], legs) + assert rig.controller.phase == RAMPING + assert not rig.session.inputs, 'the policy must not run during the ramp' + + +def _holding(policy, trained, **kwargs): + '''A rig whose legs hold the default pose after a 0.1 s ramp, the policy not ready''' + rig = Rig(policy, trained, **kwargs) + rig.until(_standing(policy), HOLDING) + return rig + + +def test_the_policy_waits_for_steady_timing_and_the_arm_controller(policy, trained): + rig = _holding(policy, trained, config='sim.yaml', wait_for_confirm=None) + good = _standing(policy) + rig.log.clear() + # upright and steady, but the arm controller has not commanded for 0.5 s + rig.feed(good, 300, upper=False) + # samples that reach the node over half faults.max_latency (12.5 ms) after + # their write keep it holding: the laptop is not keeping up. Samples that far + # apart do not, as from a sim below real time + rig.feed(good, 5, dt=0.03) + rig.feed(good, 5, age=0.03) + rig.feed(good, 5, age=0.013) + assert rig.controller.phase == HOLDING and rig.controller.fault is None + assert not rig.session.inputs + assert _messages(rig.log) == [ + 'the policy waits: no commands from the arm controller on rt/safety/lowcmd_upper_in', + 'the policy waits: rt/lowstate reaches the node late (30 ms, max 12.5 ms to start)', + ] + # then STEADY_SECONDS (1 s of ticks) of good samples starts the policy + rig.feed(good, SAMPLES_TO_START + 20) + assert rig.controller.phase == WALKING + assert any(m.startswith('the policy may start') for m in _messages(rig.log)) + + +def test_a_sim_at_a_fifth_of_real_time_starts_the_policy(policy, trained): + # 2 ms of tick every 9-18 ms of wall clock, as the G1 sim with golem_slam + # delivers rt/lowstate: past half faults.max_latency apart, each answered at once + rig = _holding(policy, trained, config='sim.yaml', wait_for_confirm=None) + good = _standing(policy) + for _ in range(2 * SAMPLES_TO_START): + rig.feed(good, 1, dt=0.009, ticks=2) + rig.feed(good, 1, dt=0.018, ticks=2) + if rig.controller.phase == WALKING: + break + assert rig.controller.phase == WALKING, _messages(rig.log) + + +def test_alternating_reasons_to_wait_are_logged_at_most_every_5_s(policy, trained): + rig = _holding(policy, trained, config='sim.yaml', wait_for_confirm=None) + good = _standing(policy) + rig.log.clear() + # two reasons that alternate for 8 s: late samples between ones with no arm controller + for _ in range(12): + rig.feed(good, 300, upper=False) + rig.feed(good, 5, age=0.03) + assert rig.controller.phase == HOLDING + kinds = [m.split(' (')[0] for m in _messages(rig.log)] + assert kinds == ['the policy waits: no commands from the arm controller on ' + 'rt/safety/lowcmd_upper_in', 'the policy waits: rt/lowstate reaches the ' + 'node late'] * 2 + + +@pytest.mark.parametrize('ticks', [1, 4], ids=['half', 'double']) +def test_a_tick_off_the_wall_clock_keeps_the_policy_waiting(policy, trained, ticks): + # rt/lowstate's tick counting 1 or 4 per 2 ms sample, not the 2 that + # tick_seconds (1 ms) means: the policy would run at half or twice its rate + rig = _holding(policy, trained, wait_for_confirm=True) + good = _standing(policy) + rig.feed(good, 1, age=0.03) # a late sample restarts the steady window + rig.log.clear() + rig.feed(good, 4 * SAMPLES_TO_START, ticks=ticks) + assert rig.controller.phase == HOLDING and rig.controller.fault is None + rate = ticks / 2 + assert _messages(rig.log) == [ + f"the policy waits: rt/lowstate's tick is off the wall clock ({rate:.2f} s of tick " + 'per s, needs 0.90-1.10; is tick_seconds right?)'] + ok, message = rig.controller.confirm() + assert not ok and message.startswith('refused: the policy cannot start yet: ') + # once the tick is in ms, the next 1 s of ticks makes it ready (a late sample + # first ends the window that began with the old tick) + rig.feed(good, 1, age=0.03) + rig.feed(good, SAMPLES_TO_START + 1) + assert _messages(rig.log)[-1] == ( + "the policy may start (rt/lowstate's tick ran at 1.00 s per s of wall clock)") + assert rig.controller.confirm()[0] + + +def test_the_upper_commands_must_be_recent(policy, trained): + rig = _holding(policy, trained, config='sim.yaml', wait_for_confirm=None) + # the arm controller's last command was over 0.5 s ago + rig.feed(_standing(policy), 2 * SAMPLES_TO_START, upper=False) + assert rig.controller.phase == HOLDING + + +def test_waits_for_first_heartbeat(policy, trained): + rig = Rig(policy, trained) + rig.beating = False + good = _standing(policy) + # upright and the arm controller commanding, but golem_safety is not relaying + assert rig.feed(good, 2 * SAMPLES_TO_START) == [None] * (2 * SAMPLES_TO_START) + # the watchdog arms on the first beat: silence before it is no fault + rig.controller.check_stale(rig.clock.t + 10.0) + assert rig.controller.phase == WAITING and rig.controller.fault is None + assert _messages(rig.log) == [ + 'waiting to start: no /safety/heartbeat yet (golem_safety beats once it can relay)'] + # the first true beat starts the ramp on the next sample + rig.controller.on_heartbeat(True, rig.clock.t) + assert rig.feed(good, 1)[0] is not None + assert rig.controller.phase == RAMPING + + +def test_false_first_beat_faults(policy, trained): + rig = Rig(policy, trained) + rig.beating = False + reason = "golem_safety's /safety/heartbeat is false: its estop has latched" + rig.controller.on_heartbeat(False, rig.clock.t) + assert rig.controller.phase == FAULTED and rig.controller.fault == reason + # silent for good: true beats and good samples change nothing + rig.beating = True + assert rig.feed(_standing(policy), 2 * SAMPLES_TO_START) == [None] * (2 * SAMPLES_TO_START) + assert rig.controller.phase == FAULTED and rig.controller.fault == reason + errors = _messages(rig.log, 'error') + assert len(errors) == 1 and errors[0].startswith(f'fault: {reason}. Stopped commanding') + + +@pytest.mark.parametrize('how', ['false beat', 'no beat']) +@pytest.mark.parametrize('phase', [WAITING, RAMPING, HOLDING, WALKING]) +def test_heartbeat_lost_in_every_phase_faults(policy, trained, phase, how): + # the shipped 2 s ramp, so the ramp lasts longer than the test feeds it; waiting + # here is waiting for an upright robot, after the first beat + rig = Rig(policy, trained, ramp_seconds=None, wait_for_confirm=True) + upright = _standing(policy, np.asarray(policy.default_angles) + 0.2) + state = upright if phase != WAITING else upright._replace(quat_wxyz=np.array(_tilted(40.0))) + upper = True + if phase != WAITING: + rig.until(state, RAMPING) + if phase in (HOLDING, WALKING): + rig.until(state, HOLDING) + if phase == WALKING: + assert rig.controller.confirm()[0] + # beats every 0.1 s keep it running, the watchdog checked on every sample as the + # driver does + for _ in range(round(0.6 / SAMPLE_DT)): + rig.feed(state, 1, upper=upper) + rig.controller.check_stale(rig.clock.t) + assert rig.controller.phase == phase and rig.controller.fault is None + + if how == 'false beat': + reason = "golem_safety's /safety/heartbeat is false: its estop has latched" + published, command = len(rig.steps), rig.controller.command + rig.controller.on_heartbeat(False, rig.clock.t) + else: + # rt/lowstate goes on and the beats stop: up to 0.5 s without one is allowed + rig.beating = False + last = rig.last_beat + while rig.clock.t + SAMPLE_DT - last < 0.5 - 1e-6: + rig.feed(state, 1, upper=upper) + rig.controller.check_stale(rig.clock.t) + assert rig.controller.phase == phase and rig.controller.fault is None + reason = 'no /safety/heartbeat for 510 ms (> 500 ms)' + published, command = len(rig.steps), rig.controller.command + rig.clock.t = last + 0.51 + rig.controller.check_stale(rig.clock.t) + assert rig.controller.phase == FAULTED and rig.controller.fault.startswith(reason) + assert rig.controller.walking_since is None + # nothing more is published, and the command stays the last one + assert rig.feed(state, 20 * SAMPLES_PER_STEP, upper=upper) == [None] * (20 * SAMPLES_PER_STEP) + assert len(rig.steps) == published + np.testing.assert_array_equal(rig.controller.command[0], command[0]) + assert rig.controller.command[1] == command[1] + errors = _messages(rig.log, 'error') + assert len(errors) == 1 and errors[0].startswith(f'fault: {reason}') + assert 'Stopped commanding the legs for good' in errors[0] + + +@pytest.mark.parametrize('config', ['g1.yaml', 'sim.yaml']) +def test_ramp_takes_two_seconds(policy, trained, config): + rig = Rig(policy, trained, config=config, ramp_seconds=None, wait_for_confirm=None) + legs = np.asarray(policy.default_angles) + np.linspace(-0.3, 0.3, 12) + default = np.asarray(policy.default_angles) + out = rig.feed(_standing(policy, legs), 102 * SAMPLES_PER_STEP) + targets = np.array([t for t in out if t is not None]) + # 101 targets 20 ms of tick apart: from the measured legs, the default pose reached + # after 100 policy steps, 2.0 s, and not before + np.testing.assert_allclose(targets[0], legs, atol=1e-12) + np.testing.assert_allclose(targets[100], default, atol=1e-12) + assert np.all(np.abs(targets[:100] - default).max(axis=1) > 1e-3) + assert rig.steps[100] - rig.steps[0] == 2000 + (moving,) = [m for m in _messages(rig.log) if m.startswith('moving the legs')] + assert moving.endswith('over 2.0 s') + assert rig.controller.phase == (HOLDING if config == 'g1.yaml' else WALKING) + + +def test_holds_until_confirm(policy, trained): + rig = Rig(policy, trained, wait_for_confirm=True) + default = np.asarray(policy.default_angles) + state = _standing(policy, default + 0.2) + rig.until(state, HOLDING) + assert rig.controller.walking_since is None + assert _messages(rig.log)[-1] == ( + 'legs at the default pose: holding it until the operator confirms (ros2 service call ' + '/lowerbody/confirm_engage std_srvs/srv/Trigger)') + # the default pose every policy step, and no policy + rig.steps.clear() + held = [t for t in rig.feed(state, 50 * SAMPLES_PER_STEP) if t is not None] + assert len(held) == 50 and np.diff(rig.steps).tolist() == [20] * 49 + for targets in held: + np.testing.assert_array_equal(targets, default) + assert rig.controller.phase == HOLDING and not rig.session.inputs + + # requests that came before the go-ahead are dropped: the policy starts at zero + # velocity and the standing height + rig.controller.set_cmd_vel(0.3, 0.1, 0.2, rig.clock.t) + rig.controller.set_height(0.6) + ok, message = rig.controller.confirm() + assert ok and message == 'confirmed: the policy starts on the next policy step' + assert rig.controller.phase == WALKING + assert rig.controller.walking_since == rig.clock.t + assert _messages(rig.log)[-1] == 'walking: policy started at zero velocity and 0.74 m height' + assert sum(t is not None for t in rig.feed(state, SAMPLES_PER_STEP)) == 1 + assert len(rig.session.inputs) == 1 + assert not rig.session.inputs[0].reshape(6, -1)[:5].any(), 'the history must start empty' + cmd, height = rig.controller.command + assert cmd.tolist() == [0.0, 0.0, 0.0] and height == pytest.approx(0.74) + # confirmed once + ok, message = rig.controller.confirm() + assert not ok and 'walking' in message + + +def test_confirm_refused_outside_holding(policy, trained): + rig = Rig(policy, trained, wait_for_confirm=True) + state = _standing(policy, np.asarray(policy.default_angles) + 0.2) + for phase in (WAITING, RAMPING, FAULTED): + if phase == RAMPING: + rig.until(state, RAMPING) + elif phase == FAULTED: + rig.controller.latch('test') + ok, message = rig.controller.confirm() + assert not ok and f'the node is {phase}' in message, message + assert rig.controller.phase == phase and rig.controller.walking_since is None + assert not rig.session.inputs + + +def test_sim_config_starts_the_policy_once_ready(policy, trained): + rig = Rig(policy, trained, config='sim.yaml', wait_for_confirm=None) # a 0.1 s ramp + state = _standing(policy, np.asarray(policy.default_angles) + 0.2) + rig.until(state, HOLDING) + assert _messages(rig.log)[-1] == ( + 'legs at the default pose: holding it until the policy may start') + # no confirm: the policy follows 1 s of steady samples from the ramp's start + rig.until(state, WALKING) + assert rig.controller.walking_since == rig.clock.t + assert _messages(rig.log)[-1] == 'walking: policy started at zero velocity and 0.74 m height' + assert len(rig.session.inputs) == 1 + + +def test_ramp_then_policy_from_zero_command_and_standing_height(policy, trained): + rig = Rig(policy, trained, ramp_seconds=0.1) # 5 policy steps + legs = np.asarray(policy.default_angles) + np.linspace(-0.3, 0.3, 12) + state = _standing(policy, legs, gyro=(0.1, -0.2, 0.05)) + rig.walk(state) + + # 6 ramp targets on the line from the measured legs to the default pose, the + # default pose held, then the first policy step, one per policy step (20 ticks) + assert set(np.diff(rig.steps).tolist()) == {20} + assert rig.controller.phase == WALKING and len(rig.session.inputs) == 1 + assert [m for m in _messages(rig.log) if not m.startswith('the policy waits')] == [ + 'moving the legs from where they are to the default pose over 0.1 s', + 'legs at the default pose: holding it until the policy may start', + "the policy may start (rt/lowstate's tick ran at 1.00 s per s of wall clock)", + 'walking: policy started at zero velocity and 0.74 m height', + ] + + # the policy starts from an empty history, zero velocity and 0.74 m, and + # observes the state as HomiePolicy.observation does + obs = rig.session.inputs[0].reshape(6, -1) + assert not obs[:5].any(), 'the history must start empty' + reference = load_policy(str(POLICY_DIR)) + expected = reference.observation( + [0.0, 0.0, 0.0], 0.74, state.gyro, state.quat_wxyz, state.q, state.dq) + np.testing.assert_array_equal(obs[5], expected) + assert rig.controller.command[1] == pytest.approx(0.74) + + +def test_band_release_restarts_the_policy_history(policy, trained): + rig = Rig(policy, trained, config='sim.yaml') + rig.session.hook = lambda: np.full(12, 0.1, dtype=np.float32) # a last action to clear + state = _standing(policy) + # before the policy runs, a release changes nothing + rig.controller.on_band_released() + assert rig.controller.phase == WAITING and not rig.log + rig.walk(state) + rig.controller.set_cmd_vel(0.4, 0.0, 0.0, rig.clock.t) + rig.controller.set_height(0.7) + rig.feed(state, 10 * SAMPLES_PER_STEP) + (vx, _, _), height = rig.controller.command + assert vx == pytest.approx(0.1) and height == pytest.approx(0.72) + full = rig.session.inputs[-1].reshape(6, -1) + assert full[:5].any() and full[5, -12:].any() + since, grid = rig.controller.walking_since, rig.steps[-1] + + rig.controller.on_band_released() + assert rig.controller.phase == WALKING and rig.controller.walking_since == since + assert _messages(rig.log)[-1] == 'elastic band off: the policy carries on from an empty history' + rig.feed(state, SAMPLES_PER_STEP) + # the next step: an empty history and no last action, as at the policy's start ... + obs = rig.session.inputs[-1].reshape(6, -1) + assert not obs[:5].any(), 'the history must restart empty' + assert not obs[5, -12:].any(), 'the last action must restart at zero' + # ... but the command, the height and the step grid carry on + (vx_next, _, _), height_next = rig.controller.command + assert vx_next == pytest.approx(vx + 0.01) and height_next == pytest.approx(height - 0.002) + assert obs[5, 0] == pytest.approx(2 * vx_next, abs=1e-6) + assert rig.steps[-1] - grid == 20 + + # a fault ends it: a later release is ignored + rig.controller.latch('test') + rig.controller.on_band_released() + assert rig.controller.phase == FAULTED + assert _messages(rig.log).count('elastic band off: the policy carries on from an empty history') == 1 + + +def test_ramp_is_linear_to_the_default_pose(policy, trained): + rig = Rig(policy, trained, ramp_seconds=0.1) + legs = np.asarray(policy.default_angles) + np.linspace(-0.3, 0.3, 12) + out = [t for t in rig.feed(_standing(policy, legs), SAMPLES_TO_START + 200) if t is not None] + ramp = np.array(out[:6]) + alpha = np.linspace(0.0, 1.0, 6)[:, None] + default = np.asarray(policy.default_angles) + np.testing.assert_allclose(ramp, legs + alpha * (default - legs), atol=1e-12) + # and the policy after it (zero actions: the default pose) + np.testing.assert_allclose(out[6], policy.default_angles, atol=1e-6) + + +def test_cmd_vel_is_rate_limited_and_times_out(policy, trained): + rig = Rig(policy, trained) + state = _standing(policy) + rig.walk(state) + rig.controller.set_cmd_vel(0.4, 0.0, 0.0, rig.clock.t) + rig.feed(state, 20 * SAMPLES_PER_STEP) # 0.4 s: 20 steps of +0.01 + vx = rig.controller.command[0][0] + assert vx == pytest.approx(0.2) + # the last policy input carries the command, scaled by cmd_scale (2) + assert rig.session.inputs[-1][0, 5 * 76] == pytest.approx(2 * vx, abs=1e-6) + # older than commands.timeout (0.5 s): heads back to zero + rig.feed(state, 20 * SAMPLES_PER_STEP) + assert rig.controller.command[0][0] < vx + rig.feed(state, 50 * SAMPLES_PER_STEP) + assert rig.controller.command[0].tolist() == pytest.approx([0.0, 0.0, 0.0], abs=1e-9) + + # a /cmd_vel that is not finite means stop; a bad height is ignored + rig.controller.set_cmd_vel(math.nan, 0.0, 0.0, rig.clock.t) + rig.controller.set_height(math.inf) + rig.controller.set_height(0.6) + rig.feed(state, 10 * SAMPLES_PER_STEP) + cmd, height = rig.controller.command + assert cmd.tolist() == pytest.approx([0.0, 0.0, 0.0]) + assert height == pytest.approx(0.74 - 10 * 0.002) + assert _messages(rig.log, 'warn') == [ + '/cmd_vel is not finite ([nan, 0.0, 0.0]): stopping', + 'ignoring a /locomotion/height that is not finite (inf)', + ] + + +def _changed(**fields): + '''Inject one sample with these State fields changed''' + def inject(rig, state): + changes = {name: np.asarray(value, dtype=np.float64) for name, value in fields.items()} + rig.feed(state._replace(**changes), 1) + return inject + + +def _knee_not_finite(rig, state): + rig.feed(state._replace(q=np.where(np.arange(27) == 3, math.inf, state.q)), 1) + + +def _session_runs(make_hook): + '''Inject a policy step whose ONNX session runs make_hook(rig)()''' + def inject(rig, state): + rig.session.hook = make_hook(rig) + rig.feed(state, SAMPLES_PER_STEP) + return inject + + +def _slow(rig): + def hook(): + rig.clock.t += 0.030 + return np.zeros(12) + return hook + + +def _failing(rig): + def hook(): + raise RuntimeError('onnxruntime failed') + return hook + + +# each fault injected while walking: (name, how, the fault's start) +FAULTS = [ + ('gap', lambda rig, state: rig.feed(state, 1, dt=0.06), + 'rt/lowstate stale: 60 ms between samples (> 50 ms)'), + ('silence', lambda rig, state: rig.controller.check_stale(rig.clock.t + 0.051), + 'rt/lowstate stale: no sample for 51 ms (> 50 ms)'), + ('gyro', _changed(gyro=[0.0, math.nan, 0.0]), 'rt/lowstate gyro is not finite (at [1])'), + ('q', _knee_not_finite, "rt/lowstate q is not finite (at ['left_knee_joint'])"), + ('quaternion', _changed(quat_wxyz=[0.5, 0.0, 0.0, 0.0]), + 'the rt/lowstate IMU quaternion is not a unit quaternion (norm 0.500)'), + ('fall', _changed(quat_wxyz=_tilted(70.0)), + 'fell: gravity z -0.34 in the pelvis frame (> -0.5)'), + ('tick back', lambda rig, state: rig.feed(state, 1, ticks=-10), + 'rt/lowstate tick went back from 2022 to 2012'), + ('late', _session_runs(_slow), + 'command late: 30.0 ms after its rt/lowstate sample (> 25 ms)'), + ('policy error', _session_runs(_failing), 'policy step failed: onnxruntime failed'), + ('policy output', _session_runs(lambda rig: lambda: np.full(12, math.nan)), + 'policy output is not 12 finite targets'), +] + + +@pytest.mark.parametrize('name, inject, reason', FAULTS, ids=[f[0] for f in FAULTS]) +def test_faults_latch_silent(policy, trained, name, inject, reason): + rig = Rig(policy, trained) + state = _standing(policy) + rig.walk(state) + assert [t is not None for t in rig.feed(state, SAMPLES_PER_STEP)].count(True) == 1 + inject(rig, state) + assert rig.controller.phase == FAULTED and rig.controller.fault.startswith(reason) + # silent for good, whatever comes next + assert rig.feed(state, 20 * SAMPLES_PER_STEP) == [None] * (20 * SAMPLES_PER_STEP) + errors = _messages(rig.log, 'error') + assert len(errors) == 1 and errors[0].startswith(f'fault: {reason}') + assert ('Stopped commanding the legs; golem_safety keeps relaying their last command, ' + 'which does not balance the robot (it estops instead when rt/lowstate has a ' + "non-finite joint value, a joint past limits.estop or another robot's " + 'mode_machine). Restart the node to clear') in errors[0] + + +@pytest.mark.parametrize('inject, reason', [ + (lambda rig, state: rig.feed(state, 1, dt=0.06), + 'rt/lowstate stale: 60 ms between samples (> 50 ms)'), + (lambda rig, state: rig.controller.check_stale(rig.clock.t + 0.051), + 'rt/lowstate stale: no sample for 51 ms (> 50 ms)'), + (_changed(quat_wxyz=_tilted(70.0)), 'fell: gravity z -0.34 in the pelvis frame (> -0.5)'), +], ids=['gap', 'silence', 'fall']) +def test_faults_latch_while_holding(policy, trained, inject, reason): + rig = Rig(policy, trained, wait_for_confirm=True) + state = _standing(policy) + rig.until(state, HOLDING) + rig.feed(state, 5 * SAMPLES_PER_STEP) + inject(rig, state) + assert rig.controller.phase == FAULTED and rig.controller.fault.startswith(reason) + assert rig.feed(state, 100) == [None] * 100 + assert not rig.controller.confirm()[0] + assert not rig.session.inputs + + +def test_a_late_command_is_no_fault_before_the_policy_runs(policy, trained): + # the pre-pose and the hold are PD targets, which a late command does not upset + rig = Rig(policy, trained, wait_for_confirm=True) + state = _standing(policy) + rig.until(state, RAMPING) + assert rig.controller.latency_ok(0.030) + rig.until(state, HOLDING) + assert rig.controller.latency_ok(0.030) + assert rig.controller.phase == HOLDING and rig.controller.fault is None + + +def test_faults_during_the_ramp_latch_too(policy, trained): + rig = Rig(policy, trained, ramp_seconds=1.0) + state = _standing(policy, np.asarray(policy.default_angles) + 0.2) + rig.feed(state, 100) + assert rig.controller.phase == RAMPING + assert rig.feed(state._replace(quat_wxyz=np.array(_tilted(70.0))), 1) == [None] + assert rig.controller.phase == FAULTED and rig.controller.fault.startswith('fell') + assert rig.feed(state, 100) == [None] * 100 + assert not rig.session.inputs + + +def test_tick_delta_wraps(): + assert tick_delta(1020, 1000) == 20 and tick_delta(1000, 1020) == -20 + assert tick_delta(5, TICK_MODULO - 15) == 20 + assert tick_delta(TICK_MODULO - 15, 5) == -20 + assert tick_delta(7, 7) == 0 + + +def _walking(policy, trained, **kwargs): + rig = Rig(policy, trained, **kwargs) + rig.walk(_standing(policy)) + rig.steps.clear() + return rig + + +@pytest.mark.parametrize('spacing', [ + [2], # every 500 Hz sample + [6], # every third sample + [2, 4, 2, 8, 6, 2, 12, 2, 16, 4], # irregular, as when the node falls behind +], ids=['every sample', 'every third', 'irregular']) +def test_policy_steps_on_the_tick_grid_whatever_samples_arrive(policy, trained, spacing): + rig = _walking(policy, trained) + state = _standing(policy) + grid = rig.controller._due_tick + for i in range(3000): + ticks = spacing[i % len(spacing)] + rig.feed(state, 1, dt=ticks * TICK_S, ticks=ticks) + steps = np.array(rig.steps) - grid + gaps = np.diff(steps) + # never closer than half a period + assert gaps.min() >= 10, gaps.min() + # each step is at the first sample at or past a grid tick (multiples of + # 20 from the walk's first step), and a grid tick is skipped only when the + # step before it came more than half a period late + late = steps % 20 + assert np.all(late < max(spacing)), late.max() + periods = np.diff(steps // 20) + assert np.all((periods == 1) | (late[:-1] > 10)), list(zip(periods, late[:-1])) + if max(spacing) <= 10: + # no step is that late: exactly 50 Hz of robot time on average + assert abs(gaps.mean() - 20.0) < 0.05, gaps.mean() + if spacing == [2]: + assert set(gaps.tolist()) == {20} + assert rig.controller.fault is None + + +def test_after_a_backlog_the_newest_sample_steps_once(policy, trained): + # the driver hands over only the newest sample: one 46 ms of ticks after the + # last (within faults.max_state_gap), then samples 2 ms apart again + rig = _walking(policy, trained) + state = _standing(policy) + rig.feed(state, SAMPLES_PER_STEP) + last = rig.steps[-1] + out = rig.feed(state, 1, dt=0.046) + assert out[0] is not None and rig.steps[-1] == last + 46, 'one step, on the newest sample' + rig.feed(state, 3 * SAMPLES_PER_STEP) + # back on the grid: the next steps at last + 60, + 80, ... + assert rig.steps[-3:] == [last + 60, last + 80, last + 100] + assert rig.controller.fault is None + + +def test_samples_whose_tick_did_not_advance_are_ignored(policy, trained): + rig = _walking(policy, trained) + state = _standing(policy) + inputs = len(rig.session.inputs) + # the same tick again and again for 60 ms: no step, and not fresh + assert rig.feed(state, 30, ticks=0) == [None] * 30 + assert len(rig.session.inputs) == inputs + rig.controller.check_stale(rig.clock.t) + assert rig.controller.fault == 'rt/lowstate stale: no sample for 60 ms (> 50 ms)' + + +def test_the_tick_wraps_while_walking(policy, trained): + rig = _walking(policy, trained, tick=TICK_MODULO - 1500) + rig.feed(_standing(policy), 50 * SAMPLES_PER_STEP) + assert rig.steps[0] > TICK_MODULO - 1500 and rig.steps[-1] < 1000, 'it wrapped' + assert {tick_delta(b, a) for a, b in zip(rig.steps, rig.steps[1:])} == {20} + assert rig.controller.fault is None + + +def test_latency_limit_and_timing_log(policy, trained): + rig = _walking(policy, trained) + state = _standing(policy) + # the walk logs its timing every STATS_SECONDS + rig.feed(state, int((STATS_SECONDS + 0.5) / SAMPLE_DT)) + timing = [m for m in _messages(rig.log, 'info') if m.startswith('timing')] + assert len(timing) == 1, timing + assert re.fullmatch(r'timing over the last 10 s: 50[0-2] commands, latency median 0\.0 ms, ' + r'max 0\.0 ms \(limit 25 ms\); steps 20-20 ms of rt/lowstate time apart, ' + r'mean 20\.0 ms', timing[0]), timing[0] + # at the limit is fine, over it a fault, and nothing more is sent + controller = rig.controller + assert controller.latency_ok(0.025) + assert not controller.latency_ok(0.0251) + assert controller.phase == FAULTED + assert controller.fault == 'command late: 25.1 ms after its rt/lowstate sample (> 25 ms)' + assert not controller.latency_ok(0.001) + assert rig.feed(state, 100) == [None] * 100 + + +def test_source_clock_measures_from_the_write(): + clock = SourceClock() + offset, fastest = 1000.0, 0.0004 # the clocks' offset and the fastest delivery + # samples 2 ms apart, delivered in 0.4-0.9 ms: the write is put at the + # source time plus offset plus the fastest delivery + for i in range(500): + source = 50.0 + 0.002 * i + received = source + offset + fastest + 0.0005 * (i % 2) + written = clock.written(source, received) + assert written == pytest.approx(source + offset + fastest, abs=1e-9) + # a sample delivered 40 ms late is measured from its write, not its arrival + source = 51.0 + written = clock.written(source, source + offset + fastest + 0.040) + assert (source + offset + fastest + 0.040) - written == pytest.approx(0.040, abs=1e-9) + # an early outlier (the clocks' offset changed) is forgotten after ~2 s + clock.written(52.0, 52.0 + offset + fastest - 0.010) + for i in range(1, 1501): + source = 52.0 + 0.002 * i + written = clock.written(source, source + offset + fastest) + assert written == pytest.approx(source + offset + fastest, abs=1e-9) + + +def test_policy_session_is_single_threaded_with_the_same_actions(policy): + import onnxruntime as ort + options = policy.session.get_session_options() + assert (options.intra_op_num_threads, options.inter_op_num_threads) == (1, 1) + # HomiePolicy's own session, with onnxruntime's default threads + default = ort.InferenceSession(policy.config['policy_path'], + providers=['CPUExecutionProvider']) + size = policy.session.get_inputs()[0].shape[-1] + obs = np.random.default_rng(0).normal(0.0, 1.0, (300, 1, size)).astype(np.float32) + for x in obs: + ours = policy.session.run(None, {policy.input_name: x})[0] + theirs = default.run(None, {policy.input_name: x})[0] + np.testing.assert_array_equal(ours, theirs) diff --git a/core_ws/src/g1_homie/test/test_locomotion_dds.py b/core_ws/src/g1_homie/test/test_locomotion_dds.py new file mode 100644 index 0000000..0e71c46 --- /dev/null +++ b/core_ws/src/g1_homie/test/test_locomotion_dds.py @@ -0,0 +1,748 @@ +''' +The walking node's DDS side on DDS domain 97 (no robot): in this process a fake G1 on +rt/lowstate, a fake arm controller on the upper input and the node's controller and driver, +on Unitree SDK channels, and the node's /safety/heartbeat subscription; beside it +golem_safety's installed safety_node as a subprocess, with robot g1 on a copy of its +split.yaml (split mode). ROS topic /x is DDS rt/x with the same unitree_hg types, so the SDK +channels here and the safety node's ROS topics meet +''' + +import itertools +import json +import math +import os +import re +import signal +import subprocess +import tempfile +import threading +import time +from contextlib import contextmanager +from pathlib import Path + +import numpy as np +import pytest +import rclpy +import yaml + +from ament_index_python.packages import get_package_prefix, get_package_share_directory +from rclpy.executors import SingleThreadedExecutor +from std_msgs.msg import Bool +from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowCmd_, LowState_ +from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowCmd_ as LowCmdDefault +from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowState_ as LowStateDefault +from unitree_sdk2py.core.channel import ( + ChannelFactoryInitialize, + ChannelPublisher, + ChannelSubscriber, +) + +from g1_homie.config import check_config +from g1_homie.driver import LocomotionDriver +from g1_homie.locomotion import ( + FAULTED, + HOLDING, + WAITING, + WALKING, + LocomotionController, + load_policy, + tick_delta, +) + +# the package's policy/ (the containers also mount it at /home/code/g1_homie) +POLICY_DIR = Path(__file__).resolve().parents[1] / 'policy' +CONFIG_DIR = Path(__file__).resolve().parents[1] / 'config' +TEST_DOMAIN = 97 # never 0 (the real robot) or 1 (the sim) +# This one process runs the fakes, the recorders and the node, and the GIL +# delays rt/lowstate by tens of ms at times (up to 90 ms seen), which the real +# node, in a process of its own, does not see. So these tests relax g1.yaml's +# 50 ms gap and 25 ms latency, and the tick rate the start accepts (a late +# sample at either end of its 1 s window moves it by as much); test_locomotion.py +# checks the real values. golem_safety's quiet-rt/lowstate report +# (stale.state_seconds, 0.1 s in split.yaml) gets MAX_STATE_GAP too, except where +# a test keeps split.yaml's value +MAX_STATE_GAP = 0.25 +MAX_LATENCY = 0.25 +TICK_RATE = [0.5, 2.0] +# rt/lowstate samples per policy step when every one arrives +SAMPLES_PER_STEP = 10 +LEGS = range(0, 12) +SHOULDER = 15 # left shoulder pitch, an arm slot +# slack for thread scheduling when timing a report of golem_safety's +LATE_S = 0.3 +# the events by which golem_safety latches its estop or reports a quiet input or a +# quiet /lowstate: none of them may appear while the node walks +ALARMS = ('estop_command_written', 'input_quiet', 'state_quiet') +# rcutils' default console format: [LEVEL] [seconds] [logger]: message +LOG_LINE = re.compile(r'^\[(DEBUG|INFO|WARN|ERROR|FATAL)\] \[([0-9.]+)\] \[[^\]]*\]: (.*)$') +_factory = [] +_runs = itertools.count() + + +def _init_dds(): + # the Unitree channel factory is process-wide; initialize it once + if not _factory: + ChannelFactoryInitialize(TEST_DOMAIN) + _factory.append(True) + + +def _tilted(degrees): + '''Pelvis quaternion (w, x, y, z) rolled by `degrees` about x''' + half = math.radians(degrees) / 2 + return [math.cos(half), math.sin(half), 0.0, 0.0] + + +class FakeG1: + ''' + rt/lowstate at 500 Hz from a G1 standing still, its tick in ms (+2 a + sample); set() changes it, pause() stops it + ''' + + def __init__(self, topic, legs): + self.lock = threading.Lock() + self.state = LowStateDefault() + self.state.mode_machine = 5 + self.state.imu_state.quaternion = [1.0, 0.0, 0.0, 0.0] + for i, q in zip(LEGS, legs): + self.state.motor_state[i].q = float(q) + self.samples = 0 + self._resume = 0.0 + self._stop = threading.Event() + self._pub = ChannelPublisher(topic, LowState_) + self._pub.Init() + self._thread = threading.Thread(target=self._loop, daemon=True) + self._thread.start() + + def set(self, quat=None, gyro=None, motor=None): + '''A new IMU quaternion or gyro, or motor = (slot, q)''' + with self.lock: + if quat is not None: + self.state.imu_state.quaternion = list(quat) + if gyro is not None: + self.state.imu_state.gyroscope = list(gyro) + if motor is not None: + self.state.motor_state[motor[0]].q = float(motor[1]) + + def pause(self, seconds): + with self.lock: + self._resume = time.monotonic() + seconds + + def _loop(self): + next_time = time.monotonic() + while not self._stop.is_set(): + with self.lock: + if time.monotonic() >= self._resume: + self.state.tick += 2 + self._pub.Write(self.state) + self.samples += 1 + next_time += 0.002 + time.sleep(max(0.0, next_time - time.monotonic())) + + def close(self): + self._stop.set() + self._thread.join() + self._pub.Close() + + +class FakeArms: + '''The arm controller: waist + arms held at 0 (kp 60) on the upper input at 100 Hz''' + + def __init__(self, topic): + self.cmd = LowCmdDefault() + for i in range(12, 29): + self.cmd.motor_cmd[i].mode = 1 + self.cmd.motor_cmd[i].kp = 60.0 + self.cmd.motor_cmd[i].kd = 1.5 + self._stop = threading.Event() + self._pub = ChannelPublisher(topic, LowCmd_) + self._pub.Init() + self._thread = threading.Thread(target=self._loop, daemon=True) + + def start(self): + self._thread.start() + + def _loop(self): + while not self._stop.is_set(): + self._pub.Write(self.cmd) + time.sleep(0.01) + + def close(self): + self._stop.set() + if self._thread.is_alive(): + self._thread.join() + self._pub.Close() + + +class Recorder: + '''Every LowCmd_ on a topic, with its receive time (monotonic)''' + + def __init__(self, topic): + self.msgs = [] + self._sub = ChannelSubscriber(topic, LowCmd_) + self._sub.Init(lambda m: self.msgs.append((time.monotonic(), m)), 50) + + def close(self): + self._sub.Close() + + +class Heartbeat: + ''' + The node's /safety/heartbeat subscription (LocomotionNode's), on `topic`: every + beat is kept in beats, (receive time, data), and handed to the driver while + `forwarding` is set + ''' + + def __init__(self, topic, driver, forwarding=True): + self.beats = [] + self.forwarding = threading.Event() + if forwarding: + self.forwarding.set() + self._driver = driver + self._context = rclpy.Context() + rclpy.init(context=self._context, domain_id=TEST_DOMAIN) + self._node = rclpy.create_node('test_locomotion_heartbeat', context=self._context) + self._node.create_subscription(Bool, topic, self._on_beat, 10) + self._executor = SingleThreadedExecutor(context=self._context) + self._executor.add_node(self._node) + self._thread = threading.Thread(target=self._executor.spin, daemon=True) + self._thread.start() + + def _on_beat(self, msg): + self.beats.append((time.monotonic(), msg.data)) + if self.forwarding.is_set(): + self._driver.on_heartbeat(msg.data) + + def close(self): + self._executor.shutdown() + self._thread.join(timeout=5.0) + self._node.destroy_node() + rclpy.shutdown(context=self._context) + + +class SafetyNode: + ''' + golem_safety's installed safety_node on TEST_DOMAIN, robot g1 on the config + at `config`, sim:=true (so it refuses domain 0) and no hardware estop, its + /safety/heartbeat remapped to `heartbeat`; a thread collects its log lines + ''' + + def __init__(self, config, heartbeat): + prefix = Path(get_package_prefix('golem_safety')) + executable = prefix / 'lib' / 'golem_safety' / 'safety_node' + env = dict(os.environ, ROS_DOMAIN_ID=str(TEST_DOMAIN), RCUTILS_COLORIZED_OUTPUT='0') + env.pop('RCUTILS_CONSOLE_OUTPUT_FORMAT', None) + env.pop('RCUTILS_LOGGING_USE_STDOUT', None) + self.lines = [] + self.exit_code = None + self.proc = subprocess.Popen( + [str(executable), '--ros-args', '-p', 'robot:=g1', '-p', f'config:={config}', + '-p', 'sim:=true', '-p', 'use_estop:=false', + '-r', f'/safety/heartbeat:={heartbeat}'], + env=env, text=True, stdout=subprocess.PIPE, stderr=subprocess.STDOUT) + self._reader = threading.Thread(target=self._read, daemon=True) + self._reader.start() + + def _read(self): + for line in self.proc.stdout: + self.lines.append(line.rstrip('\n')) + + def output(self): + return '\n'.join(self.lines) + + def events(self, names=None): + '''The JSON events the node logged, in order; only those called one of names, if given''' + events = [] + for line in list(self.lines): + match = LOG_LINE.match(line) + if match and match.group(3).startswith('{'): + event = json.loads(match.group(3)) + if names is None or event['event'] in names: + events.append(event) + return events + + def wait_for(self, name, timeout=15.0): + deadline = time.monotonic() + timeout + while not self.events((name,)): + assert self.proc.poll() is None, f'safety_node exited:\n{self.output()}' + assert time.monotonic() < deadline, f'no {name} within {timeout} s:\n{self.output()}' + time.sleep(0.02) + + def stop(self): + '''SIGINT, as Ctrl-C; SIGKILL if it hangs. Keeps the exit code''' + if self.proc.poll() is None: + self.proc.send_signal(signal.SIGINT) + try: + self.proc.wait(timeout=10.0) + except subprocess.TimeoutExpired: + self.proc.kill() + self.proc.wait() + self._reader.join(timeout=5.0) + self.exit_code = self.proc.returncode + + +class Rig: + ''' + What _rig() starts; log holds the node's (time, level, message), fed + every sample the driver gave the controller: (tick, time, whether it + returned targets), and latencies every command's (time, latency) as the + driver measured it. The heartbeat watchdog faults the node 0.5 s after + the safety node stops, so a test copies what it checks before the + teardown + ''' + + +@contextmanager +def _rig(legs, ramp_seconds=0.2, state_seconds=MAX_STATE_GAP, wait_for_confirm=False, + forwarding=True): + ''' + The node's controller and driver (g1.yaml, with a short ramp, no confirm + unless wait_for_confirm, and the relaxed timing above), its heartbeat + subscription (handing the beats to the driver from the start if + forwarding), golem_safety's safety_node on a copy of its split.yaml + (logging off; stale.state_seconds at state_seconds s, or split.yaml's for + None), a fake G1 and a fake arm controller (not started), on topics of + their own, so no channel of another test can match this run's. The safety + node is stopped first, so that the rest of the teardown adds nothing to + its log + ''' + _init_dds() + # a ROS name token must not start with a digit + suffix = f'/test_locomotion_dds/run{next(_runs)}' + split = Path(get_package_share_directory('golem_safety')) / 'config' / 'g1' / 'split.yaml' + safety = yaml.safe_load(split.read_text()) + safety['logging']['enabled'] = False + safety['topics'] = {key: name + suffix for key, name in safety['topics'].items()} + if state_seconds is not None: + safety['stale']['state_seconds'] = state_seconds + with open(CONFIG_DIR / 'g1.yaml') as f: + raw = yaml.safe_load(f) + raw['topics'] = {key: name + suffix for key, name in raw['topics'].items()} + raw['startup'] = {'ramp_seconds': ramp_seconds, 'wait_for_confirm': wait_for_confirm, + 'tick_rate': TICK_RATE} + raw['faults'] = {'max_state_gap': MAX_STATE_GAP, 'max_latency': MAX_LATENCY} + heartbeat = '/safety/heartbeat' + suffix + + rig = Rig() + rig.safety = safety + rig.policy = load_policy(str(POLICY_DIR)) + rig.config = check_config(raw, rig.policy.config['cmd_range']) + # the node's DDS topics are the safety node's ROS topics + for node_key, safety_key in (('low_cmd', 'low_cmd_lower_in'), + ('upper_cmd', 'low_cmd_upper_in'), + ('low_state', 'low_state')): + assert rig.config['topics'][node_key] == 'rt' + safety['topics'][safety_key] + rig.log = [] + controller = LocomotionController( + rig.policy, rig.config, + log=lambda level, message: rig.log.append((time.monotonic(), level, message))) + rig.fed = [] + on_lowstate = controller.on_lowstate + + def fed(state, tick, now, age=0.0): + targets = on_lowstate(state, tick, now, age) + rig.fed.append((tick, now, targets is not None)) + return targets + + controller.on_lowstate = fed + rig.latencies = [] + latency_ok = controller.latency_ok + + def timed(latency): + rig.latencies.append((time.monotonic(), latency)) + return latency_ok(latency) + + controller.latency_ok = timed + with tempfile.TemporaryDirectory() as tmp: + config = Path(tmp) / 'g1' / 'split.yaml' + config.parent.mkdir() + config.write_text(yaml.safe_dump(safety)) + rig.safety_node = SafetyNode(config, heartbeat) + # what started, closed in reverse order after the safety node, whatever + # fails while starting the rest + started = [] + try: + rig.lowcmd = Recorder('rt' + safety['topics']['low_cmd_out']) + started.append(rig.lowcmd.close) + rig.lower = Recorder(rig.config['topics']['low_cmd']) + started.append(rig.lower.close) + rig.robot = FakeG1(rig.config['topics']['low_state'], legs) + started.append(rig.robot.close) + rig.arms = FakeArms(rig.config['topics']['upper_cmd']) + started.append(rig.arms.close) + rig.driver = LocomotionDriver(controller, rig.config['topics'], TEST_DOMAIN) + started.append(rig.driver.shutdown) + rig.heartbeat = Heartbeat(heartbeat, rig.driver, forwarding) + started.append(rig.heartbeat.close) + rig.safety_node.wait_for('relay_started') + # discovery between this process and the safety node's + time.sleep(0.5) + yield rig + finally: + rig.safety_node.stop() + for close in reversed(started): + close() + + +def _wait_for(condition, timeout=3.0): + deadline = time.monotonic() + timeout + while not condition(): + assert time.monotonic() < deadline, 'timed out' + time.sleep(0.01) + + +def _legs(msg): + return np.array([msg.motor_cmd[i].q for i in LEGS]) + + +def _is_off(motors): + return all(m.mode == 0 and m.q == m.dq == m.tau == m.kp == m.kd == 0.0 for m in motors) + + +def _is_zeroed(msg): + '''Every motor off: golem_safety's zero-gain stream, and its estop command''' + return _is_off(msg.motor_cmd[:29]) + + +def _alarms(rig): + '''golem_safety's estops and quiet reports (ALARMS) of the run; the node must have exited 0''' + assert rig.safety_node.exit_code == 0, rig.safety_node.output() + return rig.safety_node.events(ALARMS) + + +def test_silent_until_upright_arms_and_heartbeat_then_ramp_hold_and_walk(): + default = np.asarray(load_policy(str(POLICY_DIR)).default_angles, dtype=np.float64) + legs = default + np.linspace(-0.2, 0.2, 12) + with _rig(legs, ramp_seconds=0.2, wait_for_confirm=True, forwarding=False) as rig: + kp, kd = rig.policy.gains(rig.policy.leg_joints) + # upright rt/lowstate but no arm controller: the node is silent, and + # golem_safety streams zero gains on every motor + time.sleep(0.4) + assert rig.lower.msgs == [] + assert rig.lowcmd.msgs and all(_is_zeroed(m) for _, m in rig.lowcmd.msgs) + # the arm controller commands, but the robot is tilted 40 deg: the node + # stays silent, and golem_safety relays the arms with the legs still off + rig.robot.set(quat=_tilted(40.0)) + rig.arms.start() + time.sleep(0.4) + assert rig.lower.msgs == [] + assert rig.lowcmd.msgs[-1][1].motor_cmd[SHOULDER].kp == 60.0 + assert all(_is_off(m.motor_cmd[:12]) for _, m in rig.lowcmd.msgs) + assert rig.driver.phase == WAITING + + # upright, and golem_safety beats, but its beats do not reach the node: + # still silent + rig.robot.set(quat=[1.0, 0.0, 0.0, 0.0]) + time.sleep(1.5) + assert rig.lower.msgs == [] and rig.driver.phase == WAITING + beats = list(rig.heartbeat.beats) + assert len(beats) >= 10 and all(data for _, data in beats) + assert [m for _, _, m in rig.log if m.startswith('waiting to start: no /safety/heartbeat')] + + # the beats reach it: the ramp (0.2 s, 11 targets), then the hold at the + # default pose until 1 s of steady samples and the operator's confirm, + # then the policy + rig.heartbeat.forwarding.set() + _wait_for(lambda: rig.driver.phase == HOLDING) + time.sleep(1.0) + assert rig.driver.phase == HOLDING + assert rig.driver.confirm() == ( + True, 'confirmed: the policy starts on the next policy step') + confirmed = time.monotonic() + assert rig.driver.phase == WALKING + time.sleep(0.3) + start_samples, start = rig.robot.samples, time.monotonic() + time.sleep(1.0) + samples, end = rig.robot.samples - start_samples, time.monotonic() + lower = list(rig.lower.msgs) + lowcmd = list(rig.lowcmd.msgs) + fed = list(rig.fed) + log = list(rig.log) + assert rig.driver.fault is None, rig.driver.fault + + # the ramp starts at the measured legs and moves each one monotonically to + # the default pose, one target per policy step + ramp = np.array([_legs(m) for _, m in lower[:11]]) + np.testing.assert_allclose(ramp[0], legs, atol=1e-6) + np.testing.assert_allclose(ramp[-1], default, atol=1e-6) + toward = np.sign(default - legs) + assert np.all(np.diff(ramp, axis=0) * toward >= -1e-6), 'must move toward the defaults' + steps = np.diff([t for t, _ in lower[:11]]) + assert 0.01 < np.median(steps) < 0.03, f'one target per policy step, not {np.median(steps)} s' + # then the default pose, at 50 Hz, until the confirm + held = [(t, m) for t, m in lower[11:] if t < confirmed] + assert abs(len(held) - 50) <= 5, len(held) + for _, msg in held: + np.testing.assert_allclose(_legs(msg), default, atol=1e-6) + + # every leg command: slots 0-11 only, mode 1, dq = tau = 0, the training gains + for _, msg in lower: + for i in LEGS: + m = msg.motor_cmd[i] + assert (m.mode, m.dq, m.tau) == (1, 0.0, 0.0) + assert m.kp == pytest.approx(kp[i]) and m.kd == pytest.approx(kd[i]) + assert _is_off(msg.motor_cmd[12:]) + + # one command per 20 ms of tick: 50 Hz on a 500 Hz robot, one per 10 samples + walking = [t for t, _ in lower if start <= t < end] + assert abs(len(walking) - samples / SAMPLES_PER_STEP) <= 3, (len(walking), samples) + rate = len(walking) / (end - start) + assert 40.0 < rate < 55.0, f'{rate:.1f} Hz' + steps = [tick for tick, t, stepped in fed if stepped and start <= t < end] + gaps = [tick_delta(b, a) for a, b in zip(steps, steps[1:])] + assert min(gaps) >= 10 and abs(np.mean(gaps) - 20.0) < 1.0, gaps + + # golem_safety engaged the arm controller's input first and the node's on + # its first command (the measured legs), each on its own; the legs were + # off until then, and from then on carry the node's legs with the training + # gains beside the arm controller's arms. Every command is stamped with the + # G1 profile's mode_pr 0 and mode_machine 5 + assert [e['source'] for e in rig.safety_node.events(('input_engaged',))] == [ + 'split_mode_upper', 'split_mode_lower'] + first = next(i for i, (_, m) in enumerate(lowcmd) if m.motor_cmd[LEGS[0]].kp > 0.0) + assert all(_is_off(m.motor_cmd[:12]) for _, m in lowcmd[:first]) + assert all((m.mode_pr, m.mode_machine) == (0, 5) for _, m in lowcmd[:first]) + np.testing.assert_allclose(_legs(lowcmd[first][1]), legs, atol=1e-6) + sent = {tuple(np.round(_legs(m), 6)) for _, m in lower} + for _, msg in lowcmd[first:]: + assert (msg.mode_pr, msg.mode_machine) == (0, 5) + assert tuple(np.round(_legs(msg), 6)) in sent + assert all(msg.motor_cmd[i].mode == 1 and msg.motor_cmd[i].kp == pytest.approx(kp[i]) and + msg.motor_cmd[i].kd == pytest.approx(kd[i]) for i in LEGS) + assert msg.motor_cmd[SHOULDER].kp == 60.0 + assert _alarms(rig) == [] + assert [m for _, level, m in log if level == 'error'] == [] + + +def _nan_gyro(rig): + rig.robot.set(gyro=[math.nan, 0.0, 0.0]) + + +def _fall(rig): + rig.robot.set(quat=_tilted(70.0)) + + +def _stale(rig): + # no rt/lowstate for longer than the node allows, then it comes back + rig.robot.pause(MAX_STATE_GAP + 0.15) + + +class SlowSession: + '''An ONNX session whose runs take longer than faults.max_latency''' + + def __init__(self, session): + self._session = session + + def run(self, output_names, feed): + time.sleep(MAX_LATENCY + 0.05) + return self._session.run(output_names, feed) + + +def _slow_policy(rig): + rig.policy.session = SlowSession(rig.policy.session) + + +@pytest.mark.parametrize('inject, reason', [ + (_stale, 'rt/lowstate stale'), + (_nan_gyro, 'rt/lowstate gyro is not finite'), + (_fall, 'fell: gravity z'), + (_slow_policy, 'command late: 3'), +], ids=['stale', 'not finite', 'fallen', 'late']) +def test_bad_state_silences_the_node_and_golem_safety_holds_its_last_command(inject, reason): + default = load_policy(str(POLICY_DIR)).default_angles + # a stale rt/lowstate meets split.yaml's own quiet-rt/lowstate time; the + # other faults leave rt/lowstate flowing + state_seconds = None if inject is _stale else MAX_STATE_GAP + with _rig(default, ramp_seconds=0.1, state_seconds=state_seconds) as rig: + lower_quiet = rig.safety['stale']['lower_seconds'] + state_quiet = rig.safety['stale']['state_seconds'] + kp, kd = rig.policy.gains(rig.policy.leg_joints) + rig.arms.start() + _wait_for(lambda: rig.driver.phase == WALKING) + time.sleep(0.3) + inject_time = time.monotonic() + inject(rig) + time.sleep(MAX_STATE_GAP + lower_quiet + LATE_S) + end = time.monotonic() + lower = list(rig.lower.msgs) + lowcmd = list(rig.lowcmd.msgs) + log = list(rig.log) + fault = rig.driver.fault + assert rig.driver.phase == FAULTED and fault.startswith(reason), fault + + # the node latched once, soon after the fault, and sent nothing after that + errors = [(t, m) for t, level, m in log if level == 'error'] + assert len(errors) == 1 and errors[0][1].startswith(f'fault: {reason}'), errors + fault_time = errors[0][0] + assert inject_time < fault_time < inject_time + MAX_STATE_GAP + 0.15 + last_lower = lower[-1][0] + assert last_lower < fault_time + 0.05, 'nothing may be sent after the fault' + # never a zero-gain or damping command from the node itself + assert all(msg.motor_cmd[i].kp > 0.0 for _, msg in lower for i in LEGS) + + # golem_safety holds a quiet input: from the first input it engaged on (before + # it, the zero-gain stream; the legs may engage before the arms), it went on + # relaying the node's last leg command, with the training gains, to the end, + # and never the estop command + first = next(i for i, (_, msg) in enumerate(lowcmd) if not _is_zeroed(msg)) + assert not any(_is_zeroed(msg) for _, msg in lowcmd[first:]) + held = [(t, msg) for t, msg in lowcmd if t > last_lower + 0.05] + assert held, 'golem_safety must keep relaying' + last_legs = _legs(lower[-1][1]) + for _, msg in held: + np.testing.assert_allclose(_legs(msg), last_legs, atol=1e-6) + assert all(msg.motor_cmd[i].mode == 1 and msg.motor_cmd[i].kp == pytest.approx(kp[i]) and + msg.motor_cmd[i].kd == pytest.approx(kd[i]) for i in LEGS) + assert held[0][0] < last_lower + 0.15 and held[-1][0] > end - 0.1, ( + last_lower, held[0][0], held[-1][0], end) + rate = (len(held) - 1) / (held[-1][0] - held[0][0]) + assert rate > 250.0, f'{rate:.0f} Hz of held commands; golem_safety relays at 500 Hz' + + # and reported the quiet leg input once, lower_quiet s after the node's last + # command (its age is rounded to 4 decimals), without an estop and with no + # command to resume it + events = rig.safety_node.events() + quiet = [e for e in events if e['event'] == 'input_quiet'] + assert [(e['source'], e['topic'], e['seconds']) for e in quiet] == [ + ('split_mode_lower', rig.safety['topics']['low_cmd_lower_in'], lower_quiet)], quiet + assert lower_quiet <= quiet[0]['age'] < lower_quiet + LATE_S, quiet + assert [e for e in events if e['event'] == 'input_resumed'] == [] + assert [e for e in events if e['event'].startswith('estop')] == [] + assert rig.safety_node.exit_code == 0, rig.safety_node.output() + # a stale rt/lowstate is reported too, and its return; the relay never waited for it + states = [(e['event'], e['topic']) for e in events + if e['event'] in ('state_quiet', 'state_resumed')] + if inject is _stale: + assert states == [('state_quiet', rig.safety['topics']['low_state']), + ('state_resumed', rig.safety['topics']['low_state'])], states + (reported,) = [e for e in events if e['event'] == 'state_quiet'] + assert reported['seconds'] == state_quiet, reported + assert state_quiet <= reported['age'] < state_quiet + LATE_S, reported + else: + assert states == [], states + + +def test_a_backlog_is_dropped_and_the_cadence_kept(): + default = load_policy(str(POLICY_DIR)).default_angles + with _rig(default, ramp_seconds=0.1) as rig: + rig.arms.start() + _wait_for(lambda: rig.driver.phase == WALKING) + time.sleep(0.3) + # the worker is held up for 120 ms while about 60 samples arrive + # (golem_safety reports a leg input quiet for 200 ms, its + # stale.lower_seconds) + with rig.driver._lock: + before = rig.robot.state.tick + time.sleep(0.12) + newest = rig.robot.state.tick + release = time.monotonic() + time.sleep(0.5) + fed = list(rig.fed) + lower = list(rig.lower.msgs) + latencies = list(rig.latencies) + log = list(rig.log) + fault = rig.driver.fault + assert fault is None, fault + assert tick_delta(newest, before) >= 50, (before, newest) + + # none of the samples that arrived while it was held reached the + # controller: it went on from the newest + ticks = [tick for tick, _, _ in fed] + assert not [t for t in ticks if 0 < tick_delta(t, before) < tick_delta(newest, before) - 2] + after = [(tick, stepped) for tick, t, stepped in fed if t >= release] + assert -2 <= tick_delta(after[0][0], newest) <= 10, (after[0], newest) + # and the policy stepped on it at once: one command, not a burst of the + # six the backlog held (the next step is at least 10 ms of tick later), + # answering a sample written a few ms before + assert after[0][1], 'the policy must step on the newest sample' + burst = [t for t, _ in lower if release <= t < release + 0.008] + assert len(burst) <= 1, burst + first_latency = next(latency for t, latency in latencies if t >= release) + assert first_latency < 0.02, first_latency + # 50 Hz of tick before and after + steps = [tick for tick, _, stepped in fed if stepped] + gaps = [tick_delta(b, a) for a, b in zip(steps, steps[1:])] + assert min(gaps) >= 10, gaps + assert sorted(gaps)[len(gaps) // 2] == 20, gaps + assert [m for _, level, m in log if level == 'error'] == [] + assert _alarms(rig) == [] + + +def test_the_latency_counts_the_time_a_sample_waited_for_the_node(): + default = load_policy(str(POLICY_DIR)).default_angles + with _rig(default, ramp_seconds=0.1) as rig: + rig.arms.start() + _wait_for(lambda: rig.driver.phase == WALKING) + time.sleep(0.3) + # the worker is held up, and rt/lowstate stops 30 ms in for 150 ms: + # when the worker goes on, the newest sample is the one written 90 ms + # before (within the relaxed gap and golem_safety's quiet-rt/lowstate + # time, and its 200 ms quiet-leg time) + with rig.driver._lock: + time.sleep(0.03) + rig.robot.pause(0.15) + paused = time.monotonic() + time.sleep(0.09) + release = time.monotonic() + time.sleep(0.4) + latencies = list(rig.latencies) + fault = rig.driver.fault + assert fault is None, fault + # the command answering it is measured from the sample's DDS write, so + # it counts the 90 ms the sample waited, not just the node's own work + waited = release - paused + first = next(latency for t, latency in latencies if t >= release) + assert waited - 0.01 < first < waited + 0.1, (first, waited) + # before and after, well under that: a median of 1-4 ms, up to 7 ms seen + # when the host was loaded (this process's threads share the GIL) + others = [latency for t, latency in latencies if not release <= t < release + 0.05] + assert sorted(others)[len(others) // 2] < 0.02, sorted(others) + assert _alarms(rig) == [] + + +def _stop_safety(rig): + rig.safety_node.stop() + + +def _latch_safety(rig): + # left hip pitch at 3.5 rad, past split.yaml's limits.estop (its URDF range, to + # 2.88 rad): golem_safety latches its estop, and its heartbeat turns false + rig.robot.set(motor=(0, 3.5)) + + +@pytest.mark.parametrize('inject, reason', [ + (_stop_safety, 'no /safety/heartbeat for '), + (_latch_safety, "golem_safety's /safety/heartbeat is false: its estop has latched"), +], ids=['safety stopped', 'estop latched']) +def test_losing_the_heartbeat_stops_the_node(inject, reason): + default = load_policy(str(POLICY_DIR)).default_angles + with _rig(default, ramp_seconds=0.1) as rig: + rig.arms.start() + _wait_for(lambda: rig.driver.phase == WALKING) + time.sleep(0.3) + inject_time = time.monotonic() + inject(rig) + _wait_for(lambda: rig.driver.phase == FAULTED) + time.sleep(0.3) + lower = list(rig.lower.msgs) + beats = list(rig.heartbeat.beats) + log = list(rig.log) + fault = rig.driver.fault + assert fault.startswith(reason), fault + + # the node latched once and sent nothing after that + errors = [(t, m) for t, level, m in log if level == 'error'] + assert len(errors) == 1 and errors[0][1].startswith(f'fault: {reason}'), errors + fault_time = errors[0][0] + assert lower[-1][0] < fault_time + 0.05, 'nothing may be sent after the fault' + assert rig.safety_node.exit_code == 0, rig.safety_node.output() + if inject is _stop_safety: + # 0.5 s after the last beat, the watchdog's heartbeat.timeout + last_beat = beats[-1][0] + assert last_beat < fault_time + assert 0.5 <= fault_time - last_beat < 0.5 + LATE_S, (last_beat, fault_time) + else: + # golem_safety latched and wrote its estop command; its next beat, false, + # stopped the node + assert rig.safety_node.events(('estop_command_written',)) + first_false = next(t for t, data in beats if not data) + assert inject_time < first_false <= fault_time < first_false + LATE_S diff --git a/core_ws/src/g1_homie/test/test_locomotion_node.py b/core_ws/src/g1_homie/test/test_locomotion_node.py new file mode 100644 index 0000000..ea77a99 --- /dev/null +++ b/core_ws/src/g1_homie/test/test_locomotion_node.py @@ -0,0 +1,247 @@ +''' +The ROS node inside one process: parameters, config, logging, the command topics, the +heartbeat, the operator's confirm and the band release +''' + +import time +from pathlib import Path + +import pytest +import rclpy +import yaml +from rclpy.executors import SingleThreadedExecutor +from std_msgs.msg import Bool, Float32 +from std_srvs.srv import Trigger +from geometry_msgs.msg import PoseStamped, Twist + +from g1_homie.locomotion import WAITING +from g1_homie.locomotion_node import LocomotionNode + +# the package's policy/ (the containers also mount it at /home/code/g1_homie) +POLICY_DIR = Path(__file__).resolve().parents[1] / 'policy' +CONFIG_DIR = Path(__file__).resolve().parents[1] / 'config' +TEST_DOMAIN = 97 # never 0 (the real robot) or 1 (the sim) + + +class FakeDriver: + '''Stands in for the node's LocomotionDriver: records what the topics set''' + + def __init__(self, walking_since=None): + self.cmd_vel = [] + self.heights = [] + self.beats = [] + self.band_released = 0 + self.walking_since = walking_since + + def on_band_released(self): + self.band_released += 1 + + def set_cmd_vel(self, vx, vy, yaw): + self.cmd_vel.append((vx, vy, yaw)) + + def set_height(self, height): + self.heights.append(height) + + def on_heartbeat(self, beat): + self.beats.append(beat) + + def shutdown(self): + pass + + +def _node_config(tmp_path, monkeypatch, band_release=False): + '''g1.yaml on domain 97, on topics of its own; band.release as given''' + monkeypatch.setenv('ROS_DOMAIN_ID', str(TEST_DOMAIN)) + with open(CONFIG_DIR / 'g1.yaml') as f: + raw = yaml.safe_load(f) + raw['network']['domain_id'] = TEST_DOMAIN + raw['topics'] = {key: name + '/test_locomotion_node' for key, name in raw['topics'].items()} + raw['band']['release'] = band_release + config = tmp_path / 'node.yaml' + with open(config, 'w') as f: + yaml.safe_dump(raw, f) + return config + + +@pytest.fixture +def node(tmp_path, monkeypatch): + config = _node_config(tmp_path, monkeypatch) + rclpy.init(args=['--ros-args', '-p', f'policy_dir:={POLICY_DIR}'], domain_id=TEST_DOMAIN) + node = LocomotionNode(config_name=str(config)) + try: + yield node + finally: + node.destroy_node() + rclpy.shutdown() + + +def _graph(node): + '''The node's own (subscriptions, services, clients), by topic or service name''' + names = (node.get_subscriber_names_and_types_by_node, + node.get_service_names_and_types_by_node, + node.get_client_names_and_types_by_node) + return [[name for name, _ in get('locomotion_node', '/')] for get in names] + + +def test_node_starts_silent_and_logs_every_severity(node): + assert node.get_parameter('policy_dir').value == str(POLICY_DIR) + assert node._driver.phase == WAITING + # rclpy refuses a change of severity at one call site, which once hid a fault + for level in ('info', 'warn', 'error', 'info', 'error'): + node._log(level, f'test {level}') + # g1.yaml: the heartbeat and the confirm, and no band (no /left_ee_pose, no toggle client) + deadline = time.monotonic() + 5.0 + subscriptions, services, clients = _graph(node) + while not ('/safety/heartbeat' in subscriptions and '/lowerbody/confirm_engage' in services): + assert time.monotonic() < deadline, (subscriptions, services) + time.sleep(0.05) + subscriptions, services, clients = _graph(node) + assert '/left_ee_pose' not in subscriptions and '/elastic_band/toggle' not in clients + + +def test_topics_reach_the_driver(node): + node._driver.shutdown() + node._driver = fake = FakeDriver() + publisher = rclpy.create_node('test_locomotion_publisher') + executor = SingleThreadedExecutor() + executor.add_node(node) + executor.add_node(publisher) + try: + cmd_vel = publisher.create_publisher(Twist, '/cmd_vel', 10) + height = publisher.create_publisher(Float32, '/locomotion/height', 10) + heartbeat = publisher.create_publisher(Bool, '/safety/heartbeat', 10) + twist = Twist() + twist.linear.x, twist.linear.y, twist.angular.z = 0.3, -0.1, 0.2 + twist.linear.z = twist.angular.x = twist.angular.y = 9.0 # not used + deadline = time.monotonic() + 5.0 + while (not (fake.cmd_vel and fake.heights and fake.beats) and + time.monotonic() < deadline): + cmd_vel.publish(twist) + height.publish(Float32(data=0.6)) + heartbeat.publish(Bool(data=True)) + executor.spin_once(timeout_sec=0.05) + assert fake.cmd_vel and fake.cmd_vel[0] == pytest.approx((0.3, -0.1, 0.2)) + assert fake.heights and fake.heights[0] == pytest.approx(0.6) + assert fake.beats and set(fake.beats) == {True} + finally: + executor.shutdown() + publisher.destroy_node() + + +def test_sim_time_is_ignored(tmp_path, monkeypatch): + # the sim bringup passes use_sim_time:=true; the node keeps the monotonic + # clock and so has no /clock subscription (500 Hz in the sim) + config = _node_config(tmp_path, monkeypatch) + rclpy.init(args=['--ros-args', '-p', f'policy_dir:={POLICY_DIR}', '-p', 'use_sim_time:=true'], + domain_id=TEST_DOMAIN) + node = LocomotionNode(config_name=str(config)) + try: + assert node.get_parameter('use_sim_time').value is False + deadline = time.monotonic() + 5.0 + topics = [] + while '/cmd_vel' not in topics and time.monotonic() < deadline: + topics = [name for name, _ in + node.get_subscriber_names_and_types_by_node('locomotion_node', '/')] + time.sleep(0.05) + assert '/cmd_vel' in topics and '/locomotion/height' in topics + assert '/clock' not in topics + finally: + node.destroy_node() + rclpy.shutdown() + + +def _spin(executor, seconds, done=lambda: False): + deadline = time.monotonic() + seconds + while not done() and time.monotonic() < deadline: + executor.spin_once(timeout_sec=0.02) + return done() + + +def test_confirm_service_and_band_client(tmp_path, monkeypatch): + # g1.yaml with band.release on, as sim.yaml has it + config = _node_config(tmp_path, monkeypatch, band_release=True) + rclpy.init(args=['--ros-args', '-p', f'policy_dir:={POLICY_DIR}'], domain_id=TEST_DOMAIN) + node = LocomotionNode(config_name=str(config)) + helper = rclpy.create_node('test_locomotion_helper') + executor = SingleThreadedExecutor() + executor.add_node(node) + executor.add_node(helper) + toggles = [] + + def toggle(request, response): + # the band was on: the call takes it off + toggles.append(time.monotonic()) + response.success = True + response.message = 'disabled' + return response + + try: + helper.create_service(Trigger, '/elastic_band/toggle', toggle) + confirm = helper.create_client(Trigger, '/lowerbody/confirm_engage') + pose = helper.create_publisher(PoseStamped, '/left_ee_pose', 10) + + # the operator's confirm, refused while the node waits to start + assert _spin(executor, 5.0, confirm.service_is_ready) + future = confirm.call_async(Trigger.Request()) + assert _spin(executor, 5.0, future.done) + assert not future.result().success and 'waiting' in future.result().message + + # the policy runs: no toggle without /left_ee_pose (band.max_wait is 30 s) + node._driver.shutdown() + node._driver = FakeDriver(walking_since=time.monotonic()) + assert not _spin(executor, 0.5, lambda: toggles) + # one /left_ee_pose: frame_task_server is up, and the band is released, once; the + # policy's history restarts once the band is off + assert _spin(executor, 5.0, lambda: pose.get_subscription_count() > 0) + pose.publish(PoseStamped()) + assert _spin(executor, 5.0, lambda: toggles) + _spin(executor, 1.0) + assert len(toggles) == 1, toggles + assert node._driver.band_released == 1 + finally: + executor.shutdown() + helper.destroy_node() + node.destroy_node() + rclpy.shutdown() + + +def test_band_toggle_that_hangs_the_robot_again_warns_once(tmp_path, monkeypatch): + # the band was already off (SPACE in the viewer): the toggle hangs the robot again and + # answers 'enabled'. The node warns, keeps the policy's history and never calls again + config = _node_config(tmp_path, monkeypatch, band_release=True) + rclpy.init(args=['--ros-args', '-p', f'policy_dir:={POLICY_DIR}'], domain_id=TEST_DOMAIN) + node = LocomotionNode(config_name=str(config)) + helper = rclpy.create_node('test_locomotion_helper') + executor = SingleThreadedExecutor() + executor.add_node(node) + executor.add_node(helper) + toggles = [] + log = [] + node._log = lambda level, message: log.append((level, message)) + + def toggle(request, response): + toggles.append(time.monotonic()) + response.success = True + response.message = 'enabled' + return response + + try: + helper.create_service(Trigger, '/elastic_band/toggle', toggle) + pose = helper.create_publisher(PoseStamped, '/left_ee_pose', 10) + node._driver.shutdown() + node._driver = FakeDriver(walking_since=time.monotonic()) + assert _spin(executor, 5.0, node._band_client.service_is_ready) + assert _spin(executor, 5.0, lambda: pose.get_subscription_count() > 0) + pose.publish(PoseStamped()) + assert _spin(executor, 5.0, lambda: toggles) + _spin(executor, 1.0) + assert len(toggles) == 1, toggles + assert node._driver.band_released == 0 + warnings = [message for level, message in log if level == 'warn' and '"enabled"' in message] + assert len(warnings) == 1 and 'is now ON' in warnings[0], log + assert not any(message.startswith('elastic band released') for _, message in log), log + finally: + executor.shutdown() + helper.destroy_node() + node.destroy_node() + rclpy.shutdown() diff --git a/core_ws/src/golem_bringup/LICENSE b/core_ws/src/golem_bringup/LICENSE new file mode 100644 index 0000000..30e8e2e --- /dev/null +++ b/core_ws/src/golem_bringup/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/common_cmds.md b/core_ws/src/golem_bringup/common_cmds.md similarity index 53% rename from core_ws/src/h1_bringup/common_cmds.md rename to core_ws/src/golem_bringup/common_cmds.md index 8540f9c..00f101c 100644 --- a/core_ws/src/h1_bringup/common_cmds.md +++ b/core_ws/src/golem_bringup/common_cmds.md @@ -23,6 +23,119 @@ actually want a bound. Ctrl-C on `send_goal` cancels the in-flight goal. --- +## Real robot: pick up a screwdriver by its handle + +Two machines on `ROS_DOMAIN_ID=0`, run natively (no Docker). Every terminal on +both machines first runs: + +```bash +export ROS_DOMAIN_ID=0 +source ~/GOLEM/core_ws/install/setup.bash +``` + +**1. Onboard PC — launch** (robot on the gantry, legs hanging free): + +```bash +ros2 launch golem_bringup h12_real_robot.launch.py start_position_verified:=true +``` + +The leg controller logs `waiting for /safety/heartbeat before pre-posing the +legs`, ramps the legs into the ALMI crouch, then holds and logs +`legs pre-posed (settled, err=...) — holding`. If it reports `NOT SETTLED` or +the legs jump, Ctrl-C and do not engage. + +Two start-up failures to watch for in this terminal: + +- **The estop box must be connected before `safety_node` starts.** + `estop_node` starts at once and `safety_node` (`golem_safety`) 3 s later + (`safety_delay`). `estop_node` publishes nothing until its first attempt to + connect ends, and while it is not connected it publishes unplugged and + triggered. A `safety_node` that sees that, because the first attempt failed + and no retry had connected by then, or the attempt outlasted the 3 s and then + failed, latches its estop until restart (`estop_command_written` with the + reason `External estop triggered`). Other serial devices, such as the gripper + boards, lengthen the attempt. Restart the whole bringup. An attempt that + outlasts the 3 s and then connects only delays the start: `safety_node` + publishes no `/safety/heartbeat` before the first estop sample (and a + `/lowstate` reporting `mode_machine` 6), so the leg controller keeps logging + `waiting for /safety/heartbeat` and the legs stay at zero gain until then. +- **The arms start only once `golem_safety` relays them.** `frame_task_server` + (`golem_upper_ik`) holds the arms where they are until it sees its commands + on `/lowcmd`. If that has not happened within 30 s + (`controller.startup_timeout`), it exits with an error that lists what + `golem_safety` waits for: `/lowstate` reporting `mode_machine` 6, a first + estop sample, the paired safety config, no estop latched. Meanwhile + `safety_node` logs `engage_refused` with the reason, at most once a second. + The rest of the bringup keeps running, without `/frame_task`. + +The leg controller's input is held like any other: if it crashes or stops, +`golem_safety` keeps relaying its last leg command and only reports it +(`input_quiet` after `relax_safety_split`'s `stale.lower_seconds`, 0.2 s), so the +legs stay on their last target until `golem_safety`'s estop latches or the +bringup stops. Its estop latches on a `/lowstate` joint past `limits.estop` (a +fall can drive one there) or on the hardware estop (triggered, unplugged, or no +sample for 0.5 s), and then writes 50 zero-gain commands and goes silent. + +**2. Onboard PC, second terminal — engage** (feet loaded, gantry slack): + +```bash +ros2 service call /lowerbody/confirm_engage std_srvs/srv/Trigger +``` + +**3. Desktop — perception and skills:** + +```bash +ros2 launch golem_bringup h12_real_desktop.launch.py +# add use_hand_cameras:=true when the grasp uses visual_servo +``` + +Wait for `golem_skills` to report ready; it looks for frame_task, the grippers +and the model servers on startup. + +**4. Grasp.** Lay the screwdriver flat in reach of the arm, shaft pointing +straight away from the robot. `top_down` closes the fingers along pelvis ±Y +whatever the object's orientation, so this puts them across the handle. Name +the **handle**, not the tool: the grasp lands on the centroid of what Gemini + +SAM segment, and for the whole screwdriver that sits near the handle–shaft +joint. Pick an arm whose gripper node is running (`ros2 node list | grep +gripper`). + +```bash +ros2 action send_goal /skill/grasp custom_ros_messages/action/SkillGrasp \ + "{target_object: 'screwdriver handle', arm: 'right', top_down: false, timeout: {sec: 180, nanosec: 0}}" \ + --feedback +``` + +Drop `top_down: true` to try the heuristic side approaches instead. + +**5. Lift.** The grasp skill closes the gripper and stops. Raise the hand +straight up from where it closed with a relative `frame_task` goal. With +`relative: true` each target is an offset from the frame's current pose: the +position is added in pelvis axes (`z: 0.05` = 5 cm up), and an identity +orientation (`w: 1.0`, not all zeros) keeps the wrist as it is. Use +`left_graspgenx_frame` for the left arm. + +```bash +ros2 action send_goal /frame_task custom_ros_messages/action/FrameTask \ + "{relative: true, plan: false, frame_names: ['right_graspgenx_frame'], + frame_targets: [{position: {x: 0.0, y: 0.0, z: 0.05}, + orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}}], + duration: {sec: 3, nanosec: 0}}" \ + --feedback +``` + +Then carry the handle back by raising the arm to `prep`: + +```bash +ros2 action send_goal /named_config custom_ros_messages/action/NamedConfig \ + "{config_name: 'prep', plan: true, slow_mode: true, duration: {sec: 30, nanosec: 0}}" \ + --feedback +``` + +Ctrl-C on a `send_goal` cancels that arm motion; the e-stop stops everything. + +--- + ## Frontier exploration (`/skill/frontier_explore`) Autonomous frontier-based exploration over the slam_toolbox `/map`. Needs nav2 + @@ -78,17 +191,36 @@ Optional goal fields (omit / `false` / `0` = off): | Field | Meaning | |---|---| -| `top_down` | skip GraspGenX; build ONE synthetic grasp above the object — the tier-1 orientation (forward azimuth, fingers across pelvis ±Y, level wrist) pitched `TOP_DOWN_PITCH_DEG` = 80° below horizontal, base backed 0.1146 m off along that approach so the fingers close on the cloud centroid. Still steeper than the arm's ~45° practical reach, and there is no second candidate to fall back to, so an unreachable pose aborts the skill. | -| `min_downward_pitch_deg` | keep only GraspGenX grasps approaching at least this far below horizontal (a *minimum* pitch, not proximity to vertical) | -| `visual_servo` | at the pre-grasp, close the loop on the arm's hand camera: translate (orientation frozen) until the object sits at a fixed setpoint in the camera optical frame — `VISUAL_SERVO_TARGET_XY_M` = (+12, 0) mm laterally, `VISUAL_SERVO_DEPTH_M` = 0.10 m in range — then close **there** instead of running the contact descent. Tolerances are **per axis**, because the camera axes mean different things to a parallel gripper: 1 mm across the jaws (optical X — what a grasp depends on), 2 mm in range (optical Z — whether the jaws close around the part or above it), 5 mm along the jaws (optical Y — the jaws span 106 mm, so this is the forgiving one). 90 s budget (~50 corrections), and it gives up early if the error stops improving. If it loses sight of the object for 5 s, **or** reads it closer than 5.5 cm on two consecutive frames, it backs straight up 5 cm (pelvis +Z) and keeps servoing — up to twice between the two cases. If it still can't converge the skill **aborts** — there is no open-loop fallback, so the result tells you the grasp was never aligned. Only works for the 7 YOLO battery classes — the hand detector knows nothing else. | +| `top_down` | use the `top_down` synthesis method for this goal, overriding `grasp_method` (below): ONE synthetic grasp above the object — the tier-1 orientation (forward azimuth, fingers across pelvis ±Y, level wrist) pitched `TOP_DOWN_PITCH_DEG` = 80° below horizontal, base backed 0.1146 m off along that approach so the fingers close on the cloud centroid. Still steeper than the arm's ~45° practical reach, and there is no second candidate to fall back to, so an unreachable pose aborts the skill. | +| `min_downward_pitch_deg` | every synthesis method keeps only grasps approaching at least this far below horizontal (a *minimum* pitch, not proximity to vertical) | +| `visual_servo` | at the pre-grasp, close the loop on the arm's hand camera: translate (orientation frozen) until the object sits at a fixed setpoint in the camera optical frame — `VISUAL_SERVO_TARGET_XY_M` = (+12, 0) mm laterally, `VISUAL_SERVO_DEPTH_M` = 0.10 m in range — then close **there** instead of running the contact descent. Tolerances are **per axis**, because the camera axes mean different things to a parallel gripper: 1 mm across the jaws (optical X — what a grasp depends on), 2 mm in range (optical Z — whether the jaws close around the part or above it), 5 mm along the jaws (optical Y — the jaws span 106 mm, so this is the forgiving one). 90 s budget (~50 corrections), and it gives up early if the error stops improving. If it loses sight of the object for 5 s, **or** reads it closer than 5.5 cm on two consecutive frames, it backs straight up 5 cm (pelvis +Z) and keeps servoing — up to twice between the two cases. If it still can't converge the skill **aborts** — there is no open-loop fallback, so the result tells you the grasp was never aligned. The 7 YOLO battery classes are boxed by the hand YOLO detector; any other object is boxed by Gemini on the hand frame (seconds per correction). Both need `yolo_server` running, which supplies the hand depth. | -Small battery-workcell parts (`Bolt`, `BusBar`, `InteriorScrew`, `Nut`, -`OrageCover`, `Screw`, `ScrewHole`) are routed to the head-camera YOLO detector -and the raw bounding-box cloud instead of Gemini + SAM — no goal field needed, -the match is on `target_object`. +**Synthesis method.** Without `top_down`, the grasp candidates come from the +skills node's `grasp_method` parameter (default `heuristic`). Every method +gets the same segmented cloud and runs through the same approach / contact / +servo executor, so switching methods changes only the grasps: -The full sequence runs: pre-grasp standoff → contact move (`APPROACH_DIST − -CONTACT_OFFSET` = 7.5 cm along the approach) → force close at +```bash +ros2 param set /golem_skills grasp_method antipodal # takes effect on the next goal +ros2 param describe /golem_skills grasp_method +``` + +The method names and what each does are in +`golem_skills/golem_skills/grasp_synthesis/` (registry: `METHODS` in its +`__init__.py`); an unknown name is rejected by `ros2 param set`. +`visual_servo` composes with any of them. + +Every target goes through Gemini + SAM, on the head camera and in the visual +servo — the pipeline the sim grasp tables measure. A battery-workcell YOLO class +label as `target_object` (e.g. `OrageCover`) is sent to Gemini as a readable +phrase (`GEMINI_NAMES` in `skills/grasp.py`); a descriptive name such as +`'orange cover with a pink dot'` is used as given. `BATTERY_USE_GEMINI = False` +in `skills/grasp.py` routes those labels to the head/hand YOLO detectors and the +raw bounding-box cloud instead. The hand YOLO stream (`use_yolo:=true`) is still +needed for the servo's aligned depth and intrinsics. + +The full sequence runs: pre-grasp standoff → contact move (`APPROACH_DIST −` the +`contact_offset` parameter, default 2.5 cm, = 7.5 cm along the approach) → force close at `base.GRIP_FORCE_N`. It does **not** lift afterwards. --- @@ -154,13 +286,15 @@ aborts, which reads as a servo failure when it is really budget starvation. ## Named arm configs (`/named_config`) -Drive the 14 arm joints to a predefined posture (defined in -`h12_ros2_controller/utility/named_config.py`). `plan: false` (or omitted) servos -directly; `plan: true` runs the planner first. `duration` of `0` uses the -controller default settle time. +Drive the 14 arm joints to a predefined posture (`NAMED_CONFIGS` in +`golem_upper_ik/golem_upper_ik/robots/h12.py`). `plan: false` (or omitted) servos +directly; `plan: true` runs the planner first. `duration` is the goal's time +limit; `0` uses the controller config's `controller.timeout` (5 s in the H1-2 +configs). `slow_mode: true` moves at a quarter of the speed. -Available configs: `home`, `t_pose`, `arms_front_45`, `arms_overhead`, -`arms_asym`, `arms_front_yaw`, `elbow_only`, `init_1`, `init_2`, `init_3`. +The available configs are the keys of `NAMED_CONFIGS` in that file. The grasp +skill itself uses `prep` (arms clear of the head camera) and +`staging_left` / `staging_right` (pre-approach posture for the grasping arm). ```bash # arms down to the home baseline @@ -208,3 +342,36 @@ ros2 action send_goal /frame_task custom_ros_messages/action/FrameTask \ duration: {sec: 4, nanosec: 0}}" \ --feedback ``` + +--- + +## Loco-manipulation game policy (`/marl_deploy_node`) + +The Locomanipulation_game MARL policy (`lmg_deploy`) drives all 27 +joints: legs walk on `/cmd_vel`, arms reach wrist-pose goals (held in the world +by the SLAM odometry, so keep `use_slam` on). You drive it yourself (see +`lmg_deploy/README.md` for all launch arguments): + +```bash +ros2 launch golem_bringup lmg_sim.launch.py policy:= # a policies// of lmg_deploy +``` + +Walk: + +```bash +ros2 topic echo /marl_deploy_node/state --once # expect "nav" once engaged +ros2 run teleop_twist_keyboard teleop_twist_keyboard # walk +``` + +To reach, then go back to walking: + +```bash +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}}]}" +ros2 topic echo /marl_deploy_node/wrist_error # metres, left and right +ros2 service call /marl_deploy_node/end_arm_goal std_srvs/srv/Trigger +``` + +Arm goals are `left_wrist_yaw_link` then `right_wrist_yaw_link` poses in the +pelvis frame; `/cmd_vel` is ignored until the goal ends. diff --git a/core_ws/src/golem_bringup/config/h12/lowerbody_real.yaml b/core_ws/src/golem_bringup/config/h12/lowerbody_real.yaml new file mode 100644 index 0000000..ba34567 --- /dev/null +++ b/core_ws/src/golem_bringup/config/h12/lowerbody_real.yaml @@ -0,0 +1,19 @@ +# lowerbody_controller_node parameters for the real robot. active_policy comes +# from h12_lowerbody.launch.py's lowerbody:= launch argument. 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/golem_bringup/config/h12/lowerbody_sim.yaml b/core_ws/src/golem_bringup/config/h12/lowerbody_sim.yaml new file mode 100644 index 0000000..7a70f1f --- /dev/null +++ b/core_ws/src/golem_bringup/config/h12/lowerbody_sim.yaml @@ -0,0 +1,17 @@ +# lowerbody_controller_node parameters for the simulators. active_policy comes +# from h12_lowerbody.launch.py's lowerbody:= launch argument. +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 + # s from the policy request to releasing the elastic band without waiting + # for frame_task_server's /left_ee_pose. Its start-up moves end about 24 s + # after this node starts in tests/stack and about 29 s under the full sim + # bringup; a band released before then drops the robot. + band_max_wait: 34.0 diff --git a/core_ws/src/h1_bringup/config/mjpc_real.yaml b/core_ws/src/golem_bringup/config/h12/mjpc_real.yaml similarity index 98% rename from core_ws/src/h1_bringup/config/mjpc_real.yaml rename to core_ws/src/golem_bringup/config/h12/mjpc_real.yaml index 847f18e..98a0f65 100644 --- a/core_ws/src/h1_bringup/config/mjpc_real.yaml +++ b/core_ws/src/golem_bringup/config/h12/mjpc_real.yaml @@ -5,7 +5,7 @@ # band. See the submodule's mjpc/deploy/deploy_common.h for the compiled-in # settled values and Command_Sheet_h12.html for the native-h12 equivalents. -h12_deploy_mjpc_estimator: +deploy_mjpc_estimator: ros__parameters: # Debug mode has no sport service, so the estimator OWNS rt/sportmodestate # (the native-B1 arrangement). Never run a second estimator instance. diff --git a/core_ws/src/golem_bringup/config/h12/mjpc_sim.yaml b/core_ws/src/golem_bringup/config/h12/mjpc_sim.yaml new file mode 100644 index 0000000..fb23ff0 --- /dev/null +++ b/core_ws/src/golem_bringup/config/h12/mjpc_sim.yaml @@ -0,0 +1,147 @@ +# MJPC lower-body deploy — RoboCasa SIM params (h12_sim.launch.py). +# Controller + estimator are the fork's deploy core +# and RW-EKF base estimator behind the deploy_mjpc shims; these params +# mirror the fork's surviving CLI flags 1:1. Everything settled (ctrl 200 Hz, +# twin-time clocking, latency comp, ramp/hold/blend timings, torque-budget +# clamp, watchdog) is compiled into the core — see the submodule's +# mjpc/deploy/deploy_common.h. + +deploy_mjpc_estimator: + ros__parameters: + # Side topic (nothing else publishes sportmodestate in sim, but the side + # topic keeps the one-writer rule trivially safe). Controller reads it below. + out_topic: rt/sportmodestate_est + rate: 200.0 + 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: 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: + # DEBUG ONLY (use_mjpc_viz:=false to drop it). Renders the MEASURED robot in its + # normal colours + a semi-transparent BLUE GHOST at the planner's current best + # trajectory, and writes the mp4 when the node is signalled. Commands nothing; + # subscribes only to rt/lowstate, the estimator's rt/sportmodestate_est, and + # rt/mjpc/plan (the controller core publishes that from its planner thread -- + # controller_launcher.py hardcodes --plan_topic, so this needs no arming here). + # MUST be a HOST-MOUNTED path. docker_run.sh runs the ros container with + # `compose run --rm`, so the container -- and everything in its /tmp -- is + # DESTROYED the moment `ros2 launch` exits. That is exactly when the mp4 is + # finalised, so a /tmp default writes a perfect video and then deletes it. + # core_ws is bind-mounted, and core_ws/.gitignore's `log/` has no leading slash + # so it matches at ANY depth -- this lands on the host at + # core_ws/src/deploy_mjpc/log/mjpc_plan.mp4 and stays out of git. + out_path: /home/code/core_ws/src/deploy_mjpc/log/mjpc_plan.mp4 + # RENDER rate, decoupled from the 200 Hz lowstate and the ~20 Hz plan (the viewer + # draws from whatever it last cached, on its own clock). 10 because osmesa is a + # software rasteriser: ~96 ms/frame for this scene with shadows off (measured), + # i.e. ~10 fps, and the mp4's fps tag is fixed when the file opens -- so a target + # the loop cannot hit yields a video that PLAYS FAST and misrepresents the + # dynamics. The node reports the achieved rate on exit if it falls short. + fps: 10.0 + width: 960 + height: 720 + # Guard an unattended sim from filling the disk. 0 = run until shutdown. + max_seconds: 0.0 + # EXPECT the ghost's BASE to sit off the measured robot's: the plan carries the + # PLANNER's base belief, the measured robot the ESTIMATOR's. That gap is real + # signal (it is how you see the planner solving for a robot that isn't where it + # thinks it is), so align_ghost_base stays false. Set true ONLY to compare + # POSTURE in isolation. + align_ghost_base: false + # GHOST MOTION. sweep = the ghost's plan step cycles 0 -> end of horizon in real + # time (each ~1 s loop walks from "now" out to the full lookahead), alpha fading + # from ghost_rgba's alpha down to ghost_alpha_far so a single frame still shows how + # far ahead it is. Do NOT use ghost_mode: single with ghost_step: 0 -- plan row 0 + # is the measured state BIT-FOR-BIT (trajectory.cc:130 seeds it from what the + # deploy core SetState'd), so that draws a ghost welded to the robot and shows + # nothing, even as it falls. That was this tool's original default and it made the + # viewer look broken. + ghost_mode: sweep + sweep_rate: 1.0 + ghost_alpha_far: 0.15 + # HOW TO READ IT: a near-1:1 ghost on a STABLE stand is CORRECT -- over the 1.0 s + # horizon the plan moves the base only ~1.8 cm and the 27 joints ~0.029 rad on + # average (measured off the live CEM planner), because a balancing robot's plan + # says "stay put". Judge the ghost DURING a fall, in the first few hundred ms: + # that is when the plan's intended recovery diverges from what the robot actually + # does, and that gap is the deploy gap (deploy_common.h:92-110). + plan_stale_sec: 1.0 + +mjpc_deploy_lowerbody_controller: + ros__parameters: + task: "Stabilize H12 Magpie" + strategy: 6 # 6 = stand (the stabilize task's only real slot) + # 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 + # 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 + # 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. 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 + # 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 + 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 (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 + # plant-clocked, so it pauses during each freeze rather than racing ahead. + # REAL stays at the compiled 0.05 (mjpc_real.yaml doesn't set this). + stale_sec: 1.5 + # Band release is owned by the SIM's slack-triggered auto-release + # (h12_mujoco --band-auto-release: |band force| <= 30 N sustained 2 sim-s, + # 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 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/h1_bringup/__init__.py b/core_ws/src/golem_bringup/golem_bringup/__init__.py similarity index 100% rename from core_ws/src/h1_bringup/h1_bringup/__init__.py rename to core_ws/src/golem_bringup/golem_bringup/__init__.py diff --git a/core_ws/src/golem_bringup/golem_bringup/livox_imu_upright.py b/core_ws/src/golem_bringup/golem_bringup/livox_imu_upright.py new file mode 100644 index 0000000..bde5627 --- /dev/null +++ b/core_ws/src/golem_bringup/golem_bringup/livox_imu_upright.py @@ -0,0 +1,120 @@ +""" +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 ~9.4 deg lidar pitch reads ~0.99 + + +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/golem_bringup/launch/g1/g1_core.launch.py b/core_ws/src/golem_bringup/launch/g1/g1_core.launch.py new file mode 100644 index 0000000..8d6b110 --- /dev/null +++ b/core_ws/src/golem_bringup/launch/g1/g1_core.launch.py @@ -0,0 +1,333 @@ +""" +Core G1 stack, shared by g1_real_robot.launch.py (real robot) and g1_sim.launch.py. + +Starts robot_state_publisher, golem_upper_ik's joint_state_publisher (the 29 +body joints), golem_safety's safety_node, the only writer of /lowcmd, the arm +controller (golem_upper_ik's frame_task_server), HOMIE's walking node +(g1_homie's locomotion_node; on the real robot only with +start_position_verified:=true) and, optionally, navigation (golem_slam), +the Dex3-1 hands and RViz. The golem nodes get robot g1 as a ROS parameter, +and safety_node and frame_task_server their configs (joint_state_publisher +reads its default, g1_split). safety_node also gets sim (it refuses sim:=true +on ROS domain 0) and use_estop false: the G1 stack has no hardware estop node. + +safety_node runs split mode (split, or sim_split in the sim): it writes every +motor on /lowcmd, the legs from the walking node's input and the waist and +arms from the controller's. From boot it streams zero gains on every motor +(every motor off), and engages each input on that input's own first valid +command once rt/lowstate reports the G1's mode_machine (5); until the walking +node commands, the leg motors stay off. + +The safety node starts first; the controller follows after `controller_delay` +seconds. It waits for a fresh /lowstate (or stops with an error) and starts +every command at the measured pose. Its follow gate then keeps each waist and +arm command within controller.follow_tolerance (0.05 rad) of the measured +joint until it sees golem_safety relay the commands, reading (never writing) +golem_safety's output, /lowcmd, which carries them from the controller's first +command on, with or without the walking node. Only then does the controller +move the arms to `home` and the waist to its hold angles, and from then on no +command follows the measured pose. The follow gate must open within +controller.startup_timeout (30 s), and the moves to home and the waist hold +must finish within controller.startup_move_timeout (30 s) of the gate opening; +otherwise the controller stops with an error. + +The walking node starts `locomotion_delay` seconds after the controller, with +g1_homie's config for the platform (locomotion_config: sim in the sim, g1 +on the real robot), and drives the legs through golem_safety's split mode. It +sends nothing until golem_safety beats on /safety/heartbeat, rt/lowstate is +upright and the controller commands. Then it ramps the legs to the policy's +default pose. In the sim it starts the policy and releases the sim's elastic +band once the controller has finished its start-up moves (or band.max_wait +after the policy started without them); on the real robot it holds the pose +until the operator calls /lowerbody/confirm_engage (g1_homie's README has +the details). The controller holds the waist as +stiffly as golem_safety allows (g1_split.yaml, sim_split.yaml), as the policy +expects. + +The sim always starts the walking node. The real robot starts it only with +start_position_verified:=true (default false), as the H1-2's leg node +(h12_lowerbody.launch.py): without it nothing commands the legs, so their +motors stay off on golem_safety's zero-gain stream while the arms run. + +use_nav (default true, as the H1-2's bringups start navigation) adds +golem_slam (odometry, slam_toolbox and Nav2, which drives the walking node +through /cmd_vel), with the core: on the sim's FAST_LIO config and clock in +the sim, and on the real robot g1_fastlio.yaml, wall time and the MID-360 +driver (g1_real_drivers.launch.py). odom_source must be fastlio: the shared +pipeline needs its deskewed cloud. Leave GOLEM_SIM_ODOM unset to avoid a second +odom -> pelvis publisher. +FAST_LIO starts while the sim's elastic band still holds the robot up: the +walking node releases it once its policy runs and the controller's start-up +moves are done (or band.max_wait after the policy started). +use_skills:=true starts golem_skills (serving /skill/*, including +/skill/frontier_explore). + +The launch refuses to start, before any node, on these settings +(startup_refusal; the config rules are golem_upper_ik.pairing's for robot g1): +- The safety config must relay the upper-body channel (split, or sim_split in + the sim). +- Each safety config runs with one controller config: split with g1_split and + sim_split with sim_split. controller_config defaults to the one the safety + config runs with. +- Sim and real configs do not mix: the real robot (sim:=false) refuses every + sim_* safety or controller config and the walking node's sim config, and the + sim refuses the real robot's (split, g1_split, and the walking node's g1). + Since g1_split is the real robot's and sim_split the sim's, this also keeps + each controller config to its own safety config. +- With use_nav, odom_source must be fastlio. +start_position_verified refuses nothing: it decides whether the real robot's +walking node starts. + +Not checked: configs are told apart by name only, so a renamed copy of a sim +config passes, and a custom controller config is not matched to the safety +config, nor checked for the stiff waist the walking policy expects; +start_position_verified is the operator's word, not a measurement. +""" + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import (DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction, + TimerAction) +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterValue + +from golem_ros2_models.paths import urdf_path +from golem_upper_ik.pairing import default_controller_config, pairing_refusal + +ROBOT = 'g1' +# real / sim defaults: golem_safety config, controller config; the controller +# config defaults to the one the safety config runs with (default_controller_config) +DEFAULT_CONFIGS = { + False: ('split', 'g1_split'), + True: ('sim_split', 'sim_split'), +} +# real / sim: the walking node's config (g1_homie's config/.yaml) +LOCOMOTION_CONFIGS = { + False: 'g1', + True: 'sim', +} +# Shared pipeline's odom sources (its launch file checks them too) +ODOM_SOURCES = ('fastlio',) + + +def _as_bool(context, name): + return LaunchConfiguration(name).perform(context).strip().lower() in ('true', '1', 'yes') + + +def _config(context, name): + return LaunchConfiguration(name).perform(context) + + +def locomotion_refusal(sim, locomotion_config): + """Why the walking node's config does not suit the platform, or None.""" + for config_sim, name in LOCOMOTION_CONFIGS.items(): + if locomotion_config == name and config_sim != sim: + meant_for = 'the sim' if config_sim else 'the real robot' + return f'locomotion_config:={name} is for {meant_for}; use {LOCOMOTION_CONFIGS[sim]}' + return None + + +def nav_refusal(sim, odom_source): + """Why shared navigation must not start with these settings, or None.""" + if odom_source not in ODOM_SOURCES: + return 'golem_slam requires FAST-LIO for its deskewed cloud' + return None + + +def startup_refusal(sim, safety_config, controller_config, locomotion_config, + use_nav=False, odom_source='fastlio'): + """Why g1_core must not start with these settings, or None.""" + refusal = nav_refusal(sim, odom_source) if use_nav else None + # golem_upper_ik's pairing rules for the G1 + if refusal is None: + refusal = pairing_refusal(ROBOT, controller_config, safety_config, sim=sim) + if refusal is None: + refusal = locomotion_refusal(sim, locomotion_config) + return refusal + + +def _setup(context): + sim = _as_bool(context, 'sim') + use_nav = _as_bool(context, 'use_nav') + odom_source = _config(context, 'odom_source') + use_sim_time = {'use_sim_time': _as_bool(context, 'use_sim_time')} + default_safety, default_controller = DEFAULT_CONFIGS[sim] + safety_config = _config(context, 'safety_config') or default_safety + # the controller config the safety config runs with: g1_split with split + default_controller = default_controller_config(ROBOT, safety_config) or default_controller + controller_config = _config(context, 'controller_config') or default_controller + locomotion_config = _config(context, 'locomotion_config') or LOCOMOTION_CONFIGS[sim] + delay = float(LaunchConfiguration('controller_delay').perform(context)) + locomotion_delay = float(LaunchConfiguration('locomotion_delay').perform(context)) + refusal = startup_refusal(sim, safety_config, controller_config, locomotion_config, + use_nav, odom_source) + if refusal is not None: + raise RuntimeError(f'g1_core: not starting: {refusal}') + + with open(urdf_path('g1', ros=True)) as urdf_file: + robot_description = urdf_file.read() + rviz_config = os.path.join(get_package_share_directory('golem_bringup'), 'rviz', 'g1', + 'g1.rviz') + + actions = [ + Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + parameters=[{'robot_description': robot_description}, use_sim_time], + output='screen', + ), + # body joints (29) from /lowstate; the finger joints come from dex3_hands + Node( + package='golem_upper_ik', + executable='joint_state_publisher', + name='joint_state_publisher', + parameters=[{'robot': ROBOT}, use_sim_time], + output='screen', + ), + # the config names as typed strings: launch_ros reads a plain string + # parameter as YAML, which would make a config named 'on' a bool + Node( + package='golem_safety', + executable='safety_node', + name='safety_node', + parameters=[{ + 'robot': ROBOT, + 'config': ParameterValue(safety_config, value_type=str), + 'sim': sim, + 'use_estop': False, + }, use_sim_time], + output='screen', + ), + TimerAction( + period=delay, + actions=[ + Node( + package='golem_upper_ik', + executable='frame_task_server', + name='frame_task_server', + parameters=[{ + 'robot': ROBOT, + 'config': ParameterValue(controller_config, value_type=str), + }, use_sim_time], + output='screen', + ), + ], + ), + Node( + package='dex_3', + executable='dex3_hands', + name='dex3_hands', + parameters=[use_sim_time], + output='screen', + condition=IfCondition(LaunchConfiguration('use_hands')), + ), + Node( + package='rviz2', + executable='rviz2', + name='rviz2', + arguments=['-d', rviz_config], + parameters=[use_sim_time], + output='screen', + condition=IfCondition(LaunchConfiguration('use_rviz')), + ), + ] + # the leg-control interlock, as the H1-2's: the sim always starts the walking + # node, the real robot only once the operator has verified its start pose + if sim or _as_bool(context, 'start_position_verified'): + actions.append(TimerAction( + period=delay + locomotion_delay, + actions=[ + Node( + package='g1_homie', + executable='locomotion_node', + name='locomotion_node', + arguments=['--config', f'{locomotion_config}.yaml'], + parameters=[use_sim_time], + output='screen', + ), + ], + )) + if _as_bool(context, 'use_skills'): + actions.append(Node( + package='golem_skills', + executable='skills', + name='golem_skills', + parameters=[use_sim_time, {'wait_for_services': False}], + output='screen', + )) + if use_nav and not sim: + # the real MID-360's driver; the sim publishes the lidar itself + actions.append(IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join( + get_package_share_directory('golem_bringup'), 'launch', 'g1', + 'g1_real_drivers.launch.py')), + launch_arguments={'use_lidar': 'true'}.items(), + )) + if use_nav: + # with the core: FAST_LIO initialises on its first scans, in the sim + # while its elastic band still holds the robot up (the walking node + # releases it once its policy runs and the controller's start-up moves + # are done) + nav = os.path.join(get_package_share_directory('golem_slam'), 'launch', + 'g1_navigation.launch.py') + # the arguments of this launch that golem_slam also declares are passed + # explicitly: Humble includes are not scoped, so an included launch file sees + # every launch configuration set so far, and an argument left out would take a + # value set elsewhere + actions.append(IncludeLaunchDescription( + PythonLaunchDescriptionSource(nav), + launch_arguments={ + 'odom_source': odom_source, + 'sim': str(sim).lower(), + 'fast_lio_config_file': 'g1_fastlio_sim.yaml' if sim else 'g1_fastlio.yaml', + 'use_sim_time': str(use_sim_time['use_sim_time']).lower(), + }.items(), + )) + return actions + + +def generate_launch_description(): + return LaunchDescription([ + DeclareLaunchArgument('sim', default_value='false'), + DeclareLaunchArgument('use_sim_time', default_value='false'), + DeclareLaunchArgument('use_rviz', default_value='true'), + DeclareLaunchArgument('use_hands', default_value='true'), + DeclareLaunchArgument( + 'safety_config', default_value='', + description='golem_safety config, a name under its config/g1/ or a path ' + '(default: split, or sim_split in sim)'), + DeclareLaunchArgument( + 'controller_config', default_value='', + description='golem_upper_ik controller config, a name under its config/g1/ or ' + 'a path (default: the one the safety config runs with: g1_split or ' + 'sim_split)'), + DeclareLaunchArgument('controller_delay', default_value='2.0', + description='s between the safety node and the controller'), + DeclareLaunchArgument( + 'locomotion_config', default_value='', + description='g1_homie config (default: g1, or sim in sim)'), + DeclareLaunchArgument('locomotion_delay', default_value='2.0', + description='s between the controller and the walking node'), + DeclareLaunchArgument( + 'use_nav', default_value='true', + description='golem_slam: odometry, SLAM and Nav2 on /cmd_vel, and on the real ' + 'robot the MID-360 driver (g1_real_drivers.launch.py)'), + DeclareLaunchArgument( + 'odom_source', default_value='fastlio', + description='with use_nav: fastlio (shared pipeline requires its deskewed cloud)'), + DeclareLaunchArgument( + 'use_skills', default_value='false', + description='Start the golem_skills action servers (/skill/*)'), + # the leg-control interlock; the sim always starts the walking node + DeclareLaunchArgument( + 'start_position_verified', default_value='false', + description="the real robot's walking node: true only after checking the robot " + 'is in a safe start pose'), + OpaqueFunction(function=_setup), + ]) diff --git a/core_ws/src/golem_bringup/launch/g1/g1_real_drivers.launch.py b/core_ws/src/golem_bringup/launch/g1/g1_real_drivers.launch.py new file mode 100644 index 0000000..be2fa5a --- /dev/null +++ b/core_ws/src/golem_bringup/launch/g1/g1_real_drivers.launch.py @@ -0,0 +1,47 @@ +""" +The real G1's sensor drivers, run in the laptop's golem_ros container. + +The MID-360 driver (livox_ros_driver2), as msg_MID360_launch.py starts it, on +golem_slam/config/g1_MID360_config.json: the laptop's wired address +(192.168.123.222, docs/g1/dds.md) as the host, the lidar at 192.168.123.120, +and no extrinsic, so /livox/lidar and /livox/imu are both in the raw sensor +frame, livox_frame, as the G1 sim publishes them. The URDF's mid360_joint holds +the sensor's upside-down mounting, and g1_fastlio.yaml's extrinsic_R is the +identity for that frame. With 10 Hz frames in CustomMsg (xfer_format 1), which +FAST_LIO's scan_rate and lidar_type expect. +""" +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 LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + mid360_config = os.path.join( + get_package_share_directory('golem_slam'), 'config', 'g1_MID360_config.json') + return LaunchDescription([ + DeclareLaunchArgument('use_lidar', default_value='true', + description='Start the MID-360 driver'), + Node( + package='livox_ros_driver2', + executable='livox_ros_driver2_node', + name='livox_lidar_publisher', + output='screen', + emulate_tty=True, + parameters=[{ + 'xfer_format': 1, # Livox CustomMsg + 'multi_topic': 0, # /livox/lidar and /livox/imu for every lidar + 'data_src': 0, # the lidar + 'publish_freq': 10.0, # Hz: g1_fastlio.yaml's scan_rate + 'output_data_type': 0, + 'frame_id': 'livox_frame', + 'user_config_path': mid360_config, + 'cmdline_input_bd_code': 'livox0000000001', + }], + condition=IfCondition(LaunchConfiguration('use_lidar')), + ), + ]) diff --git a/core_ws/src/golem_bringup/launch/g1/g1_real_robot.launch.py b/core_ws/src/golem_bringup/launch/g1/g1_real_robot.launch.py new file mode 100644 index 0000000..244cc90 --- /dev/null +++ b/core_ws/src/golem_bringup/launch/g1/g1_real_robot.launch.py @@ -0,0 +1,135 @@ +r""" +Real G1, with the whole ROS stack running on the laptop wired to the robot. + + ros2 launch golem_bringup g1_real_robot.launch.py # the arms; the legs stay off + ros2 launch golem_bringup g1_real_robot.launch.py start_position_verified:=true + ros2 launch golem_bringup g1_real_robot.launch.py use_rviz:=false use_hands:=false + +Before launching: +- The operator puts the G1 into Unitree's debug mode, on a gantry. In debug + mode Unitree's own controller is released and this stack drives every motor + on rt/lowcmd, from the moment golem_safety starts. Nothing in this stack + enters debug mode or checks that the robot is in it. +- The gantry carries the robot whenever the legs are off: they hang limp until + the walking node commands them, and split.yaml's estop turns every motor off. +- ROS_DOMAIN_ID=0 (the robot's DDS domain) in docker/.env. +- Nothing else may be commanding rt/lowcmd or rt/dex3/*: two publishers fight + over the motors. +- Clear the space around the arms: once golem_safety relays the controller's + commands, the arms move to `home` (forearms forward at waist height) at up to + 0.3 rad/s per joint. + +golem_safety runs split.yaml: it writes every motor on rt/lowcmd, the legs +from the walking node's input and the waist and arms from the arm +controller's (g1_split.yaml). From boot it streams zero gains on every motor +(every motor off) and engages each input on its own first command: the arms on +the controller's first command, and the move to home starts once the +controller sees golem_safety relay it on rt/lowcmd. + +The legs: +- start_position_verified gates the legs. Without it (the default) HOMIE's + walking node does not start: the leg motors stay off on golem_safety's + zero-gain stream while the arms run. Set start_position_verified:=true only + after checking the robot is in a safe start pose. +- With it, the walking node (g1_homie's g1.yaml) waits for golem_safety's + heartbeat, upright rt/lowstate and the arm controller's commands, moves the + legs from where they are to the policy's default pose over 2 s, and holds + that pose. golem_safety engages the leg input on its first command. +- /lowerbody/confirm_engage starts the policy. Check the stance, then + `ros2 service call /lowerbody/confirm_engage std_srvs/srv/Trigger`; until + then the legs hold the default pose. The policy starts at zero velocity and + 0.74 m height and walks on /cmd_vel. +- A false or missing heartbeat (golem_safety's estop latched, or it stopped) + stops the walking node for good. If the node stops on a stale or fallen + state, a non-finite IMU value or a late command, golem_safety keeps relaying + its last leg command, which does not balance the robot, and reports the + quiet input (input_quiet) 0.2 s later. It does not estop on it: the command + stands until golem_safety estops for another reason (a fall that drives a + joint past split.yaml's estop limits, say) or the bringup stops. A + non-finite joint value (q, dq or tau_est) in rt/lowstate is different: + golem_safety latches its estop on that sample, and split.yaml's estop (mode + zero, keep_writing) turns every motor off and keeps writing that command. + +This launch refuses sim configs (any sim_* safety_config), and the controller +config follows safety_config (g1_split with split); g1_core's docstring lists +every setting it refuses. When the controller exits it sends one last command, +with zero gains on the arms; the waist keeps its hold and golem_safety relays +it, so only the arms go limp. The controller stops with an error if +rt/lowstate does not arrive within 5 s, if golem_safety has not relayed its +commands within 30 s, or if the move to home is not done within 30 s after +that. use_nav (default true) starts the MID-360 driver +(g1_real_drivers.launch.py) and golem_slam on g1_fastlio.yaml: FAST_LIO, +slam_toolbox and Nav2, which drives the walking node through /cmd_vel once it +runs. Neither has been run on our G1. Skills are not part of the real G1 +bringup yet. + +Hazards not handled in code: +- A controller that dies without its last command leaves the waist + arms + held on its last command: golem_safety holds a quiet input and only reports + it (input_quiet), until the bringup stops. +- After any estop, restart the whole bringup, not only the safety node: the + estop stays latched until restart, and the controller's follow gate opens + for good once golem_safety relays its commands, so a restarted safety node + would meet a command held wherever the robot used to be. +- The arm IK's self-collision model was packed on the rubber-hand G1: it has + no spheres on the Dex3-1 fingers, which can hit the torso, the legs or the + other arm. It keeps its spheres 20 mm apart (controller.d_min), as on the + H1-2, but its torso spheres stand up to 19 mm inside the torso's flanks + (CL_Assets/README.md): an upper arm rolled in toward the torso stops a few + millimetres from it. +- The arms must start clear of the hips and the torso. In debug mode + golem_safety's zero-gain stream leaves the arms limp in the gantry until the + controller's first command, so the start-up begins wherever they hang. If a + sphere pair starts about 10 mm apart or closer (half the keep-out; a hand or + wrist in its hip-pitch sphere, or an upper arm rolled in against the torso), + the start-up's first IK step has no solution and frame_task_server stops at + once: it logs "the IK has no solution; closest self-collision pairs + (controller.d_min 20 mm): ..." with each pair's distance, and its "did not + start" error says how many of them are inside controller.d_min. Hanging + freely in the sim, the wrists rest on the hips with their spheres 20 mm + from the hip spheres; arms_down (elbows at pi/2) is inside them. +- The gains are not tuned on our G1: the waist's damping ratio is + about 0.42-0.45 at home (g1_split.yaml). +""" + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration + + +def generate_launch_description(): + core = os.path.join(get_package_share_directory('golem_bringup'), 'launch', 'g1', + 'g1_core.launch.py') + return LaunchDescription([ + DeclareLaunchArgument('use_rviz', default_value='true'), + DeclareLaunchArgument('use_hands', default_value='true'), + DeclareLaunchArgument('safety_config', default_value='split', + description='golem_safety config, a name under its config/g1/ ' + 'or a path'), + # the leg-control interlock + DeclareLaunchArgument( + 'start_position_verified', default_value='false', + description="HOMIE's walking node on the legs: true only after checking the " + 'robot is in a safe start pose'), + DeclareLaunchArgument('use_nav', default_value='true', + description='the MID-360 driver and golem_slam (FAST_LIO, ' + 'slam_toolbox, Nav2)'), + IncludeLaunchDescription( + PythonLaunchDescriptionSource(core), + launch_arguments={ + 'sim': 'false', + 'use_sim_time': 'false', + 'use_rviz': LaunchConfiguration('use_rviz'), + 'use_hands': LaunchConfiguration('use_hands'), + 'safety_config': LaunchConfiguration('safety_config'), + # the one safety_config runs with: g1_split with split + 'controller_config': '', + 'start_position_verified': LaunchConfiguration('start_position_verified'), + 'use_nav': LaunchConfiguration('use_nav'), + }.items(), + ), + ]) diff --git a/core_ws/src/golem_bringup/launch/g1/g1_sim.launch.py b/core_ws/src/golem_bringup/launch/g1/g1_sim.launch.py new file mode 100644 index 0000000..8c64f8e --- /dev/null +++ b/core_ws/src/golem_bringup/launch/g1/g1_sim.launch.py @@ -0,0 +1,76 @@ +r""" +Simulated G1: the same core stack on DDS domain 1 with sim time and the sim configs. + + ros2 launch golem_bringup g1_sim.launch.py + ros2 launch golem_bringup g1_sim.launch.py use_nav:=false # no SLAM or Nav2 + ros2 launch golem_bringup g1_sim.launch.py \ + use_skills:=true # also serves /skill/* including /skill/frontier_explore + +The G1 RoboCasa sim (golem_robocasa/g1/) takes rt/lowcmd and rt/dex3/*, and +holds the robot up on an elastic band until /elastic_band/toggle releases it. +golem_safety runs sim_split and the controller sim_split: golem_safety streams +zero gains on every motor from boot and engages each input on its own first +command. HOMIE's walking node always runs, with g1_homie's sim.yaml: once +golem_safety beats and the controller commands, it moves the legs to the +policy's default pose, starts the policy at zero velocity (no operator +confirm in the sim) and releases the band once the controller has finished its +start-up moves (or band.max_wait after the policy started without them). It +then walks on /cmd_vel. g1_core refuses the real robot's configs here (split, +g1_split, the walking node's g1); the sim configs keep the faster arm speeds +(1 rad/s, 0.5 m/s). + +use_nav (default true) adds golem_slam on the sim's MID-360 (/livox/lidar, +/livox/imu), with sim time and g1_fastlio_sim.yaml, and Nav2 drives the +walking node on /cmd_vel; send goals to /navigate_to_pose. +odom_source:=fastlio is required. Start the sim with GOLEM_SIM_ODOM unset; +otherwise odom -> pelvis has two publishers. The shared pipeline needs +FAST-LIO's deskewed cloud and refuses sim_gt. use_skills:=true +adds golem_skills, which serves /skill/frontier_explore to walk the robot +through Nav2 to the frontiers of /map. +tests/g1/test_sim_nav.sh is the closed-loop test. +""" + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration + + +def generate_launch_description(): + core = os.path.join(get_package_share_directory('golem_bringup'), 'launch', 'g1', + 'g1_core.launch.py') + return LaunchDescription([ + DeclareLaunchArgument('use_rviz', default_value='true'), + DeclareLaunchArgument('use_hands', default_value='true'), + DeclareLaunchArgument('use_sim_time', default_value='true'), + DeclareLaunchArgument( + 'safety_config', default_value='', + description='golem_safety config (default: sim_split)'), + DeclareLaunchArgument( + 'use_nav', default_value='true', + description='golem_slam: odometry, SLAM and Nav2 on /cmd_vel'), + DeclareLaunchArgument( + 'odom_source', default_value='fastlio', + description='with use_nav: fastlio (deskewed cloud required)'), + DeclareLaunchArgument( + 'use_skills', default_value='false', + description='Start the golem_skills action servers (/skill/*)'), + IncludeLaunchDescription( + PythonLaunchDescriptionSource(core), + launch_arguments={ + 'sim': 'true', + 'use_sim_time': LaunchConfiguration('use_sim_time'), + 'use_rviz': LaunchConfiguration('use_rviz'), + 'use_hands': LaunchConfiguration('use_hands'), + 'safety_config': LaunchConfiguration('safety_config'), + # the one safety_config runs with: sim_split + 'controller_config': '', + 'use_nav': LaunchConfiguration('use_nav'), + 'odom_source': LaunchConfiguration('odom_source'), + 'use_skills': LaunchConfiguration('use_skills'), + }.items(), + ), + ]) diff --git a/core_ws/src/golem_bringup/launch/h12/h12_control.launch.py b/core_ws/src/golem_bringup/launch/h12/h12_control.launch.py new file mode 100644 index 0000000..c1d6cd9 --- /dev/null +++ b/core_ws/src/golem_bringup/launch/h12/h12_control.launch.py @@ -0,0 +1,146 @@ +from golem_upper_ik.pairing import pairing_refusal +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, OpaqueFunction, TimerAction +from launch.conditions import evaluate_condition_expression, IfCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterValue + + +# The H1-2's command path: [estop_node] -> golem_safety's safety_node, the only +# writer of /lowcmd -> golem_upper_ik's frame_task_server (use_frame_task). +# +# Every node is a ROS node on the launching shell's ROS_DOMAIN_ID, domain 0 when +# it is unset. Domain 0 is the real robot's command bus, and safety_node refuses +# sim:=true on it, so a sim bringup needs a sim domain exported. +# +# sim has no default: every top-level file says whether the robot is simulated. +# check_pairing runs before this file starts any node. + +ROBOT = 'h12' + + +def check_pairing(context): + """ + Refuse a frame_task_config that does not run with safety_config. + + Only with use_frame_task: the bringups that run no IK pass any safety_config. + The rules are golem_upper_ik.pairing's. Raises RuntimeError with the reason; + starts nothing. + """ + def flag(name): + return evaluate_condition_expression(context, [LaunchConfiguration(name)]) + + if not flag('use_frame_task'): + return [] + refusal = pairing_refusal( + ROBOT, + LaunchConfiguration('frame_task_config').perform(context), + LaunchConfiguration('safety_config').perform(context), + sim=flag('sim')) + if refusal is not None: + raise RuntimeError(f'h12_control: not starting: {refusal}') + return [] + + +def generate_launch_description(): + args = [ + DeclareLaunchArgument( + 'use_sim_time', default_value='false', + description='Use the simulator /clock if true'), + DeclareLaunchArgument( + 'sim', + description='true for a simulator (safety_node refuses it on ROS domain 0), ' + 'false for the real robot'), + DeclareLaunchArgument( + 'use_estop', default_value='false', + description='Start the serial e-stop bridge, and have safety_node read it ' + '(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='golem_safety config: a name under its config/h12/, or a path'), + DeclareLaunchArgument( + 'frame_task_config', default_value='safety_split.yaml', + description='golem_upper_ik config for frame_task_server: a name under its ' + 'config/h12/, or a path'), + # The H1-2's start-up order, on the wall clock: estop_node at once, then + # safety_node after safety_delay, then frame_task_server after + # frame_task_delay. + # + # With use_estop, safety_node engages no input before estop_node's first + # sample, and latches until restart on a sample that says triggered or + # unplugged. estop_node publishes nothing until its first attempt to open + # and probe the serial port ends; when that attempt fails it publishes + # unplugged, and triggered (fail_safe_triggered_on_disconnect), until a + # retry connects. A safety_node that starts while estop_node is retrying + # latches, so on the real robot safety_delay must cover the time + # estop_node takes to connect. A first attempt that outlasts safety_delay + # and then connects only delays the start: safety_node engages nothing and + # publishes no /safety/heartbeat, which the leg controller waits for before + # it pre-poses the legs, until that first sample. + # + # frame_task_server's start-up waits up to its controller.startup_timeout + # for safety_node to relay its commands. + 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'), + ] + + def value(name, value_type): + return ParameterValue(LaunchConfiguration(name), value_type=value_type) + + sim_time_param = {'use_sim_time': value('use_sim_time', bool)} + + return LaunchDescription(args + [ + OpaqueFunction(function=check_pairing), + Node( + package='estop', + executable='estop_node', + name='estop_node', + parameters=[sim_time_param], + output='screen', + emulate_tty=True, + condition=IfCondition(LaunchConfiguration('use_estop')), + ), + TimerAction( + period=LaunchConfiguration('safety_delay'), + actions=[ + Node( + package='golem_safety', + executable='safety_node', + name='safety_node', + parameters=[{ + 'robot': ROBOT, + 'config': value('safety_config', str), + 'sim': value('sim', bool), + 'use_estop': value('use_estop', bool), + }, sim_time_param], + output='screen', + emulate_tty=True, + ), + ], + ), + TimerAction( + period=LaunchConfiguration('frame_task_delay'), + actions=[ + Node( + package='golem_upper_ik', + executable='frame_task_server', + name='frame_task_server', + parameters=[{ + 'robot': ROBOT, + 'config': value('frame_task_config', str), + }, sim_time_param], + output='screen', + emulate_tty=True, + condition=IfCondition(LaunchConfiguration('use_frame_task')), + ), + ], + ), + ]) diff --git a/core_ws/src/h1_bringup/launch/h1_grippers.launch.py b/core_ws/src/golem_bringup/launch/h12/h12_grippers.launch.py similarity index 51% rename from core_ws/src/h1_bringup/launch/h1_grippers.launch.py rename to core_ws/src/golem_bringup/launch/h12/h12_grippers.launch.py index 8a7aec1..6a58a6c 100644 --- a/core_ws/src/h1_bringup/launch/h1_grippers.launch.py +++ b/core_ws/src/golem_bringup/launch/h12/h12_grippers.launch.py @@ -4,9 +4,9 @@ def generate_launch_description(): # Magpie grippers, one node per side. Factored out of - # h1_real_drivers.launch.py so the calibration constants live in exactly one - # place: h1_real_drivers (full onboard driver set) and - # h1_safety_grippers (actuation-only bringup) both include this file. + # h12_real_drivers.launch.py so the calibration constants live in exactly one + # place: h12_real_drivers (full onboard driver set) and + # h12_safety_grippers (actuation-only bringup) both include this file. # # The gripper_node has no built-in left/right concept (node name, # gripper/state topic and services are all relative), so each side is @@ -21,21 +21,24 @@ def generate_launch_description(): name='gripper_node', namespace='left', output='screen', + emulate_tty=True, parameters=[{ 'auto_detect_port': False, - # OpenRB-150 serial ...FF101F14 (was ttyACM0) - 'port': '/dev/serial/by-id/usb-ROBOTIS_OpenRB-150_B0935A515157375037202020FF101F14-if00', + # 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. + # min/max are the measured servo angles at the open/closed + # stops. theta_90 (parallel-jaw reference) is estimated as + # 26.6 deg (finger1) / 26.9 deg (finger2) from the closed stop, + # the offset measured on the right gripper and the previous left. 'finger1theta_min': 87.98, - 'finger1theta_max': 162.4, - 'finger1theta_90': 135.71, - 'finger2theta_min': 130.6, - 'finger2theta_max': 209.09, - 'finger2theta_90': 157.89, + 'finger1theta_max': 170.97, + 'finger1theta_90': 144.37, + 'finger2theta_min': 136.01, + 'finger2theta_max': 211.73, + 'finger2theta_90': 162.91, }], ) @@ -45,18 +48,22 @@ def generate_launch_description(): name='gripper_node', namespace='right', output='screen', + emulate_tty=True, parameters=[{ 'auto_detect_port': False, - # OpenRB-150 serial ...FF0F0E1C (replacement board, was ...FF122F35) - 'port': '/dev/serial/by-id/usb-ROBOTIS_OpenRB-150_3EC94E825157375037202020FF0F0E1C-if00', + # OpenRB-150 serial ...FF052729 + 'port': ('/dev/serial/by-id/' + 'usb-ROBOTIS_OpenRB-150_77E78BAE50323952582E3120FF052729-if00'), 'use_eflesh': False, # Finger angle limits (degrees), per-gripper calibration. + # min/max are the measured servo angles at the open/closed + # stops; theta_90 uses the same closed-stop offsets as the left. 'finger1theta_min': 86.51, - 'finger1theta_max': 168.33, - 'finger1theta_90': 141.83, - 'finger2theta_min': 132.55, - 'finger2theta_max': 211.44, - 'finger2theta_90': 159.05, + 'finger1theta_max': 168.91, + 'finger1theta_90': 142.31, + 'finger2theta_min': 131.67, + 'finger2theta_max': 210.26, + 'finger2theta_90': 158.57, }], ) diff --git a/core_ws/src/golem_bringup/launch/h12/h12_locomanipulationgame.launch.py b/core_ws/src/golem_bringup/launch/h12/h12_locomanipulationgame.launch.py new file mode 100644 index 0000000..b7c0fbc --- /dev/null +++ b/core_ws/src/golem_bringup/launch/h12/h12_locomanipulationgame.launch.py @@ -0,0 +1,78 @@ +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 LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterValue + +# Loco-manipulation game: the Locomanipulation_game MARL policy +# (lmg_deploy's marl_deploy_node, all 27 joints through the split +# safety layer). Included by lmg_sim.launch.py +# and lmg_real.launch.py, which start the state publishers, the safety +# layer and SLAM. +# +# marl_deploy_node owns both safety channels, so frame_task_server and the +# lower-body RL node must not run beside it. + + +def generate_launch_description(): + deploy_share = get_package_share_directory('lmg_deploy') + policy_dir = PathJoinSubstitution([deploy_share, 'policies', LaunchConfiguration('policy')]) + use_sim_time = ParameterValue(LaunchConfiguration('use_sim_time'), value_type=bool) + policy_on = LaunchConfiguration('start_position_verified') + + return LaunchDescription([ + DeclareLaunchArgument('use_sim_time', default_value='false'), + DeclareLaunchArgument( + 'urdf_path', + description='Pinocchio URDF for the wrist kinematics (h1_2_magpie.urdf)'), + # Interlock: the policy node doesn't start until the operator has + # verified the robot is in a safe start position. + DeclareLaunchArgument('start_position_verified', default_value='false', + description='Start marl_deploy_node'), + DeclareLaunchArgument( + 'policy', + description='policies// of lmg_deploy: .yaml plus the export (required: ' + 'lmg_deploy ships none)'), + DeclareLaunchArgument('engage_wait_for_confirm', default_value='true', + description='hold the pre-pose until ~/confirm_engage'), + DeclareLaunchArgument('prepose_ramp_s', default_value='6.0', + description='minimum duration [s] of the ramp into the default ' + 'pose'), + DeclareLaunchArgument('prepose_max_vel', default_value='0.25', + description='peak joint target speed [rad/s] during that ramp; ' + 'lengthens it'), + DeclareLaunchArgument('disable_elastic_band', default_value='false', + description="release the sim's elastic band through " + '/elastic_band/toggle once engaged'), + *[DeclareLaunchArgument(f'imu_offset_{axis}_deg', default_value='0.0', + description=f'startup IMU {axis} trim [deg]: its reading with the ' + 'torso level (live: ros2 param set)') + for axis in ('roll', 'pitch', 'yaw')], + Node( + package='lmg_deploy', + executable='marl_deploy_node', + name='marl_deploy_node', + output='screen', + emulate_tty=True, + condition=IfCondition(policy_on), + parameters=[{ + 'use_sim_time': use_sim_time, + 'config': ParameterValue( + [policy_dir, '/', LaunchConfiguration('policy'), '.yaml'], value_type=str), + 'urdf_path': LaunchConfiguration('urdf_path'), + 'engage_wait_for_confirm': ParameterValue( + LaunchConfiguration('engage_wait_for_confirm'), value_type=bool), + 'disable_elastic_band': ParameterValue( + LaunchConfiguration('disable_elastic_band'), value_type=bool), + 'prepose_ramp_s': ParameterValue( + LaunchConfiguration('prepose_ramp_s'), value_type=float), + 'prepose_max_vel': ParameterValue( + LaunchConfiguration('prepose_max_vel'), value_type=float), + **{f'imu_offset_{axis}_deg': ParameterValue( + LaunchConfiguration(f'imu_offset_{axis}_deg'), value_type=float) + for axis in ('roll', 'pitch', 'yaw')}, + }], + ), + ]) diff --git a/core_ws/src/golem_bringup/launch/h12/h12_lowerbody.launch.py b/core_ws/src/golem_bringup/launch/h12/h12_lowerbody.launch.py new file mode 100644 index 0000000..1daed29 --- /dev/null +++ b/core_ws/src/golem_bringup/launch/h12/h12_lowerbody.launch.py @@ -0,0 +1,78 @@ +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 +# 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('golem_bringup'), 'config', 'h12') + + 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 | fame | walk; mjpc starts no RL node'), + 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')}], + output='screen', + emulate_tty=True, + condition=IfCondition(AndSubstitution( + LaunchConfiguration('start_position_verified'), + PythonExpression( + ["'", LaunchConfiguration('lowerbody'), "' != 'mjpc'"]), + )), + ), + Node( + package='deploy_mjpc', + executable='estimator_node', + name='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/golem_bringup/launch/h12/h12_models.launch.py b/core_ws/src/golem_bringup/launch/h12/h12_models.launch.py new file mode 100644 index 0000000..4aaffdf --- /dev/null +++ b/core_ws/src/golem_bringup/launch/h12/h12_models.launch.py @@ -0,0 +1,110 @@ +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 golem_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'), + # golem_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', + emulate_tty=True, + ), + Node( + package='model_server', + executable='sam_server', + name='sam_server', + parameters=[sim_time_param, model_log_params], + output='screen', + emulate_tty=True, + ), + + # 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', + emulate_tty=True, + 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', + emulate_tty=True, + 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='golem_skills', + executable='skills', + name='golem_skills', + parameters=[sim_time_param, model_log_params, skills_params], + output='screen', + emulate_tty=True, + condition=IfCondition(LaunchConfiguration('use_skills')), + ), + ]) diff --git a/core_ws/src/golem_bringup/launch/h12/h12_real_desktop.launch.py b/core_ws/src/golem_bringup/launch/h12/h12_real_desktop.launch.py new file mode 100644 index 0000000..1bb6d67 --- /dev/null +++ b/core_ws/src/golem_bringup/launch/h12/h12_real_desktop.launch.py @@ -0,0 +1,70 @@ +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 +from launch_ros.actions import Node + +# Companion-desktop bringup: model servers + skills, rviz, and optionally the +# hand cameras (use_hand_cameras). +# The state publishers, safety layer, frame_task_server and lower-body control +# run on the onboard PC (h12_real_robot.launch.py). ROS_DOMAIN_ID must +# be exported in the launching shell (see h12_control.launch.py). + + +def _include(package, launch_file, launch_arguments=None, condition=None): + return IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(get_package_share_directory(package), 'launch', launch_file)), + launch_arguments=(launch_arguments or {}).items(), + condition=condition, + ) + + +def generate_launch_description(): + bringup_share = get_package_share_directory('golem_bringup') + default_rviz = os.path.join(bringup_share, 'rviz', 'h12', 'main.rviz') + + # Real robot runs on wall-clock time (no simulator /clock). + wall_time = {'use_sim_time': 'false'} + + return LaunchDescription([ + DeclareLaunchArgument( + 'use_hand_cameras', default_value='false', + description='Start both hand RealSense cameras and their static TFs'), + DeclareLaunchArgument('use_skills', default_value='true'), + DeclareLaunchArgument('model_logging', default_value='true'), + DeclareLaunchArgument('model_visualization', default_value='true'), + DeclareLaunchArgument('model_clear_logs', default_value='true'), + DeclareLaunchArgument('record_runs', default_value='true'), + + # 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 + # (h12_real_drivers.launch.py). + _include('cl_realsense', 'h12_hand_cameras.launch.py', + condition=IfCondition(LaunchConfiguration('use_hand_cameras'))), + + _include('golem_bringup', 'h12/h12_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'), + }), + + Node( + package='rviz2', + executable='rviz2', + name='rviz2_sim', + arguments=['-d', default_rviz], + parameters=[{'use_sim_time': False}], + output='screen', + emulate_tty=True, + ), + ]) diff --git a/core_ws/src/golem_bringup/launch/h12/h12_real_drivers.launch.py b/core_ws/src/golem_bringup/launch/h12/h12_real_drivers.launch.py new file mode 100644 index 0000000..b374cbe --- /dev/null +++ b/core_ws/src/golem_bringup/launch/h12/h12_real_drivers.launch.py @@ -0,0 +1,93 @@ +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 +from launch_ros.actions import Node + + +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 golem_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('golem_bringup') + mid360_config = os.path.join( + get_package_share_directory('golem_slam'), 'config', 'MID360_config.json') + + livox_mid360_params = [ + {'xfer_format': 1}, # 0-Pointcloud2(PointXYZRTL), 1-customized pointcloud format + {'multi_topic': 0}, # 0-All LiDARs share the same topic, 1-One LiDAR one topic + {'data_src': 0}, # 0-lidar, others-Invalid data src + {'publish_freq': 30.0}, # publish frequency (Hz) + {'output_data_type': 0}, + {'frame_id': 'livox_frame'}, + {'lvx_file_path': '/home/livox/livox_test.lvx'}, + {'user_config_path': mid360_config}, + {'cmdline_input_bd_code': 'livox0000000001'}, + ] + + livox_mid360 = Node( + package='livox_ros_driver2', + executable='livox_ros_driver2_node', + name='livox_lidar_publisher', + output='screen', + emulate_tty=True, + parameters=livox_mid360_params, + condition=IfCondition(LaunchConfiguration('use_lidar')), + # 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='golem_bringup', + executable='livox_imu_upright', + name='livox_imu_upright', + output='screen', + emulate_tty=True, + parameters=[{'livox_config_path': mid360_config}], + condition=IfCondition(LaunchConfiguration('use_lidar')), + remappings=[('imu_in', '/livox/imu_raw'), ('imu_out', '/livox/imu')], + ) + + # Equivalent of `ros2 launch cl_realsense h12_headcamera.launch.py` — brings + # up only the head RealSense camera and its camera->link static TF. The hand + # cameras start on the companion desktop (h12_real_desktop.launch.py), + # so the onboard driver bringup only owns the head cam. + realsense_share = get_package_share_directory('cl_realsense') + realsense_cams = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(realsense_share, 'launch', 'h12_headcamera.launch.py') + ), + condition=IfCondition(LaunchConfiguration('use_head_camera')), + ) + + # Magpie grippers, one node per side. Both nodes (ports, per-gripper finger + # calibration) live in h12_grippers.launch.py so the actuation-only bringup + # (h12_safety_grippers.launch.py) can reuse the exact same configuration. + grippers = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(bringup_share, 'launch', 'h12', 'h12_grippers.launch.py') + ), + condition=IfCondition(LaunchConfiguration('use_grippers')), + ) + + return LaunchDescription([ + DeclareLaunchArgument('use_lidar', default_value='true', + description='Start the MID360 driver and livox_imu_upright'), + DeclareLaunchArgument('use_head_camera', default_value='true', + description='Start the head RealSense and its static TF'), + DeclareLaunchArgument('use_grippers', default_value='true', + description='Start the left and right gripper nodes'), + livox_mid360, + livox_imu_upright, + realsense_cams, + grippers, + ]) diff --git a/core_ws/src/golem_bringup/launch/h12/h12_real_robot.launch.py b/core_ws/src/golem_bringup/launch/h12/h12_real_robot.launch.py new file mode 100644 index 0000000..976e8b3 --- /dev/null +++ b/core_ws/src/golem_bringup/launch/h12/h12_real_robot.launch.py @@ -0,0 +1,56 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration + + +# Real robot, onboard PC. Every node joins the launching shell's ROS_DOMAIN_ID, +# which must be 0, the real robot's command bus; ROS uses 0 when it is unset +# (see h12_control.launch.py). +# +# Leg control (h12_lowerbody.launch.py) runs here, behind two operator gates: +# 1. start_position_verified:=true starts the lowerbody:= RL policy, which +# ramps the legs to the policy's pre-pose crouch and holds there. +# 2. ros2 service call /lowerbody/confirm_engage std_srvs/srv/Trigger +# engages the policy. + + +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'} + + bringup_share = get_package_share_directory('golem_bringup') + + return LaunchDescription([ + # Defaults to false so a bare launch never commands the legs. + DeclareLaunchArgument('start_position_verified', default_value='false'), + DeclareLaunchArgument('lowerbody', default_value='almi'), + + _include('golem_bringup', 'h12/h12_real_drivers.launch.py', wall_time), + _include('golem_slam', 'h12_navigation.launch.py', wall_time), + _include('golem_bringup', 'h12/h12_state.launch.py', wall_time), + _include('golem_bringup', 'h12/h12_control.launch.py', { + **wall_time, + 'sim': 'false', + 'use_estop': 'true', + }), + _include('golem_bringup', 'h12/h12_lowerbody.launch.py', { + **wall_time, + 'start_position_verified': LaunchConfiguration('start_position_verified'), + 'lowerbody': LaunchConfiguration('lowerbody'), + 'lowerbody_params': os.path.join(bringup_share, 'config', 'h12', + 'lowerbody_real.yaml'), + 'use_estimator': 'false', + }), + ]) diff --git a/core_ws/src/golem_bringup/launch/h12/h12_safety_grippers.launch.py b/core_ws/src/golem_bringup/launch/h12/h12_safety_grippers.launch.py new file mode 100644 index 0000000..1a17e77 --- /dev/null +++ b/core_ws/src/golem_bringup/launch/h12/h12_safety_grippers.launch.py @@ -0,0 +1,53 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration + + +# Minimal real-robot actuation bringup: estop + safety layer + both grippers. +# No lidar, no cameras, no state publishers, no arm IK -- just the pieces that +# 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 (h12_real_robot.launch.py) is still the normal path. +# Every node joins the launching shell's ROS_DOMAIN_ID, which must be 0, the real +# robot's command bus; ROS uses 0 when it is unset (see h12_control.launch.py). + + +def generate_launch_description(): + bringup_share = get_package_share_directory('golem_bringup') + + # 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='golem_safety config: a name under its config/h12/, or a path', + ) + + control = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(bringup_share, 'launch', 'h12', 'h12_control.launch.py') + ), + launch_arguments={ + 'use_sim_time': 'false', + 'sim': 'false', + 'use_estop': 'true', + 'use_frame_task': 'false', + 'safety_config': LaunchConfiguration('safety_config'), + }.items(), + ) + + grippers = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(bringup_share, 'launch', 'h12', 'h12_grippers.launch.py') + ), + ) + + return LaunchDescription([ + safety_config_arg, + control, + grippers, + ]) diff --git a/core_ws/src/golem_bringup/launch/h12/h12_sim.launch.py b/core_ws/src/golem_bringup/launch/h12/h12_sim.launch.py new file mode 100644 index 0000000..8c34ae9 --- /dev/null +++ b/core_ws/src/golem_bringup/launch/h12/h12_sim.launch.py @@ -0,0 +1,107 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from golem_ros2_models.paths import urdf_path +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 +from launch_ros.actions import Node + +# x86 sim bringup. ROS_DOMAIN_ID must be exported in the launching shell (see +# h12_control.launch.py). + + +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('golem_bringup') + default_rviz = os.path.join(bringup_share, 'rviz', 'h12', 'main.rviz') + + # 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'), + DeclareLaunchArgument('use_sliders', default_value='true'), + DeclareLaunchArgument('use_nav', default_value='true'), + DeclareLaunchArgument('use_skills', default_value='true'), + DeclareLaunchArgument('record_runs', default_value='true'), + # Which RL policy drives the legs (see h12_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 | fame | walk + 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), + + _include('golem_bringup', 'h12/h12_state.launch.py', { + **sim_time, + 'urdf_file': urdf_path('h12', ros=True), + }), + _include('golem_bringup', 'h12/h12_control.launch.py', { + **sim_time, + 'sim': 'true', + '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('golem_bringup', 'h12/h12_lowerbody.launch.py', { + **sim_time, + 'start_position_verified': 'true', + 'lowerbody': LaunchConfiguration('lowerbody'), + 'lowerbody_params': os.path.join(bringup_share, 'config', 'h12', 'lowerbody_sim.yaml'), + }), + _include('golem_bringup', 'h12/h12_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('golem_slam', 'h12_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 + # resulting camera_info / image / depth messages with the optical + # frame name below. The URDF defines only camera_link (a ROS link + # frame, +x = forward, +z = up); without this static TF the vision + # pipeline would treat optical-convention points as if they were in + # camera_link, rotating every detection ~90 deg out of place. + Node( + package='tf2_ros', + executable='static_transform_publisher', + name='camera_optical_frame_broadcaster', + arguments=['0', '0', '0', + '-1.5707963267948966', '0', '-1.5707963267948966', + 'camera_link', 'camera_color_optical_frame'], + parameters=[{'use_sim_time': True}], + output='screen', + emulate_tty=True, + ), + + Node( + package='rviz2', + executable='rviz2', + name='rviz2_sim', + arguments=['-d', LaunchConfiguration('rviz_config')], + parameters=[{'use_sim_time': True}], + output='screen', + emulate_tty=True, + condition=IfCondition(LaunchConfiguration('use_rviz')), + ), + ]) diff --git a/core_ws/src/h1_bringup/launch/h1_sim_bringup_mac.launch.py b/core_ws/src/golem_bringup/launch/h12/h12_sim_mac.launch.py similarity index 78% rename from core_ws/src/h1_bringup/launch/h1_sim_bringup_mac.launch.py rename to core_ws/src/golem_bringup/launch/h12/h12_sim_mac.launch.py index a40c46f..63a5ee7 100644 --- a/core_ws/src/h1_bringup/launch/h1_sim_bringup_mac.launch.py +++ b/core_ws/src/golem_bringup/launch/h12/h12_sim_mac.launch.py @@ -1,34 +1,34 @@ -"""Minimal, CPU-only (Apple Silicon) bringup for the H1 MuJoCo sim. +""" +Minimal, CPU-only (Apple Silicon) bringup for the H1 MuJoCo sim. -A trimmed variant of h1_sim_bringup.launch.py that runs ONLY the core robot -nodes — no vision foundation models (gemini/sam/graspgen), no h12_skills, no +A trimmed variant of h12_sim.launch.py that runs ONLY the core robot +nodes — no vision foundation models (gemini/sam/graspgen), no golem_skills, no Nav2/SLAM, no rviz/sliders. Those need the heavy x86-tuned ML stack that the slim arm64 ROS image intentionally omits. Nodes started: * static camera_link -> camera_color_optical_frame TF * static livox_link -> lidar_link TF (so RViz can place /livox/pointcloud) - * joint_state_publisher (h12_ros2_controller) + * joint_state_publisher (golem_upper_ik) * robot_state_publisher - * frame_task_server (h12_ros2_controller, Pink IK) - * safety_node (h12_safety_layer) + * frame_task_server (golem_upper_ik, Pink IK) + * safety_node (golem_safety, sim:=true) Launch by path from the slim ROS container: - ros2 launch /home/code/core_ws/src/h1_bringup/launch/h1_sim_bringup_mac.launch.py + ros2 launch /home/code/core_ws/src/golem_bringup/launch/h12/h12_sim_mac.launch.py """ import os from ament_index_python.packages import get_package_share_directory +from golem_ros2_models.paths import urdf_path from launch import LaunchDescription from launch.actions import IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node -ASSETS_DIR = '/home/code/CL_Assets' - def generate_launch_description(): - with open(os.path.join(ASSETS_DIR, 'ros_assets', 'h1_2_magpie_ros.urdf'), 'r') as urdf_file: + with open(urdf_path('h12', ros=True), 'r') as urdf_file: robot_description = urdf_file.read() # MuJoCo publishes /clock with sim time; keep all nodes on it. @@ -44,6 +44,7 @@ def generate_launch_description(): 'camera_link', 'camera_color_optical_frame'], parameters=[sim_time_param], output='screen', + emulate_tty=True, ), # The MuJoCo lidar bridge stamps /livox/{lidar,pointcloud} with frame_id # 'lidar_link', which is co-located with the URDF's 'livox_link' mount but @@ -56,13 +57,15 @@ def generate_launch_description(): arguments=['0', '0', '0', '0', '0', '0', 'livox_link', 'lidar_link'], parameters=[sim_time_param], output='screen', + emulate_tty=True, ), Node( - package='h12_ros2_controller', + package='golem_upper_ik', executable='joint_state_publisher', name='joint_state_publisher', - parameters=[sim_time_param], + parameters=[{'robot': 'h12'}, sim_time_param], output='screen', + emulate_tty=True, ), Node( package='robot_state_publisher', @@ -70,22 +73,24 @@ def generate_launch_description(): name='robot_state_publisher', parameters=[{'robot_description': robot_description}, sim_time_param], output='screen', + emulate_tty=True, ), Node( - package='h12_ros2_controller', + package='golem_upper_ik', executable='frame_task_server', name='frame_task_server', - arguments=['--config', 'sim_safety_split.yaml'], - parameters=[sim_time_param], + parameters=[{'robot': 'h12', 'config': 'sim_safety_split.yaml'}, sim_time_param], output='screen', + emulate_tty=True, ), Node( - package='h12_safety_layer', + package='golem_safety', executable='safety_node', name='safety_node', - arguments=['--config', 'sim_safety_split.yaml'], - parameters=[sim_time_param], + parameters=[{'robot': 'h12', 'config': 'sim_safety_split.yaml', 'sim': True}, + sim_time_param], output='screen', + emulate_tty=True, ), ] @@ -97,25 +102,24 @@ def generate_launch_description(): # standing UNSUPPORTED (verified). Does not locomote. # GOLEM_LOWERBODY=walk TorchScript walk policy on its own (marches in place; # topples in the cluttered kitchen). - # GOLEM_LOWERBODY=switch switchable controller: starts in FAME (stand) and - # auto-switches stand<->walk from ||/cmd_vel|| (no start - # services). This is the one to use for stand<->walk and - # the nav demo — nav2's /cmd_vel makes it walk; it stands - # again when the command drops to ~0. + # GOLEM_LOWERBODY=almi lower-body controller pinned to ALMI: stands, and + # walks on /cmd_vel. This is the one to use for the nav + # demo — nav2's /cmd_vel makes it walk; it stands again + # when the command drops to ~0. lowerbody = os.environ.get('GOLEM_LOWERBODY', '').strip().lower() _lowerbody_exec = {'fame': 'fame_node', 'walk': 'walking_node', - 'switch': 'lowerbody_controller_node'}.get(lowerbody) + 'almi': 'lowerbody_controller_node'}.get(lowerbody) if _lowerbody_exec: _lb_params = [sim_time_param] if _lowerbody_exec == 'lowerbody_controller_node': - # Start in FAME; the node then auto-switches to walk on /cmd_vel. - _lb_params.append({'active_policy': 'fame'}) + _lb_params.append({'active_policy': 'almi'}) nodes.append(Node( package='h12_lowerbody_rl', executable=_lowerbody_exec, name=_lowerbody_exec, parameters=_lb_params, output='screen', + emulate_tty=True, )) # Optional 2D SLAM (GOLEM_SLAM=1): pointcloud_to_laserscan flattens the Livox @@ -123,7 +127,7 @@ def generate_launch_description(): # /map from it against the sim's ground-truth /odom (odom -> pelvis). No # FAST-LIO needed. REQUIRES the robocasa/sim container started with # GOLEM_SIM_ODOM=1 (off by default) to publish that /odom; without it slam_toolbox - # has no odom and the map stays empty. p2l params match h1_navigation.launch.py + # has no odom and the map stays empty. p2l params match h12_navigation.launch.py # but are fed from the raw /livox/pointcloud instead of FAST-LIO's registered cloud. if os.environ.get('GOLEM_SLAM', '').strip().lower() in ('1', 'true', 'on'): nodes.append(Node( @@ -132,7 +136,7 @@ def generate_launch_description(): name='pointcloud_to_laserscan', parameters=[{ 'target_frame': 'pelvis', - # Relaxed vs h1_navigation's FAST-LIO tuning for the raw sim lidar: + # Relaxed vs h12_navigation's FAST-LIO tuning for the raw sim lidar: # a taller band (counter tops / cabinets). range_min 0.65 drops the # robot's OWN legs/feet — the torso lidar looks down and returns them # at ~0.4 m; mapping those puts a phantom obstacle under the robot and @@ -149,26 +153,28 @@ def generate_launch_description(): }, sim_time_param], remappings=[('cloud_in', '/livox/pointcloud'), ('scan', '/converted_scan')], output='screen', + emulate_tty=True, )) nodes.append(Node( package='slam_toolbox', 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('golem_slam'), 'config', 'slam_toolbox_h1.yaml'), {'odom_frame': 'odom', 'scan_topic': '/converted_scan'}, sim_time_param, ], output='screen', + emulate_tty=True, )) # Optional autonomous navigation (GOLEM_NAV2=1, implies SLAM for the map): the # nav2 stack plans a collision-free path on the SLAM map + lidar costmap and # drives it via /cmd_vel, which the walk policy consumes (nav2's # velocity_smoother publishes /cmd_vel). Send goals with the RViz "2D Nav Goal" - # tool or the /navigate_to_pose action. With GOLEM_LOWERBODY=switch the controller - # auto-switches to walk as soon as nav2 publishes /cmd_vel (no manual step). The + # tool or the /navigate_to_pose action. With GOLEM_LOWERBODY=almi the robot + # walks as soon as nav2 publishes /cmd_vel (no manual step). The # robot must be standing in open floor — nav2 rejects a goal if the robot's start # cell is occupied (e.g. pressed against a counter). # nav2_config_mac.yaml is nav2_config.yaml with the odom frame swapped from @@ -177,7 +183,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('golem_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/golem_bringup/launch/h12/h12_state.launch.py b/core_ws/src/golem_bringup/launch/h12/h12_state.launch.py new file mode 100644 index 0000000..c38e711 --- /dev/null +++ b/core_ws/src/golem_bringup/launch/h12/h12_state.launch.py @@ -0,0 +1,44 @@ +from golem_ros2_models.paths import urdf_path +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 + + +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=urdf_path('h12', ros=True), + 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='golem_upper_ik', + executable='joint_state_publisher', + name='joint_state_publisher', + parameters=[{'robot': 'h12'}, sim_time_param], + output='screen', + emulate_tty=True, + ), + Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + parameters=[{'robot_description': robot_description}, sim_time_param], + output='screen', + emulate_tty=True, + ), + ]) diff --git a/core_ws/src/golem_bringup/launch/h12/lmg_real.launch.py b/core_ws/src/golem_bringup/launch/h12/lmg_real.launch.py new file mode 100644 index 0000000..6b7031e --- /dev/null +++ b/core_ws/src/golem_bringup/launch/h12/lmg_real.launch.py @@ -0,0 +1,92 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from golem_ros2_models.paths import urdf_path +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 + +# Loco-manipulation game, real robot, onboard PC. Run it INSTEAD of +# h12_real_robot.launch.py, never beside it: it starts the same drivers, +# state publishers, estop, safety layer and golem_slam navigation stack +# (use_slam), but without frame_task_server, +# because marl_deploy_node owns both safety channels. Keep the lower-body RL +# and MJPC controllers (h12_real_robot.launch.py's leg control) off. +# +# Nothing commands the robot until both operator gates pass: +# 1. start_position_verified:=true starts marl_deploy_node; it ramps all 27 +# joints to the policy's default pose and holds there. +# 2. ros2 service call /marl_deploy_node/confirm_engage std_srvs/srv/Trigger +# engages the policy. +# The robot then walks on /cmd_vel and reaches on the arm goals the operator sets +# through /marl_deploy_node/set_mode and set_arm_goals. +# +# Every node joins the launching shell's ROS_DOMAIN_ID, which must be 0, the real +# robot's command bus; ROS uses 0 when it is unset (see h12_control.launch.py). + +GAME_ARGS = ('start_position_verified', 'policy', 'prepose_ramp_s', 'prepose_max_vel', + 'imu_offset_roll_deg', 'imu_offset_pitch_deg', 'imu_offset_yaw_deg') + + +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(): + # Real robot runs on wall-clock time (no simulator /clock). + wall_time = {'use_sim_time': 'false'} + + return LaunchDescription([ + DeclareLaunchArgument('start_position_verified', default_value='false'), + DeclareLaunchArgument('policy', description='policies// of lmg_deploy (required)'), + DeclareLaunchArgument('prepose_ramp_s', default_value='6.0'), + DeclareLaunchArgument('prepose_max_vel', default_value='0.25'), + # Startup trim of the policy's IMU, the MID360 (/livox/imu): its reading with the torso + # level. The URDF livox_joint carries the measured mount, so it starts at 0. Tune live + # with ros2 param set /marl_deploy_node imu_offset_pitch_deg . + DeclareLaunchArgument('imu_offset_roll_deg', default_value='0.0'), + DeclareLaunchArgument('imu_offset_pitch_deg', default_value='0.0'), + DeclareLaunchArgument('imu_offset_yaw_deg', default_value='0.0'), + DeclareLaunchArgument('safety_config', default_value='relax_safety_split.yaml'), + DeclareLaunchArgument('use_slam', default_value='true'), + # The game reads the MID360 for SLAM and as the policy's IMU; marl_deploy_node waits + # in the pre-pose until /livox/imu is up. + DeclareLaunchArgument('use_lidar', default_value='true', + description='MID360 driver and livox_imu_upright'), + DeclareLaunchArgument('use_head_camera', default_value='false', + description='head RealSense'), + DeclareLaunchArgument('use_grippers', default_value='false', + description='left and right gripper nodes'), + + _include('golem_bringup', 'h12/h12_real_drivers.launch.py', { + **wall_time, + **{name: LaunchConfiguration(name) + for name in ('use_lidar', 'use_head_camera', 'use_grippers')}, + }), + _include('golem_bringup', 'h12/h12_state.launch.py', wall_time), + _include('golem_bringup', 'h12/h12_control.launch.py', { + **wall_time, + 'sim': 'false', + 'use_estop': 'true', + 'use_frame_task': 'false', + 'safety_config': LaunchConfiguration('safety_config'), + }), + _include('golem_bringup', 'h12/h12_locomanipulationgame.launch.py', { + **wall_time, + **{name: LaunchConfiguration(name) for name in GAME_ARGS}, + 'urdf_path': urdf_path('h12'), + # The operator gate and the absent elastic band are fixed on the real robot, not + # arguments. + 'engage_wait_for_confirm': 'true', + 'disable_elastic_band': 'false', + }), + _include('golem_slam', 'h12_navigation.launch.py', wall_time, + condition=IfCondition(LaunchConfiguration('use_slam'))), + ]) diff --git a/core_ws/src/golem_bringup/launch/h12/lmg_sim.launch.py b/core_ws/src/golem_bringup/launch/h12/lmg_sim.launch.py new file mode 100644 index 0000000..e1d7ecd --- /dev/null +++ b/core_ws/src/golem_bringup/launch/h12/lmg_sim.launch.py @@ -0,0 +1,94 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from golem_ros2_models.paths import urdf_path +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 +from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterValue + +# Loco-manipulation game bringup (x86 sim): the state publishers, the split +# safety layer with 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), h12_locomanipulationgame.launch.py with no operator confirm, and the +# golem_slam navigation stack (use_slam), with rviz (use_rviz) for nav2 goals. Start the +# simulator first. The real-robot counterpart is +# lmg_real.launch.py. +# +# ROS_DOMAIN_ID must be exported in the launching shell (see +# h12_control.launch.py), and not as 0: this launch engages and walks without an +# operator, and domain 0 (also the DDS default when the variable is unset) is +# the real robot's command bus. + +GAME_ARGS = ('policy', 'prepose_ramp_s', 'prepose_max_vel') + + +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(): + if os.environ.get('ROS_DOMAIN_ID', '').strip() in ('', '0'): + raise RuntimeError( + 'lmg_sim: ROS_DOMAIN_ID is unset or 0, the real robot\'s bus. ' + 'Export a sim domain (e.g. ROS_DOMAIN_ID=42), or use lmg_real.launch.py ' + 'on the robot.') + + bringup_share = get_package_share_directory('golem_bringup') + sim_time = {'use_sim_time': LaunchConfiguration('use_sim_time')} + + return LaunchDescription([ + DeclareLaunchArgument('use_sim_time', default_value='true'), + DeclareLaunchArgument('policy', description='policies// of lmg_deploy (required)'), + DeclareLaunchArgument('prepose_ramp_s', default_value='6.0'), + DeclareLaunchArgument('prepose_max_vel', default_value='0.25'), + DeclareLaunchArgument('safety_config', default_value='relax_safety_split.yaml'), + DeclareLaunchArgument('use_slam', default_value='true'), + DeclareLaunchArgument('use_rviz', default_value='true'), + DeclareLaunchArgument( + 'rviz_config', default_value=os.path.join(bringup_share, 'rviz', 'h12', 'main.rviz')), + + _include('golem_bringup', 'h12/h12_state.launch.py', { + **sim_time, + 'urdf_file': urdf_path('h12', ros=True), + }), + _include('golem_bringup', 'h12/h12_control.launch.py', { + **sim_time, + 'sim': 'true', + 'use_frame_task': 'false', + 'safety_config': LaunchConfiguration('safety_config'), + # no hardware e-stop node in sim to wait for + 'safety_delay': '0.0', + }), + _include('golem_bringup', 'h12/h12_locomanipulationgame.launch.py', { + **sim_time, + **{name: LaunchConfiguration(name) for name in GAME_ARGS}, + 'urdf_path': urdf_path('h12'), + 'start_position_verified': 'true', + # engages as soon as the pre-pose settles, with no operator confirm + 'engage_wait_for_confirm': 'false', + 'disable_elastic_band': 'true', + }), + _include('golem_slam', 'h12_navigation.launch.py', sim_time, + condition=IfCondition(LaunchConfiguration('use_slam'))), + + Node( + package='rviz2', + executable='rviz2', + name='rviz2_locomanipulationgame', + arguments=['-d', LaunchConfiguration('rviz_config')], + parameters=[{'use_sim_time': ParameterValue(LaunchConfiguration('use_sim_time'), + value_type=bool)}], + output='screen', + emulate_tty=True, + condition=IfCondition(LaunchConfiguration('use_rviz')), + ), + ]) diff --git a/core_ws/src/h1_bringup/package.xml b/core_ws/src/golem_bringup/package.xml similarity index 73% rename from core_ws/src/h1_bringup/package.xml rename to core_ws/src/golem_bringup/package.xml index 0440f9f..9a95ac7 100644 --- a/core_ws/src/h1_bringup/package.xml +++ b/core_ws/src/golem_bringup/package.xml @@ -1,9 +1,9 @@ - h1_bringup + golem_bringup 0.0.0 - Launch files to bring up the H1 simulation visualization. + Launch files that start the H1-2 and the G1, on the real robot or in the sim. max MIT @@ -12,9 +12,13 @@ launch_ros estop robot_state_publisher - h12_ros2_controller - h12_safety_layer + golem_safety + golem_upper_ik h12_lowerbody_rl + lmg_deploy + golem_ros2_models + dex_3 + g1_homie cl_realsense model_server custom_ros_messages @@ -23,6 +27,9 @@ python3-opencv python3-numpy python3-scipy + golem_slam + deploy_mjpc + golem_skills fast_lio tf2_ros pointcloud_to_laserscan diff --git a/core_ws/src/h1_bringup/resource/h1_bringup b/core_ws/src/golem_bringup/resource/golem_bringup similarity index 100% rename from core_ws/src/h1_bringup/resource/h1_bringup rename to core_ws/src/golem_bringup/resource/golem_bringup diff --git a/core_ws/src/golem_bringup/rviz/g1/g1.rviz b/core_ws/src/golem_bringup/rviz/g1/g1.rviz new file mode 100644 index 0000000..4308283 --- /dev/null +++ b/core_ws/src/golem_bringup/rviz/g1/g1.rviz @@ -0,0 +1,804 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /RobotModel1/Description Topic1 + - /TF1/Frames1 + - /cloud_body_filtered1/Topic1 + - /cloud_registered_body1/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: cloud_body_filtered +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 + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Transient Local + 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 + d435_link: + Alpha: 1 + Show Axes: false + Show Trail: false + head_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + imu_in_pelvis: + Alpha: 1 + Show Axes: false + Show Trail: false + imu_in_torso: + 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_hand_index_0_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_hand_index_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_hand_middle_0_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_hand_middle_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_hand_palm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_hand_thumb_0_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_hand_thumb_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_hand_thumb_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + 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 + logo_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + mid360_link: + Alpha: 1 + Show Axes: false + Show Trail: false + pelvis: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + pelvis_contour_link: + 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_hand_index_0_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_hand_index_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_hand_middle_0_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_hand_middle_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_hand_palm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_hand_thumb_0_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_hand_thumb_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_hand_thumb_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + 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 + waist_roll_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + waist_yaw_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 + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: false + base_footprint: + Value: true + body: + Value: true + camera_init: + Value: true + d435_link: + Value: true + head_color_optical_frame: + Value: false + head_link: + Value: true + imu_in_pelvis: + Value: true + imu_in_torso: + Value: true + left_ankle_pitch_link: + Value: false + left_ankle_roll_link: + Value: false + left_elbow_link: + Value: false + left_hand_index_0_link: + Value: true + left_hand_index_1_link: + Value: true + left_hand_middle_0_link: + Value: true + left_hand_middle_1_link: + Value: true + left_hand_palm_link: + Value: true + left_hand_thumb_0_link: + Value: true + left_hand_thumb_1_link: + Value: true + left_hand_thumb_2_link: + Value: true + 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: true + livox_frame: + Value: true + logo_link: + Value: false + map: + Value: true + mid360_link: + Value: true + odom: + Value: true + pelvis: + Value: true + pelvis_contour_link: + Value: true + right_ankle_pitch_link: + Value: false + right_ankle_roll_link: + Value: false + right_elbow_link: + Value: false + right_hand_index_0_link: + Value: true + right_hand_index_1_link: + Value: true + right_hand_middle_0_link: + Value: true + right_hand_middle_1_link: + Value: true + right_hand_palm_link: + Value: true + right_hand_thumb_0_link: + Value: true + right_hand_thumb_1_link: + Value: true + right_hand_thumb_2_link: + Value: true + 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: true + torso_link: + Value: true + waist_roll_link: + Value: true + waist_yaw_link: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + map: + odom: + base_footprint: + {} + camera_init: + body: + pelvis: + imu_in_pelvis: + {} + left_hip_pitch_link: + left_hip_roll_link: + left_hip_yaw_link: + left_knee_link: + left_ankle_pitch_link: + left_ankle_roll_link: + {} + pelvis_contour_link: + {} + right_hip_pitch_link: + right_hip_roll_link: + right_hip_yaw_link: + right_knee_link: + right_ankle_pitch_link: + right_ankle_roll_link: + {} + waist_yaw_link: + waist_roll_link: + torso_link: + d435_link: + head_color_optical_frame: + {} + head_link: + {} + imu_in_torso: + {} + 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_hand_palm_link: + left_hand_index_0_link: + left_hand_index_1_link: + {} + left_hand_middle_0_link: + left_hand_middle_1_link: + {} + left_hand_thumb_0_link: + left_hand_thumb_1_link: + left_hand_thumb_2_link: + {} + logo_link: + {} + mid360_link: + livox_frame: + {} + 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: + right_hand_palm_link: + right_hand_index_0_link: + right_hand_index_1_link: + {} + right_hand_middle_0_link: + right_hand_middle_1_link: + {} + right_hand_thumb_0_link: + right_hand_thumb_1_link: + right_hand_thumb_2_link: + {} + Update Interval: 0 + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + map: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /occupied_cells_vis_array + Value: true + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Projected_Map + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /projected_map + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /projected_map_updates + Use Timestamp: false + 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: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 100 + Min Color: 0; 0; 0 + Min Intensity: 100 + Name: cloud_body_filtered + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.019999999552965164 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /cloud_body_filtered + 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: 255 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: cloud_registered_body + 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: /cloud_registered_body + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + 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: true + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + 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: 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: Path + 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: /transformed_global_plan + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + 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: 13.38548469543457 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 1.5647963285446167 + Target Frame: + Value: Orbit (rviz) + Yaw: 3.6262638568878174 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1371 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd000000040000000000000156000004c1fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000004c1000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000004c6fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000004c6000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006970000003efc0100000002fb0000000800540069006d00650100000000000006970000025300fffffffb0000000800540069006d006501000000000000045000000000000000000000053b000004c100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1687 + X: 66 + Y: 32 diff --git a/core_ws/src/h1_bringup/rviz/sim.rviz b/core_ws/src/golem_bringup/rviz/h12/main.rviz similarity index 80% rename from core_ws/src/h1_bringup/rviz/sim.rviz rename to core_ws/src/golem_bringup/rviz/h12/main.rviz index 352a88a..42ff6a8 100644 --- a/core_ws/src/h1_bringup/rviz/sim.rviz +++ b/core_ws/src/golem_bringup/rviz/h12/main.rviz @@ -6,26 +6,14 @@ Panels: Expanded: - /Global Options1 - /Status1 + - /RobotModel1/Status1 + - /RobotModel1/Description Topic1 - /TF1 - /TF1/Frames1 - - /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 + - /cloud_body_filtered1/Topic1 + - /cloud_registered_body1/Topic1 Splitter Ratio: 0.5 - Tree Height: 365 + Tree Height: 1085 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -43,7 +31,7 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: "" + SyncSource: cloud_body_filtered Visualization Manager: Class: "" Displays: @@ -65,516 +53,6 @@ Visualization Manager: 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_init: - Value: false - graspgenx_target_frame: - 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: - Value: false - left_ankle_roll_link: - Value: false - left_elbow_link: - Value: false - left_grasp_frame: - 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: - 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 - pick_place_target_frame: - Value: true - 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_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: - 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: - 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: - {} - left_hand_depth_frame: - left_hand_depth_optical_frame: - {} - 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: - {} - right_hand_depth_frame: - right_hand_depth_optical_frame: - {} - right_grasp_frame: - {} - right_graspgenx_frame: - {} - graspgenx_target_frame: - {} - pick_place_target_frame: - {} - 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: false - 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: 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: 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: false - 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: 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: "" - 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: false - 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: false - - 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: false - 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: false - - 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: false - 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: false - Alpha: 1 Class: rviz_default_plugins/RobotModel Collision Enabled: false @@ -582,7 +60,7 @@ Visualization Manager: Description Source: Topic Description Topic: Depth: 5 - Durability Policy: Volatile + Durability Policy: Transient Local History Policy: Keep Last Reliability Policy: Reliable Value: /robot_description @@ -863,70 +341,612 @@ Visualization Manager: TF Prefix: "" Update Interval: 0 Value: true - Visual Enabled: true + Visual Enabled: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: false + base_footprint: + Value: true + body: + Value: true + camera_init: + Value: true + head_camera_link: + Value: true + 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: true + imu_link: + Value: true + left_ankle_pitch_link: + Value: false + left_ankle_roll_link: + Value: false + left_elbow_link: + Value: false + left_grasp_frame: + Value: true + left_graspgenx_frame: + Value: true + 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: true + 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: true + logo_link: + Value: false + map: + Value: true + odom: + Value: true + pelvis: + Value: true + 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: true + right_graspgenx_frame: + Value: true + 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: true + torso_link: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + map: + odom: + base_footprint: + {} + 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: + 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: + {} + Update Interval: 0 + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + map: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /occupied_cells_vis_array + Value: true + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Projected_Map + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /projected_map + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /projected_map_updates + Use Timestamp: false + 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: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 255 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: cloud_body_filtered + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.019999999552965164 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /cloud_body_filtered + 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: 255 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: cloud_registered_body + 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: /cloud_registered_body + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + 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: true - Alpha: 0.699999988079071 Class: rviz_default_plugins/Map - Color Scheme: map + Color Scheme: costmap Draw Behind: false - Enabled: false - Name: Map + Enabled: true + Name: LocalCostMap Topic: Depth: 5 Durability Policy: Volatile Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /map + Value: /local_costmap/costmap Update Topic: Depth: 5 Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /map_updates + Value: /local_costmap/costmap_updates Use Timestamp: false + 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: false + 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: Best Effort + Value: /livox/pointcloud + Use Fixed Frame: true + Use rainbow: true Value: false - - Alpha: 0.699999988079071 - Class: rviz_default_plugins/Map - Color Scheme: map - Draw Behind: 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: false - Name: GlobalCostMap + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 175 + 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: /global_costmap/costmap - Update Topic: + 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: "" + 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: /global_costmap/costmap_updates - Use Timestamp: false + 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: 1.933724045753479 + Min Color: 0; 0; 0 + Min Intensity: -2.969715118408203 + 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: true + 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/depth/image_rect_raw/compressedDepth + Value: true + - 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/compressed + 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: false + 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: false + - 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: false + 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: false - Alpha: 0.699999988079071 Class: rviz_default_plugins/Map Color Scheme: map Draw Behind: false Enabled: false - Name: LocalCostMap + Name: Map Topic: Depth: 5 Durability Policy: Volatile Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /local_costmap/costmap + Value: /map Update Topic: Depth: 5 Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /local_costmap/costmap_updates + Value: /map_updates Use Timestamp: false - Value: false + Value: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: @@ -1181,7 +1201,7 @@ Visualization Manager: Value: /frame_targets Value: false - Class: rviz_default_plugins/MarkerArray - Enabled: false + Enabled: true Name: GraspGenMarkers Namespaces: {} @@ -1191,7 +1211,7 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: /graspgen_markers - Value: false + Value: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: @@ -1272,7 +1292,7 @@ Visualization Manager: Color: 255; 255; 255 Color Transformer: RGB8 Decay Time: 0 - Enabled: false + Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 Max Intensity: 4096 @@ -1293,7 +1313,7 @@ Visualization Manager: Value: /realsense/head/depth/color/points Use Fixed Frame: true Use rainbow: true - Value: false + Value: true - Alpha: 1 Autocompute Intensity Bounds: true Autocompute Value Bounds: @@ -1363,7 +1383,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 +1395,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 +1425,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,11 +1437,11 @@ 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 - Fixed Frame: pelvis + Fixed Frame: map Frame Rate: 30 Name: root Tools: @@ -1464,36 +1484,36 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 3.1023149490356445 + Distance: 3.4968740940093994 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 + X: 0 + Y: 0 + Z: 0 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.6597973704338074 + Pitch: 1.149796724319458 Target Frame: - Value: Orbit (rviz_default_plugins) - Yaw: 1.8074543476104736 + Value: Orbit (rviz) + Yaw: 0.24309346079826355 Saved: ~ Window Geometry: Displays: - collapsed: true + collapsed: false HeadAnnotatedImage: collapsed: false HeadColorImage: collapsed: false HeadDepthImage: collapsed: false - Height: 1016 + Height: 1376 Hide Left Dock: false Hide Right Dock: true LeftColorImage: @@ -1502,7 +1522,7 @@ Window Geometry: collapsed: false LeftHandAnnotatedImage: collapsed: false - QMainWindow State: 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 + QMainWindow State: 000000ff00000000fd000000040000000000000156000004c6fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000004c6000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000004c6fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000004c6000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004dd0000003efc0100000002fb0000000800540069006d00650100000000000004dd0000025300fffffffb0000000800540069006d0065010000000000000450000000000000000000000381000004c600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 RightColorImage: collapsed: false RightDepthImage: @@ -1518,7 +1538,7 @@ Window Geometry: Tool Properties: collapsed: false Views: - collapsed: false - Width: 925 - X: 995 + collapsed: true + Width: 1245 + X: 1990 Y: 27 diff --git a/core_ws/src/h1_bringup/scripts/slider_debugger.py b/core_ws/src/golem_bringup/scripts/slider_debugger.py similarity index 96% rename from core_ws/src/h1_bringup/scripts/slider_debugger.py rename to core_ws/src/golem_bringup/scripts/slider_debugger.py index 9389b22..08a3380 100755 --- a/core_ws/src/h1_bringup/scripts/slider_debugger.py +++ b/core_ws/src/golem_bringup/scripts/slider_debugger.py @@ -1,5 +1,6 @@ #!/usr/bin/env python3 -"""Live slider GUI for the H1 sim's frame_task_server and magpie grippers. +""" +Live slider GUI for the H1 sim's frame_task_server and magpie grippers. Twelve sliders (XYZ + RPY for each arm) drive the `/frame_task` action targeting the GraspGenX gripper-base frames `left_graspgenx_frame` / `right_graspgenx_frame` @@ -17,15 +18,14 @@ pelvis +Z); set RPY = 1.5708 0 1.5708 to match the wrist's identity orientation. Usage: - ros2 run h1_bringup slider_debugger.py + ros2 run golem_bringup slider_debugger.py # or - python3 core_ws/src/h1_bringup/scripts/slider_debugger.py + python3 core_ws/src/golem_bringup/scripts/slider_debugger.py """ import math import tkinter as tk -import numpy as np from scipy.spatial.transform import Rotation import rclpy @@ -61,7 +61,8 @@ def __init__(self): self.frame_task_cli = ActionClient(self, FrameTask, '/frame_task') self.left_gripper_cli = self.create_client(SetGripperPosition, self.left_gripper_srv_name) - self.right_gripper_cli = self.create_client(SetGripperPosition, self.right_gripper_srv_name) + self.right_gripper_cli = self.create_client(SetGripperPosition, + self.right_gripper_srv_name) # XYZ in pelvis + RPY (xyz-euler) of the driven frame. self._initial_left = (0.3, 0.2, 0.1, 0.0, 0.0, 0.0) diff --git a/core_ws/src/golem_bringup/setup.cfg b/core_ws/src/golem_bringup/setup.cfg new file mode 100644 index 0000000..45deca4 --- /dev/null +++ b/core_ws/src/golem_bringup/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/golem_bringup +[install] +install_scripts=$base/lib/golem_bringup diff --git a/core_ws/src/golem_bringup/setup.py b/core_ws/src/golem_bringup/setup.py new file mode 100644 index 0000000..5a2d656 --- /dev/null +++ b/core_ws/src/golem_bringup/setup.py @@ -0,0 +1,42 @@ +import os +from glob import glob + +from setuptools import find_packages, setup + +package_name = 'golem_bringup' + +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']), + # One entry per subdirectory, installed at the same relative path: + # ros2 launch finds a launch file by name anywhere under the share + # directory, so each file is installed once. + (os.path.join('share', package_name, 'launch', 'h12'), glob('launch/h12/*.launch.py')), + (os.path.join('share', package_name, 'launch', 'g1'), glob('launch/g1/*.launch.py')), + (os.path.join('share', package_name, 'config', 'h12'), glob('config/h12/*.yaml')), + (os.path.join('share', package_name, 'rviz', 'h12'), glob('rviz/h12/*.rviz')), + (os.path.join('share', package_name, 'rviz', 'g1'), glob('rviz/g1/*.rviz')), + ], + scripts=['scripts/slider_debugger.py'], + install_requires=['setuptools'], + zip_safe=True, + maintainer='max', + maintainer_email='maxlconway@gmail.com', + description='Launch files that start the H1-2 and the G1, on the real robot or in the sim.', + license='MIT', + extras_require={ + 'test': [ + 'pytest', + ], + }, + entry_points={ + 'console_scripts': [ + 'livox_imu_upright = golem_bringup.livox_imu_upright:main', + ], + }, +) diff --git a/core_ws/src/golem_bringup/test/test_bringup_packages.py b/core_ws/src/golem_bringup/test/test_bringup_packages.py new file mode 100644 index 0000000..59c3ee3 --- /dev/null +++ b/core_ws/src/golem_bringup/test/test_bringup_packages.py @@ -0,0 +1,174 @@ +""" +The bringups of both robots start golem_safety and golem_upper_ik. + +golem_bringup depends on both packages. For the H1-2, h12_control.launch.py needs sim +from every top-level file, the real-robot ones pass use_estop, and it refuses a +frame_task_config that does not run with its safety_config. For the G1, g1_core.launch.py +starts golem_safety's safety_node with robot g1, the safety config, sim as the top-level +file says and no hardware estop, and golem_upper_ik's frame_task_server and +joint_state_publisher with robot g1. The launch tests need the workspace built and +sourced. +""" + +import importlib.util +from pathlib import Path +import xml.etree.ElementTree as ElementTree + +from launch import LaunchContext +from launch.actions import (DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction, + TimerAction) +import launch.launch_description_sources +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.utilities import normalize_to_list_of_substitutions, perform_substitutions +from launch_ros.actions import Node +from launch_ros.utilities import evaluate_parameters +import pytest + +PACKAGE = Path(__file__).resolve().parents[1] + + +def test_package_depends_on_golem_safety_and_golem_upper_ik(): + root = ElementTree.parse(PACKAGE / 'package.xml').getroot() + depends = {element.text for element in root + if element.tag in ('depend', 'exec_depend', 'build_depend', 'test_depend')} + assert {'golem_safety', 'golem_upper_ik'} <= depends + + +def _load(launch_file): + (path,) = (PACKAGE / 'launch').rglob(launch_file) + spec = importlib.util.spec_from_file_location(launch_file.split('.')[0], path) + module = importlib.util.module_from_spec(spec) + spec.loader.exec_module(module) + return module + + +def _text(context, value): + return perform_substitutions(context, normalize_to_list_of_substitutions(value)) + + +def _context(description, **args): + """Return a LaunchContext with description's defaults, overridden by args.""" + context = LaunchContext() + for entity in description.entities: + if isinstance(entity, DeclareLaunchArgument) and entity.default_value is not None: + context.launch_configurations[entity.name] = _text(context, entity.default_value) + context.launch_configurations.update(args) + return context + + +def test_control_sim_has_no_default(): + description = _load('h12_control.launch.py').generate_launch_description() + (sim,) = [entity for entity in description.entities + if isinstance(entity, DeclareLaunchArgument) and entity.name == 'sim'] + assert sim.default_value is None + + +@pytest.mark.parametrize('launch_file, sim, use_estop', [ + ('h12_sim.launch.py', 'true', 'false'), + ('lmg_sim.launch.py', 'true', 'false'), + ('h12_real_robot.launch.py', 'false', 'true'), + ('lmg_real.launch.py', 'false', 'true'), + ('h12_safety_grippers.launch.py', 'false', 'true'), +]) +def test_top_level_files_pass_sim_and_use_estop(launch_file, sim, use_estop, monkeypatch): + class Source(PythonLaunchDescriptionSource): + """A PythonLaunchDescriptionSource that keeps the path it was given.""" + + def __init__(self, launch_file_path): + super().__init__(launch_file_path) + self.path = launch_file_path + + monkeypatch.setattr(launch.launch_description_sources, 'PythonLaunchDescriptionSource', + Source) + # lmg_sim refuses an unset or 0 ROS_DOMAIN_ID + monkeypatch.setenv('ROS_DOMAIN_ID', '193') + description = _load(launch_file).generate_launch_description() + context = _context(description) + (control,) = [entity for entity in description.entities + if isinstance(entity, IncludeLaunchDescription) + and Path(entity.launch_description_source.path).name == 'h12_control.launch.py'] + arguments = {_text(context, name): _text(context, value) + for name, value in control.launch_arguments} + assert arguments['sim'] == sim + # absent: h12_control.launch.py's default, false + assert arguments.get('use_estop', 'false') == use_estop + + +def test_control_checks_the_pair_before_any_node(monkeypatch): + control = _load('h12_control.launch.py') + calls = [] + monkeypatch.setattr(control, 'check_pairing', lambda context: calls.append(context) or []) + entities = [entity for entity in control.generate_launch_description().entities + if not isinstance(entity, DeclareLaunchArgument)] + assert isinstance(entities[0], OpaqueFunction) + context = LaunchContext() + assert entities[0].execute(context) == [] + assert calls == [context] + + +@pytest.mark.parametrize('safety_config, frame_task_config, use_frame_task, refused', [ + ('relax_safety_split.yaml', 'safety_split.yaml', 'true', False), + ('sim_safety_split.yaml', 'sim_safety_split.yaml', 'true', False), + ('tight_safety_full', 'safety_full', 'true', False), + ('sim_safety_split.yaml', 'safety_split.yaml', 'true', True), + ('tight_safety_full.yaml', 'safety_split.yaml', 'true', True), + ('tight_safety_full.yaml', 'safety_split.yaml', 'false', False), +]) +def test_control_refuses_a_mismatched_pair(safety_config, frame_task_config, use_frame_task, + refused): + control = _load('h12_control.launch.py') + context = _context(control.generate_launch_description(), sim='true', + safety_config=safety_config, frame_task_config=frame_task_config, + use_frame_task=use_frame_task) + if refused: + with pytest.raises(RuntimeError, match='^h12_control: not starting: safety config '): + control.check_pairing(context) + else: + assert control.check_pairing(context) == [] + + +def _passed_to_core(launch_file): + """Return the launch arguments launch_file passes to g1_core, with its defaults.""" + description = _load(launch_file).generate_launch_description() + context = _context(description) + (include,) = [e for e in description.entities if isinstance(e, IncludeLaunchDescription)] + return {_text(context, key): _text(context, value) for key, value in include.launch_arguments} + + +def _core_nodes(**args): + """Return {node name: (package, executable, parameters)} that g1_core starts with args.""" + core = _load('g1_core.launch.py') + context = _context(core.generate_launch_description(), **args) + nodes = {} + + def add(action): + if isinstance(action, TimerAction): + for inner in action.actions: + add(inner) + elif isinstance(action, Node): + parameters = {} + for params in evaluate_parameters(context, action._Node__parameters): + assert isinstance(params, dict), params + parameters.update(params) + nodes[action._Node__node_name] = (action.node_package, action.node_executable, + parameters) + + for action in core._setup(context): + add(action) + return nodes + + +@pytest.mark.parametrize('launch_file, sim, safety_config, controller_config', [ + ('g1_real_robot.launch.py', False, 'split', 'g1_split'), + ('g1_sim.launch.py', True, 'sim_split', 'sim_split'), +]) +def test_core_runs_golem_safety_and_golem_upper_ik(launch_file, sim, safety_config, + controller_config): + nodes = _core_nodes(**_passed_to_core(launch_file)) + sim_time = {'use_sim_time': sim} + assert nodes['safety_node'] == ('golem_safety', 'safety_node', { + 'robot': 'g1', 'config': safety_config, 'sim': sim, 'use_estop': False, **sim_time}) + assert nodes['frame_task_server'] == ('golem_upper_ik', 'frame_task_server', { + 'robot': 'g1', 'config': controller_config, **sim_time}) + assert nodes['joint_state_publisher'] == ('golem_upper_ik', 'joint_state_publisher', { + 'robot': 'g1', **sim_time}) diff --git a/core_ws/src/h12_skills/test/test_copyright.py b/core_ws/src/golem_bringup/test/test_copyright.py similarity index 100% rename from core_ws/src/h12_skills/test/test_copyright.py rename to core_ws/src/golem_bringup/test/test_copyright.py diff --git a/core_ws/src/h12_skills/test/test_flake8.py b/core_ws/src/golem_bringup/test/test_flake8.py similarity index 100% rename from core_ws/src/h12_skills/test/test_flake8.py rename to core_ws/src/golem_bringup/test/test_flake8.py diff --git a/core_ws/src/golem_bringup/test/test_g1_launch.py b/core_ws/src/golem_bringup/test/test_g1_launch.py new file mode 100644 index 0000000..efdd3ae --- /dev/null +++ b/core_ws/src/golem_bringup/test/test_g1_launch.py @@ -0,0 +1,222 @@ +""" +The G1's bringups: HOMIE's walking node, and the configs they refuse. + +g1_core always starts the walking node (locomotion_node) in the sim, and on the +real robot only with start_position_verified:=true, as the H1-2's leg node is +started. It runs the split-mode configs and refuses sim/real mixes, and the +real and sim launches pass their settings on. Needs the workspace built and +sourced (golem_bringup, golem_ros2_models and golem_upper_ik installed). +""" + +import importlib.util +from pathlib import Path +import re +from typing import NamedTuple + +import pytest +from launch import LaunchContext +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, TimerAction +from launch.utilities import normalize_to_list_of_substitutions, perform_substitutions +from launch_ros.actions import Node +from launch_ros.utilities import evaluate_parameters + +LAUNCH_DIR = Path(__file__).resolve().parents[1] / 'launch' / 'g1' + + +def _load(name): + spec = importlib.util.spec_from_file_location(name.split('.')[0], LAUNCH_DIR / name) + module = importlib.util.module_from_spec(spec) + spec.loader.exec_module(module) + return module + + +core = _load('g1_core.launch.py') + + +def _context(description, **args): + """Return a LaunchContext with the description's declared defaults, overridden by args.""" + context = LaunchContext() + for entity in description.entities: + if isinstance(entity, DeclareLaunchArgument): + context.launch_configurations[entity.name] = perform_substitutions( + context, entity.default_value) + context.launch_configurations.update(args) + return context + + +class Started(NamedTuple): + """A node g1_core starts: its command-line arguments, start delay and ROS parameters.""" + + arguments: list + delay: float + parameters: dict + + +def _core_nodes(**args): + """Return {executable: Started} of the nodes g1_core starts with args.""" + nodes = {} + + def add(action, delay): + if isinstance(action, TimerAction): + for inner in action.actions: + add(inner, delay + float(action.period)) + elif isinstance(action, Node): + context = LaunchContext() + arguments = [] + for part in action.cmd[1:]: + text = perform_substitutions(context, part) + if text == '--ros-args': + break + arguments.append(text) + parameters = {} + for params in evaluate_parameters(context, action._Node__parameters): + parameters.update(params) + nodes[action.node_executable] = Started(arguments, delay, parameters) + + for action in core._setup(_context(core.generate_launch_description(), **args)): + add(action, 0.0) + return nodes + + +def _passed_to_core(launch_file, **args): + """Return the launch arguments launch_file passes to g1_core, given its own args.""" + description = _load(launch_file).generate_launch_description() + context = _context(description, **args) + (include,) = [e for e in description.entities if isinstance(e, IncludeLaunchDescription)] + + def text(value): + return perform_substitutions(context, normalize_to_list_of_substitutions(value)) + return {text(key): text(value) for key, value in include.launch_arguments} + + +def _configs(nodes): + return {name: nodes[name].parameters['config'] + for name in ('safety_node', 'frame_task_server') if name in nodes} + + +def _declared(launch_file): + description = _load(launch_file).generate_launch_description() + return {e.name for e in description.entities if isinstance(e, DeclareLaunchArgument)} + + +def _refused(message, **args): + """Assert that g1_core refuses to start with args, with message.""" + with pytest.raises(RuntimeError, match='g1_core: not starting: ' + re.escape(message)): + core._setup(_context(core.generate_launch_description(), **args)) + + +def test_use_locomotion_not_declared(): + for launch_file in ('g1_sim.launch.py', 'g1_real_robot.launch.py', 'g1_core.launch.py'): + assert 'use_locomotion' not in _declared(launch_file), launch_file + for launch_file in ('g1_sim.launch.py', 'g1_real_robot.launch.py'): + assert 'use_locomotion' not in _passed_to_core(launch_file), launch_file + + +def test_sim_always_starts_locomotion(): + # the sim needs no interlock: start_position_verified changes nothing there + for args in (dict(), dict(start_position_verified='false'), + dict(start_position_verified='true')): + nodes = _core_nodes(sim='true', **args) + assert _configs(nodes) == {'safety_node': 'sim_split', 'frame_task_server': 'sim_split'} + assert nodes['locomotion_node'].arguments == ['--config', 'sim.yaml'] + + passed = _passed_to_core('g1_sim.launch.py') + assert passed['sim'] == 'true' + nodes = _core_nodes(**passed) + assert _configs(nodes) == {'safety_node': 'sim_split', 'frame_task_server': 'sim_split'} + assert nodes['locomotion_node'].arguments == ['--config', 'sim.yaml'] + + +def test_real_starts_locomotion_only_when_verified(): + # without the operator's word the legs stay on golem_safety's zero-gain stream, + # and the arms and the safety layer start + for args in (dict(), dict(start_position_verified='false'), + dict(start_position_verified='no')): + nodes = _core_nodes(**args) + assert 'locomotion_node' not in nodes + assert {'robot_state_publisher', 'joint_state_publisher'} <= set(nodes) + assert _configs(nodes) == {'safety_node': 'split', 'frame_task_server': 'g1_split'} + nodes = _core_nodes(start_position_verified='true') + assert _configs(nodes) == {'safety_node': 'split', 'frame_task_server': 'g1_split'} + assert nodes['locomotion_node'].arguments == ['--config', 'g1.yaml'] + + # the real bringup passes the interlock on, false by default + passed = _passed_to_core('g1_real_robot.launch.py') + assert (passed['sim'], passed['start_position_verified']) == ('false', 'false') + nodes = _core_nodes(**passed) + assert 'locomotion_node' not in nodes + assert _configs(nodes) == {'safety_node': 'split', 'frame_task_server': 'g1_split'} + passed = _passed_to_core('g1_real_robot.launch.py', start_position_verified='true') + assert passed['start_position_verified'] == 'true' + assert _core_nodes(**passed)['locomotion_node'].arguments == ['--config', 'g1.yaml'] + + +def test_locomotion_starts_after_the_controller(): + nodes = _core_nodes(start_position_verified='true') + assert nodes['frame_task_server'].delay == 2.0 + assert nodes['locomotion_node'].delay == nodes['frame_task_server'].delay + 2.0 + nodes = _core_nodes(sim='true', controller_delay='1.0', locomotion_delay='1.5') + assert nodes['locomotion_node'].delay == 2.5 + + +def test_controller_config_follows_the_safety_config(): + for args, safety, controller in [ + (dict(), 'split', 'g1_split'), + (dict(safety_config='split'), 'split', 'g1_split'), + (dict(sim='true'), 'sim_split', 'sim_split'), + (dict(sim='true', safety_config='sim_split'), 'sim_split', 'sim_split'), + ]: + assert _configs(_core_nodes(**args)) == {'safety_node': safety, + 'frame_task_server': controller} + + +def test_custom_controller_config_is_not_refused(): + # a controller config outside the table is not matched to the safety config + # (g1_core's "Not checked"), and the walking node starts beside it + nodes = _core_nodes(controller_config='custom', start_position_verified='true') + assert _configs(nodes) == {'safety_node': 'split', 'frame_task_server': 'custom'} + assert 'locomotion_node' in nodes + nodes = _core_nodes(sim='true', controller_config='sim_custom') + assert _configs(nodes) == {'safety_node': 'sim_split', 'frame_task_server': 'sim_custom'} + assert 'locomotion_node' in nodes + + +@pytest.mark.parametrize('args, message', [ + (dict(safety_config='sim_split'), 'safety config "sim_split" is a sim config'), + (dict(controller_config='sim_split'), 'controller config "sim_split" is a sim config'), + (dict(controller_config='sim_fast'), 'controller config "sim_fast" is a sim config'), + # the walking node's sim config too, whether or not the legs are verified + (dict(locomotion_config='sim'), 'locomotion_config:=sim is for the sim; use g1'), + (dict(locomotion_config='sim', start_position_verified='true'), + 'locomotion_config:=sim is for the sim; use g1'), +]) +def test_real_refuses_sim_configs(args, message): + _refused(message, **args) + # through the real bringup, for the arguments it declares + declared = _declared('g1_real_robot.launch.py') + if set(args) <= declared: + passed = _passed_to_core('g1_real_robot.launch.py', **args) + assert passed['sim'] == 'false' + with pytest.raises(RuntimeError, match=re.escape(message)): + _core_nodes(**passed) + + +@pytest.mark.parametrize('args, message', [ + (dict(safety_config='split'), 'safety config "split" is for the real robot'), + (dict(controller_config='g1_split'), 'controller config "g1_split" is for the real robot'), + (dict(locomotion_config='g1'), 'locomotion_config:=g1 is for the real robot; use sim'), +]) +def test_sim_refuses_real_configs(args, message): + _refused(message, sim='true', **args) + declared = _declared('g1_sim.launch.py') + if set(args) <= declared: + passed = _passed_to_core('g1_sim.launch.py', **args) + assert passed['sim'] == 'true' + with pytest.raises(RuntimeError, match=re.escape(message)): + _core_nodes(**passed) + + +def test_core_refuses_a_safety_config_without_the_upper_body_channel(): + for sim in ('false', 'true'): + _refused('safety config "full" must relay the upper-body channel', sim=sim, + safety_config='full', start_position_verified='true') diff --git a/core_ws/src/golem_bringup/test/test_launch_layout.py b/core_ws/src/golem_bringup/test/test_launch_layout.py new file mode 100644 index 0000000..336e484 --- /dev/null +++ b/core_ws/src/golem_bringup/test/test_launch_layout.py @@ -0,0 +1,63 @@ +""" +Where golem_bringup's launch, config and rviz files live, and how they install. + +`ros2 launch golem_bringup ` finds a file by name anywhere under the +package's share directory and refuses a name it finds twice, so a launch file +name must be unique across launch/h12/ and launch/g1/, and setup.py must install +each file once, at its own relative directory under share/golem_bringup. +""" + +from collections import Counter +from pathlib import Path +import runpy + +import setuptools + +PACKAGE = Path(__file__).resolve().parents[1] + +TOP_LEVEL = { + 'h12': ('h12_sim', 'h12_sim_mac', 'h12_real_robot', 'h12_real_desktop', + 'h12_safety_grippers', 'lmg_sim', 'lmg_real'), + 'g1': ('g1_sim', 'g1_real_robot'), +} + +INSTALLED = ('launch/**/*.launch.py', 'config/**/*.yaml', 'rviz/**/*.rviz') + + +def _files(pattern): + return sorted(path.relative_to(PACKAGE) for path in PACKAGE.glob(pattern)) + + +def _data_files(monkeypatch): + """Return setup.py's data_files as [(destination, [source relative to PACKAGE])].""" + captured = {} + monkeypatch.setattr(setuptools, 'setup', lambda **kwargs: captured.update(kwargs)) + monkeypatch.chdir(PACKAGE) + runpy.run_path(str(PACKAGE / 'setup.py')) + return [(Path(destination), [Path(source) for source in sources]) + for destination, sources in captured['data_files']] + + +def test_launch_names_unique(): + launch_files = _files('launch/**/*.launch.py') + assert launch_files + names = Counter(path.name for path in launch_files) + assert [name for name, count in names.items() if count > 1] == [] + + +def test_top_level_files_exist(): + for robot, names in TOP_LEVEL.items(): + for name in names: + path = PACKAGE / 'launch' / robot / f'{name}.launch.py' + assert path.is_file(), f'missing {path.relative_to(PACKAGE)}' + + +def test_setup_installs_every_launch_file(monkeypatch): + data_files = _data_files(monkeypatch) + for pattern in INSTALLED: + files = _files(pattern) + assert files, pattern + for path in files: + destinations = [destination for destination, sources in data_files + if path in sources] + assert destinations == [Path('share', 'golem_bringup', path.parent)], path diff --git a/core_ws/src/golem_bringup/test/test_nav_launch.py b/core_ws/src/golem_bringup/test/test_nav_launch.py new file mode 100644 index 0000000..5452424 --- /dev/null +++ b/core_ws/src/golem_bringup/test/test_nav_launch.py @@ -0,0 +1,201 @@ +""" +use_nav and use_skills in the G1 bringup. + +g1_core includes golem_slam's G1 launch, on by default, with the sim's or the +real robot's arguments, beside the walking node the sim always starts; on the +real robot also the MID-360 driver. The real and sim launches pass use_nav on. +use_skills starts golem_skills on the G1 bringup. Needs the workspace built and +sourced (golem_bringup, golem_ros2_models, golem_slam and golem_upper_ik installed). +""" + +import os +import pathlib +import re + +import pytest +from ament_index_python.packages import get_package_share_directory +from launch import LaunchContext +from launch.actions import (DeclareLaunchArgument, GroupAction, IncludeLaunchDescription, + OpaqueFunction) +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.utilities import normalize_to_list_of_substitutions, perform_substitutions +from launch_ros.actions import Node +from launch_ros.utilities import evaluate_parameters + +pytest.importorskip('golem_slam') + +from test_g1_launch import ( # noqa: E402 + _context, _core_nodes, _load, _passed_to_core, core) + +NAV_LAUNCH = os.path.join(get_package_share_directory('golem_slam'), 'launch', + 'g1_navigation.launch.py') +SIM = dict(sim='true', use_sim_time='true') +REAL_VERIFIED = dict(safety_config='split', start_position_verified='true') + + +def _includes(**args): + """Return the IncludeLaunchDescription actions g1_core starts with args.""" + actions = core._setup(_context(core.generate_launch_description(), **args)) + return [a for a in actions if isinstance(a, IncludeLaunchDescription)] + + +def _location(include): + include.launch_description_source.get_launch_description(LaunchContext()) + return include.launch_description_source.location + + +def _nav_include(**args): + """Return (path, {argument: value}) of golem_slam's launch file g1_core includes with args.""" + (include,) = [i for i in _includes(**args) if _location(i) == NAV_LAUNCH] + context = LaunchContext() + include.launch_description_source.get_launch_description(context) # resolves its location + path = include.launch_description_source.location + + def text(value): + return perform_substitutions(context, normalize_to_list_of_substitutions(value)) + return path, {text(key): text(value) for key, value in include.launch_arguments} + + +def _nav_nodes(path, arguments): + """ + Return the nodes the included launch file starts with arguments. + + The result is {node name: (Node, {parameter: value} of its dict parameters, its + parameter files)}. The launch file must declare every argument. + """ + description = PythonLaunchDescriptionSource(path).get_launch_description(LaunchContext()) + declared = {e.name for e in description.entities if isinstance(e, DeclareLaunchArgument)} + assert set(arguments) <= declared, f'not declared by {path}: {set(arguments) - declared}' + context = _context(description, **arguments) + (setup,) = [e for e in description.entities if isinstance(e, OpaqueFunction)] + nodes = {} + for action in setup.execute(context): + if isinstance(action, (IncludeLaunchDescription, GroupAction)): + continue # the shared pipeline includes Nav2 navigation_launch.py + assert isinstance(action, Node) + values, files = {}, [] + for params in evaluate_parameters(context, action._Node__parameters): + if isinstance(params, dict): + values.update(params) + elif isinstance(params, pathlib.Path): + files.append(str(params)) + nodes[action._Node__node_name] = (action, values, files) + return nodes + + +def test_nav_is_on_by_default(): + # the core, the sim bringup and the real bringup start navigation unless told not to + assert [_location(i) for i in _includes(**SIM)] == [NAV_LAUNCH] + for launch_file in ('g1_sim.launch.py', 'g1_real_robot.launch.py'): + passed = _passed_to_core(launch_file) + assert passed['use_nav'] == 'true', launch_file + assert _includes(**dict(passed, use_nav='false')) == [] + assert _passed_to_core('g1_sim.launch.py')['odom_source'] == 'fastlio' + + +def test_real_bringup_starts_the_lidar_driver_and_the_real_fastlio_config(): + passed = _passed_to_core('g1_real_robot.launch.py', **REAL_VERIFIED) + includes = _includes(**passed) + drivers = os.path.join(get_package_share_directory('golem_bringup'), 'launch', 'g1', + 'g1_real_drivers.launch.py') + assert sorted(_location(i) for i in includes) == sorted([drivers, NAV_LAUNCH]) + path, arguments = _nav_include(**passed) + assert arguments == {'odom_source': 'fastlio', 'sim': 'false', 'use_sim_time': 'false', + 'fast_lio_config_file': 'g1_fastlio.yaml'} + nodes = _nav_nodes(path, arguments) + _, values, files = nodes['fast_lio'] + assert files == [os.path.join(os.path.dirname(os.path.dirname(NAV_LAUNCH)), 'config', + 'g1_fastlio.yaml')] + assert values.get('use_sim_time') is False + # the driver: CustomMsg at FAST_LIO's 10 Hz, on the G1's config + description = PythonLaunchDescriptionSource(drivers).get_launch_description(LaunchContext()) + (driver,) = [e for e in description.entities if isinstance(e, Node)] + context = _context(description) + (params,) = evaluate_parameters(context, driver._Node__parameters) + assert (params['xfer_format'], params['publish_freq'], params['frame_id']) == ( + 1, 10.0, 'livox_frame') + assert params['user_config_path'].endswith('golem_slam/config/g1_MID360_config.json') + # the sim publishes its own lidar + sim = _passed_to_core('g1_sim.launch.py') + assert [_location(i) for i in _includes(**sim)] == [NAV_LAUNCH] + + +@pytest.mark.parametrize('odom_source', ['fastlio']) +def test_sim_bringup_includes_navigation(odom_source): + args = dict(use_nav='true') + if odom_source != 'fastlio': + args['odom_source'] = odom_source + passed = _passed_to_core('g1_sim.launch.py', **args) + assert (passed['use_nav'], passed['odom_source']) == ('true', odom_source) + # the walking node starts; navigation comes with the core, on sim time + assert 'locomotion_node' in _core_nodes(**passed) + path, arguments = _nav_include(**passed) + assert path == NAV_LAUNCH + assert arguments == {'odom_source': odom_source, 'sim': 'true', 'use_sim_time': 'true', + 'fast_lio_config_file': 'g1_fastlio_sim.yaml'} + + nodes = _nav_nodes(path, arguments) + assert ('fast_lio' in nodes) == (odom_source == 'fastlio') + if odom_source == 'fastlio': + # the sim's FAST_LIO config: the raw sensor frame, every point kept + _, _, files = nodes['fast_lio'] + assert files == [os.path.join(os.path.dirname(os.path.dirname(NAV_LAUNCH)), 'config', + 'g1_fastlio_sim.yaml')] + for name, (_, values, _) in nodes.items(): + if name.endswith('_tf'): + continue # static_transform_publisher: arguments only + # The shared pipeline consistently uses simulation time. + assert values.get('use_sim_time') is True, name + + +def test_use_skills_is_declared_and_starts_golem_skills(): + # an argument of the sim bringup and of g1_core, off by default + for launch_file in ('g1_sim.launch.py', 'g1_core.launch.py'): + description = _load(launch_file).generate_launch_description() + declared = {e.name: e for e in description.entities + if isinstance(e, DeclareLaunchArgument)} + assert 'use_skills' in declared, f'{launch_file} does not declare it' + default = perform_substitutions(LaunchContext(), + declared['use_skills'].default_value) + assert default == 'false', launch_file + assert _passed_to_core('g1_sim.launch.py')['use_skills'] == 'false' + + # the sim bringup hands it to g1_core, which starts golem_skills only when asked + for value in ('true', 'false'): + passed = _passed_to_core('g1_sim.launch.py', use_nav='true', use_skills=value) + assert passed['use_skills'] == value + assert ('skills' in _core_nodes(**passed)) == (value == 'true') + + +def test_core_passes_use_sim_time_on(): + # g1_core's use_sim_time reaches golem_slam as it reaches the core's nodes + _, arguments = _nav_include(**dict(SIM, use_nav='true', use_sim_time='false')) + assert arguments['use_sim_time'] == 'false' + + +def test_sim_nav_without_use_locomotion(): + # the sim always starts the walking node, which Nav2 drives through /cmd_vel + passed = _passed_to_core('g1_sim.launch.py', use_nav='true') + assert 'use_locomotion' not in passed + nodes = _core_nodes(**passed) + assert nodes['locomotion_node'].arguments == ['--config', 'sim.yaml'] + path, arguments = _nav_include(**passed) + assert path == NAV_LAUNCH + assert arguments['sim'] == 'true' + + +@pytest.mark.parametrize('args, message', [ + (dict(SIM, use_nav='true', odom_source='sim_gt'), 'golem_slam requires FAST-LIO'), + (dict(SIM, use_nav='yes', odom_source='wheel'), + "golem_slam requires FAST-LIO"), + (dict(REAL_VERIFIED, use_nav='true', odom_source='sim_gt'), 'golem_slam requires FAST-LIO'), +]) +def test_core_refuses_nav(args, message): + with pytest.raises(RuntimeError, match='g1_core: not starting: ' + re.escape(message)): + core._setup(_context(core.generate_launch_description(), **args)) + + +def test_real_nav_with_the_legs(): + passed = _passed_to_core('g1_real_robot.launch.py', **REAL_VERIFIED) + assert (passed['sim'], passed['use_nav']) == ('false', 'true') + assert 'locomotion_node' in _core_nodes(**passed) diff --git a/core_ws/src/h12_skills/test/test_pep257.py b/core_ws/src/golem_bringup/test/test_pep257.py similarity index 100% rename from core_ws/src/h12_skills/test/test_pep257.py rename to core_ws/src/golem_bringup/test/test_pep257.py diff --git a/core_ws/src/golem_bringup/test/test_real_lidar_config.py b/core_ws/src/golem_bringup/test/test_real_lidar_config.py new file mode 100644 index 0000000..ad4b8be --- /dev/null +++ b/core_ws/src/golem_bringup/test/test_real_lidar_config.py @@ -0,0 +1,67 @@ +""" +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 golem_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', 'h12') +SLAM_CONFIG = os.path.join(os.path.dirname(PKG), 'golem_slam', '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(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) + 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(SLAM_CONFIG, 'mid360.yaml')) as f: + params = yaml.safe_load(f)['/**']['ros__parameters'] + assert params['common']['imu_topic'] == '/livox/imu' + # h12_navigation.launch.py sets the extrinsic from the URDF; a copy here would drift + assert 'extrinsic_T' not in params['mapping'] + assert 'extrinsic_R' not in params['mapping'] + + +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(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(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 golem_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(): + path = os.path.join(PKG, 'launch', 'h12', 'h12_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/golem_ros2_models/.gitignore b/core_ws/src/golem_ros2_models/.gitignore new file mode 100644 index 0000000..1a26ed4 --- /dev/null +++ b/core_ws/src/golem_ros2_models/.gitignore @@ -0,0 +1 @@ +_install_staging/ diff --git a/core_ws/src/golem_ros2_models/README.md b/core_ws/src/golem_ros2_models/README.md new file mode 100644 index 0000000..d6333d0 --- /dev/null +++ b/core_ws/src/golem_ros2_models/README.md @@ -0,0 +1,86 @@ +# golem_ros2_models + +The robot models of the Unitree H1-2 (with both Magpie grippers) and the G1 (29 DoF, with both +Dex3-1 hands), installed for ROS and Pinocchio. No nodes, no joint table: each robot's joint +table is `golem_safety`'s robot profile. + +## Where the files come from + +`setup.py` reads the repo's `CL_Assets` submodule at `../../../CL_Assets`, so the package +builds only from `/core_ws/src/golem_ros2_models` with `CL_Assets` checked out and its +meshes pulled from Git-LFS. Every x86 `ros` container mounts `CL_Assets` at +`/home/code/CL_Assets`. The build stops with a message when: + +- `CL_Assets` is missing, or a file the package installs is; +- a file it installs is still a Git-LFS pointer (run `git lfs pull` in `CL_Assets`); +- a URDF names a mesh in a form the rewrite does not cover: each rewrite must change every + mesh reference in its file. The G1's sphere model must have none. + +Generated files are staged in `_install_staging/` (gitignored), cleared on every build. + +## What it installs + +Under `share/golem_ros2_models/`: + +| Path | What | From | +|---|---|---| +| `h12/h1_2_magpie.urdf` | the H1-2 for Pinocchio; mesh paths relative to `h12/` | `CL_Assets/ros_assets/`, `../meshes/h1_2/` → `meshes/`, `../meshes/magpie/` → `meshes/magpie/` | +| `h12/h1_2_magpie_ros.urdf` | its ROS copy, for `robot_state_publisher` and RViz: `package://golem_ros2_models/h12/meshes/...` URIs, and the 12 Magpie finger hinges fixed (nothing publishes their joint states, so as moving joints they would have no TF) | generated from `h1_2_magpie.urdf` with `rigidify_gripper` and `assert_well_formed` from `CL_Assets/ros_assets/gen_ros_urdf.py` | +| `h12/h1_2_magpie_sphere.urdf` | the H1-2's sphere-packed self-collision model | `CL_Assets/ros_assets/`, mesh paths rewritten as above | +| `h12/h1_2_magpie_sphere_collision.srdf` | the collision pairs it leaves out | `CL_Assets/ros_assets/`, verbatim | +| `h12/meshes/*.STL`, `h12/meshes/magpie/*.stl` | the body and gripper meshes | `CL_Assets/meshes/h1_2/`, `CL_Assets/meshes/magpie/` | +| `h12/LICENSE.unitree_ros` | Unitree's BSD 3-Clause licence for the H1-2 model | this package's `licenses/` | +| `g1/g1_29dof_with_hand_rev_1_0.urdf` | the G1 for Pinocchio; mesh paths relative to `g1/` | `CL_Assets/g1/g1_description/`, verbatim | +| `g1/g1_29dof_with_hand_rev_1_0_ros.urdf` | its ROS copy: `package://golem_ros2_models/g1/meshes/...` URIs, nothing else changed (the Dex3-1 joints stay articulated) | generated | +| `g1/g1_sphere.urdf` | the G1's sphere-packed self-collision model (24 spheres, no meshes) | `CL_Assets/g1/`, verbatim | +| `g1/g1_sphere_collision.srdf` | the collision pairs it leaves out | `CL_Assets/g1/`, verbatim | +| `g1/meshes/*.STL` | the G1 and Dex3-1 meshes | `CL_Assets/g1/g1_description/meshes/` | +| `g1/LICENSE.unitree_ros` | Unitree's BSD 3-Clause licence for the G1 model | `CL_Assets/g1/` | +| `launch/display.launch.py`, `rviz/model.rviz` | the viewer | this package | + +`CL_Assets/README.md` says where each robot's files come from. + +### The G1's sphere model covers no Dex3-1 finger + +The lab packed `g1_sphere.urdf` on the body-only G1 with rubber hands: each hand is two +spheres on a `_rubber_hand` link, fixed to `_wrist_yaw_link` where the Dex3-1 palm +mounts. A self-collision check with it covers the palms but not the fingers. Its SRDF, unlike +the H1-2's, keeps the pelvis and hip-pitch spheres active against the torso, arms and hands. + +## Python API + +`golem_ros2_models.paths` (needs the package built and sourced): + +- `ROBOTS = ('h12', 'g1')`. +- `model_dir(robot)`: `share/golem_ros2_models/`. +- `urdf_path(robot, ros=False)`: the Pinocchio URDF, or with `ros=True` its ROS copy. +- `collision_paths(robot)`: `(sphere URDF, SRDF)`. +- `model_paths(robot)`: `{'urdf_path', 'urdf_ros_path', 'collision'}`, as `golem_upper_ik`'s + robot profiles name them. + +Any other robot name raises `ValueError`. + +## Viewer + +```bash +ros2 launch golem_ros2_models display.launch.py # the H1-2 +ros2 launch golem_ros2_models display.launch.py robot:=g1 # the G1 +``` + +It starts `robot_state_publisher` on the robot's ROS copy, `joint_state_publisher_gui` and +`rviz2`. + +## Tests + +```bash +colcon test --packages-select golem_ros2_models --event-handlers console_direct+ +colcon test-result --verbose --test-result-base build/golem_ros2_models +``` + +- `test_models.py`: the installed files of both robots: the paths exist; every mesh reference + resolves to a real mesh, not an LFS pointer; each ROS copy has its Pinocchio copy's links + and joints; Pinocchio loads each model with its meshes and each sphere model with its SRDF; + each sphere model's body joints match the model's. +- `test_build.py`: `setup.py`'s refusals, on a copy of the package beside a copy of + `CL_Assets`. +- `test_display_launch.py`: the viewer's nodes and its `robot_description` for each robot. diff --git a/core_ws/src/golem_ros2_models/golem_ros2_models/__init__.py b/core_ws/src/golem_ros2_models/golem_ros2_models/__init__.py new file mode 100644 index 0000000..e312886 --- /dev/null +++ b/core_ws/src/golem_ros2_models/golem_ros2_models/__init__.py @@ -0,0 +1 @@ +'''The H1-2's and the G1's robot models, installed from CL_Assets; see paths''' diff --git a/core_ws/src/golem_ros2_models/golem_ros2_models/paths.py b/core_ws/src/golem_ros2_models/golem_ros2_models/paths.py new file mode 100644 index 0000000..3f88672 --- /dev/null +++ b/core_ws/src/golem_ros2_models/golem_ros2_models/paths.py @@ -0,0 +1,61 @@ +''' +Installed locations of each robot's model files (needs the package built and sourced). + +Each robot's directory, share/golem_ros2_models//, holds: +- the Pinocchio URDF, whose relative mesh paths resolve against that directory; +- its ROS copy (*_ros.urdf), with package://golem_ros2_models//meshes/ URIs, for + robot_state_publisher and RViz; +- the self-collision model: a sphere-packed URDF and the SRDF of the pairs it leaves out. +''' +import os + +ROBOTS = ('h12', 'g1') + +# robot -> (Pinocchio URDF, sphere URDF, SRDF); the ROS copy is the URDF's name + _ros +_FILES = { + # the H1-2 with both Magpie grippers + 'h12': ('h1_2_magpie.urdf', 'h1_2_magpie_sphere.urdf', + 'h1_2_magpie_sphere_collision.srdf'), + # the G1 (29 DoF) with both Dex3-1 hands. Its sphere model was packed on the rubber-hand + # G1: two spheres per hand at the Dex3-1 palm, none on the fingers + 'g1': ('g1_29dof_with_hand_rev_1_0.urdf', 'g1_sphere.urdf', 'g1_sphere_collision.srdf'), +} + + +def _files(robot): + if robot not in _FILES: + raise ValueError(f'unknown robot {robot!r}; the robots are {", ".join(ROBOTS)}') + return _FILES[robot] + + +def model_dir(robot): + '''share/golem_ros2_models/, the robot's installed model files''' + _files(robot) + from ament_index_python.packages import get_package_share_directory + return os.path.join(get_package_share_directory('golem_ros2_models'), robot) + + +def urdf_path(robot, ros=False): + '''The robot's URDF: for Pinocchio, or with ros=True its package:// copy''' + name = _files(robot)[0] + if ros: + name = name[:-len('.urdf')] + '_ros.urdf' + return os.path.join(model_dir(robot), name) + + +def collision_paths(robot): + '''(sphere URDF, SRDF): the robot's self-collision model''' + _, sphere, srdf = _files(robot) + return (os.path.join(model_dir(robot), sphere), os.path.join(model_dir(robot), srdf)) + + +def model_paths(robot): + ''' + {'urdf_path', 'urdf_ros_path', 'collision'}: the Pinocchio URDF, its ROS copy and + (sphere URDF, SRDF), as golem_upper_ik's robot profiles name them + ''' + return { + 'urdf_path': urdf_path(robot), + 'urdf_ros_path': urdf_path(robot, ros=True), + 'collision': collision_paths(robot), + } diff --git a/core_ws/src/golem_ros2_models/launch/display.launch.py b/core_ws/src/golem_ros2_models/launch/display.launch.py new file mode 100644 index 0000000..e4f2c56 --- /dev/null +++ b/core_ws/src/golem_ros2_models/launch/display.launch.py @@ -0,0 +1,51 @@ +''' +Show one robot's model in RViz with joint sliders. + + ros2 launch golem_ros2_models display.launch.py # the H1-2 + ros2 launch golem_ros2_models display.launch.py robot:=g1 # the G1 +''' +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + +from golem_ros2_models.paths import ROBOTS, urdf_path + + +def launch_setup(context): + robot = LaunchConfiguration('robot').perform(context) + with open(urdf_path(robot, ros=True)) as f: + robot_description = f.read() + rviz_config = os.path.join(get_package_share_directory('golem_ros2_models'), 'rviz', + 'model.rviz') + return [ + Node( + package='robot_state_publisher', + executable='robot_state_publisher', + parameters=[{'robot_description': robot_description}], + output='screen', + ), + Node( + package='joint_state_publisher_gui', + executable='joint_state_publisher_gui', + output='screen', + ), + Node( + package='rviz2', + executable='rviz2', + arguments=['-d', rviz_config], + output='screen', + ), + ] + + +def generate_launch_description(): + return LaunchDescription([ + DeclareLaunchArgument('robot', default_value=ROBOTS[0], choices=list(ROBOTS), + description='h12: the H1-2 with Magpie grippers; ' + 'g1: the G1 with Dex3-1 hands'), + OpaqueFunction(function=launch_setup), + ]) diff --git a/core_ws/src/golem_ros2_models/licenses/LICENSE.unitree_ros b/core_ws/src/golem_ros2_models/licenses/LICENSE.unitree_ros new file mode 100644 index 0000000..9ad8f6d --- /dev/null +++ b/core_ws/src/golem_ros2_models/licenses/LICENSE.unitree_ros @@ -0,0 +1,23 @@ +BSD 3-Clause License +Copyright (c) 2016-2023 HangZhou YuShu TECHNOLOGY CO.,LTD. ("Unitree Robotics") +All rights reserved. +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. +3. Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/core_ws/src/golem_ros2_models/package.xml b/core_ws/src/golem_ros2_models/package.xml new file mode 100644 index 0000000..56f7c1d --- /dev/null +++ b/core_ws/src/golem_ros2_models/package.xml @@ -0,0 +1,23 @@ + + + + golem_ros2_models + 0.1.0 + The H1-2's and the G1's robot models (URDFs, self-collision models, meshes) from CL_Assets + maxconway + MIT + + ament_index_python + launch + launch_ros + robot_state_publisher + joint_state_publisher_gui + rviz2 + + python3-pytest + python3-yaml + + + ament_python + + diff --git a/core_ws/src/golem_ros2_models/resource/golem_ros2_models b/core_ws/src/golem_ros2_models/resource/golem_ros2_models new file mode 100644 index 0000000..e69de29 diff --git a/core_ws/src/golem_ros2_models/rviz/model.rviz b/core_ws/src/golem_ros2_models/rviz/model.rviz new file mode 100644 index 0000000..8d573fa --- /dev/null +++ b/core_ws/src/golem_ros2_models/rviz/model.rviz @@ -0,0 +1,85 @@ +Panels: + - Class: rviz_common/Displays + Name: Displays + - Class: rviz_common/Views + Name: Views +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Name: Grid + Plane: XY + Plane Cell Count: 10 + Reference Frame: + 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 + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz_default_plugins/TF + Enabled: false + Frame Timeout: 15 + Frames: + All Enabled: true + Marker Scale: 0.3 + Name: TF + Show Arrows: false + Show Axes: true + Show Names: true + Update Interval: 0 + 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 + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 3.0 + Focal Point: + X: 0 + Y: 0 + Z: 0.1 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 0.25 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.8 + Saved: ~ +Window Geometry: + Height: 900 + Width: 1400 diff --git a/core_ws/src/golem_ros2_models/setup.cfg b/core_ws/src/golem_ros2_models/setup.cfg new file mode 100644 index 0000000..79041f0 --- /dev/null +++ b/core_ws/src/golem_ros2_models/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/golem_ros2_models +[install] +install_scripts=$base/lib/golem_ros2_models diff --git a/core_ws/src/golem_ros2_models/setup.py b/core_ws/src/golem_ros2_models/setup.py new file mode 100644 index 0000000..033619a --- /dev/null +++ b/core_ws/src/golem_ros2_models/setup.py @@ -0,0 +1,180 @@ +import importlib.util +import os +import re +import shutil +import sys +from glob import glob + +from setuptools import find_packages, setup + +package_name = 'golem_ros2_models' + +# The models come from the repo's CL_Assets submodule, at ../../../CL_Assets from this file. +# colcon may run this file from core_ws/src/golem_ros2_models or (--symlink-install) from +# core_ws/build/golem_ros2_models, the same depth, so the relative path holds for both. +# data_files sources must be relative paths (colcon's ament_python builder asserts it). +HERE = os.path.dirname(os.path.abspath(__file__)) +ASSETS_REL = os.path.join('..', '..', '..', 'CL_Assets') +ASSETS = os.path.normpath(os.path.join(HERE, ASSETS_REL)) +# generated files, rewritten from scratch on every build +STAGE = os.path.join(HERE, '_install_staging') + +LFS_POINTER = b'version https://git-lfs' +MESH_REF = re.compile(r']*?\bfilename="([^"]*)"') +PACKAGE_URI = f'package://{package_name}/' +# the 12 Magpie finger hinges, which the H1-2's ROS copy fixes +H12_HINGES = 12 + + +def fail(message): + sys.exit(f'{package_name}: {message}') + + +def asset(*parts): + '''A CL_Assets file, relative to HERE; the build stops if it is missing or an LFS pointer''' + path = os.path.join(ASSETS, *parts) + if not os.path.isfile(path): + fail(f'{os.path.join("CL_Assets", *parts)} is missing ({path})') + with open(path, 'rb') as f: + if f.read(len(LFS_POINTER)) == LFS_POINTER: + fail(f'{path} is a Git-LFS pointer, not the file: run `git lfs pull` in CL_Assets') + return os.path.relpath(path, HERE) + + +def meshes(*parts, pattern): + found = sorted(glob(os.path.join(ASSETS, *parts, pattern))) + if not found: + fail(f'no meshes match {os.path.join(ASSETS, *parts, pattern)}') + return [asset(*parts, os.path.basename(path)) for path in found] + + +def read(rel_path): + with open(os.path.join(HERE, rel_path)) as f: + return f.read() + + +def check_meshes_exist(rel_path, text): + ''' + Every mesh the CL_Assets URDF names, relative to its own directory, is installable (run + after rewrite_meshes, which refuses any other form of mesh reference) + ''' + directory = os.path.dirname(os.path.join(HERE, rel_path)) + for ref in MESH_REF.findall(text): + asset(os.path.relpath(os.path.normpath(os.path.join(directory, ref)), ASSETS)) + + +def rewrite_meshes(rel_path, text, prefixes): + ''' + Replace each mesh filename prefix by its new one; the build stops unless every mesh + reference in the file is rewritten exactly once + ''' + references = len(MESH_REF.findall(text)) + rewritten = 0 + for old, new in prefixes.items(): + rewritten += text.count(f'filename="{old}') + text = text.replace(f'filename="{old}', f'filename="{new}') + if references == 0 or rewritten != references: + fail(f'{rel_path}: rewrote {rewritten} of {references} mesh references; every mesh ' + f'path must start with one of {sorted(prefixes)}') + return text + + +def ros_header(source): + return (f'\n') + + +def stage(robot, name, text): + path = os.path.join(STAGE, robot, name) + with open(path, 'w') as f: + f.write(text) + return os.path.relpath(path, HERE) + + +def gen_ros_urdf(): + '''CL_Assets/ros_assets/gen_ros_urdf.py, the H1-2's gripper transform (stdlib only)''' + path = os.path.join(HERE, asset('ros_assets', 'gen_ros_urdf.py')) + spec = importlib.util.spec_from_file_location('gen_ros_urdf', path) + module = importlib.util.module_from_spec(spec) + spec.loader.exec_module(module) + return module + + +if not os.path.isdir(ASSETS): + fail(f'expected the CL_Assets submodule at {ASSETS}, which does not exist. Build this ' + 'package from /core_ws/src/golem_ros2_models with CL_Assets checked out ' + '(`git submodule update --init CL_Assets`).') + +shutil.rmtree(STAGE, ignore_errors=True) +for robot in ('h12', 'g1'): + os.makedirs(os.path.join(STAGE, robot)) + +# The H1-2: CL_Assets/ros_assets URDFs name meshes ../meshes/{h1_2,magpie}/; installed in +# h12/ beside meshes/ and meshes/magpie/ +H12_PINOCCHIO = {'../meshes/h1_2/': 'meshes/', '../meshes/magpie/': 'meshes/magpie/'} +H12_ROS = {'../meshes/h1_2/': f'{PACKAGE_URI}h12/meshes/', + '../meshes/magpie/': f'{PACKAGE_URI}h12/meshes/magpie/'} +h12_files = [] +for name in ('h1_2_magpie.urdf', 'h1_2_magpie_sphere.urdf'): + source = asset('ros_assets', name) + text = read(source) + h12_files.append(stage('h12', name, rewrite_meshes(source, text, H12_PINOCCHIO))) + check_meshes_exist(source, text) + if name == 'h1_2_magpie.urdf': + gen = gen_ros_urdf() + ros = gen.rigidify_gripper(rewrite_meshes(source, text, H12_ROS)) + fixed = len(re.findall(r'') + assert run_setup(package).returncode == 0 + assert not stale.exists() + + +def test_missing_cl_assets_is_refused(tree): + package, assets = tree + shutil.rmtree(assets) + assert_refused(run_setup(package), 'CL_Assets', str(assets)) + + +@pytest.mark.parametrize('missing', [ + 'ros_assets/h1_2_magpie_sphere_collision.srdf', + 'ros_assets/gen_ros_urdf.py', + 'g1/g1_sphere.urdf', + 'g1/LICENSE.unitree_ros', + 'g1/g1_description/g1_29dof_with_hand_rev_1_0.urdf', +]) +def test_missing_file_is_refused(tree, missing): + package, assets = tree + (assets / missing).unlink() + assert_refused(run_setup(package), missing) + + +@pytest.mark.parametrize('mesh', [ + 'meshes/h1_2/pelvis.STL', + 'meshes/magpie/base_bot.stl', + 'g1/g1_description/meshes/pelvis.STL', +]) +def test_missing_mesh_is_refused(tree, mesh): + package, assets = tree + (assets / mesh).unlink() + assert_refused(run_setup(package), os.path.basename(mesh)) + + +@pytest.mark.parametrize('pointer', [ + 'meshes/h1_2/pelvis.STL', + 'meshes/magpie/base_bot.stl', + 'g1/g1_description/meshes/pelvis.STL', + 'g1/g1_sphere.urdf', +]) +def test_lfs_pointer_is_refused(tree, pointer): + package, assets = tree + replace(assets / pointer, 'version https://git-lfs.github.com/spec/v1\n' + 'oid sha256:0000\nsize 1\n') + assert_refused(run_setup(package), os.path.basename(pointer), 'git lfs pull') + + +def test_unexpected_h12_mesh_path_is_refused(tree): + package, assets = tree + edit(assets / 'ros_assets/h1_2_magpie.urdf', 'filename="../meshes/h1_2/pelvis.STL"', + 'filename="../meshes/pelvis.STL"') + assert_refused(run_setup(package), 'h1_2_magpie.urdf', 'mesh') + + +def test_unexpected_g1_mesh_path_is_refused(tree): + package, assets = tree + edit(assets / 'g1/g1_description/g1_29dof_with_hand_rev_1_0.urdf', + 'filename="meshes/pelvis.STL"', 'filename="package://g1_description/pelvis.STL"') + assert_refused(run_setup(package), 'g1_29dof_with_hand_rev_1_0.urdf', 'mesh') + + +def test_mesh_in_the_g1_sphere_model_is_refused(tree): + package, assets = tree + edit(assets / 'g1/g1_sphere.urdf', ']*?\bfilename="([^"]*)"') +# The joints each robot's Pinocchio URDF has beyond its body joints: the Magpie grippers' +# finger hinges and rockers on the H1-2, the Dex3-1 fingers on the G1 +NON_BODY_JOINT = { + 'h12': re.compile(r'^(lg|rg)_'), + 'g1': re.compile(r'^(left|right)_hand_'), +} +# the Magpie finger hinges the H1-2's ROS copy fixes +H12_HINGES = {f'{g}_{s}_hinge_{i}' for g in ('lg', 'rg') for s in ('left', 'right') + for i in (1, 2, 3)} +# sha256 of CL_Assets' g1/g1_sphere.urdf, installed verbatim: the lab's sphere model with +# the optimiser-fitted torso column (CL_Assets/README.md). golem_upper_ik's G1 d_min and its +# tests' distances hold for this file; a changed sphere model must change this too +G1_SPHERE_SHA256 = '68c5e5cf93a6fcb5958f98dc88909be0632dd1165500adf6d1b4e8d64225dcf0' + + +def share_dir(): + from ament_index_python.packages import get_package_share_directory + return get_package_share_directory('golem_ros2_models') + + +def parse(path): + ''' + The URDF's root element. The H1-2's sphere URDF uses drake: elements without declaring + the namespace (urdfdom does not need it), so it is declared here when missing. + ''' + with open(path) as f: + text = f.read() + if 'drake:' in text and 'xmlns:drake' not in text: + text = re.sub(r' 1 + # Pinocchio names a primitive's kind (SPHERE, CYLINDER) where a mesh's path would be + mesh_paths = [geom.meshPath for geom in visual.geometryObjects + if geom.meshPath.lower().endswith('.stl')] + assert mesh_paths and collision.ngeoms > 0 + for path in mesh_paths: + assert os.path.isfile(path), path + + +@pytest.mark.parametrize('robot', paths.ROBOTS) +def test_pinocchio_loads_the_sphere_model_with_its_srdf(robot): + pin = pytest.importorskip('pinocchio') + urdf, srdf = paths.collision_paths(robot) + model, collision, _ = pin.buildModelsFromUrdf(urdf, package_dirs=os.path.dirname(urdf)) + collision.addAllCollisionPairs() + every_pair = len(collision.collisionPairs) + pin.removeCollisionPairs(model, collision, srdf) + assert 0 < len(collision.collisionPairs) < every_pair + + +def test_g1_sphere_model_is_the_pinned_file(): + with open(paths.collision_paths('g1')[0], 'rb') as f: + assert hashlib.sha256(f.read()).hexdigest() == G1_SPHERE_SHA256 + + +@pytest.mark.parametrize('robot', paths.ROBOTS) +def test_sphere_body_joints_match_the_model(robot): + '''The sphere model's actuated joints are the model's body joints, in order, alike''' + plain = actuated(parse(paths.urdf_path(robot))) + sphere = actuated(parse(paths.collision_paths(robot)[0])) + body = [j for j in plain if not NON_BODY_JOINT[robot].match(j.get('name'))] + assert [j.get('name') for j in sphere] == [j.get('name') for j in body] + assert len(sphere) == {'h12': 27, 'g1': 29}[robot] + for ours, theirs in zip(sphere, body): + name = ours.get('name') + assert ours.get('type') == theirs.get('type'), name + for tag, attribute, default in (('origin', 'xyz', '0 0 0'), ('origin', 'rpy', '0 0 0'), + ('axis', 'xyz', '1 0 0')): + np.testing.assert_allclose(floats(ours, tag, attribute, default), + floats(theirs, tag, attribute, default), + atol=1e-12, err_msg=f'{name} {tag} {attribute}') diff --git a/core_ws/src/golem_safety/CMakeLists.txt b/core_ws/src/golem_safety/CMakeLists.txt new file mode 100644 index 0000000..9be01db --- /dev/null +++ b/core_ws/src/golem_safety/CMakeLists.txt @@ -0,0 +1,110 @@ +cmake_minimum_required(VERSION 3.8) +project(golem_safety) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(unitree_hg REQUIRED) +find_package(ament_index_cpp REQUIRED) +find_package(yaml-cpp REQUIRED) +find_package(ZLIB REQUIRED) +find_package(Threads REQUIRED) + +# Every source except the node's main(); the node and the tests link it. +add_library(golem_safety_core + src/chunk_logger.cpp + src/config.cpp + src/json.cpp + src/lowcmd.cpp + src/robot_profile.cpp + src/safety_core.cpp) +target_include_directories(golem_safety_core PUBLIC + $) +ament_target_dependencies(golem_safety_core ament_index_cpp unitree_hg) +# zlib is only for crc32 in the npz writer; the archive is written stored. +target_link_libraries(golem_safety_core yaml-cpp ZLIB::ZLIB) + +# Prints a config's derived limits and parsed values as JSON. +add_executable(golem_safety_dump_config src/dump_config_main.cpp) +target_link_libraries(golem_safety_dump_config golem_safety_core) +install(TARGETS golem_safety_dump_config DESTINATION lib/${PROJECT_NAME}) + +# The node: ros2 run golem_safety safety_node --ros-args -p robot:=h12 -p config:=... +add_executable(safety_node src/safety_node.cpp) +ament_target_dependencies(safety_node rclcpp std_msgs unitree_hg) +target_link_libraries(safety_node golem_safety_core Threads::Threads) +install(TARGETS safety_node DESTINATION lib/${PROJECT_NAME}) + +# robots/.yaml is the robot data golem_upper_ik reads too; config// holds +# the safety configs, which a bare config name resolves against; launch/ runs the node. +install(DIRECTORY robots config launch DESTINATION share/${PROJECT_NAME}) + +# Concatenates a run's .npz chunks into one file. +install(PROGRAMS scripts/combine_chunks.py DESTINATION lib/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + find_package(ament_cmake_pytest REQUIRED) + + # The tests read the installed share directory, so they run after the + # package is installed and its install space is sourced. + ament_add_gtest(test_robot_profile test/test_robot_profile.cpp) + target_link_libraries(test_robot_profile golem_safety_core) + + ament_add_pytest_test(test_robot_data test/test_robot_data.py TIMEOUT 60) + + ament_add_gtest(test_config test/test_config.cpp) + target_link_libraries(test_config golem_safety_core) + + # Runs golem_safety_dump_config, so it needs the installed executable and configs. + ament_add_pytest_test(test_limit_parity test/test_limit_parity.py TIMEOUT 120) + + # test/fixtures/lowcmd_crc.json holds the Unitree SDK's CRCs (test/fixtures/README.md). + ament_add_gtest(test_lowcmd test/test_lowcmd.cpp) + target_link_libraries(test_lowcmd golem_safety_core) + target_compile_definitions(test_lowcmd PRIVATE + GOLEM_SAFETY_FIXTURE_DIR="${CMAKE_CURRENT_SOURCE_DIR}/test/fixtures") + + ament_add_gtest(test_chunk_logger test/test_chunk_logger.cpp) + target_link_libraries(test_chunk_logger golem_safety_core) + + ament_add_gtest(test_safety_core_relay test/test_safety_core_relay.cpp) + target_link_libraries(test_safety_core_relay golem_safety_core) + + ament_add_gtest(test_safety_core_startup test/test_safety_core_startup.cpp) + target_link_libraries(test_safety_core_startup golem_safety_core) + + # Writes the chunks test_chunk_files.py reads back with numpy; not installed. + add_executable(test_chunk_writer test/test_chunk_writer.cpp) + target_link_libraries(test_chunk_writer golem_safety_core) + # The installed combine_chunks.py is what the test runs, so it needs the install space. + ament_add_pytest_test(test_chunk_files test/test_chunk_files.py TIMEOUT 120 + ENV GOLEM_SAFETY_TEST_CHUNK_WRITER=${CMAKE_CURRENT_BINARY_DIR}/test_chunk_writer) + + ament_add_gtest(test_boot_clock test/test_boot_clock.cpp) + target_include_directories(test_boot_clock PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include) + + # Creates a node, on the package's test domain. + ament_add_gtest(test_spinner test/test_spinner.cpp ENV ROS_DOMAIN_ID=91) + target_include_directories(test_spinner PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include) + ament_target_dependencies(test_spinner rclcpp) + + ament_add_pytest_test(test_no_sdk test/test_no_sdk.py TIMEOUT 60) + + # The matching and statistics of tools/relay_latency_probe.py, on synthetic streams; no ROS. + ament_add_pytest_test(test_relay_latency_probe test/test_relay_latency_probe.py TIMEOUT 60) + + # Runs the installed safety_node against a fake robot over ROS, on the package's own + # test domain. + ament_add_pytest_test(test_node_ros test/test_node_ros.py TIMEOUT 1500 + ENV ROS_DOMAIN_ID=91) +endif() + +ament_package() diff --git a/core_ws/src/golem_safety/README.md b/core_ws/src/golem_safety/README.md new file mode 100644 index 0000000..3752f96 --- /dev/null +++ b/core_ws/src/golem_safety/README.md @@ -0,0 +1,298 @@ +# golem_safety + +The safety layer of the Unitree H1-2 and G1. Its node, `safety_node`, is the only writer of +the robots' body command topic, `/lowcmd`. It relays the controllers' commands from their +`/safety/*` inputs, clips them to the config's limits, checks every `/lowstate` against the +robot's identity and the estop limits, and latches an estop until restart. It is C++ +(`rclcpp`) and talks to the robot only through ROS topics: ROS topic `/x` is DDS topic +`rt/x`, with the same `unitree_hg` wire type, so the robots, the sims and the Unitree SDK's +publishers all see the same topics. + +The design is `docs/superpowers/specs/2026-10-27-golem-upper-ik-and-safety-design.md` +(section golem_safety). Every value and behavior that differs from the packages this one +replaces is in the change report beside it, +`docs/superpowers/specs/2026-10-27-golem-upper-ik-and-safety/change-report.md`. + +## Running it + +The bringups start the node (`golem_bringup`'s `h12_control.launch.py` and +`g1_core.launch.py`). On its own: + +```bash +ros2 launch golem_safety safety.launch.py robot:=h12 config:=sim_safety_split sim:=true +ros2 run golem_safety safety_node --ros-args -p robot:=g1 -p config:=sim_split -p sim:=true +``` + +| Parameter | Default | Meaning | +|---|---|---| +| `robot` | required | `h12` or `g1`: the profile in `robots/.yaml` | +| `config` | required | a config name in `config//` (with or without `.yaml`), or a path; a path whose directory is named after another robot with a profile (`.../g1/split.yaml` for the `h12`) is refused | +| `sim` | `false` | `true` for a simulator: the node then refuses ROS domain 0, the real robots' command bus, an unset `ROS_DOMAIN_ID` included | +| `use_estop` | `false` | `true` to read the hardware estop on the config's `estop` topics; `false` ignores them | + +The node refuses to start, with one `FATAL` line and exit code 1, when a parameter is +missing, the profile or config does not load (`safety_node refuses to start: robot , +config : `), or `sim:=true` meets domain 0 (`safety_node refuses to start: +sim:=true on ROS domain 0, ...`). It exits 1 as well when its relay loop +fails, when its executor thread (the heartbeat's) fails, after running the exit sequence, +or when its exit sequence overruns its length by more than 1 s +(`exit_hand_back_timeout`); 0 otherwise. + +## ROS interface + +| Direction | Topic | Type | QoS | When | +|---|---|---|---|---| +| sub | `/lowstate` | `unitree_hg/msg/LowState` | keep-last 1, best effort | always | +| sub | `/safety/lowcmd_in` | `unitree_hg/msg/LowCmd` | keep-last 1, best effort | `full_body_mode` | +| sub | `/safety/lowcmd_lower_in`, `/safety/lowcmd_upper_in` | `LowCmd` | keep-last 1, best effort | `split_mode` | +| sub | `/estop/triggered`, `/estop/plugged_in` | `std_msgs/Bool` | keep-last 1, best effort | `use_estop:=true` | +| pub | `/lowcmd` | `LowCmd` | reliable, keep-last 1 | always | +| pub | `/safety/heartbeat` | `std_msgs/Bool` | reliable, depth 10 | 10 Hz on the steady clock once relaying can begin: true, then false once the estop latches | + +The topic names are the config's `topics` (and `estop`) values; the table gives the +defaults, which every carried config uses. `h12_lowerbody_rl`, `lmg_deploy` and `golem_skills` +read `/safety/heartbeat` on the H1-2, and `h12_lowerbody_rl` starts pre-posing the legs on +the first beat, so a beat means relaying can begin. The node publishes none until the +engage conditions that do not depend on the inputs hold: the newest `/lowstate` reports the +profile's `mode_machine` and, with `use_estop`, a first hardware estop sample has arrived +(`SafetyCore::relay_ready`). From then on it beats true, and false once the estop latches. +An estop that latches before then (a non-finite command with no `/lowstate` yet, a first +estop sample that says triggered) makes the beats false from the latch on, never true. +Before engaging, a `/lowstate` from another robot stops the beats again, which the readers +time out on. + +## Robot profiles + +`robots/.yaml` is the robot's data, written by `tools/generate_robot_data.py` (do not +edit it by hand). `golem_upper_ik` reads the same file, so each robot's joint table is +defined once. + +| | `h12` | `g1` | +|---|---|---| +| `mode_machine` (what `/lowstate` reports and every command carries) | 6 | 5 | +| `mode_pr` | 0 | 0 | +| Body joints | 27, slots 0-26 | 29, slots 0-28 | +| Lower / upper slots | 0-11 / 12-26 (torso 12, arms 13-26) | 0-11 / 12-28 (waist 12-14, arms 15-28) | +| Modes | `full_body_mode`, `split_mode` | `split_mode`, `full_body_mode` | +| Joint limits | `tools/h12_joint_limits.py`, the H1-2's joint-limit table (shoulder roll ±0.38 rad, not the URDF's ±0.19) | `tools/g1_joint_limits.py`, Unitree's URDF values (ankles and waist roll/pitch 35 N·m there, 50 N·m in the MJCF) | + +Both robots' `LowCmd`/`LowState` motor arrays hold 35 slots. + +## Configs + +| Robot | Config | Mode | Run by | +|---|---|---|---| +| H1-2 | `relax_safety_split` | split | `h12_real_robot` (the `h12_control.launch.py` default), `lmg_sim`, `lmg_real`, `h12_safety_grippers` (their defaults) | +| H1-2 | `default_safety_split`, `tight_safety_split` | split | by name (`safety_config:=`) | +| H1-2 | `sim_safety_split` | split | `h12_sim`, the Mac bringup, `tests/stack` | +| H1-2 | `default_safety_full`, `relax_safety_full`, `tight_safety_full`, `locomotion_safety_full` | full body | by name | +| G1 | `split` | split | `g1_real_robot.launch.py` (default) | +| G1 | `sim_split` | split | `g1_sim.launch.py` (default) | +| G1 | `full` | full body | refused by `g1_core.launch.py` (it must relay the upper-body input) | + +What the robots' configs share: + +- **H1-2:** `estop_response` `zero`, `burst_then_silence`, 50 commands; + `control.state_lag_ms: 100`; `stale.lower_seconds` 0.2 and `state_seconds` 0.1; + `stale.upper_seconds` 2.0 in `default_safety_split` and `tight_safety_split`, 0 (off) in + `relax_safety_split` and `sim_safety_split`; logging off. +- **G1:** `estop_response` `zero`, `keep_writing`; `control.state_lag_ms: 0` (off: the real + G1's tick has not been measured); logging on, to `/tmp/golem_safety`. `sim_split` relaxes + the stale times and the estop limits, since the sim can run slower than real time. + +Each safety config runs with one `golem_upper_ik` controller config; the bringups refuse +another pair (`golem_upper_ik.pairing`). + +### Keys + +| Key | Default | Meaning | +|---|---|---| +| `mode` | the profile's first mode | `full_body_mode` or `split_mode`; a mode the profile lacks is refused, naming the profile's modes | +| `topics.low_cmd_in`, `low_cmd_lower_in`, `low_cmd_upper_in` | `/safety/lowcmd_in`, `/safety/lowcmd_lower_in`, `/safety/lowcmd_upper_in` | the inputs; absolute ROS names (`rt/...` is refused) | +| `topics.low_cmd_out` | `/lowcmd` | the output | +| `topics.low_state` | `/lowstate` | the robot's state | +| `control.publish_hz` | 500 | the relay rate | +| `control.state_lag_ms` | required | the transport-lag estop, ms; on ROS domain 0 only; <= 0 turns it off | +| `stale.upper_seconds`, `lower_seconds`, `state_seconds` | 0, 0.2, 0.1 | quiet times of the upper input, the lower or full input and `/lowstate`, s; <= 0 turns the report off | +| `single_writer.handover_seconds` | 0.5 | how long the current writer must be quiet before another is accepted, s | +| `estop.triggered_topic`, `plugged_in_topic` | `/estop/triggered`, `/estop/plugged_in` | the hardware estop's topics | +| `estop.timeout_seconds` | 0.5 | the oldest a hardware estop sample may be, s | +| `estop_response.mode` | `zero` | `zero`: every motor off (mode 0, every value 0); `damp`: every body motor on with kp 0 and `kd` | +| `estop_response.kd` | none | `damp` only (required there): one number or one per body joint, at most `limits.clip.kd_max` | +| `estop_response.after` | required | `burst_then_silence` or `keep_writing` | +| `estop_response.burst_count` | 50 | the commands of `burst_then_silence` | +| `limits.clip.position_offset`, `velocity_ratio`, `torque_ratio` | 0.01, 0.5, 0.5 | one number, or one per body joint in slot order | +| `limits.clip.kp_max`, `kd_max` | 300, 30 | one number for every joint (`kp_abs`, `kd_abs` are aliases) | +| `limits.estop.position_offset`, `velocity_ratio`, `torque_ratio` | 0.01, 0.5, 0.5 | as `limits.clip`, for the measured state | +| `logging.enabled`, `base_dir`, `chunk_prefix` | `false`, `/tmp/golem_safety`, `chunk_` | the `.npz` sample log | + +`limits.enforce` is an alias of `limits.clip`. The limits derive from the profile's joint +table: q within [low + `position_offset`, high - `position_offset`] (a negative offset widens +the range), |dq| up to velocity limit x `velocity_ratio`, |tau| up to effort limit x +`torque_ratio`. `golem_safety_dump_config ` (installed in +`lib/golem_safety`) prints every derived and parsed value as JSON. + +The loader refuses, naming the robot, the file and the key: an unknown key at any level; a +key of an older schema (`network`, `robot`, `startup`, `engage`, `estop.enabled`, ...), +saying why it was removed or which key replaces it; a non-finite or out-of-range number; a +list whose length is not the profile's joint count; an alias given with the key it aliases; +a missing required key. + +## Behavior + +**Relay, newest only.** One relay thread runs at `control.publish_hz`, sleeping to absolute +deadlines on the boot-time clock. Each cycle it takes the newest `/lowstate`, the newest +hardware estop values and the newest command of each input (keep-last 1, so no queue builds +up), and publishes one command. A cycle that ends more than a period late restarts the +schedule: a slow cycle lowers the rate, and nothing is replayed to catch up. Every timer in +the node runs on the boot-time or steady clock, never on the wall clock or on `/clock`. + +**What it writes.** Each engaged input's newest valid command on that input's own slots +(`full`: every body slot; `split`: the lower and upper slots). Every other slot, the slots +past the body and the `reserve` words carry `zero_command` (mode 0, every value 0). Every +command carries the profile's `mode_machine` and `mode_pr` and its own Unitree CRC (CRC-32 +over the message body, as the SDK computes it); nothing of an input's header is copied. + +**Clipping.** A command is valid when q, dq, tau, kp and kd are finite on every body slot, +the input's own or not; any other command latches the estop. A valid command is clipped on +its input's slots: q, dq and tau to `limits.clip`, kp and kd to [0, `kp_max`] and [0, +`kd_max`]. A gain above its maximum raises `gain_clipped` (Warn) once per joint and gain. + +**Start-up**, the same on both robots (no config key chooses it): from boot the layer +streams `zero_command` on every slot, so every motor is off. Each input engages on its own +first valid command that arrives once the newest `/lowstate` reports the profile's +`mode_machine` (and, with `use_estop`, a first hardware estop sample has arrived); its slots +carry its commands from then on (`input_engaged`). The slots of an input that has not +engaged stay at zero gain: in split mode the robot runs arms-only, with its leg motors off, +until the leg node commands. The `/lowstate` limit checks run from boot. + +A command that cannot engage raises `engage_refused` (Warn) with the first reason, at most +once a second: `no /lowstate received yet`, `/lowstate mode_machine is 0, expected 6`, or +`no hardware estop sample received yet on /estop/triggered or /estop/plugged_in`. + +**Robot identity.** Every `/lowstate` is checked against the profile's `mode_machine`. Before +engaging, another value only refuses (another robot or sim delays engaging); once engaged, it +latches the estop (`/lowstate mode_machine 5 is not the h12's 6: another robot or sim is on +this domain`). A foreign sample is never limit-checked. `docker/scripts/wait_for_sim.py` +checks the same values before a paired bringup starts, and each robot's sims report them. + +**Quiet inputs are held and reported, not estopped.** Once an input has engaged, a gap in +its commands longer than its stale time (`stale.upper_seconds` for the upper input, +`stale.lower_seconds` for the lower and full inputs) raises `input_quiet` (Warn) while its +last command keeps being relayed, and its next command raises `input_resumed` (Info). Once +engaged, a `/lowstate` gap longer than `stale.state_seconds` raises `state_quiet` and the +relay keeps running; the next sample raises `state_resumed`. Neither latches. So: + +- a controller that dies, or a walking node that faults and goes silent, leaves its last + command in force until the bringup stops. On the G1 in split mode the legs stay on a + frozen target that does not balance the robot; the layer has no tilt or fall check and + does not read the IMU; +- `frame_task_server`'s last command at exit is the one held: on the H1-2 every upper slot + is at mode 0 and zero gains, so the torso and the arms go limp; in the G1's split mode the + waist keeps its hold angle and gains and only the arms go limp. + +**Single writer.** Each input's current writer is the DDS publisher of its first command. A +command from another publisher is accepted, and its publisher becomes current (logged, Info), +when the current writer's last command is older than `single_writer.handover_seconds`; a +restarted controller is accepted that way. Within that time it latches the estop (`second +writer on : ...`). + +**Hardware estop** (`use_estop:=true`). The layer reads the `estop` node's two topics; a +sample is a value on each, taken every cycle. No input engages before the first sample. A +sample that says triggered (`External estop triggered`) or not plugged in (`External estop +unplugged`) latches, the first one included. From the first sample on, no new sample for +`estop.timeout_seconds` latches too (`External estop silent: ...`), before engaging as well. +On the H1-2 the bringups start `estop_node` `safety_delay` (3 s) before this node, and a +`safety_node` running while `estop_node` reports a failed connection latches +(`h12_control.launch.py`'s comment). Without `use_estop` the topics are ignored. + +**Transport lag.** On ROS domain 0 with `control.state_lag_ms` > 0, a `/lowstate` (with the +profile's `mode_machine`) that arrives more than `state_lag_ms` later than the robot's +millisecond tick implies latches the estop. The baseline is the smallest arrival-minus-tick +offset seen, and may rise by 0.5 ms/s to follow clock drift. The sims stamp the tick in sim +steps, so the check runs only on domain 0. + +**The estop latch.** One way until restart. Its triggers: a non-finite command value; a +second writer; once engaged, another robot's `mode_machine`; the transport lag; a measured q +past [`q_estop_low`, `q_estop_high`], |dq| or |tau_est| past its estop limit, or any of them +non-finite, on the slots of the mode's inputs (every body slot in both modes); and the +hardware estop. Each limit reason names the joint, the value, the limit and the multiple +(`motor 3 (left_knee_joint) dq out of estop range: 15 (limit 12.000, 1.25x)`), and a limit +latch also logs the commanded and measured arrays (`estop diagnostic`, Error). After the +latch: + +- `burst_then_silence` (the H1-2): `burst_count` estop commands, then no output until + restart; +- `keep_writing` (the G1): the estop command every cycle. + +The estop report (Error) is `estop_command_written`: the reason, `decision_ms` (the latch), +`action_ms` (the first step after it) and, for the hardware estop, `trigger_stamp_ms`, all on +the boot-time clock. The heartbeat turns false, or starts false when the relay was not ready +yet. + +**Exit** (SIGINT, SIGTERM, SIGHUP; `main()` takes them, and a closed stderr cannot stop the +exit). The layer stops publishing at once; an estop burst in progress is finished first, and +with `keep_writing` the estop command is written once if none has been since the latch. The +sims then turn every body motor off after `GOLEM_CMD_TIMEOUT` (0.5 s of sim time) of +`rt/lowcmd` silence; what the real robots do when it stops has not been measured. + +## Logging + +Every event goes to the node's ROS logger at its level, from a thread of its own (a stalled +stderr cannot stall the relay): `[INFO] [