From 671952f6649fad2e460529caf8dce9bda7c50974 Mon Sep 17 00:00:00 2001 From: maxconway Date: Mon, 16 Nov 2026 21:50:49 -0700 Subject: [PATCH] golem_safety: the header's mode_machine comes from /lowstate; nothing before it Every command carries the mode_machine of the newest /lowstate that reported the profile's, read from every sample, as Unitree's examples copy it; mode_pr stays the profile's (0, pitch/roll), as Unitree's examples choose it. Nothing is written before such a /lowstate, nor, before engaging, while the newest /lowstate is another robot's (relay_held). /lowstate's mode_pr is checked too: another value refuses engaging and, once engaged, latches. An estop that latches before the robot's first /lowstate writes from that /lowstate on (estop_waiting); after a foreign mode_machine the estop commands keep our robot's last value, never the other's. The suite passes (237 tests). The real-robot lmg_deploy bags replayed through it relay every command unchanged and latch at the same moments as before. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_015EG7p8sgY1ZVi6unbbLp4J --- CLAUDE.md | 12 +- .../golem_bringup/launch/g1/g1_core.launch.py | 2 +- .../launch/g1/g1_real_robot.launch.py | 2 +- .../golem_bringup/launch/g1/g1_sim.launch.py | 2 +- core_ws/src/golem_safety/README.md | 43 ++++-- .../include/golem_safety/lowcmd.hpp | 6 +- .../include/golem_safety/safety_core.hpp | 36 +++-- core_ws/src/golem_safety/src/lowcmd.cpp | 11 +- core_ws/src/golem_safety/src/safety_core.cpp | 99 ++++++++++-- .../src/golem_safety/test/test_node_ros.py | 32 ++-- .../test/test_safety_core_relay.cpp | 2 + .../test/test_safety_core_startup.cpp | 144 +++++++++++++++--- core_ws/src/golem_upper_ik/README.md | 2 +- docs/g1/control-path.md | 2 +- docs/g1/safety.md | 10 +- 15 files changed, 318 insertions(+), 87 deletions(-) diff --git a/CLAUDE.md b/CLAUDE.md index 6c717881..57f78da0 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -195,7 +195,7 @@ nests `unitree_api`, `unitree_go`, `unitree_hg`, `unitree_ros2_example`). | Directory | Role | What it does | Key entry points | |---|---|---|---| -| `golem_safety` | **safety (both robots)** | C++. The only writer of `/lowcmd` (§8): streams zero gains from boot (under `use_sim_time`, from the first `/clock`), engages each `/safety/*` input on its own first valid command and relays it, clips it, checks every `/lowstate` (robot identity, estop limits) and latches an estop until restart. A robot profile (`robots/.yaml`, the joint table `golem_upper_ik` and `golem_lowerbody_rl` read) and configs per robot (`config/h12/`, `config/g1/`) | `safety_node` (`robot`, `config`, `sim`, `use_estop` parameters; `use_sim_time` needs `sim`); `launch/safety.launch.py` (`safety_node` after `start_delay`), which both robots' bringups include | +| `golem_safety` | **safety (both robots)** | C++. The only writer of `/lowcmd` (§8): streams zero gains from the robot's first `/lowstate` (under `use_sim_time`, from the first `/clock` too) with that `/lowstate`'s `mode_machine`, engages each `/safety/*` input on its own first valid command and relays it, clips it, checks every `/lowstate` (robot identity, estop limits) and latches an estop until restart. A robot profile (`robots/.yaml`, the joint table `golem_upper_ik` and `golem_lowerbody_rl` read) and configs per robot (`config/h12/`, `config/g1/`) | `safety_node` (`robot`, `config`, `sim`, `use_estop` parameters; `use_sim_time` needs `sim`); `launch/safety.launch.py` (`safety_node` after `start_delay`), which both robots' bringups include | | `golem_upper_ik` | **controller (upper body, both robots)** | Pink IK on the 14 arm joints with the torso (H1-2) or waist (G1) held; OMPL planning and a self-collision barrier from each robot's sphere model and SRDF (the G1's spheres were packed on the rubber-hand G1: none on the Dex3-1 fingers; its torso spheres stand inside the torso's flanks, so `home` keeps the H1-2's 20 mm `d_min`; its `arms_down`, where the real hands rest on the hips, is in self-collision); the follow gate; the safety/controller config pairing (each robot's `config//pairing.yaml`, applied by `golem_upper_ik.pairing`) that `upper_ik.launch.py` checks; each robot's data (IK and held joints, frames, named postures) is `config//robot.yaml` | `frame_task_server` (`/frame_task`, `/named_config`), `joint_state_publisher`; `launch/upper_ik.launch.py` (the pairing check, then `frame_task_server` after `start_delay`) and `launch/robot_state.launch.py` (`robot_state_publisher` on the robot's URDF + `joint_state_publisher`), which both robots' bringups include | | `golem_bringup` | **bringup (both robots)** | Launch-only package that starts the whole H1-2 (`launch/h12/`) or G1 (`launch/g1/`), sim or real. See §6. | top-level + building-block launch files, RViz configs | | `golem_ros2_models` | **description (both robots)** | Installs both robots' models from `CL_Assets`: the H1-2 with its Magpie grippers and the G1 with its Dex3-1 hands, each a Pinocchio URDF, a ROS copy with `package://golem_ros2_models/...` URIs, the meshes, and a sphere-packed self-collision model with its SRDF. No nodes and no joint table (that is `golem_safety`'s robot profile) | `golem_ros2_models.paths`; `launch/display.launch.py` (`robot:=h12|g1`) | @@ -287,9 +287,10 @@ for the includes after it, so pass consistent values (all four blocks share **The H1-2's start-up** (`h12_control.launch.py`; `golem_safety`'s and `golem_upper_ik`'s READMEs have the details): -- `safety_node` streams zero-gain `/lowcmd` from boot (in the sim, from the first +- `safety_node` writes nothing until `/lowstate` reports `mode_machine` 6, then + streams zero-gain `/lowcmd` stamped with it (in the sim, also not before the first `/clock`). It engages each input on - that input's first valid command once `/lowstate` reports `mode_machine` 6, + that input's first valid command once `/lowstate` reports `mode_machine` 6 and `mode_pr` 0, and once a first estop sample has arrived; until then it logs `engage_refused` with the reason, at most once a second. - **`/safety/heartbeat` means relaying can begin.** `safety_node` publishes no @@ -419,8 +420,9 @@ which `golem_upper_ik`'s `upper_ik.launch.py` applies (`upper_ik: not starting: **The G1's start-up** (`g1_core.launch.py`'s docstring; `golem_safety`'s, `golem_upper_ik`'s and `golem_lowerbody_rl`'s READMEs have the details): -- `safety_node` streams zero-gain `/lowcmd` (every motor off) from boot (in the - sim, from the first `/clock`), checks +- `safety_node` writes nothing until `rt/lowstate` reports `mode_machine` 5, then + streams zero-gain `/lowcmd` (every motor off) stamped with it (in the sim, also not + before the first `/clock`), checks every `/lowstate` against the estop limits from the first sample (in the sim, from the first `/clock`), and engages each input on that input's own first valid command once `/lowstate` reports diff --git a/core_ws/src/golem_bringup/launch/g1/g1_core.launch.py b/core_ws/src/golem_bringup/launch/g1/g1_core.launch.py index e8b2c678..69624ea9 100644 --- a/core_ws/src/golem_bringup/launch/g1/g1_core.launch.py +++ b/core_ws/src/golem_bringup/launch/g1/g1_core.launch.py @@ -15,7 +15,7 @@ safety_node runs split mode (split, or sim_split in the sim): it writes every motor on /lowcmd, the legs from the leg node's input and the waist and -arms from the controller's. From boot it streams zero gains on every motor +arms from the controller's. From the robot's first rt/lowstate it streams zero gains on every motor (every motor off), and engages each input on that input's own first valid command once rt/lowstate reports the G1's mode_machine (5); until the leg node commands, the leg motors stay off. diff --git a/core_ws/src/golem_bringup/launch/g1/g1_real_robot.launch.py b/core_ws/src/golem_bringup/launch/g1/g1_real_robot.launch.py index 636823e2..eae7c11c 100644 --- a/core_ws/src/golem_bringup/launch/g1/g1_real_robot.launch.py +++ b/core_ws/src/golem_bringup/launch/g1/g1_real_robot.launch.py @@ -21,7 +21,7 @@ golem_safety runs split.yaml: it writes every motor on rt/lowcmd, the legs from the leg node's input and the waist and arms from the arm -controller's (g1_split.yaml). From boot it streams zero gains on every motor +controller's (g1_split.yaml). From the robot's first rt/lowstate it streams zero gains on every motor (every motor off) and engages each input on its own first command: the arms on the controller's first command, and the move to home starts once the controller sees golem_safety relay it on rt/lowcmd. diff --git a/core_ws/src/golem_bringup/launch/g1/g1_sim.launch.py b/core_ws/src/golem_bringup/launch/g1/g1_sim.launch.py index 5e254f0a..e6c9771c 100644 --- a/core_ws/src/golem_bringup/launch/g1/g1_sim.launch.py +++ b/core_ws/src/golem_bringup/launch/g1/g1_sim.launch.py @@ -9,7 +9,7 @@ The G1 RoboCasa sim (golem_robocasa/g1/) takes rt/lowcmd and rt/dex3/*, and holds the robot up on an elastic band until /elastic_band/toggle releases it. golem_safety runs sim_split and the controller sim_split: golem_safety streams -zero gains on every motor from boot and engages each input on its own first +zero gains on every motor from the robot's first rt/lowstate and engages each input on its own first command. The leg node (golem_lowerbody_rl's lowerbody_controller_node) always runs HOMIE, on golem_lowerbody_rl's config/g1/sim.yaml: once golem_safety beats and rt/lowstate is upright, it moves the legs to the policy's default pose; diff --git a/core_ws/src/golem_safety/README.md b/core_ws/src/golem_safety/README.md index c2f07787..110047cf 100644 --- a/core_ws/src/golem_safety/README.md +++ b/core_ws/src/golem_safety/README.md @@ -143,32 +143,45 @@ cycle. No timer in the node runs on the wall clock. **What it writes.** Each engaged input's newest valid command on that input's own slots (`full`: every body slot; `split`: the lower and upper slots). Every other slot, the slots past the body and the `reserve` words carry `zero_command` (mode 0, every value 0). Every -command carries the profile's `mode_machine` and `mode_pr` and its own Unitree CRC (CRC-32 -over the message body, as the SDK computes it); nothing of an input's header is copied. +command carries the `mode_machine` of the newest `/lowstate` that reported the profile's (read +from every sample, as Unitree's examples copy it), the profile's `mode_pr` (0, pitch/roll +ankles, which the layer chooses as Unitree's examples do) and its own Unitree CRC (CRC-32 over +the message body, as the SDK computes it); nothing of an input's header is copied. **Clipping.** A command is valid when q, dq, tau, kp and kd are finite on every body slot, the input's own or not; any other command latches the estop. A valid command is clipped on its input's slots: q, dq and tau to `limits.clip`, kp and kd to [0, `kp_max`] and [0, `kd_max`]. A gain above its maximum raises `gain_clipped` (Warn) once per joint and gain. -**Start-up**, the same on both robots (no config key chooses it): from boot (under -`use_sim_time`, from the first `/clock`) the layer streams `zero_command` on every slot, so -every motor is off. Each input engages on its own +**Start-up**, the same on both robots (no config key chooses it): the layer writes nothing +until a `/lowstate` reports the profile's `mode_machine` (`relay_held`, Warn, with the reason, +at most every 5 s), nor, before an input has engaged, while the newest `/lowstate` reports +another; from then on (under `use_sim_time`, from the first `/clock` as well) it streams +`zero_command` on every slot, so every motor is off. Each input engages on its own first valid command that arrives once the newest `/lowstate` reports the profile's -`mode_machine` (and, with `use_estop`, a first hardware estop sample has arrived); its slots +`mode_machine` and `mode_pr` (and, with `use_estop`, a first hardware estop sample has +arrived); its slots carry its commands from then on (`input_engaged`). The slots of an input that has not engaged stay at zero gain: in split mode the robot runs arms-only, with its leg motors off, -until the leg node commands. The `/lowstate` limit checks run from boot (under -`use_sim_time`, from the first `/clock`). +until the leg node commands. The `/lowstate` limit checks run from the first sample (under +`use_sim_time`, from the first `/clock`). An estop that latches before the robot's first +`/lowstate` (a non-finite command, a first estop sample that says triggered) writes nothing +until that `/lowstate` arrives (`estop_waiting`, Error, once), then its estop commands. A command that cannot engage raises `engage_refused` (Warn) with the first reason, at most -once a second: `no /lowstate received yet`, `/lowstate mode_machine is 0, expected 6`, or -`no hardware estop sample received yet on /estop/triggered or /estop/plugged_in`. - -**Robot identity.** Every `/lowstate` is checked against the profile's `mode_machine`. Before -engaging, another value only refuses (another robot or sim delays engaging); once engaged, it -latches the estop (`/lowstate mode_machine 5 is not the h12's 6: another robot or sim is on -this domain`). A foreign sample is never limit-checked. `docker/scripts/wait_for_sim.py` +once a second: `no /lowstate received yet`, `/lowstate mode_machine is 0, expected 6`, +`/lowstate mode_pr is 1, expected 0`, or `no hardware estop sample received yet on +/estop/triggered or /estop/plugged_in`. + +**Robot identity.** Every `/lowstate` is checked against the profile's `mode_machine` and +`mode_pr`. Before engaging, another `mode_machine` holds the output and refuses engaging +(another robot or sim delays it), and another `mode_pr` refuses engaging while the zero stream +goes on with the profile's; once engaged, either latches the estop (`/lowstate mode_machine 5 +is not the h12's 6: another robot or sim is on this domain`, `/lowstate mode_pr 1 is not the +h12's 0: the robot is in another ankle mode`), and the estop commands carry our robot's last +`mode_machine`, never the other's. Such a sample is never limit-checked. The real H1-2 reports +`mode_machine` 6 and `mode_pr` 0 (the `lmg_deploy` bags); the G1's `mode_pr` is unmeasured on +the real G1. `docker/scripts/wait_for_sim.py` checks the same values before a paired bringup starts, and each robot's sims report them. **Quiet inputs are held and reported, not estopped.** Once an input has engaged, a gap in diff --git a/core_ws/src/golem_safety/include/golem_safety/lowcmd.hpp b/core_ws/src/golem_safety/include/golem_safety/lowcmd.hpp index 58c198fe..3e08ad07 100644 --- a/core_ws/src/golem_safety/include/golem_safety/lowcmd.hpp +++ b/core_ws/src/golem_safety/include/golem_safety/lowcmd.hpp @@ -15,9 +15,13 @@ namespace golem_safety // The robots and the sims drop a command whose CRC does not match. uint32_t lowcmd_crc(const unitree_hg::msg::LowCmd & cmd); -// Set mode_pr and mode_machine from `profile`, then crc. +// Set mode_pr and mode_machine, then crc. // SAFETY: every command is stamped last, after its final edit; an edit after the stamp // needs a new stamp, or the robot drops the command. +void stamp_header_and_crc( + unitree_hg::msg::LowCmd & cmd, uint8_t mode_pr, uint8_t mode_machine); + +// Set mode_pr and mode_machine from `profile`, then crc. void stamp_header_and_crc(unitree_hg::msg::LowCmd & cmd, const RobotProfile & profile); // Return the stamped command that turns every motor off: mode 0 and q, dq, tau, kp, kd and diff --git a/core_ws/src/golem_safety/include/golem_safety/safety_core.hpp b/core_ws/src/golem_safety/include/golem_safety/safety_core.hpp index c6515f6c..bf8daa9f 100644 --- a/core_ws/src/golem_safety/include/golem_safety/safety_core.hpp +++ b/core_ws/src/golem_safety/include/golem_safety/safety_core.hpp @@ -38,11 +38,18 @@ struct Event // Relay. Each input keeps its newest valid command. step() starts from zero_command and // copies onto each engaged input's slots that input's newest command, so a slot no engaged // input owns stays at zero_command (every motor off). step() and exit_step() stamp the -// header and CRC on whatever they return, last. +// header and CRC on whatever they return, last: mode_machine from the newest /lowstate that +// reported the profile's (read from every sample), mode_pr from the profile. +// +// SAFETY: nothing is written before a /lowstate reports the profile's mode_machine, nor, +// before an input has engaged, while the newest /lowstate reports another (relay_held, +// Warn, at most every 5 s of one hold). An estop that latches before any such /lowstate +// writes nothing until one arrives (estop_waiting, Error, once), then its estop commands as +// after any latch, and estop_command_written. // // Engaging. Each input engages on its own first valid command that arrives while the newest -// /lowstate carries the profile's mode_machine and, with use_estop, a hardware estop sample -// has arrived (input_engaged, Info). A command that cannot engage raises engage_refused +// /lowstate carries the profile's mode_machine and mode_pr and, with use_estop, a hardware +// estop sample has arrived (input_engaged, Info). A command that cannot engage raises engage_refused // (Warn) with the first unmet condition, at most once a second. A /lowstate from another // robot before engaging only delays engaging. // @@ -68,13 +75,14 @@ struct Event // writer's last command, valid or not, is older than single_writer.handover_seconds; // otherwise it latches the estop with "second writer on ". // -// SAFETY: the /lowstate checks run on every sample that carries the profile's mode_machine, -// from the first one, before any input engages. +// SAFETY: the /lowstate limit checks run on every sample that carries the profile's +// mode_machine and mode_pr, from the first one, before any input engages. // // SAFETY: the estop latches, one way until the process restarts, on // - a command with a non-finite value, before or after engaging: "Command validation // failed on : motor () is "; -// - a /lowstate whose mode_machine is not the profile's, once an input has engaged; +// - a /lowstate whose mode_machine or mode_pr is not the profile's, once an input has +// engaged; after a foreign mode_machine the estop commands carry our robot's last one; // - on ROS domain 0 with control.state_lag_ms > 0, a /lowstate that arrives more than // state_lag_ms late against the robot's uint32 ms tick: its arrival minus its tick, // less a baseline that is the smallest such offset seen and may rise by 0.5 ms/s to @@ -176,8 +184,8 @@ class SafetyCore // Name why a command cannot engage. struct Refusal { - enum Kind { NoState, ModeMachine, NoEstopSample } kind; - int mode_machine = 0; // ModeMachine: the sample's + enum Kind { NoState, ModeMachine, ModePr, NoEstopSample } kind; + int value = 0; // ModeMachine, ModePr: the sample's }; // Hold the transport-lag check's state: the robot's ms tick, unwrapped, against `now`. @@ -283,8 +291,13 @@ class SafetyCore unitree_hg::msg::LowCmd merged() const; // Return this step's estop command per estop_response, or nullopt once the burst is done. std::optional estop_output(double now); - // Stamp the header and CRC on `out` and remember it as the last command written. + // Stamp the header and CRC on `out` and remember it as the last command written; nullopt + // when no /lowstate has reported the profile's mode_machine yet. std::optional finish(std::optional out); + // Raise estop_waiting once for a latch with no header to stamp yet; return nullopt. + std::optional unwritten_estop(); + // Raise relay_held, at most once every kHeldReportSeconds of one hold. + void report_held(double now); const RobotProfile profile_; const SafetyConfig config_; @@ -303,6 +316,11 @@ class SafetyCore std::optional refusal_reported_; // s, the last engage_refused std::optional state_; + // The mode_machine of the newest /lowstate that reported the profile's: every command's. + std::optional header_mode_machine_; + bool state_foreign_ = false; // the newest /lowstate's mode_machine is not the profile's + std::optional held_reported_; // s, the last relay_held of the current hold + bool estop_wait_reported_ = false; // estop_waiting raised std::optional state_time_; // s, the last /lowstate's arrival, kept while estopped double state_wall_time_ = 0.0; // s, wall clock at the newest /lowstate's arrival bool state_unlogged_ = false; // take_log_sample() has a sample to return diff --git a/core_ws/src/golem_safety/src/lowcmd.cpp b/core_ws/src/golem_safety/src/lowcmd.cpp index 1477b3c6..6bed7bbe 100644 --- a/core_ws/src/golem_safety/src/lowcmd.cpp +++ b/core_ws/src/golem_safety/src/lowcmd.cpp @@ -134,13 +134,18 @@ uint32_t lowcmd_crc(const unitree_hg::msg::LowCmd & cmd) return crc32_core(words.data(), kCrcWords); } -void stamp_header_and_crc(unitree_hg::msg::LowCmd & cmd, const RobotProfile & profile) +void stamp_header_and_crc(unitree_hg::msg::LowCmd & cmd, uint8_t mode_pr, uint8_t mode_machine) { - cmd.mode_pr = profile.mode_pr; - cmd.mode_machine = profile.mode_machine; + cmd.mode_pr = mode_pr; + cmd.mode_machine = mode_machine; cmd.crc = lowcmd_crc(cmd); } +void stamp_header_and_crc(unitree_hg::msg::LowCmd & cmd, const RobotProfile & profile) +{ + stamp_header_and_crc(cmd, profile.mode_pr, profile.mode_machine); +} + unitree_hg::msg::LowCmd zero_command(const RobotProfile & profile) { unitree_hg::msg::LowCmd cmd; diff --git a/core_ws/src/golem_safety/src/safety_core.cpp b/core_ws/src/golem_safety/src/safety_core.cpp index cdfe9092..189438af 100644 --- a/core_ws/src/golem_safety/src/safety_core.cpp +++ b/core_ws/src/golem_safety/src/safety_core.cpp @@ -33,6 +33,8 @@ const std::string kReasonLost = "the estop latched, and its reason could not be // s between engage_refused reports, so that a controller streaming before the robot is // ready does not flood the log. constexpr double kRefusalReportSeconds = 1.0; +// s of robot time between the relay_held reports of one hold. +constexpr double kHeldReportSeconds = 5.0; // ms per s the lag baseline may rise, to follow drift between the robot's tick and the host // clock: 500 ppm is well above crystal drift, so a delay that builds faster is lag. @@ -246,7 +248,7 @@ void SafetyCore::on_state(const LowState & state, double now) void SafetyCore::update_relay_ready() { const bool ready = state_ && state_->mode_machine == profile_.mode_machine && - (!use_estop_ || estop_sample_time_.has_value()); + state_->mode_pr == profile_.mode_pr && (!use_estop_ || estop_sample_time_.has_value()); relay_ready_.store(ready, std::memory_order_release); } @@ -255,15 +257,27 @@ void SafetyCore::check_state(const LowState & state, double now) const std::optional previous = state_time_; state_time_ = now; if (estopped()) { + // SAFETY: an estop that latched before our robot's first /lowstate writes its estop + // command from that /lowstate on; another robot's never stamps it. + if (!header_mode_machine_ && state.mode_machine == profile_.mode_machine) { + header_mode_machine_ = state.mode_machine; + } return; } state_ = state; state_wall_time_ = wall_seconds(); state_unlogged_ = config_.logging.enabled; - // SAFETY: before any input engages, another robot's /lowstate only blocks engaging - // (refusal() reads it from state_); once an input has engaged it latches. - if (state.mode_machine != profile_.mode_machine) { + // SAFETY: every command carries the mode_machine of the newest /lowstate that reported + // the profile's, read from every sample; before any has, nothing is written (step()). + // Before any input engages, another robot's /lowstate holds the output and blocks + // engaging (refusal() reads it from state_); once an input has engaged it latches, and + // the estop commands keep our robot's last value, never the other's. + state_foreign_ = state.mode_machine != profile_.mode_machine; + if (!state_foreign_) { + header_mode_machine_ = state.mode_machine; + } + if (state_foreign_) { if (engaged_) { latch( now, std::nullopt, [&] { @@ -274,6 +288,20 @@ void SafetyCore::check_state(const LowState & state, double now) } return; } + // SAFETY: a /lowstate in another ankle mode carries motor-pair values, not the joints + // the limits and the controllers are written for: it blocks engaging, latches once + // engaged, and is not limit-checked. The commands keep the profile's mode_pr. + if (state.mode_pr != profile_.mode_pr) { + if (engaged_) { + latch( + now, std::nullopt, [&] { + return config_.topics.low_state + " mode_pr " + std::to_string(state.mode_pr) + + " is not the " + profile_.robot + "'s " + std::to_string(profile_.mode_pr) + + ": the robot is in another ankle mode"; + }); + } + return; + } if (state_quiet_ && previous) { state_quiet_ = false; raise( @@ -474,7 +502,7 @@ void SafetyCore::try_engage(InputState & in, double now) } // The reasons are checked in this order: no /lowstate, a newest /lowstate from another -// robot, and with use_estop no hardware estop sample. +// robot, one in another ankle mode, and with use_estop no hardware estop sample. std::optional SafetyCore::refusal() const { if (!state_ || !state_time_) { @@ -483,6 +511,9 @@ std::optional SafetyCore::refusal() const if (state_->mode_machine != profile_.mode_machine) { return Refusal{Refusal::ModeMachine, state_->mode_machine}; } + if (state_->mode_pr != profile_.mode_pr) { + return Refusal{Refusal::ModePr, state_->mode_pr}; + } if (use_estop_ && !estop_sample_time_) { return Refusal{Refusal::NoEstopSample}; } @@ -496,8 +527,11 @@ std::string SafetyCore::refusal_text(const Refusal & r) const case Refusal::NoState: return "no " + state_topic + " received yet"; case Refusal::ModeMachine: - return state_topic + " mode_machine is " + std::to_string(r.mode_machine) + + return state_topic + " mode_machine is " + std::to_string(r.value) + ", expected " + std::to_string(profile_.mode_machine); + case Refusal::ModePr: + return state_topic + " mode_pr is " + std::to_string(r.value) + + ", expected " + std::to_string(profile_.mode_pr); case Refusal::NoEstopSample: return "no hardware estop sample received yet on " + config_.estop.triggered_topic + " or " + config_.estop.plugged_in_topic; @@ -708,13 +742,45 @@ std::optional SafetyCore::estop_output(double now) std::optional SafetyCore::finish(std::optional out) { - if (out) { - stamp_header_and_crc(*out, profile_); - last_written_ = *out; + if (!out || !header_mode_machine_) { + return std::nullopt; } + stamp_header_and_crc(*out, profile_.mode_pr, *header_mode_machine_); + last_written_ = *out; return out; } +std::optional SafetyCore::unwritten_estop() +{ + if (!estop_wait_reported_) { + estop_wait_reported_ = true; + raise( + Event::Error, [&] { + return "{\"event\": \"estop_waiting\", \"reason\": " + json_string(estop_reason()) + + ", \"why\": " + json_string( + "no " + config_.topics.low_state + " with the " + profile_.robot + + "'s mode_machine yet: the estop command is written from the first one") + "}"; + }); + } + return std::nullopt; +} + +void SafetyCore::report_held(double now) +{ + if (held_reported_ && now - *held_reported_ < kHeldReportSeconds) { + return; + } + held_reported_ = now; + raise( + Event::Warn, [&] { + const std::string why = !state_ ? + "no " + config_.topics.low_state + " received yet" : + config_.topics.low_state + " mode_machine is " + std::to_string(state_->mode_machine) + + ", expected " + std::to_string(profile_.mode_machine); + return "{\"event\": \"relay_held\", \"reason\": " + json_string(why) + "}"; + }); +} + std::optional SafetyCore::step(double now) { if (exiting_) { @@ -723,9 +789,18 @@ std::optional SafetyCore::step(double now) // First, so that a silent hardware estop latches in the step that notices it. check_estop_input(now); if (estopped()) { - return finish(estop_output(now)); + // SAFETY: an estop before any /lowstate with our mode_machine writes nothing until one + // arrives, as nothing was written before it; its burst then starts. + return header_mode_machine_ ? finish(estop_output(now)) : unwritten_estop(); } report_quiet(now); + // SAFETY: nothing is written before a /lowstate with the profile's mode_machine, nor, before + // an input has engaged, while the newest /lowstate is another robot's. + if (!header_mode_machine_ || (state_foreign_ && !engaged_)) { + report_held(now); + return std::nullopt; + } + held_reported_.reset(); return finish(merged()); } @@ -736,7 +811,9 @@ std::optional SafetyCore::exit_step(double now) return std::nullopt; } std::optional out; - if (estopped()) { + if (estopped() && !header_mode_machine_) { + unwritten_estop(); + } else if (estopped()) { const bool burst = config_.estop_response.after == EstopAfter::BurstThenSilence; if (burst || !estop_written_) { out = estop_output(now); diff --git a/core_ws/src/golem_safety/test/test_node_ros.py b/core_ws/src/golem_safety/test/test_node_ros.py index fc7cff7e..aea25a25 100644 --- a/core_ws/src/golem_safety/test/test_node_ros.py +++ b/core_ws/src/golem_safety/test/test_node_ros.py @@ -567,12 +567,20 @@ def test_zero_stream_engages_each_input_on_its_own(stack, first): 'split_mode_lower', 'split_mode_upper'] -def test_g1_streams_lowcmd_from_boot(stack): +@pytest.mark.parametrize('robot', ['h12', 'g1']) +def test_nothing_before_the_first_lowstate(stack, robot): """ - SAFETY: with no input and no /lowstate the G1 gets the zero stream at about 500 Hz on every - slot, and no heartbeat, since the relay cannot begin. + SAFETY: with no /lowstate nothing goes out on /lowcmd and no heartbeat, and the node says + why; the robot's first /lowstate starts the zero stream at about 500 Hz, stamped with + the mode_machine it reports. """ - run = stack('g1', 'split', stream_state=False) + run = stack(robot, SPLIT[robot], stream_state=False) + time.sleep(1.0) + assert run.outputs() == [] + assert run.robot.outputs(HEARTBEAT) == [] + assert reasons(run.node, 'relay_held')[0] == 'no /lowstate received yet' + assert run.node.events('input_engaged') == [] and estop_events(run.node) == [] + run.robot.resume() time.sleep(0.3) start = time.time() time.sleep(1.0) @@ -580,11 +588,10 @@ def test_g1_streams_lowcmd_from_boot(stack): window = [msg for t, msg in run.outputs(since=start) if t < end] hz = len(window) / (end - start) assert 450.0 <= hz <= 520.0, f'{hz:.0f} Hz' + mode_machine = run.robot.profile.mode_machine for msg in window: assert is_zeroed(msg, len(msg.motor_cmd)) - assert (msg.mode_pr, msg.mode_machine) == (0, 5) and msg.crc == msg_crc(msg) - assert run.robot.outputs(HEARTBEAT) == [] - assert run.node.events('input_engaged') == [] and estop_events(run.node) == [] + assert (msg.mode_pr, msg.mode_machine) == (0, mode_machine) and msg.crc == msg_crc(msg) @pytest.mark.parametrize('robot', ['h12', 'g1']) @@ -962,8 +969,9 @@ def test_heartbeat_false_after_a_latch_before_the_first_beat(stack, robot, latch """ SAFETY: a latch before the relay was ready makes the beats false and never true. - command: a non-finite command with no /lowstate yet; estop: a first hardware estop sample - that says triggered, with the robot's /lowstate. + command: a non-finite command with no /lowstate yet, which writes nothing and says why + (estop_waiting); estop: a first hardware estop sample that says triggered, with the + robot's /lowstate. """ if latch == 'command': run = stack(robot, SPLIT[robot], stream_state=False) @@ -976,7 +984,11 @@ def test_heartbeat_false_after_a_latch_before_the_first_beat(stack, robot, latch run.robot.estop_values = (True, True) reason_prefix = 'External estop triggered' time.sleep(1.2) - (report,) = run.node.events('estop_command_written') + if latch == 'command': + (report,) = run.node.events('estop_waiting') + assert run.outputs() == [] + else: + (report,) = run.node.events('estop_command_written') assert report['reason'].startswith(reason_prefix), report beats = run.robot.outputs(HEARTBEAT) assert beats and not any(beat for _, beat in beats) diff --git a/core_ws/src/golem_safety/test/test_safety_core_relay.cpp b/core_ws/src/golem_safety/test/test_safety_core_relay.cpp index e247c20b..c301d5a7 100644 --- a/core_ws/src/golem_safety/test/test_safety_core_relay.cpp +++ b/core_ws/src/golem_safety/test/test_safety_core_relay.cpp @@ -770,6 +770,7 @@ TEST(EstopInput, TriggeredOrUnpluggedLatchesWithUseEstop) const SafetyConfig cfg = config(h12(), "relax_safety_split"); { SafetyCore core(h12(), cfg, true, kDomain); + core.on_state(state_of(h12()), 0.0); core.on_estop(false, true, 0.0); EXPECT_FALSE(core.estopped()); core.on_estop(true, true, 0.010); @@ -967,6 +968,7 @@ TEST(Events, EstopReportIsTheOldJsonAfterTheCommand) // The hardware estop adds trigger_to_action_ms. SafetyCore box(h12(), rig.cfg, true, kDomain); + box.on_state(state_of(h12()), 0.0); box.on_estop(true, true, 0.010); ASSERT_TRUE(box.step(0.013)); reports = named(box.drain_events(), "estop_command_written"); diff --git a/core_ws/src/golem_safety/test/test_safety_core_startup.cpp b/core_ws/src/golem_safety/test/test_safety_core_startup.cpp index 12116140..11b00151 100644 --- a/core_ws/src/golem_safety/test/test_safety_core_startup.cpp +++ b/core_ws/src/golem_safety/test/test_safety_core_startup.cpp @@ -177,14 +177,20 @@ std::optional run( } // namespace // SAFETY: from the first step, with or without use_estop, the H1-2 writes the zero command. -TEST(Startup, ZeroStreamFromFirstStep) +TEST(Startup, ZeroStreamFromFirstLowstate) { + // Nothing before the robot's first /lowstate, with or without use_estop; zero_command + // from it on, before any estop sample or command. const LowCmd zero = golem_safety::zero_command(h12()); for (const char * name : {"relax_safety_split", "default_safety_full"}) { for (const bool use_estop : {false, true}) { SCOPED_TRACE(std::string(name) + (use_estop ? ", use_estop" : "")); SafetyCore core(h12(), config(h12(), name), use_estop, kDomain); - for (int i = 0; i < 100; ++i) { + for (int i = 0; i < 50; ++i) { + ASSERT_FALSE(core.step(i * kStep)) << "step " << i; + } + core.on_state(state_of(h12()), 50 * kStep); + for (int i = 50; i < 100; ++i) { const auto out = core.step(i * kStep); ASSERT_TRUE(out) << "step " << i; expect_same(*out, zero); @@ -194,12 +200,91 @@ TEST(Startup, ZeroStreamFromFirstStep) } } +TEST(Startup, HeaderFollowsEveryLowstate) +{ + // The header is the robot's own: before engaging, a /lowstate from another robot holds + // the output (relay_held) until the robot's own returns; nothing is ever stamped with + // the other robot's value. + const LowCmd zero = golem_safety::zero_command(h12()); + SafetyCore core(h12(), config(h12(), "relax_safety_split"), false, kDomain); + core.on_state(state_of(h12()), 0.0); + auto out = core.step(0.0); + ASSERT_TRUE(out); + EXPECT_EQ(out->mode_machine, 6); + EXPECT_EQ(out->mode_pr, 0); + LowState foreign = state_of(h12()); + foreign.mode_machine = 5; + core.on_state(foreign, 0.01); + EXPECT_FALSE(core.step(0.01)); + EXPECT_FALSE(core.relay_ready()); + const auto held = named(core.drain_events(), "relay_held"); + ASSERT_EQ(held.size(), 1U); + EXPECT_EQ( + held[0].second["reason"].as(), "/lowstate mode_machine is 5, expected 6"); + core.on_state(state_of(h12()), 0.02); + out = core.step(0.02); + ASSERT_TRUE(out); + expect_same(*out, zero); + EXPECT_FALSE(core.estopped()); +} + +TEST(Startup, ForeignModeMachineAfterEngagingKeepsOurHeader) +{ + // Once engaged, another robot's mode_machine latches; the burst carries our robot's + // last mode_machine, never the other's. + const SafetyConfig cfg = config(h12(), "relax_safety_split"); + SafetyCore core(h12(), cfg, false, kDomain); + core.on_state(state_of(h12()), 0.0); + core.on_command(Input::Lower, command(h12()), "lower", 0.0); + ASSERT_TRUE(core.step(0.0)); + LowState foreign = state_of(h12()); + foreign.mode_machine = 5; + core.on_state(foreign, 0.01); + ASSERT_TRUE(core.estopped()); + for (int i = 0; i < cfg.estop_response.burst_count; ++i) { + const auto out = core.step(0.01 + i * kStep); + ASSERT_TRUE(out) << "burst " << i; + expect_same(*out, golem_safety::estop_command(h12(), cfg)); + ASSERT_EQ(out->mode_machine, 6); + } + EXPECT_FALSE(core.step(1.0)) << "silence after the burst"; +} + +TEST(Startup, ModePrMustMatch) +{ + // A /lowstate in another ankle mode blocks engaging while the zero stream goes on with + // the profile's mode_pr; once engaged it latches. + const SafetyConfig cfg = config(h12(), "relax_safety_split"); + SafetyCore core(h12(), cfg, false, kDomain); + LowState ab = state_of(h12()); + ab.mode_pr = 1; + core.on_state(ab, 0.0); + core.on_command(Input::Lower, command(h12(), 0.1F), "lower", 0.0); + auto out = core.step(0.0); + ASSERT_TRUE(out); + EXPECT_EQ(out->mode_pr, 0); + EXPECT_EQ(out->motor_cmd[3].kp, 0.0F) << "not engaged"; + EXPECT_FALSE(core.relay_ready()); + const auto refused = named(core.drain_events(), "engage_refused"); + ASSERT_EQ(refused.size(), 1U); + EXPECT_EQ(refused[0].second["reason"].as(), "/lowstate mode_pr is 1, expected 0"); + core.on_state(state_of(h12()), 0.01); + core.on_command(Input::Lower, command(h12(), 0.1F), "lower", 0.01); + out = core.step(0.01); + ASSERT_TRUE(out); + EXPECT_EQ(out->motor_cmd[3].q, 0.1F); + core.on_state(ab, 0.02); + ASSERT_TRUE(core.estopped()); + EXPECT_EQ( + core.estop_reason(), "/lowstate mode_pr 1 is not the h12's 0: the robot is in another ankle mode"); +} + // SAFETY: without the robot's own /lowstate no input engages: the zero command throughout, // engage_refused at most once a second, and another robot's /lowstate only delays engaging. TEST(Startup, NoLowstateNeverEngages) { - // H1-2: commands for 5 s and no /lowstate. zero_command on every step, and the reason - // at most once a second. + // H1-2: commands for 5 s and no /lowstate. Nothing is written, and the reason for not + // engaging comes at most once a second. const LowCmd zero = golem_safety::zero_command(h12()); SafetyCore core(h12(), config(h12(), "relax_safety_split"), false, kDomain); const LowCmd cmd = command(h12(), 0.1F); @@ -207,9 +292,7 @@ TEST(Startup, NoLowstateNeverEngages) const double t = i * kStep; core.on_command(Input::Lower, cmd, "lower", t); core.on_command(Input::Upper, cmd, "upper", t); - const auto out = core.step(t); - ASSERT_TRUE(out) << "t = " << t; - ASSERT_EQ(out->motor_cmd, zero.motor_cmd) << "t = " << t; + ASSERT_FALSE(core.step(t)) << "t = " << t; } auto events = core.drain_events(); auto refused = named(events, "engage_refused"); @@ -222,14 +305,13 @@ TEST(Startup, NoLowstateNeverEngages) "\"reason\": \"no /lowstate received yet\"}"); } - // A /lowstate from the other robot: still zero_command, and why. + // A /lowstate from the other robot: still nothing, and why. LowState foreign = state_of(h12()); foreign.mode_machine = 5; core.on_state(foreign, 5.5); core.on_command(Input::Upper, cmd, "upper", 5.5); auto out = core.step(5.5); - ASSERT_TRUE(out); - EXPECT_EQ(out->motor_cmd, zero.motor_cmd); + EXPECT_FALSE(out); EXPECT_FALSE(core.estopped()); refused = named(core.drain_events(), "engage_refused"); ASSERT_EQ(refused.size(), 1U); @@ -246,8 +328,7 @@ TEST(Startup, NoLowstateNeverEngages) EXPECT_EQ(out->motor_cmd[3], zero.motor_cmd[3]); EXPECT_FALSE(core.estopped()); - // G1: zero_command likewise. - const LowCmd g1_zero = golem_safety::zero_command(g1()); + // G1: nothing likewise. const SafetyConfig g1_cfg = config(g1(), "sim_split"); SafetyCore g1_core(g1(), g1_cfg, false, kDomain); const LowCmd g1_cmd = command(g1(), 0.1F); @@ -255,9 +336,7 @@ TEST(Startup, NoLowstateNeverEngages) const double t = i * kStep; g1_core.on_command(Input::Lower, g1_cmd, "lower", t); g1_core.on_command(Input::Upper, g1_cmd, "upper", t); - const auto g1_out = g1_core.step(t); - ASSERT_TRUE(g1_out) << "t = " << t; - ASSERT_EQ(g1_out->motor_cmd, g1_zero.motor_cmd) << "t = " << t; + ASSERT_FALSE(g1_core.step(t)) << "t = " << t; } events = g1_core.drain_events(); refused = named(events, "engage_refused"); @@ -272,8 +351,7 @@ TEST(Startup, NoLowstateNeverEngages) g1_core.on_command(Input::Lower, g1_cmd, "lower", 5.5); g1_core.on_command(Input::Upper, g1_cmd, "upper", 5.5); auto g1_out = g1_core.step(5.5); - ASSERT_TRUE(g1_out); - EXPECT_EQ(g1_out->motor_cmd, g1_zero.motor_cmd); + EXPECT_FALSE(g1_out); EXPECT_FALSE(g1_core.estopped()); refused = named(g1_core.drain_events(), "engage_refused"); ASSERT_EQ(refused.size(), 1U); @@ -342,13 +420,20 @@ TEST(Startup, ZeroStreamEngagesPerInput) } // SAFETY: the G1 writes the zero command on all 35 slots from the first step. -TEST(Startup, G1StreamsZeroFromFirstStep) +TEST(Startup, G1StreamsZeroFromFirstLowstate) { - // No /lowstate and no input: zero_command on all 35 slots from the first step, stamped - // with the G1's mode_machine. + // Nothing before the first /lowstate; from it, with no input, zero_command on all 35 + // slots, stamped with the mode_machine the /lowstate reports. const LowCmd zero = golem_safety::zero_command(g1()); SafetyCore core(g1(), config(g1(), "sim_split"), false, kDomain); - for (int i = 0; i < 100; ++i) { + for (int i = 0; i < 50; ++i) { + ASSERT_FALSE(core.step(i * kStep)) << "step " << i; + } + const auto held = named(core.drain_events(), "relay_held"); + ASSERT_EQ(held.size(), 1U); + EXPECT_EQ(held[0].second["reason"].as(), "no /lowstate received yet"); + core.on_state(state_of(g1()), 50 * kStep); + for (int i = 50; i < 150; ++i) { const auto out = core.step(i * kStep); ASSERT_TRUE(out) << "step " << i; expect_same(*out, zero); @@ -428,7 +513,7 @@ TEST(Startup, G1LimitChecksFromBoot) SCOPED_TRACE(name); const SafetyConfig cfg = config(g1(), name); SafetyCore core(g1(), cfg, false, kDomain); - ASSERT_TRUE(core.step(0.0)); + ASSERT_FALSE(core.step(0.0)) << "nothing before the first /lowstate"; LowState state = state_of(g1()); state.motor_state[kKnee].q = static_cast(cfg.limits.q_estop_low[kKnee] - 0.1); core.on_state(state, 0.002); @@ -472,10 +557,20 @@ TEST(Estop, FirstSampleTriggeredLatches) core.on_estop(true, true, 0.0); ASSERT_TRUE(core.estopped()); EXPECT_EQ(core.estop_reason(), "External estop triggered"); - // The estop command, the first of the H1-2's burst and of the G1's keep_writing. - const auto out = core.step(0.002); + // Nothing before the robot's first /lowstate, and why, once. + EXPECT_FALSE(core.step(0.002)); + EXPECT_FALSE(core.step(0.004)); + const auto waiting = named(core.drain_events(), "estop_waiting"); + ASSERT_EQ(waiting.size(), 1U); + EXPECT_EQ(waiting[0].first.kind, Event::Error); + EXPECT_EQ(waiting[0].second["reason"].as(), "External estop triggered"); + // From it on, the estop command, the first of the H1-2's burst and of the G1's + // keep_writing, and its report. + core.on_state(state_of(c.profile), 0.005); + const auto out = core.step(0.006); ASSERT_TRUE(out); expect_same(*out, golem_safety::estop_command(c.profile, cfg)); + EXPECT_EQ(named(core.drain_events(), "estop_command_written").size(), 1U); } { SafetyCore core(c.profile, cfg, true, kDomain); @@ -629,6 +724,7 @@ TEST(Estop, StaleAfterEngageLatches) // From the first sample on, also before engaging. SafetyCore early(h12(), config(h12(), "relax_safety_split"), true, kDomain); + early.on_state(state_of(h12()), 0.0); early.on_estop(false, true, 0.0); ASSERT_TRUE(early.step(0.5)); EXPECT_FALSE(early.estopped()); diff --git a/core_ws/src/golem_upper_ik/README.md b/core_ws/src/golem_upper_ik/README.md index d6817f80..b89c904e 100644 --- a/core_ws/src/golem_upper_ik/README.md +++ b/core_ws/src/golem_upper_ik/README.md @@ -153,7 +153,7 @@ and OMPL's time limits are host compute budgets, in s of steady time. within `controller.startup_move_timeout` of the gate opening. A start-up that fails after step 2 sends the shutdown command (Shutdown, below) and the -node exits 1. `golem_safety` streams zero gains from boot and engages the upper-body input on +node exits 1. `golem_safety` streams zero gains from the robot's first `/lowstate` and engages the upper-body input on its first command once `/lowstate` reports the profile's `mode_machine` (and, on the real H1-2, `estop_node` has sent a first sample), so the gate opens at the first relay, on both robots whether or not the leg node commands. diff --git a/docs/g1/control-path.md b/docs/g1/control-path.md index 7c891866..ac23604a 100644 --- a/docs/g1/control-path.md +++ b/docs/g1/control-path.md @@ -18,7 +18,7 @@ everything reads <---------- rt/lowstate, rt/dex3/{left,right}/state - **The safety layer** (`golem_safety`, split mode) writes every body motor on `rt/lowcmd`: the waist and arms from the arm controller's input, the legs - from the leg node's. From boot it writes zero gains on every motor, and + from the leg node's. From the robot's first `rt/lowstate` it writes zero gains on every motor, and each input's slots carry its commands from that input's first command on ([Safety layer](safety.md), Start-up). - **Arms.** The arm controller (`golem_upper_ik`) solves IK at diff --git a/docs/g1/safety.md b/docs/g1/safety.md index 13f704ef..3127b16e 100644 --- a/docs/g1/safety.md +++ b/docs/g1/safety.md @@ -41,10 +41,12 @@ the sim bringup passes, `safety_node` refuses ROS domain 0, an unset The G1 starts as the H1-2 does; no config key chooses it. -1. **Zero gains from boot.** From its start (under `use_sim_time`, from the - first `/clock`) the layer writes `rt/lowcmd` at - `control.publish_hz` with mode 0 and every value 0 on every slot, stamped - with `mode_machine` 5: every motor is off. It checks every `rt/lowstate` +1. **Zero gains from the first `rt/lowstate`.** The layer writes nothing until + an `rt/lowstate` reports `mode_machine` 5 (`relay_held` says why); from then + on (under `use_sim_time`, not before the first `/clock` either) it writes + `rt/lowcmd` at `control.publish_hz` with mode 0 and every value 0 on every + slot, stamped with the `mode_machine` that `rt/lowstate` reports: every + motor is off. It checks every `rt/lowstate` against the estop limits from the first sample (below). 2. **The heartbeat.** Once the newest `rt/lowstate` reports `mode_machine` 5, the layer beats true on `/safety/heartbeat` at 10 Hz: relaying can begin.