public static subactInfo Pour(Objects toCup) { subactInfo rtn = new subactInfo(); URCoordinates grip = new URCoordinates(toCup.gripPos()); URCoordinates up = new URCoordinates(grip); up.Y -= 0.1f; URCoordinates debug = new URCoordinates(up); debug.Y -= 0.02f;//在高一點以Rxyz有問題 URCoordinates now = new URCoordinates(up); rtn.infotxt.Add("position"); rtn.infotxt.Add(debug.ToPos()); rtn.infotxt.Add(up.ToPos()); now.X -= 0.08f; rtn.infotxt.Add(now.ToPos()); rtn.infotxt.Add("Rmovej"); rtn.infotxt.Add("[0,0,0,0,0,-2]"); rtn.infotxt.Add("position"); rtn.infotxt.Add(debug.ToPos()); return(rtn); }
public static subactInfo Place(Objects cup, URCoordinates Wpoint) { cup.setNowPos(Wpoint); subactInfo rtn = new subactInfo(); URCoordinates up = new URCoordinates(cup.gripPos()); URCoordinates debug = new URCoordinates(up); URCoordinates down = new URCoordinates(up); down.Y += 0.07f; //下降 debug.Y -= 0.02f; //上升 rtn.infotxt.Add("position"); rtn.infotxt.Add(debug.ToPos()); rtn.infotxt.Add(up.ToPos()); rtn.infotxt.Add(down.ToPos()); rtn.infotxt.Add("gripper"); rtn.infotxt.Add("0"); rtn.infotxt.Add("sleep"); rtn.infotxt.Add("1000"); rtn.infotxt.Add("position"); rtn.infotxt.Add(up.ToPos()); return(rtn); }
public static subactInfo AddaSpoon(Objects toCup) { subactInfo rtn = new subactInfo(); URCoordinates up = new URCoordinates(toCup.gripPos()); up.Rx = 2.2f; up.Ry = -2.2f; up.Rz = 0; up.X -= 0.09f; up.Z += 0.02f; up.Y += 0.03f;//下降 URCoordinates upper = new URCoordinates(up); upper.Y -= 0.07f;//上 rtn.infotxt.Add("pmovej"); rtn.infotxt.Add(upper.ToPos()); URCoordinates add = new URCoordinates(up); add.Rx = 3.14f; add.Ry = 0f; add.Rz = 0f; add.Y -= 0.09f;//上升 add.X += 0.05f; rtn.infotxt.Add("pmovej"); rtn.infotxt.Add(up.ToPos()); rtn.infotxt.Add("pmovej"); rtn.infotxt.Add(add.ToPos()); rtn.infotxt.Add("pmovej"); add.Y -= 0.03f;//上升 rtn.infotxt.Add(add.ToPos()); return(rtn); }
public static subactInfo Pick(Objects cup) { subactInfo rtn = new subactInfo(); URCoordinates grip = new URCoordinates(cup.gripPos()); URCoordinates up = new URCoordinates(grip); up.Y -= 0.1f; URCoordinates debug = new URCoordinates(up); debug.Y -= 0.02f;//在高一點以Rxyz有問題 rtn.infotxt.Add("position"); rtn.infotxt.Add(debug.ToPos()); rtn.infotxt.Add(up.ToPos()); rtn.infotxt.Add("gripper"); rtn.infotxt.Add("0"); rtn.infotxt.Add("sleep"); rtn.infotxt.Add("1000"); rtn.infotxt.Add("position"); rtn.infotxt.Add(grip.ToPos()); rtn.infotxt.Add("gripper"); rtn.infotxt.Add("31"); rtn.infotxt.Add("sleep"); rtn.infotxt.Add("1000"); rtn.infotxt.Add("position"); rtn.infotxt.Add(up.ToPos()); return(rtn); }
public static subactInfo Pick(Objects cup) { subactInfo rtn = new subactInfo(); URCoordinates up = new URCoordinates(cup.gripPos()); URCoordinates debug = new URCoordinates(up); URCoordinates down = new URCoordinates(up); debug.Y -= 0.02f;//上升 rtn.infotxt.Add("position"); rtn.infotxt.Add(debug.ToPos()); rtn.infotxt.Add(up.ToPos()); rtn.infotxt.Add("gripper"); rtn.infotxt.Add("0"); rtn.infotxt.Add("sleep"); rtn.infotxt.Add("1000"); down.Y += 0.07f;//下降 rtn.infotxt.Add("position"); rtn.infotxt.Add(down.ToPos()); rtn.infotxt.Add("gripper"); rtn.infotxt.Add("35"); rtn.infotxt.Add("sleep"); rtn.infotxt.Add("1000"); up.Y -= 0.02f;//上升 rtn.infotxt.Add("position"); rtn.infotxt.Add(up.ToPos()); return(rtn); }
public static subactInfo Stir(Objects toCup) { subactInfo rtn = new subactInfo(); URCoordinates up = new URCoordinates(toCup.gripPos()); up.X -= 0.015f; up.Z += 0.015f; up.Y -= 0.06f;//上升 URCoordinates now = new URCoordinates(up); rtn.infotxt.Add("pmovej"); rtn.infotxt.Add(up.ToPos()); now.Y += 0.05f;//下去 now.X -= 0.01f; rtn.infotxt.Add(now.ToPos()); now.X += 0.01f; now.Z += 0.01f; rtn.infotxt.Add(now.ToPos()); now.X += 0.01f; now.Z -= 0.01f; rtn.infotxt.Add(now.ToPos()); now.X -= 0.01f; now.Z -= 0.01f; rtn.infotxt.Add(now.ToPos()); now.X -= 0.01f; now.Z += 0.01f; rtn.infotxt.Add(now.ToPos()); rtn.infotxt.Add(up.ToPos()); return(rtn); }
//取 座標數值 private void Btn_getPosition_MouseDown(object sender, MouseButtonEventArgs e) { if (e.ChangedButton == MouseButton.Left) { URCoordinates pos = URc.getPosition(); setUIPostion(pos); } }
public void setUIPostion(URCoordinates pos) { tb_urPos_x.Text = (pos.X.mm).ToString("0.0"); tb_urPos_y.Text = (pos.Y.mm).ToString("0.0"); tb_urPos_z.Text = (pos.Z.mm).ToString("0.0"); tb_urPos_rx.Text = (pos.Rx.rad).ToString("0.000"); tb_urPos_ry.Text = (pos.Ry.rad).ToString("0.000"); tb_urPos_rz.Text = (pos.Rz.rad).ToString("0.000"); }
public float Distanse(URCoordinates pos) { float avgx = avg(x, 0, fileterSize); float avgy = avg(y, 0, fileterSize); float avgz = avg(z, 0, fileterSize); double dx = (pos.X - avgx) * (pos.X - avgx); double dy = (pos.Y - avgy) * (pos.Y - avgy); double dz = (pos.Z - avgz) * (pos.Z - avgz); return((float)Math.Pow((dx + dy + dz), 0.5d)); }
private void Button_setDripTrayPos_Click(object sender, RoutedEventArgs e) { if (CheckBox_wantTrans.IsChecked == false) { MessageBox.Show("請使用世界座標系"); return; } dripTrayPos = new URCoordinates(cups[0].getX_m(), cups[0].getY_m(), cups[0].getZ_m(), 0, 0, 0); cir_setDrip.Fill = Color_Set; }
public TransformationMatrix(URCoordinates TCP) { RotationVector RV = new RotationVector(TCP.Rx.rad, TCP.Ry.rad, TCP.Rz.rad); RotateMatrix RM = RotationVetor2RotateMatrix(RV); var R = RM.toArray(); Value = new double[, ] { { R[0, 0], R[0, 1], R[0, 2], TCP.X.M }, { R[1, 0], R[1, 1], R[1, 2], TCP.Y.M }, { R[2, 0], R[2, 1], R[2, 2], TCP.Z.M }, { 0, 0, 0, 1 } }; }
private void Button_loadValue_Click(object sender, RoutedEventArgs e) { dripTrayPos = new URCoordinates(); string[] file = System.IO.File.ReadAllLines("value.txt"); TrVal[0] = float.Parse(file[0]); TrVal[1] = float.Parse(file[1]); TrVal[2] = float.Parse(file[2]); baseWz = float.Parse(file[3]); dripTrayPos.X = float.Parse(file[4]); dripTrayPos.Y = float.Parse(file[5]); dripTrayPos.Z = float.Parse(file[6]); WxOffset = float.Parse(file[7]); cir_setTrans.Fill = Color_Saved; cir_setDrip.Fill = Color_Saved; cir_setZoff.Fill = Color_Saved; }
//Drag Drop private void Grid_pos_Drop(object sender, DragEventArgs e) { if (e.Data.GetDataPresent(DataFormats.StringFormat)) { string dataString = (string)e.Data.GetData(DataFormats.StringFormat); try { URCoordinates pos = URCoordinates.str2urc(dataString); tb_inP1.Text = (pos.X.mm).ToString("0.0"); tb_inP2.Text = (pos.Y.mm).ToString("0.0"); tb_inP3.Text = (pos.Z.mm).ToString("0.0"); tb_inP4.Text = (pos.Rx.rad).ToString("0.000"); tb_inP5.Text = (pos.Ry.rad).ToString("0.000"); tb_inP6.Text = (pos.Rz.rad).ToString("0.000"); } catch { MessageBox.Show("URCoordinates transfer fail!"); } } }
private void Btn_test3_PreviewMouseDown(object sender, MouseButtonEventArgs e) { URCoordinates Pnow = URc.getPosition(); double D = Math.Sqrt((P1.X.mm - Pnow.X.mm) * (P1.X.mm - Pnow.X.mm)); double d = 20; if (D > d) { double dx = P1.X.mm - Pnow.X.mm; double cmdX = (dx * d) / D; Console.WriteLine("cmdX:" + cmdX); URCoordinates jog = new URCoordinates(cmdX.mm(), 0.mm(), 0.mm(), 0.rad(), 0.rad(), 0.rad()); UR.goJog(jog); } else { UR.goPosition(P1); } e.Handled = true; }
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 subactInfo Pour(Objects toCup) { subactInfo rtn = new subactInfo(); URCoordinates up = new URCoordinates(toCup.gripPos()); up.Y -= 0.03f;//上升 URCoordinates debug = new URCoordinates(up); debug.Y -= 0.01f;//上升 URCoordinates now = new URCoordinates(up); rtn.infotxt.Add("position"); rtn.infotxt.Add(debug.ToPos()); rtn.infotxt.Add(up.ToPos()); now.X -= 0.09f; rtn.infotxt.Add(now.ToPos()); now.Rx -= 1.0f; now.Ry += 1.0f; rtn.infotxt.Add(now.ToPos()); now.X += 0.04f; rtn.infotxt.Add(now.ToPos()); now.Rx -= 0.57f; now.Ry += 0.57f; rtn.infotxt.Add(now.ToPos()); now.X += 0.02f; rtn.infotxt.Add(now.ToPos()); now.Rx -= 1.13f; now.Ry += 1.13f; rtn.infotxt.Add(now.ToPos()); up.Y -= 0.02f;//上升 rtn.infotxt.Add(up.ToPos()); rtn.infotxt.Add(debug.ToPos()); return(rtn); }
public void setNowPos(URCoordinates urc) { nowPos = new URCoordinates(urc); }
private URCoordinates nowPos = new URCoordinates();//再action中 可能因為中間移動過 所以必須記錄移動後的位置 以免之後抓空 public void saveNowPos() { nowPos = new URCoordinates(getX_m(), getY_m(), getZ_m(), 0, 0, 0); }
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)); } }
//move control private void Btn_pmovep_Click(object sender, RoutedEventArgs e) { URCoordinates P = Main.getUIPosition(); Task.Run(() => { Main.UR.goPosition(P); }); }