private void AutoGrab3DOF(object obj)
{
if3DOF = true;
string str = obj as string;
fileOperations.GetSetoff(str, out double x, out double y, out int flag, out int id);
InverseKinematics.Transformation(new double[3] {
x, y, -8.5
}, 90, 12.4, -9, out double[] newsetoff);
try
{
if (serialPort1.IsOpen)
{
controller.ArriveSetoff3DOF(newsetoff[0], newsetoff[1], 4, 1);//吸住前过中间点
bool ifsol = controller.ArriveSetoff3DOF(newsetoff[0], newsetoff[1], newsetoff[2], 1);
if (ifsol)
{
controller.SuckUp();
controller.ArriveSetoff3DOF(10, -10, 10, 1); //吸住后过中间点
controller.ArriveSetoff3DOF(0, -15, -3, 1); //吸住后放置到目标点
fileOperations.WriteTxt("flag", "1");
Thread.Sleep(200);
controller.SuckDown();
}
AutoGrab_tag3 = 1;
if3DOF = false;
}
}
catch (Exception ex)
{
serialPort1.Close();
//捕获到异常，创建一个新的对象，之前的不可以再用
serialPort1 = new System.IO.Ports.SerialPort();
//刷新COM口选项
comboBox1.Items.Clear();
comboBox1.Items.AddRange(System.IO.Ports.SerialPort.GetPortNames());
//响铃并显示异常给用户
System.Media.SystemSounds.Beep.Play();
button_3DOF.Text = "连接小手";
button_3DOF.BackColor = Color.ForestGreen;
MessageBox.Show(ex.Message);
comboBox1.Enabled = true;
if3DOF = false;
}
}
public bool ArriveSetoff3DOF(double x, double y, double z, int mod) //模式0为顺序执行，模式1为一起执行
{
bool ifsol = InverseKinematics.IK3DOF(x, y, z, out joint3DOF);
if (ifsol)
{
if (mod == 0)
{
SortOrderRun3DOF(joint3DOF, ref lastJoint3DOF);
}
else
{
TogetherRun3DOF(joint3DOF, ref lastJoint3DOF);
}
}
else
{
lastJoint3DOF = joint3DOF;
}
return(ifsol);
}
public bool ArriveSetoff4DOF_level(double x, double y, double z, int mod) //模式0为顺序执行，模式1为一起执行
{
bool ifsol = InverseKinematics.IK4DOF_level(x, y, z, out joint4DOF_level);
if (ifsol)
{
if (mod == 0)
{
TogetherRun4DOF(joint4DOF_level, 50, ref lastJoint4DOF);
}
else
{
TogetherRun4DOF(joint4DOF_level, 16, ref lastJoint4DOF);
}
}
else
{
lastJoint4DOF = joint4DOF_level;
}
return(ifsol);
}
public bool ArriveSetoff4DOF_level_con(double x, double y, double z, int mod) //模式0为顺序执行，模式1为一起执行
{
bool ifsol = InverseKinematics.IK4DOF_level(x, y, z, out joint4DOF_level);
joint4DOF_level[1] -= 10;
joint4DOF_level[2] -= 10;
joint4DOF_level[3] -= 1;
if (ifsol)
{
if (mod == 0)
{
SortOrderRun4DOF(joint4DOF_level, ref lastJoint4DOF);
}
else
{
TogetherRun4DOF_con(joint4DOF_level, ref lastJoint4DOF);
}
}
else
{
lastJoint4DOF = joint4DOF_level;
}
return(ifsol);
}
private void Circle()//画圈
{
con = false;
InverseKinematics.Transformation(new double[3] {
12.4, 15, -6
}, -90, 12.4, 38, out double[] newsetoff);
controller.ArriveSetoff4DOF_level_con(newsetoff[0], newsetoff[1], newsetoff[2], 1);
Thread.Sleep(1500);
InverseKinematics.Transformation(new double[3] {
12.4, 15, -1.5
}, 90, 12.4, -9, out newsetoff);
controller.ArriveSetoff3DOF(newsetoff[0], newsetoff[1], newsetoff[2], 1);
InverseKinematics.Transformation(new double[3] {
16.8, 13.7, -6
}, -90, 12.4, 38, out newsetoff);
controller.ArriveSetoff4DOF_level_con(newsetoff[0], newsetoff[1], newsetoff[2], 1);
InverseKinematics.Transformation(new double[3] {
16.8, 13.7, -1.5
}, 90, 12.4, -9, out newsetoff);
controller.ArriveSetoff3DOF(newsetoff[0], newsetoff[1], newsetoff[2], 1);
//Thread.Sleep(1000);
InverseKinematics.Transformation(new double[3] {
18.6, 9.3, -6
}, -90, 12.4, 38, out newsetoff);
controller.ArriveSetoff4DOF_level_con(newsetoff[0], newsetoff[1], newsetoff[2], 1);
InverseKinematics.Transformation(new double[3] {
18.6, 9.3, -1.5
}, 90, 12.4, -9, out newsetoff);
controller.ArriveSetoff3DOF(newsetoff[0], newsetoff[1], newsetoff[2], 1);
//Thread.Sleep(1000);
InverseKinematics.Transformation(new double[3] {
16.8, 4.9, -6
}, -90, 12.4, 38, out newsetoff);
controller.ArriveSetoff4DOF_level_con(newsetoff[0], newsetoff[1], newsetoff[2], 1);
InverseKinematics.Transformation(new double[3] {
16.8, 4.9, -1.5
}, 90, 12.4, -9, out newsetoff);
controller.ArriveSetoff3DOF(newsetoff[0], newsetoff[1], newsetoff[2], 1);
//Thread.Sleep(1000);
InverseKinematics.Transformation(new double[3] {
12.4, 3, -6
}, -90, 12.4, 38, out newsetoff);
controller.ArriveSetoff4DOF_level_con(newsetoff[0], newsetoff[1], newsetoff[2], 1);
InverseKinematics.Transformation(new double[3] {
12.4, 3, -1.5
}, 90, 12.4, -9, out newsetoff);
controller.ArriveSetoff3DOF(newsetoff[0], newsetoff[1], newsetoff[2], 1);
//Thread.Sleep(1000);
InverseKinematics.Transformation(new double[3] {
8, 4.9, -6
}, -90, 12.4, 38, out newsetoff);
controller.ArriveSetoff4DOF_level_con(newsetoff[0], newsetoff[1], newsetoff[2], 1);
InverseKinematics.Transformation(new double[3] {
8, 4.9, -1.5
}, 90, 12.4, -9, out newsetoff);
controller.ArriveSetoff3DOF(newsetoff[0], newsetoff[1], newsetoff[2], 1);
//Thread.Sleep(1000);
InverseKinematics.Transformation(new double[3] {
6.2, 9.3, -6
}, -90, 12.4, 38, out newsetoff);
controller.ArriveSetoff4DOF_level_con(newsetoff[0], newsetoff[1], newsetoff[2], 1);
InverseKinematics.Transformation(new double[3] {
6.2, 9.3, -1.5
}, 90, 12.4, -9, out newsetoff);
controller.ArriveSetoff3DOF(newsetoff[0], newsetoff[1], newsetoff[2], 1);
//Thread.Sleep(1000);
InverseKinematics.Transformation(new double[3] {
8, 13.7, -6
}, -90, 12.4, 38, out newsetoff);
controller.ArriveSetoff4DOF_level_con(newsetoff[0], newsetoff[1], newsetoff[2], 1);
InverseKinematics.Transformation(new double[3] {
8, 13.7, -1.5
}, 90, 12.4, -9, out newsetoff);
controller.ArriveSetoff3DOF(newsetoff[0], newsetoff[1], newsetoff[2], 1);
//Thread.Sleep(1000);
InverseKinematics.Transformation(new double[3] {
12.4, 15, -6
}, -90, 12.4, 38, out newsetoff);
controller.ArriveSetoff4DOF_level_con(newsetoff[0], newsetoff[1], newsetoff[2], 1);
InverseKinematics.Transformation(new double[3] {
12.4, 15, -1.5
}, 90, 12.4, -9, out newsetoff);
controller.ArriveSetoff3DOF(newsetoff[0], newsetoff[1], newsetoff[2], 1);
//Thread.Sleep(1000);
con = true;
controller.ArriveSetoff3DOF(0, -19, 12, 1);//手臂躲到旁边
controller.ArriveSetoff4DOF_downward(0, 0, 37, 1);
}
private void PourWater() //倒水
{
double x = 0, y = 0;
int mod = 3;
int flag = 0;
double[] newsetoff1 = new double[3] {
0, 0, 0
};
while (flag == 0)
{
string mod_str = fileOperations.ReadTxt("mod");
fileOperations.GetSetoff(mod_str, out mod);
if (mod == 0)
{
Application.ExitThread(); break;
}
textBox_chess.Text = fileOperations.ReadTxt("setoff");
fileOperations.GetSetoff(textBox_chess.Text, out x, out y, out flag, out int id);
}
if (flag == 1)
{
controller.Openhand();
PourWaterInit();
fileOperations.WriteTxt("setoff", "0,0,0,0");
InverseKinematics.Transformation(new double[3] {
x, y, 0
}, -90, 12.4, 38, out double[] newsetoff);
try
{
bool ifsol = controller.ArriveSetoff4DOF_level(newsetoff[0], newsetoff[1], 0, 1);
controller.Closehand();
if (ifsol)
{
PourWaterMid();
//controller.ArriveSetoff4DOF_level(26.5, 0, 10.5, 1);
fileOperations.WriteTxt("flagg", "1");
int flag1 = 0;
double x1 = 0;
double y1 = 0;
while (flag1 == 0)
{
textBox_chess.Text = fileOperations.ReadTxt("setoff");
fileOperations.GetSetoff(textBox_chess.Text, out x1, out y1, out flag1, out int id);
string mod_str = fileOperations.ReadTxt("mod");
fileOperations.GetSetoff(mod_str, out mod);
if (mod == 0)
{
Application.ExitThread(); break;
}
}
if (mod != 0)
{
fileOperations.WriteTxt("setoff", "0,0,0,0");
InverseKinematics.Transformation(new double[3] {
x1, y1, 0
}, -90, 12.4, 38, out newsetoff1);
bool ifsol2 = controller.ArriveSetoff4DOF_level(newsetoff1[0] - 2.5, newsetoff1[1] - 14, 9, 1);
if (ifsol2)
{
controller.Sideturn();
Thread.Sleep(1000);
controller.Levelturn();
// controller.ArriveSetoff4DOF_level(23, 0, 0, 1);
}
controller.ArriveSetoff4DOF_level(newsetoff[0], newsetoff[1], 0, 1);
}
}
if (mod != 0)
{
controller.Openhand();
PourWaterInit();
PourWaterEndM();
controller.Openhand();
bool ifsol2 = controller.ArriveSetoff4DOF_level(newsetoff1[0], newsetoff1[1], 0, 0);
controller.Closehand();
controller.ArriveSetoff4DOF_level(newsetoff1[0], newsetoff1[1], 3, 0);
controller.ArriveSetoff4DOF_level(newsetoff1[0], newsetoff1[1], 6, 0);
controller.ArriveSetoff4DOF_level(10, 23, 4, 0);
controller.ArriveSetoff4DOF_level(10, 23, 0, 0);
controller.Openhand();
PourWaterEndend();
controller.ArriveSetoff4DOF_downward(0, 0, 37, 1);
controller.Closehand();
}
fileOperations.WriteTxt("ifend", "1");
}
catch (Exception ex)
{
serialPort2.Close();
//捕获到异常，创建一个新的对象，之前的不可以再用
serialPort2 = new System.IO.Ports.SerialPort();
//刷新COM口选项
comboBox2.Items.Clear();
comboBox2.Items.AddRange(System.IO.Ports.SerialPort.GetPortNames());
//响铃并显示异常给用户
System.Media.SystemSounds.Beep.Play();
button_4DOF.Text = "连接大手";
button_4DOF.BackColor = Color.ForestGreen;
MessageBox.Show(ex.Message);
comboBox2.Enabled = true;
}
}
}
private void RemoteControl() //遥控
{
double[] goalsetoff = new double[3] {
0, 0, 0
};
double goalx = 0, goaly = 0;
int flag1 = 0;
int counter = 0, tag = 0;
while (flag1 == 0)
{
fileOperations.GetSetoff(fileOperations.ReadTxt("mod"), out int mod); if (mod == 0)
{
Application.ExitThread(); break;
}
textBox_send.Text = fileOperations.ReadTxt("cupsetoff");
fileOperations.GetSetoff(textBox_send.Text, out goalx, out goaly, out flag1, out int id);
Thread.Sleep(100);
}
fileOperations.WriteTxt("cupsetoff", "0,0,0,0");
InverseKinematics.Transformation(new double[3] {
goalx, goaly, -1
}, 90, 12.4, -9, out goalsetoff);
while (serialPort2.IsOpen || serialPort1.IsOpen)
{
fileOperations.GetSetoff(fileOperations.ReadTxt("mod"), out int mod); if (mod == 0)
{
Application.ExitThread(); break;
}
textBox_send.Text = fileOperations.ReadTxt("setoff");
fileOperations.GetSetoff(textBox_send.Text, out double x, out double y, out int flag, out int id);
if (flag == 1)
{
counter = 0;
fileOperations.WriteTxt("setoff", "0,0,0,0");
InverseKinematics.Transformation(new double[3] {
x, y, -7.8
}, 90, 12.4, -9, out double[] newsetoff);
try
{
if (serialPort1.IsOpen)
{
controller.ArriveSetoff3DOF(newsetoff[0], newsetoff[1], -1, 1);//吸前过中间点
bool ifsol = controller.ArriveSetoff3DOF(newsetoff[0], newsetoff[1], newsetoff[2], 1);
if (ifsol)
{
controller.SuckUp();
controller.ArriveSetoff3DOF(newsetoff[0], newsetoff[1], -1, 1); //吸前过中间点
controller.ArriveSetoff3DOF(goalsetoff[0], goalsetoff[1], goalsetoff[2], 1); //吸住后放置到目标点
Thread.Sleep(500);
controller.SuckDown();
controller.ArriveSetoff3DOF(13, 0, -1, 1);//吸住后过中间点
}
fileOperations.WriteTxt("flag", "1");
tag = 1;
}
}
catch (Exception ex)
{
serialPort1.Close();
//捕获到异常，创建一个新的对象，之前的不可以再用
serialPort1 = new System.IO.Ports.SerialPort();
//刷新COM口选项
comboBox1.Items.Clear();
comboBox1.Items.AddRange(System.IO.Ports.SerialPort.GetPortNames());
//响铃并显示异常给用户
System.Media.SystemSounds.Beep.Play();
button_3DOF.Text = "连接小手";
button_3DOF.BackColor = Color.ForestGreen;
MessageBox.Show(ex.Message);
comboBox1.Enabled = true;
}
}
if (tag == 1)
{
counter++;
Thread.Sleep(50);
}
if (counter > 30)
{
tag = 0;
counter = 0;
fileOperations.WriteTxt("ifend", "1");
}
}
}
private void AutoGrab4DOF(object obj)
{
if4DOF = true;
double goalx = -12;
double goaly = 20;
double goalz = 10; //目标
string str = obj as string;
fileOperations.GetSetoff(str, out double x, out double y, out int flag, out int id);
InverseKinematics.Transformation(new double[3] {
x, y, -4
}, -90, 12.4, 38, out double[] newsetoff);
try
{
if (serialPort2.IsOpen)
{
if (newsetoff[1] >= 0)
{
newsetoff[1] = newsetoff[1] + 2;
}
if (newsetoff[1] < 0)
{
newsetoff[1] = newsetoff[1] - 1.4;
}
bool ifsol = controller.ArriveSetoff4DOF_downward(newsetoff[0], newsetoff[1], 0, 1);
if (!ifsol)
{
label6.Text = "无解";
}
else
{
label6.Text = "正常运行";
DisplayJoint(controller.joint4DOF_downward);
controller.Closehand();
bool ifsol1 = controller.ArriveSetoff4DOF_downward(newsetoff[0], newsetoff[1], 20, 1);
if (!ifsol1)
{
controller.ArriveSetoff4DOF_downward(0, 0, 37, 1);
}
controller.ArriveSetoff4DOF_downward(10, 17, 20, 1);
fileOperations.WriteTxt("flag", "1");
con = false;
controller.ArriveSetoff4DOF_downward(goalx, goaly, 20, 1);
con = true;
controller.ArriveSetoff4DOF_downward(goalx, goaly, goalz, 1);
controller.Openhand();
controller.ArriveSetoff4DOF_downward(goalx, goaly, 20, 1);
controller.ArriveSetoff4DOF_downward(0, 0, 37, 1);
}
AutoGrab_tag4 = 1;
if4DOF = false;
}
}
catch (Exception ex)
{
serialPort2.Close();
//捕获到异常，创建一个新的对象，之前的不可以再用
serialPort2 = new System.IO.Ports.SerialPort();
//刷新COM口选项
comboBox2.Items.Clear();
comboBox2.Items.AddRange(System.IO.Ports.SerialPort.GetPortNames());
//响铃并显示异常给用户
System.Media.SystemSounds.Beep.Play();
button_4DOF.Text = "连接小手";
button_4DOF.BackColor = Color.ForestGreen;
MessageBox.Show(ex.Message);
comboBox2.Enabled = true;
if4DOF = false;
}
}
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);
}