Types of Parallelism in Planning

Motions in Microseconds

• Parallel FK across configurations
• Hierarchical sphere collision checking
• Edge-parallel collision checking
• Collision-affording point trees
  (branchless approximate nearest-neighbors)

140 Hz

Full pipeline from a raw 166k-point depth camera cloud (filter, build CAPT, plan, simplify), UR5, one CPU core

Kinematic Equality Constraints

Projection-Based Sampling

McVAMP: Vectorized Projection

McVAMP Weakness

• Iterative numerical algorithms are fundamentally hard to parallelize
• McVAMP runs multiple projections in parallel to avoid this
• CPU SIMD parallelism \(\Rightarrow\) bottlenecked by slowest projection

(Re)Parameterized Planning

ReVAMP

Bimanual Transport (Sim)

0.13 ms

median planning time, 14-DoF bimanual iiwa (Dual-Follower)

• McVAMP: 2.89 ms (22× slower)
• Leader-Follower: 0.44 ms, despite 5× more iterations than McVAMP
• Dual-Follower: 58 iterations vs 310, and shorter paths (5.5 vs 14.1 rad)
• 200 random queries between 3 shelf poses

Maze Tracing (Hardware)

12 ms

median replanning time on hardware, FR3 held to the maze plane, moving obstacles, 2.5 GHz i5

• McVAMP: 146 ms (12× slower)
• Planned z-error: 0 mm (exact by construction) vs 1.02 mm
• Executed z-error: 0.19 mm vs 1.08 mm
• Sim, 200 queries: 4.8 ms vs 59.8 ms; failures end 8× sooner (57 vs 473 ms)

When Planning Is Free

Planning is expensive

• Ration planner calls
• Guess the best candidate first
• A failure costs a 30 s timeout
• Retries, fallbacks, special cases

Planning is free

• Plan every candidate
• Let the planner pick the best
• A failure costs milliseconds
• One loop over candidates

Case study: bimanual pick-and-place on the RB-Y1

Old RB-Y1 Pick-and-Place

Per box pose (20-point grid)

• IK for the grasp, carry and place poses
• Six legs; lift and place are constrained (box in both hands)
• Each leg: BiRRT \(\to\) shortcut \(\to\) trajectory optimization \(\to\) TOPPRA

Choosing the grasp

1. Fix one of 8 IK branches
2. Solve grasp IK 6 times; rank with a hand-tuned heuristic
3. Probe the best grasp: can we plan lift + place?
4. Probe fails \(\to\) probe the other grasps in parallel
5. Still failing \(\to\) retry with new seeds, then the next IK branch

43.6 s per box pose, much of it wasted

Every failed planning call runs to its 30 s timeout
79% of IK time goes to ranking candidates that are thrown away

RB-Y1 Pick-and-Place

Just plan everything

1. One IK branch; 5 IK solutions each for grasp, carry and place
2. ReVAMP plans every grasp \(\to\) carry and carry \(\to\) place pair: up to 50 calls, ~7 ms each
3. Keep the shortest of the 125 grasp \(\to\) carry \(\to\) place chains
4. Trajectory optimization, TOPPRA and dense checks as before

17.9 s

median per box pose (IFT: 43.6 s)

20/20

grid points planned (IFT: 20/20)

6 ms

per failed planning call (IFT: 30 s timeout)

And What Else…

• MPC, but always to the true goal
• Actual confidence about plan infeasibility for TAMP
• Fast pathwise IK for policy deployment