예제 #1
0
        private void MultiplyBtn_Click(object sender, EventArgs e)
        {
            SetMatrixAB();
            Matrix MatrixResult = MatrixOperator.MultiplyMatrices(MatrixA, MatrixB);

            DisplayMatrix(MatrixResult, resultText);
        }
예제 #2
0
        public object Convert(object value, Type targetType, object parameter, CultureInfo culture)
        {
            try {
                MatrixOperator op = (MatrixOperator)value;
                switch (op)
                {
                case MatrixOperator.Add:
                    return("+");

                case MatrixOperator.Sub:
                    return("-");

                case MatrixOperator.Multiply:
                    return("x");

                case MatrixOperator.First:
                    return("<");

                case MatrixOperator.Second:
                    return(">");

                default:
                    return("<");
                }
            }
            catch {
                return("<");
            }
        }
예제 #3
0
    //矩阵求逆(伴随矩阵法)
    public static Matrix MatrixInvByCom(Matrix Ma)
    {
        double d = MatrixOperator.MatrixDet(Ma);

        if (d == 0)
        {
            Exception myException = new Exception("没有逆矩阵");
            throw myException;
        }
        Matrix Ax = MatrixOperator.MatrixCom(Ma);
        Matrix An = MatrixOperator.MatrixSimpleMulti((1.0 / d), Ax);

        return(An);
    }
예제 #4
0
    public static void Main()
    {
        string[] args = Console.ReadLine()
                        .Split(new[] { ' ' }, StringSplitOptions.RemoveEmptyEntries);

        ArrayParser parser = new ArrayParser();

        int[] dimensions = parser.IntegersParser(args);
        int   rows       = dimensions[0];
        int   cols       = dimensions[1];

        MatrixGenerator matrixGenerator = new MatrixGenerator();
        MatrixOperator  matrixOperator  = new MatrixOperator();

        int[,] matrix = matrixGenerator.GenerateIntegersMatrix(rows, cols);

        string command = null;
        long   sum     = 0;

        while ((command = Console.ReadLine()) != "Let the Force be with you")
        {
            string[] ivoArgs        = command.Split();
            int[]    ivoCoordinates = parser.IntegersParser(ivoArgs);

            string[] evilArgs        = Console.ReadLine().Split();
            int[]    evilCoordinates = parser.IntegersParser(evilArgs);

            int evilRow = evilCoordinates[0];
            int evilCol = evilCoordinates[1];

            matrixOperator.DeleteElementsFromBottomRightToUpperLeft(matrix, evilRow, evilCol);

            int ivoRow = ivoCoordinates[0];
            int ivoCol = ivoCoordinates[1];

            sum += matrixOperator.SumElementsFromBottomLeftToUpperRight(matrix, ivoRow, ivoCol);
        }

        Console.WriteLine(sum);
    }
예제 #5
0
파일: TestP.cs 프로젝트: cxjxjxj/Robot
    private static void DealOrinData2(ref Matrix returnData)
    {
        Matrix TempMatrix = new Matrix(4, 4, "TempMatrix");

        TempMatrix.Detail[0, 0] = 1;
        TempMatrix.Detail[0, 1] = 0;
        TempMatrix.Detail[0, 2] = 0;
        TempMatrix.Detail[0, 3] = 0;
        TempMatrix.Detail[1, 0] = 0;
        TempMatrix.Detail[1, 1] = 1;
        TempMatrix.Detail[1, 2] = 0;
        TempMatrix.Detail[1, 3] = 0;
        TempMatrix.Detail[2, 0] = 0;
        TempMatrix.Detail[2, 1] = 0;
        TempMatrix.Detail[2, 2] = 1;
        TempMatrix.Detail[2, 3] = -215;
        TempMatrix.Detail[3, 0] = 0;
        TempMatrix.Detail[3, 1] = 0;
        TempMatrix.Detail[3, 2] = 0;
        TempMatrix.Detail[3, 3] = 1;

        returnData = MatrixOperator.MatrixMulti(returnData, MatrixOperator.MatrixInvByCom(TempMatrix));
    }
