diff --git a/docs/concepts/architecture.md b/docs/concepts/architecture.md index 759074d..a80b2e8 100644 --- a/docs/concepts/architecture.md +++ b/docs/concepts/architecture.md @@ -2,9 +2,10 @@ This page describes **how the stack runs end-to-end**: the `ros2_control` cycle (50 Hz on real hardware, 200 Hz in MuJoCo), the -five-mode finite state machine that arbitrates which controller is -active, how policies execute (in-process and real-time — this is the -System 0 layer), the safety / fallback model, and the two-machine +five control modes and the flat `joy_teleop` → `switch_controller` +arbitration that selects which controller is active, how policies execute +(in-process and real-time — this is the System 0 layer), the safety / +fallback model, and the two-machine **deployment topology** that decides which of those processes lives on the robot vs. on the operator workstation. @@ -70,9 +71,12 @@ by Berkeley's earlier Humanoid-Control deployment. ## Five-mode finite state machine -The whole control surface boils down to **one active controller at a time**, -selected by `mode_manager`. `joint_state_broadcaster` runs alongside as the -always-on state stream. +The whole control surface boils down to **one active controller at a time**. +The modes are unchanged, but the arbitration is now **flat**: the stock +`joy_teleop` node (ROS `teleop_tools`) maps each gamepad button directly to +a `/controller_manager/switch_controller` call, so any mode is reachable +from any other mode with no ordering rules and no dedicated FSM node. +`joint_state_broadcaster` runs alongside as the always-on state stream. ![Five-mode FSM](/img/diagrams/concepts__five_mode_fsm__01.svg) @@ -82,32 +86,41 @@ 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`. 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. | +| **STANDBY** | `humanoid_control/StandbyController` | Interpolates `position` from the **current measured** joint positions toward a YAML target pose, with constant target `K_p / K_d` from t=0 (no ramp) — safe to enter from any state, including a running policy, with no jump. Still publishes `StandbyState`, but nothing gates on `is_finished`. Three spawned instances — `standby_controller_a` / `_b` / `_y` (Poses A / B / Y, `L1+A` / `L1+B` / `L1+Y`); from any of them 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. | -### Transition mechanics +### Switching mechanics -Every transition is **one `switch_controller` service call** to the -controller_manager (STRICT strictness, async). The `mode_manager` node is a -plain `rclcpp::Node` that subscribes: +Every mode change is **one `switch_controller` service call** to the +controller_manager that activates one controller and deactivates its +siblings, with **BEST_EFFORT** strictness. The stock `joy_teleop` node (ROS +`teleop_tools`, built from source via `humanoid_control.repos` since it is +not in robostack-jazzy) reads `/joy` and issues that call, driven entirely +by YAML (`joy_teleop_lite.yaml`, `joy_teleop_biped.yaml`, +`joy_teleop_prime.yaml`; the mapping follows `qiayuanl/unitree_bringup`'s +`config/g1/joy.yaml`). There is no ordering and no gating — any button +fires from any current mode. -- `/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_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) +The Lite button map: -…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_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 -loaded after the first poll become visible to `dispatch_intent` without -the operator having to re-trigger. +| Button | Mode | Activates | +|---|---|---| +| `X` | DAMP | `damping_controller` | +| `L1` + `A` / `B` / `Y` | STANDBY (Pose A / B / Y) | `standby_controller_a` / `_b` / `_y` | +| `R1` + `A` | LOCOMOTION | `rl_policy_controller` | +| `R1` + `B` | REMOTE | `remote_policy_controller` | +| `BACK` | STOP | `zero_torque_controller` | + +`BACK` selects `zero_torque_controller` (motors hold zero torque but stay +enabled); it is not a power-down — full CAN Disable happens on `Ctrl+C` via +the hardware plugin's `on_deactivate`. + +There are no `/humanoid_control/mode/*` Trigger services and no +`/control_mode` topic anymore. Programmatic clients call +`/controller_manager/switch_controller` directly (headless operators use +`ros2 control switch_controllers`), and anything that needs the active mode +polls `/controller_manager/list_controllers`. ## Policy execution: System 0 (in-process, real-time) @@ -231,9 +244,9 @@ system: Concrete examples: - A Robstride bus-off → `humanoid_devices_robstride` publishes `SafetyStatus{level=FAULT, - source="humanoid_devices_robstride/can0", flags=BUS_OFF}` → `mode_manager` requests a - STRICT switch to DAMPING. If DAMPING fails (e.g. command interfaces - unavailable), `mode_manager` falls back to ZERO_TORQUE. + source="humanoid_devices_robstride/can0", flags=BUS_OFF}` on `/safety_status` as + **telemetry**. Nothing auto-DAMPs on it — the operator reacts (DAMP with + `X`, STOP with `BACK`); there is no `mode_manager` auto-DAMP path anymore. - A `RemotePolicyController` whose Python publisher stalls for >100 ms (`stale_command_timeout_ms` default) falls into a **damped hold** by default (`stale_command_policy: passive` → zero stiffness, high damping @@ -262,7 +275,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 six FSM controllers (`zero_torque` / `damping` / `standby_a` / `standby_b` / `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 seven mode controllers (`zero_torque` / `damping` / `standby_a` / `standby_b` / `standby_y` / `rl_policy` / `remote_policy`), `joy_node` + `joy_teleop`, `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). | @@ -287,31 +300,31 @@ Only DDS topics, never controller-manager service calls. | `/robot_description` | robot → host | RELIABLE + TRANSIENT_LOCAL | ~kB, latched | URDF tree (no meshes — host has its own install share). | | `/lite/joint_states` | robot → host | RELIABLE | 50 Hz, ~14 floats × 3 | Viewer input. The in-process policy reads it locally on the robot, so it no longer feeds a host process on the policy path. | | `/imu/data` | robot → host | RELIABLE | sensor-rate | Viewer / System 1/2 input. The in-process policy reads it locally on the robot. | -| `/control_mode` | robot → host | RELIABLE | 50 Hz | FSM telemetry for operator dashboards. | | `/remote_policy_controller/command` | host → robot | RELIABLE depth 4 | ~280 B | *Only* present when a System 1/2 source runs off-robot (gravity-comp, future VLA). `RemotePolicyController` uses arrival-time staleness, not header.stamp. The learned policy no longer uses this path — it runs in-process. | | `/tf` | robot → host | RELIABLE | 50 Hz | RSP fanout — viewers consume. | `/joy` (gamepad) and `/safety_status` are intentionally onboard-only: -both go straight into `mode_manager` (loopback) so the safety path -never depends on the tether. `/piano/key_state` is robot-local +`/joy` goes straight into the onboard `joy_teleop` node (loopback), and +`/safety_status` is telemetry consumed locally (rqt / operator dashboards), +so the operator's safety affordance never depends on the tether. +`/piano/key_state` is robot-local (MIDI driver / sim bridge → the in-process controller's `key_pressed` term), so it does not cross the tether either. ### Why this split (the three judgment calls) -- **Gamepad on the robot.** `DAMP` and `QUIT` are the operator's +- **Gamepad on the robot.** `DAMP` and `STOP` are the operator's safety affordance. Routing `/joy` over DDS across the tether means a flaky link can suppress an e-stop. Every legged-RL deployment this project mirrors (`legged_control2`, `instinct_onboard`, the earlier Humanoid-Control stack) keeps the gamepad onboard. Use a USB extension or a wireless dongle plugged into the onboard computer, not into the host laptop. -- **`mode_manager` on the robot.** It calls - `/controller_manager/switch_controller` (a service local to CM) - and consumes `/safety_status` from the per-bus hardware plugins. - Placing it onboard makes switch-controller, safety auto-DAMP, and - `/joy` consumption all loopback — zero cross-machine latency in - the safety path. +- **`joy_teleop` on the robot.** It reads `/joy` and calls + `/controller_manager/switch_controller` (a service local to CM). + Placing it onboard makes button-to-switch-controller and `/joy` + consumption all loopback — zero cross-machine latency in the + mode-switch path. - **`robot_state_publisher` on the robot.** RSP is a pure transform fanout. Putting it onboard means `/robot_description` (latched) and `/tf` originate at one address; host-side viewers subscribe @@ -335,8 +348,8 @@ deployed surface. ## Next -- [`mode_manager` source](https://github.com/Berkeley-Humanoids/humanoid_control_ros2/blob/main/humanoid_controllers/src/mode_manager.cpp) - — the FSM is ~150 lines of C++; readable in one sitting. +- [Five-mode FSM](./five_mode_fsm.md) — the modes and the flat `joy_teleop` + button map that replaced the old FSM node. - [Lite 101](../getting_started/lite_101.md) — see all of this run end-to-end against mock hardware and MuJoCo. - [Controllers reference](../reference/controllers.md) — per-controller diff --git a/docs/concepts/five_mode_fsm.md b/docs/concepts/five_mode_fsm.md index d8102dd..6fc1555 100644 --- a/docs/concepts/five_mode_fsm.md +++ b/docs/concepts/five_mode_fsm.md @@ -1,143 +1,175 @@ # Five-mode FSM -The operator-facing control surface is a **finite state machine over -controllers**. Each "mode" is exactly one `controller_interface::ControllerInterface` -plugin, and **only one mode controller is active at any time** — the -controller_manager's interface-claiming machinery enforces that -mutual exclusion mechanically. `joint_state_broadcaster` is the -always-on telemetry stream alongside whichever mode is active. +The operator-facing control surface is **five control modes, each one a +`controller_interface::ControllerInterface` plugin**, with **only one mode +controller active at any time** — the controller_manager's +interface-claiming machinery enforces that mutual exclusion mechanically. +`joint_state_broadcaster` is the always-on telemetry stream alongside +whichever mode is active. + +The modes have not changed. What changed is the **arbitration**. There is +no longer a finite state machine with ordering rules, and no dedicated +`mode_manager` node. Switching is now **flat**: the stock `joy_teleop` +node (from ROS `teleop_tools`) maps each gamepad button **directly** to a +`/controller_manager/switch_controller` call, and **any mode is reachable +from any other mode** with no gating. (This page keeps its historical +"FSM" name because the sidebar and several other pages link to it.) ![Five-mode FSM with joy bindings](/img/diagrams/concepts__five_mode_fsm__01.svg) -## Why a state machine - -Three reasons, all from operator UX: - -1. **Single-axis safety.** "Send DAMP" should land you somewhere safe - regardless of where you are now. A flat set of controllers with - ad-hoc transitions would have to enumerate `N²` "from X to DAMP" - paths; an FSM with one universal DAMP edge has one rule. -2. **Gated transitions.** STANDBY → REMOTE shouldn't fire while - STANDBY is still mid-trajectory. The state machine gives us a - natural place to express "only allowed when `is_finished:true`". -3. **Single source of truth for `/control_mode`.** Observers (rqt - dashboards, loggers, the cheat-sheet you print) read one topic to - know what the robot is doing. - -The cost is a little extra ceremony for the operator (you can't go -ZERO_TORQUE → REMOTE directly), but that ceremony is the safety -property. +## Why flat switching + +The previous design routed the gamepad through a hand-written FSM +(`mode_manager`) that enforced ordering rules — DAMP from anywhere, LOAD +only from DAMPING, START only from a finished STANDBY, QUIT only from a +resting state. That ceremony has been removed in favour of the +ros2-control-idiomatic pattern (reference: `qiayuanl/unitree_bringup`, +`config/g1/joy.yaml`): the gamepad drives `switch_controller` directly, and +every mode is designed to be **safe to enter from any state**, so no +gating is needed. + +Two properties make the flat model safe without an FSM: + +1. **Every button is absolute, not relative.** Each button activates + exactly one controller and deactivates its siblings, so "press DAMP" + lands you in DAMPING regardless of where you were — the same + single-axis safety the FSM's universal DAMP edge used to give, now a + property of the button map itself. +2. **Every mode is jump-free on entry.** STANDBY seeds its position + setpoint to the **current measured** joint positions and interpolates + from there to the target pose, so it can be entered from a running + policy with no discontinuity; DAMPING captures the live position; + ZERO_TORQUE writes zero. Because no entry is unsafe, the old "only + allowed when `is_finished`" gates are unnecessary. You can go + ZERO_TORQUE → LOCOMOTION or LOCOMOTION → STANDBY directly. + +The trade is deliberate: less mechanism (no FSM node, no ordering rules, +no `/control_mode` telemetry topic, no per-mode Trigger services), closer +to stock ros2-control, at the cost of the operator being able to command +any transition — including ones the FSM used to reject. ## The five modes | Mode | Plugin | What it writes per tick | Use it for | |---|---|---|---| -| **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 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. | +| **ZERO_TORQUE** | `humanoid_control::ZeroTorqueController` | `0` to all 5 MIT interfaces on every joint | Startup default and the STOP target (`BACK`). Also the final fault fallback (DAMPING falls back here). Robot is alive but inert — motors enabled, zero torque. | +| **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 native fallback target for any mode controller that errors. | +| **STANDBY** | `humanoid_control::StandbyController` | Interpolated `position` from the **current measured** joint positions toward a YAML target pose; constant target `K_p`/`K_d` from t=0 (**no ramp**) | Animate the arms to a ready pose. Because the setpoint starts at the measured position, it is safe to enter from any state — including a running policy — with no jump. Spawned as **three instances** — `standby_controller_a` / `_b` / `_y` (Poses A / B / Y) — the same plugin with different YAML poses. Still publishes `~/state` (`StandbyState`), but nothing gates on `is_finished` anymore. | +| **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. Entered directly at full authority (no soft-start). | | **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. | Full per-controller parameter tables live in [Reference → Controllers](../reference/controllers.md). -## Transition rules +## Button map -Encoded in `humanoid_controllers/src/mode_manager.cpp`. The operator's -gamepad intent goes through `dispatch_intent`, which gates based on the -current mode: +The mapping lives in `joy_teleop_lite.yaml` (with siblings +`joy_teleop_biped.yaml` and `joy_teleop_prime.yaml`) and is read by the +stock `joy_teleop` node. Each entry maps a button — or an `L1`/`R1` chord — +to **one** `switch_controller` call that **activates one controller and +deactivates its siblings**, with **BEST_EFFORT** strictness. There is no +ordering: any row fires from any current mode. -``` -DAMP (X) : any state → DAMPING -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() - (rejected from active-policy states — - operator must DAMP first) -``` +| Button | Mode | Activates | +|---|---|---| +| `X` | DAMP | `damping_controller` | +| `L1` + `A` | STANDBY (Pose A) | `standby_controller_a` | +| `L1` + `B` | STANDBY (Pose B) | `standby_controller_b` | +| `L1` + `Y` | STANDBY (Pose Y) | `standby_controller_y` | +| `R1` + `A` | LOCOMOTION | `rl_policy_controller` | +| `R1` + `B` | REMOTE | `remote_policy_controller` | +| `BACK` | STOP | `zero_torque_controller` | -The same transitions are also exposed as `std_srvs/Trigger` services -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 …`. +`teleop_tools` is not in robostack-jazzy, so it is built from source via +`humanoid_control.repos`. -`mode_manager` publishes `/control_mode` (`humanoid_control_msgs/ControlMode`) at -50 Hz. When an intent is rejected, the rejection reason goes into -`status_message` so operators see *why* a button didn't take effect. +`BACK` selects `zero_torque_controller`: the motors hold zero torque but +stay **enabled** — the robot is still on the bus. It is **not** a +power-down. Full CAN Disable happens only on `Ctrl+C`, when the hardware +plugin's `on_deactivate` runs. There is no button-driven shutdown and no +sequenced QUIT anymore. -To avoid a startup race where the operator's first DAMP press lands -before all controllers have finished loading, `mode_manager` polls -`list_controllers` every 25 ticks (500 ms at 50 Hz) so newly loaded -controllers become visible to `dispatch_intent` immediately. +### Driving it without a gamepad -## The auto-DAMP path (safety) - -Every hardware plugin publishes `/safety_status` (`humanoid_control_msgs/SafetyStatus`) -on a TRANSIENT_LOCAL QoS, with a bitmask of `BUS_OFF`, `RX_TIMEOUT`, -`TX_QUEUE_OVERRUN`, `MOTOR_FAULT`, `TEMPERATURE_LIMIT`, `INVALID_FRAME`. -`mode_manager` subscribes. On any non-`OK` level it requests DAMPING -with a STRICT switch: +There are no `/humanoid_control/mode/*` Trigger services anymore. +Programmatic clients call `/controller_manager/switch_controller` +directly; a headless operator uses the stock CLI: ``` -SafetyStatus.level != OK → request_mode(DAMPING) +ros2 control switch_controllers --activate standby_controller_a --deactivate damping_controller ``` -If DAMPING itself fails to activate (e.g. the bus is gone and the -command interfaces are unavailable), `mode_manager` falls further -back to ZERO_TORQUE and writes the failure reason into -`/control_mode.status_message`. The chain is intentionally -**conservative-to-most-conservative**: REMOTE → DAMPING → ZERO_TORQUE. +### Which mode is active? + +The `/control_mode` topic is **gone** — nothing publishes it. (The +`humanoid_control_msgs/ControlMode` message type still exists, because the +`humanoid_control_msgs_dds` wire bridge generates and tests it, but no node +publishes or subscribes the topic.) Anything that needs to know which mode +is active now polls `/controller_manager/list_controllers` and looks for +the active mode controller — the MuJoCo `HomePosePlugin` support band and +the Prime `erob_impedance_manager` both do this. + +## Fault handling is native fallback + +Fault response is now **purely** ros2-control's own `fallback_controllers` +mechanism. Each mode controller declares +`fallback_controllers: [damping_controller]` (and `damping_controller` +itself falls back to `zero_torque_controller`); if a controller's +`update()` returns `ERROR` — e.g. a non-finite observation — the +controller_manager deactivates it and activates its fallback. -See [Concepts → Safety pipeline](./safety_pipeline.md) for what each -flag means and which plugin sets it. +There is **no** automatic `/safety_status` → DAMP path anymore. Bus faults +(RX timeout, motor fault, overtemp) are still published on `/safety_status` +as **telemetry**, but nothing auto-DAMPs on them — the operator reacts +(`X` to DAMP, `BACK` to STOP) or a controller error trips the native +fallback. See [Concepts → Safety pipeline](./safety_pipeline.md) for the +full picture. ## 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* - -- **Not a safety system on its own.** Hardware plugins still have to - detect transport failures and publish them; `mode_manager` only - reacts to what plugins report. The plugin enforces "cannot apply - command" (returns the right `return_type`), the FSM enforces "this - transition is not allowed", and the controller_manager enforces "no - two controllers claim the same command interface". -- **Not the controller_manager.** `mode_manager` is a regular - `rclcpp::Node` executable that calls `switch_controller` as a - client. The actual interface-claiming and `update()` orchestration - is done by `controller_manager` running inside `ros2_control_node`. +The three `L1` combos select **which standby pose** to animate to; the two +`R1` combos select **which policy** to run. The two axes are orthogonal, +and nothing is gated: + +- `L1+A` / `L1+B` / `L1+Y` load `standby_controller_a` / `_b` / `_y` + (Poses A / B / Y). All three are the same `StandbyController` plugin + (source `standby_controller.cpp`, unchanged) configured with **different + YAML poses**. +- From any standby pose — or from any other mode — you can start **either** + policy: `R1+A` → LOCOMOTION (`rl_policy_controller`), `R1+B` → REMOTE + (`remote_policy_controller`). There is no pairing and no gate — `L1+A` + then `R1+B` is exactly as valid as `L1+A` then `R1+A`. + +Because switching is flat, you can now go **directly** from one standby +pose to another: `L1+B` from Pose A activates `standby_controller_b` and +deactivates `standby_controller_a` in one call, and because STANDBY seeds +to the current measured position the arms move smoothly to the new pose +with no DAMP in between. (Under the old FSM, LOAD was admissible only from +DAMPING, so a pose change required a DAMP first; that gate is gone.) + +The **policy target is still chosen at runtime**, not by a launch arg — it +is simply which `R1` button you press. + +## What joy_teleop is *not* + +- **Not a safety system on its own.** It only translates buttons into + `switch_controller` calls. Detecting transport failures is the hardware + plugins' job (they publish `/safety_status`); catching a bad command is + the controller's job (it returns `ERROR` and the controller_manager + activates its fallback); enforcing "no two controllers claim the same + command interface" is the controller_manager's job. +- **Not a custom node.** `joy_teleop` is the stock `teleop_tools` node + driven entirely by YAML — there is no bespoke FSM executable to maintain. + The old `mode_manager` node is deleted. - **Not running during calibration.** `calibrate.launch.py` passes - `enable_mode_manager:=false` so the FSM doesn't interfere with the - raw `/lite/joint_states` sampling. The operator drives controllers - directly via `switch_controllers` if they want a mode change during + `enable_joy_teleop:=false` (the launch arg was renamed from + `enable_mode_manager`) so button presses don't interfere with the raw + `/lite/joint_states` sampling. Drive controllers by hand with + `ros2 control switch_controllers` if you need a mode change during calibration. ## See also - [Reference → Controllers](../reference/controllers.md) — per-plugin parameter tables. -- [Concepts → Safety pipeline](./safety_pipeline.md) — what triggers auto-DAMP. -- [How-to → Switch controllers without the FSM](../how_to/switch_controllers_manually.md). -- [`mode_manager` source](https://github.com/Berkeley-Humanoids/humanoid_control_ros2/blob/main/humanoid_controllers/src/mode_manager.cpp). +- [Concepts → Safety pipeline](./safety_pipeline.md) — the native fallback model and `/safety_status` as telemetry. +- [How-to → Switch controllers by hand](../how_to/switch_controllers_manually.md). diff --git a/docs/concepts/index.md b/docs/concepts/index.md index 178385a..9c1c8c4 100644 --- a/docs/concepts/index.md +++ b/docs/concepts/index.md @@ -19,8 +19,8 @@ Concepts are for *understanding*, not for *doing*. |---|---| | [Architecture](./architecture.md) | The big picture: `ros2_control` as the spine, the in-process System 0 policy tier, the orthogonal axes (robot / hardware tier / task). | | [Workspace & environment](./workspace_and_environment.md) | The three-tier code model (ROS / tooling / Tier-3 DDS), why pixi + RoboStack, and the config-repo + monorepo version-control split. | -| [Five-mode FSM](./five_mode_fsm.md) | Why the operator surface is a state machine, what each mode is for, the transition rules and their gates. | +| [Five-mode FSM](./five_mode_fsm.md) | The five control modes, what each is for, and the flat `joy_teleop` → `switch_controller` arbitration that replaced the old FSM. | | [MIT command surface](./mit_command_surface.md) | Why every joint exposes 5 command interfaces, the `τ = K·err + D·erṙ + ff` convention, how it makes silicon / MuJoCo / mock interchangeable. | | [Calibration math](./calibration_math.md) | The `direction` × `homing_offset` transform, where it lives (URDF + YAML), why it lives in the plugin and not the controller. | -| [Safety pipeline](./safety_pipeline.md) | `SafetyStatus` flags, who publishes them, how `mode_manager` consumes them, the auto-DAMP fault path. | +| [Safety pipeline](./safety_pipeline.md) | `SafetyStatus` flags, who publishes them, native `fallback_controllers` on controller error, and why `/safety_status` is now operator telemetry. | | [Frozen schemas](./frozen_schemas.md) | What "the schema is frozen once a policy depends on it" means in practice: joint order, message fields, command interfaces. | diff --git a/docs/concepts/prime_hybrid_actuation.md b/docs/concepts/prime_hybrid_actuation.md index d35bea0..c3ce14d 100644 --- a/docs/concepts/prime_hybrid_actuation.md +++ b/docs/concepts/prime_hybrid_actuation.md @@ -174,7 +174,7 @@ the Sito gains come from the active controller's `stiffness`/`damping`. |---|---|---|---| | ZERO_TORQUE | 0 | 0 | True limp on both families | | DAMPING | 0 | 6 | Uniform across eRob + Sito (compliant fail-safe) | -| STANDBY | 20 | 2 | Position hold; ramps in over the entry trajectory | +| STANDBY | 20 | 2 | Position hold; setpoint seeds to the current measured position then interpolates to the pose (constant gains, no ramp) | | LOCOMOTION | 20 | 2 (eRob, fixed) | Sito follow the policy's per-tick gains | | REMOTE | 20 | 2 (eRob, fixed) | Sito follow the per-tick remote command | @@ -187,9 +187,11 @@ fixed mode impedance and track position in CSP. ## The eRob impedance manager `humanoid_bringup_prime/scripts/erob_impedance_manager.py` is the bridge between the -mode FSM and the eRob loop gains. It subscribes to `/control_mode` and, on each -transition, converts that mode's `(kp, kd)` to loop-gain registers and writes -them over the EtherLab `ethercat download` CLI. (It uses the CLI, not the +active control mode and the eRob loop gains. It polls +`/controller_manager/list_controllers` to see which mode controller is active +(the `/control_mode` topic no longer exists) and, on each change, converts that +mode's `(kp, kd)` to loop-gain registers and writes them over the EtherLab +`ethercat download` CLI. (It uses the CLI, not the in-process `ethercat_manager` SDO service, because the conda `libethercat` is version-mismatched against the running kernel master.) @@ -268,7 +270,7 @@ history with `0x1003`. Symptom-first entries are on the ## See also -- [Five-mode FSM](./five_mode_fsm.md) — the mode controllers and transition gating. +- [Five-mode FSM](./five_mode_fsm.md) — the mode controllers and flat `joy_teleop` switching. - [MIT command surface](./mit_command_surface.md) — the shared 5-interface model. - [Calibrate the Prime arms](../how_to/calibrate_prime_erob.md) — software calibration (direction + homing offset), single-source in diff --git a/docs/concepts/safety_pipeline.md b/docs/concepts/safety_pipeline.md index ae6e363..d8d1722 100644 --- a/docs/concepts/safety_pipeline.md +++ b/docs/concepts/safety_pipeline.md @@ -5,10 +5,12 @@ title: Safety pipeline # Safety pipeline `Humanoid Control`'s safety layer is **layered** rather than concentrated. -Three subsystems each enforce one piece of the contract; together -they make sure that "the robot is in DAMPING within a tick of a -fault" is the worst case, never "the robot is doing something -unexpected and we didn't notice". +Several mechanisms each enforce one piece of the contract: a controller +that produces a bad command is automatically swapped to a compliant +fallback by `ros2_control` itself, and transport-level faults are surfaced +as telemetry so the operator (or a supervising client) can react. There is +deliberately **no** single node that watches everything and forces a mode — +the model mirrors the minimal `ros2_control` reference. ![Safety pipeline: fault to DAMPING in ≤1 tick](/img/diagrams/concepts__safety_pipeline__01.svg) @@ -27,9 +29,9 @@ unexpected and we didn't notice". | `INVALID_FRAME` | A frame on the bus had the wrong comm-type code or DLC for the protocol. | The plugin publishes `humanoid_control_msgs/SafetyStatus` on `/safety_status` — -TRANSIENT_LOCAL durability so late-joining subscribers (like rqt or a -freshly-started `mode_manager`) immediately see the most recent -value. The `source` field carries the bus interface name +TRANSIENT_LOCAL durability so late-joining subscribers (rqt, an operator +dashboard) immediately see the most recent value. The `source` field +carries the bus interface name (`humanoid_devices_robstride/can0`, etc.), so an operator can tell which bus flagged. @@ -106,23 +108,27 @@ reserved for cases where DAMPING itself can't be applied (state interface unavailable, hardware plugin dead). It writes 0 to every interface — no risk of unintended motion regardless of state. -## Layer 4 — `mode_manager` reacts to `/safety_status` +## Layer 4 — `/safety_status` is operator telemetry -`mode_manager` subscribes to `/safety_status`. On any non-OK level: +There is deliberately **no** automatic `/safety_status` → DAMP path. The +old `mode_manager` node subscribed to `/safety_status` and forced a STRICT +switch to DAMPING on any non-OK level; that node is deleted, and nothing +replaced its auto-DAMP behavior. -``` -SafetyStatus.level != OK → request_mode(DAMPING) -``` - -The transition is `STRICT` — if it fails because the command -interfaces are unavailable, `mode_manager` requests ZERO_TORQUE -instead and writes the failure reason into `/control_mode.status_message`. +Instead, `/safety_status` is **telemetry**. Bus and motor faults +(`RX_TIMEOUT`, `MOTOR_FAULT`, `TEMPERATURE_LIMIT`, `BUS_OFF`, and the rest) +are published for a human operator — or a supervising client — to see and +react to: press `X` to DAMP or `BACK` to STOP. What *is* automatic is +Layer 3: a controller that turns a fault into a bad command (a non-finite +observation, say) returns `ERROR`, and the controller_manager activates its +`fallback_controllers` with no operator in the loop. -This is **belt-and-suspenders** on top of Layer 3: even if a -controller failed to detect its own bad command, the plugin's safety -publish path triggers the FSM-level fallback. And even if the plugin -missed an issue, the controller's own validation triggers the -controller-manager-level fallback. +This is a deliberate reduction to the reference's minimal model. The two +automatic guarantees that remain — a controller catching its own bad +command (Layer 2) and the controller_manager swapping in the fallback +(Layer 3) — cover the case that actually needs sub-tick reaction. Reacting +to a *reported* bus fault is left to the operator, who can always DAMP or +STOP from the gamepad. ## Layer 5 — RT update() discipline @@ -154,11 +160,11 @@ its slot, etc. | 1. Transport-level | hardware plugin | Bus / motor faults → `SafetyStatus.flags` | | 2. Command-validity | controller | NaN / size mismatch / stale → `return_type::ERROR` | | 3. Controller-manager fallback | controller_manager | `ERROR` → activate the controller's `fallback_controllers` | -| 4. FSM auto-DAMP | `mode_manager` | `SafetyStatus.level != OK` → `request_mode(DAMPING)` | +| 4. Fault telemetry | `/safety_status` subscribers | Bus / motor faults reported for the operator (or a client) to react — no automatic DAMP | | 5. RT discipline | every controller / plugin | (preventative — keeps the other layers responsive) | ## See also - [How-to → Recover from a fault](../how_to/recover_from_fault.md) — operator-side runbook. - [Reference → Messages → SafetyStatus](../reference/messages.md) — full bit table. -- [Five-mode FSM](./five_mode_fsm.md) — the FSM side of Layer 4. +- [Five-mode FSM](./five_mode_fsm.md) — the modes and flat `joy_teleop` switching. diff --git a/docs/extras/prime_bringup.md b/docs/extras/prime_bringup.md index 18401e5..3536f4a 100644 --- a/docs/extras/prime_bringup.md +++ b/docs/extras/prime_bringup.md @@ -111,12 +111,26 @@ That defaults to `backends:=all` (both buses) and bakes in | Arg | Default | Purpose | |---|---|---| | `backends` | `all` | `all` (both buses) · `ec` (eRob/EtherCAT only) · `can` (Sito/CAN only). The single-bus modes spawn only `joint_state_broadcaster` — for calibration / diagnostics. | -| `enable_mode_manager` | `true` | Launch the `mode_manager` FSM. | -| `enable_erob_impedance` | `true` | Spawn `erob_impedance_manager` (eRob loop gains per `/control_mode`). Pass `false` to isolate startup races or fall back to factory-gain (stiff) eRob. | +| `enable_joy_teleop` | `true` | Launch the `joy_teleop` node that maps gamepad buttons directly to `controller_manager` switches (flat, no FSM). Pass `false` for headless / CI, or when driving mode switches by hand. | +| `enable_erob_impedance` | `true` | Spawn `erob_impedance_manager` (eRob loop gains per active mode). Pass `false` to isolate startup races or fall back to factory-gain (stiff) eRob. | | `enable_gamepad` | `true` | Spawn `joy_node`. Pass `enable_gamepad:=false` for headless / CI bringups. | | `calibration_file` | bundled `prime_calibration.yaml` | Per-joint eRob software calibration, folded at launch. | | `hardware_config` | bundled `prime_hardware.yaml` | `buses:` + `joints.all_joints`. | +:::note[Gamepad buttons (Prime)] +`joy_teleop` maps each button directly to a `controller_manager` switch — +flat, with no state machine and no ordering, so any mode is reachable +from any other in a single press: **X** activates DAMP +(`damping_controller`); **L1+A** / **L1+B** activate STANDBY +(`standby_controller_a` / `_b`); **R1+A** activates LOCOMOTION +(`rl_policy_controller`); **R1+B** activates REMOTE +(`remote_policy_controller`); **BACK** activates STOP +(`zero_torque_controller`). Each press activates that one controller and +deactivates its siblings. Headless, the same switches are plain +`ros2 control switch_controllers --activate --deactivate ` +calls. +::: + :::note[eRob SYNC0 and `control_frequency`] The eRob distributed-clock SYNC0 cycle (`control_frequency`) **must** equal the `controller_manager` `update_rate` (50 Hz). `ethercat_driver` sends PDOs and @@ -145,7 +159,8 @@ reads `update_rate` from the same controllers YAML the CM loads and passes it as ```bash ros2 topic hz /prime/joint_states # 14 joints at ~50 Hz ros2 control list_controllers # joint_state_broadcaster + zero_torque active; rest inactive -ros2 topic echo /control_mode # mode_manager state, once a mode is requested + # the active mode is whichever controller is `active` here + ``` ## Single-bus / diagnostic modes @@ -166,8 +181,8 @@ EtherCAT activation and DC faults cannot stall the Sito command/feedback path. eRob impedance lives in the drive's internal CSP loop as CoE objects that are SDO-only (not PDO-mappable, so not a per-tick command interface). -`erob_impedance_manager` watches `/control_mode` and writes each eRob's loop -gains (`kp`/`kd` → `0x2382`/`0x2381`, gate `0x2383`) for the active mode via the +`erob_impedance_manager` tracks which mode controller is active and writes +each eRob's loop gains (`kp`/`kd` → `0x2382`/`0x2381`, gate `0x2383`) for the active mode via the EtherLab `ethercat` CLI. The mode controllers keep commanding eRob *position* (CSP); these gains decide whether a joint holds, damps, or goes limp. Pass `enable_erob_impedance:=false` to drop this path while isolating startup races. diff --git a/docs/getting_started/intro.md b/docs/getting_started/intro.md index 56a95c0..8180307 100644 --- a/docs/getting_started/intro.md +++ b/docs/getting_started/intro.md @@ -111,7 +111,7 @@ list; the most influential are: - [Hardware specifications](../reference/hardware_specs.md) — joint counts, effort limits, transport details for Lite and Prime. -- [Architecture](../concepts/architecture.md) — ros2_control flow, the 5-mode - FSM, and the in-process System 0 policy tier. +- [Architecture](../concepts/architecture.md) — ros2_control flow, the five + control modes (flat joy_teleop switching), and the in-process System 0 policy tier. - [Installation](./installation.md) — install the prebuilt packages from the `berkeley-humanoids` channel in ~2 minutes, or build from source. \ No newline at end of file diff --git a/docs/getting_started/lite_101.md b/docs/getting_started/lite_101.md index 10c1384..5441085 100644 --- a/docs/getting_started/lite_101.md +++ b/docs/getting_started/lite_101.md @@ -3,7 +3,7 @@ A 20-minute hands-on lesson. You'll go from "URDF loaded in RViz" to "the full controller stack running in MuJoCo, with a controller switch under your fingers". By the end you'll recognise the four moving pieces of the -project — the URDF, the controller_manager, the mode-FSM controllers, and +project — the URDF, the controller_manager, the mode controllers, and the simulation backend — and be ready to read the rest of the docs with that mental model in place. @@ -59,8 +59,12 @@ Close the launch (`Ctrl+C` in the terminal) before moving on. ## Step 2 — Bring up MuJoCo Now the full control plane against MuJoCo physics: `controller_manager` -hosted inside `mujoco_sim`, all five mode-FSM controllers loaded, the -`mode_manager` orchestrator running. Nothing on the CAN bus — the +hosted inside `mujoco_sim`, all five mode controllers loaded, with +`zero_torque_controller` active as the safe default. Switching between +modes is flat — the stock `joy_teleop` node maps gamepad buttons +directly to `controller_manager` switches (there is no FSM +orchestrator); with no gamepad here, we'll make one such switch by hand +below. Nothing on the CAN bus — the `MujocoSystem` hardware plugin applies the same MIT-mode torque `τ = K·(q_cmd − q) + D·(q̇_cmd − q̇) + effort` to `qfrc_applied` that the Robstride firmware computes on silicon. @@ -93,6 +97,7 @@ zero_torque_controller humanoid_control/ZeroTorqueController damping_controller humanoid_control/DampingController inactive standby_controller_a humanoid_control/StandbyController inactive standby_controller_b humanoid_control/StandbyController inactive +standby_controller_y humanoid_control/StandbyController inactive rl_policy_controller humanoid_control/RLPolicyController inactive remote_policy_controller humanoid_control/RemotePolicyController inactive ``` @@ -114,8 +119,7 @@ in a single service call. ### Watch the topics ```sh -ros2 topic list | grep -E "joint_states|control_mode|standby|safety" -# /control_mode +ros2 topic list | grep -E "joint_states|standby|safety" # /lite/joint_states # /safety_status # /standby_controller_a/state @@ -131,15 +135,11 @@ ros2 topic hz /lite/joint_states # average rate: 200.000 ``` -`mode_manager` publishes `/control_mode` (the current FSM state) at -50 Hz: - -```sh -ros2 topic echo --once /control_mode -# mode: 0 # ZERO_TORQUE -# controller_name: zero_torque_controller -# status_message: '' -``` +There is no `/control_mode` topic — the active mode isn't published; +it's simply whichever controller `ros2 control list_controllers` reports +as `active` (Step 2 above). `/safety_status` is published as telemetry: +an operator (or a monitor) watches it, but it does **not** switch modes +for you — a fault is caught by the controllers' native fallback instead. :::tip["See it move"] Two interchangeable live visualizers ride on `/lite/joint_states` + @@ -167,14 +167,13 @@ ship in the pixi env. See [Concepts → Architecture → Deployment topology](../concepts/architecture.md#deployment-topology). ::: -## Step 3 — Trigger a mode transition +## Step 3 — Switch modes by hand -The whole project is built around mode transitions: the operator (or a -fault) requests an *intent*, `mode_manager` resolves it to a controller -switch, and the controller_manager swaps the active controller. For -this lesson we'll just call the controller_manager service directly to -simulate what `mode_manager` would have done in response to the gamepad -`X` (DAMP) intent. +The whole project switches modes by switching controllers — flat and +direct, with no ordering rules. Pressing the gamepad **X** button makes +`joy_teleop` call the controller_manager to activate `damping_controller` +and deactivate the rest. This lesson has no gamepad, so we make that +same call by hand: ```sh ros2 control switch_controllers \ @@ -195,8 +194,8 @@ With stiffness `K = 0` there's no position-restoring force. With damping `D` nonzero the joint resists velocity. The result: the robot is **soft under gravity** (push the arm and it moves) but **damped** (it doesn't oscillate). This is the state you transition into before powering down, -before swapping a tool, or after any non-OK `SafetyStatus`. See -[Concepts → Five-mode FSM](../concepts/five_mode_fsm.md). +before swapping a tool, or that the controllers' native fault fallback +drops into. See [Concepts → Five control modes](../concepts/five_mode_fsm.md). ::: If gravity is on (it is by default in our MJCF) and you switched to @@ -228,7 +227,7 @@ Four moving pieces, all worth recognising for the rest of the docs: |---|---| | **URDF** (`lite_description`) | Kinematic + dynamic description. Same file across mock / sim / real — the `` `` is the only difference. | | **`controller_manager`** | Hosts the hardware plugin + every controller. The "tick loop" of the system. Update rate 50 Hz here. | -| **Mode-FSM controllers** (`humanoid_controllers`) | Five plugins, one active at a time. `zero_torque` (safe default), `damping` (compliant fail-safe), `standby` (interpolate to a pose), `rl_policy` (in-process ONNX — the System 0 learned-policy tier), `remote_policy` (System 1/2 external-command ingress for non-real-time `MITCommand` sources). | +| **Mode controllers** (`humanoid_controllers`) | Five plugins, one active at a time. `zero_torque` (safe default), `damping` (compliant fail-safe), `standby` (interpolate to a pose), `rl_policy` (in-process ONNX — the System 0 learned-policy tier), `remote_policy` (System 1/2 external-command ingress for non-real-time `MITCommand` sources). | | **MuJoCo / Robstride** (the simulation / real backend) | Where the MIT torque actually gets applied. Same five command interfaces in both. | ## Next @@ -237,7 +236,7 @@ Where to go from here, depending on what you want: - **I want to make the arm move on real hardware.** → [How-to → First real-hardware bringup](../how_to/first_real_bringup.md). -- **I want a hands-on look at the mode FSM, with the actual gamepad.** +- **I want a hands-on look at all five modes, with the actual gamepad.** → [Tutorials → MuJoCo + FSM walkthrough](../tutorials/mujoco_fsm_walk.md). - **I want to run a trained policy.** → [Tutorials → Run a tracking policy](../tutorials/tracking_policy.md). diff --git a/docs/how_to/calibrate_zero_pose.md b/docs/how_to/calibrate_zero_pose.md index 6693f54..f856907 100644 --- a/docs/how_to/calibrate_zero_pose.md +++ b/docs/how_to/calibrate_zero_pose.md @@ -43,7 +43,7 @@ ros2 launch humanoid_bringup_lite calibrate.launch.py ``` `calibrate.launch.py` includes `real.launch.py` with three overrides: -`calibration_file:='' enable_mode_manager:='false' enable_gamepad:='false'`. +`calibration_file:='' enable_joy_teleop:='false' enable_gamepad:='false'`. The empty calibration means `/lite/joint_states` carries the `direction × raw_motor_pos` frame — exactly what the homing-offset formula needs. `zero_torque_controller` stays active, keeping every diff --git a/docs/how_to/first_real_bringup.md b/docs/how_to/first_real_bringup.md index fc6db3f..c44a106 100644 --- a/docs/how_to/first_real_bringup.md +++ b/docs/how_to/first_real_bringup.md @@ -72,19 +72,22 @@ ros2 launch humanoid_bringup_lite real.launch.py ``` Default args: `mode:=arms hardware_config:= -calibration_file:= enable_mode_manager:=true +calibration_file:= enable_joy_teleop:=true enable_gamepad:=true joy_dev:=/dev/input/js0`. The `hardware_config` YAML provides the per-machine bus + joint mapping (left arm on `can0` with ids 11..17, right arm on `can1` with ids 21..27 on the development robot). The default `enable_gamepad:=true` hard-fails if the resolved `joy_dev` path is missing; the error message lists any other `/dev/input/js*` devices it can see so you can override with -`joy_dev:=/dev/input/jsN`. Pass `enable_gamepad:=false` on a -keyboardless lab box to use the `/humanoid_control/mode/*` services instead. +`joy_dev:=/dev/input/jsN`. Pass `enable_gamepad:=false` (or +`enable_joy_teleop:=false`) on a keyboardless lab box and switch +controllers with `ros2 control switch_controllers` directly instead +(see [Switch controllers manually](./switch_controllers_manually.md)). `real.launch.py` boots the **onboard-computer side** of the tethered -deployment split: hardware plugins, FSM controllers, `mode_manager`, -gamepad. Visualisers (`ros2 launch humanoid_bringup_lite viz.launch.py`) +deployment split: hardware plugins, the control-mode controllers, the +`joy_teleop` button mapper, and the gamepad driver. Visualisers +(`ros2 launch humanoid_bringup_lite viz.launch.py`) and policy runners (`ros2 launch humanoid_control_policy lite_policy.launch.py …`) live on the operator workstation — matching `ROS_DOMAIN_ID` is enough for them to see each other. See @@ -153,9 +156,10 @@ End in a known-safe state: Ctrl+C ``` -`mode_manager` (if enabled) and every controller's `on_deactivate` -runs, then `RobstrideSystem::on_deactivate` sends `Disable` to every -motor. The bus goes quiet within a tick; `candump can0` will confirm. +The `joy_teleop` node (if enabled) and every controller's +`on_deactivate` run, then `RobstrideSystem::on_deactivate` sends +`Disable` to every motor. The bus goes quiet within a tick; +`candump can0` will confirm. ## Common boot-time failures @@ -175,5 +179,5 @@ Wider diagnostics live in [Troubleshooting](../reference/troubleshooting.md). look offset. - [Switch controllers manually](./switch_controllers_manually.md) to exercise DAMPING / STANDBY without the gamepad. -- [MuJoCo + full FSM walkthrough](../tutorials/mujoco_fsm_walk.md) to - walk the same flow with the FSM in the loop. +- [MuJoCo mode walkthrough](../tutorials/mujoco_fsm_walk.md) to + walk the same control-mode flow in simulation. diff --git a/docs/how_to/index.md b/docs/how_to/index.md index 7c8250a..27bf19d 100644 --- a/docs/how_to/index.md +++ b/docs/how_to/index.md @@ -26,14 +26,14 @@ and now have a concrete task in mind. How-tos skip the pedagogy that |---|---| | [Drive a single joint with mit_slider_gui](./mit_slider_gui.md) | You want to inject a position/velocity/effort/K/D combination by hand and see the actuator follow. Good for sanity-checking a freshly calibrated joint. | | [Live-visualize the robot (rerun / viser)](./live_viz.md) | You want the live kinematic chain on screen while the robot runs — for tuning, demos, or watching a policy from another machine. | -| [Record experiments with rosbag (MCAP)](./record_experiments.md) | You want a replayable trace — joints, FSM transitions, safety status, policy I/O — for offline analysis, regression baselines, or Foxglove Studio. | +| [Record experiments with rosbag (MCAP)](./record_experiments.md) | You want a replayable trace — joints, controller switches, safety status, policy I/O — for offline analysis, regression baselines, or Foxglove Studio. | ## Diagnosis & recovery | Guide | Use it when | |---|---| | [Diagnose ENOBUFS / TX drops](./diagnose_enobufs.md) | The plugin spams `Network is down` or you see `tx_failed` counters climb. Usually motor power is off or `txqueuelen` is too small. | -| [Recover from a fault](./recover_from_fault.md) | `/safety_status` reported `BUS_OFF`, `RX_TIMEOUT`, or `TEMPERATURE_LIMIT` and `mode_manager` auto-dropped to DAMPING. | +| [Recover from a fault](./recover_from_fault.md) | `/safety_status` reported `BUS_OFF`, `RX_TIMEOUT`, or `TEMPERATURE_LIMIT`, or a control-mode controller errored into its `damping` fallback. | ## Extension diff --git a/docs/how_to/mit_slider_gui.md b/docs/how_to/mit_slider_gui.md index 6188ee8..3c3879b 100644 --- a/docs/how_to/mit_slider_gui.md +++ b/docs/how_to/mit_slider_gui.md @@ -116,7 +116,8 @@ to the motor. The default config tests one joint. To drive a single arm joint of the full Lite without disturbing the others, the cleanest path is: -1. Launch the normal Lite bringup with `enable_mode_manager:=false`. +1. Launch the normal Lite bringup with `enable_joy_teleop:=false` so + the gamepad layer can't switch controllers out from under you. 2. Load a forward command controller separately (inside `pixi shell`): ```bash @@ -127,7 +128,7 @@ ros2 control load_controller \ ``` 3. Switch the target joint's command claim from `zero_torque_controller` - to `forward_one_joint` — but the FSM controllers claim *all* the + to `forward_one_joint` — but each mode controller claims *all* the joints, so this requires deactivating `zero_torque_controller` on the full robot. In practice it's easier to load a `forward_command_controller` that claims the target joint AND diff --git a/docs/how_to/promote_python_to_cpp.md b/docs/how_to/promote_python_to_cpp.md index fe1b1f5..d91c7db 100644 --- a/docs/how_to/promote_python_to_cpp.md +++ b/docs/how_to/promote_python_to_cpp.md @@ -5,8 +5,8 @@ title: Run and extend an in-process policy # Run and extend an in-process policy Every learned policy on this stack — tracking, piano, locomotion — -runs **in-process** in the C++ `humanoid_control::RLPolicyController` (FSM -`LOCOMOTION` mode). This is the **System 0** real-time layer: inference +runs **in-process** in the C++ `humanoid_control::RLPolicyController` (the +`LOCOMOTION` control mode). This is the **System 0** real-time layer: inference happens inside the `ros2_control` RT `update()`, with no allocation, no blocking, and no separate process that could stall. Policies differ only by the `.onnx` checkpoint and the `.mcap` motion bag they load; the ONNX @@ -43,8 +43,10 @@ the RT loop only ever integer-indexes preloaded data. scale, `observation_names`, `body_names`, `policy_dt`, `task_type`. 2. **Inactive spawn.** The launch then spawns `rl_policy_controller` *inactive* into the running controller_manager with that overlay. -3. **FSM activation.** The operator's `START_LOCOMOTION` intent - (`/humanoid_control/mode/start_locomotion`, R1+A on the gamepad) activates it. +3. **Activation.** The operator activates it directly — `R1+A` on the + gamepad (the `joy_teleop` button mapper calls `switch_controller`), or + `ros2 control switch_controllers --activate rl_policy_controller` headless. + Any mode can switch to it directly; there is no ordering to walk. 4. **Per-tick, in-process.** Once active, each RT tick the controller packs the observation (`ObservationManager`), runs ONNX inference (`OnnxPolicy`), reads the motion reference from the preloaded `.mcap` @@ -97,15 +99,21 @@ For the piano task, use the equivalent `pianist_policy/launch/piano_policy.launch.py` from `pianist_ros2`; it runs the same prepare→inactive-spawn flow with the piano metadata. -Finally, drive the FSM to activate (third terminal, inside `pixi shell`): +Finally, activate the controller (third terminal, inside `pixi shell`). +Switching is flat — you can enter `rl_policy_controller` directly from +whatever is active, with no ordering to walk and no `is_finished` gate. +It comes up at full authority immediately, so make sure the workspace is +clear: ```bash -ros2 service call /humanoid_control/mode/damp std_srvs/srv/Trigger -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 +ros2 control switch_controllers \ + --activate rl_policy_controller \ + --deactivate zero_torque_controller ``` +(Deactivate whichever controller is currently active.) On the gamepad +this is the single `R1+A` press. + :::tip[onnxruntime is opt-in] `OnnxPolicy` (onnxruntime C++) is only compiled in when onnxruntime is found at build time — the conda `onnxruntime-cpp` package, already pinned @@ -174,7 +182,7 @@ If you have a slow, deliberative, non-real-time source — gravity compensation today (`Lite-Gravity-Compensation`), VLA / manipulation later — that belongs in the **System 1/2 external-command ingress**, not here. Such a source runs out-of-process and publishes `MITCommand` over -DDS to `humanoid_control::RemotePolicyController` (FSM `REMOTE` mode), which validates +DDS to `humanoid_control::RemotePolicyController` (the `REMOTE` control mode), which validates joint order, gates on arrival-time staleness, and falls back to damping. That controller is *not* used by learned policies. See [Switch controllers without the FSM](./switch_controllers_manually.md) diff --git a/docs/how_to/record_experiments.md b/docs/how_to/record_experiments.md index 356112a..9860aab 100644 --- a/docs/how_to/record_experiments.md +++ b/docs/how_to/record_experiments.md @@ -4,8 +4,8 @@ title: Record experiments with rosbag (MCAP) # Record experiments with rosbag (MCAP) -Capture a complete telemetry trace of any bringup — joints, FSM -transitions, safety status, policy I/O — to a [MCAP](https://mcap.dev) +Capture a complete telemetry trace of any bringup — joints, controller +switches, safety status, policy I/O — to a [MCAP](https://mcap.dev) file. MCAP is the cross-language replacement for the legacy `sqlite3` rosbag backend; it's the default in ROS 2 Jazzy and what Foxglove Studio / `mcap-cli` consume directly. @@ -37,7 +37,6 @@ explicitly: ```bash ros2 bag record -s mcap -o tuning_$(date +%Y%m%d_%H%M%S) \ /lite/joint_states \ - /control_mode \ /standby_controller_a/state \ /standby_controller_b/state \ /safety_status \ @@ -45,6 +44,13 @@ ros2 bag record -s mcap -o tuning_$(date +%Y%m%d_%H%M%S) \ /tf /tf_static ``` +There is no `/control_mode` topic to record anymore. "Which controller +is active" is a `/controller_manager/list_controllers` service query +(`ros2 control list_controllers`), not a recordable topic — capture the +per-controller state topics you care about (e.g. +`/standby_controller_a/state`) instead. `/joy` still records the raw +gamepad input that drove the switches. + For a System 1/2 ingress session (gravity-comp, VLA), also add the command topic `/remote_policy_controller/command` (`humanoid_control_msgs/MITCommand`). For piano runs, capture the live key state `/piano/key_state` @@ -133,7 +139,7 @@ is the deployment-side evaluation path. ## See also - [Frozen schemas](../concepts/frozen_schemas.md) — why - `/control_mode` / `/safety_status` / etc. fields don't shift + `/lite/joint_states` / `/safety_status` / etc. fields don't shift between releases (so old bags stay readable). - [MCAP spec & tooling](https://mcap.dev/) — file format, `mcap-cli` reference, ecosystem. diff --git a/docs/how_to/recover_from_fault.md b/docs/how_to/recover_from_fault.md index af5ab45..8142fe7 100644 --- a/docs/how_to/recover_from_fault.md +++ b/docs/how_to/recover_from_fault.md @@ -4,9 +4,27 @@ title: Recover from a fault # Recover from a fault -`/safety_status` reports a non-OK level and `mode_manager` auto-DAMPs. -What to do depends on which flag tripped. This page is the operator's -runbook. +`/safety_status` reports a non-OK level. Nothing auto-DAMPs on it +anymore — `/safety_status` is **telemetry**, and reacting to it is the +operator's job. What to do depends on which flag tripped. This page is +the operator's runbook. + +## How recovery works now + +There are two independent mechanisms, and neither is the old +`mode_manager` auto-DAMP node (that node is gone): + +- **Native controller fallback (automatic).** Each mode controller + declares `fallback_controllers: [damping_controller]`. If a + controller's `update()` returns ERROR, the controller_manager + deactivates it and activates its fallback — `damping_controller`, + which itself falls back to `zero_torque_controller`. This happens at + the controller_manager layer and is independent of `/safety_status`. +- **Operator reaction to `/safety_status` (manual).** A hardware bus + fault is published on `/safety_status` as telemetry only — it does + **not** switch modes for you. When you see a non-OK level, switch to + STOP (gamepad `BACK` → `zero_torque_controller`) or DAMP (gamepad `X` + → `damping_controller`), or use `ros2 control switch_controllers`. ## Read the status first @@ -133,29 +151,31 @@ EMI glitches; recurring means another device is sharing the bus. ## After fixing — what to do -`mode_manager` doesn't auto-recover from DAMPING. Even after the -fault clears, the FSM stays in DAMPING until the operator manually -walks back up: +There is no auto-recovery: after a fault you are sitting wherever you +(or a controller fallback) last switched to — typically `zero_torque` +(STOP) or `damping` (DAMP). Even once `/safety_status` returns to OK, +nothing walks the robot back up for you. ```bash -# /safety_status is now OK (level=0) -# But mode_manager is still in DAMPING. -# Walk back via the gamepad: +# /safety_status is now OK (level=0), robot is in DAMPING or STOP. +# Bring it back up when you're satisfied it's steady. +# Via the gamepad: # L1+A → STANDBY (Pose A) # L1+B → STANDBY (Pose B) +# L1+Y → STANDBY (Pose Y) # R1+A → LOCOMOTION # R1+B → REMOTE -# Or via the std_srvs/Trigger services: -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 +# Or from the CLI: +ros2 control switch_controllers \ + --activate standby_controller_a \ + --deactivate damping_controller ``` -Or via `ros2 control switch_controllers` directly. - -The reason for not auto-recovering: a fault that just cleared may -re-occur. The operator should observe the robot in DAMPING for at -least a few seconds, confirm it's actually steady, then proceed. +Switching is flat, so you can go straight to whichever mode you want +(see [Switch controllers manually](./switch_controllers_manually.md)). +Still, observe the robot in DAMPING/STOP for a few seconds first: a +fault that just cleared may re-occur, and you want to confirm it's +actually steady before commanding motion. ## Soft-restart in code diff --git a/docs/how_to/switch_controllers_manually.md b/docs/how_to/switch_controllers_manually.md index f5113a0..74c28d5 100644 --- a/docs/how_to/switch_controllers_manually.md +++ b/docs/how_to/switch_controllers_manually.md @@ -4,47 +4,55 @@ title: Switch controllers without the FSM # Switch controllers without the FSM -`mode_manager` is the production path for changing controllers, but -sometimes you want **raw control** — debugging, calibration, scripted -tests, or just verifying the underlying controller_manager service. -This how-to walks the mode FSM via direct `ros2 control` calls. +There is no mode-arbitration node anymore — a control-mode change is +just a `switch_controller` call on the `controller_manager`. The +gamepad's `joy_teleop` node maps buttons directly to those calls; this +how-to is the CLI equivalent, and the primary path on a dev or headless +host without a gamepad. :::note[No gamepad? This is your mode-switch path.] -Humanoid Control ships **no keyboard control** for the FSM — `mode_manager` reacts to a -gamepad (`/joy`) only. On a dev or headless host without one, the -`ros2 control` calls on this page **are** the supported way to drive modes -by hand. (There used to be a planned `termios` keyboard reader; it was -dropped in favour of this CLI path.) +Humanoid Control ships **no keyboard control** — the `joy_teleop` node +reacts to a gamepad (`/joy`) only. On a dev or headless host without one, +the `ros2 control` calls on this page **are** the supported way to drive +modes by hand. ::: -## Why bypass mode_manager +## Why switch by hand -| Use case | Why FSM is in the way | +| Use case | Why the CLI | |---|---| -| Calibration | The FSM auto-DAMPs on safety events; sometimes you want to manually drive state through faults. | -| Verifying a new controller plugin | You want to load + activate it directly, not register it as an FSM mode. | -| Recording sysid traces | The FSM transitions add unmeasurable delay; manual switches are more reproducible. | -| Debugging a controller's `on_activate` | Direct control + log inspection without the FSM's `request_mode` retry chatter. | +| Headless / dev host | No gamepad attached, so `/joy` never arrives — the CLI is the only mode-switch input. | +| Verifying a new controller plugin | Load + activate it directly by name, no gamepad binding required. | +| Recording sysid traces | Scripted switches are exactly reproducible run to run. | +| Debugging a controller's `on_activate` | Direct control + log inspection, one switch at a time. | -The FSM doesn't enforce its rules at the controller_manager layer — -the controller_manager just sees `switch_controller` service calls. -So you can call them directly without any FSM in the loop. +The gamepad and the CLI hit the **same** `switch_controller` service. +`joy_teleop` only issues a switch when a button is pressed and keeps no +state of its own, so background CLI calls never fight it. -## Disable the FSM in the launch +## Turn off the gamepad layer (optional) -Easiest: pass `enable_mode_manager:=false` so `mode_manager` isn't -spawned at all: +You don't need to disable anything — CLI switches work whether or not +`joy_teleop` is running. If you'd rather drop the gamepad layer +entirely (no button bindings on `/joy`), pass `enable_joy_teleop:=false`: ```bash -ros2 launch humanoid_bringup_lite real.launch.py enable_mode_manager:=false +ros2 launch humanoid_bringup_lite real.launch.py enable_joy_teleop:=false ``` -Now `zero_torque_controller` is active (the spawner set it active), -and the four other controllers are loaded as inactive. No FSM watches -`/safety_status`, no `/joy` is required. The operator drives every -transition. +Either way `zero_torque_controller` comes up active (the spawner sets it +active) and the other controllers load inactive. Nothing watches +`/safety_status` to switch modes for you — the operator drives every +switch. -## The four basic transitions +## Common switches + +Switching is **flat**: any controller can be activated from any state, +and each switch just deactivates the current mode and activates the new +one (`--strict` optional; the gamepad uses best-effort). The examples +below are ordered for a typical bring-up, but you can go directly +between any two — e.g. `zero_torque_controller` straight to +`rl_policy_controller`. The commands below are interactive `ros2 control` / `ros2 topic` calls — open a second terminal and `pixi shell` into the workspace so @@ -69,9 +77,10 @@ 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: +STANDBY has three poses, each a separately spawned instance of the same +plugin: `standby_controller_a` (Pose A), `standby_controller_b` +(Pose B), and `standby_controller_y` (Pose Y) — the gamepad's `L1+A`, +`L1+B`, and `L1+Y`. Activate whichever pose you want: ```bash ros2 control switch_controllers \ @@ -79,12 +88,16 @@ ros2 control switch_controllers \ --activate standby_controller_a ``` -Use `--activate standby_controller_b` instead for Pose B. +Use `--activate standby_controller_b` (or `_y`) instead for the other +poses. -**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. +**The motors will move.** Standby holds the target `K_p` / `K_d` from +t=0 — there is **no gain ramp** — and seeds its setpoint to the +**current measured** joint positions, then interpolates that setpoint to +the pose's target. Because it starts from where the joints already are, +it's safe to enter from any state with no jump; the arms still travel to +the target pose over ~4 seconds, so support them or keep the workspace +clear. Watch the state topic for the pose you activated (one per instance) for `is_finished: true`: @@ -102,7 +115,8 @@ ros2 control switch_controllers \ ``` (Deactivate whichever standby instance is active — -`standby_controller_a` or `standby_controller_b`.) +`standby_controller_a`, `_b`, or `_y`. You can also enter REMOTE +directly from any other mode.) `remote_policy_controller` (`humanoid_control/RemotePolicyController`) is the **System 1/2 external-command ingress**: it immediately starts looking @@ -142,12 +156,13 @@ shutdown. ```bash # Which controllers are loaded, and which are active? ros2 control list_controllers -# Expected after first transition: +# Expected after the first switch: # damping_controller humanoid_control/DampingController active # zero_torque_controller humanoid_control/ZeroTorqueController inactive # joint_state_broadcaster joint_state_broadcaster/... active # standby_controller_a humanoid_control/StandbyController inactive # standby_controller_b humanoid_control/StandbyController inactive +# standby_controller_y humanoid_control/StandbyController inactive # remote_policy_controller humanoid_control/RemotePolicyController inactive # What hardware components are up? @@ -176,23 +191,29 @@ ros2 control switch_controllers \ --strict ``` -## What's the FSM doing differently? +## Gamepad vs. CLI -| Operation | FSM (`mode_manager`) | Raw `ros2 control` | -|---|---|---| -| Gate `LOAD` on current state | Yes — rejects from non-DAMPING | No — happy to go ZERO_TORQUE → STANDBY directly | -| Gate `START_*` on `is_finished` | Yes | No | -| Auto-DAMP on `/safety_status` | Yes | No — you have to script it | -| Publish `/control_mode` | Yes | No — `list_controllers` is your only state view | -| React to `/joy` | Yes | No | +Both paths call the same `switch_controller` service; neither gates or +orders the switch. -When you're done debugging, **re-enable `mode_manager`** before -operating in production. Its gates and the auto-DAMP path are real -safety properties; the convenience of bypassing them is for the -operator who's watching the robot, not for unattended use. +| | Gamepad (`joy_teleop`) | CLI (`ros2 control`) | +|---|---|---| +| Trigger | Button press | You type the command | +| Backend | `switch_controller`, best-effort | `switch_controller` (add `--strict` if you want) | +| Ordering / gating | None — any button, any state | None — any switch, any state | +| Which mode is active | `ros2 control list_controllers` | same | + +There are **no FSM gates and no auto-DAMP** anymore. Safety comes from +two places: the operator (switch to STOP with `BACK` / `zero_torque` or +DAMP with `X` / `damping`, on the gamepad or the CLI), and each mode +controller's native `fallback_controllers` — a controller whose +`update()` errors is deactivated by the controller_manager, which then +activates its `damping_controller` fallback automatically. See +[Recover from a fault](./recover_from_fault.md). ## See also -- [Five-mode FSM](../concepts/five_mode_fsm.md) +- [The five control modes](../concepts/five_mode_fsm.md) - [First real-hardware bringup](./first_real_bringup.md) +- [Recover from a fault](./recover_from_fault.md) - [Reference → Controllers](../reference/controllers.md) 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 4f69f60..3f57c9a 100755 --- a/docs/how_to/talk_to_humanoid_control_from_python.md +++ b/docs/how_to/talk_to_humanoid_control_from_python.md @@ -57,9 +57,10 @@ if state is not None: ## 3. Publish a command -`RemotePolicyController` consumes `MITCommand` (in the **REMOTE** mode of the -[five-mode FSM](../concepts/five_mode_fsm.md)). Drive the FSM into REMOTE first -(gamepad, or [switch controllers manually](./switch_controllers_manually.md)). +`RemotePolicyController` consumes `MITCommand` (the **REMOTE** control mode; see +[the five control modes](../concepts/five_mode_fsm.md)). Activate +`remote_policy_controller` first — `R1+B` on the gamepad, or +[switch controllers manually](./switch_controllers_manually.md). ```python from lite_sdk2 import MITCommand, zero_mit_command @@ -84,10 +85,14 @@ pub.write(zero_mit_command(state.name, damping=2.0)) |---|---|---| | `MITCommand` | `/remote_policy_controller/command` | reliable, depth 4 | | `JointState` | `/lite/joint_states` | reliable, depth 10 | -| `ControlMode` | `/control_mode` | reliable | | `SafetyStatus` | `/safety_status` | reliable | | `StandbyState` | `/standby_controller_a/state` (one per pose; `_b` for Pose B) | transient-local (latched) | +The `ControlMode` message type still ships in `humanoid_control_msgs_dds`, but +nothing publishes `/control_mode` anymore. Read the active controller from +the `/controller_manager/list_controllers` service (`ros2 control +list_controllers`) instead. + 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 b722607..ed6fa2a 100644 --- a/docs/reference/cli_tools.md +++ b/docs/reference/cli_tools.md @@ -56,7 +56,7 @@ These are normally driven by their launch files, but are reachable via | `robstride_probe` | `humanoid_devices_robstride` | Humanoid Control | Link RTT / jitter probe against one actuator. | | `robstride_probe_report` | `humanoid_devices_robstride` | Humanoid Control | Report companion for `robstride_probe` captures. | | `mit_slider_gui` | `humanoid_devices_robstride` | Humanoid Control | Qt slider window publishing Float64MultiArray to a forward_command_controller. | -| `mode_manager` | `humanoid_controllers` | Humanoid Control | The FSM orchestrator. Normally launched by bringup; sometimes useful to start manually. | +| `joy_teleop` | `joy_teleop` (teleop_tools) | external (built from source) | Stock gamepad node; maps buttons directly to `/controller_manager/switch_controller`. Normally launched by bringup. | | `calibrate_robot` | `humanoid_bringup_lite` | Humanoid Control | Sample (min, max) per joint; write `calibration.yaml` on Ctrl+C. | | `rerun_viz` | `humanoid_bringup_lite` | Humanoid Control | Native rerun viewer subscribed to `/robot_description` + `/lite/joint_states`. | | `viser_viz` | `humanoid_bringup_lite` | Humanoid Control | Browser viewer (default port 8080). Same subscriptions. | @@ -170,29 +170,41 @@ Requires `python_qt_binding` (installed alongside `rqt_reconfigure`). Used in: [Tutorials → Drive one Robstride](../tutorials/drive_one_robstride.md), [How-to → mit_slider_gui](../how_to/mit_slider_gui.md). -### `mode_manager` +### `joy_teleop` + +The stock ROS `teleop_tools` gamepad node. There is **no** `mode_manager` +executable and no FSM any more — `joy_teleop` maps gamepad buttons +**directly** to `/controller_manager/switch_controller`, driven entirely +by a YAML config (`joy_teleop_lite.yaml` / `joy_teleop_biped.yaml` / +`joy_teleop_prime.yaml`). robostack-jazzy ships no `joy_teleop` binary, +so it is built from source via `humanoid_control.repos`. ```bash -ros2 run humanoid_controllers mode_manager -ros2 run humanoid_controllers mode_manager --ros-args -p tick_rate_hz:=100 +ros2 run joy_teleop joy_teleop --ros-args --params-file joy_teleop_lite.yaml ``` -| Parameter | Default | Description | -|---|---|---| -| `tick_rate_hz` | `50.0` | Timer rate for `tick()` | -| `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_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) | - -Normally launched by `real.launch.py` / `mujoco.launch.py`. Useful -to run standalone when debugging the joy decoder. - -Used in: [Concepts → Five-mode FSM](../concepts/five_mode_fsm.md), -[Reference → Controllers](./controllers.md#mode_manager-executable). +Each button **activates one controller and deactivates its siblings** +(flat, `BEST_EFFORT`) — any transition from any state, no gating, no +ordering. Default Lite-arm button map: + +| Button(s) | Activates | +|---|---| +| `X` | `damping_controller` | +| `L1 + A` | `standby_controller_a` | +| `L1 + B` | `standby_controller_b` | +| `L1 + Y` | `standby_controller_y` | +| `R1 + A` | `rl_policy_controller` (locomotion) | +| `R1 + B` | `remote_policy_controller` | +| `BACK` | `zero_torque_controller` (STOP) | + +`BACK` selects `zero_torque_controller` — it no longer shuts the process +down; CAN Disable still happens on `Ctrl+C` via the hardware +`on_deactivate`. Normally launched by `real.launch.py` / +`mujoco.launch.py` (when `enable_joy_teleop:=true`, the default). Without +a gamepad, switch controllers directly with +`ros2 control switch_controllers --activate --deactivate `. + +Reference config pattern: `qiayuanl/unitree_bringup` `config/g1/joy.yaml`. ### `calibrate_robot` diff --git a/docs/reference/controllers.md b/docs/reference/controllers.md index e4488e1..f7f2d31 100644 --- a/docs/reference/controllers.md +++ b/docs/reference/controllers.md @@ -1,13 +1,20 @@ # Controllers + FSM -Five `controller_interface::ControllerInterface` plugins, one standalone -`rclcpp::Node` orchestrator. **Only one mode controller is active at a time**; -`joint_state_broadcaster` runs always. +Five `controller_interface::ControllerInterface` plugins. Mode switching is +**flat**: the stock `joy_teleop` node (ROS `teleop_tools`) maps gamepad +buttons **directly** to `/controller_manager/switch_controller` — there is no +arbitrating state machine or `mode_manager` node anymore. **Only one mode +controller is active at a time**; `joint_state_broadcaster` runs always. ## FSM summary -See [Concepts → Architecture](../concepts/architecture.md#five-mode-finite-state-machine) -for the annotated version including safety-fault edges. +The five modes still exist as controllers, but there is **no finite-state +machine** arbitrating them: any mode can be entered from any other, in any +order (e.g. `ZERO_TORQUE → LOCOMOTION` directly). Switching is done by +`joy_teleop` (gamepad) or `ros2 control switch_controllers` (CLI); each button +activates one controller and deactivates its siblings with `BEST_EFFORT` +strictness. See [Concepts → Architecture](../concepts/architecture.md#five-mode-finite-state-machine) +for the mode overview. ## Plugin-by-plugin @@ -54,43 +61,52 @@ resistance. ### `humanoid_control/StandbyController` -**Role**: linearly interpolate joint positions through a pose sequence; ramp -`K_p / K_d` from `0` to target gains during the **first** segment so -activation never snaps. +**Role**: linearly interpolate joint positions through a pose sequence toward a +ready pose. Gains are **constant at the target `K_p / K_d` from `t = 0`** — +there is **no gain ramp**. On activation the setpoint is **seeded to the +current measured joint positions** and interpolated from there, so entering +STANDBY is safe from **any** prior state (no snap). -**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. +**Instances**: spawned **once per configured pose** — `standby_controller_a` +(Pose A, `L1+A`) and `standby_controller_b` (Pose B, `L1+B`); the Lite arms +config adds a third, `standby_controller_y` (Pose Y, `L1+Y`). Same plugin class +(and same `standby_controller.cpp`), different pose parameters in the bringup +YAML. Each is activated directly by its gamepad button (via `joy_teleop`) or by +`ros2 control switch_controllers`. **Parameters**: | Param | Type | Description | |---|---|---| | `joints` | `string[]` | Required. | -| `target_stiffness` | `float64[]` | Per-joint target `K_p` when ramp finishes. | -| `target_damping` | `float64[]` | Per-joint target `K_d`. | +| `target_stiffness` | `float64[]` | Per-joint target `K_p` (constant, held from activation). | +| `target_damping` | `float64[]` | Per-joint target `K_d` (constant, held from activation). | | `segment_durations` | `float64[]` | Seconds per pose segment. Length determines how many pose segments are expected. | | `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 — i.e. -`/standby_controller_a/state` and `/standby_controller_b/state`, one per instance. -Watch the one matching the pose you loaded. +**Publishes**: `~/state` (`humanoid_control_msgs/StandbyState`) with `TRANSIENT_LOCAL` QoS — +i.e. `/standby_controller_a/state`, `/standby_controller_b/state` (and +`/standby_controller_y/state` on Lite), one per instance. This is **telemetry +only**: `is_finished` reports progress but **nothing gates on it** anymore (any +transition is allowed from any state). Watch the one matching the pose you +activated. :::tip[How the bundled config interpolates] -`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. +`humanoid_control_lite_controllers.yaml` configures each instance with **two +segments**, where the final `pose_segment` is that instance's target pose — the +standby instances differ only in this final pose (Pose A vs Pose B vs Pose Y). +Activating a standby button therefore animates the arms from their **current +measured position** to the chosen pose over the configured segments, holding the +target `K_p` / `K_d` constant the whole time (no gain ramp). ::: `fallback_controllers: ["damping_controller"]` is set on the controller-manager side so any non-`OK` `return_type` from `update()` -auto-deactivates Standby and activates Damping. +auto-deactivates Standby and activates Damping. This native +`fallback_controllers` mechanism — declared on every mode controller +(`damping_controller` in turn falls back to `zero_torque_controller`) — is now +the **only** automatic fault response; there is no `/safety_status`-driven +auto-DAMP node anymore (`/safety_status` is telemetry the operator reacts to). ### `humanoid_control/RLPolicyController` @@ -101,7 +117,8 @@ runs *every* learned policy (tracking / piano / locomotion). Each RT (`ReferenceProvider`), maps the action across the full articulation (`ActionMapper`), and writes the five MIT command interfaces — never leaving the RT thread. Policies differ only by the loaded `.onnx` + -`.mcap`; the ONNX `task_type` metadata selects the term set. +`.mcap`; the ONNX `task_type` metadata selects the term set. It is entered +**directly at full authority** — there is no soft-start ramp. Its parameters come from the `rl_policy_controller` overlay that `humanoid_control_policy prepare` (or `pianist_policy prepare`) transcodes from the @@ -124,8 +141,9 @@ ONNX `custom_metadata_map` — they are not hand-written: `OnnxPolicy` (onnxruntime C++) is built only when onnxruntime is found at build time — the conda `onnxruntime-cpp` package, pinned in `pixi.toml`. Without it the controller falls -back to `PlaceholderPolicy` (zeros) — useful for smoke-testing the FSM and -the observation/reference plumbing without a real inference dependency. +back to `PlaceholderPolicy` (zeros) — useful for smoke-testing controller +switching and the observation/reference plumbing without a real inference +dependency. The contract (`PolicyMetadata` → overlay) is identical either way. See [Policy runner](policy_runner.md). ::: @@ -163,56 +181,47 @@ rather than hand-writing message mirrors — see [Policy runner](policy_runner.md) for how the in-process learned-policy path relates. -### `mode_manager` (executable) - -**NOT a controller plugin** — a regular `rclcpp::Node` compiled as the -`humanoid_controllers/mode_manager` executable. - -| Input | Topic / source | Purpose | -|---|---|---| -| Gamepad | `/joy` (`sensor_msgs/Joy`) | DAMP / LOAD / START_LOCOMOTION / START_REMOTE / QUIT intents | -| 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_a,load_b,start_remote,start_locomotion,quit}` (`std_srvs/Trigger`) | same intents from the command line | - -| Output | Topic | Purpose | -|---|---|---| -| FSM state | `/control_mode` (`ControlMode`) | 50 Hz telemetry | -| Mode switch | `/controller_manager/switch_controller` | the actual transition | - -**Joy bindings** (Xbox-layout defaults — remap via the `joy.*` params): - -| Buttons | Intent | Target | -|---|---|---| -| `X` (2) | DAMP | `damping_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 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**: - -| Param | Type | Default | Description | -|---|---|---|---| -| `tick_rate_hz` | `float64` | `50.0` | timer rate | -| `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_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) | +### `joy_teleop` (gamepad → controller switch) + +**NOT a controller plugin** — the stock `joy_teleop` node from ROS +`teleop_tools` (built from source via `humanoid_control.repos`, since +`teleop_tools` isn't in robostack-jazzy). It **replaces** the old +`mode_manager` executable: instead of a state machine, it maps each gamepad +button **directly** to a `/controller_manager/switch_controller` call, +configured entirely by YAML (`joy_teleop_lite.yaml` / `joy_teleop_biped.yaml` / +`joy_teleop_prime.yaml`). The button map follows `qiayuanl/unitree_bringup`'s +`config/g1/joy.yaml`. + +Each button **activates one controller and deactivates its siblings**, with +`strictness: BEST_EFFORT`. There is **no gating and no ordering** — any mode +can be entered from any state (e.g. `ZERO_TORQUE → LOCOMOTION` directly, with +no intermediate STANDBY). + +**Button map** (per variant; ✓ = bound, — = not present): + +| Buttons | Activates | Lite arms | Biped | Prime | +|---|---|---|---|---| +| `X` | `damping_controller` (DAMP) | ✓ | ✓ | ✓ | +| `L1+A` | `standby_controller_a` (STANDBY A) | ✓ | ✓ | ✓ | +| `L1+B` | `standby_controller_b` (STANDBY B) | ✓ | — | ✓ | +| `L1+Y` | `standby_controller_y` (STANDBY Y) | ✓ | — | — | +| `R1+A` | `rl_policy_controller` (LOCOMOTION) | ✓ | ✓ | ✓ | +| `R1+B` | `remote_policy_controller` (REMOTE) | ✓ | — | ✓ | +| `BACK` | `zero_torque_controller` (STOP) | ✓ | ✓ | ✓ | + +`BACK` (STOP) selects `zero_torque_controller`, which holds zero torque with +the drives **still enabled** — it is not a shutdown. There is no sequenced +QUIT / button power-down anymore; CAN `Disable` still fires on `Ctrl+C` via the +hardware `on_deactivate`. + +Programmatic or headless control skips `joy_teleop` and calls the same service +directly, e.g. `ros2 control switch_controllers --activate standby_controller_a +--deactivate zero_torque_controller`. The currently active mode is read back +from `/controller_manager/list_controllers` (there is no `/control_mode` +topic anymore). ## Spawn order (in launch) -The launch spawns `zero_torque_controller` active independently — so even if -`mode_manager` dies, the robot is in the safe state. \ No newline at end of file +The launch spawns `zero_torque_controller` active independently — so the robot +boots into the safe zero-torque state regardless of whether `joy_teleop` is +running (`enable_joy_teleop:=false` skips it entirely). \ No newline at end of file diff --git a/docs/reference/launch_args.md b/docs/reference/launch_args.md index 68a58f1..f337326 100644 --- a/docs/reference/launch_args.md +++ b/docs/reference/launch_args.md @@ -39,14 +39,14 @@ instances, one per physical SocketCAN bus (`LiteLeftArm` claims CAN ids | `mode` | `arms` | `arms` = 14 joints (default). `arms_neck` = 17 joints (requires neck silicon). | | `hardware_config` | `/config/lite_hardware.yaml` | Per-machine bus + joint config. Maps the two `` blocks to specific SocketCAN ifnames and joint IDs. Override to retarget a robot whose CAN ifnames differ. | | `calibration_file` | `/config/calibration.yaml` | Absolute path to the per-physical-robot zero-offset YAML. Pass `''` for identity calibration (only the URDF `direction` sign flip applies, no offset). See [Hardware specs → Bus-bring-up checklist](./hardware_specs.md#bus-bring-up-checklist) for how to regenerate. | -| `enable_mode_manager` | `true` | `false` skips spawning the FSM orchestrator. Used by `calibrate.launch.py` and for raw-debug bringups where the operator drives controllers directly via `ros2 control switch_controllers`. | -| `enable_gamepad` | `true` | `true` spawns `joy_node` so `mode_manager` can read `/joy`. **The launch hard-fails on missing `joy_dev`.** Pass `false` on a keyboardless lab box to drive the FSM via the `/humanoid_control/mode/*` `std_srvs/Trigger` services instead. | +| `enable_joy_teleop` | `true` | `true` spawns the `joy_teleop` node that maps gamepad buttons directly to `/controller_manager/switch_controller`. `false` skips it — used by `calibrate.launch.py` and raw-debug bringups where the operator drives controllers directly via `ros2 control switch_controllers`. | +| `enable_gamepad` | `true` | `true` spawns `joy_node` so `joy_teleop` can read `/joy`. **The launch hard-fails on missing `joy_dev`.** Pass `false` on a keyboardless lab box and switch controllers directly with `ros2 control switch_controllers` instead. | | `joy_dev` | `/dev/input/js0` | Path passed verbatim to `joy_node`'s `dev` parameter. Override when the onboard computer enumerates the gamepad as something other than `js0` (multiple gamepads plugged in, udev rename). The pre-launch check fails fast when the specific path is missing and lists any other `/dev/input/js*` devices it can see, so the error message tells you which override to pass. Ignored when `enable_gamepad:=false`. | -The active-policy target is picked by the START button, not a launch -arg (convention: A = local policy, B = remote): R1+A (`START_LOCOMOTION`) -activates `rl_policy_controller` (the in-process learned policy — -tracking / piano / locomotion), R1+B (`START_REMOTE`) activates +The active-policy target is picked by the gamepad button, not a launch +arg (convention: A = local policy, B = remote): R1+A activates +`rl_policy_controller` (the in-process learned policy — +tracking / piano / locomotion), R1+B activates `remote_policy_controller` (the System 1/2 external-command ingress — gravity-comp today, VLA later; see [Controllers](./controllers.md)). @@ -80,7 +80,7 @@ upstream). ## `humanoid_bringup_lite/launch/calibrate.launch.py` Bundles `real.launch.py` with three overrides -(`calibration_file:='' enable_mode_manager:='false' enable_gamepad:='false'`) +(`calibration_file:='' enable_joy_teleop:='false' enable_gamepad:='false'`) and adds the `calibrate_robot` observer node. The plugin runs with identity calibration so `/lite/joint_states` carries `direction × raw_motor_pos`, which is the frame the homing-offset formula expects. @@ -143,8 +143,8 @@ Prepares and loads the in-process tracking-family policy. It runs `humanoid_control_policy prepare` **synchronously** (resolve the ONNX, convert the LeRobot motion to a `.mcap` bag, emit the `rl_policy_controller` overlay), then spawns `rl_policy_controller` *inactive* with that -overlay into the running controller_manager. The operator's -`START_LOCOMOTION` (R1+A / `/humanoid_control/mode/start_locomotion`) activates it. +overlay into the running controller_manager. The operator's R1+A button +(via `joy_teleop`) activates it. There is no separate runner process; the task is selected by the ONNX `task_type` metadata. @@ -253,14 +253,14 @@ ros2 launch pianist_bringup mujoco.launch.py ros2 launch humanoid_bringup_lite calibrate.launch.py ``` -**Robot onboard computer** (CM + hardware + FSM + gamepad — boots +**Robot onboard computer** (CM + hardware + joy_teleop + gamepad — boots the real control plane, no visualisers, no policy runner): ```sh -# Real Lite, gamepad on by default. Press R1+B at STANDBY to start the remote policy. +# Real Lite, gamepad on by default. Press R1+B to activate the remote policy. ros2 launch humanoid_bringup_lite real.launch.py -# Same, but on a keyboardless lab box (drive the FSM via /humanoid_control/mode/* services). +# Same, but on a keyboardless lab box (switch controllers via ros2 control switch_controllers). ros2 launch humanoid_bringup_lite real.launch.py enable_gamepad:=false # Gamepad enumerated as js1 instead of js0 (multiple controllers plugged in). diff --git a/docs/reference/manual_controllers.md b/docs/reference/manual_controllers.md index 66afeca..7777334 100644 --- a/docs/reference/manual_controllers.md +++ b/docs/reference/manual_controllers.md @@ -4,12 +4,12 @@ title: Manual / debug controllers # Manual / debug controllers -A short reference of the **non-FSM** controllers — the ones the +A short reference of the **non-mode** controllers — the ones the operator activates by hand for testing, tuning, or debug. These -controllers don't appear in `mode_manager`'s transition table and +controllers aren't in the `joy_teleop` gamepad switch set and aren't part of the production safety loop. -For the FSM controllers (`zero_torque`, `damping`, `standby`, +For the mode controllers (`zero_torque`, `damping`, `standby`, `rl_policy`, `remote_policy`) see [Controllers](./controllers.md). ## When to reach for these @@ -21,9 +21,9 @@ For the FSM controllers (`zero_torque`, `damping`, `standby`, | Manually exercise one or two joints from the CLI | `forward_command_controller` claiming the target interfaces | | Test a new controller plugin you wrote | (your plugin) — see [Tutorials → Build your own controller](../tutorials/build_your_own_controller.md) | -All of these compete with the FSM controllers for the same command -interfaces. Activate them with `enable_mode_manager:=false` or -explicitly deactivate the FSM controller (typically `zero_torque`) +All of these compete with the mode controllers for the same command +interfaces. Activate them with `enable_joy_teleop:=false` or +explicitly deactivate the mode controller (typically `zero_torque`) first. ## `forward_command_controller/MultiInterfaceForwardCommandController` @@ -70,8 +70,8 @@ the same joints and publishes them once via `ros2 topic pub --once`. See `mujoco_ros2_control_demos/config/cartpole_controller_position.yaml` for the upstream pattern. -We don't bundle a YAML for this in the project — the FSM -`StandbyController` covers the production trajectory case +We don't bundle a YAML for this in the project — the +`StandbyController` mode covers the production trajectory case end-to-end. Drop in stock JTC for one-off scripted tests. ## `humanoid_control/MITJointTrajectoryController` (project-local) @@ -93,7 +93,7 @@ mit_joint_trajectory_controller: Subscribes to `~/joint_trajectory` (`trajectory_msgs/JointTrajectory`) and writes all 5 MIT fields every tick. Scalar `kp` / `kd` (no per-joint array) keeps the config simple for ad-hoc tests; for -production use, the FSM controllers are still the right home. +production use, the mode controllers are still the right home. Not in the default spawner batch; load it manually (inside `pixi shell`): @@ -149,5 +149,5 @@ want the same interface. Active-set the deactivate list first. - [How-to → Drive a single joint with mit_slider_gui](../how_to/mit_slider_gui.md) — operator-side workflow for the slider GUI. - [How-to → Switch without the FSM](../how_to/switch_controllers_manually.md) - — when to bypass `mode_manager`. -- [Reference → Controllers](./controllers.md) — the FSM controllers. + — driving controllers directly with `ros2 control switch_controllers`. +- [Reference → Controllers](./controllers.md) — the mode controllers. diff --git a/docs/reference/messages.md b/docs/reference/messages.md index ca1cc29..ad68906 100644 --- a/docs/reference/messages.md +++ b/docs/reference/messages.md @@ -48,7 +48,12 @@ re-creation fully reproducible. ## `humanoid_control_msgs/ControlMode` -Mode-FSM telemetry, published by `mode_manager` at tick rate (50 Hz). +Mode telemetry message. **No longer published on any topic** — the +`mode_manager` node that published `/control_mode` at 50 Hz has been removed, +and nothing publishes or subscribes the topic today. The message **type is +retained** because [`humanoid_control_msgs_dds`](packages.md#humanoid_control_msgs_dds) +generates and tests a wire-compatible mirror of it. Read the active mode from +`/controller_manager/list_controllers` instead. ``` std_msgs/Header header @@ -72,7 +77,7 @@ match against the `uint8 = ` defines, not hard-code integers. 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. +late-joining subscriber immediately sees the most recent value. ``` std_msgs/Header header @@ -82,8 +87,9 @@ float64 progress # [0, 1] within the current segment bool is_finished # true once final pose + final gains reached ``` -`mode_manager` gates the `START` intent (STANDBY → LOCOMOTION/REMOTE) on -`is_finished == true`. +This is **telemetry only**: `is_finished` reports when the final pose and gains +are reached, but **nothing gates on it** anymore — any mode can be entered from +STANDBY (or any other state) at any time. ## `humanoid_control_msgs/SafetyStatus` diff --git a/docs/reference/packages.md b/docs/reference/packages.md index 067cd42..239ec7d 100644 --- a/docs/reference/packages.md +++ b/docs/reference/packages.md @@ -49,8 +49,8 @@ Custom ROS 2 interfaces. Once a trained policy depends on one, it is **frozen**. | Message | Used by | |---|---| | `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_a/state` / `/standby_controller_b/state` (one topic per pose; `is_finished` gate for the `START_LOCOMOTION` / `START_REMOTE` intents). | +| `ControlMode` | Type retained for the off-ROS DDS bridge only. The `/control_mode` topic and the `mode_manager` that published it are removed; read the active mode via `ros2 control list_controllers`. | +| `StandbyState` | `StandbyController` → `/standby_controller_a/state` / `/standby_controller_b/state` / `/standby_controller_y/state` (one topic per pose). | | `SafetyStatus` | every hardware plugin / controller → `/safety_status`. Per-bus `source` field; bitmask in `flags`. | See [Messages reference](messages.md) for full schemas. @@ -157,7 +157,11 @@ Both: - Publishes a `humanoid_control_msgs/SafetyStatus` on `/safety_status` (TRANSIENT_LOCAL, per-bus source field) with bit-flags for `BUS_OFF` / `RX_TIMEOUT` / `TX_QUEUE_OVERRUN` / `MOTOR_FAULT` / `TEMPERATURE_LIMIT` / `INVALID_FRAME`. - `mode_manager` subscribes and auto-falls to DAMPING on any non-OK level. + `/safety_status` is **telemetry only** — nothing subscribes to auto-fall + to DAMPING. Controller-side faults are handled by the controller_manager's + native `fallback_controllers` (each mode controller → `damping_controller`, + `damping_controller` → `zero_torque_controller`); bus-level faults are + cleared by the operator STOP (`BACK` → `zero_torque_controller`). Ships three CLI executables alongside the plugin: @@ -169,16 +173,20 @@ Ships three CLI executables alongside the plugin: ### `humanoid_controllers` -Five mode-FSM controllers + the standalone `mode_manager` executable. +The five control-mode controller plugins, plus a manual +`MITJointTrajectoryController`. There is **no** `mode_manager` executable +and no FSM: mode switching is the stock `joy_teleop` node (from +`teleop_tools`, built from source via `humanoid_control.repos`), which maps +gamepad buttons directly to `/controller_manager/switch_controller`. | Plugin | State | Source | |---|---|---| -| `humanoid_control/ZeroTorqueController` | startup, safer fault fallback | `zero_torque_controller.cpp` | +| `humanoid_control/ZeroTorqueController` | startup, STOP, safer fault fallback | `zero_torque_controller.cpp` | | `humanoid_control/DampingController` | compliant fail-safe | `damping_controller.cpp` | -| `humanoid_control/StandbyController` | pose interpolation + gain ramp | `standby_controller.cpp` | +| `humanoid_control/StandbyController` | interpolates `position` from the measured pose toward a YAML target pose (A/B/Y), constant `K_p`/`K_d` from t=0 (no stiffness ramp) — safe from any state | `standby_controller.cpp` | | `humanoid_control/RLPolicyController` | in-process ONNX inference (System 0) — every learned policy | `rl_policy_controller.cpp` | | `humanoid_control/RemotePolicyController` | System 1/2 external-command ingress | `remote_policy_controller.cpp` | -| `mode_manager` exe | FSM orchestrator | `mode_manager.cpp` | +| `humanoid_control/MITJointTrajectoryController` | manual joint-trajectory command | `mit_joint_trajectory_controller.cpp` | `RLPolicyController` now runs full in-process inference. Its runtime modules live alongside the controllers in this package: @@ -267,10 +275,11 @@ 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 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`). + (active) + the remaining mode controllers (inactive) — `damping`, + the standby poses (`standby_controller_a` / `standby_controller_b` / + `standby_controller_y`), `rl_policy`, and `remote_policy`. +5. Start `joy_teleop` (gamepad buttons → `/controller_manager/switch_controller`) + when `enable_joy_teleop:=true` (the default). 6. Start `joy_node` (when `enable_gamepad:=true`, which is the default). `humanoid_bringup_lite/config/sim_overrides.yaml` adds `use_sim_time:=true` diff --git a/docs/reference/policy_runner.md b/docs/reference/policy_runner.md index c2488c8..b83672b 100644 --- a/docs/reference/policy_runner.md +++ b/docs/reference/policy_runner.md @@ -130,8 +130,12 @@ things, in order: `rl_policy_params.yaml` overlay. This is the dependency-heavy, non-real-time work; it must finish before the controller configures. 2. **Spawns `rl_policy_controller` *inactive*** with the overlay as a - `--param-file`. The operator's FSM `START_LOCOMOTION` - (`/humanoid_control/mode/start_locomotion`, R1+A) is what activates it. + `--param-file`. The operator activates it **directly** — there is no + FSM and no soft-start gating, so the transition is allowed from any + state: `ros2 control switch_controllers --activate rl_policy_controller + --deactivate standby_controller_a` (or whichever controller is active). + On the gamepad that is R1+A, which `joy_teleop` maps straight to + `/controller_manager/switch_controller`. The launch args are pass-throughs to the `prepare` CLI below (plus `out_dir`, defaulting to `~/.cache/humanoid_control_policy/launch/`, which fixes the diff --git a/docs/reference/quick_reference.md b/docs/reference/quick_reference.md index 9ef1f55..ac83ed5 100644 --- a/docs/reference/quick_reference.md +++ b/docs/reference/quick_reference.md @@ -48,17 +48,17 @@ ros2 launch humanoid_bringup_lite calibrate.launch.py ### Robot onboard computer (real bringup) ```bash -# Real Lite — both buses, two ros2_control blocks, gamepad + mode_manager +# Real Lite — both buses, two ros2_control blocks, gamepad + joy_teleop ros2 launch humanoid_bringup_lite real.launch.py -# Real Lite, no gamepad attached (drive the FSM via /humanoid_control/mode/* services) +# Real Lite, no gamepad attached (switch controllers via ros2 control switch_controllers) ros2 launch humanoid_bringup_lite real.launch.py enable_gamepad:=false # Gamepad enumerated as js1 (multiple controllers plugged into the Jetson) ros2 launch humanoid_bringup_lite real.launch.py joy_dev:=/dev/input/js1 -# Real Lite, no FSM (raw debug / calibration) -ros2 launch humanoid_bringup_lite real.launch.py enable_mode_manager:=false +# Real Lite, no joy button switching (raw debug / calibration) +ros2 launch humanoid_bringup_lite real.launch.py enable_joy_teleop:=false ``` `real.launch.py` boots the real-time control plane only — visualisers @@ -70,7 +70,9 @@ loaded on the robot below. ```bash # Prepare + load the in-process tracking policy (Humanoid Control → humanoid_control_policy): # runs `prepare` (ONNX → .mcap + overlay), then loads rl_policy_controller -# inactive. START_LOCOMOTION (R1+A) activates it. +# inactive. Activate it with R1+A (joy_teleop) or: +# ros2 control switch_controllers --activate rl_policy_controller --deactivate + ros2 launch humanoid_control_policy lite_policy.launch.py \ wandb_run_path:=… wandb_checkpoint_name:=model.onnx @@ -135,20 +137,30 @@ ros2 run humanoid_bringup_lite rerun_viz ## Mode-FSM gamepad bindings (Xbox layout) -| Buttons | Intent | Allowed from | Activates | -|---|---|---|---| -| `X` | DAMP | any state | `damping_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()` | - -`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). +The stock `joy_teleop` node (from `teleop_tools`) maps each button +**directly** to `/controller_manager/switch_controller` — there is no FSM. +Every binding activates one controller and deactivates its siblings +(flat, `BEST_EFFORT`); any binding works from any state, with no gating +and no ordering. + +| Buttons | Activates | +|---|---| +| `X` | `damping_controller` | +| `L1 + A` | `standby_controller_a` | +| `L1 + B` | `standby_controller_b` | +| `L1 + Y` | `standby_controller_y` | +| `R1 + A` | `rl_policy_controller` (locomotion) | +| `R1 + B` | `remote_policy_controller` | +| `BACK` | `zero_torque_controller` (STOP) | + +`BACK` selects `zero_torque_controller` — it does **not** shut the process +down; CAN Disable still happens on `Ctrl+C` via the hardware +`on_deactivate`. Pose and policy are independent, and every transition is +allowed from every state — from any standby pose (or any other state) +`R1+A` → locomotion or `R1+B` → REMOTE, and you can switch directly +between poses. See +[Manual controller switching](#manual-controller-switching-no-fsm) for the +equivalent `ros2 control` calls. ## Manual controller switching (no FSM) @@ -160,8 +172,9 @@ ros2 control switch_controllers \ --deactivate zero_torque_controller \ --activate damping_controller -# DAMPING → STANDBY Pose A (motors will move to the ready pose over ~4 s; -# use standby_controller_b for Pose B) +# any state → STANDBY Pose A (interpolates from the measured pose toward the +# Pose A target, constant PD from t=0, no stiffness ramp — safe from any state; +# standby_controller_b / _y are the other poses) ros2 control switch_controllers \ --deactivate damping_controller \ --activate standby_controller_a @@ -181,10 +194,10 @@ Always end a session with `zero_torque_controller` active before |---|---|---|---| | `/lite/joint_states` | `sensor_msgs/JointState` | 50 Hz real / 200 Hz sim | always (`joint_state_broadcaster`, remapped at bringup) | | `/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_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 | +| `/standby_controller_a/state` | `humanoid_control_msgs/StandbyState` | active-only | TRANSIENT_LOCAL; Pose A | +| `/standby_controller_b/state` | `humanoid_control_msgs/StandbyState` | active-only | TRANSIENT_LOCAL; Pose B | +| `/standby_controller_y/state` | `humanoid_control_msgs/StandbyState` | active-only | TRANSIENT_LOCAL; Pose Y | | `/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) | @@ -204,9 +217,9 @@ ros2 topic echo --once /lite/joint_states # Safety status of every active bus 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_a std_srvs/srv/Trigger # Pose A (load_b for Pose B) +# Switch controllers without a gamepad (any transition, from any state) +ros2 control switch_controllers --activate damping_controller --deactivate zero_torque_controller +ros2 control switch_controllers --activate standby_controller_a --deactivate damping_controller # Fake a System 1/2 MITCommand publish (when remote_policy_controller is active in MuJoCo) ros2 topic pub --once /remote_policy_controller/command \ @@ -215,17 +228,22 @@ ros2 topic pub --once /remote_policy_controller/command \ ## Services for FSM transitions -`std_srvs/Trigger` services on `/humanoid_control/mode/*` mirror the gamepad -intents — useful when there's no joystick attached. +The `/humanoid_control/mode/*` `std_srvs/Trigger` services were **removed** +along with the FSM. Switch controllers directly instead — every transition +is allowed from every state (activate one controller, deactivate the +current one): -| Service | Effect | +| Former intent | Command | |---|---| -| `/humanoid_control/mode/damp` | → DAMPING from any state | -| `/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) | +| DAMP | `ros2 control switch_controllers --activate damping_controller --deactivate ` | +| STANDBY A | `ros2 control switch_controllers --activate standby_controller_a --deactivate ` | +| STANDBY B | `ros2 control switch_controllers --activate standby_controller_b --deactivate ` | +| STANDBY Y | `ros2 control switch_controllers --activate standby_controller_y --deactivate ` | +| LOCOMOTION | `ros2 control switch_controllers --activate rl_policy_controller --deactivate ` | +| REMOTE | `ros2 control switch_controllers --activate remote_policy_controller --deactivate ` | +| STOP | `ros2 control switch_controllers --activate zero_torque_controller --deactivate ` | + +The gamepad does exactly this via `joy_teleop`. ## The Lite joint table @@ -271,7 +289,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_A`/`LOAD_B` from anywhere other than DAMPING | Send DAMP (`X`) first. See FSM table above. | +| A gamepad button doesn't switch the controller | Check `joy_teleop` is running and the target controller is loaded; read the active one with `ros2 control list_controllers`. | | `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 bd18b5d..2e4fb28 100644 --- a/docs/reference/topics_services.md +++ b/docs/reference/topics_services.md @@ -24,11 +24,10 @@ subscribes, or serves. Use this page to find "who publishes X" or | Topic | Type | QoS | Publisher | When present | |---|---|---|---|---| -| `/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_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*`. | +| `/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. Telemetry — no automatic consumer now (native `fallback_controllers` handles faults; operator reacts to the rest). | +| `/standby_controller_a/state` | `humanoid_control_msgs/StandbyState` | RELIABLE TRANSIENT_LOCAL depth 1 | `humanoid_control/StandbyController` instance `standby_controller_a` (when active) | Pose A. Telemetry only — `is_finished` no longer gates anything. | +| `/standby_controller_b/state` | `humanoid_control_msgs/StandbyState` | RELIABLE TRANSIENT_LOCAL depth 1 | `humanoid_control/StandbyController` instance `standby_controller_b` (when active) | Pose B. Telemetry only. (Lite also spawns `standby_controller_y` → `/standby_controller_y/state`.) | +| `/joy` | `sensor_msgs/Joy` | SENSOR_DATA | `joy_node` | When `enable_gamepad:=true` (default). Consumed by `joy_teleop` (not `mode_manager`). The launch hard-fails on missing `/dev/input/js*`. | ### Active-controller-dependent @@ -40,7 +39,7 @@ subscribes, or serves. Use this page to find "who publishes X" or ### `/parameter_events` and friends -Every node (controllers, mode_manager, plugins) publishes the +Every node (controllers, joy_teleop, plugins) publishes the standard ROS infrastructure topics: - `/parameter_events`, `/rosout` - `~/get_parameters`, `~/set_parameters`, etc. (per node) @@ -50,19 +49,15 @@ here so `ros2 topic list` output isn't confusing. ## Services -### `mode_manager` FSM-transition services +### Mode switching -`std_srvs/Trigger` services. Same intents as the gamepad — for use on -keyboardless lab boxes or scripted tests. - -| Service | Effect | -|---|---| -| `/humanoid_control/mode/damp` | → DAMPING from any state | -| `/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) | +There are **no** `/humanoid_control/mode/*` `std_srvs/Trigger` services anymore +— the `mode_manager` node that hosted them has been removed. Mode switching now +goes through the standard controller-manager service +`/controller_manager/switch_controller` +(`controller_manager_msgs/srv/SwitchController`), driven by `joy_teleop` from +the gamepad or by `ros2 control switch_controllers` on the command line. See +the controller_manager-side services below. ### controller_manager-side (under `/controller_manager`) @@ -76,9 +71,9 @@ Standard `controller_manager` services. Useful ones: | `/controller_manager/switch_controller` | `controller_manager_msgs/SwitchController` | Backing for `ros2 control switch_controllers` | | `/controller_manager/configure_controller` | `controller_manager_msgs/ConfigureController` | Force `on_configure` | -`mode_manager` is a client of `/controller_manager/switch_controller` -(async, STRICT). You can call it directly from `ros2 control` for -operator-driven debug — see +`joy_teleop` is a client of `/controller_manager/switch_controller` (each button +→ one activate + sibling deactivates, `BEST_EFFORT` strictness). You can call it +directly from `ros2 control` for operator-driven debug — see [How-to → Switch without the FSM](../how_to/switch_controllers_manually.md). ### Per-node services (parameter handling) @@ -112,7 +107,7 @@ From a sourced workspace env (`cd humanoid_control_ws && pixi shell`): ros2 topic list # Who's publishing X? -ros2 topic info /control_mode --verbose +ros2 topic info /lite/joint_states --verbose # What's the QoS? ros2 topic info /safety_status --verbose @@ -121,7 +116,7 @@ ros2 topic info /safety_status --verbose ros2 topic hz /lite/joint_states # What service does X expose? -ros2 service list | grep mode_manager +ros2 service list | grep controller_manager ros2 service info /controller_manager/switch_controller ``` diff --git a/docs/reference/troubleshooting.md b/docs/reference/troubleshooting.md index 7a3ae5b..12cdf92 100644 --- a/docs/reference/troubleshooting.md +++ b/docs/reference/troubleshooting.md @@ -72,14 +72,15 @@ hard-fails when the resolved `joy_dev` path is missing. `/dev/input/js*` the launch could see; pass `joy_dev:=/dev/input/jsN` (matching one of them) on the launch command line. -3. **Headless / CI bringup.** Pass `enable_gamepad:=false` and drive - the FSM via the `/humanoid_control/mode/*` `std_srvs/Trigger` services +3. **Headless / CI bringup.** Pass `enable_gamepad:=false` and switch + controllers directly with + `ros2 control switch_controllers --activate --deactivate ` instead. ## Gamepad is connected and `cat /dev/input/js0` shows data, but `/joy` never publishes **Diagnosis**: `joy_node` comes up and registers a `/joy` publisher but -emits **zero messages**, so `mode_manager` never sees a button — even +emits **zero messages**, so `joy_teleop` never sees a button — even though the pad is paired and `cat /dev/input/js0` streams bytes when you press it. @@ -125,8 +126,8 @@ This is distinct from the `joy_dev` error above: there the device path is *missing*; here it *exists and reads fine with `cat`*, and only the SDL2/evdev permission is wrong. -**Headless / CI**: skip the pad with `enable_gamepad:=false` and drive -the FSM via the `/humanoid_control/mode/*` `std_srvs/Trigger` services instead. +**Headless / CI**: skip the pad with `enable_gamepad:=false` and switch +controllers directly with `ros2 control switch_controllers` instead. ## ENOBUFS / "Network is down" warnings during bringup @@ -166,20 +167,30 @@ The bit table: **Fix**: see [Recover from a fault](../how_to/recover_from_fault.md). -## `mode_manager` rejects an intent +## A gamepad button (or a switch) doesn't change the controller -**Diagnosis**: the FSM transition isn't allowed from your current -state. `mode_manager` writes the reason into -`/control_mode.status_message`: +**Diagnosis**: there is no FSM and no gating any more. `joy_teleop` maps +each button directly to `/controller_manager/switch_controller`, and every +transition is allowed from every state, so a button that does nothing is +almost always a config or load problem — not a rejected transition. + +**Why**: switching is flat and `BEST_EFFORT` — activate one controller, +deactivate its siblings. If nothing happens, either `joy_teleop` isn't +running / isn't receiving `/joy`, the button map in the `joy_teleop_*.yaml` +doesn't match the pad, or the target controller was never loaded. + +**Fix**: read the current state and switch by hand to confirm the target +controller exists: ``` -ros2 topic echo /control_mode -# status_message: "LOAD_A ignored; must be in DAMPING" +ros2 control list_controllers +ros2 control switch_controllers --activate --deactivate ``` -**Fix**: walk the legal path. `LOAD` requires DAMPING; `START_*` -requires STANDBY with `is_finished:true`; `QUIT` requires -ZERO_TORQUE or DAMPING. See [Five-mode FSM](../concepts/five_mode_fsm.md). +If the manual switch works but the button doesn't, the problem is in +`joy_teleop` (`/joy` silent, or the wrong button index in the YAML). Note +that `/control_mode` no longer exists — read the active mode from +`ros2 control list_controllers`. ## Spawner times out waiting for `/controller_manager/list_controllers` @@ -358,4 +369,4 @@ distributed clock never locks. - **Calibration drift**: [Calibrate the zero pose](../how_to/calibrate_zero_pose.md) - **Bus / qdisc nitty-gritty**: [Diagnose ENOBUFS](../how_to/diagnose_enobufs.md) - **Per-flag fault meaning + recovery**: [Recover from a fault](../how_to/recover_from_fault.md) -- **FSM transition rules**: [Five-mode FSM](../concepts/five_mode_fsm.md) +- **Controller switching (flat, via `joy_teleop` / `switch_controllers`)**: [Quick reference → Manual controller switching](./quick_reference.md#manual-controller-switching-no-fsm) diff --git a/docs/tutorials/build_your_own_controller.md b/docs/tutorials/build_your_own_controller.md index 29a060e..7413ca9 100644 --- a/docs/tutorials/build_your_own_controller.md +++ b/docs/tutorials/build_your_own_controller.md @@ -230,8 +230,9 @@ ros2 control list_controller_types | grep Hello ## Step 6 — Run it ```bash -# Bring up Lite with the FSM disabled so we can hand-load: -ros2 launch humanoid_bringup_lite mujoco.launch.py enable_mode_manager:=false +# Bring up Lite with joy_teleop disabled so its button bindings don't +# switch controllers out from under our hand-loaded one: +ros2 launch humanoid_bringup_lite mujoco.launch.py enable_joy_teleop:=false ``` In another terminal, load the controller via the CLI (inside @@ -299,7 +300,7 @@ Then `Ctrl+C` the launch. |---|---| | `humanoid_control/ZeroTorqueController` | The minimal case — claims all 5 MIT interfaces, writes 0. Best baseline. | | `humanoid_control/DampingController` | Captures state on activate; uses a YAML per-joint or scalar fallback. | -| `humanoid_control/StandbyController` | Multi-segment trajectory with K_p / K_d ramp; publishes its own state topic. | +| `humanoid_control/StandbyController` | Interpolates its setpoint from the current measured joint positions to a target pose at constant PD (no gain ramp); publishes its own state topic. | | `humanoid_control/RLPolicyController` | The in-process System 0 policy: preloads a `.mcap` motion reference and runs ONNX inference inside `update()`; observation packing + action mapping, all RT-safe. | | `humanoid_control/RemotePolicyController` | The System 1/2 external-command ingress: subscribes to an `MITCommand` topic; `RealtimeBuffer` for the RT handoff; arrival-time-based stale-command policy. | diff --git a/docs/tutorials/index.md b/docs/tutorials/index.md index aadffab..e1ddd25 100644 --- a/docs/tutorials/index.md +++ b/docs/tutorials/index.md @@ -18,9 +18,9 @@ and [Lite 101](../getting_started/lite_101.md) lesson, nothing more. | # | Tutorial | What you come away with | |---|---|---| | 1 | [Drive one Robstride end-to-end](./drive_one_robstride.md) | A single motor moving under our plugin, on bench hardware. Familiarity with the CAN bus, the MIT command surface, and the calibration math. | -| 2 | [MuJoCo + full FSM walkthrough](./mujoco_fsm_walk.md) | The whole five-mode FSM exercised in sim, with the gamepad. Comfort with `mode_manager` intents and the standby pose. | +| 2 | [MuJoCo + full FSM walkthrough](./mujoco_fsm_walk.md) | All five control modes exercised in sim, with the gamepad. Comfort with flat `joy_teleop` button switching and the standby pose. | | 3 | [Run a tracking policy](./tracking_policy.md) | A trained tracking ONNX policy driving the arms in sim. Familiarity with the `humanoid_control_policy` runner, the self-describing ONNX metadata, and the LeRobot dataset path. | -| 4 | [Run the gravity-compensation demo](./run_gravity_compensation.md) | The Lite arms holding against gravity, driven by a host-side Python loop over DDS. Familiarity with the Tier-3 external-client path, the REMOTE FSM mode, and `RemotePolicyController`. | +| 4 | [Run the gravity-compensation demo](./run_gravity_compensation.md) | The Lite arms holding against gravity, driven by a host-side Python loop over DDS. Familiarity with the Tier-3 external-client path, the REMOTE mode, and `RemotePolicyController`. | | 5 | [Build your own controller plugin](./build_your_own_controller.md) | A skeleton `humanoid_control/MyController` plugin loaded by the controller_manager. Familiarity with the `pluginlib` machinery and the `ControllerInterface` lifecycle. | :::tip[Tutorial vs how-to] diff --git a/docs/tutorials/mujoco_fsm_walk.md b/docs/tutorials/mujoco_fsm_walk.md index cbad12c..3ea9424 100644 --- a/docs/tutorials/mujoco_fsm_walk.md +++ b/docs/tutorials/mujoco_fsm_walk.md @@ -5,11 +5,21 @@ sidebar_position: 2 # Tutorial: MuJoCo + the full FSM, with a gamepad -A guided lesson on the five-mode FSM. You'll bring up Lite in MuJoCo, -plug in a gamepad, and walk every transition the FSM supports. -Because this is sim, no hardware can be damaged — push every button -and see what happens. By the end you'll know what each mode looks -like under physics and how the FSM gates work. +A guided lesson on the five control modes. You'll bring up Lite in +MuJoCo, plug in a gamepad, and drive every mode from the buttons. + +Switching is **flat and direct**: the stock `joy_teleop` node maps each +gamepad button straight to a `controller_manager/switch_controller` +call — activate one mode controller, deactivate its siblings. There is +**no state machine, no ordering, no gating**. Any mode reachable from +any state: you can jump straight from ZERO_TORQUE into LOCOMOTION, or +from a running policy back to STANDBY, in a single button press. +(The "FSM" in this page's title is historical; the five modes are the +same, only the arbitration is now this flat mapping.) + +Because this is sim, no hardware can be damaged — push every button and +see what happens. By the end you'll know what each mode looks like +under physics. This is **the** tutorial to do before any operator-driven session on real hardware. @@ -50,11 +60,15 @@ If `joy_node` errors with permissions, your user isn't in the ros2 launch humanoid_bringup_lite mujoco.launch.py # `enable_gamepad:=true` is already the default; the launch hard-fails # if no joystick is detected. Pass enable_gamepad:=false to bypass. +# `enable_joy_teleop:=true` is also default — it starts the joy_teleop +# node that maps buttons → switch_controller. Pass +# enable_joy_teleop:=false to bring the stack up with no button bindings. ``` Two windows / processes come up: - The MuJoCo viewer with Lite at zero pose. -- `joy_node` reading `/dev/input/js0`. +- `joy_node` reading `/dev/input/js0`, and `joy_teleop` translating its + buttons into controller switches. For an extra live URDF view, open a second terminal and run `ros2 run humanoid_bringup_lite rerun_viz` or @@ -67,33 +81,33 @@ cd humanoid_control_ws pixi shell ros2 control list_controllers # zero_torque_controller active -# (the four other FSM controllers loaded inactive) - -ros2 topic echo /control_mode -# mode: 0 (ZERO_TORQUE) -# controller_name: zero_torque_controller +# (the other mode controllers loaded inactive) ``` -Leave the `/control_mode` echo open — you'll watch it for every -transition. +There is no `/control_mode` topic — the active mode *is* whichever +controller `ros2 control list_controllers` reports as `active`. Keep +that command handy and re-run it after every button press to watch the +mode change. -## Step 2 — Mode 1: ZERO_TORQUE (start state) +## Step 2 — Mode: ZERO_TORQUE (start state) You're already here. The motors are alive but the controller writes 0 to every command interface. Under MuJoCo physics with gravity, the arms hang at their zero-pose; if you drag a joint in the viewer mouse interaction you can move it freely (no resistance). -This is the "alive but inert" state — the operator's safe default. +This is the "alive but inert" state — the operator's safe default, and +what **BACK** (STOP) returns you to. -## Step 3 — Transition: DAMP (any state → DAMPING) +## Step 3 — Press X: DAMP -Press **X** on the gamepad. +Press **X** on the gamepad. `joy_teleop` activates `damping_controller` +and deactivates whatever was running: -`/control_mode` should show: -``` -mode: 1 (DAMPING) -controller_name: damping_controller +```bash +ros2 control list_controllers +# damping_controller active +# zero_torque_controller inactive ``` Watch the MuJoCo arms: they now *resist* dragging. Stiffness is 0 @@ -101,53 +115,60 @@ so they don't actively pull back, but damping (default 1.0 N·m·s/rad) viscously opposes velocity. The arms sag under gravity but slowly. This is the **compliant fail-safe**. Any time you're worried, press -X. The FSM accepts DAMP from any state — even mid-policy. +X — it works from any state, even mid-policy, because the switch has no +preconditions. -## Step 4 — Transition: LOAD (DAMPING → STANDBY) +:::note[Headless equivalent] +Every button is just a `switch_controllers` call under the hood. With no +gamepad (or `enable_joy_teleop:=false`) you fire the exact same +transition yourself: -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 - interpolates to zero pose. You'll see the arms gently swing to - "arms straight down". -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 (watch the topic for the pose you loaded — here -Pose A): ```bash -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 -# is_finished: true ← the gate for the next transition opens here +ros2 control switch_controllers \ + --activate damping_controller \ + --deactivate zero_torque_controller ``` -The pose-ready arms should be visibly stiffer than DAMPING — try -dragging in MuJoCo, they'll pull back to the standby pose. +`joy_teleop` reads `joy_teleop_lite.yaml`, which maps each button to one +of these calls. +::: -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_A ignored; must be in DAMPING" +## Step 4 — Press L1 + A: STANDBY + +Press **L1 + A** (hold L1, press A) — this activates Pose A +(`standby_controller_a`). `L1 + B` and `L1 + Y` activate two other +poses (`standby_controller_b`, `standby_controller_y`). The three poses +are independent, and because switching is flat you can hop **directly** +between them — L1+A then L1+B swaps A → B with no DAMPING step in +between. + +`StandbyController` has **no gain ramp**: it applies its constant target +PD from the very first tick. It seeds its setpoint to the **current +measured joint positions**, then interpolates that setpoint to the +target pose over a couple of seconds. Because the setpoint starts at +where the arms actually are, there is **no jump** — this makes STANDBY +safe to enter from any state, including from a running policy. + +```bash +ros2 control list_controllers +# standby_controller_a active ``` -That's the gating — LOAD is only legal from DAMPING. +Watch the MuJoCo arms glide from wherever they were into the piano-ready +pose (shoulders rolled out, elbows bent in). They should be visibly +stiffer than DAMPING — try dragging in MuJoCo, they'll pull back to the +standby pose. You don't have to wait for any "finished" signal before +the next switch: there is no gate. Press L1+A again from STANDBY and it +just re-seeds and re-runs the interpolation. -## Step 5 — Transition: START_REMOTE (STANDBY → REMOTE) +## Step 5 — Press R1 + B: REMOTE -**Wait for `is_finished:true`** in `/standby_controller_a/state` first -(the topic for the pose you loaded). Then press **R1 + B**. +Press **R1 + B**. `remote_policy_controller` goes active immediately — +no need to be in STANDBY first, no `is_finished` gate: -`/control_mode` shows: -``` -mode: 4 (REMOTE) -controller_name: remote_policy_controller +```bash +ros2 control list_controllers +# remote_policy_controller active ``` `remote_policy_controller` is now claiming the command interfaces @@ -190,49 +211,64 @@ 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) +## Step 6 — Press X: back to DAMP -Press **X** again. The motors are now compliant. From DAMPING you -can go anywhere — that's the safety guarantee. +Press **X** again. The motors are now compliant. Because switching is +flat, this works out of REMOTE (or any mode) with no ordering +requirement — X always lands you in DAMPING. -## Step 7 — Transition: QUIT (DAMPING → exit) +## Step 7 — Press BACK: STOP -Press **BACK**. +Press **BACK**. `joy_teleop` activates `zero_torque_controller`: -`mode_manager` shuts down (`rclcpp::shutdown()`). Watch the launch -terminal: most of the stack tears down. The launched processes -finish in their normal `on_deactivate` order, and the MuJoCo viewer -window closes. - -**Try QUIT from STANDBY or REMOTE.** It's rejected: -``` -status_message: "QUIT rejected; must be in ZERO_TORQUE or DAMPING" +```bash +ros2 control list_controllers +# zero_torque_controller active ``` -The FSM forces a DAMP first. This is the safety property: you can't -exit while a policy is driving the arms. +STOP is **not** a shutdown. `zero_torque_controller` stays enabled and +holds zero torque on every joint — you're back at the Step 2 start +state, and you can press any other button to leave it again. There is no +button that tears the stack down. -## Step 8 — Try the locomotion combos +To actually exit, `Ctrl+C` the launch terminal. The hardware plugin's +`on_deactivate` runs, sending CAN **Disable** to every joint (a no-op in +MuJoCo but the real safety stop on silicon), and `mujoco_sim` shuts down +cleanly. -Relaunch and walk LOAD → START_LOCOMOTION (`L1+A` then `R1+A`). -With no ONNX policy prepared (or a build without -`onnxruntime`), `RLPolicyController` falls back to `PlaceholderPolicy` -(zero actions), so this transitions to LOCOMOTION but the motors just -stay where they are. Useful to verify the FSM gating works -symmetrically — try START_LOCOMOTION before STANDBY has -`is_finished:true` and it'll reject: -``` -status_message: "START_LOCOMOTION rejected; standby not finished yet" +## Step 8 — Go straight to LOCOMOTION + +To prove the flat switching, don't walk up through the modes at all. +From ZERO_TORQUE (or literally any mode), press **R1 + A**: + +```bash +ros2 control list_controllers +# rl_policy_controller active ``` +`rl_policy_controller` (LOCOMOTION) is entered **directly, at full +authority** — no soft-start, no required STANDBY, no `is_finished` +precondition. With no ONNX policy prepared (or a build without +`onnxruntime`), `RLPolicyController` falls back to `PlaceholderPolicy` +(zero actions), so the motors just stay where they are; the point here +is that the switch itself is unconditional. + +If a mode controller ever errors (e.g. the policy emits a non-finite +action), it doesn't need the operator or a safety topic to catch it: the +controller_manager's native `fallback_controllers` fires automatically — +each mode controller falls back to `damping_controller`, and +`damping_controller` falls back to `zero_torque_controller`. The +`/safety_status` topic is telemetry you can watch, not a trigger that +switches modes for you. + ## What you came away with | Skill | Page where it's documented in full | |---|---| -| The five FSM modes and what each writes | [Concepts → Five-mode FSM](../concepts/five_mode_fsm.md) | -| Gating logic (LOAD only from DAMPING, START_* gated on is_finished) | same | -| The auto-DAMP safety path | [Concepts → Safety pipeline](../concepts/safety_pipeline.md) | -| Gamepad button → intent mapping | [Quick reference](../reference/quick_reference.md) | +| The five control modes and what each writes | [Concepts → Five control modes](../concepts/five_mode_fsm.md) | +| Flat `joy_teleop` button → `switch_controller` mapping (no ordering) | [Quick reference](../reference/quick_reference.md) | +| Native fault fallback (mode → damping → zero_torque) | [Concepts → Safety pipeline](../concepts/safety_pipeline.md) | +| Gamepad button → controller mapping | [Quick reference](../reference/quick_reference.md) | | The MIT publish path | [`MITCommand` schema](../reference/messages.md) | ## Next @@ -240,4 +276,4 @@ status_message: "START_LOCOMOTION rejected; standby not finished yet" - [Tutorials → Run a tracking policy](./tracking_policy.md) — drive LOCOMOTION with a real in-process ONNX policy. - [How-to → First real-hardware bringup](../how_to/first_real_bringup.md) - — same FSM, but on silicon. + — same modes, same buttons, but on silicon. diff --git a/docs/tutorials/run_gravity_compensation.md b/docs/tutorials/run_gravity_compensation.md index e897f12..a4ce467 100644 --- a/docs/tutorials/run_gravity_compensation.md +++ b/docs/tutorials/run_gravity_compensation.md @@ -7,10 +7,10 @@ sidebar_position: 4 Drive the Lite arms from a **host-side Python process** — no `rclpy`, no colcon overlay, no `--system-site-packages`. You'll bring up the stack -(MuJoCo or real), walk the FSM into **REMOTE**, and run an external -gravity-compensation loop that reads joint states and publishes +(MuJoCo or real), switch to the **REMOTE** control mode, and run an +external gravity-compensation loop that reads joint states and publishes `MITCommand` back over raw DDS. By the end you'll understand the -**Tier-3 external-client path**, the REMOTE mode of the five-mode FSM, +**Tier-3 external-client path**, the REMOTE control mode, and the difference between the torque-mode and PD-mode gravity loops. This is the worked, end-to-end companion to @@ -101,36 +101,32 @@ ros2 launch humanoid_bringup_lite mujoco.launch.py Wait for `zero_torque_controller` to come active. (For real hardware, use `real.launch.py` instead — everything downstream is identical.) -## Step 3 — Walk the FSM into REMOTE +## Step 3 — Switch to REMOTE -`RemotePolicyController` only accepts commands in the **REMOTE** mode, -and the FSM only enters REMOTE from a *finished* STANDBY. Drive the -transitions through `mode_manager`'s trigger services (in a second -terminal, inside `pixi shell`): +`RemotePolicyController` accepts commands only while it is the active +controller — i.e. in the **REMOTE** mode. Switching is flat, so you can +enter REMOTE directly from ZERO_TORQUE (or any mode), with no DAMPING or +STANDBY step first. Press **R1+B** on the gamepad, or, headless, make the +same `switch_controllers` call yourself (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_a std_srvs/srv/Trigger # → STANDBY, Pose A (gamepad: L1+A) - -# Wait until the standby ramp reports finished: -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) +ros2 control switch_controllers \ + --activate remote_policy_controller \ + --deactivate zero_torque_controller ``` -:::caution[`start_remote` is R1+B, not R1+A] -**R1+A** starts LOCOMOTION (the in-process `rl_policy_controller`); -**R1+B** starts REMOTE (`remote_policy_controller`). Use the `B` -combo / the `start_remote` service for this demo. +:::caution[R1+B is REMOTE, not R1+A] +On the gamepad, **R1+A** starts LOCOMOTION (the in-process +`rl_policy_controller`) and **R1+B** starts REMOTE +(`remote_policy_controller`). Use the **B** combo for this demo. ::: Confirm REMOTE is active before running the loop: ```bash -ros2 topic echo --once /control_mode -# mode: 4 (REMOTE) -# controller_name: remote_policy_controller +ros2 control list_controllers +# remote_policy_controller humanoid_control/RemotePolicyController active ``` ## Step 4 — Run the gravity-comp loop (torque mode) @@ -178,8 +174,8 @@ rejects a joint-order mismatch. :::tip[No bringup, no robot] `python run_mujoco.py` opens the demo's MuJoCo model in a passive viewer -with the same gravity compensation applied in-process — no DDS, no FSM. -Handy for sanity-checking the model alone. +with the same gravity compensation applied in-process — no DDS, no +bringup. Handy for sanity-checking the model alone. ::: ## Step 6 — Safe stop @@ -191,10 +187,13 @@ in REMOTE, and `RemotePolicyController`'s stale-command fallback (~100 ms of silence) treats the now-quiet topic as a fault and falls back to damping. -For a deliberate stop, drive the FSM back down first: +For a deliberate stop, switch out of REMOTE first — press **X** on the +gamepad, or headless: ```bash -ros2 service call /humanoid_control/mode/damp std_srvs/srv/Trigger # gamepad: X +ros2 control switch_controllers \ + --activate damping_controller \ + --deactivate remote_policy_controller ``` Then Ctrl-C the runner, then the bringup launch. @@ -205,7 +204,7 @@ Then Ctrl-C the runner, then the bringup launch. |---|---| | The Tier-3 external-client (no-`rclpy`) DDS path | [How-to → Talk to Humanoid Control from Python](../how_to/talk_to_humanoid_control_from_python.md) | | `lite_sdk2` / `humanoid_control_msgs_dds` topic + QoS registry | [Reference → Packages](../reference/packages.md#humanoid_control_msgs_dds) | -| The REMOTE mode + `start_remote` transition | [Concepts → Five-mode FSM](../concepts/five_mode_fsm.md) | +| The REMOTE mode + the R1+B controller switch | [Concepts → Five control modes](../concepts/five_mode_fsm.md) | | `RemotePolicyController` and the MIT command write | [Reference → Controllers](../reference/controllers.md) | | The five MIT command interfaces (torque vs PD encoding) | [Concepts → MIT command surface](../concepts/mit_command_surface.md) | @@ -219,4 +218,4 @@ Then Ctrl-C the runner, then the bringup launch. whole command surface, and how torque- and PD-mode gravity comp map onto it. - [How-to → Switch controllers manually](../how_to/switch_controllers_manually.md) - — the lower-level path if you'd rather bypass `mode_manager`. + — more on the `ros2 control switch_controllers` path used above. diff --git a/docs/tutorials/tracking_policy.md b/docs/tutorials/tracking_policy.md index cda6c3f..fec8aa6 100644 --- a/docs/tutorials/tracking_policy.md +++ b/docs/tutorials/tracking_policy.md @@ -80,34 +80,34 @@ ros2 launch humanoid_bringup_lite mujoco.launch.py Wait for `zero_torque_controller` to come active. -## Step 2 — Walk to STANDBY +## Step 2 — Switch to STANDBY -`rl_policy_controller` expects the arms in a sane starting pose. -Walk the FSM up to STANDBY. In a second terminal, drop into the env: +`rl_policy_controller` expects the arms in a sane starting pose, so put +them in a STANDBY pose first. This isn't strictly required — switching +is flat and you could activate the policy directly from any mode — but +starting from the standby pose gives the tracking policy a clean initial +condition. In a second terminal, drop into the env: ```bash cd humanoid_control_ws pixi shell ``` -Then drive the FSM through `mode_manager`'s trigger services (the same -transitions the gamepad would fire): +Then switch to `standby_controller_a`. Press **L1+A** on the gamepad +(`L1+B` / `L1+Y` load alternate poses; any of them can start either +policy), or make the same call by hand: ```bash -# Gamepad equivalents, if you have one: -# X → DAMP -# 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_a std_srvs/srv/Trigger - -# Wait for is_finished: -ros2 topic echo /standby_controller_a/state -# ... is_finished: true +ros2 control switch_controllers \ + --activate standby_controller_a \ + --deactivate zero_torque_controller ``` +`StandbyController` seeds its setpoint to the current measured joint +positions and interpolates to the pose over a couple of seconds — no +jump, no gain ramp. Give it a moment to settle; there is no "finished" +gate you have to wait for before the next switch. + ## Step 3 — Prepare and load the policy In a third terminal: @@ -159,30 +159,33 @@ changes physically. The in-process `RLPolicyController` does its own ONNX inference and reads `/lite/joint_states` and `/imu/data` locally once activated — there is no command topic to publish. -## Step 4 — STANDBY → START_LOCOMOTION +## Step 4 — Activate LOCOMOTION -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`): +Now switch to the policy. The RL policy is entered **directly, at full +authority** — no soft-start, and no precondition on STANDBY (we used +STANDBY only to get a clean starting pose). Press **R1 + A** on the +gamepad, or, in the same terminal: ```bash -ros2 service call /humanoid_control/mode/start_locomotion std_srvs/srv/Trigger +ros2 control switch_controllers \ + --activate rl_policy_controller \ + --deactivate standby_controller_a ``` -(Gamepad equivalent: **R1 + A**.) `mode_manager` switches the active -controller to `rl_policy_controller`. The motors immediately track the -policy — in MuJoCo you'll see the arms move through the tracking dataset. +The controller_manager makes `rl_policy_controller` active. The motors +immediately track the policy — in MuJoCo you'll see the arms move +through the tracking dataset. Verify the controller is active: ```bash -ros2 topic echo --once /control_mode -# mode: 3 (LOCOMOTION) -# controller_name: rl_policy_controller +ros2 control list_controllers +# rl_policy_controller humanoid_control/RLPolicyController active ``` If the ONNX returns a non-finite action, `RLPolicyController` returns -`ERROR` and the controller_manager falls back to `damping_controller` +`ERROR` and the controller_manager's native `fallback_controllers` drops +to `damping_controller` (see [Concepts → Safety pipeline](../concepts/safety_pipeline.md)). ## Step 5 — Inspect the data flow @@ -193,7 +196,7 @@ two always-on streams the controller reads locally: ```bash ros2 topic hz /lite/joint_states # joint state broadcaster (RT update rate) ros2 topic hz /imu/data # IMU, if the observation uses it -ros2 topic echo --once /control_mode # confirm LOCOMOTION / rl_policy_controller +ros2 control list_controllers # confirm rl_policy_controller active ``` There is no `MITCommand` stream to inspect: inference, motion replay, @@ -201,15 +204,18 @@ and the command write all happen inside the RT `update()`. ## Step 6 — Shut down -DAMP first, then exit (still in the `pixi shell` terminal): +Switch to DAMP first, then exit (still in the `pixi shell` terminal) — +press **X** on the gamepad, or: ```bash -ros2 service call /humanoid_control/mode/damp std_srvs/srv/Trigger +ros2 control switch_controllers \ + --activate damping_controller \ + --deactivate rl_policy_controller # Then Ctrl+C the policy launch, then the bringup launch. ``` -DAMP switches off `rl_policy_controller` and activates +That switches off `rl_policy_controller` and activates `damping_controller`; the launch's `on_deactivate` cascades through the controllers. @@ -221,7 +227,7 @@ controllers. | The launch-time `prepare` step + ONNX metadata fields | same | | LeRobot motion → `.mcap` bag conversion | same | | The in-process `RLPolicyController` lifecycle | [Reference → Controllers](../reference/controllers.md) | -| The five-mode FSM + `START_LOCOMOTION` | [Concepts → Five-mode FSM](../concepts/five_mode_fsm.md) | +| The five control modes + the R1+A controller switch | [Concepts → Five control modes](../concepts/five_mode_fsm.md) | ## Next diff --git a/scripts/draw_diagrams.py b/scripts/draw_diagrams.py index b241efe..aa440bd 100644 --- a/scripts/draw_diagrams.py +++ b/scripts/draw_diagrams.py @@ -229,9 +229,27 @@ def group_box(x: int, y: int, w: int, h: int, title_: str, *, return f"{body}\n{label}" +def _recolor(svg: str) -> str: + """Map the legacy Berkeley blue/gold palette to the site's orange theme. + + The plain-orange redesign recolored the SVGs by editing them directly and + never updated this generator, so a naive re-run would revert the theme. + This bakes the exact same attribute-aware substitution back in (verified to + reproduce every committed diagram byte-for-byte), so the script self-produces + the orange theme. To retheme, change only these five rules — the diagram + functions keep using the semantic BLUE/GOLD/... constants. + """ + return (svg + .replace('stroke="#003262"', 'stroke="#ff6633"') # BLUE stroke -> orange + .replace('fill="#003262"', 'fill="#cc4a1f"') # BLUE text -> dark orange + .replace('#FDB515', '#5f6671') # GOLD -> slate + .replace('#E7F0FF', '#ffece2') # BLUE_FILL -> light orange + .replace('#FFF6DF', '#eef0f2')) # GOLD_FILL -> light slate + + def write_svg(name: str, content: str) -> None: path = OUT / name - path.write_text(content, encoding="utf-8") + path.write_text(_recolor(content), encoding="utf-8") print(f" wrote {path.relative_to(ROOT)}") @@ -262,7 +280,7 @@ def d_intro_01_system() -> None: fill=BLUE_FILL, stroke=BLUE).render()) s.append(Box(340, 170, 220, 60, "controller_manager (50 Hz)", fill=BLUE_FILL, stroke=BLUE).render()) - s.append(Box(340, 250, 100, 50, "mode_manager", + s.append(Box(340, 250, 100, 50, "joy_teleop", fill=BLUE_FILL, stroke=BLUE).render()) s.append(Box(460, 250, 100, 50, "humanoid_control_policy", fill=GOLD_FILL, stroke=GOLD).render()) @@ -291,11 +309,10 @@ def d_intro_01_system() -> None: label_offset=-12, color=GREY)) # Onboard internal s.append(arrow(450, 150, 450, 170)) # bringup -> CM - s.append(arrow(390, 230, 390, 250)) # CM -> mode_manager + s.append(arrow(390, 230, 390, 250)) # CM -> joy_teleop s.append(arrow(510, 230, 510, 250)) # CM -> policy - s.append(arrow(390, 300, 390, 320)) # mode_manager -> hw plugin (vertically aligned) s.append(arrow(510, 300, 510, 320)) # policy -> hw plugin - # mode_manager -> CM (loop back) + # joy_teleop -> CM (switch_controller loop back) s.append(polyline([(345, 275), (320, 275), (320, 200), (340, 200)])) # Onboard -> robot (CAN) s.append(arrow(560, 350, 690, 350, label="MIT-mode CAN", @@ -329,7 +346,7 @@ def pkg(x, y, name, sub=None, fill=BLUE_FILL, stroke=BLUE): # Shared column pkg(40, 90, "humanoid_control_common", "RT helpers, MITState POD") pkg(40, 144, "humanoid_control_msgs", "MITCommand, ControlMode, ...") - pkg(40, 198, "humanoid_controllers", "5 mode-FSM + mode_manager") + pkg(40, 198, "humanoid_controllers", "5 mode controllers + MIT traj") pkg(40, 252, "humanoid_control_policy", "ONNX runner + LeRobot ref") pkg(40, 306, "humanoid_drivers_socketcan", "SocketCAN bus library") @@ -604,70 +621,57 @@ def msg(x1, y, x2, label, color=TEXT): def d_sf_fsm() -> None: - """5-mode FSM.""" - W, H = 940, 460 + """Five control modes + flat joy_teleop switching.""" + W, H = 940, 480 s = [header(W, H)] - s.append(title(20, 30, "Five-mode FSM", - sub="Only one controller is active at a time; joint_state_broadcaster always runs")) + s.append(title(20, 28, "Five control modes", + sub="Each button → switch_controller directly; any " + "transition from any state (BEST_EFFORT).")) - # Nodes - def state(x, y, label, color=BLUE): - fill = BLUE_FILL if color == BLUE else ( - GREEN_FILL if color == GREEN else - GOLD_FILL if color == GOLD else - RED_FILL if color == RED else LIGHT) - s.append(Box(x, y, 200, 56, label, fill=fill, stroke=color).render()) - - state(60, 100, "ZERO_TORQUE", GREEN) - state(60, 250, "DAMPING", GREEN) - state(360, 250, "STANDBY", BLUE) - state(660, 150, "LOCOMOTION", GOLD) - state(660, 320, "REMOTE", GOLD) - - # Edges - def edge(p1, p2, label, color=TEXT): - x1, y1 = p1 - x2, y2 = p2 - s.append(arrow(x1, y1, x2, y2, color=color)) - mx = (x1 + x2) // 2 - my = (y1 + y2) // 2 - 8 - s.append(label_pill(mx, my, label)) - - # ZERO_TORQUE <-> DAMPING - edge((130, 156), (130, 250), "DAMP (X / Ctrl+C)") - edge((180, 250), (180, 156), "manual") - - # DAMPING -> STANDBY - edge((260, 278), (360, 278), "LOAD (L1+A|B)") - # STANDBY -> DAMPING - edge((360, 290), (260, 290), "DAMP", color=GREY) - # STANDBY -> LOCOMOTION (gated on is_finished) - edge((560, 260), (660, 178), "START_LOCOMOTION (R1+A)") - s.append(text(610, 220, "+is_finished", size=10, fill=GREY, - anchor="middle")) - # STANDBY -> REMOTE (gated on is_finished) - edge((560, 290), (660, 348), "START_REMOTE (R1+B)") - # LOCOMOTION / REMOTE -> DAMPING - s.append(polyline([(660, 178), (550, 178), (550, 230), (450, 230), - (260, 230)], color=GREY)) - s.append(label_pill(450, 215, "DAMP / fault")) - s.append(polyline([(660, 348), (550, 348), (550, 320), (450, 320), - (260, 320)], color=GREY)) - s.append(label_pill(440, 305, "DAMP / fault")) - - # Quit edges - s.append(arrow(60, 130, 30, 130, color=GREY)) - s.append(text(30, 118, "QUIT", size=10, fill=GREY, anchor="end")) - s.append(arrow(60, 280, 30, 280, color=GREY)) - s.append(text(30, 268, "QUIT", size=10, fill=GREY, anchor="end")) + # joy_teleop hub (left). + s.append(Box(45, 210, 185, 66, + ["joy_teleop", "/joy → switch_controller"], + fill=BLUE_FILL, stroke=BLUE).render()) - # Legend - s.append(text(820, 410, "Green = safe / fail-safe", size=11, - anchor="end", fill=GREEN)) - s.append(text(820, 425, "Blue = transitional", size=11, anchor="end", - fill=BLUE)) - s.append(text(820, 440, "Gold = active policy", size=11, anchor="end", - fill=GOLD)) + # Controller boxes (right), one per mode. + def state(y, label, color, sub=None): + fill = (GREEN_FILL if color == GREEN else + GOLD_FILL if color == GOLD else + BLUE_FILL if color == BLUE else LIGHT) + s.append(Box(560, y, 250, 52, label, sub=sub, fill=fill, + stroke=color).render()) + return (560, y + 26) # left-edge anchor + + zt = state(64, "zero_torque_controller", GREEN, sub="BACK = STOP") + dp = state(142, "damping_controller", GREEN) + sb = state(220, "standby_controller_a / b / y", BLUE) + lo = state(298, "rl_policy_controller", GOLD, sub="LOCOMOTION") + rm = state(376, "remote_policy_controller", GOLD, sub="REMOTE") + + # Fan-out edges from joy_teleop to each controller, labelled by button. + hub = (230, 243) + for (tx, ty), lbl in [(zt, "BACK"), (dp, "X"), (sb, "L1 + A / B / Y"), + (lo, "R1 + A"), (rm, "R1 + B")]: + s.append(arrow(hub[0], hub[1], tx - 4, ty)) + s.append(label_pill((hub[0] + tx) // 2 + 20, + (hub[1] + ty) // 2 - 8, lbl)) + + # Fault path note (native fallback, not a joy button). Short lines kept + # left of the button pills (~x380). + for i, line in enumerate([ + "Fault path (native, not a joy button):", + "a controller update() → ERROR is dropped", + "to damping via fallback_controllers.", + "/safety_status is telemetry (no auto-DAMP)."]): + s.append(text(46, 372 + i * 15, line, size=11, fill=GREY, + anchor="start")) + + # Legend (by role, not colour name), bottom-right, clear of the boxes. + s.append(text(810, 444, "safe / fail-safe", size=11, anchor="end", + fill=GREEN)) + s.append(text(810, 459, "standby (interpolate to pose)", size=11, + anchor="end", fill=BLUE)) + s.append(text(810, 474, "active policy", size=11, anchor="end", fill=GOLD)) s.append(footer()) write_svg("concepts__five_mode_fsm__01.svg", "".join(s)) @@ -756,7 +760,7 @@ def d_lite_mock_launch() -> None: ("xacro", 280, GREY), ("mujoco_sim", 460, BLUE), ("controller_manager", 640, BLUE), - ("mode_manager", 820, GOLD), + ("joy_teleop", 820, GOLD), ] lt, lb = 80, 510 for name, x, color in lanes: @@ -779,11 +783,11 @@ def step(y, x1, x2, label, color=TEXT): step(280, 100, 640, "spawn jsb (active)") step(310, 100, 640, "spawn zero_torque (active)") step(340, 100, 640, "spawn 4 others (inactive)") - step(380, 100, 820, "start mode_manager") - step(410, 820, 640, "switch_controller(zero_torque)") + step(380, 100, 820, "start joy_teleop (waits for /joy)") s.append(text(W // 2, 460, - "System is now at ZERO_TORQUE, publishing /joint_states at 50 Hz", + "System is now at ZERO_TORQUE (from the spawner), publishing " + "/joint_states at 50 Hz", size=12, weight=600, fill=BLUE, anchor="middle")) s.append(footer()) @@ -832,7 +836,7 @@ def d_lite_mujoco_internals() -> None: fill=LIGHT, stroke=GREY).render()) s.append(Box(310, 380, 220, 40, "controller spawners", fill=LIGHT, stroke=GREY).render()) - s.append(Box(560, 380, 180, 40, "mode_manager", + s.append(Box(560, 380, 180, 40, "joy_teleop", fill=LIGHT, stroke=GREY).render()) s.append(arrow(180, 380, 180, 290, color=GREY, dashed=True)) s.append(label_pill(180, 340, "/robot_description")) @@ -894,7 +898,7 @@ def d_msgs_pubsub() -> None: W, H = 920, 480 s = [header(W, H)] s.append(title(20, 30, "humanoid_control_msgs pub/sub topology", - sub="Who publishes / who subscribes for each of the 4 active topics")) + sub="Who publishes / who subscribes for each active topic")) # 3 columns: publishers / topics / subscribers s.append(group_box(20, 70, 230, 380, "Publishers")) @@ -908,44 +912,30 @@ def pub(x, y, label, color=BLUE): RED_FILL if color == RED else LIGHT) s.append(Box(x, y, 200, 40, label, fill=fill, stroke=color).render()) - pub(35, 100, "StandbyController", BLUE) - pub(35, 160, "mode_manager", GOLD) - pub(35, 220, "Hardware plugins", RED) - pub(35, 280, "Active policy ctrls", BLUE) - pub(35, 340, "humanoid_control_policy (Python)", GOLD) + pub(35, 110, "StandbyController", BLUE) + pub(35, 200, "Hardware plugins", RED) + pub(35, 290, "policy source (ctrl / VLA)", BLUE) - pub(350, 100, "/standby_controller/state", GREEN) - pub(350, 160, "/control_mode", GREEN) - pub(350, 220, "/safety_status", GREEN) - pub(350, 280, "~/command (MITCommand)", GREEN) + pub(350, 110, "/standby_controller/state", GREEN) + pub(350, 200, "/safety_status", GREEN) + pub(350, 290, "~/command (MITCommand)", GREEN) - pub(680, 100, "mode_manager", GOLD) - pub(680, 160, "diagnostics / log", GREY) - pub(680, 220, "mode_manager", GOLD) - pub(680, 280, "RemotePolicyController", BLUE) + pub(680, 110, "(none - telemetry)", GREY) + pub(680, 200, "(none - telemetry)", GREY) + pub(680, 290, "RemotePolicyController", BLUE) - # Arrows - pairs = [ - (130, 130, 350, 130), # StandbyCtrl -> /standby_controller/state - (130, 200, 350, 190), # mode_manager -> /control_mode - (130, 270, 350, 250), # HW plugins -> /safety_status - (130, 270, 350, 250), - (130, 380, 350, 310), # humanoid_control_policy -> ~/command - # topics -> subs - (560, 130, 680, 130), # standby/state -> mode_manager - (560, 190, 680, 190), # control_mode -> log - (560, 250, 680, 250), # safety_status -> mode_manager - (560, 310, 680, 310), # ~/command -> RemotePolicy - ] - seen = set() - for x1, y1, x2, y2 in pairs: - k = (x1, y1, x2, y2) - if k in seen: continue - seen.add(k) - s.append(arrow(x1, y1, x2, y2, color=GREY)) + # Arrows: publisher -> topic -> subscriber, one row each. + for y in (130, 220, 310): + s.append(arrow(235, y, 350, y, color=GREY)) + s.append(arrow(550, y, 680, y, color=GREY)) - s.append(text(W // 2, 460, - "/standby_controller/state uses TRANSIENT_LOCAL QoS so a late mode_manager sees the last is_finished", + s.append(text(W // 2, 462, + "ControlMode still ships in humanoid_control_msgs (DDS wire " + "bridge); /control_mode is no longer published.", + size=11, fill=GREY, anchor="middle")) + s.append(text(W // 2, 476, + "Read the active mode from list_controllers. " + "/standby_controller/state and /safety_status are telemetry.", size=11, fill=GREY, anchor="middle")) s.append(footer()) @@ -983,7 +973,7 @@ def d_arch_module_deps() -> None: # Controllers + policies s.append(Box(70, 195, 200, 50, "humanoid_controllers", - sub="5 modes + mode_manager", + sub="5 mode controllers + MIT traj", fill=BLUE_FILL, stroke=BLUE).render()) s.append(Box(310, 195, 200, 50, "humanoid_control_policy", sub="ONNX runner (Python)", @@ -1244,13 +1234,13 @@ def d_safety_pipeline() -> None: """Safety pipeline — fault to DAMPING in one tick.""" W, H = 1000, 500 s = [header(W, H)] - s.append(title(20, 30, "Safety pipeline — fault to DAMPING in ≤1 tick", - sub="Detection (plugin) → telemetry (latched topic) → response (mode_manager)")) + s.append(title(20, 30, "Safety pipeline — detection, telemetry, native fallback", + sub="Detection (plugin) → telemetry (latched topic) → operator + fallback_controllers")) # Three layers, left to right s.append(group_box(40, 70, 280, 380, "Layer 1: detection (plugin)")) s.append(group_box(360, 70, 240, 380, "Layer 2: telemetry")) - s.append(group_box(640, 70, 320, 380, "Layer 3: response")) + s.append(group_box(640, 70, 320, 380, "Layer 3: native fallback + operator")) # Fault sources (left column) def fault(y, name, src): @@ -1281,32 +1271,40 @@ def fault(y, name, src): s.append(arrow(480, 270, 480, 290, color=GREY)) s.append(arrow(480, 330, 480, 360, color=GREY)) - # Response (right column) - s.append(Box(660, 100, 280, 40, "mode_manager.on_safety()", + # Response (right column): /safety_status is telemetry (no auto-DAMP); + # the automatic path is native fallback_controllers on a controller error. + s.append(Box(660, 100, 280, 44, + ["/safety_status = telemetry", "rqt / logs / operator"], + fill=GREY_FILL, stroke=GREY).render()) + s.append(text(800, 168, "No auto-DAMP node — the operator reacts", + size=11, anchor="middle", fill=GREY)) + s.append(text(800, 183, "(BACK = STOP / X = DAMP).", + size=11, anchor="middle", fill=GREY)) + + s.append(Box(660, 210, 280, 44, + ["controller update() → ERROR", "(e.g. non-finite obs)"], + fill=RED_FILL, stroke=RED).render()) + s.append(Box(660, 282, 280, 38, "controller_manager: fallback_controllers", fill=BLUE_FILL, stroke=BLUE).render()) - s.append(Box(660, 160, 280, 40, "level == OK?", - fill=LIGHT, stroke=GREY).render()) - s.append(Box(660, 220, 280, 40, "request_mode(Mode::Damping)", + s.append(Box(660, 340, 280, 34, "damping_controller", fill=GOLD_FILL, stroke=GOLD).render()) - s.append(Box(660, 280, 280, 40, "if STRICT switch fails:", - fill=LIGHT, stroke=GREY).render()) - s.append(Box(660, 330, 280, 40, "request_mode(Mode::ZeroTorque)", + s.append(Box(660, 392, 280, 34, "zero_torque_controller", fill=GREEN_FILL, stroke=GREEN).render()) - s.append(Box(660, 390, 280, 36, "controller_manager: switch_controller", - fill=BLUE_FILL, stroke=BLUE).render()) - s.append(arrow(800, 140, 800, 160)) - s.append(arrow(800, 200, 800, 220, label="non-OK", label_offset=8)) - s.append(arrow(800, 260, 800, 280, label="fallback", label_offset=8)) - s.append(arrow(800, 320, 800, 330)) - s.append(arrow(800, 370, 800, 390)) + s.append(arrow(800, 254, 800, 282)) + s.append(arrow(800, 320, 800, 340, label="activate", label_offset=8)) + s.append(arrow(800, 374, 800, 392, label="its fallback", label_offset=8)) # Cross-column arrows s.append(arrow(300, 250, 380, 200, label="aggregate", label_offset=-12)) - s.append(arrow(580, 385, 660, 120, label="subscribe", label_offset=-12)) + s.append(arrow(580, 385, 660, 122, label="publish", label_offset=-12)) # Note - s.append(text(W // 2, 470, - "BUS_OFF is the one sticky flag — clears only on the next on_activate(), because EMI bursts shouldn't auto-recover.", + s.append(text(W // 2, 466, + "The automatic path is native fallback_controllers (controller " + "ERROR → damping → zero_torque), independent of /safety_status.", + size=11, anchor="middle", fill=GREY)) + s.append(text(W // 2, 482, + "BUS_OFF is a sticky flag cleared only on the next on_activate().", size=11, anchor="middle", fill=GREY)) s.append(footer()) @@ -1449,7 +1447,7 @@ def d_real_bringup_spawn() -> None: ("launch", 240, GREY), ("ros2_control_node", 420, BLUE), ("RobstrideSystem", 600, GREEN), - ("mode_manager", 780, GOLD), + ("joy_teleop", 780, GOLD), ] lt, lb = 70, 550 for name, x, color in lanes: @@ -1474,12 +1472,11 @@ def step(y, x1, x2, label, color=TEXT): step(360, 240, 420, "spawn joint_state_broadcaster (active)") step(390, 240, 420, "spawn zero_torque (active)") step(420, 240, 420, "spawn damping/standby/rl/remote (inactive)") - step(460, 240, 780, "start mode_manager") - step(490, 780, 420, "switch_controller(zero_torque, asap)") - step(520, 780, 420, "subscribe /safety_status, /joy") + step(460, 240, 780, "start joy_teleop (reads /joy)") + step(490, 780, 420, "switch_controller on button press", color=GREY) s.append(text(W // 2, 565, - "After this sequence: motors compliant, /joint_states @ 50 Hz, /control_mode publishes ZERO_TORQUE.", + "After this sequence: motors compliant, /joint_states @ 50 Hz, at ZERO_TORQUE (from the spawner).", size=12, weight=600, fill=BLUE, anchor="middle")) s.append(footer()) diff --git a/static/img/diagrams/concepts__architecture__03_module_deps.svg b/static/img/diagrams/concepts__architecture__03_module_deps.svg index d4fc9de..09088fc 100644 --- a/static/img/diagrams/concepts__architecture__03_module_deps.svg +++ b/static/img/diagrams/concepts__architecture__03_module_deps.svg @@ -26,7 +26,7 @@ pianist_policy piano key-state humanoid_controllers -5 modes + mode_manager +5 mode controllers + MIT traj humanoid_control_policy ONNX runner (Python) humanoid_devices_robstride diff --git a/static/img/diagrams/concepts__five_mode_fsm__01.svg b/static/img/diagrams/concepts__five_mode_fsm__01.svg index eeab9b6..9f9c623 100644 --- a/static/img/diagrams/concepts__five_mode_fsm__01.svg +++ b/static/img/diagrams/concepts__five_mode_fsm__01.svg @@ -1,5 +1,5 @@ - -Five-mode FSM -Only one controller is active at a time; joint_state_broadcaster always runs -ZERO_TORQUE - -DAMPING - -STANDBY - -LOCOMOTION - -REMOTE - -DAMP (X / Ctrl+C) -manual -LOAD (L1+A|B) -DAMP -START_LOCOMOTION (R1+A)+is_finished -START_REMOTE (R1+B) -DAMP / fault -DAMP / faultQUITQUITGreen = safe / fail-safeOrange = transitionalSlate = active policy +Five control modes +Each button → switch_controller directly; any transition from any state (BEST_EFFORT). +joy_teleop +/joy → switch_controller + +zero_torque_controller +BACK = STOP +damping_controller + +standby_controller_a / b / y + +rl_policy_controller +LOCOMOTION +remote_policy_controller +REMOTE +BACK +X +L1 + A / B / Y +R1 + A +R1 + BFault path (native, not a joy button):a controller update() → ERROR is droppedto damping via fallback_controllers./safety_status is telemetry (no auto-DAMP).safe / fail-safestandby (interpolate to pose)active policy diff --git a/static/img/diagrams/concepts__safety_pipeline__01.svg b/static/img/diagrams/concepts__safety_pipeline__01.svg index ebd88c6..4013d59 100644 --- a/static/img/diagrams/concepts__safety_pipeline__01.svg +++ b/static/img/diagrams/concepts__safety_pipeline__01.svg @@ -12,11 +12,11 @@ -Safety pipeline — fault to DAMPING in ≤1 tick -Detection (plugin) → telemetry (latched topic) → response (mode_manager) +Safety pipeline — detection, telemetry, native fallback +Detection (plugin) → telemetry (latched topic) → operator + fallback_controllers Layer 1: detection (plugin) Layer 2: telemetry -Layer 3: response +Layer 3: native fallback + operator BUS_OFF socket open / ENETDOWN (sticky) RX_TIMEOUT @@ -39,24 +39,24 @@ /safety_status (TRANSIENT_LOCAL) - -mode_manager.on_safety() - -level == OK? - -request_mode(Mode::Damping) - -if STRICT switch fails: - -request_mode(Mode::ZeroTorque) - -controller_manager: switch_controller - - -non-OK - -fallback + +/safety_status = telemetry +rqt / logs / operator +No auto-DAMP node — the operator reacts(BACK = STOP / X = DAMP). +controller update() → ERROR +(e.g. non-finite obs) + +controller_manager: fallback_controllers + +damping_controller + +zero_torque_controller + + +activate + +its fallback -aggregate - -subscribeBUS_OFF is the one sticky flag — clears only on the next on_activate(), because EMI bursts shouldn't auto-recover. +aggregate + +publishThe automatic path is native fallback_controllers (controller ERROR → damping → zero_torque), independent of /safety_status.BUS_OFF is a sticky flag cleared only on the next on_activate(). diff --git a/static/img/diagrams/getting_started__intro__01.svg b/static/img/diagrams/getting_started__intro__01.svg index a053c6f..cf2250a 100644 --- a/static/img/diagrams/getting_started__intro__01.svg +++ b/static/img/diagrams/getting_started__intro__01.svg @@ -26,7 +26,7 @@ controller_manager (50 Hz) -mode_manager +joy_teleop humanoid_control_policy @@ -51,7 +51,7 @@ code -sim mode +sim mode MIT-mode CAN diff --git a/static/img/diagrams/getting_started__intro__02.svg b/static/img/diagrams/getting_started__intro__02.svg index f6fd922..01b3d82 100644 --- a/static/img/diagrams/getting_started__intro__02.svg +++ b/static/img/diagrams/getting_started__intro__02.svg @@ -22,7 +22,7 @@ humanoid_control_msgs MITCommand, ControlMode, ... humanoid_controllers -5 mode-FSM + mode_manager +5 mode controllers + MIT traj humanoid_control_policy ONNX runner + LeRobot ref humanoid_drivers_socketcan diff --git a/static/img/diagrams/getting_started__lite_101__01_mujoco_spawn.svg b/static/img/diagrams/getting_started__lite_101__01_mujoco_spawn.svg index 774ab96..53ba647 100644 --- a/static/img/diagrams/getting_started__lite_101__01_mujoco_spawn.svg +++ b/static/img/diagrams/getting_started__lite_101__01_mujoco_spawn.svg @@ -13,7 +13,7 @@ MuJoCo bringup spawn sequence -What happens when you run `ros2 launch humanoid_bringup_lite mujoco.launch.py`launchxacromujoco_simcontroller_managermode_manager +What happens when you run `ros2 launch humanoid_bringup_lite mujoco.launch.py`launchxacromujoco_simcontroller_managerjoy_teleop expand xacro (use_sim:=true) URDF start mujoco_sim @@ -21,6 +21,5 @@ register MujocoSystem (17 joints) spawn jsb (active) spawn zero_torque (active) -spawn 4 others (inactive) -start mode_manager -switch_controller(zero_torque)System is now at ZERO_TORQUE, publishing /joint_states at 50 Hz +spawn 4 others (inactive) +start joy_teleop (waits for /joy)System is now at ZERO_TORQUE (from the spawner), publishing /joint_states at 50 Hz diff --git a/static/img/diagrams/getting_started__lite_101__02_mujoco_internals.svg b/static/img/diagrams/getting_started__lite_101__02_mujoco_internals.svg index f13ca1d..394c57c 100644 --- a/static/img/diagrams/getting_started__lite_101__02_mujoco_internals.svg +++ b/static/img/diagrams/getting_started__lite_101__02_mujoco_internals.svg @@ -35,7 +35,7 @@ controller spawners -mode_manager +joy_teleop /robot_description /clock from MuJoCo diff --git a/static/img/diagrams/how_to__first_real_bringup__01.svg b/static/img/diagrams/how_to__first_real_bringup__01.svg index fa0e794..62a24ab 100644 --- a/static/img/diagrams/how_to__first_real_bringup__01.svg +++ b/static/img/diagrams/how_to__first_real_bringup__01.svg @@ -13,7 +13,7 @@ Lite real-hardware bringup -What happens between `pixi run launch-real` and `ZERO_TORQUE active`operatorlaunchros2_control_nodeRobstrideSystemmode_manager +What happens between `pixi run launch-real` and `ZERO_TORQUE active`operatorlaunchros2_control_nodeRobstrideSystemjoy_teleop pixi run launch-real (can interfaces UP?) ip link set can0/1 up @ 1Mbit @@ -24,7 +24,6 @@ I/O threads up (epoll) spawn joint_state_broadcaster (active) spawn zero_torque (active) -spawn damping/standby/rl/remote (inactive) -start mode_manager -switch_controller(zero_torque, asap) -subscribe /safety_status, /joyAfter this sequence: motors compliant, /joint_states @ 50 Hz, /control_mode publishes ZERO_TORQUE. +spawn damping/standby/rl/remote (inactive) +start joy_teleop (reads /joy) +switch_controller on button pressAfter this sequence: motors compliant, /joint_states @ 50 Hz, at ZERO_TORQUE (from the spawner). diff --git a/static/img/diagrams/reference__messages__01.svg b/static/img/diagrams/reference__messages__01.svg index 04cb280..696cce7 100644 --- a/static/img/diagrams/reference__messages__01.svg +++ b/static/img/diagrams/reference__messages__01.svg @@ -13,33 +13,25 @@ humanoid_control_msgs pub/sub topology -Who publishes / who subscribes for each of the 4 active topics +Who publishes / who subscribes for each active topic Publishers Topics -Subscribers -StandbyController - -mode_manager - -Hardware plugins - -Active policy ctrls - -humanoid_control_policy (Python) - -/standby_controller/state - -/control_mode - -/safety_status - -~/command (MITCommand) - -mode_manager - -diagnostics / log - -mode_manager - -RemotePolicyController -/standby_controller/state uses TRANSIENT_LOCAL QoS so a late mode_manager sees the last is_finished +Subscribers +StandbyController + +Hardware plugins + +policy source (ctrl / VLA) + +/standby_controller/state + +/safety_status + +~/command (MITCommand) + +(none - telemetry) + +(none - telemetry) + +RemotePolicyController +ControlMode still ships in humanoid_control_msgs (DDS wire bridge); /control_mode is no longer published.Read the active mode from list_controllers. /standby_controller/state and /safety_status are telemetry.