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

Migrated from github.com/Trevato/SO-ARM100

workbenchthis machine

src/limits-search.ts steps one joint's speed and acceleration up until p95 error or overshoot worsens, then backs off one step - orchestration and test only #496

Merged opened this

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

Merged

smith/arm:smith/494-src-limits-search-ts-steps-one-joint-s-s → main 2 commits fork 1 thread · 1 unresolved

  • Merged. smith/494-src-limits-search-ts-steps-one-joint-s-s was merged into main as dcd24b0.

Review

Sign in to review this change.
3 files changed +119 −1
Software/so101/bench/precision.ts +1 −1
@@ -72,7 +72,7 @@ export async function bench(script: readonly Step[] = gesture()): Promise<Card>
7272 const ws = new WebSocket(`${arm.url.replace("http", "ws")}/follow`);
7373 await new Promise((r, j) => (ws.once("open", r), ws.once("error", j)));
7474 // Loopback is its own clock: the stamp is the send time, and a watcher reports each status's age as the relay would.
75 const watch = new WebSocket(`ws://127.0.0.1:${server.port}/watch`);
75 const watch = new WebSocket(`${arm.url.replace("http", "ws")}/watch`);
7676 await new Promise((r, j) => (watch.once("open", r), watch.once("error", j)));
7777 watch.on("message", (data) => {
7878 const { readAt } = JSON.parse(String(data)) as { readAt?: number };
Software/so101/src/limits-search.test.ts added +63
@@ -0,0 +1,63 @@
1 import { expect, test } from "bun:test";
2 import { searchLimits, type LimitsDeps, type Limits } from "./limits-search.ts";
3 import type { SweepResult } from "./tuning-run.ts";
4
5 const START: Limits = { speedDegS: 90, accelDegS2: 600 };
6 const r = (p95Error: number, overshoot: number): SweepResult => ({ p95Error, overshoot, timeToArrive: 1 });
7 const step = (speedDegS: number, accelDegS2: number): Limits => ({ speedDegS, accelDegS2 });
8
9 function fake(sweeps: Array<SweepResult | Error>, idle = true) {
10 const writes: Limits[] = [];
11 const deps: LimitsDeps = {
12 isIdle: () => idle,
13 sweep: async () => {
14 const next = sweeps.shift()!;
15 if (next instanceof Error) throw next;
16 return next;
17 },
18 readLimits: async () => START,
19 writeLimits: async (_j, l) => void writes.push(l),
20 };
21 return { deps, writes };
22 }
23
24 test("refused while someone is driving", async () => {
25 const { deps, writes } = fake([], false);
26 await expect(searchLimits({ joint: 1, steps: [step(120, 800)] }, deps)).rejects.toThrow("driving");
27 expect(writes).toEqual([]);
28 });
29
30 test("a step outside the ranges is refused before any write", async () => {
31 const { deps, writes } = fake([r(5, 1)]);
32 await expect(searchLimits({ joint: 1, steps: [step(120, 800), step(400, 800)] }, deps)).rejects.toThrow();
33 await expect(searchLimits({ joint: 1, steps: [step(120, 8000)] }, deps)).rejects.toThrow();
34 expect(writes).toEqual([]);
35 });
36
37 test("stops at the first worse step and keeps the one before", async () => {
38 const { deps, writes } = fake([r(10, 5), r(8, 4), r(7, 4), r(9, 4), r(1, 1)]);
39 const steps = [step(120, 800), step(150, 1000), step(180, 1200), step(210, 1400)];
40 const out = await searchLimits({ joint: 1, steps }, deps);
41 expect(out).toMatchObject({ joint: 1, from: START, to: steps[1] });
42 expect(out.runs.map((x) => x.step)).toEqual(steps.slice(0, 3));
43 expect(writes).toEqual([steps[0]!, steps[1]!, steps[2]!, steps[1]!]);
44 });
45
46 test("worse overshoot alone stops the search", async () => {
47 const { deps } = fake([r(10, 5), r(8, 4), r(7, 6)]);
48 const out = await searchLimits({ joint: 1, steps: [step(120, 800), step(150, 1000)] }, deps);
49 expect(out.to).toEqual(step(120, 800));
50 });
51
52 test("nothing good writes today's limits back", async () => {
53 const { deps, writes } = fake([r(10, 5), r(12, 5)]);
54 const out = await searchLimits({ joint: 1, steps: [step(120, 800), step(150, 1000)] }, deps);
55 expect(out.to).toEqual(START);
56 expect(writes).toEqual([step(120, 800), START]);
57 });
58
59 test("a throw mid-search puts the starting limits back", async () => {
60 const { deps, writes } = fake([r(10, 5), r(8, 4), new Error("STOP")]);
61 await expect(searchLimits({ joint: 1, steps: [step(120, 800), step(150, 1000)] }, deps)).rejects.toThrow("STOP");
62 expect(writes).toEqual([step(120, 800), step(150, 1000), START]);
63 });
Software/so101/src/limits-search.ts added +55
@@ -0,0 +1,55 @@
1 import { validateSafety } from "./safety.ts";
2 import type { SweepResult } from "./tuning-run.ts";
3
4 export interface Limits {
5 speedDegS: number;
6 accelDegS2: number;
7 }
8
9 export interface LimitsDeps {
10 isIdle(): boolean;
11 /** Drive the joint through its test sweep. Throws if the sweep is stopped. */
12 sweep(joint: number): Promise<SweepResult>;
13 readLimits(joint: number): Promise<Limits>;
14 writeLimits(joint: number, limits: Limits): Promise<void>;
15 }
16
17 export interface LimitsSearch {
18 joint: number;
19 from: Limits;
20 to: Limits;
21 runs: Array<{ step: Limits; result: SweepResult }>;
22 at: string;
23 }
24
25 /**
26 * Step one joint's limits up through `steps` (ascending), sweeping at each. Stops at the first step where p95 error or
27 * overshoot is worse than the best so far and writes back the last good step (the starting limits if none was good).
28 * On any throw the starting limits are written back and the error rethrown.
29 */
30 export async function searchLimits({ joint, steps }: { joint: number; steps: Limits[] }, deps: LimitsDeps): Promise<LimitsSearch> {
31 for (const s of steps) validateSafety({ maxSpeedDegS: s.speedDegS, maxAccelDegS2: s.accelDegS2 }); // throws SafetyError
32 if (!deps.isIdle()) throw new Error("someone is driving; not searching limits");
33 const from = await deps.readLimits(joint);
34 const runs: LimitsSearch["runs"] = [];
35 try {
36 const base = await deps.sweep(joint);
37 let bestP95 = base.p95Error;
38 let bestOvershoot = base.overshoot;
39 let good = from;
40 for (const step of steps) {
41 await deps.writeLimits(joint, step);
42 const result = await deps.sweep(joint);
43 runs.push({ step, result });
44 if (result.p95Error > bestP95 || result.overshoot > bestOvershoot) break;
45 bestP95 = result.p95Error;
46 bestOvershoot = result.overshoot;
47 good = step;
48 }
49 await deps.writeLimits(joint, good);
50 return { joint, from, to: good, runs, at: new Date().toISOString() };
51 } catch (err) {

If this restore write throws, the sweep error that got us here is lost. Wrap it so the original error is still the one rethrown.

52 await deps.writeLimits(joint, from);
53 throw err;
54 }
55 }

↑↓ navigate ↵ open ⌘↵ its app esc close

Keyboard shortcuts

Docs