Lagrangian and Hamiltonian Methods For Nonlinear Control 2006