예제 #6
0
    /// <summary>
    /// 计算theta4、6
    /// </summary>
    /// <param name="theta1"></param>
    /// <param name="theta2"></param>
    /// <param name="theta3"></param>
    /// <param name="theta5"></param>
    /// <param name="PValue"></param>
    /// <param name="T06"></param>
    /// <param name="i"></param>
    /// <param name="theta"></param>
    private static void CalculateTheta4_6(double theta1, double theta2, double theta3, double theta5, double[] PValue, Matrix T06, int i, double[] oldtheta, ref double[,] theta)
    {
        Debug.Log("============================计算theta4_6============================");
        #region//前三个矩阵
        Matrix T01 = new Matrix(4, 4, "T01");
        T01.Detail[0, 0] = Math.Cos(theta1 / 180D * Math.PI);
        T01.Detail[0, 1] = -Math.Sin(theta1 / 180D * Math.PI);
        T01.Detail[0, 2] = 0;
        T01.Detail[0, 3] = 0;
        T01.Detail[1, 0] = Math.Sin(theta1 / 180D * Math.PI);
        T01.Detail[1, 1] = Math.Cos(theta1 / 180D * Math.PI);
        T01.Detail[1, 2] = 0;
        T01.Detail[1, 3] = 0;
        T01.Detail[2, 0] = 0;
        T01.Detail[2, 1] = 0;
        T01.Detail[2, 2] = 1;
        T01.Detail[2, 3] = PValue[0];
        T01.Detail[3, 0] = 0;
        T01.Detail[3, 1] = 0;
        T01.Detail[3, 2] = 0;
        T01.Detail[3, 3] = 1;

        Matrix T12 = new Matrix(4, 4, "T12");
        T12.Detail[0, 0] = Math.Sin(theta2 / 180D * Math.PI);
        T12.Detail[0, 1] = Math.Cos(theta2 / 180D * Math.PI);
        T12.Detail[0, 2] = 0;
        T12.Detail[0, 3] = PValue[1];
        T12.Detail[1, 0] = 0;
        T12.Detail[1, 1] = 0;
        T12.Detail[1, 2] = 1;
        T12.Detail[1, 3] = 0;
        T12.Detail[2, 0] = Math.Cos(theta2 / 180D * Math.PI);;
        T12.Detail[2, 1] = -Math.Sin(theta2 / 180D * Math.PI);
        T12.Detail[2, 2] = 0;
        T12.Detail[2, 3] = 0;
        T12.Detail[3, 0] = 0;
        T12.Detail[3, 1] = 0;
        T12.Detail[3, 2] = 0;
        T12.Detail[3, 3] = 1;

        Matrix T23 = new Matrix(4, 4, "T23");
        T23.Detail[0, 0] = Math.Cos(theta3 / 180D * Math.PI);
        T23.Detail[0, 1] = -Math.Sin(theta3 / 180D * Math.PI);
        T23.Detail[0, 2] = 0;
        T23.Detail[0, 3] = PValue[3];
        T23.Detail[1, 0] = -Math.Sin(theta3 / 180D * Math.PI);
        T23.Detail[1, 1] = -Math.Cos(theta3 / 180D * Math.PI);
        T23.Detail[1, 2] = 0;
        T23.Detail[1, 3] = 0;
        T23.Detail[2, 0] = 0;;
        T23.Detail[2, 1] = 0;
        T23.Detail[2, 2] = -1;
        T23.Detail[2, 3] = 0;
        T23.Detail[3, 0] = 0;
        T23.Detail[3, 1] = 0;
        T23.Detail[3, 2] = 0;
        T23.Detail[3, 3] = 1;

        #endregion
        Matrix iT03_T06 = MatrixOperator.MatrixMulti(MatrixOperator.MatrixInvByCom(MatrixOperator.MatrixMulti(MatrixOperator.MatrixMulti(T01, T12), T23)), T06);

        if (Math.Floor(Math.Abs(theta5)) == 0)
        {
            Debug.Log("theta5为0,其值为:" + theta5);
            theta[i, 3] = oldtheta[3];
            theta[i, 5] = Math.Atan2(-iT03_T06.Detail[2, 0], -iT03_T06.Detail[2, 1]) / Math.PI * 180 - oldtheta[3];
        }
        else
        {
            theta[i, 3] = Math.Atan2(iT03_T06.Detail[2, 2] * Math.Sin(theta5 / 180D * Math.PI), -iT03_T06.Detail[0, 2] * Math.Sin(theta5 / 180D * Math.PI)) / Math.PI * 180;
            theta[i, 5] = Math.Atan2(-iT03_T06.Detail[1, 1] * Math.Sin(theta5 / 180D * Math.PI), iT03_T06.Detail[1, 0] * Math.Sin(theta5 / 180D * Math.PI)) / Math.PI * 180;
        }
        Debug.Log("============================结束theta4_6============================");
    }
