Analytic DDP Solver: Review and improvements (adaptive regularisation)

[wxmerkt]

• Go over equations and cross-check with Yuval Tassa's thesis
• Add state and control regularisation and make adaptive
• Add revert, i.e., only store new trajectory if it's an improvement -- there was a bug of it improving the wrong trajectory
• Fix convergence checks (the function tolerance still has some issues)
• Add option to turn clamping of controls in forward-pass off
• Reduce dynamic allocation (still plenty)
• Change line-search to back-tracking line-search with early termination and fix typo in equation

Results

Quadrotor Collision Avoidance Example:

Current Exotica: 20.37s, final cost: 29.16 Fixed Pull Request: ~2s (!), final cost: ~12 [without clamping, with function tolerance active]

Difference for convergence and solution on cart-pole (example 04)

... and turning off the clamping hack (new configuration option):