Turn everything into a casadi::Function

[alaurenzi]

• double integrator
• double integrator floating base
• inverse dynamics (provide separate SXFunction for the first 6 rows)
• forward dynamics