예제 #7
0
    /// <summary>
    /// 计算theta5
    /// </summary>
    /// <param name="theta1"></param>
    /// <param name="theta2"></param>
    /// <param name="theta3"></param>
    /// <param name="PValue"></param>
    /// <param name="T06"></param>
    /// <param name="i"></param>
    /// <param name="theta"></param>
    private static void CalculateTheta5(double theta1, double theta2, double theta3, double[] PValue, Matrix T06, int i, ref double[,] theta)
    {
        Debug.Log("============================计算theta5============================");
        #region//前三个矩阵
        Matrix T01 = new Matrix(4, 4, "T01");
        T01.Detail[0, 0] = Math.Cos(theta1 / 180D * Math.PI);
        T01.Detail[0, 1] = -Math.Sin(theta1 / 180D * Math.PI);
        T01.Detail[0, 2] = 0;
        T01.Detail[0, 3] = 0;
        T01.Detail[1, 0] = Math.Sin(theta1 / 180D * Math.PI);
        T01.Detail[1, 1] = Math.Cos(theta1 / 180D * Math.PI);
        T01.Detail[1, 2] = 0;
        T01.Detail[1, 3] = 0;
        T01.Detail[2, 0] = 0;
        T01.Detail[2, 1] = 0;
        T01.Detail[2, 2] = 1;
        T01.Detail[2, 3] = PValue[0];
        T01.Detail[3, 0] = 0;
        T01.Detail[3, 1] = 0;
        T01.Detail[3, 2] = 0;
        T01.Detail[3, 3] = 1;

        Matrix T12 = new Matrix(4, 4, "T12");
        T12.Detail[0, 0] = Math.Sin(theta2 / 180D * Math.PI);
        T12.Detail[0, 1] = Math.Cos(theta2 / 180D * Math.PI);
        T12.Detail[0, 2] = 0;
        T12.Detail[0, 3] = PValue[1];
        T12.Detail[1, 0] = 0;
        T12.Detail[1, 1] = 0;
        T12.Detail[1, 2] = 1;
        T12.Detail[1, 3] = 0;
        T12.Detail[2, 0] = Math.Cos(theta2 / 180D * Math.PI);;
        T12.Detail[2, 1] = -Math.Sin(theta2 / 180D * Math.PI);
        T12.Detail[2, 2] = 0;
        T12.Detail[2, 3] = 0;
        T12.Detail[3, 0] = 0;
        T12.Detail[3, 1] = 0;
        T12.Detail[3, 2] = 0;
        T12.Detail[3, 3] = 1;

        Matrix T23 = new Matrix(4, 4, "T23");
        T23.Detail[0, 0] = Math.Cos(theta3 / 180D * Math.PI);
        T23.Detail[0, 1] = -Math.Sin(theta3 / 180D * Math.PI);
        T23.Detail[0, 2] = 0;
        T23.Detail[0, 3] = PValue[3];
        T23.Detail[1, 0] = -Math.Sin(theta3 / 180D * Math.PI);
        T23.Detail[1, 1] = -Math.Cos(theta3 / 180D * Math.PI);
        T23.Detail[1, 2] = 0;
        T23.Detail[1, 3] = 0;
        T23.Detail[2, 0] = 0;;
        T23.Detail[2, 1] = 0;
        T23.Detail[2, 2] = -1;
        T23.Detail[2, 3] = 0;
        T23.Detail[3, 0] = 0;
        T23.Detail[3, 1] = 0;
        T23.Detail[3, 2] = 0;
        T23.Detail[3, 3] = 1;

        #endregion

        Matrix iT03_T06 = MatrixOperator.MatrixMulti(MatrixOperator.MatrixInvByCom(MatrixOperator.MatrixMulti(MatrixOperator.MatrixMulti(T01, T12), T23)), T06);

        double c5 = iT03_T06.Detail[1, 2];
        double s5 = Math.Sqrt(1 - Math.Pow(c5, 2));
        if (i % 2 != 0)
        {
            s5 = -Math.Sqrt(1 - Math.Pow(c5, 2));
        }
        theta[i, 4] = Math.Atan2(s5, c5) / Math.PI * 180;

        Debug.Log("============================结束theta5============================");
    }
