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

Merged opened this

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

Merged

vesper/arm:vesper/179-the-3d-viewer-draws-the-table-at-the-mod → main 2 commits fork

  • Merged. vesper/179-the-3d-viewer-draws-the-table-at-the-mod was merged into main as 45b7233.

Review

Sign in to review this change.
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
631631 `settings.json` beside the poses. The model takes the middle of each
632632 calibrated range as zero (LeRobot's convention), which can sit a centimetre
633633 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
635637 is parked where it is and follows again once the leader backs off. Jams,
636638 blocks, stalls, peak loads and temperatures go to `metrics.jsonl`.
637639
@@ -658,7 +660,7 @@ rest the arm on its table and, as an admin, `POST /settings/table-here`: the
658660 offset becomes the arm's lowest link now, less the floor margin, rounded
659661 down, so the resting arm is not flagged. The answer is 409 when the arm is
660662 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.
662664
663665 The stream's targets go through a governor. At the command rate, it moves
664666 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", () => {
6565 expect(v === null || v.what === "base" || v.what === "table").toBe(true);
6666 });
6767
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
6880 test("ticks map to radians around the middle of the calibrated range, and back", () => {
6981 expect(ticksToRadians(2048, 1000, 3096)).toBe(0);
7082 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 {
277277 this.base = new HeightMap(base);
278278 }
279279
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
280285 /** Read a URDF and the binary STL meshes it points at (paths relative to the URDF's folder). */
281286 static load(urdfPath: string): ArmModel {
282287 const dir = dirname(urdfPath);
@@ -354,7 +359,7 @@ export class ArmModel {
354359 const poses = this.linkPoses(q);
355360 let worst: Violation | null = null;
356361 let lowestZ = Infinity, lowestLink = "";
357 const floor = this.floorZ + (settings.tableOffset ?? 0) + settings.floorMargin;
362 const floor = this.tableZ(settings) + settings.floorMargin;
358363 for (const [name, link] of this.links) {
359364 if (name === this.root) continue;
360365 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", () => {
509509 afterAll(() => Promise.race([zoneServer.close(), sleep(500)]));
510510
511511 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 };
513513 expect(info.joints.map((j) => j.id)).toEqual([1, 2, 3, 4, 5, 6]);
514514 expect(info.floorZ).toBeLessThan(0);
515 expect(info.tableZ).toBe(info.floorZ); // no table offset: the enforced table is the model's floor
515516 expect((await fetch(`${zurl}${info.urdf}`)).headers.get("content-type")).toBe("application/xml");
516517 expect((await fetch(`${zurl}/model/assets/base_so101_v2.stl`)).headers.get("content-type")).toBe("model/stl");
517518 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", () => {
534535 });
535536
536537 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 };
538539 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
539541 expect(s.violation).toBeNull();
540542 expect(s.lowestMm).toBeGreaterThan(0);
541543 expect(s.jammed).toEqual([]);
@@ -560,8 +562,11 @@ describe("the safe zone, the jam watch, and the model routes", () => {
560562 expect(here.status).toBe(200);
561563 const set = (await here.json()) as { tableOffsetMm: number };
562564 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 };
564566 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);
565570 expect(rest.lowestMm).toBeGreaterThanOrEqual(0);
566571 expect(((await (await fetch(`${zurl}/settings`)).json()) as { tableOffsetMm: number }).tableOffsetMm).toBe(set.tableOffsetMm);
567572 } finally {
Software/so101/src/server.ts +7 −4
@@ -3,7 +3,7 @@
33 * for joint targets at teleoperation rate. Runs wherever the follower's board is plugged in.
44 *
55 * 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)
77 * GET /limits each servo's calibrated Min/Max_Position_Limit
88 * GET /poses the saved poses
99 * POST /pose/save {"name": "he1", "ids"?: [1,2,3,4,5]} remember where the joints are now
@@ -30,7 +30,7 @@
3030 * PUT /settings change any of those
3131 * POST /settings/table-here put the table (tableOffsetMm, -100..100) under the arm's lowest link now, the margin below it
3232 * 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
3434 * GET /leader-map.js the leader calibration mapping, for the browser (src/leader-map.js)
3535 * GET /model/<file> the URDF and its meshes; GET /vendor/<pkg>/<file> three.js and the URDF loader
3636 * 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 = {}):
245245 const evaluate = (positions: ReadonlyMap<number, number>) => (model && settings.zone !== "off" ? model.evaluate(toQ(positions), zoneSettings()) : null);
246246 /** Only poses the arm is in, or is sent to, count toward "closest to the table"; refused frames do not. */
247247 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);
249249 };
250250 const describeViolation = (v: Violation) => {
251251 const part = v.link.replace(/_link$/, "").replace(/_so101_v1$/, "");
@@ -577,7 +577,9 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}):
577577 recording: recording?.name ?? null,
578578 zone: model ? settings.zone : "off",
579579 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,
581583 blocked,
582584 jammed: [...jams.keys()],
583585 stream: streamStats,
@@ -764,6 +766,7 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}):
764766 return json(res, 200, {
765767 urdf: `/model/${URDF}`,
766768 floorZ: model.floorZ,
769 tableZ: model.tableZ(zoneSettings()),
767770 baseTop: model.baseTop,
768771 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 })),
769772 });
Software/so101/src/terminal.ts +2
@@ -10,6 +10,8 @@
1010 * same way, so the relay lets the driver and the admins do them and nobody else. The arm's status comes over /live
1111 * too: when it is stopped, or resumed, from here or from a page, every terminal says so.
1212 * 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.
1315 *
1416 * Typed lines:
1517 * <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 @@
12071207 // ---------------------------------------------------------------- the 3D model
12081208 // three.js is most of what the page downloads, so it comes after the first view: the room and the camera work without it.
12091209 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;
12111211 function load3D() {
12121212 if (!modelInfo?.ok) return Promise.resolve(false);
12131213 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"),
12161216 ]);
1217 THREE = three; reachMath = reach;
1217 THREE = three; reachMath = reach; tablePlaneZ = plane.tablePlaneZ;
12181218 if (!customElements.get("urdf-viewer")) customElements.define("urdf-viewer", URDFManipulator);
12191219 viewer.loadMeshFunc = (path, manager, material, done) => {
12201220 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 @@
12341234 viewer.addEventListener("urdf-processed", () => {
12351235 if (!modelInfo || !THREE) return;
12361236 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);
12391239 marker = new THREE.Mesh(new THREE.SphereGeometry(0.012, 16, 12), new THREE.MeshBasicMaterial({ color: 0xf85149 })); marker.visible = false;
12401240 for (const o of [table, grid, marker]) o.raycast = () => {};
12411241 viewer.world.add(table, grid, marker);
@@ -1256,7 +1256,15 @@
12561256 intent.set(id, ticks, performance.now());
12571257 stream.send({ [id]: ticks });
12581258 });
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 }
12591266 function applyModel(d) {
1267 placeTable(d);
12601268 if (!viewer.robot || calib) return; // while calibrating, the model shows the pose to copy
12611269 for (const j of leaderMirrored() ? [] : d.joints) {
12621270 const name = NAMES[j.id];

↑↓ navigate ↵ open ⌘↵ its app esc close

Keyboard shortcuts

Docs