diff --git a/CLAUDE.md b/CLAUDE.md index 39de46a..0ffc0ac 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -4,19 +4,20 @@ This file provides guidance to Claude Code (claude.ai/code) when working with co ## Project Overview -This is a KUKA robotic arm simulation project with a Zig backend for real-time control and physics simulation, and a Next.js/React frontend for 3D visualization and user interface. +This is a KUKA robotic arm simulation project with a Zig backend for real-time control and physics simulation, and a Vite/React/Three.js frontend for 3D visualization and user interface. ## Commands ### Frontend Development -- **Development server**: `npm run dev` or `bun dev` +- **Development server**: `npm run dev` - **Build**: `npm run build` +- **Preview production build**: `npm run preview` - **Linting**: `npm run lint` -- **Production start**: `npm run start` ### Backend Development - **Build**: `cd backend && zig build` - **Run**: `cd backend && zig build run` +- **Run tests**: `cd backend && zig build test` - **Clean**: `cd backend && rm -rf zig-cache zig-out` ## Architecture @@ -31,8 +32,8 @@ The backend is structured as a modular system with clear dependency hierarchy: **Physics & Kinematics**: - `kinematics/`: Forward/inverse kinematics and collision detection -- `physics/`: Dynamics simulation and motor modeling -- `safety/`: Monitoring, limits, and emergency stop systems +- `physics/`: Dynamics simulation with motor modeling, thermal, and electrical models +- `safety/`: Monitoring, limits, emergency stop, and safety enable/disable **Control System**: - `control/`: PID controllers, motion planning, and joint management @@ -42,6 +43,7 @@ The backend is structured as a modular system with clear dependency hierarchy: **Communication**: - `communication/`: WebSocket server with binary protocol - Protocol runs on port 9001 with mixed binary/JSON messaging +- 1kHz joint state updates (binary), 100Hz system status (JSON) **Module Dependencies** (defined in build.zig:18-98): ``` @@ -52,48 +54,85 @@ utils (base) → timing → kinematics → safety → control → core → joint communication ``` -### Frontend (Next.js/React/Three.js) +### Frontend (Vite/React/Three.js) The frontend provides 3D visualization and control interface: -**Key Components**: -- `robot-arm.tsx`: Main 3D robot model using React Three Fiber -- `robot-arm-simulation.tsx`: Simulation environment and physics integration -- `zig-controller.tsx`: Backend communication bridge (WebSocket) -- `control-panel.tsx`: User controls and command interface -- `info-panel.tsx`: Status display and system information +**Key Components** (`src/components/`): +- `robot-arm.tsx`: Main 3D robot model using React Three Fiber with spring animations +- `robot-arm-simulation.tsx`: Simulation environment, backend integration, command dispatching +- `zig-controller.tsx`: WebSocket client, binary/JSON parsing, command serialization +- `control-panel.tsx`: User controls, sliders, presets, and diagnostics display +- `info-panel.tsx`: Static system information display **Libraries**: +- Vite for build tooling - React Three Fiber for 3D rendering +- @react-three/cannon for physics objects - Tailwind CSS for styling - Radix UI components for interface elements -- React Hook Form for form handling ### Data Flow -- Backend sends joint states at 1kHz, system status at 100Hz -- Frontend receives data via WebSocket and updates 3D visualization -- Commands flow from frontend controls to backend via JSON messages -- Binary protocol for high-frequency joint state data, JSON for commands and status + +``` +┌─────────────────────────────────────────────────────────────────┐ +│ BACKEND (Zig) │ +│ ┌──────────────┐ ┌──────────────┐ ┌──────────────────┐ │ +│ │ Physics Sim │───▶│Joint Manager │───▶│ WebSocket Server │ │ +│ │ (1kHz loop) │ │ │ │ (port 9001) │ │ +│ └──────────────┘ └──────────────┘ └────────┬─────────┘ │ +│ │ │ +└────────────────────────────────────────────────────┼────────────┘ + │ + WebSocket (binary + JSON) │ + ▼ +┌────────────────────────────────────────────────────┼────────────┐ +│ FRONTEND (React) │ │ +│ ┌──────────────────┐ ┌─────────────────────────▼──────────┐│ +│ │ Robot Arm (3D) │◀───│ ZigController ││ +│ │ visualization │ │ - Binary joint state parsing ││ +│ └──────────────────┘ │ - JSON status parsing ││ +│ ▲ │ - Command serialization ││ +│ │ └─────────────────────────────────────┘│ +│ ┌───────┴──────────┐ │ +│ │ Robot Simulation │◀──────────────────────────────────────────│ +│ │ - Target angles │ ┌─────────────────────────────────────┐│ +│ │ - Actual angles │ │ Control Panel ││ +│ │ - Backend sync │◀───│ - Slider controls ││ +│ └──────────────────┘ │ - Preset movements ││ +│ │ - Diagnostics display ││ +│ └─────────────────────────────────────┘│ +└─────────────────────────────────────────────────────────────────┘ +``` + +**Message Flow**: +- Backend → Frontend: Joint states (binary, 1kHz), System status (JSON, 100Hz), Collision data (JSON) +- Frontend → Backend: Position commands (JSON), Control mode changes (JSON), Safety commands (JSON) ## Integration Points The system is designed for real-time integration where: 1. Backend is the authoritative source for all robot physics and state -2. Frontend acts as visualization and user interface layer -3. Communication happens via WebSocket on localhost:9001 -4. Frontend interpolates between backend updates for smooth 60fps rendering +2. Frontend displays actual joint positions received from backend +3. User inputs set target positions, which are sent as commands to backend +4. Communication happens via WebSocket on localhost:9001 +5. Frontend uses spring animations for smooth interpolation between updates ## Important Files -- `backend/src/main.zig:12-167`: Main control loop and system initialization -- `backend/build.zig:18-98`: Module dependency configuration -- `INTEGRATION.md`: Detailed integration plan and protocol specifications -- `FE.md`: Frontend architecture and interface definitions -- `backend/BE.md`: Backend implementation details and phases +- `backend/src/main.zig`: Main control loop with signal handling for graceful shutdown +- `backend/src/communication/main.zig`: WebSocket server and message dispatching +- `backend/src/joints/manager.zig`: Joint state management and physics integration +- `backend/src/safety/safety_monitor.zig`: Safety monitoring with enable/disable/e-stop +- `backend/build.zig`: Module dependency configuration +- `src/components/robot-arm-simulation.tsx`: Main frontend component with backend integration +- `src/components/zig-controller.tsx`: WebSocket client and protocol handling ## Development Notes - Backend uses 1ms control loops for real-time performance -- All physics calculations are deterministic and use fixed-point math where appropriate -- Safety systems include collision detection, joint limits, and emergency stops -- Frontend removes local physics simulation in favor of backend authority -- Protocol handles mixed binary (joint states) and JSON (commands/status) messaging \ No newline at end of file +- Graceful shutdown: Ctrl+C or SIGTERM triggers clean shutdown sequence +- Safety system is enabled by default; can be disabled for testing +- Physics simulation includes motor dynamics, thermal modeling, and sensor noise +- Frontend automatically reconnects on WebSocket disconnection +- Target angles (user input) and actual angles (backend state) are tracked separately +- When backend is disconnected, frontend falls back to showing target angles diff --git a/FE.md b/FE.md index 998150a..e102116 100644 --- a/FE.md +++ b/FE.md @@ -1,28 +1,52 @@ # Frontend Architecture and Integration Plan +## Tech Stack +- **Build Tool**: Vite +- **Framework**: React 18 +- **3D Rendering**: React Three Fiber, @react-three/drei, @react-three/cannon +- **Styling**: Tailwind CSS +- **UI Components**: Radix UI + ## Current Frontend Components -- `robot-arm.tsx`: Main 3D model and joint management -- `robot-arm-simulation.tsx`: Simulation environment and physics -- `zig-controller.tsx`: Backend communication bridge -- `control-panel.tsx`: User interface for control -- `info-panel.tsx`: Status and feedback display + +### Core Components (`src/components/`) +- `robot-arm.tsx`: Main 3D model with 7-DOF joint hierarchy and spring animations +- `robot-arm-simulation.tsx`: Simulation environment, backend integration, command dispatching +- `zig-controller.tsx`: WebSocket client, binary/JSON parsing, command serialization +- `control-panel.tsx`: User interface with manual control, presets, and diagnostics +- `info-panel.tsx`: Static system information display ## Backend-Frontend Interface ### Data Flow FROM Backend -1. **Joint State Updates** (1kHz) + +1. **Joint State Updates** (1kHz, Binary) ```typescript interface JointState { - position: number[]; // Current joint positions (rad) - velocity: number[]; // Current joint velocities (rad/s) - torque: number[]; // Current joint torques (Nm) + position: number[]; // Joint positions (degrees) + velocity: number[]; // Joint velocities (deg/s) + torque: number[]; // Joint torques (Nm) temperature: number[]; // Motor temperatures (°C) - current: number[]; // Motor currents (A) + current: number[]; // Motor currents (A) + timestamp: number; // Timestamp in microseconds } ``` -2. **System Status** (100Hz) +2. **System Status** (100Hz, JSON) ```typescript + // Backend sends numeric values + interface BackendSystemStatus { + state: number; // 0=READY, 1=BUSY, 2=ERROR, 3=WARNING + error_code: string | null; + safety_status: { + soft_limits_active: boolean; + emergency_stop: boolean; + collision_detected: boolean; + }; + control_mode: number; // 0=POSITION, 1=VELOCITY, 2=TORQUE + } + + // Frontend converts to string enum interface SystemStatus { state: 'READY' | 'BUSY' | 'ERROR' | 'WARNING'; errorCode: string | null; @@ -35,34 +59,35 @@ } ``` -3. **Collision Data** (On Change) +3. **Collision Data** (100Hz when detected, JSON) ```typescript interface CollisionData { detected: boolean; link1: string; link2: string; - position: Vector3; - penetrationDepth: number; - contactNormal: Vector3; + position: [number, number, number]; + penetration_depth: number; + contact_normal: [number, number, number]; } ``` ### Data Flow TO Backend -1. **Joint Commands** + +1. **Position Commands** (JSON) ```typescript - interface JointCommand { - type: 'POSITION' | 'VELOCITY' | 'TORQUE'; - values: number[]; - maxVelocity?: number[]; - maxAcceleration?: number[]; + interface PositionCommand { + type: 0; // COMMAND_TYPE_POSITION + values: number[]; // Target positions (degrees) + max_velocity?: number[]; // Optional max velocities } ``` -2. **Control Mode Changes** +2. **Control Mode Changes** (JSON) ```typescript - interface ControlModeRequest { - mode: 'POSITION' | 'VELOCITY' | 'TORQUE'; - parameters: { + interface ControlModeCommand { + type: 3; // COMMAND_TYPE_CONTROL_MODE + control_mode: number; // 0=POSITION, 1=VELOCITY, 2=TORQUE + parameters?: { stiffness?: number; damping?: number; feedforward?: boolean; @@ -70,87 +95,99 @@ } ``` -3. **Safety Commands** +3. **Safety Commands** (JSON) ```typescript interface SafetyCommand { - type: 'ENABLE' | 'DISABLE' | 'RESET' | 'E_STOP'; - zoneId?: string; + type: 4; // COMMAND_TYPE_SAFETY + safety: { + type: number; // 0=ENABLE, 1=DISABLE, 2=RESET, 3=E_STOP + zone_id?: string; + }; } ``` -## Required Frontend Updates - -### 1. Visual Feedback Enhancements +## Implementation Status + +### Completed Features +- [x] WebSocket connection with auto-reconnect +- [x] Binary protocol parsing for joint states +- [x] JSON parsing for system status with numeric->string conversion +- [x] Joint position command sending +- [x] Real-time 3D visualization with spring animations +- [x] Manual control sliders (7 joints) +- [x] Preset movement buttons +- [x] Diagnostics panel with temperature and current display +- [x] Connection status indicator +- [x] Emergency stop and collision detection alerts + +### Future Enhancements - [ ] Add motor temperature visualization (color gradient on joints) - [ ] Implement torque visualization (arrows/force indicators) -- [ ] Add collision warning indicators -- [ ] Show safety zones and limits +- [ ] Add safety zone visualization - [ ] Add trajectory preview visualization - -### 2. Physics Integration -- [ ] Remove current basic physics simulation -- [ ] Update joint positions based on backend data only -- [ ] Keep collision detection for visual objects only -- [ ] Implement smooth interpolation between backend updates - -### 3. UI/UX Improvements -- [ ] Add detailed motor status panels -- [ ] Implement advanced trajectory planning interface -- [ ] Add safety zone configuration UI -- [ ] Create error and warning notification system -- [ ] Add real-time plotting of joint data - -### 4. Backend Communication -- [ ] Replace mock ZigController with real WebSocket connection -- [ ] Implement binary protocol parsing/serialization -- [ ] Add connection status monitoring -- [ ] Implement command queuing and synchronization -- [ ] Add automatic reconnection handling - -### 5. Testing & Validation -- [ ] Add unit tests for protocol handling -- [ ] Implement connection loss simulation -- [ ] Add error condition testing -- [ ] Create visual regression tests -- [ ] Add performance benchmarking +- [ ] Real-time plotting of joint data +- [ ] Advanced trajectory planning interface +- [ ] Unit tests for protocol handling +- [ ] Performance benchmarking + +## Architecture + +``` +┌─────────────────────────────────────────────────────────────┐ +│ RobotArmSimulation │ +│ ┌─────────────────────────────────────────────────────────┐│ +│ │ State: ││ +│ │ - targetAngles (user input) ││ +│ │ - actualAngles (from backend) ││ +│ │ - controlSignals (backend data + sendJointCommand) ││ +│ │ - isBackendConnected ││ +│ └─────────────────────────────────────────────────────────┘│ +│ │ │ +│ ┌──────────────────┼──────────────────┐ │ +│ ▼ ▼ ▼ │ +│ ┌─────────────┐ ┌──────────────┐ ┌────────────────┐ │ +│ │ RobotArm │ │ControlPanel │ │ ZigController │ │ +│ │ (3D viz) │ │ (UI inputs) │ │ (WebSocket) │ │ +│ │ │ │ │ │ │ │ +│ │ angles prop │ │ angles prop │ │ setControl- │ │ +│ │ from parent │ │ onAngle- │ │ Signals │ │ +│ │ │ │ Change() │ │ callback │ │ +│ └─────────────┘ └──────────────┘ └────────────────┘ │ +└─────────────────────────────────────────────────────────────┘ +``` + +### Data Flow + +1. **User Input Flow**: + - User moves slider → `handleAngleChange()` → updates `targetAngles` + - → calls `sendCommandToBackend()` → `sendJointCommand()` → WebSocket + +2. **Backend Data Flow**: + - WebSocket receives binary frame → `deserializeJointState()` + - → updates `controlSignals.jointPositions` + - → `useEffect` updates `actualAngles` + - → `displayAngles` = `actualAngles` when connected + +3. **Fallback Flow**: + - When backend disconnected, `displayAngles` = `targetAngles` + - Robot visualization continues showing last target positions ## Performance Requirements + 1. **Rendering**: Maintain 60fps during all operations 2. **Update Rates**: - - Joint position updates: 60Hz visual update + - Backend data: 1kHz (throttled to 60Hz for rendering) - Status display: 10Hz refresh - - Control input: 100Hz sampling + - Control input: Immediate on slider change 3. **Latency**: - - Command to visual feedback: <50ms - - Error condition to display: <100ms - - UI interaction to response: <16ms - -## Integration Timeline -1. **Phase 1**: Basic Communication - - Implement WebSocket connection - - Basic joint state visualization - - Simple command interface - -2. **Phase 2**: Enhanced Visualization - - Temperature and torque displays - - Safety zone visualization - - Trajectory preview - -3. **Phase 3**: Advanced Features - - Complete control panel - - Full error handling - - Performance optimization - -4. **Phase 4**: Testing & Polish - - Comprehensive testing - - UI polish - - Performance tuning - -## Notes for Developers -- All 3D visualizations should use Three.js with React Three Fiber -- Use TypeScript for all new components -- Follow the existing component structure -- Maintain real-time capability with proper React optimization -- Use React Suspense for loading states -- Implement proper error boundaries -- Follow the existing styling system (Tailwind CSS) \ No newline at end of file + - Command to backend: <10ms (WebSocket send) + - Backend data to visualization: <16ms (next frame) + +## Development Notes + +- All 3D visualizations use React Three Fiber +- TypeScript for type safety +- Tailwind CSS for styling +- Spring animations provide smooth joint movement +- React.memo and useCallback for performance optimization +- Connection status and error handling throughout diff --git a/backend/src/communication/main.zig b/backend/src/communication/main.zig index 5eedc19..72c845d 100644 --- a/backend/src/communication/main.zig +++ b/backend/src/communication/main.zig @@ -49,37 +49,49 @@ const UpdateThread = struct { fn run(self: *UpdateThread) !void { var timer = try time.Timer.start(); + var last_joint_update: u64 = 0; var last_status_update: u64 = 0; - // var last_collision_check: u64 = 0; // Temporarily unused for debugging - const update_interval_ns = time.ns_per_ms; // 1kHz - const status_interval_ns = 10 * time.ns_per_ms; // 100Hz - // const collision_interval_ns = 10 * time.ns_per_ms; // 100Hz // Temporarily unused for debugging + var last_collision_check: u64 = 0; + const joint_interval_ns = time.ns_per_ms; // 1kHz for joint states + const status_interval_ns = 10 * time.ns_per_ms; // 100Hz for system status + const collision_interval_ns = 10 * time.ns_per_ms; // 100Hz for collision checks while (self.running) { const now = timer.read(); - // TEMPORARILY DISABLED: Joint state updates (1kHz) for debugging - // try self.sendJointStates(); + // Joint state updates (1kHz) - high frequency for smooth visualization + if (now - last_joint_update >= joint_interval_ns) { + self.sendJointStates() catch |err| { + std.log.warn("Failed to send joint states: {}", .{err}); + }; + last_joint_update = now; + } - // System status updates (100Hz) - reduced frequency for debugging - if (now - last_status_update >= status_interval_ns * 10) { // 10Hz instead of 100Hz - try self.sendSystemStatus(); + // System status updates (100Hz) + if (now - last_status_update >= status_interval_ns) { + self.sendSystemStatus() catch |err| { + std.log.warn("Failed to send system status: {}", .{err}); + }; last_status_update = now; } - // TEMPORARILY DISABLED: Collision detection updates for debugging - // if (now - last_collision_check >= collision_interval_ns) { - // try self.checkAndSendCollisions(); - // last_collision_check = now; - // } + // Collision detection updates (100Hz) + if (now - last_collision_check >= collision_interval_ns) { + self.checkAndSendCollisions() catch |err| { + std.log.warn("Failed to send collision data: {}", .{err}); + }; + last_collision_check = now; + } // Process any pending commands - try self.processCommands(); + self.processCommands() catch |err| { + std.log.warn("Failed to process commands: {}", .{err}); + }; - // Sleep until next update + // Sleep until next update cycle const elapsed = timer.read() - now; - if (elapsed < update_interval_ns) { - time.sleep(update_interval_ns - elapsed); + if (elapsed < joint_interval_ns) { + time.sleep(joint_interval_ns - elapsed); } } } diff --git a/backend/src/joints/manager.zig b/backend/src/joints/manager.zig index da7b101..e0dc23e 100644 --- a/backend/src/joints/manager.zig +++ b/backend/src/joints/manager.zig @@ -182,9 +182,35 @@ pub const JointManager = struct { } pub fn getRobotState(self: *JointManager) types.RobotState { - // TODO: Implement actual logic to determine robot state - _ = self; // Keep self until proper logic is added - return types.RobotState.ready; // Placeholder + // Check for emergency stop condition + if (self.safety_monitor.isEmergencyStop()) { + return types.RobotState.emergency_stop; + } + + // Check if any joint is moving (has significant velocity) + const velocity_threshold: f32 = 0.1; // degrees/second + var is_moving = false; + for (self.joints) |joint| { + if (@abs(joint.current_velocity) > velocity_threshold) { + is_moving = true; + break; + } + } + + if (is_moving) { + return types.RobotState.moving; + } + + // Check for fault conditions (e.g., overtemperature) + const max_safe_temp: f32 = 80.0; // degrees Celsius + for (self.joints) |joint| { + if (joint.temperature > max_safe_temp) { + return types.RobotState.fault; + } + } + + // Default to ready state + return types.RobotState.ready; } pub fn reset(self: *JointManager) void { @@ -216,14 +242,14 @@ pub const JointManager = struct { } pub fn setTorques(self: *JointManager, torques: []const f32) !void { - // TODO: Implement actual logic, possibly check control mode first if (torques.len != types.NUM_JOINTS) { return error.InvalidNumberOfTorques; } for (&self.joints, torques) |*joint, torque| { joint.target_torque = torque; } - self.control_mode = types.ControlMode.position; + // Switch to torque control mode when setting torques + self.control_mode = types.ControlMode.torque; } pub fn setControlMode(self: *JointManager, mode: types.ControlMode, params: anytype) !void { diff --git a/backend/src/main.zig b/backend/src/main.zig index ad112c9..1f695f3 100644 --- a/backend/src/main.zig +++ b/backend/src/main.zig @@ -1,6 +1,7 @@ -//! By convention, main.zig is where your main function lives in the case that -//! you are building an executable. If you are making a library, the convention -//! is to delete this file and start with root.zig instead. +//! KUKA Robot Arm Control System - Main Entry Point +//! +//! This is the production entry point for the KUKA robot arm simulation backend. +//! It initializes all subsystems and runs the 1kHz real-time control loop. const std = @import("std"); const core = @import("core"); @@ -8,6 +9,30 @@ const joints = @import("joints"); const safety = @import("safety"); const kinematics = @import("kinematics"); const communication = @import("communication"); +const posix = std.posix; + +// Global shutdown flag for signal handling (atomic for thread safety) +var g_shutdown_requested = std.atomic.Value(bool).init(false); + +/// Signal handler for graceful shutdown (SIGINT, SIGTERM) +fn signalHandler(sig: i32) callconv(.C) void { + _ = sig; + g_shutdown_requested.store(true, .seq_cst); +} + +/// Setup signal handlers for graceful shutdown +fn setupSignalHandlers() !void { + const sigaction_handler = posix.Sigaction{ + .handler = .{ .handler = signalHandler }, + .mask = posix.empty_sigset, + .flags = 0, + }; + + // Handle SIGINT (Ctrl+C) + try posix.sigaction(posix.SIG.INT, &sigaction_handler, null); + // Handle SIGTERM (kill command) + try posix.sigaction(posix.SIG.TERM, &sigaction_handler, null); +} pub fn main() !void { // Initialize timing system for 1kHz control loop (1ms interval) @@ -143,17 +168,20 @@ pub fn main() !void { std.log.info("WebSocket server started on port {d}", .{web_socket_port}); + // Setup signal handlers for graceful shutdown + setupSignalHandlers() catch |err| { + std.log.warn("Failed to setup signal handlers: {} - shutdown may not be graceful", .{err}); + }; + // Power up the robot joint_manager.reset(); - - // Add graceful shutdown handling - const running: bool = true; + var iteration_count: u64 = 0; - std.log.info("Starting main control loop (production mode - infinite loop)", .{}); + std.log.info("Starting main control loop (press Ctrl+C to shutdown gracefully)", .{}); - // Main control loop - production mode - while (running) { + // Main control loop - production mode with graceful shutdown support + while (!g_shutdown_requested.load(.seq_cst)) { // Wait for next control cycle const current_time: i64 = @intCast(std.time.nanoTimestamp()); if (!timing.shouldUpdate(current_time)) continue; @@ -162,25 +190,27 @@ pub fn main() !void { joint_manager.updateStates() catch |err| { std.log.err("Joint update error: {}", .{err}); if (err == error.SafetyLimitExceeded) { - std.log.warn("Safety limits exceeded - continuing with caution", .{}); - // Reset to safe state instead of crashing + std.log.warn("Safety limits exceeded - resetting to safe state", .{}); joint_manager.reset(); continue; } break; // Exit on other errors }; - + iteration_count += 1; - + // Periodic status logging (every 10 seconds at 1kHz = 10000 iterations) if (iteration_count % 10000 == 0) { std.log.info("Control loop running: {} iterations completed", .{iteration_count}); } } - - std.log.info("Main control loop completed. Shutting down...", .{}); - + + std.log.info("Shutdown requested. Completing {} iterations...", .{iteration_count}); + std.log.info("Stopping WebSocket server...", .{}); + // Proper shutdown sequence: stop server first, then wait for thread comm_server.stop(); server_thread.join(); + + std.log.info("Shutdown complete.", .{}); } \ No newline at end of file diff --git a/backend/src/safety/safety_monitor.zig b/backend/src/safety/safety_monitor.zig index 12f8242..3c9641d 100644 --- a/backend/src/safety/safety_monitor.zig +++ b/backend/src/safety/safety_monitor.zig @@ -13,6 +13,7 @@ pub const SafetyMonitor = struct { collision_detector: *kinematics.CollisionDetection, emergency_stop_threshold: f32, is_emergency_stop: bool, + is_enabled: bool, pub fn init(joint_limits: *const [NUM_JOINTS]types.JointLimit, collision_detector: *kinematics.CollisionDetection, emergency_stop_threshold: f32) SafetyMonitor { return SafetyMonitor{ @@ -20,18 +21,33 @@ pub const SafetyMonitor = struct { .collision_detector = collision_detector, .emergency_stop_threshold = emergency_stop_threshold, .is_emergency_stop = false, + .is_enabled = true, // Safety is enabled by default }; } + /// Check all safety conditions. Returns true if safe, false if unsafe. + /// When disabled, always returns true (bypasses safety checks). pub fn checkSafety(self: *SafetyMonitor, joint_states: []const JointState) bool { + // If safety monitoring is disabled, always return safe + if (!self.is_enabled) { + return true; + } + + // Emergency stop takes precedence + if (self.is_emergency_stop) { + return false; + } + // Check joint limits for (joint_states, 0..) |state, i| { const limit = self.joint_limits.*[i]; if (state.current_angle < limit.min_angle or state.current_angle > limit.max_angle) { + std.log.warn("Safety: Joint {} angle {d:.2}° exceeds limits [{d:.2}°, {d:.2}°]", .{ i, state.current_angle, limit.min_angle, limit.max_angle }); self.is_emergency_stop = true; return false; } if (state.current_velocity < -limit.max_velocity or state.current_velocity > limit.max_velocity) { + std.log.warn("Safety: Joint {} velocity {d:.2}°/s exceeds max {d:.2}°/s", .{ i, state.current_velocity, limit.max_velocity }); self.is_emergency_stop = true; return false; } @@ -41,6 +57,7 @@ pub const SafetyMonitor = struct { const current_time = @as(f64, @floatFromInt(std.time.nanoTimestamp())); const collision_result = self.collision_detector.checkCollisions(current_time); if (collision_result.collision_detected) { + std.log.warn("Safety: Collision detected!", .{}); self.is_emergency_stop = true; return false; } @@ -48,11 +65,37 @@ pub const SafetyMonitor = struct { return true; } + /// Returns true if emergency stop is active pub fn isEmergencyStop(self: *const SafetyMonitor) bool { return self.is_emergency_stop; } + /// Returns true if safety monitoring is enabled + pub fn isEnabled(self: *const SafetyMonitor) bool { + return self.is_enabled; + } + + /// Enable safety monitoring + pub fn enable(self: *SafetyMonitor) void { + self.is_enabled = true; + std.log.info("Safety monitoring enabled", .{}); + } + + /// Disable safety monitoring (use with caution!) + pub fn disable(self: *SafetyMonitor) void { + self.is_enabled = false; + std.log.warn("Safety monitoring DISABLED - use with caution!", .{}); + } + + /// Trigger emergency stop + pub fn emergencyStop(self: *SafetyMonitor) void { + self.is_emergency_stop = true; + std.log.err("EMERGENCY STOP triggered!", .{}); + } + + /// Reset safety state (clears emergency stop, keeps enabled state) pub fn reset(self: *SafetyMonitor) void { self.is_emergency_stop = false; + std.log.info("Safety state reset", .{}); } }; \ No newline at end of file diff --git a/package.json b/package.json index 9e7e17b..62eeec7 100644 --- a/package.json +++ b/package.json @@ -1,6 +1,7 @@ { - "name": "zig-simulation", - "version": "0.1.0", + "name": "kuka-arm-simulation", + "version": "1.0.0", + "description": "KUKA Robot Arm Simulation with Zig backend and React/Three.js frontend", "private": true, "type": "module", "scripts": { @@ -38,46 +39,39 @@ "@radix-ui/react-toggle": "^1.1.1", "@radix-ui/react-toggle-group": "^1.1.1", "@radix-ui/react-tooltip": "^1.1.6", - "@react-spring/three": "latest", - "@react-three/cannon": "latest", - "@react-three/drei": "latest", - "@react-three/fiber": "latest", - "autoprefixer": "^10.4.20", + "@react-spring/three": "^9.7.3", + "@react-three/cannon": "^6.6.0", + "@react-three/drei": "^9.105.0", + "@react-three/fiber": "^8.16.0", "class-variance-authority": "^0.7.1", "clsx": "^2.1.1", "cmdk": "1.0.4", - "date-fns": "4.1.0", + "date-fns": "^3.6.0", "embla-carousel-react": "8.5.1", - "expo": "latest", - "expo-asset": "latest", - "expo-file-system": "latest", - "expo-gl": "latest", "input-otp": "1.4.1", "lucide-react": "^0.454.0", - "my-v0-project": ".", - "react": "^19", + "react": "^18.3.1", "react-day-picker": "8.10.1", - "react-dom": "^19", + "react-dom": "^18.3.1", "react-hook-form": "^7.54.1", - "react-native": "latest", "react-resizable-panels": "^2.1.7", "recharts": "2.15.0", "sonner": "^1.7.1", "tailwind-merge": "^2.5.5", "tailwindcss-animate": "^1.0.7", - "three": "latest", + "three": "^0.162.0", "vaul": "^0.9.6", - "zig-simulation": ".", "zod": "^3.24.1" }, "devDependencies": { "@types/node": "^22", - "@types/react": "^19", - "@types/react-dom": "^19", - "@types/three": "^0.169.0", + "@types/react": "^18.3.0", + "@types/react-dom": "^18.3.0", + "@types/three": "^0.162.0", "@vitejs/plugin-react": "^4.3.4", "@typescript-eslint/eslint-plugin": "^6.21.0", "@typescript-eslint/parser": "^6.21.0", + "autoprefixer": "^10.4.20", "eslint": "^8.57.0", "eslint-plugin-react-hooks": "^4.6.2", "eslint-plugin-react-refresh": "^0.4.12", @@ -86,4 +80,4 @@ "typescript": "^5", "vite": "^5.4.10" } -} \ No newline at end of file +} diff --git a/src/components/control-panel.tsx b/src/components/control-panel.tsx index 3de26f3..d2bbc73 100644 --- a/src/components/control-panel.tsx +++ b/src/components/control-panel.tsx @@ -184,33 +184,62 @@ export function ControlPanel({ angles, onAngleChange, onMovementSelect, controlS

