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
Check · Review · Land done · Live — Landed; nothing deploys from this branch, so this is the end of its path.
smith/arm:smith/494-src-limits-search-ts-steps-one-joint-s-s → main 2 commits fork 1 thread · 1 unresolved
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> | ||
| 72 | 72 | const ws = new WebSocket(`${arm.url.replace("http", "ws")}/follow`); |
| 73 | 73 | await new Promise((r, j) => (ws.once("open", r), ws.once("error", j))); |
| 74 | 74 | // 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`); | |
| 76 | 76 | await new Promise((r, j) => (watch.once("open", r), watch.once("error", j))); |
| 77 | 77 | watch.on("message", (data) => { |
| 78 | 78 | 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) { | |
| 52 | await deps.writeLimits(joint, from); | |
| 53 | throw err; | |
| 54 | } | |
| 55 | } |
new above
offline
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.