feat: tuned servo PID gains, and uncapped teleop setpoints

CLAUDE.md

@@ -526,3 +526,49 @@ calibration wizard. It is bundled into `module.tar.gz` and needs **Node ≥ 20**
   be mistaken for having done anything. `abort`'s advertised states are unchanged — only the
   guard was added. One gap remains by choice: `error` still accepts `abort` without advertising
   it, which is harmless because `reset` is the escape hatch offered there.
+- **Compliance zeroes `i_gain`, not just `p_gain` — and a failed READ falls through to the
+  WRITE, not away from it.** The integral path is independent of P, so lowering P to 8 during
+  hand-guided compliance used to leave a live integrator; inert until the tuned gains carry a
+  non-zero I (`internal/servo/gains.go`), which they now do. Gated on `p_gain > 0`, the
+  documented per-register opt-out; `d_gain` is deliberately left alone (reasoned drag, never
+  measured). Both `applyCompliance`/`restoreCompliance` and `applyServoGains` route every gain
+  write through one `writeGainIfChanged` (`components/arm/gains.go`) because these are EEPROM
+  registers entered/exited (or reconfigured) repeatedly — and it treats a read it cannot trust
+  as unable to prove a no-op, so it falls through to the write rather than skipping it. The
+  asymmetry is deliberate: `applyCompliance` failing outright leaves the arm stiff, which is
+  safe; `restoreCompliance` skipping a write on a flaky read would leave a joint at `p_gain
+  8`/`i_gain 0` in EEPROM — soft, across power cycles — on a transient this hardware really
+  produces, with nothing retrying it. Don't make either path bail out on a read error.
+- **Tuned gains are written during `doServoInitialization`, in D→I→P order, and a failed WRITE
+  is terminal for that servo.** `applyServoGains` BREAKS its per-servo loop on the first write
+  failure rather than logging and continuing: without the break, {I,P}-without-D is reachable —
+  48/32/4 on servo 3, the exact combination measured to oscillate on a gravity-loaded joint —
+  and it would be stranded in EEPROM with nothing to retry it (`applyServoGains` returns no
+  error, so `initializeServosWithRetry` never re-runs for a gain failure alone). The read-first
+  skip is the EEPROM wear guard, and it runs on every `Reconfigure` as well as up to 3× within
+  one bring-up. `servo.Gains` fields are `byte`, not `int`, so an out-of-range retune is a
+  compile error rather than a silent truncation to less damping. Config is one
+  `disable_servo_gains` bool; there are no per-joint overrides — `pidtune apply` is the
+  escape hatch for a differently-tuned arm.
+- **A streamed setpoint gets Speed 0 AND Acc 0 AND no coordination read; a profiled move gets
+  none of the three.** The `"streamed": true` key in `MoveToJointPositions`'s `extra` map
+  (`components/arm/motion.go`) branches to `moveJointsUniform` before the `currentJoints()` read
+  and forces both registers to their unbounded sentinel — 0 is MAXIMUM speed and UNLIMITED
+  acceleration, not stopped and not still. One principle covers all three: 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 ~10ms.
+  Both halves were confirmed on hardware, acceleration first (acc=32 quadrupled RMS error and
+  took lag 40ms→160ms) and speed second — raising `speed_degs_per_sec` kept improving teleop
+  tracking until the cap stopped binding, which is what motivated removing it. `minSpeedSteps`
+  and `MinAccUnits` are both still 1, so the profiled path cannot reach either sentinel: a low
+  *configured* value is not a substitute (Acc 1 delivers ~43 deg/s², worse than the acc=32 row).
+  Measured in tests: one bus packet per call instead of two. Do NOT unify the two paths, and do
+  not split `streamed` back into per-register flags — nothing wants them separately, and the
+  read-skip belongs to the same decision. **It removes a bound**, so a caller must not hand it a large
+  position error. Teleop gates itself: `readyToStream` keeps the mirror on profiled moves
+  until the follower is within `maxStreamStartGapDeg` of the leader, then LATCHES. The latch
+  is the whole point -- profiling only the first cycle would be pointless, because a servo
+  goal write supersedes the previous one, so the next cycle 10ms later would overwrite the
+  profiled goal with an unbounded one while the arm was still far away. It costs one extra
+  follower read per cycle, only until it trips, and it must NOT re-evaluate afterwards: a
+  running mirror legitimately trails the leader by more than the gate.

components/arm/arm.go

