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

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/477-speed-and-acceleration-limits-can-differ → main 4 commits fork

  • Merged. smith/477-speed-and-acceleration-limits-can-differ was merged into main as d181147.

Review

Sign in to review this change.
7 files changed +210 −26
Software/README.md +2
@@ -761,6 +761,8 @@ conservative. The owner changes them in the settings panel, or with
761761 | `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) |
762762 | `maxSpeedDegS` | 90 | no joint turns faster, on the stream, a move or a playback (a playback is slowed to fit) |
763763 | `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 |
764766 | `rateHz` | 60 | most commands per second; stream frames that come faster are merged, the latest winning |
765767 | `holdAfterMs` | 1000 | the dead-man: a stream silent this long makes the arm hold where it is |
766768 | `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 {
188188 export interface MoveOptions extends SupervisorOptions {
189189 /** Ticks per second for the joint that has the farthest to go; the others are slowed to arrive with it. */
190190 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>;
191193 /** Poll interval in ms. */
192194 pollMs?: number;
193195 }
@@ -208,7 +210,7 @@ export async function moveToPose(
208210 bus: FeetechBus,
209211 ids: readonly number[],
210212 targets: readonly number[],
211 { speed = 300, pollMs = 50, ...options }: MoveOptions = {},
213 { speed = 300, speedCaps, pollMs = 50, ...options }: MoveOptions = {},
212214 ): Promise<MoveResult> {
213215 if (!(speed >= 1 && Number.isFinite(speed))) throw new MotionError(`speed must be a positive number, got ${speed}`);
214216 if (targets.length !== ids.length || !targets.every(Number.isInteger)) {
@@ -219,7 +221,7 @@ export async function moveToPose(
219221 const { aims, adjusted } = supervisor.clamp(targets);
220222 const deltas = ids.map((id, i) => Math.abs(aims[i]! - start.get(id)!));
221223 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)));
223225 const velocities = deltas.map((d) => Math.max(20, Math.round(d / seconds)));
224226 await supervisor.goal(aims, velocities);
225227
@@ -292,6 +294,8 @@ export interface PlayOptions {
292294 rate?: number;
293295 torquePercent?: number;
294296 torqueCaps?: ReadonlyMap<number, number>;
297 /** Per-joint speed ceilings, ticks/s, for the ease to the first frame. */
298 speedCaps?: ReadonlyMap<number, number>;
295299 signal?: AbortSignal;
296300 onFrame?: (index: number) => void;
297301 }
@@ -306,7 +310,7 @@ export interface PlayOptions {
306310 export async function playTrajectory(
307311 bus: FeetechBus,
308312 trajectory: Trajectory,
309 { rate = 1, torquePercent = 70, torqueCaps, signal, onFrame }: PlayOptions = {},
313 { rate = 1, torquePercent = 70, torqueCaps, speedCaps, signal, onFrame }: PlayOptions = {},
310314 ): Promise<number> {
311315 const step = largestStep(trajectory);
312316 if (step > MAX_FRAME_STEP) {
@@ -314,7 +318,7 @@ export async function playTrajectory(
314318 }
315319 if (!(rate > 0 && rate <= 2)) throw new MotionError(`rate must be within 0-2, got ${rate}`);
316320 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 });
318322 const supervisor = new JointSupervisor(bus, ids, { torquePercent, torqueCaps, signal, tolerance: 4096 });
319323 let previous = [...(await supervisor.arm(254)).values()];
320324 const period = 1000 / (trajectory.hz * rate);
Software/so101/src/safety.test.ts +52 −1
@@ -1,5 +1,5 @@
11 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";
33
44 describe("safety settings", () => {
55 test("conservative defaults, and every knob checked against its range", () => {
@@ -113,3 +113,54 @@ describe("frames off the wire", () => {
113113 expect(parseFrame("nope")).toBeNull();
114114 });
115115 });
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 {
2727 gripperTorquePercent: number;
2828 /** Per joint, the narrower range it may be sent to, degrees from its middle; joints not named keep their calibrated range. */
2929 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>;
3034 }
3135
3236 export const DEFAULT_SAFETY: SafetySettings = {
@@ -38,6 +42,8 @@ export const DEFAULT_SAFETY: SafetySettings = {
3842 parkSpeedDegS: 20,
3943 gripperTorquePercent: 35,
4044 jointLimitsDeg: {},
45 jointSpeedDegS: {},
46 jointAccelDegS2: {},
4147 };
4248
4349 /** Each number knob and the range an admin may set it within. */
@@ -50,7 +56,7 @@ const RANGES = {
5056 gripperTorquePercent: [10, 60],
5157 } as const satisfies Record<string, readonly [number, number]>;
5258
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;
5460
5561 export const TICKS_PER_DEGREE = 4096 / 360;
5662 /** No target comes closer than this to a servo's calibrated end, ticks. */
@@ -60,7 +66,7 @@ export class SafetyError extends Error {}
6066
6167 /** The safety part of a settings object, checked field by field; throws SafetyError naming the first bad one. */
6268 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 } };
6470 for (const [key, [lo, hi]] of Object.entries(RANGES) as Array<[keyof typeof RANGES, readonly [number, number]]>) {
6571 const value = input[key];
6672 if (value === undefined) continue;
@@ -86,6 +92,21 @@ export function validateSafety(input: Partial<Record<keyof SafetySettings, unkno
8692 out.jointLimitsDeg[name] = [range[0], range[1]];
8793 }
8894 }
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 }
89110 return out;
90111 }
91112
@@ -122,9 +143,30 @@ export function bound(bounds: ReadonlyMap<number, Bounds>, id: number, ticks: nu
122143 return b ? clamp(Math.round(ticks), b.min, b.max) : null;
123144 }
124145
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
125153 /** 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;
128170 }
129171
130172 /** 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 {
149191 */
150192 export class Governor {
151193 readonly #bounds: () => ReadonlyMap<number, Bounds>;
152 readonly #limits: () => { speed: number; accel: number };
194 readonly #limits: () => MotionLimits;
153195 readonly #tracks = new Map<number, Track>();
154196
155 constructor(bounds: () => ReadonlyMap<number, Bounds>, limits: () => { speed: number; accel: number }) {
197 constructor(bounds: () => ReadonlyMap<number, Bounds>, limits: () => MotionLimits) {
156198 this.#bounds = bounds;
157199 this.#limits = limits;
158200 }
@@ -203,10 +245,11 @@ export class Governor {
203245
204246 /** Advance every joint by `dt` seconds; returns the goals that changed on the wire, rounded to ticks. */
205247 step(dt: number): Array<readonly [number, number]> {
206 const { speed, accel } = this.#limits();
248 const limits = this.#limits();
207249 const out: Array<readonly [number, number]> = [];
208250 for (const [id, t] of this.#tracks) {
209251 if (t.p === t.goal && t.v === 0) continue;
252 const { speed, accel } = jointMotion(limits, id);
210253 const before = Math.round(t.p);
211254 const vmax = Math.min(speed, t.slow ?? Infinity);
212255 const error = t.goal - t.p;
Software/so101/src/server.test.ts +70 −1
@@ -1,5 +1,5 @@
11 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";
33 import { connect } from "node:net";
44 import { tmpdir } from "node:os";
55 import { join } from "node:path";
@@ -999,6 +999,75 @@ describe("settings on disk", () => {
999999 });
10001000 });
10011001
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
10021071 describe("the safety envelope", () => {
10031072 // Pan 818-3487 (middle 2152.5), lift 813-3202, gripper 1995-3525; no model, so no zone: only the envelope is at work.
10041073 let bus: FakeBus;
Software/so101/src/server.ts +18 −13
@@ -86,8 +86,8 @@ import { MotionError, moveToPose, playTrajectory, recordTrajectory, STALL_SPAN_T
8686 import { assertPoseName, parsePoseFile, serializePoseFile, type PoseFile } from "./poses.ts";
8787 import { MAX_ZERO_OFFSET_DEG, zeroOffsetDeg } from "./joint-zero.ts";
8888 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";
9191
9292 export interface ServerOptions {
9393 host?: string;
@@ -343,9 +343,9 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}):
343343 const torqueCaps = () => new Map([[6, settings.gripperTorquePercent]]);
344344 /** The servo registers that enforce the same speed and acceleration in hardware, whatever the server sends. */
345345 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));
349349 };
350350 let estop: EStop | null = null;
351351 try {
@@ -660,10 +660,13 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}):
660660 machine.note("clamp", "zone", { what: violation.what, link: violation.link, depthMm: Math.round(violation.depth * 1000), refused: settings.zone === "enforce", source });
661661 if (settings.zone === "enforce") return { status: 409, body: { reached: false, error: `refused: ${describeViolation(violation)}`, violation } };
662662 }
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));
664666 if (typeof body.speed === "number" && body.speed > speed) machine.note("clamp", "speed limit", { asked: body.speed, sent: speed, source });
665667 const result = await moveToPose(bus, targetIds, positions, {
666668 speed,
669 speedCaps,
667670 torquePercent: number(body.torque, 70, 10, 100, "torque"),
668671 torqueCaps: torqueCaps(),
669672 signal: controller.signal,
@@ -840,11 +843,11 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}):
840843 return json(res, 200, { ...settings, model: model !== null });
841844 case "PUT /settings": {
842845 const body = await readBody(req);
843 const merged = (key: "jointOffsetsDeg" | "jointLimitsDeg") => {
846 const merged = (key: "jointOffsetsDeg" | "jointLimitsDeg" | "jointSpeedDegS" | "jointAccelDegS2") => {
844847 const patch = body[key];
845848 return patch && typeof patch === "object" && !Array.isArray(patch) ? { ...settings[key], ...patch } : (patch ?? settings[key]);
846849 };
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>);
848851 writeAtomic(settingsFile, JSON.stringify(settings, null, 1));
849852 return json(res, 200, { ...settings, model: model !== null });
850853 }
@@ -979,7 +982,7 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}):
979982 if (!Number.isInteger(body.position)) throw new HttpError(400, "position must be an integer");
980983 const position = bound(bounds(), id, body.position as number)!;
981984 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
983986 if (position !== body.position) machine.note("clamp", "joint limit", { id, asked: body.position as number, sent: position, source: "grip" });
984987 if (typeof body.torque === "number" && body.torque > torque) machine.note("clamp", "gripper force", { asked: body.torque, sent: torque, source: "grip" });
985988 claim("move");
@@ -1070,9 +1073,10 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}):
10701073 // Every frame inside the joints' bounds, and the whole playback no faster than the speed limit allows.
10711074 const b = bounds();
10721075 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);
10741076 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);
10761080 if (rate < asked) machine.note("clamp", "speed limit", { asked, sent: Number(rate.toFixed(3)), source: `play ${name}` });
10771081 const controller = claim("play");
10781082 try {
@@ -1107,6 +1111,7 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}):
11071111 rate,
11081112 torquePercent: number(body.torque, 70, 10, 100, "torque"),
11091113 torqueCaps: torqueCaps(),
1114 speedCaps: new Map(trajectory.ids.map((id) => [id, jointMotion(all, id).speed] as const)),
11101115 signal: controller.signal,
11111116 });
11121117 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 = {}):
13791384 easing = true;
13801385 for (const [id, t] of targets) tracker.target(id, t, Date.now());
13811386 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));
13831388 await bus.syncWrite(STS3215.Goal_Position, targets);
13841389 const deadline = Date.now() + 4000;
13851390 const wanted = new Map(targets);
@@ -1396,7 +1401,7 @@ export async function startServer(bus: FeetechBus, options: ServerOptions = {}):
13961401 if (worst <= 25) break;
13971402 await new Promise((r) => setTimeout(r, 50));
13981403 }
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));
14001405 } finally {
14011406 easing = false;
14021407 }
Software/so101/src/trajectory.ts +10
@@ -62,3 +62,13 @@ export function largestStep(trajectory: Trajectory): number {
6262 }
6363 return largest;
6464 }
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 }

↑↓ navigate ↵ open ⌘↵ its app esc close

Keyboard shortcuts

Docs