motionplan: honor WorldState.Transforms() in armplanning.PlanMotion

[NickPPC]

Summary

A LinkInFrame added to a PlanRequest.WorldState — the idiomatic way to model a held/attached object that moves with a frame (e.g. a cup in a gripper) — was silently dropped when calling armplanning.PlanMotion directly. Only the builtin motion service merged WorldState.Transforms() into the planning frame system; the direct path used the request's frame system verbatim. As a result a held object was either ignored (modeled as a transform) or frozen at the seed configuration (modeled as an obstacle), so it was never collision-checked along the trajectory. This makes the two paths consistent so a held object now sweeps with the arm and is checked at every state of the move.

Changes

• referenceframe: Add FrameSystem.FrameSystemWithTransforms, which returns a copy of the frame system augmented with the given transforms (mirroring how NewFrameSystem merges supplemental transforms). The receiver is left unmodified so a cached frame system can be reused across plans.
• motionplan/armplanning: validatePlanRequest now merges WorldState.Transforms() into a copy of the request's frame system before planning, so transform geometry moves with its parent frame and is collision-checked along the whole trajectory. A transform that cannot be applied (e.g. missing parent frame) now returns a wrapped error instead of being silently dropped.
• motionplan/armplanning: Document the obstacle-vs-transform distinction on PlanRequest.WorldState (obstacles freeze at the start configuration; transforms move with their parent).
• services/motion/builtin: The builtin service already pre-merges transforms into its frame system via getFrameSystem, so it now strips them from the WorldState it hands to PlanMotion (worldStateWithoutTransforms) to avoid merging the same frames twice and erroring on duplicates.

Testing

go test ./referenceframe/ ./motionplan/armplanning/ ./services/motion/builtin/ — all pass. New tests:

• referenceframe/frame_system_test.go TestFrameSystemWithTransforms: transform becomes a frame in the copy but not the original; held geometry tracks its moving parent across configurations; missing parent errors loudly; empty input returns an independent copy.
• motionplan/armplanning/held_object_test.go:
  • TestValidatePlanRequestMergesWorldStateTransforms — the transform is merged into a copy of the request frame system; a bad parent fails loudly.
  • TestPlanMotionHeldObjectCollisionCheckedAlongTrajectory — end-to-end: a held box attached via a transform is collision-checked at the goal config (plan fails) while the bare arm reaches it; characterizes that a WorldState obstacle in a moving frame does not track the arm.
  • TestPlanMotionHeldObjectCollidesBetweenEndpoints — the hard case: a held object that is collision-free at both endpoints but strikes a doorway (two walls + gap) partway through the swing. Verified deterministically at the constraint-checker level (endpoints clear, segment between them blocked) with a false-positive guard showing the same path is clear once the walls are removed.

Regression check: the existing builtin "succeeds with supplemental info in world state" test (which sends transforms and a goal expressed relative to a transform frame through the service) still passes.

🤖 Generated with Claude Code

[github-actions]

Availability

Scene #	viamrobotics:main	viamrobotics:worktree-world-state-limitations	Percent Improvement	Health
1	100%	100%	0%	➖
2	100%	100%	0%	➖
3	100%	100%	0%	➖
4	100%	100%	0%	➖
5	100%	100%	0%	➖
6	100%	100%	0%	➖
7	100%	100%	0%	➖
8	100%	100%	0%	➖
9	100%	100%	0%	➖
10	100%	100%	0%	➖
11	100%	100%	0%	➖
12	100%	100%	0%	➖

Quality

Scene #	viamrobotics:main	viamrobotics:worktree-world-state-limitations	Percent Improvement	Probability of Improvement	Health
1	1.31±0.00	1.31±0.00	-0%	36%	➖
2	0.94±0.01	0.94±0.01	0%	60%	➖
3	6.13±0.00	6.13±0.00	-0%	50%	➖
4	1.98±0.26	1.98±0.26	0%	50%	➖
5	8.18±2.14	8.50±2.01	-4%	46%	➖
6	8.69±2.54	8.75±2.54	-1%	49%	➖
7	2.10±0.63	2.10±0.63	0%	50%	➖
8	0.94±0.01	0.94±0.01	-0%	45%	➖
9	4.69±0.52	4.69±0.52	-0%	50%	➖
10	12.62±0.04	12.62±0.04	-0%	50%	➖
11	0.62±0.00	0.62±0.00	0%	57%	➖
12	0.81±0.00	0.81±0.00	-0%	50%	➖

Performance

Scene #	viamrobotics:main	viamrobotics:worktree-world-state-limitations	Percent Improvement	Probability of Improvement	Health
1	0.01±0.00	0.01±0.00	-5%	39%	➖
2	0.03±0.00	0.03±0.00	-8%	31%	➖
3	0.02±0.00	0.02±0.00	-4%	38%	➖
4	0.26±0.04	0.25±0.04	6%	60%	➖
5	1.18±0.22	1.17±0.20	1%	52%	➖
6	1.42±0.39	1.40±0.35	1%	51%	➖
7	0.59±0.11	0.57±0.10	4%	57%	➖
8	0.03±0.00	0.03±0.00	-2%	43%	➖
9	1.62±0.55	1.61±0.55	0%	50%	➖
10	1.86±0.27	1.88±0.24	-1%	49%	➖
11	0.46±0.03	0.44±0.01	4%	74%	➖
12	0.81±0.01	0.81±0.03	-0%	46%	➖

The above data was generated by running scenes defined in the motion-testing repository
The SHA1 for viamrobotics:main is: 6ab5854fd911a26ffee87f1bbc4567bb9e156cdc
The SHA1 for viamrobotics:worktree-world-state-limitations is: 6ab5854fd911a26ffee87f1bbc4567bb9e156cdc

• 12 samples were taken for each scene