feat: tuned servo PID gains, and uncapped teleop setpoints

[Nick Hehr (HipsterBrown)]

Acts on a pidtune characterization of the SO-101's servos (2026-09-02, 6-DoF arm, STS3215, 12.2–12.3 V), plus two findings from testing this branch on the arm.

Changes

1. The module writes tuned per-joint PID gains at servo init. It never wrote them before — servos ran on whatever was in EEPROM.

id	joint	P	D	I
1	shoulder pan	32	48	0
2	shoulder lift	48	64	2
3	elbow	48	64	4
4	wrist flex	48	64	2
5	wrist roll	48	48	0
6	gripper	never written — uncharacterized

Stock is 32/32/0. Steady-state error on the gravity-loaded joints drops ~4× (0.90° → 0.22° on joint 2); at stock P=32 the elbow could not complete a 100-count move against gravity inside 1.5 s, and does it in 130 ms at P=48. Nothing exceeds P=48 — P=64 oscillated at every D tested.

Writes are read-first and skipped when already correct, because EEPROM endurance is finite and this path runs on every Reconfigure and up to 3× per bring-up. Order is D → I → P and a failed write is terminal for that servo: a non-zero I is only stable against a raised D (joint 2 oscillated at D=32,I=4, stable at D=48,I=4), so damping-first keeps every reachable partial state at least as stable as stock.

2. Manual-mode compliance zeroes i_gain, not just p_gain. The integral path is independent of P, so hand-guiding accumulated standing error while a human held the arm off-target, then pushed back on release. Inert until change 1 gave the joints a non-zero I — the two must land together.

3. Teleop streams setpoints with no speed cap, no acceleration ramp, and no coordination read ("streamed": true in MoveToJointPositions's extra). A streamed caller shapes the trajectory with its own update rate, so any profile the servo applies between setpoints is pure lag, and coordinated arrival is meaningless for a goal superseded in ~10 ms.

	lag	rms err
acc=0	40 ms	25.5
acc=64	90 ms	61.4
acc=32	160 ms	103.2

Shape error stays flat at 6–7 throughout — the follower traced the right path, just late. The speed half came from testing this branch: raising speed_degs_per_sec kept improving tracking, so the cap was binding once the ramp stopped dominating.

4. Teleop default rate 20 Hz → 100 Hz. Command rate dominates smoothness more than any gain: 30 → 100 Hz cut chatter ~3.5× and tracking error ~28%, and at 30 Hz lag was pinned at 67 ms for every gain set, floored by the command period. The loop's ticker drops ticks, so a bus that can't sustain 100 Hz degrades rather than backing up.

Because 3 removes the bound that made a large initial gap safe, the mirror stays on profiled moves until the follower is within 5° of the leader, then latches. Guarding only the first cycle would not work — a servo goal write supersedes the previous one, so the next cycle 10 ms later would overwrite it while the arm was still far away. Exposed as streaming in the status DoCommand.

Reviewer notes

• New behavior on existing installs. Upgrading rewrites servo EEPROM on first init. disable_servo_gains: true opts out; pidtune apply is the path for a differently-tuned arm.
• Peak current rises steeply with D — 388 units at D=16 vs 691 at D=48 on a 100-count step. Not thermal in the study session (37–43 °C, +1–2 °C), but it would matter under sustained high-duty motion.
• API surface: new disable_servo_gains arm attribute, new streamed key in MoveToJointPositions's extra, new streaming field in teleop status, changed teleop rate_hz default.
• Unchanged on purpose: DefaultAccelDegsPerSecSq already maps to Acc 49, inside the measured optimum. MinExecutableSpeedDegsPerSec, DefaultLookaheadDeg and maxAccUnits are all now measured against stale assumptions; flagged in place, not changed.

Testing

go test ./..., gofmt -s -l ., go vet ./..., make bin/arm clean.

hardware-verified. Before merge:

1. Gains write once on bring-up; a Reconfigure writes nothing.
2. Arm holds under gravity; no oscillation on joints 2 and 3 (the study found joint 2 conditionally stable at I=4; it ships at I=2).
3. Manual mode: hold a joint off-target ~10 s, release — no push-back.
4. Teleop at 100 Hz, all 6 joints. The ticker drops ticks silently, so compare the status cycle count against elapsed time rather than trusting the absence of errors.
5. Start teleop with the arms apart — follower travels at the configured speed, then streaming flips true.

🤖 Generated with Claude Code