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);
}