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

右手规则规则！！！！！！！！！！！！！！！！！！！！！

eigen 库函数matrix3d.eularangle

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

四元数转欧拉角2π内

eigen库旋转的变换

左手变换转右手变换

旋转矩阵的转置 的意义

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

   * 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 *

正运动学结果：
rbdl计算的结果与vrep设置关节角自动计算的各连杆位姿一致，正运动学过程正确。。。