定轴旋转

50 篇文章 0 订阅
29 篇文章 9 订阅

定轴旋转 

固定轴旋转顺序 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;
}

 

  • 0
    点赞
  • 0
    收藏
    觉得还不错? 一键收藏
  • 0
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值