diff --git a/CLAUDE.md b/CLAUDE.md index 601132b..1f228be 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -526,3 +526,49 @@ calibration wizard. It is bundled into `module.tar.gz` and needs **Node ≥ 20** be mistaken for having done anything. `abort`'s advertised states are unchanged — only the guard was added. One gap remains by choice: `error` still accepts `abort` without advertising it, which is harmless because `reset` is the escape hatch offered there. +- **Compliance zeroes `i_gain`, not just `p_gain` — and a failed READ falls through to the + WRITE, not away from it.** The integral path is independent of P, so lowering P to 8 during + hand-guided compliance used to leave a live integrator; inert until the tuned gains carry a + non-zero I (`internal/servo/gains.go`), which they now do. Gated on `p_gain > 0`, the + documented per-register opt-out; `d_gain` is deliberately left alone (reasoned drag, never + measured). Both `applyCompliance`/`restoreCompliance` and `applyServoGains` route every gain + write through one `writeGainIfChanged` (`components/arm/gains.go`) because these are EEPROM + registers entered/exited (or reconfigured) repeatedly — and it treats a read it cannot trust + as unable to prove a no-op, so it falls through to the write rather than skipping it. The + asymmetry is deliberate: `applyCompliance` failing outright leaves the arm stiff, which is + safe; `restoreCompliance` skipping a write on a flaky read would leave a joint at `p_gain + 8`/`i_gain 0` in EEPROM — soft, across power cycles — on a transient this hardware really + produces, with nothing retrying it. Don't make either path bail out on a read error. +- **Tuned gains are written during `doServoInitialization`, in D→I→P order, and a failed WRITE + is terminal for that servo.** `applyServoGains` BREAKS its per-servo loop on the first write + failure rather than logging and continuing: without the break, {I,P}-without-D is reachable — + 48/32/4 on servo 3, the exact combination measured to oscillate on a gravity-loaded joint — + and it would be stranded in EEPROM with nothing to retry it (`applyServoGains` returns no + error, so `initializeServosWithRetry` never re-runs for a gain failure alone). The read-first + skip is the EEPROM wear guard, and it runs on every `Reconfigure` as well as up to 3× within + one bring-up. `servo.Gains` fields are `byte`, not `int`, so an out-of-range retune is a + compile error rather than a silent truncation to less damping. Config is one + `disable_servo_gains` bool; there are no per-joint overrides — `pidtune apply` is the + escape hatch for a differently-tuned arm. +- **A streamed setpoint gets Speed 0 AND Acc 0 AND no coordination read; a profiled move gets + none of the three.** The `"streamed": true` key in `MoveToJointPositions`'s `extra` map + (`components/arm/motion.go`) branches to `moveJointsUniform` before the `currentJoints()` read + and forces both registers to their unbounded sentinel — 0 is MAXIMUM speed and UNLIMITED + acceleration, not stopped and not still. One principle covers all three: a streamed caller + shapes the trajectory with its own update rate, so any profile the servo applies *between* + setpoints is pure lag, and coordinated arrival is meaningless for a goal superseded in ~10ms. + Both halves were confirmed on hardware, acceleration first (acc=32 quadrupled RMS error and + took lag 40ms→160ms) and speed second — raising `speed_degs_per_sec` kept improving teleop + tracking until the cap stopped binding, which is what motivated removing it. `minSpeedSteps` + and `MinAccUnits` are both still 1, so the profiled path cannot reach either sentinel: a low + *configured* value is not a substitute (Acc 1 delivers ~43 deg/s², worse than the acc=32 row). + Measured in tests: one bus packet per call instead of two. Do NOT unify the two paths, and do + not split `streamed` back into per-register flags — nothing wants them separately, and the + read-skip belongs to the same decision. **It removes a bound**, so a caller must not hand it a large + position error. Teleop gates itself: `readyToStream` keeps the mirror on profiled moves + until the follower is within `maxStreamStartGapDeg` of the leader, then LATCHES. The latch + is the whole point -- profiling only the first cycle would be pointless, because a servo + goal write supersedes the previous one, so the next cycle 10ms later would overwrite the + profiled goal with an unbounded one while the arm was still far away. It costs one extra + follower read per cycle, only until it trips, and it must NOT re-evaluate afterwards: a + running mirror legitimately trails the leader by more than the gate. 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 new file mode 100644 index 0000000..2e3dce1 --- /dev/null +++ b/components/arm/gains.go @@ -0,0 +1,50 @@ +package arm + +import ( + "context" + + "go.viam.com/rdk/logging" + + "so_arm/internal/controller" + "so_arm/internal/servo" +) + +// writeGainIfChanged writes a one-byte gain register unless it can prove the write would +// change nothing. These are EEPROM registers and both callers rewrite the same values +// repeatedly, so the skip is a wear guard. +// +// A FAILED read falls through to the write: it cannot prove the write is unnecessary, and +// skipping one on restoreCompliance's path leaves a joint soft in EEPROM. See CLAUDE.md, +// "Compliance zeroes i_gain". +func writeGainIfChanged(ctx context.Context, h *controller.ControllerHandle, id int, name string, want byte) error { + if cur, err := h.ReadServoRegister(ctx, id, name); err == nil && len(cur) == 1 && cur[0] == want { + return nil + } + return h.WriteServoRegister(ctx, id, name, []byte{want}) +} + +// applyServoGains writes the tuned P/D/I table to the arm's own servos. +// +// D, then I, then P, and a failed write BREAKS out for that servo: a non-zero I is only +// stable against a raised D, so damping-first makes every prefix of the sequence at least as +// stable as the state before it -- but only if a failure is terminal, or {I,P}-without-D +// strands a gravity-loaded joint on a combination measured to oscillate. Never returns an +// error; the arm works at stock gains. See docs/arm.md, "Servo gains". +func applyServoGains(ctx context.Context, h *controller.ControllerHandle, ids []int, gains map[int]servo.Gains, logger logging.Logger) { + for _, id := range ids { + g, ok := gains[id] + if !ok { + continue + } + for _, w := range []struct { + name string + want byte + }{{"d_gain", g.D}, {"i_gain", g.I}, {"p_gain", g.P}} { + if err := writeGainIfChanged(ctx, h, id, w.name, w.want); err != nil { + logger.Warnf("servo %d: %s write failed; leaving it and this servo's "+ + "remaining gains as found: %v", id, w.name, err) + break + } + } + } +} diff --git a/components/arm/gains_test.go b/components/arm/gains_test.go new file mode 100644 index 0000000..14bb443 --- /dev/null +++ b/components/arm/gains_test.go @@ -0,0 +1,197 @@ +package arm + +import ( + "context" + "syscall" + "testing" + + "github.com/hipsterbrown/feetech-servo/feetech" + "github.com/stretchr/testify/assert" + "github.com/stretchr/testify/require" + "go.viam.com/rdk/logging" + + "so_arm/internal/servo" + "so_arm/internal/testfake" +) + +func gainsTestArm(t *testing.T, ft feetech.Transport, cfg *SO101ArmConfig) *so101 { + t.Helper() + return &so101{ + logger: logging.NewTestLogger(t), + controller: testArmHandle(t, ft), + armServoIDs: []int{1, 2, 3, 4, 5}, + cfg: cfg, + initCtx: context.Background(), + } +} + +func seedGains(ft *testfake.FakeTransport, p, d, i byte, ids ...int) { + for _, id := range ids { + ft.SetRegister(id, feetech.RegPGain.Address, []byte{p}) + ft.SetRegister(id, feetech.RegDGain.Address, []byte{d}) + ft.SetRegister(id, feetech.RegIGain.Address, []byte{i}) + } +} + +func TestServoInitializationWritesTheTunedGains(t *testing.T) { + ft := testfake.NewFakeTransport() + seedGains(ft, 32, 32, 0, 1, 2, 3, 4, 5, 6) + s := gainsTestArm(t, ft, &SO101ArmConfig{}) + + require.NoError(t, s.initializeServos()) + + assert.Equal(t, byte(48), readServoGain(t, s, 2, "p_gain")) + assert.Equal(t, byte(64), readServoGain(t, s, 2, "d_gain")) + assert.Equal(t, byte(2), readServoGain(t, s, 2, "i_gain")) + assert.Equal(t, byte(4), readServoGain(t, s, 3, "i_gain")) + // Servo 1 is the only P that is not 48, servo 5 the only D that is not 64: a + // transcription slip in either is caught by nothing else. + assert.Equal(t, byte(32), readServoGain(t, s, 1, "p_gain")) + assert.Equal(t, byte(48), readServoGain(t, s, 5, "d_gain")) +} + +// orderRecordingTransport records the order of gain writes to one servo at the wire level. +type orderRecordingTransport struct { + *testfake.FakeTransport + id int + seen []byte +} + +func (o *orderRecordingTransport) Write(p []byte) (int, error) { + // Packet: [0xFF, 0xFF, id, len, instruction, params..., checksum]. WRITE is 0x03. + if len(p) >= 7 && p[0] == 0xFF && p[1] == 0xFF && int(p[2]) == o.id && p[4] == 0x03 && + (p[5] == feetech.RegPGain.Address || p[5] == feetech.RegDGain.Address || + p[5] == feetech.RegIGain.Address) { + o.seen = append(o.seen, p[5]) + } + return o.FakeTransport.Write(p) +} + +// A non-zero I is only stable against a raised D, so any other order leaves a window where a +// bus failure strands the servo above its stable I. Final register values are +// order-independent, so this needs the wire. +func TestApplyServoGainsWritesDampingBeforeIntegral(t *testing.T) { + ft := testfake.NewFakeTransport() + seedGains(ft, 32, 32, 0, 1, 2, 3, 4, 5, 6) + rec := &orderRecordingTransport{FakeTransport: ft, id: 3} + + require.NoError(t, gainsTestArm(t, rec, &SO101ArmConfig{}).initializeServos()) + + assert.Equal(t, []byte{ + feetech.RegDGain.Address, feetech.RegIGain.Address, feetech.RegPGain.Address, + }, rec.seen, "gains must be written D, then I, then P") +} + +func TestServoInitializationRespectsDisableServoGains(t *testing.T) { + ft := testfake.NewFakeTransport() + seedGains(ft, 32, 32, 0, 1, 2, 3, 4, 5) + s := gainsTestArm(t, ft, &SO101ArmConfig{DisableServoGains: true}) + + require.NoError(t, s.initializeServos()) + + for _, id := range []int{1, 2, 3, 4, 5} { + for _, reg := range gainRegisters() { + assert.Zero(t, ft.WriteCount(id, reg), "servo %d register %d must not be written", id, reg) + } + } +} + +// The invariant is "no servo outside this arm's own ids", not "not servo 6": a 4-DOF arm +// configured with servo_ids [1,2,3,4] hosts a gripper on servo 5, and DefaultArmGains[5] +// exists. +func TestApplyServoGainsTouchesOnlyTheArmsOwnServos(t *testing.T) { + ft := testfake.NewFakeTransport() + seedGains(ft, 32, 32, 0, 1, 2, 3, 4, 5, 6) + s := gainsTestArm(t, ft, &SO101ArmConfig{}) + s.armServoIDs = []int{1, 2, 3, 4} + + require.NoError(t, s.initializeServos()) + + for _, id := range []int{5, 6} { + for _, reg := range gainRegisters() { + assert.Zero(t, ft.WriteCount(id, reg), "servo %d is not this arm's, register %d", id, reg) + } + } +} + +// EEPROM has finite write endurance and initializeServosWithRetry runs this up to 3 times. +func TestApplyServoGainsSkipsNoOpWrites(t *testing.T) { + ft := testfake.NewFakeTransport() + for id, g := range servo.DefaultArmGains { + seedGains(ft, byte(g.P), byte(g.D), byte(g.I), id) + } + s := gainsTestArm(t, ft, &SO101ArmConfig{}) + + require.NoError(t, s.initializeServos()) + + for _, id := range []int{1, 2, 3, 4, 5} { + for _, reg := range gainRegisters() { + assert.Zero(t, ft.WriteCount(id, reg), + "servo %d register %d was already correct", id, reg) + } + } +} + +// failingWriteTransport wraps a FakeTransport and fails exactly the FIRST (servo, register) +// WRITE packet at the wire level -- everything else (PING, SYNC_READ, reads, the EEPROM +// unlock/relock writes to the lock register, writes to any other register or servo, and a +// later write to the SAME register once failsLeft is exhausted) passes through untouched. +// +// The failure is injected on the WRITE, not the read: writeGainIfChanged deliberately falls +// through to the write when a read fails, so a read failure lands the gain anyway and would +// make this test vacuous. Failing only once, not forever, is what lets the test both trigger +// the warn-and-continue path during init AND read the register back afterward. The ping and +// position read go over the SAME transport and must keep succeeding -- that is the proof +// this exercises the real contract rather than a transport that is simply broken. +type failingWriteTransport struct { + *testfake.FakeTransport + id int + address byte + failsLeft int +} + +func (f *failingWriteTransport) Write(p []byte) (int, error) { + // Packet: [0xFF, 0xFF, id, len, instruction, params..., checksum]. WRITE is + // instruction 0x03 with params [address, data...]. + if f.failsLeft > 0 && len(p) >= 7 && p[0] == 0xFF && p[1] == 0xFF && + int(p[2]) == f.id && p[4] == 0x03 && p[5] == f.address { + f.failsLeft-- + return 0, syscall.EIO + } + return f.FakeTransport.Write(p) +} + +// A gain write failing must not fail initialization: the arm works at stock gains. Only +// servo 2's d_gain WRITE fails; ping (PING) and the position read (SYNC_READ) use different +// instructions and are unaffected, so their continued success is the proof this exercises +// the real warn-and-continue path and not a transport that is simply broken outright. +func TestApplyServoGainsFailureDoesNotFailInit(t *testing.T) { + ft := testfake.NewFakeTransport() + seedGains(ft, 32, 32, 0, 1, 2, 3, 4, 5, 6) + failing := &failingWriteTransport{FakeTransport: ft, id: 2, address: feetech.RegDGain.Address, failsLeft: 1} + s := gainsTestArm(t, failing, &SO101ArmConfig{}) + + require.NoError(t, s.initializeServos()) + + // Servo 2's d_gain write failed, so it must be left exactly as found. + assert.Equal(t, byte(32), readServoGain(t, s, 2, "d_gain"), "servo 2 d_gain must be left as found after a write failure") + // Terminal for the rest of servo 2's gains on purpose: 48/32/4 is the combination the + // write order exists to prevent, and it would persist in EEPROM with no retry. + assert.Equal(t, byte(0), readServoGain(t, s, 2, "i_gain"), "servo 2 i_gain must not land over a stock d_gain") + assert.Equal(t, byte(32), readServoGain(t, s, 2, "p_gain"), "servo 2 p_gain must not land over a stock d_gain") + // It must NOT cascade to any other servo. + assert.Equal(t, byte(64), readServoGain(t, s, 3, "d_gain"), "servo 3 d_gain must still land") + assert.Equal(t, byte(4), readServoGain(t, s, 3, "i_gain"), "servo 3 i_gain must still land") +} + +func gainRegisters() []byte { + return []byte{feetech.RegPGain.Address, feetech.RegDGain.Address, feetech.RegIGain.Address} +} + +func readServoGain(t *testing.T, s *so101, id int, name string) byte { + t.Helper() + got, err := s.controller.ReadServoRegister(context.Background(), id, name) + require.NoError(t, err) + require.Len(t, got, 1) + return got[0] +} diff --git a/components/arm/manual.go b/components/arm/manual.go index 1789e1e..da53d7f 100644 --- a/components/arm/manual.go +++ b/components/arm/manual.go @@ -221,10 +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 - 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 { @@ -232,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{}, origTorque: map[int]int{}} + return &controllerManualIO{controller: c, ids: ids, limits: lm, orig: map[int]savedRegs{}} } func (a *controllerManualIO) servoIDs() []int { return a.ids } @@ -272,18 +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{} - 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) + // 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(cur) != 1 { - return fmt.Errorf("unexpected p_gain width %d for servo %d", len(cur), id) + if s.iGain, err = a.readGainByte(ctx, id, "i_gain"); err != nil { + return err } - pg[id] = int(cur[0]) } if torqueLimit > 0 { cur, err := a.controller.ReadServoRegister(ctx, id, "torque_limit") @@ -293,14 +300,19 @@ 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.origTorque = tq + a.orig, a.savedGains, a.savedTorque = orig, pGain > 0, torqueLimit > 0 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 } } @@ -316,20 +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 := a.controller.WriteServoRegister(ctx, id, "p_gain", []byte{byte(v)}); err != nil && firstErr == nil { + keep := func(err error) { + if err != nil && firstErr == nil { firstErr = err } } - 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 + 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))) + } + 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 new file mode 100644 index 0000000..e42a076 --- /dev/null +++ b/components/arm/manual_compliance_test.go @@ -0,0 +1,90 @@ +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.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)) + } +} + +// 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] +} diff --git a/components/arm/motion.go b/components/arm/motion.go index a0e6206..f5e4955 100644 --- a/components/arm/motion.go +++ b/components/arm/motion.go @@ -343,10 +343,13 @@ 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 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. 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 // 1 step/s and Acc 1 -- and a position-only write does not touch either. So without @@ -356,6 +359,10 @@ func (s *so101) moveJointsUniform(ctx context.Context, to []float64, speedDegsPe SpeedSteps: servo.DegPerSecToStepsPerSec(speedDegsPerSec), AccUnits: servo.DegPerSecSqToAccUnits(accelDegsPerSecSq), } + if streamed { + // 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)) for i := range profiles { profiles[i] = profile @@ -393,6 +400,13 @@ func (s *so101) MoveToJointPositions(ctx context.Context, positions []referencef accel := float64(s.defaultAcc) s.mu.RUnlock() + // 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)) + } + // 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 @@ -400,15 +414,16 @@ 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 "+ "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/streamed_setpoint_test.go b/components/arm/streamed_setpoint_test.go new file mode 100644 index 0000000..2e31caa --- /dev/null +++ b/components/arm/streamed_setpoint_test.go @@ -0,0 +1,85 @@ +package arm + +import ( + "context" + "testing" + + "github.com/hipsterbrown/feetech-servo/feetech" + + "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, + } +} + +// 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: 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)) + } + 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, "streamed": 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) + 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) + } + // 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") +} + +// 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} + + 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) + 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. + assert.Equal(t, 2, moved, "the coordinated path reads before it writes") +} diff --git a/docs/arm.md b/docs/arm.md index bbe7c60..d7061e8 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,23 @@ 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 `streamed` + +`MoveToJointPositions` accepts two optional booleans in its `extra` map: + +```json +{ "wait": false, "streamed": true } +``` + +- **`wait`** (default `true`) — block until the move settles, or return as soon as the goal is written. +- **`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. + +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). + +A plain point-to-point move should keep all three; do not set `streamed` there. + +**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 `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 +465,29 @@ 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. Set it if you tuned gains yourself — `pidtune apply` writes the same registers. + +The tuned 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* | | | + +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. + +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. + +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. + +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 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. diff --git a/docs/teleop.md b/docs/teleop.md index fb7479e..0cabaa6 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,18 @@ 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. + +`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`. ## Notes diff --git a/internal/controller/manager.go b/internal/controller/manager.go index 30b8c8d..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. Never 0: that means UNLIMITED. + 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/servo/gains.go b/internal/servo/gains.go new file mode 100644 index 0000000..633cda8 --- /dev/null +++ b/internal/servo/gains.go @@ -0,0 +1,26 @@ +package servo + +// 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 byte + D byte + I byte +} + +// 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..7b90597 --- /dev/null +++ b/internal/servo/gains_test.go @@ -0,0 +1,33 @@ +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) + } + } + // 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") + } +} diff --git a/internal/servo/motion_profile.go b/internal/servo/motion_profile.go index 71d4ef1..12f6768 100644 --- a/internal/servo/motion_profile.go +++ b/internal/servo/motion_profile.go @@ -28,6 +28,11 @@ 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 -- 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 33c6a3c..1d70f58 100644 --- a/internal/servo/speed.go +++ b/internal/servo/speed.go @@ -42,8 +42,10 @@ 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 (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 @@ -56,6 +58,9 @@ 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, 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 bb8ea02..98c5445 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,25 @@ 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...]. 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 + } + address, n := params[0], int(params[1]) + for i := 2; i+1+n <= len(params); 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:]...) + } + } } } @@ -183,6 +207,22 @@ func (ft *FakeTransport) CloseCount() int { return ft.closes } +// 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 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() + 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]) + } +} diff --git a/services/teleop/teleop.go b/services/teleop/teleop.go index 2144294..01c8e47 100644 --- a/services/teleop/teleop.go +++ b/services/teleop/teleop.go @@ -5,22 +5,36 @@ 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") 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 + + // 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 ) func init() { @@ -88,6 +102,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( @@ -180,7 +197,13 @@ func (tp *so101Teleop) syncOnce(ctx context.Context) error { gripperPct = pct } - if err := tp.followerArm.MoveToJointPositions(ctx, positions, map[string]interface{}{"wait": false}); err != nil { + // 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 + } + if err := tp.followerArm.MoveToJointPositions(ctx, positions, + map[string]interface{}{"wait": false, "streamed": streamed}); err != nil { return fmt.Errorf("write follower joints: %w", err) } @@ -196,6 +219,43 @@ 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. +// +// 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 + } + + 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) 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": @@ -215,6 +275,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"]) @@ -236,6 +297,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 f8206f3..434fe2e 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{} + moveStreamed 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.moveStreamed = extra["streamed"] return nil } @@ -116,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) @@ -125,12 +127,13 @@ 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.moveStreamed, test.ShouldEqual, true) test.That(t, fg.setPct, test.ShouldResemble, []float64{42}) }) 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) @@ -214,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 @@ -239,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")}