From d0e88432edbfb0fab0de7a8495e3b70b749e9806 Mon Sep 17 00:00:00 2001 From: "liqiankun.1111" Date: Wed, 7 Oct 2026 00:13:25 +0800 Subject: [PATCH] feat(robotd): execute timed action chunks for VLA runners MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit A model runner can now submit a timed trajectory directly to robotd instead of translating every prediction into predefined movement skills. The existing 50 Hz motor loop admits and executes bounded 15-joint absolute-radian chunks through Safety and RobotIo; model inference and action decoding remain outside the daemon. Add begin/submit/end JSON-RPC calls, robotctl commands and robot.state execution feedback. Robot-clock scheduling skips expired samples, replaces future trajectory suffixes, rejects stale sessions and competing joint writers, and invalidates queued results on operator cancellation. Exhaustion or body failure releases ownership into measured-pose hold without automatically resuming the locomotion policy. Shared API version is now 38. This entrypoint is enabled only on --fake/--sim at 50 Hz and is accessed through the local Unix socket. Physical hardware and BLE/WebRTC forwarding are excluded from this initial integration. No VLA weights, inference service or learned balance behavior are included. Validation: - Rust 1.99 Linux: cargo test --workspace --locked --offline — 1550 passed, 18 ignored, 0 failed. - cargo fmt --all -- --check and git diff --check passed. - Real robotd/FakeIo IPC regression covers measured motion, conflicting commands, stop and rejection of late results. - Isolated MuJoCo body: head_yaw moved from about 0.024 to 0.142 rad toward a 0.174 rad target; cancellation entered held state and rejected the old session. This checks joint execution, not balance; the unbalanced body fell with the locomotion policy disabled. Assisted-by: Codex --- README.md | 2 + btd/src/route.rs | 2 + docs/README.md | 2 + docs/design/action-chunks.md | 79 ++++++ duck-ipc-proto/src/actions.rs | 57 +++++ duck-ipc-proto/src/lib.rs | 45 +++- mediad/src/route.rs | 2 + robotctl/src/actions.rs | 36 +++ robotctl/src/main.rs | 12 + robotctl/src/monitor.rs | 1 + robotd/src/action_chunk.rs | 405 +++++++++++++++++++++++++++++++ robotd/src/action_chunk_tests.rs | 225 +++++++++++++++++ robotd/src/main.rs | 92 ++++++- robotd/tests/action_chunk_ipc.rs | 165 +++++++++++++ updater/src/ipc.rs | 2 +- 15 files changed, 1119 insertions(+), 8 deletions(-) create mode 100644 docs/design/action-chunks.md create mode 100644 duck-ipc-proto/src/actions.rs create mode 100644 robotctl/src/actions.rs create mode 100644 robotd/src/action_chunk.rs create mode 100644 robotd/src/action_chunk_tests.rs create mode 100644 robotd/tests/action_chunk_ipc.rs diff --git a/README.md b/README.md index 2aef6df..e9b7f25 100644 --- a/README.md +++ b/README.md @@ -4,6 +4,8 @@

Microduck

+Duckmind adds [native Action Chunk execution](docs/design/action-chunks.md) to this Microduck runtime, currently available on fake and MuJoCo bodies. +

A tiny biped robot that moves using reinforcement learning policies.

