High-performance, cross-platform (Linux/Windows/macOS), zero-abstraction-overhead Rust SDK for AgileX Piper Robot Arm with support for high-frequency force control (500Hz) and async CAN frame recording.
⚠️ IMPORTANT NOTICE This project is under active development. APIs may change. Please test carefully before using in production.Version Status: The current version is pre-0.1.0 (alpha quality). The SDK has NOT been fully tested on real robotic arms and may not work correctly or safely.
⚠️ SAFETY WARNING: Do NOT use this SDK in production or with real robotic arms without comprehensive testing. The software may send incorrect commands that could damage the robot or cause safety hazards.
- 🚀 Zero Abstraction Overhead: Compile-time polymorphism, no virtual function table (vtable) overhead at runtime
- ⚡ High-Performance Reads: Lock-free state reading based on
ArcSwap, nanosecond-level response - 🔄 Lock-Free Concurrency: RCU (Read-Copy-Update) mechanism for efficient state sharing
- 🎯 Type Safety: Bit-level protocol parsing using
bilge, compile-time data correctness guarantees - 🌍 Cross-Platform Support (Linux/Windows/macOS):
- Linux: Supports both SocketCAN (kernel-level performance) and GS-USB (userspace via libusb)
- Windows/macOS: GS-USB driver implementation using
rusb(driver-free/universal)
- 🎬 Async CAN Frame Recording:
- Non-blocking hooks: <1μs overhead per frame using
try_send - Bounded queues: 100,000 frame capacity prevents OOM while preserving ~1.6 min @ 1kHz / ~3.3 min @ 500Hz of burst tolerance
- Hardware timestamps: Direct use of kernel/driver interrupt timestamps
- TX safety: Only records frames after successful
send() - Drop monitoring: Built-in
dropped_framescounter for loss tracking
- Non-blocking hooks: <1μs overhead per frame using
- 📊 Advanced Health Monitoring (
embedded_bridge_host, controller-embedded non-realtime bridge/debug path):- CAN Bus Off Detection: Detects CAN Bus Off events (critical system failure) with debounce mechanism
- Error Passive Monitoring: Monitors Error Passive state (pre-Bus Off warning) for early detection
- USB STALL Tracking: Tracks USB endpoint STALL errors for USB communication health
- Performance Baseline: Dynamic FPS baseline tracking with EWMA for anomaly detection
- Health Score: Comprehensive health scoring (0-100) based on multiple metrics
Piper SDK uses a modular workspace architecture with clear separation of concerns:
piper-sdk-rs/
├── crates/
│ ├── piper-protocol/ # Protocol layer (bit-level CAN protocol)
│ ├── piper-can/ # CAN abstraction (SocketCAN/GS-USB)
│ ├── piper-driver/ # Driver layer (I/O threads, state sync, hooks)
│ ├── piper-client/ # Client layer (type-safe user API)
│ ├── piper-tools/ # Recording and analysis tools
│ └── piper-sdk/ # Compatibility layer (re-exports all)
└── apps/
└── cli/ # Command-line interface
| Layer | Crate | Purpose | Test Coverage |
|---|---|---|---|
| Protocol | piper-protocol |
Type-safe CAN protocol encoding/decoding | 214 tests ✅ |
| CAN | piper-can |
Hardware abstraction for CAN adapters | 97 tests ✅ |
| Driver | piper-driver |
I/O management, state sync, hooks | 149 tests ✅ |
| Client | piper-client |
High-level type-safe API | 105 tests ✅ |
| Tools | piper-tools |
Recording, statistics, safety | 23 tests ✅ |
| SDK | piper-sdk |
Compatibility layer (re-exports) | 588 tests ✅ |
Benefits:
- ✅ Faster compilation: Only recompile modified layers (up to 88% faster)
- ✅ Flexible dependencies: Depend on specific layers to reduce bloat
- ✅ Clear boundaries: Each layer has well-defined responsibilities
- ✅ 100% backward compatible: Existing code requires zero changes
See Workspace Migration Guide for details.
| Module | Crates | Purpose |
|---|---|---|
| CAN Interface | Custom CanAdapter |
Lightweight CAN adapter Trait (no embedded burden) |
| Linux Backend | socketcan |
Native Linux CAN support (SocketCAN interface) |
| USB Backend | rusb |
USB device operations on all platforms, implementing GS-USB protocol |
| Protocol Parsing | bilge |
Bit operations, unaligned data processing, alternative to serde |
| Concurrency Model | crossbeam-channel |
High-performance MPSC channel for sending control commands |
| State Sharing | arc-swap |
RCU mechanism for lock-free reading of latest state |
| Frame Hooks | hooks + recording |
Non-blocking async recording with bounded queues |
| Error Handling | thiserror |
Precise error enumeration within SDK |
| Logging | tracing |
Structured logging |
Add the dependency to your Cargo.toml:
[dependencies]
piper-sdk = "0.1"Enable serialization/deserialization for data types:
[dependencies]
piper-sdk = { version = "0.1", features = ["serde"] }This adds Serialize and Deserialize implementations to:
- Type units (
Rad,Deg,NewtonMeter, etc.) - Joint arrays and joint indices
- Cartesian pose and quaternion types
- CAN frames (
PiperFrame,GsUsbFrame) - for frame dump/replay
Example usage:
use piper_sdk::prelude::*;
use serde_json;
fn main() -> Result<(), Box<dyn std::error::Error>> {
// Serialize joint positions
let positions = JointArray::from([
Rad(0.0), Rad(0.5), Rad(0.0),
Rad(0.0), Rad(0.0), Rad(0.0)
]);
let json = serde_json::to_string(&positions)?;
println!("Serialized: {}", json);
// Deserialize back
let deserialized: JointArray<Rad> = serde_json::from_str(&json)?;
Ok(())
}For CAN frame recording and replay:
# Run frame dump example
cargo run -p piper-sdk --example frame_dump --features serdeThis demonstrates:
- Recording CAN frames to JSON
- Saving/loading frame data
- Debugging CAN bus communication
- Using neutral example IDs instead of live robot protocol traffic
See frame_dump.rs for details.
Features are automatically selected based on your target platform:
- Linux:
socketcan(SocketCAN support) - Linux/macOS/Windows:
gs_usb(GS-USB USB adapter)
No manual feature configuration needed for platform selection!
For reduced dependencies, you can depend on specific layers directly:
# Use only the client layer (most common)
[dependencies]
piper-client = "0.1"
# Use only the driver layer (for advanced users)
[dependencies]
piper-driver = "0.1"
# Use only tools (for recording/analysis)
[dependencies]
piper-tools = "0.1"Note: When using specific layers, update your imports:
piper_sdk::Piper→piper_client::Piperpiper_sdk::Driver→piper_driver::Piper
See Workspace Migration Guide for migration details.
Most users should use the high-level client API for type-safe, easy-to-use control:
use piper_sdk::prelude::*;
use piper_sdk::client::{MotionConnectedPiper, MotionConnectedState};
fn main() -> std::result::Result<(), Box<dyn std::error::Error>> {
// Connect using an explicit SocketCAN target on Linux.
// On macOS/Windows, use `.gs_usb_auto()` or `.gs_usb_serial(...)`.
let connected = PiperBuilder::new()
.socketcan("can0")
.baud_rate(1_000_000)
.build()?
.require_motion()?;
let robot = match connected {
MotionConnectedPiper::Strict(MotionConnectedState::Standby(robot)) => {
robot.enable_position_mode(PositionModeConfig::default())?
}
MotionConnectedPiper::Soft(MotionConnectedState::Standby(robot)) => {
robot.enable_position_mode(PositionModeConfig::default())?
}
MotionConnectedPiper::Strict(MotionConnectedState::Maintenance(_))
| MotionConnectedPiper::Soft(MotionConnectedState::Maintenance(_)) => {
return Err("robot is not in confirmed Standby".into());
}
};
// Get observer for reading state
let observer = robot.observer();
// Read state (lock-free, nanosecond-level response)
let joint_pos = observer.joint_positions();
println!("Joint positions: {:?}", joint_pos);
// Send position command with type-safe units (methods are directly on robot)
let target = JointArray::from([Rad(0.5), Rad(0.0), Rad(0.0), Rad(0.0), Rad(0.0), Rad(0.0)]);
robot.send_position_command(&target)?;
Ok(())
}Record CAN frames asynchronously with non-blocking hooks:
use piper_driver::hooks::FrameCallback;
use piper_driver::recording::AsyncRecordingHook;
use piper_sdk::driver::PiperBuilder;
use std::sync::Arc;
use std::sync::atomic::Ordering;
use std::thread;
use std::time::Duration;
fn main() -> Result<(), Box<dyn std::error::Error>> {
// Create recording hook
let (hook, rx) = AsyncRecordingHook::new();
let dropped_counter = hook.dropped_frames().clone();
// Register as callback
let callback = Arc::new(hook) as Arc<dyn FrameCallback>;
// Connect robot through the driver API
let robot = PiperBuilder::new()
.socketcan("can0")
.build()?;
// Register hook in driver layer
let hook_handle = robot.hooks().write()?.add_callback(callback);
// Spawn recording thread
let handle = thread::spawn(move || {
while let Ok(frame) = rx.recv() {
println!(
"Received frame: ID=0x{:03X}, timestamp={}us",
frame.id, frame.timestamp_us
);
}
});
// Run for 5 seconds
thread::sleep(Duration::from_secs(5));
// Check dropped frames
let dropped = dropped_counter.load(Ordering::Relaxed);
println!("Dropped frames: {}", dropped);
// Remove hook so the receiver can exit cleanly
let _removed = robot.hooks().write()?.remove_callback(hook_handle);
let _ = handle.join();
Ok(())
}Key Features:
- ✅ Non-blocking:
<1μsoverhead per frame - ✅ OOM-safe: Bounded queue (100,000 frames ≈ 1.6 min @ 1kHz / 3.3 min @ 500Hz)
- ✅ Hardware timestamps: Microsecond precision from kernel/driver
- ✅ TX safe: Only records successfully sent frames
- ✅ Loss tracking: Built-in
dropped_framescounter
Piper SDK provides three complementary APIs for CAN frame recording and replay:
| API | Use Case | Complexity | Safety |
|---|---|---|---|
| Standard Recording | Simple record-and-save workflows | ⭐ Low | ✅ Type-safe |
| Custom Diagnostics | Real-time frame analysis & custom processing | ⭐⭐ Medium | ✅ Thread-safe |
| ReplayMode | Safe replay of recorded sessions | ⭐⭐ Medium | ✅ Type-safe + Driver-level protection |
The simplest way to record CAN frames to a file:
use piper_sdk::{
ConnectedPiper,
PiperBuilder,
RecordingConfig,
RecordingMetadata,
StopCondition,
client::state::{CapabilityMarker, Piper, Standby},
};
use std::time::Duration;
async fn run_recording<C: CapabilityMarker>(
robot: Piper<Standby, C>,
) -> anyhow::Result<()> {
// Start recording with metadata
let (robot, handle) = robot.start_recording(RecordingConfig {
output_path: "demo_recording.bin".into(),
stop_condition: StopCondition::Duration(10), // Record for 10 seconds
metadata: RecordingMetadata {
notes: "Standard recording example".to_string(),
operator: "DemoUser".to_string(),
},
})?;
// Perform operations (all CAN frames are recorded)
tokio::time::sleep(Duration::from_secs(10)).await;
// Stop recording and get statistics
let (_robot, stats) = robot.stop_recording(handle)?;
println!("Recorded {} frames in {:.2}s", stats.frame_count, stats.duration.as_secs_f64());
println!("Dropped frames: {}", stats.dropped_frames);
Ok(())
}
#[tokio::main]
async fn main() -> anyhow::Result<()> {
// Connect to robot
let connected = PiperBuilder::new()
.socketcan("can0")
.build()?;
match connected {
ConnectedPiper::Strict(state) => run_recording(state.require_standby()?).await,
ConnectedPiper::Soft(state) => run_recording(state.require_standby()?).await,
ConnectedPiper::Monitor(standby) => run_recording(standby).await,
}
}Features:
- ✅ Automatic stop conditions: Duration, frame count, or manual
- ✅ Rich metadata: Record operator, notes, timestamps
- ✅ Statistics: Frame count, duration, dropped frames
- ✅ Type-safe: Recording handle prevents misuse
See standard_recording.rs for full example.
Advanced users can register custom frame callbacks for real-time analysis:
use piper_sdk::PiperBuilder;
use piper_sdk::client::{MotionConnectedPiper, MotionConnectedState};
use piper_driver::recording::AsyncRecordingHook;
use std::sync::Arc;
use std::thread;
fn main() -> anyhow::Result<()> {
// Connect and enable robot
let connected = PiperBuilder::new()
.socketcan("can0")
.build()?
.require_motion()?;
let active = match connected {
MotionConnectedPiper::Strict(MotionConnectedState::Standby(robot)) => {
robot.enable_position_mode(Default::default())?
}
MotionConnectedPiper::Soft(MotionConnectedState::Standby(robot)) => {
robot.enable_position_mode(Default::default())?
}
MotionConnectedPiper::Strict(MotionConnectedState::Maintenance(_))
| MotionConnectedPiper::Soft(MotionConnectedState::Maintenance(_)) => {
anyhow::bail!("robot is not in confirmed Standby");
}
};
// Get diagnostics interface
let diag = active.diagnostics();
// Create custom recording hook
let (hook, rx) = AsyncRecordingHook::new();
let dropped_counter = hook.dropped_frames().clone();
// Register hook
let callback = Arc::new(hook) as Arc<dyn piper_driver::FrameCallback>;
let hook_handle = diag.register_callback(callback)?;
// Process frames in background thread
thread::spawn(move || {
let mut frame_count = 0;
while let Ok(frame) = rx.recv() {
frame_count += 1;
// Custom analysis: e.g., CAN ID distribution, timing analysis
if frame_count % 1000 == 0 {
println!("Received frame: ID=0x{:03X}", frame.id);
}
}
println!("Total frames: {}", frame_count);
println!("Dropped: {}", dropped_counter.load(std::sync::atomic::Ordering::Relaxed));
});
// Run operations...
thread::sleep(std::time::Duration::from_secs(5));
// Remove hook before shutdown
let _removed = diag.unregister_callback(hook_handle)?;
// Shutdown
let _standby = active.shutdown()?;
let _summary = handle.join();
Ok(())
}Features:
- ✅ Real-time processing: Analyze frames as they arrive
- ✅ Custom logic: Implement any analysis algorithm
- ✅ Background threading: Analysis runs off the RX thread, and shutdown waits deterministically for the worker to exit
- ✅ Loss tracking: Monitor dropped frames
See custom_diagnostics.rs for full example.
Safely replay previously recorded sessions with driver-level protection:
use piper_sdk::PiperBuilder;
use piper_sdk::client::{MotionConnectedPiper, MotionConnectedState};
fn main() -> anyhow::Result<()> {
// Connect to robot
let connected = PiperBuilder::new()
.socketcan("can0")
.build()?
.require_motion()?;
let robot = match connected {
MotionConnectedPiper::Strict(MotionConnectedState::Standby(robot)) => robot,
MotionConnectedPiper::Soft(MotionConnectedState::Standby(robot)) => robot,
MotionConnectedPiper::Strict(MotionConnectedState::Maintenance(_))
| MotionConnectedPiper::Soft(MotionConnectedState::Maintenance(_)) => {
anyhow::bail!("robot is not in confirmed Standby");
}
};
// Enter ReplayMode (Driver TX thread pauses automatically)
let replay = robot.enter_replay_mode()?;
// Replay recording at 2.0x speed
let robot = replay.replay_recording("demo_recording.bin", 2.0)?;
// Automatically exits ReplayMode (TX thread resumes)
println!("Replay completed!");
Ok(())
}Safety Features:
- ✅ Driver-level protection: TX thread pauses during replay (no dual control flow)
- ✅ Speed limits: Maximum 5.0x, recommended ≤ 2.0x with warnings
- ✅ Type-safe transitions: Cannot call enable/disable in ReplayMode
- ✅ Automatic cleanup: Always returns to Standby state
Speed Guidelines:
- 1.0x: Original speed (recommended for most use cases)
- 0.1x ~ 2.0x: Safe range for testing/debugging
- > 2.0x: Use with caution - ensure safe environment
- Maximum: 5.0x (hard limit for safety)
See replay_mode.rs for full example with speed validation.
The piper-cli tool provides convenient commands for recording and replay:
# Record CAN frames
piper-cli record -o demo.bin --duration 10
# Replay recording (normal speed)
piper-cli replay -i demo.bin
# Replay at 2.0x speed
piper-cli replay -i demo.bin --speed 2.0
# Replay without confirmation prompt
piper-cli replay -i demo.bin --yes# Step 1: Record a session
cargo run -p piper-sdk --example standard_recording
# Step 2: Analyze the recording
cargo run -p piper-sdk --example custom_diagnostics
# Step 3: Replay the recording safely
cargo run -p piper-sdk --example replay_modeEach API serves a distinct purpose:
- Standard Recording: For users who want "just record this" without complexity
- Custom Diagnostics: For researchers developing custom analysis tools
- ReplayMode: For test engineers reproducing bugs or testing sequences
The ReplayMode API uses Rust's type system for compile-time safety:
// ✅ Compile-time error: cannot enable in ReplayMode
let replay = standby.enter_replay_mode()?;
let active = replay.enable_position_mode(...); // ERROR!
// ✅ Must exit ReplayMode first
let standby = replay.replay_recording(...)?;
let active = standby.enable_position_mode(...)?; // OK!ReplayMode switches the driver to DriverMode::Replay, which:
- Pauses periodic TX: Driver stops sending automatic control commands
- Allows explicit frames: Only replay frames are sent to CAN bus
- Prevents conflicts: No dual control flow (Driver vs Replay)
This design is documented in architecture analysis.
For direct CAN frame control or maximum performance, use the driver API:
use piper_sdk::driver::PiperBuilder;
fn main() -> std::result::Result<(), Box<dyn std::error::Error>> {
// Create driver instance
let robot = PiperBuilder::new()
.socketcan("can0") // Linux: explicit SocketCAN target
.baud_rate(1_000_000) // CAN baud rate
.build()?;
// Get current state (lock-free, nanosecond-level response)
let joint_pos = robot.get_joint_position();
println!("Joint positions: {:?}", joint_pos.joint_pos);
// Send control frame
let frame = piper_sdk::PiperFrame::new_standard(0x1A1, &[0x01, 0x02, 0x03]);
robot.send_frame(frame)?;
Ok(())
}For performance optimization, state data is divided into two categories:
-
High-frequency Data (200Hz):
JointPositionState: Joint positions (6 joints)EndPoseState: End-effector pose (position and orientation)JointDynamicState: Joint dynamic state (joint velocities, currents)RobotControlState: Robot control status (control mode, robot status, fault codes, etc.)GripperState: Gripper status (travel, torque, status codes, etc.)- Uses
ArcSwapfor lock-free reading, optimized for high-frequency control loops
-
Low-frequency Data (40Hz):
JointDriverLowSpeedState: Joint driver diagnostic state (temperatures, voltages, currents, driver status)CollisionProtectionState: Collision protection levels (on-demand)JointLimitConfigState: Joint angle and velocity limits (on-demand)JointAccelConfigState: Joint acceleration limits (on-demand)EndLimitConfigState: End-effector velocity and acceleration limits (on-demand)- Uses
ArcSwapfor diagnostic data,RwLockfor configuration data
The SDK uses a layered architecture from low-level to high-level:
- CAN Layer (
can): CAN hardware abstraction, supports SocketCAN and GS-USB - Protocol Layer (
protocol): Type-safe protocol encoding/decoding - Driver Layer (
driver): IO thread management, state synchronization, frame parsing- Hooks System: Runtime callback registration for frame recording
- Recording Module: Async non-blocking recording with bounded queues
- Client Layer (
client): Type-safe, user-friendly control interface - Tools Layer (
tools): Recording formats, statistics, safety validation
piper-sdk-rs/
├── crates/
│ ├── piper-protocol/
│ │ └── src/
│ │ ├── lib.rs # Protocol module entry
│ │ ├── ids.rs # CAN ID constants/enums
│ │ ├── feedback.rs # Robot arm feedback frames (bilge)
│ │ ├── control.rs # Control command frames (bilge)
│ │ └── config.rs # Configuration frames (bilge)
│ ├── piper-can/
│ │ └── src/
│ │ ├── lib.rs # CAN module entry
│ │ ├── socketcan/ # [Linux] SocketCAN implementation
│ │ └── gs_usb/ # [Win/Mac/Linux] GS-USB protocol
│ ├── piper-driver/
│ │ └── src/
│ │ ├── mod.rs # Driver module entry
│ │ ├── piper.rs # Driver-level Piper object (API)
│ │ ├── pipeline.rs # IO Loop, ArcSwap update logic
│ │ ├── state.rs # State structure definitions
│ │ ├── hooks.rs # Hook system for frame callbacks
│ │ ├── recording.rs # Async recording with bounded queues
│ │ ├── builder.rs # PiperBuilder (fluent construction)
│ │ └── metrics.rs # Performance metrics
│ ├── piper-client/
│ │ └── src/
│ │ ├── mod.rs # Client module entry
│ │ ├── observer.rs # Observer (read-only state access)
│ │ ├── state/ # Type State Pattern state machine
│ │ ├── motion.rs # Piper command interface
│ │ └── types/ # Type system (units, joints, errors)
│ └── piper-tools/
│ └── src/
│ ├── recording.rs # Recording formats and tools
│ ├── statistics.rs # CAN statistics analysis
│ └── safety.rs # Safety validation
└── apps/
└── cli/
└── src/
├── commands/ # CLI commands
└── modes/ # CLI modes (repl, oneshot)
Adopts asynchronous IO concepts but implemented with synchronous threads (ensuring deterministic latency):
- IO Thread: Responsible for CAN frame transmission/reception and state updates
- Control Thread: Lock-free reading of latest state via
ArcSwap, sending commands viacrossbeam-channel - Frame Commit Mechanism: Ensures the state read by control threads is a consistent snapshot at a specific time point
- Hook System: Non-blocking callbacks triggered on RX/TX frames for recording
Check the crates/piper-sdk/examples/ directory for more examples:
Note: Example code is under development. See crates/piper-sdk/examples/ for more examples.
Available examples:
state_api_demo.rs- Simple state reading and printingrealtime_control_demo.rs- Driver scheduling / metrics demo (--allow-raw-txonly on an isolated bus)robot_monitor.rs- Robot state monitoringtimestamp_verification.rs- Timestamp synchronization verificationstandard_recording.rs- 📼 Standard recording API usage (record CAN frames to file)custom_diagnostics.rs- 🔧 Custom diagnostics interface (real-time frame analysis)replay_mode.rs- 🔄 ReplayMode API (safe CAN frame replay)bridge_test.rs- Bridge debug / replay utility (Unix defaults to/tmp/piper_bridge.sock; non-Unix requires explicit--endpoint)bridge_latency_bench.rs- Bridge latency benchmark (Unix defaults to/tmp/piper_bridge.sock; non-Unix requires explicit--endpoint)embedded_bridge_host.rs- Controller-embedded bridge host (non-Unix requires explicit--tcp-tlsand TLS material)position_control_demo.rs- End-to-end position-mode motion flow (teaching example; uses fixed waits rather than production-grade motion completion confirmation)multi_threaded_demo.rs- SharingPiperacross threads with explicit disable on shutdowndual_arm_bilateral_control.rs- Dual-arm master/slave control examplehigh_level_simple_move.rs- Offline trajectory planning quickstarthigh_level_trajectory_demo.rs- Trajectory planner analysis / reset demohigh_level_pid_control.rs- Offline PID controller demohigh_level_gripper_control.rs- Gripper API usage walkthroughframe_dump.rs- Frame serialization / dump exampleiface_check.rs- SocketCAN interface inspection utilitygs_usb_direct_test.rs- GS-USB direct enumeration / transport demo
Planned examples:
torque_control.rs- Force control demonstrationconfigure_can.rs- CAN baud rate configuration tool
Contributions are welcome! Please see CONTRIBUTING.md for details.
This project is licensed under the MIT License. See the LICENSE file for details.
Current runnable entry points are this README, the source tree, and piper-sdk examples.
Historical design notes and migration records are archived under docs/v0/. For detailed background material, see:
- Architecture Design Document
- Protocol Document
- Real-time Configuration Guide
- Real-time Optimization Guide
- Migration Guide - Guide for migrating from v0.1.x to v0.2.0+
- Position Control & MOVE Mode User Guide - Complete guide for position control and motion types
- Hooks System Code Review - Deep dive into the recording system design
- Full Repository Code Review - Comprehensive codebase analysis