20251015 ik cache

[erh]

No description provided.

[dgottlieb]

I verified this fixed the pirouette test[1]. Or at least in my aggressive repro, brings the number of iteration failures down from ~12 per run -> 0.8 per run. Presumably this patch works great when my worst-case nemesis isn't applied.

Definitely more stuff for me to look at right now compared to before. But I'm amenable to committing this now and touching up in a later PR.

My biggest clarification is (as per the code comment), what's the intended lifetime of this cache? Again, perfectly happy pushing this in for now and revising the lifetime for a later PR. Just wanted to make sure what's there right now is intentional.

Otherwise, I'll do a more thorough pass this evening.

[1] I force it to fail with this patch:

diff --git i/motionplan/armplanning/node.go w/motionplan/armplanning/node.go
index d63c6c87b..109d3e9ad 100644
--- i/motionplan/armplanning/node.go
+++ w/motionplan/armplanning/node.go
@@ -344,6 +344,8 @@ func (sss *solutionSolvingState) shouldStopEarly() bool {
                // we're going to have to do cbirrt, so look a little less, but still look
                multiple = 75
        }
+       multiple = 0
+       minMillis = 10
 
        if elapsed > max(sss.firstSolutionTime*time.Duration(multiple), time.Duration(minMillis)*time.Millisecond) {
                sss.psc.pc.logger.Debugf("stopping early with bestScore %0.2f / %0.2f after: %v",

[dgottlieb]

The way I read everything fitting together right now -- every motion plan request creates its own cache. And then throws it away.

I expect this code can have a longer lifetime than a single motion plan. But there is a cache invalidation problem -- as frames change geometries, the cache needs to be recomputed.

[erh]

so i'm caching one layer down as the frame layer. that's where all the work is, and then if you change some obstacles or something most of the work is re-used.
and yes, it lives forever, which I think is ok...

[github-actions]

Availability

Scene #	viamrobotics:main	erh:20251015-ik-cache	Percent Improvement	Health
1	100%	100%	0%	➖
2	100%	100%	0%	➖
3	100%	100%	0%	➖
4	100%	100%	0%	➖
5	100%	100%	0%	➖
6	90%	90%	0%	➖
7	90%	80%	-11%	❌
8	100%	100%	0%	➖
9	100%	100%	0%	➖
10	100%	100%	0%	➖

Quality

Scene #	viamrobotics:main	erh:20251015-ik-cache	Percent Improvement	Probability of Improvement	Health
1	1.31±0.00	1.31±0.00	-0%	48%	➖
2	1.58±0.21	1.61±0.30	-1%	48%	➖
3	6.16±0.98	5.14±0.00	17%	85%	✅
4	3.53±0.63	3.53±0.63	-0%	50%	➖
5	12.99±5.15	12.99±5.15	-0%	50%	➖
6	12.82±5.31	12.82±5.31	-0%	50%	➖
7	7.63±2.65	6.70±2.92	12%	59%	➖
8	1.55±0.22	1.58±0.21	-2%	45%	➖
9	5.87±2.29	4.15±0.00	29%	77%	➖
10	12.67±0.19	12.67±0.19	-0%	50%	➖

Performance

Scene #	viamrobotics:main	erh:20251015-ik-cache	Percent Improvement	Probability of Improvement	Health
1	0.03±0.01	0.03±0.00	10%	59%	➖
2	0.03±0.00	0.03±0.01	-9%	39%	➖
3	0.16±0.02	0.36±0.07	-121%	1%	❌
4	0.61±0.08	0.64±0.08	-6%	38%	➖
5	4.80±4.63	4.93±4.79	-3%	49%	➖
6	3.09±1.60	3.15±1.63	-2%	49%	➖
7	3.72±1.74	3.40±1.31	8%	56%	➖
8	0.03±0.00	0.02±0.00	17%	79%	➖
9	4.99±6.03	2.29±0.12	54%	67%	➖
10	8.11±1.56	8.04±1.45	1%	51%	➖

The above data was generated by running scenes defined in the motion-testing repository
The SHA1 for viamrobotics:main is: 493ada3caf586f545262f8090fb51dac9b9e8794
The SHA1 for erh:20251015-ik-cache is: 493ada3caf586f545262f8090fb51dac9b9e8794

• 10 samples were taken for each scene