@@ -71,6 +71,10 @@ type SO101ArmConfig struct {
 	// the feature / uses built-in defaults throughout.
 	ManualMode *ManualModeConfig `json:"manual_mode,omitempty"`
 
+	// DisableServoGains leaves the servos' PID registers untouched at init. Set it if you
+	// tuned gains externally. See docs/arm.md, "Servo gains".
+	DisableServoGains bool `json:"disable_servo_gains,omitempty"`
+
 	// OrientationToleranceDeg is the half-angle, in degrees, of the cone of acceptable
 	// end-effector approach directions around the goal orientation. Roll about that axis
 	// is NOT constrained. Valid range [0, 180]. Zero or unset means
@@ -420,6 +424,10 @@ func (s *so101) doServoInitialization() error {
 	}
 	s.logger.Debug("All servos ping successful")
 
+	if !s.cfg.DisableServoGains {
+		applyServoGains(ctx, s.controller, s.armServoIDs, servo.DefaultArmGains, s.logger)
+	}
+
 	// Enable torque for all servos (controller manages all 6)
 	s.logger.Debug("Enabling torque for all servos...")
 	if err := s.controller.SetTorqueEnable(ctx, true); err != nil {

components/arm/gains.go

@@ -0,0 +1,50 @@
+package arm
+
+import (
+	"context"
+
+	"go.viam.com/rdk/logging"
+
+	"so_arm/internal/controller"
+	"so_arm/internal/servo"
+)
+
+// writeGainIfChanged writes a one-byte gain register unless it can prove the write would
+// change nothing. These are EEPROM registers and both callers rewrite the same values
+// repeatedly, so the skip is a wear guard.
+//
+// A FAILED read falls through to the write: it cannot prove the write is unnecessary, and
+// skipping one on restoreCompliance's path leaves a joint soft in EEPROM. See CLAUDE.md,
+// "Compliance zeroes i_gain".
+func writeGainIfChanged(ctx context.Context, h *controller.ControllerHandle, id int, name string, want byte) error {
+	if cur, err := h.ReadServoRegister(ctx, id, name); err == nil && len(cur) == 1 && cur[0] == want {
+		return nil
+	}
+	return h.WriteServoRegister(ctx, id, name, []byte{want})
+}
+
+// applyServoGains writes the tuned P/D/I table to the arm's own servos.
+//
+// D, then I, then P, and a failed write BREAKS out for that servo: a non-zero I is only
+// stable against a raised D, so damping-first makes every prefix of the sequence at least as
+// stable as the state before it -- but only if a failure is terminal, or {I,P}-without-D
+// strands a gravity-loaded joint on a combination measured to oscillate. Never returns an
+// error; the arm works at stock gains. See docs/arm.md, "Servo gains".
+func applyServoGains(ctx context.Context, h *controller.ControllerHandle, ids []int, gains map[int]servo.Gains, logger logging.Logger) {
+	for _, id := range ids {
+		g, ok := gains[id]
+		if !ok {
+			continue
+		}
+		for _, w := range []struct {
+			name string
+			want byte
+		}{{"d_gain", g.D}, {"i_gain", g.I}, {"p_gain", g.P}} {
+			if err := writeGainIfChanged(ctx, h, id, w.name, w.want); err != nil {
+				logger.Warnf("servo %d: %s write failed; leaving it and this servo's "+
+					"remaining gains as found: %v", id, w.name, err)
+				break
+			}
+		}
+	}
+}

components/arm/gains_test.go

@@ -0,0 +1,197 @@
+package arm
+
+import (
+	"context"
+	"syscall"
+	"testing"
+
+	"github.com/hipsterbrown/feetech-servo/feetech"
+	"github.com/stretchr/testify/assert"
+	"github.com/stretchr/testify/require"
+	"go.viam.com/rdk/logging"
+
+	"so_arm/internal/servo"
+	"so_arm/internal/testfake"
+)
+
+func gainsTestArm(t *testing.T, ft feetech.Transport, cfg *SO101ArmConfig) *so101 {
+	t.Helper()
+	return &so101{
+		logger:      logging.NewTestLogger(t),
+		controller:  testArmHandle(t, ft),
+		armServoIDs: []int{1, 2, 3, 4, 5},
+		cfg:         cfg,
+		initCtx:     context.Background(),
+	}
+}
+
+func seedGains(ft *testfake.FakeTransport, p, d, i byte, ids ...int) {
+	for _, id := range ids {
+		ft.SetRegister(id, feetech.RegPGain.Address, []byte{p})
+		ft.SetRegister(id, feetech.RegDGain.Address, []byte{d})
+		ft.SetRegister(id, feetech.RegIGain.Address, []byte{i})
+	}
+}
+
+func TestServoInitializationWritesTheTunedGains(t *testing.T) {
+	ft := testfake.NewFakeTransport()
+	seedGains(ft, 32, 32, 0, 1, 2, 3, 4, 5, 6)
+	s := gainsTestArm(t, ft, &SO101ArmConfig{})
+
+	require.NoError(t, s.initializeServos())
+
+	assert.Equal(t, byte(48), readServoGain(t, s, 2, "p_gain"))
+	assert.Equal(t, byte(64), readServoGain(t, s, 2, "d_gain"))
+	assert.Equal(t, byte(2), readServoGain(t, s, 2, "i_gain"))
+	assert.Equal(t, byte(4), readServoGain(t, s, 3, "i_gain"))
+	// Servo 1 is the only P that is not 48, servo 5 the only D that is not 64: a
+	// transcription slip in either is caught by nothing else.
+	assert.Equal(t, byte(32), readServoGain(t, s, 1, "p_gain"))
+	assert.Equal(t, byte(48), readServoGain(t, s, 5, "d_gain"))
+}
+
+// orderRecordingTransport records the order of gain writes to one servo at the wire level.
+type orderRecordingTransport struct {
+	*testfake.FakeTransport
+	id   int
+	seen []byte
+}
+
+func (o *orderRecordingTransport) Write(p []byte) (int, error) {
+	// Packet: [0xFF, 0xFF, id, len, instruction, params..., checksum]. WRITE is 0x03.
+	if len(p) >= 7 && p[0] == 0xFF && p[1] == 0xFF && int(p[2]) == o.id && p[4] == 0x03 &&
+		(p[5] == feetech.RegPGain.Address || p[5] == feetech.RegDGain.Address ||
+			p[5] == feetech.RegIGain.Address) {
+		o.seen = append(o.seen, p[5])
+	}
+	return o.FakeTransport.Write(p)
+}
+
+// A non-zero I is only stable against a raised D, so any other order leaves a window where a
+// bus failure strands the servo above its stable I. Final register values are
+// order-independent, so this needs the wire.
+func TestApplyServoGainsWritesDampingBeforeIntegral(t *testing.T) {
+	ft := testfake.NewFakeTransport()
+	seedGains(ft, 32, 32, 0, 1, 2, 3, 4, 5, 6)
+	rec := &orderRecordingTransport{FakeTransport: ft, id: 3}
+
+	require.NoError(t, gainsTestArm(t, rec, &SO101ArmConfig{}).initializeServos())
+
+	assert.Equal(t, []byte{
+		feetech.RegDGain.Address, feetech.RegIGain.Address, feetech.RegPGain.Address,
+	}, rec.seen, "gains must be written D, then I, then P")
+}
+
+func TestServoInitializationRespectsDisableServoGains(t *testing.T) {
+	ft := testfake.NewFakeTransport()
+	seedGains(ft, 32, 32, 0, 1, 2, 3, 4, 5)
+	s := gainsTestArm(t, ft, &SO101ArmConfig{DisableServoGains: true})
+
+	require.NoError(t, s.initializeServos())
+
+	for _, id := range []int{1, 2, 3, 4, 5} {
+		for _, reg := range gainRegisters() {
+			assert.Zero(t, ft.WriteCount(id, reg), "servo %d register %d must not be written", id, reg)
+		}
+	}
+}
+
+// The invariant is "no servo outside this arm's own ids", not "not servo 6": a 4-DOF arm
+// configured with servo_ids [1,2,3,4] hosts a gripper on servo 5, and DefaultArmGains[5]
+// exists.
+func TestApplyServoGainsTouchesOnlyTheArmsOwnServos(t *testing.T) {
+	ft := testfake.NewFakeTransport()
+	seedGains(ft, 32, 32, 0, 1, 2, 3, 4, 5, 6)
+	s := gainsTestArm(t, ft, &SO101ArmConfig{})
+	s.armServoIDs = []int{1, 2, 3, 4}
+
+	require.NoError(t, s.initializeServos())
+
+	for _, id := range []int{5, 6} {
+		for _, reg := range gainRegisters() {
+			assert.Zero(t, ft.WriteCount(id, reg), "servo %d is not this arm's, register %d", id, reg)
+		}
+	}
+}
+
+// EEPROM has finite write endurance and initializeServosWithRetry runs this up to 3 times.
+func TestApplyServoGainsSkipsNoOpWrites(t *testing.T) {
+	ft := testfake.NewFakeTransport()
+	for id, g := range servo.DefaultArmGains {
+		seedGains(ft, byte(g.P), byte(g.D), byte(g.I), id)
+	}
+	s := gainsTestArm(t, ft, &SO101ArmConfig{})
+
+	require.NoError(t, s.initializeServos())
+
+	for _, id := range []int{1, 2, 3, 4, 5} {
+		for _, reg := range gainRegisters() {
+			assert.Zero(t, ft.WriteCount(id, reg),
+				"servo %d register %d was already correct", id, reg)
+		}
+	}
+}
+
+// failingWriteTransport wraps a FakeTransport and fails exactly the FIRST (servo, register)
+// WRITE packet at the wire level -- everything else (PING, SYNC_READ, reads, the EEPROM
+// unlock/relock writes to the lock register, writes to any other register or servo, and a
+// later write to the SAME register once failsLeft is exhausted) passes through untouched.
+//
+// The failure is injected on the WRITE, not the read: writeGainIfChanged deliberately falls
+// through to the write when a read fails, so a read failure lands the gain anyway and would
+// make this test vacuous. Failing only once, not forever, is what lets the test both trigger
+// the warn-and-continue path during init AND read the register back afterward. The ping and
+// position read go over the SAME transport and must keep succeeding -- that is the proof
+// this exercises the real contract rather than a transport that is simply broken.
+type failingWriteTransport struct {
+	*testfake.FakeTransport
+	id        int
+	address   byte
+	failsLeft int
+}
+
+func (f *failingWriteTransport) Write(p []byte) (int, error) {
+	// Packet: [0xFF, 0xFF, id, len, instruction, params..., checksum]. WRITE is
+	// instruction 0x03 with params [address, data...].
+	if f.failsLeft > 0 && len(p) >= 7 && p[0] == 0xFF && p[1] == 0xFF &&
+		int(p[2]) == f.id && p[4] == 0x03 && p[5] == f.address {
+		f.failsLeft--
+		return 0, syscall.EIO
+	}
+	return f.FakeTransport.Write(p)
+}
+
+// A gain write failing must not fail initialization: the arm works at stock gains. Only
+// servo 2's d_gain WRITE fails; ping (PING) and the position read (SYNC_READ) use different
+// instructions and are unaffected, so their continued success is the proof this exercises
+// the real warn-and-continue path and not a transport that is simply broken outright.
+func TestApplyServoGainsFailureDoesNotFailInit(t *testing.T) {
+	ft := testfake.NewFakeTransport()
+	seedGains(ft, 32, 32, 0, 1, 2, 3, 4, 5, 6)
+	failing := &failingWriteTransport{FakeTransport: ft, id: 2, address: feetech.RegDGain.Address, failsLeft: 1}
+	s := gainsTestArm(t, failing, &SO101ArmConfig{})
+
+	require.NoError(t, s.initializeServos())
+
+	// Servo 2's d_gain write failed, so it must be left exactly as found.
+	assert.Equal(t, byte(32), readServoGain(t, s, 2, "d_gain"), "servo 2 d_gain must be left as found after a write failure")
+	// Terminal for the rest of servo 2's gains on purpose: 48/32/4 is the combination the
+	// write order exists to prevent, and it would persist in EEPROM with no retry.
+	assert.Equal(t, byte(0), readServoGain(t, s, 2, "i_gain"), "servo 2 i_gain must not land over a stock d_gain")
+	assert.Equal(t, byte(32), readServoGain(t, s, 2, "p_gain"), "servo 2 p_gain must not land over a stock d_gain")
+	// It must NOT cascade to any other servo.
+	assert.Equal(t, byte(64), readServoGain(t, s, 3, "d_gain"), "servo 3 d_gain must still land")
+	assert.Equal(t, byte(4), readServoGain(t, s, 3, "i_gain"), "servo 3 i_gain must still land")
+}
+
+func gainRegisters() []byte {
+	return []byte{feetech.RegPGain.Address, feetech.RegDGain.Address, feetech.RegIGain.Address}
+}
+
+func readServoGain(t *testing.T, s *so101, id int, name string) byte {
+	t.Helper()
+	got, err := s.controller.ReadServoRegister(context.Background(), id, name)
+	require.NoError(t, err)
+	require.Len(t, got, 1)
+	return got[0]
+}