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 #151

Parked opened this in answer to
Closed without merging

smith/arm:smith/144-top-the-arm-moves-when-you-drive-it-the → main 1 commit fork

  • Closed without merging. Nothing was applied to main.

Review

Sign in to review this change.
4 files changed +56 −6
Software/so101/src/relay.ts +7 −2
@@ -362,16 +362,21 @@ export async function startRelay(options: RelayOptions): Promise<RunningRelay> {
362362 * The arm's /status, saying which arm it is: from the link while it is live, the last one it had while reconnecting,
363363 * and from the simulation otherwise.
364364 */
365 /** Who holds the turn, as /status and the Machine panel tell it; null when nobody does. */
366 const turnHolder = () => {
367 const d = lobby?.turns.snapshot().driver;
368 return d ? { id: d.id, name: d.name, owner: d.owner } : null;
369 };
365370 const statusOf = async (path = "/status"): Promise<{ code: number; body: Record<string, unknown> }> => {
366371 if (onArm()) {
367372 const last = armLink!.last;
368373 const reconnecting = mode() === "reconnecting";
369374 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() } : {}) } };
375 return { code: 200, body: { ...last, driver: turnHolder(), held: gate.held ?? last.held ?? null, source: source(), reconnecting, ...(reconnecting ? { seenAt: new Date(armLink!.lastAt).toISOString() } : {}) } };
371376 }
372377 const answer = await fetch(simulation + path, { cache: "no-store", signal: AbortSignal.timeout(readTimeoutMs) });
373378 const body = (await answer.json()) as Record<string, unknown>;
374 return { code: answer.status, body: { ...body, held: gate.held ?? body.held ?? null, source: source() } };
379 return { code: answer.status, body: { ...body, driver: turnHolder(), held: gate.held ?? body.held ?? null, source: source() } };
375380 };
376381 const status = async (res: ServerResponse, path: string) => {
377382 const { code, body } = await statusOf(path);
Software/so101/src/server.test.ts +16
@@ -548,6 +548,22 @@ describe("the safe zone, the jam watch, and the model routes", () => {
548548 await zcall("/settings", { zone: "enforce" }, "PUT");
549549 });
550550
551 test("an arm already in the table may back out, never go deeper; \"table here\" puts the table where it rests", async () => {
552 await zcall("/settings", { zone: "warn", floorMarginMm: 0 }, "PUT");
553 await moved({ "2": ticks(2, 80) }); // the arm is in the table now
554 await zcall("/settings", { zone: "enforce" }, "PUT");
555 const deeper = await zcall("/move", { targets: { "2": ticks(2, 95) } });
556 expect(deeper.status).toBe(409);
557 expect(((await deeper.json()) as { error: string }).error).toMatch(/^refused: .*below the table/);
558 await moved({ "2": ticks(2, 55) }); // up and out: allowed though still inside
559 const here = (await (await zcall("/settings/table-here", {})).json()) as { tableOffsetMm: number };
560 expect(here.tableOffsetMm).toBeGreaterThan(0);
561 const s = (await (await fetch(`${zurl}/status`)).json()) as { violation: unknown };
562 expect(s.violation).toBeNull();
563 await zcall("/settings", { tableOffsetMm: 0, floorMarginMm: 10 }, "PUT");
564 await moved({ "2": mid(2) });
565 });
566
551567 test("a stream frame into the table holds the arm; a safe frame resumes it", async () => {
552568 const ws = new WebSocket(`ws://127.0.0.1:${zoneServer.port}/follow`);
553569 await new Promise((r) => ws.once("open", r));
Software/so101/src/server.ts +23 −4
@@ -27,6 +27,7 @@
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 * POST /settings/table-here the arm rests on the table now: put the table at its lowest point (tableOffsetMm)
3031 * PUT /settings change any of those
3132 * GET /metrics jams, blocks, stalls, peak load and temperature per joint, recent events
3233 * GET /model.json the arm model: joints, the table plane, where the URDF is served
@@ -111,11 +112,13 @@ export interface Settings extends SafetySettings {
111112 zone: "enforce" | "warn" | "off";
112113 floorMarginMm: number;
113114 baseMarginMm: number;
115 /** How far below the model's table the real one is, mm: the arm resting on it is not "in" it. */
116 tableOffsetMm: number;
114117 /** Per joint, degrees added to the model's angle, to trim the model onto the real arm. */
115118 jointOffsetsDeg: Record<string, number>;
116119 }
117120
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 };
119122
120123 /** An emergency stop in force: who hit it and when. It holds until an admin clears it, across restarts. */
121124 export interface EStop {
@@ -225,7 +228,7 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}):
225228 console.error(`settings: ${settingsFile} is not readable JSON (${err instanceof Error ? err.message : String(err)}); using the defaults, zone "${settings.zone}"`);
226229 }
227230 }
228 const zoneSettings = () => ({ floorMargin: settings.floorMarginMm / 1000, baseMargin: settings.baseMarginMm / 1000 });
231 const zoneSettings = () => ({ floorMargin: (settings.floorMarginMm - settings.tableOffsetMm) / 1000, baseMargin: settings.baseMarginMm / 1000 });
229232 /** Joint angles for the model from servo ticks, with the trims applied. The gripper maps closed→0, open→its URDF limit. */
230233 const gripperOpen = model?.movable.find((j) => j.name === "gripper")?.upper ?? 1.745;
231234 const toQ = (positions: ReadonlyMap<number, number>) => {
@@ -246,7 +249,7 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}):
246249 };
247250 const describeViolation = (v: Violation) => {
248251 const part = v.link.replace(/_link$/, "").replace(/_so101_v1$/, "");
249 const marginMm = v.what === "table" ? settings.floorMarginMm : settings.baseMarginMm;
252 const marginMm = v.what === "table" ? settings.floorMarginMm - settings.tableOffsetMm : settings.baseMarginMm;
250253 const beyond = v.depth * 1000 - marginMm; // the depth includes the margin
251254 return beyond > 0
252255 ? `${part} would be ${beyond.toFixed(0)} mm ${v.what === "table" ? "below the table" : "inside the base"} (plus the ${marginMm} mm margin)`
@@ -256,11 +259,13 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}):
256259 const checkPath = (from: ReadonlyMap<number, number>, to: ReadonlyMap<number, number>, steps = 12): Violation | null => {
257260 if (!model || settings.zone === "off") return null;
258261 let worst: Violation | null = null;
262 // A pose the arm is already in is never a reason to refuse: only going deeper than where it starts is.
263 const startDepth = model.check(toQ(from), zoneSettings())?.depth ?? 0;
259264 for (let s = 1; s <= steps; s++) {
260265 const mix = new Map<number, number>();
261266 for (const [id, a] of from) mix.set(id, Math.round(a + ((to.get(id) ?? a) - a) * (s / steps)));
262267 const v = model.check(toQ(mix), zoneSettings());
263 if (v && (!worst || v.depth > worst.depth)) worst = v;
268 if (v && v.depth > startDepth + 0.001 && (!worst || v.depth > worst.depth)) worst = v;
264269 }
265270 return worst;
266271 };
@@ -525,6 +530,10 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}):
525530 if (typeof s[key] !== "number" || !Number.isFinite(s[key]) || s[key]! < 0 || s[key]! > 100) throw new HttpError(400, `${key} must be 0-100`);
526531 out[key] = s[key]!;
527532 }
533 if (s.tableOffsetMm !== undefined) {
534 if (typeof s.tableOffsetMm !== "number" || !Number.isFinite(s.tableOffsetMm) || Math.abs(s.tableOffsetMm) > 100) throw new HttpError(400, "tableOffsetMm must be within ±100");
535 out.tableOffsetMm = s.tableOffsetMm;
536 }
528537 if (s.jointOffsetsDeg !== undefined) {
529538 if (typeof s.jointOffsetsDeg !== "object" || s.jointOffsetsDeg === null || Array.isArray(s.jointOffsetsDeg)) throw new HttpError(400, "jointOffsetsDeg must be an object");
530539 for (const [name, deg] of Object.entries(s.jointOffsetsDeg)) {
@@ -738,6 +747,16 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}):
738747 writeAtomic(settingsFile, JSON.stringify(settings, null, 1));
739748 return json(res, 200, { ...settings, model: model !== null });
740749 }
750 case "POST /settings/table-here": {
751 // The arm rests on the table now: put the table at its lowest point.
752 if (!model) throw new HttpError(409, "no model loaded on this server: no table to set");
753 const lowest = model.lowest(toQ(await bus.syncRead(ids, STS3215.Present_Position)));
754 const tableOffsetMm = Math.ceil(Math.max(-100, Math.min(100, (model.floorZ - lowest.z) * 1000)));
755 settings = validateSettings({ ...settings, tableOffsetMm });
756 writeAtomic(settingsFile, JSON.stringify(settings, null, 1));
757 machine.note("state", "table", { tableOffsetMm, link: lowest.link });
758 return json(res, 200, { ...settings, model: true });
759 }
741760 case "GET /model.json":
742761 if (!model) return json(res, 404, { error: "no model loaded on this server" });
743762 return json(res, 200, {
Software/so101/src/stop.test.ts +10
@@ -178,6 +178,16 @@ describe("STOP through the relay, to a Pi that holds nothing itself", () => {
178178 return relay.port;
179179 };
180180
181 test("/status says who holds the turn, the owner included", async () => {
182 const port = await start();
183 const status = async () => ((await (await fetch(`http://127.0.0.1:${port}/status`)).json()) as { driver: { name: string; owner: boolean } | null }).driver;
184 expect(await status()).toBeNull();
185 lobby!.turns.join({ id: "u-alice", name: "alice", owner: false });
186 expect(await status()).toMatchObject({ name: "alice", owner: false });
187 lobby!.turns.join({ id: "u-trevato", name: "trevato", owner: true });
188 expect(await status()).toMatchObject({ name: "trevato", owner: true });
189 });
190
181191 test("(b) 100 commands sent while held, then STOP and resume: the Pi receives none of them, then the one new command", async () => {
182192 const port = await start();
183193 lobby!.turns.join({ id: "u-alice", name: "alice", owner: false });

↑↓ navigate ↵ open ⌘↵ its app esc close

Keyboard shortcuts

Docs