Mostly UI Update

.gitattributes

@@ -1 +1,2 @@
 *.stl filter=lfs diff=lfs merge=lfs -text
+*.png filter=lfs diff=lfs merge=lfs -text

.prettierignore

@@ -4,3 +4,9 @@ pnpm-lock.yaml
 yarn.lock
 bun.lock
 bun.lockb
+
+# Src python
+src-python/
+node_modules/
+build/
+.svelte-kit/

.prettierrc

@@ -1,6 +1,6 @@
 {
 	"useTabs": true,
-	"singleQuote": true,
+	"singleQuote": false,
 	"trailingComma": "none",
 	"printWidth": 100,
 	"plugins": ["prettier-plugin-svelte", "prettier-plugin-tailwindcss"],

DOCKER_README.md

@@ -1,335 +0,0 @@
-# 🐳 LeRobot Arena Docker Setup
-
-This Docker setup provides a containerized environment that runs both the Python backend and Svelte frontend for LeRobot Arena using modern tools like Bun, uv, and box-packager.
-
-## 🏗️ Architecture
-
-The application consists of:
-- **Svelte Frontend**: Built as static files and served by FastAPI
-- **FastAPI Backend**: WebSocket server with HTTP API for robot control
-- **Single Port**: Both frontend and backend served on port 7860
-
-## 🚀 Quick Start
-
-### Build and Run with Docker
-
-```bash
-# Build the image
-docker build -t lerobot-arena .
-
-# Run the container
-docker run -p 7860:7860 lerobot-arena
-
-# Access the application
-open http://localhost:7860
-```
-
-### Using Docker Compose
-
-```bash
-# Start the application
-docker-compose up
-
-# Access the application
-open http://localhost:7860
-```
-
-## 🌐 Deployment
-
-### Hugging Face Spaces
-
-This project is configured for deployment on Hugging Face Spaces:
-
-1. Push to a Hugging Face Space repository
-2. The Dockerfile will automatically build and serve the application
-3. Both frontend and backend run on port 7860 (HF Spaces default)
-4. Environment detection automatically configures URLs for HF Spaces
-
-### Local Development vs Production
-
-- **Local Development**: Frontend dev server on 5173, backend on 7860
-- **Docker/Production**: Both frontend and backend served by FastAPI on 7860
-- **HF Spaces**: Single FastAPI server on 7860 with automatic HTTPS and domain detection
-
-## 🔧 Environment Variables
-
-- `SPACE_HOST`: Set automatically by Hugging Face Spaces for proper URL configuration
-- `PORT`: Override the default port (7860)
-
-## 📡 API Endpoints
-
-- **Frontend**: `http://localhost:7860/` (Served by FastAPI)
-- **API**: `http://localhost:7860/api/*`
-- **WebSocket**: `ws://localhost:7860/ws/*`
-- **Status**: `http://localhost:7860/status`
-
-## 🛠️ Development
-
-For local development without Docker:
-
-```bash
-# Start backend
-cd src-python
-uv sync
-uv run python start_server.py
-
-# Start frontend (in another terminal)
-bun install
-bun run dev
-```
-
-The frontend dev server (port 5173) will automatically proxy API requests to the backend (port 7860).
-
-## 📋 What's Included
-
-The Docker container includes:
-
-1. **Frontend**: 
-   - Svelte application built as static files using **Bun**
-   - Served on port 7860 using Python's built-in HTTP server
-   - Production-ready build with all optimizations
-
-2. **Backend**:
-   - FastAPI Python server with dependencies managed by **uv**
-   - **Standalone executable** created with **box-packager** for faster startup
-   - WebSocket support for real-time robot communication
-   - Runs on port 8080
-   - Auto-configured for container environment
-
-## 🛠️ Development vs Production
-
-### Production (Default)
-The Dockerfile builds the Svelte app as static files using Bun and packages the Python backend as a standalone executable using box-packager.
-
-### Development
-For development with hot-reload, you can use local tools:
-
-```bash
-# Terminal 1: Frontend development
-bun run dev
-
-# Terminal 2: Backend development
-cd src-python && python start_server.py
-```
-
-## 📁 Container Structure
-
-```
-/home/user/app/
-├── src-python/          # Python backend source
-│   └── target/release/  # Box-packager standalone executable
-├── static-frontend/     # Built Svelte app (static files)
-└── start_services.sh    # Startup script for both services
-```
-
-## 🔧 Build Process
-
-The Docker build process includes these key steps:
-
-1. **Frontend Build Stage (Bun)**:
-   - Uses `oven/bun:1-alpine` for fast package management
-   - Builds Svelte app with `bun run build`
-   - Produces optimized static files
-
-2. **Backend Build Stage (Python + Rust)**:
-   - Installs Rust/Cargo for box-packager
-   - Uses `uv` for fast Python dependency management  
-   - Packages backend with `box package` into standalone executable
-   - Configures proper user permissions for HF Spaces
-
-3. **Runtime**:
-   - Runs standalone executable (faster startup)
-   - Serves frontend static files
-   - Both services managed by startup script
-
-## 🚀 Performance Benefits
-
-### Box-packager Advantages:
-- **Faster startup**: No Python interpreter overhead
-- **Self-contained**: All dependencies bundled
-- **Smaller runtime**: No need for full Python environment
-- **Cross-platform**: Single executable works anywhere
-
-### Build Optimization:
-- **Bun**: Faster JavaScript package manager and bundler
-- **uv**: Ultra-fast Python package manager
-- **Multi-stage build**: Minimal final image size
-- **Alpine base**: Lightweight frontend build stage
-
-## 🔧 Customization
-
-### Environment Variables
-
-You can customize the container behavior using environment variables:
-
-```bash
-docker run -p 8080:8080 -p 7860:7860 \
-  -e PYTHONUNBUFFERED=1 \
-  -e NODE_ENV=production \
-  lerobot-arena
-```
-
-### Port Configuration
-
-To use different ports:
-
-```bash
-# Map to different host ports
-docker run -p 9000:8080 -p 3000:7860 lerobot-arena
-```
-
-Then access:
-- Frontend: http://localhost:3000
-- Backend: http://localhost:9000
-
-### Volume Mounts for Persistence
-
-```bash
-# Mount data directory for persistence
-docker run -p 8080:8080 -p 7860:7860 \
-  -v $(pwd)/data:/home/user/app/data \
-  lerobot-arena
-```
-
-## 🐛 Troubleshooting
-
-### Container won't start
-```bash
-# Check logs
-docker-compose logs lerobot-arena
-
-# Or for direct docker run
-docker logs <container-id>
-```
-
-### Port conflicts
-```bash
-# Check what's using the ports
-lsof -i :8080
-lsof -i :7860
-
-# Kill processes or use different ports
-docker run -p 8081:8080 -p 7861:7860 lerobot-arena
-```
-
-### Frontend not loading
-```bash
-# Verify the frontend was built correctly
-docker exec -it <container-id> ls -la /home/user/app/static-frontend
-
-# Check if frontend server is running
-docker exec -it <container-id> ps aux | grep python
-```
-
-### Backend API errors
-```bash
-# Check if standalone executable is running
-docker exec -it <container-id> ps aux | grep lerobot-arena-server
-
-# Test backend directly
-curl http://localhost:8080/
-```
-
-### Box-packager Build Issues
-```bash
-# Force rebuild without cache to fix cargo/box issues
-docker-compose build --no-cache
-docker-compose up
-
-# Check if Rust/Cargo is properly installed
-docker exec -it <container-id> cargo --version
-```
-
-## 🔄 Updates and Rebuilding
-
-```bash
-# Pull latest code and rebuild
-git pull
-docker-compose down
-docker-compose up --build
-
-# Force rebuild without cache
-docker-compose build --no-cache
-docker-compose up
-```
-
-## 🚀 Hugging Face Spaces Deployment
-
-This Docker setup is optimized for Hugging Face Spaces:
-
-### Key Features for HF Spaces:
-- **Port 7860**: Uses the default HF Spaces port
-- **User permissions**: Runs as user ID 1000 as required
-- **Proper ownership**: All files owned by the user
-- **Git support**: Includes git for dependency resolution
-- **Standalone executable**: Faster cold starts on HF infrastructure
-
-### Deployment Steps:
-1. Push your code to a GitHub repository
-2. Create a new Space on Hugging Face Spaces
-3. Connect your GitHub repository
-4. The YAML frontmatter in README.md will auto-configure the Space
-5. HF will build and deploy your Docker container automatically
-
-### Accessing on HF Spaces:
-- Your Space URL will serve the frontend directly
-- Backend API will be available at your-space-url/api/ (if using a reverse proxy)
-
-## 🔧 Advanced Configuration
-
-### Using with nginx (Production)
-```nginx
-server {
-    listen 80;
-    
-    # Serve frontend
-    location / {
-        proxy_pass http://localhost:7860;
-    }
-    
-    # Proxy API calls
-    location /api/ {
-        proxy_pass http://localhost:8080;
-        proxy_http_version 1.1;
-        proxy_set_header Upgrade $http_upgrade;
-        proxy_set_header Connection 'upgrade';
-        proxy_set_header Host $host;
-        proxy_cache_bypass $http_upgrade;
-    }
-    
-    # WebSocket support
-    location /ws/ {
-        proxy_pass http://localhost:8080;
-        proxy_http_version 1.1;
-        proxy_set_header Upgrade $http_upgrade;
-        proxy_set_header Connection "upgrade";
-    }
-}
-```
-
-## 📊 Container Stats
-
-```bash
-# Monitor resource usage
-docker stats lerobot-arena
-
-# View container info
-docker inspect lerobot-arena
-```
-
-## 🧹 Cleanup
-
-```bash
-# Stop and remove containers
-docker-compose down
-
-# Remove images
-docker rmi lerobot-arena
-
-# Clean up unused images and containers
-docker system prune -a
-```
-
----
-
-**Happy containerized robotics! 🤖🐳** 

README.md

@@ -37,17 +37,20 @@ The easiest way to run LeRobot Arena is using Docker, which sets up both the fro
 ### Step-by-Step Instructions
 
 1. **Clone the repository**
+
    ```bash
    git clone <your-repo-url>
    cd lerobot-arena
    ```
 
 2. **Build and start the services**
+
    ```bash
    docker-compose up --build
    ```
 
 3. **Access the application**
+
    - **Frontend**: http://localhost:7860
    - **Backend API**: http://localhost:8080
    - **API Documentation**: http://localhost:8080/docs
@@ -158,17 +161,20 @@ For detailed Docker documentation, see [DOCKER_README.md](./DOCKER_README.md).
 ## 🔧 Building for Production
 
 ### Frontend Only
+
 ```bash
 bun run build
 ```
 
 ### Backend Standalone Executable
+
 ```bash
 cd src-python
 box package
 ```
 
 ### Complete Docker Build
+
 ```bash
 docker-compose up --build
 ```
@@ -185,6 +191,7 @@ docker-compose up --build
 ## 🚨 Troubleshooting
 
 ### Port Conflicts
+
 If ports 8080 or 7860 are already in use:
 
 ```bash
@@ -197,6 +204,7 @@ docker run -p 8081:8080 -p 7861:7860 lerobot-arena
 ```
 
 ### Container Issues
+
 ```bash
 # View logs
 docker-compose logs lerobot-arena
@@ -207,6 +215,7 @@ docker-compose up
 ```
 
 ### Development Issues
+
 ```bash
 # Clear node modules and reinstall
 rm -rf node_modules
@@ -218,6 +227,7 @@ bun run dev
 ```
 
 ### Box-packager Issues
+
 ```bash
 # Clean build artifacts
 cd src-python

ROBOT_ARCHITECTURE.md

@@ -1,416 +1,555 @@
-# Robot Control Architecture v2.0 - Master/Slave Pattern
+# LeRobot Arena - Robot Control Architecture v2.0
 
-This document describes the revolutionary new **Master-Slave Architecture** that enables sophisticated robot control with clear separation between command sources (Masters) and execution targets (Slaves).
+> **Master-Slave Pattern for Scalable Robot Control**  
+> A revolutionary architecture that separates command generation (Masters) from execution (Slaves), enabling sophisticated robot control scenarios from simple manual operation to complex multi-robot coordination.
 
 ## 🏗️ Architecture Overview
 
-The new architecture follows a **Master-Slave Pattern** with complete separation of concerns:
+The architecture follows a **Master-Slave Pattern** with complete separation of concerns:
 
 ```
 ┌─────────────────┐    ┌──────────────────┐    ┌─────────────────┐
-│   UI Panel      │    │  Robot Manager   │    │    Masters      │
+│   Web Frontend  │    │  RobotManager    │    │    Masters      │
 │                 │◄──►│                  │◄──►│                 │
+│ • 3D Visualization   │ • Robot Creation │    │ • USB Master    │
 │ • Manual Control│    │ • Master/Slave   │    │ • Remote Server │
-│ • Monitoring    │    │ • Command Router │    │ • Mock Sequence │
-│ (disabled if    │    │ • State Sync     │    │ • Script Player │
+│ • Monitoring    │    │   Orchestration  │    │ • Mock Sequence │
+│ (disabled when  │    │ • State Sync     │    │   (1 per robot) │
 │  master active) │    └──────────────────┘    └─────────────────┘
 └─────────────────┘             │
                                 ▼
                        ┌──────────────────┐    ┌─────────────────┐
-                       │   Robot Model    │◄──►│     Slaves      │
+                       │   Robot Class    │◄──►│     Slaves      │
                        │                  │    │                 │
-                       │ • URDF State     │    │ • USB Robots    │
-                       │ • Joint States   │    │ • Mock Hardware │
-                       │ • Command Queue  │    │ • Simulations   │
+                       │ • Joint States   │    │ • USB Robot     │
+                       │ • URDF Model     │    │ • Remote Robot  │
+                       │ • Command Queue  │    │ • WebSocket     │
+                       │ • Calibration    │    │   (N per robot) │
                        └──────────────────┘    └─────────────────┘
+                                │
+                                ▼
+                       ┌──────────────────┐
+                       │  Python Backend  │
+                       │                  │
+                       │ • WebSocket API  │
+                       │ • Connection Mgr │
+                       │ • Robot Manager  │
+                       └──────────────────┘
 ```
 
-## 🎯 Key Concepts
-
-### Masters (Command Sources)
-- **Purpose**: Generate commands and control sequences
-- **Examples**: Remote servers, predefined sequences, scripts
-- **Limitation**: Only 1 master per robot (exclusive control)
-- **Effect**: When active, disables manual panel control
+### Control Flow States
 
-### Slaves (Execution Targets)  
-- **Purpose**: Execute commands on physical/virtual robots
-- **Examples**: USB robots, mock robots, simulations
-- **Limitation**: Multiple slaves per robot (parallel execution)
-- **Effect**: All connected slaves execute the same commands
+```
+┌─────────────────┐    Master Connected    ┌─────────────────┐
+│   Manual Mode   │ ──────────────────────► │  Master Mode    │
+│                 │                         │                 │
+│ ✅ Panel Active  │                         │ ❌ Panel Locked  │
+│ ✅ Direct Control│                         │ ✅ Master Commands│
+│ ❌ No Master     │                         │ ✅ All Slaves Exec│
+└─────────────────┘ ◄────────────────────── └─────────────────┘
+                     Master Disconnected
+```
 
-### Control Flow
-1. **No Master**: Manual panel control → All slaves
-2. **With Master**: Master commands → All slaves (panel disabled)
-3. **Bidirectional**: Slaves provide real-time feedback
+## 🎯 Core Concepts
 
-## 📁 File Structure
+### Masters (Command Sources)
+**Purpose**: Generate and provide robot control commands
+
+| Type | Description | Connection | Use Case |
+|------|-------------|------------|----------|
+| **USB Master** | Physical robot as command source | USB/Serial | Teleoperation, motion teaching |
+| **Remote Server** | WebSocket/HTTP command reception | Network | External control systems |
+| **Mock Sequence** | Predefined movement patterns | Internal | Testing, demonstrations |
+
+**Key Rules**:
+- 🔒 **Exclusive Control**: Only 1 master per robot
+- 🚫 **Panel Lock**: Manual control disabled when master active
+- 🔄 **Seamless Switch**: Masters can be swapped dynamically
+
+### Slaves (Execution Targets)
+**Purpose**: Execute commands on physical or virtual robots
+
+| Type | Description | Connection | Use Case |
+|------|-------------|------------|----------|
+| **USB Slave** | Physical robot control | USB/Serial | Hardware execution |
+| **Remote Server Slave** | Network robot control | WebSocket | Distributed robots |
+| **WebSocket Slave** | Real-time WebSocket execution | WebSocket | Cloud robots |
+
+**Key Rules**:
+- 🔢 **Multiple Allowed**: N slaves per robot
+- 🎯 **Parallel Execution**: All slaves execute same commands
+- 🔄 **Independent Operation**: Slaves can fail independently
+
+### Architecture Comparison
+
+| Aspect | v1.0 (Single Driver) | v2.0 (Master-Slave) |
+|--------|---------------------|---------------------|
+| **Connection Model** | 1 Driver ↔ 1 Robot | 1 Master + N Slaves ↔ 1 Robot |
+| **Command Source** | Always UI Panel | Master OR UI Panel |
+| **Execution Targets** | Single Connection | Multiple Parallel |
+| **Control Hierarchy** | Flat | Hierarchical |
+| **Scalability** | Limited | Unlimited |
+
+## 📁 Project Structure
+
+### Frontend Architecture (TypeScript + Svelte)
+```
+src/lib/robot/
+├── Robot.svelte.ts              # Individual robot master-slave coordination
+├── RobotManager.svelte.ts       # Global robot orchestration
+└── drivers/
+    ├── USBMaster.ts             # Physical robot as command source
+    ├── RemoteServerMaster.ts    # WebSocket command reception
+    ├── USBSlave.ts              # Physical robot execution
+    ├── RemoteServerSlave.ts     # Network robot execution
+    └── WebSocketSlave.ts        # Real-time WebSocket execution
+
+src/lib/types/
+├── robotDriver.ts               # Master/Slave interfaces
+└── robot.ts                     # Robot state management
+```
 
+### Backend Architecture (Python + FastAPI)
 ```
-src/lib/
-├── robot/
-│   ├── Robot.svelte.ts              # Master-slave robot management
-│   ├── RobotManager.svelte.ts       # Global orchestration
-│   └── drivers/
-│       ├── masters/
-│       │   ├── MockSequenceMaster.ts    # Demo sequences
-│       │   ├── RemoteServerMaster.ts    # HTTP/WebSocket commands
-│       │   └── ScriptPlayerMaster.ts    # Script execution
-│       └── slaves/
-│           ├── MockSlave.ts             # Development/testing
-│           ├── USBSlave.ts              # Physical USB robots
-│           └── SimulationSlave.ts       # Physics simulation
-├── types/
-│   └── robotDriver.ts               # Master/Slave interfaces
-└── components/
-    ├── scene/
-    │   └── Robot.svelte             # 3D visualization
-    └── panel/
-        └── RobotControlPanel.svelte # Master/Slave connection UI
+src-python/src/
+├── main.py                      # FastAPI server + WebSocket endpoints
+├── robot_manager.py             # Server-side robot lifecycle
+├── connection_manager.py        # WebSocket connection handling
+└── models.py                    # Pydantic data models
 ```
 
-## 🔧 Core Components
+## 🎮 Usage Examples
 
-### RobotManager
-**Central orchestrator with master-slave management**
+### Basic Robot Setup
 
 ```typescript
-import { robotManager } from '$lib/robot/RobotManager.svelte';
+import { robotManager } from "$lib/robot/RobotManager.svelte";
 
-// Create robot
-const robot = await robotManager.createRobot('my-robot', urdfConfig);
-
-// Connect demo sequence master (disables manual control)
-await robotManager.connectDemoSequences('my-robot', true);
+// Create robot from URDF
+const robot = await robotManager.createRobot("demo-arm", {
+  urdfPath: "/robots/so-arm100/robot.urdf",
+  jointNameIdMap: { "Rotation": 1, "Pitch": 2, "Elbow": 3 },
+  restPosition: { "Rotation": 0, "Pitch": 0, "Elbow": 0 }
+});
 
-// Connect mock slave (executes commands)  
-await robotManager.connectMockSlave('my-robot', 50);
+// Add execution targets (slaves)
+await robotManager.connectUSBSlave("demo-arm");           // Real hardware
+await robotManager.connectRemoteServerSlave("demo-arm");  // Network robot
 
-// Connect USB slave (real hardware)
-await robotManager.connectUSBSlave('my-robot');
+// Connect command source (master) - panel becomes locked
+await robotManager.connectUSBMaster("demo-arm"); 
 
-// Disconnect master (restores manual control)
-await robotManager.disconnectMaster('my-robot');
+// Result: USB master controls both USB and Remote slaves
 ```
 
-### Robot Class
-**Individual robot with master-slave coordination**
+### Master Switching Workflow
 
 ```typescript
-// Master management
-await robot.setMaster(sequenceMaster);
-await robot.removeMaster();
-
-// Slave management
-await robot.addSlave(usbSlave);
-await robot.addSlave(mockSlave);
-await robot.removeSlave(slaveId);
-
-// Control state
-robot.controlState.hasActiveMaster    // true when master connected
-robot.controlState.manualControlEnabled  // false when master active
-robot.controlState.lastCommandSource     // "master" | "manual" | "none"
-
-// Manual control (only when no master)
-await robot.updateJointValue('joint_1', 45);
-```
+const robot = robotManager.getRobot("my-robot");
 
-## 🔌 Master Drivers
+// Start with manual control
+console.log(robot.manualControlEnabled); // ✅ true
 
-### Mock Sequence Master
-**Predefined movement sequences for testing**
+// Switch to USB master (robot becomes command source)
+await robotManager.connectUSBMaster("my-robot");
+console.log(robot.manualControlEnabled); // ❌ false (panel locked)
 
-```typescript
-const config: MasterDriverConfig = {
-  type: "mock-sequence",
-  sequences: DEMO_SEQUENCES,  // Wave, Scan, Pick-Place patterns
-  autoStart: true,
-  loopMode: true
-};
-
-await robotManager.connectMaster('robot-1', config);
-```
+// Switch to remote control
+await robotManager.disconnectMaster("my-robot");
+await robotManager.connectMaster("my-robot", {
+  type: "remote-server",
+  url: "ws://robot-controller:8080/ws"
+});
 
-**Demo Sequences:**
-- **Wave Pattern**: Greeting gesture with wrist roll
-- **Scanning Pattern**: Horizontal sweep with pitch variation  
-- **Pick and Place**: Complete manipulation sequence
+// Restore manual control
+await robotManager.disconnectMaster("my-robot");
+console.log(robot.manualControlEnabled); // ✅ true (panel restored)
+```
 
-### Remote Server Master *(Planned)*
-**HTTP/WebSocket command reception**
+## 🔌 Driver Implementations
 
-```typescript
-const config: MasterDriverConfig = {
-  type: "remote-server",
-  url: "ws://robot-server:8080/ws",
-  apiKey: "your-api-key",
-  pollInterval: 100
-};
-```
+### USB Master Driver
 
-### Script Player Master *(Planned)*
-**JavaScript/Python script execution**
+**Physical robot as command source for teleoperation**
 
 ```typescript
-const config: MasterDriverConfig = {
-  type: "script-player", 
-  scriptUrl: "/scripts/robot-dance.js",
-  variables: { speed: 1.5, amplitude: 45 }
-};
+// USBMaster.ts - Core implementation
+export class USBMaster implements MasterDriver {
+  readonly type = "master" as const;
+  private feetechDriver: FeetechSerialDriver;
+  private pollIntervalId?: number;
+
+  async connect(): Promise<void> {
+    // Initialize feetech.js serial connection
+    this.feetechDriver = new FeetechSerialDriver({
+      port: this.config.port || await this.detectPort(),
+      baudRate: this.config.baudRate || 115200
+    });
+    
+    await this.feetechDriver.connect();
+    this.startPolling();
+  }
+
+  private startPolling(): void {
+    this.pollIntervalId = setInterval(async () => {
+      try {
+        // Read current joint positions from hardware
+        const jointStates = await this.readAllJoints();
+        
+        // Convert to robot commands
+        const commands = this.convertToCommands(jointStates);
+        
+        // Emit commands to slaves
+        this.notifyCommand(commands);
+      } catch (error) {
+        console.error('USB Master polling error:', error);
+      }
+    }, this.config.pollInterval || 100);
+  }
+
+  private async readAllJoints(): Promise<DriverJointState[]> {
+    const states: DriverJointState[] = [];
+    
+    for (const [jointName, servoId] of Object.entries(this.jointMap)) {
+      const position = await this.feetechDriver.readPosition(servoId);
+      states.push({
+        name: jointName,
+        servoId,
+        type: "revolute",
+        virtualValue: position,
+        realValue: position
+      });
+    }
+    
+    return states;
+  }
+}
 ```
 
-## 🤖 Slave Drivers
+**Usage Pattern:**
+- Connect USB robot as master
+- Physical robot becomes the command source
+- Move robot manually → slaves follow the movement
+- Ideal for: Teleoperation, motion teaching, demonstration recording
 
-### Mock Slave
-**Perfect simulation for development**
+### USB Slave Driver
+
+**Physical robot as execution target**
 
 ```typescript
-const config: SlaveDriverConfig = {
-  type: "mock-slave",
-  simulateLatency: 50,      // ms response delay
-  simulateErrors: false,    // random failures
-  responseDelay: 20         // command execution time
-};
-
-await robotManager.connectSlave('robot-1', config);
+// USBSlave.ts - Core implementation
+export class USBSlave implements SlaveDriver {
+  readonly type = "slave" as const;
+  private feetechDriver: FeetechSerialDriver;
+  private calibrationOffsets: Map<string, number> = new Map();
+
+  async executeCommand(command: RobotCommand): Promise<void> {
+    for (const joint of command.joints) {
+      const servoId = this.getServoId(joint.name);
+      if (!servoId) continue;
+
+      // Apply calibration offset
+      const offset = this.calibrationOffsets.get(joint.name) || 0;
+      const adjustedValue = joint.value + offset;
+
+      // Send to hardware via feetech.js
+      await this.feetechDriver.writePosition(servoId, adjustedValue, {
+        speed: joint.speed || 100,
+        acceleration: 50
+      });
+    }
+  }
+
+  async readJointStates(): Promise<DriverJointState[]> {
+    const states: DriverJointState[] = [];
+    
+    for (const joint of this.jointStates) {
+      const position = await this.feetechDriver.readPosition(joint.servoId);
+      const offset = this.calibrationOffsets.get(joint.name) || 0;
+      
+      states.push({
+        ...joint,
+        realValue: position - offset // Remove offset for accurate state
+      });
+    }
+    
+    return states;
+  }
+
+  async calibrate(): Promise<void> {
+    console.log('Calibrating USB robot...');
+    
+    for (const joint of this.jointStates) {
+      // Read current hardware position
+      const currentPos = await this.feetechDriver.readPosition(joint.servoId);
+      
+      // Calculate offset: desired_rest - actual_position
+      const offset = joint.restPosition - currentPos;
+      this.calibrationOffsets.set(joint.name, offset);
+      
+      console.log(`Joint ${joint.name}: offset=${offset.toFixed(1)}°`);
+    }
+  }
+}
 ```
 
 **Features:**
-- ✅ Perfect command execution (real = virtual)
-- ✅ Configurable latency and errors
-- ✅ Real-time state feedback
-- ✅ Instant connection
+- Direct hardware control via feetech.js
+- Real position feedback
+- Calibration offset support
+- Smooth motion interpolation
 
-### USB Slave *(Planned)*
-**Physical robot via feetech.js**
+### Remote Server Master
+
+**Network command reception via WebSocket**
 
 ```typescript
-const config: SlaveDriverConfig = {
-  type: "usb-slave",
-  port: "/dev/ttyUSB0",     // auto-detect if undefined
-  baudRate: 115200
-};
+// RemoteServerMaster.ts - Core implementation
+export class RemoteServerMaster implements MasterDriver {
+  readonly type = "master" as const;
+  private websocket?: WebSocket;
+  private reconnectAttempts = 0;
+
+  async connect(): Promise<void> {
+    const wsUrl = `${this.config.url}/ws/master/${this.robotId}`;
+    this.websocket = new WebSocket(wsUrl);
+
+    this.websocket.onopen = () => {
+      console.log(`Remote master connected: ${wsUrl}`);
+      this.reconnectAttempts = 0;
+      this.updateStatus({ isConnected: true });
+    };
+
+    this.websocket.onmessage = (event) => {
+      try {
+        const message = JSON.parse(event.data);
+        this.handleServerMessage(message);
+      } catch (error) {
+        console.error('Failed to parse server message:', error);
+      }
+    };
+
+    this.websocket.onclose = () => {
+      this.updateStatus({ isConnected: false });
+      this.attemptReconnect();
+    };
+  }
+
+  private handleServerMessage(message: any): void {
+    switch (message.type) {
+      case 'command':
+        // Convert server message to robot command
+        const command: RobotCommand = {
+          timestamp: Date.now(),
+          joints: message.data.joints.map((j: any) => ({
+            name: j.name,
+            value: j.value,
+            speed: j.speed
+          }))
+        };
+        this.notifyCommand([command]);
+        break;
+
+      case 'sequence':
+        // Handle command sequence
+        const sequence: CommandSequence = message.data;
+        this.notifySequence(sequence);
+        break;
+    }
+  }
+
+  async sendSlaveStatus(slaveStates: DriverJointState[]): Promise<void> {
+    if (!this.websocket) return;
+
+    const statusMessage = {
+      type: 'slave_status',
+      timestamp: new Date().toISOString(),
+      robot_id: this.robotId,
+      data: {
+        joints: slaveStates.map(state => ({
+          name: state.name,
+          virtual_value: state.virtualValue,
+          real_value: state.realValue
+        }))
+      }
+    };
+
+    this.websocket.send(JSON.stringify(statusMessage));
+  }
+}
 ```
 
-**Features:**
-- ✅ Direct hardware control
-- ✅ Position/speed commands
-- ✅ Real servo feedback  
-- ✅ Calibration support
+**Protocol:**
+```json
+// Command from server to robot
+{
+  "type": "command",
+  "timestamp": "2024-01-15T10:30:00Z",
+  "data": {
+    "joints": [
+      { "name": "Rotation", "value": 45, "speed": 100 },
+      { "name": "Elbow", "value": -30, "speed": 80 }
+    ]
+  }
+}
+
+// Status from robot to server
+{
+  "type": "slave_status",
+  "timestamp": "2024-01-15T10:30:01Z",
+  "robot_id": "robot-1",
+  "data": {
+    "joints": [
+      { "name": "Rotation", "virtual_value": 45, "real_value": 44.8 },
+      { "name": "Elbow", "virtual_value": -30, "real_value": -29.9 }
+    ]
+  }
+}
+```
+
+### Remote Server Slave
 
-### Simulation Slave *(Planned)*
-**Physics-based simulation**
+**Network robot execution via WebSocket**
 
 ```typescript
-const config: SlaveDriverConfig = {
-  type: "simulation-slave",
-  physics: true,
-  collisionDetection: true
-};
+// RemoteServerSlave.ts - Core implementation
+export class RemoteServerSlave implements SlaveDriver {
+  readonly type = "slave" as const;
+  private websocket?: WebSocket;
+
+  async executeCommand(command: RobotCommand): Promise<void> {
+    if (!this.websocket) throw new Error('Not connected');
+
+    const message = {
+      type: 'command',
+      timestamp: new Date().toISOString(),
+      robot_id: this.config.robotId,
+      data: {
+        joints: command.joints.map(j => ({
+          name: j.name,
+          value: j.value,
+          speed: j.speed
+        }))
+      }
+    };
+
+    this.websocket.send(JSON.stringify(message));
+    
+    // Wait for acknowledgment
+    return new Promise((resolve, reject) => {
+      const timeout = setTimeout(() => reject(new Error('Command timeout')), 5000);
+      
+      const messageHandler = (event: MessageEvent) => {
+        const response = JSON.parse(event.data);
+        if (response.type === 'command_ack') {
+          clearTimeout(timeout);
+          this.websocket?.removeEventListener('message', messageHandler);
+          resolve();
+        }
+      };
+      
+      this.websocket.addEventListener('message', messageHandler);
+    });
+  }
+
+  async readJointStates(): Promise<DriverJointState[]> {
+    if (!this.websocket) throw new Error('Not connected');
+
+    const message = {
+      type: 'status_request',
+      timestamp: new Date().toISOString(),
+      robot_id: this.config.robotId
+    };
+
+    this.websocket.send(JSON.stringify(message));
+
+    return new Promise((resolve, reject) => {
+      const timeout = setTimeout(() => reject(new Error('Status timeout')), 3000);
+      
+      const messageHandler = (event: MessageEvent) => {
+        const response = JSON.parse(event.data);
+        if (response.type === 'joint_states') {
+          clearTimeout(timeout);
+          this.websocket?.removeEventListener('message', messageHandler);
+          
+          const states = response.data.joints.map((j: any) => ({
+            name: j.name,
+            servoId: j.servo_id,
+            type: j.type,
+            virtualValue: j.virtual_value,
+            realValue: j.real_value
+          }));
+          
+          resolve(states);
+        }
+      };
+      
+      this.websocket.addEventListener('message', messageHandler);
+    });
+  }
+}
 ```
 
 ## 🔄 Command Flow Architecture
 
 ### Command Structure
+
 ```typescript
 interface RobotCommand {
   timestamp: number;
   joints: {
     name: string;
-    value: number;      // degrees for revolute, speed for continuous
-    speed?: number;     // optional movement speed
+    value: number;    // degrees for revolute, speed for continuous
+    speed?: number;   // optional movement speed
   }[];
-  duration?: number;    // optional execution time
+  duration?: number;  // optional execution time
   metadata?: Record<string, unknown>;
 }
 ```
 
-### Command Sources
-
-#### Master Commands
-```typescript
-// Continuous sequence from master
-master.onCommand((commands) => {
-  // Route to all connected slaves
-  robot.slaves.forEach(slave => slave.executeCommands(commands));
-});
-```
-
-#### Manual Commands  
-```typescript
-// Only when no master active
-if (robot.manualControlEnabled) {
-  await robot.updateJointValue('shoulder', 45);
-}
-```
-
-### Command Execution
-```typescript
-// All slaves execute in parallel
-const executePromises = robot.connectedSlaves.map(slave => 
-  slave.executeCommand(command)
-);
-await Promise.allSettled(executePromises);
-```
-
-## 🎮 Usage Examples
-
-### Basic Master-Slave Setup
-```typescript
-// 1. Create robot
-const robot = await robotManager.createRobot('arm-1', urdfConfig);
-
-// 2. Add slaves for execution
-await robotManager.connectMockSlave('arm-1');      // Development
-await robotManager.connectUSBSlave('arm-1');       // Real hardware
-
-// 3. Connect master for control
-await robotManager.connectDemoSequences('arm-1');  // Automated sequences
-
-// 4. Monitor status
-const status = robotManager.getRobotStatus('arm-1');
-console.log(`Master: ${status.masterName}, Slaves: ${status.connectedSlaves}`);
-```
-
-### Multiple Robots with Different Masters
-```typescript
-// Robot 1: Demo sequences
-await robotManager.createRobot('demo-1', armConfig);
-await robotManager.connectDemoSequences('demo-1', true);
-await robotManager.connectMockSlave('demo-1');
-
-// Robot 2: Remote control
-await robotManager.createRobot('remote-1', armConfig);
-await robotManager.connectMaster('remote-1', remoteServerConfig);
-await robotManager.connectUSBSlave('remote-1');
-
-// Robot 3: Manual control only
-await robotManager.createRobot('manual-1', armConfig);
-await robotManager.connectMockSlave('manual-1');
-// No master = manual control enabled
-```
+### Control Flow
 
-### Master Switching
-```typescript
-// Switch from manual to automated control
-await robotManager.connectDemoSequences('robot-1');
-// Panel inputs now disabled, sequences control robot
+1. **Master Generation**: Masters generate commands from various sources
+2. **Robot Routing**: Robot class routes commands to all connected slaves
+3. **Parallel Execution**: All slaves execute commands simultaneously
+4. **State Feedback**: Slaves report back real joint positions
+5. **Synchronization**: Robot maintains synchronized state across all slaves
 
-// Switch back to manual control  
-await robotManager.disconnectMaster('robot-1');
-// Panel inputs re-enabled, manual control restored
-```
+### State Management
 
-## 🚀 Benefits
-
-### 1. **Clear Control Hierarchy**
-- Masters provide commands exclusively
-- Slaves execute commands in parallel
-- No ambiguity about command source
-
-### 2. **Flexible Command Sources**
-- Remote servers for network control
-- Predefined sequences for automation
-- Manual control for testing/setup
-- Easy to add new master types
-
-### 3. **Multiple Execution Targets**
-- Physical robots via USB
-- Simulated robots for testing
-- Mock robots for development
-- All execute same commands simultaneously
-
-### 4. **Automatic Panel Management**
-- Panel disabled when master active
-- Panel re-enabled when master disconnected
-- Clear visual feedback about control state
-
-### 5. **Safe Operation**
-- Masters cannot conflict (only 1 allowed)
-- Graceful disconnection with rest positioning
-- Error isolation between slaves
-
-### 6. **Development Workflow**
-- Test with mock slaves during development
-- Add real slaves for deployment
-- Switch masters for different scenarios
-
-## 🔮 Implementation Roadmap
-
-### Phase 1: Core Architecture ✅
-- [x] Master-Slave interfaces
-- [x] Robot class with dual management
-- [x] RobotManager orchestration
-- [x] MockSequenceMaster with demo patterns
-- [x] MockSlave for testing
-
-### Phase 2: Real Hardware
-- [ ] USBSlave implementation (feetech.js integration)
-- [ ] Calibration system for hardware slaves
-- [ ] Error handling and recovery
-
-### Phase 3: Remote Control
-- [ ] RemoteServerMaster (HTTP/WebSocket)
-- [ ] Authentication and security
-- [ ] Real-time command streaming
-
-### Phase 4: Advanced Features
-- [ ] ScriptPlayerMaster (JS/Python execution)
-- [ ] SimulationSlave (physics integration)
-- [ ] Command recording and playback
-- [ ] Multi-robot coordination
-
-## 🛡️ Safety Features
-
-### Master Safety
-- Only 1 master per robot prevents conflicts
-- Master disconnect automatically restores manual control
-- Command validation before execution
-
-### Slave Safety  
-- Rest positioning before disconnect
-- Smooth movement transitions
-- Individual slave error isolation
-- Calibration offset compensation
-
-### System Safety
-- Graceful degradation on slave failures
-- Command queuing prevents overwhelming
-- Real-time state monitoring
-- Emergency stop capabilities *(planned)*
-
-## 🔧 Migration from v1.0
-
-The new architecture is **completely different** from the old driver pattern:
-
-### Old Architecture (v1.0)
 ```typescript
-// Single driver per robot
-const driver = new USBRobotDriver(config);
-await robot.setDriver(driver);
-await robot.updateJointValue('joint', 45);
-```
-
-### New Architecture (v2.0)
-```typescript
-// Separate masters and slaves
-await robotManager.connectMaster(robotId, masterConfig);   // Command source
-await robotManager.connectSlave(robotId, slaveConfig);     // Execution target
-
-// Manual control only when no master
-if (robot.manualControlEnabled) {
-  await robot.updateJointValue('joint', 45);
+// Robot.svelte.ts - Core state management
+export interface ManagedJointState {
+  name: string;
+  urdfJoint: IUrdfJoint;
+  servoId?: number;
+
+  // State values
+  virtualValue: number;      // What the UI shows
+  realValue?: number;        // What hardware reports
+  commandedValue: number;    // Last commanded value
+
+  // Calibration
+  calibrationOffset: number; // Hardware compensation
+  restPosition: number;      // Safe default position
+
+  // Synchronization
+  lastVirtualUpdate: Date;
+  lastRealUpdate?: Date;
+  lastCommandUpdate?: Date;
 }
 ```
 
-### Key Changes
-1. **Drivers → Masters + Slaves**: Split command/execution concerns
-2. **Single Connection → Multiple**: 1 master + N slaves per robot  
-3. **Always Manual → Conditional**: Panel disabled when master active
-4. **Direct Control → Command Routing**: Masters route to all slaves
+## 📊 Benefits Summary
 
-The new architecture provides **dramatically more flexibility** while maintaining complete backward compatibility for URDF visualization and joint control.
+| Benefit | Description | Impact |
+|---------|-------------|---------|
+| **🔒 Clear Control Hierarchy** | Masters provide commands exclusively, slaves execute in parallel | No command conflicts, predictable behavior |
+| **🔄 Flexible Command Sources** | Easy switching between manual, automated, and remote control | Supports development, testing, and production |
+| **📡 Multiple Execution Targets** | Same commands executed on multiple robots simultaneously | Real hardware + simulation testing |
+| **🎛️ Automatic Panel Management** | UI automatically adapts to master presence | Intuitive user experience |
+| **🚀 Development Workflow** | Clear separation enables independent development | Faster iteration cycles |
 
 ---
 
-*This architecture enables sophisticated robot control scenarios from simple manual operation to complex multi-robot coordination, all with a clean, extensible design.* 
+**This architecture provides unprecedented flexibility for robot control, from simple manual operation to sophisticated multi-robot coordination, all with a clean, extensible, and production-ready design.**

ROBOT_INSTANCING_README.md

@@ -1,73 +0,0 @@
-# Robot Instancing Optimization
-
-This implementation optimizes the rendering of multiple robots using Threlte's instancing capabilities for improved performance.
-
-## Key Features
-
-### 1. **Smart Instancing by Robot Type**
-- Robots are grouped by their URDF type (URL) for optimal instancing
-- Single robots render with full detail
-- Multiple robots of the same type use instanced rendering
-
-### 2. **Geometry-Specific Instancing**
-- **Box Geometries**: Instanced with `T.boxGeometry`
-- **Cylinder Geometries**: Instanced with `T.cylinderGeometry`  
-- **Mesh Geometries**: Instanced with simplified `T.sphereGeometry` for performance
-
-### 3. **Hybrid Rendering Strategy**
-- First robot of each type: Full detailed rendering with all URDF components
-- Additional robots: Simplified instanced representation
-- Maintains visual quality while optimizing performance
-
-### 4. **Performance Benefits**
-- Reduces draw calls when rendering multiple robots
-- Optimizes GPU memory usage through instancing
-- Scales better with increasing robot count
-- Maintains interactivity for detailed robots
-
-## Implementation Details
-
-### State Management
-```typescript
-// Robots grouped by URDF type for optimal batching
-let robotsByType: Record<string, Array<{ 
-  id: string; 
-  position: [number, number, number]; 
-  robotState: RobotState 
-}>> = $state({});
-```
-
-### Instancing Logic
-1. **Single Robot**: Full `UrdfLink` rendering with all details
-2. **Multiple Robots**: 
-   - Geometry analysis and grouping
-   - Instanced rendering for primitive shapes
-   - Simplified representations for complex meshes
-
-### Automatic Demonstration
-- Spawns additional robots after 2 seconds to showcase instancing
-- Shows performance difference between single and multiple robot rendering
-
-## Usage
-
-Simply use the `Robot.svelte` component with a `urdfConfig`. The component automatically:
-- Detects when multiple robots of the same type exist
-- Switches to instanced rendering for optimal performance
-- Maintains full detail for single robots
-
-```svelte
-<Robot {urdfConfig} />
-```
-
-## Performance Impact
-
-- **Before**: O(n) draw calls for n robots
-- **After**: O(1) draw calls per geometry type regardless of robot count
-- **Memory**: Shared geometry instances reduce GPU memory usage
-- **Scalability**: Linear performance improvement with robot count
-
-This optimization is particularly beneficial for:
-- Robot swarms
-- Multi-robot simulations  
-- Arena scenarios with many similar robots
-- Performance-critical real-time applications 

bun.lock

@@ -6,8 +6,9 @@
       "dependencies": {
         "@threlte/core": "^8.0.4",
         "@threlte/extras": "^9.2.1",
-        "@types/three": "^0.176.0",
+        "@types/three": "0.177.0",
         "clsx": "^2.1.1",
+        "feetech.js": "file:packages/feetech.js",
         "tailwind-merge": "^3.3.0",
         "three": "^0.177.0",
         "threlte-postprocessing": "^0.0.9",
@@ -148,7 +149,7 @@
 
     "@humanwhocodes/retry": ["@humanwhocodes/[email protected]", "", {}, "sha512-bV0Tgo9K4hfPCek+aMAn81RppFKv2ySDQeMoSZuvTASywNTnVJCArCZE2FWqpvIatKu7VMRLWlR1EazvVhDyhQ=="],
 
-    "@iconify/json": ["@iconify/[email protected].344", "", { "dependencies": { "@iconify/types": "*", "pathe": "^1.1.2" } }, "sha512-wY+TYoq4WSAaNunAbecUS+5S4VobTW/3cWpTLEWezRW9Wv4V3MqguNirNN3c/jSq/dfIMJNvr9Gz+N9iNFMInA=="],
+    "@iconify/json": ["@iconify/[email protected].345", "", { "dependencies": { "@iconify/types": "*", "pathe": "^1.1.2" } }, "sha512-cWcTkpSw42OcltXXlLRMp4bnoFEMvEXEIZDPazqqpT7nr4dPN/ztEqOk6T3z0fXrN2E3OEgW0GnHlQqZz4qDgw=="],
 
     "@iconify/tailwind4": ["@iconify/[email protected]", "", { "dependencies": { "@iconify/types": "^2.0.0", "@iconify/utils": "^2.2.1" }, "peerDependencies": { "tailwindcss": ">= 4" } }, "sha512-43ZXe+bC7CuE2LCgROdqbQeFYJi/J7L/k1UpSy8KDQlWVsWxPzLSWbWhlJx4uRYLOh1NRyw02YlDOgzBOFNd+A=="],
 
@@ -238,7 +239,7 @@
 
     "@sveltejs/kit": ["@sveltejs/[email protected]", "", { "dependencies": { "@sveltejs/acorn-typescript": "^1.0.5", "@types/cookie": "^0.6.0", "acorn": "^8.14.1", "cookie": "^0.6.0", "devalue": "^5.1.0", "esm-env": "^1.2.2", "kleur": "^4.1.5", "magic-string": "^0.30.5", "mrmime": "^2.0.0", "sade": "^1.8.1", "set-cookie-parser": "^2.6.0", "sirv": "^3.0.0" }, "peerDependencies": { "@sveltejs/vite-plugin-svelte": "^3.0.0 || ^4.0.0-next.1 || ^5.0.0", "svelte": "^4.0.0 || ^5.0.0-next.0", "vite": "^5.0.3 || ^6.0.0" }, "bin": { "svelte-kit": "svelte-kit.js" } }, "sha512-vLbtVwtDcK8LhJKnFkFYwM0uCdFmzioQnif0bjEYH1I24Arz22JPr/hLUiXGVYAwhu8INKx5qrdvr4tHgPwX6w=="],
 
-    "@sveltejs/vite-plugin-svelte": ["@sveltejs/[email protected].3", "", { "dependencies": { "@sveltejs/vite-plugin-svelte-inspector": "^4.0.1", "debug": "^4.4.0", "deepmerge": "^4.3.1", "kleur": "^4.1.5", "magic-string": "^0.30.15", "vitefu": "^1.0.4" }, "peerDependencies": { "svelte": "^5.0.0", "vite": "^6.0.0" } }, "sha512-MCFS6CrQDu1yGwspm4qtli0e63vaPCehf6V7pIMP15AsWgMKrqDGCPFF/0kn4SP0ii4aySu4Pa62+fIRGFMjgw=="],
+    "@sveltejs/vite-plugin-svelte": ["@sveltejs/vite-plugin-svelte@5.1.0", "", { "dependencies": { "@sveltejs/vite-plugin-svelte-inspector": "^4.0.1", "debug": "^4.4.1", "deepmerge": "^4.3.1", "kleur": "^4.1.5", "magic-string": "^0.30.17", "vitefu": "^1.0.6" }, "peerDependencies": { "svelte": "^5.0.0", "vite": "^6.0.0" } }, "sha512-wojIS/7GYnJDYIg1higWj2ROA6sSRWvcR1PO/bqEyFr/5UZah26c8Cz4u0NaqjPeVltzsVpt2Tm8d2io0V+4Tw=="],
 
     "@sveltejs/vite-plugin-svelte-inspector": ["@sveltejs/[email protected]", "", { "dependencies": { "debug": "^4.3.7" }, "peerDependencies": { "@sveltejs/vite-plugin-svelte": "^5.0.0", "svelte": "^5.0.0", "vite": "^6.0.0" } }, "sha512-J/Nmb2Q2y7mck2hyCX4ckVHcR5tu2J+MtBEQqpDrrgELZ2uvraQcK/ioCV61AqkdXFgriksOKIceDcQmqnGhVw=="],
 
@@ -296,29 +297,29 @@
 
     "@types/stats.js": ["@types/[email protected]", "", {}, "sha512-jIBvWWShCvlBqBNIZt0KAshWpvSjhkwkEu4ZUcASoAvhmrgAUI2t1dXrjSL4xXVLB4FznPrIsX3nKXFl/Dt4vA=="],
 
-    "@types/three": ["@types/three@0.176.0", "", { "dependencies": { "@dimforge/rapier3d-compat": "^0.12.0", "@tweenjs/tween.js": "~23.1.3", "@types/stats.js": "*", "@types/webxr": "*", "@webgpu/types": "*", "fflate": "~0.8.2", "meshoptimizer": "~0.18.1" } }, "sha512-FwfPXxCqOtP7EdYMagCFePNKoG1AGBDUEVKtluv2BTVRpSt7b+X27xNsirPCTCqY1pGYsPUzaM3jgWP7dXSxlw=="],
+    "@types/three": ["@types/three@0.177.0", "", { "dependencies": { "@dimforge/rapier3d-compat": "~0.12.0", "@tweenjs/tween.js": "~23.1.3", "@types/stats.js": "*", "@types/webxr": "*", "@webgpu/types": "*", "fflate": "~0.8.2", "meshoptimizer": "~0.18.1" } }, "sha512-/ZAkn4OLUijKQySNci47lFO+4JLE1TihEjsGWPUT+4jWqxtwOPPEwJV1C3k5MEx0mcBPCdkFjzRzDOnHEI1R+A=="],
 
     "@types/webxr": ["@types/[email protected]", "", {}, "sha512-Vr6Stjv5jPRqH690f5I5GLjVk8GSsoQSYJ2FVd/3jJF7KaqfwPi3ehfBS96mlQ2kPCwZaX6U0rG2+NGHBKkA/A=="],
 
-    "@typescript-eslint/eslint-plugin": ["@typescript-eslint/[email protected].0", "", { "dependencies": { "@eslint-community/regexpp": "^4.10.0", "@typescript-eslint/scope-manager": "8.33.0", "@typescript-eslint/type-utils": "8.33.0", "@typescript-eslint/utils": "8.33.0", "@typescript-eslint/visitor-keys": "8.33.0", "graphemer": "^1.4.0", "ignore": "^7.0.0", "natural-compare": "^1.4.0", "ts-api-utils": "^2.1.0" }, "peerDependencies": { "@typescript-eslint/parser": "^8.33.0", "eslint": "^8.57.0 || ^9.0.0", "typescript": ">=4.8.4 <5.9.0" } }, "sha512-CACyQuqSHt7ma3Ns601xykeBK/rDeZa3w6IS6UtMQbixO5DWy+8TilKkviGDH6jtWCo8FGRKEK5cLLkPvEammQ=="],
+    "@typescript-eslint/eslint-plugin": ["@typescript-eslint/[email protected].1", "", { "dependencies": { "@eslint-community/regexpp": "^4.10.0", "@typescript-eslint/scope-manager": "8.33.1", "@typescript-eslint/type-utils": "8.33.1", "@typescript-eslint/utils": "8.33.1", "@typescript-eslint/visitor-keys": "8.33.1", "graphemer": "^1.4.0", "ignore": "^7.0.0", "natural-compare": "^1.4.0", "ts-api-utils": "^2.1.0" }, "peerDependencies": { "@typescript-eslint/parser": "^8.33.1", "eslint": "^8.57.0 || ^9.0.0", "typescript": ">=4.8.4 <5.9.0" } }, "sha512-TDCXj+YxLgtvxvFlAvpoRv9MAncDLBV2oT9Bd7YBGC/b/sEURoOYuIwLI99rjWOfY3QtDzO+mk0n4AmdFExW8A=="],
 
-    "@typescript-eslint/parser": ["@typescript-eslint/[email protected].0", "", { "dependencies": { "@typescript-eslint/scope-manager": "8.33.0", "@typescript-eslint/types": "8.33.0", "@typescript-eslint/typescript-estree": "8.33.0", "@typescript-eslint/visitor-keys": "8.33.0", "debug": "^4.3.4" }, "peerDependencies": { "eslint": "^8.57.0 || ^9.0.0", "typescript": ">=4.8.4 <5.9.0" } }, "sha512-JaehZvf6m0yqYp34+RVnihBAChkqeH+tqqhS0GuX1qgPpwLvmTPheKEs6OeCK6hVJgXZHJ2vbjnC9j119auStQ=="],
+    "@typescript-eslint/parser": ["@typescript-eslint/[email protected].1", "", { "dependencies": { "@typescript-eslint/scope-manager": "8.33.1", "@typescript-eslint/types": "8.33.1", "@typescript-eslint/typescript-estree": "8.33.1", "@typescript-eslint/visitor-keys": "8.33.1", "debug": "^4.3.4" }, "peerDependencies": { "eslint": "^8.57.0 || ^9.0.0", "typescript": ">=4.8.4 <5.9.0" } }, "sha512-qwxv6dq682yVvgKKp2qWwLgRbscDAYktPptK4JPojCwwi3R9cwrvIxS4lvBpzmcqzR4bdn54Z0IG1uHFskW4dA=="],
 
-    "@typescript-eslint/project-service": ["@typescript-eslint/[email protected].0", "", { "dependencies": { "@typescript-eslint/tsconfig-utils": "^8.33.0", "@typescript-eslint/types": "^8.33.0", "debug": "^4.3.4" } }, "sha512-d1hz0u9l6N+u/gcrk6s6gYdl7/+pp8yHheRTqP6X5hVDKALEaTn8WfGiit7G511yueBEL3OpOEpD+3/MBdoN+A=="],
+    "@typescript-eslint/project-service": ["@typescript-eslint/[email protected].1", "", { "dependencies": { "@typescript-eslint/tsconfig-utils": "^8.33.1", "@typescript-eslint/types": "^8.33.1", "debug": "^4.3.4" }, "peerDependencies": { "typescript": ">=4.8.4 <5.9.0" } }, "sha512-DZR0efeNklDIHHGRpMpR5gJITQpu6tLr9lDJnKdONTC7vvzOlLAG/wcfxcdxEWrbiZApcoBCzXqU/Z458Za5Iw=="],
 
-    "@typescript-eslint/scope-manager": ["@typescript-eslint/[email protected].0", "", { "dependencies": { "@typescript-eslint/types": "8.33.0", "@typescript-eslint/visitor-keys": "8.33.0" } }, "sha512-LMi/oqrzpqxyO72ltP+dBSP6V0xiUb4saY7WLtxSfiNEBI8m321LLVFU9/QDJxjDQG9/tjSqKz/E3380TEqSTw=="],
+    "@typescript-eslint/scope-manager": ["@typescript-eslint/[email protected].1", "", { "dependencies": { "@typescript-eslint/types": "8.33.1", "@typescript-eslint/visitor-keys": "8.33.1" } }, "sha512-dM4UBtgmzHR9bS0Rv09JST0RcHYearoEoo3pG5B6GoTR9XcyeqX87FEhPo+5kTvVfKCvfHaHrcgeJQc6mrDKrA=="],
 
-    "@typescript-eslint/tsconfig-utils": ["@typescript-eslint/[email protected].0", "", { "peerDependencies": { "typescript": ">=4.8.4 <5.9.0" } }, "sha512-sTkETlbqhEoiFmGr1gsdq5HyVbSOF0145SYDJ/EQmXHtKViCaGvnyLqWFFHtEXoS0J1yU8Wyou2UGmgW88fEug=="],
+    "@typescript-eslint/tsconfig-utils": ["@typescript-eslint/[email protected].1", "", { "peerDependencies": { "typescript": ">=4.8.4 <5.9.0" } }, "sha512-STAQsGYbHCF0/e+ShUQ4EatXQ7ceh3fBCXkNU7/MZVKulrlq1usH7t2FhxvCpuCi5O5oi1vmVaAjrGeL71OK1g=="],
 
-    "@typescript-eslint/type-utils": ["@typescript-eslint/[email protected].0", "", { "dependencies": { "@typescript-eslint/typescript-estree": "8.33.0", "@typescript-eslint/utils": "8.33.0", "debug": "^4.3.4", "ts-api-utils": "^2.1.0" }, "peerDependencies": { "eslint": "^8.57.0 || ^9.0.0", "typescript": ">=4.8.4 <5.9.0" } }, "sha512-lScnHNCBqL1QayuSrWeqAL5GmqNdVUQAAMTaCwdYEdWfIrSrOGzyLGRCHXcCixa5NK6i5l0AfSO2oBSjCjf4XQ=="],
+    "@typescript-eslint/type-utils": ["@typescript-eslint/[email protected].1", "", { "dependencies": { "@typescript-eslint/typescript-estree": "8.33.1", "@typescript-eslint/utils": "8.33.1", "debug": "^4.3.4", "ts-api-utils": "^2.1.0" }, "peerDependencies": { "eslint": "^8.57.0 || ^9.0.0", "typescript": ">=4.8.4 <5.9.0" } }, "sha512-1cG37d9xOkhlykom55WVwG2QRNC7YXlxMaMzqw2uPeJixBFfKWZgaP/hjAObqMN/u3fr5BrTwTnc31/L9jQ2ww=="],
 
-    "@typescript-eslint/types": ["@typescript-eslint/[email protected].0", "", {}, "sha512-DKuXOKpM5IDT1FA2g9x9x1Ug81YuKrzf4mYX8FAVSNu5Wo/LELHWQyM1pQaDkI42bX15PWl0vNPt1uGiIFUOpg=="],
+    "@typescript-eslint/types": ["@typescript-eslint/[email protected].1", "", {}, "sha512-xid1WfizGhy/TKMTwhtVOgalHwPtV8T32MS9MaH50Cwvz6x6YqRIPdD2WvW0XaqOzTV9p5xdLY0h/ZusU5Lokg=="],
 
-    "@typescript-eslint/typescript-estree": ["@typescript-eslint/[email protected].0", "", { "dependencies": { "@typescript-eslint/project-service": "8.33.0", "@typescript-eslint/tsconfig-utils": "8.33.0", "@typescript-eslint/types": "8.33.0", "@typescript-eslint/visitor-keys": "8.33.0", "debug": "^4.3.4", "fast-glob": "^3.3.2", "is-glob": "^4.0.3", "minimatch": "^9.0.4", "semver": "^7.6.0", "ts-api-utils": "^2.1.0" }, "peerDependencies": { "typescript": ">=4.8.4 <5.9.0" } }, "sha512-vegY4FQoB6jL97Tu/lWRsAiUUp8qJTqzAmENH2k59SJhw0Th1oszb9Idq/FyyONLuNqT1OADJPXfyUNOR8SzAQ=="],
+    "@typescript-eslint/typescript-estree": ["@typescript-eslint/[email protected].1", "", { "dependencies": { "@typescript-eslint/project-service": "8.33.1", "@typescript-eslint/tsconfig-utils": "8.33.1", "@typescript-eslint/types": "8.33.1", "@typescript-eslint/visitor-keys": "8.33.1", "debug": "^4.3.4", "fast-glob": "^3.3.2", "is-glob": "^4.0.3", "minimatch": "^9.0.4", "semver": "^7.6.0", "ts-api-utils": "^2.1.0" }, "peerDependencies": { "typescript": ">=4.8.4 <5.9.0" } }, "sha512-+s9LYcT8LWjdYWu7IWs7FvUxpQ/DGkdjZeE/GGulHvv8rvYwQvVaUZ6DE+j5x/prADUgSbbCWZ2nPI3usuVeOA=="],
 
-    "@typescript-eslint/utils": ["@typescript-eslint/[email protected].0", "", { "dependencies": { "@eslint-community/eslint-utils": "^4.7.0", "@typescript-eslint/scope-manager": "8.33.0", "@typescript-eslint/types": "8.33.0", "@typescript-eslint/typescript-estree": "8.33.0" }, "peerDependencies": { "eslint": "^8.57.0 || ^9.0.0", "typescript": ">=4.8.4 <5.9.0" } }, "sha512-lPFuQaLA9aSNa7D5u2EpRiqdAUhzShwGg/nhpBlc4GR6kcTABttCuyjFs8BcEZ8VWrjCBof/bePhP3Q3fS+Yrw=="],
+    "@typescript-eslint/utils": ["@typescript-eslint/[email protected].1", "", { "dependencies": { "@eslint-community/eslint-utils": "^4.7.0", "@typescript-eslint/scope-manager": "8.33.1", "@typescript-eslint/types": "8.33.1", "@typescript-eslint/typescript-estree": "8.33.1" }, "peerDependencies": { "eslint": "^8.57.0 || ^9.0.0", "typescript": ">=4.8.4 <5.9.0" } }, "sha512-52HaBiEQUaRYqAXpfzWSR2U3gxk92Kw006+xZpElaPMg3C4PgM+A5LqwoQI1f9E5aZ/qlxAZxzm42WX+vn92SQ=="],
 
-    "@typescript-eslint/visitor-keys": ["@typescript-eslint/[email protected].0", "", { "dependencies": { "@typescript-eslint/types": "8.33.0", "eslint-visitor-keys": "^4.2.0" } }, "sha512-7RW7CMYoskiz5OOGAWjJFxgb7c5UNjTG292gYhWeOAcFmYCtVCSqjqSBj5zMhxbXo2JOW95YYrUWJfU0zrpaGQ=="],
+    "@typescript-eslint/visitor-keys": ["@typescript-eslint/[email protected].1", "", { "dependencies": { "@typescript-eslint/types": "8.33.1", "eslint-visitor-keys": "^4.2.0" } }, "sha512-3i8NrFcZeeDHJ+7ZUuDkGT+UHq+XoFGsymNK2jZCOHcfEzRQ0BdpRtdpSx/Iyf3MHLWIcLS0COuOPibKQboIiQ=="],
 
     "@webgpu/types": ["@webgpu/[email protected]", "", {}, "sha512-w2HbBvH+qO19SB5pJOJFKs533CdZqxl3fcGonqL321VHkW7W/iBo6H8bjDy6pr/+pbMwIu5dnuaAxH7NxBqUrQ=="],
 
@@ -462,7 +463,7 @@
 
     "eslint-config-prettier": ["[email protected]", "", { "peerDependencies": { "eslint": ">=7.0.0" }, "bin": { "eslint-config-prettier": "bin/cli.js" } }, "sha512-zc1UmCpNltmVY34vuLRV61r1K27sWuX39E+uyUnY8xS2Bex88VV9cugG+UZbRSRGtGyFboj+D8JODyme1plMpw=="],
 
-    "eslint-plugin-svelte": ["[email protected].0", "", { "dependencies": { "@eslint-community/eslint-utils": "^4.6.1", "@jridgewell/sourcemap-codec": "^1.5.0", "esutils": "^2.0.3", "globals": "^16.0.0", "known-css-properties": "^0.36.0", "postcss": "^8.4.49", "postcss-load-config": "^3.1.4", "postcss-safe-parser": "^7.0.0", "semver": "^7.6.3", "svelte-eslint-parser": "^1.2.0" }, "peerDependencies": { "eslint": "^8.57.1 || ^9.0.0", "svelte": "^3.37.0 || ^4.0.0 || ^5.0.0" }, "optionalPeers": ["svelte"] }, "sha512-nvIUNyyPGbr5922Kd1p/jXe+FfNdVPXsxLyrrXpwfSbZZEFdAYva9O/gm2lObC/wXkQo/AUmQkAihfmNJYeCjA=="],
+    "eslint-plugin-svelte": ["[email protected].1", "", { "dependencies": { "@eslint-community/eslint-utils": "^4.6.1", "@jridgewell/sourcemap-codec": "^1.5.0", "esutils": "^2.0.3", "globals": "^16.0.0", "known-css-properties": "^0.36.0", "postcss": "^8.4.49", "postcss-load-config": "^3.1.4", "postcss-safe-parser": "^7.0.0", "semver": "^7.6.3", "svelte-eslint-parser": "^1.2.0" }, "peerDependencies": { "eslint": "^8.57.1 || ^9.0.0", "svelte": "^3.37.0 || ^4.0.0 || ^5.0.0" }, "optionalPeers": ["svelte"] }, "sha512-mXFulSdD/0/p+zwENjPNsiVwAqmSRp90sy5zvVQBX1yAXhJbdhIn6C/tn8BZYjU94Ia7Y87d1Xdbvi49DeWyHQ=="],
 
     "eslint-scope": ["[email protected]", "", { "dependencies": { "esrecurse": "^4.3.0", "estraverse": "^5.2.0" } }, "sha512-pUNxi75F8MJ/GdeKtVLSbYg4ZI34J6C0C7sbL4YOp2exGwen7ZsuBqKzUhXd0qMQ362yET3z+uPwKeg/0C2XCQ=="],
 
@@ -498,6 +499,8 @@
 
     "fdir": ["[email protected]", "", { "peerDependencies": { "picomatch": "^3 || ^4" }, "optionalPeers": ["picomatch"] }, "sha512-4BG7puHpVsIYxZUbiUE3RqGloLaSSwzYie5jvasC4LWuBWzZawynvYouhjbQKw2JuIGYdm0DzIxl8iVidKlUEw=="],
 
+    "feetech.js": ["feetech.js@file:packages/feetech.js", {}],
+
     "fflate": ["[email protected]", "", {}, "sha512-cPJU47OaAoCbg0pBvzsgpTPhmhqI5eJjh/JIu8tPj5q+T7iLvW/JAYUqmE7KOB4R1ZyEhzBaIQpQpardBF5z8A=="],
 
     "file-entry-cache": ["[email protected]", "", { "dependencies": { "flat-cache": "^4.0.0" } }, "sha512-XXTUwCvisa5oacNGRP9SfNtYBNAMi+RPwBFmblZEF7N7swHYQS6/Zfk7SRwx4D5j3CH211YNRco1DEMNVfZCnQ=="],
@@ -726,7 +729,7 @@
 
     "supports-color": ["[email protected]", "", { "dependencies": { "has-flag": "^4.0.0" } }, "sha512-qpCAvRl9stuOHveKsn7HncJRvv501qIacKzQlO/+Lwxc9+0q2wLyv4Dfvt80/DPn2pqOBsJdDiogXGR9+OvwRw=="],
 
-    "svelte": ["[email protected].10", "", { "dependencies": { "@ampproject/remapping": "^2.3.0", "@jridgewell/sourcemap-codec": "^1.5.0", "@sveltejs/acorn-typescript": "^1.0.5", "@types/estree": "^1.0.5", "acorn": "^8.12.1", "aria-query": "^5.3.1", "axobject-query": "^4.1.0", "clsx": "^2.1.1", "esm-env": "^1.2.1", "esrap": "^1.4.6", "is-reference": "^3.0.3", "locate-character": "^3.0.0", "magic-string": "^0.30.11", "zimmerframe": "^1.1.2" } }, "sha512-/yArPQIBoQS2p86LKnvJywOXkVHeEXnFgrDPSxkEfIAEkykopYuy2bF6UUqHG4IbZlJD6OurLxJT8Kn7kTk9WA=="],
+    "svelte": ["[email protected].14", "", { "dependencies": { "@ampproject/remapping": "^2.3.0", "@jridgewell/sourcemap-codec": "^1.5.0", "@sveltejs/acorn-typescript": "^1.0.5", "@types/estree": "^1.0.5", "acorn": "^8.12.1", "aria-query": "^5.3.1", "axobject-query": "^4.1.0", "clsx": "^2.1.1", "esm-env": "^1.2.1", "esrap": "^1.4.6", "is-reference": "^3.0.3", "locate-character": "^3.0.0", "magic-string": "^0.30.11", "zimmerframe": "^1.1.2" } }, "sha512-kRlbhIlMTijbFmVDQFDeKXPLlX1/ovXwV0I162wRqQhRcygaqDIcu1d/Ese3H2uI+yt3uT8E7ndgDthQv5v5BA=="],
 
     "svelte-check": ["[email protected]", "", { "dependencies": { "@jridgewell/trace-mapping": "^0.3.25", "chokidar": "^4.0.1", "fdir": "^6.2.0", "picocolors": "^1.0.0", "sade": "^1.7.4" }, "peerDependencies": { "svelte": "^4.0.0 || ^5.0.0-next.0", "typescript": ">=5.0.0" }, "bin": { "svelte-check": "bin/svelte-check" } }, "sha512-e49SU1RStvQhoipkQ/aonDhHnG3qxHSBtNfBRb9pxVXoa+N7qybAo32KgA9wEb2PCYFNaDg7bZCdhLD1vHpdYA=="],
 
@@ -788,7 +791,7 @@
 
     "typescript": ["[email protected]", "", { "bin": { "tsc": "bin/tsc", "tsserver": "bin/tsserver" } }, "sha512-p1diW6TqL9L07nNxvRMM7hMMw4c5XOo/1ibL4aAIGmSAt9slTE1Xgw5KWuof2uTOvCg9BY7ZRi+GaF+7sfgPeQ=="],
 
-    "typescript-eslint": ["[email protected].0", "", { "dependencies": { "@typescript-eslint/eslint-plugin": "8.33.0", "@typescript-eslint/parser": "8.33.0", "@typescript-eslint/utils": "8.33.0" }, "peerDependencies": { "eslint": "^8.57.0 || ^9.0.0", "typescript": ">=4.8.4 <5.9.0" } }, "sha512-5YmNhF24ylCsvdNW2oJwMzTbaeO4bg90KeGtMjUw0AGtHksgEPLRTUil+coHwCfiu4QjVJFnjp94DmU6zV7DhQ=="],
+    "typescript-eslint": ["[email protected].1", "", { "dependencies": { "@typescript-eslint/eslint-plugin": "8.33.1", "@typescript-eslint/parser": "8.33.1", "@typescript-eslint/utils": "8.33.1" }, "peerDependencies": { "eslint": "^8.57.0 || ^9.0.0", "typescript": ">=4.8.4 <5.9.0" } }, "sha512-AgRnV4sKkWOiZ0Kjbnf5ytTJXMUZQ0qhSVdQtDNYLPLnjsATEYhaO94GlRQwi4t4gO8FfjM6NnikHeKjUm8D7A=="],
 
     "ufo": ["[email protected]", "", {}, "sha512-9a4/uxlTWJ4+a5i0ooc1rU7C7YOw3wT+UGqdeNNHWnOF9qcMBgLRS+4IYUqbczewFx4mLEig6gawh7X6mFlEkA=="],
 
@@ -820,7 +823,7 @@
 
     "zimmerframe": ["[email protected]", "", {}, "sha512-rAbqEGa8ovJy4pyBxZM70hg4pE6gDgaQ0Sl9M3enG3I0d6H4XSAM3GeNGLKnsBpuijUow064sf7ww1nutC5/3w=="],
 
-    "zod": ["[email protected].42", "", {}, "sha512-PcALTLskaucbeHc41tU/xfjfhcz8z0GdhhDcSgrCTmSazUuqnYqiXO63M0QUBVwpBlsLsNVn5qHSC5Dw3KZvaQ=="],
+    "zod": ["[email protected].49", "", {}, "sha512-JMMPMy9ZBk3XFEdbM3iL1brx4NUSejd6xr3ELrrGEfGb355gjhiAWtG3K5o+AViV/3ZfkIrCzXsZn6SbLwTR8Q=="],
 
     "@eslint-community/eslint-utils/eslint-visitor-keys": ["[email protected]", "", {}, "sha512-wpc+LXeiyiisxPlEkUzU6svyS1frIO3Mgxj1fdy7Pm8Ygzguax2N3Fa/D/ag1WqbOprdI+uY6wMUl8/a2G+iag=="],
 

components.json

@@ -5,9 +5,9 @@
 		"baseColor": "stone"
 	},
 	"aliases": {
-		"components": "$lib/components",
+		"components": "@/components",
 		"utils": "$lib/utils",
-		"ui": "$lib/components/ui",
+		"ui": "@/components/ui",
 		"hooks": "$lib/hooks",
 		"lib": "$lib"
 	},

docker-compose.yml

@@ -1,14 +1,14 @@
-version: '3.8'
+version: "3.8"
 
 services:
   lerobot-arena:
     build: .
     ports:
-      - "7860:7860"  # FastAPI server serving both frontend and backend
+      - "7860:7860" # FastAPI server serving both frontend and backend
     environment:
       - NODE_ENV=production
     # volumes:
     #   # Optional: Mount local development files for debugging
     #   - ./src-python:/home/user/app/src-python
     #   - ./static-frontend:/home/user/app/static-frontend
-    restart: unless-stopped 
+    restart: unless-stopped

eslint.config.js

@@ -1,13 +1,13 @@
-import prettier from 'eslint-config-prettier';
-import js from '@eslint/js';
-import { includeIgnoreFile } from '@eslint/compat';
-import svelte from 'eslint-plugin-svelte';
-import globals from 'globals';
-import { fileURLToPath } from 'node:url';
-import ts from 'typescript-eslint';
-import svelteConfig from './svelte.config.js';
+import prettier from "eslint-config-prettier";
+import js from "@eslint/js";
+import { includeIgnoreFile } from "@eslint/compat";
+import svelte from "eslint-plugin-svelte";
+import globals from "globals";
+import { fileURLToPath } from "node:url";
+import ts from "typescript-eslint";
+import svelteConfig from "./svelte.config.js";
 
-const gitignorePath = fileURLToPath(new URL('./.gitignore', import.meta.url));
+const gitignorePath = fileURLToPath(new URL("./.gitignore", import.meta.url));
 
 export default ts.config(
 	includeIgnoreFile(gitignorePath),
@@ -20,14 +20,14 @@ export default ts.config(
 		languageOptions: {
 			globals: { ...globals.browser, ...globals.node }
 		},
-		rules: { 'no-undef': 'off' }
+		rules: { "no-undef": "off" }
 	},
 	{
-		files: ['**/*.svelte', '**/*.svelte.ts', '**/*.svelte.js'],
+		files: ["**/*.svelte", "**/*.svelte.ts", "**/*.svelte.js"],
 		languageOptions: {
 			parserOptions: {
 				projectService: true,
-				extraFileExtensions: ['.svelte'],
+				extraFileExtensions: [".svelte"],
 				parser: ts.parser,
 				svelteConfig
 			}

package.json

@@ -52,8 +52,9 @@
 	"dependencies": {
 		"@threlte/core": "^8.0.4",
 		"@threlte/extras": "^9.2.1",
-		"@types/three": "^0.176.0",
+		"@types/three": "0.177.0",
 		"clsx": "^2.1.1",
+		"feetech.js": "file:packages/feetech.js",
 		"tailwind-merge": "^3.3.0",
 		"three": "^0.177.0",
 		"threlte-postprocessing": "^0.0.9",

{src/lib → packages}/feetech.js/README.md

@@ -10,7 +10,7 @@ npm install feetech.js
 ```
 
 ```javascript
-import { scsServoSDK } from 'feetech.js';
+import { scsServoSDK } from "feetech.js";
 
 await scsServoSDK.connect();
 
@@ -22,5 +22,3 @@ console.log(position); // 1122
 
 - simple example: [test.html](./test.html)
 - the bambot website: [bambot.org](https://bambot.org)
-
-

packages/feetech.js/index.d.ts

@@ -0,0 +1,37 @@
+export type ConnectionOptions = {
+	baudRate?: number;
+	protocolEnd?: number;
+};
+
+export type ServoPositions = Map<number, number> | Record<number, number>;
+export type ServoSpeeds = Map<number, number> | Record<number, number>; // New type alias for speeds
+
+export interface ScsServoSDK {
+	connect(options?: ConnectionOptions): Promise<true>;
+	disconnect(): Promise<true>;
+	readPosition(servoId: number): Promise<number>;
+	readBaudRate(servoId: number): Promise<number>;
+	readMode(servoId: number): Promise<number>;
+	writePosition(servoId: number, position: number): Promise<"success">;
+	writeTorqueEnable(servoId: number, enable: boolean): Promise<"success">;
+	writeAcceleration(servoId: number, acceleration: number): Promise<"success">;
+	setWheelMode(servoId: number): Promise<"success">;
+	setPositionMode(servoId: number): Promise<"success">;
+	writeWheelSpeed(servoId: number, speed: number): Promise<"success">;
+	syncReadPositions(servoIds: number[]): Promise<Map<number, number>>;
+	syncWritePositions(servoPositions: ServoPositions): Promise<"success">;
+	syncWriteWheelSpeed(servoSpeeds: ServoSpeeds): Promise<"success">; // Add syncWriteWheelSpeed definition
+	setBaudRate(servoId: number, baudRateIndex: number): Promise<"success">;
+	setServoId(currentServoId: number, newServoId: number): Promise<"success">;
+}
+
+export const scsServoSDK: ScsServoSDK;
+
+export interface ScsServoSDKUnlock {
+	connect(options?: ConnectionOptions): Promise<true>;
+	disconnect(): Promise<true>;
+	readPosition(servoId: number): Promise<number>;
+	syncReadPositions(servoIds: number[]): Promise<Map<number, number>>;
+}
+
+export const scsServoSDKUnlock: ScsServoSDKUnlock;

packages/feetech.js/index.mjs

@@ -0,0 +1,57 @@
+// Import all functions from the new scsServoSDK module
+import {
+	connect,
+	disconnect,
+	readPosition,
+	readBaudRate,
+	readMode,
+	writePosition,
+	writeTorqueEnable,
+	writeAcceleration,
+	setWheelMode,
+	setPositionMode,
+	writeWheelSpeed,
+	syncReadPositions,
+	syncWritePositions,
+	syncWriteWheelSpeed, // Import the new function
+	setBaudRate,
+	setServoId
+} from "./scsServoSDK.mjs";
+
+// Create an object to hold all the SCS servo functions
+export const scsServoSDK = {
+	connect,
+	disconnect,
+	readPosition,
+	readBaudRate,
+	readMode,
+	writePosition,
+	writeTorqueEnable,
+	writeAcceleration,
+	setWheelMode,
+	setPositionMode,
+	writeWheelSpeed,
+	syncReadPositions,
+	syncWritePositions,
+	syncWriteWheelSpeed, // Add the new function to the export
+	setBaudRate,
+	setServoId
+};
+
+import {
+	connect as connectUnlock,
+	disconnect as disconnectUnlock,
+	readPosition as readPositionUnlock,
+	syncReadPositions as syncReadPositionsUnlock
+} from "./scsServoSDKUnlock.mjs";
+
+export const scsServoSDKUnlock = {
+	connect: connectUnlock,
+	disconnect: disconnectUnlock,
+	readPosition: readPositionUnlock,
+	syncReadPositions: syncReadPositionsUnlock
+};
+
+// Future: You can add exports for other servo types here, e.g.:
+// export { stsServoSDK } from './stsServoSDK.mjs';
+// export { smsServoSDK } from './smsServoSDK.mjs';

packages/feetech.js/lowLevelSDK.mjs

@@ -0,0 +1,1232 @@
+// Constants
+export const BROADCAST_ID = 0xfe; // 254
+export const MAX_ID = 0xfc; // 252
+
+// Protocol instructions
+export const INST_PING = 1;
+export const INST_READ = 2;
+export const INST_WRITE = 3;
+export const INST_REG_WRITE = 4;
+export const INST_ACTION = 5;
+export const INST_SYNC_WRITE = 131; // 0x83
+export const INST_SYNC_READ = 130; // 0x82
+export const INST_STATUS = 85; // 0x55, status packet instruction (0x55)
+
+// Communication results
+export const COMM_SUCCESS = 0; // tx or rx packet communication success
+export const COMM_PORT_BUSY = -1; // Port is busy (in use)
+export const COMM_TX_FAIL = -2; // Failed transmit instruction packet
+export const COMM_RX_FAIL = -3; // Failed get status packet
+export const COMM_TX_ERROR = -4; // Incorrect instruction packet
+export const COMM_RX_WAITING = -5; // Now receiving status packet
+export const COMM_RX_TIMEOUT = -6; // There is no status packet
+export const COMM_RX_CORRUPT = -7; // Incorrect status packet
+export const COMM_NOT_AVAILABLE = -9;
+
+// Packet constants
+export const TXPACKET_MAX_LEN = 250;
+export const RXPACKET_MAX_LEN = 250;
+
+// Protocol Packet positions
+export const PKT_HEADER0 = 0;
+export const PKT_HEADER1 = 1;
+export const PKT_ID = 2;
+export const PKT_LENGTH = 3;
+export const PKT_INSTRUCTION = 4;
+export const PKT_ERROR = 4;
+export const PKT_PARAMETER0 = 5;
+
+// Protocol Error bits
+export const ERRBIT_VOLTAGE = 1;
+export const ERRBIT_ANGLE = 2;
+export const ERRBIT_OVERHEAT = 4;
+export const ERRBIT_OVERELE = 8;
+export const ERRBIT_OVERLOAD = 32;
+
+// Default settings
+const DEFAULT_BAUDRATE = 1000000;
+const LATENCY_TIMER = 16;
+
+// Global protocol end state
+let SCS_END = 0; // (STS/SMS=0, SCS=1)
+
+// Utility functions for handling word operations
+export function SCS_LOWORD(l) {
+	return l & 0xffff;
+}
+
+export function SCS_HIWORD(l) {
+	return (l >> 16) & 0xffff;
+}
+
+export function SCS_LOBYTE(w) {
+	if (SCS_END === 0) {
+		return w & 0xff;
+	} else {
+		return (w >> 8) & 0xff;
+	}
+}
+
+export function SCS_HIBYTE(w) {
+	if (SCS_END === 0) {
+		return (w >> 8) & 0xff;
+	} else {
+		return w & 0xff;
+	}
+}
+
+export function SCS_MAKEWORD(a, b) {
+	if (SCS_END === 0) {
+		return (a & 0xff) | ((b & 0xff) << 8);
+	} else {
+		return (b & 0xff) | ((a & 0xff) << 8);
+	}
+}
+
+export function SCS_MAKEDWORD(a, b) {
+	return (a & 0xffff) | ((b & 0xffff) << 16);
+}
+
+export function SCS_TOHOST(a, b) {
+	if (a & (1 << b)) {
+		return -(a & ~(1 << b));
+	} else {
+		return a;
+	}
+}
+
+export class PortHandler {
+	constructor() {
+		this.port = null;
+		this.reader = null;
+		this.writer = null;
+		this.isOpen = false;
+		this.isUsing = false;
+		this.baudrate = DEFAULT_BAUDRATE;
+		this.packetStartTime = 0;
+		this.packetTimeout = 0;
+		this.txTimePerByte = 0;
+	}
+
+	async requestPort() {
+		try {
+			this.port = await navigator.serial.requestPort();
+			return true;
+		} catch (err) {
+			console.error("Error requesting serial port:", err);
+			return false;
+		}
+	}
+
+	async openPort() {
+		if (!this.port) {
+			return false;
+		}
+
+		try {
+			await this.port.open({ baudRate: this.baudrate });
+			this.reader = this.port.readable.getReader();
+			this.writer = this.port.writable.getWriter();
+			this.isOpen = true;
+			this.txTimePerByte = (1000.0 / this.baudrate) * 10.0;
+			return true;
+		} catch (err) {
+			console.error("Error opening port:", err);
+			return false;
+		}
+	}
+
+	async closePort() {
+		if (this.reader) {
+			await this.reader.releaseLock();
+			this.reader = null;
+		}
+
+		if (this.writer) {
+			await this.writer.releaseLock();
+			this.writer = null;
+		}
+
+		if (this.port && this.isOpen) {
+			await this.port.close();
+			this.isOpen = false;
+		}
+	}
+
+	async clearPort() {
+		if (this.reader) {
+			await this.reader.releaseLock();
+			this.reader = this.port.readable.getReader();
+		}
+	}
+
+	setBaudRate(baudrate) {
+		this.baudrate = baudrate;
+		this.txTimePerByte = (1000.0 / this.baudrate) * 10.0;
+		return true;
+	}
+
+	getBaudRate() {
+		return this.baudrate;
+	}
+
+	async writePort(data) {
+		if (!this.isOpen || !this.writer) {
+			return 0;
+		}
+
+		try {
+			await this.writer.write(new Uint8Array(data));
+			return data.length;
+		} catch (err) {
+			console.error("Error writing to port:", err);
+			return 0;
+		}
+	}
+
+	async readPort(length) {
+		if (!this.isOpen || !this.reader) {
+			return [];
+		}
+
+		try {
+			// Increase timeout for more reliable data reception
+			const timeoutMs = 500;
+			let totalBytes = [];
+			const startTime = performance.now();
+
+			// Continue reading until we get enough bytes or timeout
+			while (totalBytes.length < length) {
+				// Create a timeout promise
+				const timeoutPromise = new Promise((resolve) => {
+					setTimeout(() => resolve({ value: new Uint8Array(), done: false, timeout: true }), 100); // Short internal timeout
+				});
+
+				// Race between reading and timeout
+				const result = await Promise.race([this.reader.read(), timeoutPromise]);
+
+				if (result.timeout) {
+					// Internal timeout - check if we've exceeded total timeout
+					if (performance.now() - startTime > timeoutMs) {
+						console.log(`readPort total timeout after ${timeoutMs}ms`);
+						break;
+					}
+					continue; // Try reading again
+				}
+
+				if (result.done) {
+					console.log("Reader done, stream closed");
+					break;
+				}
+
+				if (result.value.length === 0) {
+					// If there's no data but we haven't timed out yet, wait briefly and try again
+					await new Promise((resolve) => setTimeout(resolve, 10));
+
+					// Check if we've exceeded total timeout
+					if (performance.now() - startTime > timeoutMs) {
+						console.log(`readPort total timeout after ${timeoutMs}ms`);
+						break;
+					}
+					continue;
+				}
+
+				// Add received bytes to our total
+				const newData = Array.from(result.value);
+				totalBytes.push(...newData);
+				console.log(
+					`Read ${newData.length} bytes:`,
+					newData.map((b) => b.toString(16).padStart(2, "0")).join(" ")
+				);
+
+				// If we've got enough data, we can stop
+				if (totalBytes.length >= length) {
+					break;
+				}
+			}
+
+			return totalBytes;
+		} catch (err) {
+			console.error("Error reading from port:", err);
+			return [];
+		}
+	}
+
+	setPacketTimeout(packetLength) {
+		this.packetStartTime = this.getCurrentTime();
+		this.packetTimeout = this.txTimePerByte * packetLength + LATENCY_TIMER * 2.0 + 2.0;
+	}
+
+	setPacketTimeoutMillis(msec) {
+		this.packetStartTime = this.getCurrentTime();
+		this.packetTimeout = msec;
+	}
+
+	isPacketTimeout() {
+		if (this.getTimeSinceStart() > this.packetTimeout) {
+			this.packetTimeout = 0;
+			return true;
+		}
+		return false;
+	}
+
+	getCurrentTime() {
+		return performance.now();
+	}
+
+	getTimeSinceStart() {
+		const timeSince = this.getCurrentTime() - this.packetStartTime;
+		if (timeSince < 0.0) {
+			this.packetStartTime = this.getCurrentTime();
+		}
+		return timeSince;
+	}
+}
+
+export class PacketHandler {
+	constructor(protocolEnd = 0) {
+		SCS_END = protocolEnd;
+		console.log(`PacketHandler initialized with protocol_end=${protocolEnd} (STS/SMS=0, SCS=1)`);
+	}
+
+	getProtocolVersion() {
+		return 1.0;
+	}
+
+	// 获取当前协议端设置的方法
+	getProtocolEnd() {
+		return SCS_END;
+	}
+
+	getTxRxResult(result) {
+		if (result === COMM_SUCCESS) {
+			return "[TxRxResult] Communication success!";
+		} else if (result === COMM_PORT_BUSY) {
+			return "[TxRxResult] Port is in use!";
+		} else if (result === COMM_TX_FAIL) {
+			return "[TxRxResult] Failed transmit instruction packet!";
+		} else if (result === COMM_RX_FAIL) {
+			return "[TxRxResult] Failed get status packet from device!";
+		} else if (result === COMM_TX_ERROR) {
+			return "[TxRxResult] Incorrect instruction packet!";
+		} else if (result === COMM_RX_WAITING) {
+			return "[TxRxResult] Now receiving status packet!";
+		} else if (result === COMM_RX_TIMEOUT) {
+			return "[TxRxResult] There is no status packet!";
+		} else if (result === COMM_RX_CORRUPT) {
+			return "[TxRxResult] Incorrect status packet!";
+		} else if (result === COMM_NOT_AVAILABLE) {
+			return "[TxRxResult] Protocol does not support this function!";
+		} else {
+			return "";
+		}
+	}
+
+	getRxPacketError(error) {
+		if (error & ERRBIT_VOLTAGE) {
+			return "[RxPacketError] Input voltage error!";
+		}
+		if (error & ERRBIT_ANGLE) {
+			return "[RxPacketError] Angle sen error!";
+		}
+		if (error & ERRBIT_OVERHEAT) {
+			return "[RxPacketError] Overheat error!";
+		}
+		if (error & ERRBIT_OVERELE) {
+			return "[RxPacketError] OverEle error!";
+		}
+		if (error & ERRBIT_OVERLOAD) {
+			return "[RxPacketError] Overload error!";
+		}
+		return "";
+	}
+
+	async txPacket(port, txpacket) {
+		let checksum = 0;
+		const totalPacketLength = txpacket[PKT_LENGTH] + 4; // 4: HEADER0 HEADER1 ID LENGTH
+
+		if (port.isUsing) {
+			return COMM_PORT_BUSY;
+		}
+		port.isUsing = true;
+
+		// Check max packet length
+		if (totalPacketLength > TXPACKET_MAX_LEN) {
+			port.isUsing = false;
+			return COMM_TX_ERROR;
+		}
+
+		// Make packet header
+		txpacket[PKT_HEADER0] = 0xff;
+		txpacket[PKT_HEADER1] = 0xff;
+
+		// Add checksum to packet
+		for (let idx = 2; idx < totalPacketLength - 1; idx++) {
+			checksum += txpacket[idx];
+		}
+
+		txpacket[totalPacketLength - 1] = ~checksum & 0xff;
+
+		// TX packet
+		await port.clearPort();
+		const writtenPacketLength = await port.writePort(txpacket);
+		if (totalPacketLength !== writtenPacketLength) {
+			port.isUsing = false;
+			return COMM_TX_FAIL;
+		}
+
+		return COMM_SUCCESS;
+	}
+
+	async rxPacket(port) {
+		let rxpacket = [];
+		let result = COMM_RX_FAIL;
+
+		let waitLength = 6; // minimum length (HEADER0 HEADER1 ID LENGTH)
+
+		while (true) {
+			const data = await port.readPort(waitLength - rxpacket.length);
+			rxpacket.push(...data);
+
+			if (rxpacket.length >= waitLength) {
+				// Find packet header
+				let headerIndex = -1;
+				for (let i = 0; i < rxpacket.length - 1; i++) {
+					if (rxpacket[i] === 0xff && rxpacket[i + 1] === 0xff) {
+						headerIndex = i;
+						break;
+					}
+				}
+
+				if (headerIndex === 0) {
+					// Found at the beginning of the packet
+					if (rxpacket[PKT_ID] > 0xfd || rxpacket[PKT_LENGTH] > RXPACKET_MAX_LEN) {
+						// Invalid ID or length
+						rxpacket.shift();
+						continue;
+					}
+
+					// Recalculate expected packet length
+					if (waitLength !== rxpacket[PKT_LENGTH] + PKT_LENGTH + 1) {
+						waitLength = rxpacket[PKT_LENGTH] + PKT_LENGTH + 1;
+						continue;
+					}
+
+					if (rxpacket.length < waitLength) {
+						// Check timeout
+						if (port.isPacketTimeout()) {
+							result = rxpacket.length === 0 ? COMM_RX_TIMEOUT : COMM_RX_CORRUPT;
+							break;
+						}
+						continue;
+					}
+
+					// Calculate checksum
+					let checksum = 0;
+					for (let i = 2; i < waitLength - 1; i++) {
+						checksum += rxpacket[i];
+					}
+					checksum = ~checksum & 0xff;
+
+					// Verify checksum
+					if (rxpacket[waitLength - 1] === checksum) {
+						result = COMM_SUCCESS;
+					} else {
+						result = COMM_RX_CORRUPT;
+					}
+					break;
+				} else if (headerIndex > 0) {
+					// Remove unnecessary bytes before header
+					rxpacket = rxpacket.slice(headerIndex);
+					continue;
+				}
+			}
+
+			// Check timeout
+			if (port.isPacketTimeout()) {
+				result = rxpacket.length === 0 ? COMM_RX_TIMEOUT : COMM_RX_CORRUPT;
+				break;
+			}
+		}
+
+		if (result !== COMM_SUCCESS) {
+			console.log(
+				`rxPacket result: ${result}, packet: ${rxpacket.map((b) => "0x" + b.toString(16).padStart(2, "0")).join(" ")}`
+			);
+		} else {
+			console.debug(
+				`rxPacket successful: ${rxpacket.map((b) => "0x" + b.toString(16).padStart(2, "0")).join(" ")}`
+			);
+		}
+		return [rxpacket, result];
+	}
+
+	async txRxPacket(port, txpacket) {
+		let rxpacket = null;
+		let error = 0;
+		let result = COMM_TX_FAIL;
+
+		try {
+			// Check if port is already in use
+			if (port.isUsing) {
+				console.log("Port is busy, cannot start new transaction");
+				return [rxpacket, COMM_PORT_BUSY, error];
+			}
+
+			// TX packet
+			console.log(
+				"Sending packet:",
+				txpacket.map((b) => "0x" + b.toString(16).padStart(2, "0")).join(" ")
+			);
+
+			// Remove retry logic and just send once
+			result = await this.txPacket(port, txpacket);
+			console.log(`TX result: ${result}`);
+
+			if (result !== COMM_SUCCESS) {
+				console.log(`TX failed with result: ${result}`);
+				port.isUsing = false; // Important: Release the port on TX failure
+				return [rxpacket, result, error];
+			}
+
+			// If ID is broadcast, no need to wait for status packet
+			if (txpacket[PKT_ID] === BROADCAST_ID) {
+				port.isUsing = false;
+				return [rxpacket, result, error];
+			}
+
+			// Set packet timeout
+			if (txpacket[PKT_INSTRUCTION] === INST_READ) {
+				const length = txpacket[PKT_PARAMETER0 + 1];
+				// For READ instructions, we expect response to include the data
+				port.setPacketTimeout(length + 10); // Add extra buffer
+				console.log(`Set READ packet timeout for ${length + 10} bytes`);
+			} else {
+				// For other instructions, we expect a status packet
+				port.setPacketTimeout(10); // HEADER0 HEADER1 ID LENGTH ERROR CHECKSUM + buffer
+				console.log(`Set standard packet timeout for 10 bytes`);
+			}
+
+			// RX packet - no retries, just attempt once
+			console.log(`Receiving packet`);
+
+			// Clear port before receiving to ensure clean state
+			await port.clearPort();
+
+			const [rxpacketResult, resultRx] = await this.rxPacket(port);
+			rxpacket = rxpacketResult;
+
+			// Check if received packet is valid
+			if (resultRx !== COMM_SUCCESS) {
+				console.log(`Rx failed with result: ${resultRx}`);
+				port.isUsing = false;
+				return [rxpacket, resultRx, error];
+			}
+
+			// Verify packet structure
+			if (rxpacket.length < 6) {
+				console.log(`Received packet too short (${rxpacket.length} bytes)`);
+				port.isUsing = false;
+				return [rxpacket, COMM_RX_CORRUPT, error];
+			}
+
+			// Verify packet ID matches the sent ID
+			if (rxpacket[PKT_ID] !== txpacket[PKT_ID]) {
+				console.log(
+					`Received packet ID (${rxpacket[PKT_ID]}) doesn't match sent ID (${txpacket[PKT_ID]})`
+				);
+				port.isUsing = false;
+				return [rxpacket, COMM_RX_CORRUPT, error];
+			}
+
+			// Packet looks valid
+			error = rxpacket[PKT_ERROR];
+			port.isUsing = false; // Release port on success
+			return [rxpacket, resultRx, error];
+		} catch (err) {
+			console.error("Exception in txRxPacket:", err);
+			port.isUsing = false; // Release port on exception
+			return [rxpacket, COMM_RX_FAIL, error];
+		}
+	}
+
+	async ping(port, scsId) {
+		let modelNumber = 0;
+		let error = 0;
+
+		try {
+			if (scsId >= BROADCAST_ID) {
+				console.log(`Cannot ping broadcast ID ${scsId}`);
+				return [modelNumber, COMM_NOT_AVAILABLE, error];
+			}
+
+			const txpacket = new Array(6).fill(0);
+			txpacket[PKT_ID] = scsId;
+			txpacket[PKT_LENGTH] = 2;
+			txpacket[PKT_INSTRUCTION] = INST_PING;
+
+			console.log(`Pinging servo ID ${scsId}...`);
+
+			// 发送ping指令并获取响应
+			const [rxpacket, result, err] = await this.txRxPacket(port, txpacket);
+			error = err;
+
+			// 与Python SDK保持一致：如果ping成功，尝试��取地址3的型号信息
+			if (result === COMM_SUCCESS) {
+				console.log(`Ping to servo ID ${scsId} succeeded, reading model number from address 3`);
+				// 读取地址3的型号信息（2字节）
+				const [data, dataResult, dataError] = await this.readTxRx(port, scsId, 3, 2);
+
+				if (dataResult === COMM_SUCCESS && data && data.length >= 2) {
+					modelNumber = SCS_MAKEWORD(data[0], data[1]);
+					console.log(`Model number read: ${modelNumber}`);
+				} else {
+					console.log(`Could not read model number: ${this.getTxRxResult(dataResult)}`);
+				}
+			} else {
+				console.log(`Ping failed with result: ${result}, error: ${error}`);
+			}
+
+			return [modelNumber, result, error];
+		} catch (error) {
+			console.error(`Exception in ping():`, error);
+			return [0, COMM_RX_FAIL, 0];
+		}
+	}
+
+	// Read methods
+	async readTxRx(port, scsId, address, length) {
+		if (scsId >= BROADCAST_ID) {
+			console.log("Cannot read from broadcast ID");
+			return [[], COMM_NOT_AVAILABLE, 0];
+		}
+
+		// Create read packet
+		const txpacket = new Array(8).fill(0);
+		txpacket[PKT_ID] = scsId;
+		txpacket[PKT_LENGTH] = 4;
+		txpacket[PKT_INSTRUCTION] = INST_READ;
+		txpacket[PKT_PARAMETER0] = address;
+		txpacket[PKT_PARAMETER0 + 1] = length;
+
+		console.log(`Reading ${length} bytes from address ${address} for servo ID ${scsId}`);
+
+		// Send packet and get response
+		const [rxpacket, result, error] = await this.txRxPacket(port, txpacket);
+
+		// Process the result
+		if (result !== COMM_SUCCESS) {
+			console.log(`Read failed with result: ${result}, error: ${error}`);
+			return [[], result, error];
+		}
+
+		if (!rxpacket || rxpacket.length < PKT_PARAMETER0 + length) {
+			console.log(
+				`Invalid response packet: expected at least ${PKT_PARAMETER0 + length} bytes, got ${rxpacket ? rxpacket.length : 0}`
+			);
+			return [[], COMM_RX_CORRUPT, error];
+		}
+
+		// Extract data from response
+		const data = [];
+		console.log(
+			`Response packet length: ${rxpacket.length}, extracting ${length} bytes from offset ${PKT_PARAMETER0}`
+		);
+		console.log(
+			`Response data bytes: ${rxpacket
+				.slice(PKT_PARAMETER0, PKT_PARAMETER0 + length)
+				.map((b) => "0x" + b.toString(16).padStart(2, "0"))
+				.join(" ")}`
+		);
+
+		for (let i = 0; i < length; i++) {
+			data.push(rxpacket[PKT_PARAMETER0 + i]);
+		}
+
+		console.log(
+			`Successfully read ${length} bytes: ${data.map((b) => "0x" + b.toString(16).padStart(2, "0")).join(" ")}`
+		);
+		return [data, result, error];
+	}
+
+	async read1ByteTxRx(port, scsId, address) {
+		const [data, result, error] = await this.readTxRx(port, scsId, address, 1);
+		const value = data.length > 0 ? data[0] : 0;
+		return [value, result, error];
+	}
+
+	async read2ByteTxRx(port, scsId, address) {
+		const [data, result, error] = await this.readTxRx(port, scsId, address, 2);
+
+		let value = 0;
+		if (data.length >= 2) {
+			value = SCS_MAKEWORD(data[0], data[1]);
+		}
+
+		return [value, result, error];
+	}
+
+	async read4ByteTxRx(port, scsId, address) {
+		const [data, result, error] = await this.readTxRx(port, scsId, address, 4);
+
+		let value = 0;
+		if (data.length >= 4) {
+			const loword = SCS_MAKEWORD(data[0], data[1]);
+			const hiword = SCS_MAKEWORD(data[2], data[3]);
+			value = SCS_MAKEDWORD(loword, hiword);
+
+			console.log(
+				`read4ByteTxRx: data=${data.map((b) => "0x" + b.toString(16).padStart(2, "0")).join(" ")}`
+			);
+			console.log(
+				`  loword=${loword} (0x${loword.toString(16)}), hiword=${hiword} (0x${hiword.toString(16)})`
+			);
+			console.log(`  value=${value} (0x${value.toString(16)})`);
+		}
+
+		return [value, result, error];
+	}
+
+	// Write methods
+	async writeTxRx(port, scsId, address, length, data) {
+		if (scsId >= BROADCAST_ID) {
+			return [COMM_NOT_AVAILABLE, 0];
+		}
+
+		// Create write packet
+		const txpacket = new Array(length + 7).fill(0);
+		txpacket[PKT_ID] = scsId;
+		txpacket[PKT_LENGTH] = length + 3;
+		txpacket[PKT_INSTRUCTION] = INST_WRITE;
+		txpacket[PKT_PARAMETER0] = address;
+
+		// Add data
+		for (let i = 0; i < length; i++) {
+			txpacket[PKT_PARAMETER0 + 1 + i] = data[i] & 0xff;
+		}
+
+		// Send packet and get response
+		const [rxpacket, result, error] = await this.txRxPacket(port, txpacket);
+
+		return [result, error];
+	}
+
+	async write1ByteTxRx(port, scsId, address, data) {
+		const dataArray = [data & 0xff];
+		return await this.writeTxRx(port, scsId, address, 1, dataArray);
+	}
+
+	async write2ByteTxRx(port, scsId, address, data) {
+		const dataArray = [SCS_LOBYTE(data), SCS_HIBYTE(data)];
+		return await this.writeTxRx(port, scsId, address, 2, dataArray);
+	}
+
+	async write4ByteTxRx(port, scsId, address, data) {
+		const dataArray = [
+			SCS_LOBYTE(SCS_LOWORD(data)),
+			SCS_HIBYTE(SCS_LOWORD(data)),
+			SCS_LOBYTE(SCS_HIWORD(data)),
+			SCS_HIBYTE(SCS_HIWORD(data))
+		];
+		return await this.writeTxRx(port, scsId, address, 4, dataArray);
+	}
+
+	// Add syncReadTx for GroupSyncRead functionality
+	async syncReadTx(port, startAddress, dataLength, param, paramLength) {
+		// Create packet: HEADER0 HEADER1 ID LEN INST START_ADDR DATA_LEN PARAM... CHKSUM
+		const txpacket = new Array(paramLength + 8).fill(0);
+
+		txpacket[PKT_ID] = BROADCAST_ID;
+		txpacket[PKT_LENGTH] = paramLength + 4; // 4: INST START_ADDR DATA_LEN CHKSUM
+		txpacket[PKT_INSTRUCTION] = INST_SYNC_READ;
+		txpacket[PKT_PARAMETER0] = startAddress;
+		txpacket[PKT_PARAMETER0 + 1] = dataLength;
+
+		// Add parameters
+		for (let i = 0; i < paramLength; i++) {
+			txpacket[PKT_PARAMETER0 + 2 + i] = param[i];
+		}
+
+		// Calculate checksum
+		const totalLen = txpacket[PKT_LENGTH] + 4; // 4: HEADER0 HEADER1 ID LENGTH
+
+		// Add headers
+		txpacket[PKT_HEADER0] = 0xff;
+		txpacket[PKT_HEADER1] = 0xff;
+
+		// Calculate checksum
+		let checksum = 0;
+		for (let i = 2; i < totalLen - 1; i++) {
+			checksum += txpacket[i] & 0xff;
+		}
+		txpacket[totalLen - 1] = ~checksum & 0xff;
+
+		console.log(
+			`SyncReadTx: ${txpacket.map((b) => "0x" + b.toString(16).padStart(2, "0")).join(" ")}`
+		);
+
+		// Send packet
+		await port.clearPort();
+		const bytesWritten = await port.writePort(txpacket);
+		if (bytesWritten !== totalLen) {
+			return COMM_TX_FAIL;
+		}
+
+		// Set timeout based on expected response size
+		port.setPacketTimeout((6 + dataLength) * paramLength);
+
+		return COMM_SUCCESS;
+	}
+
+	// Add syncWriteTxOnly for GroupSyncWrite functionality
+	async syncWriteTxOnly(port, startAddress, dataLength, param, paramLength) {
+		// Create packet: HEADER0 HEADER1 ID LEN INST START_ADDR DATA_LEN PARAM... CHKSUM
+		const txpacket = new Array(paramLength + 8).fill(0);
+
+		txpacket[PKT_ID] = BROADCAST_ID;
+		txpacket[PKT_LENGTH] = paramLength + 4; // 4: INST START_ADDR DATA_LEN CHKSUM
+		txpacket[PKT_INSTRUCTION] = INST_SYNC_WRITE;
+		txpacket[PKT_PARAMETER0] = startAddress;
+		txpacket[PKT_PARAMETER0 + 1] = dataLength;
+
+		// Add parameters
+		for (let i = 0; i < paramLength; i++) {
+			txpacket[PKT_PARAMETER0 + 2 + i] = param[i];
+		}
+
+		// Calculate checksum
+		const totalLen = txpacket[PKT_LENGTH] + 4; // 4: HEADER0 HEADER1 ID LENGTH
+
+		// Add headers
+		txpacket[PKT_HEADER0] = 0xff;
+		txpacket[PKT_HEADER1] = 0xff;
+
+		// Calculate checksum
+		let checksum = 0;
+		for (let i = 2; i < totalLen - 1; i++) {
+			checksum += txpacket[i] & 0xff;
+		}
+		txpacket[totalLen - 1] = ~checksum & 0xff;
+
+		console.log(
+			`SyncWriteTxOnly: ${txpacket.map((b) => "0x" + b.toString(16).padStart(2, "0")).join(" ")}`
+		);
+
+		// Send packet - for sync write, we don't need a response
+		await port.clearPort();
+		const bytesWritten = await port.writePort(txpacket);
+		if (bytesWritten !== totalLen) {
+			return COMM_TX_FAIL;
+		}
+
+		return COMM_SUCCESS;
+	}
+
+	// 辅助方法：格式化数据包结构以方便调试
+	formatPacketStructure(packet) {
+		if (!packet || packet.length < 4) {
+			return "Invalid packet (too short)";
+		}
+
+		try {
+			let result = "";
+			result += `HEADER: ${packet[0].toString(16).padStart(2, "0")} ${packet[1].toString(16).padStart(2, "0")} | `;
+			result += `ID: ${packet[2]} | `;
+			result += `LENGTH: ${packet[3]} | `;
+
+			if (packet.length >= 5) {
+				result += `ERROR/INST: ${packet[4].toString(16).padStart(2, "0")} | `;
+			}
+
+			if (packet.length >= 6) {
+				result += "PARAMS: ";
+				for (let i = 5; i < packet.length - 1; i++) {
+					result += `${packet[i].toString(16).padStart(2, "0")} `;
+				}
+				result += `| CHECKSUM: ${packet[packet.length - 1].toString(16).padStart(2, "0")}`;
+			}
+
+			return result;
+		} catch (e) {
+			return "Error formatting packet: " + e.message;
+		}
+	}
+
+	/**
+	 * 从响应包中解析舵机型号
+	 * @param {Array} rxpacket - 响应数据包
+	 * @returns {number} 舵机型号
+	 */
+	parseModelNumber(rxpacket) {
+		if (!rxpacket || rxpacket.length < 7) {
+			return 0;
+		}
+
+		// 检查是否有参数字段
+		if (rxpacket.length <= PKT_PARAMETER0 + 1) {
+			return 0;
+		}
+
+		const param1 = rxpacket[PKT_PARAMETER0];
+		const param2 = rxpacket[PKT_PARAMETER0 + 1];
+
+		if (SCS_END === 0) {
+			// STS/SMS 协议的字节顺序
+			return SCS_MAKEWORD(param1, param2);
+		} else {
+			// SCS 协议的字节顺序
+			return SCS_MAKEWORD(param2, param1);
+		}
+	}
+
+	/**
+	 * Verify packet header
+	 * @param {Array} packet - The packet to verify
+	 * @returns {Number} COMM_SUCCESS if packet is valid, error code otherwise
+	 */
+	getPacketHeader(packet) {
+		if (!packet || packet.length < 4) {
+			return COMM_RX_CORRUPT;
+		}
+
+		// Check header
+		if (packet[PKT_HEADER0] !== 0xff || packet[PKT_HEADER1] !== 0xff) {
+			return COMM_RX_CORRUPT;
+		}
+
+		// Check ID validity
+		if (packet[PKT_ID] > 0xfd) {
+			return COMM_RX_CORRUPT;
+		}
+
+		// Check length
+		if (packet.length != packet[PKT_LENGTH] + 4) {
+			return COMM_RX_CORRUPT;
+		}
+
+		// Calculate checksum
+		let checksum = 0;
+		for (let i = 2; i < packet.length - 1; i++) {
+			checksum += packet[i] & 0xff;
+		}
+		checksum = ~checksum & 0xff;
+
+		// Verify checksum
+		if (packet[packet.length - 1] !== checksum) {
+			return COMM_RX_CORRUPT;
+		}
+
+		return COMM_SUCCESS;
+	}
+}
+
+/**
+ * GroupSyncRead class
+ * - This class is used to read multiple servos with the same control table address at once
+ */
+export class GroupSyncRead {
+	constructor(port, ph, startAddress, dataLength) {
+		this.port = port;
+		this.ph = ph;
+		this.startAddress = startAddress;
+		this.dataLength = dataLength;
+
+		this.isAvailableServiceID = new Set();
+		this.dataDict = new Map();
+		this.param = [];
+		this.clearParam();
+	}
+
+	makeParam() {
+		this.param = [];
+		for (const id of this.isAvailableServiceID) {
+			this.param.push(id);
+		}
+		return this.param.length;
+	}
+
+	addParam(scsId) {
+		if (this.isAvailableServiceID.has(scsId)) {
+			return false;
+		}
+
+		this.isAvailableServiceID.add(scsId);
+		this.dataDict.set(scsId, new Array(this.dataLength).fill(0));
+		return true;
+	}
+
+	removeParam(scsId) {
+		if (!this.isAvailableServiceID.has(scsId)) {
+			return false;
+		}
+
+		this.isAvailableServiceID.delete(scsId);
+		this.dataDict.delete(scsId);
+		return true;
+	}
+
+	clearParam() {
+		this.isAvailableServiceID.clear();
+		this.dataDict.clear();
+		return true;
+	}
+
+	async txPacket() {
+		if (this.isAvailableServiceID.size === 0) {
+			return COMM_NOT_AVAILABLE;
+		}
+
+		const paramLength = this.makeParam();
+		return await this.ph.syncReadTx(
+			this.port,
+			this.startAddress,
+			this.dataLength,
+			this.param,
+			paramLength
+		);
+	}
+
+	async rxPacket() {
+		let result = COMM_RX_FAIL;
+
+		if (this.isAvailableServiceID.size === 0) {
+			return COMM_NOT_AVAILABLE;
+		}
+
+		// Set all servos' data as invalid
+		for (const id of this.isAvailableServiceID) {
+			this.dataDict.set(id, new Array(this.dataLength).fill(0));
+		}
+
+		const [rxpacket, rxResult] = await this.ph.rxPacket(this.port);
+		if (rxResult !== COMM_SUCCESS || !rxpacket || rxpacket.length === 0) {
+			return rxResult;
+		}
+
+		// More tolerant of packets with unexpected values in the PKT_ERROR field
+		// Don't require INST_STATUS to be exactly 0x55
+		console.log(
+			`GroupSyncRead rxPacket: ID=${rxpacket[PKT_ID]}, ERROR/INST field=0x${rxpacket[PKT_ERROR].toString(16)}`
+		);
+
+		// Check if the packet matches any of the available IDs
+		if (!this.isAvailableServiceID.has(rxpacket[PKT_ID])) {
+			console.log(
+				`Received packet with ID ${rxpacket[PKT_ID]} which is not in the available IDs list`
+			);
+			return COMM_RX_CORRUPT;
+		}
+
+		// Extract data for the matching ID
+		const scsId = rxpacket[PKT_ID];
+		const data = new Array(this.dataLength).fill(0);
+
+		// Extract the parameter data, which should start at PKT_PARAMETER0
+		if (rxpacket.length < PKT_PARAMETER0 + this.dataLength) {
+			console.log(
+				`Packet too short: expected ${PKT_PARAMETER0 + this.dataLength} bytes, got ${rxpacket.length}`
+			);
+			return COMM_RX_CORRUPT;
+		}
+
+		for (let i = 0; i < this.dataLength; i++) {
+			data[i] = rxpacket[PKT_PARAMETER0 + i];
+		}
+
+		// Update the data dict
+		this.dataDict.set(scsId, data);
+		console.log(
+			`Updated data for servo ID ${scsId}: ${data.map((b) => "0x" + b.toString(16).padStart(2, "0")).join(" ")}`
+		);
+
+		// Continue receiving until timeout or all data is received
+		if (this.isAvailableServiceID.size > 1) {
+			result = await this.rxPacket();
+		} else {
+			result = COMM_SUCCESS;
+		}
+
+		return result;
+	}
+
+	async txRxPacket() {
+		try {
+			// First check if port is being used
+			if (this.port.isUsing) {
+				console.log("Port is busy, cannot start sync read operation");
+				return COMM_PORT_BUSY;
+			}
+
+			// Start the transmission
+			console.log("Starting sync read TX/RX operation...");
+			let result = await this.txPacket();
+			if (result !== COMM_SUCCESS) {
+				console.log(`Sync read TX failed with result: ${result}`);
+				return result;
+			}
+
+			// Get a single response with a standard timeout
+			console.log(`Attempting to receive a response...`);
+
+			// Receive a single response
+			result = await this.rxPacket();
+
+			// Release port
+			this.port.isUsing = false;
+
+			return result;
+		} catch (error) {
+			console.error("Exception in GroupSyncRead txRxPacket:", error);
+			// Make sure port is released
+			this.port.isUsing = false;
+			return COMM_RX_FAIL;
+		}
+	}
+
+	isAvailable(scsId, address, dataLength) {
+		if (!this.isAvailableServiceID.has(scsId)) {
+			return false;
+		}
+
+		const startAddr = this.startAddress;
+		const endAddr = startAddr + this.dataLength - 1;
+
+		const reqStartAddr = address;
+		const reqEndAddr = reqStartAddr + dataLength - 1;
+
+		if (reqStartAddr < startAddr || reqEndAddr > endAddr) {
+			return false;
+		}
+
+		const data = this.dataDict.get(scsId);
+		if (!data || data.length === 0) {
+			return false;
+		}
+
+		return true;
+	}
+
+	getData(scsId, address, dataLength) {
+		if (!this.isAvailable(scsId, address, dataLength)) {
+			return 0;
+		}
+
+		const startAddr = this.startAddress;
+		const data = this.dataDict.get(scsId);
+
+		// Calculate data offset
+		const dataOffset = address - startAddr;
+
+		// Combine bytes according to dataLength
+		switch (dataLength) {
+			case 1:
+				return data[dataOffset];
+			case 2:
+				return SCS_MAKEWORD(data[dataOffset], data[dataOffset + 1]);
+			case 4:
+				return SCS_MAKEDWORD(
+					SCS_MAKEWORD(data[dataOffset], data[dataOffset + 1]),
+					SCS_MAKEWORD(data[dataOffset + 2], data[dataOffset + 3])
+				);
+			default:
+				return 0;
+		}
+	}
+}
+
+/**
+ * GroupSyncWrite class
+ * - This class is used to write multiple servos with the same control table address at once
+ */
+export class GroupSyncWrite {
+	constructor(port, ph, startAddress, dataLength) {
+		this.port = port;
+		this.ph = ph;
+		this.startAddress = startAddress;
+		this.dataLength = dataLength;
+
+		this.isAvailableServiceID = new Set();
+		this.dataDict = new Map();
+		this.param = [];
+		this.clearParam();
+	}
+
+	makeParam() {
+		this.param = [];
+		for (const id of this.isAvailableServiceID) {
+			// Add ID to parameter
+			this.param.push(id);
+
+			// Add data to parameter
+			const data = this.dataDict.get(id);
+			for (let i = 0; i < this.dataLength; i++) {
+				this.param.push(data[i]);
+			}
+		}
+		return this.param.length;
+	}
+
+	addParam(scsId, data) {
+		if (this.isAvailableServiceID.has(scsId)) {
+			return false;
+		}
+
+		if (data.length !== this.dataLength) {
+			console.error(
+				`Data length (${data.length}) doesn't match required length (${this.dataLength})`
+			);
+			return false;
+		}
+
+		this.isAvailableServiceID.add(scsId);
+		this.dataDict.set(scsId, data);
+		return true;
+	}
+
+	removeParam(scsId) {
+		if (!this.isAvailableServiceID.has(scsId)) {
+			return false;
+		}
+
+		this.isAvailableServiceID.delete(scsId);
+		this.dataDict.delete(scsId);
+		return true;
+	}
+
+	changeParam(scsId, data) {
+		if (!this.isAvailableServiceID.has(scsId)) {
+			return false;
+		}
+
+		if (data.length !== this.dataLength) {
+			console.error(
+				`Data length (${data.length}) doesn't match required length (${this.dataLength})`
+			);
+			return false;
+		}
+
+		this.dataDict.set(scsId, data);
+		return true;
+	}
+
+	clearParam() {
+		this.isAvailableServiceID.clear();
+		this.dataDict.clear();
+		return true;
+	}
+
+	async txPacket() {
+		if (this.isAvailableServiceID.size === 0) {
+			return COMM_NOT_AVAILABLE;
+		}
+
+		const paramLength = this.makeParam();
+		return await this.ph.syncWriteTxOnly(
+			this.port,
+			this.startAddress,
+			this.dataLength,
+			this.param,
+			paramLength
+		);
+	}
+}

packages/feetech.js/package.json

@@ -0,0 +1,38 @@
+{
+	"name": "feetech.js",
+	"version": "0.0.8",
+	"description": "javascript sdk for feetech servos",
+	"main": "index.mjs",
+	"files": [
+		"*.mjs",
+		"*.ts"
+	],
+	"type": "module",
+	"engines": {
+		"node": ">=12.17.0"
+	},
+	"scripts": {
+		"test": "echo \"Error: no test specified\" && exit 1"
+	},
+	"repository": {
+		"type": "git",
+		"url": "git+https://github.com/timqian/bambot/tree/main/feetech.js"
+	},
+	"keywords": [
+		"feetech",
+		"sdk",
+		"js",
+		"javascript",
+		"sts3215",
+		"3215",
+		"scs",
+		"scs3215",
+		"st3215"
+	],
+	"author": "timqian",
+	"license": "MIT",
+	"bugs": {
+		"url": "https://github.com/timqian/bambot/issues"
+	},
+	"homepage": "https://github.com/timqian/bambot/tree/main/feetech.js"
+}

packages/feetech.js/scsServoSDK.mjs

@@ -0,0 +1,910 @@
+import {
+	PortHandler,
+	PacketHandler,
+	COMM_SUCCESS,
+	COMM_RX_TIMEOUT,
+	COMM_RX_CORRUPT,
+	COMM_TX_FAIL,
+	COMM_NOT_AVAILABLE,
+	SCS_LOBYTE,
+	SCS_HIBYTE,
+	SCS_MAKEWORD,
+	GroupSyncRead, // Import GroupSyncRead
+	GroupSyncWrite // Import GroupSyncWrite
+} from "./lowLevelSDK.mjs";
+
+// Import address constants from the correct file
+import {
+	ADDR_SCS_PRESENT_POSITION,
+	ADDR_SCS_GOAL_POSITION,
+	ADDR_SCS_TORQUE_ENABLE,
+	ADDR_SCS_GOAL_ACC,
+	ADDR_SCS_GOAL_SPEED
+} from "./scsservo_constants.mjs";
+
+// Define constants not present in scsservo_constants.mjs
+const ADDR_SCS_MODE = 33;
+const ADDR_SCS_LOCK = 55;
+const ADDR_SCS_ID = 5; // Address for Servo ID
+const ADDR_SCS_BAUD_RATE = 6; // Address for Baud Rate
+
+// Module-level variables for handlers
+let portHandler = null;
+let packetHandler = null;
+
+/**
+ * Connects to the serial port and initializes handlers.
+ * @param {object} [options] - Connection options.
+ * @param {number} [options.baudRate=1000000] - The baud rate for the serial connection.
+ * @param {number} [options.protocolEnd=0] - The protocol end setting (0 for STS/SMS, 1 for SCS).
+ * @returns {Promise<true>} Resolves with true on successful connection.
+ * @throws {Error} If connection fails or port cannot be opened/selected.
+ */
+export async function connect(options = {}) {
+	if (portHandler && portHandler.isOpen) {
+		console.log("Already connected.");
+		return true;
+	}
+
+	const { baudRate = 1000000, protocolEnd = 0 } = options;
+
+	try {
+		portHandler = new PortHandler();
+		const portRequested = await portHandler.requestPort();
+		if (!portRequested) {
+			portHandler = null;
+			throw new Error("Failed to select a serial port.");
+		}
+
+		portHandler.setBaudRate(baudRate);
+		const portOpened = await portHandler.openPort();
+		if (!portOpened) {
+			await portHandler.closePort().catch(console.error); // Attempt cleanup
+			portHandler = null;
+			throw new Error(`Failed to open port at baudrate ${baudRate}.`);
+		}
+
+		packetHandler = new PacketHandler(protocolEnd);
+		console.log(`Connected to serial port at ${baudRate} baud, protocol end: ${protocolEnd}.`);
+		return true;
+	} catch (err) {
+		console.error("Error during connection:", err);
+		if (portHandler) {
+			try {
+				await portHandler.closePort();
+			} catch (closeErr) {
+				console.error("Error closing port after connection failure:", closeErr);
+			}
+		}
+		portHandler = null;
+		packetHandler = null;
+		// Re-throw the original or a new error
+		throw new Error(`Connection failed: ${err.message}`);
+	}
+}
+
+/**
+ * Disconnects from the serial port.
+ * @returns {Promise<true>} Resolves with true on successful disconnection.
+ * @throws {Error} If disconnection fails.
+ */
+export async function disconnect() {
+	if (!portHandler || !portHandler.isOpen) {
+		console.log("Already disconnected.");
+		return true;
+	}
+
+	try {
+		await portHandler.closePort();
+		portHandler = null;
+		packetHandler = null;
+		console.log("Disconnected from serial port.");
+		return true;
+	} catch (err) {
+		console.error("Error during disconnection:", err);
+		// Attempt to nullify handlers even if close fails
+		portHandler = null;
+		packetHandler = null;
+		throw new Error(`Disconnection failed: ${err.message}`);
+	}
+}
+
+/**
+ * Checks if the SDK is connected. Throws an error if not.
+ * @throws {Error} If not connected.
+ */
+function checkConnection() {
+	if (!portHandler || !packetHandler) {
+		throw new Error("Not connected. Call connect() first.");
+	}
+}
+
+/**
+ * Reads the current position of a servo.
+ * @param {number} servoId - The ID of the servo (1-252).
+ * @returns {Promise<number>} Resolves with the position (0-4095).
+ * @throws {Error} If not connected, read fails, or an exception occurs.
+ */
+export async function readPosition(servoId) {
+	checkConnection();
+	try {
+		const [position, result, error] = await packetHandler.read2ByteTxRx(
+			portHandler,
+			servoId,
+			ADDR_SCS_PRESENT_POSITION
+		);
+
+		if (result !== COMM_SUCCESS) {
+			throw new Error(
+				`Error reading position from servo ${servoId}: ${packetHandler.getTxRxResult(
+					result
+				)}, Error code: ${error}`
+			);
+		}
+		return position & 0xffff; // Ensure it's within 16 bits
+	} catch (err) {
+		console.error(`Exception reading position from servo ${servoId}:`, err);
+		throw new Error(`Exception reading position from servo ${servoId}: ${err.message}`);
+	}
+}
+
+/**
+ * Reads the current baud rate index of a servo.
+ * @param {number} servoId - The ID of the servo (1-252).
+ * @returns {Promise<number>} Resolves with the baud rate index (0-7).
+ * @throws {Error} If not connected, read fails, or an exception occurs.
+ */
+export async function readBaudRate(servoId) {
+	checkConnection();
+	try {
+		const [baudIndex, result, error] = await packetHandler.read1ByteTxRx(
+			portHandler,
+			servoId,
+			ADDR_SCS_BAUD_RATE
+		);
+
+		if (result !== COMM_SUCCESS) {
+			throw new Error(
+				`Error reading baud rate from servo ${servoId}: ${packetHandler.getTxRxResult(
+					result
+				)}, Error code: ${error}`
+			);
+		}
+		return baudIndex;
+	} catch (err) {
+		console.error(`Exception reading baud rate from servo ${servoId}:`, err);
+		throw new Error(`Exception reading baud rate from servo ${servoId}: ${err.message}`);
+	}
+}
+
+/**
+ * Reads the current operating mode of a servo.
+ * @param {number} servoId - The ID of the servo (1-252).
+ * @returns {Promise<number>} Resolves with the mode (0 for position, 1 for wheel).
+ * @throws {Error} If not connected, read fails, or an exception occurs.
+ */
+export async function readMode(servoId) {
+	checkConnection();
+	try {
+		const [modeValue, result, error] = await packetHandler.read1ByteTxRx(
+			portHandler,
+			servoId,
+			ADDR_SCS_MODE
+		);
+
+		if (result !== COMM_SUCCESS) {
+			throw new Error(
+				`Error reading mode from servo ${servoId}: ${packetHandler.getTxRxResult(
+					result
+				)}, Error code: ${error}`
+			);
+		}
+		return modeValue;
+	} catch (err) {
+		console.error(`Exception reading mode from servo ${servoId}:`, err);
+		throw new Error(`Exception reading mode from servo ${servoId}: ${err.message}`);
+	}
+}
+
+/**
+ * Writes a target position to a servo.
+ * @param {number} servoId - The ID of the servo (1-252).
+ * @param {number} position - The target position value (0-4095).
+ * @returns {Promise<"success">} Resolves with "success".
+ * @throws {Error} If not connected, position is out of range, write fails, or an exception occurs.
+ */
+export async function writePosition(servoId, position) {
+	checkConnection();
+	try {
+		// Validate position range
+		if (position < 0 || position > 4095) {
+			throw new Error(
+				`Invalid position value ${position} for servo ${servoId}. Must be between 0 and 4095.`
+			);
+		}
+		const targetPosition = Math.round(position); // Ensure integer value
+
+		const [result, error] = await packetHandler.write2ByteTxRx(
+			portHandler,
+			servoId,
+			ADDR_SCS_GOAL_POSITION,
+			targetPosition
+		);
+
+		if (result !== COMM_SUCCESS) {
+			throw new Error(
+				`Error writing position to servo ${servoId}: ${packetHandler.getTxRxResult(
+					result
+				)}, Error code: ${error}`
+			);
+		}
+		return "success";
+	} catch (err) {
+		console.error(`Exception writing position to servo ${servoId}:`, err);
+		// Re-throw the original error or a new one wrapping it
+		throw new Error(`Failed to write position to servo ${servoId}: ${err.message}`);
+	}
+}
+
+/**
+ * Enables or disables the torque of a servo.
+ * @param {number} servoId - The ID of the servo (1-252).
+ * @param {boolean} enable - True to enable torque, false to disable.
+ * @returns {Promise<"success">} Resolves with "success".
+ * @throws {Error} If not connected, write fails, or an exception occurs.
+ */
+export async function writeTorqueEnable(servoId, enable) {
+	checkConnection();
+	try {
+		const enableValue = enable ? 1 : 0;
+		const [result, error] = await packetHandler.write1ByteTxRx(
+			portHandler,
+			servoId,
+			ADDR_SCS_TORQUE_ENABLE,
+			enableValue
+		);
+
+		if (result !== COMM_SUCCESS) {
+			throw new Error(
+				`Error setting torque for servo ${servoId}: ${packetHandler.getTxRxResult(
+					result
+				)}, Error code: ${error}`
+			);
+		}
+		return "success";
+	} catch (err) {
+		console.error(`Exception setting torque for servo ${servoId}:`, err);
+		throw new Error(`Exception setting torque for servo ${servoId}: ${err.message}`);
+	}
+}
+
+/**
+ * Sets the acceleration profile for a servo's movement.
+ * @param {number} servoId - The ID of the servo (1-252).
+ * @param {number} acceleration - The acceleration value (0-254).
+ * @returns {Promise<"success">} Resolves with "success".
+ * @throws {Error} If not connected, write fails, or an exception occurs.
+ */
+export async function writeAcceleration(servoId, acceleration) {
+	checkConnection();
+	try {
+		const clampedAcceleration = Math.max(0, Math.min(254, Math.round(acceleration)));
+		const [result, error] = await packetHandler.write1ByteTxRx(
+			portHandler,
+			servoId,
+			ADDR_SCS_GOAL_ACC,
+			clampedAcceleration
+		);
+
+		if (result !== COMM_SUCCESS) {
+			throw new Error(
+				`Error writing acceleration to servo ${servoId}: ${packetHandler.getTxRxResult(
+					result
+				)}, Error code: ${error}`
+			);
+		}
+		return "success";
+	} catch (err) {
+		console.error(`Exception writing acceleration to servo ${servoId}:`, err);
+		throw new Error(`Exception writing acceleration to servo ${servoId}: ${err.message}`);
+	}
+}
+
+/**
+ * Helper to attempt locking a servo, logging errors without throwing.
+ * @param {number} servoId
+ */
+async function tryLockServo(servoId) {
+	try {
+		await packetHandler.write1ByteTxRx(portHandler, servoId, ADDR_SCS_LOCK, 1);
+	} catch (lockErr) {
+		console.error(`Failed to re-lock servo ${servoId}:`, lockErr);
+	}
+}
+
+/**
+ * Sets a servo to wheel mode (continuous rotation).
+ * Requires unlocking, setting mode, and locking the configuration.
+ * @param {number} servoId - The ID of the servo (1-252).
+ * @returns {Promise<"success">} Resolves with "success".
+ * @throws {Error} If not connected, any step fails, or an exception occurs.
+ */
+export async function setWheelMode(servoId) {
+	checkConnection();
+	let unlocked = false;
+	try {
+		console.log(`Setting servo ${servoId} to wheel mode...`);
+
+		// 1. Unlock servo configuration
+		const [resUnlock, errUnlock] = await packetHandler.write1ByteTxRx(
+			portHandler,
+			servoId,
+			ADDR_SCS_LOCK,
+			0
+		);
+		if (resUnlock !== COMM_SUCCESS) {
+			throw new Error(
+				`Failed to unlock servo ${servoId}: ${packetHandler.getTxRxResult(
+					resUnlock
+				)}, Error: ${errUnlock}`
+			);
+		}
+		unlocked = true;
+
+		// 2. Set mode to 1 (Wheel/Speed mode)
+		const [resMode, errMode] = await packetHandler.write1ByteTxRx(
+			portHandler,
+			servoId,
+			ADDR_SCS_MODE,
+			1
+		);
+		if (resMode !== COMM_SUCCESS) {
+			throw new Error(
+				`Failed to set wheel mode for servo ${servoId}: ${packetHandler.getTxRxResult(
+					resMode
+				)}, Error: ${errMode}`
+			);
+		}
+
+		// 3. Lock servo configuration
+		const [resLock, errLock] = await packetHandler.write1ByteTxRx(
+			portHandler,
+			servoId,
+			ADDR_SCS_LOCK,
+			1
+		);
+		if (resLock !== COMM_SUCCESS) {
+			// Mode was set, but lock failed. Still an error state.
+			throw new Error(
+				`Failed to lock servo ${servoId} after setting mode: ${packetHandler.getTxRxResult(
+					resLock
+				)}, Error: ${errLock}`
+			);
+		}
+		unlocked = false; // Successfully locked
+
+		console.log(`Successfully set servo ${servoId} to wheel mode.`);
+		return "success";
+	} catch (err) {
+		console.error(`Exception setting wheel mode for servo ${servoId}:`, err);
+		if (unlocked) {
+			// Attempt to re-lock if an error occurred after unlocking
+			await tryLockServo(servoId);
+		}
+		// Re-throw the original error or a new one wrapping it
+		throw new Error(`Failed to set wheel mode for servo ${servoId}: ${err.message}`);
+	}
+}
+
+/**
+ * Sets a servo back to position control mode from wheel mode.
+ * Requires unlocking, setting mode, and locking the configuration.
+ * @param {number} servoId - The ID of the servo (1-252).
+ * @returns {Promise<"success">} Resolves with "success".
+ * @throws {Error} If not connected, any step fails, or an exception occurs.
+ */
+export async function setPositionMode(servoId) {
+	checkConnection();
+	let unlocked = false;
+	try {
+		console.log(`Setting servo ${servoId} back to position mode...`);
+
+		// 1. Unlock servo configuration
+		const [resUnlock, errUnlock] = await packetHandler.write1ByteTxRx(
+			portHandler,
+			servoId,
+			ADDR_SCS_LOCK,
+			0
+		);
+		if (resUnlock !== COMM_SUCCESS) {
+			throw new Error(
+				`Failed to unlock servo ${servoId}: ${packetHandler.getTxRxResult(
+					resUnlock
+				)}, Error: ${errUnlock}`
+			);
+		}
+		unlocked = true;
+
+		// 2. Set mode to 0 (Position/Servo mode)
+		const [resMode, errMode] = await packetHandler.write1ByteTxRx(
+			portHandler,
+			servoId,
+			ADDR_SCS_MODE,
+			0 // 0 for position mode
+		);
+		if (resMode !== COMM_SUCCESS) {
+			throw new Error(
+				`Failed to set position mode for servo ${servoId}: ${packetHandler.getTxRxResult(
+					resMode
+				)}, Error: ${errMode}`
+			);
+		}
+
+		// 3. Lock servo configuration
+		const [resLock, errLock] = await packetHandler.write1ByteTxRx(
+			portHandler,
+			servoId,
+			ADDR_SCS_LOCK,
+			1
+		);
+		if (resLock !== COMM_SUCCESS) {
+			throw new Error(
+				`Failed to lock servo ${servoId} after setting mode: ${packetHandler.getTxRxResult(
+					resLock
+				)}, Error: ${errLock}`
+			);
+		}
+		unlocked = false; // Successfully locked
+
+		console.log(`Successfully set servo ${servoId} back to position mode.`);
+		return "success";
+	} catch (err) {
+		console.error(`Exception setting position mode for servo ${servoId}:`, err);
+		if (unlocked) {
+			// Attempt to re-lock if an error occurred after unlocking
+			await tryLockServo(servoId);
+		}
+		throw new Error(`Failed to set position mode for servo ${servoId}: ${err.message}`);
+	}
+}
+
+/**
+ * Writes a target speed for a servo in wheel mode.
+ * @param {number} servoId - The ID of the servo
+ * @param {number} speed - The target speed value (-10000 to 10000). Negative values indicate reverse direction. 0 stops the wheel.
+ * @returns {Promise<"success">} Resolves with "success".
+ * @throws {Error} If not connected, either write fails, or an exception occurs.
+ */
+export async function writeWheelSpeed(servoId, speed) {
+	checkConnection();
+	try {
+		// Validate and clamp the speed to the new range
+		const clampedSpeed = Math.max(-10000, Math.min(10000, Math.round(speed)));
+		let speedValue = Math.abs(clampedSpeed) & 0x7fff; // Get absolute value, ensure within 15 bits
+
+		// Set the direction bit (MSB of the 16-bit value) if speed is negative
+		if (clampedSpeed < 0) {
+			speedValue |= 0x8000; // Set the 16th bit for reverse direction
+		}
+
+		// Use write2ByteTxRx to write the 16-bit speed value
+		const [result, error] = await packetHandler.write2ByteTxRx(
+			portHandler,
+			servoId,
+			ADDR_SCS_GOAL_SPEED, // Starting address for the 2-byte speed value
+			speedValue
+		);
+
+		if (result !== COMM_SUCCESS) {
+			throw new Error(
+				`Error writing wheel speed to servo ${servoId}: ${packetHandler.getTxRxResult(
+					result
+				)}, Error: ${error}`
+			);
+		}
+
+		return "success";
+	} catch (err) {
+		console.error(`Exception writing wheel speed to servo ${servoId}:`, err);
+		throw new Error(`Exception writing wheel speed to servo ${servoId}: ${err.message}`);
+	}
+}
+
+/**
+ * Writes target speeds to multiple servos in wheel mode synchronously.
+ * @param {Map<number, number> | object} servoSpeeds - A Map or object where keys are servo IDs (1-252) and values are target speeds (-10000 to 10000).
+ * @returns {Promise<"success">} Resolves with "success".
+ * @throws {Error} If not connected, any speed is out of range, transmission fails, or an exception occurs.
+ */
+export async function syncWriteWheelSpeed(servoSpeeds) {
+	checkConnection();
+
+	const groupSyncWrite = new GroupSyncWrite(
+		portHandler,
+		packetHandler,
+		ADDR_SCS_GOAL_SPEED,
+		2 // Data length for speed (2 bytes)
+	);
+	let paramAdded = false;
+
+	const entries = servoSpeeds instanceof Map ? servoSpeeds.entries() : Object.entries(servoSpeeds);
+
+	// Second pass: Add valid parameters
+	for (const [idStr, speed] of entries) {
+		const servoId = parseInt(idStr, 10); // Already validated
+
+		if (isNaN(servoId) || servoId < 1 || servoId > 252) {
+			throw new Error(`Invalid servo ID "${idStr}" in syncWriteWheelSpeed.`);
+		}
+		if (speed < -10000 || speed > 10000) {
+			throw new Error(
+				`Invalid speed value ${speed} for servo ${servoId} in syncWriteWheelSpeed. Must be between -10000 and 10000.`
+			);
+		}
+
+		const clampedSpeed = Math.max(-10000, Math.min(10000, Math.round(speed))); // Ensure integer, already validated range
+		let speedValue = Math.abs(clampedSpeed) & 0x7fff; // Get absolute value, ensure within 15 bits
+
+		// Set the direction bit (MSB of the 16-bit value) if speed is negative
+		if (clampedSpeed < 0) {
+			speedValue |= 0x8000; // Set the 16th bit for reverse direction
+		}
+
+		const data = [SCS_LOBYTE(speedValue), SCS_HIBYTE(speedValue)];
+
+		if (groupSyncWrite.addParam(servoId, data)) {
+			paramAdded = true;
+		} else {
+			// This should ideally not happen if IDs are unique, but handle defensively
+			console.warn(
+				`Failed to add servo ${servoId} to sync write speed group (possibly duplicate).`
+			);
+		}
+	}
+
+	if (!paramAdded) {
+		console.log("Sync Write Speed: No valid servo speeds provided or added.");
+		return "success"; // Nothing to write is considered success
+	}
+
+	try {
+		// Send the Sync Write instruction
+		const result = await groupSyncWrite.txPacket();
+		if (result !== COMM_SUCCESS) {
+			throw new Error(`Sync Write Speed txPacket failed: ${packetHandler.getTxRxResult(result)}`);
+		}
+		return "success";
+	} catch (err) {
+		console.error("Exception during syncWriteWheelSpeed:", err);
+		// Re-throw the original error or a new one wrapping it
+		throw new Error(`Sync Write Speed failed: ${err.message}`);
+	}
+}
+
+/**
+ * Reads the current position of multiple servos synchronously.
+ * @param {number[]} servoIds - An array of servo IDs (1-252) to read from.
+ * @returns {Promise<Map<number, number>>} Resolves with a Map where keys are servo IDs and values are positions (0-4095).
+ * @throws {Error} If not connected, transmission fails, reception fails, or data for any requested servo is unavailable.
+ */
+export async function syncReadPositions(servoIds) {
+	checkConnection();
+	if (!Array.isArray(servoIds) || servoIds.length === 0) {
+		console.log("Sync Read: No servo IDs provided.");
+		return new Map(); // Return empty map for empty input
+	}
+
+	const startAddress = ADDR_SCS_PRESENT_POSITION;
+	const dataLength = 2;
+	const groupSyncRead = new GroupSyncRead(portHandler, packetHandler, startAddress, dataLength);
+	const positions = new Map();
+	const validIds = [];
+
+	// 1. Add parameters for each valid servo ID
+	servoIds.forEach((id) => {
+		if (id >= 1 && id <= 252) {
+			if (groupSyncRead.addParam(id)) {
+				validIds.push(id);
+			} else {
+				console.warn(
+					`Sync Read: Failed to add param for servo ID ${id} (maybe duplicate or invalid).`
+				);
+			}
+		} else {
+			console.warn(`Sync Read: Invalid servo ID ${id} skipped.`);
+		}
+	});
+
+	if (validIds.length === 0) {
+		console.log("Sync Read: No valid servo IDs to read.");
+		return new Map(); // Return empty map if no valid IDs
+	}
+
+	try {
+		// 2. Send the Sync Read instruction packet
+		let txResult = await groupSyncRead.txPacket();
+		if (txResult !== COMM_SUCCESS) {
+			throw new Error(`Sync Read txPacket failed: ${packetHandler.getTxRxResult(txResult)}`);
+		}
+
+		// 3. Receive the response packets
+		let rxResult = await groupSyncRead.rxPacket();
+		// Even if rxPacket reports an overall issue (like timeout), we still check individual servos.
+		// A specific error will be thrown later if any servo data is missing.
+		if (rxResult !== COMM_SUCCESS) {
+			console.warn(
+				`Sync Read rxPacket overall result: ${packetHandler.getTxRxResult(
+					rxResult
+				)}. Checking individual servos.`
+			);
+		}
+
+		// 4. Check data availability and retrieve data for each servo
+		const failedIds = [];
+		validIds.forEach((id) => {
+			const isAvailable = groupSyncRead.isAvailable(id, startAddress, dataLength);
+			if (isAvailable) {
+				const position = groupSyncRead.getData(id, startAddress, dataLength);
+				positions.set(id, position & 0xffff);
+			} else {
+				failedIds.push(id);
+			}
+		});
+
+		// 5. Check if all requested servos responded
+		if (failedIds.length > 0) {
+			throw new Error(
+				`Sync Read failed: Data not available for servo IDs: ${failedIds.join(
+					", "
+				)}. Overall RX result: ${packetHandler.getTxRxResult(rxResult)}`
+			);
+		}
+
+		return positions;
+	} catch (err) {
+		console.error("Exception or failure during syncReadPositions:", err);
+		// Re-throw the caught error or a new one wrapping it
+		throw new Error(`Sync Read failed: ${err.message}`);
+	}
+}
+
+/**
+ * Writes target positions to multiple servos synchronously.
+ * @param {Map<number, number> | object} servoPositions - A Map or object where keys are servo IDs (1-252) and values are target positions (0-4095).
+ * @returns {Promise<"success">} Resolves with "success".
+ * @throws {Error} If not connected, any position is out of range, transmission fails, or an exception occurs.
+ */
+export async function syncWritePositions(servoPositions) {
+	checkConnection();
+
+	const groupSyncWrite = new GroupSyncWrite(
+		portHandler,
+		packetHandler,
+		ADDR_SCS_GOAL_POSITION,
+		2 // Data length for position
+	);
+	let paramAdded = false;
+
+	const entries =
+		servoPositions instanceof Map ? servoPositions.entries() : Object.entries(servoPositions);
+
+	// Second pass: Add valid parameters
+	for (const [idStr, position] of entries) {
+		const servoId = parseInt(idStr, 10); // Already validated
+		if (isNaN(servoId) || servoId < 1 || servoId > 252) {
+			throw new Error(`Invalid servo ID "${idStr}" in syncWritePositions.`);
+		}
+		if (position < 0 || position > 4095) {
+			throw new Error(
+				`Invalid position value ${position} for servo ${servoId} in syncWritePositions. Must be between 0 and 4095.`
+			);
+		}
+		const targetPosition = Math.round(position); // Ensure integer, already validated range
+		const data = [SCS_LOBYTE(targetPosition), SCS_HIBYTE(targetPosition)];
+
+		if (groupSyncWrite.addParam(servoId, data)) {
+			paramAdded = true;
+		} else {
+			// This should ideally not happen if IDs are unique, but handle defensively
+			console.warn(`Failed to add servo ${servoId} to sync write group (possibly duplicate).`);
+		}
+	}
+
+	if (!paramAdded) {
+		console.log("Sync Write: No valid servo positions provided or added.");
+		return "success"; // Nothing to write is considered success
+	}
+
+	try {
+		// Send the Sync Write instruction
+		const result = await groupSyncWrite.txPacket();
+		if (result !== COMM_SUCCESS) {
+			throw new Error(`Sync Write txPacket failed: ${packetHandler.getTxRxResult(result)}`);
+		}
+		return "success";
+	} catch (err) {
+		console.error("Exception during syncWritePositions:", err);
+		// Re-throw the original error or a new one wrapping it
+		throw new Error(`Sync Write failed: ${err.message}`);
+	}
+}
+
+/**
+ * Sets the Baud Rate of a servo.
+ * NOTE: After changing the baud rate, you might need to disconnect and reconnect
+ *       at the new baud rate to communicate with the servo further.
+ * @param {number} servoId - The current ID of the servo to configure (1-252).
+ * @param {number} baudRateIndex - The index representing the new baud rate (0-7).
+ * @returns {Promise<"success">} Resolves with "success".
+ * @throws {Error} If not connected, input is invalid, any step fails, or an exception occurs.
+ */
+export async function setBaudRate(servoId, baudRateIndex) {
+	checkConnection();
+
+	// Validate inputs
+	if (servoId < 1 || servoId > 252) {
+		throw new Error(`Invalid servo ID provided: ${servoId}. Must be between 1 and 252.`);
+	}
+	if (baudRateIndex < 0 || baudRateIndex > 7) {
+		throw new Error(`Invalid baudRateIndex: ${baudRateIndex}. Must be between 0 and 7.`);
+	}
+
+	let unlocked = false;
+	try {
+		console.log(`Setting baud rate for servo ${servoId}: Index=${baudRateIndex}`);
+
+		// 1. Unlock servo configuration
+		const [resUnlock, errUnlock] = await packetHandler.write1ByteTxRx(
+			portHandler,
+			servoId,
+			ADDR_SCS_LOCK,
+			0 // 0 to unlock
+		);
+		if (resUnlock !== COMM_SUCCESS) {
+			throw new Error(
+				`Failed to unlock servo ${servoId}: ${packetHandler.getTxRxResult(
+					resUnlock
+				)}, Error: ${errUnlock}`
+			);
+		}
+		unlocked = true;
+
+		// 2. Write new Baud Rate index
+		const [resBaud, errBaud] = await packetHandler.write1ByteTxRx(
+			portHandler,
+			servoId,
+			ADDR_SCS_BAUD_RATE,
+			baudRateIndex
+		);
+		if (resBaud !== COMM_SUCCESS) {
+			throw new Error(
+				`Failed to write baud rate index ${baudRateIndex} to servo ${servoId}: ${packetHandler.getTxRxResult(
+					resBaud
+				)}, Error: ${errBaud}`
+			);
+		}
+
+		// 3. Lock servo configuration
+		const [resLock, errLock] = await packetHandler.write1ByteTxRx(
+			portHandler,
+			servoId,
+			ADDR_SCS_LOCK,
+			1
+		);
+		if (resLock !== COMM_SUCCESS) {
+			throw new Error(
+				`Failed to lock servo ${servoId} after setting baud rate: ${packetHandler.getTxRxResult(
+					resLock
+				)}, Error: ${errLock}.`
+			);
+		}
+		unlocked = false; // Successfully locked
+
+		console.log(
+			`Successfully set baud rate for servo ${servoId}. Index: ${baudRateIndex}. Remember to potentially reconnect with the new baud rate.`
+		);
+		return "success";
+	} catch (err) {
+		console.error(`Exception during setBaudRate for servo ID ${servoId}:`, err);
+		if (unlocked) {
+			await tryLockServo(servoId);
+		}
+		throw new Error(`Failed to set baud rate for servo ${servoId}: ${err.message}`);
+	}
+}
+
+/**
+ * Sets the ID of a servo.
+ * NOTE: Changing the ID requires using the new ID for subsequent commands.
+ * @param {number} currentServoId - The current ID of the servo to configure (1-252).
+ * @param {number} newServoId - The new ID to set for the servo (1-252).
+ * @returns {Promise<"success">} Resolves with "success".
+ * @throws {Error} If not connected, input is invalid, any step fails, or an exception occurs.
+ */
+export async function setServoId(currentServoId, newServoId) {
+	checkConnection();
+
+	// Validate inputs
+	if (currentServoId < 1 || currentServoId > 252 || newServoId < 1 || newServoId > 252) {
+		throw new Error(
+			`Invalid servo ID provided. Current: ${currentServoId}, New: ${newServoId}. Must be between 1 and 252.`
+		);
+	}
+
+	if (currentServoId === newServoId) {
+		console.log(`Servo ID is already ${newServoId}. No change needed.`);
+		return "success";
+	}
+
+	let unlocked = false;
+	let idWritten = false;
+	try {
+		console.log(`Setting servo ID: From ${currentServoId} to ${newServoId}`);
+
+		// 1. Unlock servo configuration (using current ID)
+		const [resUnlock, errUnlock] = await packetHandler.write1ByteTxRx(
+			portHandler,
+			currentServoId,
+			ADDR_SCS_LOCK,
+			0 // 0 to unlock
+		);
+		if (resUnlock !== COMM_SUCCESS) {
+			throw new Error(
+				`Failed to unlock servo ${currentServoId}: ${packetHandler.getTxRxResult(
+					resUnlock
+				)}, Error: ${errUnlock}`
+			);
+		}
+		unlocked = true;
+
+		// 2. Write new Servo ID (using current ID)
+		const [resId, errId] = await packetHandler.write1ByteTxRx(
+			portHandler,
+			currentServoId,
+			ADDR_SCS_ID,
+			newServoId
+		);
+		if (resId !== COMM_SUCCESS) {
+			throw new Error(
+				`Failed to write new ID ${newServoId} to servo ${currentServoId}: ${packetHandler.getTxRxResult(
+					resId
+				)}, Error: ${errId}`
+			);
+		}
+		idWritten = true;
+
+		// 3. Lock servo configuration (using NEW ID)
+		const [resLock, errLock] = await packetHandler.write1ByteTxRx(
+			portHandler,
+			newServoId, // Use NEW ID here
+			ADDR_SCS_LOCK,
+			1 // 1 to lock
+		);
+		if (resLock !== COMM_SUCCESS) {
+			// ID was likely changed, but lock failed. Critical state.
+			throw new Error(
+				`Failed to lock servo with new ID ${newServoId}: ${packetHandler.getTxRxResult(
+					resLock
+				)}, Error: ${errLock}. Configuration might be incomplete.`
+			);
+		}
+		unlocked = false; // Successfully locked with new ID
+
+		console.log(
+			`Successfully set servo ID from ${currentServoId} to ${newServoId}. Remember to use the new ID for future commands.`
+		);
+		return "success";
+	} catch (err) {
+		console.error(`Exception during setServoId for current ID ${currentServoId}:`, err);
+		if (unlocked) {
+			// If unlock succeeded but subsequent steps failed, attempt to re-lock.
+			// If ID write failed, use current ID. If ID write succeeded but lock failed, use new ID.
+			const idToLock = idWritten ? newServoId : currentServoId;
+			console.warn(`Attempting to re-lock servo using ID ${idToLock}...`);
+			await tryLockServo(idToLock);
+		}
+		throw new Error(
+			`Failed to set servo ID from ${currentServoId} to ${newServoId}: ${err.message}`
+		);
+	}
+}

packages/feetech.js/scsServoSDKUnlock.mjs

@@ -0,0 +1,217 @@
+/**
+ * Read-Only Servo SDK for USB Master
+ * Simplified version that only reads positions without any locking
+ */
+
+import { PortHandler, PacketHandler, COMM_SUCCESS, GroupSyncRead } from "./lowLevelSDK.mjs";
+
+import { ADDR_SCS_PRESENT_POSITION } from "./scsservo_constants.mjs";
+
+// Module-level variables for handlers
+let portHandler = null;
+let packetHandler = null;
+
+/**
+ * Connects to the serial port and initializes handlers.
+ * @param {object} [options] - Connection options.
+ * @param {number} [options.baudRate=1000000] - The baud rate for the serial connection.
+ * @param {number} [options.protocolEnd=0] - The protocol end setting (0 for STS/SMS, 1 for SCS).
+ * @returns {Promise<true>} Resolves with true on successful connection.
+ * @throws {Error} If connection fails or port cannot be opened/selected.
+ */
+export async function connect(options = {}) {
+	if (portHandler && portHandler.isOpen) {
+		console.log("🔓 Already connected to USB robot (read-only mode).");
+		return true;
+	}
+
+	const { baudRate = 1000000, protocolEnd = 0 } = options;
+
+	try {
+		portHandler = new PortHandler();
+		const portRequested = await portHandler.requestPort();
+		if (!portRequested) {
+			portHandler = null;
+			throw new Error("Failed to select a serial port.");
+		}
+
+		portHandler.setBaudRate(baudRate);
+		const portOpened = await portHandler.openPort();
+		if (!portOpened) {
+			await portHandler.closePort().catch(console.error);
+			portHandler = null;
+			throw new Error(`Failed to open port at baudrate ${baudRate}.`);
+		}
+
+		packetHandler = new PacketHandler(protocolEnd);
+		console.log(
+			`🔓 Connected to USB robot (read-only mode) at ${baudRate} baud, protocol end: ${protocolEnd}.`
+		);
+		return true;
+	} catch (err) {
+		console.error("Error during USB robot connection:", err);
+		if (portHandler) {
+			try {
+				await portHandler.closePort();
+			} catch (closeErr) {
+				console.error("Error closing port after connection failure:", closeErr);
+			}
+		}
+		portHandler = null;
+		packetHandler = null;
+		throw new Error(`USB robot connection failed: ${err.message}`);
+	}
+}
+
+/**
+ * Disconnects from the serial port.
+ * @returns {Promise<true>} Resolves with true on successful disconnection.
+ * @throws {Error} If disconnection fails.
+ */
+export async function disconnect() {
+	if (!portHandler || !portHandler.isOpen) {
+		console.log("Already disconnected from USB robot.");
+		return true;
+	}
+
+	try {
+		await portHandler.closePort();
+		portHandler = null;
+		packetHandler = null;
+		console.log("🔓 Disconnected from USB robot (read-only mode).");
+		return true;
+	} catch (err) {
+		console.error("Error during USB robot disconnection:", err);
+		portHandler = null;
+		packetHandler = null;
+		throw new Error(`USB robot disconnection failed: ${err.message}`);
+	}
+}
+
+/**
+ * Checks if the SDK is connected. Throws an error if not.
+ * @throws {Error} If not connected.
+ */
+function checkConnection() {
+	if (!portHandler || !packetHandler) {
+		throw new Error("Not connected to USB robot. Call connect() first.");
+	}
+}
+
+/**
+ * Reads the current position of a servo.
+ * @param {number} servoId - The ID of the servo (1-252).
+ * @returns {Promise<number>} Resolves with the position (0-4095).
+ * @throws {Error} If not connected, read fails, or an exception occurs.
+ */
+export async function readPosition(servoId) {
+	checkConnection();
+	try {
+		const [position, result, error] = await packetHandler.read2ByteTxRx(
+			portHandler,
+			servoId,
+			ADDR_SCS_PRESENT_POSITION
+		);
+
+		if (result !== COMM_SUCCESS) {
+			throw new Error(
+				`Error reading position from servo ${servoId}: ${packetHandler.getTxRxResult(
+					result
+				)}, Error code: ${error}`
+			);
+		}
+		return position & 0xffff;
+	} catch (err) {
+		console.error(`Exception reading position from servo ${servoId}:`, err);
+		throw new Error(`Exception reading position from servo ${servoId}: ${err.message}`);
+	}
+}
+
+/**
+ * Reads the current position of multiple servos synchronously.
+ * Returns positions for all servos that respond, skipping failed ones gracefully.
+ * @param {number[]} servoIds - An array of servo IDs (1-252) to read from.
+ * @returns {Promise<Map<number, number>>} Resolves with a Map where keys are servo IDs and values are positions (0-4095).
+ * @throws {Error} If not connected or transmission fails completely.
+ */
+export async function syncReadPositions(servoIds) {
+	checkConnection();
+	if (!Array.isArray(servoIds) || servoIds.length === 0) {
+		console.log("Sync Read: No servo IDs provided.");
+		return new Map();
+	}
+
+	const startAddress = ADDR_SCS_PRESENT_POSITION;
+	const dataLength = 2;
+	const groupSyncRead = new GroupSyncRead(portHandler, packetHandler, startAddress, dataLength);
+	const positions = new Map();
+	const validIds = [];
+
+	// Add parameters for each valid servo ID
+	servoIds.forEach((id) => {
+		if (id >= 1 && id <= 252) {
+			if (groupSyncRead.addParam(id)) {
+				validIds.push(id);
+			} else {
+				console.warn(
+					`Sync Read: Failed to add param for servo ID ${id} (maybe duplicate or invalid).`
+				);
+			}
+		} else {
+			console.warn(`Sync Read: Invalid servo ID ${id} skipped.`);
+		}
+	});
+
+	if (validIds.length === 0) {
+		console.log("Sync Read: No valid servo IDs to read.");
+		return new Map();
+	}
+
+	try {
+		// Send the Sync Read instruction packet
+		let txResult = await groupSyncRead.txPacket();
+		if (txResult !== COMM_SUCCESS) {
+			throw new Error(`Sync Read txPacket failed: ${packetHandler.getTxRxResult(txResult)}`);
+		}
+
+		// Receive the response packets
+		let rxResult = await groupSyncRead.rxPacket();
+		if (rxResult !== COMM_SUCCESS) {
+			console.warn(
+				`Sync Read rxPacket overall result: ${packetHandler.getTxRxResult(
+					rxResult
+				)}. Checking individual servos.`
+			);
+		}
+
+		// Check data availability and retrieve data for each servo
+		const failedIds = [];
+		validIds.forEach((id) => {
+			const isAvailable = groupSyncRead.isAvailable(id, startAddress, dataLength);
+			if (isAvailable) {
+				const position = groupSyncRead.getData(id, startAddress, dataLength);
+				const finalPosition = position & 0xffff;
+				console.log(
+					`🔍 Debug Servo ${id}: raw=${position}, final=${finalPosition}, hex=${position.toString(16)}`
+				);
+				positions.set(id, finalPosition);
+			} else {
+				failedIds.push(id);
+			}
+		});
+
+		// Log failed servos but don't throw error - return available data
+		if (failedIds.length > 0) {
+			console.warn(
+				`Sync Read: Data not available for servo IDs: ${failedIds.join(
+					", "
+				)}. Got ${positions.size}/${validIds.length} servos successfully.`
+			);
+		}
+
+		return positions;
+	} catch (err) {
+		console.error("Exception during syncReadPositions:", err);
+		throw new Error(`Sync Read failed: ${err.message}`);
+	}
+}

{src/lib → packages}/feetech.js/scsservo_constants.mjs

@@ -1,8 +1,8 @@
 // Constants for FeetTech SCS servos
 
 // Constants
-export const BROADCAST_ID = 0xFE;  // 254
-export const MAX_ID = 0xFC;  // 252
+export const BROADCAST_ID = 0xfe; // 254
+export const MAX_ID = 0xfc; // 252
 
 // Protocol instructions
 export const INST_PING = 1;
@@ -10,19 +10,19 @@ export const INST_READ = 2;
 export const INST_WRITE = 3;
 export const INST_REG_WRITE = 4;
 export const INST_ACTION = 5;
-export const INST_SYNC_WRITE = 131;  // 0x83
-export const INST_SYNC_READ = 130;  // 0x82
-export const INST_STATUS = 85;  // 0x55, status packet instruction (0x55)
+export const INST_SYNC_WRITE = 131; // 0x83
+export const INST_SYNC_READ = 130; // 0x82
+export const INST_STATUS = 85; // 0x55, status packet instruction (0x55)
 
 // Communication results
-export const COMM_SUCCESS = 0;      // tx or rx packet communication success
-export const COMM_PORT_BUSY = -1;   // Port is busy (in use)
-export const COMM_TX_FAIL = -2;     // Failed transmit instruction packet
-export const COMM_RX_FAIL = -3;     // Failed get status packet
-export const COMM_TX_ERROR = -4;    // Incorrect instruction packet
-export const COMM_RX_WAITING = -5;  // Now receiving status packet
-export const COMM_RX_TIMEOUT = -6;  // There is no status packet
-export const COMM_RX_CORRUPT = -7;  // Incorrect status packet
+export const COMM_SUCCESS = 0; // tx or rx packet communication success
+export const COMM_PORT_BUSY = -1; // Port is busy (in use)
+export const COMM_TX_FAIL = -2; // Failed transmit instruction packet
+export const COMM_RX_FAIL = -3; // Failed get status packet
+export const COMM_TX_ERROR = -4; // Incorrect instruction packet
+export const COMM_RX_WAITING = -5; // Now receiving status packet
+export const COMM_RX_TIMEOUT = -6; // There is no status packet
+export const COMM_RX_CORRUPT = -7; // Incorrect status packet
 export const COMM_NOT_AVAILABLE = -9;
 
 // Packet constants
@@ -50,4 +50,4 @@ export const ADDR_SCS_TORQUE_ENABLE = 40;
 export const ADDR_SCS_GOAL_ACC = 41;
 export const ADDR_SCS_GOAL_POSITION = 42;
 export const ADDR_SCS_GOAL_SPEED = 46;
-export const ADDR_SCS_PRESENT_POSITION = 56; 
+export const ADDR_SCS_PRESENT_POSITION = 56;

packages/feetech.js/test.html

@@ -0,0 +1,770 @@
+<!doctype html>
+<html lang="en">
+	<head>
+		<meta charset="UTF-8" />
+		<meta name="viewport" content="width=device-width, initial-scale=1.0" />
+		<title>Feetech Servo Test</title>
+		<style>
+			body {
+				font-family: sans-serif;
+				line-height: 1.6;
+				padding: 20px;
+			}
+			.container {
+				max-width: 800px;
+				margin: auto;
+			}
+			.section {
+				border: 1px solid #ccc;
+				padding: 15px;
+				margin-bottom: 20px;
+				border-radius: 5px;
+			}
+			h2 {
+				margin-top: 0;
+			}
+			label {
+				display: inline-block;
+				min-width: 100px;
+				margin-bottom: 5px;
+			}
+			input[type="number"],
+			input[type="text"] {
+				width: 100px;
+				padding: 5px;
+				margin-right: 10px;
+				margin-bottom: 10px;
+			}
+			button {
+				padding: 8px 15px;
+				margin-right: 10px;
+				cursor: pointer;
+			}
+			pre {
+				background-color: #f4f4f4;
+				padding: 10px;
+				border: 1px solid #ddd;
+				border-radius: 3px;
+				white-space: pre-wrap;
+				word-wrap: break-word;
+			}
+			.status {
+				font-weight: bold;
+			}
+			.success {
+				color: green;
+			}
+			.error {
+				color: red;
+			}
+			.log-area {
+				margin-top: 10px;
+			}
+		</style>
+	</head>
+	<body>
+		<div class="container">
+			<h1>Feetech Servo Test Page</h1>
+
+			<details class="section">
+				<summary>Key Concepts</summary>
+				<p>Understanding these parameters is crucial for controlling Feetech servos:</p>
+				<ul>
+					<li>
+						<strong>Mode:</strong> Determines the servo's primary function.
+						<ul>
+							<li>
+								<code>Mode 0</code>: Position/Servo Mode. The servo moves to and holds a specific
+								angular position.
+							</li>
+							<li>
+								<code>Mode 1</code>: Wheel/Speed Mode. The servo rotates continuously at a specified
+								speed and direction, like a motor.
+							</li>
+						</ul>
+						Changing the mode requires unlocking, writing the mode value (0 or 1), and locking the
+						configuration.
+					</li>
+					<li>
+						<strong>Position:</strong> In Position Mode (Mode 0), this value represents the target
+						or current angular position of the servo's output shaft.
+						<ul>
+							<li>
+								Range: Typically <code>0</code> to <code>4095</code> (representing a 12-bit
+								resolution).
+							</li>
+							<li>
+								Meaning: Corresponds to the servo's rotational range (e.g., 0-360 degrees or 0-270
+								degrees, depending on the specific servo model). <code>0</code> is one end of the
+								range, <code>4095</code> is the other.
+							</li>
+						</ul>
+					</li>
+					<li>
+						<strong>Speed (Wheel Mode):</strong> In Wheel Mode (Mode 1), this value controls the
+						rotational speed and direction.
+						<ul>
+							<li>
+								Range: Typically <code>-2500</code> to <code>+2500</code>. (Note: Some documentation
+								might mention -1023 to +1023, but the SDK example uses a wider range).
+							</li>
+							<li>
+								Meaning: <code>0</code> stops the wheel. Positive values rotate in one direction
+								(e.g., clockwise), negative values rotate in the opposite direction (e.g.,
+								counter-clockwise). The magnitude determines the speed (larger absolute value means
+								faster rotation).
+							</li>
+							<li>Control Address: <code>ADDR_SCS_GOAL_SPEED</code> (Register 46/47).</li>
+						</ul>
+					</li>
+					<li>
+						<strong>Acceleration:</strong> Controls how quickly the servo changes speed to reach its
+						target position (in Position Mode) or target speed (in Wheel Mode).
+						<ul>
+							<li>Range: Typically <code>0</code> to <code>254</code>.</li>
+							<li>
+								Meaning: Defines the rate of change of speed. The unit is 100 steps/s².
+								<code>0</code> usually means instantaneous acceleration (or minimal delay). Higher
+								values result in slower, smoother acceleration and deceleration. For example, a
+								value of <code>10</code> means the speed changes by 10 * 100 = 1000 steps per
+								second, per second. This helps reduce jerky movements and mechanical stress.
+							</li>
+							<li>Control Address: <code>ADDR_SCS_GOAL_ACC</code> (Register 41).</li>
+						</ul>
+					</li>
+					<li>
+						<strong>Baud Rate:</strong> The speed of communication between the controller and the
+						servo. It must match on both ends. Servos often support multiple baud rates, selectable
+						via an index:
+						<ul>
+							<li>Index 0: 1,000,000 bps</li>
+							<li>Index 1: 500,000 bps</li>
+							<li>Index 2: 250,000 bps</li>
+							<li>Index 3: 128,000 bps</li>
+							<li>Index 4: 115,200 bps</li>
+							<li>Index 5: 76,800 bps</li>
+							<li>Index 6: 57,600 bps</li>
+							<li>Index 7: 38,400 bps</li>
+						</ul>
+					</li>
+				</ul>
+			</details>
+
+			<div class="section">
+				<h2>Connection</h2>
+				<button id="connectBtn">Connect</button>
+				<button id="disconnectBtn">Disconnect</button>
+				<p>Status: <span id="connectionStatus" class="status error">Disconnected</span></p>
+				<label for="baudRate">Baud Rate:</label>
+				<input type="number" id="baudRate" value="1000000" />
+				<label for="protocolEnd">Protocol End (0=STS/SMS, 1=SCS):</label>
+				<input type="number" id="protocolEnd" value="0" min="0" max="1" />
+			</div>
+
+			<div class="section">
+				<h2>Scan Servos</h2>
+				<label for="scanStartId">Start ID:</label>
+				<input type="number" id="scanStartId" value="1" min="1" max="252" />
+				<label for="scanEndId">End ID:</label>
+				<input type="number" id="scanEndId" value="15" min="1" max="252" />
+				<button id="scanServosBtn">Scan</button>
+				<p>Scan Results:</p>
+				<pre id="scanResultsOutput" style="max-height: 200px; overflow-y: auto"></pre>
+				<!-- Added element for results -->
+			</div>
+
+			<div class="section">
+				<h2>Single Servo Control</h2>
+				<label for="servoId">Servo ID:</label>
+				<input type="number" id="servoId" value="1" min="1" max="252" /><br />
+
+				<label for="idWrite">Change servo ID:</label>
+				<input type="number" id="idWrite" value="1" min="1" max="252" />
+				<button id="writeIdBtn">Write</button><br />
+
+				<label for="baudRead">Read Baud Rate:</label>
+				<button id="readBaudBtn">Read</button>
+				<span id="readBaudResult"></span><br />
+
+				<label for="baudWrite">Write Baud Rate Index:</label>
+				<input type="number" id="baudWrite" value="6" min="0" max="7" />
+				<!-- Assuming index 0-7 -->
+				<button id="writeBaudBtn">Write</button><br />
+
+				<label for="positionRead">Read Position:</label>
+				<button id="readPosBtn">Read</button>
+				<span id="readPosResult"></span><br />
+
+				<label for="positionWrite">Write Position:</label>
+				<input type="number" id="positionWrite" value="1000" min="0" max="4095" />
+				<button id="writePosBtn">Write</button><br />
+
+				<label for="torqueEnable">Torque:</label>
+				<button id="torqueEnableBtn">Enable</button>
+				<button id="torqueDisableBtn">Disable</button><br />
+
+				<label for="accelerationWrite">Write Acceleration:</label>
+				<input type="number" id="accelerationWrite" value="50" min="0" max="254" />
+				<button id="writeAccBtn">Write</button><br />
+
+				<label for="wheelMode">Wheel Mode:</label>
+				<button id="setWheelModeBtn">Set Wheel Mode</button>
+				<button id="removeWheelModeBtn">Set Position Mode</button><br />
+
+				<label for="wheelSpeedWrite">Write Wheel Speed:</label>
+				<input type="number" id="wheelSpeedWrite" value="0" min="-2500" max="2500" />
+				<button id="writeWheelSpeedBtn">Write Speed</button>
+			</div>
+
+			<div class="section">
+				<h2>Sync Operations</h2>
+				<label for="syncReadIds">Sync Read IDs (csv):</label>
+				<input type="text" id="syncReadIds" value="1,2,3" style="width: 150px" />
+				<button id="syncReadBtn">Sync Read Positions</button><br />
+
+				<label for="syncWriteData">Sync Write (id:pos,...):</label>
+				<input type="text" id="syncWriteData" value="1:1500,2:2500" style="width: 200px" />
+				<button id="syncWriteBtn">Sync Write Positions</button><br />
+
+				<label for="syncWriteSpeedData">Sync Write Speed (id:speed,...):</label>
+				<input type="text" id="syncWriteSpeedData" value="1:500,2:-1000" style="width: 200px" />
+				<button id="syncWriteSpeedBtn">Sync Write Speeds</button>
+				<!-- New Button -->
+			</div>
+
+			<div class="section">
+				<h2>Log Output</h2>
+				<pre id="logOutput"></pre>
+			</div>
+		</div>
+
+		<script type="module">
+			// Import the scsServoSDK object from index.mjs
+			import { scsServoSDK } from "./index.mjs";
+			// No longer need COMM_SUCCESS etc. here as errors are thrown
+
+			const connectBtn = document.getElementById("connectBtn");
+			const disconnectBtn = document.getElementById("disconnectBtn");
+			const connectionStatus = document.getElementById("connectionStatus");
+			const baudRateInput = document.getElementById("baudRate");
+			const protocolEndInput = document.getElementById("protocolEnd");
+
+			const servoIdInput = document.getElementById("servoId");
+			const readIdBtn = document.getElementById("readIdBtn"); // New
+			const readIdResult = document.getElementById("readIdResult"); // New
+			const idWriteInput = document.getElementById("idWrite"); // New
+			const writeIdBtn = document.getElementById("writeIdBtn"); // New
+			const readBaudBtn = document.getElementById("readBaudBtn"); // New
+			const readBaudResult = document.getElementById("readBaudResult"); // New
+			const baudWriteInput = document.getElementById("baudWrite"); // New
+			const writeBaudBtn = document.getElementById("writeBaudBtn"); // New
+			const readPosBtn = document.getElementById("readPosBtn");
+			const readPosResult = document.getElementById("readPosResult");
+			const positionWriteInput = document.getElementById("positionWrite");
+			const writePosBtn = document.getElementById("writePosBtn");
+			const torqueEnableBtn = document.getElementById("torqueEnableBtn");
+			const torqueDisableBtn = document.getElementById("torqueDisableBtn");
+			const accelerationWriteInput = document.getElementById("accelerationWrite");
+			const writeAccBtn = document.getElementById("writeAccBtn");
+			const setWheelModeBtn = document.getElementById("setWheelModeBtn");
+			const removeWheelModeBtn = document.getElementById("removeWheelModeBtn"); // Get reference to the new button
+			const wheelSpeedWriteInput = document.getElementById("wheelSpeedWrite");
+			const writeWheelSpeedBtn = document.getElementById("writeWheelSpeedBtn");
+
+			const syncReadIdsInput = document.getElementById("syncReadIds");
+			const syncReadBtn = document.getElementById("syncReadBtn");
+			const syncWriteDataInput = document.getElementById("syncWriteData");
+			const syncWriteBtn = document.getElementById("syncWriteBtn");
+			const syncWriteSpeedDataInput = document.getElementById("syncWriteSpeedData"); // New Input
+			const syncWriteSpeedBtn = document.getElementById("syncWriteSpeedBtn"); // New Button
+			const scanServosBtn = document.getElementById("scanServosBtn"); // Get reference to the scan button
+			const scanStartIdInput = document.getElementById("scanStartId"); // Get reference to start ID input
+			const scanEndIdInput = document.getElementById("scanEndId"); // Get reference to end ID input
+			const scanResultsOutput = document.getElementById("scanResultsOutput"); // Get reference to the new results area
+
+			const logOutput = document.getElementById("logOutput");
+
+			let isConnected = false;
+
+			function log(message) {
+				console.log(message);
+				const timestamp = new Date().toLocaleTimeString();
+				logOutput.textContent = `[${timestamp}] ${message}\n` + logOutput.textContent;
+				// Limit log size
+				const lines = logOutput.textContent.split("\n"); // Use '\n' instead of literal newline
+				if (lines.length > 50) {
+					logOutput.textContent = lines.slice(0, 50).join("\n"); // Use '\n' instead of literal newline
+				}
+			}
+
+			function updateConnectionStatus(connected, message) {
+				isConnected = connected;
+				connectionStatus.textContent = message || (connected ? "Connected" : "Disconnected");
+				connectionStatus.className = `status ${connected ? "success" : "error"}`;
+				log(`Connection status: ${connectionStatus.textContent}`);
+			}
+
+			connectBtn.onclick = async () => {
+				log("Attempting to connect...");
+				try {
+					const baudRate = parseInt(baudRateInput.value, 10);
+					const protocolEnd = parseInt(protocolEndInput.value, 10);
+					// Use scsServoSDK - throws on error
+					await scsServoSDK.connect({ baudRate, protocolEnd });
+					updateConnectionStatus(true, "Connected");
+				} catch (err) {
+					updateConnectionStatus(false, `Connection error: ${err.message}`);
+					console.error(err);
+				}
+			};
+
+			disconnectBtn.onclick = async () => {
+				log("Attempting to disconnect...");
+				try {
+					// Use scsServoSDK - throws on error
+					await scsServoSDK.disconnect();
+					updateConnectionStatus(false, "Disconnected"); // Success means disconnected
+				} catch (err) {
+					// Assuming disconnect might fail if already disconnected or other issues
+					updateConnectionStatus(false, `Disconnection error: ${err.message}`);
+					console.error(err);
+				}
+			};
+
+			writeIdBtn.onclick = async () => {
+				// New handler
+				if (!isConnected) {
+					log("Error: Not connected");
+					return;
+				}
+				const currentId = parseInt(servoIdInput.value, 10);
+				const newId = parseInt(idWriteInput.value, 10);
+				if (isNaN(newId) || newId < 1 || newId > 252) {
+					log(`Error: Invalid new ID ${newId}. Must be between 1 and 252.`);
+					return;
+				}
+				log(`Writing new ID ${newId} to servo ${currentId}...`);
+				try {
+					// Use scsServoSDK - throws on error
+					await scsServoSDK.setServoId(currentId, newId);
+					log(`Successfully wrote new ID ${newId} to servo (was ${currentId}).`);
+					// IMPORTANT: Update the main ID input to reflect the change
+					servoIdInput.value = newId;
+					log(`Servo ID input field updated to ${newId}.`);
+				} catch (err) {
+					log(`Error writing ID for servo ${currentId}: ${err.message}`);
+					console.error(err);
+				}
+			};
+
+			readBaudBtn.onclick = async () => {
+				// New handler
+				if (!isConnected) {
+					log("Error: Not connected");
+					return;
+				}
+				const id = parseInt(servoIdInput.value, 10);
+				log(`Reading Baud Rate Index for servo ${id}...`);
+				readBaudResult.textContent = "Reading...";
+				try {
+					// Use scsServoSDK - returns value directly or throws
+					const baudRateIndex = await scsServoSDK.readBaudRate(id);
+					readBaudResult.textContent = `Baud Index: ${baudRateIndex}`;
+					log(`Servo ${id} Baud Rate Index: ${baudRateIndex}`);
+				} catch (err) {
+					readBaudResult.textContent = `Error: ${err.message}`;
+					log(`Error reading Baud Rate Index for servo ${id}: ${err.message}`);
+					console.error(err);
+				}
+			};
+
+			writeBaudBtn.onclick = async () => {
+				// New handler
+				if (!isConnected) {
+					log("Error: Not connected");
+					return;
+				}
+				const id = parseInt(servoIdInput.value, 10);
+				const newBaudIndex = parseInt(baudWriteInput.value, 10);
+				if (isNaN(newBaudIndex) || newBaudIndex < 0 || newBaudIndex > 7) {
+					// Adjust max index if needed
+					log(`Error: Invalid new Baud Rate Index ${newBaudIndex}. Check valid range.`);
+					return;
+				}
+				log(`Writing new Baud Rate Index ${newBaudIndex} to servo ${id}...`);
+				try {
+					// Use scsServoSDK - throws on error
+					await scsServoSDK.setBaudRate(id, newBaudIndex);
+					log(`Successfully wrote new Baud Rate Index ${newBaudIndex} to servo ${id}.`);
+					log(
+						`IMPORTANT: You may need to disconnect and reconnect with the new baud rate if it differs from the current connection baud rate.`
+					);
+				} catch (err) {
+					log(`Error writing Baud Rate Index for servo ${id}: ${err.message}`);
+					console.error(err);
+				}
+			};
+
+			readPosBtn.onclick = async () => {
+				if (!isConnected) {
+					log("Error: Not connected");
+					return;
+				}
+				const id = parseInt(servoIdInput.value, 10);
+				log(`Reading position for servo ${id}...`);
+				readPosResult.textContent = "Reading...";
+				try {
+					// Use scsServoSDK - returns value directly or throws
+					const position = await scsServoSDK.readPosition(id);
+					readPosResult.textContent = `Position: ${position}`;
+					log(`Servo ${id} position: ${position}`);
+				} catch (err) {
+					readPosResult.textContent = `Error: ${err.message}`;
+					log(`Error reading position for servo ${id}: ${err.message}`);
+					console.error(err);
+				}
+			};
+
+			writePosBtn.onclick = async () => {
+				if (!isConnected) {
+					log("Error: Not connected");
+					return;
+				}
+				const id = parseInt(servoIdInput.value, 10);
+				const pos = parseInt(positionWriteInput.value, 10);
+				log(`Writing position ${pos} to servo ${id}...`);
+				try {
+					// Use scsServoSDK - throws on error
+					await scsServoSDK.writePosition(id, pos);
+					log(`Successfully wrote position ${pos} to servo ${id}.`);
+				} catch (err) {
+					log(`Error writing position for servo ${id}: ${err.message}`);
+					console.error(err);
+				}
+			};
+
+			torqueEnableBtn.onclick = async () => {
+				if (!isConnected) {
+					log("Error: Not connected");
+					return;
+				}
+				const id = parseInt(servoIdInput.value, 10);
+				log(`Enabling torque for servo ${id}...`);
+				try {
+					// Use scsServoSDK - throws on error
+					await scsServoSDK.writeTorqueEnable(id, true);
+					log(`Successfully enabled torque for servo ${id}.`);
+				} catch (err) {
+					log(`Error enabling torque for servo ${id}: ${err.message}`);
+					console.error(err);
+				}
+			};
+
+			torqueDisableBtn.onclick = async () => {
+				if (!isConnected) {
+					log("Error: Not connected");
+					return;
+				}
+				const id = parseInt(servoIdInput.value, 10);
+				log(`Disabling torque for servo ${id}...`);
+				try {
+					// Use scsServoSDK - throws on error
+					await scsServoSDK.writeTorqueEnable(id, false);
+					log(`Successfully disabled torque for servo ${id}.`);
+				} catch (err) {
+					log(`Error disabling torque for servo ${id}: ${err.message}`);
+					console.error(err);
+				}
+			};
+
+			writeAccBtn.onclick = async () => {
+				if (!isConnected) {
+					log("Error: Not connected");
+					return;
+				}
+				const id = parseInt(servoIdInput.value, 10);
+				const acc = parseInt(accelerationWriteInput.value, 10);
+				log(`Writing acceleration ${acc} to servo ${id}...`);
+				try {
+					// Use scsServoSDK - throws on error
+					await scsServoSDK.writeAcceleration(id, acc);
+					log(`Successfully wrote acceleration ${acc} to servo ${id}.`);
+				} catch (err) {
+					log(`Error writing acceleration for servo ${id}: ${err.message}`);
+					console.error(err);
+				}
+			};
+
+			setWheelModeBtn.onclick = async () => {
+				if (!isConnected) {
+					log("Error: Not connected");
+					return;
+				}
+				const id = parseInt(servoIdInput.value, 10);
+				log(`Setting servo ${id} to wheel mode...`);
+				try {
+					// Use scsServoSDK - throws on error
+					await scsServoSDK.setWheelMode(id);
+					log(`Successfully set servo ${id} to wheel mode.`);
+				} catch (err) {
+					log(`Error setting wheel mode for servo ${id}: ${err.message}`);
+					console.error(err);
+				}
+			};
+
+			// Add event listener for the new button
+			removeWheelModeBtn.onclick = async () => {
+				if (!isConnected) {
+					log("Error: Not connected");
+					return;
+				}
+				const id = parseInt(servoIdInput.value, 10);
+				log(`Setting servo ${id} back to position mode...`);
+				try {
+					// Use scsServoSDK - throws on error
+					await scsServoSDK.setPositionMode(id);
+					log(`Successfully set servo ${id} back to position mode.`);
+				} catch (err) {
+					log(`Error setting position mode for servo ${id}: ${err.message}`);
+					console.error(err);
+				}
+			};
+
+			writeWheelSpeedBtn.onclick = async () => {
+				if (!isConnected) {
+					log("Error: Not connected");
+					return;
+				}
+				const id = parseInt(servoIdInput.value, 10);
+				const speed = parseInt(wheelSpeedWriteInput.value, 10);
+				log(`Writing wheel speed ${speed} to servo ${id}...`);
+				try {
+					// Use scsServoSDK - throws on error
+					await scsServoSDK.writeWheelSpeed(id, speed);
+					log(`Successfully wrote wheel speed ${speed} to servo ${id}.`);
+				} catch (err) {
+					log(`Error writing wheel speed for servo ${id}: ${err.message}`);
+					console.error(err);
+				}
+			};
+
+			syncReadBtn.onclick = async () => {
+				if (!isConnected) {
+					log("Error: Not connected");
+					return;
+				}
+				const idsString = syncReadIdsInput.value;
+				const ids = idsString
+					.split(",")
+					.map((s) => parseInt(s.trim(), 10))
+					.filter((id) => !isNaN(id) && id > 0 && id < 253);
+				if (ids.length === 0) {
+					log("Sync Read: No valid servo IDs provided.");
+					return;
+				}
+				log(`Sync reading positions for servos: ${ids.join(", ")}...`);
+				try {
+					// Use scsServoSDK - returns Map or throws
+					const positions = await scsServoSDK.syncReadPositions(ids);
+					let logMsg = "Sync Read Successful:\n";
+					positions.forEach((pos, id) => {
+						logMsg += `  Servo ${id}: Position=${pos}\n`;
+					});
+					log(logMsg.trim());
+				} catch (err) {
+					log(`Sync Read Failed: ${err.message}`);
+					console.error(err);
+				}
+			};
+
+			syncWriteBtn.onclick = async () => {
+				if (!isConnected) {
+					log("Error: Not connected");
+					return;
+				}
+				const dataString = syncWriteDataInput.value;
+				const positionMap = new Map();
+				const pairs = dataString.split(",");
+				let validData = false;
+
+				pairs.forEach((pair) => {
+					const parts = pair.split(":");
+					if (parts.length === 2) {
+						const id = parseInt(parts[0].trim(), 10);
+						const pos = parseInt(parts[1].trim(), 10);
+						// Position validation (0-4095)
+						if (!isNaN(id) && id > 0 && id < 253 && !isNaN(pos) && pos >= 0 && pos <= 4095) {
+							positionMap.set(id, pos);
+							validData = true;
+						} else {
+							log(
+								`Sync Write Position: Invalid data pair "${pair}". ID (1-252), Pos (0-4095). Skipping.`
+							);
+						}
+					} else {
+						log(`Sync Write Position: Invalid format "${pair}". Skipping.`);
+					}
+				});
+
+				if (!validData) {
+					log("Sync Write Position: No valid servo position data provided.");
+					return;
+				}
+
+				log(
+					`Sync writing positions: ${Array.from(positionMap.entries())
+						.map(([id, pos]) => `${id}:${pos}`)
+						.join(", ")}...`
+				);
+				try {
+					// Use scsServoSDK - throws on error
+					await scsServoSDK.syncWritePositions(positionMap);
+					log(`Sync write position command sent successfully.`);
+				} catch (err) {
+					log(`Sync Write Position Failed: ${err.message}`);
+					console.error(err);
+				}
+			};
+
+			// New handler for Sync Write Speed
+			syncWriteSpeedBtn.onclick = async () => {
+				if (!isConnected) {
+					log("Error: Not connected");
+					return;
+				}
+				const dataString = syncWriteSpeedDataInput.value;
+				const speedMap = new Map();
+				const pairs = dataString.split(",");
+				let validData = false;
+
+				pairs.forEach((pair) => {
+					const parts = pair.split(":");
+					if (parts.length === 2) {
+						const id = parseInt(parts[0].trim(), 10);
+						const speed = parseInt(parts[1].trim(), 10);
+						// Speed validation (-10000 to 10000)
+						if (
+							!isNaN(id) &&
+							id > 0 &&
+							id < 253 &&
+							!isNaN(speed) &&
+							speed >= -10000 &&
+							speed <= 10000
+						) {
+							speedMap.set(id, speed);
+							validData = true;
+						} else {
+							log(
+								`Sync Write Speed: Invalid data pair "${pair}". ID (1-252), Speed (-10000 to 10000). Skipping.`
+							);
+						}
+					} else {
+						log(`Sync Write Speed: Invalid format "${pair}". Skipping.`);
+					}
+				});
+
+				if (!validData) {
+					log("Sync Write Speed: No valid servo speed data provided.");
+					return;
+				}
+
+				log(
+					`Sync writing speeds: ${Array.from(speedMap.entries())
+						.map(([id, speed]) => `${id}:${speed}`)
+						.join(", ")}...`
+				);
+				try {
+					// Use scsServoSDK - throws on error
+					await scsServoSDK.syncWriteWheelSpeed(speedMap);
+					log(`Sync write speed command sent successfully.`);
+				} catch (err) {
+					log(`Sync Write Speed Failed: ${err.message}`);
+					console.error(err);
+				}
+			};
+
+			scanServosBtn.onclick = async () => {
+				if (!isConnected) {
+					log("Error: Not connected");
+					return;
+				}
+
+				const startId = parseInt(scanStartIdInput.value, 10);
+				const endId = parseInt(scanEndIdInput.value, 10);
+
+				if (isNaN(startId) || isNaN(endId) || startId < 1 || endId > 252 || startId > endId) {
+					const errorMsg =
+						"Error: Invalid scan ID range. Please enter values between 1 and 252, with Start ID <= End ID.";
+					log(errorMsg);
+					scanResultsOutput.textContent = errorMsg; // Show error in results area too
+					return;
+				}
+
+				const startMsg = `Starting servo scan (IDs ${startId}-${endId})...`;
+				log(startMsg);
+				scanResultsOutput.textContent = startMsg + "\n"; // Clear and start results area
+				scanServosBtn.disabled = true; // Disable button during scan
+
+				let foundCount = 0;
+
+				for (let id = startId; id <= endId; id++) {
+					let resultMsg = `Scanning ID ${id}... `;
+					try {
+						// Attempt to read position. If it succeeds, the servo exists.
+						// If it throws, the servo likely doesn't exist or there's another issue.
+						const position = await scsServoSDK.readPosition(id);
+						foundCount++;
+
+						// Servo found, now try to read mode and baud rate
+						let mode = "ReadError";
+						let baudRateIndex = "ReadError";
+						try {
+							mode = await scsServoSDK.readMode(id);
+						} catch (modeErr) {
+							log(`  Servo ${id}: Error reading mode: ${modeErr.message}`);
+						}
+						try {
+							baudRateIndex = await scsServoSDK.readBaudRate(id);
+						} catch (baudErr) {
+							log(`  Servo ${id}: Error reading baud rate: ${baudErr.message}`);
+						}
+
+						resultMsg += `FOUND: Pos=${position}, Mode=${mode}, BaudIdx=${baudRateIndex}`;
+						log(
+							`  Servo ${id} FOUND: Position=${position}, Mode=${mode}, BaudIndex=${baudRateIndex}`
+						);
+					} catch (err) {
+						// Check if the error message indicates a timeout or non-response, which is expected for non-existent IDs
+						// This check might need refinement based on the exact error messages thrown by readPosition
+						if (
+							err.message.includes("timeout") ||
+							err.message.includes("No response") ||
+							err.message.includes("failed: RX")
+						) {
+							resultMsg += `No response`;
+							// log(`  Servo ${id}: No response`); // Optional: reduce log noise
+						} else {
+							// Log other unexpected errors
+							resultMsg += `Error: ${err.message}`;
+							log(`  Servo ${id}: Error during scan: ${err.message}`);
+							console.error(`Error scanning servo ${id}:`, err);
+						}
+					}
+					scanResultsOutput.textContent += resultMsg + "\n"; // Append result to the results area
+					scanResultsOutput.scrollTop = scanResultsOutput.scrollHeight; // Auto-scroll
+					// Optional small delay between scans if needed
+					// await new Promise(resolve => setTimeout(resolve, 10));
+				}
+
+				const finishMsg = `Servo scan finished. Found ${foundCount} servo(s).`;
+				log(finishMsg);
+				scanResultsOutput.textContent += finishMsg + "\n"; // Add finish message to results area
+				scanResultsOutput.scrollTop = scanResultsOutput.scrollHeight; // Auto-scroll
+				scanServosBtn.disabled = false; // Re-enable button
+			};
+
+			// Initial log
+			log("Test page loaded. Please connect to a servo controller.");
+		</script>
+	</body>
+</html>

src-python/README.md

@@ -5,6 +5,7 @@ A comprehensive WebSocket-based robot control platform featuring **master-slave
 ## 🏛️ Core Architecture
 
 ### Master-Slave Pattern
+
 ```
 ┌─────────────────┐    Commands     ┌─────────────────┐    Execution    ┌─────────────────┐
 │    🎮 MASTERS   │ ───────────────►│  🤖 ROBOT CORE  │ ───────────────►│   🔧 SLAVES     │
@@ -17,8 +18,9 @@ A comprehensive WebSocket-based robot control platform featuring **master-slave
 ```
 
 **Key Principles:**
+
 - 🎯 **Single Source of Truth**: Only one master per robot at a time
-- 🔒 **Safety First**: Manual control disabled when master active  
+- 🔒 **Safety First**: Manual control disabled when master active
 - 🌐 **Multi-Modal**: Same commands work across all slave types
 - 🔄 **Real-Time Sync**: Virtual and physical states synchronized
 - 📡 **Network Native**: Built for remote operation from day one
@@ -26,6 +28,7 @@ A comprehensive WebSocket-based robot control platform featuring **master-slave
 ## 🚀 Quick Start Guide
 
 ### 1. Server Setup
+
 ```bash
 # Install dependencies
 cd src-python
@@ -37,6 +40,7 @@ python start_server.py
 ```
 
 ### 2. Frontend Integration
+
 ```bash
 # In your Svelte app
 npm run dev
@@ -44,6 +48,7 @@ npm run dev
 ```
 
 ### 3. Create & Control Robot
+
 ```javascript
 // Create robot in UI or via API
 const robot = await robotManager.createRobot('my-robot', robotUrdfConfig);
@@ -67,44 +72,49 @@ curl -X POST http://localhost:8080/api/robots/my-robot/play-sequence/gentle-wave
 ## 🎮 Master Drivers (Command Sources)
 
 ### MockSequenceMaster
+
 **Pre-programmed movement patterns for testing and demos**
 
 ```typescript
 const config: MasterDriverConfig = {
-  type: "mock-sequence",
-  sequences: DEMO_SEQUENCES,
-  autoStart: true,
-  loopMode: true
+	type: "mock-sequence",
+	sequences: DEMO_SEQUENCES,
+	autoStart: true,
+	loopMode: true
 };
 
-await robotManager.connectMaster('robot-1', config);
+await robotManager.connectMaster("robot-1", config);
 ```
 
 **Available Sequences:**
+
 - **🌊 Gentle Wave Pattern** (6s): Smooth greeting gesture with wrist movements
-- **🔍 Small Scanning Pattern** (8s): Horizontal sweep for environment scanning  
+- **🔍 Small Scanning Pattern** (8s): Horizontal sweep for environment scanning
 - **💪 Tiny Flex Pattern** (8s): Articulation demonstration with elbow/jaw coordination
 
 **Features:**
+
 - ✅ Safe movement ranges (±25° max)
 - ✅ Smooth interpolation between keyframes
 - ✅ Loop mode for continuous operation
 - ✅ Automatic master takeover
 
 ### RemoteServerMaster
+
 **WebSocket connection to external control systems**
 
 ```typescript
 const config: MasterDriverConfig = {
-  type: "remote-server", 
-  url: "ws://localhost:8080",
-  apiKey: "optional-auth-token"
+	type: "remote-server",
+	url: "ws://localhost:8080",
+	apiKey: "optional-auth-token"
 };
 
-await robotManager.connectMaster('robot-1', config);
+await robotManager.connectMaster("robot-1", config);
 ```
 
 **Features:**
+
 - ✅ Real-time bidirectional communication
 - ✅ Automatic reconnection with exponential backoff
 - ✅ Heartbeat monitoring (30s intervals)
@@ -112,36 +122,37 @@ await robotManager.connectMaster('robot-1', config);
 - ✅ Status and error reporting
 
 **Message Format:**
+
 ```json
 {
-  "type": "command",
-  "timestamp": "2024-01-01T12:00:00Z", 
-  "data": {
-    "timestamp": 1704110400000,
-    "joints": [
-      {"name": "Rotation", "value": 45, "speed": 100}
-    ]
-  }
+	"type": "command",
+	"timestamp": "2024-01-01T12:00:00Z",
+	"data": {
+		"timestamp": 1704110400000,
+		"joints": [{ "name": "Rotation", "value": 45, "speed": 100 }]
+	}
 }
 ```
 
 ## 🔧 Slave Drivers (Execution Targets)
 
 ### MockSlave
+
 **Perfect simulation for development and testing**
 
 ```typescript
 const config: SlaveDriverConfig = {
-  type: "mock-slave",
-  simulateLatency: 50,      // Realistic response delay
-  simulateErrors: false,    // Random connection issues
-  responseDelay: 20         // Command execution time
+	type: "mock-slave",
+	simulateLatency: 50, // Realistic response delay
+	simulateErrors: false, // Random connection issues
+	responseDelay: 20 // Command execution time
 };
 
-await robotManager.connectSlave('robot-1', config);
+await robotManager.connectSlave("robot-1", config);
 ```
 
 **Features:**
+
 - ✅ Perfect command execution (real_value = virtual_value)
 - ✅ Configurable network latency simulation
 - ✅ Error injection for robustness testing
@@ -149,19 +160,21 @@ await robotManager.connectSlave('robot-1', config);
 - ✅ Real-time state feedback
 
 ### USBSlave
+
 **Direct serial communication with physical robots**
 
 ```typescript
 const config: SlaveDriverConfig = {
-  type: "usb-slave",
-  port: "/dev/ttyUSB0",     // Auto-detect if undefined
-  baudRate: 115200
+	type: "usb-slave",
+	port: "/dev/ttyUSB0", // Auto-detect if undefined
+	baudRate: 115200
 };
 
-await robotManager.connectSlave('robot-1', config);
+await robotManager.connectSlave("robot-1", config);
 ```
 
 **Features:**
+
 - ✅ Feetech servo protocol support
 - ✅ Position and speed control
 - ✅ Real servo position feedback
@@ -169,25 +182,28 @@ await robotManager.connectSlave('robot-1', config);
 - ✅ Error handling and recovery
 
 **Calibration Process:**
+
 1. Manually position robot to match digital twin
 2. Click "Calibrate" to sync virtual/real coordinates
 3. System calculates offset: `real_pos = raw_pos + offset`
 4. All future commands automatically compensated
 
 ### WebSocketSlave
+
 **Remote robot control via WebSocket relay**
 
 ```typescript
 const config: SlaveDriverConfig = {
-  type: "websocket-slave",
-  url: "ws://robot-proxy:8080",
-  robotId: "remote-arm-1"
+	type: "websocket-slave",
+	url: "ws://robot-proxy:8080",
+	robotId: "remote-arm-1"
 };
 
-await robotManager.connectSlave('robot-1', config);
+await robotManager.connectSlave("robot-1", config);
 ```
 
 **Use Cases:**
+
 - 🌐 Control robots across internet
 - 🏢 Enterprise robot fleet management
 - 🔒 Firewall-friendly robot access
@@ -198,104 +214,110 @@ await robotManager.connectSlave('robot-1', config);
 ### Master → Server Communication
 
 #### Send Joint Command
+
 ```json
 {
-  "type": "command",
-  "timestamp": "2024-01-01T12:00:00Z",
-  "data": {
-    "timestamp": 1704110400000,
-    "joints": [
-      {"name": "Rotation", "value": 45, "speed": 100},
-      {"name": "Pitch", "value": -30, "speed": 150}
-    ],
-    "duration": 2000,
-    "metadata": {"source": "manual_control"}
-  }
+	"type": "command",
+	"timestamp": "2024-01-01T12:00:00Z",
+	"data": {
+		"timestamp": 1704110400000,
+		"joints": [
+			{ "name": "Rotation", "value": 45, "speed": 100 },
+			{ "name": "Pitch", "value": -30, "speed": 150 }
+		],
+		"duration": 2000,
+		"metadata": { "source": "manual_control" }
+	}
 }
 ```
 
 #### Send Movement Sequence
+
 ```json
 {
-  "type": "sequence",
-  "timestamp": "2024-01-01T12:00:00Z", 
-  "data": {
-    "id": "custom-dance",
-    "name": "Custom Dance Sequence",
-    "commands": [
-      {
-        "timestamp": 0,
-        "joints": [{"name": "Rotation", "value": -30}],
-        "duration": 1000
-      },
-      {
-        "timestamp": 1000, 
-        "joints": [{"name": "Rotation", "value": 30}],
-        "duration": 1000
-      }
-    ],
-    "total_duration": 2000,
-    "loop": false
-  }
+	"type": "sequence",
+	"timestamp": "2024-01-01T12:00:00Z",
+	"data": {
+		"id": "custom-dance",
+		"name": "Custom Dance Sequence",
+		"commands": [
+			{
+				"timestamp": 0,
+				"joints": [{ "name": "Rotation", "value": -30 }],
+				"duration": 1000
+			},
+			{
+				"timestamp": 1000,
+				"joints": [{ "name": "Rotation", "value": 30 }],
+				"duration": 1000
+			}
+		],
+		"total_duration": 2000,
+		"loop": false
+	}
 }
 ```
 
 #### Heartbeat
+
 ```json
 {
-  "type": "heartbeat",
-  "timestamp": "2024-01-01T12:00:00Z"
+	"type": "heartbeat",
+	"timestamp": "2024-01-01T12:00:00Z"
 }
 ```
 
 ### Slave → Server Communication
 
 #### Status Update
+
 ```json
 {
-  "type": "status_update",
-  "timestamp": "2024-01-01T12:00:00Z",
-  "data": {
-    "isConnected": true,
-    "lastConnected": "2024-01-01T11:58:00Z",
-    "error": null
-  }
+	"type": "status_update",
+	"timestamp": "2024-01-01T12:00:00Z",
+	"data": {
+		"isConnected": true,
+		"lastConnected": "2024-01-01T11:58:00Z",
+		"error": null
+	}
 }
 ```
 
 #### Joint State Feedback
+
 ```json
 {
-  "type": "joint_states", 
-  "timestamp": "2024-01-01T12:00:00Z",
-  "data": [
-    {
-      "name": "Rotation",
-      "servo_id": 1,
-      "type": "revolute",
-      "virtual_value": 45.0,
-      "real_value": 44.8,
-      "limits": {
-        "lower": -180,
-        "upper": 180,
-        "velocity": 200
-      }
-    }
-  ]
+	"type": "joint_states",
+	"timestamp": "2024-01-01T12:00:00Z",
+	"data": [
+		{
+			"name": "Rotation",
+			"servo_id": 1,
+			"type": "revolute",
+			"virtual_value": 45.0,
+			"real_value": 44.8,
+			"limits": {
+				"lower": -180,
+				"upper": 180,
+				"velocity": 200
+			}
+		}
+	]
 }
 ```
 
 #### Error Reporting
+
 ```json
 {
-  "type": "error",
-  "timestamp": "2024-01-01T12:00:00Z", 
-  "data": {
-    "code": "SERVO_TIMEOUT",
-    "message": "Servo 3 not responding",
-    "joint": "Elbow", 
-    "severity": "warning"
-  }
+	"type": "error",
+	"timestamp": "2024-01-01T12:00:00Z",
+	"data": {
+		"code": "SERVO_TIMEOUT",
+		"message": "Servo 3 not responding",
+		"joint": "Elbow",
+		"severity": "warning"
+	}
 }
 ```
 
@@ -303,33 +325,34 @@ await robotManager.connectSlave('robot-1', config);
 
 ### Robot Management
 
-| Method | Endpoint | Description | Example |
-|--------|----------|-------------|---------|
-| GET | `/` | Server status & metrics | `curl http://localhost:8080/` |
-| GET | `/api/robots` | List all robots | `curl http://localhost:8080/api/robots` |
-| POST | `/api/robots` | Create new robot | `curl -X POST -H "Content-Type: application/json" -d '{"robot_type":"so-arm100","name":"My Robot"}' http://localhost:8080/api/robots` |
-| GET | `/api/robots/{id}` | Get robot details | `curl http://localhost:8080/api/robots/robot-123` |
-| GET | `/api/robots/{id}/status` | Get connection status | `curl http://localhost:8080/api/robots/robot-123/status` |
-| DELETE | `/api/robots/{id}` | Delete robot | `curl -X DELETE http://localhost:8080/api/robots/robot-123` |
+| Method | Endpoint                  | Description             | Example                                                                                                                               |
+| ------ | ------------------------- | ----------------------- | ------------------------------------------------------------------------------------------------------------------------------------- |
+| GET    | `/`                       | Server status & metrics | `curl http://localhost:8080/`                                                                                                         |
+| GET    | `/api/robots`             | List all robots         | `curl http://localhost:8080/api/robots`                                                                                               |
+| POST   | `/api/robots`             | Create new robot        | `curl -X POST -H "Content-Type: application/json" -d '{"robot_type":"so-arm100","name":"My Robot"}' http://localhost:8080/api/robots` |
+| GET    | `/api/robots/{id}`        | Get robot details       | `curl http://localhost:8080/api/robots/robot-123`                                                                                     |
+| GET    | `/api/robots/{id}/status` | Get connection status   | `curl http://localhost:8080/api/robots/robot-123/status`                                                                              |
+| DELETE | `/api/robots/{id}`        | Delete robot            | `curl -X DELETE http://localhost:8080/api/robots/robot-123`                                                                           |
 
 ### Sequence Control
 
-| Method | Endpoint | Description | Example |
-|--------|----------|-------------|---------|
-| GET | `/api/sequences` | List demo sequences | `curl http://localhost:8080/api/sequences` |
-| POST | `/api/robots/{id}/play-sequence/{seq_id}` | Play sequence | `curl -X POST http://localhost:8080/api/robots/robot-123/play-sequence/gentle-wave` |
-| POST | `/api/robots/{id}/stop-sequence` | Stop sequences | `curl -X POST http://localhost:8080/api/robots/robot-123/stop-sequence` |
+| Method | Endpoint                                  | Description         | Example                                                                             |
+| ------ | ----------------------------------------- | ------------------- | ----------------------------------------------------------------------------------- |
+| GET    | `/api/sequences`                          | List demo sequences | `curl http://localhost:8080/api/sequences`                                          |
+| POST   | `/api/robots/{id}/play-sequence/{seq_id}` | Play sequence       | `curl -X POST http://localhost:8080/api/robots/robot-123/play-sequence/gentle-wave` |
+| POST   | `/api/robots/{id}/stop-sequence`          | Stop sequences      | `curl -X POST http://localhost:8080/api/robots/robot-123/stop-sequence`             |
 
 ### WebSocket Endpoints
 
-| Endpoint | Purpose | Client Type | Example |
-|----------|---------|-------------|---------|
-| `/ws/master/{robot_id}` | Send commands | Control sources | `ws://localhost:8080/ws/master/robot-123` |
-| `/ws/slave/{robot_id}` | Receive commands | Execution targets | `ws://localhost:8080/ws/slave/robot-123` |
+| Endpoint                | Purpose          | Client Type       | Example                                   |
+| ----------------------- | ---------------- | ----------------- | ----------------------------------------- |
+| `/ws/master/{robot_id}` | Send commands    | Control sources   | `ws://localhost:8080/ws/master/robot-123` |
+| `/ws/slave/{robot_id}`  | Receive commands | Execution targets | `ws://localhost:8080/ws/slave/robot-123`  |
 
 ## 🎯 Usage Scenarios
 
 ### 1. 🧪 Development & Testing
+
 ```bash
 # Create robot with mock slave for safe testing
 robot = await robotManager.createRobot('test-bot', urdfConfig);
@@ -338,12 +361,14 @@ await robotManager.connectDemoSequences('test-bot');
 ```
 
 **Perfect for:**
+
 - Algorithm development without hardware risk
 - UI/UX testing with realistic feedback
 - Automated testing pipelines
 - Demo presentations
 
 ### 2. 🦾 Physical Robot Control
+
 ```bash
 # Connect real hardware
 robot = await robotManager.createRobot('real-bot', urdfConfig);
@@ -355,35 +380,39 @@ await robotManager.connectDemoSequences('real-bot');
 ```
 
 **Calibration Workflow:**
+
 1. Connect USB slave to robot
 2. Manually position to match 3D model rest pose
 3. Click "Calibrate" to sync coordinate systems
 4. Robot now mirrors 3D model movements precisely
 
 ### 3. 🌐 Remote Operation
+
 ```bash
 # Master controls slave over internet
 # Master side:
 await robotManager.connectMaster('local-avatar', {
-  type: "remote-server", 
+  type: "remote-server",
   url: "wss://robot-farm.com:8080"
 });
 
 # Slave side (at robot location):
 await robotManager.connectSlave('physical-robot', {
   type: "websocket-slave",
-  url: "wss://robot-farm.com:8080", 
+  url: "wss://robot-farm.com:8080",
   robotId: "local-avatar"
 });
 ```
 
 **Use Cases:**
+
 - Telepresence robotics
 - Remote maintenance and inspection
 - Distributed manufacturing
 - Educational robot sharing
 
 ### 4. 🤖 Multi-Robot Coordination
+
 ```bash
 # One master controlling multiple robots
 await robotManager.connectMaster('fleet-commander', masterConfig);
@@ -395,12 +424,14 @@ for (const robot of robotFleet) {
 ```
 
 **Applications:**
+
 - Synchronized dance performances
 - Assembly line coordination
 - Swarm robotics research
 - Entertainment shows
 
 ### 5. 🧠 AI Agent Integration
+
 ```javascript
 // AI agent as master driver
 class AIAgentMaster implements MasterDriver {
@@ -417,26 +448,28 @@ await robot.setMaster(new AIAgentMaster(config));
 ## 🔧 Integration Guide
 
 ### Frontend Integration (Svelte)
+
 ```typescript
-import { robotManager } from '$lib/robot/RobotManager.svelte';
-import { robotUrdfConfigMap } from '$lib/configs/robotUrdfConfig';
+import { robotManager } from "$lib/robot/RobotManager.svelte";
+import { robotUrdfConfigMap } from "$lib/configs/robotUrdfConfig";
 
 // Create robot
-const robot = await robotManager.createRobot('my-robot', robotUrdfConfigMap['so-arm100']);
+const robot = await robotManager.createRobot("my-robot", robotUrdfConfigMap["so-arm100"]);
 
-// Add visualization 
-await robotManager.connectMockSlave('my-robot');
+// Add visualization
+await robotManager.connectMockSlave("my-robot");
 
 // Add control
-await robotManager.connectDemoSequences('my-robot', true);
+await robotManager.connectDemoSequences("my-robot", true);
 
 // Monitor state
-robot.joints.forEach(joint => {
-  console.log(`${joint.name}: virtual=${joint.virtualValue}° real=${joint.realValue}°`);
+robot.joints.forEach((joint) => {
+	console.log(`${joint.name}: virtual=${joint.virtualValue}° real=${joint.realValue}°`);
 });
 ```
 
 ### Backend Integration (Python)
+
 ```python
 import asyncio
 import websockets
@@ -455,7 +488,7 @@ async def robot_controller():
             }
         }
         await websocket.send(json.dumps(command))
-        
+
         # Listen for responses
         async for message in websocket:
             data = json.loads(message)
@@ -466,6 +499,7 @@ asyncio.run(robot_controller())
 ```
 
 ### Hardware Integration (Arduino/C++)
+
 ```cpp
 #include <WiFi.h>
 #include <WebSocketsClient.h>
@@ -477,7 +511,7 @@ void onWebSocketEvent(WStype_t type, uint8_t * payload, size_t length) {
     if (type == WStype_TEXT) {
         DynamicJsonDocument doc(1024);
         deserializeJson(doc, payload);
-        
+
         if (doc["type"] == "execute_command") {
             JsonArray joints = doc["data"]["joints"];
             for (JsonObject joint : joints) {
@@ -498,22 +532,28 @@ void setup() {
 ## 🛡️ Security & Production
 
 ### Authentication
+
 ```typescript
 // API key authentication (planned)
-const master = new RemoteServerMaster({
-  type: "remote-server",
-  url: "wss://secure-robot-farm.com:8080",
-  apiKey: "your-secret-api-key"
-}, robotId);
+const master = new RemoteServerMaster(
+	{
+		type: "remote-server",
+		url: "wss://secure-robot-farm.com:8080",
+		apiKey: "your-secret-api-key"
+	},
+	robotId
+);
 ```
 
 ### TLS/SSL
+
 ```bash
 # Production deployment with SSL
 uvicorn main:app --host 0.0.0.0 --port 443 --ssl-keyfile key.pem --ssl-certfile cert.pem
 ```
 
 ### Rate Limiting & Safety
+
 ```python
 # Built-in protections
 - Command rate limiting (100 commands/second max)
@@ -526,11 +566,13 @@ uvicorn main:app --host 0.0.0.0 --port 443 --ssl-keyfile key.pem --ssl-certfile
 ## 🚀 Deployment Options
 
 ### Development
+
 ```bash
 cd src-python && python start_server.py
 ```
 
 ### Docker
+
 ```dockerfile
 FROM python:3.12-slim
 COPY src-python/ /app/
@@ -541,12 +583,14 @@ CMD ["python", "start_server.py"]
 ```
 
 ### Cloud (Railway/Heroku)
+
 ```bash
 # Procfile
 web: cd src-python && python start_server.py
 ```
 
 ### Raspberry Pi (Edge)
+
 ```bash
 # systemd service for autostart
 sudo systemctl enable lerobot-arena
@@ -556,25 +600,28 @@ sudo systemctl start lerobot-arena
 ## 🧪 Testing & Debugging
 
 ### Unit Tests
+
 ```bash
 cd src-python
 pytest tests/ -v
 ```
 
 ### Integration Tests
+
 ```javascript
 // Frontend testing
-import { expect, test } from '@playwright/test';
+import { expect, test } from "@playwright/test";
 
-test('robot creation and control', async ({ page }) => {
-  await page.goto('/');
-  await page.click('[data-testid="create-robot"]');
-  await page.click('[data-testid="connect-demo-sequences"]');
-  await expect(page.locator('[data-testid="robot-status"]')).toContainText('Master + Slaves');
+test("robot creation and control", async ({ page }) => {
+	await page.goto("/");
+	await page.click('[data-testid="create-robot"]');
+	await page.click('[data-testid="connect-demo-sequences"]');
+	await expect(page.locator('[data-testid="robot-status"]')).toContainText("Master + Slaves");
 });
 ```
 
 ### Debug Mode
+
 ```bash
 # Enable verbose logging
 export LOG_LEVEL=DEBUG
@@ -586,6 +633,7 @@ npm run dev
 ```
 
 ### Health Monitoring
+
 ```bash
 # Check server health
 curl http://localhost:8080/
@@ -597,18 +645,21 @@ curl http://localhost:8080/api/robots
 ## 🔮 Roadmap
 
 ### v2.0 - Enhanced Control
+
 - [ ] **Script Player Master**: Execute Python/JS scripts
 - [ ] **Simulation Slave**: Physics-based simulation
 - [ ] **Force Control**: Torque and compliance modes
 - [ ] **Vision Integration**: Camera feeds and computer vision
 
 ### v2.1 - Enterprise Features
+
 - [ ] **Authentication**: JWT tokens and user management
 - [ ] **Multi-tenancy**: Isolated robot fleets per organization
 - [ ] **Monitoring**: Prometheus metrics and Grafana dashboards
 - [ ] **Recording**: Command sequences and replay
 
 ### v2.2 - Advanced Robotics
+
 - [ ] **Path Planning**: Trajectory optimization
 - [ ] **Collision Detection**: Safety in shared workspaces
 - [ ] **AI Integration**: Reinforcement learning environments
@@ -617,12 +668,13 @@ curl http://localhost:8080/api/robots
 ## 🤝 Contributing
 
 ### Development Setup
+
 ```bash
 # Frontend
 npm install
 npm run dev
 
-# Backend  
+# Backend
 cd src-python
 uv sync
 python start_server.py
@@ -633,11 +685,13 @@ cd src-python && pytest
 ```
 
 ### Code Style
+
 - **TypeScript**: ESLint + Prettier
 - **Python**: Black + isort + mypy
 - **Commits**: Conventional commits format
 
 ### Pull Request Process
+
 1. Fork repository
 2. Create feature branch
 3. Add tests for new functionality
@@ -653,4 +707,4 @@ MIT License - Feel free to use in commercial and personal projects.
 
 **Built with ❤️ for the robotics community**
 
-*LeRobot Arena bridges the gap between digital twins and physical robots, making robotics accessible to developers, researchers, and enthusiasts worldwide.*
+_LeRobot Arena bridges the gap between digital twins and physical robots, making robotics accessible to developers, researchers, and enthusiasts worldwide._

src/app.css

@@ -6,117 +6,117 @@
 @custom-variant dark (&:is(.dark *));
 
 :root {
-  --radius: 0.625rem;
-  --background: oklch(1 0 0);
-  --foreground: oklch(0.147 0.004 49.25);
-  --card: oklch(1 0 0);
-  --card-foreground: oklch(0.147 0.004 49.25);
-  --popover: oklch(1 0 0);
-  --popover-foreground: oklch(0.147 0.004 49.25);
-  --primary: oklch(0.216 0.006 56.043);
-  --primary-foreground: oklch(0.985 0.001 106.423);
-  --secondary: oklch(0.97 0.001 106.424);
-  --secondary-foreground: oklch(0.216 0.006 56.043);
-  --muted: oklch(0.97 0.001 106.424);
-  --muted-foreground: oklch(0.553 0.013 58.071);
-  --accent: oklch(0.97 0.001 106.424);
-  --accent-foreground: oklch(0.216 0.006 56.043);
-  --destructive: oklch(0.577 0.245 27.325);
-  --border: oklch(0.923 0.003 48.717);
-  --input: oklch(0.923 0.003 48.717);
-  --ring: oklch(0.709 0.01 56.259);
-  --chart-1: oklch(0.646 0.222 41.116);
-  --chart-2: oklch(0.6 0.118 184.704);
-  --chart-3: oklch(0.398 0.07 227.392);
-  --chart-4: oklch(0.828 0.189 84.429);
-  --chart-5: oklch(0.769 0.188 70.08);
-  --sidebar: oklch(0.985 0.001 106.423);
-  --sidebar-foreground: oklch(0.147 0.004 49.25);
-  --sidebar-primary: oklch(0.216 0.006 56.043);
-  --sidebar-primary-foreground: oklch(0.985 0.001 106.423);
-  --sidebar-accent: oklch(0.97 0.001 106.424);
-  --sidebar-accent-foreground: oklch(0.216 0.006 56.043);
-  --sidebar-border: oklch(0.923 0.003 48.717);
-  --sidebar-ring: oklch(0.709 0.01 56.259);
+	--radius: 0.625rem;
+	--background: oklch(1 0 0);
+	--foreground: oklch(0.147 0.004 49.25);
+	--card: oklch(1 0 0);
+	--card-foreground: oklch(0.147 0.004 49.25);
+	--popover: oklch(1 0 0);
+	--popover-foreground: oklch(0.147 0.004 49.25);
+	--primary: oklch(0.216 0.006 56.043);
+	--primary-foreground: oklch(0.985 0.001 106.423);
+	--secondary: oklch(0.97 0.001 106.424);
+	--secondary-foreground: oklch(0.216 0.006 56.043);
+	--muted: oklch(0.97 0.001 106.424);
+	--muted-foreground: oklch(0.553 0.013 58.071);
+	--accent: oklch(0.97 0.001 106.424);
+	--accent-foreground: oklch(0.216 0.006 56.043);
+	--destructive: oklch(0.577 0.245 27.325);
+	--border: oklch(0.923 0.003 48.717);
+	--input: oklch(0.923 0.003 48.717);
+	--ring: oklch(0.709 0.01 56.259);
+	--chart-1: oklch(0.646 0.222 41.116);
+	--chart-2: oklch(0.6 0.118 184.704);
+	--chart-3: oklch(0.398 0.07 227.392);
+	--chart-4: oklch(0.828 0.189 84.429);
+	--chart-5: oklch(0.769 0.188 70.08);
+	--sidebar: oklch(0.985 0.001 106.423);
+	--sidebar-foreground: oklch(0.147 0.004 49.25);
+	--sidebar-primary: oklch(0.216 0.006 56.043);
+	--sidebar-primary-foreground: oklch(0.985 0.001 106.423);
+	--sidebar-accent: oklch(0.97 0.001 106.424);
+	--sidebar-accent-foreground: oklch(0.216 0.006 56.043);
+	--sidebar-border: oklch(0.923 0.003 48.717);
+	--sidebar-ring: oklch(0.709 0.01 56.259);
 }
 
 .dark {
-  --background: oklch(0.147 0.004 49.25);
-  --foreground: oklch(0.985 0.001 106.423);
-  --card: oklch(0.216 0.006 56.043);
-  --card-foreground: oklch(0.985 0.001 106.423);
-  --popover: oklch(0.216 0.006 56.043);
-  --popover-foreground: oklch(0.985 0.001 106.423);
-  --primary: oklch(0.923 0.003 48.717);
-  --primary-foreground: oklch(0.216 0.006 56.043);
-  --secondary: oklch(0.268 0.007 34.298);
-  --secondary-foreground: oklch(0.985 0.001 106.423);
-  --muted: oklch(0.268 0.007 34.298);
-  --muted-foreground: oklch(0.709 0.01 56.259);
-  --accent: oklch(0.268 0.007 34.298);
-  --accent-foreground: oklch(0.985 0.001 106.423);
-  --destructive: oklch(0.704 0.191 22.216);
-  --border: oklch(1 0 0 / 10%);
-  --input: oklch(1 0 0 / 15%);
-  --ring: oklch(0.553 0.013 58.071);
-  --chart-1: oklch(0.488 0.243 264.376);
-  --chart-2: oklch(0.696 0.17 162.48);
-  --chart-3: oklch(0.769 0.188 70.08);
-  --chart-4: oklch(0.627 0.265 303.9);
-  --chart-5: oklch(0.645 0.246 16.439);
-  --sidebar: oklch(0.216 0.006 56.043);
-  --sidebar-foreground: oklch(0.985 0.001 106.423);
-  --sidebar-primary: oklch(0.488 0.243 264.376);
-  --sidebar-primary-foreground: oklch(0.985 0.001 106.423);
-  --sidebar-accent: oklch(0.268 0.007 34.298);
-  --sidebar-accent-foreground: oklch(0.985 0.001 106.423);
-  --sidebar-border: oklch(1 0 0 / 10%);
-  --sidebar-ring: oklch(0.553 0.013 58.071);
+	--background: oklch(0.147 0.004 49.25);
+	--foreground: oklch(0.985 0.001 106.423);
+	--card: oklch(0.216 0.006 56.043);
+	--card-foreground: oklch(0.985 0.001 106.423);
+	--popover: oklch(0.216 0.006 56.043);
+	--popover-foreground: oklch(0.985 0.001 106.423);
+	--primary: oklch(0.923 0.003 48.717);
+	--primary-foreground: oklch(0.216 0.006 56.043);
+	--secondary: oklch(0.268 0.007 34.298);
+	--secondary-foreground: oklch(0.985 0.001 106.423);
+	--muted: oklch(0.268 0.007 34.298);
+	--muted-foreground: oklch(0.709 0.01 56.259);
+	--accent: oklch(0.268 0.007 34.298);
+	--accent-foreground: oklch(0.985 0.001 106.423);
+	--destructive: oklch(0.704 0.191 22.216);
+	--border: oklch(1 0 0 / 10%);
+	--input: oklch(1 0 0 / 15%);
+	--ring: oklch(0.553 0.013 58.071);
+	--chart-1: oklch(0.488 0.243 264.376);
+	--chart-2: oklch(0.696 0.17 162.48);
+	--chart-3: oklch(0.769 0.188 70.08);
+	--chart-4: oklch(0.627 0.265 303.9);
+	--chart-5: oklch(0.645 0.246 16.439);
+	--sidebar: oklch(0.216 0.006 56.043);
+	--sidebar-foreground: oklch(0.985 0.001 106.423);
+	--sidebar-primary: oklch(0.488 0.243 264.376);
+	--sidebar-primary-foreground: oklch(0.985 0.001 106.423);
+	--sidebar-accent: oklch(0.268 0.007 34.298);
+	--sidebar-accent-foreground: oklch(0.985 0.001 106.423);
+	--sidebar-border: oklch(1 0 0 / 10%);
+	--sidebar-ring: oklch(0.553 0.013 58.071);
 }
 
 @theme inline {
-  --radius-sm: calc(var(--radius) - 4px);
-  --radius-md: calc(var(--radius) - 2px);
-  --radius-lg: var(--radius);
-  --radius-xl: calc(var(--radius) + 4px);
-  --color-background: var(--background);
-  --color-foreground: var(--foreground);
-  --color-card: var(--card);
-  --color-card-foreground: var(--card-foreground);
-  --color-popover: var(--popover);
-  --color-popover-foreground: var(--popover-foreground);
-  --color-primary: var(--primary);
-  --color-primary-foreground: var(--primary-foreground);
-  --color-secondary: var(--secondary);
-  --color-secondary-foreground: var(--secondary-foreground);
-  --color-muted: var(--muted);
-  --color-muted-foreground: var(--muted-foreground);
-  --color-accent: var(--accent);
-  --color-accent-foreground: var(--accent-foreground);
-  --color-destructive: var(--destructive);
-  --color-border: var(--border);
-  --color-input: var(--input);
-  --color-ring: var(--ring);
-  --color-chart-1: var(--chart-1);
-  --color-chart-2: var(--chart-2);
-  --color-chart-3: var(--chart-3);
-  --color-chart-4: var(--chart-4);
-  --color-chart-5: var(--chart-5);
-  --color-sidebar: var(--sidebar);
-  --color-sidebar-foreground: var(--sidebar-foreground);
-  --color-sidebar-primary: var(--sidebar-primary);
-  --color-sidebar-primary-foreground: var(--sidebar-primary-foreground);
-  --color-sidebar-accent: var(--sidebar-accent);
-  --color-sidebar-accent-foreground: var(--sidebar-accent-foreground);
-  --color-sidebar-border: var(--sidebar-border);
-  --color-sidebar-ring: var(--sidebar-ring);
+	--radius-sm: calc(var(--radius) - 4px);
+	--radius-md: calc(var(--radius) - 2px);
+	--radius-lg: var(--radius);
+	--radius-xl: calc(var(--radius) + 4px);
+	--color-background: var(--background);
+	--color-foreground: var(--foreground);
+	--color-card: var(--card);
+	--color-card-foreground: var(--card-foreground);
+	--color-popover: var(--popover);
+	--color-popover-foreground: var(--popover-foreground);
+	--color-primary: var(--primary);
+	--color-primary-foreground: var(--primary-foreground);
+	--color-secondary: var(--secondary);
+	--color-secondary-foreground: var(--secondary-foreground);
+	--color-muted: var(--muted);
+	--color-muted-foreground: var(--muted-foreground);
+	--color-accent: var(--accent);
+	--color-accent-foreground: var(--accent-foreground);
+	--color-destructive: var(--destructive);
+	--color-border: var(--border);
+	--color-input: var(--input);
+	--color-ring: var(--ring);
+	--color-chart-1: var(--chart-1);
+	--color-chart-2: var(--chart-2);
+	--color-chart-3: var(--chart-3);
+	--color-chart-4: var(--chart-4);
+	--color-chart-5: var(--chart-5);
+	--color-sidebar: var(--sidebar);
+	--color-sidebar-foreground: var(--sidebar-foreground);
+	--color-sidebar-primary: var(--sidebar-primary);
+	--color-sidebar-primary-foreground: var(--sidebar-primary-foreground);
+	--color-sidebar-accent: var(--sidebar-accent);
+	--color-sidebar-accent-foreground: var(--sidebar-accent-foreground);
+	--color-sidebar-border: var(--sidebar-border);
+	--color-sidebar-ring: var(--sidebar-ring);
 }
 
 @layer base {
-  * {
-    @apply border-border outline-ring/50;
-  }
-  body {
-    @apply bg-background text-foreground;
-  }
-}
+	* {
+		@apply border-border outline-ring/50;
+	}
+	body {
+		@apply bg-background text-foreground;
+	}
+}

src/lib/components/3d/GridCustom.svelte

@@ -0,0 +1,379 @@
+<!-- Credits to Fyrestar for the https://github.com/Fyrestar/THREE.InfiniteGridHelper  -->
+<script lang="ts">
+	import { T, useTask, useThrelte } from "@threlte/core";
+	import { Color, DoubleSide, Plane, Vector3, Mesh } from "three";
+	import * as THREE from "three";
+
+	// Grid shader code with improved precision and stability
+	import { revision } from "@threlte/core";
+
+	// Props
+	let {
+		cellColor = "#71717A",
+		sectionColor = "#707070",
+		cellSize = 1,
+		backgroundColor = "#000000",
+		backgroundOpacity = 0,
+		sectionSize = 10,
+		plane = "xz",
+		gridSize = [20, 20],
+		followCamera = false,
+		infiniteGrid = false,
+		fadeDistance = 100,
+		fadeStrength = 1,
+		fadeOrigin = undefined,
+		cellThickness = 1,
+		sectionThickness = 2,
+		side = DoubleSide,
+		type = "grid",
+		axis = "x",
+		maxRadius = 0,
+		cellDividers = 6,
+		sectionDividers = 2,
+		floorColor = "#2a2a2a",
+		floorOpacity = 0.3,
+		ref = $bindable(),
+		children = undefined,
+		...props
+	} = $props();
+
+	// Shared fade calculation function for both shaders
+	const fadeCalculation = /*glsl*/ `
+		float calculateFade(vec3 worldPos, float viewZ, vec3 fadeOrigin, float fadeDistance, float fadeStrength, float cameraNear, float cameraFar) {
+			float dist = distance(fadeOrigin, worldPos);
+			float fadeFactor = 1.0 - clamp(dist / fadeDistance, 0.0, 1.0);
+			fadeFactor = pow(fadeFactor, fadeStrength);
+			
+			float viewDepthFade = 1.0 - clamp((viewZ - cameraNear) / (cameraFar - cameraNear), 0.0, 1.0);
+			viewDepthFade = smoothstep(0.0, 0.3, viewDepthFade);
+			
+			return min(fadeFactor, viewDepthFade);
+		}
+	`;
+
+	const vertexShader = /*glsl*/ `
+		varying vec3 localPosition;
+		varying vec4 worldPosition;
+		varying float vViewZ;
+
+		uniform vec3 worldCamProjPosition;
+		uniform vec3 worldPlanePosition;
+		uniform float fadeDistance;
+		uniform bool infiniteGrid;
+		uniform bool followCamera;
+		uniform int coord0, coord1, coord2;
+
+		void main() {
+			localPosition = vec3(position[coord0], position[coord1], position[coord2]);
+			if (infiniteGrid) localPosition *= 1.0 + fadeDistance;
+
+			worldPosition = modelMatrix * vec4(localPosition, 1.0);
+			if (followCamera) {
+				worldPosition.xyz += (worldCamProjPosition - worldPlanePosition);
+				localPosition = (inverse(modelMatrix) * worldPosition).xyz;
+			}
+
+			vec4 mvPosition = viewMatrix * worldPosition;
+			vViewZ = -mvPosition.z;
+			gl_Position = projectionMatrix * mvPosition;
+		}
+	`;
+
+	const fragmentShader = /*glsl*/ `
+		#define PI 3.141592653589793
+		
+		varying vec3 localPosition;
+		varying vec4 worldPosition;
+		varying float vViewZ;
+
+		uniform float cellSize, sectionSize, cellThickness, sectionThickness;
+		uniform vec3 cellColor, sectionColor, backgroundColor, fadeOrigin;
+		uniform float backgroundOpacity, fadeDistance, fadeStrength, cameraNear, cameraFar;
+		uniform bool infiniteGrid;
+		uniform int coord0, coord1, coord2, gridType, lineGridCoord;
+		uniform float circleGridMaxRadius, polarCellDividers, polarSectionDividers;
+
+		${fadeCalculation}
+
+		float getSquareGrid(float size, float thickness, vec3 localPos) {
+			vec2 coord = localPos.xy / size;
+			vec2 derivative = fwidth(coord);
+			vec2 grid = abs(fract(coord - 0.5) - 0.5) / derivative;
+			float line = min(grid.x, grid.y) + 1.0 - thickness;
+			return clamp(1.0 - line, 0.0, 1.0);
+		}
+
+		float getLinesGrid(float size, float thickness, vec3 localPos) {
+			float coord = localPos[lineGridCoord] / size;
+			float derivative = fwidth(coord);
+			float line = abs(fract(coord - 0.5) - 0.5) / derivative - thickness * 0.5;
+			return clamp(1.0 - line, 0.0, 1.0);
+		}
+
+		float getCirclesGrid(float size, float thickness, vec3 localPos) {
+			float coord = length(localPos.xy) / size;
+			float derivative = fwidth(coord);
+			float line = abs(fract(coord - 0.5) - 0.5) / derivative - thickness * 0.5;
+			if (!infiniteGrid && circleGridMaxRadius > 0.0 && coord > circleGridMaxRadius + thickness * 0.1) discard;
+			return clamp(1.0 - line, 0.0, 1.0);
+		}
+
+		float getPolarGrid(float size, float thickness, float polarDividers, vec3 localPos) {
+			float rad = length(localPos.xy) / size;
+			vec2 coord = vec2(rad, atan(localPos.x, localPos.y) * polarDividers / PI);
+			vec2 derivative = fwidth(coord);
+			vec2 grid = abs(fract(coord - 0.5) - 0.5) / derivative;
+			float line = min(grid.x, grid.y) + 1.0 - thickness;
+			if (!infiniteGrid && circleGridMaxRadius > 0.0 && rad > circleGridMaxRadius + thickness * 0.1) discard;
+			return clamp(1.0 - line, 0.0, 1.0);
+		}
+
+		void main() {
+			float g1 = 0.0, g2 = 0.0;
+			vec3 localPos = vec3(localPosition[coord0], localPosition[coord1], localPosition[coord2]);
+
+			if (gridType == 0) {
+				g1 = getSquareGrid(cellSize, cellThickness, localPos);
+				g2 = getSquareGrid(sectionSize, sectionThickness, localPos);
+			} else if (gridType == 1) {
+				g1 = getLinesGrid(cellSize, cellThickness, localPos);
+				g2 = getLinesGrid(sectionSize, sectionThickness, localPos);
+			} else if (gridType == 2) {
+				g1 = getCirclesGrid(cellSize, cellThickness, localPos);
+				g2 = getCirclesGrid(sectionSize, sectionThickness, localPos);
+			} else if (gridType == 3) {
+				g1 = getPolarGrid(cellSize, cellThickness, polarCellDividers, localPos);
+				g2 = getPolarGrid(sectionSize, sectionThickness, polarSectionDividers, localPos);
+			}
+
+			float fadeFactor = calculateFade(worldPosition.xyz, vViewZ, fadeOrigin, fadeDistance, fadeStrength, cameraNear, cameraFar);
+			vec3 color = mix(cellColor, sectionColor, clamp(sectionThickness * g2, 0.0, 1.0));
+			float gridAlpha = clamp((g1 + g2) * fadeFactor, 0.0, 1.0);
+
+			if (backgroundOpacity > 0.0) {
+				vec3 finalColor = mix(backgroundColor, color, gridAlpha);
+				float blendedAlpha = clamp(max(gridAlpha, backgroundOpacity * fadeFactor), 0.0, 1.0);
+				gl_FragColor = vec4(finalColor, blendedAlpha);
+			} else {
+				gl_FragColor = vec4(color, gridAlpha);
+			}
+
+			if (gl_FragColor.a < 0.05) discard;
+
+			#include <tonemapping_fragment>
+			#include <${revision < 154 ? "encodings_fragment" : "colorspace_fragment"}>
+		}
+	`;
+
+	// Simple floor shader
+	const floorVertexShader = /*glsl*/ `
+		varying vec3 vWorldPosition;
+		varying float vViewZ;
+		
+		void main() {
+			vec4 worldPosition = modelMatrix * vec4(position, 1.0);
+			vWorldPosition = worldPosition.xyz;
+			vec4 mvPosition = viewMatrix * worldPosition;
+			vViewZ = -mvPosition.z;
+			gl_Position = projectionMatrix * mvPosition;
+		}
+	`;
+
+	const floorFragmentShader = /*glsl*/ `
+		uniform vec3 floorColor, fadeOrigin;
+		uniform float floorOpacity, fadeDistance, fadeStrength, cameraNear, cameraFar;
+		varying vec3 vWorldPosition;
+		varying float vViewZ;
+		
+		${fadeCalculation}
+		
+		void main() {
+			float fadeFactor = calculateFade(vWorldPosition, vViewZ, fadeOrigin, fadeDistance, fadeStrength, cameraNear, cameraFar);
+			float finalOpacity = floorOpacity * fadeFactor;
+			gl_FragColor = vec4(floorColor, finalOpacity);
+			if (gl_FragColor.a < 0.01) discard;
+		}
+	`;
+
+	const mesh = new Mesh();
+	const { invalidate, camera } = useThrelte();
+	const gridPlane = new Plane();
+	const upVector = new Vector3(0, 1, 0);
+	const zeroVector = new Vector3(0, 0, 0);
+
+	const axisToInt: Record<string, number> = { x: 0, y: 1, z: 2 };
+	const planeToAxes: Record<string, string> = { xz: "xzy", xy: "xyz", zy: "zyx" };
+	const gridType = { grid: 0, lines: 1, circular: 2, polar: 3 };
+
+	// Shared uniforms (used by both grid and floor)
+	const sharedUniforms = {
+		fadeOrigin: { value: new Vector3() },
+		fadeDistance: { value: fadeDistance },
+		fadeStrength: { value: fadeStrength },
+		cameraNear: { value: 0.1 },
+		cameraFar: { value: 1000 }
+	};
+
+	// Grid uniforms
+	const uniforms = {
+		...sharedUniforms,
+		cellSize: { value: cellSize },
+		sectionSize: { value: sectionSize },
+		cellColor: { value: new Color(cellColor) },
+		sectionColor: { value: new Color(sectionColor) },
+		backgroundColor: { value: new Color(backgroundColor) },
+		backgroundOpacity: { value: backgroundOpacity },
+		cellThickness: { value: cellThickness },
+		sectionThickness: { value: sectionThickness },
+		infiniteGrid: { value: infiniteGrid },
+		followCamera: { value: followCamera },
+		coord0: { value: 0 },
+		coord1: { value: 2 },
+		coord2: { value: 1 },
+		gridType: { value: gridType.grid },
+		lineGridCoord: { value: axisToInt[axis as keyof typeof axisToInt] || 0 },
+		circleGridMaxRadius: { value: maxRadius },
+		polarCellDividers: { value: cellDividers },
+		polarSectionDividers: { value: sectionDividers },
+		worldCamProjPosition: { value: new Vector3() },
+		worldPlanePosition: { value: new Vector3() }
+	};
+
+	// Floor uniforms (simpler, reusing shared uniforms)
+	const floorUniforms = {
+		...sharedUniforms,
+		floorColor: { value: new Color(floorColor) },
+		floorOpacity: { value: floorOpacity }
+	};
+
+	// Single update effect for all uniforms
+	$effect.pre(() => {
+		const axes = planeToAxes[plane] || "xzy";
+		const [c0, c1, c2] = [axes.charAt(0), axes.charAt(1), axes.charAt(2)].map(
+			(c) => axisToInt[c as keyof typeof axisToInt]
+		);
+
+		// Update grid uniforms
+		Object.assign(uniforms, {
+			coord0: { value: c0 },
+			coord1: { value: c1 },
+			coord2: { value: c2 },
+			cellSize: { value: cellSize },
+			sectionSize: { value: sectionSize },
+			cellThickness: { value: cellThickness },
+			sectionThickness: { value: sectionThickness },
+			backgroundOpacity: { value: backgroundOpacity },
+			infiniteGrid: { value: infiniteGrid },
+			followCamera: { value: followCamera }
+		});
+
+		uniforms.cellColor.value.set(cellColor);
+		uniforms.sectionColor.value.set(sectionColor);
+		uniforms.backgroundColor.value.set(backgroundColor);
+
+		// Update shared uniforms (affects both grid and floor)
+		sharedUniforms.fadeDistance.value = fadeDistance;
+		sharedUniforms.fadeStrength.value = fadeStrength;
+		floorUniforms.floorColor.value.set(floorColor);
+		floorUniforms.floorOpacity.value = floorOpacity;
+
+		// Update camera uniforms
+		if (camera.current && "near" in camera.current && "far" in camera.current) {
+			const cam = camera.current as THREE.PerspectiveCamera;
+			sharedUniforms.cameraNear.value = cam.near;
+			sharedUniforms.cameraFar.value = cam.far;
+		}
+
+		// Update grid type
+		const typeMap = { grid: 0, lines: 1, circular: 2, polar: 3 };
+		uniforms.gridType.value = typeMap[type as keyof typeof typeMap] || 0;
+		if (type === "lines")
+			uniforms.lineGridCoord.value = axisToInt[axis as keyof typeof axisToInt] || 0;
+		if (type === "circular" || type === "polar") {
+			uniforms.circleGridMaxRadius.value = maxRadius;
+			if (type === "polar") {
+				uniforms.polarCellDividers.value = cellDividers;
+				uniforms.polarSectionDividers.value = sectionDividers;
+			}
+		}
+
+		invalidate();
+	});
+
+	// Single task for both grid and floor fade origins
+	useTask(
+		() => {
+			gridPlane.setFromNormalAndCoplanarPoint(upVector, zeroVector).applyMatrix4(mesh.matrixWorld);
+			const material = mesh.material as THREE.ShaderMaterial;
+			if (material?.uniforms) {
+				const { worldCamProjPosition, worldPlanePosition } = material.uniforms;
+				const projectedPoint = gridPlane.projectPoint(
+					camera.current.position,
+					worldCamProjPosition.value
+				);
+				if (!fadeOrigin) sharedUniforms.fadeOrigin.value = projectedPoint;
+				worldPlanePosition.value.set(0, 0, 0).applyMatrix4(mesh.matrixWorld);
+			}
+		},
+		{ autoInvalidate: false }
+	);
+</script>
+
+<!-- Shadow-receiving floor underneath -->
+<T.Mesh rotation={[-Math.PI / 2, 0, 0]} receiveShadow position.y={0} {...props}>
+	<T.PlaneGeometry
+		args={infiniteGrid
+			? [1000, 1000]
+			: typeof gridSize == "number"
+				? [gridSize, gridSize]
+				: gridSize}
+	/>
+	<T.ShadowMaterial
+		transparent={true}
+		opacity={0.3}
+		polygonOffset={true}
+		polygonOffsetFactor={1}
+		polygonOffsetUnits={1}
+	/>
+</T.Mesh>
+
+<!-- Fading floor -->
+<T.Mesh rotation={[-Math.PI / 2, 0, 0]} position.y={0} {...props}>
+	<T.PlaneGeometry
+		args={infiniteGrid
+			? [1000, 1000]
+			: typeof gridSize == "number"
+				? [gridSize, gridSize]
+				: gridSize}
+	/>
+	<T.ShaderMaterial
+		vertexShader={floorVertexShader}
+		fragmentShader={floorFragmentShader}
+		uniforms={floorUniforms}
+		transparent={true}
+		side={THREE.DoubleSide}
+		depthTest={true}
+		depthWrite={false}
+		polygonOffset={true}
+		polygonOffsetFactor={-1}
+		polygonOffsetUnits={-1}
+	/>
+</T.Mesh>
+
+<!-- Grid lines -->
+<T is={mesh} bind:ref frustumCulled={false} position.y={0.005} {...props}>
+	<T.ShaderMaterial
+		{fragmentShader}
+		{vertexShader}
+		{uniforms}
+		transparent
+		{side}
+		depthTest={true}
+		depthWrite={false}
+	/>
+	{#if children}
+		{@render children({ ref: mesh })}
+	{:else}
+		<T.PlaneGeometry args={typeof gridSize == "number" ? [gridSize, gridSize] : gridSize} />
+	{/if}
+</T>

src/lib/components/3d/Robot.svelte

@@ -1,53 +1,506 @@
 <script lang="ts">
-	import { T } from '@threlte/core';
-	import { getRootLinks } from '$lib/components/3d/robot/URDF/utils/UrdfParser';
-	import UrdfLink from '$lib/components/3d/robot/URDF/primitives/UrdfLink.svelte';
-	import { robotManager } from '$lib/robot/RobotManager.svelte';
-	import Selectable from './Selectable.svelte';
+	import * as THREE from "three";
+	import { T, useThrelte } from "@threlte/core";
+	import { getRootLinks } from "@/components/3d/robot/URDF/utils/UrdfParser";
+	import UrdfLink from "@/components/3d/robot/URDF/primitives/UrdfLink.svelte";
+	import { robotManager } from "$lib/robot/RobotManager.svelte";
+	import { Billboard, HTML } from "@threlte/extras";
+	import { scale } from "svelte/transition";
+	import { onMount } from "svelte";
+	import { robotUrdfConfigMap } from "@/configs/robotUrdfConfig";
+	import MasterConnectionModal from "@/components/interface/overlay/MasterConnectionModal.svelte";
+	import SlaveConnectionModal from "@/components/interface/overlay/SlaveConnectionModal.svelte";
+	import ManualControlModal from "@/components/interface/overlay/ManualControlSheet.svelte";
+	import type { Robot } from "$lib/robot/Robot.svelte";
+	import Hoverable from "@/components/utils/Hoverable.svelte";
+	import { generateName } from "$lib/utils/generateName";
 
 	interface Props {}
 
 	let {}: Props = $props();
 
+	// Get camera controls
+	const { camera } = useThrelte();
+
+	// Modal state using runes
+	let isMasterModalOpen = $state(false);
+	let isSlaveModalOpen = $state(false);
+	let isManualControlModalOpen = $state(false);
+	let selectedRobot = $state<Robot | null>(null);
+
 	// Get all robots from the manager
 	const robots = $derived(robotManager.robots);
+
+	// Helper function to get connection status
+	function getConnectionStatus(robot: any) {
+		const status = robotManager.getRobotStatus(robot.id);
+		if (!status) return "offline";
+
+		if (status.hasActiveMaster && status.connectedSlaves > 0) {
+			return "active";
+		} else if (status.hasActiveMaster) {
+			return "Master Only";
+		} else if (status.connectedSlaves > 0) {
+			return "Slave Only";
+		}
+		return "idle";
+	}
+
+	// Helper function to get status color
+	function getStatusVariant(status: string) {
+		switch (status) {
+			case "Master + Slaves":
+				return "default"; // Green
+			case "Master Only":
+				return "secondary"; // Yellow
+			case "Slave Only":
+				return "outline"; // Blue
+			default:
+				return "destructive"; // Red/Gray
+		}
+	}
+
+	// Camera movement function
+	function moveCameraToRobot(robot: Robot, index: number) {
+		if (!camera.current) return;
+
+		// Calculate robot position (same logic as used in the template)
+		const gridWidth = 3;
+		const spacing = 6;
+		const totalRows = Math.ceil(robots.length / gridWidth);
+		const row = Math.floor(index / gridWidth);
+		const col = 1 + (index % gridWidth);
+		const xPosition = (col - Math.floor(gridWidth / 2)) * spacing;
+		const zPosition = (row - Math.floor(totalRows / 2)) * spacing;
+
+		// Camera positioning parameters - adjust these to change the rotation angle
+		const cameraDistance = 12; // Distance from robot
+		const cameraHeight = 8; // Height above ground
+		const angleOffset = Math.PI / 4; // 45 degrees - change this for different angles
+
+		// Calculate camera position using polar coordinates for easy angle control
+		const cameraX = xPosition + Math.cos(angleOffset) * cameraDistance;
+		const cameraZ = zPosition + Math.sin(angleOffset) * cameraDistance;
+
+		// Create target positions
+		const targetPosition = new THREE.Vector3(cameraX, cameraHeight, cameraZ);
+		const lookAtPosition = new THREE.Vector3(xPosition, 1, zPosition); // Look at robot center
+
+		// Animate camera movement
+		const startPosition = camera.current.position.clone();
+		const startTime = Date.now();
+		const duration = 1000; // 1 second animation
+
+		function animateCamera() {
+			const elapsed = Date.now() - startTime;
+			const progress = Math.min(elapsed / duration, 1);
+
+			// Use easing function for smooth animation
+			const easeProgress = 1 - Math.pow(1 - progress, 3); // easeOutCubic
+
+			// Interpolate position
+			camera.current.position.lerpVectors(startPosition, targetPosition, easeProgress);
+			camera.current.lookAt(lookAtPosition);
+
+			if (progress < 1) {
+				requestAnimationFrame(animateCamera);
+			}
+		}
+
+		animateCamera();
+	}
+
+	// Modal management functions
+	function openMasterModal(robot: Robot) {
+		console.log("Opening master modal for robot:", robot.id);
+		selectedRobot = robot;
+		isMasterModalOpen = true;
+	}
+
+	function openSlaveModal(robot: Robot) {
+		console.log("Opening slave modal for robot:", robot.id);
+		selectedRobot = robot;
+		isSlaveModalOpen = true;
+	}
+
+	function openManualControlModal(robot: Robot) {
+		console.log("Opening manual control modal for robot:", robot.id);
+		selectedRobot = robot;
+		isManualControlModalOpen = true;
+	}
+
+	// Handle stop propagation for nested buttons
+	function handleAddButtonClick(event: Event, robot: Robot, tab: "master" | "slaves") {
+		console.log("Handling add button click for robot:", robot.id, "tab:", tab);
+		event.stopPropagation();
+		if (tab === "master") {
+			openMasterModal(robot);
+		} else {
+			openSlaveModal(robot);
+		}
+	}
+
+	// Handle box clicks (using mousedown since it works reliably in 3D context)
+	function handleBoxClick(robot: Robot, type: "master" | "slaves" | "manual") {
+		console.log("Box clicked:", type, "for robot:", robot.id);
+		if (type === "master") {
+			openMasterModal(robot);
+		} else if (type === "slaves") {
+			openSlaveModal(robot);
+		} else if (type === "manual") {
+			openManualControlModal(robot);
+		}
+	}
+
+	onMount(() => {
+		function createRobot() {
+			const urdfConfig = robotUrdfConfigMap["so-arm100"];
+
+			if (!urdfConfig) {
+				return;
+			}
+
+			const robotId = generateName();
+			console.log("Creating robot with ID:", robotId, "and config:", urdfConfig);
+
+			robotManager.createRobot(robotId, urdfConfig);
+		}
+
+		// If no robot then create one
+		if (robots.length === 0) {
+			createRobot();
+		}
+	});
+
+	function generateRobotName() {
+		throw new Error("Function not implemented.");
+	}
 </script>
 
 {#each robots as robot, index (robot.id)}
-	{@const xPosition = index * 5}
-	<T.Group position.x={xPosition} position.y={0} position.z={0} quaternion={[0, 0, 0, 1]} scale={[10, 10, 10]} rotation={[-Math.PI / 2, 0, 0]}>
-		<Selectable
-			hoverColor="#ff6b6b"
-			hoverOpacity={0.5}
-			enableEdit={true}
+	{@const gridWidth = 3}
+	<!-- Number of robots per row -->
+	{@const spacing = 6}
+	<!-- Space between robots -->
+	{@const totalRows = Math.ceil(robots.length / gridWidth)}
+	{@const row = Math.floor(index / gridWidth)}
+	{@const col = 1 + (index % gridWidth)}
+	{@const xPosition = (col - Math.floor(gridWidth / 2)) * spacing}
+	<!-- Center the grid on x-axis -->
+	{@const zPosition = (row - Math.floor(totalRows / 2)) * spacing}
+	<!-- Center the grid on z-axis -->
+	{@const robotStatus = robotManager.getRobotStatus(robot.id)}
+	{@const connectionStatus = getConnectionStatus(robot)}
+	{@const statusVariant = getStatusVariant(connectionStatus)}
+	<T.Group
+		position.x={xPosition}
+		position.y={0}
+		position.z={zPosition}
+		quaternion={[0, 0, 0, 1]}
+		scale={[10, 10, 10]}
+		rotation={[-Math.PI / 2, 0, 0]}
+	>
+		<Hoverable
+			onClick={() => {
+				moveCameraToRobot(robot, index);
+				handleBoxClick(robot, "manual");
+			}}
 		>
-		{#snippet content({ isHighlighted })}
-			{#each getRootLinks(robot.robotState.robot) as link}
-				<UrdfLink
-					robot={robot.robotState.robot}
-					{link}
-					textScale={0.2}
-					showName={isHighlighted}
-					showVisual={true}
-					showCollision={false}
-					visualColor="#333333"
-					visualOpacity={isHighlighted ? 0.1 : 1.0}
-					collisionOpacity={1.0}
-					collisionColor="#813d9c"
-					jointNames={isHighlighted}
-					joints={isHighlighted}
-					jointColor="#62a0ea"
-					jointIndicatorColor="#f66151"
-					nameHeight={0.1}
-					selectedLink={robot.robotState.selection.selectedLink}
-					selectedJoint={robot.robotState.selection.selectedJoint}
-					highlightColor="#ffa348"
-					showLine={isHighlighted}
-					opacity={1}
-					isInteractive={false}
-				/>
-			{/each}
-		{/snippet}
-		</Selectable>
+			{#snippet content({ isHovered, isSelected })}
+				{#each getRootLinks(robot.robotState.robot) as link}
+					<UrdfLink
+						robot={robot.robotState.robot}
+						{link}
+						textScale={0.2}
+						showName={isHovered || isSelected}
+						showVisual={true}
+						showCollision={false}
+						visualColor="#333333"
+						visualOpacity={isHovered || isSelected ? 0.4 : 1.0}
+						collisionOpacity={1.0}
+						collisionColor="#813d9c"
+						jointNames={isHovered || isSelected}
+						joints={isHovered || isSelected}
+						jointColor="#62a0ea"
+						jointIndicatorColor="#f66151"
+						nameHeight={0.1}
+						selectedLink={robot.robotState.selection.selectedLink}
+						selectedJoint={robot.robotState.selection.selectedJoint}
+						highlightColor="#ffa348"
+						showLine={isHovered || isSelected}
+						opacity={1}
+						isInteractive={false}
+					/>
+				{/each}
+
+				<T.Group position.z={0.25} rotation={[Math.PI / 2, 0, 0]} scale={[0.12, 0.12, 0.12]}>
+					<Billboard>
+						<HTML
+							transform
+							autoRender={true}
+							center={true}
+							distanceFactor={3}
+							pointerEvents="auto"
+							style="
+								pointer-events: auto !important;
+								image-rendering: auto;
+								image-rendering: smooth;
+								text-rendering: optimizeLegibility;
+								-webkit-font-smoothing: subpixel-antialiased;
+								-moz-osx-font-smoothing: auto;
+								backface-visibility: hidden;
+								transform-style: preserve-3d;
+								will-change: transform;
+							"
+						>
+							{#if isHovered || isSelected}
+								<div
+									class="pointer-events-auto select-none"
+									style="pointer-events: auto !important;"
+									in:scale={{ duration: 200, start: 0.5 }}
+								>
+									<div class="flex items-center gap-3">
+										<!-- Manual Control Box -->
+										<button
+											class={[
+												"relative min-h-[80px] min-w-[90px] cursor-pointer rounded-lg border border-purple-500/40 bg-purple-950/20 bg-slate-900/80 px-3 py-3 backdrop-blur-sm transition-colors hover:border-purple-400/60",
+												robotStatus?.hasActiveMaster
+													? "cursor-not-allowed border-dashed opacity-40"
+													: robot.manualControlEnabled
+														? ""
+														: "border-dashed opacity-60"
+											]}
+											style="pointer-events: auto !important;"
+											onmousedown={() =>
+												!robotStatus?.hasActiveMaster && handleBoxClick(robot, "manual")}
+											onclick={() =>
+												!robotStatus?.hasActiveMaster && handleBoxClick(robot, "manual")}
+											disabled={robotStatus?.hasActiveMaster}
+										>
+											{#if robotStatus?.hasActiveMaster}
+												<div class="flex flex-col items-center">
+													<div class="flex items-center gap-1">
+														<span class="icon-[mdi--lock] size-3 text-purple-400/60"></span>
+														<span
+															class="text-xs leading-tight font-medium text-purple-400/60 uppercase"
+															>CONTROL DISABLED</span
+														>
+													</div>
+													<span class="mt-0.5 text-center text-xs leading-tight text-purple-300/60">
+														Control managed by
+													</span>
+													<span
+														class="text-center text-xs leading-tight font-semibold text-purple-300/60"
+													>
+														{robot.master?.name?.slice(0, 20) || "Master"}
+													</span>
+												</div>
+											{:else if robot.manualControlEnabled}
+												<div class="flex flex-col items-center">
+													<div class="flex items-center gap-1">
+														<span class="icon-[mdi--tune] size-3 text-purple-400"></span>
+														<span
+															class="text-xs leading-tight font-semibold text-purple-400 uppercase"
+															>MANUAL</span
+														>
+													</div>
+													<span class="mt-0.5 text-xs leading-tight text-purple-200">
+														{robot.activeJoints.length} Joints Active
+													</span>
+													<span class="text-xs leading-tight text-purple-300/80">
+														Click to take manual control
+													</span>
+												</div>
+											{:else}
+												<div class="flex flex-col items-center">
+													<div class="flex items-center gap-1">
+														<span class="icon-[mdi--tune] size-3 text-purple-400/60"></span>
+														<span class="text-xs leading-tight font-medium text-purple-400/60"
+															>MANUAL OFF</span
+														>
+													</div>
+													<span class="mt-0.5 text-xs leading-tight text-purple-300/40"
+														>Click to Configure</span
+													>
+													<div class="mt-2 text-purple-400/60">
+														<span class="icon-[mdi--cog-outline] size-4"></span>
+													</div>
+												</div>
+											{/if}
+										</button>
+									</div>
+								</div>
+							{:else}
+								<div
+									class="pointer-events-auto select-none"
+									style="pointer-events: auto !important;"
+									in:scale={{ duration: 200, start: 0.5 }}
+								>
+									<div class="flex items-center gap-3">
+										<!-- Master Box -->
+										<button
+											class={[
+												"relative min-h-[80px] min-w-[90px] cursor-pointer rounded-lg border border-green-500/40 bg-green-950/20 bg-slate-900/80 px-3 py-3 backdrop-blur-sm transition-colors hover:border-green-400/60",
+												robotStatus?.hasActiveMaster ? "" : "border-dashed opacity-60"
+											]}
+											style="pointer-events: auto !important;"
+											onmousedown={() => handleBoxClick(robot, "master")}
+											onclick={() => handleBoxClick(robot, "master")}
+										>
+											{#if robotStatus?.hasActiveMaster}
+												<div class="flex flex-col items-center">
+													<div class="flex items-center gap-1">
+														<span class="icon-[mdi--speak] size-3 text-green-400"></span>
+														<span
+															class="text-xs leading-tight font-semibold text-green-400 uppercase"
+															>MASTER</span
+														>
+													</div>
+													<span class="mt-0.5 text-xs leading-tight text-green-200">
+														{robot.master?.name.slice(0, 30) || "Unknown"}
+													</span>
+													{#if robot.master?.constructor.name}
+														<span class="text-xs leading-tight text-green-300/80">
+															{robot.master?.constructor.name.replace("Driver", "").slice(0, 30)}
+														</span>
+													{:else}
+														<span class="text-xs leading-tight text-red-300/80"> N/A </span>
+													{/if}
+													<div
+														class="mt-1 h-1.5 w-1.5 animate-pulse rounded-full bg-green-400"
+													></div>
+												</div>
+											{:else}
+												<div class="flex flex-col items-center">
+													<div class="flex items-center gap-1">
+														<span class="icon-[mdi--speak] size-3 text-green-400/60"></span>
+														<span class="text-xs leading-tight font-medium text-green-400/60"
+															>NO MASTER</span
+														>
+													</div>
+													<span class="mt-0.5 text-xs leading-tight text-green-300/40"
+														>Click to Connect</span
+													>
+													<div class="mt-2 text-green-400/60">
+														<span class="icon-[mdi--plus-circle] size-4"></span>
+													</div>
+												</div>
+											{/if}
+										</button>
+
+										<!-- Arrow 1: Master to Robot -->
+										<div class="font-mono text-sm text-slate-400">
+											{#if robotStatus?.hasActiveMaster}
+												<span class="text-green-400">→</span>
+											{:else}
+												<span class="text-green-400/50">⇢</span>
+											{/if}
+										</div>
+
+										<!-- Robot Box (Simplified) -->
+										<div
+											class={[
+												"min-h-[80px] min-w-[90px] rounded-lg border border-amber-500/40 bg-slate-900/80 px-3 py-3 backdrop-blur-sm"
+											]}
+										>
+											<div class="flex flex-col items-center">
+												<div class="flex items-center gap-1">
+													<span class="icon-[mdi--connection] size-3 text-amber-400"></span>
+													<span
+														class="text-xs leading-tight font-semibold text-amber-400 uppercase"
+													>
+														Robot
+													</span>
+												</div>
+												<span class="mt-0.5 text-xs leading-tight text-amber-200">
+													{robot.id}
+												</span>
+											</div>
+										</div>
+
+										<!-- Arrow 2: Robot to Slaves -->
+										<div class="font-mono text-sm text-slate-400">
+											{#if robotStatus?.connectedSlaves && robotStatus.connectedSlaves > 0}
+												<span class="text-blue-400">→</span>
+											{:else}
+												<span class="text-blue-400/50">⇢</span>
+											{/if}
+										</div>
+
+										<!-- Slaves Box -->
+										<button
+											class={[
+												"relative min-h-[80px] min-w-[90px] cursor-pointer rounded-lg border border-blue-500/40 bg-blue-950/20 bg-slate-900/80 px-3 py-3 backdrop-blur-sm transition-colors hover:border-blue-400/60",
+												robotStatus?.connectedSlaves && robotStatus.connectedSlaves > 0
+													? ""
+													: "border-dashed opacity-60"
+											]}
+											style="pointer-events: auto !important;"
+											onmousedown={() => handleBoxClick(robot, "slaves")}
+											onclick={() => handleBoxClick(robot, "slaves")}
+										>
+											{#if robotStatus?.connectedSlaves && robotStatus.connectedSlaves > 0}
+												<div class="flex flex-col items-center">
+													<div class="flex items-center gap-1">
+														<span class="icon-[fa6-solid--ear-listen] size-3 text-blue-400"></span>
+														<span
+															class="text-xs leading-tight font-semibold text-blue-400 uppercase"
+															>SLAVES</span
+														>
+													</div>
+													{#if robot.slaves.length > 0}
+														<div class="mt-1 flex flex-col items-center gap-0.5">
+															{#each robot.slaves.slice(0, 2) as slave}
+																<div class="mt-0.5 text-xs leading-tight text-blue-300/80">
+																	{slave.name.slice(0, 30) || "Slave"}
+																</div>
+																<div class="text-xs leading-tight text-blue-200">
+																	{slave.constructor.name.replace("Driver", "").slice(0, 30) ||
+																		"N/A"}
+																</div>
+															{/each}
+															{#if robot.slaves.length > 2}
+																<span class="text-xs text-blue-400/60"
+																	>+{robot.slaves.length - 2} more</span
+																>
+															{/if}
+														</div>
+													{/if}
+												</div>
+											{:else}
+												<div class="flex flex-col items-center">
+													<div class="flex items-center gap-1">
+														<span class="icon-[fa6-solid--ear-listen] size-3 text-blue-400/60"
+														></span>
+														<span class="text-xs leading-tight font-medium text-blue-400/60"
+															>NO SLAVES</span
+														>
+													</div>
+													<span class="mt-0.5 text-xs leading-tight text-blue-300/40"
+														>Click to Connect</span
+													>
+													<div class="mt-2 text-blue-400/60">
+														<span class="icon-[mdi--plus-circle] size-4"></span>
+													</div>
+												</div>
+											{/if}
+										</button>
+									</div>
+								</div>
+							{/if}
+						</HTML>
+					</Billboard>
+				</T.Group>
+			{/snippet}
+		</Hoverable>
 	</T.Group>
-{/each}
+{/each}
+
+<!-- Master Connection Modal -->
+<MasterConnectionModal bind:open={isMasterModalOpen} robot={selectedRobot} />
+
+<!-- Slave Connection Modal -->
+<SlaveConnectionModal bind:open={isSlaveModalOpen} robot={selectedRobot} />
+
+<!-- Manual Control Modal -->
+<ManualControlModal bind:open={isManualControlModalOpen} robot={selectedRobot} />

src/lib/components/3d/robot/URDF/createRobot.svelte.ts

@@ -3,25 +3,25 @@ import type { RobotUrdfConfig } from "$lib/types/urdf";
 import { UrdfParser } from "./utils/UrdfParser";
 
 export async function createRobot(urdfConfig: RobotUrdfConfig): Promise<RobotState> {
-    const customParser = new UrdfParser(urdfConfig.urdfUrl, "/robots/so-100/");
-    const urdfData = await customParser.load();
-    const robot = customParser.fromString(urdfData);
+	const customParser = new UrdfParser(urdfConfig.urdfUrl, "/robots/so-100/");
+	const urdfData = await customParser.load();
+	const robot = customParser.fromString(urdfData);
 
-    // Make the robot data reactive so mutations to joint.rotation trigger reactivity
-    const reactiveRobot = $state(robot);
-    
-    // Make the selection state reactive as well
-    const reactiveSelection = $state({
-        isSelected: false,
-        selectedLink: undefined,
-        selectedJoint: undefined
-    });
+	// Make the robot data reactive so mutations to joint.rotation trigger reactivity
+	const reactiveRobot = $state(robot);
 
-    const robotState: RobotState = {
-        robot: reactiveRobot,
-        urdfConfig: urdfConfig,
-        selection: reactiveSelection
-    };
+	// Make the selection state reactive as well
+	const reactiveSelection = $state({
+		isSelected: false,
+		selectedLink: undefined,
+		selectedJoint: undefined
+	});
 
-    return robotState;
-} 
+	const robotState: RobotState = {
+		robot: reactiveRobot,
+		urdfConfig: urdfConfig,
+		selection: reactiveSelection
+	};
+
+	return robotState;
+}

src/lib/components/3d/robot/URDF/interfaces/IUrdfBox.ts

@@ -1,3 +1,3 @@
 export default interface IUrdfBox {
-    size: [x: number, y: number, z: number]
-  }
+	size: [x: number, y: number, z: number];
+}

src/lib/components/3d/robot/URDF/interfaces/IUrdfCylinder.ts

@@ -1,4 +1,4 @@
 export default interface IUrdfCylinder {
-    radius: number;
-    length: number
-  }
+	radius: number;
+	length: number;
+}

src/lib/components/3d/robot/URDF/interfaces/IUrdfJoint.ts

@@ -1,42 +1,42 @@
-import type IUrdfLink from "./IUrdfLink"
+import type IUrdfLink from "./IUrdfLink";
 
 export default interface IUrdfJoint {
-  name?: string
-  type?: 'revolute' | 'continuous' | 'prismatic' | 'fixed' | 'floating' | 'planar'
-  // rpy = roll, pitch, yaw (values between -pi and +pi)
-  origin_rpy: [roll: number, pitch: number, yaw: number]
-  origin_xyz: [x: number, y: number, z: number]
-  // calculated rotation for non-fixed joints based on origin_rpy and axis_xyz
-  rotation: [x: number, y: number, z: number]
-  parent: IUrdfLink
-  child: IUrdfLink
-  // axis for revolute and continuous joints defaults to (1,0,0)
-  axis_xyz?: [x: number, y: number, z: number]
-  calibration?: {
-    rising?: number,  // Calibration rising value in radians
-    falling?: number  // Calibration falling value in radians
-  }
-  dynamics?: {
-    damping?: number
-    friction?: number
-  }
-  // only for revolute joints
-  limit?: {
-    lower?: number
-    upper?: number
-    effort: number
-    velocity: number
-  }
-  mimic?: {
-    joint: string
-    multiplier?: number
-    offset?: number
-  }
-  safety_controller?: {
-    soft_lower_limit?: number
-    soft_upper_limit?: number
-    k_position?: number
-    k_velocity: number
-  }
-  elem: Element
-}
+	name?: string;
+	type?: "revolute" | "continuous" | "prismatic" | "fixed" | "floating" | "planar";
+	// rpy = roll, pitch, yaw (values between -pi and +pi)
+	origin_rpy: [roll: number, pitch: number, yaw: number];
+	origin_xyz: [x: number, y: number, z: number];
+	// calculated rotation for non-fixed joints based on origin_rpy and axis_xyz
+	rotation: [x: number, y: number, z: number];
+	parent: IUrdfLink;
+	child: IUrdfLink;
+	// axis for revolute and continuous joints defaults to (1,0,0)
+	axis_xyz?: [x: number, y: number, z: number];
+	calibration?: {
+		rising?: number; // Calibration rising value in radians
+		falling?: number; // Calibration falling value in radians
+	};
+	dynamics?: {
+		damping?: number;
+		friction?: number;
+	};
+	// only for revolute joints
+	limit?: {
+		lower?: number;
+		upper?: number;
+		effort: number;
+		velocity: number;
+	};
+	mimic?: {
+		joint: string;
+		multiplier?: number;
+		offset?: number;
+	};
+	safety_controller?: {
+		soft_lower_limit?: number;
+		soft_upper_limit?: number;
+		k_position?: number;
+		k_velocity: number;
+	};
+	elem: Element;
+}

src/lib/components/3d/robot/URDF/interfaces/IUrdfLink.ts

@@ -1,23 +1,23 @@
-import type {IUrdfVisual} from "./IUrdfVisual"
+import type { IUrdfVisual } from "./IUrdfVisual";
 
 interface IUrdfInertia {
-  ixx: number;
-  ixy: number;
-  ixz: number;
-  iyy: number;
-  iyz: number;
-  izz: number;
+	ixx: number;
+	ixy: number;
+	ixz: number;
+	iyy: number;
+	iyz: number;
+	izz: number;
 }
 
 export default interface IUrdfLink {
-  name: string
-  inertial?: {
-    origin_xyz?: [x: number, y: number, z: number]
-    origin_rpy?: [roll: number, pitch: number, yaw: number]
-    mass: number
-    inertia: IUrdfInertia
-  }
-  visual: IUrdfVisual[]
-  collision: IUrdfVisual[]
-  elem: Element
-}
+	name: string;
+	inertial?: {
+		origin_xyz?: [x: number, y: number, z: number];
+		origin_rpy?: [roll: number, pitch: number, yaw: number];
+		mass: number;
+		inertia: IUrdfInertia;
+	};
+	visual: IUrdfVisual[];
+	collision: IUrdfVisual[];
+	elem: Element;
+}

src/lib/components/3d/robot/URDF/interfaces/IUrdfMesh.ts

@@ -1,5 +1,5 @@
 export default interface IUrdfMesh {
-    filename: string;
-    type: 'stl' | 'fbx' | 'obj' | 'dae';
-    scale: [x: number, y: number, z: number];
-  }
+	filename: string;
+	type: "stl" | "fbx" | "obj" | "dae";
+	scale: [x: number, y: number, z: number];
+}

src/lib/components/3d/robot/URDF/interfaces/IUrdfRobot.ts

@@ -1,10 +1,10 @@
-import type IUrdfJoint from "./IUrdfJoint"
-import type IUrdfLink from "./IUrdfLink"
+import type IUrdfJoint from "./IUrdfJoint";
+import type IUrdfLink from "./IUrdfLink";
 
 export default interface IUrdfRobot {
-  name: string
-  links: {[name: string]: IUrdfLink}
-  joints: IUrdfJoint[]
-  // the DOM element holding the XML, so we can work non-destructive
-  elem?: Element
-}
+	name: string;
+	links: { [name: string]: IUrdfLink };
+	joints: IUrdfJoint[];
+	// the DOM element holding the XML, so we can work non-destructive
+	elem?: Element;
+}

src/lib/components/3d/robot/URDF/interfaces/IUrdfVisual.ts

@@ -4,52 +4,52 @@ import type IUrdfMesh from "./IUrdfMesh";
 
 // 1) Box‐type visual
 interface IUrdfVisualBox {
-  name: string;
-  origin_xyz: [x: number, y: number, z: number];
-  origin_rpy: [roll: number, pitch: number, yaw: number];
-  geometry: IUrdfBox;
-  material?: {
-    name: string;
-    color?: string;
-    texture?: string;
-  },
-  type: 'box';
-  // optional RGBA color override
-  color_rgba?: [r: number, g: number, b: number, a: number];
-  // XML Element reference
-  elem: Element;
+	name: string;
+	origin_xyz: [x: number, y: number, z: number];
+	origin_rpy: [roll: number, pitch: number, yaw: number];
+	geometry: IUrdfBox;
+	material?: {
+		name: string;
+		color?: string;
+		texture?: string;
+	};
+	type: "box";
+	// optional RGBA color override
+	color_rgba?: [r: number, g: number, b: number, a: number];
+	// XML Element reference
+	elem: Element;
 }
 
 // 2) Cylinder‐type visual
 interface IUrdfVisualCylinder {
-  name: string;
-  origin_xyz: [x: number, y: number, z: number];
-  origin_rpy: [roll: number, pitch: number, yaw: number];
-  geometry: IUrdfCylinder;
-  material?: {
-    name: string;
-    color?: string;
-    texture?: string;
-  },
-  type: 'cylinder';
-  color_rgba?: [r: number, g: number, b: number, a: number];
-  elem: Element;
+	name: string;
+	origin_xyz: [x: number, y: number, z: number];
+	origin_rpy: [roll: number, pitch: number, yaw: number];
+	geometry: IUrdfCylinder;
+	material?: {
+		name: string;
+		color?: string;
+		texture?: string;
+	};
+	type: "cylinder";
+	color_rgba?: [r: number, g: number, b: number, a: number];
+	elem: Element;
 }
 
 // 3) Mesh‐type visual
 interface IUrdfVisualMesh {
-  name: string;
-  origin_xyz: [x: number, y: number, z: number];
-  origin_rpy: [roll: number, pitch: number, yaw: number];
-  geometry: IUrdfMesh;
-  material?: {
-    name: string;
-    color?: string;
-    texture?: string;
-  },
-  type: 'mesh';
-  color_rgba?: [r: number, g: number, b: number, a: number];
-  elem: Element;
+	name: string;
+	origin_xyz: [x: number, y: number, z: number];
+	origin_rpy: [roll: number, pitch: number, yaw: number];
+	geometry: IUrdfMesh;
+	material?: {
+		name: string;
+		color?: string;
+		texture?: string;
+	};
+	type: "mesh";
+	color_rgba?: [r: number, g: number, b: number, a: number];
+	elem: Element;
 }
 
 // Now make a union of the three:

src/lib/components/3d/robot/URDF/interfaces/index.ts

@@ -1,7 +1,7 @@
-export * from './IUrdfBox';
-export * from './IUrdfCylinder';
-export * from './IUrdfJoint';
-export * from './IUrdfLink';
-export * from './IUrdfMesh';
-export * from './IUrdfRobot';
-export * from './IUrdfVisual';
+export * from "./IUrdfBox";
+export * from "./IUrdfCylinder";
+export * from "./IUrdfJoint";
+export * from "./IUrdfLink";
+export * from "./IUrdfMesh";
+export * from "./IUrdfRobot";
+export * from "./IUrdfVisual";

src/lib/components/3d/robot/URDF/mesh/DAE.svelte

@@ -1,8 +1,8 @@
 <script lang="ts">
-	import { T, useLoader } from '@threlte/core';
-	import type { InteractivityProps } from '@threlte/extras';
-	import { DoubleSide, Mesh, type Side } from 'three';
-	import { ColladaLoader } from 'three/examples/jsm/loaders/ColladaLoader.js';
+	import { T, useLoader } from "@threlte/core";
+	import type { InteractivityProps } from "@threlte/extras";
+	import { DoubleSide, Mesh, type Side } from "three";
+	import { ColladaLoader } from "three/examples/jsm/loaders/ColladaLoader.js";
 
 	type Props = InteractivityProps & {
 		filename: string;
@@ -19,7 +19,7 @@
 	};
 	let {
 		filename,
-		color = '#ffffff',
+		color = "#ffffff",
 		scale = [1, 1, 1],
 		position = [0, 0, 0],
 		rotation = [0, 0, 0],
@@ -47,7 +47,7 @@
 				rotation={dae.scene.rotation ? dae.scene.rotation.toArray() : [0, 0, 0]}
 			>
 				{#each dae.scene.children as obj}
-					{#if obj.type === 'Mesh'}
+					{#if obj.type === "Mesh"}
 						{@const mesh = obj as Mesh}
 						<T.Mesh
 							{castShadow}

src/lib/components/3d/robot/URDF/mesh/OBJ.svelte

@@ -1,8 +1,8 @@
 <script lang="ts">
-	import { T, useLoader } from '@threlte/core';
-	import type { InteractivityProps } from '@threlte/extras';
-	import { DoubleSide, type Side } from 'three';
-	import { OBJLoader } from 'three/examples/jsm/loaders/OBJLoader.js';
+	import { T, useLoader } from "@threlte/core";
+	import type { InteractivityProps } from "@threlte/extras";
+	import { DoubleSide, type Side } from "three";
+	import { OBJLoader } from "three/examples/jsm/loaders/OBJLoader.js";
 
 	type Props = InteractivityProps & {
 		filename: string;
@@ -19,7 +19,7 @@
 	};
 	let {
 		filename,
-		color = '#ffffff',
+		color = "#ffffff",
 		scale = [1, 1, 1],
 		rotation = [0, 0, 0],
 		position = [0, 0, 0],

src/lib/components/3d/robot/URDF/mesh/STL.svelte

@@ -1,9 +1,9 @@
 <script lang="ts">
-	import { T, useLoader } from '@threlte/core';
-	import type { InteractivityProps } from '@threlte/extras';
-	import { STLLoader } from 'three/examples/jsm/loaders/STLLoader.js';
-	import { type Side, DoubleSide } from 'three';
-	import { interactivity } from '@threlte/extras';
+	import { T, useLoader } from "@threlte/core";
+	import type { InteractivityProps } from "@threlte/extras";
+	import { STLLoader } from "three/examples/jsm/loaders/STLLoader.js";
+	import { type Side, DoubleSide } from "three";
+	import { interactivity } from "@threlte/extras";
 
 	type Props = InteractivityProps & {
 		filename: string;
@@ -21,7 +21,7 @@
 
 	let {
 		filename,
-		color = '#ffffff',
+		color = "#ffffff",
 		scale = [1, 1, 1],
 		position = [0, 0, 0],
 		rotation = [0, 0, 0],
@@ -44,7 +44,7 @@
 {#await load(filename) then stl}
 	{@html `<!-- include stl: ${filename} ${scale} -->`}
 	<T.Mesh geometry={stl} {scale} {castShadow} {receiveShadow} {position} {rotation} {...restProps}>
-		<T.MeshLambertMaterial color={color} {opacity} transparent={opacity < 1.0} {wireframe} {side} />
+		<T.MeshLambertMaterial {color} {opacity} transparent={opacity < 1.0} {wireframe} {side} />
 	</T.Mesh>
 {/await}
 

src/lib/components/3d/robot/URDF/primitives/UrdfJoint.svelte

@@ -1,20 +1,21 @@
 <script lang="ts">
-	import type IUrdfJoint from '../interfaces/IUrdfJoint';
-	import { T } from '@threlte/core';
-	import UrdfLink from './UrdfLink.svelte';
-	import { Vector3 } from 'three';
+	import type IUrdfJoint from "../interfaces/IUrdfJoint";
+	import { T } from "@threlte/core";
+	import UrdfLink from "./UrdfLink.svelte";
+	import { Vector3 } from "three";
 	import {
 		Billboard,
 		interactivity,
 		MeshLineGeometry,
 		Text,
 		type InteractivityProps
-	} from '@threlte/extras';
+	} from "@threlte/extras";
 
-	import type IUrdfLink from '../interfaces/IUrdfLink';
-	import type IUrdfRobot from '../interfaces/IUrdfRobot';
+	import type IUrdfLink from "../interfaces/IUrdfLink";
+	import type IUrdfRobot from "../interfaces/IUrdfRobot";
+	import { radiansToDegrees } from "@/utils";
 
-	const defaultOnClick: InteractivityProps['onclick'] = (event) => {
+	const defaultOnClick: InteractivityProps["onclick"] = (event) => {
 		event.stopPropagation();
 		selectedLink = undefined;
 		selectedJoint = joint;
@@ -49,9 +50,9 @@
 		selectedLink = $bindable(),
 		showLine = false,
 		nameHeight = 0.02,
-		highlightColor = '#ff0000',
-		jointColor = '#000000',
-		jointIndicatorColor = '#000000',
+		highlightColor = "#ff0000",
+		jointColor = "#000000",
+		jointIndicatorColor = "#000000",
 		opacity = 0.7,
 		isInteractive = false,
 		showName = false,
@@ -59,7 +60,7 @@
 		showCollision = true,
 		visualOpacity = 1,
 		collisionOpacity = 1,
-		collisionColor = '#000000',
+		collisionColor = "#000000",
 		jointNames = true,
 		onclick = defaultOnClick,
 		...restProps
@@ -68,10 +69,23 @@
 	if (isInteractive) {
 		interactivity();
 	}
+
+	// Helper function to get current joint rotation value in degrees
+	function getJointRotationValue(joint: any): number {
+		const axis = joint.axis_xyz || [0, 0, 1];
+		const rotation = joint.rotation || [0, 0, 0];
+
+		// Find the primary axis and get the rotation value
+		for (let i = 0; i < 3; i++) {
+			if (Math.abs(axis[i]) > 0.001) {
+				return radiansToDegrees(rotation[i] / axis[i]);
+			}
+		}
+		return 0;
+	}
 </script>
 
 {@html `<!-- Joint ${joint.name} (${joint.type}) -->`}
-
 <!-- draw line from parent-frame to joint origin -->
 {#if showLine}
 	<T.Line>
@@ -91,7 +105,7 @@
 			<UrdfLink
 				{robot}
 				link={joint.child}
-				textScale={0.02}
+				textScale={0.2}
 				{showName}
 				{showVisual}
 				{showCollision}
@@ -141,7 +155,7 @@
 		<Text
 			scale={nameHeight}
 			color={selectedJoint == joint ? highlightColor : jointColor}
-			text={joint.name}
+			text={joint.name + " " + getJointRotationValue(joint).toFixed(0) + "°"}
 			characters="ABCDEFGHIJKLMNOPQRSTUVWXYZ"
 			renderOrder={999}
 		></Text>

src/lib/components/3d/robot/URDF/primitives/UrdfLink.svelte

@@ -1,12 +1,11 @@
 <script lang="ts">
 	// The link element describes a rigid body with an inertia, visual features, and collision properties.
-	import type IUrdfLink from '../interfaces/IUrdfLink';
-	import UrdfVisual from './UrdfVisual.svelte';
-	import { getChildJoints } from '../utils/UrdfParser';
-	import UrdfJoint from './UrdfJoint.svelte';
-	import { Billboard, Text } from '@threlte/extras';
-	import type IUrdfRobot from '../interfaces/IUrdfRobot';
-	import type IUrdfJoint from '../interfaces/IUrdfJoint';
+	import type IUrdfLink from "../interfaces/IUrdfLink";
+	import UrdfVisual from "./UrdfVisual.svelte";
+	import { getChildJoints } from "../utils/UrdfParser";
+	import UrdfJoint from "./UrdfJoint.svelte";
+	import type IUrdfRobot from "../interfaces/IUrdfRobot";
+	import type IUrdfJoint from "../interfaces/IUrdfJoint";
 
 	interface Props {
 		robot: IUrdfRobot;
@@ -39,18 +38,18 @@
 		showName = true,
 		showVisual = true,
 		showCollision = true,
-		visualColor = '#000000',
+		visualColor = "#000000",
 		visualOpacity = 1,
 		collisionOpacity = 1,
-		collisionColor = '#000000',
+		collisionColor = "#000000",
 		jointNames = true,
 		joints = true,
-		jointColor = '#000000',
-		jointIndicatorColor = '#000000',
+		jointColor = "#000000",
+		jointIndicatorColor = "#000000",
 		nameHeight = 0.1,
 		selectedLink = undefined,
 		selectedJoint = undefined,
-		highlightColor = '#000000',
+		highlightColor = "#000000",
 		showLine = true,
 		opacity = 0.7,
 		isInteractive = false
@@ -58,30 +57,22 @@
 </script>
 
 {@html `<!-- Link ${link.name} -->`}
-{#if showName}
+<!-- {#if showName}
 	<Billboard position.x={0} position.y={0} position.z={0}>
 		<Text anchorY={-0.2} scale={textScale} text={link.name} characters="ABCDEFGHIJKLMNOPQRSTUVWXYZ"
 		></Text>
 	</Billboard>
-{/if}
+{/if} -->
 
 {#if showVisual}
 	{#each link.visual as visual}
-		<UrdfVisual
-			opacity={visualOpacity}
-			{visual}
-			defaultColor={visualColor}
-		/>
+		<UrdfVisual opacity={visualOpacity} {visual} defaultColor={visualColor} />
 	{/each}
 {/if}
 
 {#if showCollision}
 	{#each link.collision as visual}
-		<UrdfVisual
-			opacity={collisionOpacity}
-			visual={visual}
-			defaultColor={collisionColor}
-		/>
+		<UrdfVisual opacity={collisionOpacity} {visual} defaultColor={collisionColor} />
 	{/each}
 {/if}
 

src/lib/components/3d/robot/URDF/primitives/UrdfThree.svelte

@@ -1,12 +1,12 @@
 <script lang="ts">
 	// Three.js visualization of an URDF.
-	import { T } from '@threlte/core';
-	import { getRootLinks } from '../utils/UrdfParser';
-	import UrdfLink from './UrdfLink.svelte';
-	import { scale } from 'svelte/transition';
-	import type IUrdfLink from '../interfaces/IUrdfLink';
-	import type IUrdfJoint from '../interfaces/IUrdfJoint';
-	import type IUrdfRobot from '../interfaces/IUrdfRobot';
+	import { T } from "@threlte/core";
+	import { getRootLinks } from "../utils/UrdfParser";
+	import UrdfLink from "./UrdfLink.svelte";
+	import { scale } from "svelte/transition";
+	import type IUrdfLink from "../interfaces/IUrdfLink";
+	import type IUrdfJoint from "../interfaces/IUrdfJoint";
+	import type IUrdfRobot from "../interfaces/IUrdfRobot";
 
 	interface Props {
 		robot: IUrdfRobot;
@@ -37,16 +37,16 @@
 		showCollision = true,
 		visualOpacity = 1,
 		collisionOpacity = 1,
-		collisionColor = '#000000',
+		collisionColor = "#000000",
 		jointNames = true,
 		joints = true,
-		jointColor = '#000000',
-		jointIndicatorColor = '#000000',
+		jointColor = "#000000",
+		jointIndicatorColor = "#000000",
 		nameHeight = 0.1,
 		selectedLink = undefined,
 		selectedJoint = undefined,
 		textScale = 1,
-		highlightColor = '#000000'
+		highlightColor = "#000000"
 	}: Props = $props();
 </script>
 

src/lib/components/3d/robot/URDF/primitives/UrdfVisual.svelte

@@ -1,14 +1,12 @@
 <script lang="ts">
-	import type { IUrdfVisual } from '../interfaces/IUrdfVisual';
-	import { numberArrayToColor } from '../utils/helper';
-	import DAE from '../mesh/DAE.svelte';
-	import OBJ from '../mesh/OBJ.svelte';
-	import STL from '../mesh/STL.svelte';
-	import { T } from '@threlte/core';
-	import { interactivity, type InteractivityProps } from '@threlte/extras';
-	import type IUrdfLink from '../interfaces/IUrdfLink';
-	import { DoubleSide, type Side } from 'three';
-	import { getContext } from 'svelte';
+	import type { IUrdfVisual } from "../interfaces/IUrdfVisual";
+	import { numberArrayToColor } from "../utils/helper";
+	import DAE from "../mesh/DAE.svelte";
+	import OBJ from "../mesh/OBJ.svelte";
+	import STL from "../mesh/STL.svelte";
+	import { T } from "@threlte/core";
+	import { type InteractivityProps } from "@threlte/extras";
+	import { DoubleSide, type Side } from "three";
 
 	type Props = InteractivityProps & {
 		visual: IUrdfVisual;
@@ -26,7 +24,7 @@
 	let {
 		visual,
 		opacity = 1.0,
-		defaultColor = '#000000',
+		defaultColor = "#000000",
 		position = [0, 0, 0],
 		rotation = [0, 0, 0],
 		castShadow = true,
@@ -41,15 +39,13 @@
 	const baseColor = visual?.color_rgba
 		? numberArrayToColor(visual.color_rgba.slice(0, 3) as [number, number, number])
 		: defaultColor;
-
-		
 </script>
 
-{#if visual.type === 'mesh'}
-	{#if visual.geometry.type === 'stl'}
+{#if visual.type === "mesh"}
+	{#if visual.geometry.type === "stl"}
 		<STL
 			color={baseColor}
-			opacity={opacity}
+			{opacity}
 			filename={visual.geometry.filename}
 			scale={visual.geometry.scale}
 			{position}
@@ -60,10 +56,10 @@
 			{side}
 			{...restProps}
 		/>
-	{:else if visual.geometry.type === 'obj'}
+	{:else if visual.geometry.type === "obj"}
 		<OBJ
 			color={baseColor}
-			opacity={opacity}
+			{opacity}
 			scale={visual.geometry.scale}
 			filename={visual.geometry.filename}
 			{position}
@@ -74,11 +70,11 @@
 			{side}
 			{...restProps}
 		/>
-	{:else if visual.geometry.type === 'dae'}
+	{:else if visual.geometry.type === "dae"}
 		<DAE
 			filename={visual.geometry.filename}
 			color={baseColor}
-			opacity={opacity}
+			{opacity}
 			scale={visual.geometry.scale}
 			{position}
 			{rotation}
@@ -89,24 +85,16 @@
 			{...restProps}
 		/>
 	{/if}
-{:else if visual.type === 'cylinder'}
+{:else if visual.type === "cylinder"}
 	<T.Mesh castShadow receiveShadow rotation={[Math.PI / 2, 0, 0]} {onclick}>
 		<T.CylinderGeometry
 			args={[visual.geometry.radius, visual.geometry.radius, visual.geometry.length]}
 		/>
-		<T.MeshBasicMaterial
-			color={baseColor}
-			opacity={opacity}
-			transparent={opacity < 1.0}
-		/>
+		<T.MeshBasicMaterial color={baseColor} {opacity} transparent={opacity < 1.0} />
 	</T.Mesh>
-{:else if visual.type === 'box'}
+{:else if visual.type === "box"}
 	<T.Mesh castShadow receiveShadow scale={visual.geometry.size} {onclick}>
 		<T.BoxGeometry />
-		<T.MeshBasicMaterial
-			color={baseColor}
-			opacity={opacity}
-			transparent={opacity < 1.0}
-		/>
+		<T.MeshBasicMaterial color={baseColor} {opacity} transparent={opacity < 1.0} />
 	</T.Mesh>
 {/if}

src/lib/components/3d/robot/URDF/runes/urdf_state.svelte.ts

@@ -1,15 +1,15 @@
-import type IUrdfJoint from "../interfaces/IUrdfJoint"
-import type IUrdfLink from "../interfaces/IUrdfLink"
-import type IUrdfRobot from "../interfaces/IUrdfRobot"
+import type IUrdfJoint from "../interfaces/IUrdfJoint";
+import type IUrdfLink from "../interfaces/IUrdfLink";
+import type IUrdfRobot from "../interfaces/IUrdfRobot";
 
 // Color constants for better maintainability
 export const URDF_COLORS = {
-    COLLISION: "#813d9c", // purple
-    JOINT: "#62a0ea", // blue
-    LINK: "#57e389", // green
-    JOINT_INDICATOR: "#f66151", // red
-    HIGHLIGHT: "#ffa348", // orange
-    BACKGROUND: "#241f31", // dark purple
+	COLLISION: "#813d9c", // purple
+	JOINT: "#62a0ea", // blue
+	LINK: "#57e389", // green
+	JOINT_INDICATOR: "#f66151", // red
+	HIGHLIGHT: "#ffa348", // orange
+	BACKGROUND: "#241f31" // dark purple
 } as const;
 
 // Transform tool types
@@ -17,128 +17,128 @@ export type TransformTool = "translate" | "rotate" | "scale";
 
 // Joint state tracking
 export interface JointStates {
-    continuous: Record<string, number>;
-    revolute: Record<string, number>;
+	continuous: Record<string, number>;
+	revolute: Record<string, number>;
 }
 
 // Selection state
 export interface SelectionState {
-    selectedLink?: IUrdfLink;
-    selectedJoint?: IUrdfJoint;
+	selectedLink?: IUrdfLink;
+	selectedJoint?: IUrdfJoint;
 }
 
 // Visibility configuration
 export interface VisibilityConfig {
-    visual: boolean;
-    collision: boolean;
-    joints: boolean;
-    jointNames: boolean;
-    linkNames: boolean;
+	visual: boolean;
+	collision: boolean;
+	joints: boolean;
+	jointNames: boolean;
+	linkNames: boolean;
 }
 
 // Appearance settings
 export interface AppearanceConfig {
-    colors: {
-        collision: string;
-        joint: string;
-        link: string;
-        jointIndicator: string;
-        highlight: string;
-        background: string;
-    };
-    opacity: {
-        visual: number;
-        collision: number;
-        link: number;
-    };
+	colors: {
+		collision: string;
+		joint: string;
+		link: string;
+		jointIndicator: string;
+		highlight: string;
+		background: string;
+	};
+	opacity: {
+		visual: number;
+		collision: number;
+		link: number;
+	};
 }
 
 // Editor configuration
 export interface EditorConfig {
-    isEditMode: boolean;
-    currentTool: TransformTool;
-    snap: {
-        translation: number;
-        scale: number;
-        rotation: number;
-    };
+	isEditMode: boolean;
+	currentTool: TransformTool;
+	snap: {
+		translation: number;
+		scale: number;
+		rotation: number;
+	};
 }
 
 // View configuration
 export interface ViewConfig {
-    zoom: {
-        current: number;
-        initial: number;
-    };
-    nameHeight: number;
+	zoom: {
+		current: number;
+		initial: number;
+	};
+	nameHeight: number;
 }
 
 // Main URDF state interface
 export interface UrdfState extends SelectionState {
-    robot?: IUrdfRobot;
-    jointStates: JointStates;
-    visibility: VisibilityConfig;
-    appearance: AppearanceConfig;
-    editor: EditorConfig;
-    view: ViewConfig;
+	robot?: IUrdfRobot;
+	jointStates: JointStates;
+	visibility: VisibilityConfig;
+	appearance: AppearanceConfig;
+	editor: EditorConfig;
+	view: ViewConfig;
 }
 
 // Create the reactive state
 export const urdfState = $state<UrdfState>({
-    // Selection
-    selectedLink: undefined,
-    selectedJoint: undefined,
-    
-    // Robot data
-    robot: undefined,
-    jointStates: {
-        continuous: {},
-        revolute: {},
-    },
-    
-    // Visibility settings
-    visibility: {
-        visual: true,
-        collision: false,
-        joints: true,
-        jointNames: true,
-        linkNames: true,
-    },
-    
-    // Appearance settings
-    appearance: {
-        colors: {
-            collision: URDF_COLORS.COLLISION,
-            joint: URDF_COLORS.JOINT,
-            link: URDF_COLORS.LINK,
-            jointIndicator: URDF_COLORS.JOINT_INDICATOR,
-            highlight: URDF_COLORS.HIGHLIGHT,
-            background: URDF_COLORS.BACKGROUND,
-        },
-        opacity: {
-            visual: 1.0,
-            collision: 0.7,
-            link: 1.0,
-        },
-    },
-    
-    // Editor configuration
-    editor: {
-        isEditMode: false,
-        currentTool: 'translate',
-        snap: {
-            translation: 0.001,
-            scale: 0.001,
-            rotation: 1,
-        },
-    },
-    
-    // View configuration
-    view: {
-        zoom: {
-            current: 1.3,
-            initial: 1.3,
-        },
-        nameHeight: 0.05,
-    },
-});
+	// Selection
+	selectedLink: undefined,
+	selectedJoint: undefined,
+
+	// Robot data
+	robot: undefined,
+	jointStates: {
+		continuous: {},
+		revolute: {}
+	},
+
+	// Visibility settings
+	visibility: {
+		visual: true,
+		collision: false,
+		joints: true,
+		jointNames: true,
+		linkNames: true
+	},
+
+	// Appearance settings
+	appearance: {
+		colors: {
+			collision: URDF_COLORS.COLLISION,
+			joint: URDF_COLORS.JOINT,
+			link: URDF_COLORS.LINK,
+			jointIndicator: URDF_COLORS.JOINT_INDICATOR,
+			highlight: URDF_COLORS.HIGHLIGHT,
+			background: URDF_COLORS.BACKGROUND
+		},
+		opacity: {
+			visual: 1.0,
+			collision: 0.7,
+			link: 1.0
+		}
+	},
+
+	// Editor configuration
+	editor: {
+		isEditMode: false,
+		currentTool: "translate",
+		snap: {
+			translation: 0.001,
+			scale: 0.001,
+			rotation: 1
+		}
+	},
+
+	// View configuration
+	view: {
+		zoom: {
+			current: 1.3,
+			initial: 1.3
+		},
+		nameHeight: 0.05
+	}
+});

src/lib/components/3d/robot/URDF/utils/UrdfParser.ts

@@ -17,23 +17,23 @@ import type IUrdfRobot from "../interfaces/IUrdfRobot";
  * @returns An array of IUrdfLink objects that have no parent joint (i.e. root links)
  */
 export function getRootLinks(robot: IUrdfRobot): IUrdfLink[] {
-  const links: IUrdfLink[] = [];
-  const joints = robot.joints;
-
-  for (const link of Object.values(robot.links)) {
-    let isRoot = true;
-    for (const joint of joints) {
-      if (joint.child.name === link.name) {
-        isRoot = false;
-        break;
-      }
-    }
-    if (isRoot) {
-      links.push(link);
-    }
-  }
-
-  return links;
+	const links: IUrdfLink[] = [];
+	const joints = robot.joints;
+
+	for (const link of Object.values(robot.links)) {
+		let isRoot = true;
+		for (const joint of joints) {
+			if (joint.child.name === link.name) {
+				isRoot = false;
+				break;
+			}
+		}
+		if (isRoot) {
+			links.push(link);
+		}
+	}
+
+	return links;
 }
 
 /**
@@ -47,26 +47,26 @@ export function getRootLinks(robot: IUrdfRobot): IUrdfLink[] {
  * @returns An array of IUrdfJoint objects with no parent joint (i.e. root joints)
  */
 export function getRootJoints(robot: IUrdfRobot): IUrdfJoint[] {
-  const joints = robot.joints;
-  const rootJoints: IUrdfJoint[] = [];
-
-  for (const joint of joints) {
-    let isRoot = true;
-
-    // If any other joint's child matches this joint's parent, then this joint isn't root
-    for (const parentJoint of joints) {
-      if (joint.parent.name === parentJoint.child.name) {
-        isRoot = false;
-        break;
-      }
-    }
-
-    if (isRoot) {
-      rootJoints.push(joint);
-    }
-  }
-
-  return rootJoints;
+	const joints = robot.joints;
+	const rootJoints: IUrdfJoint[] = [];
+
+	for (const joint of joints) {
+		let isRoot = true;
+
+		// If any other joint's child matches this joint's parent, then this joint isn't root
+		for (const parentJoint of joints) {
+			if (joint.parent.name === parentJoint.child.name) {
+				isRoot = false;
+				break;
+			}
+		}
+
+		if (isRoot) {
+			rootJoints.push(joint);
+		}
+	}
+
+	return rootJoints;
 }
 
 /**
@@ -76,23 +76,20 @@ export function getRootJoints(robot: IUrdfRobot): IUrdfJoint[] {
  * @param parent - An IUrdfLink object to use as the "parent" in comparison
  * @returns A list of IUrdfJoint objects whose parent.name matches parent.name
  */
-export function getChildJoints(
-  robot: IUrdfRobot,
-  parent: IUrdfLink
-): IUrdfJoint[] {
-  const childJoints: IUrdfJoint[] = [];
-  const joints = robot.joints;
-  if (!joints) {
-    return [];
-  }
-
-  for (const joint of joints) {
-    if (joint.parent.name === parent.name) {
-      childJoints.push(joint);
-    }
-  }
-
-  return childJoints;
+export function getChildJoints(robot: IUrdfRobot, parent: IUrdfLink): IUrdfJoint[] {
+	const childJoints: IUrdfJoint[] = [];
+	const joints = robot.joints;
+	if (!joints) {
+		return [];
+	}
+
+	for (const joint of joints) {
+		if (joint.parent.name === parent.name) {
+			childJoints.push(joint);
+		}
+	}
+
+	return childJoints;
 }
 
 /**
@@ -102,9 +99,9 @@ export function getChildJoints(
  * @param posable - Either an IUrdfJoint or an IUrdfVisual whose `.elem` has an <origin> child
  */
 export function updateOrigin(posable: IUrdfJoint | IUrdfVisual) {
-  const origin = posable.elem.getElementsByTagName("origin")[0];
-  origin.setAttribute("xyz", posable.origin_xyz.join(" "));
-  origin.setAttribute("rpy", posable.origin_rpy.join(" "));
+	const origin = posable.elem.getElementsByTagName("origin")[0];
+	origin.setAttribute("xyz", posable.origin_xyz.join(" "));
+	origin.setAttribute("rpy", posable.origin_rpy.join(" "));
 }
 
 /**
@@ -116,399 +113,400 @@ export function updateOrigin(posable: IUrdfJoint | IUrdfVisual) {
  *  5) Build an IUrdfRobot data structure that is easier to traverse in JS/Three.js
  */
 export class UrdfParser {
-  filename: string;
-  prefix: string; // e.g. "robots/so_arm100/"
-  colors: { [name: string]: [number, number, number, number] } = {};
-  robot: IUrdfRobot = { name: "", links: {}, joints: [] };
-
-  /**
-   * @param filename - Path or URL to the URDF file (XML). May be relative.
-   * @param prefix   - A folder prefix used when resolving "package://" or relative mesh paths.
-   */
-  constructor(filename: string, prefix: string = "") {
-    this.filename = filename;
-    // Ensure prefix ends with exactly one slash
-    this.prefix = prefix.endsWith("/") ? prefix : prefix + "/";
-  }
-
-  /**
-   * Fetch the URDF file from `this.filename` and return its text.
-   * @returns A promise that resolves to the raw URDF XML string.
-   */
-  async load(): Promise<string> {
-    return fetch(this.filename).then((res) => res.text());
-  }
-
-  /**
-   * Clear any previously parsed robot data, preparing for a fresh parse.
-   */
-  reset() {
-    this.robot = { name: "", links: {}, joints: [] };
-  }
-
-  /**
-   * Parse a URDF XML string and produce an IUrdfRobot object.
-   *
-   * @param data - A string containing valid URDF XML.
-   * @returns The fully populated IUrdfRobot, including colors, links, and joints.
-   * @throws If the root element is not <robot>.
-   */
-  fromString(data: string): IUrdfRobot {
-    this.reset();
-    const dom = new window.DOMParser().parseFromString(data, "text/xml");
-    this.robot.elem = dom.documentElement;
-    return this.parseRobotXMLNode(dom.documentElement);
-  }
-
-  /**
-   * Internal helper: ensure the root node is <robot>, then parse its children.
-   *
-   * @param robotNode - The <robot> Element from the DOMParser.
-   * @returns The populated IUrdfRobot data structure.
-   * @throws If robotNode.nodeName !== "robot"
-   */
-  private parseRobotXMLNode(robotNode: Element): IUrdfRobot {
-    if (robotNode.nodeName !== "robot") {
-      throw new Error(`Invalid URDF: no <robot> (found <${robotNode.nodeName}>)`);
-    }
-
-    this.robot.name = robotNode.getAttribute("name") || "";
-    this.parseColorsFromRobot(robotNode);
-    this.parseLinks(robotNode);
-    this.parseJoints(robotNode);
-    return this.robot;
-  }
-
-  /**
-   * Look at all <material> tags under <robot> and store their names → RGBA values.
-   *
-   * @param robotNode - The <robot> Element.
-   */
-  private parseColorsFromRobot(robotNode: Element) {
-    const xmlMaterials = robotNode.getElementsByTagName("material");
-    for (let i = 0; i < xmlMaterials.length; i++) {
-      const matNode = xmlMaterials[i];
-      if (!matNode.hasAttribute("name")) {
-        console.warn("Found <material> with no name attribute");
-        continue;
-      }
-      const name = matNode.getAttribute("name")!;
-      const colorTags = matNode.getElementsByTagName("color");
-      if (colorTags.length === 0) continue;
-
-      const colorElem = colorTags[0];
-      if (!colorElem.hasAttribute("rgba")) continue;
-
-      // e.g. "0.06 0.4 0.1 1.0"
-      const rgba = rgbaFromString(colorElem) || [0, 0, 0, 1];
-      this.colors[name] = rgba;
-    }
-  }
-
-  /**
-   * Parse every <link> under <robot> and build an IUrdfLink entry containing:
-   *  - name
-   *  - arrays of IUrdfVisual for <visual> tags
-   *  - arrays of IUrdfVisual for <collision> tags
-   *  - a pointer to its original XML Element (elem)
-   *
-   * @param robotNode - The <robot> Element.
-   */
-  private parseLinks(robotNode: Element) {
-    const xmlLinks = robotNode.getElementsByTagName("link");
-    for (let i = 0; i < xmlLinks.length; i++) {
-      const linkXml = xmlLinks[i];
-      if (!linkXml.hasAttribute("name")) {
-        console.error("Link without a name:", linkXml);
-        continue;
-      }
-      const linkName = linkXml.getAttribute("name")!;
-
-      const linkObj: IUrdfLink = {
-        name: linkName,
-        visual: [],
-        collision: [],
-        elem: linkXml,
-      };
-      this.robot.links[linkName] = linkObj;
-
-      // Parse all <visual> children
-      const visualXmls = linkXml.getElementsByTagName("visual");
-      for (let j = 0; j < visualXmls.length; j++) {
-        linkObj.visual.push(this.parseVisual(visualXmls[j]));
-      }
-
-      // Parse all <collision> children (reuse parseVisual; color is ignored later)
-      const collXmls = linkXml.getElementsByTagName("collision");
-      for (let j = 0; j < collXmls.length; j++) {
-        linkObj.collision.push(this.parseVisual(collXmls[j]));
-      }
-    }
-  }
-
-  /**
-   * Parse a <visual> or <collision> element into an IUrdfVisual. Reads:
-   *  - <geometry> (calls parseGeometry to extract mesh, cylinder, box, etc.)
-   *  - <origin> (xyz, rpy)
-   *  - <material> (either embedded <color> or named reference)
-   *
-   * @param node - The <visual> or <collision> Element.
-   * @returns A fully populated IUrdfVisual object.
-   */
-  private parseVisual(node: Element): IUrdfVisual {
-    const visual: Partial<IUrdfVisual> = { elem: node };
-
-    for (let i = 0; i < node.childNodes.length; i++) {
-      const child = node.childNodes[i];
-      
-      // Skip non-element nodes (like text nodes containing whitespace)
-      if (child.nodeType !== Node.ELEMENT_NODE) {
-        continue;
-      }
-      
-      const childElement = child as Element;
-      switch (childElement.nodeName) {
-        case "geometry": {
-          this.parseGeometry(childElement, visual);
-          break;
-        }
-        case "origin": {
-          const pos = xyzFromString(childElement);
-          const rpy = rpyFromString(childElement);
-          if (pos) visual.origin_xyz = pos;
-          if (rpy) visual.origin_rpy = rpy;
-          break;
-        }
-        case "material": {
-          const cols = childElement.getElementsByTagName("color");
-          if (cols.length > 0 && cols[0].hasAttribute("rgba")) {
-            // Inline color specification
-            visual.color_rgba = rgbaFromString(cols[0])!;
-          } else if (childElement.hasAttribute("name")) {
-            // Named material → look up previously parsed RGBA
-            const nm = childElement.getAttribute("name")!;
-            visual.color_rgba = this.colors[nm];
-          }
-          break;
-        }
-        default: {
-          console.warn("Unknown child node:", childElement.nodeName);
-          break;
-        }
-      }
-    }
-
-    return visual as IUrdfVisual;
-  }
-
-  /**
-   * Parse a <geometry> element inside <visual> or <collision>.
-   * Currently only supports <mesh>. If you need <cylinder> or <box>,
-   * you can extend this function similarly.
-   *
-   * @param node    - The <geometry> Element.
-   * @param visual  - A partial IUrdfVisual object to populate
-   */
-  private parseGeometry(node: Element, visual: Partial<IUrdfVisual>) {
-    for (let i = 0; i < node.childNodes.length; i++) {
-      const child = node.childNodes[i];
-      
-      // Skip non-element nodes (like text nodes containing whitespace)
-      if (child.nodeType !== Node.ELEMENT_NODE) {
-        continue;
-      }
-      
-      const childElement = child as Element;
-      if (childElement.nodeName === "mesh") {
-        const rawFilename = childElement.getAttribute("filename");
-        if (!rawFilename) {
-          console.warn("<mesh> missing filename!");
-          return;
-        }
-
-        // 1) Resolve the URL (handles "package://" or relative paths)
-        const resolvedUrl = this.resolveFilename(rawFilename);
-
-        // 2) Parse optional scale (e.g. "1 1 1")
-        let scale: [number, number, number] = [1, 1, 1];
-        if (childElement.hasAttribute("scale")) {
-          const parts = childElement.getAttribute("scale")!.split(" ").map(parseFloat);
-          if (parts.length === 3) {
-            scale = [parts[0], parts[1], parts[2]];
-          }
-        }
-
-        // 3) Deduce mesh type from file extension
-        const ext = resolvedUrl.slice(resolvedUrl.lastIndexOf(".") + 1).toLowerCase();
-        let type: "stl" | "fbx" | "obj" | "dae";
-        switch (ext) {
-          case "stl":
-            type = "stl";
-            break;
-          case "fbx":
-            type = "fbx";
-            break;
-          case "obj":
-            type = "obj";
-            break;
-          case "dae":
-            type = "dae";
-            break;
-          default:
-            throw new Error("Unknown mesh extension: " + ext);
-        }
-
-        visual.geometry = { filename: resolvedUrl, type, scale } as IUrdfMesh;
-        visual.type = "mesh";
-        return;
-      }
-
-      // If you also want <cylinder> or <box>, copy your previous logic here:
-      // e.g. if (childElement.nodeName === "cylinder") { … }
-    }
-  }
-
-  /**
-   * Transform a URI‐like string into an actual URL. Handles:
-   *  1) http(s):// or data: → leave unchanged
-   *  2) package://some_package/... → replace with prefix + "some_package/...
-   *  3) package:/some_package/... → same as above
-   *  4) Anything else (e.g. "meshes/Foo.stl") is treated as relative.
-   *
-   * @param raw - The raw filename from URDF (e.g. "meshes/Base.stl" or "package://my_pkg/mesh.dae")
-   * @returns The fully resolved URL string
-   */
-  private resolveFilename(raw: string): string {
-    // 1) absolute http(s) or data URIs
-    if (/^https?:\/\//.test(raw) || raw.startsWith("data:")) {
-      return raw;
-    }
-
-    // 2) package://some_package/…
-    if (raw.startsWith("package://")) {
-      const rel = raw.substring("package://".length);
-      return this.joinUrl(this.prefix, rel);
-    }
-
-    // 3) package:/some_package/…
-    if (raw.startsWith("package:/")) {
-      const rel = raw.substring("package:/".length);
-      return this.joinUrl(this.prefix, rel);
-    }
-
-    // 4) anything else (e.g. "meshes/Foo.stl") is treated as relative
-    return this.joinUrl(this.prefix, raw);
-  }
-
-  /**
-   * Helper to join a base URL with a relative path, ensuring exactly one '/' in between
-   *
-   * @param base - e.g. "/robots/so_arm100/"
-   * @param rel  - e.g. "meshes/Base.stl" (with or without a leading slash)
-   * @returns A string like "/robots/so_arm100/meshes/Base.stl"
-   */
-  private joinUrl(base: string, rel: string): string {
-    if (!base.startsWith("/")) base = "/" + base;
-    if (!base.endsWith("/")) base = base + "/";
-    if (rel.startsWith("/")) rel = rel.substring(1);
-    return base + rel;
-  }
-
-  /**
-   * Parse every <joint> under <robot> and build an IUrdfJoint entry. For each joint:
-   *  1) parent link (lookup in `this.robot.links[parentName]`)
-   *  2) child link  (lookup in `this.robot.links[childName]`)
-   *  3) origin: xyz + rpy
-   *  4) axis (default [0,0,1] if absent)
-   *  5) limit (if present, lower/upper/effort/velocity)
-   *
-   * @param robotNode - The <robot> Element.
-   * @throws If a joint references a link name that doesn't exist.
-   */
-  private parseJoints(robotNode: Element) {
-    const links = this.robot.links;
-    const joints: IUrdfJoint[] = [];
-    this.robot.joints = joints;
-
-    const xmlJoints = robotNode.getElementsByTagName("joint");
-    for (let i = 0; i < xmlJoints.length; i++) {
-      const jointXml = xmlJoints[i];
-      const parentElems = jointXml.getElementsByTagName("parent");
-      const childElems = jointXml.getElementsByTagName("child");
-      if (parentElems.length !== 1 || childElems.length !== 1) {
-        console.warn("Joint without exactly one <parent> or <child>:", jointXml);
-        continue;
-      }
-
-      const parentName = parentElems[0].getAttribute("link")!;
-      const childName = childElems[0].getAttribute("link")!;
-
-      const parentLink = links[parentName];
-      const childLink = links[childName];
-      if (!parentLink || !childLink) {
-        throw new Error(
-          `Joint references missing link: ${parentName} or ${childName}`
-        );
-      }
-
-      // Default origin and rpy
-      let xyz: [number, number, number] = [0, 0, 0];
-      let rpy: [number, number, number] = [0, 0, 0];
-      const originTags = jointXml.getElementsByTagName("origin");
-      if (originTags.length === 1) {
-        xyz = xyzFromString(originTags[0]) || xyz;
-        rpy = rpyFromString(originTags[0]) || rpy;
-      }
-
-      // Default axis
-      let axis: [number, number, number] = [0, 0, 1];
-      const axisTags = jointXml.getElementsByTagName("axis");
-      if (axisTags.length === 1) {
-        axis = xyzFromString(axisTags[0]) || axis;
-      }
-
-      // Optional limit
-      let limit;
-      const limitTags = jointXml.getElementsByTagName("limit");
-      if (limitTags.length === 1) {
-        const lim = limitTags[0];
-        limit = {
-          lower: parseFloat(lim.getAttribute("lower") || "0"),
-          upper: parseFloat(lim.getAttribute("upper") || "0"),
-          effort: parseFloat(lim.getAttribute("effort") || "0"),
-          velocity: parseFloat(lim.getAttribute("velocity") || "0"),
-        };
-      }
-
-      joints.push({
-        name: jointXml.getAttribute("name") || undefined,
-        type: jointXml.getAttribute("type") as "revolute" | "continuous" | "prismatic" | "fixed" | "floating" | "planar",
-        origin_xyz: xyz,
-        origin_rpy: rpy,
-        axis_xyz: axis,
-        rotation: [0, 0, 0],
-        parent: parentLink,
-        child: childLink,
-        limit: limit,
-        elem: jointXml,
-      });
-    }
-  }
-
-  /**
-   * If you ever want to re‐serialize the robot back to URDF XML,
-   * this method returns the stringified root <robot> element.
-   *
-   * @returns A string beginning with '<?xml version="1.0" ?>' followed by the current XML.
-   */
-  getURDFXML(): string {
-    return this.robot.elem
-      ? '<?xml version="1.0" ?>\n' + this.robot.elem.outerHTML
-      : "";
-  }
+	filename: string;
+	prefix: string; // e.g. "robots/so_arm100/"
+	colors: { [name: string]: [number, number, number, number] } = {};
+	robot: IUrdfRobot = { name: "", links: {}, joints: [] };
+
+	/**
+	 * @param filename - Path or URL to the URDF file (XML). May be relative.
+	 * @param prefix   - A folder prefix used when resolving "package://" or relative mesh paths.
+	 */
+	constructor(filename: string, prefix: string = "") {
+		this.filename = filename;
+		// Ensure prefix ends with exactly one slash
+		this.prefix = prefix.endsWith("/") ? prefix : prefix + "/";
+	}
+
+	/**
+	 * Fetch the URDF file from `this.filename` and return its text.
+	 * @returns A promise that resolves to the raw URDF XML string.
+	 */
+	async load(): Promise<string> {
+		return fetch(this.filename).then((res) => res.text());
+	}
+
+	/**
+	 * Clear any previously parsed robot data, preparing for a fresh parse.
+	 */
+	reset() {
+		this.robot = { name: "", links: {}, joints: [] };
+	}
+
+	/**
+	 * Parse a URDF XML string and produce an IUrdfRobot object.
+	 *
+	 * @param data - A string containing valid URDF XML.
+	 * @returns The fully populated IUrdfRobot, including colors, links, and joints.
+	 * @throws If the root element is not <robot>.
+	 */
+	fromString(data: string): IUrdfRobot {
+		this.reset();
+		const dom = new window.DOMParser().parseFromString(data, "text/xml");
+		this.robot.elem = dom.documentElement;
+		return this.parseRobotXMLNode(dom.documentElement);
+	}
+
+	/**
+	 * Internal helper: ensure the root node is <robot>, then parse its children.
+	 *
+	 * @param robotNode - The <robot> Element from the DOMParser.
+	 * @returns The populated IUrdfRobot data structure.
+	 * @throws If robotNode.nodeName !== "robot"
+	 */
+	private parseRobotXMLNode(robotNode: Element): IUrdfRobot {
+		if (robotNode.nodeName !== "robot") {
+			throw new Error(`Invalid URDF: no <robot> (found <${robotNode.nodeName}>)`);
+		}
+
+		this.robot.name = robotNode.getAttribute("name") || "";
+		this.parseColorsFromRobot(robotNode);
+		this.parseLinks(robotNode);
+		this.parseJoints(robotNode);
+		return this.robot;
+	}
+
+	/**
+	 * Look at all <material> tags under <robot> and store their names → RGBA values.
+	 *
+	 * @param robotNode - The <robot> Element.
+	 */
+	private parseColorsFromRobot(robotNode: Element) {
+		const xmlMaterials = robotNode.getElementsByTagName("material");
+		for (let i = 0; i < xmlMaterials.length; i++) {
+			const matNode = xmlMaterials[i];
+			if (!matNode.hasAttribute("name")) {
+				console.warn("Found <material> with no name attribute");
+				continue;
+			}
+			const name = matNode.getAttribute("name")!;
+			const colorTags = matNode.getElementsByTagName("color");
+			if (colorTags.length === 0) continue;
+
+			const colorElem = colorTags[0];
+			if (!colorElem.hasAttribute("rgba")) continue;
+
+			// e.g. "0.06 0.4 0.1 1.0"
+			const rgba = rgbaFromString(colorElem) || [0, 0, 0, 1];
+			this.colors[name] = rgba;
+		}
+	}
+
+	/**
+	 * Parse every <link> under <robot> and build an IUrdfLink entry containing:
+	 *  - name
+	 *  - arrays of IUrdfVisual for <visual> tags
+	 *  - arrays of IUrdfVisual for <collision> tags
+	 *  - a pointer to its original XML Element (elem)
+	 *
+	 * @param robotNode - The <robot> Element.
+	 */
+	private parseLinks(robotNode: Element) {
+		const xmlLinks = robotNode.getElementsByTagName("link");
+		for (let i = 0; i < xmlLinks.length; i++) {
+			const linkXml = xmlLinks[i];
+			if (!linkXml.hasAttribute("name")) {
+				console.error("Link without a name:", linkXml);
+				continue;
+			}
+			const linkName = linkXml.getAttribute("name")!;
+
+			const linkObj: IUrdfLink = {
+				name: linkName,
+				visual: [],
+				collision: [],
+				elem: linkXml
+			};
+			this.robot.links[linkName] = linkObj;
+
+			// Parse all <visual> children
+			const visualXmls = linkXml.getElementsByTagName("visual");
+			for (let j = 0; j < visualXmls.length; j++) {
+				linkObj.visual.push(this.parseVisual(visualXmls[j]));
+			}
+
+			// Parse all <collision> children (reuse parseVisual; color is ignored later)
+			const collXmls = linkXml.getElementsByTagName("collision");
+			for (let j = 0; j < collXmls.length; j++) {
+				linkObj.collision.push(this.parseVisual(collXmls[j]));
+			}
+		}
+	}
+
+	/**
+	 * Parse a <visual> or <collision> element into an IUrdfVisual. Reads:
+	 *  - <geometry> (calls parseGeometry to extract mesh, cylinder, box, etc.)
+	 *  - <origin> (xyz, rpy)
+	 *  - <material> (either embedded <color> or named reference)
+	 *
+	 * @param node - The <visual> or <collision> Element.
+	 * @returns A fully populated IUrdfVisual object.
+	 */
+	private parseVisual(node: Element): IUrdfVisual {
+		const visual: Partial<IUrdfVisual> = { elem: node };
+
+		for (let i = 0; i < node.childNodes.length; i++) {
+			const child = node.childNodes[i];
+
+			// Skip non-element nodes (like text nodes containing whitespace)
+			if (child.nodeType !== Node.ELEMENT_NODE) {
+				continue;
+			}
+
+			const childElement = child as Element;
+			switch (childElement.nodeName) {
+				case "geometry": {
+					this.parseGeometry(childElement, visual);
+					break;
+				}
+				case "origin": {
+					const pos = xyzFromString(childElement);
+					const rpy = rpyFromString(childElement);
+					if (pos) visual.origin_xyz = pos;
+					if (rpy) visual.origin_rpy = rpy;
+					break;
+				}
+				case "material": {
+					const cols = childElement.getElementsByTagName("color");
+					if (cols.length > 0 && cols[0].hasAttribute("rgba")) {
+						// Inline color specification
+						visual.color_rgba = rgbaFromString(cols[0])!;
+					} else if (childElement.hasAttribute("name")) {
+						// Named material → look up previously parsed RGBA
+						const nm = childElement.getAttribute("name")!;
+						visual.color_rgba = this.colors[nm];
+					}
+					break;
+				}
+				default: {
+					console.warn("Unknown child node:", childElement.nodeName);
+					break;
+				}
+			}
+		}
+
+		return visual as IUrdfVisual;
+	}
+
+	/**
+	 * Parse a <geometry> element inside <visual> or <collision>.
+	 * Currently only supports <mesh>. If you need <cylinder> or <box>,
+	 * you can extend this function similarly.
+	 *
+	 * @param node    - The <geometry> Element.
+	 * @param visual  - A partial IUrdfVisual object to populate
+	 */
+	private parseGeometry(node: Element, visual: Partial<IUrdfVisual>) {
+		for (let i = 0; i < node.childNodes.length; i++) {
+			const child = node.childNodes[i];
+
+			// Skip non-element nodes (like text nodes containing whitespace)
+			if (child.nodeType !== Node.ELEMENT_NODE) {
+				continue;
+			}
+
+			const childElement = child as Element;
+			if (childElement.nodeName === "mesh") {
+				const rawFilename = childElement.getAttribute("filename");
+				if (!rawFilename) {
+					console.warn("<mesh> missing filename!");
+					return;
+				}
+
+				// 1) Resolve the URL (handles "package://" or relative paths)
+				const resolvedUrl = this.resolveFilename(rawFilename);
+
+				// 2) Parse optional scale (e.g. "1 1 1")
+				let scale: [number, number, number] = [1, 1, 1];
+				if (childElement.hasAttribute("scale")) {
+					const parts = childElement.getAttribute("scale")!.split(" ").map(parseFloat);
+					if (parts.length === 3) {
+						scale = [parts[0], parts[1], parts[2]];
+					}
+				}
+
+				// 3) Deduce mesh type from file extension
+				const ext = resolvedUrl.slice(resolvedUrl.lastIndexOf(".") + 1).toLowerCase();
+				let type: "stl" | "fbx" | "obj" | "dae";
+				switch (ext) {
+					case "stl":
+						type = "stl";
+						break;
+					case "fbx":
+						type = "fbx";
+						break;
+					case "obj":
+						type = "obj";
+						break;
+					case "dae":
+						type = "dae";
+						break;
+					default:
+						throw new Error("Unknown mesh extension: " + ext);
+				}
+
+				visual.geometry = { filename: resolvedUrl, type, scale } as IUrdfMesh;
+				visual.type = "mesh";
+				return;
+			}
+
+			// If you also want <cylinder> or <box>, copy your previous logic here:
+			// e.g. if (childElement.nodeName === "cylinder") { … }
+		}
+	}
+
+	/**
+	 * Transform a URI‐like string into an actual URL. Handles:
+	 *  1) http(s):// or data: → leave unchanged
+	 *  2) package://some_package/... → replace with prefix + "some_package/...
+	 *  3) package:/some_package/... → same as above
+	 *  4) Anything else (e.g. "meshes/Foo.stl") is treated as relative.
+	 *
+	 * @param raw - The raw filename from URDF (e.g. "meshes/Base.stl" or "package://my_pkg/mesh.dae")
+	 * @returns The fully resolved URL string
+	 */
+	private resolveFilename(raw: string): string {
+		// 1) absolute http(s) or data URIs
+		if (/^https?:\/\//.test(raw) || raw.startsWith("data:")) {
+			return raw;
+		}
+
+		// 2) package://some_package/…
+		if (raw.startsWith("package://")) {
+			const rel = raw.substring("package://".length);
+			return this.joinUrl(this.prefix, rel);
+		}
+
+		// 3) package:/some_package/…
+		if (raw.startsWith("package:/")) {
+			const rel = raw.substring("package:/".length);
+			return this.joinUrl(this.prefix, rel);
+		}
+
+		// 4) anything else (e.g. "meshes/Foo.stl") is treated as relative
+		return this.joinUrl(this.prefix, raw);
+	}
+
+	/**
+	 * Helper to join a base URL with a relative path, ensuring exactly one '/' in between
+	 *
+	 * @param base - e.g. "/robots/so_arm100/"
+	 * @param rel  - e.g. "meshes/Base.stl" (with or without a leading slash)
+	 * @returns A string like "/robots/so_arm100/meshes/Base.stl"
+	 */
+	private joinUrl(base: string, rel: string): string {
+		if (!base.startsWith("/")) base = "/" + base;
+		if (!base.endsWith("/")) base = base + "/";
+		if (rel.startsWith("/")) rel = rel.substring(1);
+		return base + rel;
+	}
+
+	/**
+	 * Parse every <joint> under <robot> and build an IUrdfJoint entry. For each joint:
+	 *  1) parent link (lookup in `this.robot.links[parentName]`)
+	 *  2) child link  (lookup in `this.robot.links[childName]`)
+	 *  3) origin: xyz + rpy
+	 *  4) axis (default [0,0,1] if absent)
+	 *  5) limit (if present, lower/upper/effort/velocity)
+	 *
+	 * @param robotNode - The <robot> Element.
+	 * @throws If a joint references a link name that doesn't exist.
+	 */
+	private parseJoints(robotNode: Element) {
+		const links = this.robot.links;
+		const joints: IUrdfJoint[] = [];
+		this.robot.joints = joints;
+
+		const xmlJoints = robotNode.getElementsByTagName("joint");
+		for (let i = 0; i < xmlJoints.length; i++) {
+			const jointXml = xmlJoints[i];
+			const parentElems = jointXml.getElementsByTagName("parent");
+			const childElems = jointXml.getElementsByTagName("child");
+			if (parentElems.length !== 1 || childElems.length !== 1) {
+				console.warn("Joint without exactly one <parent> or <child>:", jointXml);
+				continue;
+			}
+
+			const parentName = parentElems[0].getAttribute("link")!;
+			const childName = childElems[0].getAttribute("link")!;
+
+			const parentLink = links[parentName];
+			const childLink = links[childName];
+			if (!parentLink || !childLink) {
+				throw new Error(`Joint references missing link: ${parentName} or ${childName}`);
+			}
+
+			// Default origin and rpy
+			let xyz: [number, number, number] = [0, 0, 0];
+			let rpy: [number, number, number] = [0, 0, 0];
+			const originTags = jointXml.getElementsByTagName("origin");
+			if (originTags.length === 1) {
+				xyz = xyzFromString(originTags[0]) || xyz;
+				rpy = rpyFromString(originTags[0]) || rpy;
+			}
+
+			// Default axis
+			let axis: [number, number, number] = [0, 0, 1];
+			const axisTags = jointXml.getElementsByTagName("axis");
+			if (axisTags.length === 1) {
+				axis = xyzFromString(axisTags[0]) || axis;
+			}
+
+			// Optional limit
+			let limit;
+			const limitTags = jointXml.getElementsByTagName("limit");
+			if (limitTags.length === 1) {
+				const lim = limitTags[0];
+				limit = {
+					lower: parseFloat(lim.getAttribute("lower") || "0"),
+					upper: parseFloat(lim.getAttribute("upper") || "0"),
+					effort: parseFloat(lim.getAttribute("effort") || "0"),
+					velocity: parseFloat(lim.getAttribute("velocity") || "0")
+				};
+			}
+
+			joints.push({
+				name: jointXml.getAttribute("name") || undefined,
+				type: jointXml.getAttribute("type") as
+					| "revolute"
+					| "continuous"
+					| "prismatic"
+					| "fixed"
+					| "floating"
+					| "planar",
+				origin_xyz: xyz,
+				origin_rpy: rpy,
+				axis_xyz: axis,
+				rotation: [0, 0, 0],
+				parent: parentLink,
+				child: childLink,
+				limit: limit,
+				elem: jointXml
+			});
+		}
+	}
+
+	/**
+	 * If you ever want to re‐serialize the robot back to URDF XML,
+	 * this method returns the stringified root <robot> element.
+	 *
+	 * @returns A string beginning with '<?xml version="1.0" ?>' followed by the current XML.
+	 */
+	getURDFXML(): string {
+		return this.robot.elem ? '<?xml version="1.0" ?>\n' + this.robot.elem.outerHTML : "";
+	}
 }
 
-
 /**
  * ==============================================================================
  * Example of how the parsed data (IUrdfRobot) maps from the URDF XML ("so_arm100"):

src/lib/components/3d/robot/URDF/utils/helper.ts

@@ -1,50 +1,53 @@
 export function xyzFromString(child: Element): [x: number, y: number, z: number] | undefined {
-  const arr = numberStringToArray(child, 'xyz');
-  if (!arr || arr.length != 3) {
-    return
-  }
-  return arr as [x: number, y: number, z: number];
+	const arr = numberStringToArray(child, "xyz");
+	if (!arr || arr.length != 3) {
+		return;
+	}
+	return arr as [x: number, y: number, z: number];
 }
 
-export function rpyFromString(child: Element): [roll: number, pitch: number, yaw: number] | undefined {
-  const arr = numberStringToArray(child, 'rpy');
-  if (!arr || arr.length != 3) {
-    return
-  }
-  return arr as [roll: number, pitch: number, yaw: number];
+export function rpyFromString(
+	child: Element
+): [roll: number, pitch: number, yaw: number] | undefined {
+	const arr = numberStringToArray(child, "rpy");
+	if (!arr || arr.length != 3) {
+		return;
+	}
+	return arr as [roll: number, pitch: number, yaw: number];
 }
 
-export function rgbaFromString(child: Element): [r: number, g: number, b: number, a: number] | undefined {
-  const arr = numberStringToArray(child, 'rgba');
-  if (!arr || arr.length != 4) {
-    return
-  }
-  return arr as [r: number, g: number, b: number, a: number];
+export function rgbaFromString(
+	child: Element
+): [r: number, g: number, b: number, a: number] | undefined {
+	const arr = numberStringToArray(child, "rgba");
+	if (!arr || arr.length != 4) {
+		return;
+	}
+	return arr as [r: number, g: number, b: number, a: number];
 }
 
-export function numberStringToArray(
-    child: Element, name: string = 'xyz'): number[] | undefined {
-  // parse a list of values from a string 
-  // (like "1.0 2.2 3.0" into an array like [1, 2.2, 3])
-  // used in URDF for position, orientation an color values
-  if (child.hasAttribute(name)) {
-    const xyzStr = child.getAttribute(name)?.split(' ')
-    if (xyzStr) {
-      const arr = []
-      for (const nr of xyzStr) {
-        arr.push(parseFloat(nr));
-      }
-      return arr;
-    }
-  }
+export function numberStringToArray(child: Element, name: string = "xyz"): number[] | undefined {
+	// parse a list of values from a string
+	// (like "1.0 2.2 3.0" into an array like [1, 2.2, 3])
+	// used in URDF for position, orientation an color values
+	if (child.hasAttribute(name)) {
+		const xyzStr = child.getAttribute(name)?.split(" ");
+		if (xyzStr) {
+			const arr = [];
+			for (const nr of xyzStr) {
+				arr.push(parseFloat(nr));
+			}
+			return arr;
+		}
+	}
 }
 
 export function radToEuler(rad: number): number {
-  return rad * 180 / Math.PI
+	return (rad * 180) / Math.PI;
 }
 
 export function numberArrayToColor([r, g, b]: [number, number, number]): string {
-  const toHex = (n: number) => Math.round(n).toString(16).padStart(2, '0');
-  // 0.06, 0.4, 0.1, 1
-  return `#${toHex(r * 255)}${toHex(g * 255)}${toHex(b * 255)}`;
+	const toHex = (n: number) => Math.round(n).toString(16).padStart(2, "0");
+	// 0.06, 0.4, 0.1, 1
+	return `#${toHex(r * 255)}${toHex(g * 255)}${toHex(b * 255)}`;
 }

src/lib/components/ButtonBar.svelte

@@ -1,161 +0,0 @@
-<script lang="ts">
-	import { Button } from '$lib/components/ui/button';
-	import { Badge } from '$lib/components/ui/badge';
-	import * as Popover from '$lib/components/ui/popover';
-	import { toast } from 'svelte-sonner';
-	import { cn } from '$lib/utils';
-	import { robotManager } from "$lib/robot/RobotManager.svelte";
-	import { robotUrdfConfigMap } from "$lib/configs/robotUrdfConfig";
-
-	interface Props {
-		controlsOpen?: boolean;
-		onToggleControls?: () => void;
-	}
-
-	let { 
-		controlsOpen = $bindable(false),
-		onToggleControls
-	}: Props = $props();
-
-	let isCreating = $state(false);
-	let selectedRobotType = $state("so-arm100"); // Default to SO-100
-	let showRobotSelector = $state(false);
-
-	// Get available robot types
-	const robotTypes = Object.keys(robotUrdfConfigMap);
-
-	// Function to add robot using the proper robot creation logic
-	async function addRobot() {
-		if (isCreating) return;
-		
-		console.log(`Creating ${selectedRobotType} robot...`);
-		isCreating = true;
-		
-		try {
-			const urdfConfig = robotUrdfConfigMap[selectedRobotType];
-			
-			if (!urdfConfig) {
-				throw new Error(`Unknown robot type: ${selectedRobotType}`);
-			}
-			
-			const robotId = `robot-${Date.now()}`;
-			console.log('Creating robot with ID:', robotId, 'and config:', urdfConfig);
-			
-			const robot = await robotManager.createRobot(robotId, urdfConfig);
-			console.log('Robot created successfully:', robot);
-			
-			toast.success("Robot Added", {
-				description: `${selectedRobotType} robot ${robotId.slice(0, 12)}... created successfully.`
-			});
-			
-			showRobotSelector = false; // Close selector on success
-			
-		} catch (error) {
-			console.error('Robot creation failed:', error);
-			toast.error("Failed to Add Robot", {
-				description: `Could not create ${selectedRobotType} robot: ${error}`
-			});
-		} finally {
-			isCreating = false;
-		}
-	}
-
-	// Quick add SO-100 function
-	async function quickAddSO100() {
-		selectedRobotType = "so-arm100";
-		await addRobot();
-	}
-</script>
-
-<!-- Button Bar Container -->
-<div class="fixed top-4 left-4 z-50 flex gap-2">
-	<!-- Add Robot Button with Dropdown -->
-	<div class="flex">
-		<!-- Main Add Button (SO-100) -->
-		<Button 
-			variant="default" 
-			size="sm"
-			onclick={quickAddSO100}
-			disabled={isCreating}
-			class="shadow-lg bg-green-600 hover:bg-green-500 disabled:bg-gray-600 text-white border-green-500 transition-all duration-200 group rounded-r-none border-r-0"
-		>
-			<span class={cn(
-				"size-4 mr-2 transition-transform duration-200",
-				isCreating ? "icon-[mdi--loading] animate-spin" : "icon-[mdi--plus] group-hover:rotate-90"
-			)}></span>
-			{isCreating ? 'Creating...' : 'Add SO-100'}
-		</Button>
-
-		<!-- Dropdown Button -->
-		<Popover.Root bind:open={showRobotSelector}>
-			<Popover.Trigger>
-				{#snippet child({ props })}
-					<Button 
-						{...props}
-						variant="default"
-						size="sm"
-						disabled={isCreating}
-						class="shadow-lg bg-green-600 hover:bg-green-500 disabled:bg-gray-600 text-white border-green-500 transition-all duration-200 rounded-l-none border-l border-green-400 px-2"
-					>
-						<span class="icon-[mdi--chevron-down] size-4"></span>
-					</Button>
-				{/snippet}
-			</Popover.Trigger>
-			<Popover.Content class="w-64 p-3 bg-slate-800 border-slate-600">
-				<div class="space-y-3">
-					<h4 class="text-sm font-medium text-slate-100">Select Robot Type</h4>
-					<div class="space-y-2">
-						{#each robotTypes as robotType}
-							<Button
-								variant={selectedRobotType === robotType ? "default" : "ghost"}
-								size="sm"
-								onclick={async () => {
-									selectedRobotType = robotType;
-									await addRobot();
-								}}
-								disabled={isCreating}
-								class="w-full justify-start text-left"
-							>
-								<span class="icon-[mdi--robot] size-4 mr-2"></span>
-								{robotType}
-								{#if robotType === "so-arm100"}
-									<Badge variant="secondary" class="ml-auto text-xs">SO-100</Badge>
-								{/if}
-							</Button>
-						{/each}
-					</div>
-				</div>
-			</Popover.Content>
-		</Popover.Root>
-	</div>
-
-	<!-- Controls Button -->
-	<Button 
-		variant="outline" 
-		size="sm"
-		onclick={onToggleControls}
-		class={cn(
-			"shadow-lg transition-all duration-200 border-slate-600 backdrop-blur-sm group",
-			controlsOpen 
-				? "bg-blue-600/90 text-white hover:bg-blue-500 border-blue-500" 
-				: "bg-slate-800/90 text-slate-100 hover:bg-slate-700"
-		)}
-	>
-		<span class={cn(
-			"size-4 mr-2 transition-transform duration-200",
-			controlsOpen 
-				? "icon-[mdi--close] group-hover:rotate-90" 
-				: "icon-[mdi--gamepad-variant] group-hover:scale-110"
-		)}></span>
-		{controlsOpen ? 'Close' : 'Controls'}
-		
-		{#if !controlsOpen}
-			<Badge 
-				variant="secondary" 
-				class="ml-2 bg-blue-400/20 text-blue-300 border-blue-400/30 text-xs"
-			>
-				Robot
-			</Badge>
-		{/if}
-	</Button>
-</div> 

src/lib/components/ControlsSheet.svelte

@@ -1,76 +0,0 @@
-<script lang="ts">
-	import { Button } from '$lib/components/ui/button';
-	import * as Sheet from '$lib/components/ui/sheet';
-	import { Badge } from '$lib/components/ui/badge';
-	import { Separator } from '$lib/components/ui/separator';
-	import ControlPanel from '$lib/components/panel/ControlPanel.svelte';
-
-	interface Props {
-		isOpen?: boolean;
-	}
-
-	let { isOpen = $bindable(false) }: Props = $props();
-
-	function toggleSheet() {
-		isOpen = !isOpen;
-	}
-</script>
-
-<!-- Controls Sheet -->
-<Sheet.Root bind:open={isOpen}>
-	<Sheet.Content 
-		side="right" 
-		class="w-80 sm:w-96 p-0 bg-gradient-to-b from-slate-700 to-slate-800 text-white border-l border-slate-600"
-	>
-		<!-- Header -->
-		<Sheet.Header class="border-b border-slate-600 bg-slate-700/80 backdrop-blur-sm p-6">
-			<div class="flex items-center justify-between">
-				<div class="flex items-center gap-3">
-					<span class="icon-[mdi--gamepad-variant] size-6 text-blue-400"></span>
-					<div>
-						<Sheet.Title class="text-xl font-semibold text-slate-100">
-							Robot Controls
-						</Sheet.Title>
-						<p class="text-sm text-slate-400 mt-1">
-							Control your robot
-						</p>
-					</div>
-				</div>
-				<Badge variant="secondary" class="bg-blue-400 text-slate-900 font-medium px-3">
-					Active
-				</Badge>
-			</div>
-		</Sheet.Header>
-
-		<!-- Content -->
-		<div class="flex-1 overflow-y-auto scrollbar-thin scrollbar-track-slate-700 scrollbar-thumb-slate-500 px-4">
-			<div class="space-y-4">
-				<div class="flex items-center gap-2 text-sm text-slate-300">
-					<span class="icon-[mdi--information-outline] size-4"></span>
-					Control your robot's movements and actions
-				</div>
-				<Separator class="bg-slate-600" />
-				<ControlPanel />
-			</div>
-		</div>
-
-		<!-- Footer -->
-		<div class="border-t border-slate-600 bg-slate-800/50 p-4">
-			<div class="flex items-center justify-between">
-				<div class="flex items-center gap-2 text-xs text-slate-400">
-					<span class="icon-[mdi--gamepad-variant] size-4"></span>
-					<span>Robot Controls</span>
-				</div>
-				<Button 
-					variant="ghost" 
-					size="sm" 
-					onclick={toggleSheet}
-					class="text-slate-400 hover:text-slate-100 text-xs px-2 py-1 h-auto"
-				>
-					<span class="icon-[mdi--close] size-3 mr-1"></span>
-					Close
-				</Button>
-			</div>
-		</div>
-	</Sheet.Content>
-</Sheet.Root> 

src/lib/components/Overlay.svelte

@@ -1,36 +0,0 @@
-<script lang="ts">
-	import ButtonBar from '$lib/components/ButtonBar.svelte';
-	import ControlsSheet from '$lib/components/ControlsSheet.svelte';
-	import SettingsSheet from '$lib/components/SettingsSheet.svelte';
-
-	interface Props {
-		// Optional props for controlling the state externally if needed
-		controlsOpen?: boolean;
-		settingsOpen?: boolean;
-	}
-
-	let { 
-		controlsOpen = $bindable(false),
-		settingsOpen = $bindable(false)
-	}: Props = $props();
-
-	function toggleControls() {
-		controlsOpen = !controlsOpen;
-	}
-
-	function toggleSettings() {
-		settingsOpen = !settingsOpen;
-	}
-</script>
-
-<!-- Button Bar with Add Robot and Controls (Top Left) -->
-<ButtonBar 
-	bind:controlsOpen
-	onToggleControls={toggleControls}
-/>
-
-<!-- Controls Sheet (Right Side) -->
-<ControlsSheet bind:isOpen={controlsOpen} />
-
-<!-- Settings Button and Sheet (Top Right, Left Side) -->
-<SettingsSheet bind:isOpen={settingsOpen} />

src/lib/components/PanelWrapper.svelte

@@ -1,32 +0,0 @@
-<script lang="ts">
-	import * as Card from '$lib/components/ui/card';
-	import { Separator } from '$lib/components/ui/separator';
-
-	interface Props {
-		title?: string;
-		subtitle?: string;
-		children: any;
-	}
-
-	let { title, subtitle, children }: Props = $props();
-</script>
-
-<Card.Root class="bg-slate-700/50 border-slate-600">
-	{#if title}
-		<Card.Header class="pb-3">
-			<Card.Title class="text-slate-100 flex items-center gap-2">
-				{title}
-			</Card.Title>
-			{#if subtitle}
-				<Card.Description class="text-slate-400 text-sm">
-					{subtitle}
-				</Card.Description>
-			{/if}
-		</Card.Header>
-		<Separator class="bg-slate-600" />
-	{/if}
-	
-	<Card.Content class="p-4 space-y-4">
-		{@render children()}
-	</Card.Content>
-</Card.Root> 

src/lib/components/RobotStatusDemo.svelte

@@ -1,65 +0,0 @@
-<script lang="ts">
-	import StatusCard from '$lib/components/StatusCard.svelte';
-	import { Button } from '$lib/components/ui/button';
-	import { Badge } from '$lib/components/ui/badge';
-	import { environment } from '$lib/runes/env.svelte';
-
-	// Sample usage of StatusCard component
-	const robotStats = $derived(() => {
-		const robotCount = Object.keys(environment.robots).length;
-		return {
-			robotCount,
-			status: (robotCount > 0 ? 'active' : 'inactive') as 'active' | 'inactive',
-			lastUpdate: new Date().toLocaleTimeString()
-		};
-	});
-</script>
-
-<div class="space-y-4">
-	<h3 class="text-lg font-semibold text-slate-100 mb-4">Robot Status Overview</h3>
-	
-	<div class="grid grid-cols-1 gap-3">
-		<StatusCard
-			title="Connected Robots"
-			status={robotStats().status}
-			value={robotStats().robotCount}
-			description="Active robot instances"
-			icon="icon-[mdi--robot]"
-		>
-			{#if robotStats().robotCount > 0}
-				<div class="flex flex-wrap gap-1">
-					{#each Object.entries(environment.robots) as [id, robot]}
-						<Badge variant="outline" class="text-xs text-slate-300 border-slate-500">
-							{id.slice(0, 8)}...
-						</Badge>
-					{/each}
-				</div>
-			{:else}
-				<p class="text-xs text-slate-500">No robots currently active</p>
-			{/if}
-		</StatusCard>
-
-		<StatusCard
-			title="System Status"
-			status="active"
-			description="All systems operational"
-			icon="icon-[mdi--check-circle]"
-		>
-			<div class="text-xs text-slate-400">
-				Last updated: {robotStats().lastUpdate}
-			</div>
-		</StatusCard>
-
-		<StatusCard
-			title="Control Mode"
-			status="active"
-			value="Manual"
-			description="Direct joint control enabled"
-			icon="icon-[mdi--gamepad-variant]"
-		>
-			<Button size="sm" variant="outline" class="w-full text-xs">
-				Switch to Autonomous
-			</Button>
-		</StatusCard>
-	</div>
-</div> 

src/lib/components/SettingsSheet.svelte

@@ -1,109 +0,0 @@
-<script lang="ts">
-	import { Button } from '$lib/components/ui/button';
-	import * as Sheet from '$lib/components/ui/sheet';
-	import { Badge } from '$lib/components/ui/badge';
-	import { Separator } from '$lib/components/ui/separator';
-	import SettingsPanel from '$lib/components/panel/SettingsPanel.svelte';
-	import { cn } from '$lib/utils';
-
-	interface Props {
-		isOpen?: boolean;
-	}
-
-	let { isOpen = $bindable(false) }: Props = $props();
-
-	function toggleSheet() {
-		isOpen = !isOpen;
-	}
-</script>
-
-<!-- Settings Trigger Button -->
-<div class="fixed top-4 right-4 z-50">
-	<Button 
-		variant="outline" 
-		size="sm"
-		onclick={toggleSheet}
-		class={cn(
-			"shadow-lg transition-all duration-200 border-slate-600 backdrop-blur-sm group",
-			isOpen 
-				? "bg-orange-600/90 text-white hover:bg-orange-500 border-orange-500" 
-				: "bg-slate-800/90 text-slate-100 hover:bg-slate-700"
-		)}
-	>
-		<span class={cn(
-			"size-4 mr-2 transition-transform duration-200",
-			isOpen 
-				? "icon-[mdi--close] group-hover:rotate-90" 
-				: "icon-[mdi--cog] group-hover:rotate-45"
-		)}></span>
-		{isOpen ? 'Close' : 'Settings'}
-		
-		{#if !isOpen}
-			<Badge 
-				variant="secondary" 
-				class="ml-2 bg-orange-400/20 text-orange-300 border-orange-400/30 text-xs"
-			>
-				Joints
-			</Badge>
-		{/if}
-	</Button>
-</div>
-
-<!-- Settings Sheet -->
-<Sheet.Root bind:open={isOpen}>
-	<Sheet.Content 
-		side="left" 
-		class="w-80 sm:w-96 p-0 bg-gradient-to-b from-slate-700 to-slate-800 text-white border-r border-slate-600"
-	>
-		<!-- Header -->
-		<Sheet.Header class="border-b border-slate-600 bg-slate-700/80 backdrop-blur-sm p-6">
-			<div class="flex items-center justify-between">
-				<div class="flex items-center gap-3">
-					<span class="icon-[mdi--cog] size-6 text-orange-400"></span>
-					<div>
-						<Sheet.Title class="text-xl font-semibold text-slate-100">
-							Robot Settings
-						</Sheet.Title>
-						<p class="text-sm text-slate-400 mt-1">
-							Configure joints and robot parameters
-						</p>
-					</div>
-				</div>
-				<Badge variant="secondary" class="bg-orange-400 text-slate-900 font-medium px-3">
-					Config
-				</Badge>
-			</div>
-		</Sheet.Header>
-
-		<!-- Content -->
-		<div class="flex-1 overflow-y-auto scrollbar-thin scrollbar-track-slate-700 scrollbar-thumb-slate-500 p-4">
-			<div class="space-y-4">
-				<div class="flex items-center gap-2 text-sm text-slate-300">
-					<span class="icon-[mdi--wrench] size-4"></span>
-					Configure joints and robot parameters
-				</div>
-				<Separator class="bg-slate-600" />
-				<SettingsPanel />
-			</div>
-		</div>
-
-		<!-- Footer -->
-		<div class="border-t border-slate-600 bg-slate-800/50 p-4">
-			<div class="flex items-center justify-between">
-				<div class="flex items-center gap-2 text-xs text-slate-400">
-					<span class="icon-[mdi--cog] size-4"></span>
-					<span>Robot Settings</span>
-				</div>
-				<Button 
-					variant="ghost" 
-					size="sm" 
-					onclick={toggleSheet}
-					class="text-slate-400 hover:text-slate-100 text-xs px-2 py-1 h-auto"
-				>
-					<span class="icon-[mdi--close] size-3 mr-1"></span>
-					Close
-				</Button>
-			</div>
-		</div>
-	</Sheet.Content>
-</Sheet.Root>