Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
12 changes: 7 additions & 5 deletions CLAUDE.md
Original file line number Diff line number Diff line change
Expand Up @@ -195,7 +195,7 @@ nests `unitree_api`, `unitree_go`, `unitree_hg`, `unitree_ros2_example`).

| Directory | Role | What it does | Key entry points |
|---|---|---|---|
| `golem_safety` | **safety (both robots)** | C++. The only writer of `/lowcmd` (§8): streams zero gains from boot (under `use_sim_time`, from the first `/clock`), engages each `/safety/*` input on its own first valid command and relays it, clips it, checks every `/lowstate` (robot identity, estop limits) and latches an estop until restart. A robot profile (`robots/<robot>.yaml`, the joint table `golem_upper_ik` and `golem_lowerbody_rl` read) and configs per robot (`config/h12/`, `config/g1/`) | `safety_node` (`robot`, `config`, `sim`, `use_estop` parameters; `use_sim_time` needs `sim`); `launch/safety.launch.py` (`safety_node` after `start_delay`), which both robots' bringups include |
| `golem_safety` | **safety (both robots)** | C++. The only writer of `/lowcmd` (§8): streams zero gains from the robot's first `/lowstate` (under `use_sim_time`, from the first `/clock` too) with that `/lowstate`'s `mode_machine`, engages each `/safety/*` input on its own first valid command and relays it, clips it, checks every `/lowstate` (robot identity, estop limits) and latches an estop until restart. A robot profile (`robots/<robot>.yaml`, the joint table `golem_upper_ik` and `golem_lowerbody_rl` read) and configs per robot (`config/h12/`, `config/g1/`) | `safety_node` (`robot`, `config`, `sim`, `use_estop` parameters; `use_sim_time` needs `sim`); `launch/safety.launch.py` (`safety_node` after `start_delay`), which both robots' bringups include |
| `golem_upper_ik` | **controller (upper body, both robots)** | Pink IK on the 14 arm joints with the torso (H1-2) or waist (G1) held; OMPL planning and a self-collision barrier from each robot's sphere model and SRDF (the G1's spheres were packed on the rubber-hand G1: none on the Dex3-1 fingers; its torso spheres stand inside the torso's flanks, so `home` keeps the H1-2's 20 mm `d_min`; its `arms_down`, where the real hands rest on the hips, is in self-collision); the follow gate; the safety/controller config pairing (each robot's `config/<robot>/pairing.yaml`, applied by `golem_upper_ik.pairing`) that `upper_ik.launch.py` checks; each robot's data (IK and held joints, frames, named postures) is `config/<robot>/robot.yaml` | `frame_task_server` (`/frame_task`, `/named_config`), `joint_state_publisher`; `launch/upper_ik.launch.py` (the pairing check, then `frame_task_server` after `start_delay`) and `launch/robot_state.launch.py` (`robot_state_publisher` on the robot's URDF + `joint_state_publisher`), which both robots' bringups include |
| `golem_bringup` | **bringup (both robots)** | Launch-only package that starts the whole H1-2 (`launch/h12/`) or G1 (`launch/g1/`), sim or real. See §6. | top-level + building-block launch files, RViz configs |
| `golem_ros2_models` | **description (both robots)** | Installs both robots' models from `CL_Assets`: the H1-2 with its Magpie grippers and the G1 with its Dex3-1 hands, each a Pinocchio URDF, a ROS copy with `package://golem_ros2_models/...` URIs, the meshes, and a sphere-packed self-collision model with its SRDF. No nodes and no joint table (that is `golem_safety`'s robot profile) | `golem_ros2_models.paths`; `launch/display.launch.py` (`robot:=h12|g1`) |
Expand Down Expand Up @@ -287,9 +287,10 @@ for the includes after it, so pass consistent values (all four blocks share
**The H1-2's start-up** (`h12_control.launch.py`; `golem_safety`'s and
`golem_upper_ik`'s READMEs have the details):

- `safety_node` streams zero-gain `/lowcmd` from boot (in the sim, from the first
- `safety_node` writes nothing until `/lowstate` reports `mode_machine` 6, then
streams zero-gain `/lowcmd` stamped with it (in the sim, also not before the first
`/clock`). It engages each input on
that input's first valid command once `/lowstate` reports `mode_machine` 6,
that input's first valid command once `/lowstate` reports `mode_machine` 6 and `mode_pr` 0,
and once a first estop sample has arrived;
until then it logs `engage_refused` with the reason, at most once a second.
- **`/safety/heartbeat` means relaying can begin.** `safety_node` publishes no
Expand Down Expand Up @@ -419,8 +420,9 @@ which `golem_upper_ik`'s `upper_ik.launch.py` applies (`upper_ik: not starting:
**The G1's start-up** (`g1_core.launch.py`'s docstring; `golem_safety`'s,
`golem_upper_ik`'s and `golem_lowerbody_rl`'s READMEs have the details):

- `safety_node` streams zero-gain `/lowcmd` (every motor off) from boot (in the
sim, from the first `/clock`), checks
- `safety_node` writes nothing until `rt/lowstate` reports `mode_machine` 5, then
streams zero-gain `/lowcmd` (every motor off) stamped with it (in the sim, also not
before the first `/clock`), checks
every `/lowstate` against the estop limits from the first sample (in the
sim, from the first `/clock`), and engages
each input on that input's own first valid command once `/lowstate` reports
Expand Down
2 changes: 1 addition & 1 deletion core_ws/src/golem_bringup/launch/g1/g1_core.launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,7 @@

safety_node runs split mode (split, or sim_split in the sim): it writes every
motor on /lowcmd, the legs from the leg node's input and the waist and
arms from the controller's. From boot it streams zero gains on every motor
arms from the controller's. From the robot's first rt/lowstate it streams zero gains on every motor
(every motor off), and engages each input on that input's own first valid
command once rt/lowstate reports the G1's mode_machine (5); until the leg
node commands, the leg motors stay off.
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,7 @@

golem_safety runs split.yaml: it writes every motor on rt/lowcmd, the legs
from the leg node's input and the waist and arms from the arm
controller's (g1_split.yaml). From boot it streams zero gains on every motor
controller's (g1_split.yaml). From the robot's first rt/lowstate it streams zero gains on every motor
(every motor off) and engages each input on its own first command: the arms on
the controller's first command, and the move to home starts once the
controller sees golem_safety relay it on rt/lowcmd.
Expand Down
2 changes: 1 addition & 1 deletion core_ws/src/golem_bringup/launch/g1/g1_sim.launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,7 @@
The G1 RoboCasa sim (golem_robocasa/g1/) takes rt/lowcmd and rt/dex3/*, and
holds the robot up on an elastic band until /elastic_band/toggle releases it.
golem_safety runs sim_split and the controller sim_split: golem_safety streams
zero gains on every motor from boot and engages each input on its own first
zero gains on every motor from the robot's first rt/lowstate and engages each input on its own first
command. The leg node (golem_lowerbody_rl's lowerbody_controller_node) always
runs HOMIE, on golem_lowerbody_rl's config/g1/sim.yaml: once golem_safety beats
and rt/lowstate is upright, it moves the legs to the policy's default pose;
Expand Down
43 changes: 28 additions & 15 deletions core_ws/src/golem_safety/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -143,32 +143,45 @@ cycle. No timer in the node runs on the wall clock.
**What it writes.** Each engaged input's newest valid command on that input's own slots
(`full`: every body slot; `split`: the lower and upper slots). Every other slot, the slots
past the body and the `reserve` words carry `zero_command` (mode 0, every value 0). Every
command carries the profile's `mode_machine` and `mode_pr` and its own Unitree CRC (CRC-32
over the message body, as the SDK computes it); nothing of an input's header is copied.
command carries the `mode_machine` of the newest `/lowstate` that reported the profile's (read
from every sample, as Unitree's examples copy it), the profile's `mode_pr` (0, pitch/roll
ankles, which the layer chooses as Unitree's examples do) and its own Unitree CRC (CRC-32 over
the message body, as the SDK computes it); nothing of an input's header is copied.

**Clipping.** A command is valid when q, dq, tau, kp and kd are finite on every body slot,
the input's own or not; any other command latches the estop. A valid command is clipped on
its input's slots: q, dq and tau to `limits.clip`, kp and kd to [0, `kp_max`] and [0,
`kd_max`]. A gain above its maximum raises `gain_clipped` (Warn) once per joint and gain.

**Start-up**, the same on both robots (no config key chooses it): from boot (under
`use_sim_time`, from the first `/clock`) the layer streams `zero_command` on every slot, so
every motor is off. Each input engages on its own
**Start-up**, the same on both robots (no config key chooses it): the layer writes nothing
until a `/lowstate` reports the profile's `mode_machine` (`relay_held`, Warn, with the reason,
at most every 5 s), nor, before an input has engaged, while the newest `/lowstate` reports
another; from then on (under `use_sim_time`, from the first `/clock` as well) it streams
`zero_command` on every slot, so every motor is off. Each input engages on its own
first valid command that arrives once the newest `/lowstate` reports the profile's
`mode_machine` (and, with `use_estop`, a first hardware estop sample has arrived); its slots
`mode_machine` and `mode_pr` (and, with `use_estop`, a first hardware estop sample has
arrived); its slots
carry its commands from then on (`input_engaged`). The slots of an input that has not
engaged stay at zero gain: in split mode the robot runs arms-only, with its leg motors off,
until the leg node commands. The `/lowstate` limit checks run from boot (under
`use_sim_time`, from the first `/clock`).
until the leg node commands. The `/lowstate` limit checks run from the first sample (under
`use_sim_time`, from the first `/clock`). An estop that latches before the robot's first
`/lowstate` (a non-finite command, a first estop sample that says triggered) writes nothing
until that `/lowstate` arrives (`estop_waiting`, Error, once), then its estop commands.

A command that cannot engage raises `engage_refused` (Warn) with the first reason, at most
once a second: `no /lowstate received yet`, `/lowstate mode_machine is 0, expected 6`, or
`no hardware estop sample received yet on /estop/triggered or /estop/plugged_in`.

**Robot identity.** Every `/lowstate` is checked against the profile's `mode_machine`. Before
engaging, another value only refuses (another robot or sim delays engaging); once engaged, it
latches the estop (`/lowstate mode_machine 5 is not the h12's 6: another robot or sim is on
this domain`). A foreign sample is never limit-checked. `docker/scripts/wait_for_sim.py`
once a second: `no /lowstate received yet`, `/lowstate mode_machine is 0, expected 6`,
`/lowstate mode_pr is 1, expected 0`, or `no hardware estop sample received yet on
/estop/triggered or /estop/plugged_in`.

**Robot identity.** Every `/lowstate` is checked against the profile's `mode_machine` and
`mode_pr`. Before engaging, another `mode_machine` holds the output and refuses engaging
(another robot or sim delays it), and another `mode_pr` refuses engaging while the zero stream
goes on with the profile's; once engaged, either latches the estop (`/lowstate mode_machine 5
is not the h12's 6: another robot or sim is on this domain`, `/lowstate mode_pr 1 is not the
h12's 0: the robot is in another ankle mode`), and the estop commands carry our robot's last
`mode_machine`, never the other's. Such a sample is never limit-checked. The real H1-2 reports
`mode_machine` 6 and `mode_pr` 0 (the `lmg_deploy` bags); the G1's `mode_pr` is unmeasured on
the real G1. `docker/scripts/wait_for_sim.py`
checks the same values before a paired bringup starts, and each robot's sims report them.

**Quiet inputs are held and reported, not estopped.** Once an input has engaged, a gap in
Expand Down
6 changes: 5 additions & 1 deletion core_ws/src/golem_safety/include/golem_safety/lowcmd.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -15,9 +15,13 @@ namespace golem_safety
// The robots and the sims drop a command whose CRC does not match.
uint32_t lowcmd_crc(const unitree_hg::msg::LowCmd & cmd);

// Set mode_pr and mode_machine from `profile`, then crc.
// Set mode_pr and mode_machine, then crc.
// SAFETY: every command is stamped last, after its final edit; an edit after the stamp
// needs a new stamp, or the robot drops the command.
void stamp_header_and_crc(
unitree_hg::msg::LowCmd & cmd, uint8_t mode_pr, uint8_t mode_machine);

// Set mode_pr and mode_machine from `profile`, then crc.
void stamp_header_and_crc(unitree_hg::msg::LowCmd & cmd, const RobotProfile & profile);

// Return the stamped command that turns every motor off: mode 0 and q, dq, tau, kp, kd and
Expand Down
36 changes: 27 additions & 9 deletions core_ws/src/golem_safety/include/golem_safety/safety_core.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -38,11 +38,18 @@ struct Event
// Relay. Each input keeps its newest valid command. step() starts from zero_command and
// copies onto each engaged input's slots that input's newest command, so a slot no engaged
// input owns stays at zero_command (every motor off). step() and exit_step() stamp the
// header and CRC on whatever they return, last.
// header and CRC on whatever they return, last: mode_machine from the newest /lowstate that
// reported the profile's (read from every sample), mode_pr from the profile.
//
// SAFETY: nothing is written before a /lowstate reports the profile's mode_machine, nor,
// before an input has engaged, while the newest /lowstate reports another (relay_held,
// Warn, at most every 5 s of one hold). An estop that latches before any such /lowstate
// writes nothing until one arrives (estop_waiting, Error, once), then its estop commands as
// after any latch, and estop_command_written.
//
// Engaging. Each input engages on its own first valid command that arrives while the newest
// /lowstate carries the profile's mode_machine and, with use_estop, a hardware estop sample
// has arrived (input_engaged, Info). A command that cannot engage raises engage_refused
// /lowstate carries the profile's mode_machine and mode_pr and, with use_estop, a hardware
// estop sample has arrived (input_engaged, Info). A command that cannot engage raises engage_refused
// (Warn) with the first unmet condition, at most once a second. A /lowstate from another
// robot before engaging only delays engaging.
//
Expand All @@ -68,13 +75,14 @@ struct Event
// writer's last command, valid or not, is older than single_writer.handover_seconds;
// otherwise it latches the estop with "second writer on <topic>".
//
// SAFETY: the /lowstate checks run on every sample that carries the profile's mode_machine,
// from the first one, before any input engages.
// SAFETY: the /lowstate limit checks run on every sample that carries the profile's
// mode_machine and mode_pr, from the first one, before any input engages.
//
// SAFETY: the estop latches, one way until the process restarts, on
// - a command with a non-finite value, before or after engaging: "Command validation
// failed on <topic>: motor <slot> (<joint>) <field> is <value>";
// - a /lowstate whose mode_machine is not the profile's, once an input has engaged;
// - a /lowstate whose mode_machine or mode_pr is not the profile's, once an input has
// engaged; after a foreign mode_machine the estop commands carry our robot's last one;
// - on ROS domain 0 with control.state_lag_ms > 0, a /lowstate that arrives more than
// state_lag_ms late against the robot's uint32 ms tick: its arrival minus its tick,
// less a baseline that is the smallest such offset seen and may rise by 0.5 ms/s to
Expand Down Expand Up @@ -176,8 +184,8 @@ class SafetyCore
// Name why a command cannot engage.
struct Refusal
{
enum Kind { NoState, ModeMachine, NoEstopSample } kind;
int mode_machine = 0; // ModeMachine: the sample's
enum Kind { NoState, ModeMachine, ModePr, NoEstopSample } kind;
int value = 0; // ModeMachine, ModePr: the sample's
};

// Hold the transport-lag check's state: the robot's ms tick, unwrapped, against `now`.
Expand Down Expand Up @@ -283,8 +291,13 @@ class SafetyCore
unitree_hg::msg::LowCmd merged() const;
// Return this step's estop command per estop_response, or nullopt once the burst is done.
std::optional<unitree_hg::msg::LowCmd> estop_output(double now);
// Stamp the header and CRC on `out` and remember it as the last command written.
// Stamp the header and CRC on `out` and remember it as the last command written; nullopt
// when no /lowstate has reported the profile's mode_machine yet.
std::optional<unitree_hg::msg::LowCmd> finish(std::optional<unitree_hg::msg::LowCmd> out);
// Raise estop_waiting once for a latch with no header to stamp yet; return nullopt.
std::optional<unitree_hg::msg::LowCmd> unwritten_estop();
// Raise relay_held, at most once every kHeldReportSeconds of one hold.
void report_held(double now);

const RobotProfile profile_;
const SafetyConfig config_;
Expand All @@ -303,6 +316,11 @@ class SafetyCore
std::optional<double> refusal_reported_; // s, the last engage_refused

std::optional<unitree_hg::msg::LowState> state_;
// The mode_machine of the newest /lowstate that reported the profile's: every command's.
std::optional<uint8_t> header_mode_machine_;
bool state_foreign_ = false; // the newest /lowstate's mode_machine is not the profile's
std::optional<double> held_reported_; // s, the last relay_held of the current hold
bool estop_wait_reported_ = false; // estop_waiting raised
std::optional<double> state_time_; // s, the last /lowstate's arrival, kept while estopped
double state_wall_time_ = 0.0; // s, wall clock at the newest /lowstate's arrival
bool state_unlogged_ = false; // take_log_sample() has a sample to return
Expand Down
11 changes: 8 additions & 3 deletions core_ws/src/golem_safety/src/lowcmd.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -134,13 +134,18 @@ uint32_t lowcmd_crc(const unitree_hg::msg::LowCmd & cmd)
return crc32_core(words.data(), kCrcWords);
}

void stamp_header_and_crc(unitree_hg::msg::LowCmd & cmd, const RobotProfile & profile)
void stamp_header_and_crc(unitree_hg::msg::LowCmd & cmd, uint8_t mode_pr, uint8_t mode_machine)
{
cmd.mode_pr = profile.mode_pr;
cmd.mode_machine = profile.mode_machine;
cmd.mode_pr = mode_pr;
cmd.mode_machine = mode_machine;
cmd.crc = lowcmd_crc(cmd);
}

void stamp_header_and_crc(unitree_hg::msg::LowCmd & cmd, const RobotProfile & profile)
{
stamp_header_and_crc(cmd, profile.mode_pr, profile.mode_machine);
}

unitree_hg::msg::LowCmd zero_command(const RobotProfile & profile)
{
unitree_hg::msg::LowCmd cmd;
Expand Down
Loading
Loading