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)); }
/// <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============================"); }
/// <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============================"); }
/// <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); }
/// <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); }