Skip to content

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

Merged
Nick Hehr (HipsterBrown) merged 20 commits into
mainfrom
streamed-trajectory
Sep 14, 2026
Merged

Nick Hehr (HipsterBrown) merged 20 commits into
mainfrom
streamed-trajectory

Conversation

@HipsterBrown

@HipsterBrown Nick Hehr (HipsterBrown) commented Sep 4, 2026 •

Copy link
Copy Markdown
Contributor

Why

rdk ≥ v1.1.0 adds MoveThroughJointPositionsStreamed to arm.Arm: a stream of time-stamped TrajectoryPoint batches. Every in-tree rdk arm ignores Time and loops points. This module already knows that pacing by position is not the same as following a producer's timing (the streamed setpoint path teleop uses), so this implements the RPC time-scheduled, and it was measured on hardware before un-drafting.

What changed

  • rdk v1.0.0 → v1.6.0 (go 1.25.10, api v0.1.577, utils v0.10.1). The only compile break was the new interface method. referenceframe/pose_cloud.go, transformable.go, model_urdf.go, KinematicModelToProtobuf, getLocalFrameSystemParts and the gripper Kinematics() contract are byte-identical across the bump, so every CLAUDE.md invariant about PoseCloud/URDF/module-boundary behaviour still holds. One behavioural change: Frame.Transform no longer bounds-checks joints (OOBErrString is dead upstream). oob_test.go drops the assertion that pinned the old bug; so101.clampPositions is now the only write-path limit enforcement, and geometry.ComputeOOBPosition's read-path clamp is a lossy leftover (follow-up).
  • Hardware arm (components/arm/streamed.go): each point is written as an unbounded goal (Speed 0 / Acc 0, one SetGoals packet, no read in the loop) at start + p.Time through a servo.Clock seam. 5° first-point gate: one position read; if any joint is farther than that, one blocking profiled move first, then the clock starts. Time must be 0 then strictly increasing. Late points are written, not dropped. Constraints ignored. One ack per non-empty batch. Batches are received with a select on ctx, not range: Stop's opMgr.CancelRunning blocks until the op returns and cancelling our ctx does not close the channel. After the last point the arm dwells on it (as a paced waypoint would) before WaitForServosToStop, so the call does not return on the pre-write Moving zero. One log line per stream: gate, late points, settle, wall.
  • Simulated arm (components/simulated/streamed.go): same contract, re-targets the interpolator per point; Stop ends the stream between points.
  • Shared: servo.CheckTrajectoryTime, servo.Clock; FakeTransport.WriteCount counts SYNC_WRITEs by start address so tests can count goal writes.
  • Docs: docs/arm.md "Streaming trajectories (experimental)"; CLAUDE.md gotchas.

Measured on hardware (laptop-main, STS3215 arm, 1 Mbaud)

run points trajectory wall gate late points settle
arm-recorder nod @ 100 Hz 681 6.80 s 7.256 s 453 ms 0 2 ms
arm-recorder test @ 100 Hz 561 5.60 s 7.106 s 1.503 s 0 2 ms

Wall − gate − trajectory ≈ 3 ms in both: the schedule held exactly at 100 Hz; all overhead is the start gate (which depends only on where the arm was parked). Ten planned single moves at 30 and 80 deg/s, two obstacle-detour plans (8–11 waypoints), two mid-stream splices, and a 40-leg / 70 s continuous loop all held schedule with stitch late 0 and 0.7–2.5° mean joint deviation (the servos' droop under load). The dwell fix above came from those runs: before it, the stream returned with settle 2ms and a 1.2° base-joint error.

All measurement tooling (the arm-recorder replay tool, plan-through-motion-service → trajex → this RPC with a paced baseline, and online replanning) is in the stacked PR #51; the module change here is the ~200 lines that matter.

Read before running on hardware

  1. Stop does not brake. It ends the stream, but controller.Stop writes Goal_Velocity = 0, which is the max-speed sentinel, so the servo still completes its last commanded goal. Pre-existing on every move path; this path amplifies it because the last goal can be far away. Fix belongs in controller.Stop, not here. Not yet tested mid-stream; do that with power in reach before merging.
  2. rdk's arm client validates each point against the model's static DoF() limits (so101.json / URDF), not the calibrated limits the module clamps to, and tears the stream down client-side on a violation.
  3. A stream leaves the servos at Speed 0 / Acc 0; every other move path rewrites both registers, so nothing inherits the sentinel.

Follow-ups (not this PR)

  • controller.Stop should write the current position as the goal rather than velocity 0.
  • Remove geometry.ComputeOOBPosition's clamp (lossy on rdk ≥ v1.6.0).
  • The paced MoveThroughJointPositions path cut a planned obstacle detour by up to 38° at 30 deg/s in the stacked PR's measurements; separate issue.

Testing

go test ./..., -race on both arm packages and internal/servo, go vet, gofmt -s, make bin/arm all clean.

🤖 Generated with Claude Code

…ount writes not packets, sim seams)