diff --git a/btd/src/route.rs b/btd/src/route.rs index 5a796ae..c10bdca 100644 --- a/btd/src/route.rs +++ b/btd/src/route.rs @@ -234,6 +234,8 @@ fn permits(call: &proto::Call) -> bool { // not exist for the first ~73s of a boot is not a control transport. The body pose and // the mouth ride with it: all of these are a stream of small updates, and the argument is // about the stream, not about any one of them. + // First deployment uses a local fake/sim policy runner, not remote joint ownership. + RobotActionsBegin | RobotActionsSubmit(_) | RobotActionsEnd(_) => false, RobotMove(_) | RobotHead(_) | RobotLook(_) | RobotPose(_) | RobotMouth(_) => false, // **Not teleop either, and it sat in that group for the same reason a skill did**: it was diff --git a/docs/README.md b/docs/README.md index 25ce273..36f5717 100644 --- a/docs/README.md +++ b/docs/README.md @@ -1,5 +1,7 @@ # Docs +Duckmind model execution: [Action Chunk API and timing contract](design/action-chunks.md). + The [README](../README.md) is the front door — what a microduck is, and where to go. If you have one in front of you and want to drive it, start at the [cheat sheet](robot/cheatsheet.md). diff --git a/docs/design/action-chunks.md b/docs/design/action-chunks.md new file mode 100644 index 0000000..382f9a6 --- /dev/null +++ b/docs/design/action-chunks.md @@ -0,0 +1,79 @@ +# Action Chunk execution + +Duckmind accepts model-generated joint trajectories through `robot.actions.*`. A Python policy runner owns language, images, history, inference and model-specific action decoding. `robotd` owns the action timeline and the only motor-writing path. + +The API is enabled only with `--fake` or `--sim`, at a 50 Hz control rate. Physical hardware rejects these calls. BLE and WebRTC do not forward them; a policy runner on the simulation host uses the existing Unix JSON-RPC socket. The runner may call a remote model service. + +## Start and observe + +```bash +cargo run -p robotd -- --fake --no-policy --socket /tmp/duckmind.sock +# In another terminal; wait for homing to complete before acquiring the joints. +cargo run -p robotctl -- --robot-socket /tmp/duckmind.sock robot init +cargo run -p robotctl -- --robot-socket /tmp/duckmind.sock robot actions begin +``` + +`begin` requires fresh sensor feedback, warmed IMU and completed bring-up, with no shutdown, mode change, policy swap or limp-fall sequence in progress. It returns `session_id`, robot-clock `t_ns`, `step_ns`, `joint_names` and measured `positions`. + +Acquisition disables the on-board movement policy and clears its velocity intent. Theremin and chorale joint writers are deactivated. Another movement writer receives `BUSY` while the session owns the body. Stop, disable, init, relax, motor reboot and shutdown can preempt the session. + +## Submit a chunk + +Use `robotctl robot actions submit chunk.json`, or send a JSON-RPC request with method `robot.actions.submit` and these parameters: + +| Field | Meaning | +|---|---| +| `session_id` | ID returned by `begin`; invalid after cancellation, exhaustion or daemon restart. | +| `sequence` | Increasing inference-request number within this session. | +| `observation_t_ns` | Robot-clock timestamp of the state used for inference. | +| `start_t_ns` | Start of the first action interval, on the same clock. | +| `step_ns` | Exactly `20000000` (20 ms). | +| `positions` | 1–100 arrays of 15 absolute joint angles in radians, ordered as `joint_names`. | + +The 15-joint body contract includes the mouth. The upstream locomotion model produces 14 normalized offsets; a model adapter must decode those and supply a mouth target before submitting. `robotd` does not guess normalization, HOME offsets or joint mappings. + +Angles must be finite and within the actuator travel range. This is not a complete anatomical or collision constraint model. A chunk's end and its source observation must fall within the bounded two-second admission window; timestamps that overflow or use a future observation are refused. + +Admission happens on the motor loop. A successful RPC means the timeline accepted the chunk, not that joints reached their targets. The IPC caller waits at most 250 ms for admission. A request whose reply was abandoned before processing is not executed later. + +## Time and replacement + +Action `i` applies during `[start_t_ns + i * step_ns, start_t_ns + (i + 1) * step_ns)`. Cloud wall time is irrelevant: use the clock already reported by `robot.state.t_ns`. The client maintains alignment to that clock from received robot state. + +Past intervals are discarded. A late tick selects the current target instead of replaying missed targets rapidly. A future replacement preserves the old prefix before its start and discards the old tail from that start onward. An entirely expired or out-of-order response cannot replace the current timeline. + +Waiting for a future first action holds the measured pose captured at acquisition. Replacements must overlap or continue the buffered timeline; a gap is rejected without modifying the old buffer. There is no automatic interpolation, action averaging or RTC inpainting. Plan continuous boundaries in the policy runner and validate them in simulation. + +## Feedback and termination + +`robot.subscribe` adds an `actions` block on eligible backends: + +- `phase`: `idle`, `waiting`, `running` or `ended`. +- `session_id`, latest accepted `sequence` and current robot-clock `t_ns`. +- `selected_sequence` / `selected_index`: most recently selected command, not measured task completion. +- `remaining`: buffered intervals, including the currently active interval. +- `last_write_ok`: whether the session's most recent bus write succeeded. +- `reason`: why control ended, such as `cancelled`, `operator_preempted`, `buffer_exhausted`, `body_not_ready` or `bus_write_failed`. + +Measured motion remains in `robot.state.joints` and `velocities`; `targets` reports the selected target. A successful write is not proof that the mechanism moved, and chunk exhaustion is not task success. + +```bash +robotctl --robot-socket /tmp/duckmind.sock robot actions end SESSION_ID +``` + +End, stop, buffer exhaustion or loss of fresh/ready body state invalidates the session and discards its timeline. A session with no first chunk expires after two seconds. Except for explicit power/mode operations, the loop captures the last valid measured pose and holds it; the old policy is not automatically resumed. Holding a pose does not guarantee dynamic balance on a biped. A lost client can execute only the remaining bounded timeline, not an unbounded backlog. + +## Ownership and implementation + +`duck-ipc-proto/src/actions.rs` owns the wire types. `robotd/src/action_chunk.rs` owns admission, session generations and timed replacement. `main.rs` selects the source before the shared `Safety::apply → RobotIo` write; IPC never writes motors. Stop generations invalidate commands that were queued before the stop as well as already active sessions. + +This follows [LeRobot asynchronous inference](https://huggingface.co/docs/lerobot/async) and [OpenPI's action-chunk broker](https://github.com/Physical-Intelligence/openpi/blob/main/packages/openpi-client/src/openpi_client/action_chunk_broker.py) in separating inference from action consumption. [RTC](https://huggingface.co/docs/lerobot/main/rtc) additionally conditions generation on the previous action prefix; it belongs in the policy runner and is not implemented by this queue. + +## Verification + +```bash +cargo test -p duck-ipc-proto -p robotd -p robotctl +cargo test -p robotd --test action_chunk_ipc +``` + +Unit tests cover late responses, tick skips, future replacement, stale sessions, queue admission and invalid actions. The integration test launches the real `robotd --fake`, sends JSON-RPC chunks, observes joint feedback, preempts execution and verifies the cancelled tail never moves the joint. MuJoCo checks exercise the same `RobotIo` path against physics; neither test establishes a learned language skill or balance policy. diff --git a/duck-ipc-proto/src/actions.rs b/duck-ipc-proto/src/actions.rs new file mode 100644 index 0000000..bacdb17 --- /dev/null +++ b/duck-ipc-proto/src/actions.rs @@ -0,0 +1,57 @@ +//! Robot-clock action chunks. Model normalization belongs to the caller. +use serde::{Deserialize, Serialize}; + +pub const ACTION_STEP_NS: u64 = 20_000_000; +pub const MAX_ACTION_STEPS: usize = 100; + +#[derive(Debug, Clone, PartialEq, Serialize, Deserialize)] +#[serde(deny_unknown_fields)] +pub struct ActionChunkParams { + pub session_id: String, + pub sequence: u64, + /// Timestamp of the observation used for this inference, on robot.state's clock. + pub observation_t_ns: u64, + pub start_t_ns: u64, + pub step_ns: u64, + /// Absolute radians in JOINT_NAMES order, including the mouth. + pub positions: Vec<[f64; 15]>, +} + +#[derive(Debug, Clone, PartialEq, Serialize, Deserialize)] +#[serde(deny_unknown_fields)] +pub struct ActionEndParams { + pub session_id: String, +} + +#[derive(Debug, Clone, PartialEq, Serialize, Deserialize)] +pub struct ActionSession { + pub session_id: String, + pub t_ns: u64, + pub step_ns: u64, + pub joint_names: Vec, + pub positions: [f64; 15], +} + +#[derive(Debug, Clone, Default, PartialEq, Serialize, Deserialize)] +#[serde(rename_all = "snake_case")] +pub enum ActionPhase { + #[default] + Idle, + Waiting, + Running, + Ended, +} + +#[derive(Debug, Clone, Default, PartialEq, Serialize, Deserialize)] +pub struct ActionStatus { + pub session_id: Option, + pub phase: ActionPhase, + pub sequence: Option, + /// Sequence/index selected for the most recent write attempt; not measured completion. + pub selected_sequence: Option, + pub selected_index: Option, + pub remaining: usize, + pub t_ns: u64, + pub reason: Option, + pub last_write_ok: Option, +} diff --git a/duck-ipc-proto/src/lib.rs b/duck-ipc-proto/src/lib.rs index be1e0bc..d120548 100644 --- a/duck-ipc-proto/src/lib.rs +++ b/duck-ipc-proto/src/lib.rs @@ -29,6 +29,9 @@ //! including the ones on the recovery path, so nothing here may pull in http, tar, crypto //! or an async runtime. +mod actions; +pub use actions::*; + use serde::{Deserialize, Serialize}; use serde_json::Value; @@ -423,7 +426,8 @@ pub const JSONRPC_VERSION: &str = "2.0"; /// an `updaterd` that has not run its first check yet — every board for the minute after it /// starts, including the one right after the update that brought v35 in. Both warned. The attempt /// tells them apart, and its error is what the warning was pointing at the journal for. -pub const API_VERSION: u32 = 37; +// v38: robot.actions.* and robot.state.actions; fake/sim only. +pub const API_VERSION: u32 = 38; /// The observation width every policy this robot family runs is built against. /// @@ -540,6 +544,9 @@ pub const JOINT_NAMES: [&str; 15] = [ /// Method names, as they go on the wire. Namespaced so a new namespace cannot collide /// with `update.*`. [`Call`] is the typed form. pub mod method { + pub const ROBOT_ACTIONS_BEGIN: &str = "robot.actions.begin"; + pub const ROBOT_ACTIONS_SUBMIT: &str = "robot.actions.submit"; + pub const ROBOT_ACTIONS_END: &str = "robot.actions.end"; pub const HELLO: &str = "hello"; /// One raw camera frame. `mediad` answers the JSON-RPC header, followed immediately by the @@ -985,6 +992,11 @@ pub enum Call { RobotModelApi, RobotRemoteSessionActive, + /// Acquire timed joint execution. Action calls require request IDs for admission feedback. + RobotActionsBegin, + RobotActionsSubmit(ActionChunkParams), + RobotActionsEnd(ActionEndParams), + // ── intents ────────────────────────────────────────────────────────────── /// Continuous. Send as a notification. RobotMove(MoveParams), @@ -1174,6 +1186,9 @@ impl Call { Call::RobotHealth => method::ROBOT_HEALTH, Call::RobotModelApi => method::ROBOT_MODEL_API, Call::RobotRemoteSessionActive => method::ROBOT_SESSION_ACTIVE, + Call::RobotActionsBegin => method::ROBOT_ACTIONS_BEGIN, + Call::RobotActionsSubmit(_) => method::ROBOT_ACTIONS_SUBMIT, + Call::RobotActionsEnd(_) => method::ROBOT_ACTIONS_END, Call::RobotMove(_) => method::ROBOT_MOVE, Call::RobotHead(_) => method::ROBOT_HEAD, Call::RobotLook(_) => method::ROBOT_LOOK, @@ -1355,6 +1370,10 @@ impl Call { | Call::RobotPolicies | Call::RobotModel | Call::RobotMode => (Robot, Prompt), + // Action requests wait for bounded loop admission, not for motion completion. + Call::RobotActionsBegin | Call::RobotActionsSubmit(_) | Call::RobotActionsEnd(_) => { + (Robot, Prompt) + } // Intents and one-shot skills. All fast: they store a value the control loop reads on // its next tick, and none of them waits for the robot to finish anything. Call::RobotMove(_) @@ -1469,6 +1488,9 @@ impl Call { Call::Pin(p) => encode(p), Call::Log(p) => encode(p), Call::Show(p) => encode(p), + Call::RobotActionsBegin => Value::Object(serde_json::Map::new()), + Call::RobotActionsSubmit(p) => encode(p), + Call::RobotActionsEnd(p) => encode(p), Call::RobotMove(p) => encode(p), Call::RobotHead(p) => encode(p), Call::RobotLook(p) => encode(p), @@ -1563,6 +1585,9 @@ impl Call { method::ROBOT_HEALTH => Call::RobotHealth, method::ROBOT_MODEL_API => Call::RobotModelApi, method::ROBOT_SESSION_ACTIVE => Call::RobotRemoteSessionActive, + method::ROBOT_ACTIONS_BEGIN => Call::RobotActionsBegin, + method::ROBOT_ACTIONS_SUBMIT => Call::RobotActionsSubmit(decode(params)?), + method::ROBOT_ACTIONS_END => Call::RobotActionsEnd(decode(params)?), method::ROBOT_MOVE => Call::RobotMove(decode(params)?), method::ROBOT_HEAD => Call::RobotHead(decode(params)?), method::ROBOT_LOOK => Call::RobotLook(decode(params)?), @@ -1699,6 +1724,18 @@ pub mod test_support { Call::RobotHealth, Call::RobotModelApi, Call::RobotRemoteSessionActive, + Call::RobotActionsBegin, + Call::RobotActionsSubmit(ActionChunkParams { + session_id: "sample".into(), + sequence: 1, + observation_t_ns: 0, + start_t_ns: 0, + step_ns: ACTION_STEP_NS, + positions: vec![[0.0; 15]], + }), + Call::RobotActionsEnd(ActionEndParams { + session_id: "sample".into(), + }), Call::RobotMove(MoveParams { vx: 0.2, vy: -0.1, @@ -3706,6 +3743,9 @@ impl IntentResult { /// `requested` rather than the stream carrying only outcomes. #[derive(Debug, Clone, PartialEq, Serialize, Deserialize)] pub struct RobotState { + /// External action execution, independent of whether the task has succeeded. + #[serde(default, skip_serializing_if = "Option::is_none")] + pub actions: Option, /// Seconds since the daemon started. Monotonic: it is for correlating samples, not for /// telling the time. pub t: f64, @@ -5560,7 +5600,7 @@ mod tests { fn every_call_covers_every_variant() { assert_eq!( every_call().len(), - 67, + 70, "a Call variant was added or removed — update every_call() and this count" ); } @@ -6262,6 +6302,7 @@ mod tests { /// — so an additive field is one line here rather than one line per test. fn a_state() -> RobotState { RobotState { + actions: None, t: 1.5, movement: MoveState { requested: [0.0; 3], diff --git a/mediad/src/route.rs b/mediad/src/route.rs index 977cc11..b324dd8 100644 --- a/mediad/src/route.rs +++ b/mediad/src/route.rs @@ -60,6 +60,8 @@ fn permits(call: &proto::Call) -> bool { // budget and a link that does not exist for the first ~73s of a boot is not a control // transport". A datachannel is a control transport, so this is the transport those // refusals were pointing at. + // First deployment uses a local fake/sim policy runner, not remote joint ownership. + RobotActionsBegin | RobotActionsSubmit(_) | RobotActionsEnd(_) => false, RobotMove(_) | RobotHead(_) | RobotLook(_) | RobotPose(_) | RobotMouth(_) => true, // The theremin rides with the sounds: it is one, and a browser that can quack a duck // may pick its instrument up too. diff --git a/robotctl/src/actions.rs b/robotctl/src/actions.rs new file mode 100644 index 0000000..cee0c00 --- /dev/null +++ b/robotctl/src/actions.rs @@ -0,0 +1,36 @@ +//! Thin action-session client; the daemon owns timing and admission. +use std::path::PathBuf; + +use clap::Subcommand; +use duck_ipc_proto as proto; + +use crate::{Client, Failure, compact, exit, result_of}; + +#[derive(Debug, Subcommand)] +pub enum Command { + /// Acquire all joints on a ready fake/sim body; prints session and robot clock. + Begin, + /// Submit an ActionChunkParams JSON file with robot-clock timestamps. + Submit { file: PathBuf }, + /// Cancel queued actions and release this session. + End { session_id: String }, +} + +pub fn run(client: &mut Client, command: &Command) -> Result<(), Failure> { + let call = match command { + Command::Begin => proto::Call::RobotActionsBegin, + Command::Submit { file } => { + let bytes = std::fs::read(file) + .map_err(|e| Failure::new(exit::USAGE, format!("{}: {e}", file.display())))?; + let params = serde_json::from_slice(&bytes) + .map_err(|e| Failure::new(exit::USAGE, format!("invalid action chunk: {e}")))?; + proto::Call::RobotActionsSubmit(params) + } + Command::End { session_id } => proto::Call::RobotActionsEnd(proto::ActionEndParams { + session_id: session_id.clone(), + }), + }; + let result = result_of(client.call(&call)?)?; + println!("{}", compact(&result)); + Ok(()) +} diff --git a/robotctl/src/main.rs b/robotctl/src/main.rs index f55ad04..3ca0ed6 100644 --- a/robotctl/src/main.rs +++ b/robotctl/src/main.rs @@ -40,6 +40,7 @@ use clap::{Args, CommandFactory, Parser, Subcommand}; use duck_ipc_proto as proto; use robotd_params::Slot; +mod actions; mod camera; mod cells; mod configure; @@ -461,6 +462,11 @@ enum SystemCommand { #[derive(Subcommand, Debug)] enum RobotCommand { + /// Execute model action chunks on a fake or simulated body. + Actions { + #[command(subcommand)] + command: actions::Command, + }, /// Power the joints and ramp to the home pose, over about two seconds. /// /// **This moves every joint.** Have the robot on its stand, or hold it. Needs no policy — a @@ -3153,7 +3159,12 @@ fn run_robot(socket: &Path, command: RobotCommand) -> Result<(), Failure> { let mut client = Client::connect_to("robotd", socket)?; client.hello()?; + if let RobotCommand::Actions { command } = &command { + return actions::run(&mut client, command); + } + let (call, json) = match &command { + RobotCommand::Actions { .. } => unreachable!("handled above"), RobotCommand::Init { json } => (proto::Call::RobotInit, *json), RobotCommand::Relax { json, .. } => (proto::Call::RobotRelax, *json), RobotCommand::Enable { off, toggle, json } => ( @@ -3232,6 +3243,7 @@ fn run_robot(socket: &Path, command: RobotCommand) -> Result<(), Failure> { return Err(Failure::new(exit::REFUSED, reason)); } match command { + RobotCommand::Actions { .. } => unreachable!("handled above"), RobotCommand::Init { .. } => println!("standing up — about two seconds to the home pose"), RobotCommand::Relax { .. } => println!("torque off"), // The daemon's own `reason` names the state it ended in, which is the only trustworthy diff --git a/robotctl/src/monitor.rs b/robotctl/src/monitor.rs index 31edb69..5030f2b 100644 --- a/robotctl/src/monitor.rs +++ b/robotctl/src/monitor.rs @@ -4818,6 +4818,7 @@ mod tests { fn a_state() -> proto::RobotState { proto::RobotState { + actions: None, t: 1.0, movement: proto::MoveState { requested: [0.0; 3], diff --git a/robotd/src/action_chunk.rs b/robotd/src/action_chunk.rs new file mode 100644 index 0000000..3081d2b --- /dev/null +++ b/robotd/src/action_chunk.rs @@ -0,0 +1,405 @@ +//! A bounded, robot-clock timeline owned by the existing control loop. +//! +//! IPC receives an acknowledgement only after the loop has accepted the command. +//! Session generations keep a queued/late inference from undoing an operator stop. +use std::collections::VecDeque; +use std::sync::atomic::{AtomicBool, AtomicU64, Ordering}; +use std::sync::{Arc, Mutex}; +use std::time::Duration; + +use arc_swap::ArcSwap; +use duck_control::NUM_JOINTS; +use duck_control::safety::{ACTUATOR_MAX, ACTUATOR_MIN}; +use duck_ipc_proto::{self as proto, ActionPhase, ActionStatus}; +use tokio::sync::{mpsc, oneshot}; + +const CAPACITY: usize = 8; +const HORIZON_NS: u64 = proto::ACTION_STEP_NS * proto::MAX_ACTION_STEPS as u64; +type Answer = Result; + +pub struct Bridge { + pub available: AtomicBool, + pub status: ArcSwap, + generation: AtomicU64, + tx: mpsc::Sender, + rx: Mutex>>, +} + +struct Pending { + generation: u64, + call: proto::Call, + answer: oneshot::Sender, +} + +fn refused(message: impl Into) -> proto::Error { + proto::Error::new(proto::code::BUSY, message) +} + +fn invalid(message: impl Into) -> proto::Error { + proto::Error::new(proto::code::INVALID_PARAMS, message) +} + +impl Bridge { + pub fn new() -> Self { + let (tx, rx) = mpsc::channel(CAPACITY); + Self { + available: AtomicBool::new(false), + status: ArcSwap::from_pointee(ActionStatus::default()), + generation: AtomicU64::new(0), + tx, + rx: Mutex::new(Some(rx)), + } + } + + pub fn executor(&self) -> Executor { + Executor::new( + self.rx + .lock() + .unwrap() + .take() + .expect("one control-loop owner"), + ) + } + + pub fn owns_body(&self) -> bool { + matches!( + self.status.load().phase, + ActionPhase::Waiting | ActionPhase::Running + ) + } + + pub fn interrupt(&self) { + self.generation.fetch_add(1, Ordering::AcqRel); + } + + /// Stop/power commands preempt; other movement writers must wait for release. + pub fn check_call(&self, call: &proto::Call) -> Result<(), proto::Error> { + use proto::Call::*; + if matches!( + call, + RobotStop | RobotInit | RobotRelax | RobotRebootMotors(_) | RobotShutdown + ) || matches!(call, RobotEnable(p) if !p.on && !p.toggle) + { + self.interrupt(); + } else if self.owns_body() + && matches!( + call, + RobotMove(_) + | RobotHead(_) + | RobotLook(_) + | RobotPose(_) + | RobotMouth(_) + | RobotDo(_) + | RobotEnable(_) + | RobotSetMode(_) + | RobotLoadPolicy(_) + | RobotReloadPolicies + | RobotTheremin(_) + | RobotChorale(_) + ) + { + return Err(refused("action session owns the joints; end it first")); + } + Ok(()) + } + + pub async fn request(&self, call: proto::Call) -> Answer { + if !self.available.load(Ordering::Acquire) { + return Err(refused("action chunks require --fake or --sim at 50 Hz")); + } + if let proto::Call::RobotActionsSubmit(p) = &call { + validate(p)?; + } + let (answer, reply) = oneshot::channel(); + self.tx + .try_send(Pending { + generation: self.generation.load(Ordering::Acquire), + call, + answer, + }) + .map_err(|_| refused("action command queue is full or stopped"))?; + // A wedged motor loop must not wedge the IPC service. Closed replies are + // skipped by the owner, so a timed-out request cannot execute later. + tokio::time::timeout(Duration::from_millis(250), reply) + .await + .map_err(|_| refused("control loop did not accept the action request in time"))? + .map_err(|_| refused("action executor stopped"))? + } +} + +fn validate(p: &proto::ActionChunkParams) -> Result<(), proto::Error> { + if p.step_ns != proto::ACTION_STEP_NS + || p.positions.is_empty() + || p.positions.len() > proto::MAX_ACTION_STEPS + { + return Err(invalid("expected 1..100 actions with step_ns=20000000")); + } + if p.positions + .iter() + .flatten() + .any(|v| !v.is_finite() || !(ACTUATOR_MIN..=ACTUATOR_MAX).contains(v)) + { + return Err(invalid( + "positions must be finite absolute radians within actuator travel", + )); + } + if p.start_t_ns + .checked_add(p.step_ns * p.positions.len() as u64) + .is_none() + { + return Err(invalid("action timeline overflows")); + } + Ok(()) +} + +struct Frame { + at: u64, + sequence: u64, + index: usize, + target: [f64; NUM_JOINTS], +} + +pub struct Executor { + rx: mpsc::Receiver, + timeline: VecDeque, + status: ActionStatus, + generation: u64, + serial: u64, + first_chunk_deadline: u64, +} + +pub struct Tick { + pub owned: bool, + pub began: bool, + pub ended: bool, + pub target: Option<[f64; NUM_JOINTS]>, +} + +impl Executor { + fn new(rx: mpsc::Receiver) -> Self { + Self { + rx, + timeline: VecDeque::with_capacity(proto::MAX_ACTION_STEPS), + status: ActionStatus::default(), + generation: 0, + serial: 0, + first_chunk_deadline: 0, + } + } + + fn owns(&self) -> bool { + matches!( + self.status.phase, + ActionPhase::Waiting | ActionPhase::Running + ) + } + + fn finish(&mut self, reason: &str) { + if self.owns() { + tracing::info!(session = ?self.status.session_id, reason, "action session ended"); + self.timeline.clear(); + self.status.phase = ActionPhase::Ended; + self.status.reason = Some(reason.to_owned()); + self.status.remaining = 0; + } + } + + /// Called only by the motor loop; fresh state and power/mode gates remain authoritative. + pub fn tick( + &mut self, + bridge: &Bridge, + now: u64, + ready: bool, + positions: [f64; NUM_JOINTS], + ) -> Tick { + let owned_before = self.owns(); + let generation = bridge.generation.load(Ordering::Acquire); + if generation != self.generation { + self.finish("operator_preempted"); + self.generation = generation; + } + if !ready { + self.finish("body_not_ready"); + } + self.status.t_ns = now; + let mut began = false; + // Bound work per tick even if an IPC client keeps filling the mailbox. + for _ in 0..CAPACITY { + let Ok(pending) = self.rx.try_recv() else { + break; + }; + if pending.answer.is_closed() { + continue; + } + let answer = if pending.generation != self.generation { + Err(refused("request was superseded by an operator command")) + } else { + self.accept(pending.call, now, ready, positions, &mut began) + }; + self.publish(bridge); + let _ = pending.answer.send(answer); + } + // Drop expired intervals; do not burst missed samples or stretch old actions. + while self + .timeline + .front() + .is_some_and(|f| f.at + proto::ACTION_STEP_NS <= now) + { + self.timeline.pop_front(); + } + let target = self.timeline.iter().rev().find(|f| f.at <= now).map(|f| { + self.status.selected_sequence = Some(f.sequence); + self.status.selected_index = Some(f.index); + f.target + }); + if self.owns() { + if self.timeline.is_empty() + && (self.status.sequence.is_some() || now >= self.first_chunk_deadline) + { + self.finish("buffer_exhausted"); + } else { + self.status.phase = if target.is_some() { + ActionPhase::Running + } else { + ActionPhase::Waiting + }; + } + } + self.status.remaining = self.timeline.len(); + self.publish(bridge); + Tick { + owned: self.owns(), + began, + ended: owned_before && !self.owns(), + target, + } + } + + fn accept( + &mut self, + call: proto::Call, + now: u64, + ready: bool, + positions: [f64; NUM_JOINTS], + began: &mut bool, + ) -> Answer { + match call { + proto::Call::RobotActionsBegin => { + if !ready || self.owns() { + return Err(refused( + "body is not ready or another action session is active", + )); + } + self.serial += 1; + let id = format!("{now:x}-{:x}", self.serial); + self.status = ActionStatus { + session_id: Some(id.clone()), + phase: ActionPhase::Waiting, + t_ns: now, + ..Default::default() + }; + self.timeline.clear(); + self.first_chunk_deadline = now + HORIZON_NS; + *began = true; + tracing::info!(session = id, "action session acquired joints"); + Ok(serde_json::to_value(proto::ActionSession { + session_id: id, + t_ns: now, + step_ns: proto::ACTION_STEP_NS, + positions, + joint_names: proto::JOINT_NAMES.iter().map(|s| (*s).to_owned()).collect(), + }) + .unwrap()) + } + proto::Call::RobotActionsSubmit(p) => { + if !self.owns() || self.status.session_id.as_deref() != Some(&p.session_id) { + return Err(refused("action session is no longer active")); + } + validate(&p)?; + if self.status.sequence.is_some_and(|s| p.sequence <= s) { + return Err(invalid("sequence must increase within the session")); + } + let end = p.start_t_ns + p.step_ns * p.positions.len() as u64; + if p.observation_t_ns > now + || p.observation_t_ns > p.start_t_ns + || now.saturating_sub(p.observation_t_ns) > HORIZON_NS + || end <= now + || end - now > HORIZON_NS + { + return Err(invalid( + "expired observation/chunk or timeline outside the two-second horizon", + )); + } + if self + .timeline + .back() + .is_some_and(|f| p.start_t_ns > f.at + p.step_ns) + { + return Err(invalid( + "replacement must overlap or continue the buffered timeline", + )); + } + let prefix = self + .timeline + .iter() + .filter(|f| f.at < p.start_t_ns && f.at + p.step_ns > now) + .count(); + let incoming = (0..p.positions.len()) + .filter(|i| p.start_t_ns + (*i as u64 + 1) * p.step_ns > now) + .count(); + if prefix + incoming > proto::MAX_ACTION_STEPS { + return Err(invalid("combined action buffer exceeds 100 intervals")); + } + // Retain the old prefix until takeover, discard its entire obsolete tail. + self.timeline + .retain(|f| f.at < p.start_t_ns && f.at + proto::ACTION_STEP_NS > now); + for (index, target) in p.positions.into_iter().enumerate() { + let at = p.start_t_ns + index as u64 * p.step_ns; + if at + p.step_ns > now { + self.timeline.push_back(Frame { + at, + sequence: p.sequence, + index, + target, + }); + } + } + self.status.sequence = Some(p.sequence); + tracing::debug!( + sequence = p.sequence, + remaining = self.timeline.len(), + "action chunk accepted" + ); + Ok( + serde_json::json!({"accepted": true, "sequence": p.sequence, "remaining": self.timeline.len()}), + ) + } + proto::Call::RobotActionsEnd(p) => { + if !self.owns() || self.status.session_id.as_deref() != Some(&p.session_id) { + return Err(refused("action session is no longer active")); + } + self.finish("cancelled"); + Ok(serde_json::json!({"accepted": true})) + } + _ => Err(invalid("not an action request")), + } + } + + fn publish(&self, bridge: &Bridge) { + bridge.status.store(Arc::new(self.status.clone())); + } + + pub fn written(&mut self, bridge: &Bridge, ok: bool) { + if self.owns() { + self.status.last_write_ok = Some(ok); + if !ok { + self.finish("bus_write_failed"); + } + self.publish(bridge); + } + } +} + +#[cfg(test)] +#[path = "action_chunk_tests.rs"] +mod tests; diff --git a/robotd/src/action_chunk_tests.rs b/robotd/src/action_chunk_tests.rs new file mode 100644 index 0000000..739565e --- /dev/null +++ b/robotd/src/action_chunk_tests.rs @@ -0,0 +1,225 @@ +use super::*; + +fn setup() -> (Bridge, Executor) { + let bridge = Bridge::new(); + bridge.available.store(true, Ordering::Release); + let mut executor = bridge.executor(); + executor + .accept( + proto::Call::RobotActionsBegin, + 1_000_000_000, + true, + [0.0; 15], + &mut false, + ) + .unwrap(); + (bridge, executor) +} + +fn chunk(executor: &Executor, sequence: u64, start: u64) -> proto::Call { + proto::Call::RobotActionsSubmit(proto::ActionChunkParams { + session_id: executor.status.session_id.clone().unwrap(), + sequence, + observation_t_ns: 1_000_000_000, + start_t_ns: start, + step_ns: proto::ACTION_STEP_NS, + positions: (1..=5).map(|i| [i as f64 / 10.0; 15]).collect(), + }) +} + +fn accept(executor: &mut Executor, call: proto::Call, now: u64) -> Answer { + executor.accept(call, now, true, [0.0; 15], &mut false) +} + +#[test] +fn late_inference_skips_expired_prefix_and_does_not_burst() { + let (bridge, mut executor) = setup(); + let request = chunk(&executor, 1, 1_000_000_000); + accept(&mut executor, request, 1_040_000_000).unwrap(); + assert_eq!( + executor + .tick(&bridge, 1_040_000_000, true, [0.0; 15]) + .target, + Some([0.3; 15]) + ); + assert_eq!( + executor + .tick(&bridge, 1_080_000_000, true, [0.0; 15]) + .target, + Some([0.5; 15]) + ); + let exhausted = executor.tick(&bridge, 1_100_000_000, true, [0.5; 15]); + assert!(exhausted.ended); + assert!(!exhausted.owned); + assert_eq!(executor.status.reason.as_deref(), Some("buffer_exhausted")); +} + +#[test] +fn future_replacement_preserves_prefix_and_discards_old_tail() { + let (bridge, mut executor) = setup(); + let old = chunk(&executor, 1, 1_000_000_000); + accept(&mut executor, old, 1_000_000_000).unwrap(); + let next = chunk(&executor, 2, 1_040_000_000); + accept(&mut executor, next, 1_000_000_000).unwrap(); + assert_eq!( + executor + .tick(&bridge, 1_020_000_000, true, [0.0; 15]) + .target, + Some([0.2; 15]) + ); + assert_eq!( + executor + .tick(&bridge, 1_040_000_000, true, [0.0; 15]) + .target, + Some([0.1; 15]) + ); + assert_eq!(executor.status.selected_sequence, Some(2)); +} + +#[test] +fn expired_and_out_of_order_responses_leave_current_timeline_untouched() { + let (bridge, mut executor) = setup(); + let good = chunk(&executor, 2, 1_020_000_000); + accept(&mut executor, good, 1_000_000_000).unwrap(); + for (sequence, at, now) in [ + (1, 1_020_000_000, 1_020_000_000), + (3, 1_000_000_000, 1_100_000_000), + ] { + let invalid = chunk(&executor, sequence, at); + assert!(accept(&mut executor, invalid, now).is_err()); + } + assert_eq!( + executor + .tick(&bridge, 1_100_000_000, true, [0.0; 15]) + .target, + Some([0.5; 15]) + ); +} + +#[test] +fn cancellation_and_body_failure_invalidate_session() { + for operator_stop in [false, true] { + let (bridge, mut executor) = setup(); + let late = chunk(&executor, 1, 1_000_000_000); + if operator_stop { + bridge.interrupt(); + } + let tick = executor.tick(&bridge, 1_000_000_000, operator_stop, [0.0; 15]); + assert!(tick.ended); + assert!(accept(&mut executor, late, 1_020_000_000).is_err()); + } +} + +#[test] +fn end_and_new_begin_cannot_resurrect_a_previous_session() { + let (_, mut executor) = setup(); + let late = chunk(&executor, 1, 1_000_000_000); + let end = proto::Call::RobotActionsEnd(proto::ActionEndParams { + session_id: executor.status.session_id.clone().unwrap(), + }); + accept(&mut executor, end, 1_000_000_000).unwrap(); + accept(&mut executor, proto::Call::RobotActionsBegin, 1_020_000_000).unwrap(); + assert!(accept(&mut executor, late, 1_020_000_000).is_err()); +} + +#[test] +fn rejects_wrong_frequency_nonfinite_out_of_range_and_overflow() { + let (_, executor) = setup(); + let proto::Call::RobotActionsSubmit(template) = chunk(&executor, 1, 1_000_000_000) else { + unreachable!() + }; + for value in [f64::NAN, f64::INFINITY, 4.0] { + let mut p = template.clone(); + p.positions[0][0] = value; + assert!(validate(&p).is_err()); + } + let mut p = template.clone(); + p.step_ns = 1; + assert!(validate(&p).is_err()); + let mut p = template; + p.start_t_ns = u64::MAX; + assert!(validate(&p).is_err()); +} + +#[tokio::test] +async fn acknowledgement_waits_for_loop_and_stop_invalidates_pending_requests() { + let bridge = Arc::new(Bridge::new()); + bridge.available.store(true, Ordering::Release); + let mut executor = bridge.executor(); + let b = bridge.clone(); + let request = tokio::spawn(async move { b.request(proto::Call::RobotActionsBegin).await }); + tokio::task::yield_now().await; + assert!(!request.is_finished()); + bridge.interrupt(); + executor.tick(&bridge, 1_000_000_000, true, [0.0; 15]); + assert!(request.await.unwrap().is_err()); + assert!(!executor.owns()); +} + +#[test] +fn no_initial_chunk_times_out_and_write_failure_releases_control() { + let (bridge, mut executor) = setup(); + assert!(executor.tick(&bridge, 3_000_000_000, true, [0.0; 15]).ended); + accept(&mut executor, proto::Call::RobotActionsBegin, 3_000_000_000).unwrap(); + executor.written(&bridge, false); + assert!(!executor.owns()); + assert_eq!(executor.status.last_write_ok, Some(false)); + assert_eq!(executor.status.reason.as_deref(), Some("bus_write_failed")); +} + +#[tokio::test] +async fn hardware_path_is_unavailable() { + assert!( + Bridge::new() + .request(proto::Call::RobotActionsBegin) + .await + .is_err() + ); +} + +#[test] +fn rejected_gap_or_overfull_replacement_preserves_accepted_timeline() { + let (bridge, mut executor) = setup(); + let old = chunk(&executor, 1, 1_000_000_000); + accept(&mut executor, old, 1_000_000_000).unwrap(); + let gap = chunk(&executor, 2, 1_120_000_000); + assert!(accept(&mut executor, gap, 1_000_000_000).is_err()); + // A half-step offset can fit inside the time horizon yet exceed 100 entries. + let proto::Call::RobotActionsSubmit(mut full) = chunk(&executor, 2, 1_010_000_000) else { + unreachable!() + }; + full.positions = vec![[0.7; 15]; 100]; + assert!( + accept( + &mut executor, + proto::Call::RobotActionsSubmit(full), + 1_010_000_000 + ) + .is_err() + ); + assert_eq!(executor.status.sequence, Some(1)); + assert_eq!( + executor + .tick(&bridge, 1_020_000_000, true, [0.0; 15]) + .target, + Some([0.2; 15]) + ); +} + +#[test] +fn abandoned_admission_cannot_acquire_joints_later() { + let bridge = Bridge::new(); + let mut executor = bridge.executor(); + let (answer, reply) = oneshot::channel(); + bridge + .tx + .try_send(Pending { + generation: 0, + call: proto::Call::RobotActionsBegin, + answer, + }) + .unwrap(); + drop(reply); + executor.tick(&bridge, 1_000_000_000, true, [0.0; 15]); + assert!(!executor.owns()); +} diff --git a/robotd/src/main.rs b/robotd/src/main.rs index ab574b6..04d60ed 100644 --- a/robotd/src/main.rs +++ b/robotd/src/main.rs @@ -18,6 +18,7 @@ //! publishes and never calls into the loop — a wedged loop reports itself unhealthy rather //! than hanging the caller. +mod action_chunk; mod chorale; mod control; mod intents; @@ -523,6 +524,7 @@ fn drop_unloadable_overrides_with( } struct RobotState { + actions: action_chunk::Bridge, /// Epoch for every timestamp below. `Instant` so the clock cannot go backwards. started: Instant, ticks: AtomicU64, @@ -688,6 +690,7 @@ impl RobotState { force_busy: bool, ) -> Self { Self { + actions: action_chunk::Bridge::new(), started: Instant::now(), ticks: AtomicU64::new(0), missed: AtomicU64::new(0), @@ -986,6 +989,11 @@ async fn main() -> ExitCode { args.busy, )); + state.actions.available.store( + (args.fake || args.sim.is_some()) && state.period_us == 20_000, + Ordering::Release, + ); + if args.unhealthy { tracing::warn!("--unhealthy: will report unhealthy, so updates will roll back"); } @@ -1831,6 +1839,7 @@ async fn control_loop( "control loop running" ); + let mut actions = state.actions.executor(); let mut ticker = tokio::time::interval(period); // `Skip`, not `Burst` and not `Delay`. // @@ -2890,7 +2899,42 @@ 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. - let driving = snapshot.enabled + // The motor loop is the only owner of both chunk admission and target selection. + let action_tick = actions.tick( + &state.actions, + proto::clock::monotonic_ns(), + state.actions.available.load(Ordering::Acquire) + && fresh.is_some() + && bringup == Bringup::Ready + && imu_warm + && !in_limp_fall + && !powered_off + && shutdown_sit.is_none() + && mode_change.is_none() + && pending_swap.is_none(), + coast.known_positions(hold), + ); + if action_tick.began { + intents.set_enabled(false); + intents.stop(); + if let Some(controller) = controller.as_mut() { + controller.reset(); + } + if let Some(instrument) = theremin.as_mut() { + instrument.set_active(false); + } + if let Some(ensemble) = chorale.as_mut() { + ensemble.set_active(false, tick_start, None); + } + let _ = intents.take_theremin_request(); + let _ = intents.take_chorale_request(); + was_driving = false; + } + if action_tick.began || action_tick.ended { + // Capture once; continuously following measurements would sag under gravity. + hold = coast.known_positions(hold); + } + let driving = !action_tick.owned && !action_tick.ended && !action_tick.began && snapshot.enabled && bringup == Bringup::Ready && controller.is_some() // The limp-fall sequence owns the robot for its duration: the whole point is @@ -2983,6 +3027,12 @@ async fn control_loop( ), LimpFall::Idle => unreachable!("in_limp_fall excludes Idle"), }, + _ if action_tick.owned => ( + action_tick.target.unwrap_or(hold), + policy_cfg.gain, + true, + "action_chunk".into(), + ), (true, Some(sensors)) => { let controller = controller.as_mut().expect("driving implies a controller"); match controller.step(sensors, &command, snapshot.pose.active, dt, scale_mult) { @@ -3017,7 +3067,9 @@ async fn control_loop( // mouth opening. Before the mouth is written, because while an instrument is up it // *is* what the mouth is doing — the intent from a client is not competing with it. let mut theremin_state = None; - if let Some(instrument) = theremin.as_mut() { + if !action_tick.owned + && let Some(instrument) = theremin.as_mut() + { // Whether an instrument could be picked up right now, for the IPC side to // refuse on. Republished every tick because the sensor can go away under a // running daemon. @@ -3082,7 +3134,9 @@ async fn control_loop( // The chorale: where in the piece the ensemble is, and this duck's line of it. Before the // mouth, like the theremin, because while a duck is singing its beak is doing that. let mut chorale_state = None; - if let Some(ensemble) = chorale.as_mut() { + if !action_tick.owned + && let Some(ensemble) = chorale.as_mut() + { if let Some((active, piece_pin)) = intents.take_chorale_request() { ensemble.set_active(active, tick_start, piece_pin); } @@ -3195,8 +3249,17 @@ async fn control_loop( } match safety.apply(targets, hold, gain) { - Ok(applied) => limits.extend(applied.limits), - Err(e) => tracing::warn!(error = %e, "bus write failed"), + Ok(applied) => { + limits.extend(applied.limits); + actions.written(&state.actions, true); + } + Err(e) => { + actions.written(&state.actions, false); + if action_tick.owned { + hold = coast.known_positions(hold); + } + tracing::warn!(error = %e, "bus write failed"); + } } // Only assemble a frame when somebody is subscribed. On a robot nobody usually is, @@ -3206,6 +3269,11 @@ async fn control_loop( && let Some(sensors) = sensors.as_ref() { let _ = state.state_tx.send(proto::RobotState { + actions: state + .actions + .available + .load(Ordering::Acquire) + .then(|| (*state.actions.status.load_full()).clone()), t: state.started.elapsed().as_secs_f64(), movement: proto::MoveState { requested: snapshot.command.twist, @@ -3733,6 +3801,14 @@ async fn handle( } let response = match call { + Ok( + call @ (proto::Call::RobotActionsBegin + | proto::Call::RobotActionsSubmit(_) + | proto::Call::RobotActionsEnd(_)), + ) => match state.actions.request(call).await { + Ok(value) => proto::Response::ok(Some(id), &value), + Err(error) => proto::Response::err(Some(id), error), + }, Ok(call) => dispatch(&state, &intents, id, &call), Err(e) => proto::Response::err(Some(id), e), }; @@ -3748,6 +3824,9 @@ async fn handle( /// client that sends `robot.move` with an `id` is not silently ignored — the spec permits /// either, and refusing one because of a framing choice would be a surprise. fn apply_intent(state: &RobotState, intents: &Intents, call: &proto::Call) -> bool { + if state.actions.check_call(call).is_err() { + return false; + } match call { proto::Call::RobotMove(p) => { intents.set_twist([p.vx, p.vy, p.vyaw]); @@ -4301,6 +4380,9 @@ fn dispatch( id: proto::Id, call: &proto::Call, ) -> proto::Response { + if let Err(error) = state.actions.check_call(call) { + return proto::Response::err(Some(id), error); + } match call { proto::Call::RobotMove(_) | proto::Call::RobotHead(_) diff --git a/robotd/tests/action_chunk_ipc.rs b/robotd/tests/action_chunk_ipc.rs new file mode 100644 index 0000000..9caa75c --- /dev/null +++ b/robotd/tests/action_chunk_ipc.rs @@ -0,0 +1,165 @@ +//! The real daemon, IPC, control loop and FakeIo — never a replacement executor. +use std::io::{BufRead, BufReader, Write}; +use std::os::unix::net::UnixStream; +use std::path::PathBuf; +use std::process::{Child, Command, Stdio}; +use std::time::{Duration, Instant}; + +use duck_ipc_proto as proto; +use serde_json::{Value, json}; + +struct Daemon { + child: Child, + socket: PathBuf, + _dir: tempfile::TempDir, +} + +impl Drop for Daemon { + fn drop(&mut self) { + let _ = self.child.kill(); + let _ = self.child.wait(); + } +} + +impl Daemon { + fn spawn() -> Self { + let dir = tempfile::tempdir().unwrap(); + let socket = dir.path().join("robot.sock"); + let params = dir.path().join("robotd.toml"); + std::fs::write(¶ms, "[audio]\nenabled=false\n[chorale]\naccept=false\n").unwrap(); + let child = Command::new(env!("CARGO_BIN_EXE_robotd")) + .args(["--fake", "--no-policy", "--socket"]) + .arg(&socket) + .arg("--params") + .arg(params) + .env("RUST_LOG", "error") + .stdout(Stdio::null()) + .spawn() + .unwrap(); + let daemon = Self { + child, + socket, + _dir: dir, + }; + let deadline = Instant::now() + Duration::from_secs(5); + while !daemon.socket.exists() { + assert!(Instant::now() < deadline, "robotd did not open its socket"); + std::thread::sleep(Duration::from_millis(10)); + } + daemon + } + + fn call(&self, method: &str, params: Value) -> Value { + let mut stream = UnixStream::connect(&self.socket).unwrap(); + stream + .set_read_timeout(Some(Duration::from_secs(2))) + .unwrap(); + writeln!( + stream, + "{}", + json!({"jsonrpc":"2.0","id":1,"method":method,"params":params}) + ) + .unwrap(); + let mut line = String::new(); + BufReader::new(stream).read_line(&mut line).unwrap(); + serde_json::from_str(&line).unwrap() + } + + fn subscribe(&self) -> BufReader { + let mut stream = UnixStream::connect(&self.socket).unwrap(); + stream + .set_read_timeout(Some(Duration::from_secs(3))) + .unwrap(); + writeln!( + stream, + "{}", + json!({"jsonrpc":"2.0","id":2,"method":"robot.subscribe","params":{"hz":50}}) + ) + .unwrap(); + BufReader::new(stream) + } +} + +fn state( + reader: &mut BufReader, + predicate: impl Fn(&proto::RobotState) -> bool, +) -> proto::RobotState { + let deadline = Instant::now() + Duration::from_secs(4); + loop { + assert!(Instant::now() < deadline, "no matching execution feedback"); + let mut line = String::new(); + reader.read_line(&mut line).unwrap(); + let value: Value = serde_json::from_str(&line).unwrap(); + if value["method"] == "robot.state" { + let frame: proto::RobotState = serde_json::from_value(value["params"].clone()).unwrap(); + if predicate(&frame) { + return frame; + } + } + } +} + +#[test] +fn chunk_drives_joints_and_stop_cancels_the_tail_and_late_response() { + let daemon = Daemon::spawn(); + assert!(daemon.call("robot.init", json!({}))["error"].is_null()); + let deadline = Instant::now() + Duration::from_secs(6); + let session = loop { + let response = daemon.call("robot.actions.begin", json!({})); + if response["error"].is_null() { + break response["result"].clone(); + } + assert!(Instant::now() < deadline, "begin refused: {response}"); + std::thread::sleep(Duration::from_millis(20)); + }; + assert_eq!(session["joint_names"].as_array().unwrap().len(), 15); + let mut stream = daemon.subscribe(); + let initial = state(&mut stream, |s| { + s.actions + .as_ref() + .is_some_and(|a| a.phase == proto::ActionPhase::Waiting) + }); + let base: [f64; 15] = serde_json::from_value(session["positions"].clone()).unwrap(); + let mut first = base; + first[7] += 0.15; + let mut tail = base; + tail[7] -= 0.15; + let positions: Vec<_> = (0..75).map(|i| if i < 30 { first } else { tail }).collect(); + let chunk = json!({ + "session_id":session["session_id"], "sequence":1, + "observation_t_ns":initial.t_ns, + "start_t_ns":initial.actions.as_ref().unwrap().t_ns + 100_000_000, + "step_ns":20_000_000, "positions":positions, + }); + let accepted = daemon.call("robot.actions.submit", chunk.clone()); + assert_eq!(accepted["result"]["accepted"], true, "{accepted}"); + let reached = state(&mut stream, |s| (s.joints[7] - first[7]).abs() < 1e-6); + assert_eq!(reached.policy, "action_chunk"); + assert_eq!(reached.actions.as_ref().unwrap().last_write_ok, Some(true)); + assert_eq!( + daemon.call( + "robot.head", + json!({"neck_pitch":0.0,"head_pitch":0.0,"head_yaw":-1.0,"head_roll":0.0}) + )["error"]["code"], + proto::code::BUSY + ); + assert!(daemon.call("robot.stop", json!({}))["error"].is_null()); + let stopped = state(&mut stream, |s| { + s.actions + .as_ref() + .is_some_and(|a| a.phase == proto::ActionPhase::Ended) + }); + assert_eq!( + stopped.actions.as_ref().unwrap().reason.as_deref(), + Some("operator_preempted") + ); + let mut late = chunk; + late["sequence"] = json!(2); + assert!(!daemon.call("robot.actions.submit", late)["error"].is_null()); + let after_tail = state(&mut stream, |s| s.t_ns > stopped.t_ns + 800_000_000); + assert!( + (after_tail.joints[7] - first[7]).abs() < 1e-6, + "cancelled tail still moved the joint" + ); + assert_eq!(after_tail.policy, "held"); +} diff --git a/updater/src/ipc.rs b/updater/src/ipc.rs index 151afdb..b30fea5 100644 --- a/updater/src/ipc.rs +++ b/updater/src/ipc.rs @@ -780,7 +780,7 @@ impl Server { | Call::RobotHealth | Call::RobotModelApi | Call::RobotRemoteSessionActive - | Call::RobotMove(_) + | Call::RobotActionsBegin | Call::RobotActionsSubmit(_) | Call::RobotActionsEnd(_) | Call::RobotMove(_) | Call::RobotHead(_) | Call::RobotLook(_) | Call::RobotStop