Spaces:
Running
Running
feat: teleoperate in the web
Browse files- src/demo/components/PortManager.tsx +118 -116
- src/demo/components/TeleoperationPanel.tsx +367 -0
- src/demo/hooks/useRobotConnection.ts +91 -0
- src/demo/hooks/useTeleoperation.ts +486 -0
- src/demo/pages/Home.tsx +17 -0
- src/lerobot/web/motor-utils.ts +266 -0
- src/lerobot/web/robot-connection.ts +228 -0
- src/lerobot/web/teleoperate.ts +525 -0
src/demo/components/PortManager.tsx
CHANGED
|
@@ -28,12 +28,14 @@ interface PortManagerProps {
|
|
| 28 |
robotType: "so100_follower" | "so100_leader",
|
| 29 |
robotId: string
|
| 30 |
) => void;
|
|
|
|
| 31 |
}
|
| 32 |
|
| 33 |
export function PortManager({
|
| 34 |
connectedRobots,
|
| 35 |
onConnectedRobotsChange,
|
| 36 |
onCalibrate,
|
|
|
|
| 37 |
}: PortManagerProps) {
|
| 38 |
const [isConnecting, setIsConnecting] = useState(false);
|
| 39 |
const [isFindingPorts, setIsFindingPorts] = useState(false);
|
|
@@ -55,68 +57,18 @@ export function PortManager({
|
|
| 55 |
loadSavedPorts();
|
| 56 |
}, []);
|
| 57 |
|
| 58 |
-
//
|
| 59 |
-
useEffect(() => {
|
| 60 |
-
savePortsToStorage();
|
| 61 |
-
}, [connectedRobots]);
|
| 62 |
|
| 63 |
const loadSavedPorts = async () => {
|
| 64 |
try {
|
| 65 |
-
const saved = localStorage.getItem("lerobot-ports");
|
| 66 |
-
if (!saved) return;
|
| 67 |
-
|
| 68 |
-
const savedData = JSON.parse(saved);
|
| 69 |
const existingPorts = await navigator.serial.getPorts();
|
| 70 |
-
|
| 71 |
const restoredPorts: ConnectedRobot[] = [];
|
| 72 |
|
| 73 |
for (const port of existingPorts) {
|
| 74 |
-
//
|
| 75 |
-
const portInfo = port.getInfo();
|
| 76 |
-
const savedPort = savedData.find((p: any) => {
|
| 77 |
-
// Try to match by USB vendor/product ID if available
|
| 78 |
-
if (portInfo.usbVendorId && portInfo.usbProductId) {
|
| 79 |
-
return (
|
| 80 |
-
p.usbVendorId === portInfo.usbVendorId &&
|
| 81 |
-
p.usbProductId === portInfo.usbProductId
|
| 82 |
-
);
|
| 83 |
-
}
|
| 84 |
-
// Fallback to name matching
|
| 85 |
-
return p.name === getPortDisplayName(port);
|
| 86 |
-
});
|
| 87 |
-
|
| 88 |
-
// Auto-connect to paired robots
|
| 89 |
-
let isConnected = false;
|
| 90 |
-
try {
|
| 91 |
-
// Check if already open
|
| 92 |
-
if (port.readable !== null && port.writable !== null) {
|
| 93 |
-
isConnected = true;
|
| 94 |
-
console.log("Port already open, reusing connection");
|
| 95 |
-
} else {
|
| 96 |
-
// Auto-open paired robots only if they have saved configuration
|
| 97 |
-
if (savedPort?.robotType && savedPort?.robotId) {
|
| 98 |
-
console.log(
|
| 99 |
-
`Auto-connecting to saved robot: ${savedPort.robotType} (${savedPort.robotId})`
|
| 100 |
-
);
|
| 101 |
-
await port.open({ baudRate: 1000000 });
|
| 102 |
-
isConnected = true;
|
| 103 |
-
} else {
|
| 104 |
-
console.log(
|
| 105 |
-
"Port found but no saved robot configuration, skipping auto-connect"
|
| 106 |
-
);
|
| 107 |
-
isConnected = false;
|
| 108 |
-
}
|
| 109 |
-
}
|
| 110 |
-
} catch (error) {
|
| 111 |
-
console.log("Could not auto-connect to paired robot:", error);
|
| 112 |
-
isConnected = false;
|
| 113 |
-
}
|
| 114 |
-
|
| 115 |
-
// Re-detect serial number for this port
|
| 116 |
let serialNumber = null;
|
| 117 |
let usbMetadata = null;
|
| 118 |
|
| 119 |
-
// Try to get USB device info to restore serial number
|
| 120 |
try {
|
| 121 |
// Get all USB devices and try to match with this serial port
|
| 122 |
const usbDevices = await navigator.usb.getDevices();
|
|
@@ -161,12 +113,80 @@ export function PortManager({
|
|
| 161 |
.substr(2, 9)}`;
|
| 162 |
}
|
| 163 |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 164 |
restoredPorts.push({
|
| 165 |
port,
|
| 166 |
name: getPortDisplayName(port),
|
| 167 |
isConnected,
|
| 168 |
-
robotType
|
| 169 |
-
robotId
|
| 170 |
serialNumber: serialNumber!,
|
| 171 |
usbMetadata: usbMetadata || undefined,
|
| 172 |
});
|
|
@@ -178,24 +198,6 @@ export function PortManager({
|
|
| 178 |
}
|
| 179 |
};
|
| 180 |
|
| 181 |
-
const savePortsToStorage = () => {
|
| 182 |
-
try {
|
| 183 |
-
const dataToSave = connectedRobots.map((p) => {
|
| 184 |
-
const portInfo = p.port.getInfo();
|
| 185 |
-
return {
|
| 186 |
-
name: p.name,
|
| 187 |
-
robotType: p.robotType,
|
| 188 |
-
robotId: p.robotId,
|
| 189 |
-
usbVendorId: portInfo.usbVendorId,
|
| 190 |
-
usbProductId: portInfo.usbProductId,
|
| 191 |
-
};
|
| 192 |
-
});
|
| 193 |
-
localStorage.setItem("lerobot-ports", JSON.stringify(dataToSave));
|
| 194 |
-
} catch (error) {
|
| 195 |
-
console.error("Failed to save ports to storage:", error);
|
| 196 |
-
}
|
| 197 |
-
};
|
| 198 |
-
|
| 199 |
const getPortDisplayName = (port: SerialPort): string => {
|
| 200 |
try {
|
| 201 |
const info = port.getInfo();
|
|
@@ -322,6 +324,27 @@ export function PortManager({
|
|
| 322 |
|
| 323 |
onConnectedRobotsChange([...connectedRobots, newRobot]);
|
| 324 |
console.log("🤖 New robot connected with ID:", serialNumber);
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 325 |
} else {
|
| 326 |
// Existing robot - update port and connection status
|
| 327 |
const updatedRobots = connectedRobots.map((robot, index) =>
|
|
@@ -391,44 +414,8 @@ export function PortManager({
|
|
| 391 |
// Remove unified storage data
|
| 392 |
localStorage.removeItem(unifiedKey);
|
| 393 |
console.log(`🗑️ Deleted unified robot data: ${unifiedKey}`);
|
| 394 |
-
|
| 395 |
-
// Also clean up any old format keys for this robot (if they exist)
|
| 396 |
-
const oldKeys = [
|
| 397 |
-
`lerobot-robot-${portInfo.serialNumber}`,
|
| 398 |
-
`lerobot-calibration-${portInfo.serialNumber}`,
|
| 399 |
-
];
|
| 400 |
-
|
| 401 |
-
// Try to find old calibration key by checking stored robot config
|
| 402 |
-
if (portInfo.robotType && portInfo.robotId) {
|
| 403 |
-
oldKeys.push(
|
| 404 |
-
`lerobot_calibration_${portInfo.robotType}_${portInfo.robotId}`
|
| 405 |
-
);
|
| 406 |
-
}
|
| 407 |
-
|
| 408 |
-
oldKeys.forEach((key) => {
|
| 409 |
-
if (localStorage.getItem(key)) {
|
| 410 |
-
localStorage.removeItem(key);
|
| 411 |
-
console.log(`🧹 Cleaned up old key: ${key}`);
|
| 412 |
-
}
|
| 413 |
-
});
|
| 414 |
} catch (error) {
|
| 415 |
console.warn("Failed to delete unified storage data:", error);
|
| 416 |
-
|
| 417 |
-
// Fallback: try to delete old format keys directly
|
| 418 |
-
if (portInfo.robotType && portInfo.robotId) {
|
| 419 |
-
const oldKeys = [
|
| 420 |
-
`lerobot-robot-${portInfo.serialNumber}`,
|
| 421 |
-
`lerobot-calibration-${portInfo.serialNumber}`,
|
| 422 |
-
`lerobot_calibration_${portInfo.robotType}_${portInfo.robotId}`,
|
| 423 |
-
];
|
| 424 |
-
|
| 425 |
-
oldKeys.forEach((key) => {
|
| 426 |
-
if (localStorage.getItem(key)) {
|
| 427 |
-
localStorage.removeItem(key);
|
| 428 |
-
console.log(`🧹 Removed old format key: ${key}`);
|
| 429 |
-
}
|
| 430 |
-
});
|
| 431 |
-
}
|
| 432 |
}
|
| 433 |
}
|
| 434 |
|
|
@@ -683,6 +670,7 @@ export function PortManager({
|
|
| 683 |
handleUpdatePortInfo(index, robotType, robotId)
|
| 684 |
}
|
| 685 |
onCalibrate={() => handleCalibrate(portInfo)}
|
|
|
|
| 686 |
/>
|
| 687 |
))}
|
| 688 |
</div>
|
|
@@ -757,6 +745,7 @@ interface PortCardProps {
|
|
| 757 |
robotId: string
|
| 758 |
) => void;
|
| 759 |
onCalibrate: () => void;
|
|
|
|
| 760 |
}
|
| 761 |
|
| 762 |
function PortCard({
|
|
@@ -764,6 +753,7 @@ function PortCard({
|
|
| 764 |
onDisconnect,
|
| 765 |
onUpdateInfo,
|
| 766 |
onCalibrate,
|
|
|
|
| 767 |
}: PortCardProps) {
|
| 768 |
const [robotType, setRobotType] = useState<"so100_follower" | "so100_leader">(
|
| 769 |
portInfo.robotType || "so100_follower"
|
|
@@ -1167,14 +1157,26 @@ function PortCard({
|
|
| 1167 |
<span>Not calibrated yet</span>
|
| 1168 |
)}
|
| 1169 |
</div>
|
| 1170 |
-
<
|
| 1171 |
-
|
| 1172 |
-
|
| 1173 |
-
|
| 1174 |
-
|
| 1175 |
-
|
| 1176 |
-
|
| 1177 |
-
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1178 |
</div>
|
| 1179 |
|
| 1180 |
{/* Device Info Scanner */}
|
|
|
|
| 28 |
robotType: "so100_follower" | "so100_leader",
|
| 29 |
robotId: string
|
| 30 |
) => void;
|
| 31 |
+
onTeleoperate?: (robot: ConnectedRobot) => void;
|
| 32 |
}
|
| 33 |
|
| 34 |
export function PortManager({
|
| 35 |
connectedRobots,
|
| 36 |
onConnectedRobotsChange,
|
| 37 |
onCalibrate,
|
| 38 |
+
onTeleoperate,
|
| 39 |
}: PortManagerProps) {
|
| 40 |
const [isConnecting, setIsConnecting] = useState(false);
|
| 41 |
const [isFindingPorts, setIsFindingPorts] = useState(false);
|
|
|
|
| 57 |
loadSavedPorts();
|
| 58 |
}, []);
|
| 59 |
|
| 60 |
+
// Note: Robot data is now automatically saved to unified storage when robot config is updated
|
|
|
|
|
|
|
|
|
|
| 61 |
|
| 62 |
const loadSavedPorts = async () => {
|
| 63 |
try {
|
|
|
|
|
|
|
|
|
|
|
|
|
| 64 |
const existingPorts = await navigator.serial.getPorts();
|
|
|
|
| 65 |
const restoredPorts: ConnectedRobot[] = [];
|
| 66 |
|
| 67 |
for (const port of existingPorts) {
|
| 68 |
+
// Get USB device metadata to determine serial number
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 69 |
let serialNumber = null;
|
| 70 |
let usbMetadata = null;
|
| 71 |
|
|
|
|
| 72 |
try {
|
| 73 |
// Get all USB devices and try to match with this serial port
|
| 74 |
const usbDevices = await navigator.usb.getDevices();
|
|
|
|
| 113 |
.substr(2, 9)}`;
|
| 114 |
}
|
| 115 |
|
| 116 |
+
// Load robot configuration from unified storage
|
| 117 |
+
let robotType: "so100_follower" | "so100_leader" | undefined;
|
| 118 |
+
let robotId: string | undefined;
|
| 119 |
+
let shouldAutoConnect = false;
|
| 120 |
+
|
| 121 |
+
if (serialNumber) {
|
| 122 |
+
try {
|
| 123 |
+
const { getUnifiedRobotData } = await import(
|
| 124 |
+
"../lib/unified-storage"
|
| 125 |
+
);
|
| 126 |
+
const unifiedData = getUnifiedRobotData(serialNumber);
|
| 127 |
+
if (unifiedData?.device_info) {
|
| 128 |
+
robotType = unifiedData.device_info.robotType;
|
| 129 |
+
robotId = unifiedData.device_info.robotId;
|
| 130 |
+
shouldAutoConnect = true;
|
| 131 |
+
console.log(
|
| 132 |
+
`📋 Loaded robot config from unified storage: ${robotType} (${robotId})`
|
| 133 |
+
);
|
| 134 |
+
}
|
| 135 |
+
} catch (error) {
|
| 136 |
+
console.warn("Failed to load unified robot data:", error);
|
| 137 |
+
}
|
| 138 |
+
}
|
| 139 |
+
|
| 140 |
+
// Auto-connect to configured robots
|
| 141 |
+
let isConnected = false;
|
| 142 |
+
try {
|
| 143 |
+
// Check if already open
|
| 144 |
+
if (port.readable !== null && port.writable !== null) {
|
| 145 |
+
isConnected = true;
|
| 146 |
+
console.log("Port already open, reusing connection");
|
| 147 |
+
} else if (shouldAutoConnect && robotType && robotId) {
|
| 148 |
+
// Auto-open robots that have saved configuration
|
| 149 |
+
console.log(
|
| 150 |
+
`Auto-connecting to saved robot: ${robotType} (${robotId})`
|
| 151 |
+
);
|
| 152 |
+
await port.open({ baudRate: 1000000 });
|
| 153 |
+
isConnected = true;
|
| 154 |
+
|
| 155 |
+
// Register with singleton connection manager
|
| 156 |
+
try {
|
| 157 |
+
const { getRobotConnectionManager } = await import(
|
| 158 |
+
"../../lerobot/web/robot-connection"
|
| 159 |
+
);
|
| 160 |
+
const connectionManager = getRobotConnectionManager();
|
| 161 |
+
await connectionManager.connect(
|
| 162 |
+
port,
|
| 163 |
+
robotType,
|
| 164 |
+
robotId,
|
| 165 |
+
serialNumber!
|
| 166 |
+
);
|
| 167 |
+
} catch (error) {
|
| 168 |
+
console.warn(
|
| 169 |
+
"Failed to register with connection manager:",
|
| 170 |
+
error
|
| 171 |
+
);
|
| 172 |
+
}
|
| 173 |
+
} else {
|
| 174 |
+
console.log(
|
| 175 |
+
"Port found but no saved robot configuration, skipping auto-connect"
|
| 176 |
+
);
|
| 177 |
+
isConnected = false;
|
| 178 |
+
}
|
| 179 |
+
} catch (error) {
|
| 180 |
+
console.log("Could not auto-connect to robot:", error);
|
| 181 |
+
isConnected = false;
|
| 182 |
+
}
|
| 183 |
+
|
| 184 |
restoredPorts.push({
|
| 185 |
port,
|
| 186 |
name: getPortDisplayName(port),
|
| 187 |
isConnected,
|
| 188 |
+
robotType,
|
| 189 |
+
robotId,
|
| 190 |
serialNumber: serialNumber!,
|
| 191 |
usbMetadata: usbMetadata || undefined,
|
| 192 |
});
|
|
|
|
| 198 |
}
|
| 199 |
};
|
| 200 |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 201 |
const getPortDisplayName = (port: SerialPort): string => {
|
| 202 |
try {
|
| 203 |
const info = port.getInfo();
|
|
|
|
| 324 |
|
| 325 |
onConnectedRobotsChange([...connectedRobots, newRobot]);
|
| 326 |
console.log("🤖 New robot connected with ID:", serialNumber);
|
| 327 |
+
|
| 328 |
+
// Register with singleton connection manager if robot is configured
|
| 329 |
+
if (newRobot.robotType && newRobot.robotId) {
|
| 330 |
+
try {
|
| 331 |
+
const { getRobotConnectionManager } = await import(
|
| 332 |
+
"../../lerobot/web/robot-connection"
|
| 333 |
+
);
|
| 334 |
+
const connectionManager = getRobotConnectionManager();
|
| 335 |
+
await connectionManager.connect(
|
| 336 |
+
port,
|
| 337 |
+
newRobot.robotType,
|
| 338 |
+
newRobot.robotId,
|
| 339 |
+
serialNumber!
|
| 340 |
+
);
|
| 341 |
+
} catch (error) {
|
| 342 |
+
console.warn(
|
| 343 |
+
"Failed to register new connection with manager:",
|
| 344 |
+
error
|
| 345 |
+
);
|
| 346 |
+
}
|
| 347 |
+
}
|
| 348 |
} else {
|
| 349 |
// Existing robot - update port and connection status
|
| 350 |
const updatedRobots = connectedRobots.map((robot, index) =>
|
|
|
|
| 414 |
// Remove unified storage data
|
| 415 |
localStorage.removeItem(unifiedKey);
|
| 416 |
console.log(`🗑️ Deleted unified robot data: ${unifiedKey}`);
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 417 |
} catch (error) {
|
| 418 |
console.warn("Failed to delete unified storage data:", error);
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 419 |
}
|
| 420 |
}
|
| 421 |
|
|
|
|
| 670 |
handleUpdatePortInfo(index, robotType, robotId)
|
| 671 |
}
|
| 672 |
onCalibrate={() => handleCalibrate(portInfo)}
|
| 673 |
+
onTeleoperate={() => onTeleoperate?.(portInfo)}
|
| 674 |
/>
|
| 675 |
))}
|
| 676 |
</div>
|
|
|
|
| 745 |
robotId: string
|
| 746 |
) => void;
|
| 747 |
onCalibrate: () => void;
|
| 748 |
+
onTeleoperate: () => void;
|
| 749 |
}
|
| 750 |
|
| 751 |
function PortCard({
|
|
|
|
| 753 |
onDisconnect,
|
| 754 |
onUpdateInfo,
|
| 755 |
onCalibrate,
|
| 756 |
+
onTeleoperate,
|
| 757 |
}: PortCardProps) {
|
| 758 |
const [robotType, setRobotType] = useState<"so100_follower" | "so100_leader">(
|
| 759 |
portInfo.robotType || "so100_follower"
|
|
|
|
| 1157 |
<span>Not calibrated yet</span>
|
| 1158 |
)}
|
| 1159 |
</div>
|
| 1160 |
+
<div className="flex gap-2">
|
| 1161 |
+
<Button
|
| 1162 |
+
size="sm"
|
| 1163 |
+
variant={calibrationStatus ? "outline" : "default"}
|
| 1164 |
+
onClick={onCalibrate}
|
| 1165 |
+
disabled={!currentRobotType || !currentRobotId}
|
| 1166 |
+
>
|
| 1167 |
+
{calibrationStatus ? "Re-calibrate" : "Calibrate"}
|
| 1168 |
+
</Button>
|
| 1169 |
+
<Button
|
| 1170 |
+
size="sm"
|
| 1171 |
+
variant="outline"
|
| 1172 |
+
onClick={onTeleoperate}
|
| 1173 |
+
disabled={
|
| 1174 |
+
!currentRobotType || !currentRobotId || !portInfo.isConnected
|
| 1175 |
+
}
|
| 1176 |
+
>
|
| 1177 |
+
🎮 Teleoperate
|
| 1178 |
+
</Button>
|
| 1179 |
+
</div>
|
| 1180 |
</div>
|
| 1181 |
|
| 1182 |
{/* Device Info Scanner */}
|
src/demo/components/TeleoperationPanel.tsx
ADDED
|
@@ -0,0 +1,367 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
import React, { useState } from "react";
|
| 2 |
+
import { Button } from "./ui/button";
|
| 3 |
+
import { Card, CardContent, CardHeader, CardTitle } from "./ui/card";
|
| 4 |
+
import { Badge } from "./ui/badge";
|
| 5 |
+
import { Alert, AlertDescription } from "./ui/alert";
|
| 6 |
+
import { Progress } from "./ui/progress";
|
| 7 |
+
import { useTeleoperation } from "../hooks/useTeleoperation";
|
| 8 |
+
import type { ConnectedRobot } from "../types";
|
| 9 |
+
import { KEYBOARD_CONTROLS } from "../../lerobot/web/teleoperate";
|
| 10 |
+
|
| 11 |
+
interface TeleoperationPanelProps {
|
| 12 |
+
robot: ConnectedRobot;
|
| 13 |
+
onClose: () => void;
|
| 14 |
+
}
|
| 15 |
+
|
| 16 |
+
export function TeleoperationPanel({
|
| 17 |
+
robot,
|
| 18 |
+
onClose,
|
| 19 |
+
}: TeleoperationPanelProps) {
|
| 20 |
+
const [enabled, setEnabled] = useState(false);
|
| 21 |
+
|
| 22 |
+
const {
|
| 23 |
+
isConnected,
|
| 24 |
+
isActive,
|
| 25 |
+
motorConfigs,
|
| 26 |
+
keyStates,
|
| 27 |
+
error,
|
| 28 |
+
start,
|
| 29 |
+
stop,
|
| 30 |
+
goToHome,
|
| 31 |
+
simulateKeyPress,
|
| 32 |
+
simulateKeyRelease,
|
| 33 |
+
} = useTeleoperation({
|
| 34 |
+
robot,
|
| 35 |
+
enabled,
|
| 36 |
+
onError: (err: string) => console.error("Teleoperation error:", err),
|
| 37 |
+
});
|
| 38 |
+
|
| 39 |
+
const handleStart = async () => {
|
| 40 |
+
setEnabled(true);
|
| 41 |
+
await start();
|
| 42 |
+
};
|
| 43 |
+
|
| 44 |
+
const handleStop = () => {
|
| 45 |
+
stop();
|
| 46 |
+
setEnabled(false);
|
| 47 |
+
};
|
| 48 |
+
|
| 49 |
+
const handleClose = () => {
|
| 50 |
+
stop();
|
| 51 |
+
setEnabled(false);
|
| 52 |
+
onClose();
|
| 53 |
+
};
|
| 54 |
+
|
| 55 |
+
// Virtual keyboard component
|
| 56 |
+
const VirtualKeyboard = () => {
|
| 57 |
+
const isKeyPressed = (key: string) => {
|
| 58 |
+
return keyStates[key]?.pressed || false;
|
| 59 |
+
};
|
| 60 |
+
|
| 61 |
+
const KeyButton = ({
|
| 62 |
+
keyCode,
|
| 63 |
+
children,
|
| 64 |
+
className = "",
|
| 65 |
+
size = "default" as "default" | "sm" | "lg" | "icon",
|
| 66 |
+
}: {
|
| 67 |
+
keyCode: string;
|
| 68 |
+
children: React.ReactNode;
|
| 69 |
+
className?: string;
|
| 70 |
+
size?: "default" | "sm" | "lg" | "icon";
|
| 71 |
+
}) => {
|
| 72 |
+
const control =
|
| 73 |
+
KEYBOARD_CONTROLS[keyCode as keyof typeof KEYBOARD_CONTROLS];
|
| 74 |
+
const pressed = isKeyPressed(keyCode);
|
| 75 |
+
|
| 76 |
+
return (
|
| 77 |
+
<Button
|
| 78 |
+
variant={pressed ? "default" : "outline"}
|
| 79 |
+
size={size}
|
| 80 |
+
className={`
|
| 81 |
+
${className}
|
| 82 |
+
${
|
| 83 |
+
pressed
|
| 84 |
+
? "bg-blue-600 text-white shadow-inner"
|
| 85 |
+
: "hover:bg-gray-100"
|
| 86 |
+
}
|
| 87 |
+
transition-all duration-75 font-mono text-xs
|
| 88 |
+
${!isActive ? "opacity-50 cursor-not-allowed" : ""}
|
| 89 |
+
`}
|
| 90 |
+
disabled={!isActive}
|
| 91 |
+
onMouseDown={(e) => {
|
| 92 |
+
e.preventDefault();
|
| 93 |
+
if (isActive) simulateKeyPress(keyCode);
|
| 94 |
+
}}
|
| 95 |
+
onMouseUp={(e) => {
|
| 96 |
+
e.preventDefault();
|
| 97 |
+
if (isActive) simulateKeyRelease(keyCode);
|
| 98 |
+
}}
|
| 99 |
+
onMouseLeave={(e) => {
|
| 100 |
+
e.preventDefault();
|
| 101 |
+
if (isActive) simulateKeyRelease(keyCode);
|
| 102 |
+
}}
|
| 103 |
+
title={control?.description || keyCode}
|
| 104 |
+
>
|
| 105 |
+
{children}
|
| 106 |
+
</Button>
|
| 107 |
+
);
|
| 108 |
+
};
|
| 109 |
+
|
| 110 |
+
return (
|
| 111 |
+
<div className="space-y-4">
|
| 112 |
+
{/* Arrow Keys */}
|
| 113 |
+
<div className="text-center">
|
| 114 |
+
<h4 className="text-xs font-semibold mb-2 text-gray-600">Shoulder</h4>
|
| 115 |
+
<div className="flex flex-col items-center gap-1">
|
| 116 |
+
<KeyButton keyCode="ArrowUp" size="sm">
|
| 117 |
+
↑
|
| 118 |
+
</KeyButton>
|
| 119 |
+
<div className="flex gap-1">
|
| 120 |
+
<KeyButton keyCode="ArrowLeft" size="sm">
|
| 121 |
+
←
|
| 122 |
+
</KeyButton>
|
| 123 |
+
<KeyButton keyCode="ArrowDown" size="sm">
|
| 124 |
+
↓
|
| 125 |
+
</KeyButton>
|
| 126 |
+
<KeyButton keyCode="ArrowRight" size="sm">
|
| 127 |
+
→
|
| 128 |
+
</KeyButton>
|
| 129 |
+
</div>
|
| 130 |
+
</div>
|
| 131 |
+
</div>
|
| 132 |
+
|
| 133 |
+
{/* WASD Keys */}
|
| 134 |
+
<div className="text-center">
|
| 135 |
+
<h4 className="text-xs font-semibold mb-2 text-gray-600">
|
| 136 |
+
Elbow/Wrist
|
| 137 |
+
</h4>
|
| 138 |
+
<div className="flex flex-col items-center gap-1">
|
| 139 |
+
<KeyButton keyCode="w" size="sm">
|
| 140 |
+
W
|
| 141 |
+
</KeyButton>
|
| 142 |
+
<div className="flex gap-1">
|
| 143 |
+
<KeyButton keyCode="a" size="sm">
|
| 144 |
+
A
|
| 145 |
+
</KeyButton>
|
| 146 |
+
<KeyButton keyCode="s" size="sm">
|
| 147 |
+
S
|
| 148 |
+
</KeyButton>
|
| 149 |
+
<KeyButton keyCode="d" size="sm">
|
| 150 |
+
D
|
| 151 |
+
</KeyButton>
|
| 152 |
+
</div>
|
| 153 |
+
</div>
|
| 154 |
+
</div>
|
| 155 |
+
|
| 156 |
+
{/* Q/E and Space */}
|
| 157 |
+
<div className="flex justify-center gap-2">
|
| 158 |
+
<div className="text-center">
|
| 159 |
+
<h4 className="text-xs font-semibold mb-2 text-gray-600">Roll</h4>
|
| 160 |
+
<div className="flex gap-1">
|
| 161 |
+
<KeyButton keyCode="q" size="sm">
|
| 162 |
+
Q
|
| 163 |
+
</KeyButton>
|
| 164 |
+
<KeyButton keyCode="e" size="sm">
|
| 165 |
+
E
|
| 166 |
+
</KeyButton>
|
| 167 |
+
</div>
|
| 168 |
+
</div>
|
| 169 |
+
<div className="text-center">
|
| 170 |
+
<h4 className="text-xs font-semibold mb-2 text-gray-600">
|
| 171 |
+
Gripper
|
| 172 |
+
</h4>
|
| 173 |
+
<KeyButton keyCode=" " size="sm" className="min-w-16">
|
| 174 |
+
⎵
|
| 175 |
+
</KeyButton>
|
| 176 |
+
</div>
|
| 177 |
+
</div>
|
| 178 |
+
|
| 179 |
+
{/* Emergency Stop */}
|
| 180 |
+
<div className="text-center border-t pt-2">
|
| 181 |
+
<KeyButton
|
| 182 |
+
keyCode="Escape"
|
| 183 |
+
className="bg-red-100 border-red-300 hover:bg-red-200 text-red-800 text-xs"
|
| 184 |
+
>
|
| 185 |
+
ESC
|
| 186 |
+
</KeyButton>
|
| 187 |
+
</div>
|
| 188 |
+
</div>
|
| 189 |
+
);
|
| 190 |
+
};
|
| 191 |
+
|
| 192 |
+
return (
|
| 193 |
+
<div className="min-h-screen bg-gradient-to-br from-blue-50 to-indigo-100">
|
| 194 |
+
<div className="container mx-auto px-6 py-8">
|
| 195 |
+
{/* Header */}
|
| 196 |
+
<div className="flex justify-between items-center mb-6">
|
| 197 |
+
<div>
|
| 198 |
+
<h1 className="text-3xl font-bold text-gray-900">
|
| 199 |
+
🎮 Robot Teleoperation
|
| 200 |
+
</h1>
|
| 201 |
+
<p className="text-gray-600">
|
| 202 |
+
{robot.robotId || robot.name} - {robot.serialNumber}
|
| 203 |
+
</p>
|
| 204 |
+
</div>
|
| 205 |
+
<Button variant="outline" onClick={handleClose}>
|
| 206 |
+
← Back to Dashboard
|
| 207 |
+
</Button>
|
| 208 |
+
</div>
|
| 209 |
+
|
| 210 |
+
{/* Error Alert */}
|
| 211 |
+
{error && (
|
| 212 |
+
<Alert variant="destructive" className="mb-6">
|
| 213 |
+
<AlertDescription>{error}</AlertDescription>
|
| 214 |
+
</Alert>
|
| 215 |
+
)}
|
| 216 |
+
|
| 217 |
+
<div className="grid grid-cols-1 lg:grid-cols-3 gap-6">
|
| 218 |
+
{/* Status Panel */}
|
| 219 |
+
<Card>
|
| 220 |
+
<CardHeader>
|
| 221 |
+
<CardTitle className="flex items-center gap-2">
|
| 222 |
+
Status
|
| 223 |
+
<Badge variant={isConnected ? "default" : "destructive"}>
|
| 224 |
+
{isConnected ? "Connected" : "Disconnected"}
|
| 225 |
+
</Badge>
|
| 226 |
+
</CardTitle>
|
| 227 |
+
</CardHeader>
|
| 228 |
+
<CardContent className="space-y-4">
|
| 229 |
+
<div className="flex items-center justify-between">
|
| 230 |
+
<span className="text-sm text-gray-600">Teleoperation</span>
|
| 231 |
+
<Badge variant={isActive ? "default" : "secondary"}>
|
| 232 |
+
{isActive ? "Active" : "Stopped"}
|
| 233 |
+
</Badge>
|
| 234 |
+
</div>
|
| 235 |
+
|
| 236 |
+
<div className="flex items-center justify-between">
|
| 237 |
+
<span className="text-sm text-gray-600">Active Keys</span>
|
| 238 |
+
<Badge variant="outline">
|
| 239 |
+
{
|
| 240 |
+
Object.values(keyStates).filter((state) => state.pressed)
|
| 241 |
+
.length
|
| 242 |
+
}
|
| 243 |
+
</Badge>
|
| 244 |
+
</div>
|
| 245 |
+
|
| 246 |
+
<div className="space-y-2">
|
| 247 |
+
{isActive ? (
|
| 248 |
+
<Button
|
| 249 |
+
onClick={handleStop}
|
| 250 |
+
variant="destructive"
|
| 251 |
+
className="w-full"
|
| 252 |
+
>
|
| 253 |
+
⏹️ Stop Teleoperation
|
| 254 |
+
</Button>
|
| 255 |
+
) : (
|
| 256 |
+
<Button
|
| 257 |
+
onClick={handleStart}
|
| 258 |
+
disabled={!isConnected}
|
| 259 |
+
className="w-full"
|
| 260 |
+
>
|
| 261 |
+
▶️ Start Teleoperation
|
| 262 |
+
</Button>
|
| 263 |
+
)}
|
| 264 |
+
|
| 265 |
+
<Button
|
| 266 |
+
onClick={goToHome}
|
| 267 |
+
variant="outline"
|
| 268 |
+
disabled={!isConnected}
|
| 269 |
+
className="w-full"
|
| 270 |
+
>
|
| 271 |
+
🏠 Go to Home
|
| 272 |
+
</Button>
|
| 273 |
+
</div>
|
| 274 |
+
</CardContent>
|
| 275 |
+
</Card>
|
| 276 |
+
|
| 277 |
+
{/* Virtual Keyboard */}
|
| 278 |
+
<Card>
|
| 279 |
+
<CardHeader>
|
| 280 |
+
<CardTitle>Virtual Keyboard</CardTitle>
|
| 281 |
+
</CardHeader>
|
| 282 |
+
<CardContent>
|
| 283 |
+
<VirtualKeyboard />
|
| 284 |
+
</CardContent>
|
| 285 |
+
</Card>
|
| 286 |
+
|
| 287 |
+
{/* Motor Status */}
|
| 288 |
+
<Card>
|
| 289 |
+
<CardHeader>
|
| 290 |
+
<CardTitle>Motor Positions</CardTitle>
|
| 291 |
+
</CardHeader>
|
| 292 |
+
<CardContent className="space-y-3">
|
| 293 |
+
{motorConfigs.map((motor) => {
|
| 294 |
+
const range = motor.maxPosition - motor.minPosition;
|
| 295 |
+
const position = motor.currentPosition - motor.minPosition;
|
| 296 |
+
const percentage = range > 0 ? (position / range) * 100 : 0;
|
| 297 |
+
|
| 298 |
+
return (
|
| 299 |
+
<div key={motor.name} className="space-y-1">
|
| 300 |
+
<div className="flex justify-between items-center">
|
| 301 |
+
<span className="text-sm font-medium">
|
| 302 |
+
{motor.name.replace("_", " ")}
|
| 303 |
+
</span>
|
| 304 |
+
<span className="text-xs text-gray-500">
|
| 305 |
+
{motor.currentPosition}
|
| 306 |
+
</span>
|
| 307 |
+
</div>
|
| 308 |
+
<Progress value={percentage} className="h-2" />
|
| 309 |
+
<div className="flex justify-between text-xs text-gray-400">
|
| 310 |
+
<span>{motor.minPosition}</span>
|
| 311 |
+
<span>{motor.maxPosition}</span>
|
| 312 |
+
</div>
|
| 313 |
+
</div>
|
| 314 |
+
);
|
| 315 |
+
})}
|
| 316 |
+
</CardContent>
|
| 317 |
+
</Card>
|
| 318 |
+
</div>
|
| 319 |
+
|
| 320 |
+
{/* Help Card */}
|
| 321 |
+
<Card className="mt-6">
|
| 322 |
+
<CardHeader>
|
| 323 |
+
<CardTitle>Control Instructions</CardTitle>
|
| 324 |
+
</CardHeader>
|
| 325 |
+
<CardContent>
|
| 326 |
+
<div className="grid grid-cols-1 md:grid-cols-2 lg:grid-cols-4 gap-4 text-sm">
|
| 327 |
+
<div>
|
| 328 |
+
<h4 className="font-semibold mb-2">Arrow Keys</h4>
|
| 329 |
+
<ul className="space-y-1 text-gray-600">
|
| 330 |
+
<li>↑ ↓ Shoulder lift</li>
|
| 331 |
+
<li>← → Shoulder pan</li>
|
| 332 |
+
</ul>
|
| 333 |
+
</div>
|
| 334 |
+
<div>
|
| 335 |
+
<h4 className="font-semibold mb-2">WASD Keys</h4>
|
| 336 |
+
<ul className="space-y-1 text-gray-600">
|
| 337 |
+
<li>W S Elbow flex</li>
|
| 338 |
+
<li>A D Wrist flex</li>
|
| 339 |
+
</ul>
|
| 340 |
+
</div>
|
| 341 |
+
<div>
|
| 342 |
+
<h4 className="font-semibold mb-2">Other Keys</h4>
|
| 343 |
+
<ul className="space-y-1 text-gray-600">
|
| 344 |
+
<li>Q E Wrist roll</li>
|
| 345 |
+
<li>Space Gripper</li>
|
| 346 |
+
</ul>
|
| 347 |
+
</div>
|
| 348 |
+
<div>
|
| 349 |
+
<h4 className="font-semibold mb-2 text-red-700">Emergency</h4>
|
| 350 |
+
<ul className="space-y-1 text-red-600">
|
| 351 |
+
<li>ESC Emergency stop</li>
|
| 352 |
+
</ul>
|
| 353 |
+
</div>
|
| 354 |
+
</div>
|
| 355 |
+
<div className="mt-4 p-3 bg-blue-50 rounded-lg">
|
| 356 |
+
<p className="text-sm text-blue-800">
|
| 357 |
+
💡 <strong>Pro tip:</strong> Use your physical keyboard for
|
| 358 |
+
faster control, or click the virtual keys below. Hold keys down
|
| 359 |
+
for continuous movement.
|
| 360 |
+
</p>
|
| 361 |
+
</div>
|
| 362 |
+
</CardContent>
|
| 363 |
+
</Card>
|
| 364 |
+
</div>
|
| 365 |
+
</div>
|
| 366 |
+
);
|
| 367 |
+
}
|
src/demo/hooks/useRobotConnection.ts
ADDED
|
@@ -0,0 +1,91 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
import { useState, useEffect, useCallback } from "react";
|
| 2 |
+
import {
|
| 3 |
+
getRobotConnectionManager,
|
| 4 |
+
type RobotConnectionState,
|
| 5 |
+
writeMotorPosition,
|
| 6 |
+
readMotorPosition,
|
| 7 |
+
readAllMotorPositions,
|
| 8 |
+
} from "../../lerobot/web/robot-connection";
|
| 9 |
+
|
| 10 |
+
export interface UseRobotConnectionResult {
|
| 11 |
+
// Connection state
|
| 12 |
+
isConnected: boolean;
|
| 13 |
+
robotType?: "so100_follower" | "so100_leader";
|
| 14 |
+
robotId?: string;
|
| 15 |
+
serialNumber?: string;
|
| 16 |
+
lastError?: string;
|
| 17 |
+
|
| 18 |
+
// Connection management
|
| 19 |
+
connect: (
|
| 20 |
+
port: SerialPort,
|
| 21 |
+
robotType: string,
|
| 22 |
+
robotId: string,
|
| 23 |
+
serialNumber: string
|
| 24 |
+
) => Promise<void>;
|
| 25 |
+
disconnect: () => Promise<void>;
|
| 26 |
+
|
| 27 |
+
// Robot operations
|
| 28 |
+
writeMotorPosition: (motorId: number, position: number) => Promise<void>;
|
| 29 |
+
readMotorPosition: (motorId: number) => Promise<number | null>;
|
| 30 |
+
readAllMotorPositions: (motorIds: number[]) => Promise<number[]>;
|
| 31 |
+
|
| 32 |
+
// Raw port access (for advanced use cases)
|
| 33 |
+
getPort: () => SerialPort | null;
|
| 34 |
+
}
|
| 35 |
+
|
| 36 |
+
/**
|
| 37 |
+
* React hook for robot connection management
|
| 38 |
+
* Uses the singleton connection manager as single source of truth
|
| 39 |
+
*/
|
| 40 |
+
export function useRobotConnection(): UseRobotConnectionResult {
|
| 41 |
+
const manager = getRobotConnectionManager();
|
| 42 |
+
const [state, setState] = useState<RobotConnectionState>(manager.getState());
|
| 43 |
+
|
| 44 |
+
// Subscribe to connection state changes
|
| 45 |
+
useEffect(() => {
|
| 46 |
+
const unsubscribe = manager.onStateChange(setState);
|
| 47 |
+
|
| 48 |
+
// Set initial state
|
| 49 |
+
setState(manager.getState());
|
| 50 |
+
|
| 51 |
+
return unsubscribe;
|
| 52 |
+
}, [manager]);
|
| 53 |
+
|
| 54 |
+
// Connection management functions
|
| 55 |
+
const connect = useCallback(
|
| 56 |
+
async (
|
| 57 |
+
port: SerialPort,
|
| 58 |
+
robotType: string,
|
| 59 |
+
robotId: string,
|
| 60 |
+
serialNumber: string
|
| 61 |
+
) => {
|
| 62 |
+
await manager.connect(port, robotType, robotId, serialNumber);
|
| 63 |
+
},
|
| 64 |
+
[manager]
|
| 65 |
+
);
|
| 66 |
+
|
| 67 |
+
const disconnect = useCallback(async () => {
|
| 68 |
+
await manager.disconnect();
|
| 69 |
+
}, [manager]);
|
| 70 |
+
|
| 71 |
+
const getPort = useCallback(() => {
|
| 72 |
+
return manager.getPort();
|
| 73 |
+
}, [manager]);
|
| 74 |
+
|
| 75 |
+
return {
|
| 76 |
+
// State
|
| 77 |
+
isConnected: state.isConnected,
|
| 78 |
+
robotType: state.robotType,
|
| 79 |
+
robotId: state.robotId,
|
| 80 |
+
serialNumber: state.serialNumber,
|
| 81 |
+
lastError: state.lastError,
|
| 82 |
+
|
| 83 |
+
// Methods
|
| 84 |
+
connect,
|
| 85 |
+
disconnect,
|
| 86 |
+
writeMotorPosition,
|
| 87 |
+
readMotorPosition,
|
| 88 |
+
readAllMotorPositions,
|
| 89 |
+
getPort,
|
| 90 |
+
};
|
| 91 |
+
}
|
src/demo/hooks/useTeleoperation.ts
ADDED
|
@@ -0,0 +1,486 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
import { useState, useEffect, useCallback, useRef } from "react";
|
| 2 |
+
import { useRobotConnection } from "./useRobotConnection";
|
| 3 |
+
import { getUnifiedRobotData } from "../lib/unified-storage";
|
| 4 |
+
import type { ConnectedRobot } from "../types";
|
| 5 |
+
|
| 6 |
+
export interface MotorConfig {
|
| 7 |
+
name: string;
|
| 8 |
+
minPosition: number;
|
| 9 |
+
maxPosition: number;
|
| 10 |
+
currentPosition: number;
|
| 11 |
+
homePosition: number;
|
| 12 |
+
}
|
| 13 |
+
|
| 14 |
+
export interface KeyState {
|
| 15 |
+
pressed: boolean;
|
| 16 |
+
lastPressed: number;
|
| 17 |
+
}
|
| 18 |
+
|
| 19 |
+
export interface UseTeleoperationOptions {
|
| 20 |
+
robot: ConnectedRobot;
|
| 21 |
+
enabled: boolean;
|
| 22 |
+
onError?: (error: string) => void;
|
| 23 |
+
}
|
| 24 |
+
|
| 25 |
+
export interface UseTeleoperationResult {
|
| 26 |
+
// Connection state from singleton
|
| 27 |
+
isConnected: boolean;
|
| 28 |
+
isActive: boolean;
|
| 29 |
+
|
| 30 |
+
// Motor state
|
| 31 |
+
motorConfigs: MotorConfig[];
|
| 32 |
+
|
| 33 |
+
// Keyboard state
|
| 34 |
+
keyStates: Record<string, KeyState>;
|
| 35 |
+
|
| 36 |
+
// Error state
|
| 37 |
+
error: string | null;
|
| 38 |
+
|
| 39 |
+
// Control methods
|
| 40 |
+
start: () => void;
|
| 41 |
+
stop: () => void;
|
| 42 |
+
goToHome: () => Promise<void>;
|
| 43 |
+
simulateKeyPress: (key: string) => void;
|
| 44 |
+
simulateKeyRelease: (key: string) => void;
|
| 45 |
+
}
|
| 46 |
+
|
| 47 |
+
const MOTOR_CONFIGS: MotorConfig[] = [
|
| 48 |
+
{
|
| 49 |
+
name: "shoulder_pan",
|
| 50 |
+
minPosition: 0,
|
| 51 |
+
maxPosition: 4095,
|
| 52 |
+
currentPosition: 2048,
|
| 53 |
+
homePosition: 2048,
|
| 54 |
+
},
|
| 55 |
+
{
|
| 56 |
+
name: "shoulder_lift",
|
| 57 |
+
minPosition: 1024,
|
| 58 |
+
maxPosition: 3072,
|
| 59 |
+
currentPosition: 2048,
|
| 60 |
+
homePosition: 2048,
|
| 61 |
+
},
|
| 62 |
+
{
|
| 63 |
+
name: "elbow_flex",
|
| 64 |
+
minPosition: 1024,
|
| 65 |
+
maxPosition: 3072,
|
| 66 |
+
currentPosition: 2048,
|
| 67 |
+
homePosition: 2048,
|
| 68 |
+
},
|
| 69 |
+
{
|
| 70 |
+
name: "wrist_flex",
|
| 71 |
+
minPosition: 1024,
|
| 72 |
+
maxPosition: 3072,
|
| 73 |
+
currentPosition: 2048,
|
| 74 |
+
homePosition: 2048,
|
| 75 |
+
},
|
| 76 |
+
{
|
| 77 |
+
name: "wrist_roll",
|
| 78 |
+
minPosition: 0,
|
| 79 |
+
maxPosition: 4095,
|
| 80 |
+
currentPosition: 2048,
|
| 81 |
+
homePosition: 2048,
|
| 82 |
+
},
|
| 83 |
+
{
|
| 84 |
+
name: "gripper",
|
| 85 |
+
minPosition: 1800,
|
| 86 |
+
maxPosition: 2400,
|
| 87 |
+
currentPosition: 2100,
|
| 88 |
+
homePosition: 2100,
|
| 89 |
+
},
|
| 90 |
+
];
|
| 91 |
+
|
| 92 |
+
// PROVEN VALUES from Node.js implementation (conventions.md)
|
| 93 |
+
const SMOOTH_CONTROL_CONFIG = {
|
| 94 |
+
STEP_SIZE: 25, // Proven optimal from conventions.md
|
| 95 |
+
CHANGE_THRESHOLD: 0.5, // Prevents micro-movements and unnecessary commands
|
| 96 |
+
MOTOR_DELAY: 1, // Minimal delay between motor commands (from conventions.md)
|
| 97 |
+
UPDATE_INTERVAL: 30, // 30ms = ~33Hz for responsive control (was 50ms = 20Hz)
|
| 98 |
+
} as const;
|
| 99 |
+
|
| 100 |
+
const KEYBOARD_CONTROLS = {
|
| 101 |
+
ArrowUp: { motorIndex: 1, direction: 1, description: "Shoulder lift up" },
|
| 102 |
+
ArrowDown: {
|
| 103 |
+
motorIndex: 1,
|
| 104 |
+
direction: -1,
|
| 105 |
+
description: "Shoulder lift down",
|
| 106 |
+
},
|
| 107 |
+
ArrowLeft: { motorIndex: 0, direction: -1, description: "Shoulder pan left" },
|
| 108 |
+
ArrowRight: {
|
| 109 |
+
motorIndex: 0,
|
| 110 |
+
direction: 1,
|
| 111 |
+
description: "Shoulder pan right",
|
| 112 |
+
},
|
| 113 |
+
w: { motorIndex: 2, direction: 1, description: "Elbow flex up" },
|
| 114 |
+
s: { motorIndex: 2, direction: -1, description: "Elbow flex down" },
|
| 115 |
+
a: { motorIndex: 3, direction: -1, description: "Wrist flex left" },
|
| 116 |
+
d: { motorIndex: 3, direction: 1, description: "Wrist flex right" },
|
| 117 |
+
q: { motorIndex: 4, direction: -1, description: "Wrist roll left" },
|
| 118 |
+
e: { motorIndex: 4, direction: 1, description: "Wrist roll right" },
|
| 119 |
+
" ": { motorIndex: 5, direction: 1, description: "Gripper open/close" },
|
| 120 |
+
Escape: { motorIndex: -1, direction: 0, description: "Emergency stop" },
|
| 121 |
+
};
|
| 122 |
+
|
| 123 |
+
export function useTeleoperation({
|
| 124 |
+
robot,
|
| 125 |
+
enabled,
|
| 126 |
+
onError,
|
| 127 |
+
}: UseTeleoperationOptions): UseTeleoperationResult {
|
| 128 |
+
const connection = useRobotConnection();
|
| 129 |
+
const [isActive, setIsActive] = useState(false);
|
| 130 |
+
const [motorConfigs, setMotorConfigs] =
|
| 131 |
+
useState<MotorConfig[]>(MOTOR_CONFIGS);
|
| 132 |
+
const [keyStates, setKeyStates] = useState<Record<string, KeyState>>({});
|
| 133 |
+
const [error, setError] = useState<string | null>(null);
|
| 134 |
+
|
| 135 |
+
const activeKeysRef = useRef<Set<string>>(new Set());
|
| 136 |
+
const motorPositionsRef = useRef<number[]>(
|
| 137 |
+
MOTOR_CONFIGS.map((m) => m.homePosition)
|
| 138 |
+
);
|
| 139 |
+
const movementIntervalRef = useRef<NodeJS.Timeout | null>(null);
|
| 140 |
+
|
| 141 |
+
// Load calibration data
|
| 142 |
+
useEffect(() => {
|
| 143 |
+
const loadCalibration = async () => {
|
| 144 |
+
try {
|
| 145 |
+
if (!robot.serialNumber) {
|
| 146 |
+
console.warn("No serial number available for calibration loading");
|
| 147 |
+
return;
|
| 148 |
+
}
|
| 149 |
+
|
| 150 |
+
const data = getUnifiedRobotData(robot.serialNumber);
|
| 151 |
+
if (data?.calibration) {
|
| 152 |
+
// Map motor names to calibration data
|
| 153 |
+
const motorNames = [
|
| 154 |
+
"shoulder_pan",
|
| 155 |
+
"shoulder_lift",
|
| 156 |
+
"elbow_flex",
|
| 157 |
+
"wrist_flex",
|
| 158 |
+
"wrist_roll",
|
| 159 |
+
"gripper",
|
| 160 |
+
];
|
| 161 |
+
const calibratedConfigs = MOTOR_CONFIGS.map((config, index) => {
|
| 162 |
+
const motorName = motorNames[index] as keyof NonNullable<
|
| 163 |
+
typeof data.calibration
|
| 164 |
+
>;
|
| 165 |
+
const calibratedMotor = data.calibration![motorName];
|
| 166 |
+
if (
|
| 167 |
+
calibratedMotor &&
|
| 168 |
+
typeof calibratedMotor === "object" &&
|
| 169 |
+
"homing_offset" in calibratedMotor &&
|
| 170 |
+
"range_min" in calibratedMotor &&
|
| 171 |
+
"range_max" in calibratedMotor
|
| 172 |
+
) {
|
| 173 |
+
// Use 2048 as default home position, adjusted by homing offset
|
| 174 |
+
const homePosition = 2048 + (calibratedMotor.homing_offset || 0);
|
| 175 |
+
return {
|
| 176 |
+
...config,
|
| 177 |
+
homePosition,
|
| 178 |
+
currentPosition: homePosition,
|
| 179 |
+
// IMPORTANT: Use actual calibrated limits instead of hardcoded ones
|
| 180 |
+
minPosition: calibratedMotor.range_min || config.minPosition,
|
| 181 |
+
maxPosition: calibratedMotor.range_max || config.maxPosition,
|
| 182 |
+
};
|
| 183 |
+
}
|
| 184 |
+
return config;
|
| 185 |
+
});
|
| 186 |
+
setMotorConfigs(calibratedConfigs);
|
| 187 |
+
// DON'T set motorPositionsRef here - it will be set when teleoperation starts
|
| 188 |
+
// motorPositionsRef.current = calibratedConfigs.map((m) => m.homePosition);
|
| 189 |
+
console.log("✅ Loaded calibration data for", robot.serialNumber);
|
| 190 |
+
}
|
| 191 |
+
} catch (error) {
|
| 192 |
+
console.warn("Failed to load calibration:", error);
|
| 193 |
+
}
|
| 194 |
+
};
|
| 195 |
+
|
| 196 |
+
loadCalibration();
|
| 197 |
+
}, [robot.serialNumber]);
|
| 198 |
+
|
| 199 |
+
// Keyboard event handlers
|
| 200 |
+
const handleKeyDown = useCallback(
|
| 201 |
+
(event: KeyboardEvent) => {
|
| 202 |
+
if (!isActive) return;
|
| 203 |
+
|
| 204 |
+
const key = event.key;
|
| 205 |
+
if (key in KEYBOARD_CONTROLS) {
|
| 206 |
+
event.preventDefault();
|
| 207 |
+
|
| 208 |
+
if (key === "Escape") {
|
| 209 |
+
setIsActive(false);
|
| 210 |
+
activeKeysRef.current.clear();
|
| 211 |
+
return;
|
| 212 |
+
}
|
| 213 |
+
|
| 214 |
+
if (!activeKeysRef.current.has(key)) {
|
| 215 |
+
activeKeysRef.current.add(key);
|
| 216 |
+
setKeyStates((prev) => ({
|
| 217 |
+
...prev,
|
| 218 |
+
[key]: { pressed: true, lastPressed: Date.now() },
|
| 219 |
+
}));
|
| 220 |
+
}
|
| 221 |
+
}
|
| 222 |
+
},
|
| 223 |
+
[isActive]
|
| 224 |
+
);
|
| 225 |
+
|
| 226 |
+
const handleKeyUp = useCallback(
|
| 227 |
+
(event: KeyboardEvent) => {
|
| 228 |
+
if (!isActive) return;
|
| 229 |
+
|
| 230 |
+
const key = event.key;
|
| 231 |
+
if (key in KEYBOARD_CONTROLS) {
|
| 232 |
+
event.preventDefault();
|
| 233 |
+
activeKeysRef.current.delete(key);
|
| 234 |
+
setKeyStates((prev) => ({
|
| 235 |
+
...prev,
|
| 236 |
+
[key]: { pressed: false, lastPressed: Date.now() },
|
| 237 |
+
}));
|
| 238 |
+
}
|
| 239 |
+
},
|
| 240 |
+
[isActive]
|
| 241 |
+
);
|
| 242 |
+
|
| 243 |
+
// Register keyboard events
|
| 244 |
+
useEffect(() => {
|
| 245 |
+
if (enabled && isActive) {
|
| 246 |
+
window.addEventListener("keydown", handleKeyDown);
|
| 247 |
+
window.addEventListener("keyup", handleKeyUp);
|
| 248 |
+
|
| 249 |
+
return () => {
|
| 250 |
+
window.removeEventListener("keydown", handleKeyDown);
|
| 251 |
+
window.removeEventListener("keyup", handleKeyUp);
|
| 252 |
+
};
|
| 253 |
+
}
|
| 254 |
+
}, [enabled, isActive, handleKeyDown, handleKeyUp]);
|
| 255 |
+
|
| 256 |
+
// CONTINUOUS MOVEMENT: For held keys with PROVEN smooth patterns from Node.js
|
| 257 |
+
useEffect(() => {
|
| 258 |
+
if (!isActive || !connection.isConnected) {
|
| 259 |
+
if (movementIntervalRef.current) {
|
| 260 |
+
clearInterval(movementIntervalRef.current);
|
| 261 |
+
movementIntervalRef.current = null;
|
| 262 |
+
}
|
| 263 |
+
return;
|
| 264 |
+
}
|
| 265 |
+
|
| 266 |
+
const processMovement = async () => {
|
| 267 |
+
if (activeKeysRef.current.size === 0) return;
|
| 268 |
+
|
| 269 |
+
const activeKeys = Array.from(activeKeysRef.current);
|
| 270 |
+
const changedMotors: Array<{ index: number; position: number }> = [];
|
| 271 |
+
|
| 272 |
+
// PROVEN PATTERN: Process all active keys and collect changes
|
| 273 |
+
for (const key of activeKeys) {
|
| 274 |
+
const control =
|
| 275 |
+
KEYBOARD_CONTROLS[key as keyof typeof KEYBOARD_CONTROLS];
|
| 276 |
+
if (control && control.motorIndex >= 0) {
|
| 277 |
+
const motorIndex = control.motorIndex;
|
| 278 |
+
const direction = control.direction;
|
| 279 |
+
const motor = motorConfigs[motorIndex];
|
| 280 |
+
|
| 281 |
+
if (motor) {
|
| 282 |
+
const currentPos = motorPositionsRef.current[motorIndex];
|
| 283 |
+
let newPos =
|
| 284 |
+
currentPos + direction * SMOOTH_CONTROL_CONFIG.STEP_SIZE;
|
| 285 |
+
|
| 286 |
+
// Clamp to motor limits
|
| 287 |
+
newPos = Math.max(
|
| 288 |
+
motor.minPosition,
|
| 289 |
+
Math.min(motor.maxPosition, newPos)
|
| 290 |
+
);
|
| 291 |
+
|
| 292 |
+
// PROVEN PATTERN: Only update if change is meaningful (0.5 unit threshold)
|
| 293 |
+
if (
|
| 294 |
+
Math.abs(newPos - currentPos) >
|
| 295 |
+
SMOOTH_CONTROL_CONFIG.CHANGE_THRESHOLD
|
| 296 |
+
) {
|
| 297 |
+
motorPositionsRef.current[motorIndex] = newPos;
|
| 298 |
+
changedMotors.push({ index: motorIndex, position: newPos });
|
| 299 |
+
}
|
| 300 |
+
}
|
| 301 |
+
}
|
| 302 |
+
}
|
| 303 |
+
|
| 304 |
+
// PROVEN PATTERN: Only send commands for motors that actually changed
|
| 305 |
+
if (changedMotors.length > 0) {
|
| 306 |
+
try {
|
| 307 |
+
for (const { index, position } of changedMotors) {
|
| 308 |
+
await connection.writeMotorPosition(index + 1, position);
|
| 309 |
+
|
| 310 |
+
// PROVEN PATTERN: Minimal delay between motor commands (1ms)
|
| 311 |
+
if (changedMotors.length > 1) {
|
| 312 |
+
await new Promise((resolve) =>
|
| 313 |
+
setTimeout(resolve, SMOOTH_CONTROL_CONFIG.MOTOR_DELAY)
|
| 314 |
+
);
|
| 315 |
+
}
|
| 316 |
+
}
|
| 317 |
+
|
| 318 |
+
// Update UI to reflect changes
|
| 319 |
+
setMotorConfigs((prev) =>
|
| 320 |
+
prev.map((config, index) => ({
|
| 321 |
+
...config,
|
| 322 |
+
currentPosition: motorPositionsRef.current[index],
|
| 323 |
+
}))
|
| 324 |
+
);
|
| 325 |
+
} catch (error) {
|
| 326 |
+
console.warn("Failed to update robot positions:", error);
|
| 327 |
+
}
|
| 328 |
+
}
|
| 329 |
+
};
|
| 330 |
+
|
| 331 |
+
// PROVEN TIMING: 30ms interval (~33Hz) for responsive continuous movement
|
| 332 |
+
movementIntervalRef.current = setInterval(
|
| 333 |
+
processMovement,
|
| 334 |
+
SMOOTH_CONTROL_CONFIG.UPDATE_INTERVAL
|
| 335 |
+
);
|
| 336 |
+
|
| 337 |
+
return () => {
|
| 338 |
+
if (movementIntervalRef.current) {
|
| 339 |
+
clearInterval(movementIntervalRef.current);
|
| 340 |
+
movementIntervalRef.current = null;
|
| 341 |
+
}
|
| 342 |
+
};
|
| 343 |
+
}, [
|
| 344 |
+
isActive,
|
| 345 |
+
connection.isConnected,
|
| 346 |
+
connection.writeMotorPosition,
|
| 347 |
+
motorConfigs,
|
| 348 |
+
]);
|
| 349 |
+
|
| 350 |
+
// Control methods
|
| 351 |
+
const start = useCallback(async () => {
|
| 352 |
+
if (!connection.isConnected) {
|
| 353 |
+
setError("Robot not connected");
|
| 354 |
+
onError?.("Robot not connected");
|
| 355 |
+
return;
|
| 356 |
+
}
|
| 357 |
+
|
| 358 |
+
try {
|
| 359 |
+
console.log(
|
| 360 |
+
"🎮 Starting teleoperation - reading current motor positions..."
|
| 361 |
+
);
|
| 362 |
+
|
| 363 |
+
// Read current positions of all motors using PROVEN utility
|
| 364 |
+
const motorIds = [1, 2, 3, 4, 5, 6];
|
| 365 |
+
const currentPositions = await connection.readAllMotorPositions(motorIds);
|
| 366 |
+
|
| 367 |
+
// Log all positions (trust the utility's fallback handling)
|
| 368 |
+
for (let i = 0; i < currentPositions.length; i++) {
|
| 369 |
+
const position = currentPositions[i];
|
| 370 |
+
console.log(`📍 Motor ${i + 1} current position: ${position}`);
|
| 371 |
+
}
|
| 372 |
+
|
| 373 |
+
// CRITICAL: Update positions BEFORE activating movement
|
| 374 |
+
motorPositionsRef.current = currentPositions;
|
| 375 |
+
|
| 376 |
+
// Update UI to show actual current positions
|
| 377 |
+
setMotorConfigs((prev) =>
|
| 378 |
+
prev.map((config, index) => ({
|
| 379 |
+
...config,
|
| 380 |
+
currentPosition: currentPositions[index],
|
| 381 |
+
}))
|
| 382 |
+
);
|
| 383 |
+
|
| 384 |
+
// IMPORTANT: Only activate AFTER positions are synchronized
|
| 385 |
+
setIsActive(true);
|
| 386 |
+
setError(null);
|
| 387 |
+
console.log(
|
| 388 |
+
"✅ Teleoperation started with synchronized positions:",
|
| 389 |
+
currentPositions
|
| 390 |
+
);
|
| 391 |
+
} catch (error) {
|
| 392 |
+
const errorMessage =
|
| 393 |
+
error instanceof Error
|
| 394 |
+
? error.message
|
| 395 |
+
: "Failed to start teleoperation";
|
| 396 |
+
setError(errorMessage);
|
| 397 |
+
onError?.(errorMessage);
|
| 398 |
+
console.error("❌ Failed to start teleoperation:", error);
|
| 399 |
+
}
|
| 400 |
+
}, [
|
| 401 |
+
connection.isConnected,
|
| 402 |
+
connection.readAllMotorPositions,
|
| 403 |
+
motorConfigs,
|
| 404 |
+
onError,
|
| 405 |
+
]);
|
| 406 |
+
|
| 407 |
+
const stop = useCallback(() => {
|
| 408 |
+
setIsActive(false);
|
| 409 |
+
activeKeysRef.current.clear();
|
| 410 |
+
setKeyStates({});
|
| 411 |
+
console.log("🛑 Teleoperation stopped");
|
| 412 |
+
}, []);
|
| 413 |
+
|
| 414 |
+
const goToHome = useCallback(async () => {
|
| 415 |
+
if (!connection.isConnected) {
|
| 416 |
+
setError("Robot not connected");
|
| 417 |
+
return;
|
| 418 |
+
}
|
| 419 |
+
|
| 420 |
+
try {
|
| 421 |
+
for (let i = 0; i < motorConfigs.length; i++) {
|
| 422 |
+
const motor = motorConfigs[i];
|
| 423 |
+
await connection.writeMotorPosition(i + 1, motor.homePosition);
|
| 424 |
+
motorPositionsRef.current[i] = motor.homePosition;
|
| 425 |
+
}
|
| 426 |
+
|
| 427 |
+
setMotorConfigs((prev) =>
|
| 428 |
+
prev.map((config) => ({
|
| 429 |
+
...config,
|
| 430 |
+
currentPosition: config.homePosition,
|
| 431 |
+
}))
|
| 432 |
+
);
|
| 433 |
+
|
| 434 |
+
console.log("🏠 Moved to home position");
|
| 435 |
+
} catch (error) {
|
| 436 |
+
const errorMessage =
|
| 437 |
+
error instanceof Error ? error.message : "Failed to go to home";
|
| 438 |
+
setError(errorMessage);
|
| 439 |
+
onError?.(errorMessage);
|
| 440 |
+
}
|
| 441 |
+
}, [
|
| 442 |
+
connection.isConnected,
|
| 443 |
+
connection.writeMotorPosition,
|
| 444 |
+
motorConfigs,
|
| 445 |
+
onError,
|
| 446 |
+
]);
|
| 447 |
+
|
| 448 |
+
const simulateKeyPress = useCallback(
|
| 449 |
+
(key: string) => {
|
| 450 |
+
if (!isActive) return;
|
| 451 |
+
|
| 452 |
+
activeKeysRef.current.add(key);
|
| 453 |
+
setKeyStates((prev) => ({
|
| 454 |
+
...prev,
|
| 455 |
+
[key]: { pressed: true, lastPressed: Date.now() },
|
| 456 |
+
}));
|
| 457 |
+
},
|
| 458 |
+
[isActive]
|
| 459 |
+
);
|
| 460 |
+
|
| 461 |
+
const simulateKeyRelease = useCallback(
|
| 462 |
+
(key: string) => {
|
| 463 |
+
if (!isActive) return;
|
| 464 |
+
|
| 465 |
+
activeKeysRef.current.delete(key);
|
| 466 |
+
setKeyStates((prev) => ({
|
| 467 |
+
...prev,
|
| 468 |
+
[key]: { pressed: false, lastPressed: Date.now() },
|
| 469 |
+
}));
|
| 470 |
+
},
|
| 471 |
+
[isActive]
|
| 472 |
+
);
|
| 473 |
+
|
| 474 |
+
return {
|
| 475 |
+
isConnected: connection.isConnected,
|
| 476 |
+
isActive,
|
| 477 |
+
motorConfigs,
|
| 478 |
+
keyStates,
|
| 479 |
+
error,
|
| 480 |
+
start,
|
| 481 |
+
stop,
|
| 482 |
+
goToHome,
|
| 483 |
+
simulateKeyPress,
|
| 484 |
+
simulateKeyRelease,
|
| 485 |
+
};
|
| 486 |
+
}
|
src/demo/pages/Home.tsx
CHANGED
|
@@ -10,6 +10,7 @@ import {
|
|
| 10 |
import { Alert, AlertDescription } from "../components/ui/alert";
|
| 11 |
import { PortManager } from "../components/PortManager";
|
| 12 |
import { CalibrationPanel } from "../components/CalibrationPanel";
|
|
|
|
| 13 |
import { isWebSerialSupported } from "../../lerobot/web/calibrate";
|
| 14 |
import type { ConnectedRobot } from "../types";
|
| 15 |
|
|
@@ -26,6 +27,8 @@ export function Home({
|
|
| 26 |
}: HomeProps) {
|
| 27 |
const [calibratingRobot, setCalibratingRobot] =
|
| 28 |
useState<ConnectedRobot | null>(null);
|
|
|
|
|
|
|
| 29 |
const isSupported = isWebSerialSupported();
|
| 30 |
|
| 31 |
const handleCalibrate = (
|
|
@@ -40,10 +43,18 @@ export function Home({
|
|
| 40 |
}
|
| 41 |
};
|
| 42 |
|
|
|
|
|
|
|
|
|
|
|
|
|
| 43 |
const handleFinishCalibration = () => {
|
| 44 |
setCalibratingRobot(null);
|
| 45 |
};
|
| 46 |
|
|
|
|
|
|
|
|
|
|
|
|
|
| 47 |
return (
|
| 48 |
<div className="min-h-screen bg-gradient-to-br from-blue-50 to-indigo-100">
|
| 49 |
<div className="container mx-auto px-6 py-12">
|
|
@@ -83,10 +94,16 @@ export function Home({
|
|
| 83 |
onFinish={handleFinishCalibration}
|
| 84 |
/>
|
| 85 |
</div>
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 86 |
) : (
|
| 87 |
<div className="max-w-6xl mx-auto">
|
| 88 |
<PortManager
|
| 89 |
onCalibrate={handleCalibrate}
|
|
|
|
| 90 |
connectedRobots={connectedRobots}
|
| 91 |
onConnectedRobotsChange={onConnectedRobotsChange}
|
| 92 |
/>
|
|
|
|
| 10 |
import { Alert, AlertDescription } from "../components/ui/alert";
|
| 11 |
import { PortManager } from "../components/PortManager";
|
| 12 |
import { CalibrationPanel } from "../components/CalibrationPanel";
|
| 13 |
+
import { TeleoperationPanel } from "../components/TeleoperationPanel";
|
| 14 |
import { isWebSerialSupported } from "../../lerobot/web/calibrate";
|
| 15 |
import type { ConnectedRobot } from "../types";
|
| 16 |
|
|
|
|
| 27 |
}: HomeProps) {
|
| 28 |
const [calibratingRobot, setCalibratingRobot] =
|
| 29 |
useState<ConnectedRobot | null>(null);
|
| 30 |
+
const [teleoperatingRobot, setTeleoperatingRobot] =
|
| 31 |
+
useState<ConnectedRobot | null>(null);
|
| 32 |
const isSupported = isWebSerialSupported();
|
| 33 |
|
| 34 |
const handleCalibrate = (
|
|
|
|
| 43 |
}
|
| 44 |
};
|
| 45 |
|
| 46 |
+
const handleTeleoperate = (robot: ConnectedRobot) => {
|
| 47 |
+
setTeleoperatingRobot(robot);
|
| 48 |
+
};
|
| 49 |
+
|
| 50 |
const handleFinishCalibration = () => {
|
| 51 |
setCalibratingRobot(null);
|
| 52 |
};
|
| 53 |
|
| 54 |
+
const handleFinishTeleoperation = () => {
|
| 55 |
+
setTeleoperatingRobot(null);
|
| 56 |
+
};
|
| 57 |
+
|
| 58 |
return (
|
| 59 |
<div className="min-h-screen bg-gradient-to-br from-blue-50 to-indigo-100">
|
| 60 |
<div className="container mx-auto px-6 py-12">
|
|
|
|
| 94 |
onFinish={handleFinishCalibration}
|
| 95 |
/>
|
| 96 |
</div>
|
| 97 |
+
) : teleoperatingRobot ? (
|
| 98 |
+
<TeleoperationPanel
|
| 99 |
+
robot={teleoperatingRobot}
|
| 100 |
+
onClose={handleFinishTeleoperation}
|
| 101 |
+
/>
|
| 102 |
) : (
|
| 103 |
<div className="max-w-6xl mx-auto">
|
| 104 |
<PortManager
|
| 105 |
onCalibrate={handleCalibrate}
|
| 106 |
+
onTeleoperate={handleTeleoperate}
|
| 107 |
connectedRobots={connectedRobots}
|
| 108 |
onConnectedRobotsChange={onConnectedRobotsChange}
|
| 109 |
/>
|
src/lerobot/web/motor-utils.ts
ADDED
|
@@ -0,0 +1,266 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
/**
|
| 2 |
+
* Shared Motor Communication Utilities
|
| 3 |
+
* Proven patterns from calibrate.ts for consistent motor communication
|
| 4 |
+
* Used by both calibration and teleoperation
|
| 5 |
+
*/
|
| 6 |
+
|
| 7 |
+
export interface MotorCommunicationPort {
|
| 8 |
+
write(data: Uint8Array): Promise<void>;
|
| 9 |
+
read(timeout?: number): Promise<Uint8Array>;
|
| 10 |
+
}
|
| 11 |
+
|
| 12 |
+
/**
|
| 13 |
+
* STS3215 Protocol Constants
|
| 14 |
+
* Single source of truth for all motor communication
|
| 15 |
+
*/
|
| 16 |
+
export const STS3215_PROTOCOL = {
|
| 17 |
+
// Register addresses
|
| 18 |
+
PRESENT_POSITION_ADDRESS: 56,
|
| 19 |
+
GOAL_POSITION_ADDRESS: 42,
|
| 20 |
+
HOMING_OFFSET_ADDRESS: 31,
|
| 21 |
+
MIN_POSITION_LIMIT_ADDRESS: 9,
|
| 22 |
+
MAX_POSITION_LIMIT_ADDRESS: 11,
|
| 23 |
+
|
| 24 |
+
// Protocol constants
|
| 25 |
+
RESOLUTION: 4096, // 12-bit resolution (0-4095)
|
| 26 |
+
SIGN_MAGNITUDE_BIT: 11, // Bit 11 is sign bit for Homing_Offset encoding
|
| 27 |
+
|
| 28 |
+
// Communication timing (proven from calibration)
|
| 29 |
+
WRITE_TO_READ_DELAY: 10,
|
| 30 |
+
RETRY_DELAY: 20,
|
| 31 |
+
INTER_MOTOR_DELAY: 10,
|
| 32 |
+
MAX_RETRIES: 3,
|
| 33 |
+
} as const;
|
| 34 |
+
|
| 35 |
+
/**
|
| 36 |
+
* Read single motor position with PROVEN retry logic
|
| 37 |
+
* Reuses exact patterns from calibrate.ts
|
| 38 |
+
*/
|
| 39 |
+
export async function readMotorPosition(
|
| 40 |
+
port: MotorCommunicationPort,
|
| 41 |
+
motorId: number
|
| 42 |
+
): Promise<number | null> {
|
| 43 |
+
try {
|
| 44 |
+
// Create Read Position packet using proven pattern
|
| 45 |
+
const packet = new Uint8Array([
|
| 46 |
+
0xff,
|
| 47 |
+
0xff, // Header
|
| 48 |
+
motorId, // Servo ID
|
| 49 |
+
0x04, // Length
|
| 50 |
+
0x02, // Instruction: READ_DATA
|
| 51 |
+
STS3215_PROTOCOL.PRESENT_POSITION_ADDRESS, // Present_Position register address
|
| 52 |
+
0x02, // Data length (2 bytes)
|
| 53 |
+
0x00, // Checksum placeholder
|
| 54 |
+
]);
|
| 55 |
+
|
| 56 |
+
const checksum =
|
| 57 |
+
~(
|
| 58 |
+
motorId +
|
| 59 |
+
0x04 +
|
| 60 |
+
0x02 +
|
| 61 |
+
STS3215_PROTOCOL.PRESENT_POSITION_ADDRESS +
|
| 62 |
+
0x02
|
| 63 |
+
) & 0xff;
|
| 64 |
+
packet[7] = checksum;
|
| 65 |
+
|
| 66 |
+
// PROVEN PATTERN: Professional Feetech communication with retry logic
|
| 67 |
+
let attempts = 0;
|
| 68 |
+
|
| 69 |
+
while (attempts < STS3215_PROTOCOL.MAX_RETRIES) {
|
| 70 |
+
attempts++;
|
| 71 |
+
|
| 72 |
+
// CRITICAL: Clear any remaining data in buffer first (from calibration lessons)
|
| 73 |
+
try {
|
| 74 |
+
await port.read(0); // Non-blocking read to clear buffer
|
| 75 |
+
} catch (e) {
|
| 76 |
+
// Expected - buffer was empty
|
| 77 |
+
}
|
| 78 |
+
|
| 79 |
+
// Write command with PROVEN timing
|
| 80 |
+
await port.write(packet);
|
| 81 |
+
|
| 82 |
+
// PROVEN TIMING: Arduino library uses careful timing - Web Serial needs more
|
| 83 |
+
await new Promise((resolve) =>
|
| 84 |
+
setTimeout(resolve, STS3215_PROTOCOL.WRITE_TO_READ_DELAY)
|
| 85 |
+
);
|
| 86 |
+
|
| 87 |
+
try {
|
| 88 |
+
const response = await port.read(150);
|
| 89 |
+
|
| 90 |
+
if (response.length >= 7) {
|
| 91 |
+
const id = response[2];
|
| 92 |
+
const error = response[4];
|
| 93 |
+
|
| 94 |
+
if (id === motorId && error === 0) {
|
| 95 |
+
const position = response[5] | (response[6] << 8);
|
| 96 |
+
return position;
|
| 97 |
+
} else if (id === motorId && error !== 0) {
|
| 98 |
+
// Motor error, retry
|
| 99 |
+
} else {
|
| 100 |
+
// Wrong response ID, retry
|
| 101 |
+
}
|
| 102 |
+
} else {
|
| 103 |
+
// Short response, retry
|
| 104 |
+
}
|
| 105 |
+
} catch (readError) {
|
| 106 |
+
// Read timeout, retry
|
| 107 |
+
}
|
| 108 |
+
|
| 109 |
+
// PROVEN TIMING: Professional timing between attempts
|
| 110 |
+
if (attempts < STS3215_PROTOCOL.MAX_RETRIES) {
|
| 111 |
+
await new Promise((resolve) =>
|
| 112 |
+
setTimeout(resolve, STS3215_PROTOCOL.RETRY_DELAY)
|
| 113 |
+
);
|
| 114 |
+
}
|
| 115 |
+
}
|
| 116 |
+
|
| 117 |
+
// If all attempts failed, return null
|
| 118 |
+
return null;
|
| 119 |
+
} catch (error) {
|
| 120 |
+
console.warn(`Failed to read motor ${motorId} position:`, error);
|
| 121 |
+
return null;
|
| 122 |
+
}
|
| 123 |
+
}
|
| 124 |
+
|
| 125 |
+
/**
|
| 126 |
+
* Read all motor positions with PROVEN patterns
|
| 127 |
+
* Exactly matches calibrate.ts readMotorPositions() function
|
| 128 |
+
*/
|
| 129 |
+
export async function readAllMotorPositions(
|
| 130 |
+
port: MotorCommunicationPort,
|
| 131 |
+
motorIds: number[]
|
| 132 |
+
): Promise<number[]> {
|
| 133 |
+
const motorPositions: number[] = [];
|
| 134 |
+
|
| 135 |
+
for (let i = 0; i < motorIds.length; i++) {
|
| 136 |
+
const motorId = motorIds[i];
|
| 137 |
+
|
| 138 |
+
const position = await readMotorPosition(port, motorId);
|
| 139 |
+
|
| 140 |
+
if (position !== null) {
|
| 141 |
+
motorPositions.push(position);
|
| 142 |
+
} else {
|
| 143 |
+
// Use fallback value for failed reads
|
| 144 |
+
const fallback = Math.floor((STS3215_PROTOCOL.RESOLUTION - 1) / 2);
|
| 145 |
+
motorPositions.push(fallback);
|
| 146 |
+
}
|
| 147 |
+
|
| 148 |
+
// PROVEN PATTERN: Professional inter-motor delay
|
| 149 |
+
await new Promise((resolve) =>
|
| 150 |
+
setTimeout(resolve, STS3215_PROTOCOL.INTER_MOTOR_DELAY)
|
| 151 |
+
);
|
| 152 |
+
}
|
| 153 |
+
|
| 154 |
+
return motorPositions;
|
| 155 |
+
}
|
| 156 |
+
|
| 157 |
+
/**
|
| 158 |
+
* Write motor position with error handling
|
| 159 |
+
*/
|
| 160 |
+
export async function writeMotorPosition(
|
| 161 |
+
port: MotorCommunicationPort,
|
| 162 |
+
motorId: number,
|
| 163 |
+
position: number
|
| 164 |
+
): Promise<void> {
|
| 165 |
+
// STS3215 Write Goal_Position packet
|
| 166 |
+
const packet = new Uint8Array([
|
| 167 |
+
0xff,
|
| 168 |
+
0xff, // Header
|
| 169 |
+
motorId, // Servo ID
|
| 170 |
+
0x05, // Length
|
| 171 |
+
0x03, // Instruction: WRITE_DATA
|
| 172 |
+
STS3215_PROTOCOL.GOAL_POSITION_ADDRESS, // Goal_Position register address
|
| 173 |
+
position & 0xff, // Position low byte
|
| 174 |
+
(position >> 8) & 0xff, // Position high byte
|
| 175 |
+
0x00, // Checksum placeholder
|
| 176 |
+
]);
|
| 177 |
+
|
| 178 |
+
// Calculate checksum
|
| 179 |
+
const checksum =
|
| 180 |
+
~(
|
| 181 |
+
motorId +
|
| 182 |
+
0x05 +
|
| 183 |
+
0x03 +
|
| 184 |
+
STS3215_PROTOCOL.GOAL_POSITION_ADDRESS +
|
| 185 |
+
(position & 0xff) +
|
| 186 |
+
((position >> 8) & 0xff)
|
| 187 |
+
) & 0xff;
|
| 188 |
+
packet[8] = checksum;
|
| 189 |
+
|
| 190 |
+
await port.write(packet);
|
| 191 |
+
}
|
| 192 |
+
|
| 193 |
+
/**
|
| 194 |
+
* Generic function to write a 2-byte value to a motor register
|
| 195 |
+
* Matches calibrate.ts writeMotorRegister() exactly
|
| 196 |
+
*/
|
| 197 |
+
export async function writeMotorRegister(
|
| 198 |
+
port: MotorCommunicationPort,
|
| 199 |
+
motorId: number,
|
| 200 |
+
registerAddress: number,
|
| 201 |
+
value: number
|
| 202 |
+
): Promise<void> {
|
| 203 |
+
// Create Write Register packet
|
| 204 |
+
const packet = new Uint8Array([
|
| 205 |
+
0xff,
|
| 206 |
+
0xff, // Header
|
| 207 |
+
motorId, // Servo ID
|
| 208 |
+
0x05, // Length
|
| 209 |
+
0x03, // Instruction: WRITE_DATA
|
| 210 |
+
registerAddress, // Register address
|
| 211 |
+
value & 0xff, // Data_L (low byte)
|
| 212 |
+
(value >> 8) & 0xff, // Data_H (high byte)
|
| 213 |
+
0x00, // Checksum placeholder
|
| 214 |
+
]);
|
| 215 |
+
|
| 216 |
+
// Calculate checksum
|
| 217 |
+
const checksum =
|
| 218 |
+
~(
|
| 219 |
+
motorId +
|
| 220 |
+
0x05 +
|
| 221 |
+
0x03 +
|
| 222 |
+
registerAddress +
|
| 223 |
+
(value & 0xff) +
|
| 224 |
+
((value >> 8) & 0xff)
|
| 225 |
+
) & 0xff;
|
| 226 |
+
packet[8] = checksum;
|
| 227 |
+
|
| 228 |
+
// Simple write then read like calibration
|
| 229 |
+
await port.write(packet);
|
| 230 |
+
|
| 231 |
+
// Wait for response (silent unless error)
|
| 232 |
+
try {
|
| 233 |
+
await port.read(200);
|
| 234 |
+
} catch (error) {
|
| 235 |
+
// Silent - response not required for successful operation
|
| 236 |
+
}
|
| 237 |
+
}
|
| 238 |
+
|
| 239 |
+
/**
|
| 240 |
+
* Sign-magnitude encoding functions (from calibrate.ts)
|
| 241 |
+
*/
|
| 242 |
+
export function encodeSignMagnitude(
|
| 243 |
+
value: number,
|
| 244 |
+
signBitIndex: number = STS3215_PROTOCOL.SIGN_MAGNITUDE_BIT
|
| 245 |
+
): number {
|
| 246 |
+
const maxMagnitude = (1 << signBitIndex) - 1;
|
| 247 |
+
const magnitude = Math.abs(value);
|
| 248 |
+
|
| 249 |
+
if (magnitude > maxMagnitude) {
|
| 250 |
+
throw new Error(
|
| 251 |
+
`Magnitude ${magnitude} exceeds ${maxMagnitude} (max for signBitIndex=${signBitIndex})`
|
| 252 |
+
);
|
| 253 |
+
}
|
| 254 |
+
|
| 255 |
+
const directionBit = value < 0 ? 1 : 0;
|
| 256 |
+
return (directionBit << signBitIndex) | magnitude;
|
| 257 |
+
}
|
| 258 |
+
|
| 259 |
+
export function decodeSignMagnitude(
|
| 260 |
+
encodedValue: number,
|
| 261 |
+
signBitIndex: number = STS3215_PROTOCOL.SIGN_MAGNITUDE_BIT
|
| 262 |
+
): number {
|
| 263 |
+
const signBit = (encodedValue >> signBitIndex) & 1;
|
| 264 |
+
const magnitude = encodedValue & ((1 << signBitIndex) - 1);
|
| 265 |
+
return signBit ? -magnitude : magnitude;
|
| 266 |
+
}
|
src/lerobot/web/robot-connection.ts
ADDED
|
@@ -0,0 +1,228 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
/**
|
| 2 |
+
* Core Robot Connection Manager
|
| 3 |
+
* Single source of truth for robot connections in the web library
|
| 4 |
+
* Provides singleton access to robot ports and connection state
|
| 5 |
+
*/
|
| 6 |
+
|
| 7 |
+
import {
|
| 8 |
+
readMotorPosition as readMotorPositionUtil,
|
| 9 |
+
writeMotorPosition as writeMotorPositionUtil,
|
| 10 |
+
readAllMotorPositions as readAllMotorPositionsUtil,
|
| 11 |
+
writeMotorRegister,
|
| 12 |
+
type MotorCommunicationPort,
|
| 13 |
+
} from "./motor-utils.js";
|
| 14 |
+
|
| 15 |
+
export interface RobotConnectionState {
|
| 16 |
+
isConnected: boolean;
|
| 17 |
+
robotType?: "so100_follower" | "so100_leader";
|
| 18 |
+
robotId?: string;
|
| 19 |
+
serialNumber?: string;
|
| 20 |
+
lastError?: string;
|
| 21 |
+
}
|
| 22 |
+
|
| 23 |
+
export interface RobotConnectionManager {
|
| 24 |
+
// State
|
| 25 |
+
getState(): RobotConnectionState;
|
| 26 |
+
|
| 27 |
+
// Connection management
|
| 28 |
+
connect(
|
| 29 |
+
port: SerialPort,
|
| 30 |
+
robotType: string,
|
| 31 |
+
robotId: string,
|
| 32 |
+
serialNumber: string
|
| 33 |
+
): Promise<void>;
|
| 34 |
+
disconnect(): Promise<void>;
|
| 35 |
+
|
| 36 |
+
// Port access
|
| 37 |
+
getPort(): SerialPort | null;
|
| 38 |
+
|
| 39 |
+
// Serial operations (shared by calibration, teleoperation, etc.)
|
| 40 |
+
writeData(data: Uint8Array): Promise<void>;
|
| 41 |
+
readData(timeout?: number): Promise<Uint8Array>;
|
| 42 |
+
|
| 43 |
+
// Event system
|
| 44 |
+
onStateChange(callback: (state: RobotConnectionState) => void): () => void;
|
| 45 |
+
}
|
| 46 |
+
|
| 47 |
+
/**
|
| 48 |
+
* Singleton Robot Connection Manager Implementation
|
| 49 |
+
*/
|
| 50 |
+
class RobotConnectionManagerImpl implements RobotConnectionManager {
|
| 51 |
+
private port: SerialPort | null = null;
|
| 52 |
+
private state: RobotConnectionState = { isConnected: false };
|
| 53 |
+
private stateChangeCallbacks: Set<(state: RobotConnectionState) => void> =
|
| 54 |
+
new Set();
|
| 55 |
+
|
| 56 |
+
getState(): RobotConnectionState {
|
| 57 |
+
return { ...this.state };
|
| 58 |
+
}
|
| 59 |
+
|
| 60 |
+
async connect(
|
| 61 |
+
port: SerialPort,
|
| 62 |
+
robotType: string,
|
| 63 |
+
robotId: string,
|
| 64 |
+
serialNumber: string
|
| 65 |
+
): Promise<void> {
|
| 66 |
+
try {
|
| 67 |
+
// Validate port is open
|
| 68 |
+
if (!port.readable || !port.writable) {
|
| 69 |
+
throw new Error("Port is not open");
|
| 70 |
+
}
|
| 71 |
+
|
| 72 |
+
// Update connection state
|
| 73 |
+
this.port = port;
|
| 74 |
+
this.state = {
|
| 75 |
+
isConnected: true,
|
| 76 |
+
robotType: robotType as "so100_follower" | "so100_leader",
|
| 77 |
+
robotId,
|
| 78 |
+
serialNumber,
|
| 79 |
+
lastError: undefined,
|
| 80 |
+
};
|
| 81 |
+
|
| 82 |
+
this.notifyStateChange();
|
| 83 |
+
console.log(
|
| 84 |
+
`🤖 Robot connected: ${robotType} (${robotId}) - ${serialNumber}`
|
| 85 |
+
);
|
| 86 |
+
} catch (error) {
|
| 87 |
+
const errorMessage =
|
| 88 |
+
error instanceof Error ? error.message : "Connection failed";
|
| 89 |
+
this.state = {
|
| 90 |
+
isConnected: false,
|
| 91 |
+
lastError: errorMessage,
|
| 92 |
+
};
|
| 93 |
+
this.notifyStateChange();
|
| 94 |
+
throw error;
|
| 95 |
+
}
|
| 96 |
+
}
|
| 97 |
+
|
| 98 |
+
async disconnect(): Promise<void> {
|
| 99 |
+
this.port = null;
|
| 100 |
+
this.state = { isConnected: false };
|
| 101 |
+
this.notifyStateChange();
|
| 102 |
+
console.log("🤖 Robot disconnected");
|
| 103 |
+
}
|
| 104 |
+
|
| 105 |
+
getPort(): SerialPort | null {
|
| 106 |
+
return this.port;
|
| 107 |
+
}
|
| 108 |
+
|
| 109 |
+
async writeData(data: Uint8Array): Promise<void> {
|
| 110 |
+
if (!this.port?.writable) {
|
| 111 |
+
throw new Error("Robot not connected or port not writable");
|
| 112 |
+
}
|
| 113 |
+
|
| 114 |
+
const writer = this.port.writable.getWriter();
|
| 115 |
+
try {
|
| 116 |
+
await writer.write(data);
|
| 117 |
+
} finally {
|
| 118 |
+
writer.releaseLock();
|
| 119 |
+
}
|
| 120 |
+
}
|
| 121 |
+
|
| 122 |
+
async readData(timeout: number = 1000): Promise<Uint8Array> {
|
| 123 |
+
if (!this.port?.readable) {
|
| 124 |
+
throw new Error("Robot not connected or port not readable");
|
| 125 |
+
}
|
| 126 |
+
|
| 127 |
+
const reader = this.port.readable.getReader();
|
| 128 |
+
|
| 129 |
+
try {
|
| 130 |
+
const timeoutPromise = new Promise<never>((_, reject) => {
|
| 131 |
+
setTimeout(() => reject(new Error("Read timeout")), timeout);
|
| 132 |
+
});
|
| 133 |
+
|
| 134 |
+
const readPromise = reader.read().then((result) => {
|
| 135 |
+
if (result.done || !result.value) {
|
| 136 |
+
throw new Error("Read failed - port closed or no data");
|
| 137 |
+
}
|
| 138 |
+
return result.value;
|
| 139 |
+
});
|
| 140 |
+
|
| 141 |
+
return await Promise.race([readPromise, timeoutPromise]);
|
| 142 |
+
} finally {
|
| 143 |
+
reader.releaseLock();
|
| 144 |
+
}
|
| 145 |
+
}
|
| 146 |
+
|
| 147 |
+
onStateChange(callback: (state: RobotConnectionState) => void): () => void {
|
| 148 |
+
this.stateChangeCallbacks.add(callback);
|
| 149 |
+
|
| 150 |
+
// Return unsubscribe function
|
| 151 |
+
return () => {
|
| 152 |
+
this.stateChangeCallbacks.delete(callback);
|
| 153 |
+
};
|
| 154 |
+
}
|
| 155 |
+
|
| 156 |
+
private notifyStateChange(): void {
|
| 157 |
+
this.stateChangeCallbacks.forEach((callback) => {
|
| 158 |
+
try {
|
| 159 |
+
callback(this.getState());
|
| 160 |
+
} catch (error) {
|
| 161 |
+
console.warn("Error in state change callback:", error);
|
| 162 |
+
}
|
| 163 |
+
});
|
| 164 |
+
}
|
| 165 |
+
}
|
| 166 |
+
|
| 167 |
+
// Singleton instance
|
| 168 |
+
const robotConnectionManager = new RobotConnectionManagerImpl();
|
| 169 |
+
|
| 170 |
+
/**
|
| 171 |
+
* Get the singleton robot connection manager
|
| 172 |
+
* This is the single source of truth for robot connections
|
| 173 |
+
*/
|
| 174 |
+
export function getRobotConnectionManager(): RobotConnectionManager {
|
| 175 |
+
return robotConnectionManager;
|
| 176 |
+
}
|
| 177 |
+
|
| 178 |
+
/**
|
| 179 |
+
* Utility functions for common robot operations
|
| 180 |
+
* Uses shared motor communication utilities for consistency
|
| 181 |
+
*/
|
| 182 |
+
|
| 183 |
+
/**
|
| 184 |
+
* Adapter to make robot connection manager compatible with motor utils
|
| 185 |
+
*/
|
| 186 |
+
class RobotConnectionManagerAdapter implements MotorCommunicationPort {
|
| 187 |
+
private manager: RobotConnectionManager;
|
| 188 |
+
|
| 189 |
+
constructor(manager: RobotConnectionManager) {
|
| 190 |
+
this.manager = manager;
|
| 191 |
+
}
|
| 192 |
+
|
| 193 |
+
async write(data: Uint8Array): Promise<void> {
|
| 194 |
+
return this.manager.writeData(data);
|
| 195 |
+
}
|
| 196 |
+
|
| 197 |
+
async read(timeout?: number): Promise<Uint8Array> {
|
| 198 |
+
return this.manager.readData(timeout);
|
| 199 |
+
}
|
| 200 |
+
}
|
| 201 |
+
|
| 202 |
+
export async function writeMotorPosition(
|
| 203 |
+
motorId: number,
|
| 204 |
+
position: number
|
| 205 |
+
): Promise<void> {
|
| 206 |
+
const manager = getRobotConnectionManager();
|
| 207 |
+
const adapter = new RobotConnectionManagerAdapter(manager);
|
| 208 |
+
|
| 209 |
+
return writeMotorPositionUtil(adapter, motorId, position);
|
| 210 |
+
}
|
| 211 |
+
|
| 212 |
+
export async function readMotorPosition(
|
| 213 |
+
motorId: number
|
| 214 |
+
): Promise<number | null> {
|
| 215 |
+
const manager = getRobotConnectionManager();
|
| 216 |
+
const adapter = new RobotConnectionManagerAdapter(manager);
|
| 217 |
+
|
| 218 |
+
return readMotorPositionUtil(adapter, motorId);
|
| 219 |
+
}
|
| 220 |
+
|
| 221 |
+
export async function readAllMotorPositions(
|
| 222 |
+
motorIds: number[]
|
| 223 |
+
): Promise<number[]> {
|
| 224 |
+
const manager = getRobotConnectionManager();
|
| 225 |
+
const adapter = new RobotConnectionManagerAdapter(manager);
|
| 226 |
+
|
| 227 |
+
return readAllMotorPositionsUtil(adapter, motorIds);
|
| 228 |
+
}
|
src/lerobot/web/teleoperate.ts
ADDED
|
@@ -0,0 +1,525 @@
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 1 |
+
/**
|
| 2 |
+
* Web teleoperation functionality using Web Serial API
|
| 3 |
+
* Mirrors the Node.js implementation but adapted for browser environment
|
| 4 |
+
*/
|
| 5 |
+
|
| 6 |
+
import type { UnifiedRobotData } from "../../demo/lib/unified-storage.js";
|
| 7 |
+
|
| 8 |
+
/**
|
| 9 |
+
* Motor position and limits for teleoperation
|
| 10 |
+
*/
|
| 11 |
+
export interface MotorConfig {
|
| 12 |
+
id: number;
|
| 13 |
+
name: string;
|
| 14 |
+
currentPosition: number;
|
| 15 |
+
minPosition: number;
|
| 16 |
+
maxPosition: number;
|
| 17 |
+
homePosition: number;
|
| 18 |
+
}
|
| 19 |
+
|
| 20 |
+
/**
|
| 21 |
+
* Teleoperation state
|
| 22 |
+
*/
|
| 23 |
+
export interface TeleoperationState {
|
| 24 |
+
isActive: boolean;
|
| 25 |
+
motorConfigs: MotorConfig[];
|
| 26 |
+
lastUpdate: number;
|
| 27 |
+
keyStates: { [key: string]: { pressed: boolean; timestamp: number } };
|
| 28 |
+
}
|
| 29 |
+
|
| 30 |
+
/**
|
| 31 |
+
* Keyboard control mapping (matches Node.js version)
|
| 32 |
+
*/
|
| 33 |
+
export const KEYBOARD_CONTROLS = {
|
| 34 |
+
// Shoulder controls
|
| 35 |
+
ArrowUp: { motor: "shoulder_lift", direction: 1, description: "Shoulder up" },
|
| 36 |
+
ArrowDown: {
|
| 37 |
+
motor: "shoulder_lift",
|
| 38 |
+
direction: -1,
|
| 39 |
+
description: "Shoulder down",
|
| 40 |
+
},
|
| 41 |
+
ArrowLeft: {
|
| 42 |
+
motor: "shoulder_pan",
|
| 43 |
+
direction: -1,
|
| 44 |
+
description: "Shoulder left",
|
| 45 |
+
},
|
| 46 |
+
ArrowRight: {
|
| 47 |
+
motor: "shoulder_pan",
|
| 48 |
+
direction: 1,
|
| 49 |
+
description: "Shoulder right",
|
| 50 |
+
},
|
| 51 |
+
|
| 52 |
+
// WASD controls
|
| 53 |
+
w: { motor: "elbow_flex", direction: 1, description: "Elbow flex" },
|
| 54 |
+
s: { motor: "elbow_flex", direction: -1, description: "Elbow extend" },
|
| 55 |
+
a: { motor: "wrist_flex", direction: -1, description: "Wrist down" },
|
| 56 |
+
d: { motor: "wrist_flex", direction: 1, description: "Wrist up" },
|
| 57 |
+
|
| 58 |
+
// Wrist roll and gripper
|
| 59 |
+
q: { motor: "wrist_roll", direction: -1, description: "Wrist roll left" },
|
| 60 |
+
e: { motor: "wrist_roll", direction: 1, description: "Wrist roll right" },
|
| 61 |
+
" ": { motor: "gripper", direction: 1, description: "Gripper toggle" },
|
| 62 |
+
|
| 63 |
+
// Emergency stop
|
| 64 |
+
Escape: {
|
| 65 |
+
motor: "emergency_stop",
|
| 66 |
+
direction: 0,
|
| 67 |
+
description: "Emergency stop",
|
| 68 |
+
},
|
| 69 |
+
} as const;
|
| 70 |
+
|
| 71 |
+
/**
|
| 72 |
+
* Web Serial Port wrapper for teleoperation
|
| 73 |
+
* Uses the same pattern as calibration - per-operation reader/writer access
|
| 74 |
+
*/
|
| 75 |
+
class WebTeleoperationPort {
|
| 76 |
+
private port: SerialPort;
|
| 77 |
+
|
| 78 |
+
constructor(port: SerialPort) {
|
| 79 |
+
this.port = port;
|
| 80 |
+
}
|
| 81 |
+
|
| 82 |
+
get isOpen(): boolean {
|
| 83 |
+
return (
|
| 84 |
+
this.port !== null &&
|
| 85 |
+
this.port.readable !== null &&
|
| 86 |
+
this.port.writable !== null
|
| 87 |
+
);
|
| 88 |
+
}
|
| 89 |
+
|
| 90 |
+
async initialize(): Promise<void> {
|
| 91 |
+
if (!this.port.readable || !this.port.writable) {
|
| 92 |
+
throw new Error("Port is not open for teleoperation");
|
| 93 |
+
}
|
| 94 |
+
// Port is already open and ready - no need to grab persistent readers/writers
|
| 95 |
+
}
|
| 96 |
+
|
| 97 |
+
async writeMotorPosition(
|
| 98 |
+
motorId: number,
|
| 99 |
+
position: number
|
| 100 |
+
): Promise<boolean> {
|
| 101 |
+
if (!this.port.writable) {
|
| 102 |
+
throw new Error("Port not open for writing");
|
| 103 |
+
}
|
| 104 |
+
|
| 105 |
+
try {
|
| 106 |
+
// STS3215 Write Goal_Position packet (matches Node.js exactly)
|
| 107 |
+
const packet = new Uint8Array([
|
| 108 |
+
0xff,
|
| 109 |
+
0xff, // Header
|
| 110 |
+
motorId, // Servo ID
|
| 111 |
+
0x05, // Length
|
| 112 |
+
0x03, // Instruction: WRITE_DATA
|
| 113 |
+
42, // Goal_Position register address
|
| 114 |
+
position & 0xff, // Position low byte
|
| 115 |
+
(position >> 8) & 0xff, // Position high byte
|
| 116 |
+
0x00, // Checksum placeholder
|
| 117 |
+
]);
|
| 118 |
+
|
| 119 |
+
// Calculate checksum
|
| 120 |
+
const checksum =
|
| 121 |
+
~(
|
| 122 |
+
motorId +
|
| 123 |
+
0x05 +
|
| 124 |
+
0x03 +
|
| 125 |
+
42 +
|
| 126 |
+
(position & 0xff) +
|
| 127 |
+
((position >> 8) & 0xff)
|
| 128 |
+
) & 0xff;
|
| 129 |
+
packet[8] = checksum;
|
| 130 |
+
|
| 131 |
+
// Use per-operation writer like calibration does
|
| 132 |
+
const writer = this.port.writable.getWriter();
|
| 133 |
+
try {
|
| 134 |
+
await writer.write(packet);
|
| 135 |
+
return true;
|
| 136 |
+
} finally {
|
| 137 |
+
writer.releaseLock();
|
| 138 |
+
}
|
| 139 |
+
} catch (error) {
|
| 140 |
+
console.warn(`Failed to write motor ${motorId} position:`, error);
|
| 141 |
+
return false;
|
| 142 |
+
}
|
| 143 |
+
}
|
| 144 |
+
|
| 145 |
+
async readMotorPosition(motorId: number): Promise<number | null> {
|
| 146 |
+
if (!this.port.writable || !this.port.readable) {
|
| 147 |
+
throw new Error("Port not open for reading/writing");
|
| 148 |
+
}
|
| 149 |
+
|
| 150 |
+
const writer = this.port.writable.getWriter();
|
| 151 |
+
const reader = this.port.readable.getReader();
|
| 152 |
+
|
| 153 |
+
try {
|
| 154 |
+
// STS3215 Read Present_Position packet
|
| 155 |
+
const packet = new Uint8Array([
|
| 156 |
+
0xff,
|
| 157 |
+
0xff, // Header
|
| 158 |
+
motorId, // Servo ID
|
| 159 |
+
0x04, // Length
|
| 160 |
+
0x02, // Instruction: READ_DATA
|
| 161 |
+
56, // Present_Position register address
|
| 162 |
+
0x02, // Data length (2 bytes)
|
| 163 |
+
0x00, // Checksum placeholder
|
| 164 |
+
]);
|
| 165 |
+
|
| 166 |
+
const checksum = ~(motorId + 0x04 + 0x02 + 56 + 0x02) & 0xff;
|
| 167 |
+
packet[7] = checksum;
|
| 168 |
+
|
| 169 |
+
// Clear buffer first
|
| 170 |
+
try {
|
| 171 |
+
const { value, done } = await reader.read();
|
| 172 |
+
if (done) return null;
|
| 173 |
+
} catch (e) {
|
| 174 |
+
// Buffer was empty, continue
|
| 175 |
+
}
|
| 176 |
+
|
| 177 |
+
await writer.write(packet);
|
| 178 |
+
await new Promise((resolve) => setTimeout(resolve, 10));
|
| 179 |
+
|
| 180 |
+
const { value: response, done } = await reader.read();
|
| 181 |
+
if (done || !response || response.length < 7) {
|
| 182 |
+
return null;
|
| 183 |
+
}
|
| 184 |
+
|
| 185 |
+
const id = response[2];
|
| 186 |
+
const error = response[4];
|
| 187 |
+
|
| 188 |
+
if (id === motorId && error === 0) {
|
| 189 |
+
return response[5] | (response[6] << 8);
|
| 190 |
+
}
|
| 191 |
+
|
| 192 |
+
return null;
|
| 193 |
+
} catch (error) {
|
| 194 |
+
console.warn(`Failed to read motor ${motorId} position:`, error);
|
| 195 |
+
return null;
|
| 196 |
+
} finally {
|
| 197 |
+
reader.releaseLock();
|
| 198 |
+
writer.releaseLock();
|
| 199 |
+
}
|
| 200 |
+
}
|
| 201 |
+
|
| 202 |
+
async disconnect(): Promise<void> {
|
| 203 |
+
// Don't close the port itself - just cleanup wrapper
|
| 204 |
+
// The port is managed by PortManager
|
| 205 |
+
}
|
| 206 |
+
}
|
| 207 |
+
|
| 208 |
+
/**
|
| 209 |
+
* Load calibration data from unified storage with fallback to defaults
|
| 210 |
+
* Improved version that properly loads and applies calibration ranges
|
| 211 |
+
*/
|
| 212 |
+
export function loadCalibrationConfig(serialNumber: string): MotorConfig[] {
|
| 213 |
+
// Default SO-100 configuration (matches Node.js defaults)
|
| 214 |
+
const defaultConfigs: MotorConfig[] = [
|
| 215 |
+
{
|
| 216 |
+
id: 1,
|
| 217 |
+
name: "shoulder_pan",
|
| 218 |
+
currentPosition: 2048,
|
| 219 |
+
minPosition: 1024,
|
| 220 |
+
maxPosition: 3072,
|
| 221 |
+
homePosition: 2048,
|
| 222 |
+
},
|
| 223 |
+
{
|
| 224 |
+
id: 2,
|
| 225 |
+
name: "shoulder_lift",
|
| 226 |
+
currentPosition: 2048,
|
| 227 |
+
minPosition: 1024,
|
| 228 |
+
maxPosition: 3072,
|
| 229 |
+
homePosition: 2048,
|
| 230 |
+
},
|
| 231 |
+
{
|
| 232 |
+
id: 3,
|
| 233 |
+
name: "elbow_flex",
|
| 234 |
+
currentPosition: 2048,
|
| 235 |
+
minPosition: 1024,
|
| 236 |
+
maxPosition: 3072,
|
| 237 |
+
homePosition: 2048,
|
| 238 |
+
},
|
| 239 |
+
{
|
| 240 |
+
id: 4,
|
| 241 |
+
name: "wrist_flex",
|
| 242 |
+
currentPosition: 2048,
|
| 243 |
+
minPosition: 1024,
|
| 244 |
+
maxPosition: 3072,
|
| 245 |
+
homePosition: 2048,
|
| 246 |
+
},
|
| 247 |
+
{
|
| 248 |
+
id: 5,
|
| 249 |
+
name: "wrist_roll",
|
| 250 |
+
currentPosition: 2048,
|
| 251 |
+
minPosition: 1024,
|
| 252 |
+
maxPosition: 3072,
|
| 253 |
+
homePosition: 2048,
|
| 254 |
+
},
|
| 255 |
+
{
|
| 256 |
+
id: 6,
|
| 257 |
+
name: "gripper",
|
| 258 |
+
currentPosition: 2048,
|
| 259 |
+
minPosition: 1024,
|
| 260 |
+
maxPosition: 3072,
|
| 261 |
+
homePosition: 2048,
|
| 262 |
+
},
|
| 263 |
+
];
|
| 264 |
+
|
| 265 |
+
try {
|
| 266 |
+
// Load from unified storage
|
| 267 |
+
const unifiedKey = `lerobotjs-${serialNumber}`;
|
| 268 |
+
const unifiedDataRaw = localStorage.getItem(unifiedKey);
|
| 269 |
+
|
| 270 |
+
if (!unifiedDataRaw) {
|
| 271 |
+
console.log(
|
| 272 |
+
`No calibration data found for ${serialNumber}, using defaults`
|
| 273 |
+
);
|
| 274 |
+
return defaultConfigs;
|
| 275 |
+
}
|
| 276 |
+
|
| 277 |
+
const unifiedData: UnifiedRobotData = JSON.parse(unifiedDataRaw);
|
| 278 |
+
|
| 279 |
+
if (!unifiedData.calibration) {
|
| 280 |
+
console.log(
|
| 281 |
+
`No calibration in unified data for ${serialNumber}, using defaults`
|
| 282 |
+
);
|
| 283 |
+
return defaultConfigs;
|
| 284 |
+
}
|
| 285 |
+
|
| 286 |
+
// Map calibration data to motor configs
|
| 287 |
+
const calibratedConfigs: MotorConfig[] = defaultConfigs.map(
|
| 288 |
+
(defaultConfig) => {
|
| 289 |
+
const calibData = (unifiedData.calibration as any)?.[
|
| 290 |
+
defaultConfig.name
|
| 291 |
+
];
|
| 292 |
+
|
| 293 |
+
if (
|
| 294 |
+
calibData &&
|
| 295 |
+
typeof calibData === "object" &&
|
| 296 |
+
"id" in calibData &&
|
| 297 |
+
"range_min" in calibData &&
|
| 298 |
+
"range_max" in calibData
|
| 299 |
+
) {
|
| 300 |
+
// Use calibrated values but keep current position as default
|
| 301 |
+
return {
|
| 302 |
+
...defaultConfig,
|
| 303 |
+
id: calibData.id,
|
| 304 |
+
minPosition: calibData.range_min,
|
| 305 |
+
maxPosition: calibData.range_max,
|
| 306 |
+
homePosition: Math.floor(
|
| 307 |
+
(calibData.range_min + calibData.range_max) / 2
|
| 308 |
+
),
|
| 309 |
+
};
|
| 310 |
+
}
|
| 311 |
+
|
| 312 |
+
return defaultConfig;
|
| 313 |
+
}
|
| 314 |
+
);
|
| 315 |
+
|
| 316 |
+
console.log(`✅ Loaded calibration data for ${serialNumber}`);
|
| 317 |
+
return calibratedConfigs;
|
| 318 |
+
} catch (error) {
|
| 319 |
+
console.warn(`Failed to load calibration for ${serialNumber}:`, error);
|
| 320 |
+
return defaultConfigs;
|
| 321 |
+
}
|
| 322 |
+
}
|
| 323 |
+
|
| 324 |
+
/**
|
| 325 |
+
* Web teleoperation controller
|
| 326 |
+
*/
|
| 327 |
+
export class WebTeleoperationController {
|
| 328 |
+
private port: WebTeleoperationPort;
|
| 329 |
+
private motorConfigs: MotorConfig[] = [];
|
| 330 |
+
private isActive: boolean = false;
|
| 331 |
+
private updateInterval: NodeJS.Timeout | null = null;
|
| 332 |
+
private keyStates: {
|
| 333 |
+
[key: string]: { pressed: boolean; timestamp: number };
|
| 334 |
+
} = {};
|
| 335 |
+
|
| 336 |
+
// Movement parameters (matches Node.js)
|
| 337 |
+
private readonly STEP_SIZE = 8;
|
| 338 |
+
private readonly UPDATE_RATE = 60; // 60 FPS
|
| 339 |
+
private readonly KEY_TIMEOUT = 100; // ms
|
| 340 |
+
|
| 341 |
+
constructor(port: SerialPort, serialNumber: string) {
|
| 342 |
+
this.port = new WebTeleoperationPort(port);
|
| 343 |
+
this.motorConfigs = loadCalibrationConfig(serialNumber);
|
| 344 |
+
}
|
| 345 |
+
|
| 346 |
+
async initialize(): Promise<void> {
|
| 347 |
+
await this.port.initialize();
|
| 348 |
+
|
| 349 |
+
// Read current positions
|
| 350 |
+
for (const config of this.motorConfigs) {
|
| 351 |
+
const position = await this.port.readMotorPosition(config.id);
|
| 352 |
+
if (position !== null) {
|
| 353 |
+
config.currentPosition = position;
|
| 354 |
+
}
|
| 355 |
+
}
|
| 356 |
+
}
|
| 357 |
+
|
| 358 |
+
getMotorConfigs(): MotorConfig[] {
|
| 359 |
+
return [...this.motorConfigs];
|
| 360 |
+
}
|
| 361 |
+
|
| 362 |
+
getState(): TeleoperationState {
|
| 363 |
+
return {
|
| 364 |
+
isActive: this.isActive,
|
| 365 |
+
motorConfigs: [...this.motorConfigs],
|
| 366 |
+
lastUpdate: Date.now(),
|
| 367 |
+
keyStates: { ...this.keyStates },
|
| 368 |
+
};
|
| 369 |
+
}
|
| 370 |
+
|
| 371 |
+
updateKeyState(key: string, pressed: boolean): void {
|
| 372 |
+
this.keyStates[key] = {
|
| 373 |
+
pressed,
|
| 374 |
+
timestamp: Date.now(),
|
| 375 |
+
};
|
| 376 |
+
}
|
| 377 |
+
|
| 378 |
+
start(): void {
|
| 379 |
+
if (this.isActive) return;
|
| 380 |
+
|
| 381 |
+
this.isActive = true;
|
| 382 |
+
this.updateInterval = setInterval(() => {
|
| 383 |
+
this.updateMotorPositions();
|
| 384 |
+
}, 1000 / this.UPDATE_RATE);
|
| 385 |
+
|
| 386 |
+
console.log("🎮 Web teleoperation started");
|
| 387 |
+
}
|
| 388 |
+
|
| 389 |
+
stop(): void {
|
| 390 |
+
if (!this.isActive) return;
|
| 391 |
+
|
| 392 |
+
this.isActive = false;
|
| 393 |
+
|
| 394 |
+
if (this.updateInterval) {
|
| 395 |
+
clearInterval(this.updateInterval);
|
| 396 |
+
this.updateInterval = null;
|
| 397 |
+
}
|
| 398 |
+
|
| 399 |
+
// Clear all key states
|
| 400 |
+
this.keyStates = {};
|
| 401 |
+
|
| 402 |
+
console.log("⏹️ Web teleoperation stopped");
|
| 403 |
+
}
|
| 404 |
+
|
| 405 |
+
async disconnect(): Promise<void> {
|
| 406 |
+
this.stop();
|
| 407 |
+
await this.port.disconnect();
|
| 408 |
+
}
|
| 409 |
+
|
| 410 |
+
private updateMotorPositions(): void {
|
| 411 |
+
const now = Date.now();
|
| 412 |
+
|
| 413 |
+
// Clear timed-out keys
|
| 414 |
+
Object.keys(this.keyStates).forEach((key) => {
|
| 415 |
+
if (now - this.keyStates[key].timestamp > this.KEY_TIMEOUT) {
|
| 416 |
+
delete this.keyStates[key];
|
| 417 |
+
}
|
| 418 |
+
});
|
| 419 |
+
|
| 420 |
+
// Process active keys
|
| 421 |
+
const activeKeys = Object.keys(this.keyStates).filter(
|
| 422 |
+
(key) =>
|
| 423 |
+
this.keyStates[key].pressed &&
|
| 424 |
+
now - this.keyStates[key].timestamp <= this.KEY_TIMEOUT
|
| 425 |
+
);
|
| 426 |
+
|
| 427 |
+
// Emergency stop check
|
| 428 |
+
if (activeKeys.includes("Escape")) {
|
| 429 |
+
this.stop();
|
| 430 |
+
return;
|
| 431 |
+
}
|
| 432 |
+
|
| 433 |
+
// Calculate target positions based on active keys
|
| 434 |
+
const targetPositions: { [motorName: string]: number } = {};
|
| 435 |
+
|
| 436 |
+
for (const key of activeKeys) {
|
| 437 |
+
const control = KEYBOARD_CONTROLS[key as keyof typeof KEYBOARD_CONTROLS];
|
| 438 |
+
if (!control || control.motor === "emergency_stop") continue;
|
| 439 |
+
|
| 440 |
+
const motorConfig = this.motorConfigs.find(
|
| 441 |
+
(m) => m.name === control.motor
|
| 442 |
+
);
|
| 443 |
+
if (!motorConfig) continue;
|
| 444 |
+
|
| 445 |
+
// Calculate new position
|
| 446 |
+
const currentTarget =
|
| 447 |
+
targetPositions[motorConfig.name] ?? motorConfig.currentPosition;
|
| 448 |
+
const newPosition = currentTarget + control.direction * this.STEP_SIZE;
|
| 449 |
+
|
| 450 |
+
// Apply limits
|
| 451 |
+
targetPositions[motorConfig.name] = Math.max(
|
| 452 |
+
motorConfig.minPosition,
|
| 453 |
+
Math.min(motorConfig.maxPosition, newPosition)
|
| 454 |
+
);
|
| 455 |
+
}
|
| 456 |
+
|
| 457 |
+
// Send motor commands
|
| 458 |
+
Object.entries(targetPositions).forEach(([motorName, targetPosition]) => {
|
| 459 |
+
const motorConfig = this.motorConfigs.find((m) => m.name === motorName);
|
| 460 |
+
if (motorConfig && targetPosition !== motorConfig.currentPosition) {
|
| 461 |
+
this.port
|
| 462 |
+
.writeMotorPosition(motorConfig.id, Math.round(targetPosition))
|
| 463 |
+
.then((success) => {
|
| 464 |
+
if (success) {
|
| 465 |
+
motorConfig.currentPosition = targetPosition;
|
| 466 |
+
}
|
| 467 |
+
});
|
| 468 |
+
}
|
| 469 |
+
});
|
| 470 |
+
}
|
| 471 |
+
|
| 472 |
+
// Programmatic control methods
|
| 473 |
+
async moveMotor(motorName: string, targetPosition: number): Promise<boolean> {
|
| 474 |
+
const motorConfig = this.motorConfigs.find((m) => m.name === motorName);
|
| 475 |
+
if (!motorConfig) return false;
|
| 476 |
+
|
| 477 |
+
const clampedPosition = Math.max(
|
| 478 |
+
motorConfig.minPosition,
|
| 479 |
+
Math.min(motorConfig.maxPosition, targetPosition)
|
| 480 |
+
);
|
| 481 |
+
|
| 482 |
+
const success = await this.port.writeMotorPosition(
|
| 483 |
+
motorConfig.id,
|
| 484 |
+
Math.round(clampedPosition)
|
| 485 |
+
);
|
| 486 |
+
if (success) {
|
| 487 |
+
motorConfig.currentPosition = clampedPosition;
|
| 488 |
+
}
|
| 489 |
+
|
| 490 |
+
return success;
|
| 491 |
+
}
|
| 492 |
+
|
| 493 |
+
async setMotorPositions(positions: {
|
| 494 |
+
[motorName: string]: number;
|
| 495 |
+
}): Promise<boolean> {
|
| 496 |
+
const results = await Promise.all(
|
| 497 |
+
Object.entries(positions).map(([motorName, position]) =>
|
| 498 |
+
this.moveMotor(motorName, position)
|
| 499 |
+
)
|
| 500 |
+
);
|
| 501 |
+
|
| 502 |
+
return results.every((result) => result);
|
| 503 |
+
}
|
| 504 |
+
|
| 505 |
+
async goToHomePosition(): Promise<boolean> {
|
| 506 |
+
const homePositions = this.motorConfigs.reduce((acc, config) => {
|
| 507 |
+
acc[config.name] = config.homePosition;
|
| 508 |
+
return acc;
|
| 509 |
+
}, {} as { [motorName: string]: number });
|
| 510 |
+
|
| 511 |
+
return this.setMotorPositions(homePositions);
|
| 512 |
+
}
|
| 513 |
+
}
|
| 514 |
+
|
| 515 |
+
/**
|
| 516 |
+
* Create teleoperation controller for connected robot
|
| 517 |
+
*/
|
| 518 |
+
export async function createWebTeleoperationController(
|
| 519 |
+
port: SerialPort,
|
| 520 |
+
serialNumber: string
|
| 521 |
+
): Promise<WebTeleoperationController> {
|
| 522 |
+
const controller = new WebTeleoperationController(port, serialNumber);
|
| 523 |
+
await controller.initialize();
|
| 524 |
+
return controller;
|
| 525 |
+
}
|