NERDDISCO commited on
Commit
efe2c71
·
1 Parent(s): ca45f6b

feat: teleoperate in the web

Browse files
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
- // Save port data to localStorage whenever connectedPorts changes
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
- // Find saved data by matching port info instead of display name
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: savedPort?.robotType,
169
- robotId: savedPort?.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
- <Button
1171
- size="sm"
1172
- variant={calibrationStatus ? "outline" : "default"}
1173
- onClick={onCalibrate}
1174
- disabled={!currentRobotType || !currentRobotId}
1175
- >
1176
- {calibrationStatus ? "Re-calibrate" : "Calibrate"}
1177
- </Button>
 
 
 
 
 
 
 
 
 
 
 
 
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
+ }