public override void DoCalib()
{
if (PlatformUp != null)
{
var isFirst = true;
foreach (var pos in PlatformUpPos)
{
if (isFirst)
{
isFirst = false;
PlatformUp.EnterAuto(this).Jump(pos, 0);
}
else
{
PlatformUp.EnterAuto(this).Jump(pos, PlatformUpJumpHeight);
}
}
}
{
var ret = XyzPlarformCalibration.CalcAffineTransform(ProductPos.Select(p => new PosXYZ(p.X, p.Y, 0)).ToList(), PlatformUpPos.Select(p => new PosXYZ(p.X, p.Y, 0)).ToList());
Log($"计算产品坐标到上平台转换矩阵:\r\n"
+ $"{ret.Item1[0, 0]:F6},{ret.Item1[0, 1]:F6},{ret.Item1[0, 2]:F6},{ret.Item1[0, 3]:F6}\r\n"
+ $"{ret.Item1[1, 0]:F6},{ret.Item1[1, 1]:F6},{ret.Item1[1, 2]:F6},{ret.Item1[1, 3]:F6}\r\n"
+ $"{ret.Item1[2, 0]:F6},{ret.Item1[2, 1]:F6},{ret.Item1[2, 2]:F6},{ret.Item1[2, 3]:F6}\r\n"
+ $"{ret.Item1[3, 0]:F6},{ret.Item1[3, 1]:F6},{ret.Item1[3, 2]:F6},{ret.Item1[3, 3]:F6}\r\n", LogLevel.Info);
Log($"计算误差:\r\n{ret.Item2:F2}\r\n", LogLevel.Info);
OutputTransform = new TransformParams(ret.Item1);
}
}
public void Test_AffineTransform()
{
List <PosXYZ> cpuPos = new List <PosXYZ>()
{
new PosXYZ(62.69, -64.8, 0.987),
new PosXYZ(62.69, -96.8, 1.060),
new PosXYZ(94.69, -96.8, 1.108),
new PosXYZ(94.69, -64.8, 1.009),
};
List <PosXYZ> pins = new List <PosXYZ>()
{
new PosXYZ(46.69, -48.8, 2.117),
new PosXYZ(46.69, -112.8, 2.131),
new PosXYZ(110.69, -112.8, 2.179),
new PosXYZ(110.69, -48.8, 2.051),
};
{
var ret = XyzPlarformCalibration.CalcAffineTransform(cpuPos, pins);
Console.WriteLine($"CalcAffineTransform:\r\n"
+ $"{ret.Item1[0, 0]:F6},{ret.Item1[0, 1]:F6},{ret.Item1[0, 2]:F6},{ret.Item1[0, 3]:F6}\r\n"
+ $"{ret.Item1[1, 0]:F6},{ret.Item1[1, 1]:F6},{ret.Item1[1, 2]:F6},{ret.Item1[1, 3]:F6}\r\n"
+ $"{ret.Item1[2, 0]:F6},{ret.Item1[2, 1]:F6},{ret.Item1[2, 2]:F6},{ret.Item1[2, 3]:F6}\r\n"
+ $"{ret.Item1[3, 0]:F6},{ret.Item1[3, 1]:F6},{ret.Item1[3, 2]:F6},{ret.Item1[3, 3]:F6}\r\n");
var mat = DenseMatrix.OfArray(new double[, ]
{
{ ret.Item1[0, 0], ret.Item1[0, 1], ret.Item1[0, 3] },
{ ret.Item1[1, 0], ret.Item1[1, 1], ret.Item1[1, 3] },
{ 0, 0, 1 }
}).Inverse();
var newMat = DenseMatrix.OfArray(new double[, ]
{
{ mat[0, 0], mat[0, 1], mat[0, 2] },
{ mat[1, 0], mat[1, 1], mat[1, 2] },
});
Console.WriteLine($"InverseMatrix:\r\n{newMat.ToString()}");
}
{
var ret = XyzPlarformCalibration.CalcAffineTransform(pins, cpuPos);
Console.WriteLine($"CalcAffineTransform:\r\n"
+ $"{ret.Item1[0, 0]:F6},{ret.Item1[0, 1]:F6},{ret.Item1[0, 2]:F6},{ret.Item1[0, 3]:F6}\r\n"
+ $"{ret.Item1[1, 0]:F6},{ret.Item1[1, 1]:F6},{ret.Item1[1, 2]:F6},{ret.Item1[1, 3]:F6}\r\n"
+ $"{ret.Item1[2, 0]:F6},{ret.Item1[2, 1]:F6},{ret.Item1[2, 2]:F6},{ret.Item1[2, 3]:F6}\r\n"
+ $"{ret.Item1[3, 0]:F6},{ret.Item1[3, 1]:F6},{ret.Item1[3, 2]:F6},{ret.Item1[3, 3]:F6}\r\n");
}
}
public newRightMeasureUp(int id, string name, Station station) : base(id, name, station)
{
VioTransFinish = Machine.Ins.Find <IVioEx>("RTransFinishUp");
VioMeasureFinish = Machine.Ins.Find <IVioEx>("RMeasureFinishUp");
Platform = Machine.Ins.Find <PlatformEx>("RightUp");
Platform.SafeChecks.Clear();
Platform.SafeChecks.Add(new UpSafeCheck(Machine.Ins.Find <PlatformEx>("RightCarrier"), SafeCheckType.AutoHome));
Platform.SafeChecks.Add(new UpSafeCheck(Machine.Ins.Find <PlatformEx>("RightCarrier"), SafeCheckType.Auto));
Platform.SafeChecks.Add(new UpSafeCheck(Machine.Ins.Find <PlatformEx>("RightCarrier"), SafeCheckType.ManualHome));
Platform.SafeChecks.Add(new UpSafeCheck(Machine.Ins.Find <PlatformEx>("RightCarrier"), SafeCheckType.Manual));
var pToUp = new Func <double[], double[]>(d =>
{
var pnew = XyzPlarformCalibration.AffineTransform(new PosXYZ(d), Machine.Ins.Settings.Calibration.RightUpTransform);
return(pnew.Data());
});
var upToP = new Func <double[], double[]>(d =>
{
var pnew = XyzPlarformCalibration.AffineInverseTransform(Machine.Ins.Settings.Calibration.RightUpTransform, new PosXYZ(d));
return(pnew.Data());
});
var upToDown = new Func <double[], double[]>(d =>
{
var pnew = XyzPlarformCalibration.AffineTransform(new PosXYZ(d), Machine.Ins.Settings.Calibration.RightTransform);
return(pnew.Data());
});
var downToUp = new Func <double[], double[]>(d =>
{
var pnew = XyzPlarformCalibration.AffineInverseTransform(Machine.Ins.Settings.Calibration.RightTransform, new PosXYZ(d));
return(pnew.Data());
});
Platform.PosConvertFuncs.Add("P->UP", pToUp);
Platform.PosConvertFuncs.Add("UP->P", upToP);
Platform.PosConvertFuncs.Add("UP->DOWN", upToDown);
Platform.PosConvertFuncs.Add("DOWN->UP", downToUp);
}
public void Test_RigidTransform()
{
var p1 = new PosXYZ(1, 1, 0);
var p2 = new PosXYZ(1, 5, 0);
var p3 = new PosXYZ(1, 10, 0);
Console.WriteLine($"p1:{p1}");
Console.WriteLine($"p2:{p2}");
Console.WriteLine($"p3:{p3}");
var p1r = p1.RotateAt(new PosXYZ(), 2).Translate(new PosXYZ(10, 10, 0));
var p2r = p2.RotateAt(new PosXYZ(), 2).Translate(new PosXYZ(10, 10, 0));
var p3r = p3.RotateAt(new PosXYZ(), 2).Translate(new PosXYZ(10, 10, 0));
Console.WriteLine($"p1r:{p1r}");
Console.WriteLine($"p2r:{p2r}");
Console.WriteLine($"p3r:{p3r}");
var ret = XyzPlarformCalibration.CalcAlignTransform(new List <PosXYZ>()
{
p1, p2, p3
}, new List <PosXYZ>()
{
p1r, p2r, p3r
});
for (int i = 0; i < ret.Item1.GetLength(0); i++)
{
for (int j = 0; j < ret.Item1.GetLength(1); j++)
{
Console.Write(ret.Item1[i, j].ToString("F3"));
Console.Write(",");
}
Console.WriteLine();
}
Console.WriteLine();
}
public override void DoCalib()
{
Platform1?.EnterAuto(this).MoveAbs("Wait");
//上工站点位数据采集
Log($"上平台Align点位标定\n------------------------------------------------------");
List <PosXYZ> upAlignPos = new List <PosXYZ>();
bool isFirst = true;
foreach (var pos in AlignPosUp)
{
if (isFirst)
{
isFirst = false;
Platform1?.EnterAuto(this).Jump(pos, 0);
}
else
{
Platform1?.EnterAuto(this).Jump(pos, JumpHeight1);
}
upAlignPos.Add(pos);
DataList.Add(pos.ToString());
Log($"CurPos {pos.ToString()}", LogLevel.Info);
}
//复位平台1
Platform1?.EnterAuto(this).Jump("Wait", JumpHeight1);
Log($"上平台Align点位标定 完成\n------------------------------------------------------");
OnCalibProgress(50);
Platform2?.EnterAuto(this).MoveAbs("Wait");
//下工站点位 数据采集
Log($"下平台Align点位标定\n------------------------------------------------------");
List <PosXYZ> downAlingPos = new List <PosXYZ>();
isFirst = true;
foreach (var pos in AlignPosDown)
{
if (isFirst)
{
isFirst = false;
Platform2?.EnterAuto(this).Jump(pos, 0);
}
else
{
Platform2?.EnterAuto(this).Jump(pos, JumpHeight2);
}
downAlingPos.Add(pos);
DataList.Add(pos.ToString());
Log($"CurPos {pos.ToString()}", LogLevel.Info);
}
Log($"下平台Align点位标定 完成\n------------------------------------------------------");
//计算下平台GT2偏移
Log($"下平台GT1GT2偏移标定\n------------------------------------------------------");
if (Platform2GtOffsetCalibGT1 != null && Platform2GtOffsetCalibGT2 != null)
{
Platform2?.EnterAuto(this).Jump(Platform2GtOffsetCalibGT1, JumpHeight1);
Platform2?.EnterAuto(this).Jump(Platform2GtOffsetCalibGT2, JumpHeight1);
var gt1 = Platform2GtOffsetCalibGT1 as PosXYZ;
var gt2 = Platform2GtOffsetCalibGT2 as PosXYZ;
OutputPlatform2GtOffset = gt2 - gt1;
OutputPlatform2GtOffset.Z = 0;
}
//复位平台2
Platform2?.EnterAuto(this).Jump("Wait", JumpHeight2);
Log($"下平台GT1GT2偏移标定 完成\n------------------------------------------------------");
//复位治具
PlatformCarrier?.EnterAuto(this).MoveAbs("Wait");
OnCalibProgress(100);
//计算上下平台坐标转换
{
var ret = XyzPlarformCalibration.CalcAffineTransform(
upAlignPos.Select(p => new PosXYZ(p.X, p.Y, 0)).ToList(),
downAlingPos.Select(p => new PosXYZ(p.X, p.Y, 0)).ToList());
Log($"计算上下平台转换矩阵:\r\n"
+ $"{ret.Item1[0, 0]:F6},{ret.Item1[0, 1]:F6},{ret.Item1[0, 2]:F6},{ret.Item1[0, 3]:F6}\r\n"
+ $"{ret.Item1[1, 0]:F6},{ret.Item1[1, 1]:F6},{ret.Item1[1, 2]:F6},{ret.Item1[1, 3]:F6}\r\n"
+ $"{ret.Item1[2, 0]:F6},{ret.Item1[2, 1]:F6},{ret.Item1[2, 2]:F6},{ret.Item1[2, 3]:F6}\r\n"
+ $"{ret.Item1[3, 0]:F6},{ret.Item1[3, 1]:F6},{ret.Item1[3, 2]:F6},{ret.Item1[3, 3]:F6}\r\n", LogLevel.Info);
Log($"计算上下平台转换误差:\r\n{ret.Item2:F2}\r\n", LogLevel.Info);
OutputTransForm = new TransformParams(ret.Item1);
}
Log($"GT1GT2偏移:\r\n{OutputPlatform2GtOffset}\r\n", LogLevel.Info);
}
public void Test_AffineTransformWithRot()
{
List <PosXYZ> cpuPos = new List <PosXYZ>()
{
new PosXYZ(62.69, -64.8, 0.987),
new PosXYZ(62.69, -96.8, 1.060),
new PosXYZ(94.69, -96.8, 1.108),
new PosXYZ(94.69, -64.8, 1.009),
};
for (int i = 0; i < cpuPos.Count; i++)
{
cpuPos[i] = cpuPos[i].Scale(new PosXYZ(10.3, 10.31, 1)).RotateAt(new PosXYZ(), 42.2).Translate(new PosXYZ(10, 11.2d, 0));
}
List <PosXYZ> oldcpupos = new List <PosXYZ>()
{
new PosXYZ(62.69, -64.8, 0.987),
new PosXYZ(62.69, -96.8, 1.060),
new PosXYZ(94.69, -96.8, 1.108),
new PosXYZ(94.69, -64.8, 1.009),
};
{
var ret = XyzPlarformCalibration.CalcAffineTransform(cpuPos, oldcpupos);
Console.WriteLine($"CalcAffineTransform:\r\n"
+ $"{ret.Item1[0, 0]:F6},{ret.Item1[0, 1]:F6},{ret.Item1[0, 2]:F6},{ret.Item1[0, 3]:F6}\r\n"
+ $"{ret.Item1[1, 0]:F6},{ret.Item1[1, 1]:F6},{ret.Item1[1, 2]:F6},{ret.Item1[1, 3]:F6}\r\n"
+ $"{ret.Item1[2, 0]:F6},{ret.Item1[2, 1]:F6},{ret.Item1[2, 2]:F6},{ret.Item1[2, 3]:F6}\r\n"
+ $"{ret.Item1[3, 0]:F6},{ret.Item1[3, 1]:F6},{ret.Item1[3, 2]:F6},{ret.Item1[3, 3]:F6}\r\n");
Console.WriteLine($"Error:{ret.Item2:F6}");
var mat = DenseMatrix.OfArray(new double[, ]
{
{ ret.Item1[0, 0], ret.Item1[0, 1], ret.Item1[0, 3] },
{ ret.Item1[1, 0], ret.Item1[1, 1], ret.Item1[1, 3] },
{ 0, 0, 1 }
}).Inverse();
var newMat = DenseMatrix.OfArray(new double[, ]
{
{ mat[0, 0], mat[0, 1], mat[0, 2] },
{ mat[1, 0], mat[1, 1], mat[1, 2] },
});
Console.WriteLine($"InverseMatrix:\r\n{newMat.ToString()}");
}
{
var ret = XyzPlarformCalibration.CalcAffineTransform(oldcpupos, cpuPos);
Console.WriteLine($"CalcAffineTransform Inverse:\r\n"
+ $"{ret.Item1[0, 0]:F6},{ret.Item1[0, 1]:F6},{ret.Item1[0, 2]:F6},{ret.Item1[0, 3]:F6}\r\n"
+ $"{ret.Item1[1, 0]:F6},{ret.Item1[1, 1]:F6},{ret.Item1[1, 2]:F6},{ret.Item1[1, 3]:F6}\r\n"
+ $"{ret.Item1[2, 0]:F6},{ret.Item1[2, 1]:F6},{ret.Item1[2, 2]:F6},{ret.Item1[2, 3]:F6}\r\n"
+ $"{ret.Item1[3, 0]:F6},{ret.Item1[3, 1]:F6},{ret.Item1[3, 2]:F6},{ret.Item1[3, 3]:F6}\r\n");
}
{
var ret = XyzPlarformCalibration.CalcAlignTransform(cpuPos, oldcpupos);
Console.WriteLine($"CalcAlignTransform:\r\n"
+ $"{ret.Item1[0, 0]:F6},{ret.Item1[0, 1]:F6},{ret.Item1[0, 2]:F6}\r\n"
+ $"{ret.Item1[1, 0]:F6},{ret.Item1[1, 1]:F6},{ret.Item1[1, 2]:F6}\r\n"
+ $"{ret.Item1[2, 0]:F6},{ret.Item1[2, 1]:F6},{ret.Item1[2, 2]:F6}\r\n");
Console.WriteLine($"Error:{ret.Item2:F6}");
}
}
public newRightMeasureDown(int id, string name, Station station) : base(id, name, station)
{
DoGTCylinder = Machine.Ins.Find <ICylinderEx>("RGTCylinder");
VioTransInp = Machine.Ins.Find <IVioEx>("RTransInp");
VioBarcodeFinish = Machine.Ins.Find <IVioEx>("RBarcodeFinish");
VioTransFinish = Machine.Ins.Find <IVioEx>("RTransFinishDown");
VioMeasureFinish = Machine.Ins.Find <IVioEx>("RMeasureFinishDown");
Platform = Machine.Ins.Find <PlatformEx>("RightDown");
Platform.SafeChecks.Clear();
Platform.SafeChecks.Add(new DownSafeCheck(Machine.Ins.Find <PlatformEx>("RightCarrier"), DoGTCylinder, SafeCheckType.AutoHome));
Platform.SafeChecks.Add(new DownSafeCheck(Machine.Ins.Find <PlatformEx>("RightCarrier"), DoGTCylinder, SafeCheckType.Auto));
Platform.SafeChecks.Add(new DownSafeCheck(Machine.Ins.Find <PlatformEx>("RightCarrier"), DoGTCylinder, SafeCheckType.ManualHome));
Platform.SafeChecks.Add(new DownSafeCheck(Machine.Ins.Find <PlatformEx>("RightCarrier"), DoGTCylinder, SafeCheckType.Manual));
var pToDown1 = new Func <double[], double[]>(d =>
{
var pnew = XyzPlarformCalibration.AffineTransform(new PosXYZ(d), Machine.Ins.Settings.Calibration.RightUpTransform);
pnew = XyzPlarformCalibration.AffineTransform(pnew, Machine.Ins.Settings.Calibration.RightTransform);
return(pnew.Data());
});
var pToDown2 = new Func <double[], double[]>(d =>
{
var pnew = XyzPlarformCalibration.AffineTransform(new PosXYZ(d), Machine.Ins.Settings.Calibration.RightUpTransform);
pnew = XyzPlarformCalibration.AffineTransform(pnew, Machine.Ins.Settings.Calibration.RightTransform);
pnew = pnew + Machine.Ins.Settings.Calibration.RightGtOffset;
return(pnew.Data());
});
var down1ToP = new Func <double[], double[]>(d =>
{
var pnew = XyzPlarformCalibration.AffineInverseTransform(Machine.Ins.Settings.Calibration.RightTransform, new PosXYZ(d));
pnew = XyzPlarformCalibration.AffineInverseTransform(Machine.Ins.Settings.Calibration.RightUpTransform, pnew);
return(pnew.Data());
});
var down2ToP = new Func <double[], double[]>(d =>
{
var pnew = new PosXYZ(d) - Machine.Ins.Settings.Calibration.RightGtOffset;
pnew = XyzPlarformCalibration.AffineInverseTransform(Machine.Ins.Settings.Calibration.RightTransform, pnew);
pnew = XyzPlarformCalibration.AffineInverseTransform(Machine.Ins.Settings.Calibration.RightUpTransform, pnew);
return(pnew.Data());
});
var upToDown1 = new Func <double[], double[]>(d =>
{
var pnew = XyzPlarformCalibration.AffineTransform(new PosXYZ(d), Machine.Ins.Settings.Calibration.RightTransform);
return(pnew.Data());
});
var down1ToUp = new Func <double[], double[]>(d =>
{
var pnew = XyzPlarformCalibration.AffineInverseTransform(Machine.Ins.Settings.Calibration.RightTransform, new PosXYZ(d));
return(pnew.Data());
});
var upToDown2 = new Func <double[], double[]>(d =>
{
var pnew = XyzPlarformCalibration.AffineTransform(new PosXYZ(d), Machine.Ins.Settings.Calibration.RightTransform);
pnew = pnew + Machine.Ins.Settings.Calibration.RightGtOffset;
return(pnew.Data());
});
var down2ToUp = new Func <double[], double[]>(d =>
{
var pnew = new PosXYZ(d) - Machine.Ins.Settings.Calibration.RightGtOffset;
pnew = XyzPlarformCalibration.AffineInverseTransform(Machine.Ins.Settings.Calibration.RightTransform, pnew);
return(pnew.Data());
});
Platform.PosConvertFuncs.Add("P->DOWN1", pToDown1);
Platform.PosConvertFuncs.Add("P->DOWN2", pToDown2);
Platform.PosConvertFuncs.Add("DOWN1->P", down1ToP);
Platform.PosConvertFuncs.Add("DOWN2->P", down2ToP);
Platform.PosConvertFuncs.Add("UP->DOWN1", upToDown1);
Platform.PosConvertFuncs.Add("DOWN1->UP", down1ToUp);
Platform.PosConvertFuncs.Add("UP->DOWN2", upToDown2);
Platform.PosConvertFuncs.Add("DOWN2->UP", down2ToUp);
}
