Add a nonlinear version of the dynamics planning, with linear constraints only. Still it is quite slow