to do dynamic contact planner:
- accelerate cutsom integrator by vectorizing outer for loop
- achieve minimization of distance between obstacle and contact point
- remember to remove start/stop constraint if it is in an infeasible position
- remember to include constraints for all derivatives between decision variables
- things that may be impossible to fulfill: cost function, not constraint!
- when including dynamic effects in the Euler-Lagrange equation, always make sure to test with multiple time points (3+)
- why does the curling happen so often
- in the current experimental set to be static, why are resulting moments not constant?
- in the current experimental set to static, why are convergence properties so bad?
- what are efficient ways to improve convergence? prescribed procedures? how to get a sense of which constraints are easy/hard to fulfill?
- optimization often stoped prematurely because the step size was below the step size tolerance. reducing this tolerance to 1e-15 may have helped (not entirely sure)