Public: anyone can read this; the crew can change it.
Migrated from github.com/Trevato/SO-ARM100
workbenchthis machine
The table plane can be set: a tableOffsetMm setting and POST /settings/table-here from the arm's lowest point now #173
Check · Review · Land done · Live — Landed; nothing deploys from this branch, so this is the end of its path.
smith/arm:smith/162-the-table-plane-can-be-set-a-tableoffset → main 2 commits fork 2 threads · 2 unresolved
6 files
changed +79
−6
Software/README.md +9
| @@ -647,6 +647,15 @@ conservative. The owner changes them in the settings panel, or with | ||
| 647 | 647 | | `parkSpeedDegS` | 20 | how fast the park move goes | |
| 648 | 648 | | `gripperTorquePercent` | 35 | the gripper's force limit on the stream, `/grip`, moves and playbacks, and while it holds | |
| 649 | 649 | |
| 650 | **The table plane.** The zone's table starts at the bottom of the model's | |
| 651 | base, which is rarely where the real table is. `tableOffsetMm` (-100 to 100, | |
| 652 | default 0, in `settings.json`) moves it up or down. To set it from the arm, | |
| 653 | rest the arm on its table and, as an admin, `POST /settings/table-here`: the | |
| 654 | offset becomes the arm's lowest link now, less the floor margin, rounded | |
| 655 | down, so the resting arm is not flagged. The answer is 409 when the arm is | |
| 656 | more than 100 mm from where the table could be; lower it first. The 3D | |
| 657 | view still draws the model's table. | |
| 658 | ||
| 650 | 659 | The stream's targets go through a governor. At the command rate, it moves |
| 651 | 660 | each joint's goal toward the target on a trapezoid: it never goes past the |
| 652 | 661 | speed, never changes speed faster than the acceleration, and brakes in time |
Software/so101/src/kinematics.ts +3 −1
| @@ -219,6 +219,8 @@ export interface ZoneSettings { | ||
| 219 | 219 | floorMargin: number; |
| 220 | 220 | /** Moving parts stay this far outside the base's column, meters. */ |
| 221 | 221 | baseMargin: number; |
| 222 | /** The table plane's height above the model's own (the bottom of the base), meters; 0 when absent. */ | |
| 223 | tableOffset?: number; | |
| 222 | 224 | } |
| 223 | 225 | |
| 224 | 226 | export interface Violation { |
| @@ -352,7 +354,7 @@ export class ArmModel { | ||
| 352 | 354 | const poses = this.linkPoses(q); |
| 353 | 355 | let worst: Violation | null = null; |
| 354 | 356 | let lowestZ = Infinity, lowestLink = ""; |
| 355 | const floor = this.floorZ + settings.floorMargin; | |
| 357 | const floor = this.floorZ + (settings.tableOffset ?? 0) + settings.floorMargin; | |
| 356 | 358 | for (const [name, link] of this.links) { |
| 357 | 359 | if (name === this.root) continue; |
| 358 | 360 | const T = poses.get(name); |
Software/so101/src/lobby.test.ts +4
| @@ -345,6 +345,10 @@ describe("lobby", () => { | ||
| 345 | 345 | expect((await torque(port, ALICE)).status).toBe(423); // stopped: nothing moves |
| 346 | 346 | expect((await post("/estop/clear", ALICE)).status).toBe(403); |
| 347 | 347 | expect((await post("/settings", ALICE, { maxSpeedDegS: 360 }, "PUT")).status).toBe(403); |
| 348 | expect((await post("/settings/table-here", {})).status).toBe(401); | |
| 349 | expect((await post("/settings/table-here", ALICE)).status).toBe(403); | |
| 350 | expect((await post("/settings/table-here", OWNER)).status).toBe(200); // an admin gets through | |
| 351 | expect((await post("/settings", OWNER, { tableOffsetMm: 0 }, "PUT")).status).toBe(200); | |
| 348 | 352 | expect((await post("/settings", OWNER, { maxSpeedDegS: 60 }, "PUT")).status).toBe(200); |
| 349 | 353 | expect((await post("/settings", OWNER, { maxSpeedDegS: 90 }, "PUT")).status).toBe(200); |
| 350 | 354 | expect((await post("/estop/clear", OWNER)).status).toBe(200); |
Software/so101/src/relay.ts +1 −1
| @@ -142,7 +142,7 @@ const PAGE = /^\/(?:index\.html)?$/; | ||
| 142 | 142 | const STATIC = /^\/(?:index\.html)?$|^\/(?:model|vendor|ui)\//; |
| 143 | 143 | const LEADER = "/leader/calibration"; |
| 144 | 144 | /** Admins only: clearing an emergency stop and changing the settings (the safety limits among them). */ |
| 145 | const ADMIN_ONLY = new Set(["POST /estop/clear", "PUT /settings"]); | |
| 145 | const ADMIN_ONLY = new Set(["POST /estop/clear", "PUT /settings", "POST /settings/table-here"]); | |
| 146 | 146 | /** The driver's, and any admin's at any time. */ |
| 147 | 147 | const ADMIN_TOO = new Set(["POST /stop", "POST /resume", "POST /estop"]); |
| 148 | 148 | /** What still goes to the arm server while the link is reconnecting: stopping it. */ |
Software/so101/src/server.test.ts +39
| @@ -551,6 +551,24 @@ describe("the safe zone, the jam watch, and the model routes", () => { | ||
| 551 | 551 | expect(again.jointOffsetsDeg).toEqual({ wrist_flex: 2 }); |
| 552 | 552 | }); |
| 553 | 553 | |
| 554 | test("table-here puts the table under the resting arm; an out-of-range offset is a 400", async () => { | |
| 555 | expect((await zcall("/settings", { tableOffsetMm: 101 }, "PUT")).status).toBe(400); | |
| 556 | expect((await zcall("/settings", { tableOffsetMm: "up" }, "PUT")).status).toBe(400); | |
| 557 | const saved = (await (await fetch(`${zurl}/settings`)).json()) as { tableOffsetMm: number }; | |
| 558 | try { | |
| 559 | const here = await zcall("/settings/table-here", {}); | |
| 560 | expect(here.status).toBe(200); | |
| 561 | const set = (await here.json()) as { tableOffsetMm: number }; | |
| 562 | expect(Number.isInteger(set.tableOffsetMm)).toBe(true); | |
| 563 | const rest = (await (await fetch(`${zurl}/status`)).json()) as { violation: unknown; lowestMm: number }; | |
| 564 | expect(rest.violation).toBeNull(); | |
| 565 | expect(rest.lowestMm).toBeGreaterThanOrEqual(0); | |
| 566 | expect(((await (await fetch(`${zurl}/settings`)).json()) as { tableOffsetMm: number }).tableOffsetMm).toBe(set.tableOffsetMm); | |
| 567 | } finally { | |
| 568 | await zcall("/settings", { tableOffsetMm: saved.tableOffsetMm }, "PUT"); | |
| 569 | } | |
| 570 | }); | |
| 571 | ||
| 554 | 572 | test("a move into the table is refused in enforce mode and only reported in warn mode", async () => { |
| 555 | 573 | const down = { targets: { "2": ticks(2, 80) } }; // shoulder lift far forward with the elbow straight: the gripper goes under the table |
| 556 | 574 | const refused = await zcall("/move", down); |
| @@ -594,6 +612,27 @@ describe("the safe zone, the jam watch, and the model routes", () => { | ||
| 594 | 612 | } |
| 595 | 613 | }); |
| 596 | 614 | |
| 615 | test("moving the table changes what is refused", async () => { | |
| 616 | const saved = (await (await fetch(`${zurl}/settings`)).json()) as { tableOffsetMm: number; zone: string }; | |
| 617 | await zcall("/settings", { zone: "enforce", tableOffsetMm: 0 }, "PUT"); | |
| 618 | try { | |
| 619 | let deg = 0; | |
| 620 | for (const d of [10, 20, 30, 40, 50, 60, 70, 80]) { | |
| 621 | if ((await zcall("/move", { targets: { "2": ticks(2, d) } })).status === 409) { | |
| 622 | deg = d; | |
| 623 | break; | |
| 624 | } | |
| 625 | await moved({ "2": mid(2) }); | |
| 626 | } | |
| 627 | expect(deg).toBeGreaterThan(0); // some forward lean is refused with the table where the model has it | |
| 628 | await zcall("/settings", { tableOffsetMm: -100 }, "PUT"); | |
| 629 | expect((await zcall("/move", { targets: { "2": ticks(2, deg) } })).status).toBe(200); // the table 100 mm lower: allowed | |
| 630 | await moved({ "2": mid(2) }); | |
| 631 | } finally { | |
| 632 | await zcall("/settings", { zone: saved.zone, tableOffsetMm: saved.tableOffsetMm }, "PUT"); | |
| 633 | } | |
| 634 | }); | |
| 635 | ||
| 597 | 636 | test("a stream frame into the table holds the arm; a safe frame resumes it", async () => { |
| 598 | 637 | const ws = new WebSocket(`ws://127.0.0.1:${zoneServer.port}/follow`); |
| 599 | 638 | await new Promise((r) => ws.once("open", r)); |
Software/so101/src/server.ts +23 −4
| @@ -28,6 +28,7 @@ | ||
| 28 | 28 | * GET /settings the safe zone: {"zone": "enforce"|"warn"|"off", "floorMarginMm", "baseMarginMm", "jointOffsetsDeg"}, |
| 29 | 29 | * and the safety limits of src/safety.ts (speed, acceleration, rate, dead-man, park, gripper force, joint limits) |
| 30 | 30 | * PUT /settings change any of those |
| 31 | * POST /settings/table-here put the table (tableOffsetMm, -100..100) under the arm's lowest link now, the margin below it | |
| 31 | 32 | * GET /metrics jams, blocks, stalls, peak load and temperature per joint, recent events |
| 32 | 33 | * GET /model.json the arm model: joints, the table plane, where the URDF is served |
| 33 | 34 | * GET /leader-map.js the leader calibration mapping, for the browser (src/leader-map.js) |
| @@ -111,11 +112,13 @@ export interface Settings extends SafetySettings { | ||
| 111 | 112 | zone: "enforce" | "warn" | "off"; |
| 112 | 113 | floorMarginMm: number; |
| 113 | 114 | baseMarginMm: number; |
| 115 | /** How far the virtual table sits above the model's (the bottom of the base), mm; the real table is rarely where the model says. */ | |
| 116 | tableOffsetMm: number; | |
| 114 | 117 | /** Per joint, degrees added to the model's angle, to trim the model onto the real arm. */ |
| 115 | 118 | jointOffsetsDeg: Record<string, number>; |
| 116 | 119 | } |
| 117 | 120 | |
| 118 | export const DEFAULT_SETTINGS: Settings = { zone: "warn", floorMarginMm: 15, baseMarginMm: 10, jointOffsetsDeg: {}, ...DEFAULT_SAFETY }; | |
| 121 | export const DEFAULT_SETTINGS: Settings = { zone: "warn", floorMarginMm: 15, baseMarginMm: 10, tableOffsetMm: 0, jointOffsetsDeg: {}, ...DEFAULT_SAFETY }; | |
| 119 | 122 | |
| 120 | 123 | /** An emergency stop in force: who hit it and when. It holds until an admin clears it, across restarts. */ |
| 121 | 124 | export interface EStop { |
| @@ -225,7 +228,7 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 225 | 228 | console.error(`settings: ${settingsFile} is not readable JSON (${err instanceof Error ? err.message : String(err)}); using the defaults, zone "${settings.zone}"`); |
| 226 | 229 | } |
| 227 | 230 | } |
| 228 | const zoneSettings = () => ({ floorMargin: settings.floorMarginMm / 1000, baseMargin: settings.baseMarginMm / 1000 }); | |
| 231 | const zoneSettings = () => ({ floorMargin: settings.floorMarginMm / 1000, baseMargin: settings.baseMarginMm / 1000, tableOffset: settings.tableOffsetMm / 1000 }); | |
| 229 | 232 | /** Joint angles for the model from servo ticks, with the trims applied. The gripper maps closed→0, open→its URDF limit. */ |
| 230 | 233 | const gripperOpen = model?.movable.find((j) => j.name === "gripper")?.upper ?? 1.745; |
| 231 | 234 | const toQ = (positions: ReadonlyMap<number, number>) => { |
| @@ -242,7 +245,7 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 242 | 245 | const evaluate = (positions: ReadonlyMap<number, number>) => (model && settings.zone !== "off" ? model.evaluate(toQ(positions), zoneSettings()) : null); |
| 243 | 246 | /** Only poses the arm is in, or is sent to, count toward "closest to the table"; refused frames do not. */ |
| 244 | 247 | const approach = (r: ReturnType<typeof evaluate>) => { |
| 245 | if (r && model) metrics.approach(r.lowest.z - model.floorZ, r.lowest.link); | |
| 248 | if (r && model) metrics.approach(r.lowest.z - model.floorZ - settings.tableOffsetMm / 1000, r.lowest.link); | |
| 246 | 249 | }; |
| 247 | 250 | const describeViolation = (v: Violation) => { |
| 248 | 251 | const part = v.link.replace(/_link$/, "").replace(/_so101_v1$/, ""); |
| @@ -527,6 +530,10 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 527 | 530 | if (typeof s[key] !== "number" || !Number.isFinite(s[key]) || s[key]! < 0 || s[key]! > 100) throw new HttpError(400, `${key} must be 0-100`); |
| 528 | 531 | out[key] = s[key]!; |
| 529 | 532 | } |
| 533 | if (s.tableOffsetMm !== undefined) { | |
| 534 | if (typeof s.tableOffsetMm !== "number" || !Number.isFinite(s.tableOffsetMm) || s.tableOffsetMm < -100 || s.tableOffsetMm > 100) throw new HttpError(400, "tableOffsetMm must be -100 to 100"); | |
| 535 | out.tableOffsetMm = s.tableOffsetMm; | |
| 536 | } | |
| 530 | 537 | if (s.jointOffsetsDeg !== undefined) { |
| 531 | 538 | if (typeof s.jointOffsetsDeg !== "object" || s.jointOffsetsDeg === null || Array.isArray(s.jointOffsetsDeg)) throw new HttpError(400, "jointOffsetsDeg must be an object"); |
| 532 | 539 | for (const [name, deg] of Object.entries(s.jointOffsetsDeg)) { |
| @@ -570,7 +577,7 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 570 | 577 | recording: recording?.name ?? null, |
| 571 | 578 | zone: model ? settings.zone : "off", |
| 572 | 579 | violation: zone?.violation ?? null, |
| 573 | lowestMm: zone && model ? Number(((zone.lowest.z - model.floorZ) * 1000).toFixed(0)) : null, | |
| 580 | lowestMm: zone && model ? Number(((zone.lowest.z - model.floorZ) * 1000 - settings.tableOffsetMm).toFixed(0)) : null, | |
| 574 | 581 | blocked, |
| 575 | 582 | jammed: [...jams.keys()], |
| 576 | 583 | stream: streamStats, |
| @@ -740,6 +747,18 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 740 | 747 | writeAtomic(settingsFile, JSON.stringify(settings, null, 1)); |
| 741 | 748 | return json(res, 200, { ...settings, model: model !== null }); |
| 742 | 749 | } |
| 750 | case "POST /settings/table-here": { | |
| 751 | // 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. | |
| 752 | if (!model) throw new HttpError(409, "no model loaded on this server"); | |
| 753 | const lowest = model.lowest(toQ(await bus.syncRead(ids, STS3215.Present_Position))); | |
| 754 | const offset = Math.floor((lowest.z - model.floorZ) * 1000 - settings.floorMarginMm); | |
| 755 | if (offset < -100 || offset > 100) { | |
outdated
The offset can exceed 100 here, and validateSettings then throws a 400 for a request that carried no input. Clamp it or return a 409 that says what to do. |
||
| 756 | 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`); | |
| 757 | } | |
| 758 | settings = validateSettings({ ...settings, tableOffsetMm: offset }); | |
| 759 | writeAtomic(settingsFile, JSON.stringify(settings, null, 1)); | |
| 760 | return json(res, 200, { ...settings, model: true }); | |
| 761 | } | |
| 743 | 762 | case "GET /model.json": |
| 744 | 763 | if (!model) return json(res, 404, { error: "no model loaded on this server" }); |
| 745 | 764 | return json(res, 200, { |
new above
offline
Needs a relay test: non-admin gets 403, admin passes.