Add an optimization-based planner

[adlarkin]

This would be a nice complement to sampling-based planners

[sea-bass]

If MuJoCo has some way to offer automatic differentiation of dynamics / collision geometry queries, that would help here.

This is one of the reasons that Drake and Pinocchio (through CasADi) provide compelling options for optimization-based planners.