Motor Current (A)

- {controlSignals.motorCurrents.map((current, i) => ( + {(controlSignals.motorCurrents || [0,0,0,0,0,0,0]).map((current, i) => (
J{i + 1}
-
{current.toFixed(1)}
+
{(current || 0).toFixed(2)}
))}
-

Sensor Readings

+

Temperature (°C)

- {controlSignals.sensorReadings.map((reading, i) => ( + {(controlSignals.motorTemperatures || controlSignals.sensorReadings || [25,25,25,25,25,25,25]).map((temp, i) => (
-
S{i + 1}
-
{reading.toFixed(1)}
+
J{i + 1}
+
60 ? 'text-red-400' : (temp || 0) > 45 ? 'text-yellow-400' : 'text-green-400'}`}> + {(temp || 25).toFixed(1)} +
))}
+
+

Control Mode

+ + {controlSignals.controlMode || 'POSITION'} + +
+ + {controlSignals.emergencyStop && ( +
+

EMERGENCY STOP ACTIVE

+

+ System halted. Reset required to continue operation. +

+
+ )} + + {controlSignals.collisionDetected && ( +
+

Collision Detected

+

+ Robot stopped due to collision detection. +

+
+ )} + {controlSignals.errorCode && (

Error: {controlSignals.errorCode}

{controlSignals.errorCode === "TEMP_HIGH" ? "Motor temperature exceeding normal range. Check cooling system." + : controlSignals.errorCode === "FAULT" + ? "System fault detected. Check robot state." : "Unknown error. Contact maintenance."}

@@ -222,7 +251,20 @@ export function ControlPanel({ angles, onAngleChange, onMovementSelect, controlS

Camera: Click and drag to orbit. Scroll to zoom.

- Zig microcontroller status: Connected + Zig backend status:{" "} + + {controlSignals.systemStatus === "READY" || controlSignals.systemStatus === "BUSY" + ? "Connected" + : controlSignals.systemStatus} +

diff --git a/src/components/robot-arm-simulation.tsx b/src/components/robot-arm-simulation.tsx index b2e3ad3..7b3f7b5 100644 --- a/src/components/robot-arm-simulation.tsx +++ b/src/components/robot-arm-simulation.tsx @@ -1,4 +1,4 @@ -import { useState, useEffect, Suspense } from "react" +import { useState, useEffect, useCallback, Suspense } from "react" import { Canvas } from "@react-three/fiber" import { OrbitControls, Environment, PerspectiveCamera, ContactShadows, Text } from "@react-three/drei" import { RobotArm } from "./robot-arm" @@ -7,8 +7,52 @@ import { Physics, useBox, usePlane, useSphere } from "@react-three/cannon" import { InfoPanel } from "./info-panel" import { ZigController } from "./zig-controller" +// Joint angle interface matching the 7-DOF KUKA arm +interface ArmAngles { + baseRotation: number + shoulderRotation: number + elbowRotation: number + wristBendRotation: number + wristRotation: number + toolRotation: number + gripperRotation: number +} + +// Control signals received from backend +interface ControlSignals { + // Joint state data from backend (degrees) + jointPositions?: number[] + jointVelocities?: number[] + jointTorques?: number[] + motorTemperatures?: number[] + motorCurrents?: number[] + timestamp?: number + // System status + systemStatus: string + controlMode?: string + emergencyStop?: boolean + collisionDetected?: boolean + errorCode: string | null + // Legacy fields for compatibility + sensorReadings: number[] + // Function to send commands to backend + sendJointCommand?: (positions: number[], maxVelocity?: number[]) => void +} + export default function RobotArmSimulation() { - const [armAngles, setArmAngles] = useState({ + // Target angles set by user (degrees) - these are sent to backend + const [targetAngles, setTargetAngles] = useState({ + baseRotation: 0, + shoulderRotation: 0, + elbowRotation: 0, + wristBendRotation: 0, + wristRotation: 0, + toolRotation: 0, + gripperRotation: 0, + }) + + // Actual angles from backend (degrees) - these drive the visualization + const [actualAngles, setActualAngles] = useState({ baseRotation: 0, shoulderRotation: 0, elbowRotation: 0, @@ -18,15 +62,19 @@ export default function RobotArmSimulation() { gripperRotation: 0, }) - const [controlSignals, setControlSignals] = useState({ + // Control signals from backend + const [controlSignals, setControlSignals] = useState({ motorCurrents: [0, 0, 0, 0, 0, 0, 0], sensorReadings: [0, 0, 0, 0, 0, 0, 0], - systemStatus: "READY", + systemStatus: "CONNECTING", errorCode: null, }) - // Predefined movements - const movements = { + // Track if we're connected to backend + const [isBackendConnected, setIsBackendConnected] = useState(false) + + // Predefined movements (target positions in degrees) + const movements: Record = { rest: { baseRotation: 0, shoulderRotation: 0, @@ -74,35 +122,70 @@ export default function RobotArmSimulation() { }, } - // Simulate Zig controller communication + // Update actual angles when backend sends joint positions useEffect(() => { - const interval = setInterval(() => { - // Simulate motor current based on joint movement - const newMotorCurrents = Object.values(armAngles).map((angle) => Math.abs(angle) * 0.1 + Math.random() * 0.5) - - // Simulate sensor readings - const newSensorReadings = newMotorCurrents.map((current) => current + (Math.random() - 0.5) * 0.2) - - setControlSignals({ - motorCurrents: newMotorCurrents, - sensorReadings: newSensorReadings, - systemStatus: Math.random() > 0.98 ? "WARNING" : "READY", - errorCode: Math.random() > 0.98 ? "TEMP_HIGH" : null, + if (controlSignals.jointPositions && controlSignals.jointPositions.length >= 7) { + setActualAngles({ + baseRotation: controlSignals.jointPositions[0], + shoulderRotation: controlSignals.jointPositions[1], + elbowRotation: controlSignals.jointPositions[2], + wristBendRotation: controlSignals.jointPositions[3], + wristRotation: controlSignals.jointPositions[4], + toolRotation: controlSignals.jointPositions[5], + gripperRotation: controlSignals.jointPositions[6], }) - }, 500) - - return () => clearInterval(interval) - }, [armAngles]) - - const handleAngleChange = (joint, value) => { - setArmAngles((prev) => ({ ...prev, [joint]: value })) - } + setIsBackendConnected(true) + } + }, [controlSignals.jointPositions]) - const executeMovement = (movementName) => { + // Track connection status + useEffect(() => { + if (controlSignals.systemStatus === "READY" || controlSignals.systemStatus === "BUSY") { + setIsBackendConnected(true) + } else if (controlSignals.systemStatus === "CONNECTING") { + setIsBackendConnected(false) + } + }, [controlSignals.systemStatus]) + + // Send command to backend when target angles change + const sendCommandToBackend = useCallback((angles: ArmAngles) => { + if (controlSignals.sendJointCommand) { + const positions = [ + angles.baseRotation, + angles.shoulderRotation, + angles.elbowRotation, + angles.wristBendRotation, + angles.wristRotation, + angles.toolRotation, + angles.gripperRotation, + ] + // Send with default max velocity (backend will use limits) + controlSignals.sendJointCommand(positions) + } + }, [controlSignals.sendJointCommand]) + + // Handle user changing a joint angle via slider + const handleAngleChange = useCallback((joint: keyof ArmAngles, value: number) => { + setTargetAngles((prev) => { + const newAngles = { ...prev, [joint]: value } + // Send command to backend + sendCommandToBackend(newAngles) + return newAngles + }) + }, [sendCommandToBackend]) + + // Handle preset movement selection + const executeMovement = useCallback((movementName: string) => { if (movements[movementName]) { - setArmAngles(movements[movementName]) + const newAngles = movements[movementName] + setTargetAngles(newAngles) + // Send command to backend + sendCommandToBackend(newAngles) } - } + }, [movements, sendCommandToBackend]) + + // Decide which angles to show: use actual backend angles if connected, otherwise target angles + const displayAngles = isBackendConnected ? actualAngles : targetAngles return (
@@ -139,8 +222,8 @@ export default function RobotArmSimulation() { restitution: 0.2, }} > - {/* Robot arm */} - + {/* Robot arm - uses backend angles when connected */} + {/* Floor with shadows */} @@ -174,8 +257,9 @@ export default function RobotArmSimulation() { + {/* Control panel - shows target angles for user input */} + {/* Zig backend controller - manages WebSocket connection */}
) @@ -245,8 +330,8 @@ function InteractiveObjects() { ) } -function Box({ position, color, size = [0.5, 0.5, 0.5] }) { - const [ref, api] = useBox(() => ({ +function Box({ position, color, size = [0.5, 0.5, 0.5] }: { position: [number, number, number], color: string, size?: [number, number, number] }) { + const [ref] = useBox(() => ({ mass: 1, position, args: size, @@ -260,8 +345,8 @@ function Box({ position, color, size = [0.5, 0.5, 0.5] }) { ) } -function Sphere({ position, color, radius = 0.5 }) { - const [ref, api] = useSphere(() => ({ +function Sphere({ position, color, radius = 0.5 }: { position: [number, number, number], color: string, radius?: number }) { + const [ref] = useSphere(() => ({ mass: 1, position, args: [radius], @@ -275,8 +360,8 @@ function Sphere({ position, color, radius = 0.5 }) { ) } -function Cylinder({ position, color }) { - const [ref, api] = useBox(() => ({ +function Cylinder({ position, color }: { position: [number, number, number], color: string }) { + const [ref] = useBox(() => ({ mass: 1, position, args: [0.2, 0.3, 0.2], @@ -289,4 +374,3 @@ function Cylinder({ position, color }) { ) } - diff --git a/src/components/zig-controller.tsx b/src/components/zig-controller.tsx index 974e499..1ea384f 100644 --- a/src/components/zig-controller.tsx +++ b/src/components/zig-controller.tsx @@ -20,16 +20,40 @@ const COMMAND_TYPE_VELOCITY = 1 const COMMAND_TYPE_TORQUE = 2 const COMMAND_TYPE_SAFETY = 4 -// TypeScript interfaces matching FE.md +// System state constants (matching backend protocol.zig) +const SYSTEM_STATE_READY = 0 +const SYSTEM_STATE_BUSY = 1 +const SYSTEM_STATE_ERROR = 2 +const SYSTEM_STATE_WARNING = 3 + +// Control mode constants (matching backend protocol.zig) +const CONTROL_MODE_POSITION = 0 +const CONTROL_MODE_VELOCITY = 1 +const CONTROL_MODE_TORQUE = 2 + +// TypeScript interfaces matching backend protocol interface JointState { - position: number[] // Current joint positions (rad) - velocity: number[] // Current joint velocities (rad/s) + position: number[] // Current joint positions (degrees from backend) + velocity: number[] // Current joint velocities (deg/s) torque: number[] // Current joint torques (Nm) temperature: number[] // Motor temperatures (°C) current: number[] // Motor currents (A) timestamp: number // Timestamp in microseconds } +// Raw backend system status (uses numeric values) +interface BackendSystemStatus { + state: number + error_code: string | null + safety_status: { + soft_limits_active: boolean + emergency_stop: boolean + collision_detected: boolean + } + control_mode: number +} + +// Frontend-friendly system status (uses string values) interface SystemStatus { state: 'READY' | 'BUSY' | 'ERROR' | 'WARNING' errorCode: string | null @@ -45,17 +69,50 @@ interface CollisionData { detected: boolean link1?: string link2?: string - position?: { x: number, y: number, z: number } - penetrationDepth?: number - contactNormal?: { x: number, y: number, z: number } + position?: [number, number, number] + penetration_depth?: number + contact_normal?: [number, number, number] } interface ConnectionStatus { - status: 'connected' | 'disconnected' | 'error' - message: string + status: string + message: string | null timestamp_us: number } +// Helper to convert backend numeric state to frontend string +const stateToString = (state: number): 'READY' | 'BUSY' | 'ERROR' | 'WARNING' => { + switch (state) { + case SYSTEM_STATE_READY: return 'READY' + case SYSTEM_STATE_BUSY: return 'BUSY' + case SYSTEM_STATE_ERROR: return 'ERROR' + case SYSTEM_STATE_WARNING: return 'WARNING' + default: return 'ERROR' + } +} + +// Helper to convert backend numeric control mode to frontend string +const controlModeToString = (mode: number): 'POSITION' | 'VELOCITY' | 'TORQUE' => { + switch (mode) { + case CONTROL_MODE_POSITION: return 'POSITION' + case CONTROL_MODE_VELOCITY: return 'VELOCITY' + case CONTROL_MODE_TORQUE: return 'TORQUE' + default: return 'POSITION' + } +} + +// Convert backend system status to frontend format +const convertSystemStatus = (backend: BackendSystemStatus): SystemStatus => ({ + state: stateToString(backend.state), + errorCode: backend.error_code, + safetyStatus: { + softLimitsActive: backend.safety_status.soft_limits_active, + emergencyStop: backend.safety_status.emergency_stop, + collisionDetected: backend.safety_status.collision_detected + }, + controlMode: controlModeToString(backend.control_mode) +}) + type ConnectionState = 'disconnected' | 'connecting' | 'connected' | 'error' interface ZigControllerProps { @@ -207,14 +264,26 @@ export function ZigController({ controlSignals, setControlSignals }: ZigControll } else if (typeof event.data === 'string') { // Text frame - JSON data (system status, collision, connection status) const jsonData = JSON.parse(event.data) - + // Determine message type based on JSON structure - if (jsonData.state !== undefined && jsonData.controlMode !== undefined) { - // System status message - const status: SystemStatus = jsonData + if (jsonData.state !== undefined && jsonData.control_mode !== undefined) { + // System status message (uses snake_case from backend) + const backendStatus: BackendSystemStatus = jsonData + const status = convertSystemStatus(backendStatus) setSystemStatus(status) + + // Update control signals with system status + setControlSignals((prev: any) => ({ + ...prev, + systemStatus: status.state, + controlMode: status.controlMode, + emergencyStop: status.safetyStatus.emergencyStop, + collisionDetected: status.safetyStatus.collisionDetected, + errorCode: status.errorCode + })) + setLastMessage(`System Status: ${status.state}`) - } else if (jsonData.detected !== undefined) { + } else if (jsonData.detected !== undefined && jsonData.link1 !== undefined) { // Collision data message const collision: CollisionData = jsonData setCollisionData(collision)