Conversion functions between Eigen and KDL and Eigen and geometry_msgs
https://wiki.ros.org/eigen_conversions