UR运动学正逆解

参考链接

//角度转弧度
double d2r(double degree) { return (degree/180.0) * M_PI;}

//弧度转角度
double r2d(double radian) { return (radian/M_PI) * 180.0;}

Eigen::MatrixXd kinematics(double* theta)
{
    //ur5数据
    //    double d[6]     = {0.089159,  0.00000,  0.00000, 0.109150,  0.094650, 0.082300 };
    //    double a[6]     = {0.000000, -0.42500, -0.39225, 0.000000,  0.000000, 0.000000 };
    //    double alpha[6] = {1.570796,  0.00000,  0.00000, 1.570796, -1.570796, 0.000000 };
    //ur5e数据
    double d[6]     = {0.162500,  0.00000,  0.00000, 0.133300,  0.099700, 0.099600};
    double a[6]     = {0.000000, -0.4250,  -0.39220, 0.000000,  0.000000, 0.000000};
    double alpha[6] = {1.570796,  0.00000,  0.00000, 1.570796, -1.570796, 0.000000};
    Eigen::Matrix4d T[6];
    for (int i = 0; i <6; ++i)
    {
        T[i](0, 0) = cos(theta[i]);
        T[i](0, 1) = -sin(theta[i]) * cos(alpha[i]);
        T[i](0, 2) = sin(theta[i]) * sin(alpha[i]);
        T[i](0, 3) = a[i] * cos(theta[i]);
        T[i](1, 0) = sin(theta[i]);
        T[i](1, 1) = cos(theta[i]) * cos(alpha[i]);
        T[i](1, 2) = -cos(theta[i]) * sin(alpha[i]);
        T[i](1, 3) = a[i] * sin(theta[i]);
        T[i](2, 0) = 0;
        T[i](2, 1) = sin(alpha[i]);
        T[i](2, 2) = cos(alpha[i]);
        T[i](2, 3) = d[i];
        T[i](3, 0) = 0;
        T[i](3, 1) = 0;
        T[i](3, 2) = 0;
        T[i](3, 3) = 1;
    }
    Eigen::Matrix4d T06 = T[0] * T[1] * T[2] * T[3] * T[4] * T[5];
    return T06;
}
int test_02()
{
    //逆运动学 已知末端姿态, 求六个关节角
    //正运动学 已知六个关节角, 求末端姿态
    //ur5数据
    //    double d[6]     = {0.089159,  0.00000,  0.00000, 0.109150,  0.094650, 0.082300 };
    //    double a[6]     = {0.000000, -0.42500, -0.39225, 0.000000,  0.000000, 0.000000 };

    //ur5e数据
    double d[6]     = {0.162500,  0.00000,  0.00000, 0.133300,  0.099700, 0.099600 };
    double a[6]     = {0.000000, -0.4250,  -0.39220, 0.000000,  0.000000, 0.000000 };

    double theta[8][6];
    //    double tcp[6]      = {-0.117025, -0.435893, 0.151821, -0.012413, 3.111990, -0.018048};// tcp
    //    double joint_q[6]  = {-88.235500, -104.851277, -130.634377,-33.804510, 91.673247, 1.317803};//关节角 角度
    double joint_q1[6] = {-1.540, -1.830, -2.280, -0.590, 1.600, 0.023};//关节角 弧度

    Eigen::Matrix4d T06 = kinematics(joint_q1);//正运动学 已知六个关节角(弧度), 求末端姿态T

    //2.求解
    double A, B, C, D, E, F, G, M, N;//用大写字母替代常数

    //注意，由于数组下标从0开始的问题，矩阵第一行第一列的元素是(0,0)
    //theta1 两个解
    A = d[5] * T06(1, 2) - T06(1, 3);
    B = d[5] * T06(0, 2) - T06(0, 3);
    C = d[3];
    //第一个解，赋给一到四组
    theta[0][0] = atan2(A, B) - atan2(C, sqrt(A * A + B * B - C * C));
    theta[1][0] = theta[0][0];
    theta[2][0] = theta[0][0];
    theta[3][0] = theta[0][0];
    //第二个解，赋给五到八组
    theta[4][0] = atan2(A, B) - atan2(C, -sqrt(A * A + B * B - C * C));
    theta[5][0] = theta[4][0];
    theta[6][0] = theta[4][0];
    theta[7][0] = theta[4][0];

    //theta5 四个解
    //由theta[1][1]产生的第一个解，赋给一到二组
    A = sin(theta[0][0]) * T06(0, 2) - cos(theta[0][0]) * T06(1, 2);
    theta[0][4] = acos(A);
    theta[1][4] = theta[0][4];
    //由theta[1][1]产生的第二个解，赋给三到四组
    theta[2][4] = -acos(A);
    theta[3][4] = theta[2][4];
    //由theta[5][1]产生的第一个解，赋给五到六组
    A = sin(theta[4][0]) * T06(0, 2) - cos(theta[4][0]) * T06(1, 2);
    theta[4][4] = acos(A);
    theta[5][4] = theta[4][4];
    //由theta[5][1]产生的第二个解，赋给七到八组
    theta[6][4] = -acos(A);
    theta[7][4] = theta[6][4];

    //theta6 四个解
    for (int i = 0; i <8; i = i + 2)
    {
        A = (sin(theta[i][0]) * T06(0, 0) - cos(theta[i][0]) * T06(1, 0));
        B = (sin(theta[i][0]) * T06(0, 1) - cos(theta[i][0]) * T06(1, 1));
        C = sin(theta[i][4]);
        D = A * A + B * B - C * C;

        if ((C <= -0.00001) || (C >= 0.000001)) {
            theta[i][5] = atan2(A, B) - atan2(C, 0.00);
            theta[i + 1][5] = atan2(A, B) - atan2(C, 0.00);
        }
        else
        {
            theta[i][5] = 0;
            theta[i + 1][5] = 0;
        }

    }

    //theta3 8组解
    for (int i = 0; i <8; i = i + 2)
    {
        C = cos(theta[i][0]) * T06(0, 0) + sin(theta[i][0]) * T06(1, 0);
        D = cos(theta[i][0]) * T06(0, 1) + sin(theta[i][0]) * T06(1, 1);
        E = cos(theta[i][0]) * T06(0, 2) + sin(theta[i][0]) * T06(1, 2);
        F = cos(theta[i][0]) * T06(0, 3) + sin(theta[i][0]) * T06(1, 3);
        G = cos(theta[i][5]) * T06(2, 1) + sin(theta[i][5]) * T06(2, 0);
        A = d[4] * (sin(theta[i][5]) * C + cos(theta[i][5]) * D) - d[5] * E + F;
        B = T06(2, 3) - d[0] - T06(2, 2) * d[5] + d[4] * G;

        //theta3
        if (A * A + B * B <= (a[2 - 1] + a[3 - 1]) * (a[2 - 1] + a[3 - 1]))
        {
            theta[i][2] = acos((A * A + B * B - a[2 - 1] * a[2 - 1] - a[3 - 1] * a[3 - 1]) / (2 * a[2 - 1] * a[3 - 1]));
            theta[i + 1][2] = -theta[i][2];
        }
        else
        {
            theta[i][2] = 0;
            theta[i + 1][2] = 0;
        }
    }
    //theta2 theta4
    for (int i = 0; i < 8; ++i)
    {
        C = cos(theta[i][0]) * T06(0, 0) + sin(theta[i][0]) * T06(1, 0);
        D = cos(theta[i][0]) * T06(0, 1) + sin(theta[i][0]) * T06(1, 1);
        E = cos(theta[i][0]) * T06(0, 2) + sin(theta[i][0]) * T06(1, 2);
        F = cos(theta[i][0]) * T06(0, 3) + sin(theta[i][0]) * T06(1, 3);
        G = cos(theta[i][5]) * T06(2, 1) + sin(theta[i][5]) * T06(2, 0);
        A = d[4] * (sin(theta[i][5]) * C + cos(theta[i][5]) * D) - d[5] * E + F;
        B = T06(2, 3) - d[0] - T06(2, 2) * d[5] + d[4] * G;

        M = ((a[3 - 1] * cos(theta[i][2]) + a[2 - 1]) * B - a[3 - 1] * sin(theta[i][2]) * A) / (a[2 - 1] * a[2 - 1] + a[3 - 1] * a[3 - 1] + 2 * a[2 - 1] * a[3 - 1] * cos(theta[i][2]));
        N = (A + a[3 - 1] * sin(theta[i][2]) * M) / (a[3 - 1] * cos(theta[i][2]) + a[2 - 1]);
        theta[i][1] = atan2(M, N);

        //theta4
        auto th = atan2((-sin(theta[i][5]) * C - cos(theta[i][5]) * D), G) - theta[i][1] - theta[i][2] ;
        if(th > M_PI)
        {
            th -= 2*M_PI;
        }
        if(th < -M_PI)
        {
            th += 2*M_PI;
        }
        theta[i][3] = th;
    }

    for (int i = 0; i < 8; ++i)
    {
        std::cout << "[" << i << "] " ;
        for (int j = 0; j <6; ++j)
        {
            auto th = theta[i][j];
            //角度判断,如果不在[-180,180]调整
            if(th >M_PI)
            {
                th -= 2*M_PI;
            }
            if(th < -M_PI)
            {
                th += 2*M_PI;
            }
            theta[i][j] = th;
            std::cout << "theta" << j+1 << "=" << th *180/3.1415926 << "  ";
        }
        kinematics(theta[i]);
        std::cout<< std::endl;
    }

    return 0;
}