public static bool IK4DOF_downward(double x, double y, double z, out double[] jointAngle4DOF) //机械臂逆解 { double a, b; //中间变量 jointAngle4DOF = new double[4]; double L1 = 10.5, L2 = 13.5, L3 = 13, error = 1; double m, n, t, q, p; //中间变量 double x1, y1, z1, yy; //逆解后正解的值,用以验证 double ANG2RED = 3.1415926535898 / 180.0; double[,] temp = new double[300, 3]; double maxj3 = 0; //存放最大角度 int u = 0; //i为有无解标志位,u为有无精确解标志位 int counter = 0, maxcounter = 0; // if (((Math.Pow(x, 2) + Math.Pow(y, 2) + Math.Pow(z, 2)) > Math.Pow(L1 + L2 + L3, 2))) printf("超出范围"); bool ifsol = false; jointAngle4DOF[0] = Math.Atan2(y, x); b = InverseKinematics.RAD2ANG(jointAngle4DOF[0]); a = x / Math.Cos(jointAngle4DOF[0]); if (x == 0) { a = y; } b = z; for (jointAngle4DOF[1] = -90; jointAngle4DOF[1] < 90; jointAngle4DOF[1]++) { jointAngle4DOF[1] *= ANG2RED; double temp1 = (Math.Pow(a, 2) + Math.Pow(b, 2) + Math.Pow(L1, 2) - Math.Pow(L2, 2) - Math.Pow(L3, 2) - 2 * a * L1 * Math.Sin(jointAngle4DOF[1]) - 2 * b * L1 * Math.Cos(jointAngle4DOF[1])) / (2 * L2 * L3); if (Math.Abs(temp1) <= 1) { jointAngle4DOF[3] = Math.Acos(temp1); if (RAD2ANG(jointAngle4DOF[3]) >= 95 || RAD2ANG(jointAngle4DOF[3]) <= -135) { jointAngle4DOF[1] = InverseKinematics.RAD2ANG(jointAngle4DOF[1]); continue; } m = L2 * Math.Sin(jointAngle4DOF[1]) + L3 * Math.Sin(jointAngle4DOF[1]) * Math.Cos(jointAngle4DOF[3]) + L3 * Math.Cos(jointAngle4DOF[1]) * Math.Sin(jointAngle4DOF[3]); n = L2 * Math.Cos(jointAngle4DOF[1]) + L3 * Math.Cos(jointAngle4DOF[1]) * Math.Cos(jointAngle4DOF[3]) - L3 * Math.Sin(jointAngle4DOF[1]) * Math.Sin(jointAngle4DOF[3]); t = a - L1 * Math.Sin(jointAngle4DOF[1]); p = Math.Pow(Math.Pow(n, 2) + Math.Pow(m, 2), 0.5); q = Math.Asin(m / p); jointAngle4DOF[2] = -(Math.Asin(t / p) - q); if (Math.Abs(InverseKinematics.RAD2ANG(jointAngle4DOF[2])) >= 135) { jointAngle4DOF[1] = InverseKinematics.RAD2ANG(jointAngle4DOF[1]); continue; } x1 = (L1 * Math.Sin(jointAngle4DOF[1]) + L2 * Math.Sin(jointAngle4DOF[1] + jointAngle4DOF[2]) + L3 * Math.Sin(jointAngle4DOF[1] + jointAngle4DOF[2] + jointAngle4DOF[3])) * Math.Cos(jointAngle4DOF[0]); y1 = (L1 * Math.Sin(jointAngle4DOF[1]) + L2 * Math.Sin(jointAngle4DOF[1] + jointAngle4DOF[2]) + L3 * Math.Sin(jointAngle4DOF[1] + jointAngle4DOF[2] + jointAngle4DOF[3])) * Math.Sin(jointAngle4DOF[0]); z1 = L1 * Math.Cos(jointAngle4DOF[1]) + L2 * Math.Cos(jointAngle4DOF[1] + jointAngle4DOF[2]) + L3 * Math.Cos(jointAngle4DOF[1] + jointAngle4DOF[2] + jointAngle4DOF[3]); jointAngle4DOF[1] = InverseKinematics.RAD2ANG(jointAngle4DOF[1]); jointAngle4DOF[2] = InverseKinematics.RAD2ANG(jointAngle4DOF[2]); jointAngle4DOF[3] = InverseKinematics.RAD2ANG(jointAngle4DOF[3]); if (x1 < (x + 0.5) && x1 > (x - 0.5) && y1 < (y + 0.5) && y1 > (y - 0.5) && z1 < (z + 0.5) && z1 > (z - 0.5)) { counter++; ifsol = true; u = 1; for (int k = 0; k < 3; k++) { temp[counter, k] = jointAngle4DOF[k + 1]; } if (temp[counter, 2] > (maxj3)) { maxj3 = temp[counter, 2]; maxcounter = counter; } } } else { jointAngle4DOF[1] = RAD2ANG(jointAngle4DOF[1]); } } for (jointAngle4DOF[1] = -90; jointAngle4DOF[1] < 90; jointAngle4DOF[1]++) { jointAngle4DOF[1] *= ANG2RED; double temp1 = (Math.Pow(a, 2) + Math.Pow(b, 2) + Math.Pow(L1, 2) - Math.Pow(L2, 2) - Math.Pow(L3, 2) - 2 * a * L1 * Math.Sin(jointAngle4DOF[1]) - 2 * b * L1 * Math.Cos(jointAngle4DOF[1])) / (2 * L2 * L3); if (Math.Abs(temp1) <= 1) { jointAngle4DOF[3] = Math.Acos(temp1); if (RAD2ANG(jointAngle4DOF[3]) >= 95 || RAD2ANG(jointAngle4DOF[3]) <= -135) { jointAngle4DOF[1] = InverseKinematics.RAD2ANG(jointAngle4DOF[1]); continue; } m = L2 * Math.Sin(jointAngle4DOF[1]) + L3 * Math.Sin(jointAngle4DOF[1]) * Math.Cos(jointAngle4DOF[3]) + L3 * Math.Cos(jointAngle4DOF[1]) * Math.Sin(jointAngle4DOF[3]); n = L2 * Math.Cos(jointAngle4DOF[1]) + L3 * Math.Cos(jointAngle4DOF[1]) * Math.Cos(jointAngle4DOF[3]) - L3 * Math.Sin(jointAngle4DOF[1]) * Math.Sin(jointAngle4DOF[3]); t = a - L1 * Math.Sin(jointAngle4DOF[1]); p = Math.Pow(Math.Pow(n, 2) + Math.Pow(m, 2), 0.5); q = ANG2RAD(180) - Math.Asin(m / p); jointAngle4DOF[2] = -(Math.Asin(t / p) - q); if (Math.Abs(InverseKinematics.RAD2ANG(jointAngle4DOF[2])) >= 135) { jointAngle4DOF[1] = InverseKinematics.RAD2ANG(jointAngle4DOF[1]); continue; } x1 = (L1 * Math.Sin(jointAngle4DOF[1]) + L2 * Math.Sin(jointAngle4DOF[1] + jointAngle4DOF[2]) + L3 * Math.Sin(jointAngle4DOF[1] + jointAngle4DOF[2] + jointAngle4DOF[3])) * Math.Cos(jointAngle4DOF[0]); y1 = (L1 * Math.Sin(jointAngle4DOF[1]) + L2 * Math.Sin(jointAngle4DOF[1] + jointAngle4DOF[2]) + L3 * Math.Sin(jointAngle4DOF[1] + jointAngle4DOF[2] + jointAngle4DOF[3])) * Math.Sin(jointAngle4DOF[0]); z1 = L1 * Math.Cos(jointAngle4DOF[1]) + L2 * Math.Cos(jointAngle4DOF[1] + jointAngle4DOF[2]) + L3 * Math.Cos(jointAngle4DOF[1] + jointAngle4DOF[2] + jointAngle4DOF[3]); jointAngle4DOF[1] = InverseKinematics.RAD2ANG(jointAngle4DOF[1]); jointAngle4DOF[2] = InverseKinematics.RAD2ANG(jointAngle4DOF[2]); jointAngle4DOF[3] = InverseKinematics.RAD2ANG(jointAngle4DOF[3]); if (x1 < (x + 0.5) && x1 > (x - 0.5) && y1 < (y + 0.5) && y1 > (y - 0.5) && z1 < (z + 0.5) && z1 > (z - 0.5)) { counter++; ifsol = true; u = 1; for (int k = 0; k < 3; k++) { temp[counter, k] = jointAngle4DOF[k + 1]; } if (temp[counter, 2] > (maxj3)) { maxj3 = temp[counter, 2]; maxcounter = counter; } } } else { jointAngle4DOF[1] = RAD2ANG(jointAngle4DOF[1]); } } jointAngle4DOF[0] = InverseKinematics.RAD2ANG(jointAngle4DOF[0]); if (u == 1) //有解选取j1最小的 { jointAngle4DOF[1] = temp[maxcounter, 0]; jointAngle4DOF[2] = temp[maxcounter, 1]; jointAngle4DOF[3] = temp[maxcounter, 2]; } return(ifsol); }
public static bool IK4DOF_level(double x, double y, double z, out double[] jointAngle4DOF) //机械臂逆解 { double a, b; //中间变量 double [] jointAngle4DOFtemp = new double[4]; jointAngle4DOF = new double[4]; double L1 = 10.5, L2 = 13.5, L3 = 13, error = 1; double m, n, t, q, p; //中间变量 double x1, y1, z1; //逆解后正解的值,用以验证 double ANG2RED = 3.1415926535898 / 180.0; int u = 0; //i为有无解标志位,u为有无精确解标志位 // if (((Math.Pow(x, 2) + Math.Pow(y, 2) + Math.Pow(z, 2)) > Math.Pow(L1 + L2 + L3, 2))) printf("超出范围"); bool ifsol = false; jointAngle4DOFtemp[0] = Math.Atan2(y, x); b = InverseKinematics.RAD2ANG(jointAngle4DOFtemp[0]); a = x / Math.Cos(jointAngle4DOFtemp[0]); if (x == 0) { a = y; } b = z; for (jointAngle4DOFtemp[1] = -90; jointAngle4DOFtemp[1] < 90; jointAngle4DOFtemp[1]++) { jointAngle4DOFtemp[1] *= ANG2RED; double temp1 = (Math.Pow(a, 2) + Math.Pow(b, 2) + Math.Pow(L1, 2) - Math.Pow(L2, 2) - Math.Pow(L3, 2) - 2 * a * L1 * Math.Sin(jointAngle4DOFtemp[1]) - 2 * b * L1 * Math.Cos(jointAngle4DOFtemp[1])) / (2 * L2 * L3); if (Math.Abs(temp1) <= 1) { jointAngle4DOFtemp[3] = -Math.Acos(temp1); if (Math.Abs(InverseKinematics.RAD2ANG(jointAngle4DOFtemp[3])) >= 135) { jointAngle4DOFtemp[1] = InverseKinematics.RAD2ANG(jointAngle4DOFtemp[1]); continue; } m = L2 * Math.Sin(jointAngle4DOFtemp[1]) + L3 * Math.Sin(jointAngle4DOFtemp[1]) * Math.Cos(jointAngle4DOFtemp[3]) + L3 * Math.Cos(jointAngle4DOFtemp[1]) * Math.Sin(jointAngle4DOFtemp[3]); n = L2 * Math.Cos(jointAngle4DOFtemp[1]) + L3 * Math.Cos(jointAngle4DOFtemp[1]) * Math.Cos(jointAngle4DOFtemp[3]) - L3 * Math.Sin(jointAngle4DOFtemp[1]) * Math.Sin(jointAngle4DOFtemp[3]); t = a - L1 * Math.Sin(jointAngle4DOFtemp[1]); p = Math.Pow(Math.Pow(n, 2) + Math.Pow(m, 2), 0.5); q = ANG2RAD(180) - Math.Asin(m / p); jointAngle4DOFtemp[2] = -(Math.Asin(t / p) - q); if (Math.Abs(InverseKinematics.RAD2ANG(jointAngle4DOFtemp[2])) >= 135) { jointAngle4DOFtemp[1] = InverseKinematics.RAD2ANG(jointAngle4DOFtemp[1]); continue; } x1 = (L1 * Math.Sin(jointAngle4DOFtemp[1]) + L2 * Math.Sin(jointAngle4DOFtemp[1] + jointAngle4DOFtemp[2]) + L3 * Math.Sin(jointAngle4DOFtemp[1] + jointAngle4DOFtemp[2] + jointAngle4DOFtemp[3])) * Math.Cos(jointAngle4DOFtemp[0]); y1 = (L1 * Math.Sin(jointAngle4DOFtemp[1]) + L2 * Math.Sin(jointAngle4DOFtemp[1] + jointAngle4DOFtemp[2]) + L3 * Math.Sin(jointAngle4DOFtemp[1] + jointAngle4DOFtemp[2] + jointAngle4DOFtemp[3])) * Math.Sin(jointAngle4DOFtemp[0]); z1 = L1 * Math.Cos(jointAngle4DOFtemp[1]) + L2 * Math.Cos(jointAngle4DOFtemp[1] + jointAngle4DOFtemp[2]) + L3 * Math.Cos(jointAngle4DOFtemp[1] + jointAngle4DOFtemp[2] + jointAngle4DOFtemp[3]); jointAngle4DOFtemp[1] = InverseKinematics.RAD2ANG(jointAngle4DOFtemp[1]); jointAngle4DOFtemp[2] = InverseKinematics.RAD2ANG(jointAngle4DOFtemp[2]); jointAngle4DOFtemp[3] = InverseKinematics.RAD2ANG(jointAngle4DOFtemp[3]); if (Math.Abs(jointAngle4DOFtemp[1] + jointAngle4DOFtemp[2] + jointAngle4DOFtemp[3] - 90) < 3 && x1 <(x + 0.5) && x1> (x - 0.5) && y1 <(y + 0.5) && y1> (y - 0.5) && z1 <(z + 0.5) && z1> (z - 0.5)) { ifsol = true; jointAngle4DOF[1] = jointAngle4DOFtemp[1]; jointAngle4DOF[2] = jointAngle4DOFtemp[2]; jointAngle4DOF[3] = jointAngle4DOFtemp[3]; break; } if (Math.Abs(jointAngle4DOFtemp[1] + jointAngle4DOFtemp[2] + jointAngle4DOFtemp[3] - 90) < 6 && x1 <(x + 0.5) && x1> (x - 0.5) && y1 <(y + 0.5) && y1> (y - 0.5) && z1 <(z + 0.5) && z1> (z - 0.5)) { ifsol = true; jointAngle4DOF[1] = jointAngle4DOFtemp[1]; jointAngle4DOF[2] = jointAngle4DOFtemp[2]; jointAngle4DOF[3] = jointAngle4DOFtemp[3]; break; } if (Math.Abs(jointAngle4DOFtemp[1] + jointAngle4DOFtemp[2] + jointAngle4DOFtemp[3] - 90) < 10 && x1 <(x + 0.5) && x1> (x - 0.5) && y1 <(y + 0.5) && y1> (y - 0.5) && z1 <(z + 0.5) && z1> (z - 0.5)) { ifsol = true; jointAngle4DOF[1] = jointAngle4DOFtemp[1]; jointAngle4DOF[2] = jointAngle4DOFtemp[2]; jointAngle4DOF[3] = jointAngle4DOFtemp[3]; break; } } else { jointAngle4DOFtemp[1] = RAD2ANG(jointAngle4DOFtemp[1]); } } /******************************************************/ if (!ifsol) { for (jointAngle4DOFtemp[1] = -90; jointAngle4DOFtemp[1] < 90; jointAngle4DOFtemp[1]++) { jointAngle4DOFtemp[1] *= ANG2RED; double temp1 = (Math.Pow(a, 2) + Math.Pow(b, 2) + Math.Pow(L1, 2) - Math.Pow(L2, 2) - Math.Pow(L3, 2) - 2 * a * L1 * Math.Sin(jointAngle4DOFtemp[1]) - 2 * b * L1 * Math.Cos(jointAngle4DOFtemp[1])) / (2 * L2 * L3); if (Math.Abs(temp1) <= 1) { jointAngle4DOFtemp[3] = Math.Acos(temp1); if (Math.Abs(InverseKinematics.RAD2ANG(jointAngle4DOFtemp[3])) >= 135) { jointAngle4DOFtemp[1] = InverseKinematics.RAD2ANG(jointAngle4DOFtemp[1]); continue; } m = L2 * Math.Sin(jointAngle4DOFtemp[1]) + L3 * Math.Sin(jointAngle4DOFtemp[1]) * Math.Cos(jointAngle4DOFtemp[3]) + L3 * Math.Cos(jointAngle4DOFtemp[1]) * Math.Sin(jointAngle4DOFtemp[3]); n = L2 * Math.Cos(jointAngle4DOFtemp[1]) + L3 * Math.Cos(jointAngle4DOFtemp[1]) * Math.Cos(jointAngle4DOFtemp[3]) - L3 * Math.Sin(jointAngle4DOFtemp[1]) * Math.Sin(jointAngle4DOFtemp[3]); t = a - L1 * Math.Sin(jointAngle4DOFtemp[1]); p = Math.Pow(Math.Pow(n, 2) + Math.Pow(m, 2), 0.5); q = Math.Asin(m / p); jointAngle4DOFtemp[2] = -(Math.Asin(t / p) - q); if (Math.Abs(InverseKinematics.RAD2ANG(jointAngle4DOFtemp[2])) >= 135) { jointAngle4DOFtemp[1] = InverseKinematics.RAD2ANG(jointAngle4DOFtemp[1]); continue; } x1 = (L1 * Math.Sin(jointAngle4DOFtemp[1]) + L2 * Math.Sin(jointAngle4DOFtemp[1] + jointAngle4DOFtemp[2]) + L3 * Math.Sin(jointAngle4DOFtemp[1] + jointAngle4DOFtemp[2] + jointAngle4DOFtemp[3])) * Math.Cos(jointAngle4DOFtemp[0]); y1 = (L1 * Math.Sin(jointAngle4DOFtemp[1]) + L2 * Math.Sin(jointAngle4DOFtemp[1] + jointAngle4DOFtemp[2]) + L3 * Math.Sin(jointAngle4DOFtemp[1] + jointAngle4DOFtemp[2] + jointAngle4DOFtemp[3])) * Math.Sin(jointAngle4DOFtemp[0]); z1 = L1 * Math.Cos(jointAngle4DOFtemp[1]) + L2 * Math.Cos(jointAngle4DOFtemp[1] + jointAngle4DOFtemp[2]) + L3 * Math.Cos(jointAngle4DOFtemp[1] + jointAngle4DOFtemp[2] + jointAngle4DOFtemp[3]); jointAngle4DOFtemp[1] = InverseKinematics.RAD2ANG(jointAngle4DOFtemp[1]); jointAngle4DOFtemp[2] = InverseKinematics.RAD2ANG(jointAngle4DOFtemp[2]); jointAngle4DOFtemp[3] = InverseKinematics.RAD2ANG(jointAngle4DOFtemp[3]); if (Math.Abs(jointAngle4DOFtemp[1] + jointAngle4DOFtemp[2] + jointAngle4DOFtemp[3] - 90) < 3 && x1 <(x + 0.5) && x1> (x - 0.5) && y1 <(y + 0.5) && y1> (y - 0.5) && z1 <(z + 0.5) && z1> (z - 0.5)) { ifsol = true; jointAngle4DOF[1] = jointAngle4DOFtemp[1]; jointAngle4DOF[2] = jointAngle4DOFtemp[2]; jointAngle4DOF[3] = jointAngle4DOFtemp[3]; break; } if (Math.Abs(jointAngle4DOFtemp[1] + jointAngle4DOFtemp[2] + jointAngle4DOFtemp[3] - 90) < 6 && x1 <(x + 0.5) && x1> (x - 0.5) && y1 <(y + 0.5) && y1> (y - 0.5) && z1 <(z + 0.5) && z1> (z - 0.5)) { ifsol = true; jointAngle4DOF[1] = jointAngle4DOFtemp[1]; jointAngle4DOF[2] = jointAngle4DOFtemp[2]; jointAngle4DOF[3] = jointAngle4DOFtemp[3]; break; } if (Math.Abs(jointAngle4DOFtemp[1] + jointAngle4DOFtemp[2] + jointAngle4DOFtemp[3] - 90) < 10 && x1 <(x + 0.5) && x1> (x - 0.5) && y1 <(y + 0.5) && y1> (y - 0.5) && z1 <(z + 0.5) && z1> (z - 0.5)) { ifsol = true; jointAngle4DOF[1] = jointAngle4DOFtemp[1]; jointAngle4DOF[2] = jointAngle4DOFtemp[2]; jointAngle4DOF[3] = jointAngle4DOFtemp[3]; break; } } else { jointAngle4DOFtemp[1] = RAD2ANG(jointAngle4DOFtemp[1]); } } } /******************************************************/ if (!ifsol) { for (jointAngle4DOFtemp[1] = -90; jointAngle4DOFtemp[1] < 90; jointAngle4DOFtemp[1]++) { jointAngle4DOFtemp[1] *= ANG2RED; double temp1 = (Math.Pow(a, 2) + Math.Pow(b, 2) + Math.Pow(L1, 2) - Math.Pow(L2, 2) - Math.Pow(L3, 2) - 2 * a * L1 * Math.Sin(jointAngle4DOFtemp[1]) - 2 * b * L1 * Math.Cos(jointAngle4DOFtemp[1])) / (2 * L2 * L3); if (Math.Abs(temp1) <= 1) { jointAngle4DOFtemp[3] = Math.Acos(temp1); if (Math.Abs(InverseKinematics.RAD2ANG(jointAngle4DOFtemp[3])) >= 135) { jointAngle4DOFtemp[1] = InverseKinematics.RAD2ANG(jointAngle4DOFtemp[1]); continue; } m = L2 * Math.Sin(jointAngle4DOFtemp[1]) + L3 * Math.Sin(jointAngle4DOFtemp[1]) * Math.Cos(jointAngle4DOFtemp[3]) + L3 * Math.Cos(jointAngle4DOFtemp[1]) * Math.Sin(jointAngle4DOFtemp[3]); n = L2 * Math.Cos(jointAngle4DOFtemp[1]) + L3 * Math.Cos(jointAngle4DOFtemp[1]) * Math.Cos(jointAngle4DOFtemp[3]) - L3 * Math.Sin(jointAngle4DOFtemp[1]) * Math.Sin(jointAngle4DOFtemp[3]); t = a - L1 * Math.Sin(jointAngle4DOFtemp[1]); p = Math.Pow(Math.Pow(n, 2) + Math.Pow(m, 2), 0.5); q = ANG2RAD(180) - Math.Asin(m / p); jointAngle4DOFtemp[2] = -(Math.Asin(t / p) - q); if (Math.Abs(InverseKinematics.RAD2ANG(jointAngle4DOFtemp[2])) >= 135) { jointAngle4DOFtemp[1] = InverseKinematics.RAD2ANG(jointAngle4DOFtemp[1]); continue; } x1 = (L1 * Math.Sin(jointAngle4DOFtemp[1]) + L2 * Math.Sin(jointAngle4DOFtemp[1] + jointAngle4DOFtemp[2]) + L3 * Math.Sin(jointAngle4DOFtemp[1] + jointAngle4DOFtemp[2] + jointAngle4DOFtemp[3])) * Math.Cos(jointAngle4DOFtemp[0]); y1 = (L1 * Math.Sin(jointAngle4DOFtemp[1]) + L2 * Math.Sin(jointAngle4DOFtemp[1] + jointAngle4DOFtemp[2]) + L3 * Math.Sin(jointAngle4DOFtemp[1] + jointAngle4DOFtemp[2] + jointAngle4DOFtemp[3])) * Math.Sin(jointAngle4DOFtemp[0]); z1 = L1 * Math.Cos(jointAngle4DOFtemp[1]) + L2 * Math.Cos(jointAngle4DOFtemp[1] + jointAngle4DOFtemp[2]) + L3 * Math.Cos(jointAngle4DOFtemp[1] + jointAngle4DOFtemp[2] + jointAngle4DOFtemp[3]); jointAngle4DOFtemp[1] = InverseKinematics.RAD2ANG(jointAngle4DOFtemp[1]); jointAngle4DOFtemp[2] = InverseKinematics.RAD2ANG(jointAngle4DOFtemp[2]); jointAngle4DOFtemp[3] = InverseKinematics.RAD2ANG(jointAngle4DOFtemp[3]); if (Math.Abs(jointAngle4DOFtemp[1] + jointAngle4DOFtemp[2] + jointAngle4DOFtemp[3] - 90) < 3 && x1 <(x + 0.5) && x1> (x - 0.5) && y1 <(y + 0.5) && y1> (y - 0.5) && z1 <(z + 0.5) && z1> (z - 0.5)) { ifsol = true; jointAngle4DOF[1] = jointAngle4DOFtemp[1]; jointAngle4DOF[2] = jointAngle4DOFtemp[2]; jointAngle4DOF[3] = jointAngle4DOFtemp[3]; break; } if (Math.Abs(jointAngle4DOFtemp[1] + jointAngle4DOFtemp[2] + jointAngle4DOFtemp[3] - 90) < 6 && x1 <(x + 0.5) && x1> (x - 0.5) && y1 <(y + 0.5) && y1> (y - 0.5) && z1 <(z + 0.5) && z1> (z - 0.5)) { ifsol = true; jointAngle4DOF[1] = jointAngle4DOFtemp[1]; jointAngle4DOF[2] = jointAngle4DOFtemp[2]; jointAngle4DOF[3] = jointAngle4DOFtemp[3]; break; } if (Math.Abs(jointAngle4DOFtemp[1] + jointAngle4DOFtemp[2] + jointAngle4DOFtemp[3] - 90) < 10 && x1 <(x + 0.5) && x1> (x - 0.5) && y1 <(y + 0.5) && y1> (y - 0.5) && z1 <(z + 0.5) && z1> (z - 0.5)) { ifsol = true; jointAngle4DOF[1] = jointAngle4DOFtemp[1]; jointAngle4DOF[2] = jointAngle4DOFtemp[2]; jointAngle4DOF[3] = jointAngle4DOFtemp[3]; break; } } else { jointAngle4DOFtemp[1] = RAD2ANG(jointAngle4DOFtemp[1]); } } } jointAngle4DOF[0] = InverseKinematics.RAD2ANG(jointAngle4DOFtemp[0]); return(ifsol); }