From d010043663b1675ef74e87f52b07529f5f5de36e Mon Sep 17 00:00:00 2001 From: maxconway Date: Mon, 16 Nov 2026 20:21:27 -0700 Subject: [PATCH 1/3] docs: golem_safety against upstream h12_safety_layer main, measured Compares golem_safety (main d1d790e) with correlllab/h12_safety_layer main (ad06903) on the H1-2: each layer's own loader on every config, and both layers run on the same scripted rt/lowstate, safety-input and estop sequences with every rt/lowcmd frame recorded. The derived limits and the clipping are identical (bit for bit); the differences (engaging, headers and spare slots, the estop after-burst, the upper watchdog, writer, robot and estop-silence latches, relax_safety_split's ankle estop) are listed with their evidence. golem_safety's suite passed on main: 233 tests. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_015EG7p8sgY1ZVi6unbbLp4J --- core_ws/src/golem_safety/README.md | 2 + .../README.md | 116 ++++++++ .../harness/analyze.py | 116 ++++++++ .../harness/compare_limits.py | 97 +++++++ .../harness/compare_settings.py | 25 ++ .../harness/driver.py | 267 ++++++++++++++++++ .../harness/old_runner.py | 28 ++ .../harness/run_all.sh | 9 + .../results/limits.json | 24 ++ .../results/report.txt | 229 +++++++++++++++ .../results/settings.txt | 8 + 11 files changed, 921 insertions(+) create mode 100644 docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/README.md create mode 100644 docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/harness/analyze.py create mode 100644 docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/harness/compare_limits.py create mode 100644 docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/harness/compare_settings.py create mode 100644 docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/harness/driver.py create mode 100644 docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/harness/old_runner.py create mode 100644 docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/harness/run_all.sh create mode 100644 docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/results/limits.json create mode 100644 docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/results/report.txt create mode 100644 docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/results/settings.txt diff --git a/core_ws/src/golem_safety/README.md b/core_ws/src/golem_safety/README.md index c2f07787..cda70afd 100644 --- a/core_ws/src/golem_safety/README.md +++ b/core_ws/src/golem_safety/README.md @@ -12,6 +12,8 @@ The design is `docs/superpowers/specs/2026-10-27-golem-upper-ik-and-safety-desig (section golem_safety). Every value and behavior that differs from the packages this one replaces is in the change report beside it, `docs/superpowers/specs/2026-10-27-golem-upper-ik-and-safety/change-report.md`. +Its behaviour on the H1-2 against upstream `h12_safety_layer` `main`, measured on the same +inputs, is `docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/README.md`. ## Running it diff --git a/docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/README.md b/docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/README.md new file mode 100644 index 00000000..d3603b63 --- /dev/null +++ b/docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/README.md @@ -0,0 +1,116 @@ +# golem_safety against h12_safety_layer: what is the same and what differs + +This compares GOLEM's safety layer on the H1-2, `core_ws/src/golem_safety` at GOLEM `main` +`d1d790e`, with [correlllab/h12_safety_layer](https://github.com/correlllab/h12_safety_layer) +at its `main`, `ad06903` ("Sync dev to main", 2026-10-20). No other upstream branch has a +commit past GOLEM's last pin of the package (`camera_ready` `a178f0b`, which is `ad06903` +plus six commits); those six are differences from `main` too and are marked below. + +The existing change report, +[`../2026-10-27-golem-upper-ik-and-safety/change-report.md`](../2026-10-27-golem-upper-ik-and-safety/change-report.md), +lists every difference from the pinned `a178f0b`, with its reason. This page checks the two +layers against each other directly and says, for each row, how it was checked: + +- **measured**: both layers were run on the same scripted inputs (below), and the row is what + they wrote on `rt/lowcmd`; +- **loader**: each layer's own config loader, on each config, compared value by value; +- **inspection**: read in the code only, not run. The file and line are given. + +## Summary + +The derived limits, and the clipping of a valid command on the body slots, are identical: +the clipped values are bit-for-bit the same in both modes, and the estop limits trip at the +same values. Seven of the eight upstream configs give identical limits; `relax_safety_split` +differs in one value (ankle pitch torque estop), and upstream's `sim_safety_split` has no +counterpart. The behaviour around the clipping differs: when inputs engage, what a +command's header and spare slots carry, what happens after an estop, and five triggers +(two that only `golem_safety` latches on, one, the upper watchdog, that only +`h12_safety_layer` does, and two hardware-estop rules). + +## How it was tested + +`golem_safety` was built from `main` `d1d790e` into its own workspace in the `golem_ros` +container, and its suite passed: **233 tests, 0 failures, 0 skipped** (`colcon test +--packages-select golem_safety`, including the 84 ROS-level tests of `test_node_ros.py`). + +The differential harness is in [`harness/`](harness/), its outputs in [`results/`](results/): + +- `compare_limits.py` runs `h12_safety_layer`'s own `load_config` on each of its 8 configs and + `golem_safety_dump_config h12 ` on the config of the same name, and compares every + derived array (q, dq, tau clip and estop limits, kp and kd maxima) joint by joint, and the + two joint tables. Output: `results/limits.json`. `compare_settings.py` prints the other + settings side by side (`results/settings.txt`). +- `driver.py` starts one layer (`h12_safety_layer`'s `SafetyLayer` through `old_runner.py` + on DDS domain 92, `golem_safety`'s `safety_node` with `sim:=true` on domain 93), waits + 2.5 s, then plays a scenario on Unitree DDS: `rt/lowstate` at 500 Hz, the safety inputs at + 200 Hz, the hardware estop at 50 Hz in each layer's own form (the old layer's raw + `h12/estop_status_raw` struct, the new layer's ROS Bools), the same values to both. It + records every `rt/lowcmd` frame. `analyze.py` summarises and compares them + (`results/report.txt`). Both layers ran `default_safety_split` (`default_safety_full` for + the full-body scenario), whose limits are identical, with the hardware estop off except + in the two estop scenarios. +- The old layer ran as its `SafetyLayer` class, not its ROS node, so the node-level rows + (heartbeat, exit) are by inspection. Its runner does not stop on SIGINT, so the recordings + are cut at each scenario's end, and its log lines after that end (its upper watchdog + firing once the driver stopped publishing) are not results. + +Reproduce: copy `harness/` and a checkout of `h12_safety_layer` at `ad06903` (as +`harness/h12_safety_layer`) to `/tmp/safety_diff` in the `golem_ros` container, build +`golem_safety` into `/tmp/gs` (`colcon build --packages-select golem_safety` there, with +`core_ws/install` sourced), then `bash /tmp/safety_diff/run_all.sh` and +`python3 analyze.py out`. Domains 92 and 93 must be free; the harness refuses domain 0. + +## Identical + +| Behaviour | Evidence | +|---|---| +| Joint table: names, slot order, position, velocity and torque limits of the 27 joints (`h12_safety_layer/core/joint_limits.py`, `golem_safety/tools/h12_joint_limits.py`) | loader (`limits.json`: `_joint_tables` empty) | +| Derived clip and estop limits of `default_safety_full`, `default_safety_split`, `locomotion_safety_full`, `relax_safety_full`, `tight_safety_full`, `tight_safety_split` | loader (`limits.json`) | +| Relay rate target 500 Hz (`control.publish_hz`) | loader; measured 500 frames/s new, 457-484/s old (its Python loop sleeps on wall time) | +| Clipping of a valid command on the body slots: q to the clip range, dq and tau to ± their limits, kp to [0, `kp_max`], kd to [0, `kd_max`], negative gains to 0, values exactly at and one float32 step past the limits | measured (`relay_clip_split`, `relay_clip_full`: the body slots are bit-identical in both phases) | +| Split mode: legs (slots 0-11) from the lower input, torso and arms (12-26) from the upper | measured (`relay_clip_split`, `upper_silent`, `lower_silent`) | +| A non-finite value in any body slot of a command, the input's own or not, latches the estop | measured (`nan_unowned_slot`: NaN in slot 20 of the lower input; both latch within 3 ms) | +| Estop limits are strict: a measured q, \|dq\| or \|tau_est\| past its limit latches, one at it does not; comparison in double of the float32 wire value (so a knee at float32(3.2) = 3.2000000477 latches, one float32 step below does not) | measured (`estop_q_edge`, `estop_q_below`, `estop_dq`) | +| A non-finite `/lowstate` q, dq or tau_est latches | measured (`state_nan`) | +| `/lowstate` checks run on every body slot in both modes: any non-finite value first, then q, then dq, then tau, each reporting the lowest bad slot | inspection (`h12_safety_layer/core/safety_checks.py:72-96`, `golem_safety/src/safety_core.cpp` `limit_violation`); the measured reasons name the same joint | +| A quiet lower (or full) input keeps its last command relayed | measured (`lower_silent`) | +| A hardware estop sample that says triggered latches, within one cycle | measured (`hw_estop_trigger`: old t = 1.006 s, new 1.003 s) | +| The estop is one way until restart | measured (every latch above) | + +## Different + +| Behaviour | `h12_safety_layer` `ad06903` | `golem_safety` `d1d790e` | Evidence | +|---|---|---|---| +| `relax_safety_split` ankle pitch torque estop | 60 N·m | 95 N·m (`camera_ready` `0eab20b`, for the walking policy) | loader | +| `sim_safety_split` (domain 1, no estop, upper watchdog off) | a config | no H1-2 sim config: the H1-2 sims run the real `relax_safety_split` (GOLEM `61a0af1`) | loader | +| Choosing the hardware estop | `estop.enabled` in the config (true in all but `sim_safety_split`) | the node's `use_estop` parameter; the bringups pass true | loader; inspection | +| Hardware estop input | the raw DDS struct `h12/estop_status_raw`, read on the default domain 0 whatever `ROS_DOMAIN_ID` says (`estop_subscriber.py:53`) | the ROS Bools `/estop/triggered` and `/estop/plugged_in` from `estop_node`, on `ROS_DOMAIN_ID` | inspection; measured (each layer fed its own form) | +| Engaging before the hardware estop has spoken | relays from the first command; no estop sample is needed | no input engages before the first estop sample (`engage_refused`) | measured (`hw_estop_silent`, `hw_estop_trigger`: at t = 0.2 s the old layer relays, the new writes zero gains) | +| A hardware estop that falls silent | nothing happens: the last sample stands | latches after `estop.timeout_seconds` (0.5 s) | measured (`hw_estop_silent`: samples stop at t = 1.0; old relays on, new latches at 1.485) | +| Engaging before `/lowstate` reports the robot | relays from the first command, whatever `/lowstate` says, or before any | each input engages on its first command once `/lowstate` reports `mode_machine` 6 | measured (`engage_mm0`: with `mode_machine` 0 the old layer relays, the new writes zero gains until it turns 6) | +| `/lowstate` from another robot (`mode_machine` 5) once engaged | ignored | latches | measured (`foreign_mm`) | +| A second publisher on one input | both accepted, the newest wins each cycle | latches unless the current writer has been quiet for `single_writer.handover_seconds` (0.5 s) | measured (`second_writer`) | +| Upper input quiet in split mode | past `estop.upper_stale_seconds` (default 2.0 s; set to 0.0, off, only in `relax_safety_split` and `sim_safety_split`) the whole robot estops | its last command keeps being relayed and `input_quiet` is logged (`stale.upper_seconds`: 2 s in `default`/`tight_safety_split`; 0, no report, in `relax_safety_split`); never latches | measured (`upper_silent`: old latches at 2.495 s, new still relays the arms at 3.4 s). With `relax_safety_split`, the bringups' config, neither latches | +| Command header (`mode_machine`, `mode_pr`) | copied from the last command received on any input, `LowCmdDefault` (0, 0) before any (`safety_layer.py:202,240`) | always the profile's (6, 0) | measured (`startup_idle`: old (0, 0); `relay_clip_*`: an input's (7, 1) relayed by the old layer) | +| Slots 27-34 and `reserve` | split mode: copied from the upper input's command; full body: from the full input's | always zero | measured (`relay_clip_*`: an input's 9s relayed by the old layer) | +| Output before any command | `make_estop_cmd(LowCmdDefault())` at 500 Hz: every motor off, header (0, 0) | `zero_command` at 500 Hz: every motor off, header (6, 0) | measured (`startup_idle`) | +| After an estop | the estop command (body slots mode 0, all values 0) every cycle until the process stops; its header and slots 27-34 are the last command's (`safety_layer.py:262`, `make_estop_cmd`) | 50 zero commands, then silence until restart (`camera_ready` `58d0f99`) | measured (every latch: old about 480 frames/s after it, new exactly 50); the header and spare slots by inspection | +| Estop reasons | `motor 16 (left_elbow_joint) dq out of estop range: 50.0`; `Command validation failed on split_mode_lower: q contains non-finite values` | adds the limit and its multiple and names the motor: `... 50 (limit 6.000, 8.33x)`, `... motor 20 (right_shoulder_pitch_joint) q is nan` (`camera_ready` `5182fbe`) | measured | +| Late `/lowstate` | no check | on domain 0, a `/lowstate` later than `control.state_lag_ms` (100 ms) past its tick latches (`camera_ready` `e0396fc`); off off domain 0, so not measured here | inspection | +| Quiet `/lowstate` and inputs | not reported | `state_quiet`/`input_quiet`/`input_resumed` logged; relay unchanged | measured (logs) | +| Relay structure | input callbacks clip into per-input slots; a publisher thread sends at 500 Hz of wall time | newest-only keep-last-1 inputs, one relay loop on robot time (the sim's `/clock` under `use_sim_time`) (`camera_ready` `e0396fc`) | inspection; rates measured above | +| Heartbeat `/safety/heartbeat` | true at 10 Hz from start-up until the latch, then false (`ros2/safety_node.py:63`) | none until relaying can begin (`/lowstate` reports 6 and, with `use_estop`, a first estop sample), then true at 10 Hz on the steady clock, false once latched (`camera_ready` `5b08888` for the steady clock) | inspection; `golem_safety`'s `test_node_ros.py` | +| Domain 0 | every config but `sim_safety_split` sets domain 0, and the node initialises it without asking (`ros2/safety_node.py:45`); `ROS_DOMAIN_ID` wins when set | uses `ROS_DOMAIN_ID`; with `sim:=true` refuses domain 0 | inspection; `test_sim_refuses_domain_zero` | +| Exit | the node's `destroy_node` stops the publisher thread: output stops, no estop command | output stops at once; an estop burst in progress is finished first | inspection; `golem_safety` tests | +| Implementation | Python, Unitree SDK DDS (`unitree_sdk2py`) | C++ `rclcpp`, ROS topics only (`/x` is DDS `rt/x`, same `unitree_hg` types); CRC computed as the SDK does (`test_lowcmd` checks SDK values) | inspection; test suite | +| Logging | events printed; with `logging.enabled`, chunks under `/tmp/h12_safety_layer` | events through the ROS logger from their own thread; chunks per run under `/tmp/golem_safety//` | inspection | + +## Found along the way + +- `h12_safety_layer`'s raw estop reader opens its DDS participant on the default domain (0), + not `ROS_DOMAIN_ID` (`core/estop_subscriber.py:53`), so a sim run of it on another domain + still reads, and can be latched by, an estop box publishing on the real robots' domain. + The harness pins it to its own domain. +- A workspace built before GOLEM `61a0af1` keeps a dangling + `install/golem_safety/share/golem_safety/config/h12/sim_safety_split.yaml` link + (`--symlink-install`); `safety_node` refuses that config as unreadable. diff --git a/docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/harness/analyze.py b/docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/harness/analyze.py new file mode 100644 index 00000000..598a333b --- /dev/null +++ b/docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/harness/analyze.py @@ -0,0 +1,116 @@ +"""Summarise and compare the two layers' recordings, scenario by scenario. + +Usage: python3 analyze.py (prints a plain-text report) + +For each scenario and layer: frames and rate from t = 0 to the scenario's end, the +headers written, snapshots of chosen slots at chosen times, and the estop: the first +frame after which every body slot is mode 0 with zero gains once a gain had been +relayed, how many frames followed it, and the layer's own reason line. +""" +import re +import sys +from pathlib import Path + +import numpy as np + +OUT = Path(sys.argv[1]) +# scenario -> (times to snapshot, slots to show) +SNAP = { + 'startup_idle': ([-1.0, 0.5], [0, 13, 27]), + 'relay_clip_split': ([0.4, 0.9], [0, 1, 3, 13, 14, 27, 34]), + 'relay_clip_full': ([0.4, 0.9], [0, 1, 3, 13, 14, 27, 34]), + 'nan_unowned_slot': ([0.4, 0.9], [0, 13, 20]), + 'upper_silent': ([0.4, 1.0, 2.4, 3.4], [0, 13]), + 'lower_silent': ([0.4, 1.0, 1.4], [0, 13]), + 'estop_q_edge': ([0.4, 0.9, 1.4], [0, 13]), + 'estop_q_below': ([0.4, 1.4], [0, 13]), + 'estop_dq': ([0.4, 0.9], [0, 13]), + 'state_nan': ([0.4, 0.9], [0, 13]), + 'engage_mm0': ([0.5, 1.5], [0, 13]), + 'foreign_mm': ([0.5, 1.5], [0, 13]), + 'second_writer': ([0.5, 1.2], [0, 13]), + 'hw_estop_silent': ([0.2, 0.8, 1.8], [0, 13]), + 'hw_estop_trigger': ([0.2, 0.8, 1.8], [0, 13]), +} +FIELDS = ('mode', 'q', 'dq', 'tau', 'kp', 'kd') + + +def latch(t, motor): + """Return the time of the first all-zero frame after a nonzero gain, or None.""" + gains = np.abs(motor[:, :27, 4]).sum(1) + np.abs(motor[:, :27, 5]).sum(1) + on = np.nonzero(gains > 0)[0] + if not len(on): + return None + zero = np.nonzero((gains == 0) & (np.arange(len(t)) > on[0]) + & (np.abs(motor[:, :27, :]).sum((1, 2)) == 0))[0] + return float(t[zero[0]]) if len(zero) else None + + +def reason(log): + """Return the layer's estop and engage lines, shortened.""" + lines = [] + for line in log.splitlines(): + if re.search(r'estop|second writer|engage_refused|input_engaged|input_quiet|' + r'input_resumed|stale|Command validation|state_quiet', line, re.I): + line = re.sub(r'^\[\w+\] \[[\d.]+\] \[safety_node\]: ', '', line) + lines.append(line[:230]) + # keep first of each kind + seen, out = set(), [] + for line in lines: + key = line[:40] + if key not in seen: + seen.add(key) + out.append(line) + return out[:8] + + +def main(): + """Print the report.""" + bits = {} + for scen, (times, slots) in SNAP.items(): + print(f'\n=== {scen}') + for layer in ('old', 'new'): + f = OUT / f'{layer}_{scen}.npz' + if not f.exists(): + print(f' {layer}: missing') + continue + d = np.load(f) + dur = float(d['duration']) + keep = d['t'] <= dur + t, motor = d['t'][keep], d['motor'][keep] + mm, pr, res = d['mode_machine'][keep], d['mode_pr'][keep], d['reserve'][keep] + run = (t >= 0) & (t <= dur) + first = t[0] if len(t) else float('nan') + print(f' {layer}: frames in [0,{dur}]: {run.sum()} ({run.sum() / dur:.0f}/s); ' + f'first frame t={first:.2f}; headers (mm,pr) {sorted(set(zip(mm.tolist(), pr.tolist())))}; ' + f'reserve {sorted(set(map(tuple, res.tolist())))}') + lt = latch(t, motor) + if lt is not None: + after = (t >= lt).sum() + last = t[-1] + print(f' estop at t={lt:.3f}: {after} frames from then on, last t={last:.3f}') + elif len(t) and t[-1] < dur - 0.05: + print(f' output stops at t={t[-1]:.3f}') + for ts in times: + i = np.searchsorted(t, ts) - 1 + if i < 0: + print(f' t={ts}: no frame yet') + continue + row = '; '.join(f'{s}:' + ','.join(f'{v:.4g}' for v in motor[i, s]) for s in slots) + print(f' t={ts} [{"/".join(FIELDS)}] {row}') + if scen.startswith('relay_clip'): + for lo, hi in ((0.1, 0.45), (0.6, 0.95)): + w = (t > lo) & (t < hi) + rows = {r.tobytes() for r in motor[w, :27, :].astype(np.float32)} + print(f' distinct body rows in ({lo},{hi}): {len(rows)}') + bits.setdefault(scen, {}).setdefault((lo, hi), {})[layer] = rows + log = (OUT / f'{layer}_{scen}.log').read_text() + for line in reason(log): + print(f' log: {line}') + print('\n=== bitwise: clipped body slots, old vs new') + for scen, wins in bits.items(): + for win, by in wins.items(): + print(f' {scen} {win}: identical={by.get("old") == by.get("new")}') + + +main() diff --git a/docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/harness/compare_limits.py b/docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/harness/compare_limits.py new file mode 100644 index 00000000..7bae19f2 --- /dev/null +++ b/docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/harness/compare_limits.py @@ -0,0 +1,97 @@ +"""Compare h12_safety_layer's derived limits with golem_safety's, config by config. + +Usage (in the golem_ros container, with core_ws sourced): + python3 compare_limits.py > limits.json + +Loads every config of the old checkout with its own load_config, dumps the +golem_safety config of the same name with golem_safety_dump_config, and prints, +per config and per array, the joints whose values differ (old, new). It also +compares the two joint tables. Imports no DDS: config.py's channel factory import +is stubbed. +""" +import json +import subprocess +import sys +import types +from pathlib import Path + +import numpy as np + +OLD = Path(sys.argv[1]) +DUMP = '/tmp/gs/install/golem_safety/lib/golem_safety/golem_safety_dump_config' +NEW_TOOLS = Path('/home/code/core_ws/src/golem_safety/tools') + +# Stub the SDK's channel module: load_config never calls it. +channel = types.ModuleType('unitree_sdk2py.core.channel') +channel.ChannelFactoryInitialize = None +sys.modules.setdefault('unitree_sdk2py', types.ModuleType('unitree_sdk2py')) +sys.modules.setdefault('unitree_sdk2py.core', types.ModuleType('unitree_sdk2py.core')) +sys.modules['unitree_sdk2py.core.channel'] = channel +sys.path.insert(0, str(OLD)) +from h12_safety_layer.core import joint_limits as old_jl # noqa: E402 +from h12_safety_layer.core.config import load_config # noqa: E402 + +PAIRS = { # old key -> new key(s); q limits are (n, 2) in the old loader + 'q_clip_limits': ('q_clip_low', 'q_clip_high'), + 'dq_clip_limits': ('dq_clip',), + 'tau_clip_limits': ('tau_clip',), + 'kp_clip_max': ('kp_max',), + 'kd_clip_max': ('kd_max',), + 'q_estop_limits': ('q_estop_low', 'q_estop_high'), + 'dq_estop_limits': ('dq_estop',), + 'tau_estop_limits': ('tau_estop',), +} + + +def diff(name, old, new, names): + """Return {joint: [old, new]} where the arrays differ beyond 1e-9.""" + old = np.broadcast_to(np.asarray(old, float), (len(names),)) + new = np.broadcast_to(np.asarray(new, float), (len(names),)) + return {names[i]: [round(float(old[i]), 6), round(float(new[i]), 6)] + for i in np.nonzero(np.abs(old - new) > 1e-9)[0]} + + +def joint_tables(): + """Return the joints whose URDF-table limits differ between the two packages.""" + sys.path.insert(0, str(NEW_TOOLS)) + import h12_joint_limits as new_jl + out = {} + if list(new_jl.JOINT_NAMES) != list(old_jl.JOINT_NAMES): + out['names'] = [list(old_jl.JOINT_NAMES), list(new_jl.JOINT_NAMES)] + for table in ('URDF_POSITION_LIMITS', 'URDF_VELOCITY_LIMITS', 'URDF_TORQUE_LIMITS'): + old_t, new_t = getattr(old_jl, table), getattr(new_jl, table) + rows = {old_jl.JOINT_NAMES[i]: [o, n] for i, (o, n) in enumerate(zip(old_t, new_t)) + if o != n} + if rows: + out[table] = rows + return out + + +def main(): + """Print the per-config differences as JSON.""" + names = old_jl.JOINT_NAMES + report = {} + for path in sorted((OLD / 'config').glob('*.yaml')): + old = load_config(path)['limits'] + dumped = subprocess.run([DUMP, 'h12', path.stem], capture_output=True, text=True) + if dumped.returncode != 0: + report[path.stem] = {'missing_in_golem_safety': dumped.stderr.strip()[-200:]} + continue + new = json.loads(dumped.stdout)['limits'] + entry = {} + for okey, nkeys in PAIRS.items(): + if len(nkeys) == 2: + for col, nkey in enumerate(nkeys): + d = diff(nkey, old[okey][:, col], new[nkey], names) + if d: + entry[nkey] = d + else: + d = diff(nkeys[0], old[okey], new[nkeys[0]], names) + if d: + entry[nkeys[0]] = d + report[path.stem] = entry or 'identical' + report['_joint_tables'] = joint_tables() + print(json.dumps(report, indent=1)) + + +main() diff --git a/docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/harness/compare_settings.py b/docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/harness/compare_settings.py new file mode 100644 index 00000000..c4d5daf2 --- /dev/null +++ b/docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/harness/compare_settings.py @@ -0,0 +1,25 @@ +"""Print each config's non-limit settings in h12_safety_layer and golem_safety, side by side. + +Usage (golem_ros container, core_ws sourced): python3 compare_settings.py +""" +import json, subprocess, sys, types +from pathlib import Path +OLD = Path(sys.argv[1]) +ch = types.ModuleType('unitree_sdk2py.core.channel'); ch.ChannelFactoryInitialize = None +for m in ('unitree_sdk2py', 'unitree_sdk2py.core'): + sys.modules.setdefault(m, types.ModuleType(m)) +sys.modules['unitree_sdk2py.core.channel'] = ch +sys.path.insert(0, str(OLD)) +from h12_safety_layer.core.config import load_config # noqa: E402 +DUMP = '/tmp/gs/install/golem_safety/lib/golem_safety/golem_safety_dump_config' +for p in sorted((OLD / 'config').glob('*.yaml')): + o = load_config(p) + r = subprocess.run([DUMP, 'h12', p.stem], capture_output=True, text=True) + n = json.loads(r.stdout) if r.returncode == 0 else {} + print(p.stem, json.dumps({ + 'old': {'mode': o['mode'], 'hz': o['control']['publish_hz'], 'estop': o['estop'], + 'domain': o['network']['domain_id'], 'log': o['logging']['enabled'], + 'topics': o['topics']}, + 'new': {k: n.get(k) for k in ('mode', 'control', 'stale', 'single_writer', 'estop', + 'estop_response', 'topics')} | {'log': n.get('logging', {}).get('enabled')}, + })) diff --git a/docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/harness/driver.py b/docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/harness/driver.py new file mode 100644 index 00000000..0b192997 --- /dev/null +++ b/docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/harness/driver.py @@ -0,0 +1,267 @@ +"""Play one scripted scenario to one safety layer and record everything it writes on rt/lowcmd. + +Usage (golem_ros container, core_ws sourced): + python3 driver.py [] + +Starts the layer as a subprocess on (never 0), waits SETTLE s, then plays the +scenario on Unitree DDS: rt/lowstate at 500 Hz, the safety inputs at 200 Hz and the +hardware estop at 50 Hz, in the form each layer reads (old: h12/estop_status_raw; new: +the ROS Bools rt/estop/triggered and rt/estop/plugged_in). Both layers see the same +values. Writes /_.npz (every frame received, with its time from +the start of the scenario; negative while settling) and _.log (the +layer's own output). Exit code 0 unless the layer could not be started. +""" +import json +import os +import subprocess +import sys +import threading +import time +from dataclasses import dataclass +from pathlib import Path + +import numpy as np +from cyclonedds.domain import DomainParticipant +from cyclonedds.idl import IdlStruct +from cyclonedds.pub import DataWriter +from cyclonedds.topic import Topic +from unitree_sdk2py.core.channel import (ChannelFactoryInitialize, ChannelPublisher, + ChannelSubscriber) +from unitree_sdk2py.idl.default import (unitree_hg_msg_dds__LowCmd_ as LowCmdDefault, + unitree_hg_msg_dds__LowState_ as LowStateDefault) +from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowCmd_, LowState_ + +LAYER, SCENARIO, DOMAIN, OUT = sys.argv[1], sys.argv[2], int(sys.argv[3]), Path(sys.argv[4]) +OLD_CHECKOUT = sys.argv[5] if len(sys.argv) > 5 else '/tmp/safety_diff/h12_safety_layer' +# SAFETY: domain 0 is the real robots' command bus; this harness never writes there. +if DOMAIN == 0: + sys.exit('driver refuses domain 0') +SETTLE = 2.5 # s for the layer to start before the scenario's t = 0 +N = 27 # H1-2 body slots +SLOTS = 35 # motor slots on the wire +DUMP = '/tmp/gs/install/golem_safety/lib/golem_safety/golem_safety_dump_config' + + +@dataclass +class RosBool(IdlStruct, typename='std_msgs::msg::dds_::Bool_'): + """std_msgs/Bool as ROS 2 puts it on DDS.""" + + data: bool + + +@dataclass +class EStopStatus(IdlStruct, typename='h12.estop.EstopStatusRaw'): + """The old layer's raw estop sample (h12_safety_layer.core.estop_subscriber).""" + + triggered: bool + plugged_in: bool + sequence_id: int + stamp_ms: int + + +def limits(config): + """Return golem_safety's derived limits of an H1-2 config (the old ones are equal, limits.json).""" + out = subprocess.run([DUMP, 'h12', config], capture_output=True, text=True, check=True) + return {k: np.asarray(v, float) for k, v in json.loads(out.stdout)['limits'].items()} + + +def valid_cmd(q=0.1, kp=50.0, kd=2.0): + """Return per-slot (mode, q, dq, tau, kp, kd) arrays of an in-limit command.""" + return dict(mode=np.ones(N), q=np.full(N, q), dq=np.zeros(N), tau=np.zeros(N), + kp=np.full(N, kp), kd=np.full(N, kd)) + + +# Each scenario: config, mode, estop use, duration (s) and a function of t (s) returning +# {'state': {...overrides} or None, 'lower'/'upper'/'full': cmd dict or None, 'extra': ...}. +def scenarios(): + """Return the scenario table.""" + lim = limits('default_safety_split') + f32 = np.float32 + + def over_cmd(): + """Return a command past every clip limit, with header and spare-slot payloads.""" + c = valid_cmd() + c['q'] = np.where(np.arange(N) % 2 == 0, lim['q_clip_high'] + 0.5, lim['q_clip_low'] - 0.5) + c['dq'] = np.full(N, 100.0) + c['tau'] = np.full(N, -1000.0) + c['kp'] = np.where(np.arange(N) % 2 == 0, 1000.0, -5.0) + c['kd'] = np.where(np.arange(N) % 2 == 0, 100.0, -1.0) + c['header'] = (7, 1) + c['spare'] = 9.0 + return c + + def edge_cmd(): + """Return q at the float32 clip limits and one float32 step past them.""" + c = valid_cmd() + hi = lim['q_clip_high'].astype(f32) + c['q'] = np.where(np.arange(N) % 2 == 0, hi, np.nextafter(hi, f32(np.inf))) + c['dq'] = lim['dq_clip'].astype(f32) + c['kp'] = np.full(N, 500.0) + return c + + def both(c): + """Send c on both split inputs.""" + return {'lower': c, 'upper': c} + + def q_at(joint, value): + """Return a state override putting one joint's q at value.""" + q = np.zeros(N) + q[joint] = value + return {'q': q} + + knee_hi = lim['q_estop_high'][3] + s = { + 'startup_idle': ('default_safety_split', False, 1.0, lambda t: {}), + 'relay_clip_split': ('default_safety_split', False, 1.0, + lambda t: both(over_cmd() if t < 0.5 else edge_cmd())), + 'relay_clip_full': ('default_safety_full', False, 1.0, lambda t: {'full': over_cmd()}), + 'nan_unowned_slot': ('default_safety_split', False, 1.0, lambda t: { + 'lower': dict(valid_cmd(), nan_at=20) if t >= 0.5 else valid_cmd(), + 'upper': valid_cmd()}), + 'upper_silent': ('default_safety_split', False, 3.5, lambda t: { + 'lower': valid_cmd(0.05), 'upper': valid_cmd(0.2) if t < 0.5 else None}), + 'lower_silent': ('default_safety_split', False, 1.5, lambda t: { + 'lower': valid_cmd(0.05) if t < 0.5 else None, 'upper': valid_cmd(0.2)}), + 'estop_q_edge': ('default_safety_split', False, 2.0, lambda t: dict( + both(valid_cmd()), + state=q_at(3, 0.0 if t < 0.5 else f32(knee_hi) if t < 1.0 + else knee_hi + 0.01))), + 'estop_q_below': ('default_safety_split', False, 1.5, lambda t: dict( + both(valid_cmd()), state=q_at(3, np.nextafter(f32(knee_hi), f32(0)) if t >= 0.5 + else 0.0))), + 'estop_dq': ('default_safety_split', False, 1.5, lambda t: dict( + both(valid_cmd()), state={'dq': np.where(np.arange(N) == 16, 50.0, 0.0)} + if t >= 0.5 else None)), + 'state_nan': ('default_safety_split', False, 1.5, lambda t: dict( + both(valid_cmd()), state={'tau': np.where(np.arange(N) == 5, np.nan, 0.0)} + if t >= 0.5 else None)), + 'engage_mm0': ('default_safety_split', False, 2.0, lambda t: dict( + both(valid_cmd()), state={'mode_machine': 0 if t < 1.0 else 6})), + 'foreign_mm': ('default_safety_split', False, 2.0, lambda t: dict( + both(valid_cmd()), state={'mode_machine': 6 if t < 1.0 else 5})), + 'second_writer': ('default_safety_split', False, 1.5, lambda t: dict( + both(valid_cmd()), second_lower=valid_cmd(0.3) if t >= 0.8 else None)), + 'hw_estop_silent': ('default_safety_split', True, 2.0, lambda t: dict( + both(valid_cmd()), estop=(False, True) if 0.3 <= t < 1.0 else None)), + 'hw_estop_trigger': ('default_safety_split', True, 2.0, lambda t: dict( + both(valid_cmd()), estop=(t >= 1.0, True) if t >= 0.3 else None)), + } + return s + + +def fill_cmd(msg, c): + """Write command dict c into a LowCmd_.""" + mm, pr = c.get('header', (6, 0)) + msg.mode_machine, msg.mode_pr = mm, pr + for i in range(SLOTS): + m = msg.motor_cmd[i] + if i < N: + m.mode = int(c['mode'][i]) + m.q, m.dq, m.tau = float(c['q'][i]), float(c['dq'][i]), float(c['tau'][i]) + m.kp, m.kd = float(c['kp'][i]), float(c['kd'][i]) + else: + v = c.get('spare', 0.0) + m.mode, m.q, m.dq, m.tau, m.kp, m.kd = int(v), v, v, v, v, v + m.reserve = int(c.get('spare', 0)) + if 'nan_at' in c: + msg.motor_cmd[c['nan_at']].q = float('nan') + msg.reserve = [int(c.get('spare', 0))] * 4 + msg.crc = 0 + + +def start_layer(config, use_estop, log): + """Start the layer under test on DOMAIN; return its Popen.""" + env = dict(os.environ, ROS_DOMAIN_ID=str(DOMAIN)) + if LAYER == 'old': + cmd = ['python3', '-u', '/tmp/safety_diff/old_runner.py', OLD_CHECKOUT, + f'{OLD_CHECKOUT}/config/{config}.yaml', '1' if use_estop else '0'] + else: + cmd = ['/tmp/gs/install/golem_safety/lib/golem_safety/safety_node', '--ros-args', '-p', 'robot:=h12', + '-p', f'config:={config}', '-p', 'sim:=true', + '-p', f'use_estop:={"true" if use_estop else "false"}'] + return subprocess.Popen(cmd, env=env, stdout=log, stderr=subprocess.STDOUT, + start_new_session=True) + + +def main(): + """Run the scenario and save the recording.""" + config, use_estop, duration, script = scenarios()[SCENARIO] + OUT.mkdir(parents=True, exist_ok=True) + ChannelFactoryInitialize(DOMAIN) + frames, lock = [], threading.Lock() + t0 = [time.monotonic() + 1e9] + + def on_cmd(msg): + """Record one rt/lowcmd frame.""" + row = np.array([[m.mode, m.q, m.dq, m.tau, m.kp, m.kd] for m in msg.motor_cmd]) + with lock: + frames.append((time.monotonic() - t0[0], msg.mode_machine, msg.mode_pr, + row, list(msg.reserve), msg.crc)) + + sub = ChannelSubscriber('rt/lowcmd', LowCmd_) + sub.Init(on_cmd, 100) + pubs = {k: ChannelPublisher(f'rt/safety/{t}', LowCmd_) for k, t in + (('lower', 'lowcmd_lower_in'), ('upper', 'lowcmd_upper_in'), ('full', 'lowcmd_in'))} + second = ChannelPublisher('rt/safety/lowcmd_lower_in', LowCmd_) + state_pub = ChannelPublisher('rt/lowstate', LowState_) + for p in (*pubs.values(), second, state_pub): + p.Init() + dp = DomainParticipant(DOMAIN) + trig_w = DataWriter(dp, Topic(dp, 'rt/estop/triggered', RosBool)) + plug_w = DataWriter(dp, Topic(dp, 'rt/estop/plugged_in', RosBool)) + raw_w = DataWriter(dp, Topic(dp, 'h12/estop_status_raw', EStopStatus)) + + log = open(OUT / f'{LAYER}_{SCENARIO}.log', 'w') + proc = start_layer(config, use_estop, log) + start = time.monotonic() + t0[0] = start + SETTLE + end = t0[0] + duration + tick, seq, k = 0, 0, 0 + state, cmd = LowStateDefault(), LowCmdDefault() + while (now := time.monotonic()) < end: + t = now - t0[0] + step = script(max(t, 0.0)) if t >= 0 else {} + ov = step.get('state') or {} + state.mode_machine = int(ov.get('mode_machine', 6)) + tick += 2 + state.tick = tick + for i in range(N): + ms = state.motor_state[i] + ms.q = float(ov['q'][i]) if 'q' in ov else 0.0 + ms.dq = float(ov['dq'][i]) if 'dq' in ov else 0.0 + ms.tau_est = float(ov['tau'][i]) if 'tau' in ov else 0.0 + state_pub.Write(state) + if k % 5 == 0: # 200 Hz commands + for key, pub in pubs.items(): + if step.get(key) is not None: + fill_cmd(cmd, step[key]) + pub.Write(cmd) + if step.get('second_lower') is not None: + fill_cmd(cmd, step['second_lower']) + second.Write(cmd) + if k % 10 == 0 and step.get('estop') is not None: # 50 Hz estop samples + trig, plug = step['estop'] + seq += 1 + trig_w.write(RosBool(bool(trig))) + plug_w.write(RosBool(bool(plug))) + raw_w.write(EStopStatus(bool(trig), bool(plug), seq, int(time.time() * 1000))) + k += 1 + time.sleep(max(0.0, start + k * 0.002 - time.monotonic())) + time.sleep(0.2) + os.killpg(proc.pid, 2) # SIGINT to the layer's process group: its exit sequence + try: + proc.wait(3) + except subprocess.TimeoutExpired: + os.killpg(proc.pid, 9) + with lock: + rec = list(frames) + np.savez_compressed( + OUT / f'{LAYER}_{SCENARIO}.npz', + t=np.array([r[0] for r in rec]), mode_machine=np.array([r[1] for r in rec]), + mode_pr=np.array([r[2] for r in rec]), motor=np.array([r[3] for r in rec]), + reserve=np.array([r[4] for r in rec]), crc=np.array([r[5] for r in rec]), + duration=duration) + print(f'{LAYER} {SCENARIO}: {len(rec)} frames') + + +main() diff --git a/docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/harness/old_runner.py b/docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/harness/old_runner.py new file mode 100644 index 00000000..6bb72345 --- /dev/null +++ b/docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/harness/old_runner.py @@ -0,0 +1,28 @@ +"""Run h12_safety_layer's SafetyLayer on a sim domain, never domain 0. + +Usage: ROS_DOMAIN_ID= python3 old_runner.py + +The old layer's DDS channel factory reads ROS_DOMAIN_ID; its raw estop reader +opens a DomainParticipant on the default domain (0), so it is pinned here to +ROS_DOMAIN_ID as well. Logging to disk is off. Prints the layer's events. +""" +import os +import sys + +DOMAIN = int(os.environ.get('ROS_DOMAIN_ID', '0')) +# SAFETY: domain 0 is the real robots' command bus; this harness never writes there. +if DOMAIN == 0: + sys.exit('old_runner refuses ROS_DOMAIN_ID 0') +sys.path.insert(0, sys.argv[1]) + +from cyclonedds.domain import DomainParticipant # noqa: E402 +import h12_safety_layer.core.estop_subscriber as es # noqa: E402 +from h12_safety_layer.core.config import init_channel_factory, load_config # noqa: E402 +from h12_safety_layer.core.safety_layer import SafetyLayer # noqa: E402 + +es.DomainParticipant = lambda: DomainParticipant(DOMAIN) +config = load_config(sys.argv[2]) +config['estop']['enabled'] = sys.argv[3] == '1' +config['logging']['enabled'] = False +init_channel_factory(config) +SafetyLayer(config).start() diff --git a/docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/harness/run_all.sh b/docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/harness/run_all.sh new file mode 100644 index 00000000..bcf33d93 --- /dev/null +++ b/docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/harness/run_all.sh @@ -0,0 +1,9 @@ +#!/bin/bash +# Run every scenario on both layers (old on domain 92, new on 93) into ./out. +# Usage (golem_ros container): bash run_all.sh [scenario ...] +cd /tmp/safety_diff +source /opt/ros/humble/setup.bash; source /home/code/core_ws/install/setup.bash +S="${@:-startup_idle relay_clip_split relay_clip_full nan_unowned_slot upper_silent lower_silent estop_q_edge estop_dq state_nan engage_mm0 foreign_mm second_writer hw_estop_silent hw_estop_trigger}" +for s in $S; do + python3 driver.py old $s 92 out & python3 driver.py new $s 93 out; wait +done diff --git a/docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/results/limits.json b/docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/results/limits.json new file mode 100644 index 00000000..415cb9aa --- /dev/null +++ b/docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/results/limits.json @@ -0,0 +1,24 @@ +{ + "default_safety_full": "identical", + "default_safety_split": "identical", + "locomotion_safety_full": "identical", + "relax_safety_full": "identical", + "relax_safety_split": { + "tau_estop": { + "left_ankle_pitch_joint": [ + 60.0, + 95.0004 + ], + "right_ankle_pitch_joint": [ + 60.0, + 95.0004 + ] + } + }, + "sim_safety_split": { + "missing_in_golem_safety": "no config 'sim_safety_split' for robot h12: cannot read /home/code/core_ws/install/golem_safety/share/golem_safety/config/h12/sim_safety_split.yaml" + }, + "tight_safety_full": "identical", + "tight_safety_split": "identical", + "_joint_tables": {} +} diff --git a/docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/results/report.txt b/docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/results/report.txt new file mode 100644 index 00000000..c994b1dc --- /dev/null +++ b/docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/results/report.txt @@ -0,0 +1,229 @@ + +=== startup_idle + old: frames in [0,1.0]: 484 (484/s); first frame t=-2.05; headers (mm,pr) [(0, 0)]; reserve [(0, 0, 0, 0)] + t=-1.0 [mode/q/dq/tau/kp/kd] 0:0,0,0,0,0,0; 13:0,0,0,0,0,0; 27:0,0,0,0,0,0 + t=0.5 [mode/q/dq/tau/kp/kd] 0:0,0,0,0,0,0; 13:0,0,0,0,0,0; 27:0,0,0,0,0,0 + new: frames in [0,1.0]: 500 (500/s); first frame t=-2.49; headers (mm,pr) [(6, 0)]; reserve [(0, 0, 0, 0)] + t=-1.0 [mode/q/dq/tau/kp/kd] 0:0,0,0,0,0,0; 13:0,0,0,0,0,0; 27:0,0,0,0,0,0 + t=0.5 [mode/q/dq/tau/kp/kd] 0:0,0,0,0,0,0; 13:0,0,0,0,0,0; 27:0,0,0,0,0,0 + log: {"config": "/home/code/core_ws/install/golem_safety/share/golem_safety/config/h12/default_safety_split.yaml", "event": "relay_started", "mode": "split_mode", "robot": "h12", "sim": true, "use_estop": false} + +=== relay_clip_split + old: frames in [0,1.0]: 465 (465/s); first frame t=-2.05; headers (mm,pr) [(0, 0), (6, 0), (7, 1)]; reserve [(0, 0, 0, 0), (9, 9, 9, 9)] + t=0.4 [mode/q/dq/tau/kp/kd] 0:1,0.429,2.3,-120,500,30; 1:1,-3.139,2.3,-120,0,0; 3:1,-0.119,1.4,-180,0,0; 13:1,-3.139,0.9,-24,0,0; 14:1,3.399,0.9,-24,500,30; 27:9,9,9,9,9,9; 34:9,9,9,9,9,9 + t=0.9 [mode/q/dq/tau/kp/kd] 0:1,0.429,2.3,0,500,2; 1:1,2.499,2.3,0,500,2; 3:1,2.189,1.4,0,500,2; 13:1,1.569,0.9,0,500,2; 14:1,3.399,0.9,0,500,2; 27:0,0,0,0,0,0; 34:0,0,0,0,0,0 + distinct body rows in (0.1,0.45): 1 + distinct body rows in (0.6,0.95): 1 + log: {"action_ms": 1794885459540, "decision_ms": 1794885459539, "decision_to_action_ms": 1, "event": "estop_command_written", "reason": "Upper-body command stale for 2.00s (> 2.0s)", "trigger_stamp_ms": null} + new: frames in [0,1.0]: 500 (500/s); first frame t=-2.49; headers (mm,pr) [(6, 0)]; reserve [(0, 0, 0, 0)] + t=0.4 [mode/q/dq/tau/kp/kd] 0:1,0.429,2.3,-120,500,30; 1:1,-3.139,2.3,-120,0,0; 3:1,-0.119,1.4,-180,0,0; 13:1,-3.139,0.9,-24,0,0; 14:1,3.399,0.9,-24,500,30; 27:0,0,0,0,0,0; 34:0,0,0,0,0,0 + t=0.9 [mode/q/dq/tau/kp/kd] 0:1,0.429,2.3,0,500,2; 1:1,2.499,2.3,0,500,2; 3:1,2.189,1.4,0,500,2; 13:1,1.569,0.9,0,500,2; 14:1,3.399,0.9,0,500,2; 27:0,0,0,0,0,0; 34:0,0,0,0,0,0 + distinct body rows in (0.1,0.45): 1 + distinct body rows in (0.6,0.95): 1 + log: {"config": "/home/code/core_ws/install/golem_safety/share/golem_safety/config/h12/default_safety_split.yaml", "event": "relay_started", "mode": "split_mode", "robot": "h12", "sim": true, "use_estop": false} + log: {"event": "input_engaged", "source": "split_mode_lower", "topic": "/safety/lowcmd_lower_in"} + log: {"age": 0.102, "event": "state_quiet", "seconds": 0.1, "topic": "/lowstate"} + log: {"age": 0.202, "event": "input_quiet", "seconds": 0.2, "source": "split_mode_lower", "topic": "/safety/lowcmd_lower_in"} + +=== relay_clip_full + old: frames in [0,1.0]: 478 (478/s); first frame t=-2.05; headers (mm,pr) [(0, 0), (7, 1)]; reserve [(0, 0, 0, 0), (9, 9, 9, 9)] + t=0.4 [mode/q/dq/tau/kp/kd] 0:1,0.429,2.3,-120,500,30; 1:1,-3.139,2.3,-120,0,0; 3:1,-0.119,1.4,-180,0,0; 13:1,-3.139,0.9,-24,0,0; 14:1,3.399,0.9,-24,500,30; 27:9,9,9,9,9,9; 34:9,9,9,9,9,9 + t=0.9 [mode/q/dq/tau/kp/kd] 0:1,0.429,2.3,-120,500,30; 1:1,-3.139,2.3,-120,0,0; 3:1,-0.119,1.4,-180,0,0; 13:1,-3.139,0.9,-24,0,0; 14:1,3.399,0.9,-24,500,30; 27:9,9,9,9,9,9; 34:9,9,9,9,9,9 + distinct body rows in (0.1,0.45): 1 + distinct body rows in (0.6,0.95): 1 + new: frames in [0,1.0]: 500 (500/s); first frame t=-2.49; headers (mm,pr) [(6, 0)]; reserve [(0, 0, 0, 0)] + t=0.4 [mode/q/dq/tau/kp/kd] 0:1,0.429,2.3,-120,500,30; 1:1,-3.139,2.3,-120,0,0; 3:1,-0.119,1.4,-180,0,0; 13:1,-3.139,0.9,-24,0,0; 14:1,3.399,0.9,-24,500,30; 27:0,0,0,0,0,0; 34:0,0,0,0,0,0 + t=0.9 [mode/q/dq/tau/kp/kd] 0:1,0.429,2.3,-120,500,30; 1:1,-3.139,2.3,-120,0,0; 3:1,-0.119,1.4,-180,0,0; 13:1,-3.139,0.9,-24,0,0; 14:1,3.399,0.9,-24,500,30; 27:0,0,0,0,0,0; 34:0,0,0,0,0,0 + distinct body rows in (0.1,0.45): 1 + distinct body rows in (0.6,0.95): 1 + log: {"config": "/home/code/core_ws/install/golem_safety/share/golem_safety/config/h12/default_safety_full.yaml", "event": "relay_started", "mode": "full_body_mode", "robot": "h12", "sim": true, "use_estop": false} + log: {"event": "input_engaged", "source": "full_body_mode", "topic": "/safety/lowcmd_in"} + log: {"age": 0.102, "event": "state_quiet", "seconds": 0.1, "topic": "/lowstate"} + log: {"age": 0.202, "event": "input_quiet", "seconds": 0.2, "source": "full_body_mode", "topic": "/safety/lowcmd_in"} + +=== nan_unowned_slot + old: frames in [0,1.0]: 470 (470/s); first frame t=-2.05; headers (mm,pr) [(0, 0), (6, 0)]; reserve [(0, 0, 0, 0)] + estop at t=0.505: 239 frames from then on, last t=0.999 + t=0.4 [mode/q/dq/tau/kp/kd] 0:1,0.1,0,0,50,2; 13:1,0.1,0,0,50,2; 20:1,0.1,0,0,50,2 + t=0.9 [mode/q/dq/tau/kp/kd] 0:0,0,0,0,0,0; 13:0,0,0,0,0,0; 20:0,0,0,0,0,0 + log: {"action_ms": 1794885473056, "decision_ms": 1794885473054, "decision_to_action_ms": 2, "event": "estop_command_written", "reason": "Command validation failed on split_mode_lower: q contains non-finite values", "trigger_stamp_ms": + new: frames in [0,1.0]: 301 (301/s); first frame t=-2.49; headers (mm,pr) [(6, 0)]; reserve [(0, 0, 0, 0)] + estop at t=0.503: 50 frames from then on, last t=0.601 + t=0.4 [mode/q/dq/tau/kp/kd] 0:1,0.1,0,0,50,2; 13:1,0.1,0,0,50,2; 20:1,0.1,0,0,50,2 + t=0.9 [mode/q/dq/tau/kp/kd] 0:0,0,0,0,0,0; 13:0,0,0,0,0,0; 20:0,0,0,0,0,0 + log: {"config": "/home/code/core_ws/install/golem_safety/share/golem_safety/config/h12/default_safety_split.yaml", "event": "relay_started", "mode": "split_mode", "robot": "h12", "sim": true, "use_estop": false} + log: {"event": "input_engaged", "source": "split_mode_lower", "topic": "/safety/lowcmd_lower_in"} + log: {"action_ms": 3372253948, "decision_ms": 3372253948, "decision_to_action_ms": 0, "event": "estop_command_written", "reason": "Command validation failed on /safety/lowcmd_lower_in: motor 20 (right_shoulder_pitch_joint) q is nan", " + +=== upper_silent + old: frames in [0,3.5]: 1675 (479/s); first frame t=-2.05; headers (mm,pr) [(0, 0), (6, 0)]; reserve [(0, 0, 0, 0)] + estop at t=2.495: 487 frames from then on, last t=3.499 + t=0.4 [mode/q/dq/tau/kp/kd] 0:1,0.05,0,0,50,2; 13:1,0.2,0,0,50,2 + t=1.0 [mode/q/dq/tau/kp/kd] 0:1,0.05,0,0,50,2; 13:1,0.2,0,0,50,2 + t=2.4 [mode/q/dq/tau/kp/kd] 0:1,0.05,0,0,50,2; 13:1,0.2,0,0,50,2 + t=3.4 [mode/q/dq/tau/kp/kd] 0:0,0,0,0,0,0; 13:0,0,0,0,0,0 + log: {"action_ms": 1794885483043, "decision_ms": 1794885483041, "decision_to_action_ms": 2, "event": "estop_command_written", "reason": "Upper-body command stale for 2.00s (> 2.0s)", "trigger_stamp_ms": null} + new: frames in [0,3.5]: 1750 (500/s); first frame t=-2.49; headers (mm,pr) [(6, 0)]; reserve [(0, 0, 0, 0)] + t=0.4 [mode/q/dq/tau/kp/kd] 0:1,0.05,0,0,50,2; 13:1,0.2,0,0,50,2 + t=1.0 [mode/q/dq/tau/kp/kd] 0:1,0.05,0,0,50,2; 13:1,0.2,0,0,50,2 + t=2.4 [mode/q/dq/tau/kp/kd] 0:1,0.05,0,0,50,2; 13:1,0.2,0,0,50,2 + t=3.4 [mode/q/dq/tau/kp/kd] 0:1,0.05,0,0,50,2; 13:1,0.2,0,0,50,2 + log: {"config": "/home/code/core_ws/install/golem_safety/share/golem_safety/config/h12/default_safety_split.yaml", "event": "relay_started", "mode": "split_mode", "robot": "h12", "sim": true, "use_estop": false} + log: {"event": "input_engaged", "source": "split_mode_lower", "topic": "/safety/lowcmd_lower_in"} + log: {"age": 2.002, "event": "input_quiet", "seconds": 2.0, "source": "split_mode_upper", "topic": "/safety/lowcmd_upper_in"} + log: {"age": 0.102, "event": "state_quiet", "seconds": 0.1, "topic": "/lowstate"} + log: {"age": 0.2, "event": "input_quiet", "seconds": 0.2, "source": "split_mode_lower", "topic": "/safety/lowcmd_lower_in"} + +=== lower_silent + old: frames in [0,1.5]: 706 (471/s); first frame t=-2.05; headers (mm,pr) [(0, 0), (6, 0)]; reserve [(0, 0, 0, 0)] + t=0.4 [mode/q/dq/tau/kp/kd] 0:1,0.05,0,0,50,2; 13:1,0.2,0,0,50,2 + t=1.0 [mode/q/dq/tau/kp/kd] 0:1,0.05,0,0,50,2; 13:1,0.2,0,0,50,2 + t=1.4 [mode/q/dq/tau/kp/kd] 0:1,0.05,0,0,50,2; 13:1,0.2,0,0,50,2 + log: {"action_ms": 1794885494618, "decision_ms": 1794885494616, "decision_to_action_ms": 2, "event": "estop_command_written", "reason": "Upper-body command stale for 2.00s (> 2.0s)", "trigger_stamp_ms": null} + new: frames in [0,1.5]: 750 (500/s); first frame t=-2.49; headers (mm,pr) [(6, 0)]; reserve [(0, 0, 0, 0)] + t=0.4 [mode/q/dq/tau/kp/kd] 0:1,0.05,0,0,50,2; 13:1,0.2,0,0,50,2 + t=1.0 [mode/q/dq/tau/kp/kd] 0:1,0.05,0,0,50,2; 13:1,0.2,0,0,50,2 + t=1.4 [mode/q/dq/tau/kp/kd] 0:1,0.05,0,0,50,2; 13:1,0.2,0,0,50,2 + log: {"config": "/home/code/core_ws/install/golem_safety/share/golem_safety/config/h12/default_safety_split.yaml", "event": "relay_started", "mode": "split_mode", "robot": "h12", "sim": true, "use_estop": false} + log: {"event": "input_engaged", "source": "split_mode_lower", "topic": "/safety/lowcmd_lower_in"} + log: {"age": 0.202, "event": "input_quiet", "seconds": 0.2, "source": "split_mode_lower", "topic": "/safety/lowcmd_lower_in"} + log: {"age": 0.102, "event": "state_quiet", "seconds": 0.1, "topic": "/lowstate"} + +=== estop_q_edge + old: frames in [0,2.0]: 957 (478/s); first frame t=-2.05; headers (mm,pr) [(0, 0), (6, 0)]; reserve [(0, 0, 0, 0)] + estop at t=0.504: 723 frames from then on, last t=1.998 + t=0.4 [mode/q/dq/tau/kp/kd] 0:1,0.1,0,0,50,2; 13:1,0.1,0,0,50,2 + t=0.9 [mode/q/dq/tau/kp/kd] 0:0,0,0,0,0,0; 13:0,0,0,0,0,0 + t=1.4 [mode/q/dq/tau/kp/kd] 0:0,0,0,0,0,0; 13:0,0,0,0,0,0 + log: 0. 0. 0. 0. 0. 0. 0. 0. 0. ]{"action_ms": 1794885500130, "decision_ms": 1794885500128, "decision_to_action_ms": 2, "event": "estop_command_written", "reason": "motor 3 (left_knee_joint) q out of estop range: 3.20000004768 + new: frames in [0,2.0]: 300 (150/s); first frame t=-2.49; headers (mm,pr) [(6, 0)]; reserve [(0, 0, 0, 0)] + estop at t=0.502: 50 frames from then on, last t=0.600 + t=0.4 [mode/q/dq/tau/kp/kd] 0:1,0.1,0,0,50,2; 13:1,0.1,0,0,50,2 + t=0.9 [mode/q/dq/tau/kp/kd] 0:0,0,0,0,0,0; 13:0,0,0,0,0,0 + t=1.4 [mode/q/dq/tau/kp/kd] 0:0,0,0,0,0,0; 13:0,0,0,0,0,0 + log: {"config": "/home/code/core_ws/install/golem_safety/share/golem_safety/config/h12/default_safety_split.yaml", "event": "relay_started", "mode": "split_mode", "robot": "h12", "sim": true, "use_estop": false} + log: {"event": "input_engaged", "source": "split_mode_lower", "topic": "/safety/lowcmd_lower_in"} + log: estop diagnostic, body slots in order: + log: {"action_ms": 3372281022, "decision_ms": 3372281022, "decision_to_action_ms": 0, "event": "estop_command_written", "reason": "motor 3 (left_knee_joint) q out of estop range: 3.2 (limit 3.200, 1.00x)", "trigger_stamp_ms": null} + +=== estop_q_below + old: frames in [0,1.5]: 696 (464/s); first frame t=-2.05; headers (mm,pr) [(0, 0), (6, 0)]; reserve [(0, 0, 0, 0)] + t=0.4 [mode/q/dq/tau/kp/kd] 0:1,0.1,0,0,50,2; 13:1,0.1,0,0,50,2 + t=1.4 [mode/q/dq/tau/kp/kd] 0:1,0.1,0,0,50,2; 13:1,0.1,0,0,50,2 + log: {"action_ms": 1794885512125, "decision_ms": 1794885512124, "decision_to_action_ms": 1, "event": "estop_command_written", "reason": "Upper-body command stale for 2.00s (> 2.0s)", "trigger_stamp_ms": null} + new: frames in [0,1.5]: 750 (500/s); first frame t=-2.49; headers (mm,pr) [(6, 0)]; reserve [(0, 0, 0, 0)] + t=0.4 [mode/q/dq/tau/kp/kd] 0:1,0.1,0,0,50,2; 13:1,0.1,0,0,50,2 + t=1.4 [mode/q/dq/tau/kp/kd] 0:1,0.1,0,0,50,2; 13:1,0.1,0,0,50,2 + log: {"config": "/home/code/core_ws/install/golem_safety/share/golem_safety/config/h12/default_safety_split.yaml", "event": "relay_started", "mode": "split_mode", "robot": "h12", "sim": true, "use_estop": false} + log: {"event": "input_engaged", "source": "split_mode_lower", "topic": "/safety/lowcmd_lower_in"} + log: {"age": 0.1, "event": "state_quiet", "seconds": 0.1, "topic": "/lowstate"} + log: {"age": 0.2, "event": "input_quiet", "seconds": 0.2, "source": "split_mode_lower", "topic": "/safety/lowcmd_lower_in"} + +=== estop_dq + old: frames in [0,1.5]: 714 (476/s); first frame t=-2.05; headers (mm,pr) [(0, 0), (6, 0)]; reserve [(0, 0, 0, 0)] + estop at t=0.504: 482 frames from then on, last t=1.499 + t=0.4 [mode/q/dq/tau/kp/kd] 0:1,0.1,0,0,50,2; 13:1,0.1,0,0,50,2 + t=0.9 [mode/q/dq/tau/kp/kd] 0:0,0,0,0,0,0; 13:0,0,0,0,0,0 + log: 0. 0. 0.]{"action_ms": 1794885517643, "decision_ms": 1794885517641, "decision_to_action_ms": 2, "event": "estop_command_written", "reason": "motor 16 (left_elbow_joint) dq out of estop range: 50.0", "trigger_stamp_ms": null} + new: frames in [0,1.5]: 300 (200/s); first frame t=-2.49; headers (mm,pr) [(6, 0)]; reserve [(0, 0, 0, 0)] + estop at t=0.502: 50 frames from then on, last t=0.600 + t=0.4 [mode/q/dq/tau/kp/kd] 0:1,0.1,0,0,50,2; 13:1,0.1,0,0,50,2 + t=0.9 [mode/q/dq/tau/kp/kd] 0:0,0,0,0,0,0; 13:0,0,0,0,0,0 + log: {"config": "/home/code/core_ws/install/golem_safety/share/golem_safety/config/h12/default_safety_split.yaml", "event": "relay_started", "mode": "split_mode", "robot": "h12", "sim": true, "use_estop": false} + log: {"event": "input_engaged", "source": "split_mode_lower", "topic": "/safety/lowcmd_lower_in"} + log: estop diagnostic, body slots in order: + log: {"action_ms": 3372298536, "decision_ms": 3372298536, "decision_to_action_ms": 0, "event": "estop_command_written", "reason": "motor 16 (left_elbow_joint) dq out of estop range: 50 (limit 6.000, 8.33x)", "trigger_stamp_ms": null} + +=== state_nan + old: frames in [0,1.5]: 716 (477/s); first frame t=-2.05; headers (mm,pr) [(0, 0), (6, 0)]; reserve [(0, 0, 0, 0)] + estop at t=0.505: 482 frames from then on, last t=1.499 + t=0.4 [mode/q/dq/tau/kp/kd] 0:1,0.1,0,0,50,2; 13:1,0.1,0,0,50,2 + t=0.9 [mode/q/dq/tau/kp/kd] 0:0,0,0,0,0,0; 13:0,0,0,0,0,0 + log: {"action_ms": 1794885526143, "decision_ms": 1794885526143, "decision_to_action_ms": 0, "event": "estop_command_written", "reason": "low_state contains non-finite values", "trigger_stamp_ms": null}dq_cmd: [0. 0. 0. 0. 0. 0. 0. 0. 0 + new: frames in [0,1.5]: 301 (201/s); first frame t=-2.49; headers (mm,pr) [(6, 0)]; reserve [(0, 0, 0, 0)] + estop at t=0.502: 50 frames from then on, last t=0.600 + t=0.4 [mode/q/dq/tau/kp/kd] 0:1,0.1,0,0,50,2; 13:1,0.1,0,0,50,2 + t=0.9 [mode/q/dq/tau/kp/kd] 0:0,0,0,0,0,0; 13:0,0,0,0,0,0 + log: {"config": "/home/code/core_ws/install/golem_safety/share/golem_safety/config/h12/default_safety_split.yaml", "event": "relay_started", "mode": "split_mode", "robot": "h12", "sim": true, "use_estop": false} + log: {"event": "input_engaged", "source": "split_mode_lower", "topic": "/safety/lowcmd_lower_in"} + log: estop diagnostic, body slots in order: + log: {"action_ms": 3372307037, "decision_ms": 3372307037, "decision_to_action_ms": 0, "event": "estop_command_written", "reason": "motor 5 (left_ankle_roll_joint) tau is nan in /lowstate", "trigger_stamp_ms": null} + +=== engage_mm0 + old: frames in [0,2.0]: 923 (462/s); first frame t=-2.05; headers (mm,pr) [(0, 0), (6, 0)]; reserve [(0, 0, 0, 0)] + t=0.5 [mode/q/dq/tau/kp/kd] 0:1,0.1,0,0,50,2; 13:1,0.1,0,0,50,2 + t=1.5 [mode/q/dq/tau/kp/kd] 0:1,0.1,0,0,50,2; 13:1,0.1,0,0,50,2 + log: {"action_ms": 1794885538192, "decision_ms": 1794885538190, "decision_to_action_ms": 2, "event": "estop_command_written", "reason": "Upper-body command stale for 2.00s (> 2.0s)", "trigger_stamp_ms": null} + new: frames in [0,2.0]: 1000 (500/s); first frame t=-2.49; headers (mm,pr) [(6, 0)]; reserve [(0, 0, 0, 0)] + t=0.5 [mode/q/dq/tau/kp/kd] 0:0,0,0,0,0,0; 13:0,0,0,0,0,0 + t=1.5 [mode/q/dq/tau/kp/kd] 0:1,0.1,0,0,50,2; 13:1,0.1,0,0,50,2 + log: {"config": "/home/code/core_ws/install/golem_safety/share/golem_safety/config/h12/default_safety_split.yaml", "event": "relay_started", "mode": "split_mode", "robot": "h12", "sim": true, "use_estop": false} + log: {"event": "engage_refused", "mode": "split_mode", "reason": "/lowstate mode_machine is 0, expected 6"} + log: {"event": "input_engaged", "source": "split_mode_lower", "topic": "/safety/lowcmd_lower_in"} + log: {"age": 0.102, "event": "state_quiet", "seconds": 0.1, "topic": "/lowstate"} + log: {"age": 0.202, "event": "input_quiet", "seconds": 0.2, "source": "split_mode_lower", "topic": "/safety/lowcmd_lower_in"} + +=== foreign_mm + old: frames in [0,2.0]: 931 (466/s); first frame t=-2.05; headers (mm,pr) [(0, 0), (6, 0)]; reserve [(0, 0, 0, 0)] + t=0.5 [mode/q/dq/tau/kp/kd] 0:1,0.1,0,0,50,2; 13:1,0.1,0,0,50,2 + t=1.5 [mode/q/dq/tau/kp/kd] 0:1,0.1,0,0,50,2; 13:1,0.1,0,0,50,2 + log: {"action_ms": 1794885547197, "decision_ms": 1794885547195, "decision_to_action_ms": 2, "event": "estop_command_written", "reason": "Upper-body command stale for 2.00s (> 2.0s)", "trigger_stamp_ms": null} + new: frames in [0,2.0]: 551 (276/s); first frame t=-2.49; headers (mm,pr) [(6, 0)]; reserve [(0, 0, 0, 0)] + estop at t=1.002: 50 frames from then on, last t=1.100 + t=0.5 [mode/q/dq/tau/kp/kd] 0:1,0.1,0,0,50,2; 13:1,0.1,0,0,50,2 + t=1.5 [mode/q/dq/tau/kp/kd] 0:0,0,0,0,0,0; 13:0,0,0,0,0,0 + log: {"config": "/home/code/core_ws/install/golem_safety/share/golem_safety/config/h12/default_safety_split.yaml", "event": "relay_started", "mode": "split_mode", "robot": "h12", "sim": true, "use_estop": false} + log: {"event": "input_engaged", "source": "split_mode_lower", "topic": "/safety/lowcmd_lower_in"} + log: {"action_ms": 3372325097, "decision_ms": 3372325097, "decision_to_action_ms": 0, "event": "estop_command_written", "reason": "/lowstate mode_machine 5 is not the h12's 6: another robot or sim is on this domain", "trigger_stamp_ms" + +=== second_writer + old: frames in [0,1.5]: 685 (457/s); first frame t=-2.05; headers (mm,pr) [(0, 0), (6, 0)]; reserve [(0, 0, 0, 0)] + t=0.5 [mode/q/dq/tau/kp/kd] 0:1,0.1,0,0,50,2; 13:1,0.1,0,0,50,2 + t=1.2 [mode/q/dq/tau/kp/kd] 0:1,0.3,0,0,50,2; 13:1,0.1,0,0,50,2 + log: {"action_ms": 1794885555699, "decision_ms": 1794885555697, "decision_to_action_ms": 2, "event": "estop_command_written", "reason": "Upper-body command stale for 2.00s (> 2.0s)", "trigger_stamp_ms": null} + new: frames in [0,1.5]: 450 (300/s); first frame t=-2.49; headers (mm,pr) [(6, 0)]; reserve [(0, 0, 0, 0)] + estop at t=0.802: 50 frames from then on, last t=0.900 + t=0.5 [mode/q/dq/tau/kp/kd] 0:1,0.1,0,0,50,2; 13:1,0.1,0,0,50,2 + t=1.2 [mode/q/dq/tau/kp/kd] 0:0,0,0,0,0,0; 13:0,0,0,0,0,0 + log: {"config": "/home/code/core_ws/install/golem_safety/share/golem_safety/config/h12/default_safety_split.yaml", "event": "relay_started", "mode": "split_mode", "robot": "h12", "sim": true, "use_estop": false} + log: {"event": "input_engaged", "source": "split_mode_lower", "topic": "/safety/lowcmd_lower_in"} + log: {"action_ms": 3372333900, "decision_ms": 3372333900, "decision_to_action_ms": 0, "event": "estop_command_written", "reason": "second writer on /safety/lowcmd_lower_in: a new writer sent a command 0.010 s after the current writer's + +=== hw_estop_silent + old: frames in [0,2.0]: 932 (466/s); first frame t=-2.05; headers (mm,pr) [(0, 0), (6, 0)]; reserve [(0, 0, 0, 0)] + t=0.2 [mode/q/dq/tau/kp/kd] 0:1,0.1,0,0,50,2; 13:1,0.1,0,0,50,2 + t=0.8 [mode/q/dq/tau/kp/kd] 0:1,0.1,0,0,50,2; 13:1,0.1,0,0,50,2 + t=1.8 [mode/q/dq/tau/kp/kd] 0:1,0.1,0,0,50,2; 13:1,0.1,0,0,50,2 + log: {"action_ms": 1794885564702, "decision_ms": 1794885564700, "decision_to_action_ms": 2, "event": "estop_command_written", "reason": "Upper-body command stale for 2.00s (> 2.0s)", "trigger_stamp_ms": null} + new: frames in [0,2.0]: 792 (396/s); first frame t=-2.49; headers (mm,pr) [(6, 0)]; reserve [(0, 0, 0, 0)] + estop at t=1.485: 50 frames from then on, last t=1.583 + t=0.2 [mode/q/dq/tau/kp/kd] 0:0,0,0,0,0,0; 13:0,0,0,0,0,0 + t=0.8 [mode/q/dq/tau/kp/kd] 0:1,0.1,0,0,50,2; 13:1,0.1,0,0,50,2 + t=1.8 [mode/q/dq/tau/kp/kd] 0:0,0,0,0,0,0; 13:0,0,0,0,0,0 + log: {"config": "/home/code/core_ws/install/golem_safety/share/golem_safety/config/h12/default_safety_split.yaml", "event": "relay_started", "mode": "split_mode", "robot": "h12", "sim": true, "use_estop": true} + log: {"event": "engage_refused", "mode": "split_mode", "reason": "no hardware estop sample received yet on /estop/triggered or /estop/plugged_in"} + log: {"event": "input_engaged", "source": "split_mode_lower", "topic": "/safety/lowcmd_lower_in"} + log: {"action_ms": 3372343084, "decision_ms": 3372343084, "decision_to_action_ms": 0, "event": "estop_command_written", "reason": "External estop silent: no sample on /estop/triggered or /estop/plugged_in for 0.502 s (timeout 0.5 s)", + +=== hw_estop_trigger + old: frames in [0,2.0]: 946 (473/s); first frame t=-2.05; headers (mm,pr) [(0, 0), (6, 0)]; reserve [(0, 0, 0, 0)] + estop at t=1.006: 480 frames from then on, last t=1.999 + t=0.2 [mode/q/dq/tau/kp/kd] 0:1,0.1,0,0,50,2; 13:1,0.1,0,0,50,2 + t=0.8 [mode/q/dq/tau/kp/kd] 0:1,0.1,0,0,50,2; 13:1,0.1,0,0,50,2 + t=1.8 [mode/q/dq/tau/kp/kd] 0:0,0,0,0,0,0; 13:0,0,0,0,0,0 + log: {"action_ms": 1794885570718, "decision_ms": 1794885570716, "decision_to_action_ms": 2, "event": "estop_command_written", "reason": "External estop triggered", "trigger_stamp_ms": 1794885570713, "trigger_to_action_ms": 5} + new: frames in [0,2.0]: 551 (276/s); first frame t=-2.49; headers (mm,pr) [(6, 0)]; reserve [(0, 0, 0, 0)] + estop at t=1.003: 50 frames from then on, last t=1.101 + t=0.2 [mode/q/dq/tau/kp/kd] 0:0,0,0,0,0,0; 13:0,0,0,0,0,0 + t=0.8 [mode/q/dq/tau/kp/kd] 0:1,0.1,0,0,50,2; 13:1,0.1,0,0,50,2 + t=1.8 [mode/q/dq/tau/kp/kd] 0:0,0,0,0,0,0; 13:0,0,0,0,0,0 + log: {"config": "/home/code/core_ws/install/golem_safety/share/golem_safety/config/h12/default_safety_split.yaml", "event": "relay_started", "mode": "split_mode", "robot": "h12", "sim": true, "use_estop": true} + log: {"event": "engage_refused", "mode": "split_mode", "reason": "no hardware estop sample received yet on /estop/triggered or /estop/plugged_in"} + log: {"event": "input_engaged", "source": "split_mode_lower", "topic": "/safety/lowcmd_lower_in"} + log: {"action_ms": 3372351608, "decision_ms": 3372351608, "decision_to_action_ms": 0, "event": "estop_command_written", "reason": "External estop triggered", "trigger_stamp_ms": 3372351608, "trigger_to_action_ms": 0} + +=== bitwise: clipped body slots, old vs new + relay_clip_split (0.1, 0.45): identical=True + relay_clip_split (0.6, 0.95): identical=True + relay_clip_full (0.1, 0.45): identical=True + relay_clip_full (0.6, 0.95): identical=True diff --git a/docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/results/settings.txt b/docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/results/settings.txt new file mode 100644 index 00000000..7ebcd781 --- /dev/null +++ b/docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/results/settings.txt @@ -0,0 +1,8 @@ +default_safety_full {"old": {"mode": "full_body_mode", "hz": 500.0, "estop": {"enabled": true, "estop_topic": "h12/estop_status_raw", "poll_hz": 500.0, "upper_stale_seconds": 2.0}, "domain": 0, "log": false, "topics": {"low_cmd_in": "rt/safety/lowcmd_in", "low_cmd_lower_in": "rt/safety/lowcmd_lower_in", "low_cmd_upper_in": "rt/safety/lowcmd_upper_in", "low_cmd_out": "rt/lowcmd", "low_state": "rt/lowstate"}}, "new": {"mode": "full_body_mode", "control": {"publish_hz": 500, "state_lag_ms": 100}, "stale": {"upper_seconds": 0, "lower_seconds": 0.2, "state_seconds": 0.1}, "single_writer": {"handover_seconds": 0.5}, "estop": {"triggered_topic": "/estop/triggered", "plugged_in_topic": "/estop/plugged_in", "timeout_seconds": 0.5}, "estop_response": {"mode": "zero", "kd": [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], "after": "burst_then_silence", "burst_count": 50}, "topics": {"low_cmd_in": "/safety/lowcmd_in", "low_cmd_lower_in": "/safety/lowcmd_lower_in", "low_cmd_upper_in": "/safety/lowcmd_upper_in", "low_cmd_out": "/lowcmd", "low_state": "/lowstate"}, "log": false}} +default_safety_split {"old": {"mode": "split_mode", "hz": 500.0, "estop": {"enabled": true, "estop_topic": "h12/estop_status_raw", "poll_hz": 500.0, "upper_stale_seconds": 2.0}, "domain": 0, "log": false, "topics": {"low_cmd_in": "rt/safety/lowcmd_in", "low_cmd_lower_in": "rt/safety/lowcmd_lower_in", "low_cmd_upper_in": "rt/safety/lowcmd_upper_in", "low_cmd_out": "rt/lowcmd", "low_state": "rt/lowstate"}}, "new": {"mode": "split_mode", "control": {"publish_hz": 500, "state_lag_ms": 100}, "stale": {"upper_seconds": 2, "lower_seconds": 0.2, "state_seconds": 0.1}, "single_writer": {"handover_seconds": 0.5}, "estop": {"triggered_topic": "/estop/triggered", "plugged_in_topic": "/estop/plugged_in", "timeout_seconds": 0.5}, "estop_response": {"mode": "zero", "kd": [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], "after": "burst_then_silence", "burst_count": 50}, "topics": {"low_cmd_in": "/safety/lowcmd_in", "low_cmd_lower_in": "/safety/lowcmd_lower_in", "low_cmd_upper_in": "/safety/lowcmd_upper_in", "low_cmd_out": "/lowcmd", "low_state": "/lowstate"}, "log": false}} +locomotion_safety_full {"old": {"mode": "full_body_mode", "hz": 500.0, "estop": {"enabled": true, "estop_topic": "h12/estop_status_raw", "poll_hz": 500.0, "upper_stale_seconds": 2.0}, "domain": 0, "log": false, "topics": {"low_cmd_in": "rt/safety/lowcmd_in", "low_cmd_lower_in": "rt/safety/lowcmd_lower_in", "low_cmd_upper_in": "rt/safety/lowcmd_upper_in", "low_cmd_out": "rt/lowcmd", "low_state": "rt/lowstate"}}, "new": {"mode": "full_body_mode", "control": {"publish_hz": 500, "state_lag_ms": 100}, "stale": {"upper_seconds": 0, "lower_seconds": 0.2, "state_seconds": 0.1}, "single_writer": {"handover_seconds": 0.5}, "estop": {"triggered_topic": "/estop/triggered", "plugged_in_topic": "/estop/plugged_in", "timeout_seconds": 0.5}, "estop_response": {"mode": "zero", "kd": [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], "after": "burst_then_silence", "burst_count": 50}, "topics": {"low_cmd_in": "/safety/lowcmd_in", "low_cmd_lower_in": "/safety/lowcmd_lower_in", "low_cmd_upper_in": "/safety/lowcmd_upper_in", "low_cmd_out": "/lowcmd", "low_state": "/lowstate"}, "log": false}} +relax_safety_full {"old": {"mode": "full_body_mode", "hz": 500.0, "estop": {"enabled": true, "estop_topic": "h12/estop_status_raw", "poll_hz": 500.0, "upper_stale_seconds": 2.0}, "domain": 0, "log": false, "topics": {"low_cmd_in": "rt/safety/lowcmd_in", "low_cmd_lower_in": "rt/safety/lowcmd_lower_in", "low_cmd_upper_in": "rt/safety/lowcmd_upper_in", "low_cmd_out": "rt/lowcmd", "low_state": "rt/lowstate"}}, "new": {"mode": "full_body_mode", "control": {"publish_hz": 500, "state_lag_ms": 100}, "stale": {"upper_seconds": 0, "lower_seconds": 0.2, "state_seconds": 0.1}, "single_writer": {"handover_seconds": 0.5}, "estop": {"triggered_topic": "/estop/triggered", "plugged_in_topic": "/estop/plugged_in", "timeout_seconds": 0.5}, "estop_response": {"mode": "zero", "kd": [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], "after": "burst_then_silence", "burst_count": 50}, "topics": {"low_cmd_in": "/safety/lowcmd_in", "low_cmd_lower_in": "/safety/lowcmd_lower_in", "low_cmd_upper_in": "/safety/lowcmd_upper_in", "low_cmd_out": "/lowcmd", "low_state": "/lowstate"}, "log": false}} +relax_safety_split {"old": {"mode": "split_mode", "hz": 500.0, "estop": {"enabled": true, "estop_topic": "h12/estop_status_raw", "poll_hz": 500.0, "upper_stale_seconds": 0.0}, "domain": 0, "log": false, "topics": {"low_cmd_in": "rt/safety/lowcmd_in", "low_cmd_lower_in": "rt/safety/lowcmd_lower_in", "low_cmd_upper_in": "rt/safety/lowcmd_upper_in", "low_cmd_out": "rt/lowcmd", "low_state": "rt/lowstate"}}, "new": {"mode": "split_mode", "control": {"publish_hz": 500, "state_lag_ms": 100}, "stale": {"upper_seconds": 0, "lower_seconds": 0.2, "state_seconds": 0.1}, "single_writer": {"handover_seconds": 0.5}, "estop": {"triggered_topic": "/estop/triggered", "plugged_in_topic": "/estop/plugged_in", "timeout_seconds": 0.5}, "estop_response": {"mode": "zero", "kd": [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], "after": "burst_then_silence", "burst_count": 50}, "topics": {"low_cmd_in": "/safety/lowcmd_in", "low_cmd_lower_in": "/safety/lowcmd_lower_in", "low_cmd_upper_in": "/safety/lowcmd_upper_in", "low_cmd_out": "/lowcmd", "low_state": "/lowstate"}, "log": false}} +sim_safety_split {"old": {"mode": "split_mode", "hz": 500.0, "estop": {"enabled": false, "estop_topic": "h12/estop_status_raw", "poll_hz": 500.0, "upper_stale_seconds": 0.0}, "domain": 1, "log": false, "topics": {"low_cmd_in": "rt/safety/lowcmd_in", "low_cmd_lower_in": "rt/safety/lowcmd_lower_in", "low_cmd_upper_in": "rt/safety/lowcmd_upper_in", "low_cmd_out": "rt/lowcmd", "low_state": "rt/lowstate"}}, "new": {"mode": null, "control": null, "stale": null, "single_writer": null, "estop": null, "estop_response": null, "topics": null, "log": null}} +tight_safety_full {"old": {"mode": "full_body_mode", "hz": 500.0, "estop": {"enabled": true, "estop_topic": "h12/estop_status_raw", "poll_hz": 500.0, "upper_stale_seconds": 2.0}, "domain": 0, "log": false, "topics": {"low_cmd_in": "rt/safety/lowcmd_in", "low_cmd_lower_in": "rt/safety/lowcmd_lower_in", "low_cmd_upper_in": "rt/safety/lowcmd_upper_in", "low_cmd_out": "rt/lowcmd", "low_state": "rt/lowstate"}}, "new": {"mode": "full_body_mode", "control": {"publish_hz": 500, "state_lag_ms": 100}, "stale": {"upper_seconds": 0, "lower_seconds": 0.2, "state_seconds": 0.1}, "single_writer": {"handover_seconds": 0.5}, "estop": {"triggered_topic": "/estop/triggered", "plugged_in_topic": "/estop/plugged_in", "timeout_seconds": 0.5}, "estop_response": {"mode": "zero", "kd": [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], "after": "burst_then_silence", "burst_count": 50}, "topics": {"low_cmd_in": "/safety/lowcmd_in", "low_cmd_lower_in": "/safety/lowcmd_lower_in", "low_cmd_upper_in": "/safety/lowcmd_upper_in", "low_cmd_out": "/lowcmd", "low_state": "/lowstate"}, "log": false}} +tight_safety_split {"old": {"mode": "split_mode", "hz": 500.0, "estop": {"enabled": true, "estop_topic": "h12/estop_status_raw", "poll_hz": 500.0, "upper_stale_seconds": 2.0}, "domain": 0, "log": false, "topics": {"low_cmd_in": "rt/safety/lowcmd_in", "low_cmd_lower_in": "rt/safety/lowcmd_lower_in", "low_cmd_upper_in": "rt/safety/lowcmd_upper_in", "low_cmd_out": "rt/lowcmd", "low_state": "rt/lowstate"}}, "new": {"mode": "split_mode", "control": {"publish_hz": 500, "state_lag_ms": 100}, "stale": {"upper_seconds": 2, "lower_seconds": 0.2, "state_seconds": 0.1}, "single_writer": {"handover_seconds": 0.5}, "estop": {"triggered_topic": "/estop/triggered", "plugged_in_topic": "/estop/plugged_in", "timeout_seconds": 0.5}, "estop_response": {"mode": "zero", "kd": [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0], "after": "burst_then_silence", "burst_count": 50}, "topics": {"low_cmd_in": "/safety/lowcmd_in", "low_cmd_lower_in": "/safety/lowcmd_lower_in", "low_cmd_upper_in": "/safety/lowcmd_upper_in", "low_cmd_out": "/lowcmd", "low_state": "/lowstate"}, "log": false}} From dd31f4dba42436f725af978e8507c7146be3fe17 Mon Sep 17 00:00:00 2001 From: maxconway Date: Mon, 16 Nov 2026 20:23:32 -0700 Subject: [PATCH 2/3] docs: safety comparison: header source measured, counts and evidence labels corrected Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_015EG7p8sgY1ZVi6unbbLp4J --- .../README.md | 21 ++++++++++--------- .../harness/analyze.py | 2 +- .../harness/driver.py | 5 ++++- .../results/report.txt | 11 ++++++++++ 4 files changed, 27 insertions(+), 12 deletions(-) diff --git a/docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/README.md b/docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/README.md index d3603b63..0ca329ff 100644 --- a/docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/README.md +++ b/docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/README.md @@ -20,9 +20,8 @@ layers against each other directly and says, for each row, how it was checked: The derived limits, and the clipping of a valid command on the body slots, are identical: the clipped values are bit-for-bit the same in both modes, and the estop limits trip at the -same values. Seven of the eight upstream configs give identical limits; `relax_safety_split` -differs in one value (ankle pitch torque estop), and upstream's `sim_safety_split` has no -counterpart. The behaviour around the clipping differs: when inputs engage, what a +same values. Six of the eight upstream configs give identical limits; `relax_safety_split` differs in +one value (ankle pitch torque estop), and upstream's `sim_safety_split` has no counterpart. The behaviour around the clipping differs: when inputs engage, what a command's header and spare slots carry, what happens after an estop, and five triggers (two that only `golem_safety` latches on, one, the upper watchdog, that only `h12_safety_layer` does, and two hardware-estop rules). @@ -73,7 +72,7 @@ Reproduce: copy `harness/` and a checkout of `h12_safety_layer` at `ad06903` (as | Estop limits are strict: a measured q, \|dq\| or \|tau_est\| past its limit latches, one at it does not; comparison in double of the float32 wire value (so a knee at float32(3.2) = 3.2000000477 latches, one float32 step below does not) | measured (`estop_q_edge`, `estop_q_below`, `estop_dq`) | | A non-finite `/lowstate` q, dq or tau_est latches | measured (`state_nan`) | | `/lowstate` checks run on every body slot in both modes: any non-finite value first, then q, then dq, then tau, each reporting the lowest bad slot | inspection (`h12_safety_layer/core/safety_checks.py:72-96`, `golem_safety/src/safety_core.cpp` `limit_violation`); the measured reasons name the same joint | -| A quiet lower (or full) input keeps its last command relayed | measured (`lower_silent`) | +| A quiet lower input keeps its last command relayed (and, by inspection, a quiet full-body input) | measured (`lower_silent`); inspection for full body | | A hardware estop sample that says triggered latches, within one cycle | measured (`hw_estop_trigger`: old t = 1.006 s, new 1.003 s) | | The estop is one way until restart | measured (every latch above) | @@ -90,13 +89,13 @@ Reproduce: copy `harness/` and a checkout of `h12_safety_layer` at `ad06903` (as | Engaging before `/lowstate` reports the robot | relays from the first command, whatever `/lowstate` says, or before any | each input engages on its first command once `/lowstate` reports `mode_machine` 6 | measured (`engage_mm0`: with `mode_machine` 0 the old layer relays, the new writes zero gains until it turns 6) | | `/lowstate` from another robot (`mode_machine` 5) once engaged | ignored | latches | measured (`foreign_mm`) | | A second publisher on one input | both accepted, the newest wins each cycle | latches unless the current writer has been quiet for `single_writer.handover_seconds` (0.5 s) | measured (`second_writer`) | -| Upper input quiet in split mode | past `estop.upper_stale_seconds` (default 2.0 s; set to 0.0, off, only in `relax_safety_split` and `sim_safety_split`) the whole robot estops | its last command keeps being relayed and `input_quiet` is logged (`stale.upper_seconds`: 2 s in `default`/`tight_safety_split`; 0, no report, in `relax_safety_split`); never latches | measured (`upper_silent`: old latches at 2.495 s, new still relays the arms at 3.4 s). With `relax_safety_split`, the bringups' config, neither latches | -| Command header (`mode_machine`, `mode_pr`) | copied from the last command received on any input, `LowCmdDefault` (0, 0) before any (`safety_layer.py:202,240`) | always the profile's (6, 0) | measured (`startup_idle`: old (0, 0); `relay_clip_*`: an input's (7, 1) relayed by the old layer) | -| Slots 27-34 and `reserve` | split mode: copied from the upper input's command; full body: from the full input's | always zero | measured (`relay_clip_*`: an input's 9s relayed by the old layer) | +| Upper input quiet in split mode | past `estop.upper_stale_seconds` (default 2.0 s; set to 0.0, off, only in `relax_safety_split` and `sim_safety_split`) the whole robot estops | its last command keeps being relayed and `input_quiet` is logged (`stale.upper_seconds`: 2 s in `default`/`tight_safety_split`; 0, no report, in `relax_safety_split`); never latches | measured (`upper_silent`: old latches at 2.495 s, new still relays the arms at 3.4 s). With `relax_safety_split`, the bringups' config, neither latches (loader: upper stale 0.0, off, in the old config; 0, no report, in the new) | +| Command header (`mode_machine`, `mode_pr`) | copied from the last command received on either input, `LowCmdDefault` (0, 0) before any (`safety_layer.py:202,240`) | always the profile's (6, 0) | measured (`startup_idle`: old (0, 0); `split_header_source`: lower (8, 2) and upper (7, 1) both appear, alternating) | +| Slots 27-34 and `reserve` | split mode: copied from the upper input's command; full body: from the full input's | always zero | measured (`split_header_source`: only the upper input's 9s, never the lower's 5s; `relay_clip_full`) | | Output before any command | `make_estop_cmd(LowCmdDefault())` at 500 Hz: every motor off, header (0, 0) | `zero_command` at 500 Hz: every motor off, header (6, 0) | measured (`startup_idle`) | | After an estop | the estop command (body slots mode 0, all values 0) every cycle until the process stops; its header and slots 27-34 are the last command's (`safety_layer.py:262`, `make_estop_cmd`) | 50 zero commands, then silence until restart (`camera_ready` `58d0f99`) | measured (every latch: old about 480 frames/s after it, new exactly 50); the header and spare slots by inspection | | Estop reasons | `motor 16 (left_elbow_joint) dq out of estop range: 50.0`; `Command validation failed on split_mode_lower: q contains non-finite values` | adds the limit and its multiple and names the motor: `... 50 (limit 6.000, 8.33x)`, `... motor 20 (right_shoulder_pitch_joint) q is nan` (`camera_ready` `5182fbe`) | measured | -| Late `/lowstate` | no check | on domain 0, a `/lowstate` later than `control.state_lag_ms` (100 ms) past its tick latches (`camera_ready` `e0396fc`); off off domain 0, so not measured here | inspection | +| Late `/lowstate` | no check | on domain 0, a `/lowstate` later than `control.state_lag_ms` (100 ms) past its tick latches (`camera_ready` `e0396fc`); not checked off domain 0, so not measured here | inspection | | Quiet `/lowstate` and inputs | not reported | `state_quiet`/`input_quiet`/`input_resumed` logged; relay unchanged | measured (logs) | | Relay structure | input callbacks clip into per-input slots; a publisher thread sends at 500 Hz of wall time | newest-only keep-last-1 inputs, one relay loop on robot time (the sim's `/clock` under `use_sim_time`) (`camera_ready` `e0396fc`) | inspection; rates measured above | | Heartbeat `/safety/heartbeat` | true at 10 Hz from start-up until the latch, then false (`ros2/safety_node.py:63`) | none until relaying can begin (`/lowstate` reports 6 and, with `use_estop`, a first estop sample), then true at 10 Hz on the steady clock, false once latched (`camera_ready` `5b08888` for the steady clock) | inspection; `golem_safety`'s `test_node_ros.py` | @@ -109,8 +108,10 @@ Reproduce: copy `harness/` and a checkout of `h12_safety_layer` at `ad06903` (as - `h12_safety_layer`'s raw estop reader opens its DDS participant on the default domain (0), not `ROS_DOMAIN_ID` (`core/estop_subscriber.py:53`), so a sim run of it on another domain - still reads, and can be latched by, an estop box publishing on the real robots' domain. - The harness pins it to its own domain. + still reads, and can be latched by, an estop box publishing on the real robots' domain + (CycloneDDS's Python `DomainParticipant.__init__` defaults to `domain_id: int = 0`, checked in + the `golem_ros` container; GOLEM's `cyclonedds.xml` sets `Domain Id="any"`). The harness pins it + to its own domain. - A workspace built before GOLEM `61a0af1` keeps a dangling `install/golem_safety/share/golem_safety/config/h12/sim_safety_split.yaml` link (`--symlink-install`); `safety_node` refuses that config as unreadable. diff --git a/docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/harness/analyze.py b/docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/harness/analyze.py index 598a333b..cf65ec6d 100644 --- a/docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/harness/analyze.py +++ b/docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/harness/analyze.py @@ -23,6 +23,7 @@ 'upper_silent': ([0.4, 1.0, 2.4, 3.4], [0, 13]), 'lower_silent': ([0.4, 1.0, 1.4], [0, 13]), 'estop_q_edge': ([0.4, 0.9, 1.4], [0, 13]), + 'split_header_source': ([0.5], [0, 13, 27]), 'estop_q_below': ([0.4, 1.4], [0, 13]), 'estop_dq': ([0.4, 0.9], [0, 13]), 'state_nan': ([0.4, 0.9], [0, 13]), @@ -54,7 +55,6 @@ def reason(log): r'input_resumed|stale|Command validation|state_quiet', line, re.I): line = re.sub(r'^\[\w+\] \[[\d.]+\] \[safety_node\]: ', '', line) lines.append(line[:230]) - # keep first of each kind seen, out = set(), [] for line in lines: key = line[:40] diff --git a/docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/harness/driver.py b/docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/harness/driver.py index 0b192997..21bcf8f8 100644 --- a/docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/harness/driver.py +++ b/docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/harness/driver.py @@ -71,7 +71,7 @@ def valid_cmd(q=0.1, kp=50.0, kd=2.0): kp=np.full(N, kp), kd=np.full(N, kd)) -# Each scenario: config, mode, estop use, duration (s) and a function of t (s) returning +# Each scenario: config, estop use, duration (s) and a function of t (s) returning # {'state': {...overrides} or None, 'lower'/'upper'/'full': cmd dict or None, 'extra': ...}. def scenarios(): """Return the scenario table.""" @@ -126,6 +126,9 @@ def q_at(joint, value): both(valid_cmd()), state=q_at(3, 0.0 if t < 0.5 else f32(knee_hi) if t < 1.0 else knee_hi + 0.01))), + 'split_header_source': ('default_safety_split', False, 1.0, lambda t: { + 'lower': dict(valid_cmd(0.05), header=(8, 2), spare=5.0), + 'upper': dict(valid_cmd(0.2), header=(7, 1), spare=9.0)}), 'estop_q_below': ('default_safety_split', False, 1.5, lambda t: dict( both(valid_cmd()), state=q_at(3, np.nextafter(f32(knee_hi), f32(0)) if t >= 0.5 else 0.0))), diff --git a/docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/results/report.txt b/docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/results/report.txt index c994b1dc..446429d7 100644 --- a/docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/results/report.txt +++ b/docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/results/report.txt @@ -106,6 +106,17 @@ log: estop diagnostic, body slots in order: log: {"action_ms": 3372281022, "decision_ms": 3372281022, "decision_to_action_ms": 0, "event": "estop_command_written", "reason": "motor 3 (left_knee_joint) q out of estop range: 3.2 (limit 3.200, 1.00x)", "trigger_stamp_ms": null} +=== split_header_source + old: frames in [0,1.0]: 464 (464/s); first frame t=-2.05; headers (mm,pr) [(0, 0), (7, 1), (8, 2)]; reserve [(0, 0, 0, 0), (9, 9, 9, 9)] + t=0.5 [mode/q/dq/tau/kp/kd] 0:1,0.05,0,0,50,2; 13:1,0.2,0,0,50,2; 27:9,9,9,9,9,9 + log: {"action_ms": 1794885791341, "decision_ms": 1794885791339, "decision_to_action_ms": 2, "event": "estop_command_written", "reason": "Upper-body command stale for 2.00s (> 2.0s)", "trigger_stamp_ms": null} + new: frames in [0,1.0]: 500 (500/s); first frame t=-2.49; headers (mm,pr) [(6, 0)]; reserve [(0, 0, 0, 0)] + t=0.5 [mode/q/dq/tau/kp/kd] 0:1,0.05,0,0,50,2; 13:1,0.2,0,0,50,2; 27:0,0,0,0,0,0 + log: {"config": "/home/code/core_ws/install/golem_safety/share/golem_safety/config/h12/default_safety_split.yaml", "event": "relay_started", "mode": "split_mode", "robot": "h12", "sim": true, "use_estop": false} + log: {"event": "input_engaged", "source": "split_mode_lower", "topic": "/safety/lowcmd_lower_in"} + log: {"age": 0.102, "event": "state_quiet", "seconds": 0.1, "topic": "/lowstate"} + log: {"age": 0.202, "event": "input_quiet", "seconds": 0.2, "source": "split_mode_lower", "topic": "/safety/lowcmd_lower_in"} + === estop_q_below old: frames in [0,1.5]: 696 (464/s); first frame t=-2.05; headers (mm,pr) [(0, 0), (6, 0)]; reserve [(0, 0, 0, 0)] t=0.4 [mode/q/dq/tau/kp/kd] 0:1,0.1,0,0,50,2; 13:1,0.1,0,0,50,2 From 330bec5357626062312f349225c36763967375e7 Mon Sep 17 00:00:00 2001 From: maxconway Date: Mon, 16 Nov 2026 21:16:30 -0700 Subject: [PATCH 3/3] docs: safety comparison: the real-robot lmg_deploy bags replayed through golem_safety Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_015EG7p8sgY1ZVi6unbbLp4J --- .../README.md | 29 +++++++ .../harness/bags/compare.py | 85 +++++++++++++++++++ .../harness/bags/headers.py | 22 +++++ .../harness/bags/replay.sh | 27 ++++++ .../harness/bags/timeline.py | 55 ++++++++++++ 5 files changed, 218 insertions(+) create mode 100644 docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/harness/bags/compare.py create mode 100644 docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/harness/bags/headers.py create mode 100644 docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/harness/bags/replay.sh create mode 100644 docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/harness/bags/timeline.py diff --git a/docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/README.md b/docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/README.md index 0ca329ff..5d524495 100644 --- a/docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/README.md +++ b/docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/README.md @@ -59,6 +59,35 @@ Reproduce: copy `harness/` and a checkout of `h12_safety_layer` at `ad06903` (as `core_ws/install` sourced), then `bash /tmp/safety_diff/run_all.sh` and `python3 analyze.py out`. Domains 92 and 93 must be free; the harness refuses domain 0. +## The real-robot bags, replayed + +`core_ws/src/lmg_deploy/bags/policy_bag_walk` (65 s, walking to Nav2 goals) and `reach` +(31 s) were recorded on the real H1-2 with `h12_safety_layer` `camera_ready` `a178f0b` +relaying `lmg_deploy`: both safety inputs, `/lowstate`, the estop topics and `/lowcmd`. The +scripts are in [`harness/bags/`](harness/bags/). + +- **Headers on the real robot.** Every `/lowstate` (47,962 samples) reports `mode_machine` 6 + and `mode_pr` 0; every input and every `/lowcmd` carries (6, 0); slots 27-34 are zero + throughout (`headers.py`). +- **The recorded `/lowcmd` is the inputs, unchanged.** Every recorded frame with gains is + bit-identical, on all 27 body slots and every field, to the lower input (slots 0-11) and + the upper input (12-26) recorded within 200 ms before or 60 ms after it: 27,203 of 27,203 + in `policy_bag_walk`, 11,294 of 11,294 in `reach` (`compare.py`). The 58 walk frames before + the first recorded input are not counted. +- **golem_safety on the same inputs.** `replay.sh` plays each bag's inputs, `/lowstate` and + estop into `safety_node` (`relax_safety_split`, `use_estop:=true`, domain 93) and records + its `/lowcmd`. Every relayed frame is bit-identical to its inputs, header (6, 0) and zero + slots 27-34 included: 11,813 of 11,813 in `reach`, 28,556 of 28,562 in the walk; the 6 + others are the first 11 ms, when the upper input had engaged and the lower's first command + had not arrived (its slots carry zero gains until then, as in `h12_safety_layer`). +- **The estops happen at the same moment, for the same reason** (`timeline.py`): in the walk + both layers latch at 57.11 s on the right ankle roll measured at -0.2623 rad, past its + -0.2617 estop limit; in the reach both latch on the hardware estop press at 23.63 s. Both + write 50 zero commands after the latch, then nothing. + +The bags stay inside the clip limits and carry only (6, 0) headers, so they do not exercise +clipping or a foreign header; the scenarios below do. + ## Identical | Behaviour | Evidence | diff --git a/docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/harness/bags/compare.py b/docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/harness/bags/compare.py new file mode 100644 index 00000000..c5ada1a9 --- /dev/null +++ b/docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/harness/bags/compare.py @@ -0,0 +1,85 @@ +"""Check that a bag's /lowcmd is its safety inputs passed through unchanged. + +Usage: python3 compare.py [ ...] + +For every /lowcmd in the bag, builds the expected command from any +/safety/lowcmd_lower_in (slots 0-11) and /safety/lowcmd_upper_in (slots 12-26) +received from BEFORE_NS before it to AFTER_NS after it (the relay can lag, and +the recorder can log an input after the /lowcmd it produced), and compares the 27 body slots field by field (mode, q, +dq, tau, kp, kd), the header and slots 27-34. Prints the counts and, for frames +that match no input in the window, which joints and fields differ and by how much. +""" +import sys +from collections import Counter + +import numpy as np +import rosbag2_py +from rclpy.serialization import deserialize_message +from rosidl_runtime_py.utilities import get_message + +FIELDS = ('mode', 'q', 'dq', 'tau', 'kp', 'kd') +BEFORE_NS, AFTER_NS = 200_000_000, 60_000_000 # input window around a /lowcmd, by receive time + + +def body(msg): + """Return the 35 slots' (mode, q, dq, tau, kp, kd) as a float32 array.""" + return np.array([[m.mode, m.q, m.dq, m.tau, m.kp, m.kd] for m in msg.motor_cmd], + dtype=np.float32) + + +def read(uri): + """Yield (topic, receive time ns, message) for the LowCmd topics of a bag.""" + r = rosbag2_py.SequentialReader() + r.open(rosbag2_py.StorageOptions(uri=uri, storage_id='sqlite3'), + rosbag2_py.ConverterOptions('', '')) + types = {t.name: t.type for t in r.get_all_topics_and_types()} + topics = ['/lowcmd', '/safety/lowcmd_lower_in', '/safety/lowcmd_upper_in'] + r.set_filter(rosbag2_py.StorageFilter(topics=[t for t in topics if t in types])) + while r.has_next(): + topic, data, t = r.read_next() + yield topic, t, deserialize_message(data, get_message(types[topic])) + + +def compare(uri): + """Print how the bag's /lowcmd relates to its inputs.""" + lower, upper, outs = [], [], [] + for topic, t, msg in read(uri): + if topic == '/safety/lowcmd_lower_in': + lower.append((t, body(msg))) + elif topic == '/safety/lowcmd_upper_in': + upper.append((t, body(msg))) + else: + outs.append((t, msg.mode_machine, msg.mode_pr, body(msg))) + lt = np.array([x[0] for x in lower]) + ut = np.array([x[0] for x in upper]) + counts, diffs, worst = Counter(), Counter(), {} + for t, mm, pr, out in outs: + lw = [lower[i][1] for i in np.nonzero((lt >= t - BEFORE_NS) & (lt <= t + AFTER_NS))[0]] + uw = [upper[i][1] for i in np.nonzero((ut >= t - BEFORE_NS) & (ut <= t + AFTER_NS))[0]] + if not out[:27, 4:].any(): + counts['all gains zero (zero or estop command)'] += 1 + continue + if not lw or not uw: + counts['no input of each kind within the window'] += 1 + continue + counts['header (%d,%d)' % (mm, pr)] += 1 + counts['slots 27-34 zero' if not out[27:].any() else 'slots 27-34 nonzero'] += 1 + cands = [np.concatenate([lo[:12], up[12:27]]) for lo in lw for up in uw] + if any(np.array_equal(out[:27], c) for c in cands): + counts['body identical to an input within the window'] += 1 + continue + counts['body differs from every input within the window'] += 1 + best = min(cands, key=lambda c: np.abs(out[:27] - c).sum()) + for j, f in zip(*np.nonzero(out[:27] != best)): + key = (int(j), FIELDS[f]) + diffs[key] += 1 + worst[key] = max(worst.get(key, 0.0), float(abs(out[j, f] - best[j, f]))) + print(f'== {uri}: {len(outs)} /lowcmd, {len(lower)} lower, {len(upper)} upper') + for k, v in sorted(counts.items()): + print(f' {k}: {v}') + for (j, f), n in diffs.most_common(20): + print(f' differs: slot {j} {f}: {n} frames, largest change {worst[(j, f)]:.4g}') + +if __name__ == '__main__': + for uri in sys.argv[1:]: + compare(uri) diff --git a/docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/harness/bags/headers.py b/docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/harness/bags/headers.py new file mode 100644 index 00000000..272bd877 --- /dev/null +++ b/docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/harness/bags/headers.py @@ -0,0 +1,22 @@ +"""Print the header values and counts of every LowCmd/LowState topic in a bag.""" +import sys +from collections import Counter +import rosbag2_py +from rclpy.serialization import deserialize_message +from rosidl_runtime_py.utilities import get_message + +r = rosbag2_py.SequentialReader() +r.open(rosbag2_py.StorageOptions(uri=sys.argv[1], storage_id='sqlite3'), + rosbag2_py.ConverterOptions('', '')) +types = {t.name: t.type for t in r.get_all_topics_and_types()} +want = [t for t, ty in types.items() if ty.startswith('unitree_hg/msg/Low')] +r.set_filter(rosbag2_py.StorageFilter(topics=want)) +hdr, spare = {t: Counter() for t in want}, {t: Counter() for t in want} +while r.has_next(): + topic, data, _ = r.read_next() + m = deserialize_message(data, get_message(types[topic])) + hdr[topic][(m.mode_machine, m.mode_pr)] += 1 + arr = m.motor_cmd if hasattr(m, 'motor_cmd') else m.motor_state + spare[topic][any(abs(arr[i].q) > 0 or getattr(arr[i], 'kp', 0) for i in range(27, 35))] += 1 +for t in want: + print(t, '(mode_machine, mode_pr):', dict(hdr[t]), ' nonzero slots 27-34:', dict(spare[t])) diff --git a/docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/harness/bags/replay.sh b/docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/harness/bags/replay.sh new file mode 100644 index 00000000..8e0df121 --- /dev/null +++ b/docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/harness/bags/replay.sh @@ -0,0 +1,27 @@ +#!/bin/bash +# Replay a real-robot bag's safety inputs, /lowstate and estop through golem_safety +# and record its /lowcmd beside the replayed inputs. +# Usage (golem_ros container): bash replay.sh [config] +# Runs on ROS_DOMAIN_ID 93, never 0. Starts golem_safety's safety_node from /tmp/gs +# (sim:=true, use_estop:=true), a recorder, then plays the bag once; stops what it started. + +BAG=$1; OUT=$2; CONFIG=${3:-relax_safety_split} +export ROS_DOMAIN_ID=93 +source /opt/ros/humble/setup.bash; source /home/code/core_ws/install/setup.bash +TOPICS="/lowstate /safety/lowcmd_lower_in /safety/lowcmd_upper_in /estop/triggered /estop/plugged_in" +rm -rf "$OUT" +# The layer under test. +setsid /tmp/gs/install/golem_safety/lib/golem_safety/safety_node --ros-args -p robot:=h12 \ + -p config:=$CONFIG -p sim:=true -p use_estop:=true > "$OUT.safety.log" 2>&1 & +SAFETY=$! +# The recorder: the replayed inputs and the layer's output, on one clock. +setsid ros2 bag record -o "$OUT" $TOPICS /lowcmd > "$OUT.record.log" 2>&1 & +RECORD=$! +sleep 3 +# The inputs, at the recorded rate. +ros2 bag play "$BAG" --topics $TOPICS > "$OUT.play.log" 2>&1 +sleep 1 +kill -INT -$RECORD -$SAFETY 2>/dev/null +sleep 2 +kill -9 -$RECORD -$SAFETY 2>/dev/null +echo "replayed $BAG -> $OUT" diff --git a/docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/harness/bags/timeline.py b/docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/harness/bags/timeline.py new file mode 100644 index 00000000..756341e5 --- /dev/null +++ b/docs/superpowers/specs/2026-11-16-golem-safety-vs-h12-safety-layer/harness/bags/timeline.py @@ -0,0 +1,55 @@ +"""Print a bag's relay timeline: first relayed frame, estop (first all-zero frame after one with gains), frames after it, and the measured joint nearest its estop limit then. + +Usage: python3 timeline.py (times in s from the first recorded input) +""" +import json +import subprocess +import sys + +import numpy as np +import rosbag2_py +from rclpy.serialization import deserialize_message +from rosidl_runtime_py.utilities import get_message + +sys.path.insert(0, '/tmp/bag_replay') +from compare import body # noqa: E402 + +lim = json.loads(subprocess.run( + ['/tmp/gs/install/golem_safety/lib/golem_safety/golem_safety_dump_config', 'h12', sys.argv[2]], + capture_output=True, text=True, check=True).stdout)['limits'] +lo, hi = np.array(lim['q_estop_low']), np.array(lim['q_estop_high']) +r = rosbag2_py.SequentialReader() +r.open(rosbag2_py.StorageOptions(uri=sys.argv[1], storage_id='sqlite3'), rosbag2_py.ConverterOptions('', '')) +types = {t.name: t.type for t in r.get_all_topics_and_types()} +r.set_filter(rosbag2_py.StorageFilter(topics=['/lowcmd', '/lowstate', '/safety/lowcmd_lower_in', '/estop/triggered'])) +t0, first, latch, after, state, trig = None, None, None, 0, None, None +margins = [] +while r.has_next(): + topic, data, t = r.read_next() + m = deserialize_message(data, get_message(types[topic])) + if topic == '/safety/lowcmd_lower_in' and t0 is None: + t0 = t + if topic == '/estop/triggered' and m.data and trig is None: + trig = t + if topic == '/lowstate': + q = np.array([m.motor_state[i].q for i in range(27)]) + state = (t, q) + margins.append((t, float(np.min(np.minimum(q - lo, hi - q))), int(np.argmin(np.minimum(q - lo, hi - q))), q)) + if topic == '/lowcmd': + gains = body(m)[:27, 4:].any() + if gains and first is None: + first = t + if first is not None and not gains and latch is None: + latch = t + if latch is not None: + after += 1 +rel = lambda t: (t - t0) / 1e9 if t is not None else None +print(f'{sys.argv[1]}: first relayed frame {rel(first):.3f} s; estop {rel(latch)} s; frames after it {after}; ' + f'estop/triggered true first at {rel(trig)}') +if latch is not None: + near = [m for m in margins if abs(m[0] - latch) < 50e6] + worst = min(near, key=lambda m: m[1]) + print(f' within 50 ms of the estop: smallest q margin to estop limit {worst[1]:.4f} rad at slot {worst[2]} ' + f'(q {worst[3][worst[2]]:.4f}, limits [{lo[worst[2]]:.4f}, {hi[worst[2]]:.4f}]) at {rel(worst[0]):.3f} s') +print(f' smallest q margin over the whole bag: {min(m[1] for m in margins):.4f} rad ' + f'(slot {min(margins, key=lambda m: m[1])[2]} at {rel(min(margins, key=lambda m: m[1])[0]):.3f} s)')