Public: anyone can read this; the crew can change it.
Migrated from github.com/Trevato/SO-ARM100
workbenchthis machine
The 3D viewer draws the table at the model floor, not at the enforced plane #183
Check · Review · Land done · Live — Landed; nothing deploys from this branch, so this is the end of its path.
vesper/arm:vesper/179-the-3d-viewer-draws-the-table-at-the-mod → main 2 commits fork
9 files
changed +83
−16
Software/README.md +4 −2
| @@ -631,7 +631,9 @@ only reports; `off` skips the check. Margins and per-joint trims live in | ||
| 631 | 631 | `settings.json` beside the poses. The model takes the middle of each |
| 632 | 632 | calibrated range as zero (LeRobot's convention), which can sit a centimetre |
| 633 | 633 | or two off the real arm at the gripper: compare the 3D view with the camera |
| 634 | and trim. On a stream, a joint that pushes hard without moving is a jam: it | |
| 634 | and trim. The 3D view draws the table where the zone enforces it: `tableZ` | |
| 635 | (meters, the model's frame) in `/model.json` and on every status, so when the | |
| 636 | table is moved the drawing moves with it. On a stream, a joint that pushes hard without moving is a jam: it | |
| 635 | 637 | is parked where it is and follows again once the leader backs off. Jams, |
| 636 | 638 | blocks, stalls, peak loads and temperatures go to `metrics.jsonl`. |
| 637 | 639 | |
| @@ -658,7 +660,7 @@ rest the arm on its table and, as an admin, `POST /settings/table-here`: the | ||
| 658 | 660 | offset becomes the arm's lowest link now, less the floor margin, rounded |
| 659 | 661 | down, so the resting arm is not flagged. The answer is 409 when the arm is |
| 660 | 662 | more than 100 mm from where the table could be; lower it first. The 3D |
| 661 | view still draws the model's table. | |
| 663 | view draws the table there. | |
| 662 | 664 | |
| 663 | 665 | The stream's targets go through a governor. At the command rate, it moves |
| 664 | 666 | each joint's goal toward the target on a trapezoid: it never goes past the |
Software/so101/src/kinematics.test.ts +12
| @@ -65,6 +65,18 @@ describe("the SO-101 model", () => { | ||
| 65 | 65 | expect(v === null || v.what === "base" || v.what === "table").toBe(true); |
| 66 | 66 | }); |
| 67 | 67 | |
| 68 | test("the table offset moves the plane the zone enforces, and tableZ says where it is", () => { | |
| 69 | const rest = deg([0, 0, 0, 0, 0, 0]); | |
| 70 | const settings = { floorMargin: 0.005, baseMargin: 0.005 }; | |
| 71 | expect(model.tableZ(settings)).toBe(model.floorZ); | |
| 72 | const lowest = model.lowest(rest).z; | |
| 73 | const raised = { ...settings, tableOffset: lowest - model.floorZ + 0.01 }; // the table 10 mm above the arm's lowest part | |
| 74 | expect(model.tableZ(raised)).toBeCloseTo(lowest + 0.01, 9); | |
| 75 | const v = model.check(rest, raised); | |
| 76 | expect(v?.what).toBe("table"); | |
| 77 | expect(v!.depth).toBeCloseTo(0.015, 6); // 10 mm under the moved table, plus the margin | |
| 78 | }); | |
| 79 | ||
| 68 | 80 | test("ticks map to radians around the middle of the calibrated range, and back", () => { |
| 69 | 81 | expect(ticksToRadians(2048, 1000, 3096)).toBe(0); |
| 70 | 82 | expect(ticksToRadians(2048 + 4095 / 4, 1000, 3096)).toBeCloseTo(Math.PI / 2, 9); |
Software/so101/src/kinematics.ts +6 −1
| @@ -277,6 +277,11 @@ export class ArmModel { | ||
| 277 | 277 | this.base = new HeightMap(base); |
| 278 | 278 | } |
| 279 | 279 | |
| 280 | /** The table the zone enforces, in base_link coordinates: the model's floor moved by the setting's offset. */ | |
| 281 | tableZ(settings: ZoneSettings): number { | |
| 282 | return this.floorZ + (settings.tableOffset ?? 0); | |
| 283 | } | |
| 284 | ||
| 280 | 285 | /** Read a URDF and the binary STL meshes it points at (paths relative to the URDF's folder). */ |
| 281 | 286 | static load(urdfPath: string): ArmModel { |
| 282 | 287 | const dir = dirname(urdfPath); |
| @@ -354,7 +359,7 @@ export class ArmModel { | ||
| 354 | 359 | const poses = this.linkPoses(q); |
| 355 | 360 | let worst: Violation | null = null; |
| 356 | 361 | let lowestZ = Infinity, lowestLink = ""; |
| 357 | const floor = this.floorZ + (settings.tableOffset ?? 0) + settings.floorMargin; | |
| 362 | const floor = this.tableZ(settings) + settings.floorMargin; | |
| 358 | 363 | for (const [name, link] of this.links) { |
| 359 | 364 | if (name === this.root) continue; |
| 360 | 365 | const T = poses.get(name); |
Software/so101/src/server.test.ts +8 −3
| @@ -509,9 +509,10 @@ describe("the safe zone, the jam watch, and the model routes", () => { | ||
| 509 | 509 | afterAll(() => Promise.race([zoneServer.close(), sleep(500)])); |
| 510 | 510 | |
| 511 | 511 | test("serves the model, its meshes and the vendored libraries, and nothing outside them", async () => { |
| 512 | const info = (await (await fetch(`${zurl}/model.json`)).json()) as { urdf: string; joints: Array<{ name: string; id: number }>; floorZ: number }; | |
| 512 | const info = (await (await fetch(`${zurl}/model.json`)).json()) as { urdf: string; joints: Array<{ name: string; id: number }>; floorZ: number; tableZ: number }; | |
| 513 | 513 | expect(info.joints.map((j) => j.id)).toEqual([1, 2, 3, 4, 5, 6]); |
| 514 | 514 | expect(info.floorZ).toBeLessThan(0); |
| 515 | expect(info.tableZ).toBe(info.floorZ); // no table offset: the enforced table is the model's floor | |
| 515 | 516 | expect((await fetch(`${zurl}${info.urdf}`)).headers.get("content-type")).toBe("application/xml"); |
| 516 | 517 | expect((await fetch(`${zurl}/model/assets/base_so101_v2.stl`)).headers.get("content-type")).toBe("model/stl"); |
| 517 | 518 | expect((await fetch(`${zurl}/model/../package.json`)).status).toBe(404); |
| @@ -534,8 +535,9 @@ describe("the safe zone, the jam watch, and the model routes", () => { | ||
| 534 | 535 | }); |
| 535 | 536 | |
| 536 | 537 | test("status reports the zone, and the rest pose is clear", async () => { |
| 537 | const s = (await (await fetch(`${zurl}/status`)).json()) as { zone: string; violation: unknown; lowestMm: number; blocked: unknown; jammed: number[] }; | |
| 538 | const s = (await (await fetch(`${zurl}/status`)).json()) as { zone: string; violation: unknown; lowestMm: number; blocked: unknown; jammed: number[]; tableZ: number }; | |
| 538 | 539 | expect(s.zone).toBe("warn"); |
| 540 | expect(s.tableZ).toBe(((await (await fetch(`${zurl}/model.json`)).json()) as { tableZ: number }).tableZ); // the plane the 3D view draws | |
| 539 | 541 | expect(s.violation).toBeNull(); |
| 540 | 542 | expect(s.lowestMm).toBeGreaterThan(0); |
| 541 | 543 | expect(s.jammed).toEqual([]); |
| @@ -560,8 +562,11 @@ describe("the safe zone, the jam watch, and the model routes", () => { | ||
| 560 | 562 | expect(here.status).toBe(200); |
| 561 | 563 | const set = (await here.json()) as { tableOffsetMm: number }; |
| 562 | 564 | expect(Number.isInteger(set.tableOffsetMm)).toBe(true); |
| 563 | const rest = (await (await fetch(`${zurl}/status`)).json()) as { violation: unknown; lowestMm: number }; | |
| 565 | const rest = (await (await fetch(`${zurl}/status`)).json()) as { violation: unknown; lowestMm: number; tableZ: number }; | |
| 564 | 566 | expect(rest.violation).toBeNull(); |
| 567 | const info = (await (await fetch(`${zurl}/model.json`)).json()) as { floorZ: number; tableZ: number }; | |
| 568 | expect(rest.tableZ).toBeCloseTo(info.floorZ + set.tableOffsetMm / 1000, 9); // the 3D view draws the moved table | |
| 569 | expect(info.tableZ).toBe(rest.tableZ); | |
| 565 | 570 | expect(rest.lowestMm).toBeGreaterThanOrEqual(0); |
| 566 | 571 | expect(((await (await fetch(`${zurl}/settings`)).json()) as { tableOffsetMm: number }).tableOffsetMm).toBe(set.tableOffsetMm); |
| 567 | 572 | } finally { |
Software/so101/src/server.ts +7 −4
| @@ -3,7 +3,7 @@ | ||
| 3 | 3 | * for joint targets at teleoperation rate. Runs wherever the follower's board is plugged in. |
| 4 | 4 | * |
| 5 | 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 | |
| 6 | * GET /status every joint: position, load, temperature, torque; what drives the arm; zone state and the table (tableZ) | |
| 7 | 7 | * GET /limits each servo's calibrated Min/Max_Position_Limit |
| 8 | 8 | * GET /poses the saved poses |
| 9 | 9 | * POST /pose/save {"name": "he1", "ids"?: [1,2,3,4,5]} remember where the joints are now |
| @@ -30,7 +30,7 @@ | ||
| 30 | 30 | * PUT /settings change any of those |
| 31 | 31 | * POST /settings/table-here put the table (tableOffsetMm, -100..100) under the arm's lowest link now, the margin below it |
| 32 | 32 | * GET /metrics jams, blocks, stalls, peak load and temperature per joint, recent events |
| 33 | * GET /model.json the arm model: joints, the table plane, where the URDF is served | |
| 33 | * GET /model.json the arm model: joints, the model's floor (floorZ) and the enforced table (tableZ), where the URDF is served | |
| 34 | 34 | * GET /leader-map.js the leader calibration mapping, for the browser (src/leader-map.js) |
| 35 | 35 | * GET /model/<file> the URDF and its meshes; GET /vendor/<pkg>/<file> three.js and the URDF loader |
| 36 | 36 | * GET /app/<file> the modules the page imports itself (the leader arm over Web Serial), as JavaScript |
| @@ -245,7 +245,7 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 245 | 245 | const evaluate = (positions: ReadonlyMap<number, number>) => (model && settings.zone !== "off" ? model.evaluate(toQ(positions), zoneSettings()) : null); |
| 246 | 246 | /** Only poses the arm is in, or is sent to, count toward "closest to the table"; refused frames do not. */ |
| 247 | 247 | const approach = (r: ReturnType<typeof evaluate>) => { |
| 248 | if (r && model) metrics.approach(r.lowest.z - model.floorZ - settings.tableOffsetMm / 1000, r.lowest.link); | |
| 248 | if (r && model) metrics.approach(r.lowest.z - model.tableZ(zoneSettings()), r.lowest.link); | |
| 249 | 249 | }; |
| 250 | 250 | const describeViolation = (v: Violation) => { |
| 251 | 251 | const part = v.link.replace(/_link$/, "").replace(/_so101_v1$/, ""); |
| @@ -577,7 +577,9 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 577 | 577 | recording: recording?.name ?? null, |
| 578 | 578 | zone: model ? settings.zone : "off", |
| 579 | 579 | violation: zone?.violation ?? null, |
| 580 | lowestMm: zone && model ? Number(((zone.lowest.z - model.floorZ) * 1000 - settings.tableOffsetMm).toFixed(0)) : null, | |
| 580 | lowestMm: zone && model ? Number(((zone.lowest.z - model.tableZ(zoneSettings())) * 1000).toFixed(0)) : null, | |
| 581 | /** The table the zone enforces, meters in the model's frame: the 3D view draws it here, so it moves when the table does. */ | |
| 582 | tableZ: model ? model.tableZ(zoneSettings()) : null, | |
| 581 | 583 | blocked, |
| 582 | 584 | jammed: [...jams.keys()], |
| 583 | 585 | stream: streamStats, |
| @@ -764,6 +766,7 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 764 | 766 | return json(res, 200, { |
| 765 | 767 | urdf: `/model/${URDF}`, |
| 766 | 768 | floorZ: model.floorZ, |
| 769 | tableZ: model.tableZ(zoneSettings()), | |
| 767 | 770 | baseTop: model.baseTop, |
| 768 | 771 | 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 })), |
| 769 | 772 | }); |
Software/so101/src/terminal.ts +2
| @@ -10,6 +10,8 @@ | ||
| 10 | 10 | * same way, so the relay lets the driver and the admins do them and nobody else. The arm's status comes over /live |
| 11 | 11 | * too: when it is stopped, or resumed, from here or from a page, every terminal says so. |
| 12 | 12 | * Nothing here decides who may do what; the server does, and says why not. |
| 13 | * No arm settings either (setting the table plane among them): those are made looking at the arm in the page's 3D | |
| 14 | * view and camera, which a terminal does not have; the door only stops what moves. | |
| 13 | 15 | * |
| 14 | 16 | * Typed lines: |
| 15 | 17 | * <text> say it |
Software/so101/src/ui/table.js added +12
| @@ -0,0 +1,12 @@ | ||
| 1 | /** | |
| 2 | * Where the 3D view draws the table: the plane the safe zone enforces, as the arm's server says it is now | |
| 3 | * (/status `tableZ`, which follows the table-offset setting), else as /model.json had it, else the model's own floor. | |
| 4 | * Meters in the model's frame; null when nothing says. | |
| 5 | * @param {{ tableZ?: unknown } | null | undefined} status | |
| 6 | * @param {{ tableZ?: unknown, floorZ?: unknown } | null | undefined} modelInfo | |
| 7 | * @returns {number | null} | |
| 8 | */ | |
| 9 | export function tablePlaneZ(status, modelInfo) { | |
| 10 | for (const z of [status?.tableZ, modelInfo?.tableZ, modelInfo?.floorZ]) if (typeof z === "number" && Number.isFinite(z)) return z; | |
| 11 | return null; | |
| 12 | } |
Software/so101/src/ui/table.test.ts added +18
| @@ -0,0 +1,18 @@ | ||
| 1 | import { describe, expect, test } from "bun:test"; | |
| 2 | import { tablePlaneZ } from "./table.js"; | |
| 3 | ||
| 4 | describe("where the 3D view draws the table", () => { | |
| 5 | test("at the enforced plane the status carries, not the model's floor", () => { | |
| 6 | expect(tablePlaneZ({ tableZ: 0.012 }, { floorZ: -0.008, tableZ: -0.008 })).toBe(0.012); | |
| 7 | }); | |
| 8 | ||
| 9 | test("before a status, at the plane /model.json gave; from an older server, at its floor", () => { | |
| 10 | expect(tablePlaneZ(null, { floorZ: -0.008, tableZ: 0.004 })).toBe(0.004); | |
| 11 | expect(tablePlaneZ({ tableZ: null }, { floorZ: -0.008 })).toBe(-0.008); | |
| 12 | }); | |
| 13 | ||
| 14 | test("a value that is not a finite number is not drawn", () => { | |
| 15 | expect(tablePlaneZ({ tableZ: "1" }, { floorZ: Number.NaN })).toBeNull(); | |
| 16 | expect(tablePlaneZ(undefined, undefined)).toBeNull(); | |
| 17 | }); | |
| 18 | }); |
Software/so101/src/viewer.html +14 −6
| @@ -1207,14 +1207,14 @@ | ||
| 1207 | 1207 | // ---------------------------------------------------------------- the 3D model |
| 1208 | 1208 | // three.js is most of what the page downloads, so it comes after the first view: the room and the camera work without it. |
| 1209 | 1209 | const viewer = $("model"); |
| 1210 | let THREE = null, model3D = null, dragging = null, table = null, marker = null; | |
| 1210 | let THREE = null, model3D = null, dragging = null, table = null, grid = null, marker = null, tablePlaneZ = null; | |
| 1211 | 1211 | function load3D() { |
| 1212 | 1212 | if (!modelInfo?.ok) return Promise.resolve(false); |
| 1213 | 1213 | return (model3D ??= (async () => { |
| 1214 | const [three, { STLLoader }, { default: URDFManipulator }, reach] = await Promise.all([ | |
| 1215 | import("three"), import("three/examples/jsm/loaders/STLLoader.js"), import("urdf-loader/manipulator"), import("/ui/__VERSION__/reach.js"), | |
| 1214 | const [three, { STLLoader }, { default: URDFManipulator }, reach, plane] = await Promise.all([ | |
| 1215 | import("three"), import("three/examples/jsm/loaders/STLLoader.js"), import("urdf-loader/manipulator"), import("/ui/__VERSION__/reach.js"), import("/ui/__VERSION__/table.js"), | |
| 1216 | 1216 | ]); |
| 1217 | THREE = three; reachMath = reach; | |
| 1217 | THREE = three; reachMath = reach; tablePlaneZ = plane.tablePlaneZ; | |
| 1218 | 1218 | if (!customElements.get("urdf-viewer")) customElements.define("urdf-viewer", URDFManipulator); |
| 1219 | 1219 | viewer.loadMeshFunc = (path, manager, material, done) => { |
| 1220 | 1220 | new STLLoader(manager).load(path, (geometry) => done(new THREE.Mesh(geometry, material ?? new THREE.MeshPhongMaterial({ color: 0x9aa4ae, shininess: 30 }))), undefined, (err) => done(null, err)); |
| @@ -1234,8 +1234,8 @@ | ||
| 1234 | 1234 | viewer.addEventListener("urdf-processed", () => { |
| 1235 | 1235 | if (!modelInfo || !THREE) return; |
| 1236 | 1236 | table = new THREE.Mesh(new THREE.PlaneGeometry(0.9, 0.9), new THREE.MeshPhongMaterial({ color: 0x2a2f36, side: THREE.DoubleSide, transparent: true, opacity: 0.85 })); |
| 1237 | table.position.z = modelInfo.floorZ - 0.0005; | |
| 1238 | const grid = new THREE.GridHelper(0.9, 18, 0x4a5058, 0x30363d); grid.rotation.x = Math.PI / 2; grid.position.z = modelInfo.floorZ; | |
| 1237 | grid = new THREE.GridHelper(0.9, 18, 0x4a5058, 0x30363d); grid.rotation.x = Math.PI / 2; | |
| 1238 | placeTable(status); | |
| 1239 | 1239 | marker = new THREE.Mesh(new THREE.SphereGeometry(0.012, 16, 12), new THREE.MeshBasicMaterial({ color: 0xf85149 })); marker.visible = false; |
| 1240 | 1240 | for (const o of [table, grid, marker]) o.raycast = () => {}; |
| 1241 | 1241 | viewer.world.add(table, grid, marker); |
| @@ -1256,7 +1256,15 @@ | ||
| 1256 | 1256 | intent.set(id, ticks, performance.now()); |
| 1257 | 1257 | stream.send({ [id]: ticks }); |
| 1258 | 1258 | }); |
| 1259 | /** The table and its grid sit on the plane the zone enforces (src/ui/table.js), so moving the table moves the drawing. */ | |
| 1260 | function placeTable(d) { | |
| 1261 | const z = table && tablePlaneZ?.(d, modelInfo); | |
| 1262 | if (z == null) return; | |
| 1263 | table.position.z = z - 0.0005; | |
| 1264 | grid.position.z = z; | |
| 1265 | } | |
| 1259 | 1266 | function applyModel(d) { |
| 1267 | placeTable(d); | |
| 1260 | 1268 | if (!viewer.robot || calib) return; // while calibrating, the model shows the pose to copy |
| 1261 | 1269 | for (const j of leaderMirrored() ? [] : d.joints) { |
| 1262 | 1270 | const name = NAMES[j.id]; |
new above
offline