UR机器人逆运动学计算

  /// 
        /// UR机器人逆运动学运算
        /// 
        /// 末端位姿矩阵指针
        /// 6关节角度的8个解输出
        /// 求得解的数量
        public unsafe int Inverse(float* T, float* q_sols)
        {
            int num_sols = 0;
            float nx = *T; T++;
            float ox = *T; T++;
            float ax = *T; T++;
            float px = *T; T++;
            float ny = *T; T++;
            float oy = *T; T++;
            float ay = *T; T++;
            float py = *T; T++;
            float nz = *T; T++;
            float oz = *T; T++;
            float az = *T; T++;
            float pz = *T; T++;
            float[][] q = new float[6][];
            float[][] p = new float[6][];
            ////////////////////////////// J1,J5关节求解，并行两值 //////////////////////////////
            float[] p1 = new float[2];
            {
                float A = (-d6 * ay + py);
                float B = (-d6 * ax + px);
                float R = A * A + B * B;
                if (Math.Abs(A) < ZERO_THRESH)
                {
                    float div;
                    if (Math.Abs(Math.Abs(d4) - Math.Abs(B)) < ZERO_THRESH)
                        div = -Math.Sign(d4) * Math.Sign(B);
                    else
                        div = -d4 / B;
                    float arcsin = (float)Math.Asin(div);
                    if (Math.Abs(arcsin) < ZERO_THRESH)
                        arcsin = 0.0f;
                    if (arcsin < 0.0)
                        p1[0] = (float)(arcsin + 2.0 * Math.PI);
                    else
                        p1[0] = arcsin;
                    p1[1] = (float)(Math.PI - arcsin);
                }
                else if (Math.Abs(B) < ZERO_THRESH)
                {
                    float div;
                    if (Math.Abs(Math.Abs(d4) - Math.Abs(A)) < ZERO_THRESH)
                        div = Math.Sign(d4) * Math.Sign(A);
                    else
                        div = d4 / A;
                    float arccos = (float)Math.Acos(div);
                    p1[0] = arccos;
                    p1[1] = (float)(2.0 * Math.PI - arccos);
                }
                else if (d4 * d4 > R)
                {
                    return num_sols;
                }
                else
                {
                    float arccos = (float)Math.Acos(d4 / Math.Sqrt(R));
                    float arctan = (float)Math.Atan2(-B, A);
                    float pos = arccos + arctan;
                    float neg = -arccos + arctan;
                    if (Math.Abs(pos) < ZERO_THRESH)
                        pos = 0.0f;
                    if (Math.Abs(neg) < ZERO_THRESH)
                        neg = 0.0f;
                    if (pos >= 0.0)
                        p1[0] = pos;
                    else
                        p1[0] = (float)(2.0 * Math.PI + pos);
                    if (neg >= 0.0)
                        p1[1] = neg;
                    else
                        p1[1] = (float)(2.0 * Math.PI + neg);
                }
            }
            float[][] p5 = new float[2][];
            p5[0] = new float[2];
            p5[1] = new float[2];
            {
                for (int i = 0; i < 2; i++)
                {
                    ///T2345 ((-s1) * (ax)+(c1) * (ay))=s5
                    float div = (-ax * (float)Math.Sin(p1) + ay * (float)Math.Cos(p1));
                    float arcsin = (float)Math.Asin(div);
                    p5[0] = arcsin;
                    p5[1] = (float)(2.0 * Math.PI + arcsin);
                }
            }
            for (int i = 0; i < 2; i++)
            {
                for (int j = 0; j < 2; j++)
                {
                    float c1 = (float)Math.Cos(p1), s1 = (float)Math.Sin(p1);
                    float c5 = (float)Math.Cos(p5[j]), s5 = (float)Math.Sin(p5[j]);
                    ////////////////////////////// 利用T234矩阵求解一个J6 //////////////////////////////
                    ///((s1) * (nx)-(c1) * (ny)) * s6 + (-s1 * ox + c1 * oy) * c6 = c5
                    float q6;
                    if (Math.Abs(s5) < ZERO_THRESH)
                        q6 = (float)Math.Atan2((nx * s1 - ny * c1), (-ox * s1 + oy * c1));
                    else
                        q6 = (float)Math.Atan2((nx * s1 - ny * c1)/c5, (-ox * s1 + oy * c1)/c5);
                    ////////////////////////////////////////////////////////////////////////////////
                    float[] p2 = new float[2], p3 = new float[2], p4 = new float[2];
                    ///////////////////////////// 利用T234求解J2,J3,J4各两值////////////////////////////
                    ///-A3s2s3+A3c2c3+A2s2=mx=c23A3+A2s2
                    /// A3c2s3+A3s2c3-A2c2=my=s23A3-A2C2
                    ///mx^2 + my^2 = A3^2 + A2^2 + 2A2A3(c23s2-s23c2)=A3^2 + A2^2 - 2A2A3s3
                    ///kx=nx ky=ny
                    ///kx=((s2) * (-s3)+(c2) * (c3)) * (c4)+((s2) * (-c3)+(c2) * (-s3)) * (s4) =c23c4-s23s4=c234
                    ///ky=((-c2) * (-s3)+(s2) * (c3)) * (c4)+((-c2) * (-c3)+(s2) * (-s3)) * (s4)=s23c4+c23s4=s234
                    ///kxc23+kys23=c4
                    ///kxs23-kyc23=-s4
                    float s6 = (float)Math.Sin(q6), c6 = (float)Math.Cos(q6);
                    float mx =   -d5 * (c6 * (c1 * nx + s1 * ny) + s6 * (c1 * ox + s1 * oy)) - d6 * (c1 * ax + s1 * ay) + c1 * px + s1 * py;
                    float my = d5 * (nz * c6 + oz * s6) + d6 * az - pz + d1;
                    float kx = s5 * (s6 * (c1 * nx + s1 * ny) - c6 * (c1 * ox + s1 * oy)) + (c1 * ax + s1 * ay) * c5;
                    float ky = s5 * (-nz * s6 + oz * c6) - az * c5;
                    float s3 = -(mx * mx + my * my - a2 * a2 - a3 * a3) / (2.0f * a2 * a3);
                    if (Math.Abs(Math.Abs(s3) - 1.0) < ZERO_THRESH)
                        s3 = Math.Sign(s3);
                    else if (Math.Abs(s3) > 1.0)
                    {
                        continue;
                    }
                    float arcsin = (float)Math.Asin(s3);
                    p3[0] = arcsin;
                    p3[1] = (float)(Math.PI - arcsin);
                    float c3 = (float)Math.Cos(arcsin);
                    float A = (a2 - a3 * s3), B = a3 * c3;
                    float denom = a2 * a2 + a3 * a3 - 2 * a2 * a3 * s3;//A*A+B*B
                    float tmm = A * mx + B * my;
                    p2[0] = (float)Math.Atan2((A * mx + B * my) / denom, (-A * my + B * mx ) / denom);
                    p2[1] = (float)(Math.Atan2((A * mx - B * my) / denom, (-A * my - B * mx) / denom));
                    float c23_0 = (float)Math.Cos(p2[0] + p3[0]);
                    float s23_0 = (float)Math.Sin(p2[0] + p3[0]);
                    float c23_1 = (float)Math.Cos(p2[1] + p3[1]);
                    float s23_1 = (float)Math.Sin(p2[1] + p3[1]);
                    p4[0] = (float)Math.Atan2(c23_0 * ky - s23_0 * kx, kx * c23_0 + ky * s23_0);
                    p4[1] = (float)Math.Atan2(c23_1 * ky - s23_1 * kx, kx * c23_1 + ky * s23_1);
                    ////////////////////////////////////////////////////////////////////////////////
                    for (int k = 0; k < 2; k++)
                    {
                        if (Math.Abs(p2[k]) < ZERO_THRESH)
                            p2[k] = 0.0f;
                        if (Math.Abs(p4[k]) < ZERO_THRESH)
                            p4[k] = 0.0f;
                        else if (p4[k] < 0.0) p4[k] += (float)(2.0 * Math.PI);
                        q_sols[num_sols * 6 + 0] = p1; q_sols[num_sols * 6 + 1] = p2[k];
                        q_sols[num_sols * 6 + 2] = p3[k]; q_sols[num_sols * 6 + 3] = p4[k];
                        q_sols[num_sols * 6 + 4] = p5[j]; q_sols[num_sols * 6 + 5] = q6;
                        num_sols++;
                    }
                }
            }
            return num_sols;
        }