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