52 void transformKDLToEigenImpl(
const KDL::Frame &k, T &e)
55 for (
unsigned int i = 0; i < 3; ++i)
59 for (
unsigned int i = 0; i < 9; ++i)
60 e(i/3, i%3) = k.
M.
data[i];
70 void transformEigenToKDLImpl(
const T &e,
KDL::Frame &k)
72 for (
unsigned int i = 0; i < 3; ++i)
74 for (
unsigned int i = 0; i < 9; ++i)
75 k.
M.
data[i] = e(i/3, i%3);
82 transformKDLToEigenImpl(k, e);
87 transformKDLToEigenImpl(k, e);
92 transformEigenToKDLImpl(e, k);
97 transformEigenToKDLImpl(e, k);
102 for(
int i = 0; i < 6; ++i)
108 for(
int i = 0; i < 6; ++i)
114 for(
int i = 0; i < 3; ++i)
119 for(
int i = 0; i < 3; ++i)
125 for(
int i = 0; i < 6; ++i)
131 for(
int i = 0; i < 6; ++i)
static Rotation Quaternion(double x, double y, double z, double w)
void quaternionKDLToEigen(const KDL::Rotation &k, Eigen::Quaterniond &e)
Converts a KDL rotation into an Eigen quaternion.
void wrenchKDLToEigen(const KDL::Wrench &k, Eigen::Matrix< double, 6, 1 > &e)
Converts a KDL wrench into an Eigen matrix.
void transformEigenToKDL(const Eigen::Affine3d &e, KDL::Frame &k)
Converts an Eigen Affine3d into a KDL frame.
void GetQuaternion(double &x, double &y, double &z, double &w) const
void twistKDLToEigen(const KDL::Twist &k, Eigen::Matrix< double, 6, 1 > &e)
Converts a KDL twist into an Eigen matrix.
void quaternionEigenToKDL(const Eigen::Quaterniond &e, KDL::Rotation &k)
Converts an Eigen quaternion into a KDL rotation.
void vectorEigenToKDL(const Eigen::Matrix< double, 3, 1 > &e, KDL::Vector &k)
Converts an Eigen matrix into a KDL vector.
void vectorKDLToEigen(const KDL::Vector &k, Eigen::Matrix< double, 3, 1 > &e)
Converts a KDL vector into an Eigen matrix.
void twistEigenToKDL(const Eigen::Matrix< double, 6, 1 > &e, KDL::Twist &k)
Converts an Eigen matrix into a KDL Twist.
void transformKDLToEigen(const KDL::Frame &k, Eigen::Affine3d &e)
Converts a KDL frame into an Eigen Affine3d.
void wrenchEigenToKDL(const Eigen::Matrix< double, 6, 1 > &e, KDL::Wrench &k)
Converts an Eigen matrix into a KDL wrench.