Public: anyone can read this; the crew can change it.

Migrated from github.com/Trevato/SO-ARM100

workbenchthis machine

Make arm run here #2

Merged opened this in answer to

Check · Review · Land done · Live — Landed; nothing deploys from this branch, so this is the end of its path.

Merged

quill/arm:quill/1-make-arm-run-here → main 5 commits fork

  • Merged. quill/1-make-arm-run-here was merged into main as abb2ad9.

Review

Sign in to review this change.
57 files changed +10202 −1
.envrc added +1
@@ -0,0 +1,1 @@
1 use flake
.gitignore +7 −1
@@ -1,1 +1,7 @@
1 .DS_Store↵
1 .DS_Store
2 .direnv/
3 .venv/
4 node_modules/
5 poses.json
6 outputs/
7 __pycache__/
README.md +1
@@ -28,6 +28,7 @@ These arms are designed to work seamlessly with the open‑source 🤗 LeRobot l
2828
2929 ### Documentation 📖
3030 - For the SO‑101 docs, follow this page further.
31 - Software to drive what you built: [`Software/README.md`](Software/README.md): a Nix dev shell with LeRobot, a TypeScript servo driver and CLI, a browser app with a live 3D model, a safe zone and teleoperation over the network, and a camera-guided pick (`Software/vision/QUEST.md`).
3132 - Here you can find the [SO‑100 docs](SO100.md) which is deprecated.
3233
3334 ### Getting Your Own SO‑101
Software/README.md added +273
@@ -0,0 +1,273 @@
1 # SO-101 workbench
2
3 Software for building, driving and teaching an
4 [SO-101](https://github.com/TheRobotStudio/SO-ARM100) robot arm: the open
5 hardware by The Robot Studio, run with [Hugging Face LeRobot](https://huggingface.co/docs/lerobot/so101).
6 Everything here was built on one real pair of arms, a leader and a follower, in
7 the order a first build happens: one servo on a bench, ids and gear ratios, a
8 calibrated follower, poses and replay, the leader, teleoperation, the follower
9 on a Raspberry Pi behind a browser app with a live 3D model and a safe zone,
10 and a camera-guided pick. Build the arms from the hardware docs in the
11 repository root, then start here.
12
13 **What you need.** The SO-101 kit (both arms, two Waveshare bus servo boards,
14 their power supplies); a computer with Nix (macOS or Linux); optionally a
15 Raspberry Pi 4 for the follower, one or two USB webcams, and a printer for the
16 calibration board. Nothing here needs a GPU.
17
18 **The short path.**
19
20 1. `direnv allow` in the checkout: the dev shell brings Python, LeRobot, Bun
21 and Node, and syncs them on every entry.
22 2. `just scan` with one servo plugged in; `so101 set-id` and the bench scripts
23 in `so101/bench/` to id a chain; `just calibrate` for each arm.
24 3. `just teleop <follower port> <leader port>` to drive the follower from the
25 leader on one machine.
26 4. For a remote follower: the Pi section below, then open its page in a
27 browser.
28 5. For the camera-guided pick: `vision/QUEST.md`.
29
30 Everything here is under the repository's Apache-2.0 license. Issues and pull
31 requests are welcome.
32
33 | Piece | What it is |
34 | --- | --- |
35 | `flake.nix` + `.envrc` | Nix dev shell via direnv: Python 3.12, uv, Bun, Node 24, FFmpeg 7, just |
36 | `pyproject.toml` + `uv.lock` | LeRobot with the Feetech extra, in a uv-managed `.venv` |
37 | `justfile` | The bring-up recipes below, `just` lists them |
38 | `vision/` | ArUco pick: printable markers, hands-free camera calibration, hand-eye calibration, a live viewer, and the pick itself. Start with its `QUEST.md` |
39 | `so101/` | A small TypeScript driver + CLI that speaks the Feetech bus protocol directly. Bun installs and tests it; the CLI itself runs on Node because `serialport`'s native bindings crash under Bun ([bun#18546](https://github.com/oven-sh/bun/issues/18546)) |
40 | `WEBUI_CALIBRATION.md` | Guide for the browser-based 3-point calibration studio |
41
42 ## First entry
43
44 ```sh
45 direnv allow # once; the hook then syncs .venv and node_modules on every entry
46 just # list recipes
47 ```
48
49 The first shell entry downloads the LeRobot wheels (torch is most of it).
50 Later entries are instant. Editing `pyproject.toml` is followed by `just sync`.
51
52 ## Plugging in a servo
53
54 The Waveshare bus servo board enumerates on macOS as `/dev/cu.usbmodem…` with
55 no driver; the LeRobot guide notes its two jumpers must sit on the USB (B)
56 channel. The servos only answer when the board also has its barrel-jack
57 supply connected: USB powers the adapter, not the motors. STS3215s ship at
58 id 1 and 1 Mbaud, which is the only baud rate anything here uses.
59
60 ```sh
61 just ports # find the board
62 just scan # who answers: id, firmware, baud, position
63 so101 watch --ids 1 # turn the horn by hand, watch ticks and degrees
64 so101 move 1 180 --deg # drive it, gently (500 ticks/s)
65 so101 status --ids 1 # load, current, voltage, temperature, error bits
66 so101 read 1 Homing_Offset # any register by its LeRobot name
67 just release # torque off everything
68 ```
69
70 If `ports` shows the board but `scan` finds nothing at any baud rate, look at
71 the USB link before the wiring: on this machine the first plug-in produced
72 thousands of `transaction error` lines per second in `log stream --predicate
73 'eventMessage CONTAINS "USB Single Serial"'`, and a replug cleared it. A
74 cable that is marginal for data shows the same symptom.
75
76 Once the arm is assembled and calibrated, move it by demonstration rather
77 than by joint numbers. Per-joint limits do not stop links from colliding with
78 each other or the table, and a stalled STS3215 strips its gears before its
79 overload protection reacts; the follower's first elbow lost its gearbox that
80 way. Record a path with your hands and replay it:
81
82 ```sh
83 so101 pose save home # torque off, pose the arm by hand, name it (poses.json)
84 so101 pose save reach
85 so101 pose go reach # every joint moves at once and arrives together
86 so101 pose go home --via reach --release # route through a waypoint; let go at the end
87 so101 record path.json # torque off; move the arm; ctrl-c saves the path
88 so101 play path.json # eases to the first pose, then replays at recorded speed
89 ```
90
91 Every supervised move (`pose go`, `play`) caps torque at 70 % (`--torque`),
92 aims 30 ticks inside each servo's calibrated limits so it never drives into
93 a hard stop, and watches load and progress: a joint that pushes hard without
94 moving stops all of them where they are and names itself. Ctrl-C does the
95 same. Torque stays on afterward, so the arm holds; `so101 torque off`
96 releases it, which drops an extended arm, so go home first.
97
98 `so101 set-id 1 3` re-addresses the one servo on the bus. That is the manual
99 version of what `just setup-motors` does, which also walks the six joints in
100 LeRobot's order. Every servo ships as id 1, so assign ids from the top down,
101 gripper (6) first, and each new servo can join the chain without colliding
102 with one already renamed. That is why LeRobot asks for the gripper first.
103
104 On real hardware a six-servo sync read completes in about 1 ms, so `watch`
105 and LeRobot's teleop loop are limited by their own frame rate, not the bus.
106
107 Auto-detection only works with a single adapter plugged in. Exporting
108 `SO101_PORT` pins the port for the `so101` commands and the LeRobot recipes
109 alike:
110
111 ```sh
112 export SO101_PORT=/dev/cu.usbmodemXXXX
113 ```
114
115 ## Remote: the follower on a Raspberry Pi
116
117 `so101 serve` runs where the follower's board is plugged in and exposes the arm
118 on the network; `so101 leader` runs where the leader is and streams targets to
119 it. Both are the same CLI, so the Pi needs this repo, Node 24 and `bun install`
120 in `Software/so101` (or Nix: the flake has an `aarch64-linux` dev shell).
121
122 ```sh
123 # on the Pi (follower board is /dev/ttyACM0)
124 SO101_PORT=/dev/ttyACM0 so101 serve --listen 0.0.0.0:8101 --file poses.json
125
126 # on the Mac (leader board)
127 SO101_PORT=<leader port> so101 leader --to ws://<pi>:8101
128 ```
129
130 The API, all JSON (the full list with bodies is at the top of
131 `so101/src/server.ts`): `GET /status`, `/limits`, `/poses`, `/recordings`,
132 `/calibration`; `POST /pose/save`, `/pose/delete`, `/pose/go`, `/move`
133 (explicit `{"targets": {"3": 2048}}`), `/grip`, `/torque`, `/stop`,
134 `/record/start`, `/record/stop`, `/recording/delete`, `/play`;
135 `PUT /calibration/camera|handeye`; and the WebSocket `/follow` taking
136 `{"1": ticks, ...}` frames. One thing drives the arm at a time. Every move
137 goes through the joint supervisor; `/stop` ends a move, a playback, a
138 recording, or a stream, and the arm holds. The stream is clamped to the
139 follower's calibrated limits, eases over to the first frame, drops jumps no
140 hand makes, and holds the arm after half a second of silence. There is no
141 authentication: keep it on a LAN or behind Tailscale.
142
143 `GET /` is the browser app: up to two camera panes, a live 3D model of the
144 arm (its own URDF and meshes, drag a joint and the arm follows), a slider and
145 hold-to-jog buttons per joint that stream targets like a leader does, a
146 button per saved pose plus save and delete, hold and release, record and
147 play, a stop button (space bar), fullscreen (F), a metrics panel, a settings
148 panel for the safe zone, and a 3D-calibration panel that explains the vision
149 quest and shows its status. `bench/fake-serve.ts` runs the same server on a
150 simulated bus for working on the page. The page expects MJPEG streams on
151 ports 8080 and 8081 of the same host (`?stream=<url>` overrides the first).
152
153 On the forge (`forge.json` at the repository root), the app is that same
154 simulated arm: no servo board is attached, so every move arrives at once. It
155 reads `PORT` and `HOST`, and keeps poses, recordings, settings and metrics in
156 `$FORGE_APP_DATA/so101` in place of a temp dir. Run it the same way locally
157 with `PORT=8102 FORGE_APP_DATA=./data bun run Software/so101/bench/fake-serve.ts`.
158
159 **The safe zone.** The server loads `Simulation/SO101/so101_new_calib.urdf`
160 and its meshes (`--model <dir>`, or the repo layout, or `model/` beside the
161 package) and runs every target through the arm's geometry: a pose that puts
162 any part below the table or into the base is a violation. `enforce` refuses
163 moves and holds a stream at the boundary until the leader backs off; `warn`
164 only reports; `off` skips the check. Margins and per-joint trims live in
165 `settings.json` beside the poses. The model takes the middle of each
166 calibrated range as zero (LeRobot's convention), which can sit a centimetre
167 or two off the real arm at the gripper: compare the 3D view with the camera
168 and trim. On a stream, a joint that pushes hard without moving is a jam: it
169 is parked where it is and follows again once the leader backs off. Jams,
170 blocks, stalls, peak loads and temperatures go to `metrics.jsonl`.
171
172 On the Pi, camera 1 is pinned by its `/dev/v4l/by-id` name in
173 `/etc/so101/camera1` and served by `[email protected]` on port 8080;
174 `[email protected]` serves the first other camera on 8081 as soon as it
175 is plugged in. To set one up by hand:
176
177 ```sh
178 sudo apt-get install ustreamer
179 ustreamer --device=/dev/v4l/by-id/<camera>-video-index0 --host=0.0.0.0 --port=8080 \
180 --resolution=1280x720 --format=MJPEG --desired-fps=20 --allow-origin='*'
181 ```
182
183 `--allow-origin` lets the viewer read the stream from another port. Chrome
184 reads the stream in a script (frame count, 3 s stall watchdog); Safari and
185 the iPhone show it as a plain image and reconnect when the picture stops
186 changing. On an iPhone, "Add to Home Screen" gives the fullscreen view.
187
188 The vision kit in `Software/vision` works against this server and stream:
189 `SO101_ARM=http://<pi>:8101 SO101_CAMERA=http://<pi>:8080/stream`, then the
190 `just vision-*` recipes. See `Software/vision/QUEST.md`.
191
192 ## LeRobot flow
193
194 Follows the [SO-101 guide](https://huggingface.co/docs/lerobot/so101). The
195 recipes default to the follower; pass `leader` for the other arm.
196
197 ```sh
198 just find-port # interactive unplug/replug port discovery
199 just setup-motors # follower: ids 1-6, one servo at a time
200 just setup-motors leader
201 just calibrate # saved as so101_follower
202 just calibrate leader # saved as so101_leader
203 just teleop /dev/cu.usbmodemAAA /dev/cu.usbmodemBBB # follower port, leader port
204 ```
205
206 Calibration lands in `~/.cache/huggingface/lerobot/calibration/`, under
207 `robots/so_follower/` and `teleoperators/so_leader/`, named by id. Recording
208 datasets needs the `dataset` extra as well: add it to `pyproject.toml` and
209 `just sync`. Its video encoder, TorchCodec, loads FFmpeg 7 at import time,
210 which is why the shell carries that major version rather than the newest.
211
212 ## Joint map
213
214 Both arms use the same ids. The follower runs 1/345 gearing throughout; the
215 leader mixes ratios so it back-drives easily. `lerobot-setup-motors` asks for
216 the gripper first and then walks down to id 1, one servo on the bus at a time.
217
218 | id | joint | leader gear |
219 | --- | --- | --- |
220 | 1 | shoulder_pan | 1/191 |
221 | 2 | shoulder_lift | 1/345 |
222 | 3 | elbow_flex | 1/191 |
223 | 4 | wrist_flex | 1/147 |
224 | 5 | wrist_roll | 1/147 |
225 | 6 | gripper | 1/147 |
226
227 ## References
228
229 The methods behind the vision kit, so you can read the originals:
230
231 - **Camera calibration.** Z. Zhang, *A Flexible New Technique for Camera
232 Calibration*, IEEE TPAMI 22(11), 2000. [doi:10.1109/34.888718](https://doi.org/10.1109/34.888718).
233 OpenCV's `calibrateCamera` implements it; the ChArUco flow is in the
234 [Calibration with ArUco and ChArUco](https://docs.opencv.org/4.x/da/d13/tutorial_aruco_calibration.html) tutorial.
235 - **ArUco markers and ChArUco boards.** S. Garrido-Jurado, R. Muñoz-Salinas,
236 F. J. Madrid-Cuevas, M. J. Marín-Jiménez, *Automatic generation and detection
237 of highly reliable fiducial markers under occlusion*, Pattern Recognition
238 47(6), 2014. [doi:10.1016/j.patcog.2014.01.005](https://doi.org/10.1016/j.patcog.2014.01.005).
239 OpenCV: [Detection of ArUco markers](https://docs.opencv.org/4.x/d5/dae/tutorial_aruco_detection.html).
240 - **Pose of one square marker (PnP).** T. Collins, A. Bartoli, *Infinitesimal
241 Plane-Based Pose Estimation*, IJCV 109(3), 2014.
242 [doi:10.1007/s11263-014-0725-5](https://doi.org/10.1007/s11263-014-0725-5).
243 This is `SOLVEPNP_IPPE_SQUARE`; it returns the two planar solutions, and the
244 kit keeps the clearly better one (`ambiguity`).
245 - **Hand-eye calibration, the classic forms (AX = XB).** R. Y. Tsai, R. K. Lenz,
246 *A new technique for fully autonomous and efficient 3D robotics hand/eye
247 calibration*, IEEE T-RA 5(3), 1989. [doi:10.1109/70.34770](https://doi.org/10.1109/70.34770).
248 F. C. Park, B. J. Martin, *Robot sensor calibration: solving AX = XB on the
249 Euclidean group*, IEEE T-RA 10(5), 1994. [doi:10.1109/70.326576](https://doi.org/10.1109/70.326576).
250 K. Daniilidis, *Hand-Eye Calibration Using Dual Quaternions*, IJRR 18(3), 1999.
251 [doi:10.1177/02783649922066213](https://doi.org/10.1177/02783649922066213).
252 - **Robot-world / hand-eye in one solve (AX = ZB), what the kit uses.**
253 M. Shah, *Solving the Robot-World/Hand-Eye Calibration Problem Using the
254 Kronecker Product*, ASME J. Mechanisms and Robotics 5(3), 2013.
255 [doi:10.1115/1.4024473](https://doi.org/10.1115/1.4024473). A. Li, L. Wang,
256 D. Wu, *Simultaneous robot-world and hand-eye calibration using
257 dual-quaternions and Kronecker product*, Int. J. Physical Sciences 5(10), 2010.
258 OpenCV: `calibrateRobotWorldHandEye` and `calibrateHandEye` in the
259 [calib3d module](https://docs.opencv.org/4.x/d9/d0c/group__calib3d.html).
260 - **Two cameras, a full orientation.** R. Hartley, A. Zisserman, *Multiple View
261 Geometry in Computer Vision*, 2nd ed., Cambridge University Press, 2004
262 (epipolar geometry, `stereoCalibrate`; triangulation, `triangulatePoints`).
263 - **The arm's model.** The URDF and meshes in `Simulation/SO101` from this
264 repository; LeRobot's `RobotKinematics` runs them through
265 [placo](https://github.com/Rhoban/placo) for the IK; the TypeScript server
266 reads the same files for its own forward kinematics and the safe zone.
267 - **The robot and the stack.** [SO-ARM100 / SO-101](https://github.com/TheRobotStudio/SO-ARM100)
268 by The Robot Studio; [Hugging Face LeRobot](https://huggingface.co/docs/lerobot/so101),
269 whose calibration convention (zero at the middle of each joint's range) the
270 model follows; [three.js](https://threejs.org) and
271 [urdf-loader](https://github.com/gkjohnson/urdf-loaders) for the 3D view;
272 the [Web Serial API](https://developer.mozilla.org/en-US/docs/Web/API/Web_Serial_API)
273 for live mode; [µStreamer](https://github.com/pikvm/ustreamer) for the cameras.
Software/so101/bench/assemble-follower.ts added +56
@@ -0,0 +1,56 @@
1 // Bench script, 2026-10-12: as each follower servo joins the chain, center it at 2048, wiggle it, and announce its id.
2 // Run from Software/so101: SO101_PORT=/dev/cu.usbmodemXXXX node bench/assemble-follower.ts
3 import { FeetechBus } from "../src/bus.ts";
4 import { JOINTS, STS3215 } from "../src/sts3215.ts";
5 const sleep = (ms: number) => new Promise((r) => setTimeout(r, ms));
6 const log = (line: string) => console.log(line);
7
8 let bus: FeetechBus | null = null;
9 async function connect(): Promise<FeetechBus> {
10 while (true) {
11 try { return await FeetechBus.open(process.env.SO101_PORT!); } catch { await sleep(1000); }
12 }
13 }
14
15 async function settle(id: number) {
16 const b = bus!;
17 await b.write(id, STS3215.Torque_Enable, 1);
18 await b.write(id, STS3215.Acceleration, 100);
19 await b.write(id, STS3215.Goal_Velocity, 1500);
20 await b.write(id, STS3215.Goal_Position, 2048); await sleep(1800);
21 await b.write(id, STS3215.Goal_Position, 2300); await sleep(450);
22 await b.write(id, STS3215.Goal_Position, 2048); await sleep(450);
23 await b.write(id, STS3215.Goal_Position, 2300); await sleep(450);
24 await b.write(id, STS3215.Goal_Position, 2048); await sleep(600);
25 await b.write(id, STS3215.Torque_Enable, 0);
26 }
27
28 log("EVENT waiting for the board");
29 bus = await connect();
30 const order: number[] = [5];
31 if (await bus.ping(5)) {
32 log("EVENT board is up; the 345 is already id 5 and centered -> plug in an unlabeled servo");
33 } else {
34 log("EVENT board is up -> plug in the labeled 345 by itself");
35 while (!(await bus.ping(2))) await sleep(1000);
36 await sleep(1000);
37 await bus.write(2, STS3215.ID, 5);
38 await settle(5);
39 log("EVENT #1 on the chain: the 345 is now id 5 (wrist_roll), centered -> plug in an unlabeled servo");
40 }
41
42 const expected = new Set([1, 2, 3, 4, 6]);
43 while (expected.size > 0) {
44 let found: number | null = null;
45 for (const id of expected) if (await bus.ping(id)) { found = id; break; }
46 if (found === null) { await sleep(1000); continue; }
47 expected.delete(found);
48 await sleep(500);
49 await settle(found);
50 order.push(found);
51 log(`EVENT #${order.length} on the chain: id ${found} (${JOINTS[found]}), centered${expected.size ? " -> plug in the next unlabeled servo" : " -> that is all six"}`);
52 }
53 log(`ORDER plugged in: ${order.map((id, i) => `#${i + 1}=id ${id} ${JOINTS[id]}`).join(", ")}`);
54 log(`ORDER pull off in reverse: ${[...order].reverse().map((id) => `id ${id} ${JOINTS[id]}`).join(" -> ")}`);
55 log("DONE");
56 await bus.close();
Software/so101/bench/fake-serve.ts added +26
@@ -0,0 +1,26 @@
1 /**
2 * The arm server on a simulated bus, for working on the viewer without hardware:
3 * node bench/fake-serve.ts [port] then open http://127.0.0.1:8102/?stream=<a camera stream url>
4 * Goals become positions at once, so every move "arrives"; poses and recordings go to a temp dir.
5 * PORT, HOST and FORGE_APP_DATA override the port, the 127.0.0.1 bind and the temp dir (how the forge runs it).
6 */
7 import { mkdirSync, mkdtempSync } from "node:fs";
8 import { tmpdir } from "node:os";
9 import { join } from "node:path";
10 import { FeetechBus } from "../src/bus.ts";
11 import { FakeBus } from "../src/fake-bus.ts";
12 import { startServer } from "../src/server.ts";
13
14 const fake = new FakeBus([1, 2, 3, 4, 5, 6]);
15 const ranges: Record<number, [number, number]> = { 1: [818, 3487], 2: [813, 3202], 3: [900, 3100], 4: [700, 3300], 5: [600, 3400], 6: [1995, 3525] };
16 for (const [id, [min, max]] of Object.entries(ranges)) {
17 const m = fake.servos.get(Number(id))!;
18 m.set([min & 0xff, min >> 8, max & 0xff, max >> 8], 9);
19 }
20 const appData = process.env.FORGE_APP_DATA;
21 const dir = appData ? join(appData, "so101") : mkdtempSync(join(tmpdir(), "so101-fake-"));
22 mkdirSync(dir, { recursive: true });
23 const port = Number(process.env.PORT ?? process.argv[2] ?? 8102);
24 const host = process.env.HOST ?? "127.0.0.1";
25 const server = await startServer(new FeetechBus(fake), { host, port, posesFile: join(dir, "poses.json"), modelDir: new URL("../../../Simulation/SO101", import.meta.url).pathname });
26 console.log(`fake arm on http://${host}:${server.port} (data in ${dir})`);
Software/so101/bench/handoff.ts added +38
@@ -0,0 +1,38 @@
1 // Bench script, 2026-10-12: hand servos off for mounting. Waits for all six, then wiggles id 1 until it is
2 // pulled, then id 2, and so on. Run from Software/so101: SO101_PORT=/dev/cu.usbmodemXXXX node bench/handoff.ts
3 import { FeetechBus } from "../src/bus.ts";
4 import { JOINTS, STS3215 } from "../src/sts3215.ts";
5 const sleep = (ms: number) => new Promise((r) => setTimeout(r, ms));
6 const log = (line: string) => console.log(line);
7 let bus: FeetechBus;
8 while (true) { try { bus = await FeetechBus.open(process.env.SO101_PORT!); break; } catch { await sleep(1000); } }
9
10 const ids = [1, 2, 3, 4, 5, 6];
11 log("EVENT waiting for all six follower servos to answer on one chain");
12 while ((await bus.syncRead(ids, STS3215.Present_Position)).size < 6) await sleep(1500);
13 log("EVENT all six answer -> wiggling id 1 (shoulder_pan): pull the one that moves and mount it in the base");
14
15 async function wiggle(id: number) {
16 const pos = await bus.read(id, STS3215.Present_Position);
17 const target = pos > 3800 ? pos - 250 : pos + 250;
18 await bus.write(id, STS3215.Torque_Enable, 1);
19 await bus.write(id, STS3215.Goal_Velocity, 1500);
20 await bus.write(id, STS3215.Goal_Position, target); await sleep(400);
21 await bus.write(id, STS3215.Goal_Position, pos); await sleep(400);
22 await bus.write(id, STS3215.Torque_Enable, 0);
23 }
24
25 for (const id of ids) {
26 let seen = false, absent = 0;
27 while (true) {
28 const here = (await bus.ping(id)) !== null;
29 if (here) { seen = true; absent = 0; try { await wiggle(id); } catch {} await sleep(1200); continue; }
30 if (!seen) { await sleep(1000); continue; } // not reachable yet (chain gap); wait for it
31 if (++absent >= 3) break; // pulled
32 await sleep(1000);
33 }
34 const next = ids[ids.indexOf(id) + 1];
35 log(`EVENT id ${id} (${JOINTS[id]}) pulled${next ? ` -> now wiggling id ${next} (${JOINTS[next]})` : " -> all six handed off"}`);
36 }
37 log("DONE");
38 await bus.close();
Software/so101/bench/sort-leader.ts added +80
@@ -0,0 +1,80 @@
1 // Bench script, 2026-10-19: leader servo sorting. Plug each leader servo in ALONE; it is found whatever id it has,
2 // its gearing is measured by a short full-speed sweep, it gets the next id for that gearing, is centered at 2048,
3 // and wiggles. Run from Software/so101: SO101_PORT=/dev/cu.usbmodemXXXX node bench/sort-leader.ts
4 import { FeetechBus } from "../src/bus.ts";
5 import { JOINTS, STS3215 } from "../src/sts3215.ts";
6 const sleep = (ms: number) => new Promise((r) => setTimeout(r, ms));
7 const log = (line: string) => console.log(line);
8 let bus: FeetechBus;
9 while (true) { try { bus = await FeetechBus.open(process.env.SO101_PORT!); break; } catch { await sleep(1000); } }
10
11 // Measured on this bench: 1/345 ≈ 1400 ticks/s, 1/191 ≈ 4160. 1/147 is faster still.
12 function classify(peak: number): "1/345" | "1/191" | "1/147" | "unclear" {
13 const rel = peak / 4160;
14 return rel < 0.6 ? "1/345" : rel >= 0.8 && rel <= 1.15 ? "1/191" : rel > 1.2 ? "1/147" : "unclear";
15 }
16
17 async function present(): Promise<number[]> {
18 const ids: number[] = [];
19 for (let id = 0; id < 254; id++) if (await bus.ping(id)) ids.push(id);
20 return ids;
21 }
22
23 async function measure(id: number): Promise<number> {
24 const start = await bus.read(id, STS3215.Present_Position);
25 const target = start > 3000 ? start - 512 : start + 512;
26 await bus.write(id, STS3215.Goal_Position, start);
27 await bus.write(id, STS3215.Torque_Enable, 1);
28 await bus.write(id, STS3215.Acceleration, 254);
29 await bus.write(id, STS3215.Goal_Velocity, 32767);
30 const t0 = performance.now(); await bus.write(id, STS3215.Goal_Position, target);
31 const samples: Array<[number, number]> = [];
32 while (performance.now() - t0 < 1500) {
33 const pos = await bus.read(id, STS3215.Present_Position); const t = performance.now() - t0;
34 samples.push([t, pos]); if (Math.abs(pos - target) <= 3 && t > 100) break; await sleep(3);
35 }
36 let peak = 0;
37 for (let i = 4; i < samples.length; i++) { const [t1, p1] = samples[i - 4]!, [t2, p2] = samples[i]!; peak = Math.max(peak, Math.abs(p2 - p1) / ((t2 - t1) / 1000)); }
38 await bus.write(id, STS3215.Goal_Velocity, 1200);
39 await bus.write(id, STS3215.Goal_Position, start); await sleep(700);
40 return peak;
41 }
42
43 async function centerAndWiggle(id: number) {
44 await bus.write(id, STS3215.Acceleration, 100);
45 await bus.write(id, STS3215.Goal_Velocity, 1500);
46 await bus.write(id, STS3215.Torque_Enable, 1);
47 await bus.write(id, STS3215.Goal_Position, 2048); await sleep(1800);
48 for (const p of [2300, 2048, 2300, 2048]) { await bus.write(id, STS3215.Goal_Position, p); await sleep(450); }
49 await bus.write(id, STS3215.Torque_Enable, 0);
50 }
51
52 const pools: Record<string, number[]> = { "1/191": [1, 3], "1/345": [2], "1/147": [4, 5, 6] };
53 const done: string[] = [];
54 log("EVENT ready: plug in one leader servo by itself");
55 let lastSeen = "";
56 while (Object.values(pools).some((p) => p.length)) {
57 const ids = await present();
58 const key = ids.join(",");
59 if (ids.length === 0) { lastSeen = ""; await sleep(1000); continue; }
60 if (ids.length > 1) { if (key !== lastSeen) log(`EVENT more than one servo answers (ids ${key}); leave only one plugged in`); lastSeen = key; await sleep(1500); continue; }
61 const [id] = ids;
62 if (key === lastSeen) { await sleep(1000); continue; } // already handled; waiting for it to be unplugged
63 await sleep(800);
64 const peak = await measure(id!);
65 const ratio = classify(peak);
66 const next = pools[ratio]?.shift();
67 if (next === undefined) {
68 await centerAndWiggle(id!); await centerAndWiggle(id!);
69 log(`EVENT servo (id ${id}) measured ${peak.toFixed(0)} ticks/s = ${ratio}: ${ratio === "unclear" ? "could not tell its gearing" : "every slot for that gearing is filled"}; double wiggle = set it aside, plug in the next`);
70 lastSeen = key; continue;
71 }
72 if (next !== id) await bus.write(id!, STS3215.ID, next);
73 await centerAndWiggle(next);
74 done.push(`${ratio}→${next}`);
75 const left = Object.entries(pools).filter(([, p]) => p.length).map(([r, p]) => `${r}: ${p.join(",")}`).join("; ");
76 log(`EVENT ${peak.toFixed(0)} ticks/s = ${ratio} -> now id ${next} (${JOINTS[next]}), centered; mount it. ${left ? `Still needed: ${left}` : "That was the last one"}`);
77 lastSeen = String(next);
78 }
79 log(`DONE leader set complete: ${done.join(", ")}`);
80 await bus.close();
Software/so101/bench/swap-345.ts added +36
@@ -0,0 +1,36 @@
1 // Bench script, 2026-10-12: swap the borrowed labeled 1/345 (follower id 5) for the sixth unlabeled servo.
2 // Run from Software/so101: SO101_PORT=/dev/cu.usbmodemXXXX node bench/swap-345.ts
3 import { FeetechBus } from "../src/bus.ts";
4 import { STS3215 } from "../src/sts3215.ts";
5 const sleep = (ms: number) => new Promise((r) => setTimeout(r, ms));
6 const log = (line: string) => console.log(line);
7 const bus = await FeetechBus.open(process.env.SO101_PORT!);
8
9 async function settle(id: number) {
10 await bus.write(id, STS3215.Torque_Enable, 1);
11 await bus.write(id, STS3215.Acceleration, 100);
12 await bus.write(id, STS3215.Goal_Velocity, 1500);
13 await bus.write(id, STS3215.Goal_Position, 2048); await sleep(1800);
14 await bus.write(id, STS3215.Goal_Position, 2300); await sleep(450);
15 await bus.write(id, STS3215.Goal_Position, 2048); await sleep(450);
16 await bus.write(id, STS3215.Goal_Position, 2300); await sleep(450);
17 await bus.write(id, STS3215.Goal_Position, 2048); await sleep(600);
18 await bus.write(id, STS3215.Torque_Enable, 0);
19 }
20 const alone = async (id: number) => (await bus.ping(id)) && !(await bus.ping(id === 1 ? 2 : 1)) && !(await bus.ping(6));
21
22 log("EVENT waiting for the chain to be unplugged and the new servo (id 1) to be alone on the board");
23 while (!(await alone(1))) await sleep(1000);
24 await sleep(1000);
25 await bus.write(1, STS3215.ID, 5);
26 await settle(5);
27 log("EVENT new servo is now id 5 (follower wrist_roll), centered -> unplug it, then plug the 345 in alone");
28 while (await bus.ping(5)) await sleep(1000);
29 log("EVENT new servo unplugged -> waiting for the 345 (id 5) alone");
30 while (!(await alone(5))) await sleep(1000);
31 await sleep(1000);
32 await bus.write(5, STS3215.ID, 2);
33 await settle(2);
34 log("EVENT the 345 is back to id 2 (leader shoulder_lift), centered -> set it aside; chain the new servo with the five");
35 log("DONE");
36 await bus.close();
Software/so101/bench/walk-leader.ts added +129
@@ -0,0 +1,129 @@
1 // Bench script, 2026-10-19: fix servo ids on an ASSEMBLED leader by walking the chain outward from the base.
2 // Start with only the base servo connected to the board; reconnect one joint at a time when told.
3 // Run from Software/so101: SO101_PORT=/dev/cu.usbmodemXXXX node bench/walk-leader.ts
4 import { FeetechBus } from "../src/bus.ts";
5 import { JOINTS, STS3215 } from "../src/sts3215.ts";
6 const sleep = (ms: number) => new Promise((r) => setTimeout(r, ms));
7 const log = (line: string) => console.log(line);
8 let bus: FeetechBus;
9 async function connect() { while (true) { try { bus = await FeetechBus.open(process.env.SO101_PORT!); return; } catch { await sleep(1000); } } }
10 await connect();
11
12 const EXPECTED: Record<number, string> = { 1: "1/191", 2: "1/345", 3: "1/191", 4: "1/147", 5: "1/147", 6: "1/147" };
13 const TEMP = 11; // the base parks here until the others are named, so factory id 1 never collides
14
15 let lastPeak = 0;
16 function classify(peak: number): string {
17 lastPeak = peak;
18 // On a 45° sweep: 1/345 ≈ 1400 ticks/s, 1/191 ≈ 4160. This shorter swing reads a little low, so the bands are wide.
19 return peak < 2400 ? "1/345" : peak < 5200 ? "1/191" : "1/147";
20 }
21 async function present(): Promise<number[]> { const ids: number[] = []; for (let id = 0; id < 254; id++) if (await bus.ping(id)) ids.push(id); return ids; }
22 async function measure(id: number): Promise<number> {
23 const start = await bus.read(id, STS3215.Present_Position);
24 const target = start + (start > 2048 ? -400 : 400); // a swing toward the middle, long enough to hit top speed
25 await bus.write(id, STS3215.Goal_Position, start);
26 await bus.write(id, STS3215.Torque_Enable, 1);
27 await bus.write(id, STS3215.Acceleration, 254);
28 await bus.write(id, STS3215.Goal_Velocity, 32767);
29 const t0 = performance.now(); await bus.write(id, STS3215.Goal_Position, target);
30 const s: Array<[number, number]> = [];
31 while (performance.now() - t0 < 1200) { const p = await bus.read(id, STS3215.Present_Position); const t = performance.now() - t0; s.push([t, p]); if (Math.abs(p - target) <= 3 && t > 60) break; await sleep(3); }
32 let peak = 0; for (let i = 3; i < s.length; i++) { const [t1, p1] = s[i - 3]!, [t2, p2] = s[i]!; peak = Math.max(peak, Math.abs(p2 - p1) / ((t2 - t1) / 1000)); }
33 await bus.write(id, STS3215.Goal_Velocity, 1000); await bus.write(id, STS3215.Goal_Position, start); await sleep(600);
34 return peak;
35 }
36 async function wiggle(id: number, times = 2) {
37 const pos = await bus.read(id, STS3215.Present_Position); const t = pos + (pos > 2048 ? -80 : 80);
38 await bus.write(id, STS3215.Goal_Position, pos); await bus.write(id, STS3215.Torque_Limit, 300);
39 await bus.write(id, STS3215.Goal_Velocity, 600); await bus.write(id, STS3215.Torque_Enable, 1);
40 for (let i = 0; i < times; i++) { await bus.write(id, STS3215.Goal_Position, t); await sleep(350); await bus.write(id, STS3215.Goal_Position, pos); await sleep(350); }
41 await bus.write(id, STS3215.Torque_Enable, 0);
42 }
43 function gearNote(finalId: number, ratio: string): string {
44 return ratio === EXPECTED[finalId] ? `gearing ${ratio} is right for ${JOINTS[finalId]}` : `WRONG GEARING: ${JOINTS[finalId]} wants ${EXPECTED[finalId]} but this servo is ${ratio}`;
45 }
46
47
48
49 // Incremental mode: whatever already answers with a final id is kept (and its gearing checked once); every servo
50 // that appears afterwards is measured and given the next free id for its gearing. A 1/191 that must become id 1
51 // is parked at TEMP until the end so later factory-id-1 arrivals cannot collide with it.
52 const pools: Record<string, number[]> = { "1/345": [2], "1/191": [1, 3], "1/147": [4, 5, 6] };
53 const checked = new Set<number>();
54 let announced = "";
55 let parkedBase = false;
56
57 async function verifyKnown(ids: number[]) {
58 for (const id of ids) {
59 if (checked.has(id) || !(id >= 1 && id <= 6)) continue;
60 checked.add(id);
61 log(`EVENT ${JOINTS[id]} already has id ${id}`);
62 }
63 }
64
65 for (const id of [1, 2, 3, 4, 5, 6, TEMP]) { try { await bus.write(id, STS3215.Torque_Enable, 0); } catch {} }
66 log("EVENT ready (torque released everywhere)");
67 async function walk(): Promise<boolean> {
68 const ids = await present();
69 const key = ids.join(",");
70 if (ids.length === 0) { announced = ""; return false; }
71 if (ids.includes(TEMP) && !parkedBase) { parkedBase = true; const i = pools["1/191"]!.indexOf(1); if (i >= 0) pools["1/191"]!.splice(i, 1); log("EVENT a servo is already parked as shoulder_pan from the earlier run; it gets id 1 at the end"); }
72 await verifyKnown(ids.filter((i) => i >= 2 && i <= 6));
73 const fresh = ids.filter((i) => !checked.has(i) && i !== TEMP);
74 const missing = [1, 2, 3, 4, 5, 6].filter((f) => !ids.includes(f) && !(f === 1 && parkedBase));
75 if (fresh.length === 0) {
76 if (missing.length === 0) {
77 if (parkedBase) { await bus.write(TEMP, STS3215.ID, 1); await wiggle(1); checked.add(1); }
78 const all = await present();
79 log(`DONE leader chain reads ids ${all.join(",")}; torque off everywhere`);
80 for (const id of all) await bus.write(id, STS3215.Torque_Enable, 0);
81 return true;
82 }
83 if (key !== announced) {
84 // The ready signal: the shoulder lift wiggles once when the chain is stable and the next servo can be connected.
85 if (ids.includes(2)) await wiggle(2, 1);
86 log(`EVENT ready for the next servo (chain has ${ids.join(",")}; still needed: ${missing.map((m) => JOINTS[m]).join(", ")})`);
87 }
88 announced = key; return false;
89 }
90 if (fresh.length > 1) { if (key !== announced) log(`EVENT ids ${fresh.join(",")} are new at once; connect one joint at a time`); announced = key; return false; }
91 const id = fresh[0]!;
92 await sleep(500);
93 // Several factory-id-1 servos answering together look like one id to a ping but garble every read.
94 let clean = 0;
95 for (let i = 0; i < 6; i++) { try { await bus.read(id, STS3215.Present_Position); clean++; } catch {} }
96 if (clean < 6) {
97 if (key !== announced) log(`EVENT id ${id} is really several servos talking at once (${6 - clean}/6 reads garbled); disconnect so only ONE unnamed servo is on the chain`);
98 announced = key; return false;
99 }
100 // No speed test on a mounted arm: it drives joints into the structure. Ids go in connection order:
101 // elbow 3, wrist flex 4, wrist roll 5, trigger 6; the pan is parked for the end.
102 const next = [3, 4, 5, 6].find((f) => !ids.includes(f) && !checked.has(f));
103 if (next === undefined) {
104 await wiggle(id, 4);
105 log(`EVENT all of 3-6 are named already; this extra servo (id ${id}) is not expected. Long wiggle = disconnect it`);
106 announced = key; await sleep(5000); return false;
107 }
108 if (id !== next) await bus.write(id, STS3215.ID, next);
109 checked.add(next); await wiggle(next);
110 log(`EVENT ${JOINTS[next]} is id ${next}; wait for the ready wiggle, then connect the next`);
111 announced = ""; return false;
112 }
113 while (true) {
114 try { if (await walk()) break; await sleep(1500); }
115 catch (err) {
116 const msg = (err as Error).message.split("\n")[0];
117 if (/ENXIO|not open|cannot open|disconnect/i.test(msg)) {
118 log(`EVENT board dropped (${msg}); waiting for it to come back`);
119 try { await bus.close(); } catch {}
120 await connect();
121 } else {
122 log(`EVENT a servo stopped answering mid-step (${msg}); if the chain brought several id-1 servos in at once, reduce it to one`);
123 for (const id of [1, 2, 3, 4, 5, 6, TEMP]) { try { await bus.write(id, STS3215.Torque_Enable, 0); } catch {} }
124 await sleep(2000);
125 }
126 announced = "";
127 }
128 }
129 await bus.close();
Software/so101/bin/so101 added +7
@@ -0,0 +1,7 @@
1 #!/usr/bin/env node
2 // Node, not Bun: serialport's native bindings call uv_default_loop, which Bun
3 // does not provide yet (https://github.com/oven-sh/bun/issues/18546). Bun still
4 // installs and tests the package; Node 24 strips the types at load time.
5 import { main } from "../src/cli.ts";
6
7 process.exitCode = await main(process.argv.slice(2));
Software/so101/bun.lock added +121
@@ -0,0 +1,121 @@
1 {
2 "lockfileVersion": 1,
3 "configVersion": 1,
4 "workspaces": {
5 "": {
6 "name": "so101",
7 "dependencies": {
8 "serialport": "^13",
9 "three": "^0.186.1",
10 "urdf-loader": "^0.13.1",
11 "ws": "^8.22.0",
12 },
13 "devDependencies": {
14 "@types/bun": "^1.4.2",
15 "@types/ws": "^8.18.2",
16 "typescript": "^7.0.2",
17 },
18 },
19 },
20 "packages": {
21 "@serialport/binding-mock": ["@serialport/[email protected]", "", { "dependencies": { "@serialport/bindings-interface": "^1.2.1", "debug": "^4.3.3" } }, "sha512-HAFzGhk9OuFMpuor7aT5G1ChPgn5qSsklTFOTUX72Rl6p0xwcSVsRtG/xaGp6bxpN7fI9D/S8THLBWbBgS6ldw=="],
22
23 "@serialport/bindings-cpp": ["@serialport/[email protected]", "", { "dependencies": { "@serialport/bindings-interface": "1.2.2", "@serialport/parser-readline": "12.0.0", "debug": "4.4.0", "node-addon-api": "8.3.0", "node-gyp-build": "4.8.4" } }, "sha512-r25o4Bk/vaO1LyUfY/ulR6hCg/aWiN6Wo2ljVlb4Pj5bqWGcSRC4Vse4a9AcapuAu/FeBzHCbKMvRQeCuKjzIQ=="],
24
25 "@serialport/bindings-interface": ["@serialport/[email protected]", "", {}, "sha512-CJaUd5bLvtM9c5dmO9rPBHPXTa9R2UwpkJ0wdh9JCYcbrPWsKz+ErvR0hBLeo7NPeiFdjFO4sonRljiw4d2XiA=="],
26
27 "@serialport/parser-byte-length": ["@serialport/[email protected]", "", {}, "sha512-32yvqeTAqJzAEtX5zCrN1Mej56GJ5h/cVFsCDPbF9S1ZSC9FWjOqNAgtByseHfFTSTs/4ZBQZZcZBpolt8sUng=="],
28
29 "@serialport/parser-cctalk": ["@serialport/[email protected]", "", {}, "sha512-RErAe57g9gvnlieVYGIn1xymb1bzNXb2QtUQd14FpmbQQYlcrmuRnJwKa1BgTCujoCkhtaTtgHlbBWOxm8U2uA=="],
30
31 "@serialport/parser-delimiter": ["@serialport/[email protected]", "", {}, "sha512-Qqyb0FX1avs3XabQqNaZSivyVbl/yl0jywImp7ePvfZKLwx7jBZjvL+Hawt9wIG6tfq6zbFM24vzCCK7REMUig=="],
32
33 "@serialport/parser-inter-byte-timeout": ["@serialport/[email protected]", "", {}, "sha512-a0w0WecTW7bD2YHWrpTz1uyiWA2fDNym0kjmPeNSwZ2XCP+JbirZt31l43m2ey6qXItTYVuQBthm75sPVeHnGA=="],
34
35 "@serialport/parser-packet-length": ["@serialport/[email protected]", "", {}, "sha512-60ZDDIqYRi0Xs2SPZUo4Jr5LLIjtb+rvzPKMJCohrO6tAqSDponcNpcB1O4W21mKTxYjqInSz+eMrtk0LLfZIg=="],
36
37 "@serialport/parser-readline": ["@serialport/[email protected]", "", { "dependencies": { "@serialport/parser-delimiter": "13.0.0" } }, "sha512-dov3zYoyf0dt1Sudd1q42VVYQ4WlliF0MYvAMA3MOyiU1IeG4hl0J6buBA2w4gl3DOCC05tGgLDN/3yIL81gsA=="],
38
39 "@serialport/parser-ready": ["@serialport/[email protected]", "", {}, "sha512-JNUQA+y2Rfs4bU+cGYNqOPnNMAcayhhW+XJZihSLQXOHcZsFnOa2F9YtMg9VXRWIcnHldHYtisp62Etjlw24bw=="],
40
41 "@serialport/parser-regex": ["@serialport/[email protected]", "", {}, "sha512-m7HpIf56G5XcuDdA3DB34Z0pJiwxNRakThEHjSa4mG05OnWYv0IG8l2oUyYfuGMowQWaVnQ+8r+brlPxGVH+eA=="],
42
43 "@serialport/parser-slip-encoder": ["@serialport/[email protected]", "", {}, "sha512-fUHZEExm6izJ7rg0A1yjXwu4sOzeBkPAjDZPfb+XQoqgtKAk+s+HfICiYn7N2QU9gyaeCO8VKgWwi+b/DowYOg=="],
44
45 "@serialport/parser-spacepacket": ["@serialport/[email protected]", "", {}, "sha512-DoXJ3mFYmyD8X/8931agJvrBPxqTaYDsPoly9/cwQSeh/q4EjQND9ySXBxpWz5WcpyCU4jOuusqCSAPsbB30Eg=="],
46
47 "@serialport/stream": ["@serialport/[email protected]", "", { "dependencies": { "@serialport/bindings-interface": "1.2.2", "debug": "4.4.0" } }, "sha512-F7xLJKsjGo2WuEWMSEO1SimRcOA+WtWICsY13r0ahx8s2SecPQH06338g28OT7cW7uRXI7oEQAk62qh5gHJW3g=="],
48
49 "@types/bun": ["@types/[email protected]", "", { "dependencies": { "bun-types": "1.4.2" } }, "sha512-GimotNn7+ZV0uVArItBbriZsR1oNf0+WTzPkdcFrzShI7k2norL0uzEaJT8T33dWr7O/c9ZDuAFQrctKCi72oQ=="],
50
51 "@types/node": ["@types/[email protected]", "", { "dependencies": { "undici-types": "~8.9.0" } }, "sha512-uMzsv9stDdofIZBx4UGh4Z9ZgXnGC9seRDscilBcTHAeOFk67MZg/k9bWVfO+bk4qZmG+l52hRnLwUiH2LnJTg=="],
52
53 "@types/ws": ["@types/[email protected]", "", { "dependencies": { "@types/node": "*" } }, "sha512-67MQl+fpWKVTT1NYdnmo3U4sc/xPo/zQBncVnI74qmQa0z/b+1g6iYqNmGCPbxO+zz2aklb08a0oHfegiVd0/w=="],
54
55 "@typescript/typescript-aix-ppc64": ["@typescript/[email protected]", "", { "os": "aix", "cpu": "ppc64" }, "sha512-MTKKkWB7p/0E9xi1d1tHtZ5PiLkGEMIq88pK2CubZjOsLtYTLqhgIgi6zepFa+9GHZ6h05NMCkQxGKiPXMxXtQ=="],
56
57 "@typescript/typescript-darwin-arm64": ["@typescript/[email protected]", "", { "os": "darwin", "cpu": "arm64" }, "sha512-gowzar9MwS/aRWp6f3a4KUqzRjAZjOsmGNCM6LcTgXum+dBfgsBVMN+AgvOCCbguXyick6LJhpBszxMebJ8syA=="],
58
59 "@typescript/typescript-darwin-x64": ["@typescript/[email protected]", "", { "os": "darwin", "cpu": "x64" }, "sha512-SZ9xZInqApNlNGc9s0W1VSsktYSOe9cFqNOIqmN1Gs8SmkjKZYFt017G4VwPxASInODuAdbTW7sXiFUf893RgA=="],
60
61 "@typescript/typescript-freebsd-arm64": ["@typescript/[email protected]", "", { "os": "freebsd", "cpu": "arm64" }, "sha512-W5NH4y/J0plIIS5b2xvTEkU7JFxyqdMAOgf+Ilhl0vHQXKO5dZoxd+C/jEtq56c4F3wk71RB4BMRQ2XdI+bwYQ=="],
62
63 "@typescript/typescript-freebsd-x64": ["@typescript/[email protected]", "", { "os": "freebsd", "cpu": "x64" }, "sha512-UMGDx5sTpzNw3WiPebH7l90IWfJggEd+egHt/q6p7/Cm3zqoV7VxkGXt+3DxPIw8CcmvAB0j3sVVfbhX+M4Tpw=="],
64
65 "@typescript/typescript-linux-arm": ["@typescript/[email protected]", "", { "os": "linux", "cpu": "arm" }, "sha512-gffT3xPz9sR7j/YJExkyPntrI0P2EP9XbOyWzth2/Gs0RstK+90RBcO0ncXoXy/beYll1SXw846Nf2zdnEz0QQ=="],
66
67 "@typescript/typescript-linux-arm64": ["@typescript/[email protected]", "", { "os": "linux", "cpu": "arm64" }, "sha512-Qh4eU4/y3yDjnfjjyPYihMj5/ODIlmt+Bzu17OI+fiSRDW57QmU5SiN63exPRNJPKUzcc1INa1NXdrJ+MqHjUQ=="],
68
69 "@typescript/typescript-linux-loong64": ["@typescript/[email protected]", "", { "os": "linux", "cpu": "none" }, "sha512-uEHck9i8hoAzXPiYRib1O7miOnz23SxIeVl6F4LXox+qov1K35jHcEW6VHKvZI+pyvl7fZEP4MCU5LYvIq1GuQ=="],
70
71 "@typescript/typescript-linux-mips64el": ["@typescript/[email protected]", "", { "os": "linux", "cpu": "none" }, "sha512-R4KvAMnE43W5Qeqb0Ly56O3mWMWIAgsMyz36DCaycd5nbg/9kzm0liw3JocfRqyJY0KPmzFjbswozXyW0DnIYA=="],
72
73 "@typescript/typescript-linux-ppc64": ["@typescript/[email protected]", "", { "os": "linux", "cpu": "ppc64" }, "sha512-DORx5b3sd/4S7eayxm4FQv+A7CrkUIGRaHiwI8oiHTAI1fAPWhF4J0vAlkC8biAlHSVVwxMQ3tjZ2/DVbnQiiA=="],
74
75 "@typescript/typescript-linux-riscv64": ["@typescript/[email protected]", "", { "os": "linux", "cpu": "none" }, "sha512-wf0jqEDOjrPRnKwYRyyJDRo11KMbvMFrU+q4zqKyChODBzvlkbhNQfKvLxQCcwTpdDaXSHZTVuh0JoCrKCUMHQ=="],
76
77 "@typescript/typescript-linux-s390x": ["@typescript/[email protected]", "", { "os": "linux", "cpu": "s390x" }, "sha512-IkwJc3L7yhytWd/ewjyxNDfOmswCm9GWMJT/ue/dU4aZNbwZeYAetq42VyLmsmSjvoX7z74X6ZaYCtzAr0EuGw=="],
78
79 "@typescript/typescript-linux-x64": ["@typescript/[email protected]", "", { "os": "linux", "cpu": "x64" }, "sha512-EYdf2cNg7rgCWJnxCdJ+F3V39O8ihb37eHAu1LK8oAFizgTQbPOK7zHHXbPt8rX24COqODXeI3sIf0fCXG7H/A=="],
80
81 "@typescript/typescript-netbsd-arm64": ["@typescript/[email protected]", "", { "os": "none", "cpu": "arm64" }, "sha512-+polYF4MF04aPpO5FTkHran9yUQDSXqy5GiSDKpsll5jy3l3+g9QLhpf39T+ePtefhXLOGrLl0QIjkQP6VnelA=="],
82
83 "@typescript/typescript-netbsd-x64": ["@typescript/[email protected]", "", { "os": "none", "cpu": "x64" }, "sha512-8YIT0EHM/3dq10ZOVF/A7pc/YSMtbcecct4rWtexrnSCHOPcpC2KTLXfTCR6vDpnSiY12heNb1GiN/wu+T/FyA=="],
84
85 "@typescript/typescript-openbsd-arm64": ["@typescript/[email protected]", "", { "os": "openbsd", "cpu": "arm64" }, "sha512-APT8+ClYnuYm1u9+kgGXoMj2VzWzcymwh2gNSQVySHfkRDGOTVkoWLjCmOQSaO+PoqQ57B0flRp9SA+7GnnkzQ=="],
86
87 "@typescript/typescript-openbsd-x64": ["@typescript/[email protected]", "", { "os": "openbsd", "cpu": "x64" }, "sha512-yX7s+Q0Dln0Dt9tEzZsAjXXR/+ytBM7AlglaqyeMPxQszJ1JhlJdZ6jLA+IzldHtflX81em7lDao1xXu+aRRkg=="],
88
89 "@typescript/typescript-sunos-x64": ["@typescript/[email protected]", "", { "os": "sunos", "cpu": "x64" }, "sha512-dLJDGaLZ1D4HPQn62u1n8mBDkJREwMsAkCdkwd4Ieqw+x3TUyTsqY0YiBCtE6H6OzzgGk3iuZ3vFWRS+E8/d1g=="],
90
91 "@typescript/typescript-win32-arm64": ["@typescript/[email protected]", "", { "os": "win32", "cpu": "arm64" }, "sha512-Gyl1Vy6OsWesLzmq+EP0Fb7b4Nid5232AvcA2SFcdYreldpNtYFFofPjnt62y9hQy7VTaZp65ICJjuAQRaVcIQ=="],
92
93 "@typescript/typescript-win32-x64": ["@typescript/[email protected]", "", { "os": "win32", "cpu": "x64" }, "sha512-0BQ3HkAHHlKLSp1qRvf3SUhGpGsDuhB/jgFw75guyqbxJqEaS0Cw/VFO8i2nHglJUzQCRtMMR/IBAKE3ETMC4g=="],
94
95 "bun-types": ["[email protected]", "", { "dependencies": { "@types/node": "*" } }, "sha512-bxV1FgK7yBIzjRe5zBozIM4Bem11ZJcCXSrjWRG3YWLt8yFDePu4cLjpebO8OvPeIE9trbyPF4fuj3Cia4Fj3w=="],
96
97 "debug": ["[email protected]", "", { "dependencies": { "ms": "^2.1.3" } }, "sha512-6WTZ/IxCY/T6BALoZHaE4ctp9xm+Z5kY/pzYaCHRFeyVhojxlrm+46y68HA6hr0TcwEssoxNiDEUJQjfPZ/RYA=="],
98
99 "ms": ["[email protected]", "", {}, "sha512-6FlzubTLZG3J2a/NVCAleEhjzq5oxgHyaCU9yYXvcLsvoVaHJq/s5xXI6/XXP6tz7R9xAOtHnSO/tXtF3WRTlA=="],
100
101 "node-addon-api": ["[email protected]", "", {}, "sha512-8VOpLHFrOQlAH+qA0ZzuGRlALRA6/LVh8QJldbrC4DY0hXoMP0l4Acq8TzFC018HztWiRqyCEj2aTWY2UvnJUg=="],
102
103 "node-gyp-build": ["[email protected]", "", { "bin": { "node-gyp-build": "bin.js", "node-gyp-build-optional": "optional.js", "node-gyp-build-test": "build-test.js" } }, "sha512-LA4ZjwlnUblHVgq0oBF3Jl/6h/Nvs5fzBLwdEF4nuxnFdsfajde4WfxtJr3CaiH+F6ewcIB/q4jQ4UzPyid+CQ=="],
104
105 "serialport": ["[email protected]", "", { "dependencies": { "@serialport/binding-mock": "10.2.2", "@serialport/bindings-cpp": "13.0.0", "@serialport/parser-byte-length": "13.0.0", "@serialport/parser-cctalk": "13.0.0", "@serialport/parser-delimiter": "13.0.0", "@serialport/parser-inter-byte-timeout": "13.0.0", "@serialport/parser-packet-length": "13.0.0", "@serialport/parser-readline": "13.0.0", "@serialport/parser-ready": "13.0.0", "@serialport/parser-regex": "13.0.0", "@serialport/parser-slip-encoder": "13.0.0", "@serialport/parser-spacepacket": "13.0.0", "@serialport/stream": "13.0.0", "debug": "4.4.0" } }, "sha512-PHpnTd8isMGPfFTZNCzOZp9m4mAJSNWle9Jxu6BPTcWq7YXl5qN7tp8Sgn0h+WIGcD6JFz5QDgixC2s4VW7vzg=="],
106
107 "three": ["[email protected]", "", {}, "sha512-blFeqb49wRCSGUGj7gtpfnSGHy2lwDk94RhUmS1c/hTby70kvChbWpkJ4Pm1390LqzzvTmzgXKHPEafJwCb8jA=="],
108
109 "typescript": ["[email protected]", "", { "optionalDependencies": { "@typescript/typescript-aix-ppc64": "7.0.2", "@typescript/typescript-darwin-arm64": "7.0.2", "@typescript/typescript-darwin-x64": "7.0.2", "@typescript/typescript-freebsd-arm64": "7.0.2", "@typescript/typescript-freebsd-x64": "7.0.2", "@typescript/typescript-linux-arm": "7.0.2", "@typescript/typescript-linux-arm64": "7.0.2", "@typescript/typescript-linux-loong64": "7.0.2", "@typescript/typescript-linux-mips64el": "7.0.2", "@typescript/typescript-linux-ppc64": "7.0.2", "@typescript/typescript-linux-riscv64": "7.0.2", "@typescript/typescript-linux-s390x": "7.0.2", "@typescript/typescript-linux-x64": "7.0.2", "@typescript/typescript-netbsd-arm64": "7.0.2", "@typescript/typescript-netbsd-x64": "7.0.2", "@typescript/typescript-openbsd-arm64": "7.0.2", "@typescript/typescript-openbsd-x64": "7.0.2", "@typescript/typescript-sunos-x64": "7.0.2", "@typescript/typescript-win32-arm64": "7.0.2", "@typescript/typescript-win32-x64": "7.0.2" }, "bin": { "tsc": "bin/tsc" } }, "sha512-8FYau96o3NKOhbjKi/qNvG/W5jhzxkbdm5sj9AbZ/5T5sWqn3hJgLfGx27sRKZWTvyzCP8dLRBTf5tBTSRVUNA=="],
110
111 "undici-types": ["[email protected]", "", {}, "sha512-KTDyRTYX8sWmKXAikPHHSyc63CRPETMctyjKFupcC6OBLXT3xsN0e9aF7m+mIXutFWpUXuedtowG7iLOzp0kQg=="],
112
113 "urdf-loader": ["[email protected]", "", { "peerDependencies": { "three": ">=0.152.0" } }, "sha512-bHHoh0Yxrgrw91dPAmVnzKleA8HiVMeHy6xYqOVRw3LuZ2hDOwUiQVCbrzPZckO5S30F1Yym8zLCINN0HfcM3w=="],
114
115 "ws": ["[email protected]", "", { "peerDependencies": { "bufferutil": "^4.0.1", "utf-8-validate": ">=5.0.2" }, "optionalPeers": ["bufferutil", "utf-8-validate"] }, "sha512-Ydggc987+RO0AnWtZ/7Wq9FtNvcrL1b/RO0ud9mWjUPgDrsAAwQSF51sm2hm1XofbU/4jkpGEsLFsZZxU+1DOg=="],
116
117 "@serialport/bindings-cpp/@serialport/parser-readline": ["@serialport/[email protected]", "", { "dependencies": { "@serialport/parser-delimiter": "12.0.0" } }, "sha512-O7cywCWC8PiOMvo/gglEBfAkLjp/SENEML46BXDykfKP5mTPM46XMaX1L0waWU6DXJpBgjaL7+yX6VriVPbN4w=="],
118
119 "@serialport/bindings-cpp/@serialport/parser-readline/@serialport/parser-delimiter": ["@serialport/[email protected]", "", {}, "sha512-gu26tVt5lQoybhorLTPsH2j2LnX3AOP2x/34+DUSTNaUTzu2fBXw+isVjQJpUBFWu6aeQRZw5bJol5X9Gxjblw=="],
120 }
121 }
Software/so101/package.json added +25
@@ -0,0 +1,25 @@
1 {
2 "name": "so101",
3 "version": "0.1.0",
4 "private": true,
5 "type": "module",
6 "description": "Feetech STS3215 bus driver and CLI for the SO-101 arm",
7 "bin": {
8 "so101": "bin/so101"
9 },
10 "scripts": {
11 "typecheck": "tsc --noEmit",
12 "test": "bun test"
13 },
14 "dependencies": {
15 "serialport": "^13",
16 "three": "^0.186.1",
17 "urdf-loader": "^0.13.1",
18 "ws": "^8.22.0"
19 },
20 "devDependencies": {
21 "@types/bun": "^1.4.2",
22 "@types/ws": "^8.18.2",
23 "typescript": "^7.0.2"
24 }
25 }
Software/so101/src/bus.test.ts added +119
@@ -0,0 +1,119 @@
1 import { describe, expect, test } from "bun:test";
2 import { BASE_TIMEOUT_MS, FeetechBus, NoResponseError } from "./bus.ts";
3 import { FakeBus } from "./fake-bus.ts";
4 import { STS3215 } from "./sts3215.ts";
5
6 describe("FeetechBus", () => {
7 test("ping answers for present servos and null for absent ones", async () => {
8 const bus = new FeetechBus(new FakeBus([1, 2]));
9 expect((await bus.ping(1))?.id).toBe(1);
10 expect(await bus.ping(7)).toBeNull();
11 });
12
13 test("scan lists responders in order", async () => {
14 const bus = new FeetechBus(new FakeBus([5, 1, 3]));
15 expect(await bus.scan([0, 1, 2, 3, 4, 5, 6])).toEqual([1, 3, 5]);
16 });
17
18 test("read decodes little-endian and write lands in memory", async () => {
19 const fake = new FakeBus([1]);
20 const bus = new FeetechBus(fake);
21 expect(await bus.read(1, STS3215.Present_Position)).toBe(2048);
22 expect(await bus.read(1, STS3215.Model_Number)).toBe(777);
23 await bus.write(1, STS3215.Goal_Position, 4095);
24 expect([...fake.servos.get(1)!.subarray(42, 44)]).toEqual([0xff, 0x0f]);
25 });
26
27 test("read of a missing servo throws NoResponseError", async () => {
28 const bus = new FeetechBus(new FakeBus([1]));
29 await expect(bus.read(9, STS3215.Present_Position)).rejects.toBeInstanceOf(NoResponseError);
30 });
31
32 test("syncRead returns what answered and omits what did not", async () => {
33 const bus = new FeetechBus(new FakeBus([1, 2, 3], { drop: new Set([2]) }));
34 const positions = await bus.syncRead([1, 2, 3], STS3215.Present_Position);
35 expect([...positions.entries()]).toEqual([
36 [1, 2048],
37 [3, 2048],
38 ]);
39 });
40
41 test("syncWrite sets every listed servo without waiting for replies", async () => {
42 const fake = new FakeBus([1, 2]);
43 const bus = new FeetechBus(fake);
44 await bus.syncWrite(STS3215.Torque_Enable, [
45 [1, 1],
46 [2, 0],
47 ]);
48 expect(fake.writes).toEqual([
49 { id: 1, address: 40, bytes: [1] },
50 { id: 2, address: 40, bytes: [0] },
51 ]);
52 });
53
54 test("EEPROM writes follow LeRobot's torque-off, unlock, write, lock sequence, and an ID write is acknowledged by the new id", async () => {
55 const fake = new FakeBus([1]);
56 const bus = new FeetechBus(fake);
57 await bus.write(1, STS3215.ID, 4);
58 expect(fake.writes).toEqual([
59 { id: 1, address: 40, bytes: [0] },
60 { id: 1, address: 55, bytes: [0] },
61 { id: 1, address: 5, bytes: [4] },
62 { id: 4, address: 55, bytes: [1] },
63 ]);
64 expect((await bus.ping(4))?.id).toBe(4);
65 expect(await bus.ping(1)).toBeNull();
66 });
67
68 test("detectEcho tells an echoing adapter from a quiet one", async () => {
69 expect(await new FeetechBus(new FakeBus([1], { echo: true })).detectEcho()).toBe(true);
70 expect(await new FeetechBus(new FakeBus([1])).detectEcho()).toBe(false);
71 });
72
73 test("ignores the adapter echoing our own packet", async () => {
74 const bus = new FeetechBus(new FakeBus([1], { echo: true }), { echo: true });
75 expect(await bus.read(1, STS3215.Present_Position)).toBe(2048);
76 expect((await bus.ping(1))?.error).toBe(0);
77 });
78
79 test("a ping reply that merely sets the voltage bit is not mistaken for an echo", async () => {
80 // FF FF 01 02 01 FB is both the ping and a reply carrying error bit 0.
81 const quiet = new FeetechBus(new FakeBus([1], { pingError: 1 }));
82 expect((await quiet.ping(1))?.error).toBe(1);
83 const echoing = new FeetechBus(new FakeBus([1], { pingError: 1, echo: true }), { echo: true });
84 expect((await echoing.ping(1))?.error).toBe(1);
85 });
86
87 test("a reply that arrives after its deadline is not taken for the next answer", async () => {
88 const bus = new FeetechBus(new FakeBus([1], { lateOnce: new Map([[1, BASE_TIMEOUT_MS + 10]]) }));
89 await expect(bus.read(1, STS3215.Present_Position)).rejects.toBeInstanceOf(NoResponseError);
90 expect(await bus.read(1, STS3215.Present_Velocity)).toBe(16);
91 });
92
93 test("concurrent calls are serialised on the bus", async () => {
94 const bus = new FeetechBus(new FakeBus([1, 2]));
95 const [a, b, c] = await Promise.all([
96 bus.read(1, STS3215.ID),
97 bus.read(2, STS3215.ID),
98 bus.ping(3),
99 ]);
100 expect([a, b, c]).toEqual([1, 2, null]);
101 });
102 });
103
104 describe("FeetechBus syncWrite ordering for playback", () => {
105 test("writes goal positions for every servo in one packet per frame", async () => {
106 const fake = new FakeBus([1, 2, 3]);
107 const bus = new FeetechBus(fake);
108 await bus.syncWrite(STS3215.Goal_Position, [
109 [1, 1000],
110 [2, 2000],
111 [3, 3000],
112 ]);
113 expect(fake.writes.map((w) => [w.id, w.bytes])).toEqual([
114 [1, [0xe8, 0x03]],
115 [2, [0xd0, 0x07]],
116 [3, [0xb8, 0x0b]],
117 ]);
118 });
119 });
Software/so101/src/bus.ts added +294
@@ -0,0 +1,294 @@
1 import { SerialPort } from "serialport";
2 import {
3 BROADCAST_ID,
4 FrameParser,
5 Instruction,
6 bytesEqual,
7 encodeInstruction,
8 type StatusPacket,
9 } from "./protocol.ts";
10 import { STS3215, decodeValue, encodeValue, type Register } from "./sts3215.ts";
11
12 export interface OpenOptions {
13 /** STS3215s ship at 1 Mbaud; LeRobot keeps them there. */
14 baudRate?: number;
15 }
16
17 export interface BusOptions {
18 /** The adapter hands our own bytes back to us. `open()` detects this; tests set it. */
19 echo?: boolean;
20 }
21
22 /**
23 * The slice of a serialport `SerialPort` the bus needs. Anything with this
24 * shape can stand in for the wire, which is how the tests run without servos.
25 */
26 export interface Transport {
27 readonly path: string;
28 readonly baudRate: number;
29 write(data: Uint8Array, callback: (error: Error | null | undefined) => void): unknown;
30 drain(callback: (error: Error | null | undefined) => void): unknown;
31 close(callback: (error: Error | null | undefined) => void): unknown;
32 on(event: "data", listener: (chunk: Uint8Array) => void): unknown;
33 on(event: "error", listener: (error: Error) => void): unknown;
34 }
35
36 export class NoResponseError extends Error {
37 readonly id: number;
38
39 constructor(id: number, what: string) {
40 super(`no response from servo ${id} (${what})`);
41 this.name = "NoResponseError";
42 this.id = id;
43 }
44 }
45
46 interface Pending {
47 readonly remaining: Set<number>;
48 readonly frames: StatusPacket[];
49 /** How many replies complete the transaction; fewer than `remaining` when any one will do. */
50 readonly need: number;
51 readonly finish: (timedOut: boolean) => void;
52 readonly fail: (error: Error) => void;
53 /** The echo probe is the one transaction that wants to see its own packet. */
54 readonly probe: boolean;
55 }
56
57 // A status packet is ~100 µs on the wire at 1 Mbaud, but USB CDC adapters
58 // hand bytes to the host in ~1 ms batches, so budget generously per motor.
59 export const BASE_TIMEOUT_MS = 15;
60 const PER_MOTOR_MS = 3;
61 // A servo that missed its deadline may still answer. Before addressing it
62 // again, wait this long so a stale frame cannot pass for the new reply.
63 const QUIET_MS = 2 * BASE_TIMEOUT_MS;
64 // An EEPROM write is acknowledged only after the cell is programmed, which
65 // on the bench has taken longer than a plain reply budget.
66 const EEPROM_TIMEOUT_MS = 100;
67
68 /**
69 * One half-duplex Feetech bus. Every public method is a complete
70 * request/response transaction; transactions are serialised internally
71 * because the bus can only carry one conversation at a time.
72 */
73 export class FeetechBus {
74 readonly path: string;
75 readonly baudRate: number;
76 readonly #port: Transport;
77 readonly #parser = new FrameParser();
78 readonly #stragglers = new Set<number>();
79 #queue: Promise<unknown> = Promise.resolve();
80 #pending: Pending | null = null;
81 #lastSent: Uint8Array | null = null;
82 #echo: boolean;
83 #echoPending = false;
84 #failure: Error | null = null;
85
86 constructor(transport: Transport, { echo = false }: BusOptions = {}) {
87 this.#port = transport;
88 this.path = transport.path;
89 this.baudRate = transport.baudRate;
90 this.#echo = echo;
91 transport.on("data", (chunk) => this.#receive(new Uint8Array(chunk)));
92 transport.on("error", (error) => {
93 // The port is gone (unplugged mid-write, say): fail what is in flight and everything after it.
94 this.#failure = error;
95 this.#pending?.fail(error);
96 });
97 }
98
99 static async open(path: string, { baudRate = 1_000_000 }: OpenOptions = {}): Promise<FeetechBus> {
100 const port = new SerialPort({ path, baudRate, autoOpen: false });
101 await new Promise<void>((resolve, reject) => port.open((err) => (err ? reject(err) : resolve())));
102 const bus = new FeetechBus(port);
103 await bus.detectEcho();
104 return bus;
105 }
106
107 close(): Promise<void> {
108 return new Promise((resolve) => this.#port.close(() => resolve()));
109 }
110
111 /** Whether the adapter echoes transmitted bytes back to the host. */
112 get echo(): boolean {
113 return this.#echo;
114 }
115
116 /**
117 * Send a SYNC_READ that names no servo. Nothing answers it, and its
118 * instruction byte (0x82) can never be a status packet's error byte, so
119 * the only way those bytes come back is the adapter echoing us.
120 */
121 async detectEcho(): Promise<boolean> {
122 const packet = encodeInstruction(BROADCAST_ID, Instruction.SyncRead, [
123 STS3215.Present_Position.address,
124 STS3215.Present_Position.size,
125 ]);
126 const [frame] = await this.#transact(packet, [BROADCAST_ID], BASE_TIMEOUT_MS, { probe: true });
127 this.#echo = frame !== undefined;
128 return this.#echo;
129 }
130
131 /** Resolves with the status packet, or null if nothing answered in time. */
132 async ping(id: number, timeoutMs = BASE_TIMEOUT_MS): Promise<StatusPacket | null> {
133 const [frame] = await this.#transact(encodeInstruction(id, Instruction.Ping), [id], timeoutMs);
134 return frame ?? null;
135 }
136
137 /** Ids that answered a ping, in ascending order. */
138 async scan(ids: Iterable<number> = range(0, BROADCAST_ID)): Promise<number[]> {
139 const found: number[] = [];
140 for (const id of ids) if (await this.ping(id)) found.push(id);
141 return found;
142 }
143
144 async readBlock(id: number, address: number, length: number): Promise<Uint8Array> {
145 const packet = encodeInstruction(id, Instruction.Read, [address, length]);
146 const [frame] = await this.#transact(packet, [id], BASE_TIMEOUT_MS);
147 if (!frame) throw new NoResponseError(id, `read ${length} bytes at ${address}`);
148 if (frame.params.length !== length) {
149 throw new Error(`servo ${id} returned ${frame.params.length} bytes, expected ${length}`);
150 }
151 return frame.params;
152 }
153
154 async read(id: number, register: Register): Promise<number> {
155 return decodeValue(register, await this.readBlock(id, register.address, register.size));
156 }
157
158 /**
159 * Write one register. EEPROM registers are wrapped in the sequence LeRobot
160 * uses: torque off, Lock=0, write, Lock=1. A servo acknowledges an ID write
161 * already wearing its new id, so that reply is accepted from either, and
162 * the trailing Lock write goes to the new id.
163 */
164 async write(id: number, register: Register, value: number): Promise<void> {
165 if (register.area !== "eeprom") {
166 await this.#writeRaw(id, register, value);
167 return;
168 }
169 await this.#writeRaw(id, STS3215.Torque_Enable, 0);
170 await this.#writeRaw(id, STS3215.Lock, 0);
171 const renaming = register === STS3215.ID;
172 await this.#writeRaw(id, register, value, renaming ? [id, value] : [id]);
173 await this.#writeRaw(renaming ? value : id, STS3215.Lock, 1);
174 }
175
176 /** Read the same register from many servos in one bus transaction. */
177 async syncRead(ids: readonly number[], register: Register): Promise<Map<number, number>> {
178 const packet = encodeInstruction(BROADCAST_ID, Instruction.SyncRead, [
179 register.address,
180 register.size,
181 ...ids,
182 ]);
183 const frames = await this.#transact(packet, ids, BASE_TIMEOUT_MS + PER_MOTOR_MS * ids.length);
184 const values = new Map<number, number>();
185 for (const frame of frames) {
186 if (frame.params.length === register.size) values.set(frame.id, decodeValue(register, frame.params));
187 }
188 return values;
189 }
190
191 /** Write the same register on many servos in one bus transaction. No status comes back. */
192 async syncWrite(register: Register, values: Iterable<readonly [number, number]>): Promise<void> {
193 const params = [register.address, register.size];
194 for (const [id, value] of values) params.push(id, ...encodeValue(register, value));
195 await this.#transact(encodeInstruction(BROADCAST_ID, Instruction.SyncWrite, params), [], 0);
196 }
197
198 async #writeRaw(id: number, register: Register, value: number, replyFrom: readonly number[] = [id]): Promise<void> {
199 const packet = encodeInstruction(id, Instruction.Write, [
200 register.address,
201 ...encodeValue(register, value),
202 ]);
203 const timeout = register.area === "eeprom" ? EEPROM_TIMEOUT_MS : BASE_TIMEOUT_MS;
204 const [frame] = await this.#transact(packet, replyFrom, timeout, { need: 1 });
205 if (!frame) throw new NoResponseError(id, `write ${value} at ${register.address}`);
206 }
207
208 #receive(chunk: Uint8Array): void {
209 for (const frame of this.#parser.push(chunk)) {
210 const pending = this.#pending;
211 if (!pending) continue;
212 const ours = this.#lastSent !== null && bytesEqual(frame.raw, this.#lastSent);
213 if (pending.probe) {
214 if (ours) {
215 pending.frames.push(frame);
216 pending.finish(false);
217 }
218 continue;
219 }
220 // On an echoing adapter the first copy of our own packet is the echo;
221 // a genuine reply can look identical (a ping answered with the voltage
222 // bit set), so only one such frame per transaction is dropped.
223 if (ours && this.#echoPending) {
224 this.#echoPending = false;
225 continue;
226 }
227 if (frame.error & 0x80) continue; // never a status packet: an echoed sync instruction
228 if (!pending.remaining.delete(frame.id)) continue;
229 pending.frames.push(frame);
230 if (pending.frames.length >= pending.need) pending.finish(false);
231 }
232 }
233
234 /**
235 * Send one packet and collect replies from `expectIds`, finishing early
236 * once `need` of them (default: all) have arrived or the timeout passes.
237 */
238 #transact(
239 packet: Uint8Array,
240 expectIds: readonly number[],
241 timeoutMs: number,
242 options: { probe?: boolean; need?: number } = {},
243 ): Promise<StatusPacket[]> {
244 const probe = options.probe ?? false;
245 const run = async (): Promise<StatusPacket[]> => {
246 if (this.#failure) throw this.#failure;
247 if (expectIds.some((id) => this.#stragglers.has(id))) {
248 await sleep(QUIET_MS);
249 this.#stragglers.clear();
250 }
251 this.#parser.reset();
252 this.#lastSent = packet;
253 this.#echoPending = this.#echo;
254 if (expectIds.length === 0) {
255 return new Promise((resolve, reject) =>
256 this.#port.write(packet, (err) => (err ? reject(err) : this.#port.drain(() => resolve([])))),
257 );
258 }
259 return new Promise((resolve, reject) => {
260 const remaining = new Set(expectIds);
261 const frames: StatusPacket[] = [];
262 let timer: ReturnType<typeof setTimeout> | undefined;
263 const settle = () => {
264 clearTimeout(timer);
265 if (this.#pending === pending) this.#pending = null;
266 };
267 const finish = (timedOut: boolean) => {
268 settle();
269 if (timedOut && !probe) for (const id of remaining) this.#stragglers.add(id);
270 resolve(frames);
271 };
272 const need = Math.min(options.need ?? expectIds.length, expectIds.length);
273 const fail = (error: Error) => {
274 settle();
275 reject(error);
276 };
277 const pending: Pending = { remaining, frames, need, finish, fail, probe };
278 this.#pending = pending;
279 this.#port.write(packet, (err) => err && fail(err));
280 timer = setTimeout(() => finish(true), timeoutMs);
281 });
282 };
283 const result = this.#queue.then(run, run);
284 this.#queue = result.then(noop, noop);
285 return result;
286 }
287 }
288
289 const noop = () => undefined;
290 const sleep = (ms: number) => new Promise<void>((resolve) => setTimeout(resolve, ms));
291
292 export function* range(start: number, end: number): Generator<number> {
293 for (let i = start; i < end; i++) yield i;
294 }
Software/so101/src/cli.ts added +638
@@ -0,0 +1,638 @@
1 import { closeSync, openSync, readFileSync, renameSync, writeFileSync } from "node:fs";
2 import { parseArgs } from "node:util";
3 import { SerialPort } from "serialport";
4 import { FeetechBus, range } from "./bus.ts";
5 import { describeError, hex } from "./protocol.ts";
6 import { MAX_FRAME_STEP, MotionError, moveToPose, name as jointName, playTrajectory, recordTrajectory, sleep } from "./motion.ts";
7 import { streamLeader } from "./leader.ts";
8 import { assertPoseName, parsePoseFile, serializePoseFile, type PoseFile } from "./poses.ts";
9 import { startServer } from "./server.ts";
10 import { durationSeconds, largestStep, parseTrajectory, serializeTrajectory } from "./trajectory.ts";
11 import {
12 BAUD_RATES,
13 JOINTS,
14 MODEL_NUMBER,
15 OperatingMode,
16 RESOLUTION,
17 STS3215,
18 decodeAt,
19 degreesToTicks,
20 encodeValue,
21 findRegister,
22 ticksToDegrees,
23 type Register,
24 } from "./sts3215.ts";
25
26 const USAGE = `so101 — talk to the Feetech STS3215 servos on an SO-101 bus
27
28 usage: so101 <command> [options]
29
30 ports list serial ports; * marks USB adapters
31 scan [--max 253] ping every id and show what answers
32 status [--ids 1-6] one-shot telemetry per servo
33 watch [--ids 1-6] [--hz 20] live positions — move the arm by hand
34 torque on|off [--ids 1-6] hold or release the servos
35 move <id> <target> [--deg] [--speed 500] [--acc 50] [--torque 100] [--hold]
36 go to a position within the servo's limits: ticks, or degrees with --deg;
37 --torque caps the effort and stays in force, --hold returns without waiting
38 read <id> <register|address> [--len 1]
39 read a register by its LeRobot name, or raw bytes
40 set-id <current> <new> re-address a servo (connect only that servo)
41 record <file> [--ids 1-6] [--hz 30]
42 release torque and sample the joints while you move the arm; ctrl-c saves
43 play <file> [--rate 1] [--release]
44 ease to the recording's first pose, then replay it at the recorded speed
45 pose save <name> [--ids 1-6] remember the current joint positions under a name (poses.json)
46 pose go <name> [--via <name>] [--speed 300] [--torque 70] [--release]
47 move every joint to a saved pose together; stops if any joint jams
48 pose list | pose delete <name>
49 serve [--listen 0.0.0.0:8101] [--file poses.json] [--model <dir with so101_new_calib.urdf>]
50 HTTP API + WebSocket target stream for this arm (run where the follower is)
51 leader --to ws://host:8101 [--hz 60]
52 read this leader arm and drive a remote follower through its serve
53
54 options:
55 --port <path> serial port (default: $SO101_PORT, else the only USB adapter)
56 --baud <rate> bus baud rate (default: 1000000)
57 `;
58
59 const OPTIONS = {
60 port: { type: "string" },
61 baud: { type: "string" },
62 ids: { type: "string" },
63 max: { type: "string" },
64 hz: { type: "string" },
65 deg: { type: "boolean" },
66 speed: { type: "string" },
67 acc: { type: "string" },
68 len: { type: "string" },
69 rate: { type: "string" },
70 release: { type: "boolean" },
71 file: { type: "string" },
72 via: { type: "string" },
73 torque: { type: "string" },
74 hold: { type: "boolean" },
75 listen: { type: "string" },
76 to: { type: "string" },
77 model: { type: "string" },
78 } as const;
79
80 interface Values {
81 port?: string;
82 baud?: string;
83 ids?: string;
84 max?: string;
85 hz?: string;
86 deg?: boolean;
87 speed?: string;
88 acc?: string;
89 len?: string;
90 rate?: string;
91 release?: boolean;
92 file?: string;
93 via?: string;
94 torque?: string;
95 hold?: boolean;
96 listen?: string;
97 model?: string;
98 to?: string;
99 }
100
101 type Command = (values: Values, positionals: string[]) => Promise<void>;
102
103 export async function main(argv: string[]): Promise<number> {
104 const [command, ...rest] = argv;
105 if (command === undefined || ["help", "--help", "-h"].includes(command)) {
106 process.stdout.write(USAGE);
107 return 0;
108 }
109 const handler = COMMANDS[command];
110 if (!handler) {
111 process.stderr.write(`unknown command: ${command}\n\n${USAGE}`);
112 return 2;
113 }
114 try {
115 const { values, positionals } = parseArgs({ args: rest, options: OPTIONS, allowPositionals: true });
116 await handler(values, positionals);
117 return 0;
118 } catch (err) {
119 const hint = err instanceof MotionError ? " (torque is still on: so101 torque off releases the arm)" : "";
120 process.stderr.write(`error: ${message(err)}${hint}\n`);
121 return 1;
122 }
123 }
124
125 function message(err: unknown): string {
126 return (err instanceof Error ? err.message : String(err)).replace(/^Error: /, "");
127 }
128
129 const ADAPTER = /usbmodem|usbserial|ttyACM|ttyUSB/i;
130
131 const COMMANDS: Record<string, Command> = {
132 ports: async () => {
133 const list = await SerialPort.list();
134 if (list.length === 0) {
135 console.log("no serial ports found");
136 return;
137 }
138 let adapters = 0;
139 for (const port of list) {
140 const adapter = ADAPTER.test(port.path);
141 adapters += adapter ? 1 : 0;
142 const vendor = port.vendorId ? `${port.vendorId}:${port.productId}` : "";
143 const path = calloutDevice(port.path);
144 console.log(`${adapter ? "*" : " "} ${[path, port.manufacturer, vendor].filter(Boolean).join(" ")}`);
145 }
146 console.log(
147 adapters
148 ? "\n* looks like a USB serial adapter; pass it with --port or SO101_PORT"
149 : "\nnone of these look like a USB serial adapter — is the servo board plugged in?",
150 );
151 },
152
153 scan: (values) => {
154 const max = servoId(values.max ?? String(MAX_ID), "--max");
155 return withBus(values, async (bus) => {
156 console.log(`scanning ids 0-${max} on ${bus.path} at ${bus.baudRate} baud…`);
157 const rows: string[][] = [];
158 for (const id of range(0, max + 1)) {
159 if (!(await bus.ping(id))) continue;
160 try {
161 const block = await bus.readBlock(id, 0, 7);
162 const model = decodeAt(block, 0, STS3215.Model_Number);
163 const position = await bus.read(id, STS3215.Present_Position);
164 rows.push([
165 String(id),
166 JOINTS[id] ?? "",
167 model === MODEL_NUMBER ? "STS3215" : `model ${model}`,
168 `${block[0]}.${block[1]}`,
169 String(BAUD_RATES[block[6]!] ?? `code ${block[6]}`),
170 String(position),
171 ]);
172 } catch (err) {
173 rows.push([String(id), JOINTS[id] ?? "", `answered the ping but not the read: ${message(err)}`]);
174 }
175 }
176 if (rows.length === 0) {
177 console.log("nothing answered — check power to the servo board and the cable to the servo");
178 return;
179 }
180 console.log(table([["id", "joint", "model", "fw", "baud", "position"], ...rows]));
181 });
182 },
183
184 status: (values) => {
185 const ids = parseIds(values.ids);
186 return withBus(values, async (bus) => {
187 const rows: string[][] = [];
188 const start = STS3215.Operating_Mode.address;
189 const end = STS3215.Present_Current.address + STS3215.Present_Current.size;
190 for (const id of ids) {
191 const ping = await bus.ping(id);
192 if (!ping) {
193 rows.push([String(id), JOINTS[id] ?? "", "no response"]);
194 continue;
195 }
196 let block: Uint8Array;
197 try {
198 block = await bus.readBlock(id, start, end - start);
199 } catch (err) {
200 rows.push([String(id), JOINTS[id] ?? "", `answered the ping but not the read: ${message(err)}`]);
201 continue;
202 }
203 const at = (register: Register) => decodeAt(block, start, register);
204 const position = at(STS3215.Present_Position);
205 rows.push([
206 String(id),
207 JOINTS[id] ?? "",
208 String(position),
209 ticksToDegrees(position).toFixed(1),
210 String(at(STS3215.Present_Velocity)),
211 `${(at(STS3215.Present_Load) / 10).toFixed(1)}%`,
212 `${Math.round(at(STS3215.Present_Current) * 6.5)}mA`,
213 `${(at(STS3215.Present_Voltage) / 10).toFixed(1)}V`,
214 `${at(STS3215.Present_Temperature)}°C`,
215 at(STS3215.Torque_Enable) ? "on" : "off",
216 modeName(at(STS3215.Operating_Mode)),
217 describeError(at(STS3215.Status) | ping.error),
218 ]);
219 }
220 console.log(
221 table([
222 ["id", "joint", "pos", "deg", "vel", "load", "current", "volt", "temp", "torque", "mode", "status"],
223 ...rows,
224 ]),
225 );
226 });
227 },
228
229 watch: (values) => {
230 const ids = parseIds(values.ids);
231 const hz = bounded(integer(values.hz ?? "20", "--hz"), 1, 100, "--hz");
232 return withBus(values, async (bus) => {
233 const period = 1000 / hz;
234 let running = true;
235 const stop = () => {
236 running = false;
237 };
238 process.once("SIGINT", stop);
239 console.log(`watching ${ids.join(",")} at ${hz} Hz — ctrl-c to stop\n`);
240 let lines = 0;
241 while (running) {
242 const started = performance.now();
243 const positions = await bus.syncRead(ids, STS3215.Present_Position);
244 const rows = ids.map((id) => {
245 const position = positions.get(id);
246 const label = `${String(id).padStart(2)} ${(JOINTS[id] ?? "").padEnd(13)}`;
247 if (position === undefined) return `${label} —`;
248 const degrees = ticksToDegrees(position).toFixed(1).padStart(6);
249 return `${label} ${String(position).padStart(4)} ${degrees}° ${bar(position)}`;
250 });
251 const up = lines ? `\x1b[${lines}A` : "";
252 process.stdout.write(`${up}${rows.map((row) => `\x1b[2K${row}`).join("\n")}\n`);
253 lines = rows.length;
254 const remaining = period - (performance.now() - started);
255 if (remaining > 0) await sleep(remaining);
256 }
257 process.off("SIGINT", stop);
258 });
259 },
260
261 torque: (values, [state]) => {
262 if (state !== "on" && state !== "off") throw new Error("usage: so101 torque on|off [--ids 1-6]");
263 const ids = parseIds(values.ids);
264 return withBus(values, async (bus) => {
265 if (state === "on") {
266 // Hold where each joint is now; a stale goal from an earlier run must not become a lunge.
267 const positions = await bus.syncRead(ids, STS3215.Present_Position);
268 const present = ids.filter((id) => positions.has(id));
269 await bus.syncWrite(STS3215.Goal_Position, present.map((id) => [id, positions.get(id)!] as const));
270 }
271 await bus.syncWrite(
272 STS3215.Torque_Enable,
273 ids.map((id) => [id, state === "on" ? 1 : 0] as const),
274 );
275 console.log(`torque ${state} for ${ids.join(",")}`);
276 });
277 },
278
279 move: (values, [idArg, targetArg]) => {
280 if (idArg === undefined || targetArg === undefined) {
281 throw new Error("usage: so101 move <id> <target> [--deg] [--speed 500] [--acc 50]");
282 }
283 const id = servoId(idArg, "id");
284 const target = values.deg ? degreesToTicks(finite(targetArg, "target")) : integer(targetArg, "target");
285 bounded(target, 0, RESOLUTION - 1, "target");
286 // Goal_Velocity 0 means "as fast as the servo can", so it is not accepted here.
287 const speed = bounded(integer(values.speed ?? "500", "--speed"), 1, 32767, "--speed");
288 const acc = bounded(integer(values.acc ?? "50", "--acc"), 0, 255, "--acc");
289 const torque = bounded(integer(values.torque ?? "100", "--torque"), 5, 100, "--torque");
290 // Every value the move will write is encoded here, so nothing can fail after torque is on.
291 encodeValue(STS3215.Goal_Velocity, speed);
292 encodeValue(STS3215.Acceleration, acc);
293 encodeValue(STS3215.Goal_Position, target);
294 encodeValue(STS3215.Torque_Limit, torque * 10);
295 return withBus(values, async (bus) => {
296 const mode = await bus.read(id, STS3215.Operating_Mode);
297 if (mode !== OperatingMode.Position) {
298 throw new Error(`servo ${id} is in ${modeName(mode)} mode, not position mode`);
299 }
300 const min = await bus.read(id, STS3215.Min_Position_Limit);
301 const max = await bus.read(id, STS3215.Max_Position_Limit);
302 if (target < min || target > max) {
303 throw new Error(`servo ${id} allows ${min}-${max} ticks (${ticksToDegrees(min).toFixed(1)}°-${ticksToDegrees(max).toFixed(1)}°); ${target} is outside`);
304 }
305 const from = await bus.read(id, STS3215.Present_Position);
306 // Park the goal where the servo is before enabling torque, so a stale goal cannot become a lunge.
307 await bus.write(id, STS3215.Goal_Position, from);
308 await bus.write(id, STS3215.Torque_Limit, torque * 10);
309 await bus.write(id, STS3215.Torque_Enable, 1);
310 await bus.write(id, STS3215.Acceleration, acc);
311 await bus.write(id, STS3215.Goal_Velocity, speed);
312 await bus.write(id, STS3215.Goal_Position, target);
313 console.log(`servo ${id}: ${from} → ${target} ticks (${ticksToDegrees(target).toFixed(1)}°) at ${speed} ticks/s${torque < 100 ? `, torque capped at ${torque}%` : ""}`);
314 if (values.hold) return;
315
316 const deadline = performance.now() + 10_000;
317 let position = from;
318 do {
319 await sleep(50);
320 position = await bus.read(id, STS3215.Present_Position);
321 if (!(await bus.read(id, STS3215.Moving))) break;
322 } while (performance.now() < deadline);
323 console.log(`servo ${id}: now at ${position} ticks (${ticksToDegrees(position).toFixed(1)}°), torque still on`);
324 });
325 },
326
327 read: (values, [idArg, what]) => {
328 if (idArg === undefined || what === undefined) {
329 throw new Error("usage: so101 read <id> <register|address> [--len 1]");
330 }
331 const id = servoId(idArg, "id");
332 const named = findRegister(what);
333 if (!named && !/^\d+$/.test(what)) {
334 throw new Error(`unknown register "${what}"; names follow LeRobot's table (Present_Position, Goal_Position, Torque_Enable, …) or pass an address`);
335 }
336 const address = named ? named[1].address : bounded(integer(what, "register"), 0, 255, "register");
337 const length = named ? named[1].size : bounded(integer(values.len ?? "1", "--len"), 1, 256 - address, "--len");
338 return withBus(values, async (bus) => {
339 const bytes = await bus.readBlock(id, address, length);
340 if (named) {
341 console.log(`${named[0]} @${address} = ${decodeAt(bytes, address, named[1])} [${hex(bytes)}]`);
342 return;
343 }
344 const value = Array.from(bytes).reduce((sum, byte, index) => sum + byte * 256 ** index, 0);
345 console.log(`@${address}+${length} = ${value} [${hex(bytes)}]`);
346 });
347 },
348
349 record: (values, [file]) => {
350 if (file === undefined) throw new Error("usage: so101 record <file> [--ids 1-6] [--hz 30]");
351 const ids = parseIds(values.ids);
352 const hz = bounded(integer(values.hz ?? "30", "--hz"), 1, 100, "--hz");
353 // Claim the file now: refuse to overwrite anything, and find an unwritable path before the recording, not after.
354 try {
355 closeSync(openSync(file, "wx"));
356 } catch (err) {
357 throw new Error((err as NodeJS.ErrnoException).code === "EEXIST" ? `${file} already exists; pick another name` : `cannot create ${file}: ${message(err)}`);
358 }
359 return withBus(values, async (bus) => {
360 console.log("releasing torque in 2 s — support the arm");
361 await sleep(2000);
362 const interrupt = onInterrupt();
363 console.log(`torque off on ${ids.join(",")}; recording at ${hz} Hz — move the arm, ctrl-c when done`);
364 const { trajectory, filled } = await recordTrajectory(bus, ids, {
365 hz,
366 signal: interrupt.signal,
367 onFrame: (count) => {
368 if (count % hz === 0) process.stdout.write(`\r${(count / hz).toFixed(0)} s, ${count} frames`);
369 },
370 });
371 interrupt.dispose();
372 if (trajectory.frames.length === 0) throw new Error("nothing recorded — did the servos answer?");
373 writeFileSync(`${file}.tmp`, serializeTrajectory(trajectory));
374 renameSync(`${file}.tmp`, file);
375 console.log(`\nsaved ${trajectory.frames.length} frames (${(trajectory.frames.length / hz).toFixed(1)} s) to ${file}`);
376 if (filled) console.log(`note: ${filled} frames had a missed reply filled from the previous frame`);
377 });
378 },
379
380 play: (values, [file]) => {
381 if (file === undefined) throw new Error("usage: so101 play <file> [--rate 1] [--release]");
382 const rate = finite(values.rate ?? "1", "--rate");
383 if (rate <= 0 || rate > 2) throw new Error("--rate must be within 0-2");
384 const trajectory = parseTrajectory(readFileSync(file, "utf8"));
385 const ids = [...trajectory.ids];
386 const step = largestStep(trajectory);
387 if (step > MAX_FRAME_STEP) {
388 throw new Error(`${file} jumps ${step} ticks between two frames; a hand-made recording never does that, so refusing to replay it`);
389 }
390 const torque = bounded(integer(values.torque ?? "70", "--torque"), 10, 100, "--torque");
391 return withBus(values, async (bus) => {
392 const interrupt = onInterrupt();
393 console.log(`easing to the first pose of ${file} (${durationSeconds(trajectory).toFixed(1)} s at rate ${rate})`);
394 let played = 0;
395 try {
396 played = await playTrajectory(bus, trajectory, { rate, torquePercent: torque, signal: interrupt.signal, onFrame: (index) => (played = index) });
397 } finally {
398 interrupt.dispose();
399 }
400 if (values.release) {
401 await bus.syncWrite(STS3215.Torque_Enable, ids.map((id) => [id, 0] as const));
402 console.log(`played ${played} of ${trajectory.frames.length} frames; torque released`);
403 } else {
404 console.log(`played ${played} of ${trajectory.frames.length} frames; holding the final pose, torque on (so101 torque off to release)`);
405 }
406 });
407 },
408
409 pose: (values, [action, name]) => {
410 const file = values.file ?? "poses.json";
411 const load = (): PoseFile => {
412 try {
413 return parsePoseFile(readFileSync(file, "utf8"));
414 } catch (err) {
415 if ((err as NodeJS.ErrnoException).code === "ENOENT") return {};
416 throw new Error(`${file}: ${message(err)}`);
417 }
418 };
419 const need = (poses: PoseFile, wanted: string | undefined) => {
420 if (wanted === undefined) throw new Error("which pose? give its name");
421 assertPoseName(wanted);
422 const pose = Object.hasOwn(poses, wanted) ? poses[wanted] : undefined;
423 if (!pose) throw new Error(`no pose "${wanted}" in ${file}${Object.keys(poses).length ? ` (have: ${Object.keys(poses).join(", ")})` : ""}`);
424 return pose;
425 };
426
427 switch (action) {
428 case "list": {
429 const poses = load();
430 const names = Object.keys(poses);
431 if (!names.length) return Promise.resolve(console.log(`no poses in ${file}`));
432 console.log(table([["name", "ids", "positions", "saved"], ...names.map((n) => [n, poses[n]!.ids.join(","), poses[n]!.positions.join(" "), poses[n]!.saved ?? ""])]));
433 return Promise.resolve();
434 }
435 case "delete": {
436 const poses = load();
437 need(poses, name);
438 const { [name!]: _, ...rest } = poses;
439 writeFileSync(file, serializePoseFile(rest));
440 console.log(`deleted "${name}" from ${file}`);
441 return Promise.resolve();
442 }
443 case "save": {
444 if (name === undefined) throw new Error("usage: so101 pose save <name> [--ids 1-6]");
445 assertPoseName(name);
446 const ids = parseIds(values.ids);
447 return withBus(values, async (bus) => {
448 const positions = await bus.syncRead(ids, STS3215.Present_Position);
449 const missing = ids.filter((id) => !positions.has(id));
450 if (missing.length) throw new Error(`no response from servo ${missing.join(", ")}`);
451 const poses = { ...load(), [name]: { ids, positions: ids.map((id) => positions.get(id)!), saved: new Date().toISOString() } };
452 writeFileSync(file, serializePoseFile(poses));
453 console.log(`saved "${name}": ${ids.map((id) => `${JOINTS[id] ?? id}=${positions.get(id)}`).join(" ")}`);
454 const onStops: string[] = [];
455 for (const id of ids) {
456 const min = await bus.read(id, STS3215.Min_Position_Limit);
457 const max = await bus.read(id, STS3215.Max_Position_Limit);
458 const pos = positions.get(id)!;
459 if (pos - min < 40 || max - pos < 40) onStops.push(JOINTS[id] ?? String(id));
460 }
461 if (onStops.length) console.log(`note: ${onStops.join(", ")} resting on a hard stop; "go" will aim 30 ticks inside the limit instead of pushing into it`);
462 });
463 }
464 case "go": {
465 const poses = load();
466 const target = need(poses, name);
467 const via = values.via === undefined ? undefined : need(poses, values.via);
468 const speed = bounded(integer(values.speed ?? "300", "--speed"), 20, 1500, "--speed");
469 const torque = bounded(integer(values.torque ?? "70", "--torque"), 10, 100, "--torque");
470 return withBus(values, async (bus) => {
471 const interrupt = onInterrupt();
472 try {
473 for (const [label, pose] of [...(via ? [["via " + values.via, via] as const] : []), [name!, target] as const]) {
474 const result = await moveToPose(bus, pose.ids, pose.positions, { speed, torquePercent: torque, signal: interrupt.signal });
475 if (result.adjusted.size) {
476 console.log(`aimed inside the limits for ${[...result.adjusted].map(([id, aim]) => `${jointName(id)}→${aim}`).join(", ")}`);
477 }
478 console.log(`reached "${label}": ${pose.ids.map((id) => `${jointName(id)}=${result.final.get(id)}`).join(" ")}`);
479 }
480 } finally {
481 interrupt.dispose();
482 }
483 if (values.release) {
484 const all = [...new Set([...(via?.ids ?? []), ...target.ids])];
485 await bus.syncWrite(STS3215.Torque_Enable, all.map((id) => [id, 0] as const));
486 console.log("torque released");
487 } else {
488 console.log("holding, torque on (so101 torque off to release)");
489 }
490 });
491 }
492 default:
493 throw new Error("usage: so101 pose save|go|list|delete <name>");
494 }
495 },
496
497 serve: (values) => {
498 const [host, portText] = (values.listen ?? "0.0.0.0:8101").split(":");
499 const port = bounded(integer(portText ?? "8101", "--listen port"), 1, 65535, "--listen port");
500 return withBus(values, async (bus) => {
501 const server = await startServer(bus, { host, port, posesFile: values.file ?? "poses.json", modelDir: values.model });
502 console.log(`serving ${bus.path} on http://${host}:${server.port} — open it in a browser; the API is listed in src/server.ts`);
503 console.log("ctrl-c to stop");
504 await new Promise<void>((resolve) => process.once("SIGINT", () => resolve()));
505 await server.close();
506 await bus.syncWrite(STS3215.Torque_Enable, [1, 2, 3, 4, 5, 6].map((id) => [id, 0] as const));
507 console.log("stopped; torque released");
508 });
509 },
510
511 leader: (values) => {
512 if (!values.to) throw new Error("usage: so101 leader --to ws://host:8101 [--hz 60]");
513 const hz = bounded(integer(values.hz ?? "60", "--hz"), 1, 120, "--hz");
514 return withBus(values, async (bus) => {
515 const interrupt = onInterrupt();
516 console.log(`streaming this leader to ${values.to} at ${hz} Hz — ctrl-c to stop`);
517 let frames = 0;
518 const sent = await streamLeader(bus, values.to!, {
519 hz,
520 signal: interrupt.signal,
521 onFrame: () => {
522 if (++frames % hz === 0) process.stdout.write(`\r${(frames / hz).toFixed(0)} s`);
523 },
524 });
525 interrupt.dispose();
526 console.log(`\nstopped after ${sent} frames; the follower is holding`);
527 });
528 },
529
530 "set-id": (values, [currentArg, nextArg]) => {
531 if (currentArg === undefined || nextArg === undefined) throw new Error("usage: so101 set-id <current> <new>");
532 const current = servoId(currentArg, "current id");
533 const next = servoId(nextArg, "new id");
534 return withBus(values, async (bus) => {
535 if (!(await bus.ping(current))) throw new Error(`servo ${current} did not answer`);
536 if (next !== current && (await bus.ping(next))) {
537 throw new Error(`a servo already answers as ${next}; connect only the servo you are re-addressing`);
538 }
539 await bus.write(current, STS3215.ID, next);
540 if (!(await bus.ping(next))) throw new Error(`servo did not come back as ${next}`);
541 console.log(`servo ${current} is now ${next}${JOINTS[next] ? ` (${JOINTS[next]})` : ""}`);
542 });
543 },
544 };
545
546 /** A recording made by hand never moves a joint this far between two samples. */
547
548 /** Ctrl-C becomes an abort signal the supervisor checks on every poll, so the arm stops where it is instead of running on unsupervised. */
549 function onInterrupt(): { signal: AbortSignal; dispose: () => void } {
550 const controller = new AbortController();
551 const handler = () => controller.abort();
552 process.once("SIGINT", handler);
553 return { signal: controller.signal, dispose: () => process.off("SIGINT", handler) };
554 }
555
556 async function withBus<T>(values: Values, fn: (bus: FeetechBus) => Promise<T>): Promise<T> {
557 const path = values.port ?? process.env.SO101_PORT ?? (await detectPort());
558 const baudRate = values.baud ? integer(values.baud, "--baud") : 1_000_000;
559 const bus = await FeetechBus.open(path, { baudRate });
560 try {
561 return await fn(bus);
562 } finally {
563 await bus.close();
564 }
565 }
566
567 async function detectPort(): Promise<string> {
568 const candidates = (await SerialPort.list())
569 .map((port) => port.path)
570 .filter((path) => ADAPTER.test(path))
571 .map(calloutDevice);
572 if (candidates.length === 1) return candidates[0]!;
573 if (candidates.length === 0) {
574 throw new Error("no USB serial adapter found — plug in the servo board, or pass --port");
575 }
576 throw new Error(`several adapters found, pick one with --port:\n ${candidates.join("\n ")}`);
577 }
578
579 /** macOS exposes each port twice; the cu.* node is the right one for a host-initiated link. */
580 function calloutDevice(path: string): string {
581 return process.platform === "darwin" ? path.replace(/^\/dev\/tty\./, "/dev/cu.") : path;
582 }
583
584 /** Servo ids run 0-253; 254 is the broadcast address. */
585 const MAX_ID = 253;
586
587 /** "1-6", "1,3,5" or "1-3,6". */
588 function parseIds(spec = "1-6"): number[] {
589 const ids = new Set<number>();
590 for (const part of spec.split(",")) {
591 const match = /^(\d+)(?:-(\d+))?$/.exec(part);
592 if (!match) throw new Error(`--ids takes ids and ranges like 1-3,6, got "${spec}"`);
593 const lo = servoId(match[1]!, "--ids");
594 const hi = match[2] === undefined ? lo : servoId(match[2], "--ids");
595 if (hi < lo) throw new Error(`--ids range ${part} runs backwards`);
596 for (let id = lo; id <= hi; id++) ids.add(id);
597 }
598 return [...ids];
599 }
600
601 function finite(text: string, what: string): number {
602 const value = text.trim() === "" ? Number.NaN : Number(text);
603 if (!Number.isFinite(value)) throw new Error(`${what} must be a number, got "${text}"`);
604 return value;
605 }
606
607 function integer(text: string, what: string): number {
608 const value = finite(text, what);
609 if (!Number.isInteger(value)) throw new Error(`${what} must be an integer, got "${text}"`);
610 return value;
611 }
612
613 function bounded(value: number, min: number, max: number, what: string): number {
614 if (value < min || value > max) throw new Error(`${what} must be within ${min}-${max}, got ${value}`);
615 return value;
616 }
617
618 function servoId(text: string, what: string): number {
619 return bounded(integer(text, what), 0, MAX_ID, what);
620 }
621
622 function modeName(mode: number): string {
623 const entry = Object.entries(OperatingMode).find(([, value]) => value === mode);
624 return entry ? entry[0].toLowerCase() : `mode ${mode}`;
625 }
626
627 function bar(position: number, width = 32): string {
628 const filled = Math.round((position / (RESOLUTION - 1)) * width);
629 return `${"█".repeat(filled)}${"·".repeat(width - filled)}`;
630 }
631
632 function table(rows: string[][]): string {
633 const widths = rows[0]!.map((_, column) => Math.max(...rows.map((row) => (row[column] ?? "").length)));
634 return rows
635 .map((row) => widths.map((width, i) => (row[i] ?? "").padEnd(width)).join(" ").trimEnd())
636 .join("\n");
637 }
638
Software/so101/src/fake-bus.ts added +151
@@ -0,0 +1,151 @@
1 import { BROADCAST_ID, FrameParser, Instruction, checksum } from "./protocol.ts";
2 import { STS3215 } from "./sts3215.ts";
3 import type { Transport } from "./bus.ts";
4
5 export interface FakeOptions {
6 /** Hand every transmitted packet back before any reply, like a looped-back adapter. */
7 echo?: boolean;
8 /** Ids that never answer. */
9 drop?: Set<number>;
10 /** Ids whose first reply is held back this many ms (later replies are prompt). */
11 lateOnce?: Map<number, number>;
12 /** Error byte every ping reply carries. */
13 pingError?: number;
14 /** Ids whose output never moves and that report heavy load when commanded, like a jammed joint. */
15 stuck?: Set<number>;
16 /** Stuck ids wobble by ±3 ticks between reads, like a joint chattering against an obstacle. */
17 jitter?: boolean;
18 /** Ids that go silent after answering this many more times (deterministic mid-move dropout). */
19 dropAfter?: Map<number, number>;
20 }
21
22 /** Enough of an STS3215 to answer the bus: a memory map per id, replies by instruction. */
23 export class FakeBus implements Transport {
24 readonly path = "fake";
25 readonly baudRate = 1_000_000;
26 readonly servos = new Map<number, Uint8Array>();
27 readonly writes: Array<{ id: number; address: number; bytes: number[] }> = [];
28 #listener: ((chunk: Uint8Array) => void) | undefined;
29 readonly #parser = new FrameParser();
30 readonly #options: FakeOptions;
31
32 constructor(ids: number[], options: FakeOptions = {}) {
33 this.#options = options;
34 for (const id of ids) {
35 const memory = new Uint8Array(100);
36 memory[3] = 777 & 0xff;
37 memory[4] = 777 >> 8;
38 memory[5] = id;
39 memory[11] = 0xff;
40 memory[12] = 0x0f; // Max_Position_Limit 4095 (Min stays 0)
41 memory[16] = 0xe8;
42 memory[17] = 0x03; // Max_Torque_Limit 1000
43 memory[48] = 0xe8;
44 memory[49] = 0x03; // Torque_Limit 1000
45 memory[56] = 0x00;
46 memory[57] = 0x08; // Present_Position 2048
47 memory[58] = 16; // Present_Velocity 16
48 this.servos.set(id, memory);
49 }
50 }
51
52 on(event: "data" | "error", listener: ((chunk: Uint8Array) => void) | ((error: Error) => void)): void {
53 if (event === "data") this.#listener = listener as (chunk: Uint8Array) => void;
54 }
55
56 write(data: Uint8Array, callback: (error: Error | null | undefined) => void): void {
57 callback(null);
58 const replies: Array<[number, Uint8Array]> = this.#options.echo ? [[0, data]] : [];
59 const replyFrom = (id: number, frame: Uint8Array) => {
60 const left = this.#options.dropAfter?.get(id);
61 if (left !== undefined) {
62 if (left <= 0) {
63 (this.#options.drop ??= new Set()).add(id);
64 return;
65 }
66 this.#options.dropAfter!.set(id, left - 1);
67 }
68 const delay = this.#options.lateOnce?.get(id) ?? 0;
69 this.#options.lateOnce?.delete(id);
70 replies.push([delay, frame]);
71 };
72 // Instruction packets have the status-packet shape, so the parser reads them too.
73 for (const packet of this.#parser.push(data)) {
74 const instruction = packet.error;
75 const targets = packet.id === BROADCAST_ID ? [...this.servos.keys()].sort((a, b) => a - b) : [packet.id];
76 for (const id of targets) {
77 const memory = this.servos.get(id);
78 if (!memory) continue;
79 // A dropped servo still hears writes (and acts on them); it just never answers.
80 const reply = this.#options.drop?.has(id) ? () => undefined : replyFrom;
81 if (instruction === Instruction.Ping) reply(id, status(id, [], this.#options.pingError ?? 0));
82 if (instruction === Instruction.Read) {
83 const [address, length] = packet.params;
84 reply(id, status(id, [...memory.subarray(address!, address! + length!)]));
85 }
86 if (this.#options.jitter && this.#options.stuck?.has(id) && (instruction === Instruction.Read || instruction === Instruction.SyncRead)) {
87 memory[56] = (memory[56]! + 3) % 7; // wobble within 0-6 ticks of zero without ever settling
88 }
89 if (instruction === Instruction.Write) {
90 const [address, ...bytes] = packet.params;
91 memory.set(bytes, address!);
92 this.writes.push({ id, address: address!, bytes });
93 this.#follow(id, address!);
94 if (address === STS3215.ID.address) {
95 this.#rekey(id, bytes[0]!);
96 reply(bytes[0]!, status(bytes[0]!, [])); // a real STS3215 answers wearing its new id
97 } else {
98 reply(id, status(id, []));
99 }
100 }
101 if (instruction === Instruction.SyncRead && packet.id === BROADCAST_ID) {
102 const [address, length, ...ids] = packet.params;
103 if (ids.includes(id)) reply(id, status(id, [...memory.subarray(address!, address! + length!)]));
104 }
105 if (instruction === Instruction.SyncWrite && packet.id === BROADCAST_ID) {
106 const [address, length, ...rest] = packet.params;
107 for (let i = 0; i < rest.length; i += 1 + length!) {
108 if (rest[i] !== id) continue;
109 const bytes = rest.slice(i + 1, i + 1 + length!);
110 memory.set(bytes, address!);
111 this.writes.push({ id, address: address!, bytes });
112 this.#follow(id, address!);
113 }
114 }
115 }
116 }
117 for (const [delay, frame] of replies) setTimeout(() => this.#listener?.(frame), delay);
118 }
119
120 drain(callback: (error: Error | null | undefined) => void): void {
121 callback(null);
122 }
123
124 close(callback: (error: Error | null | undefined) => void): void {
125 callback(null);
126 }
127
128 /** A written goal becomes the present position at once, unless the servo is stuck, which then reports 90 % load. */
129 #follow(id: number, address: number): void {
130 if (address !== STS3215.Goal_Position.address) return;
131 const memory = this.servos.get(id)!;
132 if (this.#options.stuck?.has(id)) {
133 memory[60] = 900 & 0xff;
134 memory[61] = 900 >> 8;
135 return;
136 }
137 memory[56] = memory[42]!;
138 memory[57] = memory[43]!;
139 }
140
141 #rekey(from: number, to: number): void {
142 const memory = this.servos.get(from)!;
143 this.servos.delete(from);
144 this.servos.set(to, memory);
145 }
146 }
147
148 function status(id: number, params: number[], error = 0): Uint8Array {
149 const body = [id, params.length + 2, error, ...params];
150 return Uint8Array.from([0xff, 0xff, ...body, checksum(body)]);
151 }
Software/so101/src/fake-serve.test.ts added +43
@@ -0,0 +1,43 @@
1 import { afterEach, expect, test } from "bun:test";
2 import { existsSync, mkdtempSync, rmSync } from "node:fs";
3 import { tmpdir } from "node:os";
4 import { join } from "node:path";
5
6 const script = new URL("../bench/fake-serve.ts", import.meta.url).pathname;
7 let child: ReturnType<typeof Bun.spawn> | undefined;
8 let dir: string | undefined;
9
10 afterEach(() => {
11 child?.kill();
12 if (dir) rmSync(dir, { recursive: true, force: true });
13 child = dir = undefined;
14 });
15
16 /** Start the fake arm with this environment and wait for its "fake arm on http://host:port" line. */
17 async function start(env: Record<string, string>): Promise<{ host: string; port: number; line: string }> {
18 child = Bun.spawn(["bun", "run", script], { env: { ...process.env, ...env }, stdout: "pipe", stderr: "inherit" });
19 const reader = (child.stdout as ReadableStream<Uint8Array>).getReader();
20 let text = "";
21 while (!text.includes("\n")) {
22 const { value, done } = await reader.read();
23 if (done) throw new Error(`fake-serve exited: ${text}`);
24 text += new TextDecoder().decode(value);
25 }
26 const match = /http:\/\/([^:]+):(\d+)/.exec(text);
27 if (!match) throw new Error(`no address in: ${text}`);
28 return { host: match[1]!, port: Number(match[2]), line: text };
29 }
30
31 test("fake-serve listens on PORT and HOST and keeps its data under FORGE_APP_DATA", async () => {
32 dir = mkdtempSync(join(tmpdir(), "so101-appdata-"));
33 const { host, port, line } = await start({ PORT: "0", HOST: "127.0.0.1", FORGE_APP_DATA: dir });
34 expect(host).toBe("127.0.0.1");
35 expect(port).toBeGreaterThan(0);
36 expect(line).toContain(join(dir, "so101"));
37
38 const status = await fetch(`http://127.0.0.1:${port}/status`);
39 expect(status.status).toBe(200);
40 const saved = await fetch(`http://127.0.0.1:${port}/pose/save`, { method: "POST", headers: { "content-type": "application/json" }, body: JSON.stringify({ name: "here" }) });
41 expect(saved.status).toBe(200);
42 expect(existsSync(join(dir, "so101", "poses.json"))).toBe(true);
43 }, 20_000);
Software/so101/src/fk-fixtures.json added +440
@@ -0,0 +1,440 @@
1 [
2 {
3 "q_deg": [
4 0.0,
5 0.0,
6 0.0,
7 0.0,
8 0.0,
9 0.0
10 ],
11 "T_base_gripper_frame": [
12 [
13 8.7e-06,
14 -1.03e-05,
15 1.0,
16 0.3913615
17 ],
18 [
19 0.0486629,
20 0.9988153,
21 9.9e-06,
22 -9.2e-06
23 ],
24 [
25 -0.9988153,
26 0.0486629,
27 9.2e-06,
28 0.2264697
29 ],
30 [
31 0.0,
32 0.0,
33 0.0,
34 1.0
35 ]
36 ]
37 },
38 {
39 "q_deg": [
40 30.0,
41 -40.0,
42 50.0,
43 20.0,
44 10.0,
45 5.0
46 ],
47 "T_base_gripper_frame": [
48 [
49 -0.4923424,
50 0.4416853,
51 0.7500087,
52 0.2522536
53 ],
54 [
55 0.139313,
56 0.8905606,
57 -0.4330054,
58 -0.1248154
59 ],
60 [
61 -0.8591803,
62 -0.108701,
63 -0.4999932,
64 0.1161503
65 ],
66 [
67 0.0,
68 0.0,
69 0.0,
70 1.0
71 ]
72 ]
73 },
74 {
75 "q_deg": [
76 -60.0,
77 20.0,
78 -30.0,
79 -45.0,
80 90.0,
81 40.0
82 ],
83 "T_base_gripper_frame": [
84 [
85 0.8849344,
86 0.366947,
87 0.2867769,
88 0.2051419
89 ],
90 [
91 -0.4648788,
92 0.7329032,
93 0.4967299,
94 0.2718971
95 ],
96 [
97 -0.0279062,
98 -0.5728898,
99 0.8191572,
100 0.3716882
101 ],
102 [
103 0.0,
104 0.0,
105 0.0,
106 1.0
107 ]
108 ]
109 },
110 {
111 "q_deg": [
112 22.5172,
113 71.4985,
114 49.6234,
115 -49.4627,
116 -35.9701,
117 67.2396
118 ],
119 "T_base_gripper_frame": [
120 [
121 -0.443997,
122 0.8475652,
123 0.2906886,
124 0.1559663
125 ],
126 [
127 0.8617756,
128 0.4927697,
129 -0.1205025,
130 -0.0435794
131 ],
132 [
133 -0.2453763,
134 0.1970056,
135 -0.9491993,
136 -0.1455222
137 ],
138 [
139 0.0,
140 0.0,
141 0.0,
142 1.0
143 ]
144 ]
145 },
146 {
147 "q_deg": [
148 -89.0522,
149 57.8211,
150 53.4725,
151 -5.7717,
152 -35.4542,
153 -39.8834
154 ],
155 "T_base_gripper_frame": [
156 [
157 -0.6314297,
158 -0.7754205,
159 -0.0044266,
160 0.0350838
161 ],
162 [
163 -0.7464086,
164 0.6093326,
165 -0.2675593,
166 0.0477965
167 ],
168 [
169 0.2101682,
170 -0.1656408,
171 -0.9635312,
172 -0.1264504
173 ],
174 [
175 0.0,
176 0.0,
177 0.0,
178 1.0
179 ]
180 ]
181 },
182 {
183 "q_deg": [
184 -44.1235,
185 -9.8863,
186 0.8187,
187 9.6295,
188 89.1901,
189 52.6791
190 ],
191 "T_base_gripper_frame": [
192 [
193 0.6944013,
194 -0.0507262,
195 0.7177978,
196 0.2815356
197 ],
198 [
199 -0.7168447,
200 0.0382541,
201 0.6961826,
202 0.2241383
203 ],
204 [
205 -0.0627734,
206 -0.9979797,
207 -0.0097991,
208 0.2568626
209 ],
210 [
211 0.0,
212 0.0,
213 0.0,
214 1.0
215 ]
216 ]
217 },
218 {
219 "frame": "shoulder_link",
220 "q_deg": [
221 30.0,
222 -40.0,
223 50.0,
224 20.0,
225 10.0,
226 5.0
227 ],
228 "T": [
229 [
230 -0.8660267,
231 0.4999977,
232 -0.0,
233 0.0388353
234 ],
235 [
236 0.4999977,
237 0.8660267,
238 2.7e-06,
239 -0.0
240 ],
241 [
242 1.3e-06,
243 2.3e-06,
244 -1.0,
245 0.0624
246 ],
247 [
248 0.0,
249 0.0,
250 0.0,
251 1.0
252 ]
253 ]
254 },
255 {
256 "frame": "upper_arm_link",
257 "q_deg": [
258 30.0,
259 -40.0,
260 50.0,
261 20.0,
262 10.0,
263 5.0
264 ],
265 "T": [
266 [
267 0.5566749,
268 -0.6634143,
269 0.4999945,
270 0.056023
271 ],
272 [
273 -0.3213897,
274 0.3830185,
275 0.8660286,
276 -0.0310287
277 ],
278 [
279 -0.7660429,
280 -0.6427894,
281 2.3e-06,
282 0.1165999
283 ],
284 [
285 0.0,
286 0.0,
287 0.0,
288 1.0
289 ]
290 ]
291 },
292 {
293 "frame": "lower_arm_link",
294 "q_deg": [
295 30.0,
296 -40.0,
297 50.0,
298 20.0,
299 10.0,
300 5.0
301 ],
302 "T": [
303 [
304 -0.8528716,
305 0.1503843,
306 0.4999945,
307 0.0119337
308 ],
309 [
310 0.492398,
311 -0.0868258,
312 0.8660286,
313 -0.0055744
314 ],
315 [
316 0.1736495,
317 0.9848075,
318 2.3e-06,
319 0.2208315
320 ],
321 [
322 0.0,
323 0.0,
324 0.0,
325 1.0
326 ]
327 ]
328 },
329 {
330 "frame": "wrist_link",
331 "q_deg": [
332 30.0,
333 -40.0,
334 50.0,
335 20.0,
336 10.0,
337 5.0
338 ],
339 "T": [
340 [
341 -0.4330115,
342 -0.7500043,
343 0.4999945,
344 0.1277681
345 ],
346 [
347 0.249998,
348 0.4330075,
349 0.8660286,
350 -0.0724504
351 ],
352 [
353 -0.8660266,
354 0.499998,
355 2.3e-06,
356 0.2025272
357 ],
358 [
359 0.0,
360 0.0,
361 0.0,
362 1.0
363 ]
364 ]
365 },
366 {
367 "frame": "gripper_link",
368 "q_deg": [
369 30.0,
370 -40.0,
371 50.0,
372 20.0,
373 10.0,
374 5.0
375 ],
376 "T": [
377 [
378 0.4923444,
379 0.4416853,
380 -0.7500074,
381 0.1826432
382 ],
383 [
384 -0.1393141,
385 0.8905606,
386 0.4330051,
387 -0.0832321
388 ],
389 [
390 0.859179,
391 -0.108701,
392 0.4999955,
393 0.1719773
394 ],
395 [
396 0.0,
397 0.0,
398 0.0,
399 1.0
400 ]
401 ]
402 },
403 {
404 "frame": "moving_jaw_so101_v1_link",
405 "q_deg": [
406 30.0,
407 -40.0,
408 50.0,
409 20.0,
410 10.0,
411 5.0
412 ],
413 "T": [
414 [
415 0.4251032,
416 -0.7900657,
417 -0.4416826,
418 0.2184424
419 ],
420 [
421 -0.1010454,
422 0.4434961,
423 -0.8905622,
424 -0.079436
425 ],
426 [
427 0.8994871,
428 0.4232108,
429 0.1086992,
430 0.1755893
431 ],
432 [
433 0.0,
434 0.0,
435 0.0,
436 1.0
437 ]
438 ]
439 }
440 ]↵
Software/so101/src/kinematics.test.ts added +85
@@ -0,0 +1,85 @@
1 import { describe, expect, test } from "bun:test";
2 import { ArmModel, fromRpyXyz, gripperRadiansToTicks, gripperTicksToRadians, multiply, parseXml, radiansToTicks, rotationAbout, ticksToRadians, transformPoint } from "./kinematics.ts";
3 import fixtures from "./fk-fixtures.json";
4
5 const URDF = new URL("../../../Simulation/SO101/so101_new_calib.urdf", import.meta.url).pathname;
6 const ORDER = ["shoulder_pan", "shoulder_lift", "elbow_flex", "wrist_flex", "wrist_roll", "gripper"];
7 const deg = (q: number[]) => new Map(ORDER.map((name, i) => [name, (q[i]! * Math.PI) / 180]));
8 const model = ArmModel.load(URDF);
9
10 describe("matrices", () => {
11 test("rpy follows the URDF convention and rotations compose", () => {
12 const T = fromRpyXyz([Math.PI / 2, 0, 0], [1, 2, 3]);
13 const round = (v: number) => Math.round(v * 1e9) / 1e9 + 0; // + 0 turns a -0 into 0 for toEqual
14 expect(transformPoint(T, [0, 1, 0]).map(round)).toEqual([1, 2, 4]); // roll about x takes y to z
15 const R = multiply(rotationAbout([0, 0, 1], Math.PI / 2), rotationAbout([1, 0, 0], Math.PI / 2));
16 expect(transformPoint(R, [0, 1, 0]).map(round)).toEqual([0, 0, 1]);
17 });
18
19 test("the xml reader handles self-closing tags, comments and nesting", () => {
20 const root = parseXml(`<?xml version="1.0"?><robot name="r"><!-- c --><joint name="j" type="revolute"><origin xyz="1 2 3"/><axis xyz="0 0 1"/></joint></robot>`);
21 const robot = root.children[0]!;
22 expect(robot.attrs.name).toBe("r");
23 expect(robot.children[0]!.children.map((c) => c.name)).toEqual(["origin", "axis"]);
24 });
25 });
26
27 describe("the SO-101 model", () => {
28 test("loads every link's mesh points and the base geometry", () => {
29 expect(model.movable.map((j) => j.name)).toEqual(ORDER);
30 expect(model.root).toBe("base_link");
31 for (const name of ["base_link", "shoulder_link", "upper_arm_link", "lower_arm_link", "wrist_link", "gripper_link", "moving_jaw_so101_v1_link"]) {
32 expect(model.links.get(name)!.points.length).toBeGreaterThan(26 * 3);
33 }
34 expect(model.floorZ).toBeLessThan(0);
35 expect(model.floorZ).toBeGreaterThan(-0.05); // the base's bottom sits a few mm under base_link's origin
36 expect(model.baseTop).toBeGreaterThan(0.05);
37 expect(model.base.at(0.0388, 0)).toBeGreaterThan(0.02); // the base has height under the pan axis
38 expect(model.base.at(0.5, 0.5)).toBe(-Infinity); // and nothing far away
39 });
40
41 test("forward kinematics matches LeRobot's placo solver on the gripper frame", () => {
42 for (const f of fixtures.filter((f) => "T_base_gripper_frame" in f)) {
43 const T = model.linkPoses(deg(f.q_deg)).get("gripper_frame_link")!;
44 for (let i = 0; i < 16; i++) expect(T[i]).toBeCloseTo((f as { T_base_gripper_frame: number[][] }).T_base_gripper_frame[Math.floor(i / 4)]![i % 4]!, 5);
45 }
46 });
47
48 test("and on every intermediate link", () => {
49 for (const f of fixtures.filter((f) => "frame" in f)) {
50 const T = model.linkPoses(deg(f.q_deg)).get(f.frame!)!;
51 for (let i = 0; i < 16; i++) expect(T[i]).toBeCloseTo((f as { T: number[][] }).T[Math.floor(i / 4)]![i % 4]!, 5);
52 }
53 });
54
55 test("the rest pose is clear of the table; pointing the arm down is not", () => {
56 const settings = { floorMargin: 0.005, baseMargin: 0.005 };
57 expect(model.check(deg([0, 0, 0, 0, 0, 0]), settings)).toBeNull();
58 // Shoulder lift forward past horizontal with the elbow straight puts the gripper far below the table.
59 const down = model.check(deg([0, 80, 0, 0, 0, 0]), settings);
60 expect(down?.what).toBe("table");
61 expect(down!.depth).toBeGreaterThan(0.05);
62 // Folding the lower arm back into the base column is caught too.
63 const folded = deg([0, -90, -95, 0, 0, 0]);
64 const v = model.check(folded, settings);
65 expect(v === null || v.what === "base" || v.what === "table").toBe(true);
66 });
67
68 test("ticks map to radians around the middle of the calibrated range, and back", () => {
69 expect(ticksToRadians(2048, 1000, 3096)).toBe(0);
70 expect(ticksToRadians(2048 + 4095 / 4, 1000, 3096)).toBeCloseTo(Math.PI / 2, 9);
71 expect(radiansToTicks(ticksToRadians(1234, 800, 3300), 800, 3300)).toBe(1234);
72 // The gripper: closed at the range's minimum is the jaw's zero; open at the maximum is the URDF's upper limit.
73 expect(gripperTicksToRadians(1995, 1995, 3525, 1.745)).toBe(0);
74 expect(gripperTicksToRadians(3525, 1995, 3525, 1.745)).toBeCloseTo(1.745, 9);
75 expect(gripperRadiansToTicks(gripperTicksToRadians(2500, 1995, 3525, 1.745), 1995, 3525, 1.745)).toBe(2500);
76 });
77
78 test("a full check is fast enough for a 60 Hz stream", () => {
79 const q = deg([10, -20, 30, 10, 0, 0]);
80 const settings = { floorMargin: 0.01, baseMargin: 0.005 };
81 const t0 = performance.now();
82 for (let i = 0; i < 100; i++) model.check(q, settings);
83 expect((performance.now() - t0) / 100).toBeLessThan(5); // ms per check on this machine; the Pi is slower but has 16 ms per frame
84 });
85 });
Software/so101/src/kinematics.ts added +398
@@ -0,0 +1,398 @@
1 /**
2 * The arm's geometry, from its own URDF and meshes: forward kinematics for every link, and a
3 * sampled point cloud per link so a joint target can be checked against the table and the base
4 * before anything moves. No dependencies: a tiny XML reader and a binary STL reader are enough
5 * for the files in Simulation/SO101.
6 */
7
8 import { readFileSync } from "node:fs";
9 import { dirname, join } from "node:path";
10
11 export type Vec3 = readonly [number, number, number];
12 /** Row-major 4x4. */
13 export type Mat4 = Float64Array;
14
15 export const identity = (): Mat4 => new Float64Array([1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1]);
16
17 export function multiply(a: Mat4, b: Mat4): Mat4 {
18 const out = new Float64Array(16);
19 for (let r = 0; r < 4; r++) {
20 for (let c = 0; c < 4; c++) {
21 out[r * 4 + c] = a[r * 4]! * b[c]! + a[r * 4 + 1]! * b[4 + c]! + a[r * 4 + 2]! * b[8 + c]! + a[r * 4 + 3]! * b[12 + c]!;
22 }
23 }
24 return out;
25 }
26
27 /** URDF origin: rotation Rz(yaw)·Ry(pitch)·Rx(roll), then translation. */
28 export function fromRpyXyz(rpy: Vec3, xyz: Vec3): Mat4 {
29 const [r, p, y] = rpy;
30 const cr = Math.cos(r), sr = Math.sin(r), cp = Math.cos(p), sp = Math.sin(p), cy = Math.cos(y), sy = Math.sin(y);
31 return new Float64Array([
32 cy * cp, cy * sp * sr - sy * cr, cy * sp * cr + sy * sr, xyz[0],
33 sy * cp, sy * sp * sr + cy * cr, sy * sp * cr - cy * sr, xyz[1],
34 -sp, cp * sr, cp * cr, xyz[2],
35 0, 0, 0, 1,
36 ]);
37 }
38
39 /** Rotation about a unit axis (Rodrigues). */
40 export function rotationAbout(axis: Vec3, angle: number): Mat4 {
41 const [x, y, z] = axis;
42 const c = Math.cos(angle), s = Math.sin(angle), t = 1 - c;
43 return new Float64Array([
44 t * x * x + c, t * x * y - s * z, t * x * z + s * y, 0,
45 t * x * y + s * z, t * y * y + c, t * y * z - s * x, 0,
46 t * x * z - s * y, t * y * z + s * x, t * z * z + c, 0,
47 0, 0, 0, 1,
48 ]);
49 }
50
51 export const transformPoint = (m: Mat4, p: Vec3): Vec3 => [
52 m[0]! * p[0] + m[1]! * p[1] + m[2]! * p[2] + m[3]!,
53 m[4]! * p[0] + m[5]! * p[1] + m[6]! * p[2] + m[7]!,
54 m[8]! * p[0] + m[9]! * p[1] + m[10]! * p[2] + m[11]!,
55 ];
56
57 // ---------------------------------------------------------------- a small XML reader
58 interface Node {
59 name: string;
60 attrs: Record<string, string>;
61 children: Node[];
62 }
63
64 export function parseXml(text: string): Node {
65 const root: Node = { name: "#root", attrs: {}, children: [] };
66 const stack = [root];
67 const clean = text.replace(/<\?[\s\S]*?\?>/g, "").replace(/<!--[\s\S]*?-->/g, "");
68 const tag = /<(\/?)([A-Za-z_][\w:.-]*)([^>]*?)(\/?)>/g;
69 for (let m = tag.exec(clean); m; m = tag.exec(clean)) {
70 const [, closing, name, rawAttrs, selfClosing] = m;
71 if (closing) {
72 if (stack.length > 1 && stack[stack.length - 1]!.name === name) stack.pop();
73 continue;
74 }
75 const attrs: Record<string, string> = {};
76 const attr = /([\w:.-]+)\s*=\s*"([^"]*)"/g;
77 for (let a = attr.exec(rawAttrs!); a; a = attr.exec(rawAttrs!)) attrs[a[1]!] = a[2]!;
78 const node: Node = { name: name!, attrs, children: [] };
79 stack[stack.length - 1]!.children.push(node);
80 if (!selfClosing) stack.push(node);
81 }
82 return root;
83 }
84
85 const child = (n: Node, name: string) => n.children.find((c) => c.name === name);
86 const vec3 = (text: string | undefined, fallback: Vec3): Vec3 => {
87 if (!text) return fallback;
88 const v = text.trim().split(/\s+/).map(Number);
89 return v.length === 3 && v.every(Number.isFinite) ? [v[0]!, v[1]!, v[2]!] : fallback;
90 };
91
92 // ---------------------------------------------------------------- binary STL
93 /** Unique vertices of a binary STL, in its own units (meters here). */
94 export function stlVertices(data: Uint8Array): Float64Array {
95 const view = new DataView(data.buffer, data.byteOffset, data.byteLength);
96 const count = view.getUint32(80, true);
97 if (84 + count * 50 > data.byteLength) throw new Error("not a binary STL (or truncated)"); // trailing padding is tolerated
98 const seen = new Set<string>();
99 const out: number[] = [];
100 for (let i = 0; i < count; i++) {
101 const base = 84 + i * 50 + 12; // skip the normal
102 for (let v = 0; v < 3; v++) {
103 const x = view.getFloat32(base + v * 12, true), y = view.getFloat32(base + v * 12 + 4, true), z = view.getFloat32(base + v * 12 + 8, true);
104 const key = `${x},${y},${z}`;
105 if (seen.has(key)) continue;
106 seen.add(key);
107 out.push(x, y, z);
108 }
109 }
110 return Float64Array.from(out);
111 }
112
113 /**
114 * A small set of points that stands in for a mesh in collision checks: its extreme vertex along
115 * each of 26 directions (so the lowest point of any rotation is found to within a few mm) plus an
116 * even sample of the rest.
117 */
118 export function samplePoints(vertices: Float64Array, keep = 300): Float64Array {
119 const n = vertices.length / 3;
120 const picked = new Set<number>();
121 for (let dx = -1; dx <= 1; dx++) {
122 for (let dy = -1; dy <= 1; dy++) {
123 for (let dz = -1; dz <= 1; dz++) {
124 if (!dx && !dy && !dz) continue;
125 let best = -Infinity, at = 0;
126 for (let i = 0; i < n; i++) {
127 const d = vertices[i * 3]! * dx + vertices[i * 3 + 1]! * dy + vertices[i * 3 + 2]! * dz;
128 if (d > best) (best = d), (at = i);
129 }
130 picked.add(at);
131 }
132 }
133 }
134 const stride = Math.max(1, Math.floor(n / keep));
135 for (let i = 0; i < n; i += stride) picked.add(i);
136 const out = new Float64Array(picked.size * 3);
137 let k = 0;
138 for (const i of picked) {
139 out[k++] = vertices[i * 3]!;
140 out[k++] = vertices[i * 3 + 1]!;
141 out[k++] = vertices[i * 3 + 2]!;
142 }
143 return out;
144 }
145
146 // ---------------------------------------------------------------- the model
147 export interface Joint {
148 readonly name: string;
149 readonly type: string;
150 readonly parent: string;
151 readonly child: string;
152 readonly origin: Mat4;
153 readonly axis: Vec3;
154 readonly lower: number;
155 readonly upper: number;
156 }
157
158 export interface Link {
159 readonly name: string;
160 /** Collision sample points in the link's own frame, xyz triples. */
161 readonly points: Float64Array;
162 /** Every mesh vertex, kept only for the root link, whose shape becomes the base height map. */
163 readonly vertices?: Float64Array;
164 }
165
166 /**
167 * The base as terrain: the top of the base at each cell of a grid over its footprint, so a part
168 * hovering over the low front plate is fine while one pushed into the tall motor holder is not.
169 */
170 export class HeightMap {
171 readonly cell: number;
172 readonly x0: number;
173 readonly y0: number;
174 readonly cols: number;
175 readonly rows: number;
176 readonly top: Float64Array; // -Infinity where the base has nothing
177
178 constructor(points: Float64Array, cell = 0.005) {
179 this.cell = cell;
180 let xmin = Infinity, xmax = -Infinity, ymin = Infinity, ymax = -Infinity;
181 for (let i = 0; i < points.length; i += 3) {
182 xmin = Math.min(xmin, points[i]!); xmax = Math.max(xmax, points[i]!);
183 ymin = Math.min(ymin, points[i + 1]!); ymax = Math.max(ymax, points[i + 1]!);
184 }
185 this.x0 = xmin - cell; this.y0 = ymin - cell;
186 this.cols = Math.ceil((xmax - this.x0) / cell) + 2;
187 this.rows = Math.ceil((ymax - this.y0) / cell) + 2;
188 const top = new Float64Array(this.cols * this.rows).fill(-Infinity);
189 for (let i = 0; i < points.length; i += 3) {
190 const c = Math.floor((points[i]! - this.x0) / cell), r = Math.floor((points[i + 1]! - this.y0) / cell);
191 top[r * this.cols + c] = Math.max(top[r * this.cols + c]!, points[i + 2]!);
192 }
193 // One dilation: a cell with no vertex inside a solid part takes its neighbours' height, so faces between vertices count.
194 const dilated = Float64Array.from(top);
195 for (let r = 0; r < this.rows; r++) {
196 for (let c = 0; c < this.cols; c++) {
197 if (top[r * this.cols + c]! > -Infinity) continue;
198 let best = -Infinity;
199 for (let dr = -1; dr <= 1; dr++) for (let dc = -1; dc <= 1; dc++) {
200 const rr = r + dr, cc = c + dc;
201 if (rr >= 0 && rr < this.rows && cc >= 0 && cc < this.cols) best = Math.max(best, top[rr * this.cols + cc]!);
202 }
203 dilated[r * this.cols + c] = best;
204 }
205 }
206 this.top = dilated;
207 }
208
209 /** Height of the base under a point, or -Infinity outside its footprint. */
210 at(x: number, y: number): number {
211 const c = Math.floor((x - this.x0) / this.cell), r = Math.floor((y - this.y0) / this.cell);
212 if (c < 0 || r < 0 || c >= this.cols || r >= this.rows) return -Infinity;
213 return this.top[r * this.cols + c]!;
214 }
215 }
216
217 export interface ZoneSettings {
218 /** Moving parts stay this far above the table, meters. */
219 floorMargin: number;
220 /** Moving parts stay this far outside the base's column, meters. */
221 baseMargin: number;
222 }
223
224 export interface Violation {
225 /** "table": below the table plane. "base": inside the base's own shape. */
226 readonly what: "table" | "base";
227 readonly link: string;
228 /** How far inside the forbidden region, meters. */
229 readonly depth: number;
230 readonly point: Vec3;
231 }
232
233 /** Links that sit on or around the base and may legitimately overlap its column. */
234 const NEAR_BASE = new Set(["base_link", "shoulder_link", "upper_arm_link"]);
235
236 export class ArmModel {
237 readonly joints: readonly Joint[];
238 readonly links: ReadonlyMap<string, Link>;
239 readonly movable: readonly Joint[];
240 readonly root: string;
241 /** The table's plane: the bottom of the base, in base_link coordinates. */
242 readonly floorZ: number;
243 /** The base's shape as terrain. */
244 readonly base: HeightMap;
245 readonly baseTop: number;
246
247 constructor(joints: Joint[], links: Map<string, Link>) {
248 this.links = links;
249 const children = new Set(joints.map((j) => j.child));
250 const root = joints.map((j) => j.parent).find((p) => !children.has(p));
251 if (!root) throw new Error("the URDF has no root link");
252 this.root = root;
253 // Chain order from the root, whatever order the file lists the joints in.
254 const ordered: Joint[] = [];
255 const queue = [root];
256 while (queue.length) {
257 const parent = queue.shift()!;
258 for (const j of joints.filter((j) => j.parent === parent)) {
259 ordered.push(j);
260 queue.push(j.child);
261 }
262 }
263 if (ordered.length !== joints.length) throw new Error("the URDF has joints that do not connect to the root");
264 this.joints = ordered;
265 this.movable = ordered.filter((j) => j.type === "revolute" || j.type === "continuous");
266 const base = links.get(root)?.vertices ?? links.get(root)?.points;
267 if (!base || !base.length) throw new Error(`the root link ${root} has no mesh to stand on`);
268 let floor = Infinity, top = -Infinity;
269 for (let i = 0; i < base.length; i += 3) {
270 floor = Math.min(floor, base[i + 2]!);
271 top = Math.max(top, base[i + 2]!);
272 }
273 this.floorZ = floor;
274 this.baseTop = top;
275 this.base = new HeightMap(base);
276 }
277
278 /** Read a URDF and the binary STL meshes it points at (paths relative to the URDF's folder). */
279 static load(urdfPath: string): ArmModel {
280 const dir = dirname(urdfPath);
281 const robot = child(parseXml(readFileSync(urdfPath, "utf8")), "robot");
282 if (!robot) throw new Error(`${urdfPath}: no <robot>`);
283 const meshes = new Map<string, Float64Array>();
284 const mesh = (file: string) => {
285 let v = meshes.get(file);
286 if (!v) meshes.set(file, (v = stlVertices(readFileSync(join(dir, file)))));
287 return v;
288 };
289 const links = new Map<string, Link>();
290 const joints: Joint[] = robot.children
291 .filter((c) => c.name === "joint")
292 .map((j) => {
293 const o = child(j, "origin");
294 const lim = child(j, "limit");
295 return {
296 name: j.attrs.name!,
297 type: j.attrs.type ?? "fixed",
298 parent: child(j, "parent")!.attrs.link!,
299 child: child(j, "child")!.attrs.link!,
300 origin: fromRpyXyz(vec3(o?.attrs.rpy, [0, 0, 0]), vec3(o?.attrs.xyz, [0, 0, 0])),
301 axis: vec3(child(j, "axis")?.attrs.xyz, [0, 0, 1]),
302 lower: Number(lim?.attrs.lower ?? -Infinity),
303 upper: Number(lim?.attrs.upper ?? Infinity),
304 };
305 });
306 const children = new Set(joints.map((j) => j.child));
307 const root = joints.map((j) => j.parent).find((p) => !children.has(p));
308 for (const l of robot.children.filter((c) => c.name === "link")) {
309 const parts: number[] = [];
310 const all: number[] = [];
311 for (const visual of l.children.filter((c) => c.name === "visual")) {
312 const file = child(child(visual, "geometry") ?? visual, "mesh")?.attrs.filename;
313 if (!file) continue;
314 const o = child(visual, "origin");
315 const T = fromRpyXyz(vec3(o?.attrs.rpy, [0, 0, 0]), vec3(o?.attrs.xyz, [0, 0, 0]));
316 const vertices = mesh(file);
317 const pts = samplePoints(vertices);
318 for (let i = 0; i < pts.length; i += 3) parts.push(...transformPoint(T, [pts[i]!, pts[i + 1]!, pts[i + 2]!]));
319 if (l.attrs.name === root) for (let i = 0; i < vertices.length; i += 3) all.push(...transformPoint(T, [vertices[i]!, vertices[i + 1]!, vertices[i + 2]!]));
320 }
321 links.set(l.attrs.name!, { name: l.attrs.name!, points: Float64Array.from(parts), ...(all.length ? { vertices: Float64Array.from(all) } : {}) });
322 }
323 return new ArmModel(joints, links);
324 }
325
326 /** Every link's pose in root coordinates for the given joint angles (radians, by joint name; missing = 0). */
327 linkPoses(q: ReadonlyMap<string, number>): Map<string, Mat4> {
328 const poses = new Map<string, Mat4>([[this.root, identity()]]);
329 let pending = [...this.joints];
330 while (pending.length) {
331 const rest: Joint[] = [];
332 for (const j of pending) {
333 const parent = poses.get(j.parent);
334 if (!parent) {
335 rest.push(j);
336 continue;
337 }
338 const angle = j.type === "revolute" || j.type === "continuous" ? (q.get(j.name) ?? 0) : 0;
339 poses.set(j.child, multiply(multiply(parent, j.origin), angle ? rotationAbout(j.axis, angle) : identity()));
340 }
341 if (rest.length === pending.length) throw new Error(`joints without a reachable parent: ${rest.map((j) => j.name).join(", ")}`);
342 pending = rest;
343 }
344 return poses;
345 }
346
347 /**
348 * One pass over every moving part: the worst way the pose breaks the zone (or null), and the
349 * lowest point above the table, for the metrics and the viewer.
350 */
351 evaluate(q: ReadonlyMap<string, number>, settings: ZoneSettings): { violation: Violation | null; lowest: { z: number; link: string } } {
352 const poses = this.linkPoses(q);
353 let worst: Violation | null = null;
354 let lowestZ = Infinity, lowestLink = "";
355 const floor = this.floorZ + settings.floorMargin;
356 for (const [name, link] of this.links) {
357 if (name === this.root) continue;
358 const T = poses.get(name);
359 if (!T) continue;
360 const pts = link.points;
361 for (let i = 0; i < pts.length; i += 3) {
362 const p = transformPoint(T, [pts[i]!, pts[i + 1]!, pts[i + 2]!]);
363 if (p[2] < lowestZ) (lowestZ = p[2]), (lowestLink = name);
364 if (p[2] < floor) {
365 const depth = floor - p[2];
366 if (!worst || depth > worst.depth) worst = { what: "table", link: name, depth, point: p };
367 } else if (!NEAR_BASE.has(name)) {
368 const top = this.base.at(p[0], p[1]) + settings.baseMargin;
369 if (p[2] < top) {
370 const depth = top - p[2];
371 if (!worst || depth > worst.depth) worst = { what: "base", link: name, depth, point: p };
372 }
373 }
374 }
375 }
376 return { violation: worst, lowest: { z: lowestZ, link: lowestLink } };
377 }
378
379 /** The worst way the pose breaks the zone, or null. */
380 check(q: ReadonlyMap<string, number>, settings: ZoneSettings): Violation | null {
381 return this.evaluate(q, settings).violation;
382 }
383
384 /** The lowest sampled point of the moving links. */
385 lowest(q: ReadonlyMap<string, number>): { z: number; link: string } {
386 return this.evaluate(q, { floorMargin: 0, baseMargin: 0 }).lowest;
387 }
388 }
389
390 /** LeRobot's convention for the arm joints, which the *_new_calib URDF follows: zero at the middle of the calibrated range, 4095 ticks per turn. */
391 export const ticksToRadians = (ticks: number, min: number, max: number): number => ((ticks - (min + max) / 2) * 2 * Math.PI) / 4095;
392 export const radiansToTicks = (rad: number, min: number, max: number): number => Math.round((rad * 4095) / (2 * Math.PI) + (min + max) / 2);
393 /**
394 * The gripper is different: LeRobot normalizes it 0-100 from closed (the range's minimum) to open (its maximum),
395 * and the URDF's jaw is closed at zero and open at its upper limit. So the calibrated range maps onto [0, upper].
396 */
397 export const gripperTicksToRadians = (ticks: number, min: number, max: number, upper: number): number => ((ticks - min) / (max - min)) * upper;
398 export const gripperRadiansToTicks = (rad: number, min: number, max: number, upper: number): number => Math.round(min + (rad / upper) * (max - min));
Software/so101/src/leader.ts added +66
@@ -0,0 +1,66 @@
1 /** Read a leader arm and stream follower targets to a `so101 serve` over a WebSocket. */
2
3 import WebSocket from "ws";
4 import type { FeetechBus } from "./bus.ts";
5 import { mapGripper, mapJoint, type Frame, type Range } from "./mapping.ts";
6 import { STS3215 } from "./sts3215.ts";
7
8 export interface LeaderOptions {
9 hz?: number;
10 ids?: readonly number[];
11 gripperId?: number;
12 signal?: AbortSignal;
13 onFrame?: (frame: Frame) => void;
14 }
15
16 export async function fetchLimits(serverUrl: string): Promise<Record<string, Range>> {
17 const http = serverUrl.replace(/^ws/, "http").replace(/\/follow$/, "");
18 const res = await fetch(`${http}/limits`);
19 if (!res.ok) throw new Error(`${http}/limits answered ${res.status}`);
20 return (await res.json()) as Record<string, Range>;
21 }
22
23 /** Pure: leader ticks → follower frame, given both arms' calibrated ranges. */
24 export function mapFrame(leader: ReadonlyMap<number, number>, leaderLimits: Map<number, Range>, followerLimits: Record<string, Range>, gripperId: number): Frame {
25 const frame: Frame = {};
26 for (const [id, ticks] of leader) {
27 const l = leaderLimits.get(id);
28 const f = followerLimits[String(id)];
29 if (!l || !f) continue;
30 frame[String(id)] = id === gripperId ? mapGripper(ticks, l, f) : mapJoint(ticks, l, f);
31 }
32 return frame;
33 }
34
35 export async function streamLeader(bus: FeetechBus, serverUrl: string, { hz = 60, ids = [1, 2, 3, 4, 5, 6], gripperId = 6, signal, onFrame }: LeaderOptions = {}): Promise<number> {
36 const followerLimits = await fetchLimits(serverUrl);
37 const leaderLimits = new Map<number, Range>();
38 for (const id of ids) {
39 leaderLimits.set(id, { min: await bus.read(id, STS3215.Min_Position_Limit), max: await bus.read(id, STS3215.Max_Position_Limit) });
40 }
41 await bus.syncWrite(STS3215.Torque_Enable, ids.map((id) => [id, 0] as const)); // the leader is moved by hand
42 const ws = new WebSocket(serverUrl.endsWith("/follow") ? serverUrl : `${serverUrl.replace(/\/$/, "")}/follow`);
43 await new Promise<void>((resolve, reject) => {
44 ws.once("open", resolve);
45 ws.once("error", reject);
46 });
47 const period = 1000 / hz;
48 let sent = 0;
49 const started = performance.now();
50 try {
51 while (!signal?.aborted && ws.readyState === WebSocket.OPEN) {
52 const leader = await bus.syncRead(ids, STS3215.Present_Position);
53 const frame = mapFrame(leader, leaderLimits, followerLimits, gripperId);
54 if (Object.keys(frame).length) {
55 ws.send(JSON.stringify(frame));
56 sent++;
57 onFrame?.(frame);
58 }
59 const wait = started + (sent + 1) * period - performance.now();
60 if (wait > 0) await new Promise((r) => setTimeout(r, wait));
61 }
62 } finally {
63 ws.close();
64 }
65 return sent;
66 }
Software/so101/src/mapping.test.ts added +27
@@ -0,0 +1,27 @@
1 import { describe, expect, test } from "bun:test";
2 import { mapGripper, mapJoint } from "./mapping.ts";
3
4 describe("leader → follower mapping", () => {
5 test("arm joints keep their angle from the middle of the range", () => {
6 const leader = { min: 857, max: 3218 }; // mid 2037.5
7 const follower = { min: 818, max: 3487 }; // mid 2152.5
8 expect(mapJoint(2037.5, leader, follower)).toBe(2153);
9 expect(mapJoint(2037.5 + 500, leader, follower)).toBe(2653);
10 });
11
12 test("arm joints never aim past the follower's limits minus the margin", () => {
13 const leader = { min: 857, max: 3218 };
14 const follower = { min: 818, max: 3487 };
15 expect(mapJoint(0, leader, follower)).toBe(848);
16 expect(mapJoint(4095, leader, follower)).toBe(3457);
17 });
18
19 test("the trigger's travel maps onto the jaw's travel", () => {
20 const trigger = { min: 2047, max: 3515 };
21 const jaw = { min: 1995, max: 3525 };
22 expect(mapGripper(2047, trigger, jaw)).toBe(1995);
23 expect(mapGripper(3515, trigger, jaw)).toBe(3525);
24 expect(mapGripper(2781, trigger, jaw)).toBe(2760);
25 expect(mapGripper(1000, trigger, jaw)).toBe(1995);
26 });
27 });
Software/so101/src/mapping.ts added +30
@@ -0,0 +1,30 @@
1 /**
2 * Leader → follower joint mapping, the way LeRobot does it with both arms calibrated:
3 * arm joints keep the same angle relative to the middle of each calibrated range (both servos have
4 * 4096 ticks per turn, so a tick offset from the middle carries over unchanged); the gripper maps the
5 * trigger's full travel onto the jaw's full travel.
6 */
7
8 export interface Range {
9 readonly min: number;
10 readonly max: number;
11 }
12
13 export const mid = (r: Range): number => (r.min + r.max) / 2;
14
15 export function mapJoint(leaderTicks: number, leader: Range, follower: Range, margin = 30): number {
16 const target = Math.round(leaderTicks - mid(leader) + mid(follower));
17 return clamp(target, follower.min + margin, follower.max - margin);
18 }
19
20 export function mapGripper(leaderTicks: number, leader: Range, follower: Range, margin = 0): number {
21 const span = Math.max(1, leader.max - leader.min);
22 const fraction = clamp((leaderTicks - leader.min) / span, 0, 1);
23 const target = Math.round(follower.min + fraction * (follower.max - follower.min));
24 return clamp(target, follower.min + margin, follower.max - margin);
25 }
26
27 export const clamp = (v: number, lo: number, hi: number): number => Math.min(hi, Math.max(lo, v));
28
29 /** A target frame on the wire: servo id → ticks. */
30 export type Frame = Record<string, number>;
Software/so101/src/metrics.test.ts added +38
@@ -0,0 +1,38 @@
1 import { describe, expect, test } from "bun:test";
2 import { mkdtempSync, readFileSync } from "node:fs";
3 import { tmpdir } from "node:os";
4 import { join } from "node:path";
5 import { Metrics } from "./metrics.ts";
6
7 describe("metrics", () => {
8 test("counts events, keeps peaks, and persists across restarts", () => {
9 const file = join(mkdtempSync(join(tmpdir(), "so101-metrics-")), "events.jsonl");
10 const m = new Metrics(file);
11 m.event("jam", { id: 3 });
12 m.event("block", { what: "table" });
13 m.sample([
14 { id: 3, joint: "elbow_flex", load: -62.5, temperature: 41 },
15 { id: 6, joint: "gripper", load: 10, temperature: 56 },
16 ]);
17 m.sample([{ id: 6, joint: "gripper", load: 5, temperature: 57 }]); // still hot: no second event
18 m.approach(0.012, "gripper_link");
19 m.approach(0.03, "wrist_link");
20 expect(m.counters.jam).toBe(1);
21 expect(m.counters.hot).toBe(1);
22 expect(m.peaks.get(3)!.load).toBe(62.5);
23 expect(m.peaks.get(6)!.temperature).toBe(57);
24 expect(m.closest!.link).toBe("gripper_link");
25 expect(readFileSync(file, "utf8").trim().split("\n")).toHaveLength(3);
26
27 const again = new Metrics(file);
28 expect(again.counters.jam).toBe(1);
29 expect(again.counters.hot).toBe(1);
30 expect(again.recent.map((e) => e.kind)).toEqual(["jam", "block", "hot"]);
31 });
32
33 test("works without a file", () => {
34 const m = new Metrics();
35 m.event("stream", { frames: 10 });
36 expect(m.summary()).toMatchObject({ counters: { stream: 1 } });
37 });
38 });
Software/so101/src/metrics.ts added +103
@@ -0,0 +1,103 @@
1 /**
2 * What the arm went through: jams, zone blocks, stalls, lost servos, sessions, and the peak load
3 * and temperature of every joint. Events append to a JSONL file so nothing is lost across restarts;
4 * the summary is what the viewer shows.
5 */
6
7 import { appendFileSync, mkdirSync, readFileSync } from "node:fs";
8 import { dirname } from "node:path";
9
10 export type EventKind = "jam" | "block" | "stall" | "missing" | "stream" | "move" | "play" | "record" | "hot";
11
12 export interface MetricEvent {
13 readonly t: string;
14 readonly kind: EventKind;
15 readonly detail: Record<string, unknown>;
16 }
17
18 export interface JointPeak {
19 load: number;
20 temperature: number;
21 loadAt: string;
22 temperatureAt: string;
23 }
24
25 const RECENT = 200;
26 const HOT = 55;
27 const COOL = 50;
28
29 export class Metrics {
30 readonly counters: Record<EventKind, number> = { jam: 0, block: 0, stall: 0, missing: 0, stream: 0, move: 0, play: 0, record: 0, hot: 0 };
31 readonly peaks = new Map<number, JointPeak>();
32 readonly recent: MetricEvent[] = [];
33 readonly since = new Date().toISOString();
34 /** Closest any moving part came to the table, meters above it (negative = below), with the link and the time. */
35 closest: { z: number; link: string; at: string } | null = null;
36 readonly #file: string | undefined;
37 readonly #hot = new Set<number>();
38
39 constructor(file?: string) {
40 this.#file = file;
41 if (file) {
42 try {
43 // Carry the counters over from earlier runs, so "jams" means jams ever, not since the last restart.
44 for (const line of readFileSync(file, "utf8").split("\n")) {
45 if (!line.trim()) continue;
46 const e = JSON.parse(line) as MetricEvent;
47 if (e.kind in this.counters) this.counters[e.kind]++;
48 this.recent.push(e);
49 }
50 this.recent.splice(0, Math.max(0, this.recent.length - RECENT));
51 } catch {
52 /* no file yet, or a line that is not an event */
53 }
54 }
55 }
56
57 event(kind: EventKind, detail: Record<string, unknown>): MetricEvent {
58 const e: MetricEvent = { t: new Date().toISOString(), kind, detail };
59 this.counters[kind]++;
60 this.recent.push(e);
61 if (this.recent.length > RECENT) this.recent.shift();
62 if (this.#file) {
63 try {
64 mkdirSync(dirname(this.#file), { recursive: true });
65 appendFileSync(this.#file, `${JSON.stringify(e)}\n`);
66 } catch (err) {
67 console.error(`metrics: cannot append to ${this.#file}: ${err instanceof Error ? err.message : String(err)}`);
68 }
69 }
70 return e;
71 }
72
73 /** Fold a status reading into the peaks; a joint reaching 55 °C logs one "hot" event until it cools below 50. */
74 sample(joints: ReadonlyArray<{ id: number; joint?: string | null; load: number | null; temperature: number | null }>): void {
75 const now = new Date().toISOString();
76 for (const j of joints) {
77 const peak = this.peaks.get(j.id) ?? { load: 0, temperature: 0, loadAt: now, temperatureAt: now };
78 if (j.load !== null && Math.abs(j.load) > peak.load) (peak.load = Math.abs(j.load)), (peak.loadAt = now);
79 if (j.temperature !== null && j.temperature > peak.temperature) (peak.temperature = j.temperature), (peak.temperatureAt = now);
80 this.peaks.set(j.id, peak);
81 if (j.temperature !== null) {
82 if (j.temperature >= HOT && !this.#hot.has(j.id)) {
83 this.#hot.add(j.id);
84 this.event("hot", { id: j.id, joint: j.joint ?? null, temperature: j.temperature });
85 } else if (j.temperature < COOL) this.#hot.delete(j.id);
86 }
87 }
88 }
89
90 approach(z: number, link: string): void {
91 if (!this.closest || z < this.closest.z) this.closest = { z, link, at: new Date().toISOString() };
92 }
93
94 summary(): Record<string, unknown> {
95 return {
96 since: this.since,
97 counters: this.counters,
98 peaks: Object.fromEntries(this.peaks),
99 closest: this.closest,
100 recent: this.recent.slice(-40),
101 };
102 }
103 }
Software/so101/src/motion.test.ts added +89
@@ -0,0 +1,89 @@
1 import { describe, expect, test } from "bun:test";
2 import { FeetechBus } from "./bus.ts";
3 import { FakeBus } from "./fake-bus.ts";
4 import { MotionError, moveToPose } from "./motion.ts";
5
6 const fast = { speed: 500, pollMs: 2 };
7
8 describe("moveToPose", () => {
9 test("reaches the pose, scales velocities to arrive together, and restores the torque cap", async () => {
10 const fake = new FakeBus([1, 2]);
11 const bus = new FeetechBus(fake);
12 const result = await moveToPose(bus, [1, 2], [2548, 2148], fast);
13 expect([...result.final.values()]).toEqual([2548, 2148]);
14 const velocity = fake.writes.filter((w) => w.address === 46).map((w) => [w.id, w.bytes[0]! | (w.bytes[1]! << 8)]);
15 expect(velocity.slice(-2)).toEqual([
16 [1, 500],
17 [2, 100],
18 ]); // joint 2 has a fifth of the distance, so a fifth of the speed
19 const caps = fake.writes.filter((w) => w.address === 48).map((w) => w.bytes[0]! | (w.bytes[1]! << 8));
20 expect(caps).toEqual([700, 700, 1000, 1000]);
21 // torque is enabled only after the goal is parked at the present position, so nothing can lunge
22 const order = fake.writes.filter((w) => w.id === 1).map((w) => w.address);
23 expect(order.indexOf(42)).toBeLessThan(order.indexOf(40));
24 });
25
26 test("respects a servo whose own ceiling is below the requested cap, and puts that ceiling back afterward", async () => {
27 const fake = new FakeBus([1]);
28 fake.servos.get(1)!.set([0xf4, 0x01], 16); // Max_Torque_Limit 500, like LeRobot's gripper config
29 fake.servos.get(1)!.set([0x2c, 0x01], 48); // Torque_Limit 300 left behind by a grip: not the servo's own cap
30 await moveToPose(new FeetechBus(fake), [1], [2100], fast);
31 const caps = fake.writes.filter((w) => w.address === 48).map((w) => w.bytes[0]! | (w.bytes[1]! << 8));
32 expect(caps).toEqual([500, 500]);
33 });
34
35 test("targets on a hard stop are pulled inside the margin; a narrow range aims at its middle", async () => {
36 const fake = new FakeBus([1, 2]);
37 fake.servos.get(2)!.set([0xd0, 0x07, 0xe4, 0x07], 9); // limits 2000-2020
38 const bus = new FeetechBus(fake);
39 const result = await moveToPose(bus, [1, 2], [4095, 2000], fast);
40 expect([...result.adjusted]).toEqual([
41 [1, 4065],
42 [2, 2010],
43 ]);
44 });
45
46 test("refuses a servo whose limits are not calibrated", async () => {
47 const fake = new FakeBus([1]);
48 fake.servos.get(1)!.set([0, 0, 0, 0], 9); // min = max = 0
49 await expect(moveToPose(new FeetechBus(fake), [1], [100], fast)).rejects.toThrow("not calibrated");
50 });
51
52 test("a jammed joint stops the move and holds where it is", async () => {
53 const fake = new FakeBus([1, 2], { stuck: new Set([2]) });
54 const bus = new FeetechBus(fake);
55 await expect(moveToPose(bus, [1, 2], [2548, 2548], fast)).rejects.toThrow("shoulder_lift jammed");
56 const lastGoals = fake.writes.filter((w) => w.address === 42).slice(-2).map((w) => [w.id, w.bytes[0]! | (w.bytes[1]! << 8)]);
57 expect(lastGoals).toEqual([
58 [1, 2548],
59 [2, 2048],
60 ]); // goals reset to present positions
61 const caps = fake.writes.filter((w) => w.address === 48).map((w) => w.bytes[0]! | (w.bytes[1]! << 8));
62 expect(caps.slice(-2)).toEqual([1000, 1000]);
63 });
64
65 test("a jammed joint that chatters by a few ticks is still caught", async () => {
66 const fake = new FakeBus([1], { stuck: new Set([1]), jitter: true });
67 fake.servos.get(1)!.set([0, 0], 56); // start at 0 so the wobble stays within 0-6
68 const started = performance.now();
69 await expect(moveToPose(new FeetechBus(fake), [1], [2000], fast)).rejects.toThrow("jammed");
70 expect(performance.now() - started).toBeLessThan(1000); // the 3 s timeout did not do the job
71 });
72
73 test("a servo that stops answering mid-move aborts and releases it", async () => {
74 // arm() reads limits (2) and position (1); each poll reads position then load: go silent at the second poll's position read
75 const fake = new FakeBus([1, 2], { stuck: new Set([2]), dropAfter: new Map([[2, 4]]) }); // arm() reads each servo 4 times; the first poll then finds 2 gone
76 const bus = new FeetechBus(fake);
77 const err = await moveToPose(bus, [1, 2], [2548, 2548], fast).catch((e) => e);
78 expect(err).toBeInstanceOf(MotionError);
79 expect(err.missing).toEqual([2]);
80 expect(fake.writes.filter((w) => w.address === 40 && w.id === 2).at(-1)?.bytes).toEqual([0]); // torque released on the lost joint
81 });
82
83 test("an abort signal stops the move where it is", async () => {
84 const fake = new FakeBus([1], { stuck: new Set([1]) });
85 const controller = new AbortController();
86 setTimeout(() => controller.abort(), 3);
87 await expect(moveToPose(new FeetechBus(fake), [1], [3000], { ...fast, signal: controller.signal })).rejects.toThrow("interrupted");
88 });
89 });
Software/so101/src/motion.ts added +329
@@ -0,0 +1,329 @@
1 import type { FeetechBus } from "./bus.ts";
2 import { JOINTS, STS3215 } from "./sts3215.ts";
3 import { largestStep, type Trajectory } from "./trajectory.ts";
4
5 export interface SupervisorOptions {
6 /** Torque cap while supervised, in percent of the servo's maximum. */
7 torquePercent?: number;
8 /** Targets are pulled this many ticks inside each servo's limits so a move never drives into a hard stop. */
9 marginTicks?: number;
10 /** How close a joint must get to count as arrived, in ticks. */
11 tolerance?: number;
12 /** Stops the motion where it is when aborted (wire this to Ctrl-C). */
13 signal?: AbortSignal;
14 }
15
16 export interface Poll {
17 readonly positions: ReadonlyMap<number, number>;
18 /** Joints that pushed hard without net progress across the whole window: a collision, seen from the bus. */
19 readonly stalled: readonly number[];
20 /** Joints that stopped answering. */
21 readonly missing: readonly number[];
22 /** Every joint within tolerance of its target. */
23 readonly arrived: boolean;
24 }
25
26 export class MotionError extends Error {
27 readonly stalled: readonly number[];
28 readonly missing: readonly number[];
29
30 constructor(message: string, stalled: readonly number[] = [], missing: readonly number[] = []) {
31 super(message);
32 this.name = "MotionError";
33 this.stalled = stalled;
34 this.missing = missing;
35 }
36 }
37
38 const STALL_WINDOW = 6;
39 // Net travel across the window below this is "not moving": a jammed joint chattering a few ticks against an obstacle still counts.
40 export const STALL_SPAN_TICKS = 8;
41
42 /**
43 * Keeps a set of joints safe while something else decides where they go:
44 * reads and sanity-checks their limits, caps torque, keeps targets inside a
45 * margin, and watches load and progress so a jammed joint stops everything
46 * instead of grinding.
47 */
48 export class JointSupervisor {
49 readonly ids: readonly number[];
50 readonly #bus: FeetechBus;
51 readonly #torquePercent: number;
52 readonly #margin: number;
53 readonly #tolerance: number;
54 readonly #signal: AbortSignal | undefined;
55 #limits: [number, number][] = [];
56 /** Each servo's own ceiling (Max_Torque_Limit), put back afterward: a cap left by an earlier grip or hold is not "original". */
57 readonly #ceilings = new Map<number, number>();
58 /** The cap actually in force per servo: the requested percent, or less if the servo's Max_Torque_Limit is lower. */
59 readonly #caps = new Map<number, number>();
60 readonly #windows = new Map<number, number[]>();
61 readonly #pushing = new Map<number, number>();
62
63 constructor(bus: FeetechBus, ids: readonly number[], options: SupervisorOptions = {}) {
64 this.#bus = bus;
65 this.ids = ids;
66 this.#torquePercent = options.torquePercent ?? 70;
67 this.#margin = options.marginTicks ?? 30;
68 this.#tolerance = options.tolerance ?? 15;
69 this.#signal = options.signal;
70 }
71
72 get limits(): readonly (readonly [number, number])[] {
73 return this.#limits;
74 }
75
76 /**
77 * Read limits, cap torque, and enable torque without a lunge: the goal is
78 * set to the present position and the velocity made gentle before the
79 * servo is allowed to act, so a stale goal from an earlier run is harmless.
80 * Call once before moving.
81 */
82 async arm(acceleration = 30): Promise<ReadonlyMap<number, number>> {
83 if (!uniqueIds(this.ids)) throw new MotionError("servo ids must be distinct");
84 this.#limits = [];
85 for (const id of this.ids) {
86 const min = await this.#bus.read(id, STS3215.Min_Position_Limit);
87 const max = await this.#bus.read(id, STS3215.Max_Position_Limit);
88 if (min >= max) throw new MotionError(`${name(id)} has limits ${min}-${max}: not calibrated, refusing to move it`);
89 this.#limits.push([min, max]);
90 const ceiling = await this.#bus.read(id, STS3215.Max_Torque_Limit);
91 this.#ceilings.set(id, ceiling);
92 this.#caps.set(id, Math.min(Math.round(this.#torquePercent * 10), ceiling));
93 }
94 const start = await this.#bus.syncRead(this.ids, STS3215.Present_Position);
95 const missing = this.ids.filter((id) => !start.has(id));
96 if (missing.length) throw new MotionError(`no response from ${missing.map(name).join(", ")}`, [], missing);
97 for (const id of this.ids) {
98 this.#windows.set(id, [start.get(id)!]);
99 this.#pushing.set(id, 0);
100 }
101 await this.#bus.syncWrite(STS3215.Goal_Position, this.ids.map((id) => [id, start.get(id)!] as const));
102 await this.#bus.syncWrite(STS3215.Goal_Velocity, this.ids.map((id) => [id, 20] as const));
103 await this.#bus.syncWrite(STS3215.Acceleration, this.ids.map((id) => [id, acceleration] as const));
104 await this.#bus.syncWrite(STS3215.Torque_Limit, this.ids.map((id) => [id, this.#caps.get(id)!] as const));
105 await this.#bus.syncWrite(STS3215.Torque_Enable, this.ids.map((id) => [id, 1] as const));
106 return start;
107 }
108
109 /** Pull targets inside the limit margin; a range too narrow for the margin aims at its middle. */
110 clamp(targets: readonly number[]): { aims: number[]; adjusted: Map<number, number> } {
111 if (targets.length !== this.ids.length || !targets.every(Number.isInteger)) {
112 throw new MotionError(`expected ${this.ids.length} integer targets, got ${targets.length}`);
113 }
114 const adjusted = new Map<number, number>();
115 const aims = targets.map((target, i) => {
116 const [min, max] = this.#limits[i]!;
117 const aim =
118 max - min < 2 * this.#margin
119 ? Math.round((min + max) / 2)
120 : Math.min(max - this.#margin, Math.max(min + this.#margin, target));
121 if (aim !== target) adjusted.set(this.ids[i]!, aim);
122 return aim;
123 });
124 return { aims, adjusted };
125 }
126
127 /** Write one set of goals (already clamped) and per-joint velocities. */
128 async goal(aims: readonly number[], velocities: readonly number[]): Promise<void> {
129 await this.#bus.syncWrite(STS3215.Goal_Velocity, this.ids.map((id, i) => [id, velocities[i]!] as const));
130 await this.#bus.syncWrite(STS3215.Goal_Position, this.ids.map((id, i) => [id, aims[i]!] as const));
131 }
132
133 /** One observation. Throws MotionError after stopping everything if a joint stalls, vanishes, or the signal aborts. */
134 async poll(aims: readonly number[]): Promise<Poll> {
135 const positions = await this.#bus.syncRead(this.ids, STS3215.Present_Position);
136 const loads = await this.#bus.syncRead(this.ids, STS3215.Present_Load);
137 const missing = this.ids.filter((id) => !positions.has(id) || !loads.has(id));
138 if (missing.length) {
139 await this.stop(positions);
140 throw new MotionError(`${missing.map(name).join(", ")} stopped answering mid-move; stopped and released it`, [], missing);
141 }
142 const stalled: number[] = [];
143 let arrived = true;
144 for (const [i, id] of this.ids.entries()) {
145 const pos = positions.get(id)!;
146 const away = Math.abs(pos - aims[i]!);
147 if (away > this.#tolerance) arrived = false;
148 const window = this.#windows.get(id)!;
149 window.push(pos);
150 if (window.length > STALL_WINDOW) window.shift();
151 const span = Math.max(...window) - Math.min(...window);
152 const pushing = Math.abs(loads.get(id)!) >= this.#caps.get(id)! * 0.85; // Present_Load is in 0.1 %
153 const count = away > this.#tolerance && pushing ? (this.#pushing.get(id) ?? 0) + 1 : 0;
154 this.#pushing.set(id, count);
155 if (count >= STALL_WINDOW && window.length >= STALL_WINDOW && span < STALL_SPAN_TICKS) stalled.push(id);
156 }
157 if (stalled.length) {
158 await this.stop(positions);
159 throw new MotionError(`${stalled.map(name).join(", ")} jammed; stopped everything where it is`, stalled);
160 }
161 if (this.#signal?.aborted) {
162 await this.stop(positions);
163 throw new MotionError("interrupted; stopped everything where it is");
164 }
165 return { positions, stalled, missing, arrived };
166 }
167
168 /** Make every answering joint's goal its present position, release any that do not answer, restore the torque cap. */
169 async stop(positions: ReadonlyMap<number, number>): Promise<void> {
170 const known = this.ids.filter((id) => positions.has(id));
171 if (known.length) await this.#bus.syncWrite(STS3215.Goal_Position, known.map((id) => [id, positions.get(id)!] as const));
172 const unknown = this.ids.filter((id) => !positions.has(id));
173 if (unknown.length) await this.#bus.syncWrite(STS3215.Torque_Enable, unknown.map((id) => [id, 0] as const));
174 await this.release();
175 }
176
177 /** Put each servo's own torque cap (its Max_Torque_Limit) back; torque itself stays on. */
178 async release(): Promise<void> {
179 await this.#bus.syncWrite(STS3215.Torque_Limit, this.ids.map((id) => [id, this.#ceilings.get(id) ?? 1000] as const));
180 }
181 }
182
183 export interface MoveOptions extends SupervisorOptions {
184 /** Ticks per second for the joint that has the farthest to go; the others are slowed to arrive with it. */
185 speed?: number;
186 /** Poll interval in ms. */
187 pollMs?: number;
188 }
189
190 export interface MoveResult {
191 readonly final: ReadonlyMap<number, number>;
192 /** Joints whose target was pulled inside the limit margin, with the position actually aimed at. */
193 readonly adjusted: ReadonlyMap<number, number>;
194 }
195
196 /**
197 * Drive several joints to a pose at once under supervision. Velocities are
198 * scaled so every joint arrives together. Resolves when all are within
199 * tolerance; throws MotionError (after stopping) on a jam, a lost servo, an
200 * abort, or a timeout. Torque stays on afterward.
201 */
202 export async function moveToPose(
203 bus: FeetechBus,
204 ids: readonly number[],
205 targets: readonly number[],
206 { speed = 300, pollMs = 50, ...options }: MoveOptions = {},
207 ): Promise<MoveResult> {
208 if (!(speed >= 1 && Number.isFinite(speed))) throw new MotionError(`speed must be a positive number, got ${speed}`);
209 if (targets.length !== ids.length || !targets.every(Number.isInteger)) {
210 throw new MotionError(`expected ${ids.length} integer targets, got ${targets.length}`);
211 }
212 const supervisor = new JointSupervisor(bus, ids, options);
213 const start = await supervisor.arm();
214 const { aims, adjusted } = supervisor.clamp(targets);
215 const deltas = ids.map((id, i) => Math.abs(aims[i]! - start.get(id)!));
216 const farthest = Math.max(...deltas, 1);
217 const seconds = farthest / speed;
218 const velocities = deltas.map((d) => Math.max(20, Math.round(d / seconds)));
219 await supervisor.goal(aims, velocities);
220
221 const deadline = Date.now() + (seconds + 3) * 1000;
222 for (;;) {
223 await sleep(pollMs);
224 const poll = await supervisor.poll(aims);
225 if (poll.arrived) {
226 // Settle on where it actually is, so a joint stopped a few ticks short by an obstacle is not pushed at full torque once the cap comes off.
227 await supervisor.stop(poll.positions);
228 return { final: poll.positions, adjusted };
229 }
230 if (Date.now() > deadline) {
231 await supervisor.stop(poll.positions);
232 throw new MotionError("timed out before arriving; stopped everything where it is");
233 }
234 }
235 }
236
237 export const name = (id: number): string => JOINTS[id] ?? `servo ${id}`;
238 export const uniqueIds = (ids: readonly number[]): boolean => new Set(ids).size === ids.length;
239 export const sleep = (ms: number): Promise<void> => new Promise((resolve) => setTimeout(resolve, ms));
240
241 /** A hand-made recording never jumps more than this between two frames; a file that does is refused. */
242 export const MAX_FRAME_STEP = 400;
243 /** Ceiling on the velocity asked of any joint while a recording plays, ticks/s. */
244 export const MAX_PLAY_VELOCITY = 1500;
245
246 export interface RecordOptions {
247 hz?: number;
248 /** Ends the recording. */
249 signal: AbortSignal;
250 onFrame?: (count: number) => void;
251 }
252
253 /**
254 * Release torque and sample the joints until the signal fires. A missed reply
255 * is filled from the previous frame so the recording stays dense; how many
256 * frames needed that is reported alongside.
257 */
258 export async function recordTrajectory(
259 bus: FeetechBus,
260 ids: readonly number[],
261 { hz = 30, signal, onFrame }: RecordOptions,
262 ): Promise<{ trajectory: Trajectory; filled: number }> {
263 await bus.syncWrite(STS3215.Torque_Enable, ids.map((id) => [id, 0] as const));
264 const frames: number[][] = [];
265 let last: number[] | null = null;
266 let filled = 0;
267 const period = 1000 / hz;
268 let next = performance.now();
269 while (!signal.aborted) {
270 const positions = await bus.syncRead(ids, STS3215.Present_Position);
271 if (positions.size < ids.length && last) filled++;
272 const frame = ids.map((id, i) => positions.get(id) ?? last?.[i] ?? Number.NaN);
273 if (frame.every(Number.isFinite)) {
274 frames.push(frame);
275 last = frame;
276 onFrame?.(frames.length);
277 }
278 next += period;
279 const wait = next - performance.now();
280 if (wait > 0) await sleep(wait);
281 }
282 return { trajectory: { hz, ids: [...ids], frames, recorded: new Date().toISOString() }, filled };
283 }
284
285 export interface PlayOptions {
286 /** Playback speed multiplier, within 0-2. */
287 rate?: number;
288 torquePercent?: number;
289 signal?: AbortSignal;
290 onFrame?: (index: number) => void;
291 }
292
293 /**
294 * Replay a recording under supervision: ease to its first frame, then stream
295 * the frames with each joint given only the speed the recording asks of it,
296 * so a dropped sample cannot become a lunge. Resolves with the number of
297 * frames played; throws MotionError on a jam, a lost servo, an abort, or a
298 * recording with an impossible jump. Torque stays on afterward.
299 */
300 export async function playTrajectory(
301 bus: FeetechBus,
302 trajectory: Trajectory,
303 { rate = 1, torquePercent = 70, signal, onFrame }: PlayOptions = {},
304 ): Promise<number> {
305 const step = largestStep(trajectory);
306 if (step > MAX_FRAME_STEP) {
307 throw new MotionError(`the recording jumps ${step} ticks between two frames; a hand-made recording never does that, so refusing to replay it`);
308 }
309 if (!(rate > 0 && rate <= 2)) throw new MotionError(`rate must be within 0-2, got ${rate}`);
310 const ids = [...trajectory.ids];
311 await moveToPose(bus, ids, trajectory.frames[0]!, { speed: 300, torquePercent, signal });
312 const supervisor = new JointSupervisor(bus, ids, { torquePercent, signal, tolerance: 4096 });
313 let previous = [...(await supervisor.arm(254)).values()];
314 const period = 1000 / (trajectory.hz * rate);
315 const started = performance.now();
316 let index = 0;
317 for (; index < trajectory.frames.length; index++) {
318 const { aims } = supervisor.clamp(trajectory.frames[index]!);
319 const velocities = aims.map((aim, i) => Math.min(MAX_PLAY_VELOCITY, Math.max(20, Math.round(Math.abs(aim - previous[i]!) * trajectory.hz * rate * 1.5))));
320 await supervisor.goal(aims, velocities);
321 previous = aims;
322 await supervisor.poll(aims);
323 onFrame?.(index + 1);
324 const wait = started + (index + 1) * period - performance.now();
325 if (wait > 0) await sleep(wait);
326 }
327 await supervisor.release();
328 return index;
329 }
Software/so101/src/poses.test.ts added +24
@@ -0,0 +1,24 @@
1 import { describe, expect, test } from "bun:test";
2 import { parsePoseFile, serializePoseFile } from "./poses.ts";
3
4 describe("pose file", () => {
5 test("round-trips", () => {
6 const poses = { home: { ids: [1, 2], positions: [2048, 2000], saved: "2026-10-13T00:00:00Z" } };
7 expect(parsePoseFile(serializePoseFile(poses))).toEqual(poses);
8 });
9
10 test("rejects malformed entries with the pose name", () => {
11 expect(() => parsePoseFile("[]")).toThrow("object");
12 expect(() => parsePoseFile('{"x":{"ids":[1],"positions":[1,2]}}')).toThrow('pose "x"');
13 expect(() => parsePoseFile('{"x":{"ids":[300],"positions":[1]}}')).toThrow('pose "x"');
14 });
15 });
16
17 describe("pose names and ids", () => {
18 test("rejects names that are also object properties, and duplicate ids", () => {
19 expect(() => parsePoseFile('{"constructor":{"ids":[1],"positions":[1]}}')).toThrow("not a usable pose name");
20 expect(() => parsePoseFile('{"__proto__":{"ids":[1],"positions":[1]}}')).toThrow("not a usable pose name");
21 expect(() => parsePoseFile('{"a b":{"ids":[1],"positions":[1]}}')).toThrow("not a usable pose name");
22 expect(() => parsePoseFile('{"x":{"ids":[1,1],"positions":[1,2]}}')).toThrow("distinct");
23 });
24 });
Software/so101/src/poses.ts added +49
@@ -0,0 +1,49 @@
1 /** Named joint poses, kept as plain JSON so they can be read and edited by hand. */
2 export interface Pose {
3 readonly ids: readonly number[];
4 readonly positions: readonly number[];
5 readonly saved?: string;
6 }
7
8 export type PoseFile = Readonly<Record<string, Pose>>;
9
10 /** Letters, digits, dash, underscore, dot: nothing that is also a property of every JavaScript object. */
11 export const POSE_NAME = /^[A-Za-z0-9][A-Za-z0-9_.-]{0,63}$/;
12
13 export function assertPoseName(name: string): void {
14 if (!POSE_NAME.test(name) || name in Object.prototype) {
15 throw new Error(`"${name}" is not a usable pose name (letters, digits, - _ . only)`);
16 }
17 }
18
19 export function uniqueIds(ids: readonly number[]): boolean {
20 return new Set(ids).size === ids.length;
21 }
22
23 export function parsePoseFile(text: string): PoseFile {
24 let data: unknown;
25 try {
26 data = JSON.parse(text);
27 } catch {
28 throw new Error("not valid JSON");
29 }
30 if (typeof data !== "object" || data === null || Array.isArray(data)) throw new Error("expected an object of poses");
31 const poses: Record<string, Pose> = Object.create(null);
32 for (const [name, value] of Object.entries(data as Record<string, unknown>)) {
33 assertPoseName(name);
34 if (typeof value !== "object" || value === null) throw new Error(`pose "${name}" must be an object`);
35 const { ids, positions, saved } = value as Record<string, unknown>;
36 if (!Array.isArray(ids) || ids.length === 0 || !ids.every((id) => Number.isInteger(id) && id >= 0 && id <= 253) || !uniqueIds(ids)) {
37 throw new Error(`pose "${name}": ids must be a non-empty list of distinct servo ids`);
38 }
39 if (!Array.isArray(positions) || positions.length !== ids.length || !positions.every(Number.isInteger)) {
40 throw new Error(`pose "${name}": positions must hold one integer per id`);
41 }
42 poses[name] = { ids, positions, ...(typeof saved === "string" ? { saved } : {}) };
43 }
44 return poses;
45 }
46
47 export function serializePoseFile(poses: PoseFile): string {
48 return JSON.stringify(poses, null, 1);
49 }
Software/so101/src/protocol.test.ts added +74
@@ -0,0 +1,74 @@
1 import { describe, expect, test } from "bun:test";
2 import {
3 FrameParser,
4 Instruction,
5 checksum,
6 describeError,
7 encodeInstruction,
8 hex,
9 } from "./protocol.ts";
10
11 describe("checksum", () => {
12 test("is the inverted low byte of the sum", () => {
13 expect(checksum([0x01, 0x02, 0x01])).toBe(0xfb);
14 expect(checksum([0xff, 0xff])).toBe(0x01);
15 expect(checksum([])).toBe(0xff);
16 });
17 });
18
19 describe("encodeInstruction", () => {
20 test("ping id 1 matches the Feetech manual's example", () => {
21 expect(hex(encodeInstruction(1, Instruction.Ping))).toBe("ff ff 01 02 01 fb");
22 });
23
24 test("read present position (addr 56, 2 bytes) from id 1", () => {
25 expect(hex(encodeInstruction(1, Instruction.Read, [56, 2]))).toBe("ff ff 01 04 02 38 02 be");
26 });
27
28 test("sync write goal position for two servos", () => {
29 const packet = encodeInstruction(0xfe, Instruction.SyncWrite, [42, 2, 1, 0x00, 0x08, 2, 0xff, 0x0f]);
30 expect(hex(packet)).toBe("ff ff fe 0a 83 2a 02 01 00 08 02 ff 0f 2f");
31 expect(packet[3]).toBe(packet.length - 4);
32 });
33 });
34
35 describe("FrameParser", () => {
36 test("parses a status packet with params", () => {
37 const parser = new FrameParser();
38 const frames = parser.push(Uint8Array.from([0xff, 0xff, 0x01, 0x04, 0x00, 0x00, 0x08, 0xf2]));
39 expect(frames).toHaveLength(1);
40 expect(frames[0]!.id).toBe(1);
41 expect(frames[0]!.error).toBe(0);
42 expect(Array.from(frames[0]!.params)).toEqual([0x00, 0x08]);
43 });
44
45 test("reassembles frames split across chunks and skips leading noise", () => {
46 const parser = new FrameParser();
47 expect(parser.push(Uint8Array.from([0x13, 0xff, 0xff, 0x01]))).toHaveLength(0);
48 expect(parser.push(Uint8Array.from([0x02]))).toHaveLength(0);
49 const frames = parser.push(Uint8Array.from([0x00, 0xfc, 0xff, 0xff, 0x02, 0x02, 0x00, 0xfb]));
50 expect(frames.map((f) => f.id)).toEqual([1, 2]);
51 });
52
53 test("an absurd length byte does not swallow the frames behind it", () => {
54 const parser = new FrameParser();
55 const frames = parser.push(Uint8Array.from([0xff, 0xff, 0x01, 0xff, 0xff, 0xff, 0x03, 0x02, 0x00, 0xfa]));
56 expect(frames.map((f) => f.id)).toEqual([3]);
57 });
58
59 test("drops a frame with a bad checksum without losing the next one", () => {
60 const parser = new FrameParser();
61 const frames = parser.push(
62 Uint8Array.from([0xff, 0xff, 0x01, 0x02, 0x00, 0x00, 0xff, 0xff, 0x03, 0x02, 0x00, 0xfa]),
63 );
64 expect(frames.map((f) => f.id)).toEqual([3]);
65 });
66 });
67
68 describe("describeError", () => {
69 test("names set bits", () => {
70 expect(describeError(0)).toBe("ok");
71 expect(describeError(0b100101)).toBe("voltage+overheat+overload");
72 expect(describeError(0x40)).toBe("0x40");
73 });
74 });
Software/so101/src/protocol.ts added +135
@@ -0,0 +1,135 @@
1 /**
2 * Feetech serial bus protocol (the "SCS/STS" half-duplex packet format).
3 *
4 * Every packet on the wire is:
5 *
6 * 0xFF 0xFF | ID | LENGTH | INSTRUCTION-or-ERROR | PARAMS... | CHECKSUM
7 *
8 * LENGTH counts everything after itself (so PARAMS.length + 2), and
9 * CHECKSUM is the bitwise NOT of the low byte of ID + LENGTH + ... + PARAMS.
10 * A status packet has the same shape, with the servo's error byte where the
11 * instruction byte was. This module is pure: framing in, framing out.
12 */
13
14 export const BROADCAST_ID = 0xfe;
15
16 export const Instruction = {
17 Ping: 0x01,
18 Read: 0x02,
19 Write: 0x03,
20 RegWrite: 0x04,
21 Action: 0x05,
22 SyncRead: 0x82,
23 SyncWrite: 0x83,
24 } as const;
25 export type Instruction = (typeof Instruction)[keyof typeof Instruction];
26
27 /** Bits of the status packet's error byte. */
28 export const ErrorBit = {
29 Voltage: 1 << 0,
30 Angle: 1 << 1,
31 Overheat: 1 << 2,
32 Overcurrent: 1 << 3,
33 Overload: 1 << 5,
34 } as const;
35
36 export function describeError(error: number): string {
37 if (error === 0) return "ok";
38 const names = (Object.keys(ErrorBit) as (keyof typeof ErrorBit)[])
39 .filter((name) => error & ErrorBit[name])
40 .map((name) => name.toLowerCase());
41 const known = Object.values(ErrorBit).reduce((acc, bit) => acc | bit, 0);
42 if (error & ~known) names.push(`0x${error.toString(16)}`);
43 return names.join("+");
44 }
45
46 export function checksum(bytes: Iterable<number>): number {
47 let sum = 0;
48 for (const byte of bytes) sum += byte;
49 return ~sum & 0xff;
50 }
51
52 /** The length byte counts instruction, params and checksum, and tops out at 250. */
53 export const MAX_PARAMS = 248;
54
55 export function encodeInstruction(
56 id: number,
57 instruction: Instruction,
58 params: ArrayLike<number> = [],
59 ): Uint8Array {
60 if (params.length > MAX_PARAMS) throw new RangeError(`${params.length} params exceed the ${MAX_PARAMS}-byte packet limit`);
61 const length = params.length + 2;
62 const packet = new Uint8Array(4 + length);
63 packet[0] = 0xff;
64 packet[1] = 0xff;
65 packet[2] = id;
66 packet[3] = length;
67 packet[4] = instruction;
68 packet.set(params, 5);
69 packet[packet.length - 1] = checksum(packet.subarray(2, packet.length - 1));
70 return packet;
71 }
72
73 export interface StatusPacket {
74 readonly id: number;
75 readonly error: number;
76 readonly params: Uint8Array;
77 /** The exact bytes of the frame, for callers that need to recognise echoes. */
78 readonly raw: Uint8Array;
79 }
80
81 /**
82 * Incremental status-packet parser. Feed it whatever the serial port hands
83 * you; it returns every complete, checksum-valid frame and keeps the rest.
84 * Garbage between frames is skipped a byte at a time.
85 */
86 export class FrameParser {
87 #buffer = new Uint8Array(0);
88
89 push(chunk: Uint8Array): StatusPacket[] {
90 const joined = new Uint8Array(this.#buffer.length + chunk.length);
91 joined.set(this.#buffer);
92 joined.set(chunk, this.#buffer.length);
93
94 const frames: StatusPacket[] = [];
95 let offset = 0;
96 while (joined.length - offset >= 4) {
97 if (joined[offset] !== 0xff || joined[offset + 1] !== 0xff) {
98 offset += 1;
99 continue;
100 }
101 const id = joined[offset + 2]!;
102 const length = joined[offset + 3]!;
103 // 0xFE is kept: an echoed broadcast instruction is how the bus detects echo.
104 if (id === 0xff || length < 2 || length > MAX_PARAMS + 2) {
105 offset += 1;
106 continue;
107 }
108 const end = offset + 4 + length;
109 if (end > joined.length) break;
110 const raw = joined.slice(offset, end);
111 if (checksum(raw.subarray(2, raw.length - 1)) !== raw[raw.length - 1]) {
112 offset += 1;
113 continue;
114 }
115 frames.push({ id, error: raw[4]!, params: raw.slice(5, raw.length - 1), raw });
116 offset = end;
117 }
118 this.#buffer = joined.slice(offset);
119 return frames;
120 }
121
122 reset(): void {
123 this.#buffer = new Uint8Array(0);
124 }
125 }
126
127 export function bytesEqual(a: Uint8Array, b: Uint8Array): boolean {
128 if (a.length !== b.length) return false;
129 for (let i = 0; i < a.length; i++) if (a[i] !== b[i]) return false;
130 return true;
131 }
132
133 export function hex(bytes: Uint8Array): string {
134 return Array.from(bytes, (b) => b.toString(16).padStart(2, "0")).join(" ");
135 }
Software/so101/src/server.test.ts added +576
@@ -0,0 +1,576 @@
1 import { afterAll, beforeAll, describe, expect, test } from "bun:test";
2 import { existsSync, mkdtempSync, readFileSync, writeFileSync } from "node:fs";
3 import { tmpdir } from "node:os";
4 import { join } from "node:path";
5 import WebSocket from "ws";
6 import { FeetechBus } from "./bus.ts";
7 import { FakeBus } from "./fake-bus.ts";
8 import { mapFrame } from "./leader.ts";
9 import { startServer, type RunningServer } from "./server.ts";
10
11 const sleep = (ms: number) => new Promise((r) => setTimeout(r, ms));
12 let fake: FakeBus;
13 let server: RunningServer;
14 let base: string;
15 let dataDir: string;
16 const post = (path: string, body: unknown, method = "POST") => fetch(`${base}${path}`, { method, body: JSON.stringify(body) });
17
18 beforeAll(async () => {
19 dataDir = mkdtempSync(join(tmpdir(), "so101-serve-"));
20 fake = new FakeBus([1, 2, 6]);
21 fake.servos.get(1)!.set([0x32, 0x03, 0x9f, 0x0d], 9); // limits 818-3487
22 fake.servos.get(2)!.set([0x2d, 0x03, 0x82, 0x0c], 9); // 813-3202
23 fake.servos.get(6)!.set([0xcb, 0x07, 0xc5, 0x0d], 9); // 1995-3525
24 server = await startServer(new FeetechBus(fake), { host: "127.0.0.1", port: 0, ids: [1, 2, 6], posesFile: join(dataDir, "poses.json"), watchdogMs: 60 });
25 base = `http://127.0.0.1:${server.port}`;
26 });
27 // Under Node the close resolves at once in every scenario (probed). Bun's node:http keeps a connection the
28 // server closed itself from ending, so the test runner would wait on it for ever: cap the wait.
29 afterAll(() => Promise.race([server.close(), sleep(500)]));
30
31 describe("so101 serve", () => {
32 test("reports status and limits", async () => {
33 const status = (await (await fetch(`${base}/status`)).json()) as { joints: Array<{ id: number; position: number }> };
34 expect(status.joints.map((j) => [j.id, j.position])).toEqual([
35 [1, 2048],
36 [2, 2048],
37 [6, 2048],
38 ]);
39 const limits = (await (await fetch(`${base}/limits`)).json()) as Record<string, { min: number; max: number }>;
40 expect(limits["6"]).toEqual({ min: 1995, max: 3525 });
41 });
42
43 test("a stream drives the arm inside the limits and drops impossible jumps", async () => {
44 const ws = new WebSocket(`ws://127.0.0.1:${server.port}/follow`);
45 await new Promise((r) => ws.once("open", r));
46 ws.send(JSON.stringify({ "1": 2100, "2": 100, "6": 9999 })); // 2: below min → clamped; 6: above max → clamped
47 await sleep(30);
48 ws.send(JSON.stringify({ "1": 3000 })); // 900 ticks in one frame: dropped
49 await sleep(30);
50 ws.close();
51 await sleep(30);
52 const goals = fake.writes.filter((w) => w.address === 42).map((w) => [w.id, w.bytes[0]! | (w.bytes[1]! << 8)]);
53 expect(goals).toContainEqual([1, 2100]);
54 expect(goals).toContainEqual([2, 843]);
55 expect(goals).toContainEqual([6, 3495]);
56 expect(goals).not.toContainEqual([1, 3000]);
57 // closing the stream holds at the present position and restores the torque cap
58 const caps = fake.writes.filter((w) => w.address === 48).map((w) => w.bytes[0]! | (w.bytes[1]! << 8));
59 expect(caps.at(-1)).toBe(1000);
60 });
61
62 test("a silent stream makes the arm hold", async () => {
63 const before = fake.writes.length;
64 const ws = new WebSocket(`ws://127.0.0.1:${server.port}/follow`);
65 await new Promise((r) => ws.once("open", r));
66 ws.send(JSON.stringify({ "1": 2200 }));
67 await sleep(150); // past the 60 ms watchdog
68 const holds = fake.writes.slice(before).filter((w) => w.address === 42 && w.id === 1).map((w) => w.bytes[0]! | (w.bytes[1]! << 8));
69 expect(holds.at(-1)).toBe(2200); // present position (the fake follows goals), written again by the watchdog
70 expect(holds.length).toBeGreaterThanOrEqual(2);
71 ws.close();
72 await sleep(30);
73 });
74
75 test("torque and unknown poses over HTTP", async () => {
76 const t = await fetch(`${base}/torque`, { method: "POST", body: JSON.stringify({ on: false }) });
77 expect(((await t.json()) as { torque: boolean }).torque).toBe(false);
78 const p = await fetch(`${base}/pose/go`, { method: "POST", body: JSON.stringify({ name: "nope" }) });
79 expect(p.status).toBe(404); // no poses file at all: nothing saved yet
80 });
81 });
82
83 describe("leader mapping of a whole frame", () => {
84 test("maps each joint by its own rule", () => {
85 const leaderLimits = new Map([
86 [1, { min: 857, max: 3218 }],
87 [6, { min: 2047, max: 3515 }],
88 ]);
89 const followerLimits = { "1": { min: 818, max: 3487 }, "6": { min: 1995, max: 3525 } };
90 const frame = mapFrame(new Map([[1, 2037.5], [6, 3515], [5, 100]]), leaderLimits, followerLimits, 6);
91 expect(frame).toEqual({ "1": 2153, "6": 3525 }); // id 5 has no limits on either side and is skipped
92 });
93 });
94
95 describe("viewer page", () => {
96 test("is served at the root", async () => {
97 const r = await fetch(`${base}/`);
98 expect(r.status).toBe(200);
99 expect(r.headers.get("content-type")).toContain("text/html");
100 expect(await r.text()).toContain("Fullscreen");
101 });
102 });
103
104 describe("moves over HTTP", () => {
105 test("/move drives the joints it names under supervision and reports where they ended", async () => {
106 const before = ((await (await fetch(`${base}/status`)).json()) as { joints: Array<{ id: number; position: number }> }).joints.find((j) => j.id === 2)!.position;
107 const r = await post("/move", { targets: { "1": 2500, "6": 3000 } });
108 expect(r.status).toBe(200);
109 const d = (await r.json()) as { reached: boolean; final: Record<string, number> };
110 expect(d.reached).toBe(true);
111 expect(d.final).toEqual({ "1": 2500, "6": 3000 });
112 const status = (await (await fetch(`${base}/status`)).json()) as { joints: Array<{ id: number; position: number }>; driving: string | null };
113 expect(status.joints.find((j) => j.id === 2)!.position).toBe(before); // untouched
114 expect(status.driving).toBeNull();
115 });
116
117 test("/move pulls a target inside the limit margin and refuses bad input", async () => {
118 const r = await post("/move", { targets: { "6": 4000 } }); // above the gripper's 3525 max
119 const d = (await r.json()) as { reached: boolean; final: Record<string, number>; adjusted: Record<string, number> };
120 expect(d.reached).toBe(true);
121 expect(d.final["6"]).toBe(3525 - 30);
122 expect((await post("/move", { targets: { "9": 100 } })).status).toBe(400);
123 expect((await post("/move", { targets: { "1": 10.5 } })).status).toBe(400);
124 expect((await post("/move", { targets: {} })).status).toBe(400);
125 expect((await post("/move", "nonsense")).status).toBe(400);
126 });
127
128 test("/grip closes the gripper toward a position at a low torque cap and keeps torque on", async () => {
129 const r = await post("/grip", { position: 2000, torque: 30 });
130 expect(await r.json()).toEqual({ gripping: 2025, torque: 30 }); // clamped to min + margin
131 const memory = fake.servos.get(6)!;
132 expect(memory[48]! + memory[49]! * 256).toBe(300); // Torque_Limit 30 %
133 expect(memory[40]).toBe(1); // Torque_Enable
134 });
135
136 test("/stop with nothing running is harmless", async () => {
137 expect(await (await post("/stop", {})).json()).toEqual({ stopped: null });
138 });
139 });
140
141 describe("poses over HTTP", () => {
142 test("save, list, go, delete", async () => {
143 await post("/move", { targets: { "1": 2200, "2": 2300, "6": 2400 } });
144 const saved = (await (await post("/pose/save", { name: "bench", ids: [1, 2] })).json()) as { name: string; positions: number[]; onStops: number[] };
145 expect(saved.name).toBe("bench");
146 expect(saved.positions).toEqual([2200, 2300]);
147 expect(saved.onStops).toEqual([]);
148 const file = JSON.parse(readFileSync(join(dataDir, "poses.json"), "utf8")) as Record<string, { ids: number[] }>;
149 expect(file.bench!.ids).toEqual([1, 2]);
150 expect(Object.keys((await (await fetch(`${base}/poses`)).json()) as object)).toEqual(["bench"]);
151
152 await post("/move", { targets: { "1": 3000 } });
153 const go = (await (await post("/pose/go", { name: "bench" })).json()) as { reached: boolean; final: Record<string, number> };
154 expect(go.reached).toBe(true);
155 expect(go.final).toEqual({ "1": 2200, "2": 2300 });
156
157 expect((await post("/pose/delete", { name: "bench" })).status).toBe(200);
158 expect((await post("/pose/delete", { name: "bench" })).status).toBe(404);
159 expect((await post("/pose/go", { name: "bench" })).status).toBe(404);
160 });
161
162 test("a pose resting on a hard stop is reported, and names are checked", async () => {
163 await post("/move", { targets: { "6": 3525 } }); // aims at max - 30, inside 40 of the stop
164 const saved = (await (await post("/pose/save", { name: "edge", ids: [6] })).json()) as { onStops: number[] };
165 expect(saved.onStops).toEqual([6]);
166 expect((await post("/pose/save", { name: "__proto__" })).status).toBe(400);
167 expect((await post("/pose/save", { name: "ok", ids: [9] })).status).toBe(400);
168 await post("/pose/delete", { name: "edge" });
169 });
170 });
171
172 describe("recordings over HTTP", () => {
173 test("record, list, play, and stop a playback", async () => {
174 const start = await post("/record/start", { name: "wave", hz: 50 });
175 expect(start.status).toBe(200);
176 expect(fake.servos.get(1)![40]).toBe(0); // torque released for the recording
177 let status = (await (await fetch(`${base}/status`)).json()) as { driving: string | null; recording: string | null };
178 expect(status.driving).toBe("record");
179 expect(status.recording).toBe("wave");
180 expect((await post("/move", { targets: { "1": 100 } })).status).toBe(409); // the recording owns the arm
181 await sleep(120);
182 const stop = (await (await post("/record/stop", {})).json()) as { saved: string; frames: number };
183 expect(stop.saved).toBe("wave");
184 expect(stop.frames).toBeGreaterThan(2);
185 expect(existsSync(join(dataDir, "recordings", "wave.json"))).toBe(true);
186 const list = (await (await fetch(`${base}/recordings`)).json()) as Record<string, { hz: number; frames: number }>;
187 expect(list.wave!.hz).toBe(50);
188 expect(list.wave!.frames).toBe(stop.frames);
189 expect((await post("/record/start", { name: "wave" })).status).toBe(409); // no silent overwrite
190
191 const play = (await (await post("/play", { name: "wave", rate: 2 })).json()) as { played: number; frames: number };
192 expect(play.played).toBe(play.frames);
193 status = (await (await fetch(`${base}/status`)).json()) as { driving: string | null; recording: string | null };
194 expect(status.driving).toBeNull();
195 expect((await post("/play", { name: "nothing" })).status).toBe(404);
196 expect((await post("/record/stop", {})).status).toBe(409);
197 expect((await post("/recording/delete", { name: "wave" })).status).toBe(200);
198 expect((await post("/recording/delete", { name: "wave" })).status).toBe(404);
199 expect(Object.keys((await (await fetch(`${base}/recordings`)).json()) as object)).toEqual([]);
200 });
201
202 test("/stop ends a recording and saves it", async () => {
203 await post("/record/start", { name: "short", hz: 50 });
204 await sleep(60);
205 const stopped = (await (await post("/stop", {})).json()) as { stopped: string; saved: string };
206 expect(stopped.stopped).toBe("record");
207 expect(stopped.saved).toBe("short");
208 });
209 });
210
211 describe("calibration store", () => {
212 test("round trip", async () => {
213 expect(await (await fetch(`${base}/calibration`)).json()).toEqual({ camera: null, handeye: null });
214 expect((await post("/calibration/camera", { width: 1280, height: 720, rms: 0.4 }, "PUT")).status).toBe(200);
215 const c = (await (await fetch(`${base}/calibration`)).json()) as { camera: { width: number; saved: string }; handeye: null };
216 expect(c.camera.width).toBe(1280);
217 expect(typeof c.camera.saved).toBe("string");
218 expect(c.handeye).toBeNull();
219 });
220 });
221
222 describe("/stop on a stream", () => {
223 test("closes the socket and the arm holds", async () => {
224 const ws = new WebSocket(`ws://127.0.0.1:${server.port}/follow`);
225 await new Promise((r) => ws.once("open", r));
226 ws.send(JSON.stringify({ "1": 2100 }));
227 await sleep(30);
228 let status = (await (await fetch(`${base}/status`)).json()) as { driving: string | null };
229 expect(status.driving).toBe("stream");
230 const closed = new Promise<number>((r) => ws.once("close", (code) => r(code)));
231 expect(await (await post("/stop", {})).json()).toEqual({ stopped: "stream" });
232 expect(await closed).toBe(1000);
233 await sleep(30);
234 status = (await (await fetch(`${base}/status`)).json()) as { driving: string | null };
235 expect(status.driving).toBeNull();
236 });
237 });
238
239 describe("a bus that fails mid-session", () => {
240 // The same server on a bus whose reads can be made to throw, like a USB unplug.
241 let faulty: FakeBus;
242 let bus: FeetechBus;
243 let srv: RunningServer;
244 let url: string;
245 let broken = false;
246 const call = (path: string, body: unknown) => fetch(`${url}${path}`, { method: "POST", body: JSON.stringify(body) });
247 beforeAll(async () => {
248 faulty = new FakeBus([1, 2, 6]);
249 bus = new FeetechBus(faulty);
250 const real = bus.syncRead.bind(bus);
251 bus.syncRead = (...args) => (broken ? Promise.reject(new Error("usb gone")) : real(...args));
252 srv = await startServer(bus, { host: "127.0.0.1", port: 0, ids: [1, 2, 6], posesFile: join(mkdtempSync(join(tmpdir(), "so101-faulty-")), "poses.json"), watchdogMs: 60 });
253 url = `http://127.0.0.1:${srv.port}`;
254 });
255 afterAll(() => Promise.race([srv.close(), sleep(500)]));
256
257 test("a stream whose close hits a dead bus still frees the arm", async () => {
258 const ws = new WebSocket(`ws://127.0.0.1:${srv.port}/follow`);
259 await new Promise((r) => ws.once("open", r));
260 ws.send(JSON.stringify({ "1": 2100 }));
261 await sleep(30);
262 broken = true;
263 ws.close();
264 await sleep(80);
265 broken = false;
266 const status = (await (await fetch(`${url}/status`)).json()) as { driving: string | null };
267 expect(status.driving).toBeNull();
268 expect((await call("/move", { targets: { "1": 2200 } })).status).toBe(200);
269 });
270
271 test("a recording whose bus dies reports the failure on stop and frees the arm", async () => {
272 expect((await call("/record/start", { name: "dead", hz: 50 })).status).toBe(200);
273 await sleep(30);
274 broken = true;
275 await sleep(60);
276 broken = false;
277 const stop = await call("/record/stop", {});
278 expect(stop.status).toBe(500);
279 expect(((await stop.json()) as { error: string }).error).toContain("usb gone");
280 const status = (await (await fetch(`${url}/status`)).json()) as { driving: string | null; recording: string | null };
281 expect(status.driving).toBeNull();
282 expect(status.recording).toBeNull();
283 const list = (await (await fetch(`${url}/recordings`)).json()) as Record<string, unknown>;
284 expect(Object.keys(list)).toEqual([]);
285 });
286
287 test("/stop and /record/stop racing: one saves, the other finds nothing to stop", async () => {
288 await call("/record/start", { name: "race", hz: 50 });
289 await sleep(60);
290 const [a, b] = await Promise.all([call("/stop", {}), call("/record/stop", {})]);
291 expect([a.status, b.status].sort()).toEqual([200, 409]);
292 const list = (await (await fetch(`${url}/recordings`)).json()) as Record<string, unknown>;
293 expect(Object.keys(list)).toEqual(["race"]);
294 });
295
296 test("/grip owns the arm while it writes, and the next supervised gripper move puts the servo's own cap back", async () => {
297 const ws = new WebSocket(`ws://127.0.0.1:${srv.port}/follow`);
298 await new Promise((r) => ws.once("open", r));
299 expect((await call("/grip", { position: 2500 })).status).toBe(409); // the stream drives
300 const closed = new Promise((r) => ws.once("close", r));
301 ws.close();
302 await closed;
303 await sleep(40);
304 expect((await call("/grip", { position: 2500, torque: 30 })).status).toBe(200);
305 const memory = faulty.servos.get(6)!;
306 expect(memory[48]! + memory[49]! * 256).toBe(300);
307 expect((await call("/move", { targets: { "6": 2600 } })).status).toBe(200);
308 expect(memory[48]! + memory[49]! * 256).toBe(1000); // Max_Torque_Limit, not the cap a grip left behind
309 });
310
311 test("/move rejects ids that are not plain integers", async () => {
312 expect((await call("/move", { targets: { "03": 2000 } })).status).toBe(400);
313 expect((await call("/move", { targets: { " 1": 2000 } })).status).toBe(400);
314 expect((await call("/move", { targets: { "1.0": 2000 } })).status).toBe(400);
315 });
316 });
317
318 describe("the safe zone, the jam watch, and the model routes", () => {
319 // A six-joint fake with the follower's real calibrated ranges, the repo's model, and a fast jam watch.
320 let fake6: FakeBus;
321 let zoneServer: RunningServer;
322 let zurl: string;
323 const zcall = (path: string, body: unknown, method = "POST") => fetch(`${zurl}${path}`, { method, body: JSON.stringify(body) });
324 /** Wait until nothing drives the arm: a closed stream releases it a few bus operations later. */
325 const idle = async () => {
326 for (let i = 0; i < 150; i++) {
327 const s = (await (await fetch(`${zurl}/status`)).json()) as { driving: string | null };
328 if (s.driving === null) return;
329 await sleep(20);
330 }
331 throw new Error("the arm never went idle");
332 };
333 const moved = async (targets: Record<string, number>) => {
334 const r = await zcall("/move", { targets });
335 if (r.status !== 200) throw new Error(`move ${JSON.stringify(targets)}: ${r.status} ${await r.text()}`);
336 };
337 const RANGES: Record<number, [number, number]> = { 1: [818, 3487], 2: [813, 3202], 3: [873, 3083], 4: [902, 2916], 5: [0, 4095], 6: [1995, 3525] };
338 const mid = (id: number) => Math.round((RANGES[id]![0] + RANGES[id]![1]) / 2);
339 const ticks = (id: number, deg: number) => Math.round(mid(id) + (deg * 4095) / 360);
340 beforeAll(async () => {
341 fake6 = new FakeBus([1, 2, 3, 4, 5, 6], { stuck: new Set([3]) });
342 for (const [id, [min, max]] of Object.entries(RANGES)) {
343 fake6.servos.get(Number(id))!.set([min & 0xff, min >> 8, max & 0xff, max >> 8], 9);
344 fake6.servos.get(Number(id))!.set([mid(Number(id)) & 0xff, mid(Number(id)) >> 8], 56); // present = middle of range = the model's zero
345 }
346 zoneServer = await startServer(new FeetechBus(fake6), {
347 host: "127.0.0.1",
348 port: 0,
349 posesFile: join(mkdtempSync(join(tmpdir(), "so101-zone-")), "poses.json"),
350 modelDir: new URL("../../../Simulation/SO101", import.meta.url).pathname,
351 watchdogMs: 60,
352 jamSampleMs: 15,
353 });
354 zurl = `http://127.0.0.1:${zoneServer.port}`;
355 });
356 afterAll(() => Promise.race([zoneServer.close(), sleep(500)]));
357
358 test("serves the model, its meshes and the vendored libraries, and nothing outside them", async () => {
359 const info = (await (await fetch(`${zurl}/model.json`)).json()) as { urdf: string; joints: Array<{ name: string; id: number }>; floorZ: number };
360 expect(info.joints.map((j) => j.id)).toEqual([1, 2, 3, 4, 5, 6]);
361 expect(info.floorZ).toBeLessThan(0);
362 expect((await fetch(`${zurl}${info.urdf}`)).headers.get("content-type")).toBe("application/xml");
363 expect((await fetch(`${zurl}/model/assets/base_so101_v2.stl`)).headers.get("content-type")).toBe("model/stl");
364 expect((await fetch(`${zurl}/model/../package.json`)).status).toBe(404);
365 expect((await fetch(`${zurl}/model/README.md`)).status).toBe(404);
366 expect((await fetch(`${zurl}/vendor/three/build/three.module.js`)).status).toBe(200);
367 expect((await fetch(`${zurl}/vendor/urdf-loader/src/urdf-manipulator-element.js`)).status).toBe(200);
368 expect((await fetch(`${zurl}/vendor/ws/index.js`)).status).toBe(404);
369 expect((await fetch(`${zurl}/vendor/three/package.json`)).status).toBe(200);
370 expect((await fetch(`${zurl}/vendor/three/../ws/index.js`)).status).toBe(404);
371 });
372
373 test("serves the printable sheets, and nothing else from that folder", async () => {
374 const list = (await (await fetch(`${zurl}/printables`)).json()) as Array<{ file: string; url: string; bytes: number }>;
375 expect(list.map((p) => p.file)).toEqual(["charuco_board.png", "cube_markers.png", "gripper_marker.png"]);
376 const png = await fetch(`${zurl}${list[0]!.url}`);
377 expect(png.headers.get("content-type")).toBe("image/png");
378 expect((await png.arrayBuffer()).byteLength).toBe(list[0]!.bytes);
379 expect((await fetch(`${zurl}/printables/../markers.py`)).status).toBe(404);
380 expect((await fetch(`${zurl}/printables/nope.png`)).status).toBe(404);
381 });
382
383 test("status reports the zone, and the rest pose is clear", async () => {
384 const s = (await (await fetch(`${zurl}/status`)).json()) as { zone: string; violation: unknown; lowestMm: number; blocked: unknown; jammed: number[] };
385 expect(s.zone).toBe("warn");
386 expect(s.violation).toBeNull();
387 expect(s.lowestMm).toBeGreaterThan(0);
388 expect(s.jammed).toEqual([]);
389 });
390
391 test("settings are validated and persisted", async () => {
392 expect((await zcall("/settings", { zone: "sideways" }, "PUT")).status).toBe(400);
393 expect((await zcall("/settings", { floorMarginMm: 500 }, "PUT")).status).toBe(400);
394 expect((await zcall("/settings", { jointOffsetsDeg: { nope: 1 } }, "PUT")).status).toBe(400);
395 const s = (await (await zcall("/settings", { zone: "enforce", floorMarginMm: 10, jointOffsetsDeg: { wrist_flex: 2 } }, "PUT")).json()) as { zone: string; floorMarginMm: number; jointOffsetsDeg: Record<string, number>; model: boolean };
396 expect(s).toMatchObject({ zone: "enforce", floorMarginMm: 10, jointOffsetsDeg: { wrist_flex: 2 }, model: true });
397 const again = (await (await fetch(`${zurl}/settings`)).json()) as { jointOffsetsDeg: Record<string, number> };
398 expect(again.jointOffsetsDeg).toEqual({ wrist_flex: 2 });
399 });
400
401 test("a move into the table is refused in enforce mode and only reported in warn mode", async () => {
402 const down = { targets: { "2": ticks(2, 80) } }; // shoulder lift far forward with the elbow straight: the gripper goes under the table
403 const refused = await zcall("/move", down);
404 expect(refused.status).toBe(409);
405 const body = (await refused.json()) as { reached: boolean; error: string; violation: { what: string; link: string } };
406 expect(body.reached).toBe(false);
407 expect(body.error).toMatch(/^refused: .* below the table \(plus the \d+ mm margin\)$/);
408 expect(body.violation.what).toBe("table");
409 const status = (await (await fetch(`${zurl}/status`)).json()) as { joints: Array<{ id: number; position: number }>; driving: null };
410 expect(status.joints.find((j) => j.id === 2)!.position).toBe(mid(2)); // nothing moved
411 expect(status.driving).toBeNull();
412 const m = (await (await fetch(`${zurl}/metrics`)).json()) as { counters: { block: number } };
413 expect(m.counters.block).toBe(1);
414
415 await zcall("/settings", { zone: "warn" }, "PUT");
416 const warned = (await (await zcall("/move", down)).json()) as { reached: boolean; warning: string };
417 expect(warned.reached).toBe(true);
418 expect(warned.warning).toContain("below the table");
419 await moved({ "2": mid(2) });
420 await zcall("/settings", { zone: "enforce" }, "PUT");
421 });
422
423 test("a stream frame into the table holds the arm; a safe frame resumes it", async () => {
424 const ws = new WebSocket(`ws://127.0.0.1:${zoneServer.port}/follow`);
425 await new Promise((r) => ws.once("open", r));
426 ws.send(JSON.stringify({ "1": mid(1) + 50 })); // first frame: safe, eases
427 await sleep(80);
428 ws.send(JSON.stringify({ "2": ticks(2, 80) })); // the leader dives at the table
429 await sleep(60);
430 let s = (await (await fetch(`${zurl}/status`)).json()) as { blocked: { what: string } | null; joints: Array<{ id: number; position: number }> };
431 expect(s.blocked?.what).toBe("table");
432 expect(s.joints.find((j) => j.id === 2)!.position).toBe(mid(2)); // held
433 ws.send(JSON.stringify({ "2": mid(2) + 100 })); // the leader comes back up
434 await sleep(60);
435 s = (await (await fetch(`${zurl}/status`)).json()) as typeof s;
436 expect(s.blocked).toBeNull();
437 expect(s.joints.find((j) => j.id === 2)!.position).toBe(mid(2) + 100);
438 const closed = new Promise((r) => ws.once("close", r));
439 ws.close();
440 await closed;
441 await idle();
442 });
443
444 test("a joint the stream has not driven yet is eased over, never jumped", async () => {
445 const ws = new WebSocket(`ws://127.0.0.1:${zoneServer.port}/follow`);
446 await new Promise((r) => ws.once("open", r));
447 ws.send(JSON.stringify({ "6": mid(6) + 100 }));
448 await sleep(120);
449 const before = fake6.writes.length;
450 ws.send(JSON.stringify({ "6": mid(6) + 100, "1": mid(1) + 900 })); // the pan joins the stream 900 ticks away
451 await sleep(150);
452 const writes = fake6.writes.slice(before).filter((w) => w.id === 1);
453 const velocity = writes.find((w) => w.address === 46);
454 expect(velocity!.bytes[0]! | (velocity!.bytes[1]! << 8)).toBe(300); // eased at easeSpeed, not Goal_Velocity 0 (max)
455 const s = (await (await fetch(`${zurl}/status`)).json()) as { joints: Array<{ id: number; position: number }>; stream: { frames: number; dropped: number } };
456 expect(s.joints.find((j) => j.id === 1)!.position).toBe(mid(1) + 900);
457 expect(s.stream.frames).toBe(2);
458 expect(s.stream.dropped).toBe(0);
459 const closed = new Promise((r) => ws.once("close", r));
460 ws.close();
461 await closed;
462 await idle();
463 await moved({ "1": mid(1), "6": mid(6) });
464 });
465
466 test("a joint left far behind after a block is eased over once the leader has stayed away long enough", async () => {
467 const ws = new WebSocket(`ws://127.0.0.1:${zoneServer.port}/follow`);
468 await new Promise((r) => ws.once("open", r));
469 // Frames keep coming every 10 ms, as a leader's do, so the 60 ms watchdog of this server never turns this into a plain hold.
470 const send = async (target: number, times: number) => {
471 for (let i = 0; i < times; i++) {
472 ws.send(JSON.stringify({ "2": target }));
473 await sleep(10);
474 }
475 };
476 await send(mid(2) + 200, 10); // safe, eases
477 await send(ticks(2, 80), 4); // dives at the table: dropped as a jump, or refused by the zone
478 await send(mid(2) - 500, 25); // safe, but 700 ticks from where the joint was held
479 await sleep(150);
480 const s = (await (await fetch(`${zurl}/status`)).json()) as { joints: Array<{ id: number; position: number }>; blocked: unknown; stream: { dropped: number } };
481 expect(s.joints.find((j) => j.id === 2)!.position).toBe(mid(2) - 500);
482 expect(s.blocked).toBeNull();
483 expect(s.stream.dropped).toBeGreaterThanOrEqual(19);
484 const closed = new Promise((r) => ws.once("close", r));
485 ws.close();
486 await closed;
487 await idle();
488 await moved({ "2": mid(2) });
489 });
490
491 test("from inside a violation, a stream frame may back out but not go deeper", async () => {
492 await zcall("/settings", { zone: "warn" }, "PUT");
493 await moved({ "2": ticks(2, 60) }); // warn mode lets the arm into the table
494 await zcall("/settings", { zone: "enforce" }, "PUT");
495 const ws = new WebSocket(`ws://127.0.0.1:${zoneServer.port}/follow`);
496 await new Promise((r) => ws.once("open", r));
497 ws.send(JSON.stringify({ "2": ticks(2, 70) })); // deeper: refused
498 await sleep(120);
499 let s = (await (await fetch(`${zurl}/status`)).json()) as { joints: Array<{ id: number; position: number }> };
500 expect(s.joints.find((j) => j.id === 2)!.position).toBe(ticks(2, 60));
501 ws.send(JSON.stringify({ "2": ticks(2, 50) })); // backing out: let through
502 await sleep(120);
503 s = (await (await fetch(`${zurl}/status`)).json()) as typeof s;
504 expect(s.joints.find((j) => j.id === 2)!.position).toBe(ticks(2, 50));
505 const closed = new Promise((r) => ws.once("close", r));
506 ws.close();
507 await closed;
508 await idle();
509 await zcall("/settings", { zone: "warn" }, "PUT");
510 await moved({ "2": mid(2) });
511 await zcall("/settings", { zone: "enforce" }, "PUT");
512 });
513
514 test("a recording is judged with the joints it does not drive where they are, and so is the way to its first frame", async () => {
515 // Record the pan alone while the rest sits at the safe middle: safe to play.
516 expect((await zcall("/record/start", { name: "pan", hz: 50, ids: [1] })).status).toBe(200);
517 await sleep(80);
518 expect((await zcall("/record/stop", {})).status).toBe(200);
519 expect((await zcall("/play", { name: "pan", rate: 2 })).status).toBe(200);
520 // The same recording with the lift pointing at the table is refused before anything moves.
521 await zcall("/settings", { zone: "warn" }, "PUT");
522 await moved({ "2": ticks(2, 60) });
523 await zcall("/settings", { zone: "enforce" }, "PUT");
524 const r = await zcall("/play", { name: "pan", rate: 2 });
525 expect(r.status).toBe(409);
526 expect(((await r.json()) as { error: string }).error).toContain("refused");
527 await zcall("/settings", { zone: "warn" }, "PUT");
528 await moved({ "2": mid(2) });
529 await zcall("/settings", { zone: "enforce" }, "PUT");
530 await zcall("/recording/delete", { name: "pan" });
531 });
532
533 test("a joint that pushes without moving on the stream is parked, and follows again once the leader backs off", async () => {
534 const ws = new WebSocket(`ws://127.0.0.1:${zoneServer.port}/follow`);
535 await new Promise((r) => ws.once("open", r));
536 const push = setInterval(() => ws.send(JSON.stringify({ "3": mid(3) + 300 })), 20); // the elbow (stuck in this fake) is asked to move
537 await sleep(500);
538 let s = (await (await fetch(`${zurl}/status`)).json()) as { jammed: number[] };
539 expect(s.jammed).toEqual([3]);
540 const goal = fake6.writes.filter((w) => w.id === 3 && w.address === 42).at(-1)!;
541 expect(goal.bytes[0]! | (goal.bytes[1]! << 8)).toBe(mid(3)); // parked where it is
542 clearInterval(push);
543 const m = (await (await fetch(`${zurl}/metrics`)).json()) as { counters: { jam: number } };
544 expect(m.counters.jam).toBe(1);
545 ws.send(JSON.stringify({ "3": mid(3) - 20 })); // the leader backs off (the fake is stuck both ways, so stay inside the 30-tick dead band)
546 await sleep(60);
547 s = (await (await fetch(`${zurl}/status`)).json()) as { jammed: number[] };
548 expect(s.jammed).toEqual([]);
549 const closed = new Promise((r) => ws.once("close", r));
550 ws.close();
551 await closed;
552 await idle();
553 const after = (await (await fetch(`${zurl}/metrics`)).json()) as { counters: { stream: number }; recent: Array<{ kind: string }> };
554 expect(after.counters.stream).toBeGreaterThanOrEqual(2);
555 expect(after.recent.at(-1)!.kind).toBe("stream");
556 });
557 });
558
559 describe("settings on disk", () => {
560 test("a bad value is skipped and the rest kept; unreadable JSON falls back to the defaults, with a log line", async () => {
561 const dir = mkdtempSync(join(tmpdir(), "so101-settings-"));
562 writeFileSync(join(dir, "settings.json"), JSON.stringify({ zone: "enforce", floorMarginMm: 40, jointOffsetsDeg: { wrist_roll: 45 } }));
563 const fake = new FakeBus([1, 2, 6]);
564 const s1 = await startServer(new FeetechBus(fake), { host: "127.0.0.1", port: 0, ids: [1, 2, 6], posesFile: join(dir, "poses.json") });
565 const got = (await (await fetch(`http://127.0.0.1:${s1.port}/settings`)).json()) as { zone: string; floorMarginMm: number; jointOffsetsDeg: Record<string, number> };
566 expect(got.zone).toBe("enforce");
567 expect(got.floorMarginMm).toBe(40);
568 expect(got.jointOffsetsDeg).toEqual({});
569 await Promise.race([s1.close(), sleep(300)]);
570 writeFileSync(join(dir, "settings.json"), "{ not json");
571 const s2 = await startServer(new FeetechBus(new FakeBus([1, 2, 6])), { host: "127.0.0.1", port: 0, ids: [1, 2, 6], posesFile: join(dir, "poses.json") });
572 const again = (await (await fetch(`http://127.0.0.1:${s2.port}/settings`)).json()) as { zone: string };
573 expect(again.zone).toBe("warn");
574 await Promise.race([s2.close(), sleep(300)]);
575 });
576 });
Software/so101/src/server.ts added +899
@@ -0,0 +1,899 @@
1 /**
2 * The network face of one arm: an HTTP API for state and supervised moves, and a WebSocket stream
3 * for joint targets at teleoperation rate. Runs wherever the follower's board is plugged in.
4 *
5 * GET / the browser app: cameras, 3D model, live joints, poses, jog, record and play, metrics
6 * GET /status every joint: position, load, temperature, torque; what drives the arm; zone state
7 * GET /limits each servo's calibrated Min/Max_Position_Limit
8 * GET /poses the saved poses
9 * POST /pose/save {"name": "he1", "ids"?: [1,2,3,4,5]} remember where the joints are now
10 * POST /pose/delete {"name": "he1"}
11 * POST /pose/go {"name": "home", "speed"?: 300, "torque"?: 70, "release"?: false}
12 * POST /move {"targets": {"3": 2048}, "speed"?, "torque"?, "release"?} supervised, any joints
13 * POST /grip {"position": 2000, "torque": 30} close the gripper toward a position at a low torque cap and hold
14 * POST /torque {"on": true|false}
15 * POST /stop end whatever drives the arm: a move, a playback, a recording, or a stream; the arm holds
16 * GET /recordings the saved recordings with their length
17 * POST /record/start {"name": "wave", "hz"?: 30} torque off, sample the joints until /record/stop
18 * POST /record/stop save the recording
19 * POST /recording/delete {"name": "wave"}
20 * POST /play {"name": "wave", "rate"?: 1, "torque"?: 70, "release"?: false}
21 * GET /calibration {"camera": {...}|null, "handeye": {...}|null, ...} what the vision kit published
22 * PUT /calibration/<kind> store that JSON next to the poses (kind: camera, camera2, handeye, ...)
23 * GET /settings the safe zone: {"zone": "enforce"|"warn"|"off", "floorMarginMm", "baseMarginMm", "jointOffsetsDeg"}
24 * PUT /settings change any of those
25 * GET /metrics jams, blocks, stalls, peak load and temperature per joint, recent events
26 * GET /model.json the arm model: joints, the table plane, where the URDF is served
27 * GET /model/<file> the URDF and its meshes; GET /vendor/<pkg>/<file> three.js and the URDF loader
28 * GET /printables the sheets to print for the vision quest (board, cube markers, gripper marker); GET /printables/<file>
29 * WS /follow client sends {"1": ticks, "2": ticks, ...} frames; the server drives the arm
30 *
31 * One thing drives the arm at a time. Every target, whether a move or a stream frame, is run through
32 * the arm's own geometry first: a pose that would push a part into the table or the base is refused
33 * (or only reported, in "warn" mode). On the stream, a joint that pushes hard without moving is a jam:
34 * its goal is parked where it is, and it follows again once the leader backs off.
35 */
36
37 import { existsSync, mkdirSync, readdirSync, readFileSync, renameSync, statSync, unlinkSync, writeFileSync } from "node:fs";
38 import { createServer, type IncomingMessage, type ServerResponse } from "node:http";
39 import { dirname, extname, join, resolve } from "node:path";
40 import { fileURLToPath } from "node:url";
41 import { WebSocketServer, type WebSocket } from "ws";
42 import type { FeetechBus } from "./bus.ts";
43 import { ArmModel, gripperTicksToRadians, ticksToRadians, type Violation } from "./kinematics.ts";
44 import { clamp, type Frame } from "./mapping.ts";
45 import { Metrics } from "./metrics.ts";
46 import { MotionError, moveToPose, playTrajectory, recordTrajectory, STALL_SPAN_TICKS } from "./motion.ts";
47 import { assertPoseName, parsePoseFile, serializePoseFile, type PoseFile } from "./poses.ts";
48 import { JOINTS, STS3215, ticksToDegrees } from "./sts3215.ts";
49 import { parseTrajectory, serializeTrajectory, type Trajectory } from "./trajectory.ts";
50
51 export interface ServerOptions {
52 host?: string;
53 port?: number;
54 ids?: readonly number[];
55 /** The poses file; recordings, calibration, settings and metrics live beside it. */
56 posesFile?: string;
57 /** Folder holding so101_new_calib.urdf and assets/; without it there is no safe zone and no 3D model. */
58 modelDir?: string;
59 /** Folder with the printable PNG sheets of the vision kit; found beside the package or in the repo layout. */
60 printablesDir?: string;
61 /** Torque cap while a stream drives the arm, percent. */
62 streamTorque?: number;
63 /** Largest per-frame change accepted on the stream, ticks. */
64 maxStep?: number;
65 /** With no frame for this long, the arm is told to hold where it is. */
66 watchdogMs?: number;
67 /** Speed used to catch up with the first frame of a stream, ticks/s. */
68 easeSpeed?: number;
69 /** How often the stream's jam watch samples position and load, ms. */
70 jamSampleMs?: number;
71 }
72
73 export interface RunningServer {
74 readonly port: number;
75 close(): Promise<void>;
76 }
77
78 export type Driver = "stream" | "move" | "play" | "record";
79
80 export interface Settings {
81 zone: "enforce" | "warn" | "off";
82 floorMarginMm: number;
83 baseMarginMm: number;
84 /** Per joint, degrees added to the model's angle, to trim the model onto the real arm. */
85 jointOffsetsDeg: Record<string, number>;
86 }
87
88 export const DEFAULT_SETTINGS: Settings = { zone: "warn", floorMarginMm: 15, baseMarginMm: 10, jointOffsetsDeg: {} };
89
90 class HttpError extends Error {
91 readonly status: number;
92 constructor(status: number, message: string) {
93 super(message);
94 this.status = status;
95 }
96 }
97
98 const MAX_BODY = 1_000_000;
99 const URDF = "so101_new_calib.urdf";
100 /** Load, in percent of the servo's maximum, that counts as pushing against something. */
101 const JAM_LOAD = 45;
102 const JAM_WINDOW = 6;
103 /** Consecutive too-far frames for one joint before it is eased over to the leader instead of left behind. */
104 const RESUME_AFTER_DROPS = 20;
105 const VENDORED = new Set(["three", "urdf-loader"]);
106 const MIME: Record<string, string> = { ".js": "text/javascript", ".mjs": "text/javascript", ".json": "application/json", ".urdf": "application/xml", ".stl": "model/stl", ".dae": "model/vnd.collada+xml", ".png": "image/png", ".jpg": "image/jpeg" };
107
108 export async function startServer(bus: FeetechBus, options: ServerOptions = {}): Promise<RunningServer> {
109 const ids = options.ids ?? [1, 2, 3, 4, 5, 6];
110 const posesFile = resolve(options.posesFile ?? "poses.json");
111 const dataDir = dirname(posesFile);
112 const recordingsDir = join(dataDir, "recordings");
113 const calibrationDir = join(dataDir, "calibration");
114 const settingsFile = join(dataDir, "settings.json");
115 const streamTorque = options.streamTorque ?? 70;
116 const maxStep = options.maxStep ?? 400;
117 const watchdogMs = options.watchdogMs ?? 500;
118 const easeSpeed = options.easeSpeed ?? 300;
119 const jamSampleMs = options.jamSampleMs ?? 100;
120 const margin = 30;
121 const metrics = new Metrics(join(dataDir, "metrics.jsonl"));
122
123 const limits = new Map<number, { min: number; max: number }>();
124 for (const id of ids) {
125 limits.set(id, {
126 min: await bus.read(id, STS3215.Min_Position_Limit),
127 max: await bus.read(id, STS3215.Max_Position_Limit),
128 });
129 }
130
131 // ------------------------------------------------------------------ the model and the zone
132 const here = dirname(fileURLToPath(import.meta.url));
133 const modelDir = options.modelDir ?? [join(here, "../../../Simulation/SO101"), join(here, "../model")].find((d) => existsSync(join(d, URDF)));
134 const printablesDir = options.printablesDir ?? [join(here, "../../vision/printables"), join(here, "../printables")].find((d) => existsSync(d));
135 let model: ArmModel | null = null;
136 if (modelDir && existsSync(join(modelDir, URDF))) {
137 try {
138 model = ArmModel.load(join(modelDir, URDF));
139 } catch (err) {
140 console.error(`model: ${err instanceof Error ? err.message : String(err)}; no safe zone`);
141 }
142 } else console.error(`model: no ${URDF} in ${modelDir ?? "(no model dir)"}; no safe zone and no 3D view`);
143
144 let settings: Settings = { ...DEFAULT_SETTINGS };
145 if (existsSync(settingsFile)) {
146 // Field by field: one bad value must not throw away the zone mode or the other trims.
147 try {
148 const stored = JSON.parse(readFileSync(settingsFile, "utf8")) as Record<string, unknown>;
149 for (const [key, value] of Object.entries(stored)) {
150 try {
151 settings = validateSettings({ ...settings, [key]: value });
152 } catch (err) {
153 console.error(`settings: ignoring ${key} in ${settingsFile}: ${err instanceof Error ? err.message : String(err)}`);
154 }
155 }
156 } catch (err) {
157 console.error(`settings: ${settingsFile} is not readable JSON (${err instanceof Error ? err.message : String(err)}); using the defaults, zone "${settings.zone}"`);
158 }
159 }
160 const zoneSettings = () => ({ floorMargin: settings.floorMarginMm / 1000, baseMargin: settings.baseMarginMm / 1000 });
161 /** Joint angles for the model from servo ticks, with the trims applied. The gripper maps closed→0, open→its URDF limit. */
162 const gripperOpen = model?.movable.find((j) => j.name === "gripper")?.upper ?? 1.745;
163 const toQ = (positions: ReadonlyMap<number, number>) => {
164 const q = new Map<string, number>();
165 for (const [id, ticks] of positions) {
166 const name = JOINTS[id];
167 const lim = limits.get(id);
168 if (!name || !lim) continue;
169 const rad = name === "gripper" ? gripperTicksToRadians(ticks, lim.min, lim.max, gripperOpen) : ticksToRadians(ticks, lim.min, lim.max);
170 q.set(name, rad + ((settings.jointOffsetsDeg[name] ?? 0) * Math.PI) / 180);
171 }
172 return q;
173 };
174 const evaluate = (positions: ReadonlyMap<number, number>) => (model && settings.zone !== "off" ? model.evaluate(toQ(positions), zoneSettings()) : null);
175 /** Only poses the arm is in, or is sent to, count toward "closest to the table"; refused frames do not. */
176 const approach = (r: ReturnType<typeof evaluate>) => {
177 if (r && model) metrics.approach(r.lowest.z - model.floorZ, r.lowest.link);
178 };
179 const describeViolation = (v: Violation) => {
180 const part = v.link.replace(/_link$/, "").replace(/_so101_v1$/, "");
181 const marginMm = v.what === "table" ? settings.floorMarginMm : settings.baseMarginMm;
182 const beyond = v.depth * 1000 - marginMm; // the depth includes the margin
183 return beyond > 0
184 ? `${part} would be ${beyond.toFixed(0)} mm ${v.what === "table" ? "below the table" : "inside the base"} (plus the ${marginMm} mm margin)`
185 : `${part} would come within ${(-beyond).toFixed(0)} mm of the ${v.what}, inside the ${marginMm} mm margin`;
186 };
187 /** The worst violation on the straight joint-space path from one pose to another, or null. */
188 const checkPath = (from: ReadonlyMap<number, number>, to: ReadonlyMap<number, number>, steps = 12): Violation | null => {
189 if (!model || settings.zone === "off") return null;
190 let worst: Violation | null = null;
191 for (let s = 1; s <= steps; s++) {
192 const mix = new Map<number, number>();
193 for (const [id, a] of from) mix.set(id, Math.round(a + ((to.get(id) ?? a) - a) * (s / steps)));
194 const v = model.check(toQ(mix), zoneSettings());
195 if (v && (!worst || v.depth > worst.depth)) worst = v;
196 }
197 return worst;
198 };
199
200 // ------------------------------------------------------------------ who drives the arm
201 let driver: Driver | null = null;
202 let current: AbortController | null = null;
203 const claim = (kind: Driver): AbortController => {
204 if (driver) throw new HttpError(409, `${describe(driver)} is driving the arm`);
205 driver = kind;
206 current = new AbortController();
207 return current;
208 };
209 const release = () => {
210 driver = null;
211 current = null;
212 };
213 const describe = (d: Driver) => ({ stream: "a stream", move: "a move", play: "a playback", record: "a recording" })[d];
214 /** What the stream is up against right now, for /status. */
215 let blocked: Violation | null = null;
216 const jams = new Map<number, { position: number; direction: number }>();
217 let streamStats: { frames: number; dropped: number; refused: number } | null = null;
218
219 // ------------------------------------------------------------------ files
220 const writeAtomic = (path: string, text: string) => {
221 mkdirSync(dirname(path), { recursive: true });
222 writeFileSync(`${path}.tmp`, text);
223 renameSync(`${path}.tmp`, path);
224 };
225 const loadPoses = (): PoseFile => {
226 try {
227 return parsePoseFile(readFileSync(posesFile, "utf8"));
228 } catch (err) {
229 if ((err as NodeJS.ErrnoException).code === "ENOENT") return {};
230 throw new HttpError(500, `${posesFile}: ${err instanceof Error ? err.message : String(err)}`);
231 }
232 };
233 const poseNamed = (name: unknown): string => {
234 if (typeof name !== "string") throw new HttpError(400, "name must be a string");
235 try {
236 assertPoseName(name);
237 } catch (err) {
238 throw new HttpError(400, (err as Error).message);
239 }
240 return name;
241 };
242 const recordingPath = (name: string) => join(recordingsDir, `${name}.json`);
243 const loadRecording = (name: string): Trajectory => {
244 try {
245 return parseTrajectory(readFileSync(recordingPath(name), "utf8"));
246 } catch (err) {
247 if ((err as NodeJS.ErrnoException).code === "ENOENT") throw new HttpError(404, `no recording "${name}"`);
248 throw new HttpError(500, `recording "${name}": ${err instanceof Error ? err.message : String(err)}`);
249 }
250 };
251 const listRecordings = () => {
252 const out: Record<string, { hz: number; frames: number; seconds: number; recorded?: string }> = {};
253 let names: string[] = [];
254 try {
255 names = readdirSync(recordingsDir).filter((f) => f.endsWith(".json"));
256 } catch {
257 return out;
258 }
259 for (const file of names) {
260 try {
261 const t = parseTrajectory(readFileSync(join(recordingsDir, file), "utf8"));
262 out[file.slice(0, -5)] = { hz: t.hz, frames: t.frames.length, seconds: Number((t.frames.length / t.hz).toFixed(1)), ...(t.recorded ? { recorded: t.recorded } : {}) };
263 } catch {
264 /* a file that is not a recording is not listed */
265 }
266 }
267 return out;
268 };
269 const loadCalibration = (): Record<string, unknown> => {
270 const out: Record<string, unknown> = { camera: null, handeye: null };
271 try {
272 for (const file of readdirSync(calibrationDir).filter((f) => f.endsWith(".json"))) {
273 try {
274 out[file.slice(0, -5)] = JSON.parse(readFileSync(join(calibrationDir, file), "utf8"));
275 } catch {
276 /* skip a broken file */
277 }
278 }
279 } catch {
280 /* none yet */
281 }
282 return out;
283 };
284 /** A file under a root, or null for anything that tries to leave it. */
285 const safeFile = (root: string, rel: string): string | null => {
286 if (rel.includes("\0") || rel.split("/").some((part) => part === "..")) return null;
287 const path = resolve(root, rel);
288 if (path !== root && !path.startsWith(root + "/")) return null;
289 try {
290 return statSync(path).isFile() ? path : null;
291 } catch {
292 return null;
293 }
294 };
295
296 // ------------------------------------------------------------------ helpers
297 const json = (res: ServerResponse, status: number, body: unknown) => {
298 res.writeHead(status, { "content-type": "application/json" });
299 res.end(JSON.stringify(body));
300 };
301 const file = (res: ServerResponse, path: string, cache: string) => {
302 res.writeHead(200, { "content-type": MIME[extname(path).toLowerCase()] ?? "application/octet-stream", "cache-control": cache });
303 res.end(readFileSync(path));
304 };
305 const readBody = (req: IncomingMessage) =>
306 new Promise<Record<string, unknown>>((resolve, reject) => {
307 let data = "";
308 req.on("data", (c) => {
309 data += c;
310 if (data.length > MAX_BODY) {
311 req.destroy();
312 reject(new HttpError(413, "body too large"));
313 }
314 });
315 req.on("end", () => {
316 if (!data.trim()) return resolve({});
317 try {
318 const parsed: unknown = JSON.parse(data);
319 if (typeof parsed !== "object" || parsed === null || Array.isArray(parsed)) throw new Error();
320 resolve(parsed as Record<string, unknown>);
321 } catch {
322 reject(new HttpError(400, "body must be a JSON object"));
323 }
324 });
325 req.on("error", reject);
326 });
327 const number = (value: unknown, fallback: number, min: number, max: number, what: string): number => {
328 if (value === undefined) return fallback;
329 if (typeof value !== "number" || !Number.isFinite(value)) throw new HttpError(400, `${what} must be a number`);
330 return clamp(value, min, max);
331 };
332 const idList = (value: unknown): number[] => {
333 if (value === undefined) return [...ids];
334 if (!Array.isArray(value) || !value.length || !value.every((id) => Number.isInteger(id) && ids.includes(id as number))) {
335 throw new HttpError(400, `ids must be a non-empty list drawn from ${ids.join(", ")}`);
336 }
337 return [...new Set(value as number[])];
338 };
339 function validateSettings(s: Partial<Settings>): Settings {
340 const out: Settings = { ...DEFAULT_SETTINGS, jointOffsetsDeg: {} };
341 if (s.zone !== undefined) {
342 if (s.zone !== "enforce" && s.zone !== "warn" && s.zone !== "off") throw new HttpError(400, 'zone must be "enforce", "warn" or "off"');
343 out.zone = s.zone;
344 }
345 for (const key of ["floorMarginMm", "baseMarginMm"] as const) {
346 if (s[key] === undefined) continue;
347 if (typeof s[key] !== "number" || !Number.isFinite(s[key]) || s[key]! < 0 || s[key]! > 100) throw new HttpError(400, `${key} must be 0-100`);
348 out[key] = s[key]!;
349 }
350 if (s.jointOffsetsDeg !== undefined) {
351 if (typeof s.jointOffsetsDeg !== "object" || s.jointOffsetsDeg === null || Array.isArray(s.jointOffsetsDeg)) throw new HttpError(400, "jointOffsetsDeg must be an object");
352 for (const [name, deg] of Object.entries(s.jointOffsetsDeg)) {
353 if (!Object.values(JOINTS).includes(name)) throw new HttpError(400, `no joint "${name}"`);
354 if (typeof deg !== "number" || !Number.isFinite(deg) || Math.abs(deg) > 30) throw new HttpError(400, `offset for ${name} must be within ±30 degrees`);
355 if (deg !== 0) out.jointOffsetsDeg[name] = deg;
356 }
357 }
358 return out;
359 }
360
361 const status = async () => {
362 const positions = await bus.syncRead(ids, STS3215.Present_Position);
363 const loads = await bus.syncRead(ids, STS3215.Present_Load);
364 const temps = await bus.syncRead(ids, STS3215.Present_Temperature);
365 const torque = await bus.syncRead(ids, STS3215.Torque_Enable);
366 const joints = ids.map((id) => ({
367 id,
368 joint: JOINTS[id] ?? null,
369 position: positions.get(id) ?? null,
370 degrees: positions.has(id) ? Number(ticksToDegrees(positions.get(id)!).toFixed(1)) : null,
371 load: loads.has(id) ? loads.get(id)! / 10 : null,
372 temperature: temps.get(id) ?? null,
373 torque: torque.has(id) ? torque.get(id) === 1 : null,
374 }));
375 metrics.sample(joints);
376 const zone = evaluate(positions);
377 approach(zone);
378 return {
379 joints,
380 busy: driver !== null,
381 driving: driver,
382 recording: recording?.name ?? null,
383 zone: model ? settings.zone : "off",
384 violation: zone?.violation ?? null,
385 lowestMm: zone && model ? Number(((zone.lowest.z - model.floorZ) * 1000).toFixed(0)) : null,
386 blocked,
387 jammed: [...jams.keys()],
388 stream: streamStats,
389 };
390 };
391
392 /** A supervised move to explicit targets; the one way joints move over HTTP. */
393 const move = async (targetIds: number[], positions: number[], body: Record<string, unknown>, source: string) => {
394 const controller = claim("move");
395 try {
396 const from = await bus.syncRead(ids, STS3215.Present_Position);
397 const to = new Map(from);
398 targetIds.forEach((id, i) => to.set(id, positions[i]!));
399 const violation = checkPath(from, to);
400 if (violation) {
401 metrics.event("block", { source, what: violation.what, link: violation.link, depthMm: Math.round(violation.depth * 1000), mode: settings.zone });
402 if (settings.zone === "enforce") return { status: 409, body: { reached: false, error: `refused: ${describeViolation(violation)}`, violation } };
403 }
404 const result = await moveToPose(bus, targetIds, positions, {
405 speed: number(body.speed, 300, 20, 1500, "speed"),
406 torquePercent: number(body.torque, 70, 10, 100, "torque"),
407 signal: controller.signal,
408 });
409 if (body.release === true) await bus.syncWrite(STS3215.Torque_Enable, targetIds.map((id) => [id, 0] as const));
410 metrics.counters.move++;
411 return { status: 200, body: { reached: true, final: Object.fromEntries(result.final), adjusted: Object.fromEntries(result.adjusted), ...(violation ? { warning: describeViolation(violation) } : {}) } };
412 } catch (err) {
413 if (err instanceof MotionError) {
414 metrics.event(err.stalled.length ? "stall" : err.missing.length ? "missing" : "move", { source, error: err.message, stalled: err.stalled, missing: err.missing });
415 return { status: 409, body: { reached: false, error: err.message, stalled: err.stalled, missing: err.missing } };
416 }
417 throw err;
418 } finally {
419 release();
420 }
421 };
422
423 // ------------------------------------------------------------------ recording
424 type Recorded = { trajectory: Trajectory; filled: number } | { error: string };
425 let recording: { name: string; hz: number; done: Promise<Recorded> } | null = null;
426 const finishRecording = async () => {
427 if (!recording || !current) throw new HttpError(409, "nothing is being recorded");
428 const { name, done } = recording;
429 const controller = current;
430 recording = null; // taken: a second caller, racing this one, finds nothing to stop
431 controller.abort();
432 const result = await done;
433 release();
434 if ("error" in result) throw new HttpError(500, `recording "${name}" failed: ${result.error}; nothing saved`);
435 const { trajectory, filled } = result;
436 if (!trajectory.frames.length) throw new HttpError(409, "nothing recorded; did the servos answer?");
437 writeAtomic(recordingPath(name), serializeTrajectory(trajectory));
438 metrics.event("record", { name, frames: trajectory.frames.length, seconds: trajectory.frames.length / trajectory.hz });
439 return { saved: name, frames: trajectory.frames.length, seconds: Number((trajectory.frames.length / trajectory.hz).toFixed(1)), filled };
440 };
441
442 // ------------------------------------------------------------------ routes
443 const viewerPath = join(here, "viewer.html");
444 const nodeModules = resolve(here, "../node_modules");
445 const http = createServer(async (req, res) => {
446 try {
447 const url = new URL(req.url ?? "/", "http://x");
448 const route = `${req.method} ${url.pathname}`;
449 const decode = (text: string) => {
450 try {
451 return decodeURIComponent(text);
452 } catch {
453 return "\0"; // never a file
454 }
455 };
456 if (req.method === "GET" && url.pathname.startsWith("/model/") && modelDir) {
457 const path = safeFile(resolve(modelDir), decode(url.pathname.slice("/model/".length)));
458 if (!path || ![".urdf", ".stl", ".dae", ".png", ".jpg"].includes(extname(path).toLowerCase())) return json(res, 404, { error: "no such model file" });
459 return file(res, path, "max-age=86400");
460 }
461 if (req.method === "GET" && url.pathname === "/printables") {
462 let files: string[] = [];
463 try {
464 files = printablesDir ? readdirSync(printablesDir).filter((f) => f.endsWith(".png")).sort() : [];
465 } catch {
466 /* none */
467 }
468 return json(res, 200, files.map((f) => ({ file: f, url: `/printables/${f}`, bytes: statSync(join(printablesDir!, f)).size })));
469 }
470 if (req.method === "GET" && url.pathname.startsWith("/printables/") && printablesDir) {
471 const path = safeFile(resolve(printablesDir), decode(url.pathname.slice("/printables/".length)));
472 if (!path || extname(path).toLowerCase() !== ".png") return json(res, 404, { error: "no such printable" });
473 return file(res, path, "max-age=3600");
474 }
475 if (req.method === "GET" && url.pathname.startsWith("/vendor/")) {
476 const [pkg, ...rest] = decode(url.pathname.slice("/vendor/".length)).split("/");
477 const path = pkg && VENDORED.has(pkg) && rest.length ? safeFile(join(nodeModules, pkg), rest.join("/")) : null;
478 if (!path || ![".js", ".mjs", ".json"].includes(extname(path).toLowerCase())) return json(res, 404, { error: "no such vendored file" });
479 return file(res, path, "max-age=86400");
480 }
481 if (req.method === "PUT" && url.pathname.startsWith("/calibration/")) {
482 const kind = url.pathname.slice("/calibration/".length);
483 if (!/^[a-z][a-z0-9_-]{0,31}$/.test(kind)) throw new HttpError(400, "calibration kind: lowercase letters, digits, - and _");
484 const body = await readBody(req);
485 writeAtomic(join(calibrationDir, `${kind}.json`), JSON.stringify({ ...body, saved: new Date().toISOString() }, null, 1));
486 return json(res, 200, { stored: kind });
487 }
488 switch (route) {
489 case "GET /":
490 case "GET /index.html":
491 res.writeHead(200, { "content-type": "text/html; charset=utf-8", "cache-control": "no-store" });
492 return res.end(readFileSync(viewerPath)); // read per request: a viewer change deploys with rsync alone
493 case "GET /status":
494 return json(res, 200, await status());
495 case "GET /limits":
496 return json(res, 200, Object.fromEntries(limits));
497 case "GET /poses":
498 return json(res, 200, loadPoses());
499 case "GET /recordings":
500 return json(res, 200, listRecordings());
501 case "GET /calibration":
502 return json(res, 200, loadCalibration());
503 case "GET /metrics":
504 return json(res, 200, metrics.summary());
505 case "GET /settings":
506 return json(res, 200, { ...settings, model: model !== null });
507 case "PUT /settings": {
508 const body = await readBody(req);
509 settings = validateSettings({ ...settings, ...body, jointOffsetsDeg: { ...settings.jointOffsetsDeg, ...((body.jointOffsetsDeg as Record<string, number> | undefined) ?? {}) } });
510 writeAtomic(settingsFile, JSON.stringify(settings, null, 1));
511 return json(res, 200, { ...settings, model: model !== null });
512 }
513 case "GET /model.json":
514 if (!model) return json(res, 404, { error: "no model loaded on this server" });
515 return json(res, 200, {
516 urdf: `/model/${URDF}`,
517 floorZ: model.floorZ,
518 baseTop: model.baseTop,
519 joints: model.movable.map((j) => ({ name: j.name, id: Number(Object.entries(JOINTS).find(([, n]) => n === j.name)?.[0] ?? 0), lower: j.lower, upper: j.upper })),
520 });
521 case "POST /pose/save": {
522 const body = await readBody(req);
523 const name = poseNamed(body.name);
524 const wanted = idList(body.ids);
525 const positions = await bus.syncRead(wanted, STS3215.Present_Position);
526 const missing = wanted.filter((id) => !positions.has(id));
527 if (missing.length) throw new HttpError(409, `no response from servo ${missing.join(", ")}`);
528 const pose = { ids: wanted, positions: wanted.map((id) => positions.get(id)!), saved: new Date().toISOString() };
529 writeAtomic(posesFile, serializePoseFile({ ...loadPoses(), [name]: pose }));
530 const onStops = wanted.filter((id) => {
531 const lim = limits.get(id)!;
532 return positions.get(id)! - lim.min < 40 || lim.max - positions.get(id)! < 40;
533 });
534 return json(res, 200, { name, ...pose, onStops });
535 }
536 case "POST /pose/delete": {
537 const name = poseNamed((await readBody(req)).name);
538 const poses = loadPoses();
539 if (!Object.hasOwn(poses, name)) throw new HttpError(404, `no pose "${name}"`);
540 const { [name]: _, ...rest } = poses;
541 writeAtomic(posesFile, serializePoseFile(rest));
542 return json(res, 200, { deleted: name });
543 }
544 case "POST /pose/go": {
545 const body = await readBody(req);
546 const name = poseNamed(body.name);
547 const poses = loadPoses();
548 if (!Object.hasOwn(poses, name)) throw new HttpError(404, `no pose "${name}"`);
549 const pose = poses[name]!;
550 const result = await move([...pose.ids], [...pose.positions], body, `pose ${name}`);
551 return json(res, result.status, result.body);
552 }
553 case "POST /move": {
554 const body = await readBody(req);
555 const targets = body.targets;
556 if (typeof targets !== "object" || targets === null || Array.isArray(targets)) throw new HttpError(400, "targets must be an object of id: ticks");
557 const targetIds: number[] = [];
558 const positions: number[] = [];
559 for (const [key, value] of Object.entries(targets as Record<string, unknown>)) {
560 const id = Number(key);
561 if (!/^\d+$/.test(key) || !ids.includes(id)) throw new HttpError(400, `no servo ${key} on this arm`);
562 if (!Number.isInteger(value)) throw new HttpError(400, `target for servo ${key} must be an integer`);
563 targetIds.push(id);
564 positions.push(value as number);
565 }
566 if (!targetIds.length) throw new HttpError(400, "targets is empty");
567 const result = await move(targetIds, positions, body, "move");
568 return json(res, result.status, result.body);
569 }
570 case "POST /grip": {
571 // Not supervised on purpose: a supervised close stops at the first contact and holds with no force.
572 // The servo's own torque cap does the squeezing, and its overload protection holds a steady grip.
573 const body = await readBody(req);
574 const id = 6;
575 const lim = limits.get(id);
576 if (!lim) throw new HttpError(400, "this arm has no gripper (servo 6)");
577 if (!Number.isInteger(body.position)) throw new HttpError(400, "position must be an integer");
578 const position = clamp(body.position as number, lim.min + margin, lim.max - margin);
579 const torque = number(body.torque, 30, 10, 60, "torque");
580 claim("move");
581 try {
582 const present = await bus.syncRead([id], STS3215.Present_Position);
583 if (!present.has(id)) throw new HttpError(409, "no response from the gripper");
584 await bus.syncWrite(STS3215.Goal_Position, [[id, present.get(id)!]]);
585 await bus.syncWrite(STS3215.Torque_Limit, [[id, Math.round(torque * 10)]]); // stays low while it holds; the next supervised gripper move puts the ceiling back
586 await bus.syncWrite(STS3215.Goal_Velocity, [[id, 300]]);
587 await bus.syncWrite(STS3215.Torque_Enable, [[id, 1]]);
588 await bus.syncWrite(STS3215.Goal_Position, [[id, position]]);
589 } finally {
590 release();
591 }
592 return json(res, 200, { gripping: position, torque });
593 }
594 case "POST /torque": {
595 const { on } = await readBody(req);
596 if (typeof on !== "boolean") throw new HttpError(400, "on must be true or false");
597 if (driver) throw new HttpError(409, `${describe(driver)} is driving the arm`);
598 if (on) {
599 const positions = await bus.syncRead(ids, STS3215.Present_Position);
600 await bus.syncWrite(STS3215.Goal_Position, ids.filter((id) => positions.has(id)).map((id) => [id, positions.get(id)!] as const));
601 }
602 await bus.syncWrite(STS3215.Torque_Enable, ids.map((id) => [id, on ? 1 : 0] as const));
603 return json(res, 200, { torque: on });
604 }
605 case "POST /stop": {
606 const was = driver;
607 if (was === "stream") for (const client of wss.clients) client.close(1000, "stopped from the viewer"); // its close handler holds the arm
608 else if (was === "record") {
609 const summary = await finishRecording();
610 return json(res, 200, { stopped: was, ...summary });
611 } else current?.abort(); // the supervisor stops the move where it is
612 return json(res, 200, { stopped: was });
613 }
614 case "POST /record/start": {
615 const body = await readBody(req);
616 const name = poseNamed(body.name);
617 const hz = number(body.hz, 30, 1, 100, "hz");
618 const wanted = idList(body.ids);
619 if (Object.hasOwn(listRecordings(), name)) throw new HttpError(409, `recording "${name}" already exists; pick another name`);
620 const controller = claim("record");
621 const done: Promise<Recorded> = recordTrajectory(bus, wanted, { hz, signal: controller.signal }).then(
622 (r) => r,
623 (err: unknown) => ({ error: err instanceof Error ? err.message : String(err) }), // reported by /record/stop, never unhandled
624 );
625 recording = { name, hz, done };
626 return json(res, 200, { recording: name, hz, ids: wanted });
627 }
628 case "POST /record/stop":
629 return json(res, 200, await finishRecording());
630 case "POST /recording/delete": {
631 const name = poseNamed((await readBody(req)).name);
632 if (recording?.name === name) throw new HttpError(409, `"${name}" is being recorded right now`);
633 try {
634 unlinkSync(recordingPath(name));
635 } catch (err) {
636 if ((err as NodeJS.ErrnoException).code === "ENOENT") throw new HttpError(404, `no recording "${name}"`);
637 throw err;
638 }
639 return json(res, 200, { deleted: name });
640 }
641 case "POST /play": {
642 const body = await readBody(req);
643 const name = poseNamed(body.name);
644 const trajectory = loadRecording(name);
645 const controller = claim("play");
646 try {
647 if (model && settings.zone !== "off") {
648 // The way to the first frame, then every frame (sampled when the recording is long), each judged as the whole
649 // pose with the joints the recording does not drive left where they are. Nothing moves before this passes.
650 const present = await bus.syncRead(ids, STS3215.Present_Position);
651 const poseAt = (frame: readonly number[]) => {
652 const pose = new Map(present);
653 trajectory.ids.forEach((id, i) => pose.set(id, frame[i]!));
654 return pose;
655 };
656 const refuse = (v: Violation, where: string): boolean => {
657 metrics.event("block", { source: `play ${name}`, where, what: v.what, link: v.link, depthMm: Math.round(v.depth * 1000), mode: settings.zone });
658 if (settings.zone !== "enforce") return false;
659 json(res, 409, { played: 0, error: `refused: ${where}: ${describeViolation(v)}`, violation: v });
660 return true;
661 };
662 const onTheWay = checkPath(present, poseAt(trajectory.frames[0]!));
663 if (onTheWay && refuse(onTheWay, "on the way to the first frame")) return;
664 const stride = Math.max(1, Math.ceil(trajectory.frames.length / 3000));
665 for (let index = 0; index < trajectory.frames.length; index += stride) {
666 const v = model.check(toQ(poseAt(trajectory.frames[index]!)), zoneSettings());
667 if (v) {
668 if (refuse(v, `frame ${index} of ${trajectory.frames.length}`)) return;
669 break;
670 }
671 }
672 }
673 const played = await playTrajectory(bus, trajectory, {
674 rate: number(body.rate, 1, 0.1, 2, "rate"),
675 torquePercent: number(body.torque, 70, 10, 100, "torque"),
676 signal: controller.signal,
677 });
678 if (body.release === true) await bus.syncWrite(STS3215.Torque_Enable, trajectory.ids.map((id) => [id, 0] as const));
679 metrics.event("play", { name, played, frames: trajectory.frames.length });
680 return json(res, 200, { played, frames: trajectory.frames.length });
681 } catch (err) {
682 if (err instanceof MotionError) {
683 metrics.event(err.stalled.length ? "stall" : err.missing.length ? "missing" : "play", { source: `play ${name}`, error: err.message, stalled: err.stalled, missing: err.missing });
684 return json(res, 409, { played: 0, error: err.message, stalled: err.stalled, missing: err.missing });
685 }
686 throw err;
687 } finally {
688 release();
689 }
690 }
691 }
692 json(res, 404, { error: "no such route" });
693 } catch (err) {
694 if (err instanceof HttpError) return json(res, err.status, { error: err.message });
695 json(res, 500, { error: err instanceof Error ? err.message : String(err) });
696 }
697 });
698
699 // ------------------------------------------------------------------ the stream
700 const wss = new WebSocketServer({ server: http, path: "/follow" });
701 wss.on("connection", (ws: WebSocket) => {
702 if (driver) {
703 ws.close(1013, `${describe(driver)} is driving the arm`);
704 return;
705 }
706 claim("stream");
707 const startedAt = Date.now();
708 const stats = { frames: 0, dropped: 0, refused: 0 };
709 streamStats = stats;
710 let last: Map<number, number> | null = null;
711 /** Consecutive frames dropped per joint for jumping too far; after enough of them the joint is eased over instead of left behind. */
712 const drops = new Map<number, number>();
713 /** How deep in a violation the pose the arm was last sent to is; a frame may never go deeper, but may back out. */
714 let heldDepth = 0;
715 /** The latest sampled positions, so a frame that names only some joints is judged with the rest where they are. */
716 let present = new Map<number, number>();
717 let armed = false;
718 let timer: ReturnType<typeof setTimeout> | undefined;
719 let chain: Promise<void> = Promise.resolve();
720 const run = (fn: () => Promise<void>) => (chain = chain.then(fn).catch((err: unknown) => console.error(`stream: ${err instanceof Error ? err.message : String(err)}`)));
721
722 const hold = () =>
723 run(async () => {
724 const positions = await bus.syncRead(ids, STS3215.Present_Position);
725 await bus.syncWrite(STS3215.Goal_Position, ids.filter((id) => positions.has(id)).map((id) => [id, positions.get(id)!] as const));
726 last = null;
727 });
728 const arm = () =>
729 run(async () => {
730 const positions = await bus.syncRead(ids, STS3215.Present_Position);
731 present = new Map(positions);
732 const ready = ids.filter((id) => positions.has(id));
733 await bus.syncWrite(STS3215.Goal_Position, ready.map((id) => [id, positions.get(id)!] as const));
734 await bus.syncWrite(STS3215.Torque_Limit, ready.map((id) => [id, streamTorque * 10] as const));
735 await bus.syncWrite(STS3215.Acceleration, ready.map((id) => [id, 254] as const));
736 await bus.syncWrite(STS3215.Goal_Velocity, ready.map((id) => [id, 0] as const));
737 await bus.syncWrite(STS3215.Torque_Enable, ready.map((id) => [id, 1] as const));
738 last = null; // the first frame eases the arm over, however far away the leader is
739 heldDepth = evaluate(positions)?.violation?.depth ?? 0;
740 armed = true;
741 });
742 let easing = false;
743
744 // The jam watch: a joint that pushes hard without moving toward its target is parked where it is.
745 const windows = new Map<number, Array<{ p: number; l: number }>>();
746 const observe = async (id: number, p: number, l: number, target: number | undefined): Promise<boolean> => {
747 present.set(id, p);
748 const w = windows.get(id) ?? [];
749 w.push({ p, l });
750 if (w.length > JAM_WINDOW) w.shift();
751 windows.set(id, w);
752 if (jams.has(id) || w.length < JAM_WINDOW || target === undefined) return jams.has(id);
753 const pushing = w.every((s) => Math.abs(s.l) >= JAM_LOAD * 10);
754 const travel = Math.max(...w.map((s) => s.p)) - Math.min(...w.map((s) => s.p));
755 if (!(pushing && travel < STALL_SPAN_TICKS && Math.abs(target - p) > 30)) return false;
756 await bus.syncWrite(STS3215.Goal_Position, [[id, p]]);
757 jams.set(id, { position: p, direction: Math.sign(target - p) });
758 last?.set(id, p);
759 metrics.event("jam", { source: "stream", id, joint: JOINTS[id] ?? null, position: p, target, load: l / 10 });
760 return true;
761 };
762 const watch = setInterval(
763 () =>
764 void run(async () => {
765 if (!armed || last === null) return;
766 const positions = await bus.syncRead(ids, STS3215.Present_Position);
767 const loads = await bus.syncRead(ids, STS3215.Present_Load);
768 for (const id of ids) {
769 const p = positions.get(id), l = loads.get(id);
770 if (p !== undefined && l !== undefined) await observe(id, p, l, last.get(id));
771 }
772 }),
773 jamSampleMs,
774 );
775
776 ws.on("message", (data) => {
777 let frame: Frame;
778 try {
779 frame = JSON.parse(String(data));
780 } catch {
781 return;
782 }
783 clearTimeout(timer);
784 timer = setTimeout(() => void hold(), watchdogMs);
785 if (!armed) void arm();
786 if (easing) return; // frames that arrive while catching up are superseded by the next one
787 run(async () => {
788 const targets: Array<readonly [number, number]> = [];
789 let ease = last === null; // after a hold, everything eases
790 for (const [key, value] of Object.entries(frame)) {
791 const id = Number(key);
792 const lim = limits.get(id);
793 if (!lim || !Number.isFinite(value)) continue;
794 const target = clamp(Math.round(value), lim.min + margin, lim.max - margin);
795 const jam = jams.get(id);
796 if (jam) {
797 if ((target - jam.position) * jam.direction > -10) continue; // still pushing into the obstacle: wait for the leader to back off
798 jams.delete(id);
799 windows.delete(id);
800 }
801 const previous = last?.get(id);
802 if (previous === undefined) ease = true; // a joint this stream has not driven yet is eased over, never jumped
803 else if (Math.abs(target - previous) > maxStep) {
804 stats.dropped++;
805 const n = (drops.get(id) ?? 0) + 1;
806 drops.set(id, n);
807 if (n < RESUME_AFTER_DROPS) continue; // no hand moves that far in one frame
808 // The leader has stayed far away (after a block or a jam): ease over to it rather than leave the joint stranded.
809 drops.delete(id);
810 ease = true;
811 } else drops.delete(id);
812 targets.push([id, target]);
813 }
814 if (!targets.length) return;
815 stats.frames++;
816 // The zone: the frame is judged as the whole pose it would make, with uncommanded joints where they are.
817 const pose = new Map(present);
818 for (const [id, t] of last ?? []) pose.set(id, t);
819 for (const [id, t] of targets) pose.set(id, t);
820 const zone = evaluate(pose);
821 if (zone?.violation) {
822 if (!blocked) metrics.event("block", { source: "stream", what: zone.violation.what, link: zone.violation.link, depthMm: Math.round(zone.violation.depth * 1000), mode: settings.zone });
823 blocked = zone.violation;
824 if (settings.zone === "enforce" && zone.violation.depth > heldDepth + 0.001) {
825 stats.refused++;
826 return; // deeper in than the arm already is: it stays at the last good targets; a frame that backs out is let through
827 }
828 } else blocked = null;
829 heldDepth = zone?.violation?.depth ?? 0;
830 approach(zone);
831 if (ease) {
832 // Catch up slowly with every joint in this frame, then hand over to the full-rate stream.
833 // A joint that jams on the way is parked and no longer waited for.
834 easing = true;
835 try {
836 await bus.syncWrite(STS3215.Goal_Velocity, targets.map(([id]) => [id, easeSpeed] as const));
837 await bus.syncWrite(STS3215.Goal_Position, targets);
838 const deadline = Date.now() + 4000;
839 const wanted = new Map(targets);
840 while (Date.now() < deadline) {
841 const positions = await bus.syncRead([...wanted.keys()], STS3215.Present_Position);
842 const loads = await bus.syncRead([...wanted.keys()], STS3215.Present_Load);
843 let worst = 0;
844 for (const [id, t] of wanted) {
845 const p = positions.get(id), l = loads.get(id);
846 if (p === undefined || l === undefined) continue;
847 if (await observe(id, p, l, t)) continue; // jammed: parked, not waited for
848 worst = Math.max(worst, Math.abs(p - t));
849 }
850 if (worst <= 25) break;
851 await new Promise((r) => setTimeout(r, 50));
852 }
853 await bus.syncWrite(STS3215.Goal_Velocity, targets.map(([id]) => [id, 0] as const));
854 } finally {
855 easing = false;
856 }
857 last = new Map([...(last ?? []), ...targets]);
858 for (const [id, jam] of jams) last.set(id, jam.position);
859 return;
860 }
861 await bus.syncWrite(STS3215.Goal_Position, targets);
862 for (const [id, t] of targets) last!.set(id, t);
863 });
864 });
865 ws.on("close", () => {
866 clearTimeout(timer);
867 clearInterval(watch);
868 void run(async () => {
869 try {
870 if (armed) {
871 const positions = await bus.syncRead(ids, STS3215.Present_Position);
872 await bus.syncWrite(STS3215.Goal_Position, ids.filter((id) => positions.has(id)).map((id) => [id, positions.get(id)!] as const));
873 await bus.syncWrite(STS3215.Torque_Limit, ids.map((id) => [id, 1000] as const));
874 }
875 } finally {
876 blocked = null;
877 jams.clear();
878 streamStats = null;
879 metrics.event("stream", { seconds: Number(((Date.now() - startedAt) / 1000).toFixed(1)), ...stats });
880 release(); // whatever the bus did, the arm is free for the next driver
881 }
882 });
883 });
884 });
885
886 await new Promise<void>((resolve) => http.listen(options.port ?? 8101, options.host ?? "0.0.0.0", resolve));
887 const address = http.address();
888 const port = typeof address === "object" && address ? address.port : options.port ?? 8101;
889 return {
890 port,
891 close: () =>
892 new Promise<void>((resolve) => {
893 for (const client of wss.clients) client.terminate();
894 wss.close();
895 http.close(() => resolve());
896 http.closeAllConnections(); // the viewer keeps idle keep-alive connections open; close() alone would wait for them
897 }),
898 };
899 }
Software/so101/src/sts3215.test.ts added +64
@@ -0,0 +1,64 @@
1 import { describe, expect, test } from "bun:test";
2 import {
3 STS3215,
4 decodeAt,
5 decodeSignMagnitude,
6 decodeValue,
7 degreesToTicks,
8 encodeSignMagnitude,
9 encodeValue,
10 findRegister,
11 ticksToDegrees,
12 } from "./sts3215.ts";
13
14 describe("sign-magnitude", () => {
15 test("round-trips through LeRobot's encoding", () => {
16 expect(encodeSignMagnitude(-5, 15)).toBe(0x8005);
17 expect(encodeSignMagnitude(5, 15)).toBe(5);
18 expect(decodeSignMagnitude(0x8005, 15)).toBe(-5);
19 expect(decodeSignMagnitude(0x0405, 10)).toBe(-5);
20 expect(() => encodeSignMagnitude(-2048, 11)).toThrow(RangeError);
21 });
22 });
23
24 describe("encodeValue / decodeValue", () => {
25 test("two-byte values are little-endian", () => {
26 expect(encodeValue(STS3215.Goal_Position, 2048)).toEqual([0x00, 0x08]);
27 expect(decodeValue(STS3215.Present_Position, Uint8Array.from([0xff, 0x0f]))).toBe(4095);
28 });
29
30 test("single-byte values pass through", () => {
31 expect(encodeValue(STS3215.Torque_Enable, 1)).toEqual([1]);
32 expect(decodeValue(STS3215.Present_Temperature, Uint8Array.from([37]))).toBe(37);
33 });
34
35 test("signed registers apply their sign bit", () => {
36 expect(encodeValue(STS3215.Homing_Offset, -100)).toEqual([0x64, 0x08]);
37 expect(decodeValue(STS3215.Present_Load, Uint8Array.from([0x2c, 0x05]))).toBe(-300);
38 });
39
40 test("rejects values that do not fit", () => {
41 expect(() => encodeValue(STS3215.ID, 256)).toThrow(RangeError);
42 expect(() => encodeValue(STS3215.Goal_Position, 1.5)).toThrow(RangeError);
43 });
44 });
45
46 describe("decodeAt", () => {
47 test("pulls a register out of a block read", () => {
48 const block = Uint8Array.from([0x00, 0x08, 0x10, 0x00]);
49 expect(decodeAt(block, STS3215.Present_Position.address, STS3215.Present_Position)).toBe(2048);
50 expect(decodeAt(block, STS3215.Present_Position.address, STS3215.Present_Velocity)).toBe(16);
51 });
52 });
53
54 describe("conversions", () => {
55 test("4096 ticks per revolution", () => {
56 expect(ticksToDegrees(2048)).toBe(180);
57 expect(degreesToTicks(90)).toBe(1024);
58 });
59
60 test("findRegister is case-insensitive", () => {
61 expect(findRegister("present_position")?.[0]).toBe("Present_Position");
62 expect(findRegister("nope")).toBeUndefined();
63 });
64 });
Software/so101/src/sts3215.ts added +158
@@ -0,0 +1,158 @@
1 /**
2 * Feetech STS3215 control table, named exactly as LeRobot names it
3 * (lerobot/motors/feetech/tables.py) so `so101 read 1 Present_Position`
4 * reads the same way the LeRobot docs do. Addresses are bytes into the
5 * servo's memory map; multi-byte values are little-endian.
6 */
7
8 export type Area = "eeprom" | "sram" | "factory";
9
10 export interface Register {
11 readonly address: number;
12 readonly size: 1 | 2;
13 /** EEPROM registers persist across power cycles and need the Lock dance to write. */
14 readonly area: Area;
15 /** Bit index of the sign for sign-magnitude values (LeRobot's encoding table). */
16 readonly signBit?: number;
17 }
18
19 const reg = (address: number, size: 1 | 2, area: Area, signBit?: number): Register =>
20 signBit === undefined ? { address, size, area } : { address, size, area, signBit };
21
22 export const STS3215 = {
23 // EEPROM
24 Firmware_Major_Version: reg(0, 1, "eeprom"),
25 Firmware_Minor_Version: reg(1, 1, "eeprom"),
26 Model_Number: reg(3, 2, "eeprom"),
27 ID: reg(5, 1, "eeprom"),
28 Baud_Rate: reg(6, 1, "eeprom"),
29 Return_Delay_Time: reg(7, 1, "eeprom"),
30 Response_Status_Level: reg(8, 1, "eeprom"),
31 Min_Position_Limit: reg(9, 2, "eeprom"),
32 Max_Position_Limit: reg(11, 2, "eeprom"),
33 Max_Temperature_Limit: reg(13, 1, "eeprom"),
34 Max_Voltage_Limit: reg(14, 1, "eeprom"),
35 Min_Voltage_Limit: reg(15, 1, "eeprom"),
36 Max_Torque_Limit: reg(16, 2, "eeprom"),
37 Phase: reg(18, 1, "eeprom"),
38 Unloading_Condition: reg(19, 1, "eeprom"),
39 LED_Alarm_Condition: reg(20, 1, "eeprom"),
40 P_Coefficient: reg(21, 1, "eeprom"),
41 D_Coefficient: reg(22, 1, "eeprom"),
42 I_Coefficient: reg(23, 1, "eeprom"),
43 Minimum_Startup_Force: reg(24, 2, "eeprom"),
44 CW_Dead_Zone: reg(26, 1, "eeprom"),
45 CCW_Dead_Zone: reg(27, 1, "eeprom"),
46 Protection_Current: reg(28, 2, "eeprom"),
47 Angular_Resolution: reg(30, 1, "eeprom"),
48 Homing_Offset: reg(31, 2, "eeprom", 11),
49 Operating_Mode: reg(33, 1, "eeprom"),
50 Protective_Torque: reg(34, 1, "eeprom"),
51 Protection_Time: reg(35, 1, "eeprom"),
52 Overload_Torque: reg(36, 1, "eeprom"),
53 Velocity_closed_loop_P_proportional_coefficient: reg(37, 1, "eeprom"),
54 Over_Current_Protection_Time: reg(38, 1, "eeprom"),
55 Velocity_closed_loop_I_integral_coefficient: reg(39, 1, "eeprom"),
56 // SRAM
57 Torque_Enable: reg(40, 1, "sram"),
58 Acceleration: reg(41, 1, "sram"),
59 Goal_Position: reg(42, 2, "sram", 15),
60 Goal_Time: reg(44, 2, "sram"),
61 Goal_Velocity: reg(46, 2, "sram", 15),
62 Torque_Limit: reg(48, 2, "sram"),
63 Lock: reg(55, 1, "sram"),
64 Present_Position: reg(56, 2, "sram", 15),
65 Present_Velocity: reg(58, 2, "sram", 15),
66 Present_Load: reg(60, 2, "sram", 10),
67 Present_Voltage: reg(62, 1, "sram"),
68 Present_Temperature: reg(63, 1, "sram"),
69 Status: reg(65, 1, "sram"),
70 Moving: reg(66, 1, "sram"),
71 Present_Current: reg(69, 2, "sram"),
72 Goal_Position_2: reg(71, 2, "sram"),
73 // Factory
74 Moving_Velocity: reg(80, 1, "factory"),
75 Moving_Velocity_Threshold: reg(80, 1, "factory"),
76 DTs: reg(81, 1, "factory"),
77 Velocity_Unit_factor: reg(82, 1, "factory"),
78 Hts: reg(83, 1, "factory"),
79 Maximum_Velocity_Limit: reg(84, 1, "factory"),
80 Maximum_Acceleration: reg(85, 1, "factory"),
81 Acceleration_Multiplier: reg(86, 1, "factory"),
82 } as const satisfies Record<string, Register>;
83
84 export type RegisterName = keyof typeof STS3215;
85
86 export const MODEL_NUMBER = 777;
87 export const RESOLUTION = 4096;
88
89 /** Baud_Rate register value → bits per second. */
90 export const BAUD_RATES: Readonly<Record<number, number>> = {
91 0: 1_000_000,
92 1: 500_000,
93 2: 250_000,
94 3: 128_000,
95 4: 115_200,
96 5: 57_600,
97 6: 38_400,
98 7: 19_200,
99 };
100
101 export const OperatingMode = { Position: 0, Velocity: 1, Pwm: 2, Step: 3 } as const;
102
103 /** SO-101 joint names by servo id, as LeRobot assigns them. */
104 export const JOINTS: Readonly<Record<number, string>> = {
105 1: "shoulder_pan",
106 2: "shoulder_lift",
107 3: "elbow_flex",
108 4: "wrist_flex",
109 5: "wrist_roll",
110 6: "gripper",
111 };
112
113 export function findRegister(name: string): [RegisterName, Register] | undefined {
114 const wanted = name.toLowerCase();
115 for (const key of Object.keys(STS3215) as RegisterName[]) {
116 if (key.toLowerCase() === wanted) return [key, STS3215[key]];
117 }
118 return undefined;
119 }
120
121 export const ticksToDegrees = (ticks: number): number => (ticks * 360) / RESOLUTION;
122 export const degreesToTicks = (degrees: number): number => Math.round((degrees * RESOLUTION) / 360);
123
124 export function encodeSignMagnitude(value: number, signBit: number): number {
125 const magnitude = Math.abs(value);
126 const max = (1 << signBit) - 1;
127 if (magnitude > max) throw new RangeError(`|${value}| exceeds ${max} for a sign bit at ${signBit}`);
128 return (value < 0 ? 1 << signBit : 0) | magnitude;
129 }
130
131 export function decodeSignMagnitude(encoded: number, signBit: number): number {
132 const magnitude = encoded & ((1 << signBit) - 1);
133 return (encoded >> signBit) & 1 ? -magnitude : magnitude;
134 }
135
136 /** Register value → the bytes that go on the wire. */
137 export function encodeValue(register: Register, value: number): number[] {
138 if (!Number.isInteger(value)) throw new RangeError(`register values are integers, got ${value}`);
139 const raw = register.signBit === undefined ? value : encodeSignMagnitude(value, register.signBit);
140 const max = register.size === 1 ? 0xff : 0xffff;
141 if (raw < 0 || raw > max) throw new RangeError(`${value} does not fit in ${register.size} byte(s)`);
142 return register.size === 1 ? [raw] : [raw & 0xff, raw >> 8];
143 }
144
145 /** Bytes off the wire → register value. */
146 export function decodeValue(register: Register, bytes: Uint8Array): number {
147 if (bytes.length !== register.size) {
148 throw new RangeError(`expected ${register.size} byte(s), got ${bytes.length}`);
149 }
150 const raw = register.size === 1 ? bytes[0]! : bytes[0]! | (bytes[1]! << 8);
151 return register.signBit === undefined ? raw : decodeSignMagnitude(raw, register.signBit);
152 }
153
154 /** Decode a register out of a block read that started at `blockAddress`. */
155 export function decodeAt(block: Uint8Array, blockAddress: number, register: Register): number {
156 const offset = register.address - blockAddress;
157 return decodeValue(register, block.subarray(offset, offset + register.size));
158 }
Software/so101/src/trajectory.test.ts added +31
@@ -0,0 +1,31 @@
1 import { describe, expect, test } from "bun:test";
2 import { clampFrame, durationSeconds, parseTrajectory, serializeTrajectory } from "./trajectory.ts";
3
4 describe("trajectory", () => {
5 test("round-trips through JSON", () => {
6 const original = { hz: 30, ids: [1, 2], frames: [[100, 200], [110, 210]], recorded: "2026-10-12T00:00:00Z" };
7 expect(parseTrajectory(serializeTrajectory(original))).toEqual(original);
8 expect(durationSeconds(original)).toBeCloseTo(2 / 30);
9 });
10
11 test("rejects malformed files with a reason", () => {
12 expect(() => parseTrajectory("nope")).toThrow("not valid JSON");
13 expect(() => parseTrajectory('{"hz":0,"ids":[1],"frames":[[1]]}')).toThrow("hz");
14 expect(() => parseTrajectory('{"hz":30,"ids":[],"frames":[[1]]}')).toThrow("ids");
15 expect(() => parseTrajectory('{"hz":30,"ids":[1],"frames":[]}')).toThrow("frames");
16 expect(() => parseTrajectory('{"hz":30,"ids":[1,2],"frames":[[1]]}')).toThrow("frame 0");
17 expect(() => parseTrajectory('{"hz":30,"ids":[1],"frames":[[1.5]]}')).toThrow("frame 0");
18 });
19
20 test("clampFrame keeps positions inside each servo's limits", () => {
21 expect(clampFrame([50, 4000, 2048], [[100, 3000], [100, 3000], [0, 4095]])).toEqual([100, 3000, 2048]);
22 });
23 });
24
25 describe("trajectory bounds", () => {
26 test("rejects an infinite or absurd rate and duplicate ids", () => {
27 expect(() => parseTrajectory('{"hz":1e999,"ids":[1],"frames":[[1]]}')).toThrow("hz");
28 expect(() => parseTrajectory('{"hz":500,"ids":[1],"frames":[[1]]}')).toThrow("hz");
29 expect(() => parseTrajectory('{"hz":30,"ids":[1,1],"frames":[[1,2]]}')).toThrow("distinct");
30 });
31 });
Software/so101/src/trajectory.ts added +64
@@ -0,0 +1,64 @@
1 /**
2 * A recorded motion: joint positions sampled at a fixed rate. Plain JSON on
3 * disk so it can be inspected, trimmed, or generated by hand.
4 */
5 export interface Trajectory {
6 readonly hz: number;
7 readonly ids: readonly number[];
8 /** One entry per sample, each holding one position per id, in `ids` order. */
9 readonly frames: readonly (readonly number[])[];
10 readonly recorded?: string;
11 }
12
13 export function serializeTrajectory(trajectory: Trajectory): string {
14 return JSON.stringify(trajectory, null, 1);
15 }
16
17 export function parseTrajectory(text: string): Trajectory {
18 let data: unknown;
19 try {
20 data = JSON.parse(text);
21 } catch {
22 throw new Error("not valid JSON");
23 }
24 if (typeof data !== "object" || data === null) throw new Error("expected an object");
25 const { hz, ids, frames, recorded } = data as Record<string, unknown>;
26 if (typeof hz !== "number" || !(hz >= 1 && hz <= 100)) throw new Error("hz must be between 1 and 100");
27 if (
28 !Array.isArray(ids) ||
29 ids.length === 0 ||
30 !ids.every((id) => Number.isInteger(id) && id >= 0 && id <= 253) ||
31 new Set(ids).size !== ids.length
32 ) {
33 throw new Error("ids must be a non-empty list of distinct servo ids");
34 }
35 if (!Array.isArray(frames) || frames.length === 0) throw new Error("frames must be a non-empty list");
36 for (const [index, frame] of frames.entries()) {
37 if (!Array.isArray(frame) || frame.length !== ids.length || !frame.every((p) => Number.isInteger(p))) {
38 throw new Error(`frame ${index} must hold ${ids.length} integer positions`);
39 }
40 }
41 return { hz, ids, frames, ...(typeof recorded === "string" ? { recorded } : {}) };
42 }
43
44 /** Keep every position inside the servo's own limits, so a replay can never push a joint past a hard stop. */
45 export function clampFrame(frame: readonly number[], limits: readonly (readonly [number, number])[]): number[] {
46 return frame.map((position, i) => {
47 const [min, max] = limits[i]!;
48 return Math.min(max, Math.max(min, position));
49 });
50 }
51
52 export function durationSeconds(trajectory: Trajectory): number {
53 return trajectory.frames.length / trajectory.hz;
54 }
55
56 /** The largest change of any joint between consecutive frames, in ticks. */
57 export function largestStep(trajectory: Trajectory): number {
58 let largest = 0;
59 for (let i = 1; i < trajectory.frames.length; i++) {
60 const prev = trajectory.frames[i - 1]!, next = trajectory.frames[i]!;
61 for (let j = 0; j < prev.length; j++) largest = Math.max(largest, Math.abs(next[j]! - prev[j]!));
62 }
63 return largest;
64 }
Software/so101/src/viewer.html added +755
@@ -0,0 +1,755 @@
1 <!doctype html>
2 <html lang="en">
3 <head>
4 <meta charset="utf-8">
5 <meta name="viewport" content="width=device-width, initial-scale=1, viewport-fit=cover">
6 <meta name="mobile-web-app-capable" content="yes">
7 <meta name="apple-mobile-web-app-capable" content="yes">
8 <meta name="apple-mobile-web-app-status-bar-style" content="black-translucent">
9 <title>SO-101</title>
10 <script type="importmap">
11 { "imports": {
12 "three": "/vendor/three/build/three.module.js",
13 "three/examples/jsm/": "/vendor/three/examples/jsm/",
14 "urdf-loader/manipulator": "/vendor/urdf-loader/src/urdf-manipulator-element.js"
15 } }
16 </script>
17 <style>
18 :root { color-scheme: dark; --bg: #0b0d10; --fg: #e8eaed; --dim: #9aa4ae; --ok: #3fb950; --warn: #d29922; --bad: #f85149; --line: #30363d; --btn: #21262d; --blue: #388bfd; }
19 * { box-sizing: border-box; }
20 html, body { margin: 0; height: 100%; background: var(--bg); color: var(--fg); font: 14px/1.4 ui-sans-serif, system-ui, -apple-system, sans-serif; -webkit-tap-highlight-color: transparent; overflow: hidden; }
21 #app { position: relative; width: 100%; height: 100%; display: flex; flex-direction: column; background: #000; }
22 #stage { flex: 1; min-height: 0; display: flex; gap: 2px; }
23 .pane { flex: 1; min-width: 0; position: relative; background: #000; display: none; }
24 .pane.on { display: block; }
25 .pane img { width: 100%; height: 100%; object-fit: contain; display: block; }
26 .pane .tag { position: absolute; left: 8px; top: 8px; font-size: 11px; color: var(--dim); background: rgba(0,0,0,.5); padding: 2px 6px; border-radius: 4px; }
27 urdf-viewer { display: block; width: 100%; height: 100%; }
28 @media (max-aspect-ratio: 1/1) { #stage { flex-direction: column; } }
29 #top { position: absolute; top: 8px; right: 10px; z-index: 5; display: flex; gap: 8px; align-items: center; font-size: 13px; background: rgba(0,0,0,.65); padding: 5px 6px 5px 10px; border-radius: 8px; flex-wrap: wrap; justify-content: flex-end; }
30 #top .seg button { padding: 5px 9px; min-height: 30px; } #top .seg button.on { background: var(--blue); border-color: var(--blue); }
31 .dot { width: 8px; height: 8px; border-radius: 50%; background: var(--bad); display: inline-block; } .dot.on { background: var(--ok); }
32 #banner { position: absolute; top: 54px; left: 50%; transform: translateX(-50%); z-index: 5; max-width: min(640px, 90vw); padding: 8px 14px; border-radius: 8px; font-size: 13px; background: rgba(0,0,0,.75); border: 1px solid var(--line); display: none; }
33 #banner.bad { display: block; border-color: var(--bad); color: #ffb3ad; } #banner.warn { display: block; border-color: var(--warn); color: #f2cc60; }
34 #hud { padding: 8px 12px calc(8px + env(safe-area-inset-bottom)); background: #0d1117; border-top: 1px solid var(--line); display: flex; flex-direction: column; gap: 6px; max-height: 46%; overflow: auto; }
35 #hud[hidden] { display: none; }
36 .row { display: flex; gap: 8px; flex-wrap: wrap; align-items: center; }
37 #joints { display: grid; grid-template-columns: repeat(auto-fit, minmax(170px, 1fr)); gap: 4px 12px; }
38 .j { display: grid; grid-template-columns: 1fr auto; grid-template-areas: "name val" "slide slide"; align-items: center; column-gap: 6px; padding: 2px 0; border-radius: 6px; }
39 .j.jam { background: rgba(248,81,73,.18); } .j.drag { background: rgba(56,139,253,.15); }
40 .j b { grid-area: name; font-size: 11px; color: #c9d1d9; letter-spacing: .03em; text-transform: uppercase; font-weight: 600; }
41 .j .v { grid-area: val; display: flex; align-items: center; gap: 6px; font-variant-numeric: tabular-nums; font-size: 15px; }
42 .j small { color: var(--dim); font-size: 11px; }
43 .j .slide { grid-area: slide; display: flex; align-items: center; gap: 4px; }
44 .j input[type=range] { flex: 1; min-width: 0; accent-color: var(--blue); height: 22px; }
45 .j .jog button { min-width: 30px; min-height: 28px; padding: 0 6px; font-size: 15px; line-height: 1; touch-action: none; user-select: none; }
46 button { background: var(--btn); color: var(--fg); border: 1px solid var(--line); border-radius: 6px; padding: 7px 12px; min-height: 34px; font: inherit; cursor: pointer; }
47 button:hover { background: #30363d; } button:disabled { opacity: .4; cursor: default; }
48 button.danger { border-color: var(--bad); color: var(--bad); } button.go { border-color: var(--blue); } button.rec { border-color: var(--bad); }
49 button.x { padding: 0 8px; min-height: 26px; color: var(--bad); font-size: 13px; }
50 .pose { display: inline-flex; gap: 4px; align-items: center; }
51 select, input[type=number] { background: var(--btn); color: var(--fg); border: 1px solid var(--line); border-radius: 6px; padding: 6px 8px; min-height: 34px; font: inherit; }
52 input[type=number] { width: 80px; }
53 label.inline { display: inline-flex; gap: 6px; align-items: center; color: var(--dim); font-size: 12px; }
54 #msg { color: var(--dim); font-size: 12px; min-height: 1.2em; } #msg.bad { color: var(--bad); }
55 #strip { color: var(--dim); font-size: 12px; display: flex; gap: 14px; flex-wrap: wrap; }
56 #strip b { color: var(--fg); font-weight: 500; }
57 dialog { background: #161b22; color: var(--fg); border: 1px solid var(--line); border-radius: 10px; padding: 16px 18px; min-width: min(360px, 92vw); max-width: 92vw; }
58 dialog::backdrop { background: rgba(0,0,0,.6); }
59 dialog h3 { margin: 0 0 10px; font-size: 15px; } dialog p { margin: 6px 0; color: var(--dim); font-size: 13px; }
60 dialog input[type=text] { width: 100%; font: inherit; padding: 8px 10px; border-radius: 6px; border: 1px solid var(--line); background: #0d1117; color: var(--fg); }
61 dialog .actions { display: flex; gap: 8px; justify-content: flex-end; margin-top: 12px; }
62 .list { display: flex; flex-direction: column; gap: 6px; max-height: 40vh; overflow: auto; margin: 8px 0; }
63 .list .item { display: flex; gap: 8px; align-items: center; } .list .item span { flex: 1; }
64 .panel { position: absolute; inset: 0; z-index: 10; overflow: auto; background: rgba(11,13,16,.97); padding: 24px max(18px, env(safe-area-inset-right)) 40px max(18px, env(safe-area-inset-left)); }
65 .panel[hidden] { display: none; }
66 .panel article { max-width: 760px; margin: 0 auto; }
67 .panel h2 { margin: 0 0 12px; font-size: 20px; } .panel h3 { margin: 18px 0 6px; font-size: 15px; }
68 .panel p, .panel li { color: #c9d1d9; font-size: 14px; line-height: 1.5; } .panel ol, .panel ul { padding-left: 20px; }
69 .panel a { color: #58a6ff; } .refs li { margin-bottom: 6px; font-size: 13px; }
70 .panel pre { background: #0d1117; border: 1px solid var(--line); border-radius: 8px; padding: 10px 12px; font-size: 12.5px; overflow: auto; }
71 .panel .close { position: absolute; top: 12px; right: 14px; }
72 .status { display: grid; grid-template-columns: max-content 1fr; gap: 4px 14px; font-size: 14px; margin: 8px 0; } .status b { color: var(--dim); font-weight: 500; }
73 .ok { color: var(--ok); } .no { color: var(--warn); }
74 table { border-collapse: collapse; font-size: 13px; } td, th { padding: 4px 10px; border-bottom: 1px solid var(--line); text-align: left; } th { color: var(--dim); font-weight: 500; }
75 .events { font-size: 12.5px; font-family: ui-monospace, SFMono-Regular, Menlo, monospace; } .events div { padding: 2px 0; border-bottom: 1px solid #1b2129; }
76 .trim { display: grid; grid-template-columns: max-content auto; gap: 6px 12px; align-items: center; max-width: 360px; }
77 :fullscreen #hud { opacity: .35; transition: opacity .3s; } :fullscreen #hud:hover { opacity: 1; }
78 </style>
79 </head>
80 <body>
81 <div id="app">
82 <div id="stage">
83 <div class="pane on" id="pane1"><img id="cam1" alt="camera 1"><span class="tag" id="tag1">camera 1</span></div>
84 <div class="pane" id="pane2"><img id="cam2" alt="camera 2"><span class="tag" id="tag2">camera 2</span></div>
85 <div class="pane on" id="pane3"><urdf-viewer id="model" up="Z" ignore-limits display-shadow ambient-color="#b9c0c8"></urdf-viewer><span class="tag">3D · drag a joint to move the arm · drag empty space to orbit</span></div>
86 </div>
87 <div id="top">
88 <span class="seg"><button id="v1" class="on">Cam 1</button><button id="v2" hidden>Cam 2</button><button id="v3" class="on">3D</button></span>
89 <span id="live"></span><span class="dot" id="dot"></span><span id="state">…</span>
90 <button id="zoneBadge" title="safe zone settings">zone</button>
91 <button id="stop" class="danger" hidden title="space">Stop</button><button id="fs" title="F">Fullscreen</button>
92 </div>
93 <div id="banner"></div>
94 <div id="hud">
95 <div id="joints"></div>
96 <div class="row" id="poses"></div>
97 <div class="row">
98 <label class="inline">jog <select id="rate"><option value="100">slow</option><option value="300" selected>medium</option><option value="800">fast</option></select></label>
99 <button id="hold" title="torque on, stay where it is">Hold</button>
100 <button id="releaseBtn" class="danger" title="torque off">Release</button>
101 <button id="record" class="rec">● Record</button>
102 <button id="play">▶ Play…</button>
103 <button id="liveBtn" class="go" hidden title="read the leader arm through this computer's USB port and stream it (Chrome)">Live from leader</button>
104 <button id="metricsBtn">Metrics</button>
105 <button id="settingsBtn">Settings</button>
106 <button id="calib">3D calibration</button>
107 </div>
108 <div id="strip"></div>
109 <div id="msg"></div>
110 </div>
111
112 <div id="metricsPanel" class="panel" hidden>
113 <button class="close" data-close="metricsPanel">Close</button>
114 <article>
115 <h2>Metrics</h2>
116 <div class="status" id="metricsSummary"></div>
117 <h3>Per joint</h3>
118 <table id="peaks"></table>
119 <h3>Recent events</h3>
120 <div class="events" id="events"></div>
121 </article>
122 </div>
123
124 <div id="settingsPanel" class="panel" hidden>
125 <button class="close" data-close="settingsPanel">Close</button>
126 <article>
127 <h2>Safe zone</h2>
128 <p>The server runs every target through the arm's own geometry: the URDF and its meshes. A pose that puts any part below the table, or into the base, is a violation. <b>enforce</b> refuses such moves and holds a stream at the boundary until the leader backs off. <b>warn</b> only reports them, in the banner and the metrics. <b>off</b> skips the check.</p>
129 <div class="row">
130 <label class="inline">mode <select id="zone"><option value="enforce">enforce</option><option value="warn">warn</option><option value="off">off</option></select></label>
131 <label class="inline">table margin <input type="number" id="floorMargin" min="0" max="100" step="1"> mm</label>
132 <label class="inline">base margin <input type="number" id="baseMargin" min="0" max="100" step="1"> mm</label>
133 </div>
134 <h3>Trim the model onto the arm</h3>
135 <p>The model takes the middle of each calibrated range as zero, the LeRobot convention. The real zero can be a few degrees off, which puts the model a centimetre or two away from the arm at the gripper. Compare the 3D view with the camera and adjust a trim until they match. The zone uses the trimmed model.</p>
136 <div class="trim" id="trims"></div>
137 </article>
138 </div>
139
140 <div id="panel" class="panel" hidden>
141 <button class="close" data-close="panel">Close</button>
142 <article>
143 <h2>3D calibration: teach the arm where the camera is</h2>
144 <p>The camera sees a marker. The arm needs that as a position in its own space. Two calibrations give it that.</p>
145 <ol>
146 <li><b>Camera.</b> The lens geometry: focal length and distortion. Found from 15 views of the printed <em>ChArUco board</em>: a checkerboard with a small marker in each white square. Download it below and print it at 100 %. Output: <code>camera.json</code>.</li>
147 <li><b>Hand-eye.</b> The arm holds a marker in 6 to 8 poses. The camera says where the marker is. The arm's model says where the gripper is. The solver finds the camera's position relative to the arm's base. Output: <code>handeye.json</code>.</li>
148 </ol>
149 <p>After both, a cube the camera sees becomes a target the arm can reach. With two cameras, each one gets its own pair of calibrations (<code>camera2</code>, <code>handeye2</code>); two views then give a full orientation.</p>
150 <h3>Print these</h3>
151 <p>Three sheets, at 100 % scale on matte paper. Check with a ruler: a cube marker's black square is 40 mm, a board square 30 mm. Tape the six cube markers on a 40 to 50 mm block, and the gripper marker flat on the outside of the fixed jaw.</p>
152 <div class="row" id="printables">loading…</div>
153 <h3>Status</h3>
154 <div class="status" id="calStatus"></div>
155 <h3>Steps, on the Mac</h3>
156 <pre id="calSteps"></pre>
157 <p>For the poses: press <b>Release</b>, move the arm by hand, press <b>Save pose</b>, and uncheck <em>with gripper</em>. The hand-eye poses need different tilts and rolls of the gripper, with the marker facing the camera.</p>
158 <h3>References</h3>
159 <ul class="refs">
160 <li>Camera calibration: Z. Zhang, <em>A Flexible New Technique for Camera Calibration</em>, IEEE TPAMI 2000, <a href="https://doi.org/10.1109/34.888718" target="_blank" rel="noopener">doi:10.1109/34.888718</a>. OpenCV tutorial: <a href="https://docs.opencv.org/4.x/da/d13/tutorial_aruco_calibration.html" target="_blank" rel="noopener">Calibration with ArUco and ChArUco</a>.</li>
161 <li>ArUco markers and ChArUco boards: S. Garrido-Jurado et al., <em>Automatic generation and detection of highly reliable fiducial markers under occlusion</em>, Pattern Recognition 2014, <a href="https://doi.org/10.1016/j.patcog.2014.01.005" target="_blank" rel="noopener">doi:10.1016/j.patcog.2014.01.005</a>.</li>
162 <li>Pose of one square marker: T. Collins, A. Bartoli, <em>Infinitesimal Plane-Based Pose Estimation</em>, IJCV 2014, <a href="https://doi.org/10.1007/s11263-014-0725-5" target="_blank" rel="noopener">doi:10.1007/s11263-014-0725-5</a> (OpenCV's <code>SOLVEPNP_IPPE_SQUARE</code>).</li>
163 <li>Hand-eye calibration: Tsai &amp; Lenz 1989 <a href="https://doi.org/10.1109/70.34770" target="_blank" rel="noopener">doi:10.1109/70.34770</a>; Park &amp; Martin 1994 <a href="https://doi.org/10.1109/70.326576" target="_blank" rel="noopener">doi:10.1109/70.326576</a>; Daniilidis 1999 <a href="https://doi.org/10.1177/02783649922066213" target="_blank" rel="noopener">doi:10.1177/02783649922066213</a>.</li>
164 <li>Robot-world and hand-eye in one solve, what this kit uses: M. Shah 2013, <a href="https://doi.org/10.1115/1.4024473" target="_blank" rel="noopener">doi:10.1115/1.4024473</a>; A. Li, L. Wang, D. Wu 2010. OpenCV: <a href="https://docs.opencv.org/4.x/d9/d0c/group__calib3d.html" target="_blank" rel="noopener"><code>calibrateRobotWorldHandEye</code></a>.</li>
165 <li>Two cameras, a full orientation: Hartley &amp; Zisserman, <em>Multiple View Geometry in Computer Vision</em>, 2nd ed. 2004 (stereo calibration, triangulation).</li>
166 </ul>
167 <h3>About</h3>
168 <p>This page drives an <a href="https://github.com/TheRobotStudio/SO-ARM100" target="_blank" rel="noopener">SO-101</a>, the open robot arm by The Robot Studio, with the calibration conventions of <a href="https://huggingface.co/docs/lerobot/so101" target="_blank" rel="noopener">Hugging Face LeRobot</a>. The 3D view uses the arm's own URDF through <a href="https://github.com/gkjohnson/urdf-loaders" target="_blank" rel="noopener">urdf-loader</a> and three.js. The code, the build notes and the quest are in the repository's <code>Software/</code> folder.</p>
169 </article>
170 </div>
171
172 <dialog id="saveDlg">
173 <form method="dialog">
174 <h3>Save the current pose</h3>
175 <input type="text" id="saveName" placeholder="name: home, drop, he1 …" pattern="[A-Za-z0-9][A-Za-z0-9_.\-]{0,63}" required autocomplete="off">
176 <p><label class="inline"><input type="checkbox" id="saveGripper" checked> with gripper (servo 6)</label></p>
177 <p>Poses for the 3D calibration and for pick-and-place are saved without the gripper.</p>
178 <div class="actions"><button value="cancel">Cancel</button><button value="save" class="go">Save</button></div>
179 </form>
180 </dialog>
181 <dialog id="recDlg">
182 <form method="dialog">
183 <h3>Record a motion</h3>
184 <p>CAUTION: torque goes off. Hold the arm before you press Start, then move it by hand. Press Stop recording when done.</p>
185 <input type="text" id="recName" placeholder="name" pattern="[A-Za-z0-9][A-Za-z0-9_.\-]{0,63}" required autocomplete="off">
186 <div class="actions"><button value="cancel">Cancel</button><button value="start" class="rec">Start</button></div>
187 </form>
188 </dialog>
189 <dialog id="playDlg">
190 <h3>Play a recording</h3>
191 <p>The arm first moves slowly to the recording's first pose. Keep the workspace clear.</p>
192 <div class="list" id="recList"></div>
193 <div class="actions"><label class="inline">rate <select id="playRate"><option value="0.5">0.5×</option><option value="1" selected>1×</option><option value="1.5">1.5×</option></select></label><button id="playClose">Close</button></div>
194 </dialog>
195 </div>
196 <script type="module">
197 import * as THREE from "three";
198 import { STLLoader } from "three/examples/jsm/loaders/STLLoader.js";
199 import URDFManipulator from "urdf-loader/manipulator";
200 customElements.define("urdf-viewer", URDFManipulator);
201
202 const $ = (id) => document.getElementById(id);
203 const q = new URLSearchParams(location.search);
204 const msg = (text, bad = false) => { $("msg").textContent = text; $("msg").className = bad ? "bad" : ""; };
205 const api = async (path, body, method = body === undefined ? "GET" : "POST") => {
206 const r = await fetch(path, { method, body: body === undefined ? undefined : JSON.stringify(body), cache: "no-store" });
207 const data = await r.json().catch(() => ({}));
208 return Object.defineProperties(data, { ok: { value: r.ok }, status: { value: r.status } });
209 };
210 const NAMES = { 1: "shoulder_pan", 2: "shoulder_lift", 3: "elbow_flex", 4: "wrist_flex", 5: "wrist_roll", 6: "gripper" };
211 const IDS = Object.fromEntries(Object.entries(NAMES).map(([id, n]) => [n, Number(id)]));
212
213 // ---------------------------------------------------------------- cameras
214 // Plain <img> streams work in every browser; a canvas comparison once a second notices a frozen picture and reconnects.
215 const camUrl = (n) => q.get(`stream${n}`) || q.get(n === 1 ? "stream" : "") || `${location.protocol}//${location.hostname}:${8079 + n}/stream`;
216 const cams = [1, 2].map((n) => ({ n, img: $(`cam${n}`), url: camUrl(n), sig: "", unchanged: 0, live: false }));
217 const canvas = document.createElement("canvas"); canvas.width = 32; canvas.height = 18;
218 const ctx = canvas.getContext("2d", { willReadFrequently: true });
219 const fresh = (url) => url + (url.includes("?") ? "&" : "?") + Date.now();
220 for (const c of cams) {
221 c.img.crossOrigin = "anonymous";
222 c.img.onerror = () => { if ($(`pane${c.n}`).classList.contains("on")) setTimeout(() => (c.img.src = fresh(c.url)), 3000); };
223 }
224 const showCam = (c, on) => { $(`pane${c.n}`).classList.toggle("on", on); c.img.src = on ? fresh(c.url) : ""; };
225 setInterval(() => {
226 for (const c of cams) {
227 if (!$(`pane${c.n}`).classList.contains("on")) continue;
228 let changed = false;
229 try {
230 ctx.drawImage(c.img, 0, 0, 32, 18);
231 const sig = Array.from(ctx.getImageData(0, 0, 32, 18).data.filter((_, i) => i % 16 === 0)).join(",");
232 changed = sig !== c.sig; c.sig = sig;
233 } catch { /* nothing decodable yet */ }
234 c.unchanged = changed ? 0 : c.unchanged + 1;
235 c.live = changed;
236 if (c.unchanged >= 15) { c.img.src = fresh(c.url); c.unchanged = 0; }
237 }
238 $("live").textContent = cams.filter((c) => $(`pane${c.n}`).classList.contains("on")).map((c) => (c.live ? "live" : "waiting")).join(" · ");
239 }, 1000);
240 showCam(cams[0], true);
241 // Camera 2 appears when its stream answers.
242 fetch(cams[1].url.replace(/\/stream.*$/, "/state"), { cache: "no-store" }).then((r) => { if (r.ok) $("v2").hidden = false; }).catch(() => {});
243 const panes = { v1: "pane1", v2: "pane2", v3: "pane3" };
244 for (const [btn, pane] of Object.entries(panes)) {
245 $(btn).onclick = () => {
246 const on = !$(pane).classList.contains("on");
247 if (!on && document.querySelectorAll(".pane.on").length === 1) return; // keep one pane
248 $(btn).classList.toggle("on", on);
249 if (pane === "pane3") $(pane).classList.toggle("on", on); else showCam(cams[pane === "pane1" ? 0 : 1], on);
250 try { localStorage.setItem("so101.panes", JSON.stringify([...document.querySelectorAll(".pane.on")].map((p) => p.id))); } catch {}
251 viewer.recenter?.();
252 };
253 }
254
255 // ---------------------------------------------------------------- the stream: the page as a leader
256 // Opened while a slider, a jog button or a model joint is held; frames go at most every 33 ms; closed shortly after letting go.
257 const stream = {
258 ws: null, pending: null, timer: null, closeTimer: null, halted: false,
259 open() {
260 clearTimeout(this.closeTimer);
261 if (this.ws && this.ws.readyState <= 1) return;
262 if (this.halted) return; // the server ended this hold (Stop, or the arm is busy): nothing more until the next press
263 const ws = new WebSocket(`${location.protocol === "https:" ? "wss" : "ws"}://${location.host}/follow`);
264 ws.onclose = (e) => {
265 if (this.ws !== ws) return;
266 this.ws = null;
267 if (e.code === 1013 || e.code === 1000) { this.halted = true; msg(e.reason || "the arm is busy", true); }
268 };
269 this.ws = ws;
270 },
271 send(frame) {
272 if (this.halted) return;
273 this.open();
274 if (!this.ws) return;
275 this.pending = { ...(this.pending ?? {}), ...frame };
276 if (this.timer) return;
277 this.timer = setTimeout(() => {
278 this.timer = null;
279 const data = JSON.stringify(this.pending); this.pending = null;
280 if (this.ws.readyState === 1) this.ws.send(data);
281 else if (this.ws.readyState === 0) this.ws.addEventListener("open", () => this.ws.send(data), { once: true });
282 }, 33);
283 },
284 end() { clearTimeout(this.closeTimer); this.closeTimer = setTimeout(() => { const ws = this.ws; this.ws = null; this.halted = false; ws?.close(); }, 250); },
285 };
286
287 // ---------------------------------------------------------------- the arm
288 let limits = {}, status = null, poses = {}, editing = false, settings = null, modelInfo = null;
289 const driving = () => status?.driving ?? null;
290 const mid = (id) => (limits[id].min + limits[id].max) / 2;
291 // Arm joints: zero at the middle of the calibrated range (LeRobot's convention). The gripper: closed at the range's
292 // minimum is the jaw's zero, open at the maximum is the URDF's upper limit. Trims are added in degrees.
293 const trim = (id) => (((settings?.jointOffsetsDeg ?? {})[NAMES[id]] ?? 0) * Math.PI) / 180;
294 const gripperOpen = () => modelInfo?.joints?.find((j) => j.name === "gripper")?.upper ?? 1.745;
295 const toRad = (id, ticks) => (id === 6 ? ((ticks - limits[id].min) / (limits[id].max - limits[id].min)) * gripperOpen() : ((ticks - mid(id)) * 2 * Math.PI) / 4095) + trim(id);
296 const toTicks = (id, rad) => Math.round(id === 6 ? limits[id].min + ((rad - trim(id)) / gripperOpen()) * (limits[id].max - limits[id].min) : ((rad - trim(id)) * 4095) / (2 * Math.PI) + mid(id));
297 const clampTicks = (id, t) => Math.max(limits[id].min + 30, Math.min(limits[id].max - 30, Math.round(t)));
298
299 async function loadPoses() {
300 poses = await api("/poses");
301 const row = $("poses"); row.innerHTML = "";
302 for (const name of Object.keys(poses)) {
303 const wrap = document.createElement("span"); wrap.className = "pose";
304 const b = document.createElement("button"); b.textContent = name; b.className = "go"; b.title = `${poses[name].ids.length} joints`;
305 b.onclick = () => goPose(name);
306 wrap.append(b);
307 if (editing) {
308 const x = document.createElement("button"); x.className = "x"; x.textContent = "×"; x.title = "delete";
309 x.onclick = async () => { if (!confirm(`Delete pose "${name}"?`)) return; const r = await api("/pose/delete", { name }); msg(r.ok ? `deleted ${name}` : r.error, !r.ok); loadPoses(); };
310 wrap.append(x);
311 }
312 row.append(wrap);
313 }
314 const save = document.createElement("button"); save.textContent = "+ Save pose"; save.onclick = () => $("saveDlg").showModal();
315 const edit = document.createElement("button"); edit.textContent = editing ? "done" : "✎"; edit.title = "delete poses";
316 edit.onclick = () => { editing = !editing; loadPoses(); };
317 row.append(save, edit);
318 }
319 async function goPose(name) {
320 msg(`going to ${name}…`);
321 const r = await api("/pose/go", { name });
322 msg(r.reached ? `reached ${name}${r.warning ? ` — ${r.warning}` : ""}` : `${r.error}`, !r.reached);
323 }
324 async function stop() {
325 const r = await api("/stop", {});
326 msg(r.stopped ? `stopped ${r.stopped}${r.saved ? `; saved recording ${r.saved}` : ""}; the arm holds` : "nothing was driving");
327 if (r.saved) loadRecordings();
328 }
329 $("stop").onclick = stop;
330 $("hold").onclick = async () => { const r = await api("/torque", { on: true }); msg(r.ok ? "holding: torque on" : r.error, !r.ok); };
331 $("releaseBtn").onclick = async () => {
332 if (!confirm("Release torque? Hold the arm so it does not fall.")) return;
333 const r = await api("/torque", { on: false }); msg(r.ok ? "torque released: the arm is free" : r.error, !r.ok);
334 };
335 $("saveDlg").addEventListener("close", async () => {
336 if ($("saveDlg").returnValue !== "save") return;
337 const name = $("saveName").value.trim();
338 const ids = $("saveGripper").checked ? [1, 2, 3, 4, 5, 6] : [1, 2, 3, 4, 5];
339 const r = await api("/pose/save", { name, ids });
340 if (!r.ok) return msg(r.error, true);
341 msg(`saved ${name}` + (r.onStops.length ? ` — joint ${r.onStops.join(", ")} rests on a hard stop; "go" aims 30 ticks inside` : ""));
342 $("saveName").value = "";
343 loadPoses();
344 });
345
346 // ---------------------------------------------------------------- recordings
347 let recordings = {};
348 async function loadRecordings() {
349 recordings = await api("/recordings");
350 const list = $("recList"); list.innerHTML = "";
351 const names = Object.keys(recordings);
352 if (!names.length) list.innerHTML = '<p>No recordings yet. Press ● Record, move the arm by hand, then Stop recording.</p>';
353 for (const name of names) {
354 const item = document.createElement("div"); item.className = "item";
355 const label = document.createElement("span"); label.textContent = `${name} · ${recordings[name].seconds} s`;
356 const play = document.createElement("button"); play.textContent = "▶"; play.className = "go";
357 play.onclick = async () => {
358 $("playDlg").close(); msg(`playing ${name}…`);
359 const r = await api("/play", { name, rate: Number($("playRate").value) });
360 msg(r.ok ? `played ${name}: ${r.played} frames; holding` : r.error, !r.ok);
361 };
362 const x = document.createElement("button"); x.className = "x"; x.textContent = "×";
363 x.onclick = async () => { if (!confirm(`Delete recording "${name}"?`)) return; await api("/recording/delete", { name }); loadRecordings(); };
364 item.append(label, play, x); list.append(item);
365 }
366 }
367 $("record").onclick = async () => {
368 if (driving() === "record") {
369 const r = await api("/record/stop", {});
370 msg(r.ok ? `saved recording ${r.saved}: ${r.frames} frames, ${r.seconds} s; torque is off` : r.error, !r.ok);
371 loadRecordings();
372 } else $("recDlg").showModal();
373 };
374 $("recDlg").addEventListener("close", async () => {
375 if ($("recDlg").returnValue !== "start") return;
376 const name = $("recName").value.trim();
377 const r = await api("/record/start", { name });
378 msg(r.ok ? `recording ${name}: torque is off, move the arm by hand, then press ■ Stop recording` : r.error, !r.ok);
379 $("recName").value = "";
380 });
381 $("play").onclick = () => { loadRecordings(); $("playDlg").showModal(); };
382 $("playClose").onclick = () => $("playDlg").close();
383
384 // ---------------------------------------------------------------- live mode: the leader arm through Web Serial
385 // Chrome can open the leader board's USB serial port from the page. The page then does what `so101 leader` does:
386 // torque off, read the six positions at 60 Hz, map them onto the follower's ranges, and stream them.
387 const REG = { Min_Position_Limit: [9, 2], Max_Position_Limit: [11, 2], Torque_Enable: [40, 1], Present_Position: [56, 2, 15] };
388 const decode = (params, size, signBit) => {
389 let v = 0; for (let k = 0; k < size; k++) v |= params[k] << (8 * k);
390 return signBit !== undefined && v & (1 << signBit) ? -(v & ((1 << signBit) - 1)) : v;
391 };
392 class SerialBus {
393 constructor(port) { this.port = port; this.buf = new Uint8Array(0); this.wake = null; }
394 async open() {
395 await this.port.open({ baudRate: 1000000 });
396 this.writer = this.port.writable.getWriter();
397 this.reader = this.port.readable.getReader();
398 this.pump();
399 }
400 async pump() {
401 try {
402 for (;;) {
403 const { value, done } = await this.reader.read();
404 if (done) break;
405 const joined = new Uint8Array(this.buf.length + value.length); joined.set(this.buf); joined.set(value, this.buf.length); this.buf = joined;
406 this.wake?.();
407 }
408 } catch { /* the port went away */ }
409 }
410 async close() {
411 try { await this.reader?.cancel(); } catch {}
412 try { this.writer?.releaseLock(); } catch {}
413 try { await this.port.close(); } catch {}
414 }
415 packet(id, instruction, params) {
416 const body = [id, params.length + 2, instruction, ...params];
417 return new Uint8Array([0xff, 0xff, ...body, ~body.reduce((a, b) => a + b, 0) & 0xff]);
418 }
419 frames() {
420 const out = []; const b = this.buf; let i = 0;
421 while (i + 4 <= b.length) {
422 if (b[i] !== 0xff || b[i + 1] !== 0xff) { i++; continue; }
423 const id = b[i + 2], len = b[i + 3];
424 if (i + 4 + len > b.length) break;
425 out.push({ id, error: b[i + 4], params: b.slice(i + 5, i + 3 + len) });
426 i += 4 + len;
427 }
428 this.buf = b.slice(i);
429 return out;
430 }
431 tick() { return new Promise((r) => { this.wake = r; setTimeout(r, 2); }); }
432 async syncRead(ids, [address, size, signBit]) {
433 this.buf = new Uint8Array(0);
434 await this.writer.write(this.packet(0xfe, 0x82, [address, size, ...ids]));
435 const got = new Map(); const deadline = performance.now() + 15 + ids.length * 3;
436 while (got.size < ids.length && performance.now() < deadline) {
437 for (const f of this.frames()) if (ids.includes(f.id) && f.params.length === size) got.set(f.id, decode(f.params, size, signBit));
438 if (got.size < ids.length) await this.tick();
439 }
440 return got;
441 }
442 async syncWrite([address, size], pairs) {
443 const params = [address, size];
444 for (const [id, v] of pairs) { params.push(id); for (let k = 0; k < size; k++) params.push((v >> (8 * k)) & 0xff); }
445 await this.writer.write(this.packet(0xfe, 0x83, params));
446 }
447 async read(id, [address, size, signBit]) {
448 this.buf = new Uint8Array(0);
449 await this.writer.write(this.packet(id, 0x02, [address, size]));
450 const deadline = performance.now() + 100;
451 for (;;) {
452 for (const f of this.frames()) if (f.id === id && f.params.length === size) return decode(f.params, size, signBit);
453 if (performance.now() > deadline) throw new Error(`no reply from leader servo ${id}: is the leader board powered and plugged in here?`);
454 await this.tick();
455 }
456 }
457 }
458 let live = null;
459 const midOf = (r) => (r.min + r.max) / 2;
460 const clampTo = (v, lo, hi) => Math.min(hi, Math.max(lo, v));
461 const mapLeader = (positions, leaderLimits) => {
462 const frame = {};
463 for (const [id, ticks] of positions) {
464 const L = leaderLimits[id], F = limits[id]; if (!L || !F) continue;
465 if (id === 6) { const f = clampTo((ticks - L.min) / Math.max(1, L.max - L.min), 0, 1); frame[id] = Math.round(F.min + f * (F.max - F.min)); }
466 else frame[id] = clampTo(Math.round(ticks - midOf(L) + midOf(F)), F.min + 30, F.max - 30);
467 }
468 return frame;
469 };
470 async function startLive() {
471 const port = await navigator.serial.requestPort({ filters: [{ usbVendorId: 0x1a86 }] }).catch(() => null);
472 if (!port) return;
473 const bus = new SerialBus(port);
474 try {
475 await bus.open();
476 const ids = [1, 2, 3, 4, 5, 6];
477 const leaderLimits = {};
478 for (const id of ids) leaderLimits[id] = { min: await bus.read(id, REG.Min_Position_Limit), max: await bus.read(id, REG.Max_Position_Limit) };
479 await bus.syncWrite(REG.Torque_Enable, ids.map((id) => [id, 0])); // the leader is moved by hand
480 const ws = new WebSocket(`${location.protocol === "https:" ? "wss" : "ws"}://${location.host}/follow`);
481 await new Promise((resolve, reject) => { ws.onopen = resolve; ws.onerror = () => reject(new Error("no stream socket")); });
482 live = { bus, ws, ids, leaderLimits, frames: 0, busy: false, timer: null };
483 ws.onclose = (e) => { if (live?.ws === ws) stopLive(e.reason || (e.code === 1013 ? "the arm is busy" : "the server closed the stream")); };
484 live.timer = setInterval(async () => {
485 if (!live || live.busy) return;
486 live.busy = true;
487 try {
488 const positions = await bus.syncRead(ids, REG.Present_Position);
489 const frame = mapLeader(positions, leaderLimits);
490 if (ws.readyState === 1 && Object.keys(frame).length) { ws.send(JSON.stringify(frame)); live.frames++; }
491 } catch { /* a missed read; the next tick tries again */ }
492 live.busy = false;
493 }, 16);
494 $("liveBtn").textContent = "■ Stop live";
495 msg("live: the follower follows the leader. CAUTION: it first eases over to the leader's pose.");
496 } catch (err) {
497 await bus.close();
498 msg(`live mode: ${err.message}`, true);
499 }
500 }
501 async function stopLive(reason) {
502 const l = live; if (!l) return;
503 live = null;
504 clearInterval(l.timer);
505 try { l.ws.close(); } catch {}
506 await l.bus.close();
507 $("liveBtn").textContent = "Live from leader";
508 msg(`live mode ended${reason ? `: ${reason}` : ""}; the follower holds (${l.frames} frames)`);
509 }
510 if (navigator.serial) {
511 $("liveBtn").hidden = false;
512 $("liveBtn").onclick = () => (live ? stopLive() : startLive());
513 navigator.serial.addEventListener("disconnect", () => { if (live) stopLive("the leader was unplugged"); });
514 }
515
516 // ---------------------------------------------------------------- the 3D model
517 const viewer = $("model");
518 // The element leaves mesh loading to the page: binary STL through three's loader, the URDF's colour where it names one.
519 viewer.loadMeshFunc = (path, manager, material, done) => {
520 new STLLoader(manager).load(path, (geometry) => done(new THREE.Mesh(geometry, material ?? new THREE.MeshPhongMaterial({ color: 0x9aa4ae, shininess: 30 }))), undefined, (err) => done(null, err));
521 };
522 let dragging = null, table = null, marker = null;
523 viewer.addEventListener("urdf-processed", () => {
524 if (!modelInfo) return;
525 table = new THREE.Mesh(new THREE.PlaneGeometry(0.9, 0.9), new THREE.MeshPhongMaterial({ color: 0x2a2f36, side: THREE.DoubleSide, transparent: true, opacity: 0.85 }));
526 table.position.z = modelInfo.floorZ - 0.0005;
527 const grid = new THREE.GridHelper(0.9, 18, 0x4a5058, 0x30363d); grid.rotation.x = Math.PI / 2; grid.position.z = modelInfo.floorZ;
528 marker = new THREE.Mesh(new THREE.SphereGeometry(0.012, 16, 12), new THREE.MeshBasicMaterial({ color: 0xf85149 })); marker.visible = false;
529 // Not for the mouse: the drag controls take the first thing a ray hits, and three.js hits a line from a metre away.
530 for (const o of [table, grid, marker]) o.raycast = () => {};
531 viewer.world.add(table, grid, marker);
532 viewer.plane.visible = false; // the element's own shadow plane sits under the lowest part, which would be below the table
533 if (status) applyModel(status);
534 // A three-quarter view from the front right, close enough for a 40 cm arm (the element starts 10 m away).
535 viewer.camera.position.set(0.5, 0.38, 0.62);
536 viewer.controls.target.set(0.08, 0.1, 0);
537 viewer.controls.update();
538 viewer.recenter();
539 });
540 // The drag controls listen for mouse events only. A finger is translated into them, so the model can be dragged on a phone or an iPad.
541 {
542 const canvas = viewer.renderer.domElement;
543 canvas.style.touchAction = "none";
544 const asMouse = (type, t) => canvas.dispatchEvent(new MouseEvent(type, { clientX: t.clientX, clientY: t.clientY, bubbles: true }));
545 canvas.addEventListener("touchstart", (e) => { if (e.touches.length === 1) asMouse("mousedown", e.touches[0]); }, { passive: true });
546 canvas.addEventListener("touchmove", (e) => { if (e.touches.length === 1 && dragging) { e.preventDefault(); asMouse("mousemove", e.touches[0]); } }, { passive: false });
547 canvas.addEventListener("touchend", (e) => { if (dragging) asMouse("mouseup", e.changedTouches[0]); }, { passive: true });
548 canvas.addEventListener("touchcancel", (e) => { if (dragging) asMouse("mouseup", e.changedTouches[0]); }, { passive: true });
549 }
550 viewer.addEventListener("manipulate-start", (e) => { dragging = e.detail; stream.open(); });
551 viewer.addEventListener("manipulate-end", () => { dragging = null; stream.end(); });
552 viewer.addEventListener("angle-change", (e) => {
553 if (dragging !== e.detail || !viewer.robot) return;
554 const id = IDS[e.detail]; if (!id || !limits[id]) return;
555 stream.send({ [id]: clampTicks(id, toTicks(id, viewer.robot.joints[e.detail].angle)) });
556 });
557 function applyModel(d) {
558 if (!viewer.robot) return;
559 for (const j of d.joints) {
560 const name = NAMES[j.id];
561 if (!name || j.position == null || name === dragging || !limits[j.id]) continue;
562 viewer.setJointValue(name, toRad(j.id, j.position));
563 }
564 if (marker) {
565 const v = d.blocked ?? d.violation;
566 marker.visible = Boolean(v);
567 if (v) marker.position.set(...v.point);
568 }
569 }
570
571 // ---------------------------------------------------------------- screen
572 const app = $("app");
573 const fullscreen = () => (document.fullscreenElement ? document.exitFullscreen() : app.requestFullscreen());
574 if (app.requestFullscreen) $("fs").onclick = fullscreen; else $("fs").hidden = true;
575 for (const c of cams) c.img.onclick = () => { $("hud").hidden = !$("hud").hidden; };
576 addEventListener("keydown", (e) => {
577 if (e.target.closest("input, select, textarea, dialog")) return;
578 if (e.key === "f" || e.key === "F") fullscreen();
579 if (e.key === " ") { e.preventDefault(); if (e.target instanceof HTMLButtonElement) e.target.blur(); stop(); }
580 });
581 for (const b of document.querySelectorAll("[data-close]")) b.onclick = () => { $(b.dataset.close).hidden = true; };
582
583 // ---------------------------------------------------------------- settings
584 async function loadSettings() {
585 settings = await api("/settings");
586 $("zone").value = settings.zone; $("floorMargin").value = settings.floorMarginMm; $("baseMargin").value = settings.baseMarginMm;
587 const trims = $("trims"); trims.innerHTML = "";
588 for (const [id, name] of Object.entries(NAMES)) {
589 const label = document.createElement("label"); label.textContent = name;
590 const input = document.createElement("input"); input.type = "number"; input.min = -30; input.max = 30; input.step = 0.5; input.value = settings.jointOffsetsDeg[name] ?? 0;
591 input.onchange = () => saveSettings({ jointOffsetsDeg: { [name]: Number(input.value) || 0 } });
592 trims.append(label, input);
593 }
594 $("zoneBadge").textContent = `zone: ${settings.model ? settings.zone : "no model"}`;
595 $("zoneBadge").style.borderColor = settings.zone === "enforce" ? "var(--ok)" : settings.zone === "warn" ? "var(--warn)" : "var(--line)";
596 }
597 async function saveSettings(patch) {
598 const r = await api("/settings", patch, "PUT");
599 if (!r.ok) return msg(r.error, true);
600 msg("settings saved");
601 loadSettings();
602 }
603 $("zone").onchange = () => saveSettings({ zone: $("zone").value });
604 $("floorMargin").onchange = () => saveSettings({ floorMarginMm: Number($("floorMargin").value) });
605 $("baseMargin").onchange = () => saveSettings({ baseMarginMm: Number($("baseMargin").value) });
606 $("settingsBtn").onclick = $("zoneBadge").onclick = () => { loadSettings(); $("settingsPanel").hidden = false; };
607
608 // ---------------------------------------------------------------- metrics
609 async function showMetrics() {
610 const m = await api("/metrics");
611 const c = m.counters;
612 $("metricsSummary").innerHTML = [
613 ["since", (m.since ?? "").replace("T", " ").slice(0, 19)],
614 ["jams", c.jam], ["zone blocks", c.block], ["stalls on moves", c.stall], ["lost servos", c.missing], ["hot joints", c.hot],
615 ["sessions", `${c.stream} streams · ${c.move} moves · ${c.play} playbacks · ${c.record} recordings`],
616 ["closest to the table", m.closest ? `${(m.closest.z * 1000).toFixed(0)} mm (${m.closest.link.replace(/_link$/, "")}, ${m.closest.at.slice(11, 19)})` : "—"],
617 ].map(([k, v]) => `<b>${k}</b><span>${v}</span>`).join("");
618 $("peaks").innerHTML = "<tr><th>joint</th><th>peak load</th><th>peak temperature</th></tr>" + Object.entries(m.peaks).map(([id, p]) => `<tr><td>${NAMES[id] ?? id}</td><td>${p.load.toFixed(0)} %</td><td>${p.temperature} °C</td></tr>`).join("");
619 $("events").innerHTML = [...(m.recent ?? [])].reverse().map((e) => `<div>${e.t.slice(5, 19).replace("T", " ")} <b>${e.kind}</b> ${Object.entries(e.detail).map(([k, v]) => `${k}=${typeof v === "object" ? JSON.stringify(v) : v}`).join(" ")}</div>`).join("") || "<div>none yet</div>";
620 $("metricsPanel").hidden = false;
621 }
622 $("metricsBtn").onclick = showMetrics;
623 setInterval(() => { if (!$("metricsPanel").hidden) showMetrics(); }, 5000);
624
625 // ---------------------------------------------------------------- 3D calibration
626 $("calib").onclick = async () => {
627 $("panel").hidden = false;
628 $("calSteps").textContent = [
629 `export SO101_ARM=http://${location.host}`,
630 `export SO101_CAMERA=${cams[0].url}`,
631 `just vision-calibrate # hold the printed board in front of the camera, tilt it`,
632 `# save poses from this page: home, drop, he1 … he8 (without gripper)`,
633 `just vision-handeye he1,he2,he3,he4,he5,he6,he7,he8`,
634 `just vision-rehearse # sees the cube, plans the joints, moves nothing`,
635 `just vision-pick`,
636 ``,
637 `# camera 2, once it streams: SO101_CAMERA=${cams[1].url} and --publish-as camera2 / handeye2`,
638 ].join("\n");
639 const sheets = await api("/printables");
640 $("printables").innerHTML = Array.isArray(sheets) && sheets.length
641 ? sheets.map((s) => `<a href="${s.url}" download="${s.file}"><button class="go">⬇ ${s.file.replace(/_/g, " ").replace(/\.png$/, "")}</button></a>`).join("")
642 : "<span>no printables on this server: copy Software/vision/printables next to the package</span>";
643 const cal = await api("/calibration");
644 const saved = await api("/poses");
645 const poseNames = Object.keys(saved);
646 const he = poseNames.filter((n) => /^he\d+$/.test(n) && !saved[n].ids.includes(6)).length;
647 const mark = (ok, yes, no) => `<span class="${ok ? "ok" : "no"}">${ok ? yes : no}</span>`;
648 const armOnly = (name) => !saved[name] ? mark(false, "", "missing") : saved[name].ids.includes(6) ? mark(false, "", "saved with the gripper: save it again without") : mark(true, "saved");
649 const camLine = (c) => c ? mark(true, `calibrated ${(c.saved ?? "").slice(0, 10)} · ${c.width}×${c.height} · ${Number(c.rms).toFixed(2)} px error`) : mark(false, "", "not yet");
650 const heLine = (h) => h ? mark(true, `solved ${(h.saved ?? "").slice(0, 10)} · ${h.poses?.length ?? "?"} poses · mean ${h.residuals_mm ? (h.residuals_mm.reduce((a, b) => a + b, 0) / h.residuals_mm.length).toFixed(1) : "?"} mm`) : mark(false, "", "not yet");
651 $("calStatus").innerHTML = [
652 ["camera 1", camLine(cal.camera)], ["hand-eye 1", heLine(cal.handeye)],
653 ...($("v2").hidden ? [] : [["camera 2", camLine(cal.camera2)], ["hand-eye 2", heLine(cal.handeye2)]]),
654 ["pose home", armOnly("home")], ["pose drop", armOnly("drop")],
655 ["poses he1…", mark(he >= 6, `${he} saved`, `${he} of 6 to 8 saved (without the gripper)`)],
656 ].map(([k, v]) => `<b>${k}</b><span>${v}</span>`).join("");
657 };
658
659 // ---------------------------------------------------------------- live status, sliders and jog
660 const joints = $("joints");
661 const built = new Map();
662 let sliding = null;
663 function render(d) {
664 for (const j of d.joints) {
665 let el = built.get(j.id);
666 if (!el) {
667 const lim = limits[j.id] ?? { min: 0, max: 4095 };
668 const div = document.createElement("div"); div.className = "j";
669 div.innerHTML = `<b>${j.joint ?? j.id}</b><div class="v"><span class="pos"></span><small class="t"></small></div>
670 <div class="slide"><span class="jog"><button data-d="-1">−</button></span><input type="range" min="${lim.min + 30}" max="${lim.max - 30}" step="1"><span class="jog"><button data-d="1">+</button></span></div>`;
671 const range = div.querySelector("input");
672 range.addEventListener("pointerdown", () => { sliding = j.id; stream.open(); });
673 range.addEventListener("input", () => { if (sliding === j.id) stream.send({ [j.id]: Number(range.value) }); });
674 const done = () => { if (sliding === j.id) { sliding = null; stream.end(); } };
675 range.addEventListener("pointerup", done); range.addEventListener("pointercancel", done); range.addEventListener("change", done);
676 for (const b of div.querySelectorAll(".jog button")) {
677 // Hold to jog: the target runs ahead of the arm at the chosen speed while the button is down.
678 let timer = null, target = null;
679 const start = (e) => {
680 e.preventDefault();
681 if (timer) return; // a second finger on the same button must not start a second run
682 try { b.setPointerCapture?.(e.pointerId); } catch {}
683 const joint = status?.joints.find((x) => x.id === j.id);
684 if (!joint || joint.position == null) return msg("no position for that joint", true);
685 target = joint.position; stream.open();
686 timer = setInterval(() => { target = clampTicks(j.id, target + (Number(b.dataset.d) * Number($("rate").value)) / 20); stream.send({ [j.id]: target }); }, 50);
687 };
688 const end = () => { if (timer) { clearInterval(timer); timer = null; stream.end(); } };
689 for (const ev of ["pointerup", "pointercancel", "lostpointercapture", "pointerleave", "blur"]) b.addEventListener(ev, end);
690 addEventListener("blur", end);
691 b.addEventListener("pointerdown", start);
692 b.addEventListener("contextmenu", (e) => e.preventDefault());
693 }
694 joints.append(div);
695 el = { div, pos: div.querySelector(".pos"), t: div.querySelector(".t"), range, buttons: div.querySelectorAll("button") };
696 built.set(j.id, el);
697 }
698 const hot = j.temperature >= 55, load = Math.abs(j.load ?? 0);
699 el.pos.textContent = j.position ?? "—";
700 el.pos.style.color = hot ? "var(--bad)" : load > 50 ? "var(--warn)" : "var(--fg)";
701 el.t.textContent = `${j.temperature ?? "—"}°C · ${load.toFixed(0)}%${j.torque ? " ·T" : ""}`;
702 if (sliding !== j.id && j.position != null) el.range.value = j.position;
703 el.div.classList.toggle("jam", d.jammed?.includes(j.id));
704 el.div.classList.toggle("drag", dragging === j.joint || sliding === j.id);
705 const locked = (d.driving !== null && d.driving !== "stream") || live !== null; // the page's own stream may run; a leader's may not be fought
706 el.range.disabled = locked; for (const b of el.buttons) b.disabled = locked;
707 }
708 }
709 function banner(d) {
710 const b = $("banner");
711 // The depth includes the margin: say where the part really is, and how much of that is margin.
712 const say = (v) => {
713 const part = v.link.replace(/_link$/, "").replace(/_so101_v1$/, "");
714 const marginMm = v.what === "table" ? settings?.floorMarginMm ?? 0 : settings?.baseMarginMm ?? 0;
715 const beyond = v.depth * 1000 - marginMm;
716 if (beyond > 0) return `${part} is ${beyond.toFixed(0)} mm ${v.what === "table" ? "below the table" : "inside the base"} (plus the ${marginMm} mm margin)`;
717 return `${part} is ${(-beyond).toFixed(0)} mm ${v.what === "table" ? "above the table" : "from the base"}, inside the ${marginMm} mm margin`;
718 };
719 if (d.jammed?.length) { b.className = "bad"; b.textContent = `jam: ${d.jammed.map((id) => NAMES[id] ?? id).join(", ")} pushed against something and stopped — back the leader off`; }
720 else if (d.blocked) { b.className = d.zone === "enforce" ? "bad" : "warn"; b.textContent = `${d.zone === "enforce" ? "held at the boundary" : "warning"}: ${say(d.blocked)}`; }
721 else if (d.violation) { b.className = "warn"; b.textContent = `the arm is ${say(d.violation)} (zone: ${d.zone})`; }
722 else b.className = "";
723 }
724 async function poll() {
725 try {
726 const d = await api("/status");
727 if (!d.ok) throw new Error();
728 status = d;
729 $("dot").className = "dot on";
730 $("state").textContent = d.driving === null ? "idle" : d.driving === "record" ? `recording ${d.recording}` : live ? `live · ${live.frames} frames` : d.driving;
731 $("stop").hidden = d.driving === null;
732 $("record").textContent = d.driving === "record" ? "■ Stop recording" : "● Record";
733 for (const id of ["hold", "releaseBtn", "play"]) $(id).disabled = d.driving !== null;
734 render(d); banner(d); applyModel(d);
735 $("strip").innerHTML = `<span>lowest part <b>${d.lowestMm ?? "—"} mm</b> above the table</span><span>zone <b>${d.zone}</b></span>`;
736 } catch { $("dot").className = "dot"; $("state").textContent = "no arm"; }
737 setTimeout(poll, 200);
738 }
739 (async () => {
740 limits = await api("/limits");
741 await loadSettings();
742 await loadPoses();
743 modelInfo = await api("/model.json");
744 if (modelInfo.ok) viewer.urdf = modelInfo.urdf; else { $("v3").hidden = true; $("pane3").classList.remove("on"); }
745 try {
746 const saved = JSON.parse(localStorage.getItem("so101.panes") || "null");
747 const usable = (pane) => pane === "pane1" || (pane === "pane2" && !$("v2").hidden) || (pane === "pane3" && modelInfo.ok);
748 const wanted = Array.isArray(saved) ? saved.filter(usable) : [];
749 if (wanted.length) for (const [btn, pane] of Object.entries(panes)) { const on = wanted.includes(pane); $(btn).classList.toggle("on", on); if (pane === "pane3") $(pane).classList.toggle("on", on); else showCam(cams[pane === "pane1" ? 0 : 1], on); }
750 } catch {}
751 poll();
752 })();
753 </script>
754 </body>
755 </html>
Software/so101/tsconfig.json added +18
@@ -0,0 +1,18 @@
1 {
2 "compilerOptions": {
3 "lib": ["ESNext"],
4 "target": "ESNext",
5 "module": "Preserve",
6 "moduleResolution": "bundler",
7 "allowImportingTsExtensions": true,
8 "verbatimModuleSyntax": true,
9 "noEmit": true,
10 "strict": true,
11 "skipLibCheck": true,
12 "noUncheckedIndexedAccess": true,
13 "noFallthroughCasesInSwitch": true,
14 "erasableSyntaxOnly": true,
15 "types": ["bun"]
16 },
17 "include": ["src", "bin"]
18 }
Software/vision/QUEST.md added +156
@@ -0,0 +1,156 @@
1 # Quest: the arm picks up a cube it can see
2
3 One session, roughly an hour of your time, most of it watching. Every arm
4 move is a supervised pose move, so a mistake ends in "stopped, holding", not a
5 stripped gear. Run everything from the repo root in your shell, with the arm
6 clamped.
7
8 With the arm and the webcam on the Pi (the usual setup), point the kit there:
9
10 export SO101_ARM=http://<pi>:8101
11 export SO101_CAMERA=http://<pi>:8080/stream
12
13 With the arm and the webcam on this machine instead: `SO101_PORT` exported and
14 `SO101_CAMERA` set to the webcam's index from `lerobot-find-cameras opencv`.
15
16 Poses are saved from the arm's web page (`http://<arm>:8101/`): press
17 **Release**, move the arm by hand, press **Save pose**, and uncheck *with
18 gripper*. The page's **3D calibration** button shows this quest's status.
19
20 ## 0. Loot (before you start)
21
22 - Print the three sheets in `Software/vision/printables/` at 100 % scale on
23 matte paper: the arm's page offers them for download under **3D
24 calibration**, and `just vision-markers` regenerates them. Check a cube
25 marker's black square with a ruler: 40 mm. Cut out the six cube markers and
26 the gripper marker. Tape the cube markers to the faces of a cube-ish object
27 of about 40 to 50 mm (a wooden block, a stack of Lego). Tape the gripper
28 marker flat on the outside of the fixed jaw, facing where the camera will be.
29 - Put the webcam on the overhead mount, or on anything rigid, looking down at
30 the table in front of the arm. It must not move again after step 2.
31 - The arm grasps straight down, so the cube must sit where a vertical gripper
32 can reach. Computed from the arm's own model and your calibration: the sweet
33 spot is 18 to 24 cm in front of the base, up to 12 cm to either side. Right
34 in front of the base closer than 16 cm is awkward for the wrist; avoid it.
35
36 ## 1. See (2 min)
37
38 just vision-view
39
40 A viewer opens with the camera image and every marker it recognizes.
41 Confirm the cube and the gripper marker are both detected where you'll work.
42 Ctrl-C.
43
44 ## 2. Calibrate the camera (5 min, hands-free capture)
45
46 just vision-calibrate
47
48 The board is the ChArUco board: a checkerboard with a small marker inside
49 each white square, `Software/vision/printables/charuco_board.png`, printed at 100 %. Hold it in
50 front of the camera and slowly move and tilt it.
51 Views are captured automatically as the angle changes; the counter stops at 15.
52 Reprojection error under 1 px is good. Saves `outputs/camera.json`.
53
54 ## 3. Tell the arm where the camera is (10 min)
55
56 First two poses of the arm alone (no gripper), used by every later step:
57 `home` (the arm resting, nothing in reach) and `drop` (gripper over the bin
58 or a mark). Save them from the web page without the gripper, or from a shell
59 on the machine with the arm:
60
61 so101 torque off
62 so101 pose save home --ids 1-5
63 so101 pose save drop --ids 1-5
64
65 Then, by hand, put the arm in six to eight poses where the gripper marker
66 faces the camera from different places AND different tilts and rolls, saving
67 each as `he1`, `he2`, ... `he8`, without the gripper. Variety of orientation
68 is what the solver needs; six positions at the same angle will be refused.
69
70 Then step back:
71
72 just vision-handeye he1,he2,he3,he4,he5,he6,he7,he8
73
74 The arm visits each pose on its own, parks at `home`, and the solver prints
75 the camera's position relative to the base plus a residual per pose. It saves
76 only when the mean residual is under 12 mm and the worst under 25 mm, and
77 publishes the result to the arm's server so the web page shows it. A
78 correct setup gives a few millimeters. If it refuses: between 8 and 12 mm,
79 add two more poses with bigger tilts and rerun; above that, a joint direction
80 disagrees between the servo and the model, so say so and we'll find which
81 with the viewer.
82
83 ## 4. Pick (the payoff)
84
85 Put the cube in the zone, then first a rehearsal that moves nothing:
86
87 just vision-rehearse
88
89 It prints where it sees the cube in arm coordinates and the joint angles it
90 plans. If that looks sane, the real thing:
91
92 just vision-pick
93
94 Sequence: open gripper, pre-grasp above the cube, descend, close until the
95 jaws meet the cube (the jam detector is the grasp sensor, and the jaw
96 position confirms something is between them), squeeze gently, lift, go to
97 `drop`, open, return to `home`, release.
98
99 ## When something is off
100
101 - Nothing detected: light, glare, or a marker too small in the image.
102 - Hand-eye residuals large: run `just vision-view` with the arm holding a pose
103 and compare the printed "base" coordinates of the gripper marker with where
104 the gripper physically is.
105 - Pick misses by a constant offset: the grasp depth and approach heights are
106 constants at the top of `pick.py`. A block that is not 40 mm wide:
107 `--cube-mm` (the recipes pass none; run `python Software/vision/pick.py`
108 directly for that).
109
110 ## References
111
112 The methods behind this quest, so you can read the originals:
113
114 - **Camera calibration.** Z. Zhang, *A Flexible New Technique for Camera
115 Calibration*, IEEE TPAMI 22(11), 2000. [doi:10.1109/34.888718](https://doi.org/10.1109/34.888718).
116 OpenCV's `calibrateCamera` implements it; the ChArUco flow is in the
117 [Calibration with ArUco and ChArUco](https://docs.opencv.org/4.x/da/d13/tutorial_aruco_calibration.html) tutorial.
118 - **ArUco markers and ChArUco boards.** S. Garrido-Jurado, R. Muñoz-Salinas,
119 F. J. Madrid-Cuevas, M. J. Marín-Jiménez, *Automatic generation and detection
120 of highly reliable fiducial markers under occlusion*, Pattern Recognition
121 47(6), 2014. [doi:10.1016/j.patcog.2014.01.005](https://doi.org/10.1016/j.patcog.2014.01.005).
122 OpenCV: [Detection of ArUco markers](https://docs.opencv.org/4.x/d5/dae/tutorial_aruco_detection.html).
123 - **Pose of one square marker (PnP).** T. Collins, A. Bartoli, *Infinitesimal
124 Plane-Based Pose Estimation*, IJCV 109(3), 2014.
125 [doi:10.1007/s11263-014-0725-5](https://doi.org/10.1007/s11263-014-0725-5).
126 This is `SOLVEPNP_IPPE_SQUARE`; it returns the two planar solutions, and the
127 kit keeps the clearly better one (`ambiguity`).
128 - **Hand-eye calibration, the classic forms (AX = XB).** R. Y. Tsai, R. K. Lenz,
129 *A new technique for fully autonomous and efficient 3D robotics hand/eye
130 calibration*, IEEE T-RA 5(3), 1989. [doi:10.1109/70.34770](https://doi.org/10.1109/70.34770).
131 F. C. Park, B. J. Martin, *Robot sensor calibration: solving AX = XB on the
132 Euclidean group*, IEEE T-RA 10(5), 1994. [doi:10.1109/70.326576](https://doi.org/10.1109/70.326576).
133 K. Daniilidis, *Hand-Eye Calibration Using Dual Quaternions*, IJRR 18(3), 1999.
134 [doi:10.1177/02783649922066213](https://doi.org/10.1177/02783649922066213).
135 - **Robot-world / hand-eye in one solve (AX = ZB), what the kit uses.**
136 M. Shah, *Solving the Robot-World/Hand-Eye Calibration Problem Using the
137 Kronecker Product*, ASME J. Mechanisms and Robotics 5(3), 2013.
138 [doi:10.1115/1.4024473](https://doi.org/10.1115/1.4024473). A. Li, L. Wang,
139 D. Wu, *Simultaneous robot-world and hand-eye calibration using
140 dual-quaternions and Kronecker product*, Int. J. Physical Sciences 5(10), 2010.
141 OpenCV: `calibrateRobotWorldHandEye` and `calibrateHandEye` in the
142 [calib3d module](https://docs.opencv.org/4.x/d9/d0c/group__calib3d.html).
143 - **Two cameras, a full orientation.** R. Hartley, A. Zisserman, *Multiple View
144 Geometry in Computer Vision*, 2nd ed., Cambridge University Press, 2004
145 (epipolar geometry, `stereoCalibrate`; triangulation, `triangulatePoints`).
146 - **The arm's model.** The URDF and meshes in `Simulation/SO101` from this
147 repository; LeRobot's `RobotKinematics` runs them through
148 [placo](https://github.com/Rhoban/placo) for the IK; the TypeScript server
149 reads the same files for its own forward kinematics and the safe zone.
150 - **The robot and the stack.** [SO-ARM100 / SO-101](https://github.com/TheRobotStudio/SO-ARM100)
151 by The Robot Studio; [Hugging Face LeRobot](https://huggingface.co/docs/lerobot/so101),
152 whose calibration convention (zero at the middle of each joint's range) the
153 model follows; [three.js](https://threejs.org) and
154 [urdf-loader](https://github.com/gkjohnson/urdf-loaders) for the 3D view;
155 the [Web Serial API](https://developer.mozilla.org/en-US/docs/Web/API/Web_Serial_API)
156 for live mode; [µStreamer](https://github.com/pikvm/ustreamer) for the cameras.
Software/vision/calibrate_camera.py added +108
@@ -0,0 +1,108 @@
1 """Camera intrinsics from a ChArUco board, hands-free: hold the board in view and move it around; views are
2 captured automatically whenever the board is seen well and from a new angle. Finishes itself.
3
4 python Software/vision/calibrate_camera.py --camera 1 --out outputs/camera.json
5 """
6
7 from __future__ import annotations
8
9 import argparse
10 import json
11 import time
12 from pathlib import Path
13
14 import cv2
15 import numpy as np
16 import rerun as rr
17
18 from common import Intrinsics, arm, camera_source, grab, open_camera
19 from markers import charuco_board
20
21 NEEDED = 15
22 MIN_CORNERS = 12
23 TILTED_NEEDED = 6 # views where the board is clearly not parallel to the image plane
24 TILT_SKEW_DEG = 4.0 # opposite board edges converge by at least this much in the image
25
26
27 def skew_deg(corners: np.ndarray) -> float:
28 """How much the board's outline departs from a parallelogram: 0 when parallel to the sensor, larger when tilted."""
29 hull = cv2.convexHull(corners.astype(np.float32)).reshape(-1, 2)
30 quad = cv2.approxPolyDP(hull, 0.02 * cv2.arcLength(hull, True), True).reshape(-1, 2)
31 if len(quad) != 4:
32 return 0.0
33 def angle(a, b):
34 return np.degrees(np.arctan2(b[1] - a[1], b[0] - a[0]))
35 def diff(a, b):
36 d = abs(a - b) % 180
37 return min(d, 180 - d)
38 return diff(angle(quad[0], quad[1]), angle(quad[3], quad[2])) + diff(angle(quad[1], quad[2]), angle(quad[0], quad[3]))
39
40
41 def main() -> None:
42 ap = argparse.ArgumentParser()
43 ap.add_argument("--camera", type=camera_source, default=1, help="an OpenCV index, or the ustreamer URL on the Pi")
44 ap.add_argument("--width", type=int, default=1280)
45 ap.add_argument("--height", type=int, default=720)
46 ap.add_argument("--out", type=Path, default=Path("outputs/camera.json"))
47 ap.add_argument("--publish-as", default="camera", help="name on the arm's server: camera for the first camera, camera2 for the second")
48 args = ap.parse_args()
49
50 rr.init("so101 camera calibration", spawn=True)
51 board = charuco_board()
52 cdet = cv2.aruco.CharucoDetector(board)
53 cap = open_camera(args.camera, args.width, args.height)
54
55 views: list[tuple[np.ndarray, np.ndarray]] = [] # (charuco corners, ids)
56 fingerprints: list[np.ndarray] = []
57 tilted = 0
58 last_capture = 0.0
59 print(f"show the board to camera {args.camera}; move it around AND tilt it; {NEEDED} views, at least {TILTED_NEEDED} tilted")
60 while len(views) < NEEDED or tilted < TILTED_NEEDED:
61 frame = grab(cap)
62 corners, ids, _, _ = cdet.detectBoard(frame)
63 n = 0 if ids is None else len(ids)
64 rr.log("camera/image", rr.Image(frame[..., ::-1]))
65 status = f"{len(views)}/{NEEDED} views, {tilted}/{TILTED_NEEDED} tilted"
66 if n >= MIN_CORNERS:
67 pts = corners.reshape(-1, 2)
68 rr.log("camera/corners", rr.Points2D(pts, radii=4, colors=[0, 255, 0]))
69 tilt = skew_deg(pts)
70 # A new view must look different: position, spread, or tilt moved by a margin.
71 fp = np.array([*pts.mean(axis=0), *pts.std(axis=0), tilt * 10])
72 fresh = all(np.linalg.norm(fp - f) > 40 for f in fingerprints)
73 need_tilt = len(views) >= NEEDED and tilted < TILTED_NEEDED
74 if fresh and time.time() - last_capture > 0.7 and (tilt >= TILT_SKEW_DEG or not need_tilt):
75 views.append((corners, ids))
76 fingerprints.append(fp)
77 tilted += tilt >= TILT_SKEW_DEG
78 last_capture = time.time()
79 status += " captured" + (" (tilted)" if tilt >= TILT_SKEW_DEG else "")
80 elif need_tilt:
81 status += " tilt the board more"
82 else:
83 rr.log("camera/corners", rr.Clear(recursive=False))
84 rr.log("status", rr.TextLog(status))
85 print(f"\r{status} ", end="", flush=True)
86 cap.release()
87
88 objp, imgp = [], []
89 for corners, ids in views:
90 o, i = board.matchImagePoints(corners, ids)
91 objp.append(o)
92 imgp.append(i)
93 rms, K, dist, _, _ = cv2.calibrateCamera(objp, imgp, (args.width, args.height), None, None, flags=cv2.CALIB_FIX_K3)
94 intr = Intrinsics(args.width, args.height, K, dist, float(rms))
95 f = (K[0, 0] + K[1, 1]) / 2
96 print(f"\nfocal {K[0, 0]:.0f}/{K[1, 1]:.0f} px, center ({K[0, 2]:.0f}, {K[1, 2]:.0f}), reprojection rms {rms:.2f} px")
97 if not (0.5 * args.width < f < 3.0 * args.width):
98 raise SystemExit("that focal length is not a webcam's; the views did not constrain it. Run again with the board tilted more")
99 if rms > 1.0:
100 print("rms above 1 px is mediocre: flatter board, better light, or more varied angles would help")
101 args.out.parent.mkdir(parents=True, exist_ok=True)
102 intr.save(args.out)
103 print(f"saved {args.out}")
104 arm().publish_calibration(args.publish_as, json.loads(args.out.read_text()))
105
106
107 if __name__ == "__main__":
108 main()
Software/vision/common.py added +442
@@ -0,0 +1,442 @@
1 """Shared pieces for the ArUco pick: camera, markers, poses, servo units, and the bridge to the so101 CLI.
2
3 Frames: every 4x4 matrix here is T_a_b, "pose of b expressed in a", so T_a_b @ p_b = p_a.
4 Units: meters and radians inside, millimeters and degrees at the edges where humans look.
5 """
6
7 from __future__ import annotations
8
9 import json
10 import os
11 import os
12 import subprocess
13 import urllib.error
14 import urllib.parse
15 import urllib.request
16 import tempfile
17 from dataclasses import dataclass
18 from pathlib import Path
19
20 import cv2
21 import numpy as np
22
23 REPO = Path(__file__).resolve().parents[2]
24 URDF = REPO / "Simulation/SO101/so101_new_calib.urdf"
25 POSES = REPO / "poses.json" # where `so101 pose save` writes when run from the repo root
26 DICTIONARY = cv2.aruco.DICT_4X4_50
27 CUBE_IDS = (0, 1, 2, 3, 4, 5)
28 GRIPPER_ID = 10
29 JOINTS = ("shoulder_pan", "shoulder_lift", "elbow_flex", "wrist_flex", "wrist_roll", "gripper")
30 SERVO_IDS = (1, 2, 3, 4, 5, 6)
31 RESOLUTION = 4095 # LeRobot's max_res for the STS3215's degrees normalization
32
33
34 # ---------------------------------------------------------------- transforms
35 def make_T(R: np.ndarray, t: np.ndarray) -> np.ndarray:
36 T = np.eye(4)
37 T[:3, :3] = R
38 T[:3, 3] = np.asarray(t, dtype=float).ravel()
39 return T
40
41
42 def invert(T: np.ndarray) -> np.ndarray:
43 R, t = T[:3, :3], T[:3, 3]
44 return make_T(R.T, -R.T @ t)
45
46
47 def rodrigues_T(rvec: np.ndarray, tvec: np.ndarray) -> np.ndarray:
48 R, _ = cv2.Rodrigues(np.asarray(rvec, dtype=float))
49 return make_T(R, tvec)
50
51
52 def rotation_about_z(yaw: float) -> np.ndarray:
53 c, s = np.cos(yaw), np.sin(yaw)
54 return np.array([[c, -s, 0.0], [s, c, 0.0], [0.0, 0.0, 1.0]])
55
56
57 def average_T(Ts: list[np.ndarray]) -> np.ndarray:
58 """Mean of nearby poses: translation averaged, rotation via SVD projection of the mean matrix."""
59 t = np.mean([T[:3, 3] for T in Ts], axis=0)
60 M = np.mean([T[:3, :3] for T in Ts], axis=0)
61 U, _, Vt = np.linalg.svd(M)
62 R = U @ Vt
63 if np.linalg.det(R) < 0:
64 U[:, -1] *= -1
65 R = U @ Vt
66 return make_T(R, t)
67
68
69 # ---------------------------------------------------------------- servo units
70 @dataclass(frozen=True)
71 class JointRange:
72 min: int
73 max: int
74
75 @property
76 def mid(self) -> float:
77 return (self.min + self.max) / 2
78
79
80 def load_joint_ranges(calibration_json: Path | None = None) -> dict[str, JointRange]:
81 """The per-joint min/max LeRobot calibrated; degrees are measured from the middle of each range."""
82 path = calibration_json or (
83 Path(os.environ.get("HF_LEROBOT_CALIBRATION", Path.home() / ".cache/huggingface/lerobot/calibration"))
84 / "robots/so_follower/so101_follower.json"
85 )
86 data = json.loads(path.read_text())
87 return {name: JointRange(int(data[name]["range_min"]), int(data[name]["range_max"])) for name in JOINTS}
88
89
90 def ticks_to_degrees(ticks: float, rng: JointRange) -> float:
91 return (ticks - rng.mid) * 360.0 / RESOLUTION
92
93
94 def degrees_to_ticks(deg: float, rng: JointRange) -> int:
95 return int(round(deg * RESOLUTION / 360.0 + rng.mid))
96
97
98 # ---------------------------------------------------------------- camera
99 @dataclass
100 class Intrinsics:
101 width: int
102 height: int
103 K: np.ndarray
104 dist: np.ndarray
105 rms: float = 0.0
106
107 @staticmethod
108 def load(path: Path) -> "Intrinsics":
109 d = json.loads(path.read_text())
110 return Intrinsics(d["width"], d["height"], np.array(d["K"]), np.array(d["dist"]), d.get("rms", 0.0))
111
112 def save(self, path: Path) -> None:
113 path.write_text(
114 json.dumps(
115 {"width": self.width, "height": self.height, "K": self.K.tolist(), "dist": self.dist.ravel().tolist(), "rms": self.rms},
116 indent=1,
117 )
118 )
119
120
121 def camera_source(text: str) -> int | str:
122 """`--camera 1` is an OpenCV index; `--camera http://pi:8080/stream` is the ustreamer on the Pi."""
123 return int(text) if text.isdigit() else text
124
125
126 class HttpCamera:
127 """Frames from ustreamer: each read fetches /snapshot, the newest frame, so there is no buffered lag.
128
129 Quacks like cv2.VideoCapture for the few calls the kit makes."""
130
131 def __init__(self, url: str):
132 # http://pi:8080/stream, with or without a trailing slash or a query, becomes http://pi:8080/snapshot;
133 # the endless MJPEG stream itself would never finish a read.
134 parts = urllib.parse.urlsplit(url)
135 path = parts.path.rstrip("/")
136 if path.endswith("/stream"):
137 path = path[: -len("/stream")] + "/snapshot"
138 self.url = urllib.parse.urlunsplit((parts.scheme, parts.netloc, path or "/snapshot", "", ""))
139
140 def isOpened(self) -> bool:
141 return True
142
143 def set(self, *_: object) -> bool:
144 return False # the resolution is ustreamer's --resolution
145
146 def grab(self) -> bool:
147 return True
148
149 def read(self) -> tuple[bool, np.ndarray | None]:
150 try:
151 with urllib.request.urlopen(self.url, timeout=5) as r:
152 data = r.read()
153 except (urllib.error.URLError, TimeoutError, OSError):
154 return False, None
155 frame = cv2.imdecode(np.frombuffer(data, np.uint8), cv2.IMREAD_COLOR)
156 return frame is not None, frame
157
158 def release(self) -> None:
159 pass
160
161
162 def open_camera(source: int | str, width: int, height: int) -> cv2.VideoCapture | HttpCamera:
163 cap = HttpCamera(source) if isinstance(source, str) else cv2.VideoCapture(source)
164 if not cap.isOpened():
165 raise SystemExit(f"camera {source} did not open; `lerobot-find-cameras opencv` lists what exists")
166 cap.set(cv2.CAP_PROP_FRAME_WIDTH, width)
167 cap.set(cv2.CAP_PROP_FRAME_HEIGHT, height)
168 cap.set(cv2.CAP_PROP_BUFFERSIZE, 1)
169 for _ in range(5): # let exposure settle and drop stale frames
170 cap.read()
171 ok, frame = cap.read()
172 if not ok or frame is None:
173 raise SystemExit(f"camera {source} opened but returned no frame")
174 if frame.shape[1] != width or frame.shape[0] != height:
175 fix = f"; restart ustreamer with --resolution={width}x{height}" if isinstance(source, str) else ""
176 raise SystemExit(f"camera {source} gives {frame.shape[1]}x{frame.shape[0]}, not the {width}x{height} it was calibrated at{fix}")
177 return cap
178
179
180 def grab(cap: cv2.VideoCapture | HttpCamera) -> np.ndarray:
181 cap.grab() # discard the buffered frame so the next one is current
182 ok, frame = cap.read()
183 if not ok:
184 raise SystemExit("camera stopped delivering frames")
185 return frame
186
187
188 # ---------------------------------------------------------------- markers
189 def detector() -> cv2.aruco.ArucoDetector:
190 params = cv2.aruco.DetectorParameters()
191 params.cornerRefinementMethod = cv2.aruco.CORNER_REFINE_SUBPIX
192 return cv2.aruco.ArucoDetector(cv2.aruco.getPredefinedDictionary(DICTIONARY), params)
193
194
195 def marker_object_points(size_m: float) -> np.ndarray:
196 """Marker corners in the marker's own frame: z out of the face, origin at the center, corner order as OpenCV detects it."""
197 h = size_m / 2
198 return np.array([[-h, h, 0], [h, h, 0], [h, -h, 0], [-h, -h, 0]], dtype=np.float32)
199
200
201 @dataclass
202 class Detection:
203 id: int
204 corners: np.ndarray # 4x2 pixels
205 T_cam_marker: np.ndarray | None = None # set when intrinsics and the marker's size are known
206 reprojection_px: float = float("nan")
207 # how clearly the planar pose was decided: second-best over best reprojection error (bigger is clearer)
208 ambiguity: float = float("nan")
209
210
211 def detect(
212 frame_bgr: np.ndarray,
213 det: cv2.aruco.ArucoDetector,
214 intr: Intrinsics | None = None,
215 sizes_m: dict[int, float] | float | None = None,
216 ) -> list[Detection]:
217 """Find markers; with intrinsics and sizes (per id, or one for all) also estimate each pose.
218
219 IPPE_SQUARE returns two candidate poses for a planar marker; the one that reprojects better wins, and
220 `ambiguity` records how clearly, so callers can drop frames where a near-frontal marker could flip.
221 """
222 gray = cv2.cvtColor(frame_bgr, cv2.COLOR_BGR2GRAY)
223 corners, ids, _ = det.detectMarkers(gray)
224 found: list[Detection] = []
225 if ids is None:
226 return found
227 for c, i in zip(corners, ids.ravel()):
228 d = Detection(int(i), c.reshape(4, 2))
229 size = sizes_m.get(d.id) if isinstance(sizes_m, dict) else sizes_m
230 if intr is not None and size is not None:
231 n, rvecs, tvecs, errs = cv2.solvePnPGeneric(
232 marker_object_points(size), d.corners.astype(np.float32), intr.K, intr.dist, flags=cv2.SOLVEPNP_IPPE_SQUARE
233 )
234 if n:
235 errs = np.asarray(errs).ravel()
236 best = int(np.argmin(errs))
237 d.T_cam_marker = rodrigues_T(rvecs[best], tvecs[best])
238 d.reprojection_px = float(errs[best])
239 d.ambiguity = float(np.sort(errs)[1] / max(errs[best], 1e-6)) if n > 1 else float("inf")
240 found.append(d)
241 return found
242
243
244 def median_T(Ts: list[np.ndarray]) -> np.ndarray:
245 """Translation by coordinate-wise median (robust to a few bad frames), rotation by the SVD mean."""
246 T = average_T(Ts)
247 T[:3, 3] = np.median([t[:3, 3] for t in Ts], axis=0)
248 return T
249
250
251 # ---------------------------------------------------------------- the arm
252 # Two ways to reach it, with the same supervision underneath: the so101 CLI on this machine (SO101_PORT),
253 # or `so101 serve` on the Pi (SO101_ARM=http://pi:8101). Every move is a supervised pose move; the CLI
254 # and the server own the serial port, the limit margins, and the jam detection.
255
256
257 def so101(*args: str, check: bool = True) -> subprocess.CompletedProcess[str]:
258 """Run the TypeScript CLI."""
259 try:
260 r = subprocess.run(["so101", *args], text=True, capture_output=True)
261 except FileNotFoundError:
262 raise SystemExit("`so101` is not on PATH; run from the repo's direnv shell") from None
263 if check and r.returncode != 0:
264 raise SystemExit(f"so101 {' '.join(args)} failed: {last_line(r.stdout + r.stderr)}")
265 return r
266
267
268 def last_line(text: str) -> str:
269 lines = [ln for ln in text.strip().splitlines() if ln.strip()]
270 return lines[-1] if lines else "(no output)"
271
272
273 class CliArm:
274 """The arm on this machine, through the so101 CLI."""
275
276 def __init__(self, poses_file: Path = POSES):
277 self.poses_file = poses_file
278 self._dir = tempfile.TemporaryDirectory()
279 self._scratch = Path(self._dir.name) / "poses.json"
280
281 def __str__(self) -> str:
282 return "the arm on this machine (so101 CLI)"
283
284 def limits(self) -> dict[int, tuple[int, int]]:
285 out = {}
286 for sid in SERVO_IDS:
287 lo = int(so101("read", str(sid), "Min_Position_Limit").stdout.split("=")[1].split()[0])
288 hi = int(so101("read", str(sid), "Max_Position_Limit").stdout.split("=")[1].split()[0])
289 out[sid] = (lo, hi)
290 return out
291
292 def positions(self) -> dict[int, int]:
293 so101("pose", "save", "now", "--file", str(self._scratch))
294 pose = json.loads(self._scratch.read_text())["now"]
295 return {int(sid): int(p) for sid, p in zip(pose["ids"], pose["positions"])}
296
297 def poses(self) -> dict:
298 return json.loads(self.poses_file.read_text()) if self.poses_file.exists() else {}
299
300 def go_pose(self, name: str, *, speed: int = 200, release: bool = False) -> tuple[bool, str]:
301 args = ["pose", "go", name, "--file", str(self.poses_file), "--speed", str(speed)] + (["--release"] if release else [])
302 r = so101(*args, check=False)
303 return r.returncode == 0, (r.stdout + r.stderr).strip()
304
305 def move(self, targets: dict[int, int], *, speed: int = 250, torque: int = 70, release: bool = False) -> tuple[bool, str]:
306 self._scratch.write_text(json.dumps({"step": {"ids": list(targets), "positions": list(targets.values())}}))
307 args = ["pose", "go", "step", "--file", str(self._scratch), "--speed", str(speed), "--torque", str(torque)] + (["--release"] if release else [])
308 r = so101(*args, check=False)
309 return r.returncode == 0, (r.stdout + r.stderr).strip()
310
311 def grip(self, position: int, *, torque: int = 30) -> None:
312 so101("move", "6", str(position), "--speed", "300", "--torque", str(torque), "--hold")
313
314 def torque(self, on: bool) -> None:
315 so101("torque", "on" if on else "off", check=False)
316
317 def publish_calibration(self, kind: str, data: dict) -> None:
318 pass # the files already live here
319
320
321 class HttpArm:
322 """The arm behind `so101 serve`, usually on the Pi. Same API as CliArm."""
323
324 def __init__(self, base: str):
325 self.base = base.rstrip("/")
326
327 def __str__(self) -> str:
328 return f"the arm at {self.base}"
329
330 def _call(self, method: str, path: str, body: dict | None = None) -> tuple[int, dict]:
331 data = json.dumps(body).encode() if body is not None else None
332 req = urllib.request.Request(self.base + path, data=data, method=method, headers={"content-type": "application/json"})
333 try:
334 with urllib.request.urlopen(req, timeout=120) as r:
335 return r.status, self._json(r.read())
336 except urllib.error.HTTPError as e:
337 return e.code, self._json(e.read()) # the server answers errors as JSON
338 except (urllib.error.URLError, TimeoutError, OSError) as e:
339 raise SystemExit(f"{self.base}: {getattr(e, 'reason', e)}; is `so101 serve` running there?") from None
340
341 @staticmethod
342 def _json(raw: bytes) -> dict:
343 """The server's JSON, or a proxy's or a restarting Pi's HTML turned into an error the callers can show."""
344 try:
345 data = json.loads(raw or b"{}")
346 except ValueError:
347 return {"error": "not a JSON reply: " + raw[:200].decode("utf-8", "replace").strip()}
348 return data if isinstance(data, dict) else {"error": f"unexpected reply: {data!r}"[:200]}
349
350 def limits(self) -> dict[int, tuple[int, int]]:
351 _, d = self._call("GET", "/limits")
352 return {int(k): (v["min"], v["max"]) for k, v in d.items()}
353
354 def positions(self) -> dict[int, int]:
355 _, d = self._call("GET", "/status")
356 return {j["id"]: j["position"] for j in d["joints"] if j["position"] is not None}
357
358 def poses(self) -> dict:
359 return self._call("GET", "/poses")[1]
360
361 def go_pose(self, name: str, *, speed: int = 200, release: bool = False) -> tuple[bool, str]:
362 code, d = self._call("POST", "/pose/go", {"name": name, "speed": speed, "release": release})
363 return code == 200 and bool(d.get("reached")), d.get("error") or f"reached {name}"
364
365 def move(self, targets: dict[int, int], *, speed: int = 250, torque: int = 70, release: bool = False) -> tuple[bool, str]:
366 body = {"targets": {str(k): v for k, v in targets.items()}, "speed": speed, "torque": torque, "release": release}
367 code, d = self._call("POST", "/move", body)
368 return code == 200 and bool(d.get("reached")), d.get("error") or "reached"
369
370 def grip(self, position: int, *, torque: int = 30) -> None:
371 code, d = self._call("POST", "/grip", {"position": position, "torque": torque})
372 if code != 200:
373 raise SystemExit(f"grip refused: {d.get('error')}")
374
375 def torque(self, on: bool) -> None:
376 self._call("POST", "/torque", {"on": on})
377
378 def publish_calibration(self, kind: str, data: dict) -> None:
379 code, d = self._call("PUT", f"/calibration/{kind}", data)
380 if code != 200:
381 raise SystemExit(f"could not publish {kind} calibration: {d.get('error')}")
382
383
384 Arm = CliArm | HttpArm
385
386
387 def arm(poses_file: Path = POSES) -> Arm:
388 """SO101_ARM=http://pi:8101 selects the arm behind `so101 serve`; otherwise the CLI on this machine."""
389 url = os.environ.get("SO101_ARM")
390 return HttpArm(url) if url else CliArm(poses_file)
391
392
393 def servo_ranges_match(ranges: dict[str, JointRange], arm: Arm) -> list[str]:
394 """Joints whose servo limits differ from the calibration file (a stale file would send the arm to the wrong angles)."""
395 bad = []
396 limits = arm.limits()
397 for joint, sid in zip(JOINTS, SERVO_IDS):
398 lo, hi = limits.get(sid, (None, None))
399 if (lo, hi) != (ranges[joint].min, ranges[joint].max):
400 bad.append(f"{joint}: servo {lo}-{hi}, file {ranges[joint].min}-{ranges[joint].max}")
401 return bad
402
403
404 def read_joint_ticks(arm: Arm) -> dict[str, int]:
405 """Current servo positions, by joint name."""
406 positions = arm.positions()
407 missing = [JOINTS[SERVO_IDS.index(sid)] for sid in SERVO_IDS if sid not in positions]
408 if missing:
409 raise SystemExit(f"no position from {', '.join(missing)}")
410 return {JOINTS[SERVO_IDS.index(sid)]: int(positions[sid]) for sid in SERVO_IDS}
411
412
413 def read_joint_degrees(ranges: dict[str, JointRange], arm: Arm) -> np.ndarray:
414 ticks = read_joint_ticks(arm)
415 return np.array([ticks_to_degrees(ticks[j], ranges[j]) for j in JOINTS])
416
417
418 class ArmMove:
419 """Supervised moves to joint targets in degrees, validated against the calibrated ranges first."""
420
421 def __init__(self, ranges: dict[str, JointRange], arm: Arm, speed: int = 250, torque: int = 70):
422 self.ranges = ranges
423 self.arm = arm
424 self.speed = speed
425 self.torque = torque
426
427 def validate(self, degrees: dict[str, float]) -> tuple[list[int], list[int]]:
428 """Joint targets as servo ids and ticks, inside the calibrated range by the supervisor's own 30-tick margin, or a clear error."""
429 ids, positions = [], []
430 for joint, deg in degrees.items():
431 rng = self.ranges[joint]
432 ticks = degrees_to_ticks(deg, rng)
433 if not (rng.min + 30 <= ticks <= rng.max - 30):
434 raise SystemExit(f"{joint} target {deg:.1f}° is {ticks} ticks, outside its usable {rng.min + 30}-{rng.max - 30}")
435 ids.append(SERVO_IDS[JOINTS.index(joint)])
436 positions.append(ticks)
437 return ids, positions
438
439 def go(self, name: str, degrees: dict[str, float], *, release: bool = False) -> tuple[bool, str]:
440 """Returns (reached, message). A jam on the way is reported, not raised, so a gripper closing on an object reads as a grasp."""
441 ids, positions = self.validate(degrees)
442 return self.arm.move(dict(zip(ids, positions)), speed=self.speed, torque=self.torque, release=release)
Software/vision/handeye.py added +162
@@ -0,0 +1,162 @@
1 """Where is the camera relative to the arm? Solved by looking at a marker taped to the gripper from several arm poses.
2
3 Save 6-10 poses by hand first (`so101 pose save he1` ... with the gripper marker facing the camera, spread around the
4 workspace), then:
5
6 python Software/vision/handeye.py --camera 1 --poses he1,he2,he3,he4,he5,he6 --out outputs/handeye.json
7
8 The arm visits each pose by itself. Nothing else to do but keep hands clear.
9 """
10
11 from __future__ import annotations
12
13 import argparse
14 import json
15 import math
16 import time
17 from pathlib import Path
18
19 import cv2
20 import numpy as np
21
22 from common import (
23 GRIPPER_ID,
24 JOINTS,
25 POSES,
26 URDF,
27 Intrinsics,
28 detect,
29 detector,
30 grab,
31 invert,
32 last_line,
33 load_joint_ranges,
34 make_T,
35 median_T,
36 open_camera,
37 read_joint_degrees,
38 servo_ranges_match,
39 arm,
40 camera_source,
41 )
42
43 GRIPPER_MM = 25.0
44
45
46 def solve_hand_eye(T_cam_marker: list[np.ndarray], T_base_gripper: list[np.ndarray]) -> tuple[np.ndarray, np.ndarray]:
47 """Fixed camera, marker on the gripper. Returns (T_base_cam, T_gripper_marker).
48
49 OpenCV's calibrateRobotWorldHandEye solves T_cam'_world' . T_world'_base' = T_cam'_gripper' . T_gripper'_base'.
50 Our relation T_marker_cam . T_cam_base = T_marker_gripper . T_gripper_base has that shape with
51 cam' = marker, world' = cam, base' = base, gripper' = gripper.
52 """
53 R_w2c, t_w2c, R_b2g, t_b2g = [], [], [], []
54 for A, B in zip(T_cam_marker, T_base_gripper):
55 Ai, Bi = invert(A), invert(B) # T_marker_cam, T_gripper_base
56 R_w2c.append(Ai[:3, :3]); t_w2c.append(Ai[:3, 3])
57 R_b2g.append(Bi[:3, :3]); t_b2g.append(Bi[:3, 3])
58 R_b2w, t_b2w, R_g2c, t_g2c = cv2.calibrateRobotWorldHandEye(R_w2c, t_w2c, R_b2g, t_b2g)
59 T_cam_base = make_T(R_b2w, t_b2w) # world'_base' = cam_base
60 T_marker_gripper = make_T(R_g2c, t_g2c) # cam'_gripper' = marker_gripper
61 return invert(T_cam_base), invert(T_marker_gripper)
62
63
64 def residuals(T_base_cam, T_gripper_marker, T_cam_marker, T_base_gripper) -> list[float]:
65 """Per pose: distance in mm between the marker as the camera saw it and where the arm says it was."""
66 out = []
67 for A, B in zip(T_cam_marker, T_base_gripper):
68 seen = (T_base_cam @ A)[:3, 3]
69 told = (B @ T_gripper_marker)[:3, 3]
70 out.append(float(np.linalg.norm(seen - told) * 1000))
71 return out
72
73
74 def orientation_spread_deg(Ts: list[np.ndarray]) -> float:
75 """Largest rotation angle between any two gripper orientations; the solver needs variety here, not just positions."""
76 worst = 0.0
77 for i in range(len(Ts)):
78 for j in range(i + 1, len(Ts)):
79 R = Ts[i][:3, :3].T @ Ts[j][:3, :3]
80 worst = max(worst, math.degrees(math.acos(max(-1.0, min(1.0, (np.trace(R) - 1) / 2)))))
81 return worst
82
83
84 def park(arm) -> None:
85 """Leave the arm somewhere safe before letting go of it."""
86 if "home" in arm.poses():
87 ok, msg = arm.go_pose("home", speed=200, release=True)
88 print(f"home: {last_line(msg)}")
89 else:
90 print("no `home` pose saved; leaving torque on so the arm does not drop (so101 torque off when it is supported)")
91
92
93 def main() -> None:
94 ap = argparse.ArgumentParser()
95 ap.add_argument("--camera", type=camera_source, default=1, help="an OpenCV index, or the ustreamer URL on the Pi")
96 ap.add_argument("--intrinsics", type=Path, default=Path("outputs/camera.json"))
97 ap.add_argument("--poses", required=True, help="comma-separated pose names saved with `so101 pose save`")
98 ap.add_argument("--frames", type=int, default=15, help="frames averaged per pose")
99 ap.add_argument("--poses-file", type=Path, default=POSES, help="where the he* poses were saved (a remote arm keeps its own)")
100 ap.add_argument("--out", type=Path, default=Path("outputs/handeye.json"))
101 ap.add_argument("--publish-as", default="handeye", help="name on the arm's server: handeye for camera 1, handeye2 for camera 2")
102 args = ap.parse_args()
103
104 from lerobot.model.kinematics import RobotKinematics
105
106 intr = Intrinsics.load(args.intrinsics)
107 ranges = load_joint_ranges()
108 the_arm = arm(args.poses_file)
109 mismatch = servo_ranges_match(ranges, the_arm)
110 if mismatch:
111 raise SystemExit("the servos' limits differ from the calibration file; re-run `just calibrate`:\n " + "\n ".join(mismatch))
112 kin = RobotKinematics(str(URDF), "gripper_frame_link")
113 det = detector()
114 cap = open_camera(args.camera, intr.width, intr.height)
115 print(f"using {the_arm}")
116
117 T_cam_marker, T_base_gripper, used = [], [], []
118 for name in args.poses.split(","):
119 print(f"going to {name}…", end=" ", flush=True)
120 ok, msg = the_arm.go_pose(name, speed=200)
121 if not ok:
122 print(f"\n could not reach {name}: {last_line(msg)}")
123 continue
124 time.sleep(0.8)
125 seen = []
126 for _ in range(args.frames):
127 for d in detect(grab(cap), det, intr, {GRIPPER_ID: GRIPPER_MM / 1000}):
128 if d.id == GRIPPER_ID and d.T_cam_marker is not None and d.ambiguity > 1.5:
129 seen.append(d.T_cam_marker)
130 if len(seen) < args.frames // 2:
131 print(f"gripper marker seen clearly in only {len(seen)}/{args.frames} frames, skipping this pose")
132 continue
133 q = read_joint_degrees(ranges, the_arm)
134 T_cam_marker.append(median_T(seen))
135 T_base_gripper.append(kin.forward_kinematics(q))
136 used.append(name)
137 print(f"marker at {np.round(T_cam_marker[-1][:3, 3] * 1000).astype(int)} mm in camera, gripper at {np.round(T_base_gripper[-1][:3, 3] * 1000).astype(int)} mm in base")
138 cap.release()
139 park(the_arm)
140
141 if len(used) < 4:
142 raise SystemExit(f"only {len(used)} usable poses; need at least 4, ideally 6-10 with varied orientations")
143 spread = orientation_spread_deg(T_base_gripper)
144 print(f"orientation spread across poses: {spread:.0f}°")
145 if spread < 30:
146 raise SystemExit("the gripper pointed almost the same way in every pose; the solver cannot separate camera position from marker offset. Save poses that tilt and roll the gripper differently (aim for 60° or more of spread) and run again")
147 T_base_cam, T_gripper_marker = solve_hand_eye(T_cam_marker, T_base_gripper)
148 res = residuals(T_base_cam, T_gripper_marker, T_cam_marker, T_base_gripper)
149 cam = T_base_cam[:3, 3] * 1000
150 print(f"camera sits at x={cam[0]:.0f} y={cam[1]:.0f} z={cam[2]:.0f} mm from the arm base")
151 print(f"residuals per pose: {', '.join(f'{r:.1f}' for r in res)} mm (mean {np.mean(res):.1f}, worst {max(res):.1f})")
152 if np.mean(res) > 12 or max(res) > 25:
153 raise SystemExit("not saved: residuals too large. Under ~8 mm mean is what a correct model gives; this size means a joint direction disagrees between servo and model, or the camera moved. Compare `just vision-view` base coordinates of the gripper marker with a ruler at one held pose")
154 args.out.parent.mkdir(parents=True, exist_ok=True)
155 result = {"T_base_cam": T_base_cam.tolist(), "T_gripper_marker": T_gripper_marker.tolist(), "poses": used, "residuals_mm": res, "orientation_spread_deg": spread}
156 args.out.write_text(json.dumps(result, indent=1))
157 print(f"saved {args.out}")
158 the_arm.publish_calibration(args.publish_as, result)
159
160
161 if __name__ == "__main__":
162 main()
Software/vision/markers.py added +79
@@ -0,0 +1,79 @@
1 """Generate the printables: a ChArUco board for camera calibration, six 40 mm cube markers, one 25 mm gripper marker.
2
3 python Software/vision/markers.py out/ # PNGs at 300 DPI; print at 100 % scale, matte paper
4 """
5
6 from __future__ import annotations
7
8 import sys
9 from pathlib import Path
10
11 import cv2
12 import numpy as np
13
14 from common import CUBE_IDS, DICTIONARY, GRIPPER_ID
15
16 DPI = 300
17 MM = DPI / 25.4 # pixels per millimeter
18
19 BOARD_SQUARES = (7, 5)
20 BOARD_SQUARE_MM = 30.0
21 BOARD_MARKER_MM = 22.0
22 CUBE_MARKER_MM = 40.0
23 GRIPPER_MARKER_MM = 25.0
24
25
26 def charuco_board() -> cv2.aruco.CharucoBoard:
27 dictionary = cv2.aruco.getPredefinedDictionary(DICTIONARY)
28 # Board markers use ids after the cube and gripper ones so nothing collides in a frame.
29 ids = np.arange(20, 20 + (BOARD_SQUARES[0] * BOARD_SQUARES[1]) // 2, dtype=np.int32)
30 return cv2.aruco.CharucoBoard(BOARD_SQUARES, BOARD_SQUARE_MM / 1000, BOARD_MARKER_MM / 1000, dictionary, ids)
31
32
33 def marker_tile(marker_id: int, size_mm: float, label: str) -> np.ndarray:
34 dictionary = cv2.aruco.getPredefinedDictionary(DICTIONARY)
35 px = int(round(size_mm * MM))
36 img = cv2.aruco.generateImageMarker(dictionary, marker_id, px)
37 border = int(round(size_mm * 0.25 * MM)) # white quiet zone: a quarter of the marker on each side
38 tile = cv2.copyMakeBorder(img, border, border + int(8 * MM), border, border, cv2.BORDER_CONSTANT, value=255)
39 cv2.putText(tile, label, (border, tile.shape[0] - int(2 * MM)), cv2.FONT_HERSHEY_SIMPLEX, 0.8, 0, 2)
40 # Cut marks at the corners only: a closed rectangle around the tile is itself a candidate square and confuses detection.
41 h, w = tile.shape
42 tick = int(4 * MM)
43 for (x, y, dx, dy) in ((0, 0, 1, 1), (w - 1, 0, -1, 1), (0, h - 1, 1, -1), (w - 1, h - 1, -1, -1)):
44 cv2.line(tile, (x, y), (x + dx * tick, y), 128, 1)
45 cv2.line(tile, (x, y), (x, y + dy * tick), 128, 1)
46 return tile
47
48
49 def sheet(tiles: list[np.ndarray], columns: int) -> np.ndarray:
50 h = max(t.shape[0] for t in tiles)
51 w = max(t.shape[1] for t in tiles)
52 rows = (len(tiles) + columns - 1) // columns
53 canvas = np.full((rows * h, columns * w), 255, np.uint8)
54 for i, t in enumerate(tiles):
55 r, c = divmod(i, columns)
56 canvas[r * h : r * h + t.shape[0], c * w : c * w + t.shape[1]] = t
57 return canvas
58
59
60 def main(out: Path) -> None:
61 out.mkdir(parents=True, exist_ok=True)
62 board = charuco_board()
63 margin = int(10 * MM) # generateImage fits the squares inside the margins, so add them to the canvas
64 board_px = (int(BOARD_SQUARES[0] * BOARD_SQUARE_MM * MM) + 2 * margin, int(BOARD_SQUARES[1] * BOARD_SQUARE_MM * MM) + 2 * margin)
65 board_img = board.generateImage(board_px, marginSize=margin)
66 cv2.imwrite(str(out / "charuco_board.png"), board_img)
67
68 cube = [marker_tile(i, CUBE_MARKER_MM, f"cube {i} {CUBE_MARKER_MM:.0f}mm") for i in CUBE_IDS]
69 cv2.imwrite(str(out / "cube_markers.png"), sheet(cube, 3))
70 cv2.imwrite(str(out / "gripper_marker.png"), sheet([marker_tile(GRIPPER_ID, GRIPPER_MARKER_MM, f"gripper {GRIPPER_ID} {GRIPPER_MARKER_MM:.0f}mm")], 1))
71
72 for name in ("charuco_board", "cube_markers", "gripper_marker"):
73 img = cv2.imread(str(out / f"{name}.png"), cv2.IMREAD_GRAYSCALE)
74 print(f"{name}.png {img.shape[1] / MM:.0f} x {img.shape[0] / MM:.0f} mm at {DPI} DPI")
75 print(f"print at 100 % scale; check with a ruler: a cube marker's black square is {CUBE_MARKER_MM:.0f} mm, a board square {BOARD_SQUARE_MM:.0f} mm")
76
77
78 if __name__ == "__main__":
79 main(Path(sys.argv[1] if len(sys.argv) > 1 else "printables"))
Software/vision/pick.py added +235
@@ -0,0 +1,235 @@
1 """Find the marked cube, pick it up, put it at the saved `drop` pose.
2
3 python Software/vision/pick.py --camera 1 [--dry-run] [--cube-mm 40]
4
5 Needs outputs/camera.json, outputs/handeye.json, and two poses saved from the repo root with the arm only
6 (`so101 pose save drop --ids 1-5` over the bin, `so101 pose save home --ids 1-5` at rest), plus a cube wearing
7 the 40 mm markers in the camera's view. Every move is a supervised `so101 pose go`; a jam stops everything.
8 """
9
10 from __future__ import annotations
11
12 import argparse
13 import json
14 import math
15 from pathlib import Path
16
17 import numpy as np
18
19 from common import (
20 CUBE_IDS,
21 JOINTS,
22 POSES,
23 URDF,
24 ArmMove,
25 Intrinsics,
26 degrees_to_ticks,
27 detect,
28 detector,
29 grab,
30 last_line,
31 load_joint_ranges,
32 make_T,
33 median_T,
34 open_camera,
35 read_joint_degrees,
36 read_joint_ticks,
37 rotation_about_z,
38 servo_ranges_match,
39 arm,
40 camera_source,
41 ticks_to_degrees,
42 )
43
44 MARKER_MM = 40.0
45 APPROACH_HEIGHTS_M = (0.03, 0.025, 0.02) # tried in order: a vertical gripper cannot reach much higher than 30 mm over the table
46 GRASP_DEPTH_M = 0.005 # how far below the cube's top face the jaws aim
47 IK_ITERATIONS = 40
48 IK_TOLERANCE_MM = 8
49 SQUEEZE_TORQUE = 30 # percent, for holding the cube after the jaws meet it
50
51 # The arm's natural "pointing down" orientation at pan 0: frame x = -X, y = +Y, z = -Z of the base.
52 # Any other roll costs wrist_roll range it does not have.
53 DOWN = np.diag([-1.0, 1.0, -1.0])
54
55 # A mid-workspace grasp solution; a good second guess when the current pose seeds the solver into a corner.
56 READY = np.array([0.0, -30.0, 47.0, 73.0, 0.0, 0.0])
57
58
59 def find_cube(cap, det, intr, marker_m: float, cube_m: float) -> tuple[np.ndarray, int] | None:
60 """Cube center pose in the camera frame from the face that looks most squarely at the camera."""
61 seen: dict[int, list[np.ndarray]] = {}
62 facing: dict[int, list[float]] = {}
63 for _ in range(12):
64 for d in detect(grab(cap), det, intr, {i: marker_m for i in CUBE_IDS}):
65 if d.id in CUBE_IDS and d.T_cam_marker is not None and d.ambiguity > 1.5:
66 seen.setdefault(d.id, []).append(d.T_cam_marker)
67 facing.setdefault(d.id, []).append(-d.T_cam_marker[2, 2]) # marker z against camera z: 1 = square-on
68 usable = {i: p for i, p in seen.items() if len(p) >= 5}
69 if not usable:
70 return None
71 face_id = max(usable, key=lambda i: float(np.mean(facing[i])))
72 T_cam_face = median_T(usable[face_id])
73 # The face marker's z points out of the cube; the center is half a cube behind it.
74 return T_cam_face @ make_T(np.eye(3), [0, 0, -cube_m / 2]), face_id
75
76
77 def within_ranges(q: np.ndarray, ranges) -> list[str]:
78 """Joints whose IK angle falls outside the calibrated servo range (the URDF's limits are wider than the servo's)."""
79 bad = []
80 for i, joint in enumerate(JOINTS[:5]):
81 t = degrees_to_ticks(float(q[i]), ranges[joint])
82 if not (ranges[joint].min + 30 <= t <= ranges[joint].max - 30):
83 bad.append(f"{joint}={q[i]:.0f}°")
84 return bad
85
86
87 def solve(kin, q_start: np.ndarray, T_target: np.ndarray, ranges=None) -> np.ndarray:
88 """Iterate LeRobot's one-step IK to convergence from a few seeds; raises if the target is out of reach."""
89 best, best_err = None, float("inf")
90 for seed in (q_start, READY, np.zeros(6)):
91 q = np.array(seed, dtype=float)
92 for _ in range(IK_ITERATIONS):
93 q = kin.inverse_kinematics(q, T_target, position_weight=1.0, orientation_weight=0.05)
94 err = np.linalg.norm(kin.forward_kinematics(q)[:3, 3] - T_target[:3, 3]) * 1000
95 if err <= IK_TOLERANCE_MM and (ranges is None or not within_ranges(q, ranges)):
96 return q
97 if err < best_err:
98 best, best_err = q, err
99 if best_err > IK_TOLERANCE_MM:
100 raise ValueError(f"off by {best_err:.0f} mm")
101 return best
102
103
104 def fold_yaw(yaw: float) -> float:
105 """A cube looks the same every 90°; pick the equivalent yaw closest to zero so the wrist barely rolls."""
106 return (yaw + math.pi / 4) % (math.pi / 2) - math.pi / 4
107
108
109 def main() -> None:
110 ap = argparse.ArgumentParser()
111 ap.add_argument("--camera", type=camera_source, default=1, help="an OpenCV index, or the ustreamer URL on the Pi")
112 ap.add_argument("--intrinsics", type=Path, default=Path("outputs/camera.json"))
113 ap.add_argument("--handeye", type=Path, default=Path("outputs/handeye.json"))
114 ap.add_argument("--poses-file", type=Path, default=POSES, help="where `drop` and `home` were saved")
115 ap.add_argument("--cube-mm", type=float, default=40.0, help="width of the object the markers are on")
116 ap.add_argument("--dry-run", action="store_true", help="see the cube and plan every move, move nothing")
117 args = ap.parse_args()
118
119 from lerobot.model.kinematics import RobotKinematics
120
121 the_arm = arm(args.poses_file)
122 poses = the_arm.poses()
123 for needed in ("drop", "home"):
124 if needed not in poses:
125 raise SystemExit(f"no `{needed}` pose on {the_arm}: save it from the arm's web page without the gripper, or `so101 pose save {needed} --ids 1-5`")
126 if 6 in poses[needed]["ids"]:
127 raise SystemExit(f"`{needed}` was saved with the gripper; save it again without the gripper (web page) or `so101 pose save {needed} --ids 1-5`")
128 intr = Intrinsics.load(args.intrinsics)
129 handeye = json.loads(args.handeye.read_text())
130 T_base_cam = np.array(handeye["T_base_cam"])
131 ranges = load_joint_ranges()
132 mismatch = servo_ranges_match(ranges, the_arm)
133 if mismatch:
134 raise SystemExit("the servos' limits differ from the calibration file; re-run `just calibrate`:\n " + "\n ".join(mismatch))
135 kin = RobotKinematics(str(URDF), "gripper_frame_link")
136 det = detector()
137 cube_m = args.cube_mm / 1000
138 jaw_offset_m = cube_m / 2 # the gripper frame sits on the fixed jaw's inner face; the cube is centered half a cube toward -x
139
140 cap = open_camera(args.camera, intr.width, intr.height)
141 found = find_cube(cap, det, intr, MARKER_MM / 1000, cube_m)
142 cap.release()
143 if found is None:
144 raise SystemExit("no cube marker seen clearly; check `just vision-view`")
145 T_cam_cube, face = found
146 T_base_cube = T_base_cam @ T_cam_cube
147 c = T_base_cube[:3, 3]
148 yaw = fold_yaw(math.atan2(T_base_cube[1, 0], T_base_cube[0, 0]))
149 print(f"cube (face {face}) at x={c[0]*1000:.0f} y={c[1]*1000:.0f} z={c[2]*1000:.0f} mm from the base; jaws will roll {math.degrees(yaw):.0f}°")
150 if not (-0.02 < c[2] < 0.12):
151 raise SystemExit(f"a cube on the table cannot be {c[2]*1000:.0f} mm above the base; the hand-eye calibration is off")
152
153 R = rotation_about_z(yaw) @ DOWN
154 frame_at_cube = c + R[:, 0] * jaw_offset_m
155 T_grasp = make_T(R, frame_at_cube + [0, 0, cube_m / 2 - GRASP_DEPTH_M])
156 q_now = read_joint_degrees(ranges)
157 try:
158 q_grasp = solve(kin, q_now, T_grasp, ranges)
159 except ValueError as e:
160 raise SystemExit(f"the grasp pose is out of reach ({e}); move the cube into the zone in QUEST.md") from None
161 if bad := within_ranges(q_grasp, ranges):
162 raise SystemExit(f"grasp needs {', '.join(bad)}, outside the calibrated range; move the cube closer")
163 q_pre, approach = None, None
164 for h in APPROACH_HEIGHTS_M:
165 try:
166 q = solve(kin, q_grasp, make_T(R, T_grasp[:3, 3] + [0, 0, h]), ranges)
167 except ValueError:
168 continue
169 if not within_ranges(q, ranges):
170 q_pre, approach = q, h
171 break
172 if q_pre is None:
173 raise SystemExit("no reachable approach pose above the cube; move the cube closer to the arm")
174
175 def arm_joints(q: np.ndarray) -> dict[str, float]:
176 return {j: float(q[i]) for i, j in enumerate(JOINTS[:5])}
177
178 def saved_joints(name: str) -> dict[str, float]:
179 p = poses[name]
180 return {JOINTS[sid - 1]: ticks_to_degrees(t, ranges[JOINTS[sid - 1]]) for sid, t in zip(p["ids"], p["positions"])}
181
182 g = ranges["gripper"]
183 gripper_open, gripper_closed = g.max - 60, g.min + 40
184 plan = [
185 ("open", {"gripper": ticks_to_degrees(gripper_open, g)}),
186 ("pre-grasp", arm_joints(q_pre)),
187 ("grasp", arm_joints(q_grasp)),
188 ("close", {"gripper": ticks_to_degrees(gripper_closed, g)}),
189 ("lift", arm_joints(q_pre)),
190 ("drop", saved_joints("drop")),
191 ("release", {"gripper": ticks_to_degrees(gripper_open, g)}),
192 ("home", saved_joints("home")),
193 ]
194 print(f"plan (approach from {approach*1000:.0f} mm above the grasp):")
195 for label, joints in plan:
196 print(f" {label:9s} " + " ".join(f"{j}={v:.0f}°" for j, v in joints.items()))
197 mover = ArmMove(ranges, the_arm, speed=200, torque=70)
198 for label, joints in plan:
199 mover.validate(joints) # every step within the calibrated ranges, before anything moves
200 if args.dry_run:
201 print("dry run: nothing moved")
202 return
203
204 def step(label: str, joints: dict[str, float]) -> None:
205 ok, msg = mover.go(label, joints)
206 print(f"{label}: {last_line(msg)}")
207 if not ok:
208 raise SystemExit(f"stopped at {label}; the arm is holding where it is (so101 torque off to release)")
209
210 step("open", plan[0][1])
211 step("pre-grasp", plan[1][1])
212 step("grasp", plan[2][1])
213
214 ok, msg = mover.go("close", plan[3][1])
215 print(f"close: {last_line(msg)}")
216 ticks = read_joint_ticks(the_arm)["gripper"]
217 grasped = (not ok) and "jammed" in msg and gripper_closed + 60 < ticks < gripper_open - 60
218 if ok or not grasped:
219 why = "the jaws closed on nothing" if ok else f"the close stopped for another reason at {ticks} ticks"
220 raise SystemExit(f"no grasp: {why}; holding at the grasp pose (so101 torque off to release)")
221 print(f"grasped: jaws met the cube at {ticks} ticks")
222 # A jam parks the goal at the present position, which holds the cube with almost no force. Squeeze gently
223 # toward closed at a low torque cap; the servo's own overload protection then holds at a steady grip.
224 the_arm.grip(gripper_closed, torque=SQUEEZE_TORQUE)
225
226 step("lift", plan[4][1])
227 step("drop", plan[5][1])
228 step("release", plan[6][1])
229 step("home", plan[7][1])
230 the_arm.torque(False)
231 print("done")
232
233
234 if __name__ == "__main__":
235 main()
Software/vision/printables/charuco_board.png added
Binary file not shown.
Software/vision/printables/cube_markers.png added
Binary file not shown.
Software/vision/printables/gripper_marker.png added
Binary file not shown.
Software/vision/test_vision.py added +285
@@ -0,0 +1,285 @@
1 """Offline checks for the vision math; none of these touch a camera or the arm. Run: pytest Software/vision"""
2
3 from __future__ import annotations
4
5 import json
6 from pathlib import Path
7
8 import cv2
9 import numpy as np
10 import pytest
11
12 from common import (
13 JointRange,
14 Intrinsics,
15 average_T,
16 degrees_to_ticks,
17 detect,
18 detector,
19 invert,
20 make_T,
21 marker_object_points,
22 rodrigues_T,
23 rotation_about_z,
24 ticks_to_degrees,
25 )
26 from common import ArmMove, CliArm, HttpArm, JOINTS, arm, camera_source, grab, open_camera, read_joint_ticks
27 from handeye import residuals, solve_hand_eye
28 from markers import charuco_board, marker_tile
29
30 rng = np.random.default_rng(7)
31
32
33 def random_T(scale: float = 0.3) -> np.ndarray:
34 R, _ = cv2.Rodrigues(rng.normal(size=3))
35 return make_T(R, rng.normal(size=3) * scale)
36
37
38 def test_transform_algebra():
39 T = random_T()
40 assert np.allclose(T @ invert(T), np.eye(4), atol=1e-12)
41 assert np.allclose(rotation_about_z(np.pi / 2) @ np.array([1, 0, 0]), [0, 1, 0], atol=1e-12)
42 Ts = [T @ make_T(cv2.Rodrigues(rng.normal(size=3) * 0.01)[0], rng.normal(size=3) * 0.001) for _ in range(20)]
43 assert np.linalg.norm(average_T(Ts)[:3, 3] - T[:3, 3]) < 0.002
44
45
46 def test_servo_units_follow_lerobot():
47 r = JointRange(813, 3202) # the follower's shoulder lift
48 assert ticks_to_degrees(r.mid, r) == 0.0
49 assert degrees_to_ticks(0.0, r) == round(r.mid)
50 assert abs(ticks_to_degrees(r.mid + 4095 / 4, r) - 90.0) < 1e-9
51 assert degrees_to_ticks(ticks_to_degrees(1234, r), r) == 1234
52
53
54 def test_marker_round_trip_through_a_synthetic_camera():
55 """Generate a marker, detect it, and recover a known pose with solvePnP through made-up intrinsics."""
56 K = np.array([[900.0, 0, 640], [0, 900.0, 360], [0, 0, 1]])
57 intr = Intrinsics(1280, 720, K, np.zeros(5))
58 size = 0.04
59 # A marker the camera can read faces it: its z axis points back at the lens, so start from a half-turn about x.
60 facing = cv2.Rodrigues(np.array([np.pi, 0.0, 0.0]))[0]
61 T_true = make_T(facing @ cv2.Rodrigues(np.array([0.2, -0.3, 0.1]))[0], [0.05, -0.02, 0.5])
62 # Render: warp the marker image onto the projected corners.
63 tile = cv2.aruco.generateImageMarker(cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_50), 3, 200)
64 tile = cv2.copyMakeBorder(tile, 50, 50, 50, 50, cv2.BORDER_CONSTANT, value=255)
65 obj = marker_object_points(size)
66 quiet = obj * 1.5 # the white border's corners, so the warp includes the quiet zone
67 img_pts, _ = cv2.projectPoints(quiet, cv2.Rodrigues(T_true[:3, :3])[0], T_true[:3, 3], K, None)
68 src = np.float32([[0, 0], [300, 0], [300, 300], [0, 300]])
69 H = cv2.getPerspectiveTransform(src, img_pts.reshape(4, 2).astype(np.float32))
70 frame = cv2.warpPerspective(tile, H, (1280, 720), borderValue=255)
71 frame = cv2.cvtColor(frame, cv2.COLOR_GRAY2BGR)
72 found = detect(frame, detector(), intr, size)
73 assert [d.id for d in found] == [3]
74 T = found[0].T_cam_marker
75 assert np.linalg.norm(T[:3, 3] - T_true[:3, 3]) < 0.003, "position within 3 mm at half a meter"
76 assert np.allclose(T[:3, :3], T_true[:3, :3], atol=0.03)
77
78
79 def test_hand_eye_convention_on_synthetic_data():
80 """The one that matters: with a known camera placement and marker offset, the solver must give them back."""
81 T_base_cam_true = make_T(cv2.Rodrigues(np.array([2.0, 0.3, -0.4]))[0], [0.35, -0.25, 0.45])
82 T_gripper_marker_true = make_T(cv2.Rodrigues(np.array([0.1, 1.5, 0.0]))[0], [0.02, 0.0, 0.03])
83 T_cam_marker, T_base_gripper = [], []
84 for _ in range(8):
85 B = make_T(cv2.Rodrigues(rng.normal(size=3) * 0.8)[0], [0.25 + rng.normal() * 0.05, rng.normal() * 0.1, 0.15 + rng.normal() * 0.05])
86 A = invert(T_base_cam_true) @ B @ T_gripper_marker_true
87 # a little measurement noise
88 A = A @ make_T(cv2.Rodrigues(rng.normal(size=3) * 0.003)[0], rng.normal(size=3) * 0.0005)
89 T_cam_marker.append(A)
90 T_base_gripper.append(B)
91 T_base_cam, T_gripper_marker = solve_hand_eye(T_cam_marker, T_base_gripper)
92 assert np.linalg.norm(T_base_cam[:3, 3] - T_base_cam_true[:3, 3]) < 0.005
93 assert np.allclose(T_base_cam[:3, :3], T_base_cam_true[:3, :3], atol=0.02)
94 assert np.linalg.norm(T_gripper_marker[:3, 3] - T_gripper_marker_true[:3, 3]) < 0.005
95 assert max(residuals(T_base_cam, T_gripper_marker, T_cam_marker, T_base_gripper)) < 5.0
96
97
98 def test_charuco_board_renders_and_is_detected():
99 board = charuco_board()
100 img = board.generateImage((1400, 1000), marginSize=60)
101 corners, ids, _, _ = cv2.aruco.CharucoDetector(board).detectBoard(img)
102 assert ids is not None and len(ids) >= 20
103
104
105 def test_marker_tiles_detect_with_their_ids():
106 for mid in (0, 5, 10):
107 tile = marker_tile(mid, 40.0, "x")
108 corners, ids, _ = detector().detectMarkers(tile)
109 assert ids is not None and ids.ravel().tolist() == [mid]
110
111
112 def test_kinematics_round_trip_on_the_repo_urdf():
113 pytest.importorskip("placo")
114 from lerobot.model.kinematics import RobotKinematics
115 from common import URDF
116 from pick import solve
117
118 kin = RobotKinematics(str(URDF), "gripper_frame_link")
119 q_true = np.array([20.0, -35.0, 55.0, 25.0, 10.0, 0.0])
120 T = kin.forward_kinematics(q_true)
121 q = solve(kin, np.zeros(6), T)
122 assert np.linalg.norm(kin.forward_kinematics(q)[:3, 3] - T[:3, 3]) < 0.002
123
124
125 def test_pick_helpers():
126 from pick import fold_yaw, within_ranges, DOWN
127 import math
128 assert abs(fold_yaw(math.radians(100)) - math.radians(10)) < 1e-9
129 assert abs(fold_yaw(math.radians(-50)) - math.radians(40)) < 1e-9
130 assert abs(fold_yaw(math.radians(44)) - math.radians(44)) < 1e-9
131 # DOWN is a proper rotation whose z points down
132 assert np.allclose(DOWN @ DOWN.T, np.eye(3)) and np.isclose(np.linalg.det(DOWN), 1.0) and DOWN[2, 2] == -1
133 ranges = {j: JointRange(800, 3300) for j in ("shoulder_pan", "shoulder_lift", "elbow_flex", "wrist_flex", "wrist_roll", "gripper")}
134 assert within_ranges(np.zeros(6), ranges) == []
135 assert within_ranges(np.array([0, 0, 0, 120.0, 0, 0]), ranges) == ["wrist_flex=120°"]
136
137
138 def test_hand_eye_orientation_spread_and_tilt_skew():
139 from handeye import orientation_spread_deg
140 from calibrate_camera import skew_deg
141 I = np.eye(4)
142 R45 = make_T(cv2.Rodrigues(np.array([0, np.radians(45), 0]))[0], [0, 0, 0])
143 assert orientation_spread_deg([I, I]) < 1e-6
144 assert abs(orientation_spread_deg([I, R45, I]) - 45) < 1e-6
145 square = np.array([[100, 100], [500, 100], [500, 400], [100, 400]], dtype=float)
146 assert skew_deg(square) < 1e-6
147 trapezoid = np.array([[150, 100], [450, 100], [500, 400], [100, 400]], dtype=float)
148 assert skew_deg(trapezoid) > 10
149
150
151 # ---------------------------------------------------------------- the remote arm and camera
152 def _stub_pi():
153 """Enough of `so101 serve` and ustreamer to exercise the bridges: answers like the real ones, logs every request."""
154 import http.server
155 import threading
156
157 log = []
158 jpeg = cv2.imencode(".jpg", np.full((36, 64, 3), 128, np.uint8))[1].tobytes()
159
160 class Handler(http.server.BaseHTTPRequestHandler):
161 def log_message(self, *_):
162 pass
163
164 def _send(self, code, body, ctype="application/json"):
165 data = body if isinstance(body, bytes) else json.dumps(body).encode()
166 self.send_response(code)
167 self.send_header("content-type", ctype)
168 self.send_header("content-length", str(len(data)))
169 self.end_headers()
170 self.wfile.write(data)
171
172 def do_GET(self):
173 log.append(("GET", self.path, None))
174 if self.path == "/limits":
175 self._send(200, {"1": {"min": 818, "max": 3487}, "6": {"min": 1995, "max": 3525}})
176 elif self.path == "/status":
177 self._send(200, {"joints": [{"id": i, "position": 2000 + i} for i in range(1, 6)] + [{"id": 6, "position": None}]})
178 elif self.path == "/poses":
179 self._send(200, {"home": {"ids": [1, 2, 3, 4, 5], "positions": [1, 2, 3, 4, 5]}})
180 elif self.path == "/snapshot":
181 self._send(200, jpeg, "image/jpeg")
182 elif self.path == "/html":
183 self._send(502, b"<html>Bad Gateway</html>", "text/html")
184 else:
185 self._send(404, {"error": "no such route"})
186
187 def _body(self):
188 return json.loads(self.rfile.read(int(self.headers["content-length"])))
189
190 def do_POST(self):
191 body = self._body()
192 log.append(("POST", self.path, body))
193 if self.path == "/pose/go":
194 self._send(200, {"reached": True}) if body["name"] == "home" else self._send(404, {"error": 'no pose "x"'})
195 elif self.path == "/move":
196 if "6" in body["targets"]:
197 self._send(409, {"reached": False, "error": "gripper jammed; stopped everything where it is", "stalled": [6]})
198 else:
199 self._send(200, {"reached": True, "final": body["targets"]})
200 else:
201 self._send(200, {})
202
203 def do_PUT(self):
204 body = self._body()
205 log.append(("PUT", self.path, body))
206 self._send(200, {"stored": self.path.rsplit("/", 1)[1]})
207
208 server = http.server.ThreadingHTTPServer(("127.0.0.1", 0), Handler)
209 threading.Thread(target=server.serve_forever, daemon=True).start()
210 return server, f"http://127.0.0.1:{server.server_address[1]}", log
211
212
213 def test_camera_source_is_an_index_or_a_stream_url():
214 assert camera_source("1") == 1
215 assert camera_source("http://pi:8080/stream") == "http://pi:8080/stream"
216
217
218 def test_arm_selection_follows_the_environment(monkeypatch):
219 monkeypatch.delenv("SO101_ARM", raising=False)
220 assert isinstance(arm(), CliArm)
221 monkeypatch.setenv("SO101_ARM", "http://pi:8101/")
222 remote = arm()
223 assert isinstance(remote, HttpArm) and remote.base == "http://pi:8101"
224
225
226 def test_http_arm_speaks_the_serve_api():
227 server, base, log = _stub_pi()
228 try:
229 a = HttpArm(base)
230 assert a.limits() == {1: (818, 3487), 6: (1995, 3525)}
231 assert a.positions() == {1: 2001, 2: 2002, 3: 2003, 4: 2004, 5: 2005}
232 with pytest.raises(SystemExit, match="gripper"):
233 read_joint_ticks(a) # the gripper gave no position
234 assert a.poses() == {"home": {"ids": [1, 2, 3, 4, 5], "positions": [1, 2, 3, 4, 5]}}
235 assert a.go_pose("home", speed=150) == (True, "reached home")
236 ok, msg = a.go_pose("x")
237 assert not ok and "no pose" in msg
238 assert a.move({1: 2100}, speed=200, torque=70) == (True, "reached")
239 ok, msg = a.move({6: 2000})
240 assert not ok and "jammed" in msg # the pick reads a jam as a grasp
241 a.grip(2010, torque=30)
242 a.torque(False)
243 a.publish_calibration("camera", {"rms": 0.3})
244 assert ("POST", "/pose/go", {"name": "home", "speed": 150, "release": False}) in log
245 assert ("POST", "/move", {"targets": {"1": 2100}, "speed": 200, "torque": 70, "release": False}) in log
246 assert ("POST", "/grip", {"position": 2010, "torque": 30}) in log
247 assert ("POST", "/torque", {"on": False}) in log
248 assert ("PUT", "/calibration/camera", {"rms": 0.3}) in log
249
250 ranges = {j: JointRange(800, 3300) for j in JOINTS}
251 mover = ArmMove(ranges, a, speed=200, torque=70)
252 ok, _ = mover.go("step", {"shoulder_pan": 0.0})
253 assert ok
254 assert log[-1] == ("POST", "/move", {"targets": {"1": 2050}, "speed": 200, "torque": 70, "release": False})
255 with pytest.raises(SystemExit, match="outside"):
256 mover.go("bad", {"shoulder_pan": 200.0})
257 finally:
258 server.shutdown()
259
260
261 def test_http_camera_reads_snapshots_from_ustreamer():
262 server, base, _ = _stub_pi()
263 try:
264 cap = open_camera(base + "/stream", 64, 36)
265 assert grab(cap).shape == (36, 64, 3)
266 with pytest.raises(SystemExit, match="--resolution=1280x720"):
267 open_camera(base + "/stream", 1280, 720)
268 finally:
269 server.shutdown()
270
271
272 def test_http_camera_normalizes_the_stream_url():
273 from common import HttpCamera
274
275 for url in ("http://pi:8080/stream", "http://pi:8080/stream/", "http://pi:8080/stream?x=1", "http://pi:8080/snapshot"):
276 assert HttpCamera(url).url == "http://pi:8080/snapshot", url
277
278
279 def test_http_arm_turns_a_non_json_reply_into_an_error():
280 server, base, _ = _stub_pi()
281 try:
282 code, d = HttpArm(base)._call("GET", "/html")
283 assert code == 502 and d["error"].startswith("not a JSON reply: <html>")
284 finally:
285 server.shutdown()
Software/vision/view.py added +62
@@ -0,0 +1,62 @@
1 """Live view of what the camera sees and which markers it recognizes, with poses in millimeters.
2
3 python Software/vision/view.py --camera 1 [--intrinsics outputs/camera.json] [--handeye outputs/handeye.json]
4 """
5
6 from __future__ import annotations
7
8 import argparse
9 import json
10 from pathlib import Path
11
12 import numpy as np
13 import rerun as rr
14
15 from common import CUBE_IDS, GRIPPER_ID, Intrinsics, camera_source, detect, detector, grab, open_camera
16
17 SIZES = {**{i: 0.040 for i in CUBE_IDS}, GRIPPER_ID: 0.025}
18
19
20
21 def main() -> None:
22 ap = argparse.ArgumentParser()
23 ap.add_argument("--camera", type=camera_source, default=1, help="an OpenCV index, or the ustreamer URL on the Pi")
24 ap.add_argument("--intrinsics", type=Path, default=Path("outputs/camera.json"))
25 ap.add_argument("--handeye", type=Path, default=Path("outputs/handeye.json"))
26 args = ap.parse_args()
27
28 intr = Intrinsics.load(args.intrinsics) if args.intrinsics.exists() else None
29 T_base_cam = np.array(json.loads(args.handeye.read_text())["T_base_cam"]) if args.handeye.exists() else None
30 cap = open_camera(args.camera, intr.width if intr else 1280, intr.height if intr else 720)
31 det = detector()
32 rr.init("so101 vision", spawn=True)
33 print("viewer open; ctrl-c to stop")
34 try:
35 while True:
36 frame = grab(cap)
37 rr.log("camera/image", rr.Image(frame[..., ::-1]))
38 strips, labels, lines = [], [], []
39 for d in sorted(detect(frame, det, intr, SIZES), key=lambda d: d.id):
40 label = f"id {d.id}"
41 T = d.T_cam_marker
42 if T is not None:
43 t = T[:3, 3] * 1000
44 label += f" cam {t[0]:.0f},{t[1]:.0f},{t[2]:.0f} mm"
45 if T_base_cam is not None:
46 b = (T_base_cam @ T)[:3, 3] * 1000
47 label += f" base {b[0]:.0f},{b[1]:.0f},{b[2]:.0f} mm"
48 if d.ambiguity < 1.5:
49 label += " (pose unsure: tilt the marker a little)"
50 strips.append(np.vstack([d.corners, d.corners[:1]]))
51 labels.append(label)
52 lines.append(label)
53 rr.log("camera/markers", rr.LineStrips2D(strips, labels=labels, colors=[0, 255, 0]) if strips else rr.Clear(recursive=False))
54 print("\r" + (" | ".join(lines) if lines else "no markers") + " " * 20, end="", flush=True)
55 except KeyboardInterrupt:
56 print()
57 finally:
58 cap.release()
59
60
61 if __name__ == "__main__":
62 main()
flake.lock added +27
@@ -0,0 +1,27 @@
1 {
2 "nodes": {
3 "nixpkgs": {
4 "locked": {
5 "lastModified": 1791487292,
6 "narHash": "sha256-tqM7xDEsaeK1p6VvRygeZ9LkQRpUUkbRT02BHv6hnTM=",
7 "owner": "NixOS",
8 "repo": "nixpkgs",
9 "rev": "8edc0c72e3a38faf5434e40d1f19431125fe4b30",
10 "type": "github"
11 },
12 "original": {
13 "owner": "NixOS",
14 "ref": "nixpkgs-unstable",
15 "repo": "nixpkgs",
16 "type": "github"
17 }
18 },
19 "root": {
20 "inputs": {
21 "nixpkgs": "nixpkgs"
22 }
23 }
24 },
25 "root": "root",
26 "version": 7
27 }
flake.nix added +59
@@ -0,0 +1,59 @@
1 {
2 description = "SO-101 arm — LeRobot + Feetech bus tooling";
3
4 inputs.nixpkgs.url = "github:NixOS/nixpkgs/nixpkgs-unstable";
5
6 outputs =
7 { nixpkgs, ... }:
8 let
9 systems = [
10 "aarch64-darwin"
11 "x86_64-darwin"
12 "aarch64-linux"
13 "x86_64-linux"
14 ];
15 forAll = f: nixpkgs.lib.genAttrs systems (s: f nixpkgs.legacyPackages.${s});
16 in
17 {
18 devShells = forAll (
19 pkgs:
20 let
21 python = pkgs.python312;
22 in
23 {
24 default = pkgs.mkShell {
25 packages = with pkgs; [
26 python
27 uv
28 bun
29 nodejs_24
30 ffmpeg_7
31 just
32 jq
33 ];
34
35 # LeRobot ships as PyPI wheels (torch, opencv, feetech-servo-sdk),
36 # so the Python side lives in a uv-managed .venv pinned by uv.lock
37 # rather than in nixpkgs. The hook keeps that venv in sync with the
38 # lock on every shell entry; first entry downloads ~400 MB of wheels.
39 shellHook = ''
40 export UV_PYTHON=${python}/bin/python3
41 export UV_PYTHON_DOWNLOADS=never
42 ROOT=$(git -C "$PWD" rev-parse --show-toplevel 2>/dev/null || echo "$PWD")
43 if ! uv sync --locked --quiet --project "$ROOT"; then
44 echo "› uv sync failed — run 'uv sync' in $ROOT to see why"
45 fi
46 if ! bun install --frozen-lockfile --silent --cwd "$ROOT/Software/so101"; then
47 echo "› bun install failed — run 'bun install' in $ROOT/Software/so101 to see why"
48 fi
49 export VIRTUAL_ENV="$ROOT/.venv"
50 export PATH="$VIRTUAL_ENV/bin:$ROOT/Software/so101/bin:$PATH"
51 # TorchCodec (LeRobot's video codec) dlopens FFmpeg 7's libraries by name; the fallback path
52 # lets it find the nix ones without touching anything that resolves its libraries normally.
53 export DYLD_FALLBACK_LIBRARY_PATH="${pkgs.ffmpeg_7.lib}/lib''${DYLD_FALLBACK_LIBRARY_PATH:+:$DYLD_FALLBACK_LIBRARY_PATH}"
54 '';
55 };
56 }
57 );
58 };
59 }
forge.json added +8
@@ -0,0 +1,8 @@
1 {
2 "app": {
3 "install": "bun install --cwd Software/so101 --frozen-lockfile",
4 "run": "bun run Software/so101/bench/fake-serve.ts",
5 "test": "bun --cwd Software/so101 test",
6 "health": "/status"
7 }
8 }
justfile added +101
@@ -0,0 +1,101 @@
1 # SO-101 workbench. `just` lists recipes; the dev shell (direnv) provides the tools.
2 # Set SO101_PORT to the servo board's port once (`just ports` shows it) and every
3 # recipe below picks it up.
4
5 set shell := ["bash", "-euo", "pipefail", "-c"]
6
7 default:
8 @just --list --unsorted
9
10 # ── Feetech bus, directly (Software/so101) ──────────────────────────────
11
12 # List serial ports; * marks USB adapters
13 ports:
14 so101 ports
15
16 # Ping every id on the bus and show what answers
17 scan *args:
18 so101 scan {{ args }}
19
20 # One-shot telemetry for ids 1-6
21 status *args:
22 so101 status {{ args }}
23
24 # Live positions — move the arm by hand
25 watch *args:
26 so101 watch {{ args }}
27
28 # Release every servo so the arm can be posed by hand
29 release:
30 so101 torque off
31
32 # Re-address the single connected servo: `just set-id 1 3`
33 set-id current new:
34 so101 set-id {{ current }} {{ new }}
35
36 # ── LeRobot flow (https://huggingface.co/docs/lerobot/so101) ────────────
37
38 # Interactive: unplug/replug the board to learn its port
39 find-port:
40 lerobot-find-port
41
42 # Assign ids 1-6 one servo at a time, in LeRobot's order. `just setup-motors leader` for the other arm
43 setup-motors arm="follower":
44 role=$([ "{{ arm }}" = leader ] && echo teleop || echo robot); \
45 lerobot-setup-motors --$role.type=so101_{{ arm }} ${SO101_PORT:+--$role.port=$SO101_PORT}
46
47 # Record joint ranges; saved under ~/.cache/huggingface/lerobot/calibration/ as so101_<arm>
48 calibrate arm="follower":
49 role=$([ "{{ arm }}" = leader ] && echo teleop || echo robot); \
50 lerobot-calibrate --$role.type=so101_{{ arm }} --$role.id=so101_{{ arm }} ${SO101_PORT:+--$role.port=$SO101_PORT}
51
52 # Drive the follower from the leader: `just teleop <follower port> <leader port>`
53 teleop follower_port leader_port:
54 lerobot-teleoperate \
55 --robot.type=so101_follower --robot.port={{ follower_port }} --robot.id=so101_follower \
56 --teleop.type=so101_leader --teleop.port={{ leader_port }} --teleop.id=so101_leader
57
58 # ── Housekeeping ────────────────────────────────────────────────────────
59
60 # Typecheck and test the TypeScript app
61 check:
62 cd Software/so101 && bunx tsc --noEmit && bun test
63
64 # Re-lock and re-sync the Python side after editing pyproject.toml
65 sync:
66 uv lock && uv sync --locked
67
68 # ── Vision: ArUco pick (Software/vision, see its QUEST.md) ──────────────
69 # SO101_CAMERA is an OpenCV index (default 1; `lerobot-find-cameras opencv` lists them) or the
70 # ustreamer URL on the Pi, http://<pi>:8080/stream. SO101_ARM=http://<pi>:8101 sends every arm
71 # move to `so101 serve` there instead of the so101 CLI on this machine.
72
73 camera := env("SO101_CAMERA", "1")
74
75 # Regenerate the printable markers and calibration board in Software/vision/printables/ (the arm's page serves them too)
76 vision-markers:
77 cd Software/vision && python markers.py printables
78
79 # Live view of detected markers, with poses once calibrated
80 vision-view:
81 cd Software/vision && python view.py --camera {{ camera }} --intrinsics ../../outputs/camera.json --handeye ../../outputs/handeye.json
82
83 # Calibrate camera intrinsics from the ChArUco board, hands-free
84 vision-calibrate:
85 cd Software/vision && python calibrate_camera.py --camera {{ camera }} --out ../../outputs/camera.json
86
87 # Solve where the camera is relative to the arm: `just vision-handeye he1,he2,he3,he4,he5,he6`
88 vision-handeye poses:
89 cd Software/vision && python handeye.py --camera {{ camera }} --poses {{ poses }} --intrinsics ../../outputs/camera.json --out ../../outputs/handeye.json
90
91 # Rehearse the pick: see the cube, plan the joints, move nothing
92 vision-rehearse:
93 cd Software/vision && python pick.py --camera {{ camera }} --intrinsics ../../outputs/camera.json --handeye ../../outputs/handeye.json --dry-run
94
95 # Pick the cube and put it at the `drop` pose
96 vision-pick:
97 cd Software/vision && python pick.py --camera {{ camera }} --intrinsics ../../outputs/camera.json --handeye ../../outputs/handeye.json
98
99 # Offline tests for the vision math
100 vision-test:
101 cd Software/vision && python -m pytest -q
pyproject.toml added +14
@@ -0,0 +1,14 @@
1 [project]
2 name = "so101-dev"
3 version = "0.0.0"
4 description = "LeRobot + Feetech toolchain for the SO-101 arm"
5 requires-python = ">=3.12,<3.13"
6 dependencies = [
7 "lerobot[feetech,dataset,viz,kinematics]>=0.6.1,<0.7",
8 ]
9
10 [dependency-groups]
11 dev = ["pytest>=8"]
12
13 [tool.uv]
14 package = false
uv.lock added +1967

Diff not shown — 231,499 bytes of diff (limit 100,000). +1967

View file

↑↓ navigate ↵ open ⌘↵ its app esc close

Keyboard shortcuts

Docs