Example #1
0
        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);
            }
        }
Example #2
0
        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");
            }
        }
Example #3
0
        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);
        }
Example #4
0
        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}");
            }
        }