Add a method to convert Eigen::AutoDiffScalar in double.
Store parentToSon transform in FK.
Add static inverse dynamics algorithm.