feat(arm): time-scheduled MoveThroughJointPositionsStreamed (rdk v1.6.0)

CLAUDE.md

@@ -591,3 +591,29 @@ calibration wizard. It is bundled into `module.tar.gz` and needs **Node ≥ 20**
   through `tolerateCondition`; a new read path must too. Since feetech-servo v0.7.1 *writes*
   also return nil on condition-only flags, nothing on main can infer "overloaded" from a
   write failure.
+- **`MoveThroughJointPositionsStreamed` is time-scheduled and UNBOUNDED; the 5° gate is what
+  makes that safe.** Each point goes out on the `streamed` path (`Speed 0 / Acc 0`, one
+  `SetGoals` packet) at `start + p.Time`, via `servo.Clock` so tests never sleep. Before the
+  first point, `closeStreamStartGap` (`components/arm/streamed.go`) reads once and runs one
+  profiled `moveJoints` if any joint is > `streamStartGapDeg` away -- the same 5 as teleop's
+  `maxStreamStartGapDeg`, duplicated because components must not import services. ONLY the
+  first point is gated; a mid-stream jump or a schedule slip reaches the servos unbounded, and
+  `TestStreamedDoesNotGateAMidStreamJump` pins that so a later policy is a visible change. Late
+  points are WRITTEN, not dropped (no policy has been measured). `Constraints` are ignored. Both
+  arms receive batches with a `select` on `ctx.Done()` rather than `range batches`, because
+  `Stop`'s `opMgr.CancelRunning` blocks until the op returns and cancelling our ctx does not
+  close the channel (the server's recv goroutine watches the stream ctx). Requires rdk ≥ v1.1.0
+  (the method is on `arm.Arm`); `FakeTransport.WriteCount` counts `SYNC_WRITE`s by start
+  address so tests can count goal writes -- `SetGoals` is a sync write at `RegAcceleration`.
+  A REMOTE caller is also validated by rdk's arm client against the model's static `DoF()`
+  limits (so101.json / URDF), point by point, and the stream is torn down client-side on a
+  violation -- before any point reaches the module's `clampPositions`, which clamps to the
+  CALIBRATED limits. Two different limit sets; in-process tests only ever see the second.
+- **rdk ≥ v1.6.0's `Frame.Transform` does NOT bounds-check joints** (`OOBErrString` is dead
+  upstream). On the WRITE path that makes `so101.clampPositions` the only joint-limit
+  enforcement in this module -- do not "simplify it away" on the assumption that rdk rejects an
+  out-of-limit input; it composes the full chain and returns a pose with no error. On the READ
+  path `geometry.ComputeOOBPosition`'s clamp was a workaround for the OLD truncate-and-error
+  behaviour and is now lossy: `EndPosition` for a joint drooped past its limit reports the pose
+  AT the limit where an unclamped `Transform` would be correct. `oob_test.go` now pins only that
+  clamp; removing it is a deliberate small follow-up, not an accident to guard against.

components/arm/arm.go

