Hdmove2 -

The lower level is solved using a fast alternating direction method of multipliers (ADMM) that converges in under 5 ms for ( n \leq 128 ). Re-planning is triggered when:

| Algorithm | Success Rate (Bench B) | Planning Time (ms) | Cumulative Jerk (m²/s⁵) | Real-time feasible (>30 Hz) | |-----------|------------------------|--------------------|--------------------------|-------------------------------| | RRT* | 0.12 ± 0.05 | 3420 ± 450 | 18.4 ± 3.2 | No | | CHOMP | 0.68 ± 0.12 | 520 ± 85 | 9.2 ± 1.8 | No (for n>30) | | hdmove1 | 0.71 ± 0.10 | 88 ± 12 | 5.3 ± 0.9 | Yes (at 35 Hz) | | | 0.94 ± 0.04 | 41 ± 6 | 1.4 ± 0.3 | Yes (at 95 Hz) | hdmove2

[ \mathcalJ[\tau] = \int_0^T \left( \underbrace \textkinetic energy + \lambda_1 \underbrace^2 \textjerk + \lambda_2 \underbracec_obs(\tau(t))_\textcollision cost \right) dt ] The lower level is solved using a fast

[ \exists t: | q_actual(t) - \tau_planned(t) | > \sigma \cdot \textVar s \in [t-\delta,t] \left[ \frac\partial c obs\partial q(s) \right] ] \sigma \cdot \textVar s \in [t-\delta