Support auto differentiation

[marip8]

It would be very helpful for many optimization applications if various elements of tesseract (like the scene graph) could support auto differentiation. Instead of manually defining gradients for custom optimization costs, auto differentiation could provide them out of the box.

Looking at some trajopt_ifoptconstraints, it seems that theSceneGraphandJointGroupobjects would be good first targets for supporting autodiff. I'm not sure how deep the support of autodiff types would need to go (e.g., regardingLink, Joint`, and geometry types), but my first interest would be in supporting autodiff only for the calculation of the position of links in the scene graph. It would also be interesting to support autodiff at the geometry level as well for costs like collision checking.

I the following high-level tasks would be required to support this change:

• Choose autodiff library (e.g., Eigen)
• Reformat relevant classes as templates (with current class names being the double template for backwards compatibility)
• Replace gradient calculations with autodiff implementation
• Test (e.g., in trajopt_ifopt Cartesian position constraint)