tools: measure the streamed RPC — planned motion through trajex, online replanning, continuous loop

[Nick Hehr (HipsterBrown)]

Stacked on #48; draft. Needs the system nlopt library for online_stream (build-tagged; CI never links it). Review after #48.

What this adds

Four Go programs under tools/ (outside the module binary) plus a shared untagged package, all built to answer one question on hardware: does time-scheduled streaming track a planned path better than the paced MoveThroughJointPositions, and can the pipeline keep the stream fed while replanning?

• tools/stream_trajectory: replays an arm-recorder session (Time = i / frequency_hz, optional linear densify) through the streamed RPC; the first thing that exercised it on hardware.
• tools/internal/streamx (untagged, CI-tested, 24 tests): trajex tensor names/marshalling (literals, no cgo import), a joint-space path-deviation metric, goal/obstacle parsing, and the live-stream splice math (At, Window, Splice) and event helpers the online tool uses.
• tools/plan_stream: for each -goal, plan with the machine's motion service (same goal cone as MoveToPosition), time-parameterise the waypoints with the trajex ML model service on the machine (viam:trajex:mlmodel), stream via MoveThroughJointPositionsStreamed, return to start, execute the same plan through motion's paced execute. Both runs sampled and scored against the planned polyline. -obstacle boxes give the planner something to route around.
• tools/online_stream (//go:build nlopt): plans in-process with armplanning from the configuration the arm will be in at a stitch time, so a mid-motion event (-switch-after, repeatable -obstacle-after) is replanned and spliced into the live stream without stopping; a Stop → replan → paced-move baseline runs the same scenario. -loop cycles 2–4 poses until Ctrl-C, each leg planned while the previous executes.
• CLAUDE.md gotchas: the nlopt tag; RobotClient.CurrentInputs fails against this module (the gripper's is ErrUnsupported), so start inputs are built with NewZeroInputs + the arm.

What it found

scenario (30 deg/s unless noted)	streamed	paced
straight line, 5 goals × 2 speeds	dev 0.7–1.8° mean; wall = trajex duration	same dev at 80 deg/s; 1.1 s floor on short moves (WaitForServosToStop's 1000 ms minimum timeout on an elbow parked 1.5° short)
goal 4 (all joints, different travel)	0.85° mean	4.1° mean / 7.5° max: short-travel joints clamped up to the 12 deg/s speed floor arrive early
obstacle detour, 8–11 waypoints, 80 deg/s	1.3–1.5° mean, 2.9° max	12.7° mean / 38° max: the arm flew through part of the planned detour
online: obstacle appears @0.5 s + goal switch @0.6 s	two splices, both stitch late 0, replans 13 / 17 ms, wall 2.46 s	Stop, 952 ms replan from the stopped pose, 31° max deviation on plan B, wall 3.13 s
-loop, 4 poses, 40 legs / ~70 s	every leg planned in 6–26 ms, stitch late 0 throughout, dev 0.7–1.9° mean, identical numbers each lap	—

The replan-latency gap (13 ms vs 952 ms) is the point: planning from the predicted stitch pose, already past the obstacle, is a straight line; planning from the stopped pose next to it is a search. The paced path's 38° excursion through a planned detour is a safety finding about main and is filed separately.

Caveats: the streamed stitch late and the sampler are on the tool's clock, which leads the arm by the RPC + gate offset; trajex's ML model Infer is rest-to-rest, so every splice decelerates to rest and re-accelerates (that hitch is the measured cost of a stateless TOTG service); the sampler shares the bus with the 100 Hz stream (-sample-hz is the knob).

Prerequisites

Arm with a frame; builtin motion service; viam-server ≥ 1.1.0 with the module from #48; the trajex module with one ML model service ({"name":"trajex","api":"rdk:service:mlmodel","model":"viam:trajex:mlmodel"}); for online_stream, brew install nlopt / apt install libnlopt-dev and go run -tags nlopt ./tools/online_stream ....

Testing

go test ./... (30 tool tests), go test -tags nlopt -race ./tools/online_stream/ (10: 3 planner against the module's own kinematics, 7 producer over a fake arm), go vet tagged and untagged, gofmt -s. Every hardware-facing risk called out in review (motion plan request encoding incl. the goal cloud, sampler vs move lock, the trajex C++ service's required tensors and int64 sampling rate, gRPC Canceled on Ctrl-C) was either verified against rdk source or fixed after a hardware run.

🤖 Generated with Claude Code

https://claude.ai/code/session_018G9kxn9BZtxsAen11ecEmW