Public: anyone can read this; the crew can change it.
Migrated from github.com/Trevato/SO-ARM100
workbenchthis machine
Speed and acceleration limits can differ per joint: maxSpeedDegS and maxAccelDegS2 take a per-joint override, defaults unchanged #484
Check · Review · Land done · Live — Landed; nothing deploys from this branch, so this is the end of its path.
smith/arm:smith/477-speed-and-acceleration-limits-can-differ → main 4 commits fork
7 files
changed +210
−26
Software/README.md +2
| @@ -761,6 +761,8 @@ conservative. The owner changes them in the settings panel, or with | ||
| 761 | 761 | | `jointLimitsDeg` | the calibrated range | per joint, `[min, max]` in degrees from the middle of its range; a target outside is pulled in (`null` removes the limit) | |
| 762 | 762 | | `maxSpeedDegS` | 90 | no joint turns faster, on the stream, a move or a playback (a playback is slowed to fit) | |
| 763 | 763 | | `maxAccelDegS2` | 360 | no joint speeds up or slows down faster; also written to the servos' own acceleration register | |
| 764 | | `jointSpeedDegS` | none | per joint, its own top speed in °/s in place of `maxSpeedDegS`, 30-360; a joint may be set above the global value, up to that ceiling; a patch keeps the joints it does not name, `null` removes a joint's value; also written to that servo's velocity register; API-only for now (`PUT /settings`): the settings page and the terminal have no control for it until #469; the limit holds on a move, a grip, an ease-over and a playback (slowed by its slowest joint) | | |
| 765 | | `jointAccelDegS2` | none | the same for acceleration in °/s² in place of `maxAccelDegS2`, 30-3600 | | |
| 764 | 766 | | `rateHz` | 60 | most commands per second; stream frames that come faster are merged, the latest winning | |
| 765 | 767 | | `holdAfterMs` | 1000 | the dead-man: a stream silent this long makes the arm hold where it is | |
| 766 | 768 | | `parkPose` | `park` | where the arm goes, slowly, when its driver's connection is lost (a stream that drops or stops answering pings, not one that is closed); with no pose of that name, the middle of every arm joint's range | |
Software/so101/src/motion.ts +8 −4
| @@ -188,6 +188,8 @@ export class JointSupervisor { | ||
| 188 | 188 | export interface MoveOptions extends SupervisorOptions { |
| 189 | 189 | /** Ticks per second for the joint that has the farthest to go; the others are slowed to arrive with it. */ |
| 190 | 190 | speed?: number; |
| 191 | /** A lower ceiling for some joints, ticks per second by id: the move is slowed until each joint's own cap holds. */ | |
| 192 | speedCaps?: ReadonlyMap<number, number>; | |
| 191 | 193 | /** Poll interval in ms. */ |
| 192 | 194 | pollMs?: number; |
| 193 | 195 | } |
| @@ -208,7 +210,7 @@ export async function moveToPose( | ||
| 208 | 210 | bus: FeetechBus, |
| 209 | 211 | ids: readonly number[], |
| 210 | 212 | targets: readonly number[], |
| 211 | { speed = 300, pollMs = 50, ...options }: MoveOptions = {}, | |
| 213 | { speed = 300, speedCaps, pollMs = 50, ...options }: MoveOptions = {}, | |
| 212 | 214 | ): Promise<MoveResult> { |
| 213 | 215 | if (!(speed >= 1 && Number.isFinite(speed))) throw new MotionError(`speed must be a positive number, got ${speed}`); |
| 214 | 216 | if (targets.length !== ids.length || !targets.every(Number.isInteger)) { |
| @@ -219,7 +221,7 @@ export async function moveToPose( | ||
| 219 | 221 | const { aims, adjusted } = supervisor.clamp(targets); |
| 220 | 222 | const deltas = ids.map((id, i) => Math.abs(aims[i]! - start.get(id)!)); |
| 221 | 223 | const farthest = Math.max(...deltas, 1); |
| 222 | const seconds = farthest / speed; | |
| 224 | const seconds = Math.max(farthest / speed, ...ids.map((id, i) => deltas[i]! / Math.min(speed, speedCaps?.get(id) ?? speed))); | |
| 223 | 225 | const velocities = deltas.map((d) => Math.max(20, Math.round(d / seconds))); |
| 224 | 226 | await supervisor.goal(aims, velocities); |
| 225 | 227 | |
| @@ -292,6 +294,8 @@ export interface PlayOptions { | ||
| 292 | 294 | rate?: number; |
| 293 | 295 | torquePercent?: number; |
| 294 | 296 | torqueCaps?: ReadonlyMap<number, number>; |
| 297 | /** Per-joint speed ceilings, ticks/s, for the ease to the first frame. */ | |
| 298 | speedCaps?: ReadonlyMap<number, number>; | |
| 295 | 299 | signal?: AbortSignal; |
| 296 | 300 | onFrame?: (index: number) => void; |
| 297 | 301 | } |
| @@ -306,7 +310,7 @@ export interface PlayOptions { | ||
| 306 | 310 | export async function playTrajectory( |
| 307 | 311 | bus: FeetechBus, |
| 308 | 312 | trajectory: Trajectory, |
| 309 | { rate = 1, torquePercent = 70, torqueCaps, signal, onFrame }: PlayOptions = {}, | |
| 313 | { rate = 1, torquePercent = 70, torqueCaps, speedCaps, signal, onFrame }: PlayOptions = {}, | |
| 310 | 314 | ): Promise<number> { |
| 311 | 315 | const step = largestStep(trajectory); |
| 312 | 316 | if (step > MAX_FRAME_STEP) { |
| @@ -314,7 +318,7 @@ export async function playTrajectory( | ||
| 314 | 318 | } |
| 315 | 319 | if (!(rate > 0 && rate <= 2)) throw new MotionError(`rate must be within 0-2, got ${rate}`); |
| 316 | 320 | const ids = [...trajectory.ids]; |
| 317 | await moveToPose(bus, ids, trajectory.frames[0]!, { speed: 300, torquePercent, torqueCaps, signal }); | |
| 321 | await moveToPose(bus, ids, trajectory.frames[0]!, { speed: 300, speedCaps, torquePercent, torqueCaps, signal }); | |
| 318 | 322 | const supervisor = new JointSupervisor(bus, ids, { torquePercent, torqueCaps, signal, tolerance: 4096 }); |
| 319 | 323 | let previous = [...(await supervisor.arm(254)).values()]; |
| 320 | 324 | const period = 1000 / (trajectory.hz * rate); |
Software/so101/src/safety.test.ts +52 −1
| @@ -1,5 +1,5 @@ | ||
| 1 | 1 | import { describe, expect, test } from "bun:test"; |
| 2 | import { accelRegister, bound, DEFAULT_SAFETY, Governor, jointBounds, motionLimits, parseFrame, validateSafety, type Bounds } from "./safety.ts"; | |
| 2 | import { accelRegister, bound, DEFAULT_SAFETY, Governor, jointBounds, jointMotion, motionLimits, parseFrame, validateSafety, type Bounds } from "./safety.ts"; | |
| 3 | 3 | |
| 4 | 4 | describe("safety settings", () => { |
| 5 | 5 | test("conservative defaults, and every knob checked against its range", () => { |
| @@ -113,3 +113,54 @@ describe("frames off the wire", () => { | ||
| 113 | 113 | expect(parseFrame("nope")).toBeNull(); |
| 114 | 114 | }); |
| 115 | 115 | }); |
| 116 | ||
| 117 | describe("per-joint speed and acceleration", () => { | |
| 118 | const bounds = new Map<number, Bounds>([[1, { min: 0, max: 4095 }], [2, { min: 0, max: 4095 }]]); | |
| 119 | ||
| 120 | const run = (limits: ReturnType<typeof motionLimits>) => { | |
| 121 | const g = new Governor(() => bounds, () => limits); | |
| 122 | g.reset(new Map([[1, 500], [2, 500]])); | |
| 123 | g.aim(1, 3000); | |
| 124 | g.aim(2, 3000); | |
| 125 | const trace: Array<readonly [number, number]> = []; | |
| 126 | for (let i = 0; i < 240; i++) trace.push(...g.step(1 / 60)); | |
| 127 | return trace; | |
| 128 | }; | |
| 129 | ||
| 130 | test("with nothing set the limits and every motion are the global ones", () => { | |
| 131 | const limits = motionLimits(DEFAULT_SAFETY); | |
| 132 | expect(limits).toEqual({ speed: 1024, accel: 4096 }); | |
| 133 | expect(jointMotion(limits, 1)).toBe(limits); | |
| 134 | const trace = run(limits); | |
| 135 | // joints 1 and 2 follow the same profile; the first ticks are a fixed trace of the trapezoid | |
| 136 | expect(trace.filter(([id]) => id === 1).map(([, p]) => p)).toEqual(trace.filter(([id]) => id === 2).map(([, p]) => p)); | |
| 137 | // recorded from the code before per-joint limits existed | |
| 138 | expect(trace.filter(([id]) => id === 1).slice(0, 8).map(([, p]) => p)).toEqual([501, 503, 507, 511, 517, 524, 532, 541]); | |
| 139 | }); | |
| 140 | ||
| 141 | test("a joint with its own lower limit is held to it; the others keep the global", () => { | |
| 142 | const limits = motionLimits({ ...DEFAULT_SAFETY, jointSpeedDegS: { shoulder_pan: 30 }, jointAccelDegS2: { shoulder_pan: 60 } }); | |
| 143 | expect(limits.joints).toEqual({ 1: { speed: 341, accel: 683 } }); | |
| 144 | const trace = run(limits); | |
| 145 | const of = (id: number) => trace.filter(([i]) => i === id).map(([, p]) => p); | |
| 146 | let prev = 500; | |
| 147 | for (const p of of(1)) { | |
| 148 | expect(Math.abs(p - prev)).toBeLessThanOrEqual(341 / 60 + 1); | |
| 149 | prev = p; | |
| 150 | } | |
| 151 | expect(of(1).at(-1)!).toBeLessThan(3000); // still on its way after 4 s | |
| 152 | expect(of(2).at(-1)).toBe(3000); | |
| 153 | }); | |
| 154 | ||
| 155 | test("validated at the boundary: joint names, finite numbers within the global range, null clears", () => { | |
| 156 | const s = validateSafety({ jointSpeedDegS: { elbow_flex: 45 }, jointAccelDegS2: { elbow_flex: 100 } }); | |
| 157 | expect(s.jointSpeedDegS).toEqual({ elbow_flex: 45 }); | |
| 158 | expect(s.jointAccelDegS2).toEqual({ elbow_flex: 100 }); | |
| 159 | expect(validateSafety({ jointSpeedDegS: { elbow_flex: null } }, s).jointSpeedDegS).toEqual({}); | |
| 160 | expect(() => validateSafety({ jointSpeedDegS: { elbow: 45 } })).toThrow('no joint "elbow"'); | |
| 161 | expect(() => validateSafety({ jointSpeedDegS: { elbow_flex: 1000 } })).toThrow("30-360"); | |
| 162 | expect(() => validateSafety({ jointAccelDegS2: { elbow_flex: 5000 } })).toThrow("30-3600"); | |
| 163 | expect(() => validateSafety({ jointSpeedDegS: { elbow_flex: Infinity } })).toThrow("jointSpeedDegS"); | |
| 164 | expect(() => validateSafety({ jointSpeedDegS: [45] })).toThrow("object"); | |
| 165 | }); | |
| 166 | }); |
Software/so101/src/safety.ts +50 −7
| @@ -27,6 +27,10 @@ export interface SafetySettings { | ||
| 27 | 27 | gripperTorquePercent: number; |
| 28 | 28 | /** Per joint, the narrower range it may be sent to, degrees from its middle; joints not named keep their calibrated range. */ |
| 29 | 29 | jointLimitsDeg: Record<string, [number, number]>; |
| 30 | /** Per joint, its own top speed in place of maxSpeedDegS, degrees per second; joints not named use the global one. */ | |
| 31 | jointSpeedDegS: Record<string, number>; | |
| 32 | /** Per joint, its own acceleration in place of maxAccelDegS2, degrees per second squared; joints not named use the global one. */ | |
| 33 | jointAccelDegS2: Record<string, number>; | |
| 30 | 34 | } |
| 31 | 35 | |
| 32 | 36 | export const DEFAULT_SAFETY: SafetySettings = { |
| @@ -38,6 +42,8 @@ export const DEFAULT_SAFETY: SafetySettings = { | ||
| 38 | 42 | parkSpeedDegS: 20, |
| 39 | 43 | gripperTorquePercent: 35, |
| 40 | 44 | jointLimitsDeg: {}, |
| 45 | jointSpeedDegS: {}, | |
| 46 | jointAccelDegS2: {}, | |
| 41 | 47 | }; |
| 42 | 48 | |
| 43 | 49 | /** Each number knob and the range an admin may set it within. */ |
| @@ -50,7 +56,7 @@ const RANGES = { | ||
| 50 | 56 | gripperTorquePercent: [10, 60], |
| 51 | 57 | } as const satisfies Record<string, readonly [number, number]>; |
| 52 | 58 | |
| 53 | export const SAFETY_KEYS = [...Object.keys(RANGES), "parkPose", "jointLimitsDeg"] as const; | |
| 59 | export const SAFETY_KEYS = [...Object.keys(RANGES), "parkPose", "jointLimitsDeg", "jointSpeedDegS", "jointAccelDegS2"] as const; | |
| 54 | 60 | |
| 55 | 61 | export const TICKS_PER_DEGREE = 4096 / 360; |
| 56 | 62 | /** No target comes closer than this to a servo's calibrated end, ticks. */ |
| @@ -60,7 +66,7 @@ export class SafetyError extends Error {} | ||
| 60 | 66 | |
| 61 | 67 | /** The safety part of a settings object, checked field by field; throws SafetyError naming the first bad one. */ |
| 62 | 68 | export function validateSafety(input: Partial<Record<keyof SafetySettings, unknown>>, base: SafetySettings = DEFAULT_SAFETY): SafetySettings { |
| 63 | const out: SafetySettings = { ...base, jointLimitsDeg: { ...base.jointLimitsDeg } }; | |
| 69 | const out: SafetySettings = { ...base, jointLimitsDeg: { ...base.jointLimitsDeg }, jointSpeedDegS: { ...base.jointSpeedDegS }, jointAccelDegS2: { ...base.jointAccelDegS2 } }; | |
| 64 | 70 | for (const [key, [lo, hi]] of Object.entries(RANGES) as Array<[keyof typeof RANGES, readonly [number, number]]>) { |
| 65 | 71 | const value = input[key]; |
| 66 | 72 | if (value === undefined) continue; |
| @@ -86,6 +92,21 @@ export function validateSafety(input: Partial<Record<keyof SafetySettings, unkno | ||
| 86 | 92 | out.jointLimitsDeg[name] = [range[0], range[1]]; |
| 87 | 93 | } |
| 88 | 94 | } |
| 95 | for (const key of ["jointSpeedDegS", "jointAccelDegS2"] as const) { | |
| 96 | const patch = input[key]; | |
| 97 | if (patch === undefined) continue; | |
| 98 | const [lo, hi] = RANGES[key === "jointSpeedDegS" ? "maxSpeedDegS" : "maxAccelDegS2"]; | |
| 99 | if (typeof patch !== "object" || patch === null || Array.isArray(patch)) throw new SafetyError(`${key} must be an object of joint: number`); | |
| 100 | for (const [name, value] of Object.entries(patch)) { | |
| 101 | if (!Object.values(JOINTS).includes(name)) throw new SafetyError(`no joint "${name}"`); | |
| 102 | if (value === null) { | |
| 103 | delete out[key][name]; // back to the global limit | |
| 104 | continue; | |
| 105 | } | |
| 106 | if (typeof value !== "number" || !Number.isFinite(value) || value < lo || value > hi) throw new SafetyError(`${key} for ${name} must be ${lo}-${hi}`); | |
| 107 | out[key][name] = value; | |
| 108 | } | |
| 109 | } | |
| 89 | 110 | return out; |
| 90 | 111 | } |
| 91 | 112 | |
| @@ -122,9 +143,30 @@ export function bound(bounds: ReadonlyMap<number, Bounds>, id: number, ticks: nu | ||
| 122 | 143 | return b ? clamp(Math.round(ticks), b.min, b.max) : null; |
| 123 | 144 | } |
| 124 | 145 | |
| 146 | export interface MotionLimits { | |
| 147 | speed: number; | |
| 148 | accel: number; | |
| 149 | /** By servo id, the joints that have their own speed or acceleration (the other one is the global); absent when none do. */ | |
| 150 | joints?: Record<number, { speed: number; accel: number }>; | |
| 151 | } | |
| 152 | ||
| 125 | 153 | /** The limits in the servo's own units: ticks per second, ticks per second squared. */ |
| 126 | export function motionLimits(safety: SafetySettings): { speed: number; accel: number } { | |
| 127 | return { speed: Math.round(safety.maxSpeedDegS * TICKS_PER_DEGREE), accel: Math.round(safety.maxAccelDegS2 * TICKS_PER_DEGREE) }; | |
| 154 | export function motionLimits(safety: SafetySettings): MotionLimits { | |
| 155 | const speed = Math.round(safety.maxSpeedDegS * TICKS_PER_DEGREE); | |
| 156 | const accel = Math.round(safety.maxAccelDegS2 * TICKS_PER_DEGREE); | |
| 157 | const joints: Record<number, { speed: number; accel: number }> = {}; | |
| 158 | for (const [id, name] of Object.entries(JOINTS)) { | |
| 159 | const s = safety.jointSpeedDegS[name]; | |
| 160 | const a = safety.jointAccelDegS2[name]; | |
| 161 | if (s === undefined && a === undefined) continue; | |
| 162 | joints[Number(id)] = { speed: s === undefined ? speed : Math.round(s * TICKS_PER_DEGREE), accel: a === undefined ? accel : Math.round(a * TICKS_PER_DEGREE) }; | |
| 163 | } | |
| 164 | return Object.keys(joints).length ? { speed, accel, joints } : { speed, accel }; | |
| 165 | } | |
| 166 | ||
| 167 | /** One joint's limits: its own when it has them, the global otherwise. */ | |
| 168 | export function jointMotion(limits: MotionLimits, id: number): { speed: number; accel: number } { | |
| 169 | return limits.joints?.[id] ?? limits; | |
| 128 | 170 | } |
| 129 | 171 | |
| 130 | 172 | /** The STS3215 Acceleration register counts 100 ticks/s² per unit; 0 would mean "no limit", so it is never written. */ |
| @@ -149,10 +191,10 @@ interface Track { | ||
| 149 | 191 | */ |
| 150 | 192 | export class Governor { |
| 151 | 193 | readonly : () => ReadonlyMap<number, Bounds>; |
| 152 | readonly : () => { speed: number; accel: number }; | |
| 194 | readonly : () => MotionLimits; | |
| 153 | 195 | readonly = new Map<number, Track>(); |
| 154 | 196 | |
| 155 | constructor(bounds: () => ReadonlyMap<number, Bounds>, limits: () => { speed: number; accel: number }) { | |
| 197 | constructor(bounds: () => ReadonlyMap<number, Bounds>, limits: () => MotionLimits) { | |
| 156 | 198 | this. = bounds; |
| 157 | 199 | this. = limits; |
| 158 | 200 | } |
| @@ -203,10 +245,11 @@ export class Governor { | ||
| 203 | 245 | |
| 204 | 246 | /** Advance every joint by `dt` seconds; returns the goals that changed on the wire, rounded to ticks. */ |
| 205 | 247 | step(dt: number): Array<readonly [number, number]> { |
| 206 | const { speed, accel } = this.(); | |
| 248 | const limits = this.(); | |
| 207 | 249 | const out: Array<readonly [number, number]> = []; |
| 208 | 250 | for (const [id, t] of this.) { |
| 209 | 251 | if (t.p === t.goal && t.v === 0) continue; |
| 252 | const { speed, accel } = jointMotion(limits, id); | |
| 210 | 253 | const before = Math.round(t.p); |
| 211 | 254 | const vmax = Math.min(speed, t.slow ?? Infinity); |
| 212 | 255 | const error = t.goal - t.p; |
Software/so101/src/server.test.ts +70 −1
| @@ -1,5 +1,5 @@ | ||
| 1 | 1 | import { afterAll, beforeAll, describe, expect, test } from "bun:test"; |
| 2 | import { existsSync, mkdtempSync, readFileSync, writeFileSync } from "node:fs"; | |
| 2 | import { existsSync, mkdirSync, mkdtempSync, readFileSync, writeFileSync } from "node:fs"; | |
| 3 | 3 | import { connect } from "node:net"; |
| 4 | 4 | import { tmpdir } from "node:os"; |
| 5 | 5 | import { join } from "node:path"; |
| @@ -999,6 +999,75 @@ describe("settings on disk", () => { | ||
| 999 | 999 | }); |
| 1000 | 1000 | }); |
| 1001 | 1001 | |
| 1002 | describe("per-joint speed and acceleration limits", () => { | |
| 1003 | const ids = [1, 2, 6]; | |
| 1004 | test("PUT /settings merges the maps, null clears, a bad name is 400, and they survive a restart; the servos get each joint's own limits", async () => { | |
| 1005 | const dir = mkdtempSync(join(tmpdir(), "so101-jointspeed-")); | |
| 1006 | const fake = new FakeBus(ids); | |
| 1007 | const opts = { host: "127.0.0.1", port: 0, ids, posesFile: join(dir, "poses.json"), watchdogMs: 60 }; | |
| 1008 | const s1 = await startServer(new FeetechBus(fake), opts); | |
| 1009 | const url = `http://127.0.0.1:${s1.port}`; | |
| 1010 | const put = (body: unknown) => fetch(`${url}/settings`, { method: "PUT", body: JSON.stringify(body) }); | |
| 1011 | type Got = { jointSpeedDegS: Record<string, number>; jointAccelDegS2: Record<string, number>; maxSpeedDegS: number }; | |
| 1012 | expect(((await (await fetch(`${url}/settings`)).json()) as Got).jointSpeedDegS).toEqual({}); | |
| 1013 | await put({ jointSpeedDegS: { shoulder_pan: 45 }, jointAccelDegS2: { shoulder_pan: 100 } }); | |
| 1014 | const merged = (await (await put({ jointSpeedDegS: { shoulder_lift: 200 } })).json()) as Got; | |
| 1015 | expect(merged.jointSpeedDegS).toEqual({ shoulder_pan: 45, shoulder_lift: 200 }); // a partial patch keeps the other joints | |
| 1016 | expect(merged.maxSpeedDegS).toBe(90); | |
| 1017 | expect((await put({ jointSpeedDegS: { elbow: 45 } })).status).toBe(400); | |
| 1018 | expect((await put({ jointAccelDegS2: { shoulder_pan: 9000 } })).status).toBe(400); | |
| 1019 | expect(((await (await put({ jointSpeedDegS: { shoulder_lift: null } })).json()) as Got).jointSpeedDegS).toEqual({ shoulder_pan: 45 }); | |
| 1020 | ||
| 1021 | // Opening a stream arms the servos: Goal_Velocity and Acceleration per joint. | |
| 1022 | const ws = new WebSocket(`ws://127.0.0.1:${s1.port}/follow`); | |
| 1023 | await new Promise((r) => ws.once("open", r)); | |
| 1024 | ws.send(JSON.stringify({ "1": 2000 })); | |
| 1025 | await sleep(200); | |
| 1026 | ws.close(); | |
| 1027 | const speed = (id: number) => fake.servos.get(id)![46]! + fake.servos.get(id)![47]! * 256; | |
| 1028 | expect(speed(1)).toBe(512); // 45 °/s | |
| 1029 | expect(speed(2)).toBe(1024); // the global 90 °/s | |
| 1030 | expect(fake.servos.get(1)![41]).toBe(12); // ceil(100 °/s² = 1138 ticks/s² / 100) | |
| 1031 | expect(fake.servos.get(2)![41]).toBe(41); // the global 360 °/s² | |
| 1032 | await Promise.race([s1.close(), sleep(300)]); | |
| 1033 | ||
| 1034 | const s2 = await startServer(new FeetechBus(new FakeBus(ids)), opts); | |
| 1035 | const again = (await (await fetch(`http://127.0.0.1:${s2.port}/settings`)).json()) as Got; | |
| 1036 | expect(again.jointSpeedDegS).toEqual({ shoulder_pan: 45 }); | |
| 1037 | expect(again.jointAccelDegS2).toEqual({ shoulder_pan: 100 }); | |
| 1038 | await Promise.race([s2.close(), sleep(300)]); | |
| 1039 | }); | |
| 1040 | ||
| 1041 | test("a move and a playback hold the lower cap of a joint that has one", async () => { | |
| 1042 | const dir = mkdtempSync(join(tmpdir(), "so101-jointcap-")); | |
| 1043 | const fake = new FakeBus(ids); | |
| 1044 | const server = await startServer(new FeetechBus(fake), { host: "127.0.0.1", port: 0, ids, posesFile: join(dir, "poses.json"), watchdogMs: 60 }); | |
| 1045 | const url = `http://127.0.0.1:${server.port}`; | |
| 1046 | const post = (path: string, body: unknown, method = "POST") => fetch(`${url}${path}`, { method, body: JSON.stringify(body) }); | |
| 1047 | await post("/settings", { jointSpeedDegS: { shoulder_pan: 30 } }, "PUT"); // 341 ticks/s; the others keep 90 °/s = 1024 | |
| 1048 | const velocities = () => fake.writes.filter((w) => w.address === 46 && (w.id === 1 || w.id === 2)).map((w) => [w.id, w.bytes[0]! | (w.bytes[1]! << 8)] as const); | |
| 1049 | ||
| 1050 | // Both joints have 100 ticks to go: asked 1000 ticks/s, the capped joint sets the pace and the other arrives with it. | |
| 1051 | const before = fake.writes.length; | |
| 1052 | expect((await post("/move", { targets: { "1": 2148, "2": 2148 }, speed: 1000 })).status).toBe(200); | |
| 1053 | const sent = velocities().slice(-2); | |
| 1054 | expect(Math.max(...sent.map(([, v]) => v))).toBeLessThanOrEqual(341); | |
| 1055 | expect(fake.writes.length).toBeGreaterThan(before); | |
| 1056 | ||
| 1057 | mkdirSync(join(dir, "recordings"), { recursive: true }); | |
| 1058 | // A recording moving the capped joint 100 ticks a frame at 10 Hz asks 1000 ticks/s: the rate drops to fit 341. | |
| 1059 | const frames = [2048, 2148, 2248, 2348].map((p) => [p, 2048]); | |
| 1060 | writeFileSync(join(dir, "recordings", "sweep.json"), JSON.stringify({ hz: 10, ids: [1, 2], frames, recorded: new Date().toISOString() })); | |
| 1061 | const played = (await (await post("/play", { name: "sweep", rate: 1 })).json()) as { rate: number; played: number }; | |
| 1062 | expect(played.played).toBe(4); | |
| 1063 | expect(played.rate).toBeCloseTo(0.341, 2); | |
| 1064 | // the same recording on the uncapped joint 2 plays at the asked rate | |
| 1065 | writeFileSync(join(dir, "recordings", "other.json"), JSON.stringify({ hz: 10, ids: [1, 2], frames: frames.map(([a, b]) => [b, a]), recorded: new Date().toISOString() })); | |
| 1066 | expect(((await (await post("/play", { name: "other", rate: 1 })).json()) as { rate: number }).rate).toBe(1); | |
| 1067 | await Promise.race([server.close(), sleep(300)]); | |
| 1068 | }); | |
| 1069 | }); | |
| 1070 | ||
| 1002 | 1071 | describe("the safety envelope", () => { |
| 1003 | 1072 | // Pan 818-3487 (middle 2152.5), lift 813-3202, gripper 1995-3525; no model, so no zone: only the envelope is at work. |
| 1004 | 1073 | let bus: FakeBus; |
Software/so101/src/server.ts +18 −13
| @@ -86,8 +86,8 @@ import { MotionError, moveToPose, playTrajectory, recordTrajectory, STALL_SPAN_T | ||
| 86 | 86 | import { assertPoseName, parsePoseFile, serializePoseFile, type PoseFile } from "./poses.ts"; |
| 87 | 87 | import { MAX_ZERO_OFFSET_DEG, zeroOffsetDeg } from "./joint-zero.ts"; |
| 88 | 88 | import { JOINTS, registerName, STS3215, ticksToDegrees } from "./sts3215.ts"; |
| 89 | import { largestStep, parseTrajectory, serializeTrajectory, type Trajectory } from "./trajectory.ts"; | |
| 90 | import { accelRegister, bound, DEFAULT_SAFETY, Governor, jointBounds, motionLimits, SAFETY_KEYS, SafetyError, TICKS_PER_DEGREE, validateSafety, type SafetySettings } from "./safety.ts"; | |
| 89 | import { jointSteps, parseTrajectory, serializeTrajectory, type Trajectory } from "./trajectory.ts"; | |
| 90 | import { accelRegister, bound, DEFAULT_SAFETY, Governor, jointBounds, jointMotion, motionLimits, SAFETY_KEYS, SafetyError, TICKS_PER_DEGREE, validateSafety, type SafetySettings } from "./safety.ts"; | |
| 91 | 91 | |
| 92 | 92 | export interface ServerOptions { |
| 93 | 93 | host?: string; |
| @@ -343,9 +343,9 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 343 | 343 | const torqueCaps = () => new Map([[6, settings.gripperTorquePercent]]); |
| 344 | 344 | /** The servo registers that enforce the same speed and acceleration in hardware, whatever the server sends. */ |
| 345 | 345 | const servoLimits = async (targetIds: readonly number[]) => { |
| 346 | const { speed, accel } = speedLimits(); | |
| 347 | await bus.syncWrite(STS3215.Goal_Velocity, targetIds.map((id) => [id, speed] as const)); | |
| 348 | await bus.syncWrite(STS3215.Acceleration, targetIds.map((id) => [id, accelRegister(accel)] as const)); | |
| 346 | const all = speedLimits(); | |
| 347 | await bus.syncWrite(STS3215.Goal_Velocity, targetIds.map((id) => [id, jointMotion(all, id).speed] as const)); | |
| 348 | await bus.syncWrite(STS3215.Acceleration, targetIds.map((id) => [id, accelRegister(jointMotion(all, id).accel)] as const)); | |
| 349 | 349 | }; |
| 350 | 350 | let estop: EStop | null = null; |
| 351 | 351 | try { |
| @@ -660,10 +660,13 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 660 | 660 | machine.note("clamp", "zone", { what: violation.what, link: violation.link, depthMm: Math.round(violation.depth * 1000), refused: settings.zone === "enforce", source }); |
| 661 | 661 | if (settings.zone === "enforce") return { status: 409, body: { reached: false, error: `refused: ${describeViolation(violation)}`, violation } }; |
| 662 | 662 | } |
| 663 | const speed = number(body.speed, 300, 20, speedLimits().speed, "speed"); | |
| 663 | const all = speedLimits(); | |
| 664 | const speed = number(body.speed, 300, 20, all.speed, "speed"); | |
| 665 | const speedCaps = new Map(targetIds.map((id) => [id, jointMotion(all, id).speed] as const)); | |
| 664 | 666 | if (typeof body.speed === "number" && body.speed > speed) machine.note("clamp", "speed limit", { asked: body.speed, sent: speed, source }); |
| 665 | 667 | const result = await moveToPose(bus, targetIds, positions, { |
| 666 | 668 | speed, |
| 669 | speedCaps, | |
| 667 | 670 | torquePercent: number(body.torque, 70, 10, 100, "torque"), |
| 668 | 671 | torqueCaps: torqueCaps(), |
| 669 | 672 | signal: controller.signal, |
| @@ -840,11 +843,11 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 840 | 843 | return json(res, 200, { ...settings, model: model !== null }); |
| 841 | 844 | case "PUT /settings": { |
| 842 | 845 | const body = await readBody(req); |
| 843 | const merged = (key: "jointOffsetsDeg" | "jointLimitsDeg") => { | |
| 846 | const merged = (key: "jointOffsetsDeg" | "jointLimitsDeg" | "jointSpeedDegS" | "jointAccelDegS2") => { | |
| 844 | 847 | const patch = body[key]; |
| 845 | 848 | return patch && typeof patch === "object" && !Array.isArray(patch) ? { ...settings[key], ...patch } : (patch ?? settings[key]); |
| 846 | 849 | }; |
| 847 | settings = validateSettings({ ...settings, ...body, jointOffsetsDeg: merged("jointOffsetsDeg"), jointLimitsDeg: merged("jointLimitsDeg") } as Partial<Settings>); | |
| 850 | settings = validateSettings({ ...settings, ...body, jointOffsetsDeg: merged("jointOffsetsDeg"), jointLimitsDeg: merged("jointLimitsDeg"), jointSpeedDegS: merged("jointSpeedDegS"), jointAccelDegS2: merged("jointAccelDegS2") } as Partial<Settings>); | |
| 848 | 851 | writeAtomic(settingsFile, JSON.stringify(settings, null, 1)); |
| 849 | 852 | return json(res, 200, { ...settings, model: model !== null }); |
| 850 | 853 | } |
| @@ -979,7 +982,7 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 979 | 982 | if (!Number.isInteger(body.position)) throw new HttpError(400, "position must be an integer"); |
| 980 | 983 | const position = bound(bounds(), id, body.position as number)!; |
| 981 | 984 | const torque = Math.min(number(body.torque, 30, 10, 60, "torque"), settings.gripperTorquePercent); // the force limit, whatever is asked |
| 982 | const speed = Math.min(Math.round(number(body.speed, 300, 20, 300, "speed")), speedLimits().speed); // and the speed limit | |
| 985 | const speed = Math.min(Math.round(number(body.speed, 300, 20, 300, "speed")), jointMotion(speedLimits(), id).speed); // and the joint's speed limit | |
| 983 | 986 | if (position !== body.position) machine.note("clamp", "joint limit", { id, asked: body.position as number, sent: position, source: "grip" }); |
| 984 | 987 | if (typeof body.torque === "number" && body.torque > torque) machine.note("clamp", "gripper force", { asked: body.torque, sent: torque, source: "grip" }); |
| 985 | 988 | claim("move"); |
| @@ -1070,9 +1073,10 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 1070 | 1073 | // Every frame inside the joints' bounds, and the whole playback no faster than the speed limit allows. |
| 1071 | 1074 | const b = bounds(); |
| 1072 | 1075 | const trajectory: Trajectory = { ...recorded, frames: recorded.frames.map((f) => f.map((p, i) => bound(b, recorded.ids[i]!, p) ?? p)) }; |
| 1073 | const step = largestStep(trajectory); | |
| 1074 | 1076 | const asked = number(body.rate, 1, 0.1, 2, "rate"); |
| 1075 | const rate = step > 0 ? Math.min(asked, speedLimits().speed / (step * trajectory.hz)) : asked; | |
| 1077 | const all = speedLimits(); | |
| 1078 | // Slowed by the joint that is closest to its own limit, each judged against the speed it is capped at. | |
| 1079 | const rate = jointSteps(trajectory).reduce((r, st, i) => (st > 0 ? Math.min(r, jointMotion(all, trajectory.ids[i]!).speed / (st * trajectory.hz)) : r), asked); | |
| 1076 | 1080 | if (rate < asked) machine.note("clamp", "speed limit", { asked, sent: Number(rate.toFixed(3)), source: `play ${name}` }); |
| 1077 | 1081 | const controller = claim("play"); |
| 1078 | 1082 | try { |
| @@ -1107,6 +1111,7 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 1107 | 1111 | rate, |
| 1108 | 1112 | torquePercent: number(body.torque, 70, 10, 100, "torque"), |
| 1109 | 1113 | torqueCaps: torqueCaps(), |
| 1114 | speedCaps: new Map(trajectory.ids.map((id) => [id, jointMotion(all, id).speed] as const)), | |
| 1110 | 1115 | signal: controller.signal, |
| 1111 | 1116 | }); |
| 1112 | 1117 | if (body.release === true) await bus.syncWrite(STS3215.Torque_Enable, trajectory.ids.map((id) => [id, 0] as const)); |
| @@ -1379,7 +1384,7 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 1379 | 1384 | easing = true; |
| 1380 | 1385 | for (const [id, t] of targets) tracker.target(id, t, Date.now()); |
| 1381 | 1386 | try { |
| 1382 | await bus.syncWrite(STS3215.Goal_Velocity, targets.map(([id]) => [id, Math.min(easeSpeed, speedLimits().speed)] as const)); | |
| 1387 | await bus.syncWrite(STS3215.Goal_Velocity, targets.map(([id]) => [id, Math.min(easeSpeed, jointMotion(speedLimits(), id).speed)] as const)); | |
| 1383 | 1388 | await bus.syncWrite(STS3215.Goal_Position, targets); |
| 1384 | 1389 | const deadline = Date.now() + 4000; |
| 1385 | 1390 | const wanted = new Map(targets); |
| @@ -1396,7 +1401,7 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}): | ||
| 1396 | 1401 | if (worst <= 25) break; |
| 1397 | 1402 | await new Promise((r) => setTimeout(r, 50)); |
| 1398 | 1403 | } |
| 1399 | if (!closed) await bus.syncWrite(STS3215.Goal_Velocity, targets.map(([id]) => [id, speedLimits().speed] as const)); | |
| 1404 | if (!closed) await bus.syncWrite(STS3215.Goal_Velocity, targets.map(([id]) => [id, jointMotion(speedLimits(), id).speed] as const)); | |
| 1400 | 1405 | } finally { |
| 1401 | 1406 | easing = false; |
| 1402 | 1407 | } |
Software/so101/src/trajectory.ts +10
| @@ -62,3 +62,13 @@ export function largestStep(trajectory: Trajectory): number { | ||
| 62 | 62 | } |
| 63 | 63 | return largest; |
| 64 | 64 | } |
| 65 | ||
| 66 | /** Each joint's largest change between consecutive frames, in ticks, in the order of `trajectory.ids`. */ | |
| 67 | export function jointSteps(trajectory: Trajectory): number[] { | |
| 68 | const steps = trajectory.ids.map(() => 0); | |
| 69 | for (let i = 1; i < trajectory.frames.length; i++) { | |
| 70 | const prev = trajectory.frames[i - 1]!, next = trajectory.frames[i]!; | |
| 71 | for (let j = 0; j < steps.length; j++) steps[j] = Math.max(steps[j]!, Math.abs(next[j]! - prev[j]!)); | |
| 72 | } | |
| 73 | return steps; | |
| 74 | } |
new above
offline