动轴旋转

tf::Transform中的getBasis().getRPY(roll,pitch,yaw)

的api文档是不对的 

    /**@brief Get the matrix represented as roll pitch and yaw about fixed axes XYZ（此处是错误的，经过实际验证实际上是动轴旋转）

    * @param roll around X axis 

    * @param pitch Pitch around Y axis

    * @param yaw Yaw around Z axis

    * @param solution_number Which solution of two possible solutions ( 1 or 2) are possible values*/   

    void getRPY(tfScalar& roll, tfScalar& pitch, tfScalar& yaw, unsigned int solution_number = 1) const

    {

    getEulerYPR(yaw, pitch, roll, solution_number);

    }

     listener.lookupTransform("base_link", "shoulder_link",

                               ros::Time(0), transform_stamped[0]); // (0 will get the latest),      

tf::Transform trans(transform_stamped[i].getBasis(), transform_stamped[i].getOrigin());  get the transform from the stamped transform    

  trans.getBasis().getRPY(roll,pitch,yaw);//动轴旋转顺序是：先绕z轴旋转，然后按照y轴旋转，最后绕x轴旋转，应该是动轴旋转，roll（x），pitch（y），yaw（z）

      std::cout<<"tf dynamic axis   roll（x）,pitch（y）,yaw（z） []"<<i<<"is:"<<roll<<" "<<pitch<<" "<<yaw<<endl;

shoulder link坐标系到upper_arm_link坐标系的旋转为：

动轴旋转角度tf  roll（x）,pitch（y）,yaw（z） []1is:-1.5708 1.5708 0

则旋转顺序为先绕z轴旋转0，然后绕y轴旋转1.5708=PI/2，最后绕x旋转-1.5708=-PI/2

 

   动轴旋转方法二：

   [roll(x),pitch(y),yaw(z)]=Eigen：：eulerAngles（0，1，2）

旋转顺序为 ：动轴旋转，先绕x轴旋转，然后旋转y轴，最后旋转z轴：  

     Eigen::Vector3d rpy_rad_dyn = R_rot[i].eulerAngles(0, 1, 2);

     std::cout << "eigen rpy_rad_dyn of [] roll（x）  pitch（y） yaw（z）" << i << "is:" << rpy_rad_dyn << std::endl;

eigen rpy_rad_dyn of [] roll（x）  pitch（y） yaw（z） 1 is: 1.5708 3.14159 -1.5708

shoulder link坐标系到upper_arm_link坐标系的旋转为：

动轴旋转角度Eigen  roll（x）,pitch（y）,yaw（z） []1is:1.5708 3.14159 -1.5708

则旋转顺序为先绕x轴旋转1.5708=PI/2，然后绕y轴旋转3.14159=PI，最后绕z旋转-1.5708=-PI/2

总结：

一个坐标系到另一个坐标系的变换可以有好几种变换方式。