Public: anyone can read this; the crew can change it.

Migrated from github.com/Trevato/SO-ARM100

workbenchthis machine

TOP: the arm moves when you drive it - the collision guard blocks every move because the virtual table is 3 cm too high, and the driver is not registered #150

Parked opened this in answer to
Closed without merging

quill/arm:quill/144-top-the-arm-moves-when-you-drive-it-the → main 3 commits fork

  • Closed without merging. Nothing was applied to main.

Review

Sign in to review this change.
8 files changed +278 −31
Software/README.md +11 −1
@@ -623,7 +623,17 @@ and its meshes (`--model <dir>`, or the repo layout, or `model/` beside the
623623 package) and runs every target through the arm's geometry: a pose that puts
624624 any part below the table or into the base is a violation. `enforce` refuses
625625 moves and holds a stream at the boundary until the leader backs off; `warn`
626 only reports; `off` skips the check. Margins and per-joint trims live in
626 only reports; `off` skips the check. Every target is judged against where
627 the arm already is: a move that goes no deeper into a violation than the arm
628 already is (backing out, or sideways at the same depth) always goes, so an arm
629 resting on the table can always be lifted off it. A refused move says which
630 part, how far, and the way out ("move up"); a refused stream frame is answered
631 on the stream with the same words, and the page shows them. The table is a
632 setting (`tableMm`, mm from the bottom of the model's base, negative below
633 it): until an admin sets it, it is where the arm's lowest part rests when the
634 server starts (never above it, since the real arm cannot be inside the
635 table). Rest the arm on the table and press **set it here** in the Admin
636 tab, or `POST /table/here`. Margins, the table and per-joint trims live in
627637 `settings.json` beside the poses. The model takes the middle of each
628638 calibrated range as zero (LeRobot's convention), which can sit a centimetre
629639 or two off the real arm at the gripper: compare the 3D view with the camera
Software/so101/src/kinematics.ts +3 −1
@@ -219,6 +219,8 @@ export interface ZoneSettings {
219219 floorMargin: number;
220220 /** Moving parts stay this far outside the base's column, meters. */
221221 baseMargin: number;
222 /** Where the table really is, meters above the bottom of the base (negative: below it); 0 when unset. */
223 table?: number;
222224 }
223225
224226 export interface Violation {
@@ -352,7 +354,7 @@ export class ArmModel {
352354 const poses = this.linkPoses(q);
353355 let worst: Violation | null = null;
354356 let lowestZ = Infinity, lowestLink = "";
355 const floor = this.floorZ + settings.floorMargin;
357 const floor = this.floorZ + (settings.table ?? 0) + settings.floorMargin;
356358 for (const [name, link] of this.links) {
357359 if (name === this.root) continue;
358360 const T = poses.get(name);
Software/so101/src/lobby.test.ts +56
@@ -228,6 +228,62 @@ describe("lobby", () => {
228228 await alice.next(drivingIs(null));
229229 });
230230
231 test("the owner takes control: /status names them the driver, and their slider move reaches the arm within a frame", async () => {
232 const port = await start();
233 const owner = await join(port, OWNER);
234 const statusOf = async () => (await (await fetch(`http://127.0.0.1:${port}/status`)).json()) as { driver: { id: string; name: string; owner: boolean } | null };
235 expect((await statusOf()).driver).toBeNull();
236 owner.send({ type: "join" });
237 await owner.next(drivingIs("trevato"));
238 expect((await statusOf()).driver).toEqual({ id: "u-owner", name: "trevato", owner: true });
239 const ws = new WebSocket(`ws://127.0.0.1:${port}/follow`, { headers: OWNER });
240 sockets.push(ws);
241 await new Promise((r) => ws.once("open", r));
242 const sim = `http://127.0.0.1:${simulation.port}`;
243 const target = async () => ((await (await fetch(`${sim}/status`)).json()) as { joints: Array<{ id: number; target: number | null }> }).joints.find((j) => j.id === 1)!.target;
244 const sentAt = Date.now();
245 ws.send(stamper()({ "1": 2150 }));
246 while ((await target()) !== 2150 && Date.now() - sentAt < 1000) await sleep(2);
247 expect(await target()).toBe(2150);
248 expect(Date.now() - sentAt).toBeLessThan(100); // a frame is 33 ms; the HTTP polling here is most of this
249 ws.close();
250 await sleep(50);
251 });
252
253 test("the owner's command takes control from whoever drives, after their own turn ran out too", async () => {
254 const port = await start(new Turns({ idleMs: 100 }));
255 const alice = await join(port, ALICE);
256 const owner = await join(port, OWNER);
257 alice.send({ type: "join" });
258 await owner.next(drivingIs("alice"));
259 const took = alice.next(drivingIs("trevato"));
260 expect((await torque(port, OWNER)).status).toBe(200); // no "take control first": it took control
261 await took;
262 await sleep(300);
263 expect(alice.last("state")?.driver).toBeNull(); // the owner's turn idled out
264 const taken = alice.next(drivingIs("trevato"));
265 void follow(port, OWNER); // and the next stream takes it back (then idles out again, in this test's 100 ms)
266 await taken;
267 expect((await torque(port, ALICE)).status).toBe(403); // everyone else still waits for a turn
268 });
269
270 test("a frame the relay does not pass on is answered with why, never dropped silently", async () => {
271 const port = await start();
272 const alice = await join(port, ALICE);
273 alice.send({ type: "join" });
274 await alice.next(drivingIs("alice"));
275 const ws = new WebSocket(`ws://127.0.0.1:${port}/follow`, { headers: ALICE });
276 sockets.push(ws);
277 const heard: Array<{ refused?: string }> = [];
278 ws.on("message", (d) => heard.push(JSON.parse(String(d))));
279 await new Promise((r) => ws.once("open", r));
280 ws.send(JSON.stringify({ "1": 2100 })); // no seq and t
281 await sleep(50);
282 expect(heard.at(-1)?.refused).toBe("not sent to the arm: the frame had no seq and t; reload the page");
283 ws.close();
284 await sleep(50);
285 });
286
231287 test("leader calibration: each signed-in person keeps their own, any time, turn or no turn", async () => {
232288 const port = await start();
233289 const url = `http://127.0.0.1:${port}/leader/calibration`;
Software/so101/src/lobby.ts +6 −1
@@ -145,11 +145,16 @@ export class Lobby {
145145 return !who.owner && !this.local && this.accounts.banned(who.id, who.name, from?.ip ?? null);
146146 }
147147
148 /** Why this person may not command the arm right now, or null when they may (and then it counts as activity). */
148 /**
149 * Why this person may not command the arm right now, or null when they may (and then it counts as activity).
150 * The owner (forge-signed: nobody else is) may always drive: a command of theirs takes control first, so they are
151 * the driver of record whenever they act, even after their own turn ran out.
152 */
149153 refusal(who: Who, req?: IncomingMessage): { status: number; error: string } | null {
150154 if (this.local) return null;
151155 if (!who) return { status: 401, error: "join to take control" };
152156 if (this.barred(who, req ? visitor(req) : null)) return { status: 403, error: "you are banned: you can still watch" };
157 if (who.owner && !this.turns.isDriver(who.id)) this.takeControl(who);
153158 if (!this.turns.isDriver(who.id)) {
154159 const d = this.turns.driver;
155160 return { status: 403, error: d ? `${d.name} has control; take control to join the queue` : "take control first" };
Software/so101/src/relay.ts +36 −8
@@ -13,7 +13,8 @@
1313 *
1414 * With a lobby (src/lobby.ts), anyone may watch but only the person whose turn it is may command the arm: every
1515 * request that is not a read, and the /follow stream, is refused for anyone else before it reaches an arm. When a
16 * turn ends, that person's streams are closed and whatever they started is stopped. The driver and any admin may hit
16 * turn ends, that person's streams are closed and whatever they started is stopped. The owner (forge-signed) may always
17 * drive: a command of theirs takes control first, so /status names them the driver. The driver and any admin may hit
1718 * the emergency stop (/estop), /stop and /resume; only an admin may clear the emergency stop or change the settings.
1819 *
1920 * STOP is held here as well as on the arm server (src/command-gate.ts), so an arm server too old to hold, or one the
@@ -27,6 +28,8 @@
2728 *
2829 * A /follow stream whose browser goes away without closing it (or stops answering pings) is closed toward the arm
2930 * with LOST_CLOSE: the arm server parks the arm then, where a stream closed on purpose only holds.
31 * A frame that is not passed on is never dropped silently: the page hears {"refused": "<why>"} on its stream (and the
32 * arm server's own refusals, the zone's, come back the same way), at most once a second for the same reason.
3033 *
3134 * The owner's speed cap (src/speed-cap.ts) is applied here too, to moves, poses, grips, playbacks and every stream frame.
3235 *
@@ -37,7 +40,8 @@
3740 * sets the HttpOnly session cookie and names you: {"you": {"id", "name", "owner"}}
3841 * GET /admin, POST /admin the owner's page: pause, end a turn, take control, the settings, the log
3942 * GET /link {"mode": "live"|"reconnecting"|"offline"|"simulated", "host": "<host:port>"|null, "problem": "…"|null}
40 * GET /status the arm's own /status, and which arm said it: "source": "live <host:port>", "reconnecting to the arm:
43 * GET /status the arm's own /status, who drives it ("driver": {"id", "name", "owner"}|null, the person whose turn
44 * it is), and which arm said it: "source": "live <host:port>", "reconnecting to the arm:
4145 * <why>" (the last known status, with "reconnecting": true and "seenAt"; 503 before
4246 * there is one), or "simulated: <why>" ("simulated: offline, <why>" after 60 s down)
4347 * GET /cameras the listed cameras: [{"id", "name", "online"}]
@@ -144,12 +148,21 @@ const PAGE = /^\/(?:index\.html)?$/;
144148 const STATIC = /^\/(?:index\.html)?$|^\/(?:model|vendor|ui)\//;
145149 const LEADER = "/leader/calibration";
146150 /** Admins only: clearing an emergency stop and changing the settings (the safety limits among them). */
147 const ADMIN_ONLY = new Set(["POST /estop/clear", "PUT /settings"]);
151 const ADMIN_ONLY = new Set(["POST /estop/clear", "PUT /settings", "POST /table/here"]);
148152 /** The driver's, and any admin's at any time. */
149153 const ADMIN_TOO = new Set(["POST /stop", "POST /resume", "POST /estop"]);
150154 /** What still goes to the arm server while the link is reconnecting: stopping it. */
151155 const STOPS = new Set(["POST /stop", "POST /estop"]);
152156 const RECONNECTING = "reconnecting to the arm";
157 /** What the page is told when the gate drops a frame. */
158 const TOLD: Record<string, string> = {
159 held: "the arm is stopped: resume to drive",
160 stale: "the frame took over 200 ms to get here; a slow connection, or this computer's clock jumped",
161 replayed: "the frame came out of order",
162 unstamped: "the frame had no seq and t; reload the page",
163 "before-resume": "the frame was sent before the resume",
164 };
165
153166 /** The close code that tells the arm server its driver was lost, not done: it parks the arm. */
154167 export const LOST_CLOSE = 4001;
155168 const MAX_JOIN_BODY = 1024;
@@ -367,11 +380,16 @@ export async function startRelay(options: RelayOptions): Promise<RunningRelay> {
367380 const last = armLink!.last;
368381 const reconnecting = mode() === "reconnecting";
369382 if (!last) return { code: 503, body: { error: `${RECONNECTING}…`, source: source(), reconnecting } };
370 return { code: 200, body: { ...last, held: gate.held ?? last.held ?? null, source: source(), reconnecting, ...(reconnecting ? { seenAt: new Date(armLink!.lastAt).toISOString() } : {}) } };
383 return { code: 200, body: { ...last, held: gate.held ?? last.held ?? null, driver: driverNow(), source: source(), reconnecting, ...(reconnecting ? { seenAt: new Date(armLink!.lastAt).toISOString() } : {}) } };
371384 }
372385 const answer = await fetch(simulation + path, { cache: "no-store", signal: AbortSignal.timeout(readTimeoutMs) });
373386 const body = (await answer.json()) as Record<string, unknown>;
374 return { code: answer.status, body: { ...body, held: gate.held ?? body.held ?? null, source: source() } };
387 return { code: answer.status, body: { ...body, held: gate.held ?? body.held ?? null, driver: driverNow(), source: source() } };
388 };
389 /** Whose turn it is: the one person whose commands reach the arm (null: nobody's, or everyone's on a local run). */
390 const driverNow = () => {
391 const d = lobby?.turns.snapshot().driver;
392 return d ? { id: d.id, name: d.name, owner: d.owner } : null;
375393 };
376394 const status = async (res: ServerResponse, path: string) => {
377395 const { code, body } = await statusOf(path);
@@ -748,6 +766,16 @@ export async function startRelay(options: RelayOptions): Promise<RunningRelay> {
748766 const rate = new FrameRate();
749767 /** Set when this relay ends the stream for breaking the limits: the arm then holds, as for a stream closed on purpose. */
750768 let overLimit = false;
769 /** A frame that does not go on: noted for the Machine panel, and told to the page (the same reason at most once a second). */
770 let told = { text: "", at: 0 };
771 const notPassed = (why: string, extra: Record<string, unknown> = {}) => {
772 machine.note("clamp", "not passed on", { why, ...extra });
773 const text = `not sent to the arm: ${TOLD[why] ?? why}`;
774 const now = Date.now();
775 if (text === told.text && now - told.at < 1000) return;
776 told = { text, at: now };
777 if (browser.readyState === WebSocket.OPEN) browser.send(JSON.stringify({ refused: text }));
778 };
751779 browser.on("message", (raw) => {
752780 if (overLimit) return;
753781 if (frameBytes(raw) > MAX_FRAME_BYTES) {
@@ -764,13 +792,13 @@ export async function startRelay(options: RelayOptions): Promise<RunningRelay> {
764792 if (pace === "drop") return;
765793 if (driver !== null) {
766794 // The turn may have ended while this frame was on its way, before the close reaches the page: it is not passed on.
767 if (lobby && !lobby.turns.isDriver(driver) && !lobby.local) return void machine.note("clamp", "not passed on", { why: "the turn ended" });
795 if (lobby && !lobby.turns.isDriver(driver) && !lobby.local) return notPassed("the turn ended");
768796 lobby?.activity(driver);
769797 }
770798 const verdict = admission.admit(String(raw));
771 if (!verdict.ok) return void machine.note("clamp", "not passed on", { why: verdict.drop });
799 if (!verdict.ok) return verdict.drop === "malformed" ? void machine.note("clamp", "not passed on", { why: verdict.drop }) : notPassed(verdict.drop);
772800 if (arm.readyState !== WebSocket.OPEN) return;
773 if (real && mode() !== "live") return void machine.note("clamp", "not passed on", { why: RECONNECTING });
801 if (real && mode() !== "live") return notPassed(RECONNECTING);
774802 const data = JSON.stringify(Object.fromEntries(verdict.joints));
775803 const percent = lobby?.maxSpeedPercent ?? 100;
776804 const text = String(data);
Software/so101/src/server.test.ts +64 −1
@@ -532,7 +532,7 @@ describe("the safe zone, the jam watch, and the model routes", () => {
532532 expect(refused.status).toBe(409);
533533 const body = (await refused.json()) as { reached: boolean; error: string; violation: { what: string; link: string } };
534534 expect(body.reached).toBe(false);
535 expect(body.error).toMatch(/^refused: .* below the table \(plus the \d+ mm margin\)$/);
535 expect(body.error).toMatch(/^refused: .* below the table \(plus the \d+ mm margin\) — move up$/);
536536 expect(body.violation.what).toBe("table");
537537 const status = (await (await fetch(`${zurl}/status`)).json()) as { joints: Array<{ id: number; position: number }>; driving: null };
538538 expect(status.joints.find((j) => j.id === 2)!.position).toBe(mid(2)); // nothing moved
@@ -639,6 +639,69 @@ describe("the safe zone, the jam watch, and the model routes", () => {
639639 await zcall("/settings", { zone: "enforce" }, "PUT");
640640 });
641641
642 test("the table is set where the arm rests; from 30 mm below it a move up goes, a move further down is refused with a reason", async () => {
643 await zcall("/settings", { zone: "warn" }, "PUT");
644 await moved({ "2": ticks(2, 45) }); // the gripper some 4 cm below the bottom of the model's base: the real table is lower than the model's
645 const here = (await (await zcall("/table/here", {})).json()) as { tableMm: number; table: { mm: number; from: string } };
646 expect(here.table.from).toBe("set");
647 expect(here.tableMm).toBeLessThan(-30);
648 let s = (await (await fetch(`${zurl}/status`)).json()) as { lowestMm: number; tableMm: number };
649 expect(Math.abs(s.lowestMm)).toBeLessThanOrEqual(1); // resting on the table it was just set to
650 expect((await zcall("/settings", { tableMm: here.tableMm + 30 }, "PUT")).status).toBe(200); // now the arm is 30 mm "in" the table
651 await zcall("/settings", { zone: "enforce" }, "PUT");
652 const blocksBefore = ((await (await fetch(`${zurl}/metrics`)).json()) as { counters: { block: number } }).counters.block;
653 const down = await zcall("/move", { targets: { "2": ticks(2, 50) } });
654 expect(down.status).toBe(409);
655 expect(((await down.json()) as { error: string }).error).toMatch(/^refused: gripper would be \d+ mm below the table .* — move up$/);
656 const up = await zcall("/move", { targets: { "2": ticks(2, 40) } });
657 expect(up.status).toBe(200);
658 s = (await (await fetch(`${zurl}/status`)) .json()) as typeof s;
659 expect(s.tableMm).toBe(here.tableMm + 30);
660 const across = await zcall("/move", { targets: { "1": mid(1) + 100 } }); // the pan keeps the gripper's height: no deeper, so it goes
661 expect(across.status).toBe(200);
662 const m = (await (await fetch(`${zurl}/metrics`)).json()) as { counters: { block: number } };
663 expect(m.counters.block).toBe(blocksBefore + 1); // only the refused one counts
664 expect((await zcall("/settings", { tableMm: 500 }, "PUT")).status).toBe(400);
665 await zcall("/settings", { zone: "warn", tableMm: null }, "PUT");
666 await moved({ "1": mid(1), "2": mid(2) });
667 await zcall("/settings", { zone: "enforce" }, "PUT");
668 });
669
670 test("with no table set, the table is where the arm rests when the server starts, never above it", async () => {
671 const resting = new FakeBus([1, 2, 3, 4, 5, 6]);
672 for (const [id, [min, max]] of Object.entries(RANGES)) {
673 const at = Number(id) === 2 ? ticks(2, 45) : mid(Number(id)); // the gripper on the real table, below the model's
674 resting.servos.get(Number(id))!.set([min & 0xff, min >> 8, max & 0xff, max >> 8], 9);
675 resting.servos.get(Number(id))!.set([at & 0xff, at >> 8], 56);
676 }
677 const s2 = await startServer(new FeetechBus(resting), { host: "127.0.0.1", port: 0, posesFile: join(mkdtempSync(join(tmpdir(), "so101-rest-")), "poses.json"), modelDir: new URL("../../../Simulation/SO101", import.meta.url).pathname });
678 const got = (await (await fetch(`http://127.0.0.1:${s2.port}/settings`)).json()) as { tableMm: null; table: { mm: number; from: string } };
679 expect(got.tableMm).toBeNull();
680 expect(got.table.from).toBe("rest");
681 expect(got.table.mm).toBeLessThan(-30);
682 const st = (await (await fetch(`http://127.0.0.1:${s2.port}/status`)).json()) as { lowestMm: number };
683 expect(Math.abs(st.lowestMm)).toBeLessThanOrEqual(1);
684 await Promise.race([s2.close(), sleep(300)]);
685 });
686
687 test("a stream frame refused by the zone is answered on the stream with the reason", async () => {
688 await zcall("/settings", { zone: "enforce" }, "PUT");
689 const ws = new WebSocket(`ws://127.0.0.1:${zoneServer.port}/follow`);
690 const heard: Array<{ refused?: string }> = [];
691 ws.on("message", (d) => heard.push(JSON.parse(String(d))));
692 await new Promise((r) => ws.once("open", r));
693 ws.send(JSON.stringify({ "1": mid(1) + 50 }));
694 await sleep(80);
695 ws.send(JSON.stringify({ "2": ticks(2, 60) }));
696 await sleep(80);
697 expect(heard.at(-1)?.refused).toMatch(/^blocked: gripper would be \d+ mm below the table .* — move up$/);
698 const closed = new Promise((r) => ws.once("close", r));
699 ws.close();
700 await closed;
701 await idle();
702 await moved({ "1": mid(1) });
703 });
704
642705 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 () => {
643706 // Record the pan alone while the rest sits at the safe middle: safe to play.
644707 expect((await zcall("/record/start", { name: "pan", hz: 50, ids: [1] })).status).toBe(200);
Software/so101/src/server.ts +71 −14
@@ -27,7 +27,10 @@
2727 * PUT /calibration/<kind> store that JSON next to the poses (kind: camera, camera2, handeye, ...)
2828 * GET /settings the safe zone: {"zone": "enforce"|"warn"|"off", "floorMarginMm", "baseMarginMm", "jointOffsetsDeg"},
2929 * and the safety limits of src/safety.ts (speed, acceleration, rate, dead-man, park, gripper force, joint limits)
30 * "tableMm": the table, mm from the bottom of the model's base (null: where the arm rested at start-up),
31 * and "table": {"mm", "from": "set"|"rest"}, the one in force
3032 * PUT /settings change any of those
33 * POST /table/here the arm rests on the table now: set the table under its lowest part
3134 * GET /metrics jams, blocks, stalls, peak load and temperature per joint, recent events
3235 * GET /model.json the arm model: joints, the table plane, where the URDF is served
3336 * GET /leader-map.js the leader calibration mapping, for the browser (src/leader-map.js)
@@ -46,8 +49,8 @@
4649 *
4750 * One thing drives the arm at a time. Every target is pulled inside the joint limits and every motion held to the speed and
4851 * acceleration limits (src/safety.ts); a stream that goes silent holds, one whose connection is lost parks. Every target, whether a move or a stream frame, is run through
49 * the arm's own geometry first: a pose that would push a part into the table or the base is refused
50 * (or only reported, in "warn" mode). On the stream, a joint that pushes hard without moving is a jam:
52 * the arm's own geometry first: a pose that would push a part into the table or the base, deeper than the arm already is,
53 * is refused with the reason and the way out (or only reported, in "warn" mode); backing out of a violation always goes. On the stream, a joint that pushes hard without moving is a jam:
5154 * its goal is parked where it is, and it follows again once the leader backs off.
5255 */
5356
@@ -113,9 +116,16 @@ export interface Settings extends SafetySettings {
113116 baseMarginMm: number;
114117 /** Per joint, degrees added to the model's angle, to trim the model onto the real arm. */
115118 jointOffsetsDeg: Record<string, number>;
119 /**
120 * The table's height, mm above the bottom of the model's base (negative: below it), as an admin set it;
121 * null until then, and the table is taken from where the arm rests when the server starts.
122 */
123 tableMm: number | null;
116124 }
117125
118 export const DEFAULT_SETTINGS: Settings = { zone: "warn", floorMarginMm: 15, baseMarginMm: 10, jointOffsetsDeg: {}, ...DEFAULT_SAFETY };
126 export const DEFAULT_SETTINGS: Settings = { zone: "warn", floorMarginMm: 15, baseMarginMm: 10, jointOffsetsDeg: {}, tableMm: null, ...DEFAULT_SAFETY };
127 /** How far the table may be set from the bottom of the model's base, mm. */
128 export const TABLE_RANGE_MM = 100;
119129
120130 /** An emergency stop in force: who hit it and when. It holds until an admin clears it, across restarts. */
121131 export interface EStop {
@@ -225,7 +235,12 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}):
225235 console.error(`settings: ${settingsFile} is not readable JSON (${err instanceof Error ? err.message : String(err)}); using the defaults, zone "${settings.zone}"`);
226236 }
227237 }
228 const zoneSettings = () => ({ floorMargin: settings.floorMarginMm / 1000, baseMargin: settings.baseMarginMm / 1000 });
238 /** The table from the arm at rest: never above its lowest part, since the real arm cannot be inside the table. */
239 let restTableMm = 0;
240 const tableMm = () => settings.tableMm ?? restTableMm;
241 const zoneSettings = () => ({ floorMargin: settings.floorMarginMm / 1000, baseMargin: settings.baseMarginMm / 1000, table: tableMm() / 1000 });
242 /** Where the arm's lowest part is, mm above the bottom of the model's base. */
243 const lowestAt = (positions: ReadonlyMap<number, number>) => (model ? Math.round((model.lowest(toQ(positions)).z - model.floorZ) * 1000) : 0);
229244 /** Joint angles for the model from servo ticks, with the trims applied. The gripper maps closed→0, open→its URDF limit. */
230245 const gripperOpen = model?.movable.find((j) => j.name === "gripper")?.upper ?? 1.745;
231246 const toQ = (positions: ReadonlyMap<number, number>) => {
@@ -242,15 +257,21 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}):
242257 const evaluate = (positions: ReadonlyMap<number, number>) => (model && settings.zone !== "off" ? model.evaluate(toQ(positions), zoneSettings()) : null);
243258 /** Only poses the arm is in, or is sent to, count toward "closest to the table"; refused frames do not. */
244259 const approach = (r: ReturnType<typeof evaluate>) => {
245 if (r && model) metrics.approach(r.lowest.z - model.floorZ, r.lowest.link);
260 if (r && model) metrics.approach(r.lowest.z - model.floorZ - tableMm() / 1000, r.lowest.link);
246261 };
262 /** How far the arm is in the zone where it is now, meters: a move that goes no deeper is never refused. */
263 const depthNow = (positions: ReadonlyMap<number, number>) => (model && settings.zone !== "off" ? (model.check(toQ(positions), zoneSettings())?.depth ?? 0) : 0);
264 /** Deeper than the arm already is, by more than a millimetre of rounding. */
265 const deeper = (v: Violation, depth: number) => v.depth > depth + 0.001;
266 /** Which way out of a violation. */
267 const wayOut = (v: Violation) => (v.what === "table" ? "move up" : "move away from the base");
247268 const describeViolation = (v: Violation) => {
248269 const part = v.link.replace(/_link$/, "").replace(/_so101_v1$/, "");
249270 const marginMm = v.what === "table" ? settings.floorMarginMm : settings.baseMarginMm;
250271 const beyond = v.depth * 1000 - marginMm; // the depth includes the margin
251272 return beyond > 0
252 ? `${part} would be ${beyond.toFixed(0)} mm ${v.what === "table" ? "below the table" : "inside the base"} (plus the ${marginMm} mm margin)`
253 : `${part} would come within ${(-beyond).toFixed(0)} mm of the ${v.what}, inside the ${marginMm} mm margin`;
273 ? `${part} would be ${beyond.toFixed(0)} mm ${v.what === "table" ? "below the table" : "inside the base"} (plus the ${marginMm} mm margin) — ${wayOut(v)}`
274 : `${part} would come within ${(-beyond).toFixed(0)} mm of the ${v.what}, inside the ${marginMm} mm margin — ${wayOut(v)}`;
254275 };
255276 /** The worst violation on the straight joint-space path from one pose to another, or null. */
256277 const checkPath = (from: ReadonlyMap<number, number>, to: ReadonlyMap<number, number>, steps = 12): Violation | null => {
@@ -264,6 +285,13 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}):
264285 }
265286 return worst;
266287 };
288 if (model) {
289 const rest = await bus.syncRead(ids, STS3215.Present_Position);
290 if (rest.size === ids.length) restTableMm = Math.max(-TABLE_RANGE_MM, Math.min(0, lowestAt(rest)));
291 }
292
293 /** The settings, with the table in force and where it came from. */
294 const shownSettings = () => ({ ...settings, model: model !== null, table: { mm: tableMm(), from: settings.tableMm === null ? "rest" : "set" } });
267295
268296 // ------------------------------------------------------------------ who drives the arm
269297 let driver: Driver | null = null;
@@ -363,7 +391,7 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}):
363391 if (!targets.length) return;
364392 const from = await bus.syncRead(ids, STS3215.Present_Position);
365393 const violation = checkPath(from, new Map([...from, ...targets]));
366 if (violation && settings.zone === "enforce") {
394 if (violation && settings.zone === "enforce" && deeper(violation, depthNow(from))) {
367395 metrics.event("park", { why, parked: false, error: `refused: ${describeViolation(violation)}` });
368396 machine.note("clamp", "zone", { what: violation.what, link: violation.link, depthMm: Math.round(violation.depth * 1000), refused: true, source: "park" });
369397 return;
@@ -525,6 +553,10 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}):
525553 if (typeof s[key] !== "number" || !Number.isFinite(s[key]) || s[key]! < 0 || s[key]! > 100) throw new HttpError(400, `${key} must be 0-100`);
526554 out[key] = s[key]!;
527555 }
556 if (s.tableMm !== undefined && s.tableMm !== null) {
557 if (typeof s.tableMm !== "number" || !Number.isFinite(s.tableMm) || Math.abs(s.tableMm) > TABLE_RANGE_MM) throw new HttpError(400, `tableMm must be within ±${TABLE_RANGE_MM} mm, or null to take it from the arm at rest`);
558 out.tableMm = Math.round(s.tableMm);
559 }
528560 if (s.jointOffsetsDeg !== undefined) {
529561 if (typeof s.jointOffsetsDeg !== "object" || s.jointOffsetsDeg === null || Array.isArray(s.jointOffsetsDeg)) throw new HttpError(400, "jointOffsetsDeg must be an object");
530562 for (const [name, deg] of Object.entries(s.jointOffsetsDeg)) {
@@ -568,7 +600,8 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}):
568600 recording: recording?.name ?? null,
569601 zone: model ? settings.zone : "off",
570602 violation: zone?.violation ?? null,
571 lowestMm: zone && model ? Number(((zone.lowest.z - model.floorZ) * 1000).toFixed(0)) : null,
603 lowestMm: zone && model ? Number(((zone.lowest.z - model.floorZ) * 1000 - tableMm()).toFixed(0)) : null,
604 tableMm: model ? tableMm() : null,
572605 blocked,
573606 jammed: [...jams.keys()],
574607 stream: streamStats,
@@ -590,7 +623,10 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}):
590623 const from = await bus.syncRead(ids, STS3215.Present_Position);
591624 const to = new Map(from);
592625 targetIds.forEach((id, i) => to.set(id, positions[i]!));
593 const violation = checkPath(from, to);
626 // Judged against where the arm already is: a pose it is in is never a reason to refuse, and backing out always goes.
627 const found = checkPath(from, to);
628 const violation = found && deeper(found, depthNow(from)) ? found : null;
629 if (found && !violation) machine.note("clamp", "zone", { what: found.what, link: found.link, depthMm: Math.round(found.depth * 1000), refused: false, why: "no deeper than the arm already is", source });
594630 if (violation) {
595631 metrics.event("block", { source, what: violation.what, link: violation.link, depthMm: Math.round(violation.depth * 1000), mode: settings.zone });
596632 machine.note("clamp", "zone", { what: violation.what, link: violation.link, depthMm: Math.round(violation.depth * 1000), refused: settings.zone === "enforce", source });
@@ -727,7 +763,7 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}):
727763 case "GET /metrics":
728764 return json(res, 200, metrics.summary());
729765 case "GET /settings":
730 return json(res, 200, { ...settings, model: model !== null });
766 return json(res, 200, shownSettings());
731767 case "PUT /settings": {
732768 const body = await readBody(req);
733769 const merged = (key: "jointOffsetsDeg" | "jointLimitsDeg") => {
@@ -736,13 +772,24 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}):
736772 };
737773 settings = validateSettings({ ...settings, ...body, jointOffsetsDeg: merged("jointOffsetsDeg"), jointLimitsDeg: merged("jointLimitsDeg") } as Partial<Settings>);
738774 writeAtomic(settingsFile, JSON.stringify(settings, null, 1));
739 return json(res, 200, { ...settings, model: model !== null });
775 return json(res, 200, shownSettings());
776 }
777 case "POST /table/here": {
778 // The arm rests on the table now: the table is where its lowest part is.
779 if (!model) throw new HttpError(404, "no model on this server: there is no table to set");
780 const at = lowestAt(await bus.syncRead(ids, STS3215.Present_Position));
781 if (Math.abs(at) > TABLE_RANGE_MM) throw new HttpError(409, `the arm's lowest part is ${at} mm from the bottom of the base: rest it on the table first`);
782 settings = { ...settings, tableMm: at };
783 writeAtomic(settingsFile, JSON.stringify(settings, null, 1));
784 machine.note("state", "table set", { tableMm: at });
785 return json(res, 200, shownSettings());
740786 }
741787 case "GET /model.json":
742788 if (!model) return json(res, 404, { error: "no model loaded on this server" });
743789 return json(res, 200, {
744790 urdf: `/model/${URDF}`,
745791 floorZ: model.floorZ,
792 tableZ: model.floorZ + tableMm() / 1000,
746793 baseTop: model.baseTop,
747794 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 })),
748795 });
@@ -1004,6 +1051,14 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}):
10041051 let last: Map<number, number> | null = null;
10051052 /** Consecutive frames dropped per joint for jumping too far; after enough of them the joint is eased over instead of left behind. */
10061053 const drops = new Map<number, number>();
1054 /** Tell the driver why a frame did not move the arm: the page shows it. The same reason at most once a second. */
1055 let told = { text: "", at: 0 };
1056 const tell = (body: { refused: string; violation?: Violation }) => {
1057 const now = Date.now();
1058 if (body.refused === told.text && now - told.at < 1000) return;
1059 told = { text: body.refused, at: now };
1060 if (ws.readyState === ws.OPEN) ws.send(JSON.stringify(body));
1061 };
10071062 /** How deep in a violation the pose the arm was last sent to is; a frame may never go deeper, but may back out. */
10081063 let heldDepth = 0;
10091064 /** The latest sampled positions, so a frame that names only some joints is judged with the rest where they are. */
@@ -1149,11 +1204,13 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}):
11491204 for (const [id, t] of targets) pose.set(id, t);
11501205 const zone = evaluate(pose);
11511206 if (zone?.violation) {
1152 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 });
1207 const goingDeeper = deeper(zone.violation, heldDepth);
1208 if (!blocked && goingDeeper) metrics.event("block", { source: "stream", what: zone.violation.what, link: zone.violation.link, depthMm: Math.round(zone.violation.depth * 1000), mode: settings.zone });
11531209 blocked = zone.violation;
1154 if (settings.zone === "enforce" && zone.violation.depth > heldDepth + 0.001) {
1210 if (settings.zone === "enforce" && goingDeeper) {
11551211 stats.refused++;
11561212 machine.note("clamp", "zone", { what: zone.violation.what, link: zone.violation.link, depthMm: Math.round(zone.violation.depth * 1000), refused: true, source: "stream" });
1213 tell({ refused: `blocked: ${describeViolation(zone.violation)}`, violation: zone.violation });
11571214 return; // deeper in than the arm already is: it stays at the last good targets; a frame that backs out is let through
11581215 }
11591216 } else blocked = null;
Software/so101/src/viewer.html +31 −5
@@ -419,6 +419,11 @@
419419 <label class="inline">table margin <input type="number" id="floorMargin" min="0" max="100" step="1" inputmode="numeric"> mm</label>
420420 <label class="inline">base margin <input type="number" id="baseMargin" min="0" max="100" step="1" inputmode="numeric"> mm</label>
421421 </div>
422 <div class="row" id="tableRow">
423 <label class="inline">table <input type="number" id="tableMm" min="-100" max="100" step="1" inputmode="numeric"> mm from the base's bottom</label>
424 <button id="tableHere" type="button">The arm rests on the table: set it here</button>
425 </div>
426 <p id="tableNote" class="note"></p>
422427 <p id="zoneNote"></p>
423428 </div>
424429 <div class="card">
@@ -782,6 +787,8 @@
782787 // ---------------------------------------------------------------- the stream: the page as a leader
783788 // Opened while a slider, a jog button, the joystick or a model joint is held; frames go at most every 33 ms; closed shortly after letting go.
784789 // Stamped targets (src/ui/frames.js): while the socket opens only the newest waits, and goes only if still fresh.
790 // A frame not acted on comes back with why
791 const told = (e) => { try { const m = JSON.parse(e.data); if (typeof m?.refused === "string") msg(m.refused, true); } catch {} };
785792 const stream = {
786793 ws: null, closeTimer: null, halted: false, waiting: null,
787794 frames: new Frames((data) => {
@@ -805,6 +812,7 @@
805812 if (this.ws && this.ws.readyState <= 1) return;
806813 if (this.halted) return; // the server ended this hold (Stop, or the arm is busy): nothing more until the next press
807814 const ws = new WebSocket(`${location.protocol === "https:" ? "wss" : "ws"}://${location.host}/follow`);
815 ws.onmessage = told;
808816 this.waiting = null;
809817 ws.onopen = () => {
810818 const w = this.waiting;
@@ -1054,7 +1062,7 @@
10541062 const l = { arm, port, stop: new AbortController(), pose: null, targets: null, calibration: mine?.ok && mine.joints ? mine : null };
10551063 leader = l;
10561064 teleop ??= new tele.Teleop({
1057 connect: () => new WebSocket(`${location.protocol === "https:" ? "wss" : "ws"}://${location.host}/follow`),
1065 connect: () => { const ws = new WebSocket(`${location.protocol === "https:" ? "wss" : "ws"}://${location.host}/follow`); ws.addEventListener("message", told); return ws; },
10581066 mayDrive, visible: () => document.visibilityState === "visible", onChange: teleopChanged,
10591067 });
10601068 teleop.setLeader(true);
@@ -1206,7 +1214,12 @@
12061214 // ---------------------------------------------------------------- the 3D model
12071215 // three.js is most of what the page downloads, so it comes after the first view: the room and the camera work without it.
12081216 const viewer = $("model");
1209 let THREE = null, model3D = null, dragging = null, table = null, marker = null;
1217 let THREE = null, model3D = null, dragging = null, table = null, grid = null, marker = null;
1218 const placeTable = () => {
1219 const z = settings?.table ? modelInfo.floorZ + settings.table.mm / 1000 : (modelInfo.tableZ ?? modelInfo.floorZ);
1220 table.position.z = z - 0.0005;
1221 grid.position.z = z;
1222 };
12101223 function load3D() {
12111224 if (!modelInfo?.ok) return Promise.resolve(false);
12121225 return (model3D ??= (async () => {
@@ -1233,8 +1246,8 @@
12331246 viewer.addEventListener("urdf-processed", () => {
12341247 if (!modelInfo || !THREE) return;
12351248 table = new THREE.Mesh(new THREE.PlaneGeometry(0.9, 0.9), new THREE.MeshPhongMaterial({ color: 0x2a2f36, side: THREE.DoubleSide, transparent: true, opacity: 0.85 }));
1236 table.position.z = modelInfo.floorZ - 0.0005;
1237 const grid = new THREE.GridHelper(0.9, 18, 0x4a5058, 0x30363d); grid.rotation.x = Math.PI / 2; grid.position.z = modelInfo.floorZ;
1249 grid = new THREE.GridHelper(0.9, 18, 0x4a5058, 0x30363d); grid.rotation.x = Math.PI / 2;
1250 placeTable();
12381251 marker = new THREE.Mesh(new THREE.SphereGeometry(0.012, 16, 12), new THREE.MeshBasicMaterial({ color: 0xf85149 })); marker.visible = false;
12391252 for (const o of [table, grid, marker]) o.raycast = () => {};
12401253 viewer.world.add(table, grid, marker);
@@ -1491,6 +1504,10 @@
14911504 async function loadSettings() {
14921505 settings = await api("/settings");
14931506 $("zone").value = settings.zone; $("floorMargin").value = settings.floorMarginMm; $("baseMargin").value = settings.baseMarginMm;
1507 const tableAt = settings.table?.mm ?? 0;
1508 $("tableMm").value = tableAt;
1509 $("tableNote").textContent = settings.table?.from === "rest" ? "taken from where the arm rested at start-up" : "";
1510 if (table && modelInfo) placeTable();
14941511 const trims = $("trims"); trims.innerHTML = "";
14951512 for (const [id, name] of Object.entries(NAMES)) {
14961513 const input = el("input", { type: "number", min: -30, max: 30, step: 0.5, value: settings.jointOffsetsDeg[name] ?? 0, id: `trim-${id}`, inputMode: "decimal" });
@@ -1525,6 +1542,14 @@
15251542 $("zone").onchange = () => saveSettings({ zone: $("zone").value });
15261543 $("floorMargin").onchange = () => saveSettings({ floorMarginMm: Number($("floorMargin").value) });
15271544 $("baseMargin").onchange = () => saveSettings({ baseMarginMm: Number($("baseMargin").value) });
1545 $("tableMm").onchange = () => { if ($("tableMm").checkValidity()) saveSettings({ tableMm: Number($("tableMm").value) }); };
1546 $("tableHere").onclick = async () => {
1547 if (!confirm("Is the arm resting on the table now?")) return;
1548 const r = await api("/table/here", {});
1549 if (!r.ok) return msg(r.error, true);
1550 msg(`the table is set ${r.table.mm} mm from the base's bottom`);
1551 loadSettings();
1552 };
15281553
15291554 // ---------------------------------------------------------------- metrics
15301555 async function showMetrics() {
@@ -1554,7 +1579,8 @@
15541579 let wordsSent = false;
15551580 $("wordsForm").addEventListener("submit", (e) => { e.preventDefault(); wordsSent = true; say({ type: "words", words: $("wordsForm").elements.words.value.split(",").map((w) => w.trim()).filter(Boolean) }); });
15561581 const lobby = { ws: null, hello: null, state: null, offset: 0, retry: 500, mutedList: [], bannedList: [], admin: null, statusSeen: false };
1557 function mayDrive() { return !relayed || (lobby.hello !== null && (!lobby.hello.signin || (lobby.hello.you !== null && lobby.state?.driver?.id === lobby.hello.you.id))); }
1582 // The owner may always drive: their first command takes control
1583 function mayDrive() { return !relayed || (lobby.hello !== null && (!lobby.hello.signin || Boolean(lobby.hello.you?.owner) || (lobby.hello.you !== null && lobby.state?.driver?.id === lobby.hello.you.id))); }
15581584 function isOwner() { return !relayed || Boolean(lobby.hello?.you?.owner); }
15591585 const say = (m) => { if (lobby.ws?.readyState === 1) lobby.ws.send(JSON.stringify(m)); };
15601586 let drove = false;

↑↓ navigate ↵ open ⌘↵ its app esc close

Keyboard shortcuts

Docs