KDL与Eigen数据转换

在ROS使用过程中，KDL和Eigen是使用比较多的两个库了，两个库都各有优势。

比如KDL是针对机器人运动学的，可以用来求解正逆向运动学，速度、雅克比矩阵等，也支持力的表述，很方便，但对矩阵运算却不是那么方便，Eigen有着强大的矩阵计算能力。其实KDL内部有些数据结构也是采用Eigen的，比如JntArray，它的成员data就是一个VectorXd。

在前几天使用过程中，需要转换数据类型，有点懒不想自己写，就查了一下，还真的查到了一个：eigen_conversions  https://github.com/ros/geometry . 

使用的时候需要包含头文件 #include <eigen_conversions/eigen_kdl.h>

这个头文件里包含的函数有：

 void quaternionKDLToEigen(const KDL::Rotation &k, Eigen::Quaterniond &e);   /// Converts an Eigen quaternion into a KDL rotation void quaternionEigenToKDL(const Eigen::Quaterniond &e, KDL::Rotation &k);   /// Converts a KDL frame into an Eigen Affine3d void transformKDLToEigen(const KDL::Frame &k, Eigen::Affine3d &e);   /// Converts a KDL frame into an Eigen Isometry3d void transformKDLToEigen(const KDL::Frame &k, Eigen::Isometry3d &e);   /// Converts an Eigen Affine3d into a KDL frame void transformEigenToKDL(const Eigen::Affine3d &e, KDL::Frame &k);   /// Converts an Eigen Isometry3d into a KDL frame void transformEigenToKDL(const Eigen::Isometry3d &e, KDL::Frame &k);   /// Converts a KDL twist into an Eigen matrix void twistKDLToEigen(const KDL::Twist &k, Eigen::Matrix<double,6,1> &e);   /// Converts an Eigen matrix into a KDL Twist void twistEigenToKDL(const Eigen::Matrix<double,6,1> &e, KDL::Twist &k);   /// Converts a KDL vector into an Eigen matrix void vectorKDLToEigen(const KDL::Vector &k, Eigen::Matrix<double,3,1> &e);   /// Converts an Eigen matrix into a KDL vector void vectorEigenToKDL(const Eigen::Matrix<double,3,1> &e, KDL::Vector &k);   /// Converts a KDL wrench into an Eigen matrix void wrenchKDLToEigen(const KDL::Wrench &k, Eigen::Matrix<double,6,1> &e);   /// Converts an Eigen matrix into a KDL wrench void wrenchEigenToKDL(const Eigen::Matrix<double,6,1> &e, KDL::Wrench &k);

这里面有常见需要用到的类型转换，可以轻松将KDL的数据转为Eigen的数据结构，从而方便进行矩阵运算。