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