2022-2-22 rbdl 浮动基座正运动学计算姿态变换

本文链接：https://blog.csdn.net/HITORANGE/article/details/123068698

vrep里的姿态表示欧拉角规则

在这里插入图片描述
右手规则规则！！！！！！！！！！！！！！！！！！！！！

eigen 库函数matrix3d.eularangle

四元数转旋转矩阵后转欧拉角可能两个解
eularangle(0,1,2)
左手规则旋转！！！！！！！！！！！！！！！

手推的正运动学姿态变换：

   * forward kinematics *
    if(floatingbase)
    {
        index = 6;
        Quaternion quat(q[3],q[4],q[5],q[12]);
        Matrix3d baseleftrpym = quat.toMatrix();
        baserpym = baseleftrpym.transpose();
    }

    Matrix3d joint1 = Eigen::AngleAxisd(q[index],Vector3d(0,0,1)).toRotationMatrix();

    Matrix3d joint2 = Eigen::AngleAxisd(q[1+index]-M_PI/2,Vector3d(0,-1,0)).toRotationMatrix();
    Matrix3d joint3 = Eigen::AngleAxisd(q[2+index],Vector3d(0,-1,0)).toRotationMatrix();

    Matrix3d link34 = Eigen::AngleAxisd(M_PI/2,Vector3d(0,-1,0)).toRotationMatrix();
    Matrix3d link4 = Eigen::AngleAxisd(q[3+index],Vector3d(0,0,1)).toRotationMatrix();
    Matrix3d link5 = Eigen::AngleAxisd(q[4+index],Vector3d(0,-1,0)).toRotationMatrix();

    linkrpym[0] = baserpym;
    linkrpy[0] = linkrpym[0].eulerAngles(0,1,2);

    linkrpym[1] = baserpym*joint1*joint2;
    linkrpy[1] = linkrpym[1].eulerAngles(0,1,2);

    linkrpym[2] = baserpym*joint1*joint2*joint3;
    linkrpy[2] = linkrpym[2].eulerAngles(0,1,2);

    linkrpym[3] = baserpym*joint1*joint2*joint3*link34*link4;
    linkrpy[3] = linkrpym[3].eulerAngles(0,1,2);

    linkrpym[4] = baserpym*joint1*joint2*joint3*link34*link4*link5;
    linkrpy[4] = linkrpym[4].eulerAngles(0,1,2);


    * forward kinematics *