예제 #8
0
    /// <summary>
    /// 输入数据xyzwpr换算为T06的计算
    /// </summary>
    /// <param name="IKInput"></param>
    /// <returns></returns>
    private static Matrix DealOrinData(double[] IKInput)
    {
        Debug.Log("开始==================输入数据处理为矩阵=====================");
        #region                                         //变量定义
        Matrix returnData = new Matrix(4, 4, "T06");    //T06:{6}在{0}下的位姿
        Matrix XW         = new Matrix(3, 3, "R(X,W)"); //沿着x旋转的矩阵
        Matrix YP         = new Matrix(3, 3, "R(Y,P)"); //沿着Y旋转的矩阵
        Matrix ZR         = new Matrix(3, 3, "R(Z,R)"); //沿着Z旋转的矩阵
        Matrix temp       = new Matrix(3, 3, "R06");    //过渡
        #endregion

        #region//X的赋值
        XW.Detail[0, 0] = 1;
        XW.Detail[0, 1] = 0;
        XW.Detail[0, 2] = 0;
        XW.Detail[1, 0] = 0;
        XW.Detail[1, 1] = Math.Cos(IKInput[3] / 180D * Math.PI);//度对应的数
        XW.Detail[1, 2] = -Math.Sin(IKInput[3] / 180D * Math.PI);
        XW.Detail[2, 0] = 0;
        XW.Detail[2, 1] = Math.Sin(IKInput[3] / 180D * Math.PI);
        XW.Detail[2, 2] = Math.Cos(IKInput[3] / 180D * Math.PI);
        #endregion

        #region//Y的赋值
        YP.Detail[0, 0] = Math.Cos(IKInput[4] / 180D * Math.PI);
        YP.Detail[0, 1] = 0;
        YP.Detail[0, 2] = Math.Sin(IKInput[4] / 180D * Math.PI);
        YP.Detail[1, 0] = 0;
        YP.Detail[1, 1] = 1;
        YP.Detail[1, 2] = 0;
        YP.Detail[2, 0] = -Math.Sin(IKInput[4] / 180D * Math.PI);
        YP.Detail[2, 1] = 0;
        YP.Detail[2, 2] = Math.Cos(IKInput[4] / 180D * Math.PI);
        #endregion

        #region//Z的赋值
        ZR.Detail[0, 0] = Math.Cos(IKInput[5] / 180D * Math.PI);
        ZR.Detail[0, 1] = -Math.Sin(IKInput[5] / 180D * Math.PI);
        ZR.Detail[0, 2] = 0;
        ZR.Detail[1, 0] = Math.Sin(IKInput[5] / 180D * Math.PI);
        ZR.Detail[1, 1] = Math.Cos(IKInput[5] / 180D * Math.PI);
        ZR.Detail[1, 2] = 0;
        ZR.Detail[2, 0] = 0;
        ZR.Detail[2, 1] = 0;
        ZR.Detail[2, 2] = 1;
        #endregion

        #region//数据精度整理
        for (int i = 0; i < 3; i++)
        {
            for (int j = 0; j < 3; j++)
            {
                if (Math.Abs(XW.Detail[i, j]) < 0.0000001)
                {
                    XW.Detail[i, j] = 0;
                }
                if (Math.Abs(YP.Detail[i, j]) < 0.0000001)
                {
                    YP.Detail[i, j] = 0;
                }
                if (Math.Abs(ZR.Detail[i, j]) < 0.0000001)
                {
                    ZR.Detail[i, j] = 0;
                }
            }
        }
        #endregion

        #region//XYZ旋转矩阵数据计算打印
        //for (int i = 0; i < 3; i++)
        //{
        //    Debug.Log("XW" + i + ": " + XW.Detail[i, 0] + " " + XW.Detail[i, 1] + " " + XW.Detail[i, 2]);
        //    Debug.Log("YP" + i + ": " + YP.Detail[i, 0] + " " + YP.Detail[i, 1] + " " + YP.Detail[i, 2]);
        //    Debug.Log("ZR" + i + ": " + ZR.Detail[i, 0] + " " + ZR.Detail[i, 1] + " " + ZR.Detail[i, 2]);
        //}
        #endregion

        temp = MatrixOperator.MatrixMulti(MatrixOperator.MatrixMulti(ZR, YP), XW);

        #region//赋值:T其余数值
        for (int i = 0; i < returnData.getM - 1; i++)
        {
            for (int j = 0; j < returnData.getN - 1; j++)
            {
                returnData.Detail[i, j] = temp.Detail[i, j];
            }
        }
        returnData.Detail[0, 3] = IKInput[0];
        returnData.Detail[1, 3] = IKInput[1];
        returnData.Detail[2, 3] = IKInput[2];
        returnData.Detail[3, 0] = 0;
        returnData.Detail[3, 1] = 0;
        returnData.Detail[3, 2] = 0;
        returnData.Detail[3, 3] = 1;
        #endregion

        DealOrinData2(ref returnData);

        #region//T的打印
        for (int i = 0; i < returnData.getM; i++)
        {
            Debug.Log("第" + i + "行: " + returnData.Detail[i, 0] + " " + returnData.Detail[i, 1] + " "
                      + returnData.Detail[i, 2] + " " + returnData.Detail[i, 3]);
        }
        #endregion
        Debug.Log("结束==================输入数据处理为矩阵=====================");
        return(returnData);
    }
