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 |
+
}
|