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