Real-time Robot Arm Motion Planning and Control with Nonlinear Model Predictive Control using Augmented Lagrangian on a First-Order Solver

In this work we implement motion planning and control of a robot arm with nonlinear model predictive control using the optimization algorithm PANOC. PANOC is a first order nonlinear optimization solver, with convergence guarantees, that is matrix-free unlike the popular sequential quadratic programming and nonlinear interior-point methods. We extend this solver to deal with hard constraints using an augmented Lagrangian method. This is used to implement a multipleshooting MPC algorithm with collision avoidance capabilities on a robot arm. The computational time is benchmarked against other nonlinear optimization solvers. The algorithm is validated with simulations.