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