Use Eigen::Isometry3d::linear() instead of Eigen::Isometry3d::rotation().
Remove unused lines in BodyNode.cpp
Use Eigen::Translation3d(...) and Eigen::Isometry3d::translation().