예제 #9
0
    /// <summary>
    /// 机器人正解
    /// </summary>
    /// <param name="theta">输入的六个关节角度</param>
    /// <param name="staticValue">DH链表中机器人的尺寸</param>
    /// <param name="TempMatrix">6个旋转矩阵的初始化</param>
    /// <returns>正解得出的T06</returns>
    private static Matrix FK(double[] theta, double[] staticValue, ref List <Matrix> TempMatrix)
    {
        Debug.Log("============================计算正解============================");
        Debug.Log("所输入的角度为:" + theta[0] + " " + theta[1] + " " + theta[2] + " "
                  + theta[3] + " " + theta[4] + " " + theta[5] + " ");

        Matrix ReturnMatrix = new Matrix(4, 4, "正解T06");

        #region//给6个位置矩阵赋值
        for (int i = 0; i < TempMatrix.Count; i++)
        {
            switch (i)
            {
            case 0:
                TempMatrix[0].Detail[0, 0] = Math.Cos(theta[0]);
                TempMatrix[0].Detail[0, 1] = -Math.Sin(theta[0]);
                TempMatrix[0].Detail[0, 2] = 0;
                TempMatrix[0].Detail[0, 3] = 0;
                TempMatrix[0].Detail[1, 0] = Math.Sin(theta[0]);
                TempMatrix[0].Detail[1, 1] = Math.Cos(theta[0]);
                TempMatrix[0].Detail[1, 2] = 0;
                TempMatrix[0].Detail[1, 3] = 0;
                TempMatrix[0].Detail[2, 0] = 0;
                TempMatrix[0].Detail[2, 1] = 0;
                TempMatrix[0].Detail[2, 2] = 1;
                TempMatrix[0].Detail[2, 3] = staticValue[0];
                TempMatrix[0].Detail[3, 0] = 0;
                TempMatrix[0].Detail[3, 1] = 0;
                TempMatrix[0].Detail[3, 2] = 0;
                TempMatrix[0].Detail[3, 3] = 1;
                break;

            case 1:
                TempMatrix[1].Detail[0, 0] = Math.Sin(theta[1]);
                TempMatrix[1].Detail[0, 1] = Math.Cos(theta[1]);
                TempMatrix[1].Detail[0, 2] = 0;
                TempMatrix[1].Detail[0, 3] = staticValue[1];
                TempMatrix[1].Detail[1, 0] = 0;
                TempMatrix[1].Detail[1, 1] = 0;
                TempMatrix[1].Detail[1, 2] = 1;
                TempMatrix[1].Detail[1, 3] = 0;
                TempMatrix[1].Detail[2, 0] = Math.Cos(theta[1]);
                TempMatrix[1].Detail[2, 1] = -Math.Sin(theta[1]);
                TempMatrix[1].Detail[2, 2] = 0;
                TempMatrix[1].Detail[2, 3] = 0;
                TempMatrix[1].Detail[3, 0] = 0;
                TempMatrix[1].Detail[3, 1] = 0;
                TempMatrix[1].Detail[3, 2] = 0;
                TempMatrix[1].Detail[3, 3] = 1;
                break;

            case 2:

                //TempMatrix[2].Detail[0, 0] = Math.Cos(theta[2] + theta[1]);用于没使用最优解时
                //TempMatrix[2].Detail[0, 1] = -Math.Sin(theta[2] + theta[1]);
                TempMatrix[2].Detail[0, 0] = Math.Cos(theta[2]);
                TempMatrix[2].Detail[0, 1] = -Math.Sin(theta[2]);
                TempMatrix[2].Detail[0, 2] = 0;
                TempMatrix[2].Detail[0, 3] = staticValue[2];
                //TempMatrix[2].Detail[1, 0] = -Math.Sin(theta[2] + theta[1]);
                //TempMatrix[2].Detail[1, 1] = -Math.Cos(theta[2] + theta[1]);
                TempMatrix[2].Detail[1, 0] = -Math.Sin(theta[2]);
                TempMatrix[2].Detail[1, 1] = -Math.Cos(theta[2]);
                TempMatrix[2].Detail[1, 2] = 0;
                TempMatrix[2].Detail[1, 3] = 0;
                TempMatrix[2].Detail[2, 0] = 0;
                TempMatrix[2].Detail[2, 1] = 0;
                TempMatrix[2].Detail[2, 2] = -1;
                TempMatrix[2].Detail[2, 3] = 0;
                TempMatrix[2].Detail[3, 0] = 0;
                TempMatrix[2].Detail[3, 1] = 0;
                TempMatrix[2].Detail[3, 2] = 0;
                TempMatrix[2].Detail[3, 3] = 1;
                break;

            case 3:
                TempMatrix[3].Detail[0, 0] = Math.Cos(theta[3]);
                TempMatrix[3].Detail[0, 1] = -Math.Sin(theta[3]);
                TempMatrix[3].Detail[0, 2] = 0;
                TempMatrix[3].Detail[0, 3] = staticValue[3];
                TempMatrix[3].Detail[1, 0] = 0;
                TempMatrix[3].Detail[1, 1] = 0;
                TempMatrix[3].Detail[1, 2] = 1;
                TempMatrix[3].Detail[1, 3] = -staticValue[4];
                TempMatrix[3].Detail[2, 0] = -Math.Sin(theta[3]);;
                TempMatrix[3].Detail[2, 1] = -Math.Cos(theta[3]);
                TempMatrix[3].Detail[2, 2] = 0;
                TempMatrix[3].Detail[2, 3] = 0;
                TempMatrix[3].Detail[3, 0] = 0;
                TempMatrix[3].Detail[3, 1] = 0;
                TempMatrix[3].Detail[3, 2] = 0;
                TempMatrix[3].Detail[3, 3] = 1;
                break;

            case 4:
                TempMatrix[4].Detail[0, 0] = Math.Cos(theta[4]);
                TempMatrix[4].Detail[0, 1] = -Math.Sin(theta[4]);
                TempMatrix[4].Detail[0, 2] = 0;
                TempMatrix[4].Detail[0, 3] = 0;
                TempMatrix[4].Detail[1, 0] = 0;
                TempMatrix[4].Detail[1, 1] = 0;
                TempMatrix[4].Detail[1, 2] = -1;
                TempMatrix[4].Detail[1, 3] = 0;
                TempMatrix[4].Detail[2, 0] = Math.Sin(theta[4]);;
                TempMatrix[4].Detail[2, 1] = Math.Cos(theta[4]);
                TempMatrix[4].Detail[2, 2] = 0;
                TempMatrix[4].Detail[2, 3] = 0;
                TempMatrix[4].Detail[3, 0] = 0;
                TempMatrix[4].Detail[3, 1] = 0;
                TempMatrix[4].Detail[3, 2] = 0;
                TempMatrix[4].Detail[3, 3] = 1;
                break;

            case 5:
                TempMatrix[5].Detail[0, 0] = Math.Cos(theta[5]);
                TempMatrix[5].Detail[0, 1] = -Math.Sin(theta[5]);
                TempMatrix[5].Detail[0, 2] = 0;
                TempMatrix[5].Detail[0, 3] = 0;
                TempMatrix[5].Detail[1, 0] = 0;
                TempMatrix[5].Detail[1, 1] = 0;
                TempMatrix[5].Detail[1, 2] = 1;
                TempMatrix[5].Detail[1, 3] = 0;
                TempMatrix[5].Detail[2, 0] = -Math.Sin(theta[5]);;
                TempMatrix[5].Detail[2, 1] = -Math.Cos(theta[5]);
                TempMatrix[5].Detail[2, 2] = 0;
                TempMatrix[5].Detail[2, 3] = 0;
                TempMatrix[5].Detail[3, 0] = 0;
                TempMatrix[5].Detail[3, 1] = 0;
                TempMatrix[5].Detail[3, 2] = 0;
                TempMatrix[5].Detail[3, 3] = 1;
                break;

            default:
                break;
            }
        }
        #endregion
        #region//打印6个赋值了的位姿矩阵,看是否正确
        //for (int j = 0; j < TempMatrix.Count; j++)
        //{
        //    Matrix item = TempMatrix[j];
        //    Debug.Log("这是矩阵:" + item.Name);
        //    for (int i = 0; i < item.getM; i++)
        //    {
        //        Debug.Log("第" + i + "行:" + item.Detail[i, 0] + "  " + item.Detail[i, 1] + "  " + item.Detail[i, 2]);
        //    }
        //}
        #endregion

        #region//6个位置矩阵相乘

        ReturnMatrix = TempMatrix[0];
        for (int i = 0; i < TempMatrix.Count - 1; i++)
        {
            ReturnMatrix = MatrixOperator.MatrixMulti(ReturnMatrix, TempMatrix[i + 1]);
        }
        #endregion

        for (int i = 0; i < ReturnMatrix.getM; i++)
        {
            Debug.Log(ReturnMatrix.Name + "的第" + i + "行:" + ReturnMatrix.Detail[i, 0] + "  " +
                      ReturnMatrix.Detail[i, 1] + "  " + ReturnMatrix.Detail[i, 2] + "  " + ReturnMatrix.Detail[i, 3]);
        }

        Debug.Log("============================结束正解============================");
        return(ReturnMatrix);
    }