@@ -217,6 +217,9 @@ type so101 struct {
 	// servosMoving, when non-nil, replaces the bus read behind anyServoMoving. Tests only.
 	servosMoving func(context.Context) (bool, error)
 
+	// clock schedules streamed trajectory points. Zero value is the real clock; tests only.
+	clock servo.Clock
+
 	mu       sync.RWMutex
 	moveLock sync.Mutex
 	isMoving atomic.Bool

components/arm/oob_test.go

@@ -11,11 +11,10 @@ import (
 	"so_arm/internal/testfake"
 )
 
-// TestComputeOOBPositionFullChain guards against the rdk v0.123 regression where
-// Frame.Transform early-returns at the first out-of-bounds joint, yielding a pose
-// truncated at that joint (and every link downstream). geometry.ComputeOOBPosition must still
-// compose the full chain for a joint driven past its calibrated limit -- otherwise
-// EndPosition silently reports a wildly wrong TCP (the base-mount pose) with no error.
+// TestComputeOOBPositionFullChain pins that ComputeOOBPosition clamps an out-of-range joint
+// to its limit and composes the full chain. It began as a guard against rdk v0.123's
+// Frame.Transform truncating the pose at the first out-of-bounds joint; rdk >= v1.6.0
+// composes the full chain itself, so the clamp is now the only behavior this adds.
 func TestComputeOOBPositionFullChain(t *testing.T) {
 	for _, useURDF := range []bool{false, true} {
 		name := "json"
@@ -36,11 +35,6 @@ func TestComputeOOBPositionFullChain(t *testing.T) {
 			// pose truncated at joint 0.
 			base := []referenceframe.Input{0, 0.5, 0.3, 0.1, 0.1}
 
-			// Raw Transform surfaces the OOB error along with its truncated pose; the
-			// OOBErrString it carries is what previously got swallowed here.
-			_, err = model.Transform([]referenceframe.Input{limits[0].Max + 1.0, 0.5, 0.3, 0.1, 0.1})
-			require.ErrorContains(t, err, referenceframe.OOBErrString)
-
 			// Drive joint 0 just past its max limit.
 			overMax := make([]referenceframe.Input, 5)
 			copy(overMax, base)

components/arm/streamed.go

@@ -0,0 +1,149 @@
+package arm
+
+import (
+	"context"
+	"fmt"
+	"time"
+
+	"go.viam.com/rdk/components/arm"
+
+	"so_arm/internal/servo"
+)
+
+// streamStartGapDeg is how close the arm must already be to a stream's first point before
+// it is written on the unbounded (Speed 0 / Acc 0) path. Same value as teleop's
+// maxStreamStartGapDeg; not imported because components must not depend on services.
+const streamStartGapDeg = 5.0
+
+// streamSettleDwellMs bounds the dwell on a stream's last point. It exists only to get past
+// the ~2ms it takes Moving to rise after a goal write, so WaitForServosToStop does not read
+// the pre-write zero and return while the arm is still travelling.
+const streamSettleDwellMs = 250
+
+// MoveThroughJointPositionsStreamed writes each point as an unbounded goal at start + Time.
+// The producer owns the timing: a late point is written immediately, never dropped, and
+// Constraints are ignored.
+//
+// Only the FIRST point is gated. A large jump between consecutive points, or a schedule
+// slip that lets the goal race ahead of the arm, reaches the servos unbounded; no policy
+// for either has been measured yet -- see docs/arm.md, "Streaming trajectories". extra is
+// accepted and ignored.
+func (s *so101) MoveThroughJointPositionsStreamed(
+	ctx context.Context,
+	batches <-chan []arm.TrajectoryPoint,
+	responses chan<- arm.Response,
+	extra map[string]interface{},
+) error {
+	ctx, done := s.opMgr.New(ctx)
+	defer done()
+
+	s.mu.Lock()
+	s.exitManualLocked("motion command received")
+	s.mu.Unlock()
+
+	s.moveLock.Lock()
+	defer s.moveLock.Unlock()
+
+	s.isMoving.Store(true)
+	defer s.isMoving.Store(false)
+
+	s.mu.RLock()
+	speed := float64(s.defaultSpeed)
+	accel := float64(s.defaultAcc)
+	s.mu.RUnlock()
+
+	var start, wall time.Time
+	var gate, maxLate time.Duration
+	var last []float64
+	prev := time.Duration(-1) // no previous point yet
+	idx, late := 0, 0
+	for {
+		// Not `range batches`: Stop cancels ctx but cannot close the channel, and
+		// CancelRunning blocks until this returns.
+		var batch []arm.TrajectoryPoint
+		var ok bool
+		select {
+		case <-ctx.Done():
+			return ctx.Err()
+		case batch, ok = <-batches:
+		}
+		if !ok {
+			break
+		}
+		for _, p := range batch {
+			if err := servo.CheckTrajectoryTime(prev, p.Time); err != nil {
+				return err
+			}
+			clamped, _, err := s.clampPositions(p.Positions, true)
+			if err != nil {
+				return err
+			}
+			if idx == 0 {
+				wall = s.clock.Time()
+				if err := s.closeStreamStartGap(ctx, clamped, speed, accel); err != nil {
+					return err
+				}
+				start = s.clock.Time()
+				gate = start.Sub(wall)
+			}
+			due := start.Add(p.Time)
+			// >= 1ms, not > 0: point 0 is due at start and the clock is read again after it.
+			if behind := s.clock.Time().Sub(due); behind >= time.Millisecond {
+				late++
+				maxLate = max(maxLate, behind)
+			}
+			if err := s.clock.WaitUntil(ctx, due); err != nil {
+				return err
+			}
+			if err := s.moveJointsUniform(ctx, clamped, speed, accel, true, false); err != nil {
+				return err
+			}
+			prev, idx, last = p.Time, idx+1, clamped
+		}
+		if len(batch) == 0 {
+			continue
+		}
+		select {
+		case responses <- arm.Response{}:
+		case <-ctx.Done():
+			return ctx.Err()
+		}
+	}
+	if idx == 0 {
+		return nil
+	}
+	settleFrom := s.clock.Time()
+	// Dwell first: it consults Moving only from its second poll, which is what the paced path
+	// does to avoid ending a move on the pre-write zero.
+	lookahead := s.lookaheadDeg
+	if lookahead == 0 {
+		lookahead = servo.LookaheadDegFor(speed, accel)
+	}
+	_, err := s.dwellUntilNear(ctx, last, lookahead, speed, streamSettleDwellMs)
+	if err == nil {
+		err = s.controller.WaitForServosToStop(ctx, s.armServoIDs, servo.MaxMoveTimeoutMs)
+	}
+	now := s.clock.Time()
+	// One line per stream so a hardware run decomposes its wall time without a debugger.
+	s.logger.Infof("streamed %d points over %v: gate %v, late %d (max %v), settle %v, wall %v",
+		idx, prev, gate.Round(time.Millisecond), late, maxLate.Round(time.Millisecond),
+		now.Sub(settleFrom).Round(time.Millisecond), now.Sub(wall).Round(time.Millisecond))
+	return err
+}
+
+// closeStreamStartGap runs one blocking profiled move to `target` when any joint is more than
+// streamStartGapDeg away. The streamed path removes every bound, so the FIRST point must not
+// hand it a large position error.
+func (s *so101) closeStreamStartGap(ctx context.Context, target []float64, speed, accel float64) error {
+	current, err := s.currentJoints(ctx)
+	if err != nil {
+		return fmt.Errorf("failed to read positions before a streamed trajectory: %w", err)
+	}
+	_, gapDeg := servo.JointTravelsDeg(current, target)
+	if gapDeg <= streamStartGapDeg {
+		return nil
+	}
+	s.logger.Debugf("streamed trajectory starts %.1f deg away; profiled move to its first point", gapDeg)
+	_, err = s.moveJoints(ctx, current, target, speed, accel, nil, true)
+	return err
+}