private void Btn_getJoint_MouseDown(object sender, MouseButtonEventArgs e)
{
if (e.ChangedButton == MouseButton.Left)
{
URJoint joint = URc.getJoint();
setUIJoint(joint);
}
}
public void setUIJoint(URJoint joint)
{
tb_urJoint_1.Text = (joint.J1.rad).ToString("0.000");
tb_urJoint_2.Text = (joint.J2.rad).ToString("0.000");
tb_urJoint_3.Text = (joint.J3.rad).ToString("0.000");
tb_urJoint_4.Text = (joint.J4.rad).ToString("0.000");
tb_urJoint_5.Text = (joint.J5.rad).ToString("0.000");
tb_urJoint_6.Text = (joint.J6.rad).ToString("0.000");
}
public static TransformationMatrix ForwardKinematic(URJoint joints)
{
double[,] T_06 = { { 1, 0, 0, 0 }, { 0, 1, 0, 0 }, { 0, 0, 1, 0 }, { 0, 0, 0, 1 } };//T_06 = np.matrix(np.identity(4))
double[][,] DH = new double[6][, ];
DH[0] = new double[4, 4];
DH[1] = new double[4, 4];
DH[2] = new double[4, 4];
DH[3] = new double[4, 4];
DH[4] = new double[4, 4];
DH[5] = new double[4, 4];
for (int i = 0; i < 6; i++)
{
T_06 = Matrix.MatrixProduct(T_06,
HTM(i, new double[] { joints.J1.rad, joints.J2.rad, joints.J3.rad, joints.J4.rad, joints.J5.rad, joints.J6.rad }));
DH[i] = T_06;
}
TransformationMatrix rtn = new TransformationMatrix(DH[5]);//回傳第6軸的座標
return(rtn);
double[,] HTM(int i, double[] theta)
{ //theta unit : rad
double[,] Rot_z = { { 1, 0, 0, 0 }, { 0, 1, 0, 0 }, { 0, 0, 1, 0 }, { 0, 0, 0, 1 } }; //Rot_z = np.matrix(np.identity(4))
Rot_z[0, 0] = Math.Cos(theta[i]);
Rot_z[1, 1] = Math.Cos(theta[i]);
Rot_z[0, 1] = -Math.Sin(theta[i]);
Rot_z[1, 0] = Math.Sin(theta[i]);
double[,] Trans_z = { { 1, 0, 0, 0 }, { 0, 1, 0, 0 }, { 0, 0, 1, 0 }, { 0, 0, 0, 1 } };//Trans_z = np.matrix(np.identity(4))
Trans_z[2, 3] = DHtable.D[i];
double[,] Trans_x = { { 1, 0, 0, 0 }, { 0, 1, 0, 0 }, { 0, 0, 1, 0 }, { 0, 0, 0, 1 } };//Trans_z = np.matrix(np.identity(4))
Trans_x[0, 3] = DHtable.A[i];
double[,] Rot_x = { { 1, 0, 0, 0 }, { 0, 1, 0, 0 }, { 0, 0, 1, 0 }, { 0, 0, 0, 1 } };//Rot_x = np.matrix(np.identity(4))
Rot_x[1, 1] = Math.Cos(DHtable.alpha[i]);
Rot_x[2, 2] = Math.Cos(DHtable.alpha[i]);
Rot_x[1, 2] = -Math.Sin(DHtable.alpha[i]);
Rot_x[2, 1] = Math.Sin(DHtable.alpha[i]);
//A_i = Rot_z * Trans_z * Trans_x * Rot_x
double[,] A_i = Matrix.MatrixProduct(Matrix.MatrixProduct(Matrix.MatrixProduct(Rot_z, Trans_z), Trans_x), Rot_x);
return(A_i);
}
}
public static URJoint Select(URJoint[] candidate, URJoint nowJoint)
{
double[] delta = new double[8];
for (int i = 0; i < 8; i++)
{
delta[i] += Math.Abs(candidate[i].J1.rad - nowJoint.J1.rad);
delta[i] += Math.Abs(candidate[i].J2.rad - nowJoint.J2.rad);
delta[i] += Math.Abs(candidate[i].J3.rad - nowJoint.J3.rad);
delta[i] += Math.Abs(candidate[i].J4.rad - nowJoint.J4.rad);
delta[i] += Math.Abs(candidate[i].J5.rad - nowJoint.J5.rad);
delta[i] += Math.Abs(candidate[i].J6.rad - nowJoint.J6.rad);
}
double minValue = delta.Min();
int minIndex = delta.ToList().IndexOf(minValue);
return(candidate[minIndex]);
}
private void Grid_joint_Drop(object sender, DragEventArgs e)
{
if (e.Data.GetDataPresent(DataFormats.StringFormat))
{
string dataString = (string)e.Data.GetData(DataFormats.StringFormat);
try
{
URJoint joint = URJoint.str2joint(dataString);
tb_inJ1.Text = (joint.J1.rad).ToString("0.000");
tb_inJ2.Text = (joint.J2.rad).ToString("0.000");
tb_inJ3.Text = (joint.J3.rad).ToString("0.000");
tb_inJ4.Text = (joint.J4.rad).ToString("0.000");
tb_inJ5.Text = (joint.J5.rad).ToString("0.000");
tb_inJ6.Text = (joint.J6.rad).ToString("0.000");
}
catch
{
MessageBox.Show("URJoint transfer fail!");
}
}
}
public void UI_initial()
{
btn_urc_play.RollUp();
btn_urc_stop.RollUp();
//update UI timer
timer_UI.Interval = TimeSpan.FromMilliseconds(500);
timer_UI.Tick += _timer_Tick;
void _timer_Tick(object ss, EventArgs ee)
{
if (loop_UIpos == true)
{
URCoordinates pos = URc.getPosition();
setUIPostion(pos);
}
if (loop_UIjoint == true)
{
URJoint joint = URc.getJoint();
setUIJoint(joint);
}
}
}
public static URJoint[] InverseKinematic(URCoordinates TCP)
{
TransformationMatrix TM_TCP = new TransformationMatrix(TCP);
double[,] desired_pos = TM_TCP.toArray();
double[,] th = new double[6, 8];
double[,] A = new double[, ] {
{ 0, 0, -DHtable.d6, 1 }
};
double[,] B = new double[, ] {
{ 0, 0, 0, 1 }
};
double[,] P_05 = Matrix.Minus(Matrix.MatrixProduct(desired_pos, A.Transpose()), B.Transpose());
//---theta 1---
double psi = Math.Atan2(P_05[1, 0], P_05[0, 0]);
double p05 = Math.Sqrt(P_05[2 - 1, 0] * P_05[2 - 1, 0] + P_05[1 - 1, 0] * P_05[1 - 1, 0]);
double check = DHtable.d4 / p05;
if (check > 1)
{
check = 1;
}
else if (check < -1)
{
check = -1;
}
double phi = Math.Acos(check);
double tmp = Math.PI / 2.0 + psi + phi;
th[0, 0] = tmp;
th[0, 1] = tmp;
th[0, 2] = tmp;
th[0, 3] = tmp;
tmp = Math.PI / 2.0 + psi - phi;
th[0, 4] = tmp;
th[0, 5] = tmp;
th[0, 6] = tmp;
th[0, 7] = tmp;
//---theta5---
double[] cl = new double[] { 0, 4 };
for (int i = 0; i < cl.Length; i++)
{
int c = (int)cl[i];
double[,] T_10 = AH(1, th, c).Inverse();
double[,] T_16 = Matrix.MatrixProduct(T_10, desired_pos);
th[4, c] = +Math2.Acos((T_16[2, 3] - DHtable.d4) / DHtable.d6);
th[4, c + 1] = +Math2.Acos((T_16[2, 3] - DHtable.d4) / DHtable.d6);
th[4, c + 2] = -Math2.Acos((T_16[2, 3] - DHtable.d4) / DHtable.d6);
th[4, c + 3] = -Math2.Acos((T_16[2, 3] - DHtable.d4) / DHtable.d6);
}
//---theta6---
cl = new double[] { 0, 2, 4, 6 };
for (int i = 0; i < cl.Length; i++)
{
int c = (int)cl[i];
double[,] T_10 = AH(1, th, c).Inverse();
double[,] T_16 = Matrix.MatrixProduct(T_10, desired_pos).Inverse2();
th[5, c] = Math2.Atan2((-T_16[1, 2] / Math.Sin(th[4, c])), (T_16[0, 2] / Math.Sin(th[4, c])));
th[5, c + 1] = Math2.Atan2((-T_16[1, 2] / Math.Sin(th[4, c])), (T_16[0, 2] / Math.Sin(th[4, c])));
}
//---theta3---
cl = new double[] { 0, 2, 4, 6 };
for (int i = 0; i < cl.Length; i++)
{
int c = (int)cl[i];
double[,] T_10 = AH(1, th, c).Inverse();
double[,] T_65 = AH(6, th, c);
double[,] T_54 = AH(5, th, c);
//var tm00 = Matrix.MatrixProduct(T_54, T_65);
//var tm1 = tm00.Inverse();
double[,] T_14 = Matrix.MatrixProduct(Matrix.MatrixProduct(T_10, desired_pos), Matrix.MatrixProduct(T_54, T_65).Inverse2());
//double[,] T_14 = Matrix.MatrixProduct(Matrix.MatrixProduct(Matrix.MatrixProduct(T_10, desired_pos), T_54.Inverse()), T_65.Inverse());
double[,] AA = new double[, ] {
{ 0, -DHtable.d4, 0, 1 }
};
double[,] BB = new double[, ] {
{ 0, 0, 0, 1 }
};
double[,] P_13 = Matrix.Minus(Matrix.MatrixProduct(T_14, AA.Transpose()), BB.Transpose());
// check = (linalg.norm(P_13)**2 - a2**2 - a3**2) / (2 * a2 * a3)
double norm_squre = P_13[0, 0] * P_13[0, 0] + P_13[1, 0] * P_13[1, 0] + P_13[2, 0] * P_13[2, 0] + P_13[3, 0] * P_13[3, 0];
double t3 = Math2.Acos((norm_squre - DHtable.a2 * DHtable.a2 - DHtable.a3 * DHtable.a3) / (2 * DHtable.a2 * DHtable.a3));
th[2, c] = t3;
th[2, c + 1] = -t3;
}
//---theta2-- //---theta4---
cl = new double[] { 0, 1, 2, 3, 4, 5, 6, 7 };
for (int i = 0; i < cl.Length; i++)
{
int c = (int)cl[i];
double[,] T_10 = AH(1, th, c).Inverse();
double[,] T_65 = AH(6, th, c).Inverse2();
double[,] T_54 = AH(5, th, c).Inverse();
double[,] T_14 = Matrix.MatrixProduct(Matrix.MatrixProduct(Matrix.MatrixProduct(T_10, desired_pos), T_65), T_54);
double[,] AA = new double[, ] {
{ 0, -DHtable.d4, 0, 1 }
};
double[,] BB = new double[, ] {
{ 0, 0, 0, 1 }
};
double[,] P_13 = Matrix.Minus(Matrix.MatrixProduct(T_14, AA.Transpose()), BB.Transpose());
//theta2
double norm = Math.Sqrt(P_13[0, 0] * P_13[0, 0] + P_13[1, 0] * P_13[1, 0] + P_13[2, 0] * P_13[2, 0] + P_13[3, 0] * P_13[3, 0]);
th[1, c] = -Math.Atan2(P_13[1, 0], -P_13[0, 0]) + Math.Asin(DHtable.a3 * Math.Sin(th[2, c]) / norm);
//theta2
double[,] T_32 = AH(3, th, c).Inverse();
double[,] T_21 = AH(2, th, c).Inverse();
double[,] T_34 = Matrix.MatrixProduct(Matrix.MatrixProduct(T_32, T_21), T_14);
th[3, c] = Math.Atan2(T_34[1, 0], T_34[0, 0]);
}
th = th.Transpose();
URJoint[] rtn = new URJoint[8];
for (int i = 0; i < 8; i++)
{
rtn[i] = new URJoint(th[i, 0].rad(), th[i, 1].rad(), th[i, 2].rad(), th[i, 3].rad(), th[i, 4].rad(), th[i, 5].rad());
}
return(rtn);
double[,] AH(int n, double[,] _th, int c)
{//th = new double[6, 8];
double[,] T_a = { { 1, 0, 0, 0 }, { 0, 1, 0, 0 }, { 0, 0, 1, 0 }, { 0, 0, 0, 1 } };
T_a[0, 3] = DHtable.A[n - 1];
double[,] T_d = { { 1, 0, 0, 0 }, { 0, 1, 0, 0 }, { 0, 0, 1, 0 }, { 0, 0, 0, 1 } };
T_d[2, 3] = DHtable.D[n - 1];
double[,] Rzt = new double[, ] {
{ Math.Cos(_th[n - 1, c]), -Math.Sin(_th[n - 1, c]), 0, 0 }, { Math.Sin(_th[n - 1, c]), Math.Cos(_th[n - 1, c]), 0, 0 }, { 0, 0, 1, 0 }, { 0, 0, 0, 1 }
};
double[,] Rxa = new double[, ] {
{ 1, 0, 0, 0 }, { 0, Math.Cos(DHtable.alpha[n - 1]), -Math.Sin(DHtable.alpha[n - 1]), 0 }, { 0, Math.Sin(DHtable.alpha[n - 1]), Math.Cos(DHtable.alpha[n - 1]), 0 }, { 0, 0, 0, 1 }
};
return(Matrix.MatrixProduct(Matrix.MatrixProduct(Matrix.MatrixProduct(T_d, Rzt), T_a), Rxa));
}
}
private void Btn_pmovej_Click(object sender, RoutedEventArgs e)
{
URJoint J = Main.getUIJoint();
Task.Run(() => { Main.UR.goJoint(J); });
}
