Public: anyone can read this; the crew can change it.
Migrated from github.com/Trevato/SO-ARM100
workbenchthis machine
Three table touches set the table: src/table-plane.ts fits the plane and POST /settings/table-touch applies it #474
Check · Review · Land done · Live — Landed; nothing deploys from this branch, so this is the end of its path.
smith/arm:smith/447-three-table-touches-set-the-table-src-ta → main 6 commits fork
10 files
changed +259
−14
Software/README.md +16
| @@ -551,6 +551,10 @@ watches. A line you type is said in the chat; commands start with `/`: | ||
| 551 | 551 | `POST /settings/table-here`: the table plane goes under the arm's lowest |
| 552 | 552 | link now. `/table <mm>`, owner: sets `tableOffsetMm` (-100 to 100) by |
| 553 | 553 | `PUT /settings`. Both print the offset the server now holds, or its refusal. |
| 554 | - `/table touch 1|2|3`, owner: the three-touch fit, | |
| 555 | `POST /settings/table-touch {"n"}`. Touch the table with the gripper tip at | |
| 556 | three spread-out points, one command each; the third prints the fitted | |
| 557 | `heightMm` and `tiltDeg` and says whether it was applied, or the 409 reason. | |
| 554 | 558 | - `/zero <joint> [degrees]`, owner: `POST /settings/joint-zero` for that joint |
| 555 | 559 | (default 0°); prints the offset applied, or the refusal. |
| 556 | 560 | - `/machine`: the Machine panel, the same rows the page's Machine tab shows |
| @@ -772,6 +776,18 @@ down, so the resting arm is not flagged. The answer is 409 when the arm is | ||
| 772 | 776 | more than 100 mm from where the table could be; lower it first. The 3D |
| 773 | 777 | view draws the table there. |
| 774 | 778 | |
| 779 | **Three touches.** `POST /settings/table-touch {"n": 1|2|3}` (admin only; `/table | |
| 780 | touch 1|2|3` in the terminal) fits the plane instead of trusting one point. Put | |
| 781 | the gripper tip on the table, send `n` 1, then move to another point and send 2, | |
| 782 | then 3; the points must be at least 5 cm apart and not nearly in a line. The tip | |
| 783 | is the origin of the URDF's `gripper_frame_link`, from forward kinematics of | |
| 784 | Present_Position. Touches 1 and 2 answer 200 with the tip stored; touch 3 | |
| 785 | answers the settings plus `heightMm` (the plane's height at the base), | |
| 786 | `tiltDeg` and `applied`, and sets `tableOffsetMm` with the same floor-margin | |
| 787 | rule as `table-here`. A tilt over 3°, an offset outside ±100 mm, a bad fit, or | |
| 788 | a missing touch 1 or 2 answers 409 with the numbers and changes nothing. The | |
| 789 | touches live in memory, are lost on restart, and expire after 60 s (an old one counts as missing, so take the three together); `n` 1 starts a new round. | |
| 790 | ||
| 775 | 791 | To make the model read a known mechanical reference for one joint, put that |
| 776 | 792 | joint at the reference (a mark, a stop) and, as an admin, |
| 777 | 793 | `POST /settings/joint-zero {"joint": "shoulder_pan", "referenceDeg": 0}`, or |
Software/so101/src/kinematics.ts +6
| @@ -388,6 +388,12 @@ export class ArmModel { | ||
| 388 | 388 | return this.evaluate(q, settings).violation; |
| 389 | 389 | } |
| 390 | 390 | |
| 391 | /** The gripper tip (the URDF's gripper_frame_link origin) in root coordinates, metres; null when the model has none. */ | |
| 392 | tip(q: ReadonlyMap<string, number>): Vec3 | null { | |
| 393 | const T = this.linkPoses(q).get("gripper_frame_link"); | |
| 394 | return T ? transformPoint(T, [0, 0, 0]) : null; | |
| 395 | } | |
| 396 | ||
| 391 | 397 | /** The lowest sampled point of the moving links. */ |
| 392 | 398 | lowest(q: ReadonlyMap<string, number>): { z: number; link: string } { |
| 393 | 399 | return this.evaluate(q, { floorMargin: 0, baseMargin: 0 }).lowest; |
Software/so101/src/lobby.test.ts +2
| @@ -780,6 +780,8 @@ describe("lobby", () => { | ||
| 780 | 780 | expect((await post("/settings/table-here", {})).status).toBe(401); |
| 781 | 781 | expect((await post("/settings/table-here", ALICE)).status).toBe(403); |
| 782 | 782 | expect((await post("/settings/table-here", OWNER)).status).toBe(200); // an admin gets through |
| 783 | expect((await post("/settings/table-touch", ALICE, { n: 1 })).status).toBe(403); | |
| 784 | expect((await post("/settings/table-touch", { ...OWNER, origin: "https://evil.example" }, { n: 1 })).status).toBe(403); // only this page may touch the table | |
| 783 | 785 | expect((await post("/settings/joint-zero", ALICE)).status).toBe(403); |
| 784 | 786 | expect((await post("/settings", OWNER, { tableOffsetMm: 0 }, "PUT")).status).toBe(200); |
| 785 | 787 | expect((await post("/settings", OWNER, { maxSpeedDegS: 60 }, "PUT")).status).toBe(200); |
Software/so101/src/relay.ts +1 −1
| @@ -166,7 +166,7 @@ const PAGE = /^\/(?:index\.html)?$/; | ||
| 166 | 166 | const STATIC = /^\/(?:index\.html)?$|^\/(?:model|vendor|ui)\//; |
| 167 | 167 | const LEADER = "/leader/calibration"; |
| 168 | 168 | /** Admins only: clearing an emergency stop and changing the settings (the safety limits among them). */ |
| 169 | const ADMIN_ONLY = new Set(["POST /estop/clear", "PUT /settings", "POST /settings/table-here", "POST /settings/joint-zero", "PUT /servo-params", "POST /servo-params/factory"]); | |
| 169 | const ADMIN_ONLY = new Set(["POST /estop/clear", "PUT /settings", "POST /settings/table-here", "POST /settings/table-touch", "POST /settings/joint-zero", "PUT /servo-params", "POST /servo-params/factory"]); | |
| 170 | 170 | /** Routes only the owner may reach; anyone else is told there is no such page. */ |
| 171 | 171 | const OWNER_READS = new Set(["GET /servo-params", "PUT /servo-params", "POST /servo-params/factory"]); |
| 172 | 172 | /** The driver's, and any admin's at any time. */ |
Software/so101/src/server.test.ts +94
| @@ -542,6 +542,7 @@ describe("the safe zone, the jam watch, and the model routes", () => { | ||
| 542 | 542 | let fake6: FakeBus; |
| 543 | 543 | let zoneServer: RunningServer; |
| 544 | 544 | let zurl: string; |
| 545 | let touchClock = Date.now(); | |
| 545 | 546 | const zcall = (path: string, body: unknown, method = "POST") => fetch(`${zurl}${path}`, { method, body: JSON.stringify(body) }); |
| 546 | 547 | /** Wait until nothing drives the arm: a closed stream releases it a few bus operations later. */ |
| 547 | 548 | const idle = async () => { |
| @@ -572,6 +573,7 @@ describe("the safe zone, the jam watch, and the model routes", () => { | ||
| 572 | 573 | modelDir: new URL("../../../Simulation/SO101", import.meta.url).pathname, |
| 573 | 574 | watchdogMs: 60, |
| 574 | 575 | jamSampleMs: 15, |
| 576 | now: () => touchClock, | |
| 575 | 577 | }); |
| 576 | 578 | zurl = `http://127.0.0.1:${zoneServer.port}`; |
| 577 | 579 | }); |
| @@ -643,6 +645,98 @@ describe("the safe zone, the jam watch, and the model routes", () => { | ||
| 643 | 645 | } |
| 644 | 646 | }); |
| 645 | 647 | |
| 648 | test("three table touches fit the plane and set the offset; a missing touch is a 409, a bad n a 400", async () => { | |
| 649 | const saved = (await (await fetch(`${zurl}/settings`)).json()) as { tableOffsetMm: number; zone: string }; | |
| 650 | try { | |
| 651 | await zcall("/settings", { zone: "off" }, "PUT"); | |
| 652 | expect((await zcall("/settings/table-touch", { n: 4 })).status).toBe(400); | |
| 653 | expect((await zcall("/settings/table-touch", {})).status).toBe(400); | |
| 654 | const early = await zcall("/settings/table-touch", { n: 3 }); | |
| 655 | expect(early.status).toBe(409); | |
| 656 | expect(((await early.json()) as { error: string }).error).toContain("touch 1 is missing"); | |
| 657 | // The same height at three pan angles: a flat table under the reaching arm. | |
| 658 | for (const [n, pan] of [[1, -30], [2, 0], [3, 30]] as const) { | |
| 659 | await moved({ 2: ticks(2, 45), 1: ticks(1, pan) }); | |
| 660 | await idle(); | |
| 661 | const r = await zcall("/settings/table-touch", { n }); | |
| 662 | const body = (await r.json()) as { tableOffsetMm: number; tiltDeg: number; applied: boolean; touches: number; error?: string }; | |
| 663 | if (n < 3) { | |
| 664 | expect(r.status).toBe(200); | |
| 665 | expect(body.touches).toBe(n); | |
| 666 | } else { | |
| 667 | expect(r.status).toBe(200); | |
| 668 | expect(body.applied).toBe(true); | |
| 669 | expect(body.tiltDeg).toBeLessThan(0.5); | |
| 670 | expect(Number.isInteger(body.tableOffsetMm)).toBe(true); | |
| 671 | expect(((await (await fetch(`${zurl}/settings`)).json()) as { tableOffsetMm: number }).tableOffsetMm).toBe(body.tableOffsetMm); | |
| 672 | } | |
| 673 | } | |
| 674 | } finally { | |
| 675 | await moved({ 1: mid(1), 2: mid(2) }); | |
| 676 | await idle(); | |
| 677 | await zcall("/settings", { zone: saved.zone, tableOffsetMm: saved.tableOffsetMm }, "PUT"); | |
| 678 | } | |
| 679 | }); | |
| 680 | ||
| 681 | /** Touch the table at each [shoulder lift, pan] in degrees, in order; the answer to the last. */ | |
| 682 | const touchRound = async (poses: Array<[number, number]>) => { | |
| 683 | let last!: Response; | |
| 684 | for (const [i, [lift, pan]] of poses.entries()) { | |
| 685 | await moved({ 2: ticks(2, lift), 1: ticks(1, pan) }); | |
| 686 | await idle(); | |
| 687 | last = await zcall("/settings/table-touch", { n: i + 1 }); | |
| 688 | } | |
| 689 | return last; | |
| 690 | }; | |
| 691 | const offsetNow = async () => ((await (await fetch(`${zurl}/settings`)).json()) as { tableOffsetMm: number }).tableOffsetMm; | |
| 692 | ||
| 693 | test("a table touch older than 60 s is missing; touches 1 and 2 fresh again make the third work", async () => { | |
| 694 | const saved = (await (await fetch(`${zurl}/settings`)).json()) as { tableOffsetMm: number; zone: string }; | |
| 695 | try { | |
| 696 | await zcall("/settings", { zone: "off", tableOffsetMm: 0 }, "PUT"); | |
| 697 | await touchRound([[45, -30], [45, 0]]); | |
| 698 | touchClock += 60_001; | |
| 699 | await moved({ 2: ticks(2, 45), 1: ticks(1, 30) }); | |
| 700 | await idle(); | |
| 701 | const stale = await zcall("/settings/table-touch", { n: 3 }); | |
| 702 | expect(stale.status).toBe(409); | |
| 703 | expect(((await stale.json()) as { error: string }).error).toContain("touch 1 is missing or older than 60 s"); | |
| 704 | expect(await offsetNow()).toBe(0); | |
| 705 | touchClock += 1000; | |
| 706 | expect((await touchRound([[45, -30], [45, 0], [45, 30]])).status).toBe(200); | |
| 707 | } finally { | |
| 708 | await moved({ 1: mid(1), 2: mid(2) }); | |
| 709 | await idle(); | |
| 710 | await zcall("/settings", { zone: saved.zone, tableOffsetMm: saved.tableOffsetMm }, "PUT"); | |
| 711 | } | |
| 712 | }); | |
| 713 | ||
| 714 | test("a refused fit, a steep tilt and an offset out of range are 409s that leave the table where it was", async () => { | |
| 715 | const saved = (await (await fetch(`${zurl}/settings`)).json()) as { tableOffsetMm: number; zone: string }; | |
| 716 | try { | |
| 717 | await zcall("/settings", { zone: "off", tableOffsetMm: 7 }, "PUT"); | |
| 718 | const close = await touchRound([[45, 0], [45, 0], [45, 2]]); | |
| 719 | expect(close.status).toBe(409); | |
| 720 | expect(((await close.json()) as { error: string }).error).toContain("refused"); | |
| 721 | expect(await offsetNow()).toBe(7); | |
| 722 | const tilted = await touchRound([[45, -30], [45, 0], [38, 30]]); | |
| 723 | expect(tilted.status).toBe(409); | |
| 724 | const tilt = (await tilted.json()) as { error: string; applied: boolean; tiltDeg: number }; | |
| 725 | expect(tilt.error).toContain("tilts"); | |
| 726 | expect(tilt).toMatchObject({ applied: false }); | |
| 727 | expect(tilt.tiltDeg).toBeGreaterThan(3); | |
| 728 | expect(await offsetNow()).toBe(7); | |
| 729 | const low = await touchRound([[60, -30], [60, 0], [60, 30]]); // flat, but 118 mm under the model's table | |
| 730 | expect(low.status).toBe(409); | |
| 731 | expect(((await low.json()) as { error: string; applied: boolean })).toMatchObject({ applied: false, error: expect.stringContaining("past ±100") }); | |
| 732 | expect(await offsetNow()).toBe(7); | |
| 733 | } finally { | |
| 734 | await moved({ 1: mid(1), 2: mid(2) }); | |
| 735 | await idle(); | |
| 736 | await zcall("/settings", { zone: saved.zone, tableOffsetMm: saved.tableOffsetMm }, "PUT"); | |
| 737 | } | |
| 738 | }); | |
| 739 | ||
| 646 | 740 | test("a move into the table is refused in enforce mode and only reported in warn mode", async () => { |
| 647 | 741 | const down = { targets: { "2": ticks(2, 80) } }; // shoulder lift far forward with the elbow straight: the gripper goes under the table |
| 648 | 742 | const refused = await zcall("/move", down); |
Software/so101/src/server.ts +44 −2
| @@ -32,6 +32,7 @@ | ||
| 32 | 32 | * and the safety limits of src/safety.ts (speed, acceleration, rate, dead-man, park, gripper force, joint limits) |
| 33 | 33 | * PUT /settings change any of those |
| 34 | 34 | * POST /settings/table-here put the table (tableOffsetMm, -100..100) under the arm's lowest link now, the margin below it |
| 35 | * POST /settings/table-touch {"n": 1|2|3} store the gripper tip as touch n; the third fits the table plane and sets tableOffsetMm | |
| 35 | 36 | * POST /settings/joint-zero {"joint": "shoulder_pan", "referenceDeg": 0} trim that joint's offset so the model reads referenceDeg at the pose it is in now |
| 36 | 37 | * (owner only; 409 when the offset would pass ±15°: put the arm at the reference first) |
| 37 | 38 | * GET /metrics jams, blocks, stalls, peak load and temperature per joint, recent events |
| @@ -74,7 +75,8 @@ import { browserModule } from "./browser-modules.ts"; | ||
| 74 | 75 | import { Clock, CommandGate, HELD_CLOSE } from "./command-gate.ts"; |
| 75 | 76 | import { FLOOD_CLOSE, FrameRate, frameBytes, MAX_FRAME_BYTES, TOO_BIG_CLOSE } from "./frame-limit.ts"; |
| 76 | 77 | import type { FeetechBus } from "./bus.ts"; |
| 77 | import { ArmModel, gripperTicksToRadians, ticksToRadians, type Violation } from "./kinematics.ts"; | |
| 78 | import { ArmModel, gripperTicksToRadians, ticksToRadians, type Vec3, type Violation } from "./kinematics.ts"; | |
| 79 | import { fitTablePlane, MAX_TILT_DEG } from "./table-plane.ts"; | |
| 78 | 80 | import { clamp, type Frame } from "./mapping.ts"; |
| 79 | 81 | import { MachineLog, perServo } from "./machine-log.ts"; |
| 80 | 82 | import { Metrics } from "./metrics.ts"; |
| @@ -115,6 +117,8 @@ export interface ServerOptions { | ||
| 115 | 117 | jamSampleMs?: number; |
| 116 | 118 | /** How often the status is read for /watch, ms; it is sent when it changed (and at least every watchQuietMs). */ |
| 117 | 119 | watchEveryMs?: number; |
| 120 | /** The clock the table touches expire by, ms; Date.now unless a test sets it. */ | |
| 121 | now?: () => number; | |
| 118 | 122 | /** Longest /watch goes without sending an unchanged status, ms (200): keep it inside the relay link's staleMs. */ |
| 119 | 123 | watchQuietMs?: number; |
| 120 | 124 | } |
| @@ -167,6 +171,8 @@ class HttpError extends Error { | ||
| 167 | 171 | } |
| 168 | 172 | |
| 169 | 173 | const MAX_BODY = 1_000_000; |
| 174 | /** A table touch older than this is forgotten: the arm or the table may have moved since. */ | |
| 175 | const TOUCH_TTL_MS = 60_000; | |
| 170 | 176 | const URDF = "so101_new_calib.urdf"; |
| 171 | 177 | /** Load, in percent of the servo's maximum, that counts as pushing against something. */ |
| 172 | 178 | /** The present block status reads in one sync read; Present_Velocity and Present_Voltage ride along unread. */ |
| @@ -258,6 +264,16 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 258 | 264 | console.error(`settings: ${settingsFile} is not readable JSON (${err instanceof Error ? err.message : String(err)}); using the defaults, zone "${settings.zone}"`); |
| 259 | 265 | } |
| 260 | 266 | } |
| 267 | /** The gripper tip of table touches 1..3, held in memory only: a restart forgets them. */ | |
| 268 | const tableTouches: Array<{ tip: Vec3; at: number } | undefined> = [undefined, undefined, undefined]; | |
| 269 | const clockNow = options.now ?? Date.now; | |
| 270 | /** The touches still fresh: an old one is as good as missing. */ | |
| 271 | const freshTouch = (i: number) => { | |
| 272 | const t = tableTouches[i]; | |
| 273 | return t && clockNow() - t.at <= TOUCH_TTL_MS ? t : undefined; | |
| 274 | }; | |
| 275 | /** The offset that puts the table at height z (base frame) with the zone's floor margin below it: rounded down, as the resting arm must not be flagged. */ | |
| 276 | const tableOffsetAt = (z: number) => Math.floor((z - model!.floorZ) * 1000 - settings.floorMarginMm); | |
| 261 | 277 | const zoneSettings = () => ({ floorMargin: settings.floorMarginMm / 1000, baseMargin: settings.baseMarginMm / 1000, tableOffset: settings.tableOffsetMm / 1000 }); |
| 262 | 278 | /** Joint angles for the model from servo ticks, with the trims applied. The gripper maps closed→0, open→its URDF limit. */ |
| 263 | 279 | const gripperOpen = model?.movable.find((j) => j.name === "gripper")?.upper ?? 1.745; |
| @@ -836,7 +852,7 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 836 | 852 | // The table is where the arm's lowest link is now, less the margin the zone keeps above it, so the resting arm is not flagged. |
| 837 | 853 | if (!model) throw new HttpError(409, "no model loaded on this server"); |
| 838 | 854 | const lowest = model.lowest(toQ(await bus.syncRead(ids, STS3215.Present_Position))); |
| 839 | const offset = Math.floor((lowest.z - model.floorZ) * 1000 - settings.floorMarginMm); | |
| 855 | const offset = tableOffsetAt(lowest.z); | |
| 840 | 856 | if (offset < -100 || offset > 100) { |
| 841 | 857 | throw new HttpError(409, `the table would be ${offset} mm from the model's, past ±100: lowest point ${Math.round((lowest.z - model.floorZ) * 1000)} mm above the model floor (${lowest.link.replace(/_link$/, "")}); put the arm near the table first`); |
| 842 | 858 | } |
| @@ -844,6 +860,32 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 844 | 860 | writeAtomic(settingsFile, JSON.stringify(settings, null, 1)); |
| 845 | 861 | return json(res, 200, { ...settings, model: true }); |
| 846 | 862 | } |
| 863 | case "POST /settings/table-touch": { | |
| 864 | // Three touches of the table with the gripper tip fit its plane; a bad fit or a steep tilt is reported and nothing changes. | |
| 865 | if (!model) throw new HttpError(409, "no model loaded on this server"); | |
| 866 | const n = (await readBody(req)).n; | |
| 867 | if (n !== 1 && n !== 2 && n !== 3) throw new HttpError(400, "n must be 1, 2 or 3"); | |
| 868 | const tip = model.tip(toQ(await bus.syncRead(ids, STS3215.Present_Position))); | |
| 869 | if (!tip) throw new HttpError(409, "the model has no gripper tip"); | |
| 870 | if (n === 3) { | |
| 871 | const missing = [0, 1].findIndex((i) => !freshTouch(i)); | |
| 872 | if (missing >= 0) throw new HttpError(409, `touch ${missing + 1} is missing or older than ${TOUCH_TTL_MS / 1000} s; touch 1 and 2 first`); | |
| 873 | } | |
| 874 | if (n === 1) tableTouches.fill(undefined); // a new round | |
| 875 | tableTouches[n - 1] = { tip, at: clockNow() }; | |
| 876 | if (n < 3) return json(res, 200, { n, touches: [0, 1, 2].filter((i) => freshTouch(i)).length, tipMm: tip.map((v) => Math.round(v * 1000)) }); | |
| 877 | const fit = fitTablePlane(tableTouches.map((t) => t!.tip)); | |
| 878 | if (!fit.ok) throw new HttpError(409, `the touches were refused: ${fit.reason}`); | |
| 879 | const heightMm = Math.round(fit.heightM * 1000 * 10) / 10; | |
| 880 | const tiltDeg = Math.round(fit.tiltDeg * 100) / 100; | |
| 881 | const offset = tableOffsetAt(fit.heightM); | |
| 882 | if (fit.tiltDeg > MAX_TILT_DEG) return json(res, 409, { error: `the table tilts ${tiltDeg}° (over ${MAX_TILT_DEG}°): not applied`, heightMm, tiltDeg, applied: false }); | |
| 883 | if (offset < -100 || offset > 100) return json(res, 409, { error: `the table would be ${offset} mm from the model's, past ±100: not applied`, heightMm, tiltDeg, applied: false }); | |
| 884 | settings = validateSettings({ ...settings, tableOffsetMm: offset }); | |
| 885 | writeAtomic(settingsFile, JSON.stringify(settings, null, 1)); | |
| 886 | tableTouches.fill(undefined); | |
| 887 | return json(res, 200, { ...settings, model: true, heightMm, tiltDeg, applied: true }); | |
| 888 | } | |
| 847 | 889 | case "POST /settings/joint-zero": { |
| 848 | 890 | const body = await readBody(req); |
| 849 | 891 | if (typeof body.joint !== "string") throw new HttpError(400, "joint must be a joint name"); |
Software/so101/src/table-plane.test.ts added +28
| @@ -0,0 +1,28 @@ | ||
| 1 | import { describe, expect, test } from "bun:test"; | |
| 2 | import type { Vec3 } from "./kinematics.ts"; | |
| 3 | import { fitTablePlane } from "./table-plane.ts"; | |
| 4 | ||
| 5 | const plane = (h: number, gx: number, gy: number) => (x: number, y: number): Vec3 => [x, y, h + gx * x + gy * y]; | |
| 6 | ||
| 7 | describe("fitTablePlane", () => { | |
| 8 | test("a flat table gives its height and no tilt", () => { | |
| 9 | const f = plane(-0.02, 0, 0); | |
| 10 | expect(fitTablePlane([f(0.2, 0), f(0.3, 0.1), f(0.3, -0.1)])).toEqual({ ok: true, heightM: -0.02, tiltDeg: 0 }); | |
| 11 | }); | |
| 12 | ||
| 13 | test("a tilted table gives the height at the base and the angle", () => { | |
| 14 | const f = plane(0.01, Math.tan(0.05), 0.02); | |
| 15 | const r = fitTablePlane([f(0.2, 0), f(0.3, 0.1), f(0.3, -0.1)]); | |
| 16 | if (!r.ok) throw new Error(r.reason); | |
| 17 | expect(r.heightM).toBeCloseTo(0.01, 9); | |
| 18 | expect(r.tiltDeg).toBeCloseTo((Math.atan(Math.hypot(Math.tan(0.05), 0.02)) * 180) / Math.PI, 9); | |
| 19 | }); | |
| 20 | ||
| 21 | test("collinear, near-collinear, close, and non-finite points are refused with a reason", () => { | |
| 22 | const f = plane(0, 0, 0); | |
| 23 | expect(fitTablePlane([f(0.1, 0), f(0.2, 0), f(0.3, 0)])).toMatchObject({ ok: false, reason: expect.stringContaining("line") }); | |
| 24 | expect(fitTablePlane([f(0.1, 0), f(0.2, 0.005), f(0.3, 0)])).toMatchObject({ ok: false }); | |
| 25 | expect(fitTablePlane([f(0.1, 0), f(0.12, 0), f(0.3, 0.1)])).toMatchObject({ ok: false, reason: expect.stringContaining("apart") }); | |
| 26 | expect(fitTablePlane([f(0.1, 0), f(0.2, 0.1), [NaN, 0, 0]])).toMatchObject({ ok: false }); | |
| 27 | }); | |
| 28 | }); |
Software/so101/src/table-plane.ts added +31
| @@ -0,0 +1,31 @@ | ||
| 1 | /** | |
| 2 | * The table plane from three touches of the gripper tip (metres, base frame, z up). | |
| 3 | * Pure: the server turns the answer into tableOffsetMm and decides whether to apply it. | |
| 4 | */ | |
| 5 | import type { Vec3 } from "./kinematics.ts"; | |
| 6 | ||
| 7 | export const MIN_SPACING_M = 0.05; | |
| 8 | /** The smallest altitude of the triangle the touches span, seen from above: a thinner one is a line, and a line fixes no tilt. */ | |
| 9 | export const MIN_ALTITUDE_M = 0.02; | |
| 10 | /** A table tilted more than this is reported, not applied. */ | |
| 11 | export const MAX_TILT_DEG = 3; | |
| 12 | ||
| 13 | export type PlaneFit = { ok: true; heightM: number; tiltDeg: number } | { ok: false; reason: string }; | |
| 14 | ||
| 15 | export function fitTablePlane(points: readonly Vec3[]): PlaneFit { | |
| 16 | if (points.length !== 3 || points.some((p) => p.length !== 3 || p.some((v) => !Number.isFinite(v)))) return { ok: false, reason: "three finite points are needed" }; | |
| 17 | const [a, b, c] = points as [Vec3, Vec3, Vec3]; | |
| 18 | const xy = (p: Vec3, q: Vec3) => Math.hypot(q[0] - p[0], q[1] - p[1]); | |
| 19 | const sides = [xy(a, b), xy(b, c), xy(a, c)]; | |
| 20 | const shortest = Math.min(...sides); | |
| 21 | if (shortest < MIN_SPACING_M) return { ok: false, reason: `two touches are ${Math.round(shortest * 1000)} mm apart; they must be at least ${MIN_SPACING_M * 1000} mm apart` }; | |
| 22 | const cross = (b[0] - a[0]) * (c[1] - a[1]) - (b[1] - a[1]) * (c[0] - a[0]); // twice the triangle's area from above | |
| 23 | const altitude = Math.abs(cross) / Math.max(...sides); | |
| 24 | if (altitude < MIN_ALTITUDE_M) return { ok: false, reason: "the touches are nearly in a line; spread them into a triangle" }; | |
| 25 | // z = gx*x + gy*y + h, solved from the two edge vectors; h is the height at x = y = 0, under the base. | |
| 26 | const [ux, uy, uz] = [b[0] - a[0], b[1] - a[1], b[2] - a[2]]; | |
| 27 | const [vx, vy, vz] = [c[0] - a[0], c[1] - a[1], c[2] - a[2]]; | |
| 28 | const gx = (uz * vy - vz * uy) / cross; | |
| 29 | const gy = (ux * vz - vx * uz) / cross; | |
| 30 | return { ok: true, heightM: a[2] - gx * a[0] - gy * a[1], tiltDeg: (Math.atan(Math.hypot(gx, gy)) * 180) / Math.PI }; | |
| 31 | } |
Software/so101/src/terminal.test.ts +22 −1
| @@ -129,7 +129,8 @@ describe("terminal table", () => { | ||
| 129 | 129 | expect(typed(room, "/table here")).toEqual({ table: "here" }); |
| 130 | 130 | expect(typed(room, "/table -12")).toEqual({ table: -12 }); |
| 131 | 131 | expect(typed(room, "/table 0")).toEqual({ table: 0 }); |
| 132 | for (const bad of ["/table", "/table 101", "/table -101", "/table 1.5", "/table abc", "/table 1e2"]) expect(typed(room, bad)).toEqual({ print: "usage: /table here | /table <mm, -100 to 100>" }); | |
| 132 | expect(typed(room, "/table touch 2")).toEqual({ table: { touch: 2 } }); | |
| 133 | for (const bad of ["/table", "/table 101", "/table -101", "/table 1.5", "/table abc", "/table 1e2", "/table touch", "/table touch 4"]) expect(typed(room, bad)).toEqual({ print: "usage: /table here | /table touch 1|2|3 | /table <mm, -100 to 100>" }); | |
| 133 | 134 | }); |
| 134 | 135 | |
| 135 | 136 | test("/zero <joint> [deg] is asked for with a default of 0; anything else is a usage line", () => { |
| @@ -178,6 +179,26 @@ describe("terminal table", () => { | ||
| 178 | 179 | expect(refused).toEqual({ status: 403, error: "admins only" }); |
| 179 | 180 | }); |
| 180 | 181 | |
| 182 | test("/table touch posts n; the third prints the fit, a refusal prints its reason", async () => { | |
| 183 | const printed: string[] = []; | |
| 184 | const asked: unknown[] = []; | |
| 185 | const door = new Door("ws://127.0.0.1:1/live", {}, (l) => printed.push(l), async (path, body) => { | |
| 186 | asked.push([path, body]); | |
| 187 | const n = (body as { n: number }).n; | |
| 188 | if (n === 1) return { status: 200, error: null }; | |
| 189 | if (n === 2) return { status: 409, error: "touch 1 is missing; touch 1 and 2 first" }; | |
| 190 | return { status: 200, error: null, tableOffsetMm: -12, heightMm: 3.5, tiltDeg: 0.4, applied: true }; | |
| 191 | }); | |
| 192 | door.input("/table touch 1"); | |
| 193 | await Bun.sleep(20); | |
| 194 | door.input("/table touch 2"); | |
| 195 | await Bun.sleep(20); | |
| 196 | door.input("/table touch 3"); | |
| 197 | await Bun.sleep(50); | |
| 198 | expect(printed).toEqual(["touch 1 taken", "error: touch 1 is missing; touch 1 and 2 first", "table is 3.5 mm at the base, tilt 0.4°, applied: offset -12 mm"]); | |
| 199 | expect(asked).toEqual([["/settings/table-touch", { n: 1 }], ["/settings/table-touch", { n: 2 }], ["/settings/table-touch", { n: 3 }]]); | |
| 200 | }); | |
| 201 | ||
| 181 | 202 | test("/visitors sends the frame and the answer prints the peak and 24 hours as text", () => { |
| 182 | 203 | expect(typed(emptyRoom(), "/visitors")).toEqual({ send: { type: "visitors" } }); |
| 183 | 204 | const hours = Array.from({ length: 24 }, (_, i) => i); |
Software/so101/src/terminal.ts +15 −10
| @@ -29,6 +29,7 @@ | ||
| 29 | 29 | * /visitors owner: today's peak viewers and joins per hour for the last 24 h (the counts /admin shows) |
| 30 | 30 | * /restore #<n> owner: put a removed line back for everyone (Lobby.restoreLine; it is logged) |
| 31 | 31 | * /table here admin: put the table under the arm's lowest link now (POST /settings/table-here) |
| 32 | * /table touch 1|2|3 admin: touch the table with the gripper tip at three points; the third fits the plane (POST /settings/table-touch) | |
| 32 | 33 | * /table <mm> admin: set the table offset, -100..100 mm (PUT /settings tableOffsetMm) |
| 33 | 34 | * /zero <joint> [deg] admin: make the model read deg (default 0) for that joint at the pose it is in now (POST /settings/joint-zero) |
| 34 | 35 | * /health owner or moderator: each open page's link as it last reported it (GET /admin/health, the page's Admin -> Health) |
| @@ -80,17 +81,17 @@ function appPort(env: Env): number { | ||
| 80 | 81 | return port; |
| 81 | 82 | } |
| 82 | 83 | |
| 83 | export type Path = "/stop" | "/resume" | "/settings/table-here" | "/settings/joint-zero" | "/settings"; | |
| 84 | export type Path = "/stop" | "/resume" | "/settings/table-here" | "/settings/table-touch" | "/settings/joint-zero" | "/settings"; | |
| 84 | 85 | /** `tableOffsetMm` is the offset the server holds after a settings call, when it said. */ |
| 85 | export type Post = (path: Path, body?: Record<string, unknown>) => Promise<{ status: number; error: string | null; tableOffsetMm?: number; offsetDeg?: number }>; | |
| 86 | export type Post = (path: Path, body?: Record<string, unknown>) => Promise<{ status: number; error: string | null; tableOffsetMm?: number; offsetDeg?: number; heightMm?: number; tiltDeg?: number; applied?: boolean }>; | |
| 86 | 87 | |
| 87 | 88 | /** POST to the app as the page does (PUT for /settings), signed afresh for each request (a signature is good for a minute). */ |
| 88 | 89 | export function poster(env: Env, fetcher: typeof fetch = fetch): Post { |
| 89 | 90 | return async (path, payload = {}) => { |
| 90 | 91 | try { |
| 91 | 92 | const r = await fetcher(`${appUrl(env)}${path}`, { method: path === "/settings" ? "PUT" : "POST", headers: { "content-type": "application/json", ...terminalHeaders(env) }, body: JSON.stringify(payload), signal: AbortSignal.timeout(5000) }); |
| 92 | const body = (await r.json().catch(() => ({}))) as { error?: unknown; tableOffsetMm?: unknown; offsetDeg?: unknown }; | |
| 93 | return { status: r.status, error: r.ok ? null : plain(body.error ?? `status ${r.status}`), ...(typeof body.tableOffsetMm === "number" ? { tableOffsetMm: body.tableOffsetMm } : {}), ...(typeof body.offsetDeg === "number" ? { offsetDeg: body.offsetDeg } : {}) }; | |
| 93 | const body = (await r.json().catch(() => ({}))) as { error?: unknown; tableOffsetMm?: unknown; heightMm?: unknown; tiltDeg?: unknown; applied?: unknown; offsetDeg?: unknown }; | |
| 94 | return { status: r.status, error: r.ok ? null : plain(body.error ?? `status ${r.status}`), ...(typeof body.tableOffsetMm === "number" ? { tableOffsetMm: body.tableOffsetMm } : {}), ...(typeof body.heightMm === "number" ? { heightMm: body.heightMm } : {}), ...(typeof body.tiltDeg === "number" ? { tiltDeg: body.tiltDeg } : {}), ...(typeof body.applied === "boolean" ? { applied: body.applied } : {}), ...(typeof body.offsetDeg === "number" ? { offsetDeg: body.offsetDeg } : {}) }; | |
| 94 | 95 | } catch (err) { |
| 95 | 96 | return { status: 0, error: plain((err as Error).message) }; |
| 96 | 97 | } |
| @@ -211,7 +212,7 @@ const said = (room: Room, m: unknown): string => { | ||
| 211 | 212 | }; |
| 212 | 213 | |
| 213 | 214 | const ZERO_USAGE = "usage: /zero <joint, e.g. shoulder_pan> [degrees the model should read, default 0]"; |
| 214 | export const HELP = "type to chat · /join /leave /ready /extend · /stop /resume (driver, owner) · /end [who] /table here|<mm> /zero <joint> [deg] (owner) · /mute <who> /unmute <who> (owner, moderator) · /delete <who|#id> (owner, moderator) · /report <who|#id> · /reports /dismiss #<id> (owner, moderator) · /removed /restore #<id> /visitors (owner) · /health /precision (owner, moderator) · /machine [off] · /who · /quit"; | |
| 215 | export const HELP = "type to chat · /join /leave /ready /extend · /stop /resume (driver, owner) · /end [who] /table here|touch 1|2|3|<mm> /zero <joint> [deg] (owner) · /mute <who> /unmute <who> (owner, moderator) · /delete <who|#id> (owner, moderator) · /report <who|#id> · /reports /dismiss #<id> (owner, moderator) · /removed /restore #<id> /visitors (owner) · /health /precision (owner, moderator) · /machine [off] · /who · /quit"; | |
| 215 | 216 | |
| 216 | 217 | /** The Machine feed's socket beside the lobby's: same app, same port. */ |
| 217 | 218 | export const machineUrl = (live: string): string => live.replace(/\/live$/, "/machine"); |
| @@ -354,7 +355,7 @@ function target(room: Room, word: string): string | null { | ||
| 354 | 355 | return ID.test(word) ? word : null; |
| 355 | 356 | } |
| 356 | 357 | |
| 357 | export type Typed = { send: Frame } | { post: "/stop" | "/resume" } | { table: "here" | number } | { zero: { joint: string; referenceDeg: number } } | { print: string } | { lines: string[] } | { machine: boolean } | { health: true } | { precision: true } | { quit: true } | null; | |
| 358 | export type Typed = { send: Frame } | { post: "/stop" | "/resume" } | { table: "here" | number | { touch: 1 | 2 | 3 } } | { zero: { joint: string; referenceDeg: number } } | { print: string } | { lines: string[] } | { machine: boolean } | { health: true } | { precision: true } | { quit: true } | null; | |
| 358 | 359 | |
| 359 | 360 | /** What a typed line asks for. Validation the server repeats anyway; this only saves a round trip on a typo. */ |
| 360 | 361 | export function typed(room: Room, input: string): Typed { |
| @@ -369,7 +370,8 @@ export function typed(room: Room, input: string): Typed { | ||
| 369 | 370 | } |
| 370 | 371 | const timed = /^mute$/i.test(verb) && rest.length === 1 && /^(10m|1h|24h|forever)$/i.test(rest[0]!); |
| 371 | 372 | if (rest.length && !timed && /^unmute$/i.test(verb)) return { print: "usage: /unmute <name>" }; |
| 372 | if (rest.length && !timed) return { print: `error: one name at a time` }; | |
| 373 | const touching = /^table$/i.test(verb) && /^touch$/i.test(arg) && rest.length === 1; | |
| 374 | if (rest.length && !timed && !touching) return { print: `error: one name at a time` }; | |
| 373 | 375 | switch (verb.toLowerCase()) { |
| 374 | 376 | case "join": |
| 375 | 377 | case "leave": |
| @@ -421,8 +423,10 @@ export function typed(room: Room, input: string): Typed { | ||
| 421 | 423 | return { post: `/${verb.toLowerCase()}` as "/stop" | "/resume" }; |
| 422 | 424 | case "table": { |
| 423 | 425 | if (arg.toLowerCase() === "here") return { table: "here" }; |
| 426 | const touch = touching ? /^([123])$/.exec(rest[0]!) : null; | |
| 427 | if (touch) return { table: { touch: Number(touch[1]) as 1 | 2 | 3 } }; | |
| 424 | 428 | if (/^-?\d{1,3}$/.test(arg) && Math.abs(Number(arg)) <= 100) return { table: Number(arg) }; |
| 425 | return { print: "usage: /table here | /table <mm, -100 to 100>" }; | |
| 429 | return { print: "usage: /table here | /table touch 1|2|3 | /table <mm, -100 to 100>" }; | |
| 426 | 430 | } |
| 427 | 431 | case "health": |
| 428 | 432 | return { health: true }; |
| @@ -532,10 +536,11 @@ export class Door { | ||
| 532 | 536 | } |
| 533 | 537 | |
| 534 | 538 | /** The server decides who may; a refusal is printed, success says the offset the server now holds. */ |
| 535 | private async table(what: "here" | number): Promise<void> { | |
| 539 | private async table(what: "here" | number | { touch: 1 | 2 | 3 }): Promise<void> { | |
| 536 | 540 | if (!this.post) return this.print("error: the table is not available here"); |
| 537 | const r = await (what === "here" ? this.post("/settings/table-here") : this.post("/settings", { tableOffsetMm: what })); | |
| 541 | const r = await (what === "here" ? this.post("/settings/table-here") : typeof what === "object" ? this.post("/settings/table-touch", { n: what.touch }) : this.post("/settings", { tableOffsetMm: what })); | |
| 538 | 542 | if (r.error) this.print(`error: ${r.error}`); |
| 543 | else if (typeof what === "object") this.print(r.heightMm === undefined ? `touch ${what.touch} taken` : `table is ${r.heightMm} mm at the base, tilt ${r.tiltDeg}°, ${r.applied ? `applied: offset ${r.tableOffsetMm} mm` : "not applied"}`); | |
| 539 | 544 | else this.print(r.tableOffsetMm === undefined ? "table set" : `table offset is ${r.tableOffsetMm} mm`); |
| 540 | 545 | } |
| 541 | 546 |
new above
offline