Co-Authored-By: Claude Fable 5.1 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01Qv1ZbWg7v4RH7HcR6xUgAi
rdk >= v1.1.0 adds MoveThroughJointPositionsStreamed to arm.Arm; both arm
models stub it with errors.ErrUnsupported until the real implementation lands.

rdk v1.6.0's Frame.Transform no longer truncates at an out-of-bounds joint,
so oob_test.go drops the assertion that pinned the old bug. ComputeOOBPosition
still clamps to the limit; whether it should stay at all is a follow-up.

Co-Authored-By: Claude Fable 5.1 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01Qv1ZbWg7v4RH7HcR6xUgAi
Co-Authored-By: Claude Fable 5.1 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01Qv1ZbWg7v4RH7HcR6xUgAi
…deg start gate

Co-Authored-By: Claude Fable 5.1 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01Qv1ZbWg7v4RH7HcR6xUgAi
Name the ungated mid-stream ceiling at the code site and pin it with a test;
make the Stop test deterministic; Clock.WaitUntil measures against its own Now.

Co-Authored-By: Claude Fable 5.1 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01Qv1ZbWg7v4RH7HcR6xUgAi
…amed RPC

Co-Authored-By: Claude Fable 5.1 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01Qv1ZbWg7v4RH7HcR6xUgAi
… the client on error

Co-Authored-By: Claude Fable 5.1 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01Qv1ZbWg7v4RH7HcR6xUgAi
…e tool binary

Co-Authored-By: Claude Fable 5.1 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01Qv1ZbWg7v4RH7HcR6xUgAi
…ools/ layout

Co-Authored-By: Claude Fable 5.1 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01Qv1ZbWg7v4RH7HcR6xUgAi
Also adapts the streamed path to main's three-value clampPositions after the
rebase.

Co-Authored-By: Claude Fable 5.1 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_018G9kxn9BZtxsAen11ecEmW
…y >= 1ms

Point 0 is due at start and the clock is read again after start is captured,
so it always counted as late by a few microseconds.

Co-Authored-By: Claude Fable 5.1 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_018G9kxn9BZtxsAen11ecEmW
…rvos to stop

WaitForServosToStop straight after the final goal write read Moving before it
had risen (~2ms) and returned while the arm was still travelling: hardware
runs showed a 2ms settle and a 1.2 deg base-joint error at return. Reuse the
paced path's dwell, which consults Moving only from its second poll.

Co-Authored-By: Claude Fable 5.1 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_018G9kxn9BZtxsAen11ecEmW
@HipsterBrown
Nick Hehr (HipsterBrown) marked this pull request as ready for review September 11, 2026 22:00
@HipsterBrown Nick Hehr (HipsterBrown) changed the title feat(arm): experimental time-scheduled MoveThroughJointPositionsStreamed (rdk v1.6.0) feat(arm): time-scheduled MoveThroughJointPositionsStreamed (rdk v1.6.0) Sep 11, 2026
…e tools PR

CheckTrajectoryTime takes only (prev, t); the simulated Stop check lives in
startMove; docs/arm.md's streaming section is a third of its length.
tools/stream_trajectory moves to the stacked tools PR with the other tools.

Co-Authored-By: Claude Fable 5.1 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_018G9kxn9BZtxsAen11ecEmW
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

1 participant