[ q^* = \arg\min_q \in Q | q - D(z^*) |^2 \quad \texts.t. \quad q \in Q_free ]
[5] T. P. Lillicrap et al., "Continuous control with deep reinforcement learning," International Conference on Learning Representations (ICLR) , 2016. hdmove2
[2] N. Ratliff, M. Zucker, J. A. Bagnell, and S. Srinivasa, "CHOMP: Gradient optimization algorithms for efficient motion planning," IEEE International Conference on Robotics and Automation (ICRA) , 2009, pp. 1292–1299. [ q^* = \arg\min_q \in Q | q - D(z^*) |^2 \quad \texts
[ z^* = \arg\min_z(t) \in Z \mathcalJ[D(z(t))] \quad \texts.t. \quad \texthomotopy constraints ] Lillicrap et al
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:
[ \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] ]
