Public: anyone can read this; the crew can change it.
Migrated from github.com/Trevato/SO-ARM100
workbenchthis machine
Make arm run here #2
Check · Review · Land done · Live — Landed; nothing deploys from this branch, so this is the end of its path.
quill/arm:quill/1-make-arm-run-here → main 5 commits fork
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 | ||
| 28 | 28 | |
| 29 | 29 | ### Documentation 📖 |
| 30 | 30 | - For the SO‑101 docs, follow this page further. |
| 31 | - Software to drive what you built: [](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`). | |
| 31 | 32 | - Here you can find the [](SO100.md) which is deprecated. |
| 32 | 33 | |
| 33 | 34 | ### 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 | [](https://github.com/TheRobotStudio/SO-ARM100) robot arm: the open | |
| 5 | hardware by The Robot Studio, run with [](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 ([](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 [](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. [](https://doi.org/10.1109/34.888718). | |
| 233 | OpenCV's `calibrateCamera` implements it; the ChArUco flow is in the | |
| 234 | [](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. [](https://doi.org/10.1016/j.patcog.2014.01.005). | |
| 239 | OpenCV: [](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 | [](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. [](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. [](https://doi.org/10.1109/70.326576). | |
| 250 | K. Daniilidis, *Hand-Eye Calibration Using Dual Quaternions*, IJRR 18(3), 1999. | |
| 251 | [](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 | [](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 | [](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 | [](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.** [](https://github.com/TheRobotStudio/SO-ARM100) | |
| 268 | by The Robot Studio; [](https://huggingface.co/docs/lerobot/so101), | |
| 269 | whose calibration convention (zero at the middle of each joint's range) the | |
| 270 | model follows; [](https://threejs.org) and | |
| 271 | [](https://github.com/gkjohnson/urdf-loaders) for the 3D view; | |
| 272 | the [](https://developer.mozilla.org/en-US/docs/Web/API/Web_Serial_API) | |
| 273 | for live mode; [](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 : Transport; | |
| 77 | readonly = new FrameParser(); | |
| 78 | readonly = new Set<number>(); | |
| 79 | : Promise<unknown> = Promise.resolve(); | |
| 80 | : Pending | null = null; | |
| 81 | : Uint8Array | null = null; | |
| 82 | : boolean; | |
| 83 | = false; | |
| 84 | : Error | null = null; | |
| 85 | ||
| 86 | constructor(transport: Transport, { echo = false }: BusOptions = {}) { | |
| 87 | this. = transport; | |
| 88 | this.path = transport.path; | |
| 89 | this.baudRate = transport.baudRate; | |
| 90 | this. = echo; | |
| 91 | transport.on("data", (chunk) => this.(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. = error; | |
| 95 | this.?.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..close(() => resolve())); | |
| 109 | } | |
| 110 | ||
| 111 | /** Whether the adapter echoes transmitted bytes back to the host. */ | |
| 112 | get echo(): boolean { | |
| 113 | return this.; | |
| 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.(packet, [BROADCAST_ID], BASE_TIMEOUT_MS, { probe: true }); | |
| 127 | this. = frame !== undefined; | |
| 128 | return this.; | |
| 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.(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.(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.(id, register, value); | |
| 167 | return; | |
| 168 | } | |
| 169 | await this.(id, STS3215.Torque_Enable, 0); | |
| 170 | await this.(id, STS3215.Lock, 0); | |
| 171 | const renaming = register === STS3215.ID; | |
| 172 | await this.(id, register, value, renaming ? [id, value] : [id]); | |
| 173 | await this.(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.(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.(encodeInstruction(BROADCAST_ID, Instruction.SyncWrite, params), [], 0); | |
| 196 | } | |
| 197 | ||
| 198 | async (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.(packet, replyFrom, timeout, { need: 1 }); | |
| 205 | if (!frame) throw new NoResponseError(id, `write ${value} at ${register.address}`); | |
| 206 | } | |
| 207 | ||
| 208 | (chunk: Uint8Array): void { | |
| 209 | for (const frame of this..push(chunk)) { | |
| 210 | const pending = this.; | |
| 211 | if (!pending) continue; | |
| 212 | const ours = this. !== null && bytesEqual(frame.raw, this.); | |
| 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.) { | |
| 224 | this. = 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 | ( | |
| 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.) throw this.; | |
| 247 | if (expectIds.some((id) => this..has(id))) { | |
| 248 | await sleep(QUIET_MS); | |
| 249 | this..clear(); | |
| 250 | } | |
| 251 | this..reset(); | |
| 252 | this. = packet; | |
| 253 | this. = this.; | |
| 254 | if (expectIds.length === 0) { | |
| 255 | return new Promise((resolve, reject) => | |
| 256 | this..write(packet, (err) => (err ? reject(err) : this..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) this. = null; | |
| 266 | }; | |
| 267 | const finish = (timedOut: boolean) => { | |
| 268 | settle(); | |
| 269 | if (timedOut && !probe) for (const id of remaining) this..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; | |
| 279 | this..write(packet, (err) => err && fail(err)); | |
| 280 | timer = setTimeout(() => finish(true), timeoutMs); | |
| 281 | }); | |
| 282 | }; | |
| 283 | const result = this..then(run, run); | |
| 284 | this. = 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 | : ((chunk: Uint8Array) => void) | undefined; | |
| 29 | readonly = new FrameParser(); | |
| 30 | readonly : FakeOptions; | |
| 31 | ||
| 32 | constructor(ids: number[], options: FakeOptions = {}) { | |
| 33 | this. = 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 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..echo ? [[0, data]] : []; | |
| 59 | const replyFrom = (id: number, frame: Uint8Array) => { | |
| 60 | const left = this..dropAfter?.get(id); | |
| 61 | if (left !== undefined) { | |
| 62 | if (left <= 0) { | |
| 63 | (this..drop ??= new Set()).add(id); | |
| 64 | return; | |
| 65 | } | |
| 66 | this..dropAfter!.set(id, left - 1); | |
| 67 | } | |
| 68 | const delay = this..lateOnce?.get(id) ?? 0; | |
| 69 | this..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..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..drop?.has(id) ? () => undefined : replyFrom; | |
| 81 | if (instruction === Instruction.Ping) reply(id, status(id, [], this..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..jitter && this..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.(id, address!); | |
| 94 | if (address === STS3215.ID.address) { | |
| 95 | this.(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.(id, address!); | |
| 113 | } | |
| 114 | } | |
| 115 | } | |
| 116 | } | |
| 117 | for (const [delay, frame] of replies) setTimeout(() => this.?.(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 | (id: number, address: number): void { | |
| 130 | if (address !== STS3215.Goal_Position.address) return; | |
| 131 | const memory = this.servos.get(id)!; | |
| 132 | if (this..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 | (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 : string | undefined; | |
| 37 | readonly = new Set<number>(); | |
| 38 | ||
| 39 | constructor(file?: string) { | |
| 40 | this. = 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.) { | |
| 63 | try { | |
| 64 | mkdirSync(dirname(this.), { recursive: true }); | |
| 65 | appendFileSync(this., `${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..has(j.id)) { | |
| 83 | this..add(j.id); | |
| 84 | this.event("hot", { id: j.id, joint: j.joint ?? null, temperature: j.temperature }); | |
| 85 | } else if (j.temperature < COOL) this..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 : FeetechBus; | |
| 51 | readonly : number; | |
| 52 | readonly : number; | |
| 53 | readonly : number; | |
| 54 | readonly : AbortSignal | undefined; | |
| 55 | : [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 = 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 = new Map<number, number>(); | |
| 60 | readonly = new Map<number, number[]>(); | |
| 61 | readonly = new Map<number, number>(); | |
| 62 | ||
| 63 | constructor(bus: FeetechBus, ids: readonly number[], options: SupervisorOptions = {}) { | |
| 64 | this. = bus; | |
| 65 | this.ids = ids; | |
| 66 | this. = options.torquePercent ?? 70; | |
| 67 | this. = options.marginTicks ?? 30; | |
| 68 | this. = options.tolerance ?? 15; | |
| 69 | this. = options.signal; | |
| 70 | } | |
| 71 | ||
| 72 | get limits(): readonly (readonly [number, number])[] { | |
| 73 | return this.; | |
| 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. = []; | |
| 85 | for (const id of this.ids) { | |
| 86 | const min = await this..read(id, STS3215.Min_Position_Limit); | |
| 87 | const max = await this..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..push([min, max]); | |
| 90 | const ceiling = await this..read(id, STS3215.Max_Torque_Limit); | |
| 91 | this..set(id, ceiling); | |
| 92 | this..set(id, Math.min(Math.round(this. * 10), ceiling)); | |
| 93 | } | |
| 94 | const start = await this..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..set(id, [start.get(id)!]); | |
| 99 | this..set(id, 0); | |
| 100 | } | |
| 101 | await this..syncWrite(STS3215.Goal_Position, this.ids.map((id) => [id, start.get(id)!] as const)); | |
| 102 | await this..syncWrite(STS3215.Goal_Velocity, this.ids.map((id) => [id, 20] as const)); | |
| 103 | await this..syncWrite(STS3215.Acceleration, this.ids.map((id) => [id, acceleration] as const)); | |
| 104 | await this..syncWrite(STS3215.Torque_Limit, this.ids.map((id) => [id, this..get(id)!] as const)); | |
| 105 | await this..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.[i]!; | |
| 117 | const aim = | |
| 118 | max - min < 2 * this. | |
| 119 | ? Math.round((min + max) / 2) | |
| 120 | : Math.min(max - this., Math.max(min + this., 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..syncWrite(STS3215.Goal_Velocity, this.ids.map((id, i) => [id, velocities[i]!] as const)); | |
| 130 | await this..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..syncRead(this.ids, STS3215.Present_Position); | |
| 136 | const loads = await this..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.) arrived = false; | |
| 148 | const window = this..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..get(id)! * 0.85; // Present_Load is in 0.1 % | |
| 153 | const count = away > this. && pushing ? (this..get(id) ?? 0) + 1 : 0; | |
| 154 | this..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.?.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..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..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..syncWrite(STS3215.Torque_Limit, this.ids.map((id) => [id, this..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 | = new Uint8Array(0); | |
| 88 | ||
| 89 | push(chunk: Uint8Array): StatusPacket[] { | |
| 90 | const joined = new Uint8Array(this..length + chunk.length); | |
| 91 | joined.set(this.); | |
| 92 | joined.set(chunk, this..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. = joined.slice(offset); | |
| 119 | return frames; | |
| 120 | } | |
| 121 | ||
| 122 | reset(): void { | |
| 123 | this. = 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 | ||
| 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; : #0b0d10; : #e8eaed; : #9aa4ae; : #3fb950; : #d29922; : #f85149; : #30363d; : #21262d; : #388bfd; } | |
| 19 | * { box-sizing: border-box; } | |
| 20 | html, body { margin: 0; height: 100%; background: var(); color: var(); 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(); 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(); border-color: var(); } | |
| 31 | .dot { width: 8px; height: 8px; border-radius: 50%; background: var(); display: inline-block; } .dot.on { background: var(); } | |
| 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(); display: none; } | |
| 33 | #banner.bad { display: block; border-color: var(); color: #ffb3ad; } #banner.warn { display: block; border-color: var(); color: #f2cc60; } | |
| 34 | #hud { padding: 8px 12px calc(8px + env(safe-area-inset-bottom)); background: #0d1117; border-top: 1px solid var(); 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(); 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(); 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(); color: var(); border: 1px solid var(); 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(); color: var(); } button.go { border-color: var(); } button.rec { border-color: var(); } | |
| 49 | button.x { padding: 0 8px; min-height: 26px; color: var(); font-size: 13px; } | |
| 50 | .pose { display: inline-flex; gap: 4px; align-items: center; } | |
| 51 | select, input[type=number] { background: var(); color: var(); border: 1px solid var(); 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(); font-size: 12px; } | |
| 54 | #msg { color: var(); font-size: 12px; min-height: 1.2em; } #msg.bad { color: var(); } | |
| 55 | #strip { color: var(); font-size: 12px; display: flex; gap: 14px; flex-wrap: wrap; } | |
| 56 | #strip b { color: var(); font-weight: 500; } | |
| 57 | dialog { background: #161b22; color: var(); border: 1px solid var(); 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(); font-size: 13px; } | |
| 60 | dialog input[type=text] { width: 100%; font: inherit; padding: 8px 10px; border-radius: 6px; border: 1px solid var(); background: #0d1117; color: var(); } | |
| 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(); 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(); font-weight: 500; } | |
| 73 | .ok { color: var(); } .no { color: var(); } | |
| 74 | table { border-collapse: collapse; font-size: 13px; } td, th { padding: 4px 10px; border-bottom: 1px solid var(); text-align: left; } th { color: var(); 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 & Lenz 1989 <a href="https://doi.org/10.1109/70.34770" target="_blank" rel="noopener">doi:10.1109/70.34770</a>; Park & 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 & 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. [](https://doi.org/10.1109/34.888718). | |
| 116 | OpenCV's `calibrateCamera` implements it; the ChArUco flow is in the | |
| 117 | [](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. [](https://doi.org/10.1016/j.patcog.2014.01.005). | |
| 122 | OpenCV: [](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 | [](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. [](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. [](https://doi.org/10.1109/70.326576). | |
| 133 | K. Daniilidis, *Hand-Eye Calibration Using Dual Quaternions*, IJRR 18(3), 1999. | |
| 134 | [](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 | [](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 | [](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 | [](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.** [](https://github.com/TheRobotStudio/SO-ARM100) | |
| 151 | by The Robot Studio; [](https://huggingface.co/docs/lerobot/so101), | |
| 152 | whose calibration convention (zero at the middle of each joint's range) the | |
| 153 | model follows; [](https://threejs.org) and | |
| 154 | [](https://github.com/gkjohnson/urdf-loaders) for the 3D view; | |
| 155 | the [](https://developer.mozilla.org/en-US/docs/Web/API/Web_Serial_API) | |
| 156 | for live mode; [](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 | (frozen=True) | |
| 71 | class JointRange: | |
| 72 | min: int | |
| 73 | max: int | |
| 74 | ||
| 75 | ||
| 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 | ||
| 100 | class Intrinsics: | |
| 101 | width: int | |
| 102 | height: int | |
| 103 | K: np.ndarray | |
| 104 | dist: np.ndarray | |
| 105 | rms: float = 0.0 | |
| 106 | ||
| 107 | ||
| 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 | ||
| 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 | ||
| 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=; \ | |
| 45 | lerobot-setup-motors --.type=so101_{{ arm }} | |
| 46 | ||
| 47 | # Record joint ranges; saved under ~/.cache/huggingface/lerobot/calibration/ as so101_<arm> | |
| 48 | calibrate arm="follower": | |
| 49 | role=; \ | |
| 50 | lerobot-calibrate --.type=so101_{{ arm }} --.id=so101_{{ arm }} | |
| 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
new above
offline