UR6机器人雅可比矩阵及微分运算

        /// <summary>
        /// UR六关节机器人雅可比矩阵
        /// </summary>
        /// <param name="q">每个关节的角度值数组指针</param>
        /// <param name="T">雅可比矩阵指针</param>
        public unsafe void Jacobian(float* q, float* T)
        {
            float s1 = (float)Math.Sin(*q), c1 = (float)Math.Cos(*q); q++; // q1
            float q23 = *q, q234 = *q, s2 = (float)Math.Sin(*q), c2 = (float)Math.Cos(*q); q++; // q2
            float s3 = (float)Math.Sin(*q), c3 = (float)Math.Cos(*q); q23 += *q; q234 += *q; q++; // q3
            q234 += *q; q++; // q4
            float s5 = (float)Math.Sin(*q), c5 = (float)Math.Cos(*q); q++; // q5
            float s6 = (float)Math.Sin(*q), c6 = (float)Math.Cos(*q); // q6
            float s23 = (float)Math.Sin(q23), c23 = (float)Math.Cos(q23);
            float s234 = (float)Math.Sin(q234), c234 = (float)Math.Cos(q234);

            *T = (-s1 * c234 * c5 - c1 * s5) * d6 - s1 * s234 * d5 - c1 * d4 - s1 * c23 * a3 - s1 * a2 * s2;T++;
            *T = (-c1 * s234 * c5) * d6 + c1 * c234 * d5 - c1 * s23 * a3 + c1 * a2 * c2; T++;
            *T = (-c1 * s234 * c5) * d6 + c1 * c234 * d5 - c1 * s23 * a3; T++;
            *T = (-c1 * s234 * c5) * d6 + c1 * c234 * d5; T++;
            *T = (-c1 * c234 * s5 - s1 * c5) * d6; T++;
            *T = 0; T++;

            *T = (c1 * c234 * c5 - s1 * s5) * d6 + c1 * s234 * d5 - s1 * d4 + c1 * c23 * a3 + c1 * a2 * s2;T++;
            *T = (-s1 * s234 * c5) * d6 + s1 * c234 * d5 - s1 * s23 * a3 + s1 * a2 * c2; T++;
            *T = (-s1 * s234 * c5) * d6 + s1 * c234 * d5 - s1 * s23 * a3; T++;
            *T = (-s1 * s234 * c5) * d6 + s1 * c234 * d5; T++;
            *T = (-s1 * c234 * s5 + c1 * c5) * d6; T++;
            *T = 0; T++;

            *T = 0;T++;
            *T = -c234 * c5 * d6 - s234 * d5 - a3 * c23 - a2 * s2; T++;
            *T = -c234 * c5 * d6 - s234 * d5 - a3 * c23; T++;
            *T = -c234 * c5 * d6 - s234 * d5; T++;
            *T = s234 * s5 * d6; T++;
            *T = 0; T++;

            *T = 0; T++;
            *T = -s1; T++;
            *T = -s1; T++;
            *T = -s1; T++;
            *T = s234 * c1; T++;
            *T = c1 * c234 * c5 - s1 * s5; T++;

            *T = 0; T++;
            *T = c1; T++;
            *T = c1; T++;
            *T = c1; T++;
            *T = s234 * s1; T++;
            *T = s1 * c234 * c5 + c1 * s5; T++;

            *T = 1; T++;
            *T = 0; T++;
            *T = 0; T++;
            *T = 0; T++;
            *T = c234; T++;
            *T = -s234 * c5; T++;
        }

        /// <summary>
        /// UR六关节机器人关节微分运算(高斯约旦消元法求逆)
        /// </summary>
        /// <param name="q">每个关节的角度值</param>
        /// <param name="diffMat">末端空间位置姿态微分数组[dpx,dpy,dpz,δx,δy,δz]</param>
        /// <returns>六关节角度微分数组[dӨ1,dӨ2,dӨ3,dӨ4,dӨ5,dӨ6]</returns>
        public unsafe float[] Differential(float[] q, float[] diffMat)
        {
            float[] A = diffMat;
            float[,] JacobianMat = new float[6, 6];
            fixed (float* t = JacobianMat,p = q)
            {
                float* J = t;
                Jacobian(p, J);
                for (int i = 0; i < 6; i++)
                {
                    float m = J[i*6+i];
                    for (int j = i; j < 6; j++)
                    {
                        J[i*6+ j] /= m;
                    }
                    A[i] /= m;
                    for (int k = 0; k < 6; k++)
                    {
                        if (k != i)
                        {
                            m = J[k*6+ i];
                            for (int l = i; l < 6; l++)
                            {
                                J[k*6+ l] -= m * J[i*6+ l];
                            }
                            A[k] -= m * A[i];
                        }
                    }
                }
            }
            return A;
        }