定轴旋转
固定轴旋转顺序 x-y-z
左乘 rotz*roty*rotx
listener.lookupTransform("shoulder_link", "upper_arm_link",
ros::Time(0), transform_stamped[1]); // (0 will get the latest),
R_rot.resize(JOINT_NUM_);
for (int i = 0; i < JOINT_NUM_; i++)
{
tf::Transform trans(transform_stamped[i].getBasis(), transform_stamped[i].getOrigin()); get the transform from the stamped transform
//cout<<"trans[]"<<i<<".w ,x y, z is:"<<trans.getRotation().getW()<<" "<<trans.getRotation().getX()<<" "<<trans.getRotation().getY()<<" "<<trans.getRotation().getZ()<<endl;
//Eigen::Matrix3d temp_mat;
Eigen::Isometry3d eignIsom3d;
tf::transformTFToEigen(trans, eignIsom3d);
//cout << "eignIsom3d.matrix()" << eignIsom3d.matr
Eigen::Vector3d ypr_rad_fixed = R_rot[i].eulerAngles(2, 1, 0);
//Eigen::Vector3d ypr_rad_fixed = R_rot[i].eulerAngles(2, 1, 0); //定轴旋转, 得到【yaw(z),pitch(y),roll(x)】 左乘 rotz*roty*rotx,先绕定轴x轴旋转roll,然后绕定轴y轴旋转pitch,之后绕动轴z轴旋转yaw
std::cout << "ypr_rad_fixed of []" << i << "is:" << ypr_rad_fixed << std::endl;
}
ypr_rad_fixed of []1is: 1.57084 1.5708 4.53479e-05
则yaw(1.57084),pitch( 1.5708),roll(4.53479e-05)
将shoulder_link坐标系到upper_arm_link坐标系的定轴旋转的顺序为:
先定轴旋转x轴旋转roll(4.53479e-05 ),然后定轴旋转y轴旋转pitch(1.5708=PI/2),最后定轴旋转z轴旋转yaw(1.57084 =PI/2):
示例如下:
#include <iostream>
#include <Eigen/Core>
#include <Eigen/Geometry>
#define PI (3.1415926535897932346f)
int main(int argc, char **argv)
{
using ::std::cout;
using ::std::endl;
double yaw = PI/3,pitching = PI/4,droll = PI/6;
//EulerAngles to RotationMatrix
::Eigen::Vector3d ea0(yaw,pitching,droll);
::Eigen::Matrix3d R;
R = ::Eigen::AngleAxisd(ea0[0], ::Eigen::Vector3d::UnitZ())
* ::Eigen::AngleAxisd(ea0[1], ::Eigen::Vector3d::UnitY())
* ::Eigen::AngleAxisd(ea0[2], ::Eigen::Vector3d::UnitX());
cout << R << endl << endl;
//RotationMatrix to Quaterniond
::Eigen::Quaterniond q;
q = R;
cout << q.x() << endl << endl;
cout << q.y() << endl << endl;
cout << q.z() << endl << endl;
cout << q.w() << endl << endl;
//Quaterniond to RotationMatrix
::Eigen::Matrix3d Rx = q.toRotationMatrix();
cout << Rx << endl << endl;
//RotationMatrix to EulerAngles
::Eigen::Vector3d ea1 = Rx.eulerAngles(2,1,0);
cout << ea1/PI*180 << endl << endl;
std::cin.ignore();
return 0;
}