Trajopt implementation

[jslee02]

Procedure

1. Implement most basic functionalities for trajectory optimization that uses nonlinear optimization solver like nlopt, ipopt.
2. define the cost of a trajectory CHOMP-like as the integral of distances from obstacles, including self-collisions, and whatever else you want.
3. Define constraints on joint positions [if you need to be dynamic include vel and acc]
4. Send this to an optimizer.
5. Add functionality of swept-volume collision checking
6. Add SQP optimization

Todos

• Available optimizers through DART are nlopt and ipopt, which are for nonlinear programming. It would be nice to search quadratic programming (QP) specific solvers.

References

• trajopt (github)
• https://en.wikipedia.org/wiki/Quadratic_programming
• https://doc.cgal.org/latest/QP_solver/index.html
• http://www.numerical.rl.ac.uk/people/nimg/qp/qp.html
• http://plato.asu.edu/sub/nlores.html#QP-problem