- From std::array to Eigen::Matrix
Eigen::Map<Eigen::Matrix<double, 4, 4, Eigen::RowMajor>> T(T_init.data());
Eigen::Matrix4d T = Eigen::Map<Eigen::Matrix<double,4,4,Eigen::RowMajor>>(T_init.data());
Eigen::Matrix4d T(Eigen::Matrix<double,4,4,Eigen::RowMajor>::Map(T_init.data()));
Eigen::Affine3d T(Eigen::Matrix4d::Map(T_init.data()));
- From Eigen::Affine3d to Pose::Stamped
position_des = T.translation();
orientation_des = T.rotation(); // orientation_des in quaternion (T.rotation() is rotation matrix)
or
Eigen::Vector3d position(T.translation());
Eigen::Quaterniond orientation(T.linear()); // orientation in quaternion (T.linear() means matrix, i.e. rotation matrix)
std::cout << position << std::endl;
std::cout << orientation.coeff() << std::endl;
- From Pose::Stamped to Eigen::Matrix
Eigen::Matrix4d T_des;
T_des.setIdentity();
T_des.block<3,3>(0,0) = Eigen::Matrix3d(orientation_des); // orientation_des in quaternion
T_des.block<3,1>(0,3) = Eigen::Vector3d(position_des);
std::cout << T_des << std::endl;
- From Eigen::Matrix4d to std::array<double, 16> (Should be better way for conversion)
for (size_t i=0; i<16; i++)
T_des[i] = T_des_temp.data()[i];
- From Quaterniond to AngleAxisd
Eigen::AngleAxisd rot_des;
rot_des = Eigen::Quaterniond(orientation_des);
std::cout << rot_des.angle() << std::endl;
std::cout << rot_des.axis() << std::endl;
- From (pos, rot_vec) to a generalized coordinate in task space
Eigen::Matrix<double, 6, 1> x_des;
x_des.block<3,1>(0,0) = Eigen::Vector3d(position_des);
x_des.block<3,1>(3,0) = Eigen::Vector3d(rot_des.axis()*rot_des.angle());
- From Eigen::Matrix to std::vector
std::vector<std::array<double, 6>> x_des;
Eigen::Matrix<double, 6, 1>::Map(x_des[0].data()) = x_des_;
- From Eigen::Vector to Eigen::Quaternion
Eigen::Vector3d orientation_temp;
orientation_temp << x[3], x[4], x[5];
Eigen::Quaterniond orientation(Eigen::AngleAxisd(orientation_temp.norm(), orientation_temp.normalized()));
Eigen::IOFormat CleanFmt(4, 0, ", ", "\n", "[", "]");
std::string sep = "\n----------------------------------------\n";
std::cout << T_des.format(CleanFmt) << sep;