From e995bd74073bb8ea146d35857b9cd8a8353a3be9 Mon Sep 17 00:00:00 2001 From: cagataycali Date: Thu, 27 Aug 2026 18:53:28 -0400 Subject: [PATCH 1/7] docs(rfc): external per-joint target streaming (robot.setJoints) Scoping RFC for an external per-joint-write path: a robot.setJoints intent + an External drive mode that uses streamed joint targets in place of the on-device policy, all through the existing duck-control Safety chokepoint. Closes the safety.rs actuator-travel-vs-anatomical-limit gap, adds a per-tick step clamp and an external deadman. Unlocks the strands-robots passthrough driver + running off-robot/mid-iteration RL policies on the real robot. Distinct from the updater model-deploy path. Draft for Pollen review. --- docs/rfc/0001-external-per-joint-write.md | 167 ++++++++++++++++++++++ 1 file changed, 167 insertions(+) create mode 100644 docs/rfc/0001-external-per-joint-write.md diff --git a/docs/rfc/0001-external-per-joint-write.md b/docs/rfc/0001-external-per-joint-write.md new file mode 100644 index 00000000..3155c9f8 --- /dev/null +++ b/docs/rfc/0001-external-per-joint-write.md @@ -0,0 +1,167 @@ +# RFC: `robot.setJoints` — external per-joint target streaming + +Status: **draft / scoping** · Author: @cagataycali · Target: pollen-robotics/microduck + +## Summary + +Add one new intent to robotd's `robot.*` socket — `robot.setJoints` — that lets an +**off-robot** controller stream joint targets at the control rate, and a matching +**External** drive mode in which the control loop uses those targets in place of the +on-device policy. Everything still flows through the existing `duck-control` safety +layer; nothing bypasses `Safety::apply`. + +## Motivation + +robotd today is intent-level by design: `robot.move` / `robot.pose` / `robot.head` / +`robot.do` describe *what* to do, and the on-device `Policy` (walking + skills) turns +that into joint targets at 50 Hz. That is the right default and should stay the default. +But two real workflows have no path: + +1. **Run an off-robot policy on the real robot.** A policy that is too large for the + Pi, or one being iterated on a workstation/GPU, cannot drive the hardware without + re-flashing a `model` component through the updater on every change. The + teleoperation / VLA / lerobot / MHS pattern is: *controller computes joint targets, + streams them to the robot at rate.* There is no verb for that here. +2. **Bring-up / calibration / scripted motion.** Moving one joint to a commanded angle + for a test, a calibration sweep, or a recorded trajectory replay — all want direct + joint targets, not an intent the policy re-interprets. + +This is distinct from **deploying** a finished on-device policy, which is already served +by the updater's `model` component (`update.apply`, gated by `robot.modelApi`). This RFC +does **not** touch that path. + +### Downstream unlock + +`strands-robots`' native driver (`Robot("microduck", mode="real")`) is delegate-only +*because the wire has no per-joint write* — it sends intents and reads `robot.state`, +and refuses `run_policy`. `robot.setJoints` turns that refusal into a real **passthrough +mode**: the driver streams an off-robot policy's actions straight to the joints. + +## Non-goals + +- Not a replacement for the on-device policy or the model-update deploy path. +- Not a general teleop protocol (no bilateral force, no trajectory interpolation server — + the controller owns interpolation and sends targets at rate). +- No new IO handle in `duck-control::control` — the Runtime still only *proposes* + targets; `Safety` still owns the only motor write. + +## Current architecture (what we build on) + +Control tick (robotd/src/main.rs:1040): `read → observe (fall) → gate (deadman) → policy → safety.apply` + +- **`duck_control::control::Runtime::step(sensors, command, …) -> targets[NUM_JOINTS]`** + (control.rs) turns a `Command` (twist / head_pose / body_pose) + the active skill into + `targets[j] = DEFAULT_POSITION[j] + action_scale · offset[j]`. It holds no IO handle. +- **`duck_control::safety::Safety::apply`** (safety.rs) is the sole writer. It already: + refuses non-finite targets (NaN is *refused*, not clamped), clamps to actuator travel, + runs the fall gate + limp gain, and enforces a **deadman** (`SafetyConfig.deadman`, + default 500 ms) that zeroes the velocity when intents go stale. +- **`robotd/src/intents.rs`** tracks intent *age*; the deadman reads the twist's age, and + a head write must not refresh the twist clock. +- `NUM_JOINTS = 15`, `MOUTH_INDEX = 9` (duck-control/src/model.rs); the policy drives 14, + the mouth is slot 9. Targets are absolute joint angles in **radians**. + +Known gap this RFC must close (safety.rs:42–46, in Pollen's own words): the range clamp is +the *actuator's travel, not a per-joint anatomical limit* — "the real joint limits live in +the MJCF, which is not vendored here." A policy trained in that MJCF stays inside those +limits implicitly; **arbitrary external targets do not**, so external write needs real +per-joint bounds. + +## Design + +### 1. Protocol — `duck-ipc-proto` + +New method constant + `Call` variant + params struct, wired through `method()`, `parse()`, +`params()`, with a serde round-trip test in the style of the existing ones (assert the exact +wire line, e.g. field names, and `from_str(to_string(x)) == x`). + +```rust +// method:: +pub const ROBOT_SET_JOINTS: &str = "robot.setJoints"; + +// Call:: +RobotSetJoints(SetJointsParams), + +/// Absolute joint targets, radians, in JOINT_NAMES order (len == NUM_JOINTS). +/// A NOTIFICATION (no id) — it is a high-rate stream like robot.move. +#[derive(Debug, Clone, PartialEq, Serialize, Deserialize)] +pub struct SetJointsParams { + pub targets: Vec, // len must equal NUM_JOINTS; validated on parse + #[serde(default, skip_serializing_if = "Option::is_none")] + pub gain: Option, // optional stiffness; defaults to gain_running +} +``` + +Wire (notification): +```json +{"jsonrpc":"2.0","method":"robot.setJoints","params":{"targets":[0.0,-0.087, … 15 values …]}} +``` + +Open question: absolute radians (proposed, easiest to clamp) vs. offsets from +`DEFAULT_POSITION`. Absolute is safer — the clamp bounds are absolute. + +### 2. robotd — an **External** drive mode + +- Extend the drive mode (`robot.setMode`, main.rs) with `External`. `robot.setJoints` is + **only** honoured in `External` mode and only with torque enabled; in any other mode it + is refused with a named error (exactly as `run_policy` is refused on the strands side + today). Entering `External` requires an explicit `robot.setMode external`. +- `intents.rs` gains an external-targets slot with its **own age clock** (independent of + the twist deadman). A fresh `robot.setJoints` stores `targets` + `Instant::now()`. +- Control tick becomes `read → observe → gate → {External ? external_targets : policy} → safety.apply`. + In `External` mode the policy stage is skipped and the stored external targets are used. + +### 3. Safety envelope (the whole point) + +`robot.setJoints` must be *no less safe* than the policy path. Additions, all inside the +existing `Safety` chokepoint: + +1. **Mode + torque gate** — refuse unless `External` and enabled (above). +2. **Deadman on external targets** — reuse the deadman mechanism with the external clock: + if no fresh `setJoints` within `SafetyConfig.deadman`, hold the last safe target and + drop toward limp gain. A dropped/stalled off-robot controller must not leave a live + command. (500 ms is generous for a 50 Hz stream; consider a tighter external deadman, + e.g. 100–150 ms.) +3. **Per-tick step clamp (new)** — bound `|target[j] − previous[j]|` per tick to a max + joint velocity, so a single bad frame can't snap a joint. The policy path is inherently + smooth; raw external targets are not, so this is required. +4. **Per-joint anatomical limits (new — closes safety.rs:42–46)** — vendor the per-joint + `[min,max]` from the MJCF and clamp external targets to them, not just to actuator + travel. Without this, "range clamp" is weaker than the guarantee external write implies. +5. **NaN refusal** — already present; a non-finite target in the vector is refused outright. + +### 4. strands-robots driver (downstream, separate PR) + +Add a passthrough path to `MicroduckDriver`: `set_mode("external")` then stream +`robot.setJoints` from `send_action`/a policy loop. This upgrades the driver from +delegate-only to true external control and is what lets an off-robot / mid-iteration RL +policy drive the physical robot. Ships after this RFC lands. + +## Testing + +- **Protocol**: serde round-trip + exact-wire assertions, matching the existing + `duck-ipc-proto` test discipline; reject `targets.len() != NUM_JOINTS` on parse. +- **Safety** (fake `RobotIo`): a NaN in the vector is refused; a target outside a joint's + anatomical limit is clamped to it; a jump larger than the per-tick bound is rate-limited; + external targets older than the deadman hold + limp; `setJoints` outside `External` mode + is refused. +- **Mode**: `robot.setMode external` → `setJoints` moves a joint; `robot.setMode …` back to + a policy mode resumes the policy with no residual external target. + +## Rollout / PR plan + +1. `duck-ipc-proto`: method + `Call` variant + `SetJointsParams` + round-trip test. (self-contained) +2. `duck-control`: per-joint limits + per-tick step clamp + external deadman in `Safety`; the + `External` branch in `Runtime`. (safety-critical; most review here) +3. `robotd`: `External` mode in `robot.setMode`, external-target slot + clock in `intents.rs`, + the loop branch, refusals. +4. Docs + a `duckctl` example that streams a sine sweep on one joint in `External` mode. +5. (separate, downstream) strands-robots driver passthrough mode. + +## Open questions for Pollen + +- Absolute radians vs. offsets from `DEFAULT_POSITION`? (RFC proposes absolute.) +- Is a new `External` mode preferred, or gating `setJoints` on an existing mode? +- Per-joint limits: vendor from the MJCF into `duck-control`, or a config file robotd loads? +- External deadman: reuse 500 ms, or a tighter dedicated value for rate-streamed control? +- Should the mouth (slot 9) be writable via `setJoints`, or masked like the policy path masks it? From 99206ada611ec6543bd50ea2ad05e6db3bd9e7c4 Mon Sep 17 00:00:00 2001 From: cagataycali Date: Thu, 27 Aug 2026 19:16:12 -0400 Subject: [PATCH 2/7] =?UTF-8?q?feat(proto):=20robot.setJoints=20=E2=80=94?= =?UTF-8?q?=20external=20per-joint=20target=20stream?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Add the RFC's protocol layer: ROBOT_SET_JOINTS method, Call::RobotSetJoints and SetJointsParams { targets: Vec (len NUM_JOINTS), gain: Option }. Wired through method(), params(), destination() (Robot/Prompt) and parse(), which rejects targets.len() != JOINT_NAMES.len() by name with INVALID_PARAMS. A notification like robot.move (no id). Covered by every_call() plus exact-wire and length-rejection round-trip tests. cargo test -p duck-ipc-proto green (51). --- duck-ipc-proto/src/lib.rs | 123 +++++++++++++++++++++++++++++++++++++- 1 file changed, 122 insertions(+), 1 deletion(-) diff --git a/duck-ipc-proto/src/lib.rs b/duck-ipc-proto/src/lib.rs index 33224abe..17c136f6 100644 --- a/duck-ipc-proto/src/lib.rs +++ b/duck-ipc-proto/src/lib.rs @@ -316,6 +316,17 @@ pub mod method { /// Turn policy execution on or off. pub const ROBOT_ENABLE: &str = "robot.enable"; + /// Stream absolute joint targets from an off-robot controller. Continuous; send as a + /// notification, like [`ROBOT_MOVE`]. + /// + /// Honoured **only** in the `External` drive mode ([`ROBOT_SET_MODE`] `external`), and + /// refused by name in any other mode — the on-device policy owns the joints otherwise. + /// The targets are absolute joint angles in radians, in [`super::JOINT_NAMES`] order, and + /// still pass through `duck-control`'s safety layer: per-joint anatomical limits, a + /// per-tick step clamp, and a dedicated external deadman. See + /// `docs/rfc/0001-external-per-joint-write.md`. + pub const ROBOT_SET_JOINTS: &str = "robot.setJoints"; + // ── power to the joints ────────────────────────────────────────────────── // // The pair, and they are a pair: nothing else in this API turns the motors on or off. @@ -614,6 +625,9 @@ pub enum Call { RobotLook(LookParams), RobotStop, RobotEnable(EnableParams), + /// Stream absolute joint targets from an off-robot controller. Continuous; send as a + /// notification. Honoured only in `External` mode. See [`method::ROBOT_SET_JOINTS`]. + RobotSetJoints(SetJointsParams), /// Power the joints and ramp to the home pose. No policy needed. RobotInit, /// Cut power to the joints. The robot collapses if nothing holds it. @@ -756,6 +770,7 @@ impl Call { Call::RobotLook(_) => method::ROBOT_LOOK, Call::RobotStop => method::ROBOT_STOP, Call::RobotEnable(_) => method::ROBOT_ENABLE, + Call::RobotSetJoints(_) => method::ROBOT_SET_JOINTS, Call::RobotInit => method::ROBOT_INIT, Call::RobotRelax => method::ROBOT_RELAX, Call::RobotDo(_) => method::ROBOT_DO, @@ -877,6 +892,7 @@ impl Call { | Call::RobotLook(_) | Call::RobotStop | Call::RobotEnable(_) + | Call::RobotSetJoints(_) | Call::RobotInit | Call::RobotRelax | Call::RobotDo(_) @@ -967,6 +983,7 @@ impl Call { Call::RobotHead(p) => encode(p), Call::RobotLook(p) => encode(p), Call::RobotEnable(p) => encode(p), + Call::RobotSetJoints(p) => encode(p), Call::RobotDo(p) => encode(p), Call::RobotPose(p) => encode(p), Call::RobotMouth(p) => encode(p), @@ -1041,6 +1058,23 @@ impl Call { method::ROBOT_LOOK => Call::RobotLook(decode(params)?), method::ROBOT_STOP => Call::RobotStop, method::ROBOT_ENABLE => Call::RobotEnable(decode(params)?), + method::ROBOT_SET_JOINTS => { + let p: SetJointsParams = decode(params)?; + // The joint vector is positional and indexed as `JOINT_NAMES`, so a vector of + // any other length is not a partial command — it is a command whose slot 5 the + // sender and the robot disagree about. Refused here, at the door, by name. + if p.targets.len() != JOINT_NAMES.len() { + return Err(Error::new( + code::INVALID_PARAMS, + format!( + "robot.setJoints needs exactly {} targets, got {}", + JOINT_NAMES.len(), + p.targets.len() + ), + )); + } + Call::RobotSetJoints(p) + } method::ROBOT_INIT => Call::RobotInit, method::ROBOT_RELAX => Call::RobotRelax, method::ROBOT_DO => Call::RobotDo(decode(params)?), @@ -1173,6 +1207,13 @@ pub mod test_support { on: true, toggle: false, }), + Call::RobotSetJoints(SetJointsParams { + targets: vec![ + 0.0, -0.087, -0.458, -0.005, 0.453, 0.349, 0.349, 0.0, 0.0, 0.0, 0.087, 0.458, + 0.005, -0.453, 0.0, + ], + gain: Some(180), + }), Call::RobotInit, Call::RobotRelax, Call::RobotDo(DoParams { @@ -1824,6 +1865,32 @@ pub struct EnableParams { pub toggle: bool, } +/// Absolute joint targets streamed from an off-robot controller — see +/// [`method::ROBOT_SET_JOINTS`]. +/// +/// A **notification** like [`MoveParams`], sent at the control rate; last-writer-wins, and +/// expiring through a dedicated external deadman. The targets are absolute joint angles in +/// **radians**, indexed exactly as [`JOINT_NAMES`], so `targets.len()` must equal +/// [`JOINT_NAMES`]`.len()` — a shorter or longer vector is refused on parse rather than read +/// as a partial command, because the vector is positional and a length mismatch means the two +/// sides disagree about which number drives which joint. +/// +/// Absolute rather than offsets from `DEFAULT_POSITION`, because the safety clamp's bounds are +/// absolute — clamping an absolute target to an absolute anatomical limit needs no reference +/// pose to be agreed first. +#[derive(Debug, Clone, PartialEq, Serialize, Deserialize)] +#[serde(deny_unknown_fields)] +pub struct SetJointsParams { + /// Absolute joint angles, radians, in [`JOINT_NAMES`] order. Length must equal + /// [`JOINT_NAMES`]`.len()`. + pub targets: Vec, + /// Position P gain to hold the targets at. `None` uses the running gain the mode was + /// configured with, which is the ordinary case — a controller streaming targets rarely + /// wants to think about stiffness per frame. + #[serde(default, skip_serializing_if = "Option::is_none")] + pub gain: Option, +} + /// What an apply should move to. #[derive(Debug, Clone, PartialEq, Eq, Serialize, Deserialize)] #[serde(rename_all = "snake_case")] @@ -3845,7 +3912,7 @@ mod tests { fn every_call_covers_every_variant() { assert_eq!( every_call().len(), - 46, + 47, "a Call variant was added or removed — update every_call() and this count" ); } @@ -3922,6 +3989,60 @@ mod tests { ); } + /// `robot.setJoints` is a notification carrying an absolute joint vector, and it goes on + /// the wire exactly as the RFC draws it: `targets` present, `gain` omitted when unset. + #[test] + fn set_joints_is_a_notification_on_the_wire() { + let call = Call::RobotSetJoints(SetJointsParams { + targets: vec![0.0; JOINT_NAMES.len()], + gain: None, + }); + let line = serde_json::to_string(&Request::notify(&call)).unwrap(); + + assert!(line.contains(r#""jsonrpc":"2.0""#), "{line}"); + assert!(line.contains(r#""method":"robot.setJoints""#), "{line}"); + assert!(line.contains(r#""targets":["#), "{line}"); + // A notification carries no id, like every other continuous intent. + assert!(!line.contains("\"id\""), "{line}"); + // The gain is omitted when unset, so the common streamed frame stays small. + assert!(!line.contains("gain"), "{line}"); + + let back: Request = serde_json::from_str(&line).unwrap(); + assert!(back.is_notification()); + assert_eq!(back.as_call().unwrap(), call); + + // And the gain reaches the wire when set, round-tripping unchanged. + let with_gain = Call::RobotSetJoints(SetJointsParams { + targets: vec![0.1; JOINT_NAMES.len()], + gain: Some(180), + }); + let line = serde_json::to_string(&Request::notify(&with_gain)).unwrap(); + assert!(line.contains(r#""gain":180"#), "{line}"); + let back: Request = serde_json::from_str(&line).unwrap(); + assert_eq!(back.as_call().unwrap(), with_gain); + } + + /// The joint vector is positional, so a length other than `JOINT_NAMES.len()` is refused + /// on parse — by name, with `INVALID_PARAMS` — rather than read as a partial command. + #[test] + fn set_joints_rejects_a_wrong_length_vector() { + for wrong in [0usize, 14, 16] { + let params = serde_json::json!({ "targets": vec![0.0f64; wrong] }); + let error = Call::parse(method::ROBOT_SET_JOINTS, Some(¶ms)) + .expect_err(&format!("{wrong} targets were accepted")); + assert_eq!(error.code, code::INVALID_PARAMS, "{wrong} targets"); + assert!( + error.message.contains(&JOINT_NAMES.len().to_string()), + "the refusal names the required count: {}", + error.message + ); + } + // The one right length parses. + let ok = serde_json::json!({ "targets": vec![0.0f64; JOINT_NAMES.len()] }); + assert!(Call::parse(method::ROBOT_SET_JOINTS, Some(&ok)).is_ok()); + } + + /// `from_dir` survives the wire, and only appears when it was asked for. /// /// The absence half is the load-bearing one. Every other client of this type — `btd` From 61c6a8a9ef3a2e5894115ff820bace2879166098 Mon Sep 17 00:00:00 2001 From: cagataycali Date: Thu, 27 Aug 2026 19:23:16 -0400 Subject: [PATCH 3/7] feat(control): external per-joint safety envelope for robot.setJoints MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Safety::apply_external — the sink for the RFC's external joint stream, through the same single motor-write chokepoint as apply(), adding the guarantees raw off-robot targets need: - external deadman (SafetyConfig.external_deadman, 150ms; tighter than the 500ms twist deadman): a stalled controller holds the fallback pose and drops to limp - per-joint anatomical limit table ANATOMICAL_LIMITS (conservative placeholders, TODO source from the MJCF) — closes the actuator-travel-vs-anatomical gap the module admitted at safety.rs:42-46; clamps external targets to real bounds - per-tick step clamp (SafetyConfig.external_max_step, 0.2 rad) vs the last external target, so one bad frame cannot snap a joint - NaN/inf refused outright (delegated to apply, holds — never clamped) New Limit::Step; clear_external() drops the step baseline on mode exit. 8 unit tests vs FakeIo (passthrough, NaN, out-of-limit clamp, over-fast step, stale=hold+limp, external` rather than hand-picking them — +/// the MJCF is not vendored in this repo yet (same note as `microduck_rl`'s env). Until it is, +/// every bound here is a safe under-approximation: it will refuse travel a real joint has before +/// it will allow travel a real joint does not. Mirror joints (left/right) use mirrored bounds. +/// +/// The mouth (index [`crate::model::MOUTH_INDEX`]) is bounded to its own travel +/// ([`crate::model::MOUTH_CLOSED`]..[`crate::model::MOUTH_OPEN`]); whether external write may +/// move it at all is a masking decision `robotd` makes, not this table's. +pub const ANATOMICAL_LIMITS: [(f64, f64); NUM_JOINTS] = [ + (-0.80, 0.80), // left_hip_yaw (home 0.0000) + (-0.90, 0.70), // left_hip_roll (home -0.0873) + (-1.60, 0.70), // left_hip_pitch (home -0.4579) + (-1.80, 0.20), // left_knee (home -0.0049) + (-0.60, 1.40), // left_ankle (home 0.4530) + (-0.70, 1.30), // neck_pitch (home 0.3491) + (-0.70, 1.30), // head_pitch (home 0.3491) + (-1.40, 1.40), // head_yaw (home 0.0000) + (-0.80, 0.80), // head_roll (home 0.0000) + (MOUTH_CLOSED, MOUTH_OPEN), // mouth (home 0.0000; -5°..+30°) + (-0.80, 0.80), // right_hip_yaw (home 0.0000) + (-0.70, 0.90), // right_hip_roll (home 0.0873) + (-0.70, 1.60), // right_hip_pitch(home 0.4579) + (-0.20, 1.80), // right_knee (home 0.0049) + (-1.40, 0.60), // right_ankle (home -0.4530) +]; + #[derive(Debug, Clone, Copy, PartialEq)] pub struct SafetyConfig { /// Projected-gravity z above which the robot counts as falling. Upright reads about @@ -57,6 +94,22 @@ pub struct SafetyConfig { pub fall_debounce: Duration, /// Intent age past which the velocity command is zeroed. pub deadman: Duration, + /// Intent age past which a streamed *external* joint command ([`Safety::apply_external`]) + /// is dropped: the robot holds its last safe pose and drops toward the limp gain. + /// + /// Tighter than [`Self::deadman`] on purpose. The twist deadman guards a velocity a human + /// keeps refreshing from a gamepad, and 500 ms of a held stick is nothing; an external + /// controller streams *positions* at the control rate, so a gap of even a few frames means + /// the controller stalled, and a stalled controller must not leave a live joint command. At + /// 50 Hz, 150 ms is seven or eight missed frames — long enough not to trip on jitter, short + /// enough that a dropped controller cannot walk the robot into anything. + pub external_deadman: Duration, + /// The most a single external target may move in one tick, radians per joint — + /// [`Safety::apply_external`]'s rate limit. The policy path is inherently smooth; raw + /// external targets are not, so a single bad frame is bounded to this rather than being + /// allowed to snap a joint across its travel. ~0.2 rad/tick is ~10 rad/s at 50 Hz, above any + /// gait and well below a hardware slam. + pub external_max_step: f64, /// Gain while running. pub gain_running: u16, /// Gain to yield at rather than fight the floor. Nothing here applies it — `robotd` @@ -72,6 +125,8 @@ impl Default for SafetyConfig { fall_gravity_z: -0.5, fall_debounce: Duration::from_millis(200), deadman: Duration::from_millis(500), + external_deadman: Duration::from_millis(150), + external_max_step: 0.2, gain_running: 200, gain_limp: 50, } @@ -82,10 +137,15 @@ impl Default for SafetyConfig { /// rather than watching the robot ignore it. #[derive(Debug, Clone, Copy, PartialEq, Eq)] pub enum Limit { - /// Intents went stale; the velocity was zeroed. + /// Intents went stale; the velocity was zeroed (or an external stream stopped and the + /// robot was held and dropped toward limp — see [`Safety::apply_external`]). Deadman, - /// A target was outside the actuator's travel. + /// A target was outside its bound — the actuator's travel on the policy path, or a joint's + /// anatomical limit ([`ANATOMICAL_LIMITS`]) on the external path. Range, + /// An external target moved more in one tick than [`SafetyConfig::external_max_step`] + /// allows, and was rate-limited toward it. + Step, /// A target was `NaN` or infinite. NotFinite, } @@ -111,6 +171,11 @@ pub struct Safety { /// Tracks the last gain written so an unchanged one is not rewritten every tick — that /// would be fifteen bus writes per tick for no reason. gain: Option, + /// The last external target actually written, for [`Self::apply_external`]'s per-tick step + /// clamp. `None` before the first external command and after [`Self::clear_external`], so + /// the first frame of a stream rate-limits from the pose the robot is already in rather than + /// from a stale one. + last_external: Option<[f64; NUM_JOINTS]>, } impl Safety { @@ -121,6 +186,7 @@ impl Safety { falling_for: Duration::ZERO, fallen: false, gain: None, + last_external: None, } } @@ -278,6 +344,105 @@ impl Safety { Ok(()) } + /// Apply a streamed external joint command — the sink for `robot.setJoints`. + /// + /// The same chokepoint as [`Self::apply`], with the guarantees raw off-robot targets need + /// on top of the ones the policy path already has, in order: + /// + /// 1. **External deadman.** If `external_age` exceeds [`SafetyConfig::external_deadman`], + /// the stream is treated as dropped: the robot is commanded to `hold` and dropped to the + /// limp gain, reported as [`Limit::Deadman`]. A stalled off-robot controller must not + /// leave a live joint command. + /// 2. **Non-finite refusal.** A `NaN`/inf anywhere in the vector refuses the whole frame and + /// holds — identical to [`Self::apply`], never a clamp to a plausible-looking boundary. + /// 3. **Per-joint anatomical clamp** to [`ANATOMICAL_LIMITS`], reported as [`Limit::Range`] — + /// the bound the actuator-travel clamp cannot give, and the reason external write is safe. + /// 4. **Per-tick step clamp** to [`SafetyConfig::external_max_step`], reported as + /// [`Limit::Step`], measured against the last external target actually written (or `hold` + /// on the first frame), so one bad frame cannot snap a joint. + /// + /// The conditioned targets are then written through [`Self::apply`], so the actuator-travel + /// clamp, the gain write and the single motor-write path are exactly the ones every other + /// command goes through — nothing here reaches a servo on its own. + /// + /// `running_gain` is what to hold the targets at while the stream is live (the mode's gain, or + /// a per-frame `gain` the client asked for). `hold` is the fallback pose used when the stream + /// is stale or refused, normally the pose the robot is already in. + pub fn apply_external( + &mut self, + targets: [f64; NUM_JOINTS], + hold: [f64; NUM_JOINTS], + external_age: Duration, + running_gain: u16, + ) -> Result { + // 1. External deadman: a dropped controller must not leave a live command. Hold the + // fallback pose and drop toward limp — the joint equivalent of the twist deadman + // zeroing a velocity, and stricter, because a stale *position* is a held command. + if external_age > self.config.external_deadman { + let mut applied = self.apply(hold, hold, self.config.gain_limp)?; + if !applied.limited_by(Limit::Deadman) { + applied.limits.push(Limit::Deadman); + } + // The baseline for a resumed stream is the pose we are now holding, not a stale + // pre-dropout target. + self.last_external = Some(hold); + return Ok(applied); + } + + // 2. Non-finite is refused outright, before any clamp — delegating to `apply` so the + // refusal-and-hold behaviour is defined in exactly one place. + if targets.iter().any(|v| !v.is_finite()) { + let applied = self.apply(targets, hold, running_gain)?; + self.last_external = Some(hold); + return Ok(applied); + } + + let mut applied = Applied::default(); + let baseline = self.last_external.unwrap_or(hold); + let mut safe = targets; + + for (j, value) in safe.iter_mut().enumerate() { + // 3. Anatomical clamp — the per-joint bound ±π cannot express. + let (lo, hi) = ANATOMICAL_LIMITS[j]; + let clamped = value.clamp(lo, hi); + if clamped != *value { + if !applied.limited_by(Limit::Range) { + applied.limits.push(Limit::Range); + } + *value = clamped; + } + + // 4. Per-tick step clamp against the last written external target. + let step = *value - baseline[j]; + if step.abs() > self.config.external_max_step { + *value = baseline[j] + self.config.external_max_step.copysign(step); + if !applied.limited_by(Limit::Step) { + applied.limits.push(Limit::Step); + } + } + } + + self.last_external = Some(safe); + + // Write through the ordinary chokepoint. The actuator clamp there is a no-op given the + // anatomical bounds already sit inside ±π, but the single write path and the gain-change + // bookkeeping are what we want to share rather than duplicate. + let inner = self.apply(safe, hold, running_gain)?; + for limit in inner.limits { + if !applied.limits.contains(&limit) { + applied.limits.push(limit); + } + } + Ok(applied) + } + + /// Forget the last external target, so the next [`Self::apply_external`] rate-limits from the + /// robot's current pose rather than a target from a previous session. `robotd` calls this when + /// it leaves the External drive mode, so switching back to a policy leaves no residual command. + pub fn clear_external(&mut self) { + self.last_external = None; + } + /// Borrow the wrapped IO. Test-only, and deliberately not public: handing this out in /// production would defeat the point of safety owning the writer. #[cfg(test)] @@ -577,4 +742,210 @@ mod tests { assert_eq!(s.io().last_gain, Some(SafetyConfig::default().gain_running)); assert_eq!(s.gain, Some(SafetyConfig::default().gain_running)); } + + // ── external per-joint write (robot.setJoints) ─────────────────────────── + + /// A fresh, in-range, small-step external command goes straight through: the targets are + /// written unchanged and nothing is reported. Any clamp firing here would mean the whole + /// external path is mangling ordinary commands. + #[test] + fn a_fresh_external_command_passes_through() { + let mut s = safety(); + // Seed the step baseline so the first frame is not itself rate-limited from home. + s.clear_external(); + // A tiny nudge from the home pose — well inside every limit and the per-tick step. + let mut wanted = DEFAULT_POSITION; + wanted[0] += 0.05; + // First frame rate-limits from `hold` (= wanted's neighbour, DEFAULT_POSITION here). + let applied = s + .apply_external( + wanted, + DEFAULT_POSITION, + Duration::from_millis(20), + SafetyConfig::default().gain_running, + ) + .unwrap(); + assert!(applied.limits.is_empty(), "{:?}", applied.limits); + assert_eq!(s.io().last_written.unwrap().positions, wanted); + assert_eq!(s.io().last_gain, Some(SafetyConfig::default().gain_running)); + } + + /// A `NaN` in an external vector is refused outright and the robot holds — never clamped to + /// a boundary, exactly as on the policy path. + #[test] + fn a_non_finite_external_target_is_refused_not_clamped() { + let mut s = safety(); + let mut poisoned = DEFAULT_POSITION; + poisoned[4] = f64::INFINITY; + let applied = s + .apply_external( + poisoned, + DEFAULT_POSITION, + Duration::from_millis(20), + SafetyConfig::default().gain_running, + ) + .unwrap(); + assert!(applied.limited_by(Limit::NotFinite)); + assert!(!applied.limited_by(Limit::Range), "must not be clamped"); + assert_eq!(s.io().last_written.unwrap().positions, DEFAULT_POSITION); + } + + /// A target outside a joint's anatomical limit is clamped to that limit — the bound the + /// actuator-travel clamp (±π) cannot give — and reported. + #[test] + fn an_out_of_limit_external_target_is_clamped_to_the_joint_bound() { + // A generous per-tick step budget isolates the anatomical clamp from the step clamp: + // left_knee (index 3) limit is [-1.80, 0.20], right_hip_pitch (12) is [-0.70, 1.60]. + let cfg = SafetyConfig { + external_max_step: 100.0, + ..SafetyConfig::default() + }; + let mut s = Safety::new(FakeIo::at(DEFAULT_POSITION), cfg); + + let mut wild = DEFAULT_POSITION; + wild[3] = 2.0; // well past the +0.20 knee limit + wild[12] = -2.0; // right_hip_pitch limit is [-0.70, 1.60]; well past the -0.70 bound + let applied = s + .apply_external(wild, DEFAULT_POSITION, Duration::from_millis(20), cfg.gain_running) + .unwrap(); + + assert!(applied.limited_by(Limit::Range)); + let written = s.io().last_written.unwrap().positions; + assert_eq!(written[3], ANATOMICAL_LIMITS[3].1, "clamped to the knee's max"); + assert_eq!( + written[12], ANATOMICAL_LIMITS[12].0, + "clamped to the hip pitch min" + ); + } + + /// A target that jumps further than the per-tick step allows is rate-limited toward it, + /// not applied whole — one bad frame cannot snap a joint across its travel. + #[test] + fn an_over_fast_external_step_is_rate_limited() { + let cfg = SafetyConfig::default(); + let mut s = Safety::new(FakeIo::at(DEFAULT_POSITION), cfg); + let max = cfg.external_max_step; + + // First frame establishes the baseline at (near) the home pose without tripping the + // step clamp: ask for exactly one max-step move on joint 0. + let mut f1 = DEFAULT_POSITION; + f1[0] = DEFAULT_POSITION[0] + max; + let a1 = s + .apply_external(f1, DEFAULT_POSITION, Duration::from_millis(20), cfg.gain_running) + .unwrap(); + assert!(!a1.limited_by(Limit::Step), "exactly max is allowed"); + let after_first = s.io().last_written.unwrap().positions[0]; + + // Second frame demands a jump ten times the budget; only one step of it lands. + let mut f2 = DEFAULT_POSITION; + f2[0] = after_first + 10.0 * max; + let a2 = s + .apply_external(f2, DEFAULT_POSITION, Duration::from_millis(20), cfg.gain_running) + .unwrap(); + assert!(a2.limited_by(Limit::Step)); + let written = s.io().last_written.unwrap().positions[0]; + assert!( + (written - (after_first + max)).abs() < 1e-9, + "expected one step of {max} from {after_first}, got {written}" + ); + } + + /// A stalled off-robot controller — external targets older than the external deadman — must + /// not leave a live command: the robot holds the fallback pose and drops toward the limp gain. + #[test] + fn stale_external_targets_hold_and_go_limp() { + let cfg = SafetyConfig::default(); + let mut s = Safety::new(FakeIo::at(DEFAULT_POSITION), cfg); + + // A perfectly reasonable target, but it arrived too long ago. + let mut wanted = DEFAULT_POSITION; + wanted[0] = 0.3; + let applied = s + .apply_external( + wanted, + DEFAULT_POSITION, + cfg.external_deadman + Duration::from_millis(1), + cfg.gain_running, + ) + .unwrap(); + + assert!(applied.limited_by(Limit::Deadman)); + assert_eq!( + s.io().last_written.unwrap().positions, + DEFAULT_POSITION, + "a dropped stream holds the fallback pose, not the stale target" + ); + assert_eq!( + s.io().last_gain, + Some(cfg.gain_limp), + "and drops toward the limp gain" + ); + } + + /// The external deadman is genuinely tighter than the twist deadman — a gap that is fine for + /// a held gamepad stick already drops a streamed joint command. + #[test] + fn the_external_deadman_is_tighter_than_the_twist_deadman() { + let cfg = SafetyConfig::default(); + assert!(cfg.external_deadman < cfg.deadman); + let mut s = Safety::new(FakeIo::at(DEFAULT_POSITION), cfg); + + // Older than the external deadman, younger than the twist deadman. + let age = (cfg.external_deadman + cfg.deadman) / 2; + let mut wanted = DEFAULT_POSITION; + wanted[0] = 0.3; + let applied = s + .apply_external(wanted, DEFAULT_POSITION, age, cfg.gain_running) + .unwrap(); + assert!( + applied.limited_by(Limit::Deadman), + "an age the twist deadman would pass must still drop a joint stream" + ); + } + + /// `clear_external` drops the step baseline, so re-entering the External path rate-limits + /// from the robot's current pose rather than a target from a previous session. + #[test] + fn clearing_external_resets_the_step_baseline() { + let cfg = SafetyConfig::default(); + let mut s = Safety::new(FakeIo::at(DEFAULT_POSITION), cfg); + let max = cfg.external_max_step; + + // Drive the baseline far from home over several ticks. + let mut far = DEFAULT_POSITION; + far[0] = DEFAULT_POSITION[0] + 5.0 * max; + for _ in 0..20 { + s.apply_external(far, far, Duration::from_millis(20), cfg.gain_running) + .unwrap(); + } + + // Leaving External and coming back must forget that baseline. + s.clear_external(); + let mut home = DEFAULT_POSITION; + home[0] = DEFAULT_POSITION[0] + max; // one step from home + let applied = s + .apply_external(home, DEFAULT_POSITION, Duration::from_millis(20), cfg.gain_running) + .unwrap(); + assert!( + !applied.limited_by(Limit::Step), + "a fresh session rate-limits from the current pose, so one step is allowed" + ); + } + + /// The limit table must contain the home pose: a bound that clamped `DEFAULT_POSITION` would + /// mean the robot could not even be commanded to stand at home through the external path. + #[test] + fn the_home_pose_is_inside_the_anatomical_limits() { + for (j, &(lo, hi)) in ANATOMICAL_LIMITS.iter().enumerate() { + assert!(lo < hi, "joint {j}: empty limit [{lo}, {hi}]"); + assert!( + DEFAULT_POSITION[j] >= lo && DEFAULT_POSITION[j] <= hi, + "home pose {} for joint {j} is outside [{lo}, {hi}]", + DEFAULT_POSITION[j] + ); + // And every anatomical bound sits inside the actuator travel, or the clamp order + // in `apply_external` would be wrong. + assert!(lo >= ACTUATOR_MIN && hi <= ACTUATOR_MAX, "joint {j} exceeds actuator travel"); + } + } } diff --git a/robotd/src/main.rs b/robotd/src/main.rs index aa7c46bc..078a7eac 100644 --- a/robotd/src/main.rs +++ b/robotd/src/main.rs @@ -1142,6 +1142,9 @@ async fn control_loop( deadman: Duration::from_millis(params.safety.deadman_ms), gain_running: policy_cfg.gain, gain_limp: params.safety.gain_limp, + // The external per-joint stream's deadman and step clamp default here; wiring them + // to dedicated params is a `robotd-params` follow-up (see the RFC's External mode). + ..SafetyConfig::default() }, ); @@ -2352,6 +2355,7 @@ fn limit_name(limit: duck_control::safety::Limit) -> &'static str { match limit { Limit::Deadman => "deadman", Limit::Range => "joint_range", + Limit::Step => "external_step", Limit::NotFinite => "not_finite", } } From d30573984092cc94b639ac18d23b7edec7ca9d93 Mon Sep 17 00:00:00 2001 From: cagataycali Date: Thu, 27 Aug 2026 19:31:36 -0400 Subject: [PATCH 4/7] feat(robotd): External drive path in the control Runtime (Step::external) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The RFC's 'Runtime' is robotd's Controller (robotd/src/control.rs), which turns a Command into a Step of joint targets. Add the External path as Step::external: in External mode the whole policy stage (observe -> infer -> scatter -> scale -> low-pass) is skipped and the tick IS the off-robot controller's supplied targets, carried through unchanged for the safety chokepoint (Safety::apply_external) to clamp downstream. Labelled 'external', carries the caller's gain, busy=false; holds no IO handle like every producer here. main.rs wires it in Step 4 (hence a temporary #[allow(dead_code)]). Also: adding Call::RobotSetJoints in Step 1 broke updater's exhaustive dispatch match — add it to the 'robot.* is served by robotd, not updaterd' group so the robotd build (which depends on updater) compiles. robotd builds AND tests on macOS: cargo test -p robotd control:: green (4), including the_external_branch_returns_the_supplied_targets. (mediad/btd/padd route tables also match Call exhaustively and are updated in the workspace-green pass; they are not in robotd's dependency graph.) --- robotd/src/control.rs | 54 ++++++++++++++++++++++++++++++++++++++++++- updater/src/ipc.rs | 1 + 2 files changed, 54 insertions(+), 1 deletion(-) diff --git a/robotd/src/control.rs b/robotd/src/control.rs index bd258ddd..0367fa30 100644 --- a/robotd/src/control.rs +++ b/robotd/src/control.rs @@ -120,7 +120,8 @@ impl Default for SkillTuning { pub struct Step { pub targets: [f64; NUM_JOINTS], /// Which network drove, as the wire label: `walk`, `stand`, `ground_pick`, `kick_left`, - /// `kick_right`, `sit`, `rise`. + /// `kick_right`, `sit`, `rise` — or `external` when an off-robot controller is driving the + /// joints directly ([`Step::external`]). pub label: &'static str, /// What the gain should be for this tick. pub gain: u16, @@ -129,6 +130,34 @@ pub struct Step { pub busy: bool, } +impl Step { + /// The **External** drive path: joint targets streamed by an off-robot controller (via + /// `robot.setJoints`) in place of the on-device policy's output. + /// + /// External mode skips the whole policy stage — `observe → infer → scatter → scale → + /// low-pass` — so there is nothing to run here: the tick *is* the supplied targets. They are + /// carried straight to the safety layer's external chokepoint, + /// [`duck_control::safety::Safety::apply_external`], which owns the per-joint anatomical + /// limits, the per-tick step clamp and the dedicated external deadman. Nothing is clamped + /// here — this is a decision object, and like every other producer in this module it holds + /// no IO handle: it proposes targets, it does not command a motor. + /// + /// `busy` is false: External is a drive mode a client enters and leaves deliberately, not a + /// scripted move mid-flight, so it does not block a restart the way a roulade does — the mode + /// gate, not this flag, is what refuses `robot.setJoints` outside External. + // Wired into the control loop by the robotd External-mode branch (RFC step 4); until that + // lands the only caller is the test below, so the loader is the sole user this build sees. + #[allow(dead_code)] + pub fn external(targets: [f64; NUM_JOINTS], gain: u16) -> Self { + Self { + targets, + label: "external", + gain, + busy: false, + } + } +} + /// Where the robot is in the sit↔stand cycle. #[derive(Debug, Clone, Copy, PartialEq)] enum Sit { @@ -535,4 +564,27 @@ mod tests { assert_eq!(GROUND_PICK_END_PHASE, 0.7); assert_eq!(RISE_SECS, 1.0); } + + /// The External branch produces exactly the supplied targets — the whole point of a + /// passthrough is that it does not reinterpret them. The clamping (anatomical limits, the + /// per-tick step, the deadman) is the safety layer's job downstream, verified there; here + /// what matters is that the Runtime hands the joint vector through untouched, labels the + /// tick `external`, carries the caller's gain, and is not `busy`. + /// + /// Tested via `Step::external` rather than a full `Controller`, because constructing a + /// `Controller` needs a loaded `Policy` (ONNX Runtime + a model file), which the External + /// path deliberately never touches — skipping the policy stage is the point of the mode. + #[test] + fn the_external_branch_returns_the_supplied_targets() { + let mut targets = DEFAULT_POSITION; + targets[0] = 0.3; + targets[duck_control::model::MOUTH_INDEX] = 0.2; + + let step = Step::external(targets, 175); + + assert_eq!(step.targets, targets, "the joint vector passes through unchanged"); + assert_eq!(step.label, "external"); + assert_eq!(step.gain, 175); + assert!(!step.busy, "External is a mode, not a scripted move mid-flight"); + } } diff --git a/updater/src/ipc.rs b/updater/src/ipc.rs index 1f04de33..69b43d79 100644 --- a/updater/src/ipc.rs +++ b/updater/src/ipc.rs @@ -630,6 +630,7 @@ impl Server { | Call::RobotLook(_) | Call::RobotStop | Call::RobotEnable(_) + | Call::RobotSetJoints(_) | Call::RobotInit | Call::RobotRelax | Call::RobotDo(_) From 2caf932000d85767f3ecf08ffdffe6ff097e7a54 Mon Sep 17 00:00:00 2001 From: cagataycali Date: Thu, 27 Aug 2026 19:41:29 -0400 Subject: [PATCH 5/7] =?UTF-8?q?feat(robotd):=20External=20drive=20mode=20?= =?UTF-8?q?=E2=80=94=20intents=20slot=20+=20IPC=20surface=20(Step=204a)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The External drive plumbing, minus the control-loop branch (Step 4b): intents.rs: a stamped external-target slot on its OWN clock plus an external_active flag, so streaming joints never touches the twist deadman and vice versa (the deadman-critical invariant). set_external (used by setJoints, engages the mode implicitly), set_external_mode (setMode external / handback), external_engaged, and Snapshot.external: Option {targets, age, gain}. request_relax now also leaves External. 8 unit tests: independent clock, implicit engage, gain carry, clear on leave/relax. main.rs dispatch: - robot.setJoints: the External door. Refuses (named reason, touches nothing) on wrong arity, non-finite target, or a robot not powered+homed ('call robot.init first'); otherwise stores the frame and enters External. The safety envelope (limits/step/deadman) stays Safety::apply_external's job. - robot.setMode 'external' engages the mode (no policy needed, no walk/roller switch); walk/roller leave External and hand back to the policy. - robot.mode reports 'external' while engaged. - robot.enable(on) leaves External (policy takes back over). setMode refusal message now lists external. New dispatch test gates the setJoints door end to end; setMode test covers external engage/handback. Loop consumption is Step 4b (transient #[allow(dead_code)] on Snapshot.external + Step::external until then). cargo test -p robotd green (103 + 7 integ), clippy clean. --- robotd/src/intents.rs | 176 +++++++++++++++++++++++++++++++++++++++++ robotd/src/main.rs | 179 ++++++++++++++++++++++++++++++++++++++---- 2 files changed, 339 insertions(+), 16 deletions(-) diff --git a/robotd/src/intents.rs b/robotd/src/intents.rs index a0c28936..25c6b5a1 100644 --- a/robotd/src/intents.rs +++ b/robotd/src/intents.rs @@ -37,6 +37,7 @@ const POWER_RELAX: u8 = 2; use std::time::{Duration, Instant}; use arc_swap::ArcSwap; +use duck_control::NUM_JOINTS; use duck_control::obs::{BodyPose, Command}; /// A value and when it arrived. @@ -66,6 +67,30 @@ impl Default for PoseIntent { } } +/// External per-joint targets, as they sit in [`Intents`] — the raw payload of +/// `robot.setJoints`, kept on its own [`Stamped`] clock (see [`Intents::external`]). +#[derive(Debug, Clone, Copy, PartialEq)] +struct ExternalTargets { + /// Absolute joint angles, radians, in `JOINT_NAMES` order. + targets: [f64; NUM_JOINTS], + /// The hold gain the client asked for, or `None` to use the mode's running gain. + gain: Option, +} + +/// The External drive intent, as the loop consumes it: the streamed joint targets, how old +/// the newest frame is (on the external clock, *not* the twist deadman's), and the gain to +/// hold them at. Present in a [`Snapshot`] only while External mode is engaged. +#[derive(Debug, Clone, Copy, PartialEq)] +pub struct ExternalDrive { + pub targets: [f64; NUM_JOINTS], + /// Age of the newest external frame, for [`duck_control::safety::Safety::apply_external`]'s + /// dedicated external deadman. Measured on the external slot's own stamp, so streaming + /// joints never refreshes — nor is refreshed by — the twist's deadman clock. + pub age: Duration, + /// The per-frame hold gain, or `None` for the mode default. + pub gain: Option, +} + /// Pending one-shot skill requests, taken once per tick. /// /// Booleans rather than a queue: within one 20 ms tick a second press of the same button @@ -168,6 +193,13 @@ pub struct Intents { /// The wheee hold, as a stamped level: `padd` re-notifies while the trigger is down, /// and the loop reads value + age so a dead client's ride decays instead of looping. wheee: ArcSwap>, + /// The newest external joint targets (`robot.setJoints`), on their own stamp — see + /// [`ExternalDrive::age`] for why the clock is separate from the twist's. + external: ArcSwap>, + /// Whether the External drive mode is engaged. Set by `robot.setJoints` (which enters the + /// mode implicitly) and by `robot.setMode "external"`; cleared when the policy is handed + /// back control (`robot.setMode walk|roller`, `robot.enable on`, `robot.relax`). + external_active: AtomicBool, } /// What a client asked for, once. @@ -192,6 +224,13 @@ pub struct Snapshot { pub pose: PoseIntent, /// Mouth opening, 0..1. pub mouth: f64, + /// The External drive intent, present only while External mode is engaged. When `Some`, + /// the control loop drives these joint targets in place of the policy, through + /// [`duck_control::safety::Safety::apply_external`]. + // Consumed by the control loop's External branch (RFC step 4b); until that lands the field + // is written by `snapshot` but not yet read, so the loader is the sole writer this build sees. + #[allow(dead_code)] + pub external: Option, } impl Default for Intents { @@ -232,6 +271,17 @@ impl Intents { value: false, at_us: 0, }), + // Stamped at zero so, before any frame arrives, an engaged External mode reads as + // maximally stale and the external deadman holds the robot rather than driving it + // to a zero pose. + external: ArcSwap::from_pointee(Stamped { + value: ExternalTargets { + targets: [0.0; NUM_JOINTS], + gain: None, + }, + at_us: 0, + }), + external_active: AtomicBool::new(false), } } @@ -349,6 +399,32 @@ impl Intents { self.mode_switch.store(code, Ordering::Relaxed); } + /// Store the newest external joint targets and enter External drive. + /// + /// `robot.setJoints`' sink. Stamps on the external clock — never the twist's — and sets + /// the mode active, so a single `setJoints` both supplies a frame and takes the joints + /// from the policy (the RFC's "setJoints implicitly enables External"). `gain` is the + /// per-frame hold gain, `None` for the mode default. + pub fn set_external(&self, targets: [f64; NUM_JOINTS], gain: Option) { + self.external.store(Arc::new(Stamped { + value: ExternalTargets { targets, gain }, + at_us: self.now_us(), + })); + self.external_active.store(true, Ordering::Relaxed); + } + + /// Engage or leave External drive without supplying a frame — `robot.setMode "external"` + /// on, and the policy-handback paths off. Engaging with no frame yet is safe: the newest + /// external stamp stays old, so the external deadman holds the robot until targets arrive. + pub fn set_external_mode(&self, on: bool) { + self.external_active.store(on, Ordering::Relaxed); + } + + /// Whether External drive is engaged — what `robot.mode` reports as `external`. + pub fn external_engaged(&self) -> bool { + self.external_active.load(Ordering::Relaxed) + } + /// Take a pending mode switch. Taken, so the sequence runs once per request. pub fn take_mode_switch(&self) -> Option { match self.mode_switch.swap(MODE_NONE, Ordering::Relaxed) { @@ -388,6 +464,9 @@ impl Intents { /// keep driving, and leaving that flag set would have the next tick bring it straight back up. pub fn request_relax(&self) { self.enabled.store(false, Ordering::Relaxed); + // A robot asked to go limp is not being driven by anyone — leave External too, so the + // next frame does not silently resume it and torque comes back up under a stale target. + self.external_active.store(false, Ordering::Relaxed); self.power.store(POWER_RELAX, Ordering::Relaxed); } @@ -469,6 +548,16 @@ impl Intents { let twist = self.twist.load(); let head = self.head.load(); let pose = **self.pose.load(); + let external = if self.external_active.load(Ordering::Relaxed) { + let e = self.external.load(); + Some(ExternalDrive { + targets: e.value.targets, + age: Duration::from_micros(now.saturating_sub(e.at_us)), + gain: e.value.gain, + }) + } else { + None + }; Snapshot { command: Command { twist: twist.value, @@ -482,6 +571,7 @@ impl Intents { enabled: self.enabled.load(Ordering::Relaxed), pose, mouth: f64::from_bits(self.mouth.load(std::sync::atomic::Ordering::Relaxed)), + external, } } } @@ -598,4 +688,90 @@ mod tests { intents.set_twist([1.0, 0.0, 0.0]); assert_eq!(intents.snapshot().command.body, BodyPose::default()); } + + // ── external drive (robot.setJoints) ───────────────────────────────────── + + /// Nothing is external until something asks: a fresh `Intents` has no external drive. + #[test] + fn there_is_no_external_drive_until_asked() { + let intents = Intents::new(); + assert!(!intents.external_engaged()); + assert!(intents.snapshot().external.is_none()); + } + + /// `set_external` supplies the targets, records the gain and engages the mode in one step — + /// the RFC's "setJoints implicitly enables External". + #[test] + fn setting_external_targets_engages_the_mode_and_snapshots_them() { + let intents = Intents::new(); + let mut targets = [0.0; NUM_JOINTS]; + targets[0] = 0.3; + targets[NUM_JOINTS - 1] = -0.2; + + intents.set_external(targets, Some(175)); + assert!(intents.external_engaged()); + + let ext = intents.snapshot().external.expect("external is engaged"); + assert_eq!(ext.targets, targets); + assert_eq!(ext.gain, Some(175)); + } + + /// The external clock is the deadman-critical invariant: streaming joints must not refresh + /// the twist's deadman (a stalled controller cannot mask itself by looking like a live + /// gamepad), and driving the twist must not refresh the external frame's age. + #[test] + fn the_external_clock_is_independent_of_the_twist_deadman() { + let intents = Intents::new(); + + // Age the twist, then write an external frame. + intents.set_twist([0.5, 0.0, 0.0]); + std::thread::sleep(Duration::from_millis(10)); + intents.set_external([0.0; NUM_JOINTS], None); + + let snap = intents.snapshot(); + assert!( + snap.twist_age >= Duration::from_millis(10), + "an external write must not refresh the twist's deadman clock" + ); + let ext = snap.external.expect("engaged"); + assert!( + ext.age < snap.twist_age, + "the fresh external frame must be younger than the aged twist" + ); + + // And the reverse: a twist write does not touch the external frame's age. + std::thread::sleep(Duration::from_millis(10)); + intents.set_twist([0.1, 0.0, 0.0]); + let ext_after = intents.snapshot().external.expect("still engaged"); + assert!( + ext_after.age >= Duration::from_millis(10), + "a twist write must not refresh the external frame's clock" + ); + } + + /// Leaving External mode removes it from the snapshot at once — the loop's cue to hand the + /// joints back to the policy. + #[test] + fn leaving_external_mode_clears_the_snapshot() { + let intents = Intents::new(); + intents.set_external([0.0; NUM_JOINTS], None); + assert!(intents.snapshot().external.is_some()); + + intents.set_external_mode(false); + assert!(!intents.external_engaged()); + assert!(intents.snapshot().external.is_none()); + } + + /// `robot.relax` leaves External: a robot told to go limp is not being driven, and the mode + /// must not silently resume torque under a stale target on the next frame. + #[test] + fn relax_leaves_external() { + let intents = Intents::new(); + intents.set_external([0.0; NUM_JOINTS], Some(200)); + assert!(intents.external_engaged()); + + intents.request_relax(); + assert!(!intents.external_engaged(), "relax must leave External"); + assert!(intents.snapshot().external.is_none()); + } } diff --git a/robotd/src/main.rs b/robotd/src/main.rs index 078a7eac..5252240a 100644 --- a/robotd/src/main.rs +++ b/robotd/src/main.rs @@ -2813,19 +2813,35 @@ fn dispatch( // state and that state is what they get, and a pad held a beat too long should not report // an error. proto::Call::RobotSetMode(p) => { - let target = match p.mode.as_str() { - "walk" => Some(Mode::Walk), - "roller" => Some(Mode::Roller), - _ => None, - }; - let result = match target { - None => proto::IntentResult::refused("mode must be \"walk\" or \"roller\""), - Some(_) if state.policies.load().walk.is_none() => proto::IntentResult::refused( - "no policy on this robot, so there is nothing to switch between", - ), - Some(mode) => { - intents.request_mode_switch(mode_code(mode)); - proto::IntentResult::accepted() + // "external" is a drive *source*, not a policy mode: it hands the joints to an + // off-robot controller via robot.setJoints, needs no policy loaded, and does not + // touch the walk/roller mode machinery. walk/roller leave External and hand the + // joints back to the policy. + let result = if p.mode.as_str() == "external" { + intents.set_external_mode(true); + proto::IntentResult::accepted() + } else { + let target = match p.mode.as_str() { + "walk" => Some(Mode::Walk), + "roller" => Some(Mode::Roller), + _ => None, + }; + match target { + None => proto::IntentResult::refused( + "mode must be \"walk\", \"roller\" or \"external\"", + ), + Some(_) if state.policies.load().walk.is_none() => { + proto::IntentResult::refused( + "no policy on this robot, so there is nothing to switch between", + ) + } + Some(mode) => { + // Handing back to the policy: leave External first, so the loop stops + // driving external targets the moment the switch is requested. + intents.set_external_mode(false); + intents.request_mode_switch(mode_code(mode)); + proto::IntentResult::accepted() + } } }; proto::Response::ok(Some(id), &result) @@ -2834,9 +2850,15 @@ fn dispatch( proto::Call::RobotMode => proto::Response::ok( Some(id), &proto::ModeResult { - mode: mode_of(state.mode.load(Ordering::Relaxed)) - .as_str() - .to_owned(), + // External is a drive source layered over the policy mode: while it is engaged + // it is what is actually driving the joints, so it is what `robot.mode` reports. + mode: if intents.external_engaged() { + "external".to_owned() + } else { + mode_of(state.mode.load(Ordering::Relaxed)) + .as_str() + .to_owned() + }, }, ), @@ -2925,6 +2947,41 @@ fn dispatch( proto::Response::ok(Some(id), &proto::IntentResult::accepted()) } + // Stream absolute joint targets from an off-robot controller. Accepting a frame enters + // External drive implicitly (the RFC's `setMode external` is the explicit door). Refused, + // with a named reason, when the request cannot be honoured safely: + // - the wrong number of targets — the vector is positional (`JOINT_NAMES` order), so a + // short or long one is a client bug, refused rather than zero-padded into a lurch; + // - a non-finite target — refused here for a clear error, and again in the safety layer; + // - the joints not powered and at home (`homed`) — a limp robot cannot hold a commanded + // position, so streaming to it is a silent no-op dressed as motion; `robot.init` first. + // Everything the frame *can* violate once accepted — anatomical limits, per-tick step, + // the external deadman — is the safety layer's job (`Safety::apply_external`), not this + // door's. + proto::Call::RobotSetJoints(p) => { + use duck_control::NUM_JOINTS; + let result = if p.targets.len() != NUM_JOINTS { + proto::IntentResult::refused(format!( + "robot.setJoints needs exactly {NUM_JOINTS} joint targets in JOINT_NAMES \ + order, got {}", + p.targets.len() + )) + } else if p.targets.iter().any(|v| !v.is_finite()) { + proto::IntentResult::refused("robot.setJoints targets must all be finite") + } else if !state.homed.load(Ordering::Relaxed) { + proto::IntentResult::refused( + "robot.setJoints needs the joints powered and at the home pose — call \ + robot.init first (a limp robot cannot hold a commanded position)", + ) + } else { + let mut targets = [0.0f64; NUM_JOINTS]; + targets.copy_from_slice(&p.targets); + intents.set_external(targets, p.gain); + proto::IntentResult::accepted() + }; + proto::Response::ok(Some(id), &result) + } + // A refusal here is a normal answer with a reason, not an error: the client asked // something reasonable and the daemon declined. Gravity is never one of those // reasons — see below. @@ -2933,6 +2990,12 @@ fn dispatch( // in the client, because a client-side belief drifts (relax, shutdown, either // side restarting) and a stale one turns Start into a no-op every other press. let on = if p.toggle { !intents.enabled() } else { p.on }; + // Enabling the policy hands the joints back from any External drive — the two are + // alternatives, and a stale external target must not shadow the policy that is now + // asked to drive. + if on { + intents.set_external_mode(false); + } // Never refused for being down. Start on a robot lying on the floor is exactly // how someone asks it to stand back up, and it brings the robot up and hands it // to the standing policy like any other enable. @@ -3261,6 +3324,21 @@ mod tests { "a refused switch must not reach the loop" ); + // "external" is a drive source, not a policy mode: accepted, engages External, and + // does NOT queue a walk/roller mode switch. + assert!(set("external").accepted, "external is a valid mode"); + assert!(intents.external_engaged(), "external must engage the mode"); + assert_eq!( + intents.take_mode_switch(), + None, + "external is not a policy-mode switch" + ); + + // Switching back to a policy mode leaves External and hands the joints back. + assert!(set("walk").accepted); + assert!(!intents.external_engaged(), "walk must leave External"); + assert_eq!(intents.take_mode_switch(), Some(mode_code(Mode::Walk))); + // A robot with no policy at all: nothing to switch between, and saying so beats homing // the robot for a swap that would load nothing. let mut bare = Params::default(); @@ -3283,6 +3361,75 @@ mod tests { ); } + /// `robot.setJoints`: the dispatch door for the External stream. It engages External and + /// stores the frame only when the request is well-formed AND the robot can hold it (powered + /// and homed); otherwise it refuses with a named reason and touches nothing. Everything the + /// frame can violate once accepted is the safety layer's job, not this test's. + #[test] + fn robot_set_joints_gates_the_external_stream() { + use duck_control::model::{DEFAULT_POSITION, NUM_JOINTS}; + + let s = RobotState::new(&Params::default(), false, false); + let intents = Arc::new(Intents::new()); + let id = || proto::Id::Number(1); + let call = |targets: Vec, gain: Option| -> proto::IntentResult { + dispatch( + &s, + &intents, + id(), + &proto::Call::RobotSetJoints(proto::SetJointsParams { targets, gain }), + ) + .result_as() + .expect("IntentResult") + }; + + // A limp robot (not homed) refuses — a commanded position it cannot hold is a silent + // no-op, so say so rather than pretend to move. + assert!(!s.homed.load(Ordering::Relaxed)); + let limp = call(DEFAULT_POSITION.to_vec(), None); + assert!(!limp.accepted); + assert!( + limp.reason.unwrap_or_default().contains("robot.init"), + "the refusal must point at the fix" + ); + assert!( + !intents.external_engaged(), + "a refused frame must not engage External" + ); + + // Power and home it; now the door is open. + s.homed.store(true, Ordering::Relaxed); + + // Wrong arity is refused, with both counts, and still engages nothing. + let short = call(vec![0.0; NUM_JOINTS - 1], None); + assert!(!short.accepted); + let reason = short.reason.unwrap_or_default(); + assert!(reason.contains(&NUM_JOINTS.to_string()), "{reason}"); + assert!(!intents.external_engaged()); + + // A non-finite target is refused at the door too. + let mut poisoned = DEFAULT_POSITION.to_vec(); + poisoned[3] = f64::NAN; + let nan = call(poisoned, None); + assert!(!nan.accepted); + assert!(nan.reason.unwrap_or_default().contains("finite")); + assert!(!intents.external_engaged()); + + // A well-formed frame on a ready robot is accepted, engages External implicitly, and the + // targets + gain reach the snapshot the loop reads. + let mut targets = DEFAULT_POSITION; + targets[0] += 0.1; + let ok = call(targets.to_vec(), Some(180)); + assert!(ok.accepted, "a valid frame on a ready robot is accepted"); + assert!( + intents.external_engaged(), + "accepting a frame enters External implicitly" + ); + let ext = intents.snapshot().external.expect("External is engaged"); + assert_eq!(ext.targets, targets); + assert_eq!(ext.gain, Some(180)); + } + /// Roller mode has no standing network, and the published set must say so. /// /// This is the reason the names are swapped as a set rather than left at what startup From 7bd275ea7c85331bf6040df9fb88d4f2f9c3f3d5 Mon Sep 17 00:00:00 2001 From: cagataycali Date: Thu, 27 Aug 2026 19:55:23 -0400 Subject: [PATCH 6/7] feat(robotd): wire the External drive branch into the control loop (Step 4b) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The control loop now consumes Snapshot.external and drives it end to end. robotd/src/main.rs control loop: - Reads snapshot.external; external_drive = engaged AND powered+homed (Ready), not mid limp-fall, not powered off. The policy 'driving' now yields to it (&& external.is_none()), so a controller streaming joints supersedes the on-device policy for the same servos. - New match arm produces the tick straight from the supplied targets via control::Step::external (gain = client's per-frame ask, else the mode's running gain); moving=true so safeToRestart won't say yes mid-stream. - The apply site routes External through Safety::apply_external(targets, hold, age, gain) — anatomical clamp + per-tick step + external deadman — instead of apply(); everything else (policy/homing/limp-fall/hold) still uses apply. - On the falling edge of external_drive the loop calls safety.clear_external() so a later re-entry rate-limits from the current pose, not a stale target. New tokio loop test: powers+homes a robot, streams a head_yaw frame, asserts it reaches the bus through apply_external while un-commanded joints stay home (proving the external frame drives, not a policy). Transient #[allow(dead_code)] on Snapshot.external / Step::external removed — both are now consumed. Transport permission gates (both exhaustive by design, so the new Call variant forced a decision in each): - btd/src/route.rs: robot.setJoints => false. Raw per-joint streaming is the most direct motor control there is; refused over BLE for every reason robot.move is, and then some (20-byte, ~73s-late notification link). - mediad/src/route.rs: robot.setJoints => true. The WebRTC datachannel is the teleop transport External drive is for; the peer has the camera and the safety layer bounds every frame. Refused over BLE, permitted here. cargo test --workspace green; clippy --all-targets clean; fmt clean (folded in a rustfmt pass over the ANATOMICAL_LIMITS table from an earlier step). --- btd/src/route.rs | 7 +++ duck-control/src/safety.rs | 66 ++++++++++++++------- duck-ipc-proto/src/lib.rs | 1 - mediad/src/route.rs | 7 +++ robotd/src/control.rs | 13 +++-- robotd/src/intents.rs | 3 - robotd/src/main.rs | 115 ++++++++++++++++++++++++++++++++++++- 7 files changed, 181 insertions(+), 31 deletions(-) diff --git a/btd/src/route.rs b/btd/src/route.rs index ba02ec45..62d328af 100644 --- a/btd/src/route.rs +++ b/btd/src/route.rs @@ -205,6 +205,13 @@ fn permits(call: &proto::Call) -> bool { RobotMove(_) | RobotHead(_) | RobotLook(_) | RobotEnable(_) | RobotDo(_) | RobotPose(_) | RobotMouth(_) => false, + // Raw per-joint targets streamed from an off-robot controller (the External drive path). + // This is the most direct motor control there is — it bypasses the policy and commands the + // servos straight — so it is refused over BLE for every reason `robot.move` is, and then + // some: a 20-byte, ~73-s-late notification link is exactly the wrong transport for a joint + // stream. External drive belongs on the local socket / a trusted controller over WebRTC. + RobotSetJoints(_) => false, + // Harmless and rather charming from a phone — but it rides the same refusal as the // rest of robot.* until the app path exists to want it: opening one call to the // radio ahead of a client that can use it buys nothing and widens the surface. diff --git a/duck-control/src/safety.rs b/duck-control/src/safety.rs index a6c78a2b..2cfd183c 100644 --- a/duck-control/src/safety.rs +++ b/duck-control/src/safety.rs @@ -67,21 +67,21 @@ pub const ACTUATOR_MAX: f64 = std::f64::consts::PI; /// ([`crate::model::MOUTH_CLOSED`]..[`crate::model::MOUTH_OPEN`]); whether external write may /// move it at all is a masking decision `robotd` makes, not this table's. pub const ANATOMICAL_LIMITS: [(f64, f64); NUM_JOINTS] = [ - (-0.80, 0.80), // left_hip_yaw (home 0.0000) - (-0.90, 0.70), // left_hip_roll (home -0.0873) - (-1.60, 0.70), // left_hip_pitch (home -0.4579) - (-1.80, 0.20), // left_knee (home -0.0049) - (-0.60, 1.40), // left_ankle (home 0.4530) - (-0.70, 1.30), // neck_pitch (home 0.3491) - (-0.70, 1.30), // head_pitch (home 0.3491) - (-1.40, 1.40), // head_yaw (home 0.0000) - (-0.80, 0.80), // head_roll (home 0.0000) + (-0.80, 0.80), // left_hip_yaw (home 0.0000) + (-0.90, 0.70), // left_hip_roll (home -0.0873) + (-1.60, 0.70), // left_hip_pitch (home -0.4579) + (-1.80, 0.20), // left_knee (home -0.0049) + (-0.60, 1.40), // left_ankle (home 0.4530) + (-0.70, 1.30), // neck_pitch (home 0.3491) + (-0.70, 1.30), // head_pitch (home 0.3491) + (-1.40, 1.40), // head_yaw (home 0.0000) + (-0.80, 0.80), // head_roll (home 0.0000) (MOUTH_CLOSED, MOUTH_OPEN), // mouth (home 0.0000; -5°..+30°) - (-0.80, 0.80), // right_hip_yaw (home 0.0000) - (-0.70, 0.90), // right_hip_roll (home 0.0873) - (-0.70, 1.60), // right_hip_pitch(home 0.4579) - (-0.20, 1.80), // right_knee (home 0.0049) - (-1.40, 0.60), // right_ankle (home -0.4530) + (-0.80, 0.80), // right_hip_yaw (home 0.0000) + (-0.70, 0.90), // right_hip_roll (home 0.0873) + (-0.70, 1.60), // right_hip_pitch(home 0.4579) + (-0.20, 1.80), // right_knee (home 0.0049) + (-1.40, 0.60), // right_ankle (home -0.4530) ]; #[derive(Debug, Clone, Copy, PartialEq)] @@ -806,12 +806,20 @@ mod tests { wild[3] = 2.0; // well past the +0.20 knee limit wild[12] = -2.0; // right_hip_pitch limit is [-0.70, 1.60]; well past the -0.70 bound let applied = s - .apply_external(wild, DEFAULT_POSITION, Duration::from_millis(20), cfg.gain_running) + .apply_external( + wild, + DEFAULT_POSITION, + Duration::from_millis(20), + cfg.gain_running, + ) .unwrap(); assert!(applied.limited_by(Limit::Range)); let written = s.io().last_written.unwrap().positions; - assert_eq!(written[3], ANATOMICAL_LIMITS[3].1, "clamped to the knee's max"); + assert_eq!( + written[3], ANATOMICAL_LIMITS[3].1, + "clamped to the knee's max" + ); assert_eq!( written[12], ANATOMICAL_LIMITS[12].0, "clamped to the hip pitch min" @@ -831,7 +839,12 @@ mod tests { let mut f1 = DEFAULT_POSITION; f1[0] = DEFAULT_POSITION[0] + max; let a1 = s - .apply_external(f1, DEFAULT_POSITION, Duration::from_millis(20), cfg.gain_running) + .apply_external( + f1, + DEFAULT_POSITION, + Duration::from_millis(20), + cfg.gain_running, + ) .unwrap(); assert!(!a1.limited_by(Limit::Step), "exactly max is allowed"); let after_first = s.io().last_written.unwrap().positions[0]; @@ -840,7 +853,12 @@ mod tests { let mut f2 = DEFAULT_POSITION; f2[0] = after_first + 10.0 * max; let a2 = s - .apply_external(f2, DEFAULT_POSITION, Duration::from_millis(20), cfg.gain_running) + .apply_external( + f2, + DEFAULT_POSITION, + Duration::from_millis(20), + cfg.gain_running, + ) .unwrap(); assert!(a2.limited_by(Limit::Step)); let written = s.io().last_written.unwrap().positions[0]; @@ -924,7 +942,12 @@ mod tests { let mut home = DEFAULT_POSITION; home[0] = DEFAULT_POSITION[0] + max; // one step from home let applied = s - .apply_external(home, DEFAULT_POSITION, Duration::from_millis(20), cfg.gain_running) + .apply_external( + home, + DEFAULT_POSITION, + Duration::from_millis(20), + cfg.gain_running, + ) .unwrap(); assert!( !applied.limited_by(Limit::Step), @@ -945,7 +968,10 @@ mod tests { ); // And every anatomical bound sits inside the actuator travel, or the clamp order // in `apply_external` would be wrong. - assert!(lo >= ACTUATOR_MIN && hi <= ACTUATOR_MAX, "joint {j} exceeds actuator travel"); + assert!( + lo >= ACTUATOR_MIN && hi <= ACTUATOR_MAX, + "joint {j} exceeds actuator travel" + ); } } } diff --git a/duck-ipc-proto/src/lib.rs b/duck-ipc-proto/src/lib.rs index 17c136f6..a3f92e52 100644 --- a/duck-ipc-proto/src/lib.rs +++ b/duck-ipc-proto/src/lib.rs @@ -4042,7 +4042,6 @@ mod tests { assert!(Call::parse(method::ROBOT_SET_JOINTS, Some(&ok)).is_ok()); } - /// `from_dir` survives the wire, and only appears when it was asked for. /// /// The absence half is the load-bearing one. Every other client of this type — `btd` diff --git a/mediad/src/route.rs b/mediad/src/route.rs index b52f1db8..17ecafd6 100644 --- a/mediad/src/route.rs +++ b/mediad/src/route.rs @@ -61,6 +61,13 @@ fn permits(call: &proto::Call) -> bool { // transport". A datachannel is a control transport, so this is the transport those // refusals were pointing at. RobotMove(_) | RobotHead(_) | RobotLook(_) | RobotPose(_) | RobotMouth(_) => true, + // Raw per-joint targets from an off-robot controller — the External drive stream. This is + // exactly the case the doc's "the calls BLE refuses on capacity grounds are the ones this + // transport exists to carry" points at: a joint stream is high-rate teleop, and the + // datachannel is its transport. The peer holding the session has the camera — it is + // looking at the robot — and the safety layer (anatomical clamp, per-tick step, external + // deadman) bounds every frame regardless. Refused over BLE, permitted here. + RobotSetJoints(_) => true, // The theremin rides with the sounds: it is one, and a browser that can quack a duck // may pick its instrument up too. RobotDo(_) | RobotSound(_) | RobotTheremin(_) => true, diff --git a/robotd/src/control.rs b/robotd/src/control.rs index 0367fa30..5a45aa63 100644 --- a/robotd/src/control.rs +++ b/robotd/src/control.rs @@ -145,9 +145,6 @@ impl Step { /// `busy` is false: External is a drive mode a client enters and leaves deliberately, not a /// scripted move mid-flight, so it does not block a restart the way a roulade does — the mode /// gate, not this flag, is what refuses `robot.setJoints` outside External. - // Wired into the control loop by the robotd External-mode branch (RFC step 4); until that - // lands the only caller is the test below, so the loader is the sole user this build sees. - #[allow(dead_code)] pub fn external(targets: [f64; NUM_JOINTS], gain: u16) -> Self { Self { targets, @@ -582,9 +579,15 @@ mod tests { let step = Step::external(targets, 175); - assert_eq!(step.targets, targets, "the joint vector passes through unchanged"); + assert_eq!( + step.targets, targets, + "the joint vector passes through unchanged" + ); assert_eq!(step.label, "external"); assert_eq!(step.gain, 175); - assert!(!step.busy, "External is a mode, not a scripted move mid-flight"); + assert!( + !step.busy, + "External is a mode, not a scripted move mid-flight" + ); } } diff --git a/robotd/src/intents.rs b/robotd/src/intents.rs index 25c6b5a1..b620de06 100644 --- a/robotd/src/intents.rs +++ b/robotd/src/intents.rs @@ -227,9 +227,6 @@ pub struct Snapshot { /// The External drive intent, present only while External mode is engaged. When `Some`, /// the control loop drives these joint targets in place of the policy, through /// [`duck_control::safety::Safety::apply_external`]. - // Consumed by the control loop's External branch (RFC step 4b); until that lands the field - // is written by `snapshot` but not yet read, so the loader is the sole writer this build sees. - #[allow(dead_code)] pub external: Option, } diff --git a/robotd/src/main.rs b/robotd/src/main.rs index 5252240a..0d157bd6 100644 --- a/robotd/src/main.rs +++ b/robotd/src/main.rs @@ -1201,6 +1201,10 @@ async fn control_loop( let mut window_ticks = 0u64; let mut last_summary = Instant::now(); let mut was_driving = false; + // Whether External drive actually drove last tick, so its falling edge can clear the safety + // layer's external step-baseline (a later re-entry then rate-limits from the current pose, + // not a stale target from this session). + let mut was_external_drive = false; let mut bringup = Bringup::Limp; // A mode switch in flight: the mode to end up in, once the robot is home. `None` the rest of // the time, which is nearly always. @@ -1881,6 +1885,17 @@ async fn control_loop( // And only once the ramp is done, or the policy's first step would come from wherever the // robot was slumped. A fall does not stop the driving, as the prototype does not // stop it: the policy keeps going and the humans stay in charge. + // + // External drive: an off-robot controller streaming absolute joint targets + // (robot.setJoints) in place of the policy. It owns the joints when engaged AND the robot + // can hold them — torque on and homed (Ready), not mid limp-fall, not powered off. It + // needs no IMU warmup and no policy: the tick *is* the supplied targets, clamped by + // Safety::apply_external (anatomical limits, per-tick step, external deadman). The + // dispatch door refuses setJoints unless homed, so a stale frame cannot arrive here. + let external = snapshot.external; + let external_drive = + external.is_some() && bringup == Bringup::Ready && !in_limp_fall && !powered_off; + let driving = snapshot.enabled && bringup == Bringup::Ready && controller.is_some() @@ -1889,7 +1904,10 @@ async fn control_loop( && !in_limp_fall && sensors.is_some() && imu_warm - && !powered_off; + && !powered_off + // External drive supersedes the policy: while a controller streams joints, the + // on-device policy must not also be producing targets for the same servos. + && external.is_none(); if driving && !was_driving { // Starting fresh: a stale previous action in the observation, or a filter @@ -1926,6 +1944,14 @@ async fn control_loop( } was_driving = driving; + if was_external_drive && !external_drive { + // Left the External stream — forget the last written target, so a later re-entry + // rate-limits from the robot's current pose rather than a stale command from this + // session (Safety::clear_external drops the step baseline). + safety.clear_external(); + } + was_external_drive = external_drive; + // Voltage adaptation: the servos' effective kP tracks their supply, so scaling the // action by (nominal / measured) holds the robot's response steady as the pack // sags. The EMA is clamped to a plausible band so a bad reading cannot become a @@ -1969,6 +1995,18 @@ async fn control_loop( ), LimpFall::Idle => unreachable!("in_limp_fall excludes Idle"), }, + // External drive, ahead of the policy arms: `driving` is already false while a + // controller streams (it yields to External above), so the tick is simply the + // supplied targets. `moving` is true — the joints are travelling to a commanded + // pose and a restart mid-stream would drop the robot, so `safeToRestart` must not + // say yes here. The gain is the client's per-frame ask, or the mode's running gain. + // Everything the frame can violate is clamped downstream by apply_external. + _ if external_drive => { + let ext = external.expect("external_drive implies Some"); + let step = + control::Step::external(ext.targets, ext.gain.unwrap_or(policy_cfg.gain)); + (step.targets, step.gain, true, step.label) + } (true, Some(sensors)) => { let controller = controller.as_mut().expect("driving implies a controller"); match controller.step(sensors, &command, snapshot.pose.active, dt, scale_mult) { @@ -2180,7 +2218,17 @@ async fn control_loop( duck_control::model::mouth_target(snapshot.mouth); } - match safety.apply(targets, hold, gain) { + // The External stream goes through the dedicated chokepoint: apply_external adds the + // per-joint anatomical clamp, the per-tick step limit and the external deadman (which + // holds `hold` and drops toward limp on a dropped controller) on top of the ordinary + // actuator clamp. Everything else — policy, homing, limp-fall, hold — uses apply. + let written = if external_drive { + let age = external.map(|e| e.age).unwrap_or_default(); + safety.apply_external(targets, hold, age, gain) + } else { + safety.apply(targets, hold, gain) + }; + match written { Ok(applied) => limits.extend(applied.limits), Err(e) => tracing::warn!(error = %e, "bus write failed"), } @@ -4689,6 +4737,69 @@ mod tests { ); } + /// **External drive reaches the bus.** Once the robot is powered and homed, a client + /// streaming `robot.setJoints` supersedes the policy: the loop writes the supplied joint + /// targets (through `Safety::apply_external`) rather than a policy or hold pose. The head_yaw + /// target is chosen past one tick's step so the ramp is real; the other joints must stay at + /// home, proving it is the external frame — not a policy — driving. + #[tokio::test] + async fn external_drive_streams_joint_targets_to_the_bus() { + // Not frozen: reported positions follow the writes, so the external step-limiter advances + // its baseline toward the target tick by tick, as it would on the real bus. + let io = FakeIo::at(DEFAULT_POSITION); + let mut params = Params::default(); + params.policy.enabled = false; + let s = Arc::new(RobotState::new(¶ms, false, false)); + let intents = Arc::new(Intents::new()); + intents.request_init(); + + let mut target = DEFAULT_POSITION; + const HEAD_YAW: usize = 7; // wide range (±1.4), home 0.0 + target[HEAD_YAW] = 0.5; // past external_max_step (0.2), so it takes a few ticks + + let (tx, rx) = std::sync::mpsc::channel(); + let loop_state = Arc::clone(&s); + let loop_intents = Arc::clone(&intents); + let handle = tokio::spawn(async move { + let mut io = io; + control_loop_probe_with(&mut io, loop_state, loop_intents, Duration::from_millis(2)) + .await; + tx.send(io.last_written).unwrap(); + }); + + // Wait for the 2 s home ramp to complete — external drive requires a powered, homed robot. + let homed_by = Instant::now() + Duration::from_secs(6); + while !s.homed.load(Ordering::Relaxed) { + assert!(Instant::now() < homed_by, "robot never homed"); + tokio::time::sleep(Duration::from_millis(5)).await; + } + + // Stream the frame for well over the external deadman (150 ms) so the step-limited ramp + // reaches the target — a real controller re-sends every tick. + let stream_until = Instant::now() + Duration::from_millis(250); + while Instant::now() < stream_until { + intents.set_external(target, None); + tokio::time::sleep(Duration::from_millis(3)).await; + } + s.shutdown.store(true, Ordering::Relaxed); + handle.await.unwrap(); + + let written = rx.recv().unwrap().expect("the loop must command something"); + assert!( + (written.positions[HEAD_YAW] - target[HEAD_YAW]).abs() < 0.05, + "external drive did not reach the head_yaw target: wrote {}, wanted {}", + written.positions[HEAD_YAW], + target[HEAD_YAW] + ); + // The joints the client did not move stay home — it is the external frame driving, not a + // policy that would have moved the legs. + assert!( + (written.positions[0] - DEFAULT_POSITION[0]).abs() < 0.05, + "a non-commanded joint drifted: hip_yaw wrote {}", + written.positions[0] + ); + } + /// **`robot.relax` cuts power and goes back to the start**, so the next bring-up ramps from /// wherever the robot ended up rather than assuming it is still standing at home. #[tokio::test] From f5e6a2bc9e376d7b4540d68292aa644d837f0ffa Mon Sep 17 00:00:00 2001 From: cagataycali Date: Thu, 27 Aug 2026 20:03:11 -0400 Subject: [PATCH 7/7] feat(robotctl): 'robot external' sine-sweep client + docs for External drive (Step 5) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Rollout item 4: the reference External-drive client and the docs for it. The RFC named a duckctl example, but setJoints is (correctly) refused over BLE — the transport duckctl speaks — so the streaming client lives in robotctl, which reaches robotd's dispatch directly over the local Unix socket. robotctl robot external [--amplitude --hz --seconds --rate --gain]: - resolves the joint by JOINT_NAMES name or index; reads the current pose from one robot.state frame so the sweep oscillates around the live angle and every other joint is held where it stands; - enters External (robot.setMode external), streams robot.setJoints frames (targets[j] = base[j] + amplitude*sin(2π·hz·t)) at --rate, checking each IntentResult so a refusal — e.g. a robot that isn't powered+homed — is surfaced with the fix, not swallowed; - on finish OR Ctrl-C settles the joint back and hands control to the walking policy (setMode walk); a hard kill instead relies on the external deadman (~150 ms hold-then-limp). SIGINT handler shared with the theremin path. It is a downstream driver in miniature: read pose, enter External, stream absolute targets, leave cleanly. docs/design/robotd-design.md: new §3.1 'External drive' subsection (setMode external + setJoints wire examples; drive-source-not-policy-mode; separate clock; the apply_external safety envelope; BLE-refused / WebRTC-permitted); §3.5 reconciled so setJoints reads as an intent gated per transport, distinct from the maintenance-namespace raw joint writes. cargo test -p robotctl green (138; cli_definition_is_valid + bash_completions_cover_the_command_tree pass with the new subcommand); workspace build + clippy --all-targets clean; fmt clean. --- docs/design/robotd-design.md | 40 +++++++ robotctl/src/main.rs | 215 +++++++++++++++++++++++++++++++++++ 2 files changed, 255 insertions(+) diff --git a/docs/design/robotd-design.md b/docs/design/robotd-design.md index 2fbac267..0a8c0d71 100644 --- a/docs/design/robotd-design.md +++ b/docs/design/robotd-design.md @@ -552,6 +552,43 @@ that is what the deadman reads. `look` (gaze direction) is deferred; both gaze forms will be exposed, and arbitration between them is last-writer-wins with no blending. +#### External drive — `robot.setMode "external"` + `robot.setJoints` + +The intents above ask the *policy* to walk, look and pose; **External drive** takes the policy +out of the loop entirely and lets an off-robot controller command the servos directly. It is the +path a mid-training RL policy or a teleoperation rig takes to drive the physical robot without +running on it. + +```jsonc +{"jsonrpc":"2.0","id":9,"method":"robot.setMode","params":{"mode":"external"}} +// absolute joint angles, radians, in JOINT_NAMES order; length must equal NUM_JOINTS +{"jsonrpc":"2.0","id":10,"method":"robot.setJoints","params":{"targets":[/*15*/],"gain":180}} +``` + +Three properties make this safe to expose: + +- **It is a drive *source*, not a policy mode.** `external` sits alongside `walk`/`roller` in + `robot.setMode`, but it loads no policy and does no homing — the tick *is* the supplied targets. + `robot.setJoints` enters it implicitly (a single frame both supplies targets and takes the + joints from the policy); `robot.enable on`, `robot.relax` and `setMode walk|roller` leave it and + hand the servos back. `robot.mode` reports `external` while it is engaged. +- **Its clock is separate.** External targets are stamped on their own slot, so a streamed joint + command never refreshes — nor is refreshed by — the twist deadman, and each has a deadman that + reads the right question. +- **The safety envelope is the whole point.** Every frame passes through `Safety::apply_external`, + which is stricter than the policy path: each joint is clamped to its **anatomical limit**, each + tick to a **maximum step** from the last written target (so a jump is rate-limited, not snapped), + and a controller that stops streaming trips a dedicated **external deadman** — the robot holds + the fallback pose and drops toward limp, the joint-space equivalent of the twist deadman zeroing + a velocity. `robot.setJoints` is refused at the door on the wrong number of targets, a non-finite + value, or a robot that is not powered and homed (a limp robot cannot hold a commanded position). + +External drive is refused over BLE for the same capacity reasons `robot.move` is (§1.2) and then +some — a joint stream is the most direct motor control there is — and permitted over the WebRTC +`control` channel, which is the transport it is for. `robotctl robot external ` is the +reference client: it reads the current pose, enters External, streams a sine sweep on one joint, +and hands back cleanly. + ### 3.2 State out One stream, subscribable, decimated per subscriber. It must report what was **refused**, not @@ -699,6 +736,9 @@ control mid-stride is how a robot falls over (`updater-design.md` §7.2). `init`, emergency torque-off, calibration and raw joint writes are not intents. They live in their own namespace so the relay's per-transport allow-list can keep them off remote transports. +(External drive's `robot.setJoints` is different: it *is* an intent — a live joint stream the +control loop clamps through `Safety::apply_external` — and is gated per transport in the route +files, refused over BLE and permitted over WebRTC, rather than living in the maintenance namespace.) Signaling gating decides *who connects*; it does not say a teleoperator is also a mechanic — and `update.*` reaching a DataChannel would mean a remote peer can trigger a rollback. diff --git a/robotctl/src/main.rs b/robotctl/src/main.rs index c7f0fc59..2aaa3a19 100644 --- a/robotctl/src/main.rs +++ b/robotctl/src/main.rs @@ -416,6 +416,39 @@ enum RobotCommand { #[arg(long)] json: bool, }, + + /// Stream a sine sweep on one joint in **External** drive — the reference client for + /// `robot.setJoints`. + /// + /// External drive hands the joints to an off-robot controller: instead of the on-device + /// policy, whatever you stream is what the servos hold, clamped by the safety layer (each + /// joint to its anatomical limit, each tick to a maximum step, and a dead controller held + /// then dropped to limp by the external deadman). This sweeps ONE joint around its current + /// angle with a sine and holds every other joint where it is. + /// + /// **Stand the robot up first** (`robotctl robot init`): a limp robot cannot hold a commanded + /// position, so `robot.setJoints` is refused until the joints are powered and homed. On the + /// way out the sweep settles the joint back and hands control to the walking policy; a Ctrl-C + /// instead relies on the external deadman, which holds and then limps within ~150 ms. + External { + /// Joint to sweep: a `JOINT_NAMES` name (e.g. `head_yaw`) or an index `0..14`. + joint: String, + /// Peak amplitude of the sweep, radians. The safety layer clamps to the joint's limit. + #[arg(long, default_value_t = 0.3)] + amplitude: f64, + /// Sweep frequency, Hz. + #[arg(long, default_value_t = 0.5)] + hz: f64, + /// How long to stream, seconds. + #[arg(long, default_value_t = 5.0)] + seconds: f64, + /// Command rate, Hz — how often a frame is sent. + #[arg(long, default_value_t = 50.0)] + rate: f64, + /// Hold gain (position P gain). Omit for the mode's running gain. + #[arg(long)] + gain: Option, + }, } /// `robotctl quack` — the loudest way to tell ducks apart. SSH into one, quack it, and the @@ -2150,6 +2183,19 @@ fn run_system(socket: &Path, command: SystemCommand) -> Result<(), Failure> { /// Power to the joints, through `robotd`. fn run_robot(socket: &Path, command: RobotCommand) -> Result<(), Failure> { + // The External sweep is a streaming client, not a single call/response — it has its own path. + if let RobotCommand::External { + joint, + amplitude, + hz, + seconds, + rate, + gain, + } = &command + { + return run_robot_external(socket, joint, *amplitude, *hz, *seconds, *rate, *gain); + } + // Asked before connecting, so a robot is not dropped by a command the operator then aborts. // Same shape as `system reboot`, and for a more immediate reason: this one takes effect in // milliseconds and the robot is standing. @@ -2189,6 +2235,7 @@ fn run_robot(socket: &Path, command: RobotCommand) -> Result<(), Failure> { }), *json, ), + RobotCommand::External { .. } => unreachable!("streaming path handled above"), }; let result = result_of(client.call(&call)?)?; @@ -2236,10 +2283,178 @@ fn run_robot(socket: &Path, command: RobotCommand) -> Result<(), Failure> { RobotCommand::Relax { .. } => println!("torque off"), RobotCommand::Do { skill, .. } => println!("{skill:?} queued"), RobotCommand::Mode { .. } | RobotCommand::Look { .. } => unreachable!("answered above"), + RobotCommand::External { .. } => unreachable!("streaming path handled above"), } Ok(()) } +/// `robotctl robot external ` — the reference External-drive client. Streams a sine sweep +/// on one joint through `robot.setJoints`, holding the rest of the pose where it is, then hands +/// control back to the policy. It is what a downstream driver (an RL policy over the socket) does +/// in miniature: read the pose, enter External, stream absolute joint targets, leave cleanly. +#[allow(clippy::too_many_arguments)] +fn run_robot_external( + socket: &Path, + joint: &str, + amplitude: f64, + hz: f64, + seconds: f64, + rate: f64, + gain: Option, +) -> Result<(), Failure> { + let index = resolve_joint(joint)?; + if !(rate > 0.0 && rate <= 200.0) { + return Err(Failure::new( + exit::USAGE, + format!("--rate must be in (0, 200] Hz, got {rate}"), + )); + } + if seconds <= 0.0 { + return Err(Failure::new( + exit::USAGE, + "--seconds must be positive".to_owned(), + )); + } + + // The base pose is wherever the robot is right now: read one state frame so the sweep + // oscillates around the current angle and every other joint is held where it stands. + let base = read_pose(socket)?; + + let mut client = Client::connect_to("robotd", socket)?; + client.hello()?; + + // Announce the mode explicitly. `setJoints` would enter it implicitly, but this makes + // `robot.mode` report `external` and reads clearly. + result_of( + client.call(&proto::Call::RobotSetMode(proto::SetModeParams { + mode: "external".to_owned(), + }))?, + )?; + + // From here a Ctrl-C leaves the loop; if we cannot hand back cleanly the external deadman + // holds the robot and drops it to limp within ~150 ms. Installed after the mode is set, so an + // interrupt before this simply kills the process with nothing to unwind. + unsafe { + libc::signal( + libc::SIGINT, + note_interrupt as *const () as libc::sighandler_t, + ); + } + + println!( + "external sweep: {} (joint {index}) ±{amplitude:.2} rad @ {hz:.2} Hz for {seconds:.1}s, \ + {rate:.0} Hz · Ctrl-C to stop", + proto::JOINT_NAMES[index] + ); + + let period = std::time::Duration::from_secs_f64(1.0 / rate); + let start = std::time::Instant::now(); + let mut frames = 0u64; + let outcome = loop { + if INTERRUPTED.load(std::sync::atomic::Ordering::Relaxed) { + break Ok(()); + } + let t = start.elapsed().as_secs_f64(); + if t >= seconds { + break Ok(()); + } + let mut targets = base.clone(); + targets[index] = base[index] + amplitude * (std::f64::consts::TAU * hz * t).sin(); + + let result: proto::IntentResult = decode(&result_of(client.call( + &proto::Call::RobotSetJoints(proto::SetJointsParams { targets, gain }), + )?)?)?; + if !result.accepted { + break Err(result.reason.unwrap_or_else(|| { + "the robot refused the frame — is it powered and homed? (`robotctl robot init`)" + .to_owned() + })); + } + frames += 1; + std::thread::sleep(period); + }; + + // Settle the swept joint back to its base angle (best-effort), then hand the joints to the + // walking policy so the robot is not left in External with a dead client. + let _ = client.call(&proto::Call::RobotSetJoints(proto::SetJointsParams { + targets: base.clone(), + gain, + })); + let _ = client.call(&proto::Call::RobotSetMode(proto::SetModeParams { + mode: "walk".to_owned(), + })); + + match outcome { + Err(reason) => Err(Failure::new(exit::REFUSED, reason)), + Ok(()) => { + let how = if INTERRUPTED.load(std::sync::atomic::Ordering::Relaxed) { + "interrupted" + } else { + "done" + }; + println!("{how} — {frames} frames streamed, handed back to the policy (mode: walk)"); + Ok(()) + } + } +} + +/// Resolve a joint argument: a [`proto::JOINT_NAMES`] name, or a bare index into it. +fn resolve_joint(joint: &str) -> Result { + if let Some(i) = proto::JOINT_NAMES.iter().position(|n| *n == joint) { + return Ok(i); + } + if let Ok(i) = joint.parse::() + && i < proto::JOINT_NAMES.len() + { + return Ok(i); + } + Err(Failure::new( + exit::USAGE, + format!( + "unknown joint {joint:?} — expected a name ({}) or an index 0..{}", + proto::JOINT_NAMES.join(", "), + proto::JOINT_NAMES.len() - 1 + ), + )) +} + +/// Read one measured joint pose from `robotd`'s state stream — the base the sweep oscillates +/// around. A short-lived subscription, dropped as soon as the first full frame arrives. +fn read_pose(socket: &Path) -> Result, Failure> { + let mut stream = Client::connect_to("robotd", socket)?; + stream.hello()?; + stream.send(&proto::Request::call( + proto::Id::Number(1), + &proto::Call::RobotSubscribe(proto::SubscribeParams { hz: Some(30) }), + ))?; + + let mut line = String::new(); + for _ in 0..200 { + line.clear(); + match stream.reader.read_line(&mut line) { + Ok(0) => break, + Ok(_) => {} + Err(e) => { + return Err(Failure::new( + exit::UNREACHABLE, + format!("the state stream stopped: {e}"), + )); + } + } + if let Some(state) = serde_json::from_str::(&line) + .ok() + .and_then(|r| r.as_state()) + && state.joints.len() == proto::JOINT_NAMES.len() + { + return Ok(state.joints); + } + } + Err(Failure::new( + exit::UNREACHABLE, + "no joint state from robotd — is the control loop running?".to_owned(), + )) +} + /// The unit paused while a pad bonds. See [`BtdPaused`]. const BTD_UNIT: &str = "btd.service";