机械臂位姿三个夹角与位姿矩阵的关系：

机械臂位姿三个夹角与位姿矩阵的关系：

//    //由姿态矩阵->计算旋转角度
//    double sin2 = ax;
//    double cos2_1 = sqrt(1-pow(ax,2));
//    double cos2_2 = -sqrt(1-pow(ax,2));
//    std::vector<double> cosVector;
//    cosVector.push_back(cos2_1);
//    if(cos2_1-cos2_2>Eps)//whether = or not ?
//        cosVector.push_back(cos2_2);
//
//    for (auto cos2:cosVector)
//    {
//        double t2 = atan2(sin2,cos2);
//        double sin1 = -ay/cos2;
//        double cos1 = az/cos2;
//        double t1 = atan2(sin1,cos1);
//        double sin3 = -ox/cos2;
//        double cos3 = nx/cos2;
//        double t3 = atan2(sin3,cos3);
//       
       
}

..得到旋转角度->计算姿态矩阵
//        //旋转角度：rx,ry,rz
//        Eigen::Vector3d  Rotation_vector(t1,t2,t3);
//        double rx = Rotation_vector(0,0);
//        double ry = Rotation_vector(1,0);
//        double rz = Rotation_vector(2,0);
//
//        //直接按角度旋转计算
//
//        Eigen::Matrix4d matrixTr;
//        matrixTr.setIdentity(4,4);
//        matrixTr(0,0) =cos(ry)*cos(rz);
//        matrixTr(1,0) =sin(rx)*sin(ry)*cos(rz)+cos(rx)*sin(ry);
//        matrixTr(2,0) =-cos(rx)*sin(ry)*cos(rz)+sin(rx)*sin(rz);
//
//        matrixTr(0,1) =-cos(ry)*sin(rz);
//        matrixTr(1,1) =-sin(rx)*sin(ry)*cos(rz)+cos(rx)*cos(rz);
//        matrixTr(2,1) =cos(rx)*sin(ry)*cos(rz)+sin(rx)*cos(rz);
//
//        matrixTr(0,2) =sin(ry);
//        matrixTr(1,2) =-sin(rx)*cos(ry);
//        matrixTr(2,2) =cos(rx)*cos(ry);
//
//        matrixTr(0,3) = matrixT(0,3);
//        matrixTr(1,3) = matrixT(1,3);
//        matrixTr(2,3) = matrixT(2,3);