From 039a617f2b80eadc1f15d7c468de38cf5d68ba87 Mon Sep 17 00:00:00 2001 From: HipsterBrown Date: Wed, 2 Sep 2026 13:20:27 -0400 Subject: [PATCH 01/14] test(testfake): record sync writes and count register writes Co-Authored-By: Claude Opus 5 (1M context) Claude-Session: https://claude.ai/code/session_01NHef7PBha9FHmWoCSduR8J --- internal/testfake/fake_transport.go | 31 +++++++++++++++++++++++- internal/testfake/fake_transport_test.go | 30 +++++++++++++++++++++++ 2 files changed, 60 insertions(+), 1 deletion(-) diff --git a/internal/testfake/fake_transport.go b/internal/testfake/fake_transport.go index bb8ea02..1074a1e 100644 --- a/internal/testfake/fake_transport.go +++ b/internal/testfake/fake_transport.go @@ -25,6 +25,7 @@ type FakeTransport struct { mu sync.Mutex proto *feetech.Protocol registers map[int]map[byte][]byte // servo id -> address -> value + regWrites map[int]map[byte]int // servo id -> address -> write count pending []byte // queued response bytes unplugged bool writes int @@ -38,6 +39,7 @@ func NewFakeTransport() *FakeTransport { ft := &FakeTransport{ proto: feetech.NewProtocol(feetech.ProtocolSTS), registers: make(map[int]map[byte][]byte), + regWrites: make(map[int]map[byte]int), } for id := 1; id <= 6; id++ { ft.registers[id] = map[byte][]byte{ @@ -133,6 +135,10 @@ func (ft *FakeTransport) reply(req []byte) { } if regs, ok := ft.registers[id]; ok { regs[params[0]] = append([]byte(nil), params[1:]...) + if _, ok := ft.regWrites[id]; !ok { + ft.regWrites[id] = map[byte]int{} + } + ft.regWrites[id][params[0]]++ } ft.queue(id, nil) case 0x82: // SYNC_READ [address, length, ids...] @@ -143,7 +149,21 @@ func (ft *FakeTransport) reply(req []byte) { for _, raw := range params[2:] { ft.queue(int(raw), ft.value(int(raw), address, length)) } - case 0x83: // SYNC_WRITE -- no response on the wire + case 0x83: // SYNC_WRITE -- broadcast, no response on the wire + // Params are [address, dataLen, id, data..., id, data...]. The whole blob is stored + // at `address`, so a read of the FIRST register in the blob reads back correctly -- + // which is what a goal write's `acceleration` needs. A read of a register further + // into the blob (goal_position, at address+1) does NOT; split per register if a + // test ever needs that. + if len(params) < 2 { + return + } + address, n := params[0], int(params[1]) + for i := 2; i+1+n <= len(params); i += 1 + n { + if regs, ok := ft.registers[int(params[i])]; ok { + regs[address] = append([]byte(nil), params[i+1:i+1+n]...) + } + } } } @@ -183,6 +203,15 @@ func (ft *FakeTransport) CloseCount() int { return ft.closes } +// WriteCount reports how many WRITE packets have landed on one register, so a test can +// assert a read-first guard actually skipped a redundant write -- reading the value back +// cannot distinguish "already correct" from "written again". +func (ft *FakeTransport) WriteCount(id int, address byte) int { + ft.mu.Lock() + defer ft.mu.Unlock() + return ft.regWrites[id][address] +} + // EncodeWordLE encodes a 16-bit register value the way the STS protocol expects. func EncodeWordLE(v int) []byte { return feetech.NewProtocol(feetech.ProtocolSTS).EncodeWord(uint16(v)) diff --git a/internal/testfake/fake_transport_test.go b/internal/testfake/fake_transport_test.go index 16aed5a..edd2fd8 100644 --- a/internal/testfake/fake_transport_test.go +++ b/internal/testfake/fake_transport_test.go @@ -49,3 +49,33 @@ func TestFakeTransportIsSilentForAbsentServos(t *testing.T) { assert.ErrorIs(t, err, feetech.ErrNoResponse, "a silent servo must be distinguishable from an unplugged port") } + +func TestWriteCountTracksRegisterWrites(t *testing.T) { + ft := NewFakeTransport() + if got := ft.WriteCount(1, feetech.RegPGain.Address); got != 0 { + t.Fatalf("expected 0 writes before any traffic, got %d", got) + } + // Seeding a register is not a write. + ft.SetRegister(1, feetech.RegPGain.Address, []byte{32}) + if got := ft.WriteCount(1, feetech.RegPGain.Address); got != 0 { + t.Fatalf("SetRegister must not count as a write, got %d", got) + } +} + +// Goal writes reach the bus as SYNC_WRITE, so without this a test cannot see what speed or +// acceleration was commanded. +func TestSyncWriteLandsInTheRegisterStore(t *testing.T) { + ft := NewFakeTransport() + // [address, dataLen, id, data..., id, data...] + params := []byte{feetech.RegAcceleration.Address, 2, 1, 0x11, 0x22, 3, 0x33, 0x44} + ft.mu.Lock() + ft.reply(append([]byte{0xFF, 0xFF, 0xFE, byte(len(params) + 2), 0x83}, append(params, 0)...)) + ft.mu.Unlock() + + if got := ft.value(1, feetech.RegAcceleration.Address, 1); got[0] != 0x11 { + t.Fatalf("servo 1 acc: got %#x, want 0x11", got[0]) + } + if got := ft.value(3, feetech.RegAcceleration.Address, 1); got[0] != 0x33 { + t.Fatalf("servo 3 acc: got %#x, want 0x33", got[0]) + } +} From 71ddfb460235177d1e112463d132031c5c014919 Mon Sep 17 00:00:00 2001 From: HipsterBrown Date: Wed, 2 Sep 2026 13:22:55 -0400 Subject: [PATCH 02/14] fix(arm): zero i_gain during compliance, behind a shared EEPROM wear guard Co-Authored-By: Claude Opus 5 (1M context) Claude-Session: https://claude.ai/code/session_01NHef7PBha9FHmWoCSduR8J --- components/arm/gains.go | 29 ++++++++ components/arm/manual.go | 31 ++++++++- components/arm/manual_compliance_test.go | 84 ++++++++++++++++++++++++ 3 files changed, 141 insertions(+), 3 deletions(-) create mode 100644 components/arm/gains.go create mode 100644 components/arm/manual_compliance_test.go diff --git a/components/arm/gains.go b/components/arm/gains.go new file mode 100644 index 0000000..0187596 --- /dev/null +++ b/components/arm/gains.go @@ -0,0 +1,29 @@ +package arm + +import ( + "context" + "fmt" + + "so_arm/internal/controller" +) + +// writeGainIfChanged writes a one-byte gain register only when it does not already hold the +// target value. +// +// p_gain, d_gain and i_gain are all EEPROM registers (addresses 21-23) with finite write +// endurance, and every write costs three bus transactions for the unlock/write/relock dance. +// Both callers rewrite the same values repeatedly -- manual mode on every entry and exit, +// arm init on every Reconfigure -- so the skip is the wear guard, not an optimization. +func writeGainIfChanged(ctx context.Context, h *controller.ControllerHandle, id int, name string, want byte) error { + cur, err := h.ReadServoRegister(ctx, id, name) + if err != nil { + return fmt.Errorf("read %s servo %d: %w", name, id, err) + } + if len(cur) != 1 { + return fmt.Errorf("unexpected %s width %d for servo %d", name, len(cur), id) + } + if cur[0] == want { + return nil + } + return h.WriteServoRegister(ctx, id, name, []byte{want}) +} diff --git a/components/arm/manual.go b/components/arm/manual.go index 1789e1e..6d50b17 100644 --- a/components/arm/manual.go +++ b/components/arm/manual.go @@ -222,6 +222,7 @@ type controllerManualIO struct { ids []int limits map[int][2]float64 origPGain map[int]int // captured lazily on applyCompliance for restore + origIGain map[int]int // captured lazily on applyCompliance for restore origTorque map[int]int // captured lazily on applyCompliance for restore } @@ -232,7 +233,7 @@ func newControllerManualIO(c *controller.ControllerHandle, ids []int, limits [][ lm[id] = limits[i] } } - return &controllerManualIO{controller: c, ids: ids, limits: lm, origPGain: map[int]int{}, origTorque: map[int]int{}} + return &controllerManualIO{controller: c, ids: ids, limits: lm, origPGain: map[int]int{}, origIGain: map[int]int{}, origTorque: map[int]int{}} } func (a *controllerManualIO) servoIDs() []int { return a.ids } @@ -273,6 +274,7 @@ func (a *controllerManualIO) applyCompliance(ctx context.Context, pGain, torqueL // later restore always covers every servo we touch (fail-safe: on any read error, change // nothing and let manual mode run at full stiffness — the arm still holds). pg := map[int]int{} + ig := map[int]int{} tq := map[int]int{} for _, id := range a.ids { if pGain > 0 { @@ -284,6 +286,18 @@ func (a *controllerManualIO) applyCompliance(ctx context.Context, pGain, torqueL return fmt.Errorf("unexpected p_gain width %d for servo %d", len(cur), id) } pg[id] = int(cur[0]) + + // i_gain is a gain register too: the integral path is independent of P, so + // lowering P alone leaves it live. Gated on the same pGain>0 opt-out as p_gain + // itself, per the manualIO interface comment. + curI, err := a.controller.ReadServoRegister(ctx, id, "i_gain") + if err != nil { + return fmt.Errorf("read i_gain servo %d: %w", id, err) + } + if len(curI) != 1 { + return fmt.Errorf("unexpected i_gain width %d for servo %d", len(curI), id) + } + ig[id] = int(curI[0]) } if torqueLimit > 0 { cur, err := a.controller.ReadServoRegister(ctx, id, "torque_limit") @@ -297,10 +311,16 @@ func (a *controllerManualIO) applyCompliance(ctx context.Context, pGain, torqueL } } a.origPGain = pg + a.origIGain = ig a.origTorque = tq for _, id := range a.ids { if pGain > 0 { - if err := a.controller.WriteServoRegister(ctx, id, "p_gain", []byte{byte(pGain)}); err != nil { + // Zeroed, not lowered: the integral path is independent of P, so an arm held + // off-target by hand accumulates error and pushes back when released. + if err := writeGainIfChanged(ctx, a.controller, id, "i_gain", 0); err != nil { + return err + } + if err := writeGainIfChanged(ctx, a.controller, id, "p_gain", byte(pGain)); err != nil { return err } } @@ -317,7 +337,12 @@ func (a *controllerManualIO) applyCompliance(ctx context.Context, pGain, torqueL func (a *controllerManualIO) restoreCompliance(ctx context.Context) error { var firstErr error for id, v := range a.origPGain { - if err := a.controller.WriteServoRegister(ctx, id, "p_gain", []byte{byte(v)}); err != nil && firstErr == nil { + if err := writeGainIfChanged(ctx, a.controller, id, "p_gain", byte(v)); err != nil && firstErr == nil { + firstErr = err + } + } + for id, v := range a.origIGain { + if err := writeGainIfChanged(ctx, a.controller, id, "i_gain", byte(v)); err != nil && firstErr == nil { firstErr = err } } diff --git a/components/arm/manual_compliance_test.go b/components/arm/manual_compliance_test.go new file mode 100644 index 0000000..9140e0a --- /dev/null +++ b/components/arm/manual_compliance_test.go @@ -0,0 +1,84 @@ +package arm + +import ( + "context" + "testing" + + "github.com/hipsterbrown/feetech-servo/feetech" + "github.com/stretchr/testify/assert" + "github.com/stretchr/testify/require" + + "so_arm/internal/testfake" +) + +// Compliance must zero the INTEGRAL term, not just lower P. With tuned gains in EEPROM the +// integrator accumulates the standing error while a hand holds the arm off-target and then +// pushes back on release; P=8 does not touch that path. +func TestApplyComplianceZeroesIGain(t *testing.T) { + ft := testfake.NewFakeTransport() + ids := []int{1, 2, 3, 4, 5} + for _, id := range ids { + ft.SetRegister(id, feetech.RegPGain.Address, []byte{48}) + ft.SetRegister(id, feetech.RegIGain.Address, []byte{2}) + ft.SetRegister(id, feetech.RegDGain.Address, []byte{64}) + ft.SetRegister(id, feetech.RegTorqueLimit.Address, testfake.EncodeWordLE(1000)) + } + io := &controllerManualIO{controller: testArmHandle(t, ft), ids: ids} + + require.NoError(t, io.applyCompliance(context.Background(), 8, 50)) + + for _, id := range ids { + assert.Equal(t, byte(0), readReg(t, io, id, "i_gain"), "servo %d i_gain during compliance", id) + assert.Equal(t, byte(64), readReg(t, io, id, "d_gain"), "servo %d d_gain must be left alone", id) + } + + require.NoError(t, io.restoreCompliance(context.Background())) + + for _, id := range ids { + assert.Equal(t, byte(2), readReg(t, io, id, "i_gain"), "servo %d i_gain restored", id) + assert.Equal(t, byte(48), readReg(t, io, id, "p_gain"), "servo %d p_gain restored", id) + } +} + +// p_gain 0 is the documented opt-out for gain writes (manualIO's interface comment). i_gain +// is a gain register, so it must honour the same opt-out. +func TestApplyComplianceHonoursThePGainOptOut(t *testing.T) { + ft := testfake.NewFakeTransport() + for _, id := range []int{1, 2} { + ft.SetRegister(id, feetech.RegIGain.Address, []byte{4}) + ft.SetRegister(id, feetech.RegTorqueLimit.Address, testfake.EncodeWordLE(1000)) + } + io := &controllerManualIO{controller: testArmHandle(t, ft), ids: []int{1, 2}} + + require.NoError(t, io.applyCompliance(context.Background(), 0, 50)) + + for _, id := range []int{1, 2} { + assert.Equal(t, byte(4), readReg(t, io, id, "i_gain"), "servo %d i_gain must be untouched", id) + assert.Zero(t, ft.WriteCount(id, feetech.RegIGain.Address)) + } +} + +// These are EEPROM registers and manual mode is entered and left repeatedly. A servo already +// holding the target value must not be written. +func TestComplianceSkipsNoOpGainWrites(t *testing.T) { + ft := testfake.NewFakeTransport() + // Already at the compliance values: p_gain 8, i_gain 0. + ft.SetRegister(1, feetech.RegPGain.Address, []byte{8}) + ft.SetRegister(1, feetech.RegIGain.Address, []byte{0}) + ft.SetRegister(1, feetech.RegTorqueLimit.Address, testfake.EncodeWordLE(1000)) + io := &controllerManualIO{controller: testArmHandle(t, ft), ids: []int{1}} + + require.NoError(t, io.applyCompliance(context.Background(), 8, 50)) + require.NoError(t, io.restoreCompliance(context.Background())) + + assert.Zero(t, ft.WriteCount(1, feetech.RegPGain.Address), "p_gain was already 8") + assert.Zero(t, ft.WriteCount(1, feetech.RegIGain.Address), "i_gain was already 0") +} + +func readReg(t *testing.T, io *controllerManualIO, id int, name string) byte { + t.Helper() + got, err := io.controller.ReadServoRegister(context.Background(), id, name) + require.NoError(t, err) + require.Len(t, got, 1) + return got[0] +} From 4342d57db13f26190583ebf8694a273958ea7c17 Mon Sep 17 00:00:00 2001 From: HipsterBrown Date: Wed, 2 Sep 2026 13:29:14 -0400 Subject: [PATCH 03/14] feat(arm): write tuned per-joint PID gains during servo initialization Co-Authored-By: Claude Opus 5 (1M context) Claude-Session: https://claude.ai/code/session_01NHef7PBha9FHmWoCSduR8J --- components/arm/arm.go | 8 ++ components/arm/gains.go | 31 +++++++ components/arm/gains_test.go | 158 +++++++++++++++++++++++++++++++++++ internal/servo/gains.go | 23 +++++ internal/servo/gains_test.go | 27 ++++++ 5 files changed, 247 insertions(+) create mode 100644 components/arm/gains_test.go create mode 100644 internal/servo/gains.go create mode 100644 internal/servo/gains_test.go diff --git a/components/arm/arm.go b/components/arm/arm.go index 17b1be7..aa691c8 100644 --- a/components/arm/arm.go +++ b/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 { diff --git a/components/arm/gains.go b/components/arm/gains.go index 0187596..9dc5a5f 100644 --- a/components/arm/gains.go +++ b/components/arm/gains.go @@ -4,7 +4,10 @@ import ( "context" "fmt" + "go.viam.com/rdk/logging" + "so_arm/internal/controller" + "so_arm/internal/servo" ) // writeGainIfChanged writes a one-byte gain register only when it does not already hold the @@ -27,3 +30,31 @@ func writeGainIfChanged(ctx context.Context, h *controller.ControllerHandle, id } return h.WriteServoRegister(ctx, id, name, []byte{want}) } + +// applyServoGains writes the tuned P/D/I table to the arm's own servos, skipping registers +// that already hold the target value. +// +// The write ORDER is fixed at D, I, P and is not incidental: a non-zero I is only stable +// against a raised D (measured -- joint 2 oscillated at D=32/I=4 and was stable at +// D=48/I=4), so damping first means every prefix of the sequence is at least as stable as +// the state before it. A partial failure then cannot strand a gravity-loaded joint on a +// combination measured to oscillate, in EEPROM. +// +// It NEVER returns an error: the arm works at stock gains, so a bus problem here must not +// prevent it from coming up. +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 int + }{{"d_gain", g.D}, {"i_gain", g.I}, {"p_gain", g.P}} { + if err := writeGainIfChanged(ctx, h, id, w.name, byte(w.want)); err != nil { + logger.Warnf("servo %d: leaving %s as found: %v", id, w.name, err) + } + } + } +} diff --git a/components/arm/gains_test.go b/components/arm/gains_test.go new file mode 100644 index 0000000..60ff128 --- /dev/null +++ b/components/arm/gains_test.go @@ -0,0 +1,158 @@ +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")) +} + +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) + } + } +} + +// failingReadTransport wraps a FakeTransport and fails exactly the FIRST (servo, register) +// READ packet at the wire level -- everything else (PING, SYNC_READ, WRITE/SYNC_WRITE, reads +// of any other register or servo, and a later re-read of the SAME register once failsLeft is +// exhausted) passes through untouched. Failing only once, not forever, is what lets a test +// both trigger the warn-and-continue path during init AND read the register back afterward +// to assert it was left alone. This is what makes TestApplyServoGainsFailureDoesNotFailInit a +// real test of the contract rather than a vacuous one: the ping and position read below go +// over the SAME transport and must keep succeeding. +type failingReadTransport struct { + *testfake.FakeTransport + id int + address byte + failsLeft int +} + +func (f *failingReadTransport) Write(p []byte) (int, error) { + // Packet: [0xFF, 0xFF, id, len, instruction, params..., checksum]. READ is + // instruction 0x02 with params [address, length]. + if f.failsLeft > 0 && len(p) >= 7 && p[0] == 0xFF && p[1] == 0xFF && + int(p[2]) == f.id && p[4] == 0x02 && 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 READ fails (via failingReadTransport); ping (PING packets) and the +// position read (a 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 := &failingReadTransport{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 read failed, so writeGainIfChanged bailed out before writing -- + // it must be left exactly as found, not stranded at some other value. + assert.Equal(t, byte(32), readServoGain(t, s, 2, "d_gain"), "servo 2 d_gain must be left as found after a read failure") + // The failure must not cascade to the rest of servo 2's own gains... + assert.Equal(t, byte(2), readServoGain(t, s, 2, "i_gain"), "servo 2 i_gain must still land") + assert.Equal(t, byte(48), readServoGain(t, s, 2, "p_gain"), "servo 2 p_gain must still land") + // ...or to any other servo. + assert.Equal(t, byte(64), readServoGain(t, s, 3, "d_gain"), "servo 3 d_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] +} diff --git a/internal/servo/gains.go b/internal/servo/gains.go new file mode 100644 index 0000000..4d5d0cc --- /dev/null +++ b/internal/servo/gains.go @@ -0,0 +1,23 @@ +package servo + +// Gains is one servo's position-loop PID register triple. Each value is a single byte. +type Gains struct { + P int + D int + I int +} + +// DefaultArmGains is the tuned per-joint gain table for arm servos 1-5, against a stock +// 32/32/0. A tuning value, not a hardware constant -- measured on ONE arm at 12.2-12.3 V. +// See docs/arm.md, "Servo gains", for the measurements, the stability ceiling, and the +// peak-current cost. +// +// Servo 6 (gripper) is deliberately absent: never characterized, and its lower stock P=16 is +// likely a deliberate soft-grasp choice. +var DefaultArmGains = map[int]Gains{ + 1: {P: 32, D: 48, I: 0}, // shoulder pan -- gravity-free; D 32->48 nearly halves overshoot + 2: {P: 48, D: 64, I: 2}, // shoulder lift -- gravity-loaded; I halves the residual droop + 3: {P: 48, D: 64, I: 4}, // elbow -- tolerates I=4 where the shoulder does not + 4: {P: 48, D: 64, I: 2}, // wrist flex + 5: {P: 48, D: 48, I: 0}, // wrist roll -- gravity-free, needs no integral term +} diff --git a/internal/servo/gains_test.go b/internal/servo/gains_test.go new file mode 100644 index 0000000..ded549f --- /dev/null +++ b/internal/servo/gains_test.go @@ -0,0 +1,27 @@ +package servo + +import "testing" + +func TestDefaultArmGainsInvariants(t *testing.T) { + if len(DefaultArmGains) != 5 { + t.Fatalf("expected exactly the 5 arm servos, got %d entries", len(DefaultArmGains)) + } + for _, id := range []int{1, 2, 3, 4, 5} { + g, ok := DefaultArmGains[id] + if !ok { + t.Fatalf("servo %d missing from the tuned gain table", id) + } + // P=64 oscillated at every D tested (report finding 3), which is why nothing here + // may exceed 48. + if g.P > 48 { + t.Fatalf("servo %d: P=%d is above the measured stability ceiling of 48", id, g.P) + } + // Finding 4: raising D is what makes a non-zero I usable. + if g.I > 0 && g.D < 48 { + t.Fatalf("servo %d: I=%d needs D>=48 to be stable, got D=%d", id, g.I, g.D) + } + } + if _, ok := DefaultArmGains[6]; ok { + t.Fatal("servo 6 (gripper) is untested and must not carry a tuned default") + } +} From 3a1795a9cfc6a30dd432d959a57660b16768d4b2 Mon Sep 17 00:00:00 2001 From: HipsterBrown Date: Wed, 2 Sep 2026 13:31:18 -0400 Subject: [PATCH 04/14] fix(arm): write the gain register when the read cannot prove a no-op writeGainIfChanged returned early on a read error, so a single transient read on manual-mode exit left that joint at p_gain 8 / i_gain 0 in EEPROM -- soft, across power cycles -- with only a log line. restoreCompliance records the error and continues, and its only consumer logs a warning. The asymmetry is the point: a failed apply leaves the arm stiff, which is safe; a failed restore leaves it drooping, which is not. A read that fails cannot prove the write is unnecessary, so it now falls through to the write. The wear guard is unaffected in practice -- read failures are rare. Retargets the Task 4 failure test from a read failure to a write failure, which is now the only way to exercise warn-and-continue. Co-Authored-By: Claude Opus 5 (1M context) Claude-Session: https://claude.ai/code/session_01NHef7PBha9FHmWoCSduR8J --- components/arm/gains.go | 20 ++++++++--------- components/arm/gains_test.go | 43 ++++++++++++++++++------------------ 2 files changed, 31 insertions(+), 32 deletions(-) diff --git a/components/arm/gains.go b/components/arm/gains.go index 9dc5a5f..d73f9fc 100644 --- a/components/arm/gains.go +++ b/components/arm/gains.go @@ -2,7 +2,6 @@ package arm import ( "context" - "fmt" "go.viam.com/rdk/logging" @@ -10,22 +9,21 @@ import ( "so_arm/internal/servo" ) -// writeGainIfChanged writes a one-byte gain register only when it does not already hold the -// target value. +// writeGainIfChanged writes a one-byte gain register unless it can prove the write would +// change nothing. // // p_gain, d_gain and i_gain are all EEPROM registers (addresses 21-23) with finite write // endurance, and every write costs three bus transactions for the unlock/write/relock dance. // Both callers rewrite the same values repeatedly -- manual mode on every entry and exit, // arm init on every Reconfigure -- so the skip is the wear guard, not an optimization. +// +// A read that FAILS therefore falls through to the write rather than returning: it cannot +// prove the write is unnecessary, and the wear guard is not worth the alternative. On +// restoreCompliance's path, skipping the write leaves a joint at p_gain 8 / i_gain 0 in +// EEPROM -- soft, across power cycles -- and a bare register read failing outright is a +// transient this hardware really produces, with nothing retrying it. func writeGainIfChanged(ctx context.Context, h *controller.ControllerHandle, id int, name string, want byte) error { - cur, err := h.ReadServoRegister(ctx, id, name) - if err != nil { - return fmt.Errorf("read %s servo %d: %w", name, id, err) - } - if len(cur) != 1 { - return fmt.Errorf("unexpected %s width %d for servo %d", name, len(cur), id) - } - if cur[0] == want { + 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}) diff --git a/components/arm/gains_test.go b/components/arm/gains_test.go index 60ff128..b90ae90 100644 --- a/components/arm/gains_test.go +++ b/components/arm/gains_test.go @@ -96,26 +96,29 @@ func TestApplyServoGainsSkipsNoOpWrites(t *testing.T) { } } -// failingReadTransport wraps a FakeTransport and fails exactly the FIRST (servo, register) -// READ packet at the wire level -- everything else (PING, SYNC_READ, WRITE/SYNC_WRITE, reads -// of any other register or servo, and a later re-read of the SAME register once failsLeft is -// exhausted) passes through untouched. Failing only once, not forever, is what lets a test -// both trigger the warn-and-continue path during init AND read the register back afterward -// to assert it was left alone. This is what makes TestApplyServoGainsFailureDoesNotFailInit a -// real test of the contract rather than a vacuous one: the ping and position read below go -// over the SAME transport and must keep succeeding. -type failingReadTransport struct { +// 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 *failingReadTransport) Write(p []byte) (int, error) { - // Packet: [0xFF, 0xFF, id, len, instruction, params..., checksum]. READ is - // instruction 0x02 with params [address, length]. +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] == 0x02 && p[5] == f.address { + int(p[2]) == f.id && p[4] == 0x03 && p[5] == f.address { f.failsLeft-- return 0, syscall.EIO } @@ -123,21 +126,19 @@ func (f *failingReadTransport) Write(p []byte) (int, error) { } // A gain write failing must not fail initialization: the arm works at stock gains. Only -// servo 2's d_gain READ fails (via failingReadTransport); ping (PING packets) and the -// position read (a 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. +// 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 := &failingReadTransport{FakeTransport: ft, id: 2, address: feetech.RegDGain.Address, failsLeft: 1} + 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 read failed, so writeGainIfChanged bailed out before writing -- - // it must be left exactly as found, not stranded at some other value. - assert.Equal(t, byte(32), readServoGain(t, s, 2, "d_gain"), "servo 2 d_gain must be left as found after a read failure") + // 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") // The failure must not cascade to the rest of servo 2's own gains... assert.Equal(t, byte(2), readServoGain(t, s, 2, "i_gain"), "servo 2 i_gain must still land") assert.Equal(t, byte(48), readServoGain(t, s, 2, "p_gain"), "servo 2 p_gain must still land") From a33bd49a84406497dcbcab17b411d2552fb0a70b Mon Sep 17 00:00:00 2001 From: HipsterBrown Date: Wed, 2 Sep 2026 13:36:01 -0400 Subject: [PATCH 05/14] feat(teleop): stream follower setpoints at unlimited acceleration Co-Authored-By: Claude Opus 5 (1M context) Claude-Session: https://claude.ai/code/session_01NHef7PBha9FHmWoCSduR8J --- components/arm/motion.go | 28 +++++++++--- components/arm/unlimited_accel_test.go | 62 ++++++++++++++++++++++++++ internal/controller/manager.go | 2 +- services/teleop/teleop.go | 5 ++- services/teleop/teleop_test.go | 17 ++++--- 5 files changed, 99 insertions(+), 15 deletions(-) create mode 100644 components/arm/unlimited_accel_test.go diff --git a/components/arm/motion.go b/components/arm/motion.go index a0e6206..a418a17 100644 --- a/components/arm/motion.go +++ b/components/arm/motion.go @@ -343,18 +343,26 @@ func (s *so101) dwellUntilNear(ctx context.Context, goal []float64, lookaheadDeg } } -// moveJointsUniform is the pre-coordination path, retained only as the fallback for when -// the travel-reference read fails. It commands every joint at one shared speed, which is -// why joints arrive at different times. -func (s *so101) moveJointsUniform(ctx context.Context, to []float64, speedDegsPerSec, accelDegsPerSecSq float64, wait bool) error { +// moveJointsUniform is the pre-coordination path, used as the fallback for when the +// travel-reference read fails and as the teleop unlimited-acceleration path. It commands +// every joint at one shared speed, which is why joints arrive at different times. +// +// unlimitedAccel forces Acc 0 (UNLIMITED) instead of the deg/s^2 conversion -- the teleop +// bypass in MoveToJointPositions is the only caller that sets it true. See CLAUDE.md, +// "Acc: 0 means UNLIMITED". +func (s *so101) moveJointsUniform(ctx context.Context, to []float64, speedDegsPerSec, accelDegsPerSecSq float64, unlimitedAccel, wait bool) error { // Writes an explicit UNIFORM profile rather than a bare position write. A coordinated // move leaves each servo's Speed and Acc registers at its own scaled value -- as low as // 1 step/s and Acc 1 -- and a position-only write does not touch either. So without // this the "uniform" fallback would inherit whatever the previous coordinated move left // behind, and would be uniform in neither speed nor acceleration. + accUnits := servo.DegPerSecSqToAccUnits(accelDegsPerSecSq) + if unlimitedAccel { + accUnits = 0 // UNLIMITED on this register -- see the teleop branch in MoveToJointPositions + } profile := controller.ServoProfile{ SpeedSteps: servo.DegPerSecToStepsPerSec(speedDegsPerSec), - AccUnits: servo.DegPerSecSqToAccUnits(accelDegsPerSecSq), + AccUnits: accUnits, } profiles := make([]controller.ServoProfile, len(s.armServoIDs)) for i := range profiles { @@ -393,6 +401,14 @@ func (s *so101) MoveToJointPositions(ctx context.Context, positions []referencef accel := float64(s.defaultAcc) s.mu.RUnlock() + // Streamed setpoints (teleop) skip both the ramp and the coordination read. An + // acceleration limit is catastrophic for tracking -- measured, acc=32 quadrupled RMS + // error and took lag from 40 ms to 160 ms -- and coordinated arrival is meaningless for + // a goal that will be superseded in ~10 ms. See docs/arm.md, "Motion and speed". + if unlimited, _ := extra["unlimited_accel"].(bool); unlimited { + return s.moveJointsUniform(ctx, clamped, speed, accel, true, servocmd.WaitArg(extra)) + } + // The position read is now unconditional, where it used to happen only when waiting. // Coordination needs a travel reference, and on v0.6.1 this read costs about 1ms // against roughly 12ms on v0.6.0 -- which is why the version bump is a correctness @@ -408,7 +424,7 @@ func (s *so101) MoveToJointPositions(ctx context.Context, positions []referencef "uniform speed: %v", err) // No arm.MoveOptions here, so caps is always nil -- kept for symmetry with // MoveThroughJointPositions' fallback. - return s.moveJointsUniform(ctx, clamped, servo.UniformSpeedUnderCaps(speed, nil), accel, servocmd.WaitArg(extra)) + return s.moveJointsUniform(ctx, clamped, servo.UniformSpeedUnderCaps(speed, nil), accel, false, servocmd.WaitArg(extra)) } // Single move: there is no next waypoint, so the dwell timeout is not wanted. diff --git a/components/arm/unlimited_accel_test.go b/components/arm/unlimited_accel_test.go new file mode 100644 index 0000000..a7b0d90 --- /dev/null +++ b/components/arm/unlimited_accel_test.go @@ -0,0 +1,62 @@ +package arm + +import ( + "context" + "testing" + + "github.com/stretchr/testify/assert" + "github.com/stretchr/testify/require" + "go.viam.com/rdk/logging" + "go.viam.com/rdk/operation" + "go.viam.com/rdk/referenceframe" + + "so_arm/internal/servo" + "so_arm/internal/testfake" +) + +func accelTestArm(t *testing.T, ft *testfake.FakeTransport) *so101 { + t.Helper() + return &so101{ + logger: logging.NewTestLogger(t), + opMgr: operation.NewSingleOperationManager(), + controller: testArmHandle(t, ft), + armServoIDs: []int{1, 2, 3, 4, 5}, + defaultSpeed: 50, + defaultAcc: servo.DefaultAccelDegsPerSecSq, + lookaheadDeg: servo.DefaultLookaheadDeg, + } +} + +// Teleop streams small setpoints; an acceleration ramp on each one quadruples tracking error +// (measured: acc=32 -> 160 ms lag vs 40 ms at acc=0). Acc 0 is the servo's "unlimited" +// sentinel, which the deg/s^2 conversion path deliberately cannot produce. +func TestMoveToJointPositionsHonoursUnlimitedAccel(t *testing.T) { + ft := testfake.NewFakeTransport() + s := accelTestArm(t, ft) + goal := []referenceframe.Input{0.3, 0.3, 0.3, 0.3, 0.3} + + require.NoError(t, s.MoveToJointPositions(context.Background(), goal, + map[string]interface{}{"wait": false, "unlimited_accel": true})) + + for _, id := range s.armServoIDs { + got, err := s.controller.ReadServoRegister(context.Background(), id, "acceleration") + require.NoError(t, err) + assert.Equal(t, byte(0), got[0], "servo %d must be commanded at unlimited acceleration", id) + } +} + +// The point-to-point default keeps its ramp: 500 deg/s^2 maps to Acc 49. +func TestMoveToJointPositionsRampsByDefault(t *testing.T) { + ft := testfake.NewFakeTransport() + s := accelTestArm(t, ft) + goal := []referenceframe.Input{0.3, 0.3, 0.3, 0.3, 0.3} + + require.NoError(t, s.MoveToJointPositions(context.Background(), goal, + map[string]interface{}{"wait": false})) + + for _, id := range s.armServoIDs { + got, err := s.controller.ReadServoRegister(context.Background(), id, "acceleration") + require.NoError(t, err) + assert.NotZero(t, got[0], "servo %d must keep its acceleration ramp", id) + } +} diff --git a/internal/controller/manager.go b/internal/controller/manager.go index 30b8c8d..0b9ed51 100644 --- a/internal/controller/manager.go +++ b/internal/controller/manager.go @@ -112,7 +112,7 @@ func (h *ControllerHandle) MoveServosToPositions(ctx context.Context, servoIDs [ // ServoProfile is one servo's speed and acceleration for a coordinated move. type ServoProfile struct { SpeedSteps int // goal velocity, steps/sec. Never 0: that means MAX SPEED. - AccUnits int // acceleration register. Never 0: that means UNLIMITED. + AccUnits int // acceleration register. 0 means UNLIMITED -- only the teleop bypass sends it. } // MoveServosWithProfiles commands each servo to its angle under its OWN speed and diff --git a/services/teleop/teleop.go b/services/teleop/teleop.go index 2144294..a090b10 100644 --- a/services/teleop/teleop.go +++ b/services/teleop/teleop.go @@ -180,7 +180,10 @@ func (tp *so101Teleop) syncOnce(ctx context.Context) error { gripperPct = pct } - if err := tp.followerArm.MoveToJointPositions(ctx, positions, map[string]interface{}{"wait": false}); err != nil { + // acc=0 (unlimited) for streamed setpoints -- an acceleration limit is catastrophic for + // tracking. The command stream itself shapes the trajectory here. + if err := tp.followerArm.MoveToJointPositions(ctx, positions, + map[string]interface{}{"wait": false, "unlimited_accel": true}); err != nil { return fmt.Errorf("write follower joints: %w", err) } diff --git a/services/teleop/teleop_test.go b/services/teleop/teleop_test.go index f8206f3..a622117 100644 --- a/services/teleop/teleop_test.go +++ b/services/teleop/teleop_test.go @@ -20,13 +20,14 @@ type fakeArm struct { arm.Arm name resource.Name - mu sync.Mutex - jp []referenceframe.Input - jpErr error - moved []referenceframe.Input - moveWait interface{} - moveErr error - torqueLog []bool + mu sync.Mutex + jp []referenceframe.Input + jpErr error + moved []referenceframe.Input + moveWait interface{} + moveUnlimited interface{} + moveErr error + torqueLog []bool } func (f *fakeArm) Name() resource.Name { return f.name } @@ -48,6 +49,7 @@ func (f *fakeArm) MoveToJointPositions(ctx context.Context, positions []referenc } f.moved = positions f.moveWait = extra["wait"] + f.moveUnlimited = extra["unlimited_accel"] return nil } @@ -125,6 +127,7 @@ func TestTeleopSyncOnce(t *testing.T) { test.That(t, err, test.ShouldBeNil) test.That(t, fa.moved, test.ShouldResemble, la.jp) test.That(t, fa.moveWait, test.ShouldEqual, false) + test.That(t, fa.moveUnlimited, test.ShouldEqual, true) test.That(t, fg.setPct, test.ShouldResemble, []float64{42}) }) From e26fa9c821c5dcb00bc340138a00d375d5377e0c Mon Sep 17 00:00:00 2001 From: HipsterBrown Date: Wed, 2 Sep 2026 13:37:39 -0400 Subject: [PATCH 06/14] feat(teleop): raise the default mirror rate to 100 Hz Command rate dominates teleop smoothness far more than gain tuning: 30->100 Hz cut chatter ~3.5x and tracking error ~28%, and at 30 Hz lag was pinned at 67 ms for every gain set, floored by the command period. The shipped 20 Hz default was below the lowest rate measured. Lands after the acceleration bypass on purpose -- those figures were measured at acc=0, and the shipped Acc 49 is the ~90 ms-lag row. Co-Authored-By: Claude Opus 5 (1M context) Claude-Session: https://claude.ai/code/session_01NHef7PBha9FHmWoCSduR8J --- docs/teleop.md | 10 ++++++++-- services/teleop/teleop.go | 7 ++++++- 2 files changed, 14 insertions(+), 3 deletions(-) diff --git a/docs/teleop.md b/docs/teleop.md index fb7479e..ae57211 100644 --- a/docs/teleop.md +++ b/docs/teleop.md @@ -25,7 +25,7 @@ Leader and follower must be separate SO-101 arm components, typically on separat | `follower_arm` | string | **Required** | Name of the arm component to drive. | | `leader_gripper` | string | Optional | Name of the leader gripper component. When both `leader_gripper` and `follower_gripper` are set, gripper opening is mirrored in addition to joint positions. | | `follower_gripper` | string | Optional | Name of the follower gripper component. | -| `rate_hz` | number | Optional | Mirror loop frequency in Hz. Default `20`. Must be > 0 and ≤ 1000. | +| `rate_hz` | number | Optional | Mirror loop frequency in Hz. Default `100`. Must be > 0 and ≤ 1000. | | `manage_leader_torque` | bool | Optional | When `true`, disables the leader arm's servo torque on start and restores it on stop, so the leader can be moved by hand. Default `true`. | | `auto_start` | bool | Optional | When `true`, starts the mirror loop immediately when the service is configured. Default `false`. | | `max_consecutive_errors` | int | Optional | Number of consecutive read/write failures after which the loop stops automatically and reports the error via `status`. Default `10`. | @@ -76,10 +76,16 @@ Response: "cycles": 142, "consecutive_errors": 0, "last_error": "", - "rate_hz": 20 + "rate_hz": 100 } ``` +Raising the rate is the single most effective smoothness lever — measured 30 → 100 Hz cut +chatter ~3.5× and tracking error ~28%, more than any servo gain change, and those figures +were measured at unlimited acceleration, which is how teleop now drives the follower. Lower +it only if the serial bus or host cannot keep up; the mirror loop's ticker drops ticks rather +than backing up, so a rate the bus cannot sustain degrades silently to the rate it can. + `cycles` is the total number of successful mirror cycles since the last start. `consecutive_errors` resets to zero on any successful cycle. When `max_consecutive_errors` is reached the loop sets `running` to `false` and records the triggering error in `last_error`. ## Notes diff --git a/services/teleop/teleop.go b/services/teleop/teleop.go index a090b10..0da1072 100644 --- a/services/teleop/teleop.go +++ b/services/teleop/teleop.go @@ -18,7 +18,12 @@ import ( var SO101TeleopModel = resource.NewModel("devrel", "so101", "teleop") const ( - defaultTeleopRateHz = 20.0 + // 100 Hz, from the 2026-09-02 servo characterization: command rate dominates teleop + // smoothness far more than gain tuning (30->100 Hz cut chatter ~3.5x and pinned lag at + // 40 ms). Measured at unlimited acceleration, which is what the follower now gets. + // The loop's ticker drops ticks, so a bus that cannot sustain this degrades to the rate + // it can rather than backing up. + defaultTeleopRateHz = 100.0 defaultMaxConsecutiveErrors = 10 maxTeleopRateHz = 1000.0 ) From 251f702ed3e984d2f0b65642c0131bd1e0cce13a Mon Sep 17 00:00:00 2001 From: HipsterBrown Date: Wed, 2 Sep 2026 13:41:07 -0400 Subject: [PATCH 07/14] fix(arm): make a failed gain write terminal for that servo The D->I->P write order was justified as a safety property -- every prefix at least as stable as the state before it -- but the loop logged and continued, so the reachable states were all eight subsets rather than the four prefixes. {I,P} without D leaves servo 3 at 48/32/4, the exact combination the ordering exists to prevent: measured to oscillate, in EEPROM, on a gravity-loaded joint, with no retry to clear it (applyServoGains returns nothing, so initializeServosWithRetry never re-runs for a gain failure). 01d57ea widened the window by attempting all three writes even when the reads fail, so the break is now the only thing bounding the failure set. Per servo, not per call. The order had no test: reversing the slice left the whole suite green. Adds a wire-level recorder, since final register values are order-independent. Gains fields become byte so an out-of-range retune is a compile error rather than silent truncation to less damping, and the invariant test pins the report's caveat that servo 2 oscillated at I=4 in 2 of 3 repeat passes. Co-Authored-By: Claude Opus 5 (1M context) Claude-Session: https://claude.ai/code/session_01NHef7PBha9FHmWoCSduR8J --- components/arm/gains.go | 18 +++++++++++--- components/arm/gains_test.go | 48 +++++++++++++++++++++++++++++++++--- internal/servo/gains.go | 11 ++++++--- internal/servo/gains_test.go | 6 +++++ 4 files changed, 71 insertions(+), 12 deletions(-) diff --git a/components/arm/gains.go b/components/arm/gains.go index d73f9fc..febee53 100644 --- a/components/arm/gains.go +++ b/components/arm/gains.go @@ -30,7 +30,7 @@ func writeGainIfChanged(ctx context.Context, h *controller.ControllerHandle, id } // applyServoGains writes the tuned P/D/I table to the arm's own servos, skipping registers -// that already hold the target value. +// it can prove already hold the target value. // // The write ORDER is fixed at D, I, P and is not incidental: a non-zero I is only stable // against a raised D (measured -- joint 2 oscillated at D=32/I=4 and was stable at @@ -38,6 +38,14 @@ func writeGainIfChanged(ctx context.Context, h *controller.ControllerHandle, id // the state before it. A partial failure then cannot strand a gravity-loaded joint on a // combination measured to oscillate, in EEPROM. // +// The prefix argument only holds if a failure is TERMINAL for that servo, hence the break: +// without it the reachable states are all eight subsets rather than the four prefixes, and +// {I,P}-without-D is 48/32/4 on servo 3 -- precisely the state this ordering exists to +// prevent. It is load-bearing for a second reason since writeGainIfChanged began writing +// through an unreadable register: all three writes are now attempted even when the reads +// fail, so the break is the only thing bounding the failure set. Per servo, not per call -- +// one servo's bus trouble must not skip the rest. +// // It NEVER returns an error: the arm works at stock gains, so a bus problem here must not // prevent it from coming up. func applyServoGains(ctx context.Context, h *controller.ControllerHandle, ids []int, gains map[int]servo.Gains, logger logging.Logger) { @@ -48,10 +56,12 @@ func applyServoGains(ctx context.Context, h *controller.ControllerHandle, ids [] } for _, w := range []struct { name string - want int + want byte }{{"d_gain", g.D}, {"i_gain", g.I}, {"p_gain", g.P}} { - if err := writeGainIfChanged(ctx, h, id, w.name, byte(w.want)); err != nil { - logger.Warnf("servo %d: leaving %s as found: %v", id, w.name, err) + 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 } } } diff --git a/components/arm/gains_test.go b/components/arm/gains_test.go index b90ae90..741ad9a 100644 --- a/components/arm/gains_test.go +++ b/components/arm/gains_test.go @@ -44,6 +44,43 @@ func TestServoInitializationWritesTheTunedGains(t *testing.T) { 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) +} + +// Damping is written before the integral term, and the integral term before proportional. 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. Reading final register values cannot +// catch this -- they are order-independent -- so it 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) { @@ -139,11 +176,14 @@ func TestApplyServoGainsFailureDoesNotFailInit(t *testing.T) { // 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") - // The failure must not cascade to the rest of servo 2's own gains... - assert.Equal(t, byte(2), readServoGain(t, s, 2, "i_gain"), "servo 2 i_gain must still land") - assert.Equal(t, byte(48), readServoGain(t, s, 2, "p_gain"), "servo 2 p_gain must still land") - // ...or to any other servo. + // The failure IS terminal for the rest of servo 2's gains, on purpose: 48/32/4 -- I and + // P applied over a stock D -- is the combination the write order exists to prevent, and + // it would persist in EEPROM with no retry to clear it. + 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 { diff --git a/internal/servo/gains.go b/internal/servo/gains.go index 4d5d0cc..633cda8 100644 --- a/internal/servo/gains.go +++ b/internal/servo/gains.go @@ -1,10 +1,13 @@ package servo -// Gains is one servo's position-loop PID register triple. Each value is a single byte. +// Gains is one servo's position-loop PID register triple. byte, not int, so a retune out of +// register range is a compile error rather than a silent truncation on the wire -- a D of +// 300 would reach the servo as 44, i.e. LESS damping than intended, which is the direction +// that breaks applyServoGains' write-order argument. type Gains struct { - P int - D int - I int + P byte + D byte + I byte } // DefaultArmGains is the tuned per-joint gain table for arm servos 1-5, against a stock diff --git a/internal/servo/gains_test.go b/internal/servo/gains_test.go index ded549f..7b90597 100644 --- a/internal/servo/gains_test.go +++ b/internal/servo/gains_test.go @@ -21,6 +21,12 @@ func TestDefaultArmGainsInvariants(t *testing.T) { t.Fatalf("servo %d: I=%d needs D>=48 to be stable, got D=%d", id, g.I, g.D) } } + // The report's caveat, not a general rule: joint 2 looked good at I=4 in the first grid + // but oscillated in 2 of 3 repeat passes, even at D=64. It is the one retune the source + // evidence explicitly warns against, and every other invariant here would allow it. + if DefaultArmGains[2].I > 2 { + t.Fatalf("servo 2: I=%d exceeds the 2 that was clean in all three repeat passes", DefaultArmGains[2].I) + } if _, ok := DefaultArmGains[6]; ok { t.Fatal("servo 6 (gripper) is untested and must not carry a tuned default") } From c9b9f092b268db71faedbe0f2845852a66ca100c Mon Sep 17 00:00:00 2001 From: HipsterBrown Date: Wed, 2 Sep 2026 13:47:58 -0400 Subject: [PATCH 08/14] test(arm): stop the unlimited-accel test passing vacuously An unset register reads back as zero, so asserting Acc == 0 could not tell "commanded unlimited" from "never wrote acceleration at all" -- replacing the branch body with `return nil` left the test green. Seeds a non-zero acceleration first. Nothing pinned the skipped coordination read either, which is half the justification for the branch: reinstating the read left the whole suite green. Adds FakeTransport.PacketCount and asserts one packet on the unlimited path against two on the coordinated one, which also confirms the bus-traffic claim rather than just asserting it in a comment. The count is snapshotted before the assertion loop, whose own reads are packets too. Also drops the stale claim that teleop depends on the read-failure fallback -- it no longer reaches that read. Co-Authored-By: Claude Opus 5 (1M context) Claude-Session: https://claude.ai/code/session_01NHef7PBha9FHmWoCSduR8J --- components/arm/motion.go | 5 +++-- components/arm/unlimited_accel_test.go | 19 +++++++++++++++++++ internal/testfake/fake_transport.go | 9 +++++++++ 3 files changed, 31 insertions(+), 2 deletions(-) diff --git a/components/arm/motion.go b/components/arm/motion.go index a418a17..48438d1 100644 --- a/components/arm/motion.go +++ b/components/arm/motion.go @@ -416,8 +416,9 @@ func (s *so101) MoveToJointPositions(ctx context.Context, positions []referencef // // A read failure degrades to the old uniform-speed path rather than failing the move. // Before this change the read happened only when waiting and a failure was a warning; - // making it fatal would mean a transient bus error aborts a teleop setpoint that used - // to go through, at 30-50 Hz on the wait:false path. + // making it fatal would mean a transient bus error aborts a streamed wait:false setpoint + // that used to go through. (Teleop no longer reaches this read at all -- it takes the + // unlimited-acceleration branch above -- but other wait:false callers still do.) current, err := s.currentJoints(ctx) if err != nil { s.logger.Warnf("failed to read positions for coordinated move, falling back to "+ diff --git a/components/arm/unlimited_accel_test.go b/components/arm/unlimited_accel_test.go index a7b0d90..581908d 100644 --- a/components/arm/unlimited_accel_test.go +++ b/components/arm/unlimited_accel_test.go @@ -4,6 +4,8 @@ import ( "context" "testing" + "github.com/hipsterbrown/feetech-servo/feetech" + "github.com/stretchr/testify/assert" "github.com/stretchr/testify/require" "go.viam.com/rdk/logging" @@ -33,16 +35,28 @@ func accelTestArm(t *testing.T, ft *testfake.FakeTransport) *so101 { func TestMoveToJointPositionsHonoursUnlimitedAccel(t *testing.T) { ft := testfake.NewFakeTransport() s := accelTestArm(t, ft) + // Seed a NON-zero acceleration first. An unset register reads back as zero, so without + // this the assertion below cannot tell "commanded Acc 0" from "never wrote Acc at all". + for _, id := range s.armServoIDs { + ft.SetRegister(id, feetech.RegAcceleration.Address, []byte{0x7F}) + } goal := []referenceframe.Input{0.3, 0.3, 0.3, 0.3, 0.3} + before := ft.PacketCount() require.NoError(t, s.MoveToJointPositions(context.Background(), goal, map[string]interface{}{"wait": false, "unlimited_accel": true})) + // Snapshot before the assertion loop below -- each of its reads is a packet too. + moved := ft.PacketCount() - before for _, id := range s.armServoIDs { got, err := s.controller.ReadServoRegister(context.Background(), id, "acceleration") require.NoError(t, err) assert.Equal(t, byte(0), got[0], "servo %d must be commanded at unlimited acceleration", id) } + // One SyncWrite and nothing else. Skipping the coordination read is half the point of + // the branch -- at 100 Hz the read plus the write is ~3 ms of a 10 ms budget -- and it + // leaves no trace in the register store, so only the packet count can pin it. + assert.Equal(t, 1, moved, "the unlimited path must issue one packet: the goal write") } // The point-to-point default keeps its ramp: 500 deg/s^2 maps to Acc 49. @@ -51,12 +65,17 @@ func TestMoveToJointPositionsRampsByDefault(t *testing.T) { s := accelTestArm(t, ft) goal := []referenceframe.Input{0.3, 0.3, 0.3, 0.3, 0.3} + before := ft.PacketCount() require.NoError(t, s.MoveToJointPositions(context.Background(), goal, map[string]interface{}{"wait": false})) + moved := ft.PacketCount() - before for _, id := range s.armServoIDs { got, err := s.controller.ReadServoRegister(context.Background(), id, "acceleration") require.NoError(t, err) assert.NotZero(t, got[0], "servo %d must keep its acceleration ramp", id) } + // SyncRead for the travel reference, then SyncWrite. This is the traffic the unlimited + // path exists to avoid. + assert.Equal(t, 2, moved, "the coordinated path reads before it writes") } diff --git a/internal/testfake/fake_transport.go b/internal/testfake/fake_transport.go index 1074a1e..3a53e09 100644 --- a/internal/testfake/fake_transport.go +++ b/internal/testfake/fake_transport.go @@ -203,6 +203,15 @@ func (ft *FakeTransport) CloseCount() int { return ft.closes } +// PacketCount reports how many request packets have been written to the transport, so a +// test can assert a code path's bus traffic rather than only its effect -- a skipped read +// leaves no trace in the register store. +func (ft *FakeTransport) PacketCount() int { + ft.mu.Lock() + defer ft.mu.Unlock() + return ft.writes +} + // WriteCount reports how many WRITE packets have landed on one register, so a test can // assert a read-first guard actually skipped a redundant write -- reading the value back // cannot distinguish "already correct" from "written again". From 97eb4e127c3f18344a46a02bcb68c8bbcc0f900a Mon Sep 17 00:00:00 2001 From: HipsterBrown Date: Wed, 2 Sep 2026 13:53:13 -0400 Subject: [PATCH 09/14] docs: record the servo-gain defaults and the teleop acceleration bypass Co-Authored-By: Claude Opus 5 (1M context) Claude-Session: https://claude.ai/code/session_01NHef7PBha9FHmWoCSduR8J --- CLAUDE.md | 33 +++++++++++++++++++++++++++++++++ docs/arm.md | 39 +++++++++++++++++++++++++++++++++++++++ 2 files changed, 72 insertions(+) diff --git a/CLAUDE.md b/CLAUDE.md index 601132b..14a76bc 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -526,3 +526,36 @@ 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. +- **`Acc: 0` is right for teleop and wrong for everything else, and the bypass skips the + coordination READ too, not just the ramp.** `MinAccUnits = 1` still forbids Acc 0 on the + profiled path — Acc 1 delivers ~43 deg/s², measurably worse than the measured acc=32 row, so a + low *configured* acceleration is not a substitute. The teleop bypass in `MoveToJointPositions` + (`components/arm/motion.go`) branches to `moveJointsUniform` BEFORE the `currentJoints()` read + that a ramped move uses to give `CoordinatedProfiles` a travel reference — coordinated arrival + is meaningless for a goal superseded in ~10ms, so the read buys nothing there either. Measured + in tests: one bus packet per call (the goal write) instead of two (a position read, then the + write). Do not unify this path with the profiled one. diff --git a/docs/arm.md b/docs/arm.md index bbe7c60..c9f7624 100644 --- a/docs/arm.md +++ b/docs/arm.md @@ -34,6 +34,7 @@ The following attributes are available for the arm component: | `use_urdf` | bool | Optional | When `true`, sources kinematics and collision geometry from the bundled `assets/urdf/so101.urdf` instead of the embedded `so101.json`, and requires `VIAM_MODULE_ROOT` (viam-server sets this). A drop-in swap — frame names, TCP, and kinematics are identical; only the collision geometry upgrades from primitive shapes to per-link meshes. Default `false`. | | `mesh_decimation_ratios` | []float | Optional | Per-mesh simplification ratios, one per arm link in document order (base, shoulder, upper_arm, lower_arm, wrist). Only values strictly inside `(0, 1)` decimate; lower is more aggressive. Used only with `use_urdf`. Defaults to `0.9` for all five. | | `manual_mode` | object | Optional | Tuning for hand-guided [manual mode](#manual-mode) (see below). Omit for the built-in defaults. | +| `disable_servo_gains` | bool | Optional | When `true`, leaves the servos' P/D/I registers untouched at init instead of writing the tuned defaults. Set it if you tuned gains externally (e.g. with `pidtune apply`). Default `false`. See [Servo gains](#servo-gains). | | `orientation_tolerance_deg` | float | Optional | Half-angle, in degrees, of the cone of acceptable end-effector approach directions around the goal orientation, used by `MoveToPosition`. **Roll about that axis is NOT constrained.** Range `[0, 180]`; `0`/unset means `30`. See [Approach-axis orientation planning](#approach-axis-orientation-planning). | | `position_tolerance_mm` | float | Optional | Per-axis positional leeway, in millimetres, of the `MoveToPosition` goal cloud. Default `1.0`. See [Approach-axis orientation planning](#approach-axis-orientation-planning). | **If you're building and setting up an arm for the first time, please see the [calibration sensor component](calibration.md#model-devrelso101calibration) for setup instructions.** @@ -373,6 +374,19 @@ A per-joint `MaxVelRadsJoints` cap still wins over the floor: a cap slower than `MoveThroughJointPositions` accepts `MoveOptions.MaxVelRads` and `MaxAccRads` to override the configured defaults for one move, and the per-joint forms `MaxVelRadsJoints` and `MaxAccRadsJoints`. Setting a per-joint slice makes the matching scalar ignored, per `arm.proto`; a slice whose length doesn't match the arm's joint count is rejected. A per-joint limit slows the whole move rather than that joint, so the joints stay coordinated. `GoToInputs` routes through this same path. +### Streamed setpoints: `wait` and `unlimited_accel` + +`MoveToJointPositions` accepts two optional booleans in its `extra` map: + +```json +{ "wait": false, "unlimited_accel": true } +``` + +- **`wait`** (default `true`) — block until the move settles, or return as soon as the goal is written. +- **`unlimited_accel`** (default `false`) — command the servos' acceleration register to `0`, the servo's "unlimited" sentinel, instead of the configured `acceleration_degs_per_sec_per_sec` ramp, and skip the coordination read that a ramped move uses to give the other joints a travel reference for arriving together. + +Both skips are for a caller **streaming setpoints** rather than making a one-off point-to-point move — the [teleop service](teleop.md) sets it on every follower command. An acceleration ramp is close to a free win for a single move, but it is measurably catastrophic for continuously-updated tracking: `acc=32` quadrupled RMS tracking error and took lag from 40ms to 160ms versus `acc=0`, because the servo re-ramps on every new goal instead of ever reaching speed. The coordination read is skipped for the same reason it's pointless here — coordinated arrival is meaningless for a goal about to be superseded in ~10ms — and skipping it is measured to halve the bus traffic per call, one packet (the goal write) instead of two (a position read, then the write). A plain point-to-point move should keep both the ramp and the coordinated read; do not set `unlimited_accel` there. + ### Waypoint streams `MoveThroughJointPositions` (and `GoToInputs`, which routes through it) paces its waypoints against the arm's real position: a waypoint is commanded, then held until every joint is within `waypoint_lookahead_deg` of it, and only then is the next one written. The arm waits for a full stop only after the **final** waypoint, so intermediate waypoints are still not stop-points — the arm flows through them. @@ -447,6 +461,31 @@ Both have floors you cannot configure away. The planner adds a `0.001` epsilon t Passing both `goal_metric_type` and `pose_cloud` in the same `extra` map is an error. +## Servo gains + +`disable_servo_gains` (bool, default `false`) leaves the servos' `p_gain`/`d_gain`/`i_gain` registers untouched at init. Leave it `false` to get the tuned table below; set it if you have tuned gains of your own (`pidtune apply` writes the same registers this module does). + +The tuned per-joint table, against a stock `32/32/0` (gripper `16/32/0`): + +| servo 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 | *untested — never written* | | | + +These are EEPROM writes, applied during `doServoInitialization` (so on every `Reconfigure`, not just the first construction), and skipped register-by-register when the servo already holds the target value — both because EEPROM has finite write endurance and because `initializeServosWithRetry` can run this same path up to 3 times in one bring-up. They are never sent to a servo outside the arm's own `servo_ids`: a 4-DOF arm configured with `servo_ids: [1,2,3,4]` leaves servo 5 alone even though the table has an entry for it. A gain write failing does not fail arm construction — the arm comes up at whatever gains the servo already holds. + +**Why this table.** Stock `P=32` is too weak to hold the gravity-loaded joints (shoulder lift, elbow) against load: at `I=0` they sit at a permanent, direction-independent steady-state error that scales as `1/P`. Adding integral gain removes it. Measured on joint 2: steady-state error drops roughly **4×, 0.90° → 0.22°** (10.2 → 2.5 encoder counts, 0.088°/count). The gravity-free joints (pan, wrist roll) need no integral term and get only the derivative bump. + +**The write order is D, then I, then 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` and was stable at `D=48, I=4` — so writing derivative first means every prefix of the sequence (`{D}`, `{D,I}`, `{D,I,P}`) is at least as stable as the state before it. If a write fails partway, the loop stops for that servo rather than continuing to the next register: continuing could land `{I,P}` without `D` — 48/32/4 on servo 3, the exact combination measured to oscillate — and strand it there in EEPROM with nothing to retry it (a gain-write failure never fails the arm, so nothing re-triggers this path outside the next `Reconfigure`). + +**Peak current rises steeply with `D`.** Taking a joint's `D` from 32 to 64 is not free: at `P=32`, a 100-count step measured 388 current units at `D=16` versus 691 at `D=48`. Servos idled at 37–43°C and rose only 1–2°C over the measurement session, so this was not a thermal problem there — but it would matter under sustained high-duty motion. + +**Measured on one arm, once.** 12.2–12.3 V bus, 2026-09-02. Unit-to-unit and supply-voltage variation are unquantified. If your arm needs different gains, set `disable_servo_gains: true` and run `pidtune apply` — it writes the same registers this module does. + ## Manual Mode Manual mode is a live jog-by-hand mode using position-deviation admittance. While active, a background control loop keeps each arm servo in position mode holding a goal — so the arm holds against gravity and never goes limp. When you backdrive a joint past a small angular deadband, the goal follows your hand, trailing by the deadband (which leaves a small restoring force so the joint still holds against gravity); when released, the joint holds its new pose. Torque is never fully disabled. The servo's `present_load` register is not used for control — it saturates on the gentlest push and doesn't encode push direction — and is reported only as telemetry. From acd344dd45af9bd50586aefcebeff70045fe34f4 Mon Sep 17 00:00:00 2001 From: HipsterBrown Date: Wed, 2 Sep 2026 13:54:49 -0400 Subject: [PATCH 10/14] docs: flag the three constants the new gains leave unverified The tuned gains change the ground these were measured against, and two of the comments still name p_gain 32 as though it were what ships. MinExecutableSpeedDegsPerSec and DefaultLookaheadDeg were both sized against the p_gain 32 droop band, which the new gains cut ~4x. Both are now wider than they need to be -- an opportunity, not a regression, and neither is re-derived, so the notes say measure before shrinking. maxAccUnits records the open conflict: this module's calibration found the Acc register saturating in the low 50s, while the 2026-09-02 step response shows overshoot and peak current still climbing at 64/96/128. Likely the two measured different things (arrival timing vs deceleration shape). Also records that teleop's speed cap is deliberately not bypassed with the acceleration ramp, and may bind first at 100 Hz. Co-Authored-By: Claude Opus 5 (1M context) Claude-Session: https://claude.ai/code/session_01NHef7PBha9FHmWoCSduR8J --- docs/arm.md | 2 ++ internal/servo/motion_profile.go | 7 +++++++ internal/servo/speed.go | 11 +++++++++-- 3 files changed, 18 insertions(+), 2 deletions(-) diff --git a/docs/arm.md b/docs/arm.md index c9f7624..68c4e32 100644 --- a/docs/arm.md +++ b/docs/arm.md @@ -387,6 +387,8 @@ A per-joint `MaxVelRadsJoints` cap still wins over the floor: a cap slower than Both skips are for a caller **streaming setpoints** rather than making a one-off point-to-point move — the [teleop service](teleop.md) sets it on every follower command. An acceleration ramp is close to a free win for a single move, but it is measurably catastrophic for continuously-updated tracking: `acc=32` quadrupled RMS tracking error and took lag from 40ms to 160ms versus `acc=0`, because the servo re-ramps on every new goal instead of ever reaching speed. The coordination read is skipped for the same reason it's pointless here — coordinated arrival is meaningless for a goal about to be superseded in ~10ms — and skipping it is measured to halve the bus traffic per call, one packet (the goal write) instead of two (a position read, then the write). A plain point-to-point move should keep both the ramp and the coordinated read; do not set `unlimited_accel` there. +One caveat, unmeasured: the speed cap is **not** bypassed alongside the ramp. The characterization's advice was to leave speed at maximum and shape motion with acceleration alone, but this module never commands the servo's `Speed: 0` (which means *maximum*, not stopped), so a streamed setpoint is still capped by `speed_degs_per_sec`. At 100 Hz that cap can bind before the acceleration bypass helps — a 50 °/s cap needs ~12 ms to cover a 0.6° per-tick leader motion against a 10 ms budget. The cap is kept deliberately, as a safety bound on a follower tracking a human hand; if teleop still feels laggy after this, suspect the cap before the ramp. + ### Waypoint streams `MoveThroughJointPositions` (and `GoToInputs`, which routes through it) paces its waypoints against the arm's real position: a waypoint is commanded, then held until every joint is within `waypoint_lookahead_deg` of it, and only then is the next one written. The arm waits for a full stop only after the **final** waypoint, so intermediate waypoints are still not stop-points — the arm flows through them. diff --git a/internal/servo/motion_profile.go b/internal/servo/motion_profile.go index 71d4ef1..a1fb454 100644 --- a/internal/servo/motion_profile.go +++ b/internal/servo/motion_profile.go @@ -28,6 +28,13 @@ const ( // maxAccUnits is a conservative round-down below the LOWEST measured knee. Measured // knees were 52, 55 and 63 across three joints; 50 sits under all of them. Above the // knee the register has no further effect, so scaling silently stops working. + // + // UNRESOLVED: step-response data from 2026-09-02 disagrees. On joint 2, well past that + // knee, overshoot and peak current keep climbing (Acc 64 -> 96 -> 128: 2.5 -> 6.0 -> 7.8 + // counts, 274 -> 489 -> 560 units) where a saturated register would be flat. Most likely + // the two studies measured different things -- this calibration recovered arrival timing, + // that one endpoint overshoot -- i.e. the register may stop affecting when the joint + // arrives while still shaping its deceleration. Settle it before relying on either bound. maxAccUnits = 50 ) diff --git a/internal/servo/speed.go b/internal/servo/speed.go index 33c6a3c..220a735 100644 --- a/internal/servo/speed.go +++ b/internal/servo/speed.go @@ -42,8 +42,11 @@ const ( // floor CoordinatedProfiles applies to every joint it scales down. Below it the register // fails in OPPOSITE directions depending on load, so no single correction recovers it. // -// A tuning value, not a hardware constant -- measured on one arm at p_gain 32. See -// docs/arm.md, "Motion and speed", for the measurements and their limits. +// A tuning value, not a hardware constant -- measured on one arm at p_gain 32, which is NO +// LONGER what ships: the tuned table runs the gravity-loaded joints at p_gain 48 (docs/arm.md, +// "Servo gains"). Not re-derived against it. Higher P partly addresses the load-dependent +// direction error this floor exists for, so this is an OPPORTUNITY to lower it -- re-measure +// before doing so. See docs/arm.md, "Motion and speed". const MinExecutableSpeedDegsPerSec = 12.0 // Waypoint-lookahead bounds, in degrees: how far ahead of the arm the commanded goal is @@ -56,6 +59,10 @@ const ( // margin; it deliberately does not clear p_gain 16, which the dwell's stall escape // backstops instead. NOT sufficient alone -- see LookaheadDegFor for the other bound. // Measurements: docs/arm.md, "Waypoint streams". + // + // The shipped gains cut that droop band ~4x (0.90 -> 0.22 deg on joint 2), so this floor + // is now wider than it needs to be -- a smaller one would track waypoints more tightly + // and dwell less. Not re-derived; do NOT shrink it on the arithmetic alone. DefaultLookaheadDeg = 3.0 MinLookaheadDeg = 0.1 MaxLookaheadDeg = 45.0 From 25fa99a9ed8c365cd2753aa4209bfaf840dbcfca Mon Sep 17 00:00:00 2001 From: HipsterBrown Date: Wed, 2 Sep 2026 14:12:09 -0400 Subject: [PATCH 11/14] feat(teleop): uncap speed on streamed setpoints, not just acceleration Found on hardware: raising speed_degs_per_sec kept improving teleop tracking, so the cap was binding once the acceleration ramp stopped dominating -- the open question left at the bottom of docs/arm.md. A streamed caller shapes the trajectory with its own update rate, so any profile the servo applies between setpoints is pure lag, speed included. Renames the extra key from unlimited_accel to streamed. It now controls speed, acceleration and the coordination read, so a name describing one register was already wrong, and the key is unreleased. minSpeedSteps and MinAccUnits both stay 1: the profiled path still cannot reach either sentinel, and a low configured value is not a substitute. The fake transport now stores every register inside a sync-written range rather than only the first, so the speed assertion can read goal_velocity back. Without that it would have passed vacuously against an unset register, the same way the acceleration one did. This removes a bound. Teleop does not gate its first cycle on the follower being near the leader, so a large initial gap is now closed at full servo speed with no ramp. Documented at both call sites; not gated in code. Co-Authored-By: Claude Opus 5 (1M context) Claude-Session: https://claude.ai/code/session_01NHef7PBha9FHmWoCSduR8J --- CLAUDE.md | 26 ++++++++----- components/arm/motion.go | 38 +++++++++++-------- ...ccel_test.go => streamed_setpoint_test.go} | 29 +++++++++----- docs/arm.md | 12 +++--- internal/controller/manager.go | 4 +- internal/testfake/fake_transport.go | 19 ++++++---- services/teleop/teleop.go | 6 +-- services/teleop/teleop_test.go | 20 +++++----- 8 files changed, 93 insertions(+), 61 deletions(-) rename components/arm/{unlimited_accel_test.go => streamed_setpoint_test.go} (62%) diff --git a/CLAUDE.md b/CLAUDE.md index 14a76bc..735872c 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -550,12 +550,20 @@ calibration wizard. It is bundled into `module.tar.gz` and needs **Node ≥ 20** 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. -- **`Acc: 0` is right for teleop and wrong for everything else, and the bypass skips the - coordination READ too, not just the ramp.** `MinAccUnits = 1` still forbids Acc 0 on the - profiled path — Acc 1 delivers ~43 deg/s², measurably worse than the measured acc=32 row, so a - low *configured* acceleration is not a substitute. The teleop bypass in `MoveToJointPositions` - (`components/arm/motion.go`) branches to `moveJointsUniform` BEFORE the `currentJoints()` read - that a ramped move uses to give `CoordinatedProfiles` a travel reference — coordinated arrival - is meaningless for a goal superseded in ~10ms, so the read buys nothing there either. Measured - in tests: one bus packet per call (the goal write) instead of two (a position read, then the - write). Do not unify this path with the profiled one. +- **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:** teleop does not gate its first + cycle on the follower being near the leader, so a large initial gap is now closed at full + servo speed with no ramp. diff --git a/components/arm/motion.go b/components/arm/motion.go index 48438d1..c2e3f8a 100644 --- a/components/arm/motion.go +++ b/components/arm/motion.go @@ -344,25 +344,30 @@ func (s *so101) dwellUntilNear(ctx context.Context, goal []float64, lookaheadDeg } // moveJointsUniform is the pre-coordination path, used as the fallback for when the -// travel-reference read fails and as the teleop unlimited-acceleration path. It commands -// every joint at one shared speed, which is why joints arrive at different times. +// travel-reference read fails and as the streamed-setpoint path. It commands every joint at +// one shared speed, which is why joints arrive at different times. // -// unlimitedAccel forces Acc 0 (UNLIMITED) instead of the deg/s^2 conversion -- the teleop -// bypass in MoveToJointPositions is the only caller that sets it true. See CLAUDE.md, -// "Acc: 0 means UNLIMITED". -func (s *so101) moveJointsUniform(ctx context.Context, to []float64, speedDegsPerSec, accelDegsPerSecSq float64, unlimitedAccel, wait bool) error { +// streamed forces BOTH registers to 0 -- max speed, unlimited acceleration -- instead of the +// deg/s and deg/s^2 conversions. The teleop bypass in MoveToJointPositions is the only caller +// that sets it true. See CLAUDE.md, "Speed: 0 means MAXIMUM". +func (s *so101) moveJointsUniform(ctx context.Context, to []float64, speedDegsPerSec, accelDegsPerSecSq float64, streamed, wait bool) error { // Writes an explicit UNIFORM profile rather than a bare position write. A coordinated // move leaves each servo's Speed and Acc registers at its own scaled value -- as low as // 1 step/s and Acc 1 -- and a position-only write does not touch either. So without // this the "uniform" fallback would inherit whatever the previous coordinated move left // behind, and would be uniform in neither speed nor acceleration. - accUnits := servo.DegPerSecSqToAccUnits(accelDegsPerSecSq) - if unlimitedAccel { - accUnits = 0 // UNLIMITED on this register -- see the teleop branch in MoveToJointPositions - } profile := controller.ServoProfile{ SpeedSteps: servo.DegPerSecToStepsPerSec(speedDegsPerSec), - AccUnits: accUnits, + AccUnits: servo.DegPerSecSqToAccUnits(accelDegsPerSecSq), + } + if streamed { + // Both registers to their "unbounded" sentinel: 0 is MAX SPEED and UNLIMITED + // acceleration, not stopped and not still. A streamed caller shapes the trajectory + // with its own update rate, so any profile the servo applies between setpoints is + // pure lag -- confirmed on hardware in both directions, first for acceleration and + // then for speed, where raising the cap improved teleop tracking until it stopped + // binding at all. See the branch in MoveToJointPositions. + profile.SpeedSteps, profile.AccUnits = 0, 0 } profiles := make([]controller.ServoProfile, len(s.armServoIDs)) for i := range profiles { @@ -401,11 +406,12 @@ func (s *so101) MoveToJointPositions(ctx context.Context, positions []referencef accel := float64(s.defaultAcc) s.mu.RUnlock() - // Streamed setpoints (teleop) skip both the ramp and the coordination read. An - // acceleration limit is catastrophic for tracking -- measured, acc=32 quadrupled RMS - // error and took lag from 40 ms to 160 ms -- and coordinated arrival is meaningless for - // a goal that will be superseded in ~10 ms. See docs/arm.md, "Motion and speed". - if unlimited, _ := extra["unlimited_accel"].(bool); unlimited { + // A streamed caller (teleop) gets no speed cap, no acceleration ramp, and no + // coordination read. An acceleration limit is catastrophic for tracking -- measured, + // acc=32 quadrupled RMS error and took lag from 40 ms to 160 ms -- a speed cap binds + // the same way once the ramp stops dominating, and coordinated arrival is meaningless + // for a goal that will be superseded in ~10 ms. See docs/arm.md, "Motion and speed". + if streamed, _ := extra["streamed"].(bool); streamed { return s.moveJointsUniform(ctx, clamped, speed, accel, true, servocmd.WaitArg(extra)) } diff --git a/components/arm/unlimited_accel_test.go b/components/arm/streamed_setpoint_test.go similarity index 62% rename from components/arm/unlimited_accel_test.go rename to components/arm/streamed_setpoint_test.go index 581908d..e217170 100644 --- a/components/arm/unlimited_accel_test.go +++ b/components/arm/streamed_setpoint_test.go @@ -29,22 +29,25 @@ func accelTestArm(t *testing.T, ft *testfake.FakeTransport) *so101 { } } -// Teleop streams small setpoints; an acceleration ramp on each one quadruples tracking error -// (measured: acc=32 -> 160 ms lag vs 40 ms at acc=0). Acc 0 is the servo's "unlimited" -// sentinel, which the deg/s^2 conversion path deliberately cannot produce. -func TestMoveToJointPositionsHonoursUnlimitedAccel(t *testing.T) { +// Teleop streams small setpoints, so any profile the servo applies between them is pure lag: +// an acceleration ramp quadruples tracking error (acc=32 -> 160 ms lag vs 40 ms at acc=0), +// and a speed cap binds the same way once the ramp stops dominating. 0 in either register is +// the "unbounded" sentinel -- MAX SPEED and UNLIMITED acceleration -- which neither +// conversion path can produce (minSpeedSteps and MinAccUnits are both 1). +func TestMoveToJointPositionsHonoursStreamed(t *testing.T) { ft := testfake.NewFakeTransport() s := accelTestArm(t, ft) - // Seed a NON-zero acceleration first. An unset register reads back as zero, so without - // this the assertion below cannot tell "commanded Acc 0" from "never wrote Acc at all". + // Seed NON-zero values first. An unset register reads back as zero, so without this the + // assertions below cannot tell "commanded 0" from "never wrote the register at all". for _, id := range s.armServoIDs { ft.SetRegister(id, feetech.RegAcceleration.Address, []byte{0x7F}) + ft.SetRegister(id, feetech.RegGoalVelocity.Address, testfake.EncodeWordLE(1234)) } goal := []referenceframe.Input{0.3, 0.3, 0.3, 0.3, 0.3} before := ft.PacketCount() require.NoError(t, s.MoveToJointPositions(context.Background(), goal, - map[string]interface{}{"wait": false, "unlimited_accel": true})) + map[string]interface{}{"wait": false, "streamed": true})) // Snapshot before the assertion loop below -- each of its reads is a packet too. moved := ft.PacketCount() - before @@ -52,6 +55,11 @@ func TestMoveToJointPositionsHonoursUnlimitedAccel(t *testing.T) { got, err := s.controller.ReadServoRegister(context.Background(), id, "acceleration") require.NoError(t, err) assert.Equal(t, byte(0), got[0], "servo %d must be commanded at unlimited acceleration", id) + // 0 here is the servo's MAX SPEED sentinel, not "stopped". A cap binds teleop + // tracking once the ramp stops dominating -- found on hardware. + vel, err := s.controller.ReadServoRegister(context.Background(), id, "goal_velocity") + require.NoError(t, err) + assert.Equal(t, []byte{0, 0}, vel, "servo %d must be commanded at max speed", id) } // One SyncWrite and nothing else. Skipping the coordination read is half the point of // the branch -- at 100 Hz the read plus the write is ~3 ms of a 10 ms budget -- and it @@ -59,8 +67,8 @@ func TestMoveToJointPositionsHonoursUnlimitedAccel(t *testing.T) { assert.Equal(t, 1, moved, "the unlimited path must issue one packet: the goal write") } -// The point-to-point default keeps its ramp: 500 deg/s^2 maps to Acc 49. -func TestMoveToJointPositionsRampsByDefault(t *testing.T) { +// The point-to-point default keeps both its ramp and its speed cap. +func TestMoveToJointPositionsProfilesByDefault(t *testing.T) { ft := testfake.NewFakeTransport() s := accelTestArm(t, ft) goal := []referenceframe.Input{0.3, 0.3, 0.3, 0.3, 0.3} @@ -74,6 +82,9 @@ func TestMoveToJointPositionsRampsByDefault(t *testing.T) { got, err := s.controller.ReadServoRegister(context.Background(), id, "acceleration") require.NoError(t, err) assert.NotZero(t, got[0], "servo %d must keep its acceleration ramp", id) + vel, err := s.controller.ReadServoRegister(context.Background(), id, "goal_velocity") + require.NoError(t, err) + assert.NotEqual(t, []byte{0, 0}, vel, "servo %d must keep its speed cap", id) } // SyncRead for the travel reference, then SyncWrite. This is the traffic the unlimited // path exists to avoid. diff --git a/docs/arm.md b/docs/arm.md index 68c4e32..a5c2e0c 100644 --- a/docs/arm.md +++ b/docs/arm.md @@ -374,20 +374,22 @@ A per-joint `MaxVelRadsJoints` cap still wins over the floor: a cap slower than `MoveThroughJointPositions` accepts `MoveOptions.MaxVelRads` and `MaxAccRads` to override the configured defaults for one move, and the per-joint forms `MaxVelRadsJoints` and `MaxAccRadsJoints`. Setting a per-joint slice makes the matching scalar ignored, per `arm.proto`; a slice whose length doesn't match the arm's joint count is rejected. A per-joint limit slows the whole move rather than that joint, so the joints stay coordinated. `GoToInputs` routes through this same path. -### Streamed setpoints: `wait` and `unlimited_accel` +### Streamed setpoints: `wait` and `streamed` `MoveToJointPositions` accepts two optional booleans in its `extra` map: ```json -{ "wait": false, "unlimited_accel": true } +{ "wait": false, "streamed": true } ``` - **`wait`** (default `true`) — block until the move settles, or return as soon as the goal is written. -- **`unlimited_accel`** (default `false`) — command the servos' acceleration register to `0`, the servo's "unlimited" sentinel, instead of the configured `acceleration_degs_per_sec_per_sec` ramp, and skip the coordination read that a ramped move uses to give the other joints a travel reference for arriving together. +- **`streamed`** (default `false`) — the caller is streaming setpoints, not making a one-off move. Commands the servos' acceleration **and** speed registers to `0` — the "unlimited" and "maximum" sentinels, not "none" and "stopped" — instead of the configured `acceleration_degs_per_sec_per_sec` ramp and `speed_degs_per_sec` cap, and skips the coordination read that a profiled move uses to give the joints a travel reference for arriving together. -Both skips are for a caller **streaming setpoints** rather than making a one-off point-to-point move — the [teleop service](teleop.md) sets it on every follower command. An acceleration ramp is close to a free win for a single move, but it is measurably catastrophic for continuously-updated tracking: `acc=32` quadrupled RMS tracking error and took lag from 40ms to 160ms versus `acc=0`, because the servo re-ramps on every new goal instead of ever reaching speed. The coordination read is skipped for the same reason it's pointless here — coordinated arrival is meaningless for a goal about to be superseded in ~10ms — and skipping it is measured to halve the bus traffic per call, one packet (the goal write) instead of two (a position read, then the write). A plain point-to-point move should keep both the ramp and the coordinated read; do not set `unlimited_accel` there. +The [teleop service](teleop.md) sets this on every follower command. The principle behind all three effects is the same: a streamed caller shapes the trajectory with its own update rate, so **any profile the servo applies between setpoints is pure lag**. An acceleration ramp is close to a free win for a single move but is measurably catastrophic for continuously-updated tracking — `acc=32` quadrupled RMS tracking error and took lag from 40ms to 160ms versus `acc=0`, because the servo re-ramps on every new goal instead of ever reaching speed. The speed cap binds the same way once the ramp stops dominating: raising `speed_degs_per_sec` was found on hardware to keep improving teleop tracking, which is what motivated uncapping it entirely. And coordinated arrival is meaningless for a goal about to be superseded in ~10ms, so skipping the read costs nothing and halves the bus traffic per call — one packet (the goal write) instead of two (a position read, then the write). -One caveat, unmeasured: the speed cap is **not** bypassed alongside the ramp. The characterization's advice was to leave speed at maximum and shape motion with acceleration alone, but this module never commands the servo's `Speed: 0` (which means *maximum*, not stopped), so a streamed setpoint is still capped by `speed_degs_per_sec`. At 100 Hz that cap can bind before the acceleration bypass helps — a 50 °/s cap needs ~12 ms to cover a 0.6° per-tick leader motion against a 10 ms budget. The cap is kept deliberately, as a safety bound on a follower tracking a human hand; if teleop still feels laggy after this, suspect the cap before the ramp. +A plain point-to-point move should keep all three; do not set `streamed` there. + +**This removes a bound, so mind the first tick.** With no speed cap and no ramp, a large position error is closed as fast as the servo physically can. Teleop does not gate its first cycle on the follower already being near the leader, so starting teleop with the two arms far apart will snap the follower to the leader pose. Match the poses roughly before starting, or keep hands clear when you do. ### Waypoint streams diff --git a/internal/controller/manager.go b/internal/controller/manager.go index 0b9ed51..49f8634 100644 --- a/internal/controller/manager.go +++ b/internal/controller/manager.go @@ -111,8 +111,8 @@ func (h *ControllerHandle) MoveServosToPositions(ctx context.Context, servoIDs [ // ServoProfile is one servo's speed and acceleration for a coordinated move. type ServoProfile struct { - SpeedSteps int // goal velocity, steps/sec. Never 0: that means MAX SPEED. - AccUnits int // acceleration register. 0 means UNLIMITED -- only the teleop bypass sends it. + SpeedSteps int // goal velocity, steps/sec. 0 means MAX SPEED -- only the streamed bypass sends it. + AccUnits int // acceleration register. 0 means UNLIMITED -- only the streamed bypass sends it. } // MoveServosWithProfiles commands each servo to its angle under its OWN speed and diff --git a/internal/testfake/fake_transport.go b/internal/testfake/fake_transport.go index 3a53e09..f23863b 100644 --- a/internal/testfake/fake_transport.go +++ b/internal/testfake/fake_transport.go @@ -150,18 +150,23 @@ func (ft *FakeTransport) reply(req []byte) { ft.queue(int(raw), ft.value(int(raw), address, length)) } case 0x83: // SYNC_WRITE -- broadcast, no response on the wire - // Params are [address, dataLen, id, data..., id, data...]. The whole blob is stored - // at `address`, so a read of the FIRST register in the blob reads back correctly -- - // which is what a goal write's `acceleration` needs. A read of a register further - // into the blob (goal_position, at address+1) does NOT; split per register if a - // test ever needs that. + // Params are [address, dataLen, id, data..., id, data...]. A sync write covers a + // RANGE of registers (a goal write is acceleration, position, time and velocity in + // one 7-byte blob), so store the blob at `address` AND every suffix of it at the + // address it starts from. value() copies min(len, stored) bytes, so a later read of + // any register inside the range returns its own bytes rather than the blob's head. if len(params) < 2 { return } address, n := params[0], int(params[1]) for i := 2; i+1+n <= len(params); i += 1 + n { - if regs, ok := ft.registers[int(params[i])]; ok { - regs[address] = append([]byte(nil), params[i+1:i+1+n]...) + regs, ok := ft.registers[int(params[i])] + if !ok { + continue + } + blob := params[i+1 : i+1+n] + for off := 0; off < n; off++ { + regs[address+byte(off)] = append([]byte(nil), blob[off:]...) } } } diff --git a/services/teleop/teleop.go b/services/teleop/teleop.go index 0da1072..064cccd 100644 --- a/services/teleop/teleop.go +++ b/services/teleop/teleop.go @@ -185,10 +185,10 @@ func (tp *so101Teleop) syncOnce(ctx context.Context) error { gripperPct = pct } - // acc=0 (unlimited) for streamed setpoints -- an acceleration limit is catastrophic for - // tracking. The command stream itself shapes the trajectory here. + // No speed cap, no acceleration ramp, no coordination read: the command stream itself + // shapes the trajectory, so anything the servo applies between setpoints is pure lag. if err := tp.followerArm.MoveToJointPositions(ctx, positions, - map[string]interface{}{"wait": false, "unlimited_accel": true}); err != nil { + map[string]interface{}{"wait": false, "streamed": true}); err != nil { return fmt.Errorf("write follower joints: %w", err) } diff --git a/services/teleop/teleop_test.go b/services/teleop/teleop_test.go index a622117..1cd87dc 100644 --- a/services/teleop/teleop_test.go +++ b/services/teleop/teleop_test.go @@ -20,14 +20,14 @@ type fakeArm struct { arm.Arm name resource.Name - mu sync.Mutex - jp []referenceframe.Input - jpErr error - moved []referenceframe.Input - moveWait interface{} - moveUnlimited interface{} - moveErr error - torqueLog []bool + mu sync.Mutex + jp []referenceframe.Input + jpErr error + moved []referenceframe.Input + moveWait interface{} + moveStreamed interface{} + moveErr error + torqueLog []bool } func (f *fakeArm) Name() resource.Name { return f.name } @@ -49,7 +49,7 @@ func (f *fakeArm) MoveToJointPositions(ctx context.Context, positions []referenc } f.moved = positions f.moveWait = extra["wait"] - f.moveUnlimited = extra["unlimited_accel"] + f.moveStreamed = extra["streamed"] return nil } @@ -127,7 +127,7 @@ func TestTeleopSyncOnce(t *testing.T) { test.That(t, err, test.ShouldBeNil) test.That(t, fa.moved, test.ShouldResemble, la.jp) test.That(t, fa.moveWait, test.ShouldEqual, false) - test.That(t, fa.moveUnlimited, test.ShouldEqual, true) + test.That(t, fa.moveStreamed, test.ShouldEqual, true) test.That(t, fg.setPct, test.ShouldResemble, []float64{42}) }) From a1104e91a832d2c3889e849dcc081de98ae08bc3 Mon Sep 17 00:00:00 2001 From: HipsterBrown Date: Wed, 2 Sep 2026 14:40:41 -0400 Subject: [PATCH 12/14] feat(teleop): mirror profiled until the follower catches up Uncapping speed and acceleration removed the bound that made a large initial position error safe: with neither, the follower closes the gap as fast as the servo physically can, and nothing stopped teleop handing a streamed setpoint two arms in completely different poses. readyToStream keeps the mirror on profiled moves until the follower is within 5 deg of the leader on every joint, then latches. The latch is the point -- profiling only the first cycle would do nothing, because a servo goal write supersedes the previous one: the next cycle 10ms later would overwrite the profiled goal with an unbounded one while the arm was still far away. It must not re-evaluate after tripping either, since a running mirror legitimately trails the leader by more than the gate. Costs one extra follower read per cycle, only until it trips. Reported as `streaming` in the status DoCommand, and cleared on start so a second session does not inherit the first one's catch-up. Co-Authored-By: Claude Opus 5 (1M context) Claude-Session: https://claude.ai/code/session_01NHef7PBha9FHmWoCSduR8J --- CLAUDE.md | 11 ++++-- docs/arm.md | 2 +- docs/teleop.md | 2 ++ services/teleop/teleop.go | 61 +++++++++++++++++++++++++++++++- services/teleop/teleop_test.go | 63 ++++++++++++++++++++++++++++++++-- 5 files changed, 131 insertions(+), 8 deletions(-) diff --git a/CLAUDE.md b/CLAUDE.md index 735872c..1f228be 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -564,6 +564,11 @@ calibration wizard. It is bundled into `module.tar.gz` and needs **Node ≥ 20** *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:** teleop does not gate its first - cycle on the follower being near the leader, so a large initial gap is now closed at full - servo speed with no ramp. + 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. diff --git a/docs/arm.md b/docs/arm.md index a5c2e0c..9df302b 100644 --- a/docs/arm.md +++ b/docs/arm.md @@ -389,7 +389,7 @@ The [teleop service](teleop.md) sets this on every follower command. The princip A plain point-to-point move should keep all three; do not set `streamed` there. -**This removes a bound, so mind the first tick.** With no speed cap and no ramp, a large position error is closed as fast as the servo physically can. Teleop does not gate its first cycle on the follower already being near the leader, so starting teleop with the two arms far apart will snap the follower to the leader pose. Match the poses roughly before starting, or keep hands clear when you do. +**This removes a bound, so a caller must not hand `streamed` a large position error** — it is closed as fast as the servo physically can. It is the caller's job to be near its target first; the teleop service gates itself on exactly this (see below). ### Waypoint streams diff --git a/docs/teleop.md b/docs/teleop.md index ae57211..b07c4dd 100644 --- a/docs/teleop.md +++ b/docs/teleop.md @@ -86,6 +86,8 @@ were measured at unlimited acceleration, which is how teleop now drives the foll it only if the serial bus or host cannot keep up; the mirror loop's ticker drops ticks rather than backing up, so a rate the bus cannot sustain degrades silently to the rate it can. +`streaming` reports whether the mirror has switched from profiled moves to streamed setpoints. Teleop drives the follower with no speed cap and no acceleration ramp, which is what keeps tracking lag down — but that means a large position error is closed at full servo speed, so the mirror will not use a streamed setpoint until the follower is within 5° of the leader on every joint. Until then it mirrors with the arm's configured `speed_degs_per_sec` and acceleration, and `streaming` reads `false`. The switch latches for the session: a running mirror legitimately trails the leader, and dropping back would reintroduce the lag. Starting with the arms in very different poses is therefore safe, but the follower will travel to meet the leader at the configured speed as soon as the loop starts. + `cycles` is the total number of successful mirror cycles since the last start. `consecutive_errors` resets to zero on any successful cycle. When `max_consecutive_errors` is reached the loop sets `running` to `false` and records the triggering error in `last_error`. ## Notes diff --git a/services/teleop/teleop.go b/services/teleop/teleop.go index 064cccd..6e23c1e 100644 --- a/services/teleop/teleop.go +++ b/services/teleop/teleop.go @@ -5,14 +5,17 @@ package teleop import ( "context" "fmt" + "math" "sync" "time" "go.viam.com/rdk/components/arm" "go.viam.com/rdk/components/gripper" "go.viam.com/rdk/logging" + "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/resource" "go.viam.com/rdk/services/generic" + "go.viam.com/rdk/utils" ) var SO101TeleopModel = resource.NewModel("devrel", "so101", "teleop") @@ -26,6 +29,15 @@ const ( defaultTeleopRateHz = 100.0 defaultMaxConsecutiveErrors = 10 maxTeleopRateHz = 1000.0 + + // maxStreamStartGapDeg is how close the follower must get to the leader before the + // mirror switches from profiled moves to streamed setpoints. A streamed setpoint carries + // no speed cap and no acceleration ramp, so handing one a large gap closes it at full + // servo speed -- fine for the sub-degree deltas of a running mirror, not for the arms + // starting in different poses. 5 deg is comfortably above the arm's steady-state droop + // (~0.22 deg tuned, 0.9 deg stock) so it is reachable, and small enough that the switch + // happens at a speed a hand is already tracking. + maxStreamStartGapDeg = 5.0 ) func init() { @@ -93,6 +105,9 @@ type so101Teleop struct { cycles uint64 consecutiveErrors int lastError string + // streaming latches true once the follower has caught up with the leader. Until then the + // mirror runs profiled -- see maxStreamStartGapDeg. + streaming bool } func newSO101Teleop( @@ -187,8 +202,12 @@ func (tp *so101Teleop) syncOnce(ctx context.Context) error { // No speed cap, no acceleration ramp, no coordination read: the command stream itself // shapes the trajectory, so anything the servo applies between setpoints is pure lag. + streamed, err := tp.readyToStream(ctx, positions) + if err != nil { + return err + } if err := tp.followerArm.MoveToJointPositions(ctx, positions, - map[string]interface{}{"wait": false, "streamed": true}); err != nil { + map[string]interface{}{"wait": false, "streamed": streamed}); err != nil { return fmt.Errorf("write follower joints: %w", err) } @@ -204,6 +223,44 @@ func (tp *so101Teleop) syncOnce(ctx context.Context) error { return nil } +// readyToStream reports whether this cycle may use a streamed setpoint, latching true the +// first time the follower is within maxStreamStartGapDeg of the leader on every joint. +// +// The latch is the point. A streamed setpoint has no speed cap and no acceleration ramp, so +// it must not be handed a large gap -- and profiling only the FIRST cycle would not help, +// because a servo goal write supersedes the previous one: the next cycle 10 ms later would +// overwrite the profiled goal with an unbounded one while the arm was still far away. So the +// mirror stays profiled until the gap is actually closed. +// +// It costs one extra follower read per cycle, and only until the latch trips. +func (tp *so101Teleop) readyToStream(ctx context.Context, leader []referenceframe.Input) (bool, error) { + tp.mu.Lock() + streaming := tp.streaming + tp.mu.Unlock() + if streaming { + return true, nil + } + + follower, err := tp.followerArm.JointPositions(ctx, nil) + if err != nil { + return false, fmt.Errorf("read follower joints: %w", err) + } + if len(follower) != len(leader) { + return false, fmt.Errorf("follower reports %d joints, leader %d", len(follower), len(leader)) + } + for i := range leader { + if math.Abs(utils.RadToDeg(float64(leader[i])-float64(follower[i]))) > maxStreamStartGapDeg { + return false, nil + } + } + + tp.mu.Lock() + tp.streaming = true + tp.mu.Unlock() + tp.logger.Infof("Follower caught up (within %.1f deg); streaming setpoints", maxStreamStartGapDeg) + return true, nil +} + func (tp *so101Teleop) DoCommand(ctx context.Context, cmd map[string]interface{}) (map[string]interface{}, error) { switch cmd["command"] { case "start": @@ -223,6 +280,7 @@ func (tp *so101Teleop) DoCommand(ctx context.Context, cmd map[string]interface{} "consecutive_errors": tp.consecutiveErrors, "last_error": tp.lastError, "rate_hz": tp.rateHz, + "streaming": tp.streaming, }, nil default: return nil, fmt.Errorf("unknown command: %v", cmd["command"]) @@ -244,6 +302,7 @@ func (tp *so101Teleop) start(ctx context.Context) error { tp.cycles = 0 tp.consecutiveErrors = 0 tp.lastError = "" + tp.streaming = false tp.wg.Add(1) go tp.runLoop(loopCtx) tp.logger.Infof("Teleop started at %.1f Hz", tp.rateHz) diff --git a/services/teleop/teleop_test.go b/services/teleop/teleop_test.go index 1cd87dc..434fe2e 100644 --- a/services/teleop/teleop_test.go +++ b/services/teleop/teleop_test.go @@ -118,7 +118,7 @@ func newTestTeleop(t testing.TB, la, fa *fakeArm, lg, fg *fakeGripper) *so101Tel func TestTeleopSyncOnce(t *testing.T) { t.Run("forwards joints non-blocking and gripper percentage", func(t *testing.T) { la := &fakeArm{name: arm.Named("l"), jp: []referenceframe.Input{0.1, 0.2, 0.3, 0.4, 0.5}} - fa := &fakeArm{name: arm.Named("f")} + fa := &fakeArm{name: arm.Named("f"), jp: []referenceframe.Input{0.1, 0.2, 0.3, 0.4, 0.5}} lg := &fakeGripper{name: gripper.Named("lg"), pct: 42} fg := &fakeGripper{name: gripper.Named("fg")} tp := newTestTeleop(t, la, fa, lg, fg) @@ -133,7 +133,7 @@ func TestTeleopSyncOnce(t *testing.T) { t.Run("arm-only when grippers absent", func(t *testing.T) { la := &fakeArm{name: arm.Named("l"), jp: []referenceframe.Input{1}} - fa := &fakeArm{name: arm.Named("f")} + fa := &fakeArm{name: arm.Named("f"), jp: []referenceframe.Input{1}} tp := newTestTeleop(t, la, fa, nil, nil) err := tp.syncOnce(context.Background()) test.That(t, err, test.ShouldBeNil) @@ -217,7 +217,7 @@ func TestTeleopStartStopStatus(t *testing.T) { func TestTeleopTickErrorThreshold(t *testing.T) { la := &fakeArm{name: arm.Named("l"), jpErr: errors.New("boom")} - fa := &fakeArm{name: arm.Named("f")} + fa := &fakeArm{name: arm.Named("f"), jp: []referenceframe.Input{0.5}} tp := newTestTeleop(t, la, fa, nil, nil) tp.maxConsecutiveErr = 3 tp.running = true @@ -242,6 +242,63 @@ func TestTeleopTickErrorThreshold(t *testing.T) { test.That(t, tp.cycles, test.ShouldEqual, uint64(1)) } +// A streamed setpoint has no speed cap and no ramp, so the mirror must not use one until +// the follower has actually caught up -- otherwise starting teleop with the arms in +// different poses snaps the follower across at full servo speed. +func TestTeleopMirrorsProfiledUntilTheFollowerCatchesUp(t *testing.T) { + la := &fakeArm{name: arm.Named("l"), jp: []referenceframe.Input{1.0}} + fa := &fakeArm{name: arm.Named("f"), jp: []referenceframe.Input{0.0}} // ~57 deg away + tp := newTestTeleop(t, la, fa, nil, nil) + + test.That(t, tp.syncOnce(context.Background()), test.ShouldBeNil) + test.That(t, fa.moveStreamed, test.ShouldEqual, false) + test.That(t, tp.streaming, test.ShouldBeFalse) + + // Follower arrives within the gate. + fa.mu.Lock() + fa.jp = []referenceframe.Input{1.0} + fa.mu.Unlock() + + test.That(t, tp.syncOnce(context.Background()), test.ShouldBeNil) + test.That(t, fa.moveStreamed, test.ShouldEqual, true) + test.That(t, tp.streaming, test.ShouldBeTrue) +} + +// Once latched it stays latched: a running mirror legitimately runs a lag behind the leader, +// and dropping back to profiled moves mid-session would reintroduce the lag the streamed +// path exists to remove. +func TestTeleopStreamingLatches(t *testing.T) { + la := &fakeArm{name: arm.Named("l"), jp: []referenceframe.Input{0.0}} + fa := &fakeArm{name: arm.Named("f"), jp: []referenceframe.Input{0.0}} + tp := newTestTeleop(t, la, fa, nil, nil) + + test.That(t, tp.syncOnce(context.Background()), test.ShouldBeNil) + test.That(t, fa.moveStreamed, test.ShouldEqual, true) + + // The leader lunges far away; the mirror must keep streaming. + la.mu.Lock() + la.jp = []referenceframe.Input{2.0} + la.mu.Unlock() + + test.That(t, tp.syncOnce(context.Background()), test.ShouldBeNil) + test.That(t, fa.moveStreamed, test.ShouldEqual, true) +} + +// start() must clear the latch, or a second session inherits the first one's catch-up. +func TestTeleopStartClearsTheStreamingLatch(t *testing.T) { + la := &fakeArm{name: arm.Named("l"), jp: []referenceframe.Input{0.0}} + fa := &fakeArm{name: arm.Named("f"), jp: []referenceframe.Input{0.0}} + tp := newTestTeleop(t, la, fa, nil, nil) + tp.streaming = true + + test.That(t, tp.start(context.Background()), test.ShouldBeNil) + defer tp.stop() + tp.mu.Lock() + streaming := tp.streaming + tp.mu.Unlock() + test.That(t, streaming, test.ShouldBeFalse) +} + func TestTeleopCloseStopsLoop(t *testing.T) { la := &fakeArm{name: arm.Named("l"), jp: []referenceframe.Input{0.1}} fa := &fakeArm{name: arm.Named("f")} From f5f0e265240b1c1c0aaf9658e644df210516669f Mon Sep 17 00:00:00 2001 From: HipsterBrown Date: Wed, 2 Sep 2026 19:35:41 -0400 Subject: [PATCH 13/14] refactor: trim the servo-gain and teleop-guard diffs Collapses controllerManualIO's three parallel per-servo maps into one map of a struct with one restore loop, factors the twice-duplicated read-one-byte-gain-and-check-width block into readGainByte, and cuts two comment blocks that restated docs/arm.md and CLAUDE.md verbatim -- the docs commit was meant to make those the single home. The map collapse trades a structural invariant for a boolean: with three maps, restoring a register the apply never captured was impossible; with one, it depends on savedGains/savedTorque. Deleting that guard left the whole suite green while writing p_gain 0 -- no position control -- to every servo of a user who opted out of gain writes. TestApplyComplianceHonours- ThePGainOptOut now restores as well as applies, and fails on that mutation. Not applied: replacing writeGainIfChanged in applyCompliance with a compare against the captured value. It saves two bus reads per servo but costs four lines, and the reads are not what needed cutting. Co-Authored-By: Claude Opus 5 (1M context) Claude-Session: https://claude.ai/code/session_01NHef7PBha9FHmWoCSduR8J --- components/arm/gains.go | 40 ++++-------- components/arm/manual.go | 82 +++++++++++++----------- components/arm/manual_compliance_test.go | 6 ++ services/teleop/teleop.go | 11 ++-- 4 files changed, 67 insertions(+), 72 deletions(-) diff --git a/components/arm/gains.go b/components/arm/gains.go index febee53..2e3dce1 100644 --- a/components/arm/gains.go +++ b/components/arm/gains.go @@ -10,18 +10,12 @@ import ( ) // writeGainIfChanged writes a one-byte gain register unless it can prove the write would -// change nothing. +// change nothing. These are EEPROM registers and both callers rewrite the same values +// repeatedly, so the skip is a wear guard. // -// p_gain, d_gain and i_gain are all EEPROM registers (addresses 21-23) with finite write -// endurance, and every write costs three bus transactions for the unlock/write/relock dance. -// Both callers rewrite the same values repeatedly -- manual mode on every entry and exit, -// arm init on every Reconfigure -- so the skip is the wear guard, not an optimization. -// -// A read that FAILS therefore falls through to the write rather than returning: it cannot -// prove the write is unnecessary, and the wear guard is not worth the alternative. On -// restoreCompliance's path, skipping the write leaves a joint at p_gain 8 / i_gain 0 in -// EEPROM -- soft, across power cycles -- and a bare register read failing outright is a -// transient this hardware really produces, with nothing retrying it. +// 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 @@ -29,25 +23,13 @@ func writeGainIfChanged(ctx context.Context, h *controller.ControllerHandle, id return h.WriteServoRegister(ctx, id, name, []byte{want}) } -// applyServoGains writes the tuned P/D/I table to the arm's own servos, skipping registers -// it can prove already hold the target value. -// -// The write ORDER is fixed at D, I, P and is not incidental: a non-zero I is only stable -// against a raised D (measured -- joint 2 oscillated at D=32/I=4 and was stable at -// D=48/I=4), so damping first means every prefix of the sequence is at least as stable as -// the state before it. A partial failure then cannot strand a gravity-loaded joint on a -// combination measured to oscillate, in EEPROM. -// -// The prefix argument only holds if a failure is TERMINAL for that servo, hence the break: -// without it the reachable states are all eight subsets rather than the four prefixes, and -// {I,P}-without-D is 48/32/4 on servo 3 -- precisely the state this ordering exists to -// prevent. It is load-bearing for a second reason since writeGainIfChanged began writing -// through an unreadable register: all three writes are now attempted even when the reads -// fail, so the break is the only thing bounding the failure set. Per servo, not per call -- -// one servo's bus trouble must not skip the rest. +// applyServoGains writes the tuned P/D/I table to the arm's own servos. // -// It NEVER returns an error: the arm works at stock gains, so a bus problem here must not -// prevent it from coming up. +// 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] diff --git a/components/arm/manual.go b/components/arm/manual.go index 6d50b17..da53d7f 100644 --- a/components/arm/manual.go +++ b/components/arm/manual.go @@ -221,11 +221,16 @@ type controllerManualIO struct { controller *controller.ControllerHandle ids []int limits map[int][2]float64 - origPGain map[int]int // captured lazily on applyCompliance for restore - origIGain map[int]int // captured lazily on applyCompliance for restore - origTorque map[int]int // captured lazily on applyCompliance for restore + // orig holds the pre-compliance register values, captured on applyCompliance. The two + // flags say which fields in it are live: compliance is per-register opt-out, so a + // restore must not write a register the apply never touched. + orig map[int]savedRegs + savedGains bool + savedTorque bool } +type savedRegs struct{ pGain, iGain, torque int } + func newControllerManualIO(c *controller.ControllerHandle, ids []int, limits [][2]float64) *controllerManualIO { lm := map[int][2]float64{} for i, id := range ids { @@ -233,7 +238,7 @@ func newControllerManualIO(c *controller.ControllerHandle, ids []int, limits [][ lm[id] = limits[i] } } - return &controllerManualIO{controller: c, ids: ids, limits: lm, origPGain: map[int]int{}, origIGain: map[int]int{}, origTorque: map[int]int{}} + return &controllerManualIO{controller: c, ids: ids, limits: lm, orig: map[int]savedRegs{}} } func (a *controllerManualIO) servoIDs() []int { return a.ids } @@ -273,31 +278,19 @@ func (a *controllerManualIO) applyCompliance(ctx context.Context, pGain, torqueL // Capture originals for every register we intend to change BEFORE writing anything, so a // later restore always covers every servo we touch (fail-safe: on any read error, change // nothing and let manual mode run at full stiffness — the arm still holds). - pg := map[int]int{} - ig := map[int]int{} - tq := map[int]int{} + orig := map[int]savedRegs{} for _, id := range a.ids { + var s savedRegs if pGain > 0 { - cur, err := a.controller.ReadServoRegister(ctx, id, "p_gain") - if err != nil { - return fmt.Errorf("read p_gain servo %d: %w", id, err) - } - if len(cur) != 1 { - return fmt.Errorf("unexpected p_gain width %d for servo %d", len(cur), id) - } - pg[id] = int(cur[0]) - - // i_gain is a gain register too: the integral path is independent of P, so - // lowering P alone leaves it live. Gated on the same pGain>0 opt-out as p_gain - // itself, per the manualIO interface comment. - curI, err := a.controller.ReadServoRegister(ctx, id, "i_gain") - if err != nil { - return fmt.Errorf("read i_gain servo %d: %w", id, err) + // i_gain is captured alongside p_gain because the integral path is independent + // of P, so lowering P alone leaves it live. Same pGain>0 opt-out, per manualIO. + var err error + if s.pGain, err = a.readGainByte(ctx, id, "p_gain"); err != nil { + return err } - if len(curI) != 1 { - return fmt.Errorf("unexpected i_gain width %d for servo %d", len(curI), id) + if s.iGain, err = a.readGainByte(ctx, id, "i_gain"); err != nil { + return err } - ig[id] = int(curI[0]) } if torqueLimit > 0 { cur, err := a.controller.ReadServoRegister(ctx, id, "torque_limit") @@ -307,12 +300,11 @@ func (a *controllerManualIO) applyCompliance(ctx context.Context, pGain, torqueL if len(cur) != 2 { return fmt.Errorf("unexpected torque_limit width %d for servo %d", len(cur), id) } - tq[id] = int(cur[0]) | int(cur[1])<<8 // STS3215 is little-endian + s.torque = int(cur[0]) | int(cur[1])<<8 // STS3215 is little-endian } + orig[id] = s } - a.origPGain = pg - a.origIGain = ig - a.origTorque = tq + a.orig, a.savedGains, a.savedTorque = orig, pGain > 0, torqueLimit > 0 for _, id := range a.ids { if pGain > 0 { // Zeroed, not lowered: the integral path is independent of P, so an arm held @@ -336,25 +328,37 @@ func (a *controllerManualIO) applyCompliance(ctx context.Context, pGain, torqueL func (a *controllerManualIO) restoreCompliance(ctx context.Context) error { var firstErr error - for id, v := range a.origPGain { - if err := writeGainIfChanged(ctx, a.controller, id, "p_gain", byte(v)); err != nil && firstErr == nil { + keep := func(err error) { + if err != nil && firstErr == nil { firstErr = err } } - for id, v := range a.origIGain { - if err := writeGainIfChanged(ctx, a.controller, id, "i_gain", byte(v)); err != nil && firstErr == nil { - firstErr = err + for id, v := range a.orig { + if a.savedGains { + keep(writeGainIfChanged(ctx, a.controller, id, "p_gain", byte(v.pGain))) + keep(writeGainIfChanged(ctx, a.controller, id, "i_gain", byte(v.iGain))) } - } - for id, v := range a.origTorque { - if err := a.controller.WriteServoRegister(ctx, id, "torque_limit", - []byte{byte(v & 0xFF), byte((v >> 8) & 0xFF)}); err != nil && firstErr == nil { - firstErr = err + if a.savedTorque { + keep(a.controller.WriteServoRegister(ctx, id, "torque_limit", + []byte{byte(v.torque & 0xFF), byte((v.torque >> 8) & 0xFF)})) } } return firstErr } +// readGainByte reads a one-byte gain register, rejecting a short read rather than indexing +// into it. +func (a *controllerManualIO) readGainByte(ctx context.Context, id int, name string) (int, error) { + cur, err := a.controller.ReadServoRegister(ctx, id, name) + if err != nil { + return 0, fmt.Errorf("read %s servo %d: %w", name, id, err) + } + if len(cur) != 1 { + return 0, fmt.Errorf("unexpected %s width %d for servo %d", name, len(cur), id) + } + return int(cur[0]), nil +} + // Bench-validated compliance defaults: the lowest settings found to still hold a fully // extended arm against gravity while staying easy to guide by hand. These are arm- and // payload-dependent, so a heavily loaded arm may need a higher torqueLimit; see README. diff --git a/components/arm/manual_compliance_test.go b/components/arm/manual_compliance_test.go index 9140e0a..e42a076 100644 --- a/components/arm/manual_compliance_test.go +++ b/components/arm/manual_compliance_test.go @@ -45,16 +45,22 @@ func TestApplyComplianceZeroesIGain(t *testing.T) { func TestApplyComplianceHonoursThePGainOptOut(t *testing.T) { ft := testfake.NewFakeTransport() for _, id := range []int{1, 2} { + ft.SetRegister(id, feetech.RegPGain.Address, []byte{48}) ft.SetRegister(id, feetech.RegIGain.Address, []byte{4}) ft.SetRegister(id, feetech.RegTorqueLimit.Address, testfake.EncodeWordLE(1000)) } io := &controllerManualIO{controller: testArmHandle(t, ft), ids: []int{1, 2}} require.NoError(t, io.applyCompliance(context.Background(), 0, 50)) + // Restore too: it writes whatever it captured, so a register the apply never touched + // must not be written back from a zero value -- p_gain 0 is no position control at all. + require.NoError(t, io.restoreCompliance(context.Background())) for _, id := range []int{1, 2} { assert.Equal(t, byte(4), readReg(t, io, id, "i_gain"), "servo %d i_gain must be untouched", id) + assert.Equal(t, byte(48), readReg(t, io, id, "p_gain"), "servo %d p_gain must be untouched", id) assert.Zero(t, ft.WriteCount(id, feetech.RegIGain.Address)) + assert.Zero(t, ft.WriteCount(id, feetech.RegPGain.Address)) } } diff --git a/services/teleop/teleop.go b/services/teleop/teleop.go index 6e23c1e..e691f9a 100644 --- a/services/teleop/teleop.go +++ b/services/teleop/teleop.go @@ -234,10 +234,7 @@ func (tp *so101Teleop) syncOnce(ctx context.Context) error { // // It costs one extra follower read per cycle, and only until the latch trips. func (tp *so101Teleop) readyToStream(ctx context.Context, leader []referenceframe.Input) (bool, error) { - tp.mu.Lock() - streaming := tp.streaming - tp.mu.Unlock() - if streaming { + if tp.latched() { return true, nil } @@ -261,6 +258,12 @@ func (tp *so101Teleop) readyToStream(ctx context.Context, leader []referencefram return true, nil } +func (tp *so101Teleop) latched() bool { + tp.mu.Lock() + defer tp.mu.Unlock() + return tp.streaming +} + func (tp *so101Teleop) DoCommand(ctx context.Context, cmd map[string]interface{}) (map[string]interface{}, error) { switch cmd["command"] { case "start": From 20b5577d525a5d4835c2adc6ce9648a543f505a9 Mon Sep 17 00:00:00 2001 From: HipsterBrown Date: Wed, 2 Sep 2026 20:37:09 -0400 Subject: [PATCH 14/14] docs: cut the commentary down to what a reader cannot infer The measurements belong in docs/arm.md and the invariants in CLAUDE.md; several code comments had grown into a third copy of both. Trims those to the claim plus a pointer, and tightens the arm.md and teleop.md prose to the same. No behaviour change. Co-Authored-By: Claude Opus 5 (1M context) Claude-Session: https://claude.ai/code/session_01NHef7PBha9FHmWoCSduR8J --- components/arm/gains_test.go | 12 +++++------- components/arm/motion.go | 20 ++++++-------------- components/arm/streamed_setpoint_test.go | 21 +++++++-------------- docs/arm.md | 14 ++++++-------- docs/teleop.md | 2 +- internal/servo/motion_profile.go | 10 ++++------ internal/servo/speed.go | 12 +++++------- internal/testfake/fake_transport.go | 19 ++++++++----------- services/teleop/teleop.go | 24 ++++++++---------------- 9 files changed, 50 insertions(+), 84 deletions(-) diff --git a/components/arm/gains_test.go b/components/arm/gains_test.go index 741ad9a..14bb443 100644 --- a/components/arm/gains_test.go +++ b/components/arm/gains_test.go @@ -67,10 +67,9 @@ func (o *orderRecordingTransport) Write(p []byte) (int, error) { return o.FakeTransport.Write(p) } -// Damping is written before the integral term, and the integral term before proportional. 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. Reading final register values cannot -// catch this -- they are order-independent -- so it needs the wire. +// 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) @@ -176,9 +175,8 @@ func TestApplyServoGainsFailureDoesNotFailInit(t *testing.T) { // 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") - // The failure IS terminal for the rest of servo 2's gains, on purpose: 48/32/4 -- I and - // P applied over a stock D -- is the combination the write order exists to prevent, and - // it would persist in EEPROM with no retry to clear it. + // 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. diff --git a/components/arm/motion.go b/components/arm/motion.go index c2e3f8a..f5e4955 100644 --- a/components/arm/motion.go +++ b/components/arm/motion.go @@ -347,9 +347,8 @@ func (s *so101) dwellUntilNear(ctx context.Context, goal []float64, lookaheadDeg // travel-reference read fails and as the streamed-setpoint path. It commands every joint at // one shared speed, which is why joints arrive at different times. // -// streamed forces BOTH registers to 0 -- max speed, unlimited acceleration -- instead of the -// deg/s and deg/s^2 conversions. The teleop bypass in MoveToJointPositions is the only caller -// that sets it true. See CLAUDE.md, "Speed: 0 means MAXIMUM". +// streamed forces BOTH registers to 0 -- max speed, unlimited acceleration. See CLAUDE.md, +// "A streamed setpoint gets Speed 0 AND Acc 0". func (s *so101) moveJointsUniform(ctx context.Context, to []float64, speedDegsPerSec, accelDegsPerSecSq float64, streamed, wait bool) error { // Writes an explicit UNIFORM profile rather than a bare position write. A coordinated // move leaves each servo's Speed and Acc registers at its own scaled value -- as low as @@ -361,12 +360,7 @@ func (s *so101) moveJointsUniform(ctx context.Context, to []float64, speedDegsPe AccUnits: servo.DegPerSecSqToAccUnits(accelDegsPerSecSq), } if streamed { - // Both registers to their "unbounded" sentinel: 0 is MAX SPEED and UNLIMITED - // acceleration, not stopped and not still. A streamed caller shapes the trajectory - // with its own update rate, so any profile the servo applies between setpoints is - // pure lag -- confirmed on hardware in both directions, first for acceleration and - // then for speed, where raising the cap improved teleop tracking until it stopped - // binding at all. See the branch in MoveToJointPositions. + // 0 is MAX SPEED and UNLIMITED acceleration, not stopped and not still. profile.SpeedSteps, profile.AccUnits = 0, 0 } profiles := make([]controller.ServoProfile, len(s.armServoIDs)) @@ -406,11 +400,9 @@ func (s *so101) MoveToJointPositions(ctx context.Context, positions []referencef accel := float64(s.defaultAcc) s.mu.RUnlock() - // A streamed caller (teleop) gets no speed cap, no acceleration ramp, and no - // coordination read. An acceleration limit is catastrophic for tracking -- measured, - // acc=32 quadrupled RMS error and took lag from 40 ms to 160 ms -- a speed cap binds - // the same way once the ramp stops dominating, and coordinated arrival is meaningless - // for a goal that will be superseded in ~10 ms. See docs/arm.md, "Motion and speed". + // A streamed caller shapes the trajectory with its own update rate, so any profile the + // servo applies between setpoints is pure lag and the coordination read buys nothing. + // See docs/arm.md, "Motion and speed". if streamed, _ := extra["streamed"].(bool); streamed { return s.moveJointsUniform(ctx, clamped, speed, accel, true, servocmd.WaitArg(extra)) } diff --git a/components/arm/streamed_setpoint_test.go b/components/arm/streamed_setpoint_test.go index e217170..2e31caa 100644 --- a/components/arm/streamed_setpoint_test.go +++ b/components/arm/streamed_setpoint_test.go @@ -29,16 +29,13 @@ func accelTestArm(t *testing.T, ft *testfake.FakeTransport) *so101 { } } -// Teleop streams small setpoints, so any profile the servo applies between them is pure lag: -// an acceleration ramp quadruples tracking error (acc=32 -> 160 ms lag vs 40 ms at acc=0), -// and a speed cap binds the same way once the ramp stops dominating. 0 in either register is -// the "unbounded" sentinel -- MAX SPEED and UNLIMITED acceleration -- which neither -// conversion path can produce (minSpeedSteps and MinAccUnits are both 1). +// 0 in either register is the "unbounded" sentinel -- MAX SPEED and UNLIMITED acceleration -- +// which neither conversion path can produce (minSpeedSteps and MinAccUnits are both 1). func TestMoveToJointPositionsHonoursStreamed(t *testing.T) { ft := testfake.NewFakeTransport() s := accelTestArm(t, ft) - // Seed NON-zero values first. An unset register reads back as zero, so without this the - // assertions below cannot tell "commanded 0" from "never wrote the register at all". + // Seed NON-zero: an unset register reads back as zero, so without this the assertions + // cannot tell "commanded 0" from "never wrote the register". for _, id := range s.armServoIDs { ft.SetRegister(id, feetech.RegAcceleration.Address, []byte{0x7F}) ft.SetRegister(id, feetech.RegGoalVelocity.Address, testfake.EncodeWordLE(1234)) @@ -55,15 +52,12 @@ func TestMoveToJointPositionsHonoursStreamed(t *testing.T) { got, err := s.controller.ReadServoRegister(context.Background(), id, "acceleration") require.NoError(t, err) assert.Equal(t, byte(0), got[0], "servo %d must be commanded at unlimited acceleration", id) - // 0 here is the servo's MAX SPEED sentinel, not "stopped". A cap binds teleop - // tracking once the ramp stops dominating -- found on hardware. vel, err := s.controller.ReadServoRegister(context.Background(), id, "goal_velocity") require.NoError(t, err) assert.Equal(t, []byte{0, 0}, vel, "servo %d must be commanded at max speed", id) } - // One SyncWrite and nothing else. Skipping the coordination read is half the point of - // the branch -- at 100 Hz the read plus the write is ~3 ms of a 10 ms budget -- and it - // leaves no trace in the register store, so only the packet count can pin it. + // Skipping the coordination read leaves no trace in the register store, so only the + // packet count can pin it. assert.Equal(t, 1, moved, "the unlimited path must issue one packet: the goal write") } @@ -86,7 +80,6 @@ func TestMoveToJointPositionsProfilesByDefault(t *testing.T) { require.NoError(t, err) assert.NotEqual(t, []byte{0, 0}, vel, "servo %d must keep its speed cap", id) } - // SyncRead for the travel reference, then SyncWrite. This is the traffic the unlimited - // path exists to avoid. + // SyncRead for the travel reference, then SyncWrite. assert.Equal(t, 2, moved, "the coordinated path reads before it writes") } diff --git a/docs/arm.md b/docs/arm.md index 9df302b..d7061e8 100644 --- a/docs/arm.md +++ b/docs/arm.md @@ -467,9 +467,9 @@ Passing both `goal_metric_type` and `pose_cloud` in the same `extra` map is an e ## Servo gains -`disable_servo_gains` (bool, default `false`) leaves the servos' `p_gain`/`d_gain`/`i_gain` registers untouched at init. Leave it `false` to get the tuned table below; set it if you have tuned gains of your own (`pidtune apply` writes the same registers this module does). +`disable_servo_gains` (bool, default `false`) leaves the servos' `p_gain`/`d_gain`/`i_gain` registers untouched at init. Set it if you tuned gains yourself — `pidtune apply` writes the same registers. -The tuned per-joint table, against a stock `32/32/0` (gripper `16/32/0`): +The tuned table, against a stock `32/32/0` (gripper `16/32/0`): | servo id | joint | P | D | I | |---|---|---|---|---| @@ -480,15 +480,13 @@ The tuned per-joint table, against a stock `32/32/0` (gripper `16/32/0`): | 5 | wrist roll | 48 | 48 | 0 | | 6 | gripper | *untested — never written* | | | -These are EEPROM writes, applied during `doServoInitialization` (so on every `Reconfigure`, not just the first construction), and skipped register-by-register when the servo already holds the target value — both because EEPROM has finite write endurance and because `initializeServosWithRetry` can run this same path up to 3 times in one bring-up. They are never sent to a servo outside the arm's own `servo_ids`: a 4-DOF arm configured with `servo_ids: [1,2,3,4]` leaves servo 5 alone even though the table has an entry for it. A gain write failing does not fail arm construction — the arm comes up at whatever gains the servo already holds. +Stock `P=32` is too weak for the gravity-loaded joints: at `I=0` they park at a permanent steady-state error scaling as `1/P`, and only integral gain removes it. On joint 2 the error drops ~4×, **0.90° → 0.22°**. The gravity-free joints need no integral term. Nothing exceeds `P=48` — `P=64` oscillated at every `D` tested. -**Why this table.** Stock `P=32` is too weak to hold the gravity-loaded joints (shoulder lift, elbow) against load: at `I=0` they sit at a permanent, direction-independent steady-state error that scales as `1/P`. Adding integral gain removes it. Measured on joint 2: steady-state error drops roughly **4×, 0.90° → 0.22°** (10.2 → 2.5 encoder counts, 0.088°/count). The gravity-free joints (pan, wrist roll) need no integral term and get only the derivative bump. +These are EEPROM writes applied during servo init, so they run on every `Reconfigure`; each register is skipped when the servo already holds the value. Servos outside the arm's own `servo_ids` are never written, and a failed write does not fail arm construction. -**The write order is D, then I, then 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` and was stable at `D=48, I=4` — so writing derivative first means every prefix of the sequence (`{D}`, `{D,I}`, `{D,I,P}`) is at least as stable as the state before it. If a write fails partway, the loop stops for that servo rather than continuing to the next register: continuing could land `{I,P}` without `D` — 48/32/4 on servo 3, the exact combination measured to oscillate — and strand it there in EEPROM with nothing to retry it (a gain-write failure never fails the arm, so nothing re-triggers this path outside the next `Reconfigure`). +Writes go D, then I, then P, and a failed one stops 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. -**Peak current rises steeply with `D`.** Taking a joint's `D` from 32 to 64 is not free: at `P=32`, a 100-count step measured 388 current units at `D=16` versus 691 at `D=48`. Servos idled at 37–43°C and rose only 1–2°C over the measurement session, so this was not a thermal problem there — but it would matter under sustained high-duty motion. - -**Measured on one arm, once.** 12.2–12.3 V bus, 2026-09-02. Unit-to-unit and supply-voltage variation are unquantified. If your arm needs different gains, set `disable_servo_gains: true` and run `pidtune apply` — it writes the same registers this module does. +Two caveats. Peak current rises steeply with `D` — a 100-count step drew 388 units at `D=16` versus 691 at `D=48`; not thermal in testing (37–43°C, +1–2°C), but it would matter under sustained high-duty motion. And this was measured on one arm at 12.2–12.3 V; unit-to-unit and voltage variation are unquantified. ## Manual Mode diff --git a/docs/teleop.md b/docs/teleop.md index b07c4dd..0cabaa6 100644 --- a/docs/teleop.md +++ b/docs/teleop.md @@ -86,7 +86,7 @@ were measured at unlimited acceleration, which is how teleop now drives the foll it only if the serial bus or host cannot keep up; the mirror loop's ticker drops ticks rather than backing up, so a rate the bus cannot sustain degrades silently to the rate it can. -`streaming` reports whether the mirror has switched from profiled moves to streamed setpoints. Teleop drives the follower with no speed cap and no acceleration ramp, which is what keeps tracking lag down — but that means a large position error is closed at full servo speed, so the mirror will not use a streamed setpoint until the follower is within 5° of the leader on every joint. Until then it mirrors with the arm's configured `speed_degs_per_sec` and acceleration, and `streaming` reads `false`. The switch latches for the session: a running mirror legitimately trails the leader, and dropping back would reintroduce the lag. Starting with the arms in very different poses is therefore safe, but the follower will travel to meet the leader at the configured speed as soon as the loop starts. +`streaming` reports whether the mirror has switched to streamed setpoints, which carry no speed cap and no acceleration ramp. Because that means a large position error is closed at full servo speed, the mirror uses the arm's configured speed and acceleration until the follower is within 5° of the leader, then switches and stays switched. Starting with the arms far apart is safe, but the follower will travel to meet the leader as soon as the loop starts. `cycles` is the total number of successful mirror cycles since the last start. `consecutive_errors` resets to zero on any successful cycle. When `max_consecutive_errors` is reached the loop sets `running` to `false` and records the triggering error in `last_error`. diff --git a/internal/servo/motion_profile.go b/internal/servo/motion_profile.go index a1fb454..12f6768 100644 --- a/internal/servo/motion_profile.go +++ b/internal/servo/motion_profile.go @@ -29,12 +29,10 @@ const ( // knees were 52, 55 and 63 across three joints; 50 sits under all of them. Above the // knee the register has no further effect, so scaling silently stops working. // - // UNRESOLVED: step-response data from 2026-09-02 disagrees. On joint 2, well past that - // knee, overshoot and peak current keep climbing (Acc 64 -> 96 -> 128: 2.5 -> 6.0 -> 7.8 - // counts, 274 -> 489 -> 560 units) where a saturated register would be flat. Most likely - // the two studies measured different things -- this calibration recovered arrival timing, - // that one endpoint overshoot -- i.e. the register may stop affecting when the joint - // arrives while still shaping its deceleration. Settle it before relying on either bound. + // UNRESOLVED: step-response data from 2026-09-02 disagrees -- past that knee, overshoot + // and peak current keep climbing where a saturated register would be flat. Likely the two + // studies measured different things (arrival timing vs deceleration shape). Settle it + // before relying on either bound. maxAccUnits = 50 ) diff --git a/internal/servo/speed.go b/internal/servo/speed.go index 220a735..1d70f58 100644 --- a/internal/servo/speed.go +++ b/internal/servo/speed.go @@ -43,10 +43,9 @@ const ( // fails in OPPOSITE directions depending on load, so no single correction recovers it. // // A tuning value, not a hardware constant -- measured on one arm at p_gain 32, which is NO -// LONGER what ships: the tuned table runs the gravity-loaded joints at p_gain 48 (docs/arm.md, -// "Servo gains"). Not re-derived against it. Higher P partly addresses the load-dependent -// direction error this floor exists for, so this is an OPPORTUNITY to lower it -- re-measure -// before doing so. See docs/arm.md, "Motion and speed". +// LONGER what ships (docs/arm.md, "Servo gains"). Higher P partly addresses the error this +// floor exists for, so it is likely lowerable; not re-derived, so re-measure first. +// See docs/arm.md, "Motion and speed". const MinExecutableSpeedDegsPerSec = 12.0 // Waypoint-lookahead bounds, in degrees: how far ahead of the arm the commanded goal is @@ -60,9 +59,8 @@ const ( // backstops instead. NOT sufficient alone -- see LookaheadDegFor for the other bound. // Measurements: docs/arm.md, "Waypoint streams". // - // The shipped gains cut that droop band ~4x (0.90 -> 0.22 deg on joint 2), so this floor - // is now wider than it needs to be -- a smaller one would track waypoints more tightly - // and dwell less. Not re-derived; do NOT shrink it on the arithmetic alone. + // The shipped gains cut that droop band ~4x, so this floor is now wider than it needs to + // be. Not re-derived; do NOT shrink it on the arithmetic alone. DefaultLookaheadDeg = 3.0 MinLookaheadDeg = 0.1 MaxLookaheadDeg = 45.0 diff --git a/internal/testfake/fake_transport.go b/internal/testfake/fake_transport.go index f23863b..98c5445 100644 --- a/internal/testfake/fake_transport.go +++ b/internal/testfake/fake_transport.go @@ -150,11 +150,10 @@ func (ft *FakeTransport) reply(req []byte) { ft.queue(int(raw), ft.value(int(raw), address, length)) } case 0x83: // SYNC_WRITE -- broadcast, no response on the wire - // Params are [address, dataLen, id, data..., id, data...]. A sync write covers a - // RANGE of registers (a goal write is acceleration, position, time and velocity in - // one 7-byte blob), so store the blob at `address` AND every suffix of it at the - // address it starts from. value() copies min(len, stored) bytes, so a later read of - // any register inside the range returns its own bytes rather than the blob's head. + // Params are [address, dataLen, id, data..., id, data...]. A sync write spans a RANGE + // of registers, so store every suffix of the blob at the address it starts from -- + // value() copies min(len, stored), so a read of any register inside the range then + // returns its own bytes rather than the blob's head. if len(params) < 2 { return } @@ -208,18 +207,16 @@ func (ft *FakeTransport) CloseCount() int { return ft.closes } -// PacketCount reports how many request packets have been written to the transport, so a -// test can assert a code path's bus traffic rather than only its effect -- a skipped read -// leaves no trace in the register store. +// PacketCount reports total request packets, so a test can assert a code path's bus traffic +// rather than only its effect -- a skipped read leaves no trace in the register store. func (ft *FakeTransport) PacketCount() int { ft.mu.Lock() defer ft.mu.Unlock() return ft.writes } -// WriteCount reports how many WRITE packets have landed on one register, so a test can -// assert a read-first guard actually skipped a redundant write -- reading the value back -// cannot distinguish "already correct" from "written again". +// WriteCount reports WRITE packets landed on one register, so a test can assert a read-first +// guard skipped a redundant write -- the value alone cannot show that. func (ft *FakeTransport) WriteCount(id int, address byte) int { ft.mu.Lock() defer ft.mu.Unlock() diff --git a/services/teleop/teleop.go b/services/teleop/teleop.go index e691f9a..01c8e47 100644 --- a/services/teleop/teleop.go +++ b/services/teleop/teleop.go @@ -30,13 +30,10 @@ const ( defaultMaxConsecutiveErrors = 10 maxTeleopRateHz = 1000.0 - // maxStreamStartGapDeg is how close the follower must get to the leader before the - // mirror switches from profiled moves to streamed setpoints. A streamed setpoint carries - // no speed cap and no acceleration ramp, so handing one a large gap closes it at full - // servo speed -- fine for the sub-degree deltas of a running mirror, not for the arms - // starting in different poses. 5 deg is comfortably above the arm's steady-state droop - // (~0.22 deg tuned, 0.9 deg stock) so it is reachable, and small enough that the switch - // happens at a speed a hand is already tracking. + // maxStreamStartGapDeg is how close the follower must get before the mirror switches to + // streamed setpoints, which carry no speed cap and no ramp. Above the arm's steady-state + // droop (~0.22 deg tuned) so it is reachable, small enough that the switch happens at a + // speed a hand is already tracking. maxStreamStartGapDeg = 5.0 ) @@ -200,8 +197,7 @@ func (tp *so101Teleop) syncOnce(ctx context.Context) error { gripperPct = pct } - // No speed cap, no acceleration ramp, no coordination read: the command stream itself - // shapes the trajectory, so anything the servo applies between setpoints is pure lag. + // No speed cap, no ramp, no coordination read once the follower has caught up. streamed, err := tp.readyToStream(ctx, positions) if err != nil { return err @@ -226,13 +222,9 @@ func (tp *so101Teleop) syncOnce(ctx context.Context) error { // readyToStream reports whether this cycle may use a streamed setpoint, latching true the // first time the follower is within maxStreamStartGapDeg of the leader on every joint. // -// The latch is the point. A streamed setpoint has no speed cap and no acceleration ramp, so -// it must not be handed a large gap -- and profiling only the FIRST cycle would not help, -// because a servo goal write supersedes the previous one: the next cycle 10 ms later would -// overwrite the profiled goal with an unbounded one while the arm was still far away. So the -// mirror stays profiled until the gap is actually closed. -// -// It costs one extra follower read per cycle, and only until the latch trips. +// It LATCHES because a servo goal write supersedes the previous one: profiling only the first +// cycle would be overwritten 10 ms later while the arm was still far away. Costs one extra +// follower read per cycle until it trips. See CLAUDE.md, "A streamed setpoint". func (tp *so101Teleop) readyToStream(ctx context.Context, leader []referenceframe.Input) (bool, error) { if tp.latched() { return true, nil