Add inverse dynamic model

Add old-style inverse dynamic model.
Code cleanup
Add base acceleration computation
Fix computation (equation) mistakes

Fix computation of base acceleration to correctly remove the gravity
effect.
Fix computation of link acceleration to use the correct transformation
(screw) matrix.
Fix computation of `composite_beta` by using the correct equation (this
was a mistake in the paper).
Fix `composite_beta` computation

Fix bug in `composite_beta` computation due to wrong sign usage.
Replace with efficient composite terms computation

Replace computation of `composite_inertia` and `composite_beta` with
efficient methods. For `composite_inertia` calculation, method in
Appendix 8 of Khalil and Dombre is used. For `composite_beta`
calculation, the multiplication order of matrices are chosen to have the
least number of operations.
small kinematics fix
Merge branch 'geom_revamp' into floating