diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 4309212..ddf52d6 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -34,7 +34,7 @@ jobs: - name: http-sim command: cargo check --no-default-features --features sim,http - name: hardware-adapter - command: cargo check --no-default-features --features sim,http,mcp,waveshare-ugv,mavlink-drone,bridge-compat + command: cargo check --no-default-features --features sim,http,mcp,waveshare-ugv,mavlink-drone,manipulator,bridge-compat - name: all-features command: cargo check --all-features steps: diff --git a/Cargo.toml b/Cargo.toml index 8cd6a95..456bff2 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -32,6 +32,7 @@ http = ["dep:axum", "dep:futures-util", "dep:tower-http"] mcp = ["dep:rmcp", "dep:schemars"] waveshare-ugv = ["dep:serialport"] mavlink-drone = [] +manipulator = [] bridge-compat = ["http"] cuda = [] diff --git a/README.md b/README.md index 2997093..cf4546f 100644 --- a/README.md +++ b/README.md @@ -329,6 +329,7 @@ required gates without changing core command safety policy. | bridge compatibility | `compatibility` | `beta` | `sim`, `bridge-compat` | none | | Waveshare UGV | `mobile-base` | `alpha` | `waveshare-ugv` | `physical-actuation`, `policy-token-or-approval` | | MAVLink drone | `drone` | `experimental` | `mavlink-drone` | `physical-actuation`, `policy-token-or-approval` | +| manipulator | `manipulator` | `experimental` | `manipulator` | `physical-actuation`, `policy-token-or-approval` | Maturity levels are: @@ -354,6 +355,7 @@ token gate their commands require. | `mcp` | MCP stdio server | ✓ | | `waveshare-ugv` | Waveshare UGV physical adapter | opt-in | | `mavlink-drone` | MAVLink drone adapter skeleton, sim/replay proof, and gated physical profile | opt-in | +| `manipulator` | Manipulator adapter skeleton, mock arm teleop, and gated physical profile | opt-in | | `bridge-compat` | Legacy robot bridge compatibility | opt-in | ## MAVLink Drone Skeleton @@ -382,6 +384,31 @@ Flight capabilities remain policy-gated high-risk actions. The current physical profile is a skeleton and refuses real flight command execution until a concrete MAVLink adapter is wired behind this boundary. +## Manipulator Skeleton + +The `manipulator` feature adds a mock arm teleop boundary for joint state, +joint command, pose command, home, and stop flows. Joint and pose command +responses carry the `leash-manipulator-v1` schema version so clients can pin +against the skeleton while a real IK or planning provider is still external. + +```bash +cargo run --features manipulator -- list +cargo run --features manipulator -- run manipulator-sim +cargo run --features manipulator -- run manipulator-replay --replay-source examples/replay/sim-basic.jsonl +``` + +Physical manipulator startup is gated just like other physical adapters: + +```bash +LEASH_ALLOW_PHYSICAL_ACTUATION=1 \ +cargo run --features manipulator -- run manipulator-http --allow-physical-actuation +``` + +The current physical profile is a skeleton and refuses real joint or pose +execution until a concrete adapter is wired. IK, collision checking, motion +planning, retries, calibration, and native SDK bindings belong out-of-process or +in a future feature/example adapter crate, not in the core command registry. + ## Smoke Tests ```bash diff --git a/scripts/smoke-all.sh b/scripts/smoke-all.sh index c49d6a3..301fd88 100755 --- a/scripts/smoke-all.sh +++ b/scripts/smoke-all.sh @@ -124,6 +124,38 @@ const checks = [ return "mavlink drone sim/replay/physical stack metadata resolved without hardware"; }, }, + { + name: "stack-catalog-manipulator", + argv: ["cargo", "run", "--quiet", "--features", "manipulator", "--", "list", "--format", "json"], + validate: (stdout) => { + const stacks = JSON.parse(stdout); + const byName = new Map(stacks.map((stack) => [stack.name, stack])); + for (const required of ["manipulator-sim", "manipulator-replay", "manipulator-http"]) { + if (!byName.has(required)) { + throw new Error(`missing stack ${required}`); + } + } + const sim = byName.get("manipulator-sim"); + const replay = byName.get("manipulator-replay"); + const physical = byName.get("manipulator-http"); + if (sim.hardware_required || replay.hardware_required) { + throw new Error("manipulator sim/replay stacks should not require hardware"); + } + if (!physical.hardware_required) { + throw new Error("manipulator physical stack did not declare hardware_required"); + } + if (sim.adapter.category !== "manipulator" || replay.adapter.category !== "manipulator" || physical.adapter.category !== "manipulator") { + throw new Error("manipulator stacks did not declare manipulator adapter metadata"); + } + if (!sim.adapter.capabilities.includes("manipulator_joint_state") || !replay.adapter.capabilities.includes("manipulator_pose_command")) { + throw new Error("manipulator sim/replay stacks did not expose manipulator capabilities"); + } + if (!physical.adapter.required_gates.includes("physical-actuation")) { + throw new Error("manipulator physical stack did not declare physical-actuation gate"); + } + return "manipulator sim/replay/physical stack metadata resolved without hardware"; + }, + }, { name: "graph-sim-json", argv: ["cargo", "run", "--quiet", "--", "graph", "sim", "--format", "json"], diff --git a/scripts/smoke-physical-gate.sh b/scripts/smoke-physical-gate.sh index 7382231..89cfe84 100755 --- a/scripts/smoke-physical-gate.sh +++ b/scripts/smoke-physical-gate.sh @@ -28,4 +28,7 @@ check_refuses_without_gate "waveshare-ugv" \ check_refuses_without_gate "mavlink-drone" \ cargo run --quiet --features mavlink-drone -- serve http --profile mavlink-drone --listen 127.0.0.1:18082 --mavlink-endpoint udp://127.0.0.1:14550 +check_refuses_without_gate "manipulator" \ + cargo run --quiet --features manipulator -- serve http --profile manipulator --listen 127.0.0.1:18083 + echo "physical gate smoke ok" diff --git a/src/bin/leash.rs b/src/bin/leash.rs index b7d66ea..43d865d 100644 --- a/src/bin/leash.rs +++ b/src/bin/leash.rs @@ -788,6 +788,7 @@ fn resolve_config_stack(name: &str) -> Result { "replay" => Profile::Replay, "waveshare-ugv" => Profile::WaveshareUgv, "mavlink-drone" => Profile::MavlinkDrone, + "manipulator" => Profile::Manipulator, other => { let stacks = built_in_stacks() .into_iter() @@ -795,7 +796,7 @@ fn resolve_config_stack(name: &str) -> Result { .collect::>() .join(", "); bail!( - "unknown stack or profile '{other}'; expected sim, replay, waveshare-ugv, mavlink-drone, or one of: {stacks}" + "unknown stack or profile '{other}'; expected sim, replay, waveshare-ugv, mavlink-drone, manipulator, or one of: {stacks}" ); } }; diff --git a/src/capability.rs b/src/capability.rs index ca54bb9..24e6265 100644 --- a/src/capability.rs +++ b/src/capability.rs @@ -405,6 +405,52 @@ impl CapabilityRegistry { )?) .map_err(Into::into) } + #[cfg(feature = "manipulator")] + "manipulator_joint_state" => { + ensure_fields(&args, &[])?; + serde_json::to_value(self.harness.manipulator_joint_state()).map_err(Into::into) + } + #[cfg(feature = "manipulator")] + "manipulator_joint_command" => { + ensure_fields(&args, &["token", "joints"])?; + let token = optional_string(&args, "token")?; + let joints = args + .get("joints") + .cloned() + .ok_or_else(|| anyhow!("joints is required"))?; + serde_json::to_value(self.harness.manipulator_command( + "joint_command", + token.as_deref(), + json!({ "joints": joints }), + )?) + .map_err(Into::into) + } + #[cfg(feature = "manipulator")] + "manipulator_pose_command" => { + ensure_fields(&args, &["token", "pose"])?; + let token = optional_string(&args, "token")?; + let pose = args + .get("pose") + .cloned() + .ok_or_else(|| anyhow!("pose is required"))?; + serde_json::to_value(self.harness.manipulator_command( + "pose_command", + token.as_deref(), + json!({ "pose": pose }), + )?) + .map_err(Into::into) + } + #[cfg(feature = "manipulator")] + "manipulator_home" => { + ensure_fields(&args, &["token"])?; + let token = optional_string(&args, "token")?; + serde_json::to_value(self.harness.manipulator_command( + "home", + token.as_deref(), + json!({}), + )?) + .map_err(Into::into) + } other => Err(anyhow!("unknown capability '{other}'")), } } @@ -695,16 +741,25 @@ pub fn default_capability_descriptors() -> Vec { "SpatialMemoryStatus", ), ]; + feature_capability_descriptors(descriptors) +} + +#[cfg(any(feature = "mavlink-drone", feature = "manipulator"))] +fn feature_capability_descriptors( + mut descriptors: Vec, +) -> Vec { #[cfg(feature = "mavlink-drone")] - { - let mut descriptors = descriptors; - descriptors.extend(drone_capability_descriptors()); - descriptors - } - #[cfg(not(feature = "mavlink-drone"))] - { - descriptors - } + descriptors.extend(drone_capability_descriptors()); + #[cfg(feature = "manipulator")] + descriptors.extend(manipulator_capability_descriptors()); + descriptors +} + +#[cfg(not(any(feature = "mavlink-drone", feature = "manipulator")))] +fn feature_capability_descriptors( + descriptors: Vec, +) -> Vec { + descriptors } #[cfg(feature = "mavlink-drone")] @@ -772,6 +827,48 @@ fn drone_capability_descriptors() -> Vec { ] } +#[cfg(feature = "manipulator")] +fn manipulator_capability_descriptors() -> Vec { + vec![ + descriptor( + "manipulator_joint_state", + "Read the versioned manipulator joint state", + SafetyClass::ObserveOnly, + object_schema(&[]), + "ManipulatorJointState", + ), + descriptor( + "manipulator_joint_command", + "Send a versioned manipulator joint command", + SafetyClass::PhysicalHighRisk, + object_schema(&[ + ("token", "string", false), + ("approval", "boolean", false), + ("joints", "ManipulatorJointCommandV1", true), + ]), + "ManipulatorCommandStatus", + ), + descriptor( + "manipulator_pose_command", + "Send a versioned manipulator end-effector pose command", + SafetyClass::PhysicalHighRisk, + object_schema(&[ + ("token", "string", false), + ("approval", "boolean", false), + ("pose", "ManipulatorPoseCommandV1", true), + ]), + "ManipulatorCommandStatus", + ), + descriptor( + "manipulator_home", + "Move the manipulator to its configured home pose", + SafetyClass::PhysicalHighRisk, + object_schema(&[("token", "string", false), ("approval", "boolean", false)]), + "ManipulatorCommandStatus", + ), + ] +} + fn descriptor( name: &str, description: &str, @@ -837,6 +934,19 @@ fn canonical_name(name: &str) -> &str { | "drone.move-velocity" | "drone/move-velocity" => "drone_move_velocity", "drone.fly_to" | "drone/fly_to" | "drone.fly-to" | "drone/fly-to" => "drone_fly_to", + "manipulator.joint_state" + | "manipulator/joint_state" + | "manipulator.joint-state" + | "manipulator/joint-state" => "manipulator_joint_state", + "manipulator.joint_command" + | "manipulator/joint_command" + | "manipulator.joint-command" + | "manipulator/joint-command" => "manipulator_joint_command", + "manipulator.pose_command" + | "manipulator/pose_command" + | "manipulator.pose-command" + | "manipulator/pose-command" => "manipulator_pose_command", + "manipulator.home" | "manipulator/home" => "manipulator_home", other => other, } } @@ -964,7 +1074,7 @@ fn optional_spatial_memory_kind( #[cfg(test)] mod tests { use super::*; - #[cfg(feature = "mavlink-drone")] + #[cfg(any(feature = "mavlink-drone", feature = "manipulator"))] use crate::config::Profile; use crate::{config::PolicyMode, types::TelemetryFrame, HarnessConfig}; @@ -1089,6 +1199,78 @@ mod tests { assert!(err.contains("gated skeleton")); } + #[cfg(feature = "manipulator")] + #[tokio::test] + async fn manipulator_capabilities_are_simulated_and_policy_gated() { + let harness = Harness::new(HarnessConfig { + policy_mode: PolicyMode::RequireApproval, + ..HarnessConfig::default() + }) + .unwrap(); + let registry = CapabilityRegistry::new(harness); + + let state = registry + .invoke_value("manipulator.joint-state", json!({})) + .unwrap(); + assert_eq!(state["version"], "leash-manipulator-v1"); + assert_eq!(state["simulated"], true); + assert_eq!(state["joints"][0]["name"], "shoulder_pan"); + + let err = registry + .invoke_value( + "manipulator_joint_command", + json!({ "joints": [{ "name": "elbow", "position_rad": 0.4 }] }), + ) + .unwrap_err() + .to_string(); + assert!(err.contains("policy require-approval")); + + let commanded = registry + .invoke_value( + "manipulator.joint-command", + json!({ + "approval": true, + "joints": [{ "name": "elbow", "position_rad": 0.4 }] + }), + ) + .unwrap(); + assert_eq!(commanded["version"], "leash-manipulator-v1"); + assert_eq!(commanded["command"], "joint_command"); + assert_eq!(commanded["simulated"], true); + + let homed = registry + .invoke_value("manipulator_home", json!({ "approval": true })) + .unwrap(); + assert_eq!(homed["command"], "home"); + } + + #[cfg(feature = "manipulator")] + #[tokio::test] + async fn physical_manipulator_profile_stays_a_gated_skeleton() { + let err = match Harness::new(HarnessConfig { + profile: Profile::Manipulator, + ..HarnessConfig::default() + }) { + Ok(_) => panic!("expected manipulator profile to require the physical gate"), + Err(err) => err.to_string(), + }; + assert!(err.contains("LEASH_ALLOW_PHYSICAL_ACTUATION")); + + let harness = Harness::new(HarnessConfig { + profile: Profile::Manipulator, + allow_physical_actuation: true, + policy_mode: PolicyMode::RequireApproval, + ..HarnessConfig::default() + }) + .unwrap(); + let registry = CapabilityRegistry::new(harness); + let err = registry + .invoke_value("manipulator_home", json!({ "approval": true })) + .unwrap_err() + .to_string(); + assert!(err.contains("gated skeleton")); + } + #[tokio::test] async fn patrol_capabilities_start_stop_and_report_status() { let harness = Harness::new(HarnessConfig::default()).unwrap(); diff --git a/src/config.rs b/src/config.rs index f45685e..b234df9 100644 --- a/src/config.rs +++ b/src/config.rs @@ -17,6 +17,7 @@ pub enum Profile { Replay, WaveshareUgv, MavlinkDrone, + Manipulator, } impl Profile { @@ -26,11 +27,15 @@ impl Profile { Self::Replay => "replay", Self::WaveshareUgv => "waveshare-ugv", Self::MavlinkDrone => "mavlink-drone", + Self::Manipulator => "manipulator", } } pub fn is_physical(self) -> bool { - matches!(self, Self::WaveshareUgv | Self::MavlinkDrone) + matches!( + self, + Self::WaveshareUgv | Self::MavlinkDrone | Self::Manipulator + ) } } @@ -443,7 +448,8 @@ fn parse_profile(value: &str) -> anyhow::Result { "replay" => Ok(Profile::Replay), "waveshare-ugv" => Ok(Profile::WaveshareUgv), "mavlink-drone" => Ok(Profile::MavlinkDrone), - _ => anyhow::bail!("expected sim, replay, waveshare-ugv, or mavlink-drone"), + "manipulator" => Ok(Profile::Manipulator), + _ => anyhow::bail!("expected sim, replay, waveshare-ugv, mavlink-drone, or manipulator"), } } @@ -691,6 +697,12 @@ impl ConfigBuilder { self.allow_untokened_drive .set(false, "stack:mavlink-drone".to_string()); } + if self.profile.value == Profile::Manipulator + && self.allow_untokened_drive.source == "default" + { + self.allow_untokened_drive + .set(false, "stack:manipulator".to_string()); + } } fn finish(self) -> ResolvedHarnessConfig { diff --git a/src/module.rs b/src/module.rs index 30f135f..af9f6c1 100644 --- a/src/module.rs +++ b/src/module.rs @@ -318,6 +318,7 @@ pub fn default_module_graph(config: &HarnessConfig, capabilities: Vec) - Profile::Replay => "replay-driver", Profile::WaveshareUgv => "waveshare-ugv-driver", Profile::MavlinkDrone => "mavlink-drone-driver", + Profile::Manipulator => "manipulator-driver", }; let driver_capabilities = match config.profile { Profile::MavlinkDrone => vec![ @@ -330,14 +331,24 @@ pub fn default_module_graph(config: &HarnessConfig, capabilities: Vec) - "stop".to_string(), "estop".to_string(), ], + Profile::Manipulator => vec![ + "manipulator_joint_state".to_string(), + "manipulator_joint_command".to_string(), + "manipulator_pose_command".to_string(), + "manipulator_home".to_string(), + "stop".to_string(), + "estop".to_string(), + ], _ => vec!["drive".to_string(), "stop".to_string(), "estop".to_string()], }; let driver_input = match config.profile { Profile::MavlinkDrone => "FlightCommand", + Profile::Manipulator => "ManipulatorCommand", _ => "DriveReq", }; let driver_output = match config.profile { Profile::MavlinkDrone => "DroneCommandStatus", + Profile::Manipulator => "ManipulatorCommandStatus", _ => "OdometryStatus", }; let transport = config.stream_transport; @@ -378,6 +389,8 @@ pub fn default_module_graph(config: &HarnessConfig, capabilities: Vec) - inputs: vec![stream( if config.profile == Profile::MavlinkDrone { "flight_command" + } else if config.profile == Profile::Manipulator { + "manipulator_command" } else { "drive_command" }, @@ -388,6 +401,8 @@ pub fn default_module_graph(config: &HarnessConfig, capabilities: Vec) - outputs: vec![stream( if config.profile == Profile::MavlinkDrone { "flight_status" + } else if config.profile == Profile::Manipulator { + "manipulator_status" } else { "odometry" }, diff --git a/src/runtime.rs b/src/runtime.rs index 1e7c5bf..034865e 100644 --- a/src/runtime.rs +++ b/src/runtime.rs @@ -17,7 +17,7 @@ use anyhow::{anyhow, Result}; use parking_lot::{Mutex, RwLock}; #[cfg(feature = "waveshare-ugv")] use serde_json::json; -#[cfg(feature = "mavlink-drone")] +#[cfg(any(feature = "mavlink-drone", feature = "manipulator"))] use serde_json::Value; use sha2::{Digest, Sha256}; use tokio::{sync::broadcast, time}; @@ -25,6 +25,10 @@ use tracing::{debug, warn}; #[cfg(feature = "mavlink-drone")] use crate::types::DroneCommandStatus; +#[cfg(feature = "manipulator")] +use crate::types::{ + ManipulatorCommandStatus, ManipulatorJoint, ManipulatorJointState, MANIPULATOR_SCHEMA_VERSION, +}; use crate::{ accelerator::{resolve_accelerator, AcceleratorStatus}, capability::{default_capability_descriptors, CapabilityRegistry}, @@ -141,6 +145,28 @@ impl RobotDriver for MavlinkDroneDriver { } } +#[cfg(feature = "manipulator")] +#[derive(Debug)] +struct ManipulatorDriver; + +#[cfg(feature = "manipulator")] +impl ManipulatorDriver { + fn open(_config: &HarnessConfig) -> Self { + Self + } +} + +#[cfg(feature = "manipulator")] +impl RobotDriver for ManipulatorDriver { + fn drive(&self, left: f64, right: f64) -> Result<()> { + debug!( + left, + right, "manipulator skeleton ignored differential drive command" + ); + Ok(()) + } +} + #[cfg(feature = "waveshare-ugv")] struct WaveshareUgvDriver { writer: Mutex>, @@ -311,6 +337,7 @@ impl Harness { Profile::Replay => Arc::new(ReplayDriver), Profile::WaveshareUgv => open_physical_driver(&config)?, Profile::MavlinkDrone => open_physical_driver(&config)?, + Profile::Manipulator => open_physical_driver(&config)?, }; let raw = match config.profile { @@ -318,6 +345,7 @@ impl Harness { Profile::Replay => RawTelemetry::replay(), Profile::WaveshareUgv => RawTelemetry::physical("waveshare-ugv"), Profile::MavlinkDrone => RawTelemetry::physical("mavlink-drone"), + Profile::Manipulator => RawTelemetry::physical("manipulator"), }; let replay = config @@ -1256,6 +1284,74 @@ impl Harness { }) } + #[cfg(feature = "manipulator")] + pub fn manipulator_joint_state(&self) -> ManipulatorJointState { + let simulated = matches!(self.config.profile, Profile::Sim | Profile::Replay); + ManipulatorJointState { + version: MANIPULATOR_SCHEMA_VERSION.to_string(), + ok: true, + profile: self.config.profile.as_str().to_string(), + simulated, + source: if simulated { + "mock-arm".to_string() + } else { + "manipulator-skeleton".to_string() + }, + joints: vec![ + ManipulatorJoint { + name: "shoulder_pan".to_string(), + position_rad: 0.0, + velocity_radps: 0.0, + effort_nm: Some(0.0), + }, + ManipulatorJoint { + name: "elbow".to_string(), + position_rad: 0.0, + velocity_radps: 0.0, + effort_nm: Some(0.0), + }, + ManipulatorJoint { + name: "wrist".to_string(), + position_rad: 0.0, + velocity_radps: 0.0, + effort_nm: Some(0.0), + }, + ], + } + } + + #[cfg(feature = "manipulator")] + pub fn manipulator_command( + &self, + command: &str, + token: Option<&str>, + args: Value, + ) -> Result { + self.validate_session(token)?; + let simulated = matches!(self.config.profile, Profile::Sim | Profile::Replay); + if self.config.profile == Profile::Manipulator { + return Err(anyhow!( + "manipulator profile is a gated skeleton; configure a concrete manipulator adapter before executing '{command}'" + )); + } + if !simulated { + return Err(anyhow!( + "manipulator capability '{command}' requires sim, replay, or manipulator profile" + )); + } + + Ok(ManipulatorCommandStatus { + version: MANIPULATOR_SCHEMA_VERSION.to_string(), + ok: true, + command: command.to_string(), + profile: self.config.profile.as_str().to_string(), + simulated, + status: "simulated".to_string(), + message: format!("mock manipulator {command} accepted"), + args, + }) + } + fn validate_session(&self, token: Option<&str>) -> Result> { if self.config.allow_untokened_drive && token.is_none() { return Ok(None); @@ -1395,6 +1491,19 @@ fn open_physical_driver(config: &HarnessConfig) -> Result> )) } } + Profile::Manipulator => { + #[cfg(feature = "manipulator")] + { + Ok(Arc::new(ManipulatorDriver::open(config))) + } + #[cfg(not(feature = "manipulator"))] + { + let _ = config; + Err(anyhow!( + "profile 'manipulator' requires building with --features manipulator" + )) + } + } } } diff --git a/src/stack.rs b/src/stack.rs index 26139ac..1786f10 100644 --- a/src/stack.rs +++ b/src/stack.rs @@ -184,24 +184,34 @@ impl Stack { } pub fn built_in_stacks() -> Vec { - let stacks = vec![ + feature_stacks(vec![ sim_http_stack(), sim_mcp_stack(), bridge_compat_http_stack(), waveshare_ugv_http_stack(), - ]; + ]) +} + +#[cfg(any(feature = "mavlink-drone", feature = "manipulator"))] +fn feature_stacks(mut stacks: Vec) -> Vec { #[cfg(feature = "mavlink-drone")] { - let mut stacks = stacks; stacks.push(mavlink_drone_sim_stack()); stacks.push(mavlink_drone_replay_stack()); stacks.push(mavlink_drone_http_stack()); - stacks } - #[cfg(not(feature = "mavlink-drone"))] + #[cfg(feature = "manipulator")] { - stacks + stacks.push(manipulator_sim_stack()); + stacks.push(manipulator_replay_stack()); + stacks.push(manipulator_http_stack()); } + stacks +} + +#[cfg(not(any(feature = "mavlink-drone", feature = "manipulator")))] +fn feature_stacks(stacks: Vec) -> Vec { + stacks } pub fn find_stack(name: &str) -> Option { @@ -229,6 +239,7 @@ pub fn adapter_profile_for_profile(profile: Profile) -> AdapterProfile { Profile::Replay => replay_adapter_profile(), Profile::WaveshareUgv => waveshare_ugv_adapter_profile(), Profile::MavlinkDrone => mavlink_drone_adapter_profile(), + Profile::Manipulator => manipulator_adapter_profile(), } } @@ -382,6 +393,72 @@ fn mavlink_drone_http_stack() -> Stack { } } +#[cfg(feature = "manipulator")] +fn manipulator_sim_stack() -> Stack { + Stack { + name: "manipulator-sim".to_string(), + description: "No-hardware manipulator teleop skeleton with mock joint state".to_string(), + profile: Profile::Sim, + transport: TransportBinding { + kind: StackTransport::Http, + listen: Some(socket("127.0.0.1:8020")), + }, + required_features: strings(&["sim", "http", "manipulator"]), + hardware_required: false, + adapter: manipulator_sim_adapter_profile(), + config_overrides: PartialHarnessConfig { + listen: Some(socket("127.0.0.1:8020")), + ..PartialHarnessConfig::default() + }, + modules: module_refs(Profile::Sim), + command: "leash run manipulator-sim".to_string(), + } +} + +#[cfg(feature = "manipulator")] +fn manipulator_replay_stack() -> Stack { + Stack { + name: "manipulator-replay".to_string(), + description: "Fixture-backed manipulator skeleton for replay proof".to_string(), + profile: Profile::Replay, + transport: TransportBinding { + kind: StackTransport::Mcp, + listen: None, + }, + required_features: strings(&["mcp", "manipulator"]), + hardware_required: false, + adapter: manipulator_replay_adapter_profile(), + config_overrides: PartialHarnessConfig::default(), + modules: module_refs(Profile::Replay), + command: "leash run manipulator-replay --replay-source examples/replay/sim-basic.jsonl" + .to_string(), + } +} + +#[cfg(feature = "manipulator")] +fn manipulator_http_stack() -> Stack { + Stack { + name: "manipulator-http".to_string(), + description: "Gated physical manipulator HTTP runtime skeleton".to_string(), + profile: Profile::Manipulator, + transport: TransportBinding { + kind: StackTransport::Http, + listen: Some(socket("0.0.0.0:8020")), + }, + required_features: strings(&["http", "manipulator"]), + hardware_required: true, + adapter: manipulator_adapter_profile(), + config_overrides: PartialHarnessConfig { + listen: Some(socket("0.0.0.0:8020")), + ..PartialHarnessConfig::default() + }, + modules: module_refs(Profile::Manipulator), + command: + "LEASH_ALLOW_PHYSICAL_ACTUATION=1 leash run manipulator-http --allow-physical-actuation" + .to_string(), + } +} + fn simulation_adapter_profile() -> AdapterProfile { AdapterProfile { category: AdapterCategory::Simulation, @@ -482,12 +559,55 @@ fn mavlink_drone_replay_adapter_profile() -> AdapterProfile { } } +fn manipulator_capabilities() -> Vec { + strings(&[ + "manipulator_joint_state", + "manipulator_joint_command", + "manipulator_pose_command", + "manipulator_home", + "stop", + ]) +} + +fn manipulator_adapter_profile() -> AdapterProfile { + AdapterProfile { + category: AdapterCategory::Manipulator, + maturity: AdapterMaturity::Experimental, + capabilities: manipulator_capabilities(), + feature_flags: strings(&["manipulator"]), + required_gates: strings(&["physical-actuation", "policy-token-or-approval"]), + boundary: "feature-gated manipulator skeleton; IK, planning, and hardware SDK bindings stay out-of-process or in a future adapter crate".to_string(), + } +} + +#[cfg(feature = "manipulator")] +fn manipulator_sim_adapter_profile() -> AdapterProfile { + AdapterProfile { + maturity: AdapterMaturity::Alpha, + required_gates: Vec::new(), + boundary: "mock manipulator teleop adapter; no hardware or IK process required".to_string(), + ..manipulator_adapter_profile() + } +} + +#[cfg(feature = "manipulator")] +fn manipulator_replay_adapter_profile() -> AdapterProfile { + AdapterProfile { + maturity: AdapterMaturity::Beta, + required_gates: Vec::new(), + boundary: "replay-backed manipulator capability surface; deterministic and non-physical" + .to_string(), + ..manipulator_adapter_profile() + } +} + fn module_refs(profile: Profile) -> Vec { let driver = match profile { Profile::Sim => ("sim-driver", false), Profile::Replay => ("replay-driver", false), Profile::WaveshareUgv => ("waveshare-ugv-driver", true), Profile::MavlinkDrone => ("mavlink-drone-driver", true), + Profile::Manipulator => ("manipulator-driver", true), }; vec![ stack_module("harness-runtime", "runtime", true, false), @@ -541,6 +661,14 @@ mod tests { .iter() .any(|stack| stack.name == "mavlink-drone-http")); } + #[cfg(feature = "manipulator")] + { + assert!(stacks.iter().any(|stack| stack.name == "manipulator-sim")); + assert!(stacks + .iter() + .any(|stack| stack.name == "manipulator-replay")); + assert!(stacks.iter().any(|stack| stack.name == "manipulator-http")); + } for stack in stacks { stack.validate().unwrap(); @@ -595,6 +723,35 @@ mod tests { assert!(!drone_sim.hardware_required); assert!(drone_sim.adapter.required_gates.is_empty()); } + + #[cfg(feature = "manipulator")] + { + let manipulator = stacks + .iter() + .find(|stack| stack.name == "manipulator-http") + .unwrap(); + assert_eq!(manipulator.adapter.category, AdapterCategory::Manipulator); + assert_eq!(manipulator.adapter.maturity, AdapterMaturity::Experimental); + assert!(manipulator + .adapter + .capabilities + .contains(&"manipulator_joint_command".to_string())); + assert!(manipulator + .adapter + .required_gates + .contains(&"physical-actuation".to_string())); + + let manipulator_sim = stacks + .iter() + .find(|stack| stack.name == "manipulator-sim") + .unwrap(); + assert_eq!( + manipulator_sim.adapter.category, + AdapterCategory::Manipulator + ); + assert!(!manipulator_sim.hardware_required); + assert!(manipulator_sim.adapter.required_gates.is_empty()); + } } #[test] @@ -639,4 +796,22 @@ mod tests { assert!(err.contains("physical profile 'mavlink-drone' refuses to start")); assert_eq!(config.mavlink_endpoint, "udp://127.0.0.1:14550"); } + + #[cfg(feature = "manipulator")] + #[test] + fn manipulator_stack_resolves_before_refusing_without_gate() { + let stack = resolve_stack("manipulator-http").unwrap(); + let config = resolve_config(ConfigRequest { + config_path: None, + stack: Some(stack.profile), + stack_defaults: stack.config_overrides, + env: BTreeMap::new(), + cli: PartialHarnessConfig::default(), + }) + .unwrap() + .config; + + let err = config.validate().unwrap_err().to_string(); + assert!(err.contains("physical profile 'manipulator' refuses to start")); + } } diff --git a/src/types.rs b/src/types.rs index 2501927..b2a4905 100644 --- a/src/types.rs +++ b/src/types.rs @@ -35,6 +35,7 @@ pub const OCCUPANCY_OCCUPIED: i8 = 100; pub const COST_FREE: u8 = 0; pub const COST_LETHAL: u8 = 254; pub const COST_UNKNOWN: u8 = 255; +pub const MANIPULATOR_SCHEMA_VERSION: &str = "leash-manipulator-v1"; #[derive(Debug, Clone, Serialize, Deserialize)] #[cfg_attr(feature = "mcp", derive(schemars::JsonSchema))] @@ -642,6 +643,40 @@ pub struct DroneCommandStatus { pub args: Value, } +#[derive(Debug, Clone, Serialize, Deserialize, PartialEq)] +#[cfg_attr(feature = "mcp", derive(schemars::JsonSchema))] +pub struct ManipulatorJoint { + pub name: String, + pub position_rad: f64, + pub velocity_radps: f64, + pub effort_nm: Option, +} + +#[derive(Debug, Clone, Serialize, Deserialize, PartialEq)] +#[cfg_attr(feature = "mcp", derive(schemars::JsonSchema))] +pub struct ManipulatorJointState { + pub version: String, + pub ok: bool, + pub profile: String, + pub simulated: bool, + pub source: String, + pub joints: Vec, +} + +#[derive(Debug, Clone, Serialize, Deserialize, PartialEq)] +#[cfg_attr(feature = "mcp", derive(schemars::JsonSchema))] +pub struct ManipulatorCommandStatus { + pub version: String, + pub ok: bool, + pub command: String, + pub profile: String, + pub simulated: bool, + pub status: String, + pub message: String, + #[serde(default)] + pub args: Value, +} + #[cfg(test)] mod tests { use super::*;