From d0f72a7029880e69c33a247b3de6538c24277dfc Mon Sep 17 00:00:00 2001 From: "-T.K.-" Date: Fri, 3 Jul 2026 10:52:55 -0700 Subject: [PATCH] docs: dual standby poses (A/B) + REMOTE damped init Reflect humanoid_control#15 across the site: STANDBY is now two StandbyController instances (standby_controller_a/_b, Pose A/B) with independent pose/policy selection; the gate topic /standby_controller/state becomes per-pose /standby_controller_{a,b}/state; the mode/load service splits into mode/load_a + mode/load_b; joy params load_combo_{locomotion,remote} become load_combo_{a,b}. Also corrects REMOTE's passive stale-command policy from fully-limp to a damped hold (kP=0, high kD, like DAMPING) and documents the new damping/damping_scalar params. Plugin class and standby_controller.cpp references left unchanged. Co-Authored-By: Claude Opus 4.8 (1M context) --- docs/concepts/architecture.md | 19 ++++--- docs/concepts/five_mode_fsm.md | 44 ++++++++++------ docs/concepts/safety_pipeline.md | 11 ++-- docs/getting_started/lite_101.md | 7 +-- docs/how_to/add_new_joint.md | 8 ++- docs/how_to/first_real_bringup.md | 2 +- docs/how_to/promote_python_to_cpp.md | 4 +- docs/how_to/record_experiments.md | 3 +- docs/how_to/recover_from_fault.md | 10 ++-- docs/how_to/switch_controllers_manually.md | 26 +++++++--- .../talk_to_humanoid_control_from_python.md | 2 +- docs/reference/cli_tools.md | 4 +- docs/reference/controllers.md | 51 +++++++++++-------- docs/reference/messages.md | 5 +- docs/reference/packages.md | 6 ++- docs/reference/quick_reference.md | 26 ++++++---- docs/reference/topics_services.md | 6 ++- docs/reference/troubleshooting.md | 2 +- docs/tutorials/mujoco_fsm_walk.md | 33 +++++++----- docs/tutorials/run_gravity_compensation.md | 6 +-- docs/tutorials/tracking_policy.md | 10 ++-- 21 files changed, 178 insertions(+), 107 deletions(-) diff --git a/docs/concepts/architecture.md b/docs/concepts/architecture.md index 35d2745..ce00f46 100644 --- a/docs/concepts/architecture.md +++ b/docs/concepts/architecture.md @@ -82,7 +82,7 @@ Behavior per state: |---|---|---| | **ZERO_TORQUE** | `humanoid_control/ZeroTorqueController` | 0 to all 5 cmd interfaces. Startup default, fault fallback. | | **DAMPING** | `humanoid_control/DampingController` | `K=0`, `D=damping value`, `q_cmd=q_captured` — soft under gravity, resists velocity. | -| **STANDBY** | `humanoid_control/StandbyController` | Linear pose interpolation through a YAML sequence; ramps `K_p / K_d` on first segment. Publishes `StandbyState` with `is_finished`. | +| **STANDBY** | `humanoid_control/StandbyController` | Linear pose interpolation through a YAML sequence; ramps `K_p / K_d` on first segment. Publishes `StandbyState` with `is_finished`. Two spawned instances of this plugin — `standby_controller_a` (Pose A, `L1+A`) and `standby_controller_b` (Pose B, `L1+B`) — provide two independently selectable poses; from either you can START either policy. | | **LOCOMOTION** | `humanoid_control/RLPolicyController` | In-process ONNX inference (System 0): packs observations, replays the `.mcap` motion reference, decodes + writes commands — all in the RT `update()`. Runs every learned policy (tracking / piano / locomotion); they differ only by the loaded `.onnx` + `.mcap`. | | **REMOTE** | `humanoid_control/RemotePolicyController` | System 1/2 external-command ingress: subscribes `~/command` (`MITCommand` over DDS) from a *non*-real-time source (gravity-comp today, VLA / manipulation later) with arrival-time stale-command gating. | @@ -94,14 +94,15 @@ plain `rclcpp::Node` that subscribes: - `/joy` (gamepad intents; on by default — bringup hard-fails if `/dev/input/js*` is missing unless you opt out with `enable_gamepad:=false`) -- `/standby_controller/state` (the `is_finished` gate for the two `START_*` intents) +- `/standby_controller_a/state` and `/standby_controller_b/state` (one per standby pose; the `is_finished` gate for the two `START_*` intents) - `/safety_status` (the auto-DAMP trigger) -…and exposes five `std_srvs/Trigger` services so transitions can also be +…and exposes six `std_srvs/Trigger` services so transitions can also be driven from the command line: -- `/humanoid_control/mode/damp`, `/humanoid_control/mode/load`, `/humanoid_control/mode/start_remote`, - `/humanoid_control/mode/start_locomotion`, `/humanoid_control/mode/quit` +- `/humanoid_control/mode/damp`, `/humanoid_control/mode/load_a`, `/humanoid_control/mode/load_b`, + `/humanoid_control/mode/start_remote`, `/humanoid_control/mode/start_locomotion`, + `/humanoid_control/mode/quit` `/control_mode` is published at 50 Hz. The manager polls `list_controllers` periodically (every 25 ticks = 500 ms) so controllers @@ -234,8 +235,10 @@ Concrete examples: STRICT switch to DAMPING. If DAMPING fails (e.g. command interfaces unavailable), `mode_manager` falls back to ZERO_TORQUE. - A `RemotePolicyController` whose Python publisher stalls for >100 ms - (`stale_command_timeout_ms` default) writes **passive commands** (zero - stiffness/damping) by default, or zero-order-holds the last command if + (`stale_command_timeout_ms` default) falls into a **damped hold** by + default (`stale_command_policy: passive` → zero stiffness, high damping + like DAMPING mode, holding the live joint position — the arms stay + damped, not limp), or zero-order-holds the last command if `stale_command_policy: hold` is set. Staleness is measured against **arrival time at the subscription callback**, not against `MITCommand.header.stamp`, so publisher clock skew is irrelevant. @@ -259,7 +262,7 @@ ships the piano-task-specific launches. | Side | Machine | Launch | What lives here | |---|---|---|---| -| **Robot** | Onboard computer (RT kernel, wired tether) | `humanoid_bringup_lite/launch/real.launch.py` (Humanoid Control) | `ros2_control_node`, `humanoid_devices_robstride` / `humanoid_devices_sito` hardware plugins, `joint_state_broadcaster`, the five FSM controllers (`zero_torque` / `damping` / `standby` / `rl_policy` / `remote_policy`), `mode_manager`, `joy_node`, `robot_state_publisher`, IMU driver | +| **Robot** | Onboard computer (RT kernel, wired tether) | `humanoid_bringup_lite/launch/real.launch.py` (Humanoid Control) | `ros2_control_node`, `humanoid_devices_robstride` / `humanoid_devices_sito` hardware plugins, `joint_state_broadcaster`, the six FSM controllers (`zero_torque` / `damping` / `standby_a` / `standby_b` / `rl_policy` / `remote_policy`), `mode_manager`, `joy_node`, `robot_state_publisher`, IMU driver | | **Host** | Operator workstation | `humanoid_bringup_lite/launch/viz.launch.py` (Humanoid Control) | `viser_viz` *or* `rerun_viz` (selected by `viewer:=`) | | **Robot** | Onboard computer | `humanoid_control_policy/launch/lite_policy.launch.py` (Humanoid Control) / `pianist_policy/launch/piano_policy.launch.py` (pianist_ros2) | Runs `prepare` (resolve ONNX, convert motion → `.mcap` + overlay) then loads `rl_policy_controller` into the local CM. Inference is in-process, so the `.onnx` / `.mcap` artifacts **and** the W&B / HF Hub / `onnxruntime` *prepare-time* deps live here. The RT path itself pulls none of them. | | **Robot** | Onboard computer | `pianist_policy/launch/midi_keyboard_driver.launch.py` (pianist_ros2) | USB-MIDI keyboard driver → `/piano/key_state` (`std_msgs/Float32MultiArray`); feeds the on-robot controller's `key_pressed` extern term locally (loopback, does **not** cross the tether). | diff --git a/docs/concepts/five_mode_fsm.md b/docs/concepts/five_mode_fsm.md index 16d72e1..7973695 100644 --- a/docs/concepts/five_mode_fsm.md +++ b/docs/concepts/five_mode_fsm.md @@ -34,7 +34,7 @@ property. |---|---|---|---| | **ZERO_TORQUE** | `humanoid_control::ZeroTorqueController` | `0` to all 5 MIT interfaces on every joint | Startup default. Fault fallback when DAMPING can't be applied (e.g. state not yet valid). Robot is alive but inert. | | **DAMPING** | `humanoid_control::DampingController` | `stiffness=0`, `damping=damping_value`, `position=captured`, `velocity=0`, `effort=0` | Compliant fail-safe. Robot stays soft under gravity but resists velocity. The state you pass through between operator-driven transitions. | -| **STANDBY** | `humanoid_control::StandbyController` | Interpolated `position` along a YAML pose sequence; `K_p`/`K_d` ramped 0→target during segment 0 | Animate the arms to the piano-ready pose with gain ramp-in. Publishes `~/state.is_finished` so transitions out are gated correctly. | +| **STANDBY** | `humanoid_control::StandbyController` | Interpolated `position` along a YAML pose sequence; `K_p`/`K_d` ramped 0→target during segment 0 | Animate the arms to a ready pose with gain ramp-in. Spawned as **two instances** — `standby_controller_a` (Pose A) and `standby_controller_b` (Pose B) — the same plugin with different YAML poses. Each publishes `~/state.is_finished` so transitions out are gated correctly. | | **LOCOMOTION** | `humanoid_control::RLPolicyController` | In-process ONNX inference (low-latency, C++): packs obs, replays the `.mcap` motion reference, writes commands | **Every learned policy** — tracking, piano, locomotion. They differ only by the loaded `.onnx` + `.mcap`; the ONNX `task_type` selects the term set. This is the System 0 real-time path. | | **REMOTE** | `humanoid_control::RemotePolicyController` | `MITCommand` consumed from `~/command` over DDS | System 1/2 external-command ingress: a *non*-real-time source publishes commands (gravity-comp today via `Lite-Gravity-Compensation`; VLA / manipulation later). Not used by the learned policies. | @@ -49,7 +49,8 @@ current mode: ``` DAMP (X) : any state → DAMPING -LOAD (L1+A/B) : DAMPING → STANDBY +LOAD_A (L1+A) : DAMPING → STANDBY (standby_controller_a, Pose A) +LOAD_B (L1+B) : DAMPING → STANDBY (standby_controller_b, Pose B) START_LOCOMOTION (R1+A) : STANDBY ∧ is_finished → LOCOMOTION START_REMOTE (R1+B) : STANDBY ∧ is_finished → REMOTE QUIT (BACK) : ZERO_TORQUE or DAMPING → rclcpp::shutdown() @@ -58,7 +59,7 @@ QUIT (BACK) : ZERO_TORQUE or DAMPING → rclcpp::shutdown() ``` The same transitions are also exposed as `std_srvs/Trigger` services -under `/humanoid_control/mode/{damp,load,start_remote,start_locomotion,quit}`, so a +under `/humanoid_control/mode/{damp,load_a,load_b,start_remote,start_locomotion,quit}`, so a keyboardless lab box can drive the FSM with `ros2 service call …`. `mode_manager` publishes `/control_mode` (`humanoid_control_msgs/ControlMode`) at @@ -91,19 +92,30 @@ back to ZERO_TORQUE and writes the failure reason into See [Concepts → Safety pipeline](./safety_pipeline.md) for what each flag means and which plugin sets it. -## Pairing convention for the START combos - -`L1+A → R1+A` is the "locomotion" (local) path; `L1+B → R1+B` is the -"remote policy" path. **Both LOAD combos land in the same STANDBY** — -the A/B distinction is just operator UX so your thumb stays on one -column through `LOAD → START`. The FSM doesn't enforce it; you could -press `L1+A` then `R1+B` and the state machine would happily route -you DAMPING → STANDBY → REMOTE. - -The reason for two combos at all is the **policy target is chosen at -runtime**, not by a launch arg. There's no `policy_mode` parameter on -`mode_manager` — the choice between `RemotePolicyController` and -`RLPolicyController` is the button that ends the START combo. +## Pose and policy are independent + +The two LOAD combos select **which standby pose** to animate to; the two +START combos select **which policy** to run. The two axes are orthogonal: + +- `L1+A` → LOAD_A loads `standby_controller_a` (Pose A); `L1+B` → LOAD_B + loads `standby_controller_b` (Pose B). The two instances are the same + `StandbyController` plugin (source `standby_controller.cpp`, unchanged) + configured with **different YAML poses**. +- From **either** standby pose you can start **either** policy: `R1+A` → + LOCOMOTION (`rl_policy_controller`), `R1+B` → REMOTE + (`remote_policy_controller`). There is no pairing — `L1+A` then `R1+B` + is exactly as valid as `L1+A` then `R1+A`. + +The one thing you **cannot** do is switch directly from Pose A to Pose B +(or back): LOAD is admissible only from DAMPING (unchanged), so to change +pose you DAMP first, then LOAD the other pose. On `/control_mode` both +poses report `mode = STANDBY`; the `controller_name` field tells you which +one is active (`standby_controller_a` vs `standby_controller_b`). + +The **policy target is still chosen at runtime**, not by a launch arg. +There's no `policy_mode` parameter on `mode_manager` — the choice between +`RemotePolicyController` and `RLPolicyController` is the button that ends +the START combo. ## What mode_manager is *not* diff --git a/docs/concepts/safety_pipeline.md b/docs/concepts/safety_pipeline.md index a8bd564..ae6e363 100644 --- a/docs/concepts/safety_pipeline.md +++ b/docs/concepts/safety_pipeline.md @@ -70,10 +70,13 @@ additionally has a **stale-command policy**: if the external source's (default 100 ms = 5 ticks at 50 Hz, measured against arrival time at the subscription callback — not against `MITCommand.header.stamp` — so publisher clock skew is irrelevant), the controller writes a fallback -pattern rather than re-using the last command. Default `passive` → zero -stiffness / damping → motors go limp. Alternative `hold` → freeze at the -last commanded pose. Either way the controller stays alive and active; -the choice is whether to "fail compliant" or "fail rigid". +pattern rather than re-using the last command. Default `passive` → a +**damped hold**: zero stiffness, high damping (`kD = damping_scalar`, +default 6.0, matching DAMPING mode), holding the live joint position — the +arms stay damped, not limp. (The same damped hold also applies when REMOTE +is first entered, before any `MITCommand` has arrived.) Alternative `hold` +→ freeze at the last commanded pose. Either way the controller stays alive +and active; the choice is whether to "fail compliant" or "fail rigid". `RLPolicyController` has no such command stream — it runs inference in-process (System 0), so there is nothing to go stale. It guards diff --git a/docs/getting_started/lite_101.md b/docs/getting_started/lite_101.md index d5ef17a..1693f94 100644 --- a/docs/getting_started/lite_101.md +++ b/docs/getting_started/lite_101.md @@ -91,7 +91,8 @@ Expected output (give or take): joint_state_broadcaster joint_state_broadcaster/JointStateBroadcaster active zero_torque_controller humanoid_control/ZeroTorqueController active damping_controller humanoid_control/DampingController inactive -standby_controller humanoid_control/StandbyController inactive +standby_controller_a humanoid_control/StandbyController inactive +standby_controller_b humanoid_control/StandbyController inactive rl_policy_controller humanoid_control/RLPolicyController inactive remote_policy_controller humanoid_control/RemotePolicyController inactive ``` @@ -105,7 +106,7 @@ claims all 5 command interfaces on every joint and writes 0 to all of them every tick. From an operator's perspective the robot is "alive but inert". -The four other controllers are **loaded but inactive**. Loading them +The five other controllers are **loaded but inactive**. Loading them runs their `on_configure` (params parsed, publishers/subscribers created) without claiming the command interfaces. They sit ready to be activated in a single service call. @@ -117,7 +118,7 @@ ros2 topic list | grep -E "joint_states|control_mode|standby|safety" # /control_mode # /lite/joint_states # /safety_status -# /standby_controller/state +# /standby_controller_a/state ``` `joint_state_broadcaster` is remapped at bringup so that it publishes on diff --git a/docs/how_to/add_new_joint.md b/docs/how_to/add_new_joint.md index 5e8708e..65ee29c 100644 --- a/docs/how_to/add_new_joint.md +++ b/docs/how_to/add_new_joint.md @@ -119,7 +119,7 @@ length. Use the same K/D as a similar-class joint as a starting point: ```yaml -standby_controller: +standby_controller_a: ros__parameters: joints: [...] # length 17 now target_stiffness: [20, 20, ..., 20, 30, 30, 30] # was 14 entries, now 17 @@ -128,6 +128,12 @@ standby_controller: pose_segment_1: [0.3, -1.0, ..., -0.3, 0, 0, 0] # piano-ready + neck-zero ``` +STANDBY now has two poses, each a separate spawned instance of the same +plugin with its own params block — `standby_controller_a` (Pose A) and +`standby_controller_b` (Pose B). Both blocks own per-joint arrays, so +extend **both** the same way when you add a joint: the arrays in +`standby_controller_b:` must grow to the new length too. + ## Step 4 — Calibrate Once the URDF + YAML are updated and the build is clean, plug in the diff --git a/docs/how_to/first_real_bringup.md b/docs/how_to/first_real_bringup.md index 55085db..1d20577 100644 --- a/docs/how_to/first_real_bringup.md +++ b/docs/how_to/first_real_bringup.md @@ -116,7 +116,7 @@ pixi shell # 14 joints reporting at the configured update rate ros2 control list_controllers # Expect: joint_state_broadcaster (active), zero_torque_controller (active), -# damping_controller / standby_controller / remote_policy_controller (inactive) +# damping_controller / standby_controller_a / standby_controller_b / remote_policy_controller (inactive) ros2 topic hz /lite/joint_states # average rate: 50.0 (± 0.2) diff --git a/docs/how_to/promote_python_to_cpp.md b/docs/how_to/promote_python_to_cpp.md index d7aa971..22e932b 100644 --- a/docs/how_to/promote_python_to_cpp.md +++ b/docs/how_to/promote_python_to_cpp.md @@ -101,8 +101,8 @@ Finally, drive the FSM to activate (third terminal, inside `pixi shell`): ```bash ros2 service call /humanoid_control/mode/damp std_srvs/srv/Trigger -ros2 service call /humanoid_control/mode/load std_srvs/srv/Trigger -# wait for /standby_controller/state.is_finished == true +ros2 service call /humanoid_control/mode/load_a std_srvs/srv/Trigger # or load_b for Pose B +# wait for /standby_controller_a/state.is_finished == true (per pose) ros2 service call /humanoid_control/mode/start_locomotion std_srvs/srv/Trigger ``` diff --git a/docs/how_to/record_experiments.md b/docs/how_to/record_experiments.md index 74acaf2..356112a 100644 --- a/docs/how_to/record_experiments.md +++ b/docs/how_to/record_experiments.md @@ -38,7 +38,8 @@ explicitly: ros2 bag record -s mcap -o tuning_$(date +%Y%m%d_%H%M%S) \ /lite/joint_states \ /control_mode \ - /standby_controller/state \ + /standby_controller_a/state \ + /standby_controller_b/state \ /safety_status \ /joy \ /tf /tf_static diff --git a/docs/how_to/recover_from_fault.md b/docs/how_to/recover_from_fault.md index 1548f6f..98812eb 100644 --- a/docs/how_to/recover_from_fault.md +++ b/docs/how_to/recover_from_fault.md @@ -141,11 +141,13 @@ walks back up: # /safety_status is now OK (level=0) # But mode_manager is still in DAMPING. # Walk back via the gamepad: -# L1+A or L1+B → STANDBY -# R1+A → LOCOMOTION -# R1+B → REMOTE +# L1+A → STANDBY (Pose A) +# L1+B → STANDBY (Pose B) +# R1+A → LOCOMOTION +# R1+B → REMOTE # Or via the std_srvs/Trigger services: -ros2 service call /humanoid_control/mode/load std_srvs/srv/Trigger +ros2 service call /humanoid_control/mode/load_a std_srvs/srv/Trigger +# (use /humanoid_control/mode/load_b for Pose B) ros2 service call /humanoid_control/mode/start_remote std_srvs/srv/Trigger ``` diff --git a/docs/how_to/switch_controllers_manually.md b/docs/how_to/switch_controllers_manually.md index e01b86d..f5113a0 100644 --- a/docs/how_to/switch_controllers_manually.md +++ b/docs/how_to/switch_controllers_manually.md @@ -69,36 +69,47 @@ oscillating. ### DAMPING → STANDBY +STANDBY has two poses, each a separately spawned instance of the same +plugin: `standby_controller_a` (Pose A) and `standby_controller_b` +(Pose B). Activate whichever pose you want: + ```bash ros2 control switch_controllers \ --deactivate damping_controller \ - --activate standby_controller + --activate standby_controller_a ``` +Use `--activate standby_controller_b` instead for Pose B. + **The motors will move.** Standby ramps `K_p` / `K_d` from 0 to the target gains during segment 0, then interpolates to the piano-ready pose during segment 1. Total runtime ~4 seconds. Support the arms or have a clear workspace. -Watch `/standby_controller/state` for `is_finished: true`: +Watch the state topic for the pose you activated (one per instance) for +`is_finished: true`: ```bash -ros2 topic echo /standby_controller/state +ros2 topic echo /standby_controller_a/state ``` ### STANDBY → REMOTE (or LOCOMOTION) ```bash ros2 control switch_controllers \ - --deactivate standby_controller \ + --deactivate standby_controller_a \ --activate remote_policy_controller ``` +(Deactivate whichever standby instance is active — +`standby_controller_a` or `standby_controller_b`.) + `remote_policy_controller` (`humanoid_control/RemotePolicyController`) is the **System 1/2 external-command ingress**: it immediately starts looking for `MITCommand` on `/remote_policy_controller/command`. Without a publisher it'll trip its stale-command policy (`passive` by default -→ motors go limp) within 100 ms. To use this for real, start a +→ a damped hold: zero stiffness, high damping like DAMPING, holding +live position) within 100 ms. To use this for real, start a non-real-time `MITCommand` source first — gravity compensation (`Lite-Gravity-Compensation`) today, VLA / manipulation later. This controller is **not** fed by any learned policy; learned policies run @@ -135,7 +146,8 @@ ros2 control list_controllers # damping_controller humanoid_control/DampingController active # zero_torque_controller humanoid_control/ZeroTorqueController inactive # joint_state_broadcaster joint_state_broadcaster/... active -# standby_controller humanoid_control/StandbyController inactive +# standby_controller_a humanoid_control/StandbyController inactive +# standby_controller_b humanoid_control/StandbyController inactive # remote_policy_controller humanoid_control/RemotePolicyController inactive # What hardware components are up? @@ -160,7 +172,7 @@ ros2cs zero_torque_controller damping_controller # Force a strict switch (fail if either controller is in the wrong state) ros2 control switch_controllers \ --deactivate damping_controller \ - --activate standby_controller \ + --activate standby_controller_a \ --strict ``` diff --git a/docs/how_to/talk_to_humanoid_control_from_python.md b/docs/how_to/talk_to_humanoid_control_from_python.md index 9821884..4f69f60 100755 --- a/docs/how_to/talk_to_humanoid_control_from_python.md +++ b/docs/how_to/talk_to_humanoid_control_from_python.md @@ -86,7 +86,7 @@ pub.write(zero_mit_command(state.name, damping=2.0)) | `JointState` | `/lite/joint_states` | reliable, depth 10 | | `ControlMode` | `/control_mode` | reliable | | `SafetyStatus` | `/safety_status` | reliable | -| `StandbyState` | `/standby_controller/state` | transient-local (latched) | +| `StandbyState` | `/standby_controller_a/state` (one per pose; `_b` for Pose B) | transient-local (latched) | QoS reliability and durability **must** match the bringup for DDS to pair a writer with a reader — the registry already encodes the matching values. diff --git a/docs/reference/cli_tools.md b/docs/reference/cli_tools.md index 01d2d89..5fb0447 100644 --- a/docs/reference/cli_tools.md +++ b/docs/reference/cli_tools.md @@ -136,8 +136,8 @@ ros2 run humanoid_controllers mode_manager --ros-args -p tick_rate_hz:=100 | `controller_manager` | `/controller_manager` | CM namespace | | `joy.damp_button` | `2` | DAMP button index (default = X on Xbox) | | `joy.quit_button` | `6` | QUIT button index (default = BACK) | -| `joy.load_combo_locomotion` | `[4, 0]` | LOAD combo paired with R1+A (default = L1+A) | -| `joy.load_combo_remote` | `[4, 1]` | LOAD combo paired with R1+B (default = L1+B) | +| `joy.load_combo_a` | `[4, 0]` | LOAD_A combo (default = L1+A → Pose A) | +| `joy.load_combo_b` | `[4, 1]` | LOAD_B combo (default = L1+B → Pose B) | | `joy.start_combo_locomotion` | `[5, 0]` | START_LOCOMOTION (default = R1+A) | | `joy.start_combo_remote` | `[5, 1]` | START_REMOTE (default = R1+B) | diff --git a/docs/reference/controllers.md b/docs/reference/controllers.md index b0cc118..e4488e1 100644 --- a/docs/reference/controllers.md +++ b/docs/reference/controllers.md @@ -58,6 +58,11 @@ resistance. `K_p / K_d` from `0` to target gains during the **first** segment so activation never snaps. +**Instances**: spawned **twice** — `standby_controller_a` (Pose A, loaded by +`L1+A` / `/humanoid_control/mode/load_a`) and `standby_controller_b` (Pose B, +loaded by `L1+B` / `/humanoid_control/mode/load_b`). Same plugin class (and same +`standby_controller.cpp`), different pose parameters in the bringup YAML. + **Parameters**: | Param | Type | Description | @@ -69,16 +74,18 @@ activation never snaps. | `pose_segment_` | `float64[]` | Per-segment target pose vector; one parameter per segment index in `[0, len(segment_durations))`. Each is a per-joint position array sized to `len(joints)`. | **Publishes**: `~/state` (`humanoid_control_msgs/StandbyState`) with `TRANSIENT_LOCAL` QoS -so `mode_manager` sees `is_finished` even on late join. +so `mode_manager` sees `is_finished` even on late join — i.e. +`/standby_controller_a/state` and `/standby_controller_b/state`, one per instance. +Watch the one matching the pose you loaded. :::tip[How the bundled config interpolates] -`humanoid_control_lite_controllers.yaml` ships **two segments**: `pose_segment_0` is -the zero-pose (where the robot starts), `pose_segment_1` is the -piano-ready training default — mirror-symmetric across the sagittal plane -(shoulders roll outward, elbows bend in, wrists relax). The LOAD intent -therefore animates the arms from zero to the piano-ready pose over two -2-second segments while ramping `K_p` / `K_d` from 0 to the target gains -during the first segment. +`humanoid_control_lite_controllers.yaml` configures **both instances** with +**two segments** each: `pose_segment_0` is the zero-pose (where the robot +starts) and `pose_segment_1` is that instance's target pose — +`standby_controller_a` and `standby_controller_b` differ only in this final +pose (Pose A vs Pose B). A LOAD_A / LOAD_B intent therefore animates the arms +from zero to the chosen pose over two 2-second segments while ramping `K_p` / +`K_d` from 0 to the target gains during the first segment. ::: `fallback_controllers: ["damping_controller"]` is set on the @@ -137,8 +144,10 @@ anymore — those run in-process in `RLPolicyController`. | Param | Type | Default | Description | |---|---|---|---| | `joints` | `string[]` | — | Required. | -| `stale_command_policy` | `string` | `passive` | `passive` or `hold` | +| `stale_command_policy` | `string` | `passive` | `passive` or `hold`. `passive` is a **damped hold** — zero stiffness (`kP=0`) and high damping (`kD = damping_scalar`) while holding the live joint position, exactly like DAMPING mode; applied both on entering REMOTE before the first command **and** on stale dropouts (it no longer goes fully limp). `hold` is unchanged. | | `stale_command_timeout_ms` | `int` | `100` | Staleness window measured against the message's **arrival time at the subscription callback**, not against `MITCommand.header.stamp`. Publisher clock skew is irrelevant. | +| `damping` | `float64[]` | `[]` | Per-joint `K_d` for the `passive` damped hold. Empty → use `damping_scalar`. | +| `damping_scalar` | `float64` | `6.0` | Damping used when `damping` is empty — matches the DAMPING mode's damping. | The controller **rejects** any `MITCommand` whose `joint_names` doesn't match its claimed order, or whose array lengths don't all match `joints.size()`. @@ -162,9 +171,9 @@ path relates. | Input | Topic / source | Purpose | |---|---|---| | Gamepad | `/joy` (`sensor_msgs/Joy`) | DAMP / LOAD / START_LOCOMOTION / START_REMOTE / QUIT intents | -| Standby done | `/standby_controller/state` (`StandbyState`) | gate the START intents on `is_finished` | +| Standby done | `/standby_controller_a/state` or `/standby_controller_b/state` (`StandbyState`) | gate the START intents on `is_finished` (watch the loaded pose's instance) | | Safety | `/safety_status` (`SafetyStatus`) | auto-fall to DAMPING on non-OK | -| Trigger services | `/humanoid_control/mode/{damp,load,start_remote,start_locomotion,quit}` (`std_srvs/Trigger`) | same intents from the command line | +| Trigger services | `/humanoid_control/mode/{damp,load_a,load_b,start_remote,start_locomotion,quit}` (`std_srvs/Trigger`) | same intents from the command line | | Output | Topic | Purpose | |---|---|---| @@ -176,17 +185,19 @@ path relates. | Buttons | Intent | Target | |---|---|---| | `X` (2) | DAMP | `damping_controller` | -| `L1+A` (4+0) **or** `L1+B` (4+1) | LOAD | `standby_controller` | +| `L1+A` (4+0) | LOAD_A | `standby_controller_a` (Pose A) | +| `L1+B` (4+1) | LOAD_B | `standby_controller_b` (Pose B) | | `R1+A` (5+0) | START_LOCOMOTION | `rl_policy_controller` | | `R1+B` (5+1) | START_REMOTE | `remote_policy_controller` | | `BACK` (6) | QUIT | `rclcpp::shutdown()` | -The two LOAD combos and two START combos are paired by **operator -convention** (A = local policy, B = remote policy): `L1+A → R1+A` for the -locomotion (local) path, `L1+B → R1+B` for the remote-policy path. The `LOAD` transition lands in -the same `STANDBY` state either way — the pairing just lets the -operator's thumb stay on the same column through the LOAD → START -sequence. +The two LOAD combos load **different poses** (`standby_controller_a` vs +`standby_controller_b`); the two START combos pick the **policy**. The two +axes are independent — from either standby pose you can start either policy +(`R1+A` → LOCOMOTION, `R1+B` → REMOTE), so `L1+A → R1+B` is just as valid as +`L1+A → R1+A`. The only constraint is that you cannot switch A↔B directly: +`LOAD` is admissible only from DAMPING, so to change pose you DAMP first, +then load the other pose. **Parameters**: @@ -196,8 +207,8 @@ sequence. | `controller_manager` | `string` | `/controller_manager` | CM namespace | | `joy.damp_button` | `int` | `2` | DAMP button index | | `joy.quit_button` | `int` | `6` | QUIT button index | -| `joy.load_combo_locomotion` | `int[]` | `[4, 0]` | LOAD combo (locomotion-paired = L1+A) | -| `joy.load_combo_remote` | `int[]` | `[4, 1]` | LOAD combo (remote-paired = L1+B) | +| `joy.load_combo_a` | `int[]` | `[4, 0]` | LOAD_A combo (L1+A → Pose A) | +| `joy.load_combo_b` | `int[]` | `[4, 1]` | LOAD_B combo (L1+B → Pose B) | | `joy.start_combo_locomotion` | `int[]` | `[5, 0]` | START_LOCOMOTION combo (R1+A) | | `joy.start_combo_remote` | `int[]` | `[5, 1]` | START_REMOTE combo (R1+B) | diff --git a/docs/reference/messages.md b/docs/reference/messages.md index f8f369f..ca1cc29 100644 --- a/docs/reference/messages.md +++ b/docs/reference/messages.md @@ -69,8 +69,9 @@ match against the `uint8 = ` defines, not hard-code integers. ## `humanoid_control_msgs/StandbyState` -Published by `StandbyController` on `~/state` (which resolves to -`/standby_controller/state`) with `TRANSIENT_LOCAL` (latched) QoS so a +Published by each `StandbyController` instance on `~/state` — which resolves +to `/standby_controller_a/state` (Pose A) or `/standby_controller_b/state` +(Pose B), one topic per pose — with `TRANSIENT_LOCAL` (latched) QoS so a late-joining `mode_manager` immediately sees the most recent value. ``` diff --git a/docs/reference/packages.md b/docs/reference/packages.md index e8a5a98..8810101 100644 --- a/docs/reference/packages.md +++ b/docs/reference/packages.md @@ -50,7 +50,7 @@ Custom ROS 2 interfaces. Once a trained policy depends on one, it is **frozen**. |---|---| | `MITCommand` | System 1/2 source → `RemotePolicyController`. The on-wire command format (also written internally by `RLPolicyController`). | | `ControlMode` | `mode_manager` → `/control_mode` telemetry. | -| `StandbyState` | `StandbyController` → `/standby_controller/state` (`is_finished` gate for the `START_LOCOMOTION` / `START_REMOTE` intents). | +| `StandbyState` | `StandbyController` → `/standby_controller_a/state` / `/standby_controller_b/state` (one topic per pose; `is_finished` gate for the `START_LOCOMOTION` / `START_REMOTE` intents). | | `SafetyStatus` | every hardware plugin / controller → `/safety_status`. Per-bus `source` field; bitmask in `flags`. | See [Messages reference](messages.md) for full schemas. @@ -277,7 +277,9 @@ The first two launches: `MujocoRos2ControlPlugin` loaded as a pluginlib physics plugin). 3. Start `robot_state_publisher`. 4. Spawn `joint_state_broadcaster` (active) + `zero_torque_controller` - (active) + the four remaining mode controllers (inactive). + (active) + the five remaining mode controllers (inactive) — `damping`, + the two standby poses (`standby_controller_a` / `standby_controller_b`), + `rl_policy`, and `remote_policy`. 5. Start `mode_manager` (when `enable_mode_manager:=true`). 6. Start `joy_node` (when `enable_gamepad:=true`, which is the default). diff --git a/docs/reference/quick_reference.md b/docs/reference/quick_reference.md index 27a6bbb..ef2bb78 100644 --- a/docs/reference/quick_reference.md +++ b/docs/reference/quick_reference.md @@ -134,14 +134,17 @@ hc viz rerun # native rerun window | Buttons | Intent | Allowed from | Activates | |---|---|---|---| | `X` | DAMP | any state | `damping_controller` | -| `L1 + A` *or* `L1 + B` | LOAD | DAMPING | `standby_controller` | +| `L1 + A` | LOAD_A | DAMPING | `standby_controller_a` (Pose A) | +| `L1 + B` | LOAD_B | DAMPING | `standby_controller_b` (Pose B) | | `R1 + A` | START_LOCOMOTION | STANDBY (gated on `is_finished`) | `rl_policy_controller` | | `R1 + B` | START_REMOTE | STANDBY (gated on `is_finished`) | `remote_policy_controller` | | `BACK` | QUIT | ZERO_TORQUE or DAMPING only | `rclcpp::shutdown()` | -Pair conventionally (A = local, B = remote): `L1+A → R1+A` for locomotion, -`L1+B → R1+B` for remote-policy. Combos are identical functionally; the operator's thumb -just stays on one column. See [Concepts → Five-mode FSM](../concepts/five_mode_fsm.md). +`L1+A` and `L1+B` load **different poses** (`standby_controller_a` / +`standby_controller_b`); the START combo picks the policy. Pose and policy are +independent — from either pose, `R1+A` → LOCOMOTION or `R1+B` → REMOTE. You +cannot switch A↔B directly (LOAD is admissible only from DAMPING); DAMP first, +then load the other pose. See [Concepts → Five-mode FSM](../concepts/five_mode_fsm.md). ## Manual controller switching (no FSM) @@ -153,10 +156,11 @@ ros2 control switch_controllers \ --deactivate zero_torque_controller \ --activate damping_controller -# DAMPING → STANDBY (motors will move to piano-ready over ~4 s) +# DAMPING → STANDBY Pose A (motors will move to the ready pose over ~4 s; +# use standby_controller_b for Pose B) ros2 control switch_controllers \ --deactivate damping_controller \ - --activate standby_controller + --activate standby_controller_a # Back to safe ros2 control switch_controllers \ @@ -175,7 +179,8 @@ Always end a session with `zero_torque_controller` active before | `/imu/data` | `sensor_msgs/Imu` | sensor-rate | always; RELIABLE | | `/control_mode` | `humanoid_control_msgs/ControlMode` | 50 Hz | always (`mode_manager`) | | `/safety_status` | `humanoid_control_msgs/SafetyStatus` | on-change, latched | TRANSIENT_LOCAL; `source` field per bus | -| `/standby_controller/state` | `humanoid_control_msgs/StandbyState` | active-only | TRANSIENT_LOCAL; watch for `is_finished:true` before R1+A | +| `/standby_controller_a/state` | `humanoid_control_msgs/StandbyState` | active-only | TRANSIENT_LOCAL; Pose A. Watch for `is_finished:true` before R1+A/R1+B | +| `/standby_controller_b/state` | `humanoid_control_msgs/StandbyState` | active-only | TRANSIENT_LOCAL; Pose B. Watch the instance matching the loaded pose | | `/remote_policy_controller/command` | `humanoid_control_msgs/MITCommand` | source rate | when a System 1/2 source (gravity-comp, VLA) feeds `remote_policy_controller` | | `/piano/key_state` | `std_msgs/Float32MultiArray` | sensor / sim rate | piano runs only (RELIABLE + KEEP_LAST(1)); live key state, in-process `key_pressed` term | | `/joy` | `sensor_msgs/Joy` | sensor-rate | when `enable_gamepad:=true` (default) | @@ -197,7 +202,7 @@ ros2 topic echo --once /safety_status # Drive an FSM transition without a gamepad ros2 service call /humanoid_control/mode/damp std_srvs/srv/Trigger -ros2 service call /humanoid_control/mode/load std_srvs/srv/Trigger +ros2 service call /humanoid_control/mode/load_a std_srvs/srv/Trigger # Pose A (load_b for Pose B) # Fake a System 1/2 MITCommand publish (when remote_policy_controller is active in MuJoCo) ros2 topic pub --once /remote_policy_controller/command \ @@ -212,7 +217,8 @@ intents — useful when there's no joystick attached. | Service | Effect | |---|---| | `/humanoid_control/mode/damp` | → DAMPING from any state | -| `/humanoid_control/mode/load` | DAMPING → STANDBY | +| `/humanoid_control/mode/load_a` | DAMPING → STANDBY (Pose A, `standby_controller_a`) | +| `/humanoid_control/mode/load_b` | DAMPING → STANDBY (Pose B, `standby_controller_b`) | | `/humanoid_control/mode/start_remote` | STANDBY → REMOTE (gated on `is_finished`) | | `/humanoid_control/mode/start_locomotion` | STANDBY → LOCOMOTION (gated on `is_finished`) | | `/humanoid_control/mode/quit` | exit (only from ZERO_TORQUE or DAMPING) | @@ -261,7 +267,7 @@ Five command interfaces per joint: `position`, `velocity`, `effort`, | `ENOBUFS` / `Network is down` warnings | Motor power off → frames don't ACK → qdisc fills. Power the motors. | | `/lite/joint_states` shows exactly 0.0 for every joint | Motors un-Enabled (no power, or Enable frame dropped). Check `/safety_status flags`. | | Launch dies with "`joy_dev:=/dev/input/jsN` does not exist" | `enable_gamepad:=true` is the default and the bringup hard-fails when the resolved joystick path is missing. Plug a gamepad in, pass `joy_dev:=` (the error message lists any other `/dev/input/js*` it found), or pass `enable_gamepad:=false`. | -| `mode_manager` rejects `LOAD` from anywhere other than DAMPING | Send DAMP (`X`) first. See FSM table above. | +| `mode_manager` rejects `LOAD_A`/`LOAD_B` from anywhere other than DAMPING | Send DAMP (`X`) first. See FSM table above. | | `ros2 topic echo /safety_status` reports `flags ≠ 0` | Check [Concepts → Safety pipeline](../concepts/safety_pipeline.md) for the bit definitions. | Full guidance: [Troubleshooting](./troubleshooting.md). diff --git a/docs/reference/topics_services.md b/docs/reference/topics_services.md index dea268b..bd18b5d 100644 --- a/docs/reference/topics_services.md +++ b/docs/reference/topics_services.md @@ -26,7 +26,8 @@ subscribes, or serves. Use this page to find "who publishes X" or |---|---|---|---|---| | `/control_mode` | `humanoid_control_msgs/ControlMode` | RELIABLE depth 10 | `mode_manager` | When `enable_mode_manager:=true` (default for `real.launch.py` / `mujoco.launch.py`). 50 Hz. | | `/safety_status` | `humanoid_control_msgs/SafetyStatus` | RELIABLE TRANSIENT_LOCAL depth 1 | every hardware plugin | Per-bus (`humanoid_devices_robstride/can0`, `humanoid_devices_robstride/can1` for Lite). Published only on change. | -| `/standby_controller/state` | `humanoid_control_msgs/StandbyState` | RELIABLE TRANSIENT_LOCAL depth 1 | `humanoid_control/StandbyController` (when active) | Carries `is_finished` — the gate for `START_*` intents. | +| `/standby_controller_a/state` | `humanoid_control_msgs/StandbyState` | RELIABLE TRANSIENT_LOCAL depth 1 | `humanoid_control/StandbyController` instance `standby_controller_a` (when active) | Pose A. Carries `is_finished` — the gate for `START_*` intents. | +| `/standby_controller_b/state` | `humanoid_control_msgs/StandbyState` | RELIABLE TRANSIENT_LOCAL depth 1 | `humanoid_control/StandbyController` instance `standby_controller_b` (when active) | Pose B. Carries `is_finished` — the gate for `START_*` intents. | | `/joy` | `sensor_msgs/Joy` | SENSOR_DATA | `joy_node` | When `enable_gamepad:=true` (default). The launch hard-fails on missing `/dev/input/js*`. | ### Active-controller-dependent @@ -57,7 +58,8 @@ keyboardless lab boxes or scripted tests. | Service | Effect | |---|---| | `/humanoid_control/mode/damp` | → DAMPING from any state | -| `/humanoid_control/mode/load` | DAMPING → STANDBY | +| `/humanoid_control/mode/load_a` | DAMPING → STANDBY (Pose A, `standby_controller_a`) | +| `/humanoid_control/mode/load_b` | DAMPING → STANDBY (Pose B, `standby_controller_b`) | | `/humanoid_control/mode/start_remote` | STANDBY → REMOTE (gated on `is_finished`) | | `/humanoid_control/mode/start_locomotion` | STANDBY → LOCOMOTION (gated on `is_finished`) | | `/humanoid_control/mode/quit` | exit (only from ZERO_TORQUE or DAMPING) | diff --git a/docs/reference/troubleshooting.md b/docs/reference/troubleshooting.md index 40b845b..7a3ae5b 100644 --- a/docs/reference/troubleshooting.md +++ b/docs/reference/troubleshooting.md @@ -174,7 +174,7 @@ state. `mode_manager` writes the reason into ``` ros2 topic echo /control_mode -# status_message: "Rejected LOAD: requires DAMPING (currently ZERO_TORQUE)" +# status_message: "LOAD_A ignored; must be in DAMPING" ``` **Fix**: walk the legal path. `LOAD` requires DAMPING; `START_*` diff --git a/docs/tutorials/mujoco_fsm_walk.md b/docs/tutorials/mujoco_fsm_walk.md index 682aadf..cbad12c 100644 --- a/docs/tutorials/mujoco_fsm_walk.md +++ b/docs/tutorials/mujoco_fsm_walk.md @@ -105,7 +105,11 @@ X. The FSM accepts DAMP from any state — even mid-policy. ## Step 4 — Transition: LOAD (DAMPING → STANDBY) -Press **L1 + A** (hold L1, press A). +Press **L1 + A** (hold L1, press A) — this loads **Pose A** +(`standby_controller_a`). `L1 + B` loads a *different* pose, **Pose B** +(`standby_controller_b`); the two poses are independent, and you can only +switch from one to the other by going through DAMPING first (there is no +direct A ↔ B switch). The arms now ramp through a two-segment trajectory: 1. Segment 0 (~2 s): K_p/K_d ramp 0 → target while position @@ -114,9 +118,10 @@ The arms now ramp through a two-segment trajectory: 2. Segment 1 (~2 s): position interpolates to the piano-ready pose (shoulders rolled out, elbows bent in). K_p/K_d stay at target. -While Standby is running: +While Standby is running (watch the topic for the pose you loaded — here +Pose A): ```bash -ros2 topic echo /standby_controller/state +ros2 topic echo /standby_controller_a/state # current_segment: 0, progress: 0.45, is_finished: false # ... # current_segment: 1, progress: 0.95, is_finished: false @@ -129,15 +134,15 @@ dragging in MuJoCo, they'll pull back to the standby pose. Now try pressing L1 + A again from STANDBY. The FSM rejects the intent and writes the reason to `/control_mode.status_message`: ``` -status_message: "LOAD ignored; must be in DAMPING" +status_message: "LOAD_A ignored; must be in DAMPING" ``` That's the gating — LOAD is only legal from DAMPING. ## Step 5 — Transition: START_REMOTE (STANDBY → REMOTE) -**Wait for `is_finished:true`** in `/standby_controller/state` first. -Then press **R1 + B**. +**Wait for `is_finished:true`** in `/standby_controller_a/state` first +(the topic for the pose you loaded). Then press **R1 + B**. `/control_mode` shows: ``` @@ -147,10 +152,12 @@ controller_name: remote_policy_controller `remote_policy_controller` is now claiming the command interfaces and looking for `MITCommand` on `/remote_policy_controller/command`. -Nothing is publishing yet — within 100 ms (the -`stale_command_timeout_ms`) it activates its stale-command policy -(default `passive` → zero stiffness/damping). The arms in MuJoCo go -limp. +Nothing is publishing yet. On entering REMOTE before any `MITCommand` +arrives — and again after >100 ms (the `stale_command_timeout_ms`) +without one — its `passive` stale-command policy holds a **damped** pose: +zero stiffness, high damping (like DAMPING mode), holding the live joint +position. The arms in MuJoCo sag and settle under damping but stay +damped — they are not free-swinging. This is **the expected behavior** without an external command source. `RemotePolicyController` is the System 1/2 external-command ingress: to @@ -179,9 +186,9 @@ ros2 topic pub --once /remote_policy_controller/command \ ``` The arms snap toward that pose — one publish only, so within 100 ms -they go limp again as the stale-command policy kicks in. Repeat the -publish to drive continuously, or move to the policy tutorial for -the auto-publish path. +they relax back into the damped hold (zero stiffness, high damping) as +the stale-command policy kicks in. Repeat the publish to drive +continuously, or move to the policy tutorial for the auto-publish path. ## Step 6 — Transition: DAMP (out of REMOTE) diff --git a/docs/tutorials/run_gravity_compensation.md b/docs/tutorials/run_gravity_compensation.md index 624ff2a..e897f12 100644 --- a/docs/tutorials/run_gravity_compensation.md +++ b/docs/tutorials/run_gravity_compensation.md @@ -109,11 +109,11 @@ transitions through `mode_manager`'s trigger services (in a second terminal, inside `pixi shell`): ```bash -ros2 service call /humanoid_control/mode/damp std_srvs/srv/Trigger # → DAMPING (gamepad: X) -ros2 service call /humanoid_control/mode/load std_srvs/srv/Trigger # → STANDBY (gamepad: L1+A) +ros2 service call /humanoid_control/mode/damp std_srvs/srv/Trigger # → DAMPING (gamepad: X) +ros2 service call /humanoid_control/mode/load_a std_srvs/srv/Trigger # → STANDBY, Pose A (gamepad: L1+A) # Wait until the standby ramp reports finished: -ros2 topic echo /standby_controller/state +ros2 topic echo /standby_controller_a/state # ... is_finished: true ros2 service call /humanoid_control/mode/start_remote std_srvs/srv/Trigger # → REMOTE (gamepad: R1+B) diff --git a/docs/tutorials/tracking_policy.md b/docs/tutorials/tracking_policy.md index 93d6030..446ce3c 100644 --- a/docs/tutorials/tracking_policy.md +++ b/docs/tutorials/tracking_policy.md @@ -96,13 +96,15 @@ transitions the gamepad would fire): ```bash # Gamepad equivalents, if you have one: # X → DAMP -# L1+A → LOAD (STANDBY); wait ~4 s for is_finished:true +# L1+A → LOAD_A (STANDBY, Pose A); wait ~4 s for is_finished:true +# L1+B → LOAD_B (STANDBY, Pose B) — an alternate pose; either pose +# can start either policy # ros2 service call /humanoid_control/mode/damp std_srvs/srv/Trigger -ros2 service call /humanoid_control/mode/load std_srvs/srv/Trigger +ros2 service call /humanoid_control/mode/load_a std_srvs/srv/Trigger # Wait for is_finished: -ros2 topic echo /standby_controller/state +ros2 topic echo /standby_controller_a/state # ... is_finished: true ``` @@ -159,7 +161,7 @@ once activated — there is no command topic to publish. ## Step 4 — STANDBY → START_LOCOMOTION -Wait for `is_finished: true` in `/standby_controller/state`, then +Wait for `is_finished: true` in `/standby_controller_a/state`, then activate the policy through the FSM (in the FSM-walk terminal, inside `pixi shell`):