Spaces:
Running
Running
import type { | |
JointState, | |
RobotCommand, | |
ConnectionStatus, | |
USBDriverConfig, | |
RemoteDriverConfig, | |
Consumer, | |
Producer | |
} from "./models.js"; | |
import type { Positionable, Position3D } from "$lib/types/positionable.js"; | |
import { USBConsumer } from "./drivers/USBConsumer.js"; | |
import { USBProducer } from "./drivers/USBProducer.js"; | |
import { RemoteConsumer } from "./drivers/RemoteConsumer.js"; | |
import { RemoteProducer } from "./drivers/RemoteProducer.js"; | |
import { USBServoDriver } from "./drivers/USBServoDriver.js"; | |
import { ROBOT_CONFIG } from "./config.js"; | |
import type IUrdfRobot from "@/components/3d/elements/robot/URDF/interfaces/IUrdfRobot.js"; | |
export class Robot implements Positionable { | |
// Core robot data | |
readonly id: string; | |
private unsubscribeFns: (() => void)[] = []; | |
// Command synchronization to prevent state conflicts | |
private commandMutex = $state(false); | |
private pendingCommands: RobotCommand[] = []; | |
// Command deduplication to prevent rapid duplicate commands | |
private lastCommandTime = 0; | |
private lastCommandValues: Record<string, number> = {}; | |
// Memory management | |
private lastCleanup = 0; | |
// Single consumer and multiple producers using Svelte 5 runes - PUBLIC for reactive access | |
consumer = $state<Consumer | null>(null); | |
producers = $state<Producer[]>([]); | |
// Reactive state using Svelte 5 runes - PUBLIC for reactive access | |
joints = $state<Record<string, JointState>>({}); | |
position = $state<Position3D>({ x: 0, y: 0, z: 0 }); | |
isManualControlEnabled = $state(true); | |
connectionStatus = $state<ConnectionStatus>({ isConnected: false }); | |
// URDF robot state for 3D visualization - PUBLIC for reactive access | |
urdfRobotState = $state<IUrdfRobot | null>(null); | |
// Derived reactive values for components | |
jointArray = $derived(Object.values(this.joints)); | |
hasProducers = $derived(this.producers.length > 0); | |
hasConsumer = $derived(this.consumer !== null && this.consumer.status.isConnected); | |
outputDriverCount = $derived(this.producers.filter((d) => d.status.isConnected).length); | |
constructor(id: string, initialJoints: JointState[], urdfRobotState?: IUrdfRobot) { | |
this.id = id; | |
// Store URDF robot state if provided | |
this.urdfRobotState = urdfRobotState || null; | |
// Initialize joints with normalized values | |
initialJoints.forEach((joint) => { | |
const isGripper = | |
joint.name.toLowerCase() === "jaw" || joint.name.toLowerCase() === "gripper"; | |
this.joints[joint.name] = { | |
...joint, | |
value: isGripper ? 0 : 0 // Start at neutral position | |
}; | |
}); | |
} | |
// Method to set URDF robot state after creation (for async loading) | |
setUrdfRobotState(urdfRobotState: any): void { | |
this.urdfRobotState = urdfRobotState; | |
} | |
/** | |
* Update position (implements Positionable interface) | |
*/ | |
updatePosition(newPosition: Position3D): void { | |
this.position = { ...newPosition }; | |
} | |
// Get all USB drivers (both consumer and producers) for calibration | |
getUSBDrivers(): USBServoDriver[] { | |
const usbDrivers: USBServoDriver[] = []; | |
// Check consumer | |
if (this.consumer && USBServoDriver.isUSBDriver(this.consumer)) { | |
usbDrivers.push(this.consumer); | |
} | |
// Check producers | |
this.producers.forEach((producer) => { | |
if (USBServoDriver.isUSBDriver(producer)) { | |
usbDrivers.push(producer); | |
} | |
}); | |
return usbDrivers; | |
} | |
// Get uncalibrated USB drivers that need calibration | |
getUncalibratedUSBDrivers(): USBServoDriver[] { | |
return this.getUSBDrivers().filter((driver) => driver.needsCalibration); | |
} | |
// Check if robot has any USB drivers | |
hasUSBDrivers(): boolean { | |
return this.getUSBDrivers().length > 0; | |
} | |
// Check if all USB drivers are calibrated | |
areAllUSBDriversCalibrated(): boolean { | |
const usbDrivers = this.getUSBDrivers(); | |
return usbDrivers.length > 0 && usbDrivers.every((driver) => driver.isCalibrated); | |
} | |
// Joint value updates (normalized) - for manual control | |
updateJoint(name: string, normalizedValue: number): void { | |
if (!this.isManualControlEnabled) { | |
console.warn("Manual control is disabled"); | |
return; | |
} | |
this.updateJointValue(name, normalizedValue, true); | |
} | |
// Internal joint value update (used by both manual control and USB calibration sync) | |
updateJointValue(name: string, normalizedValue: number, sendToProducers: boolean = false): void { | |
const joint = this.joints[name]; | |
if (!joint) { | |
console.warn(`Joint ${name} not found`); | |
return; | |
} | |
// Clamp to appropriate normalized range based on joint type | |
if (name.toLowerCase() === "jaw" || name.toLowerCase() === "gripper") { | |
normalizedValue = Math.max(0, Math.min(100, normalizedValue)); | |
} else { | |
normalizedValue = Math.max(-100, Math.min(100, normalizedValue)); | |
} | |
console.debug( | |
`[Robot ${this.id}] Update joint ${name} to ${normalizedValue} (normalized, sendToProducers: ${sendToProducers})` | |
); | |
// Create a new joint object to ensure reactivity | |
this.joints[name] = { ...joint, value: normalizedValue }; | |
// Send normalized command to producers if requested | |
if (sendToProducers) { | |
this.sendToProducers({ joints: [{ name, value: normalizedValue }] }); | |
} | |
} | |
executeCommand(command: RobotCommand): void { | |
// Command deduplication - skip if same values sent within dedup window | |
const now = Date.now(); | |
if (now - this.lastCommandTime < ROBOT_CONFIG.commands.dedupWindow) { | |
const hasChanges = command.joints.some( | |
(joint) => Math.abs((this.lastCommandValues[joint.name] || 0) - joint.value) > 0.5 | |
); | |
if (!hasChanges) { | |
console.debug( | |
`[Robot ${this.id}] 🔄 Skipping duplicate command within ${ROBOT_CONFIG.commands.dedupWindow}ms window` | |
); | |
return; | |
} | |
} | |
// Update deduplication tracking | |
this.lastCommandTime = now; | |
command.joints.forEach((joint) => { | |
this.lastCommandValues[joint.name] = joint.value; | |
}); | |
// Queue command if mutex is locked to prevent race conditions | |
if (this.commandMutex) { | |
if (this.pendingCommands.length >= ROBOT_CONFIG.commands.maxQueueSize) { | |
console.warn(`[Robot ${this.id}] Command queue full, dropping oldest command`); | |
this.pendingCommands.shift(); | |
} | |
this.pendingCommands.push(command); | |
return; | |
} | |
this.commandMutex = true; | |
try { | |
console.debug( | |
`[Robot ${this.id}] Executing command with ${command.joints.length} joints:`, | |
command.joints.map((j) => `${j.name}=${j.value}`).join(", ") | |
); | |
// Update virtual robot joints with normalized values | |
command.joints.forEach((jointCmd) => { | |
const joint = this.joints[jointCmd.name]; | |
if (joint) { | |
// Clamp to appropriate normalized range based on joint type | |
let normalizedValue: number; | |
if (jointCmd.name.toLowerCase() === "jaw" || jointCmd.name.toLowerCase() === "gripper") { | |
normalizedValue = Math.max(0, Math.min(100, jointCmd.value)); | |
} else { | |
normalizedValue = Math.max(-100, Math.min(100, jointCmd.value)); | |
} | |
console.debug( | |
`[Robot ${this.id}] Joint ${jointCmd.name}: ${jointCmd.value} -> ${normalizedValue} (normalized)` | |
); | |
// Create a new joint object to ensure reactivity | |
this.joints[jointCmd.name] = { ...joint, value: normalizedValue }; | |
} else { | |
console.warn(`[Robot ${this.id}] Joint ${jointCmd.name} not found`); | |
} | |
}); | |
// Send normalized command to producers | |
this.sendToProducers(command); | |
} finally { | |
this.commandMutex = false; | |
// Periodic cleanup to prevent memory leaks | |
const now = Date.now(); | |
if (now - this.lastCleanup > ROBOT_CONFIG.performance.memoryCleanupInterval) { | |
// Clear old command values that haven't been updated recently | |
Object.keys(this.lastCommandValues).forEach((jointName) => { | |
if (now - this.lastCommandTime > ROBOT_CONFIG.performance.memoryCleanupInterval) { | |
delete this.lastCommandValues[jointName]; | |
} | |
}); | |
this.lastCleanup = now; | |
} | |
// Process any pending commands | |
if (this.pendingCommands.length > 0) { | |
const nextCommand = this.pendingCommands.shift(); | |
if (nextCommand) { | |
// Use setTimeout to prevent stack overflow with rapid commands | |
setTimeout(() => this.executeCommand(nextCommand), 0); | |
} | |
} | |
} | |
} | |
// Consumer management (input driver) - SINGLE consumer only | |
async setConsumer(config: USBDriverConfig | RemoteDriverConfig): Promise<string> { | |
return this._setConsumer(config, false); | |
} | |
// Join existing room as consumer (for Inference Session integration) | |
async joinAsConsumer(config: RemoteDriverConfig): Promise<string> { | |
if (config.type !== "remote") { | |
throw new Error("joinAsConsumer only supports remote drivers"); | |
} | |
return this._setConsumer(config, true); | |
} | |
private async _setConsumer( | |
config: USBDriverConfig | RemoteDriverConfig, | |
joinExistingRoom: boolean | |
): Promise<string> { | |
// Remove existing consumer if any | |
if (this.consumer) { | |
await this.removeConsumer(); | |
} | |
const consumer = this.createConsumer(config); | |
// Set up calibration completion callback for USB drivers | |
if (USBServoDriver.isUSBDriver(consumer)) { | |
const calibrationUnsubscribe = consumer.onCalibrationCompleteWithPositions( | |
async (finalPositions: Record<string, number>) => { | |
console.log(`[Robot ${this.id}] Calibration complete, syncing robot to final positions`); | |
consumer.syncRobotPositions( | |
finalPositions, | |
(jointName: string, normalizedValue: number) => { | |
this.updateJointValue(jointName, normalizedValue, false); // Don't send to producers to avoid feedback loop | |
} | |
); | |
// Start listening now that calibration is complete | |
if ("startListening" in consumer && consumer.startListening) { | |
try { | |
await consumer.startListening(); | |
console.log(`[Robot ${this.id}] Started listening after calibration completion`); | |
} catch (error) { | |
console.error( | |
`[Robot ${this.id}] Failed to start listening after calibration:`, | |
error | |
); | |
} | |
} | |
} | |
); | |
this.unsubscribeFns.push(calibrationUnsubscribe); | |
} | |
// Only pass joinExistingRoom to remote drivers | |
if (config.type === "remote") { | |
await (consumer as RemoteConsumer).connect(joinExistingRoom); | |
} else { | |
await consumer.connect(); | |
} | |
// Set up command listening | |
const commandUnsubscribe = consumer.onCommand((command: RobotCommand) => { | |
this.executeCommand(command); | |
}); | |
this.unsubscribeFns.push(commandUnsubscribe); | |
// Monitor status changes | |
const statusUnsubscribe = consumer.onStatusChange(() => { | |
this.updateStates(); | |
}); | |
this.unsubscribeFns.push(statusUnsubscribe); | |
// Start listening for consumers with this capability (only if calibrated for USB) | |
if ("startListening" in consumer && consumer.startListening) { | |
// For USB consumers, only start listening if calibrated | |
if (USBServoDriver.isUSBDriver(consumer)) { | |
if (consumer.isCalibrated) { | |
await consumer.startListening(); | |
} | |
// If not calibrated, startListening will be called after calibration completion | |
} else { | |
// For non-USB consumers, start listening immediately | |
await consumer.startListening(); | |
} | |
} | |
this.consumer = consumer; | |
this.updateStates(); | |
return consumer.id; | |
} | |
// Producer management (output drivers) - MULTIPLE allowed | |
async addProducer(config: USBDriverConfig | RemoteDriverConfig): Promise<string> { | |
return this._addProducer(config, false); | |
} | |
// Join existing room as producer (for Inference Session integration) | |
async joinAsProducer(config: RemoteDriverConfig): Promise<string> { | |
if (config.type !== "remote") { | |
throw new Error("joinAsProducer only supports remote drivers"); | |
} | |
return this._addProducer(config, true); | |
} | |
private async _addProducer( | |
config: USBDriverConfig | RemoteDriverConfig, | |
joinExistingRoom: boolean | |
): Promise<string> { | |
const producer = this.createProducer(config); | |
// Set up calibration completion callback for USB drivers | |
if (USBServoDriver.isUSBDriver(producer)) { | |
const calibrationUnsubscribe = producer.onCalibrationCompleteWithPositions( | |
async (finalPositions: Record<string, number>) => { | |
console.log(`[Robot ${this.id}] Calibration complete, syncing robot to final positions`); | |
producer.syncRobotPositions( | |
finalPositions, | |
(jointName: string, normalizedValue: number) => { | |
this.updateJointValue(jointName, normalizedValue, false); // Don't send to producers to avoid feedback loop | |
} | |
); | |
console.log( | |
`[Robot ${this.id}] USB Producer calibration completed and ready for commands` | |
); | |
} | |
); | |
this.unsubscribeFns.push(calibrationUnsubscribe); | |
} | |
// Only pass joinExistingRoom to remote drivers | |
if (config.type === "remote") { | |
await (producer as RemoteProducer).connect(joinExistingRoom); | |
} else { | |
await producer.connect(); | |
} | |
// Monitor status changes | |
const statusUnsubscribe = producer.onStatusChange(() => { | |
this.updateStates(); | |
}); | |
this.unsubscribeFns.push(statusUnsubscribe); | |
this.producers.push(producer); | |
this.updateStates(); | |
return producer.id; | |
} | |
async removeConsumer(): Promise<void> { | |
if (this.consumer) { | |
// Stop listening for consumers with this capability | |
if ("stopListening" in this.consumer && this.consumer.stopListening) { | |
await this.consumer.stopListening(); | |
} | |
await this.consumer.disconnect(); | |
this.consumer = null; | |
this.updateStates(); | |
} | |
} | |
async removeProducer(driverId: string): Promise<void> { | |
const driverIndex = this.producers.findIndex((d) => d.id === driverId); | |
if (driverIndex >= 0) { | |
const driver = this.producers[driverIndex]; | |
await driver.disconnect(); | |
this.producers.splice(driverIndex, 1); | |
this.updateStates(); | |
} | |
} | |
// Private methods | |
private createConsumer(config: USBDriverConfig | RemoteDriverConfig): Consumer { | |
switch (config.type) { | |
case "usb": | |
return new USBConsumer(config); | |
case "remote": | |
return new RemoteConsumer(config); | |
default: | |
const _exhaustive: never = config; | |
throw new Error(`Unknown consumer type: ${JSON.stringify(_exhaustive)}`); | |
} | |
} | |
private createProducer(config: USBDriverConfig | RemoteDriverConfig): Producer { | |
switch (config.type) { | |
case "usb": | |
return new USBProducer(config); | |
case "remote": | |
return new RemoteProducer(config); | |
default: | |
const _exhaustive: never = config; | |
throw new Error(`Unknown producer type: ${JSON.stringify(_exhaustive)}`); | |
} | |
} | |
// Convert normalized values to URDF radians for 3D visualization | |
convertNormalizedToUrdfRadians(jointName: string, normalizedValue: number): number { | |
const joint = this.joints[jointName]; | |
if (!joint?.limits || joint.limits.lower === undefined || joint.limits.upper === undefined) { | |
// Default ranges | |
if (jointName.toLowerCase() === "jaw" || jointName.toLowerCase() === "gripper") { | |
return (normalizedValue / 100) * Math.PI; | |
} else { | |
return (normalizedValue / 100) * Math.PI; | |
} | |
} | |
const { lower, upper } = joint.limits; | |
// Map normalized value to URDF range | |
let normalizedRatio: number; | |
if (jointName.toLowerCase() === "jaw" || jointName.toLowerCase() === "gripper") { | |
normalizedRatio = normalizedValue / 100; // 0-100 -> 0-1 | |
} else { | |
normalizedRatio = (normalizedValue + 100) / 200; // -100-+100 -> 0-1 | |
} | |
const urdfRadians = lower + normalizedRatio * (upper - lower); | |
console.debug( | |
`[Robot ${this.id}] Joint ${jointName}: ${normalizedValue} (norm) -> ${urdfRadians.toFixed(3)} (rad)` | |
); | |
return urdfRadians; | |
} | |
private async sendToProducers(command: RobotCommand): Promise<void> { | |
const connectedProducers = this.producers.filter((d) => d.status.isConnected); | |
console.debug( | |
`[Robot ${this.id}] Sending command to ${connectedProducers.length} producers:`, | |
command | |
); | |
// Send to all connected producers | |
await Promise.all( | |
connectedProducers.map(async (producer) => { | |
try { | |
await producer.sendCommand(command); | |
} catch (error) { | |
console.error( | |
`[Robot ${this.id}] Failed to send command to producer ${producer.id}:`, | |
error | |
); | |
} | |
}) | |
); | |
} | |
private updateStates(): void { | |
// Update connection status | |
const hasConnectedDrivers = | |
this.consumer?.status.isConnected || this.producers.some((d) => d.status.isConnected); | |
this.connectionStatus = { | |
isConnected: hasConnectedDrivers, | |
lastConnected: hasConnectedDrivers ? new Date() : this.connectionStatus.lastConnected | |
}; | |
// Manual control is enabled when no connected consumer | |
this.isManualControlEnabled = !this.consumer?.status.isConnected; | |
} | |
// Cleanup | |
async destroy(): Promise<void> { | |
// Unsubscribe from all callbacks | |
this.unsubscribeFns.forEach((fn) => fn()); | |
this.unsubscribeFns = []; | |
// Disconnect all drivers | |
const allDrivers = [this.consumer, ...this.producers].filter(Boolean) as ( | |
| Consumer | |
| Producer | |
)[]; | |
await Promise.allSettled( | |
allDrivers.map(async (driver) => { | |
try { | |
await driver.disconnect(); | |
} catch (error) { | |
console.error(`Error disconnecting driver ${driver.id}:`, error); | |
} | |
}) | |
); | |
// Calibration cleanup is handled by individual USB drivers | |
} | |
} | |