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