add a way to visualize plans the same way builtin execute runs

[JohnN193]

found a test case where depending on the order that the motion gets executed, a collision can occur. was annoying to get to repro on my machine since random orders are random so I updated cmd to visualize it instead

plan-2026-03-06T18:39:40-05:00-ms-3843--goals-1-traj-14-l2-11.38.json

run with --viz-order descending to get the collision to occur. the setup is jank(an arm attached to an arm) but it seemed like the best way to make sure both arms were moving for something else i was testing.

Screencast.from.2026-03-06.14-03-39.mp4

[github-actions]

Availability

Scene #	viamrobotics:main	JohnN193:cmd-vis-sequential	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%	➖

Quality

Scene #	viamrobotics:main	JohnN193:cmd-vis-sequential	Percent Improvement	Probability of Improvement	Health
1	1.31±0.00	1.31±0.00	-0%	31%	➖
2	0.94±0.01	0.94±0.01	-0%	39%	➖
3	6.13±0.00	6.13±0.00	-0%	50%	➖
4	1.98±0.27	1.98±0.27	0%	50%	➖
5	8.80±1.87	8.78±1.87	0%	50%	➖
6	9.33±2.99	8.94±2.48	4%	54%	➖
7	1.95±0.38	1.95±0.38	-0%	50%	➖
8	0.94±0.01	0.94±0.01	-0%	33%	➖
9	4.54±0.41	4.53±0.37	0%	51%	➖
10	12.63±0.03	12.63±0.03	-0%	50%	➖
11	0.62±0.00	0.62±0.00	0%	64%	➖

Performance

Scene #	viamrobotics:main	JohnN193:cmd-vis-sequential	Percent Improvement	Probability of Improvement	Health
1	0.02±0.00	0.02±0.00	7%	75%	➖
2	0.03±0.00	0.03±0.00	-0%	50%	➖
3	0.02±0.00	0.02±0.00	23%	91%	✅
4	0.30±0.05	0.29±0.06	4%	56%	➖
5	1.27±0.21	1.28±0.25	-0%	49%	➖
6	1.50±0.42	1.54±0.46	-2%	48%	➖
7	0.77±0.11	0.71±0.09	8%	66%	➖
8	0.03±0.00	0.03±0.00	-10%	20%	➖
9	1.30±0.25	1.30±0.38	-0%	50%	➖
10	3.72±0.43	3.68±0.42	1%	52%	➖
11	0.53±0.08	0.55±0.08	-2%	46%	➖

The above data was generated by running scenes defined in the motion-testing repository
The SHA1 for viamrobotics:main is: 56429e7f3402c283974c725fd2725a0b654ff64c
The SHA1 for JohnN193:cmd-vis-sequential is: 56429e7f3402c283974c725fd2725a0b654ff64c

• 11 samples were taken for each scene

[dgottlieb]

Good news, maybe, is that if I instead have motion planning return the interpolated trajectory that we collision detect with, those are fine grained enough for the motion service's execution to work without collisions:

diff --git i/motionplan/armplanning/plan_manager.go w/motionplan/armplanning/plan_manager.go
index e91d68c0a..df8bd99de 100644
--- i/motionplan/armplanning/plan_manager.go
+++ w/motionplan/armplanning/plan_manager.go
@@ -331,7 +331,18 @@ func initRRTSolutions(ctx context.Context, psc *planSegmentContext, logger loggi
                if solution.checkPath {
                        // If we've already checked the path of a solution that is "reasonable", we can just
                        // return now. Otherwise, continue to initialize goal map with keys.
-                       rrt.steps = []*referenceframe.LinearInputs{solution.inputs}
+
+                       // rrt.steps = []*referenceframe.LinearInputs{solution.inputs}
+
+                       rrt.steps, err = motionplan.InterpolateSegmentFS(&motionplan.SegmentFS{
+                               StartConfiguration: psc.start,
+                               EndConfiguration:   solution.inputs,
+                               FS:                 psc.pc.fs,
+                       }, 2.0)
+                       if err != nil {
+                               panic(err)
+                       }
+
                        return rrt, nil
                }
                rrt.maps.goalMap[&node{inputs: solution.inputs}] = nil

[JohnN193]

Good news, maybe, is that if I instead have motion planning return the interpolated trajectory that we collision detect with, those are fine grained enough for the motion service's execution to work without collisions:

diff --git i/motionplan/armplanning/plan_manager.go w/motionplan/armplanning/plan_manager.go
index e91d68c0a..df8bd99de 100644
--- i/motionplan/armplanning/plan_manager.go
+++ w/motionplan/armplanning/plan_manager.go
@@ -331,7 +331,18 @@ func initRRTSolutions(ctx context.Context, psc *planSegmentContext, logger loggi
                if solution.checkPath {
                        // If we've already checked the path of a solution that is "reasonable", we can just
                        // return now. Otherwise, continue to initialize goal map with keys.
-                       rrt.steps = []*referenceframe.LinearInputs{solution.inputs}
+
+                       // rrt.steps = []*referenceframe.LinearInputs{solution.inputs}
+
+                       rrt.steps, err = motionplan.InterpolateSegmentFS(&motionplan.SegmentFS{
+                               StartConfiguration: psc.start,
+                               EndConfiguration:   solution.inputs,
+                               FS:                 psc.pc.fs,
+                       }, 2.0)
+                       if err != nil {
+                               panic(err)
+                       }
+
                        return rrt, nil
                }
                rrt.maps.goalMap[&node{inputs: solution.inputs}] = nil

tried plugging this in but it doesn't seem to get used?