/// <summary> /// Tính toán ma trận chuyển đổi điểm TT(PBASE) từ cmd /// </summary> /// <returns></returns> public static Matrix4x4 Calculate(PointWithTheta inputPoint, PointWithTheta alignPointOCam, Matrix4x4 transMatrixPOROC) { float tempX = inputPoint.X; float tempY = inputPoint.Y; float tempTheta = inputPoint.Theta; Matrix4x4 PGET = new Matrix4x4((float)Math.Cos(tempTheta), (float)-Math.Sin(tempTheta), 0, tempX, (float)Math.Sin(tempTheta), (float)Math.Cos(tempTheta), 0, tempY, 0, 0, 1, 0, 0, 0, 0, 1); tempX = alignPointOCam.X; tempY = alignPointOCam.Y; tempTheta = alignPointOCam.Theta; Matrix4x4 PVISION = new Matrix4x4((float)Math.Cos(tempTheta), (float)-Math.Sin(tempTheta), 0, tempX, (float)Math.Sin(tempTheta), (float)Math.Cos(tempTheta), 0, tempY, 0, 0, 1, 0, 0, 0, 0, 1); Matrix4x4 PGET_OCINV; Matrix4x4.Invert(PGET, out PGET_OCINV); Matrix4x4 outputMatrix = Matrix4x4.Multiply(PGET_OCINV, transMatrixPOROC); outputMatrix = Matrix4x4.Multiply(outputMatrix, PVISION); return(outputMatrix); }
/// <summary> /// Tính toán TT /// Lấy điểm Input từ Robot /// Chuyển đổi từ điểm Align Camera qua Tool chuyển NPoint /// Tính toán từ 2 điểm trên ra ma trận TT /// </summary> /// <param name="cmd"></param> public void CalTTTransMatrix(string cmd, PointWithTheta alignPoint) { PointWithTheta inputPoint = Helper.GetRobotPointFromCmd(cmd); PointWithTheta alignPointOCam = Helper.TransPointFromNPoint(PointTransformToolFromNPointCalib, alignPoint); TransformTT = TransformTTCal.Calculate(inputPoint, alignPointOCam, TransMatrixPOROV); CalTTMatrixOK = true; }
/// <summary> /// /// </summary> /// <returns></returns> public PointWithTheta CalculateAlignRB() { PointWithTheta outputAlign = null; CogAcqFifoEdit.Subject.Run(); // Chụp ảnh, gửi ảnh sang Tool Calib if (CogAcqFifoEdit.Subject.RunStatus.Result != CogToolResultConstants.Accept) { MessageBox.Show(CogAcqFifoEdit.Subject.RunStatus.Exception.Message); return(null); } else { CogCalibGrid.Subject.InputImage = CogAcqFifoEdit.Subject.OutputImage; } CogCalibGrid.Subject.Run(); // Calib xong gửi ảnh qua Tool Align if (CogCalibGrid.Subject.RunStatus.Result != CogToolResultConstants.Accept) { MessageBox.Show(CogCalibGrid.Subject.RunStatus.Exception.Message); return(null); } else { CogPMAlign.Subject.InputImage = CogCalibGrid.Subject.OutputImage; } CogPMAlign.Subject.Run(); // Chạy xong Tool Align trả tọa độ + góc ra đầu ra if (CogPMAlign.Subject.RunStatus.Result != CogToolResultConstants.Accept) { MessageBox.Show(CogPMAlign.Subject.RunStatus.Exception.Message); return(null); } else { DisplayGraphic(); try { float tempX = (float)CogPMAlign.Subject.Results[0].GetPose().TranslationX; float tempY = (float)CogPMAlign.Subject.Results[0].GetPose().TranslationY; float tempTheta = (float)CogPMAlign.Subject.Results[0].GetPose().Rotation; // Trả hiển thị ra CogDisplay //CogDisplayOut.Image = CogAcqFifoEdit.Subject.OutputImage; //CogDisplayOut.StaticGraphics.Add(CogPMAlign.Subject.Results[0].CreateResultGraphics(CogPMAlignResultGraphicConstants.CoordinateAxes), ""); outputAlign = new PointWithTheta(tempX, tempY, tempTheta); } catch { outputAlign = null; } } if (outputAlign != null) { return(RTCAutoCalibTool.Trans(outputAlign)); } return(null); }
/// <summary> /// Chuyển đổi tọa độ 1 điểm qua ma trận chuyển đổi N Point /// </summary> /// <param name="pointTransformToolFromNPointCalib"></param> /// <param name="inputPoint"></param> /// <returns></returns> public static PointWithTheta TransPointFromNPoint(ICogTransform2D pointTransformToolFromNPointCalib, PointWithTheta inputPoint) { double tempX, tempY; pointTransformToolFromNPointCalib.MapPoint(inputPoint.X, inputPoint.Y, out tempX, out tempY); PointWithTheta outputPoint = new PointWithTheta((float)tempX, (float)tempY, inputPoint.Theta); return(outputPoint); }
public static Matrix4x4[] CalPBaseAndPOROC(PointWithTheta robotPoint1, PointWithTheta robotPoint2, PointWithTheta camPoint1, PointWithTheta camPoint2) { Matrix4x4 PGetOC = CalPGET_OC(robotPoint1, robotPoint2, camPoint1, camPoint2); Matrix4x4 PBase = CalPBASE(PGetOC, robotPoint1, camPoint1); Matrix4x4 POROC = CalPOROC(robotPoint1, PGetOC); return(new Matrix4x4[2] { PBase, POROC }); }
/// <summary> /// Tính toán ma trận chuyển đổi hệ Robot sang hệ Camera /// Đầu vào 2 điểm góc xoay hệ tọa độ Camera + điểm tọa độ Robot /// </summary> /// <param name="pointCam1"></param> /// <param name="pointCam2"></param> /// <param name="pointRB"></param> /// <returns></returns> public static Matrix4x4 Calculate(PointWithTheta pointCam1, PointWithTheta pointCam2, PointWithTheta pointRB) { Matrix4x4 outputMatrixInv; double[] PxyGetOV = CalPxPy(pointCam1.X, pointCam1.Y, pointCam1.Theta, pointCam2.X, pointCam2.Y, pointCam2.Theta); Matrix4x4 outputMatrix = CalMatrix(pointRB.X, pointRB.Y, pointRB.Theta, PxyGetOV[0], PxyGetOV[1]); Matrix4x4.Invert(outputMatrix, out outputMatrixInv); return(outputMatrixInv); }
/// <summary> /// Tool chuyển đổi tọa độ Align trả về từ Camera thành tọa độ Robot /// Nếu chưa đủ điều kiện trả về null /// </summary> /// <param name="outputAlign"></param> /// <returns></returns> public PointWithTheta Trans(PointWithTheta outputAlign) { if (CalNpointOK && CalTTMatrixOK) { PointWithTheta inputPointOCam = Helper.TransPointFromNPoint(PointTransformToolFromNPointCalib, outputAlign); PointWithTheta outputRBPoint = Helper.CalTransAlignToRobot(inputPointOCam, TransformTT, TransMatrixPOROV); return(outputRBPoint); } else { return(null); } }
public static Matrix4x4 CalPOROC(PointWithTheta robotpoint1, Matrix4x4 PGetOC) { float tempX = robotpoint1.X; float tempY = robotpoint1.Y; float tempTheta = robotpoint1.Theta; Matrix4x4 PGet = new Matrix4x4((float)Math.Cos(tempTheta), (float)-Math.Sin(tempTheta), 0, (float)tempX, (float)Math.Sin(tempTheta), (float)Math.Cos(tempTheta), 0, (float)tempY, 0, 0, 1, 0, 0, 0, 0, 1); Matrix4x4 invPgetOC; Matrix4x4.Invert(PGetOC, out invPgetOC); return(Matrix4x4.Multiply(PGet, invPgetOC)); }
public static Matrix4x4 CalPBASE(Matrix4x4 mPGETOC, PointWithTheta robotPoint1, PointWithTheta camPoint1) { float x1 = camPoint1.X; float y1 = camPoint1.Y; float thetaGet = robotPoint1.Theta; float xGetOC = mPGETOC.M14; float yGetOC = mPGETOC.M24; float xBase = xBase = (float)((y1 * Math.Sin((System.Double)(2 * thetaGet)) - yGetOC * Math.Sin((System.Double)(2 * thetaGet)) + x1 * Math.Cos((System.Double)(2 * thetaGet)) - xGetOC * Math.Cos((System.Double)(2 * thetaGet)) + x1 - xGetOC) / Math.Cos((System.Double)thetaGet) / 0.2e1); float yBase = (float)(-Math.Sin(thetaGet) * x1 + Math.Cos(thetaGet) * y1 + Math.Sin(thetaGet) * xGetOC - yGetOC * Math.Cos(thetaGet)); Matrix4x4 mPBASEReturn = new Matrix4x4(1, 0, 0, (float)xBase, 0, 1, 0, (float)yBase, 0, 0, 1, 0, 0, 0, 0, 1); return(mPBASEReturn); }
/// <summary> /// Tính toán điểm khi qua ma trận chuyển vị /// </summary> /// <param name="pointWithTheta"></param> /// <param name="transformMatrixPOROV"></param> /// <returns></returns> public static PointWithTheta TransPoint(PointWithTheta pointWithTheta, Matrix4x4 transformMatrixPOROV) { float tempX = pointWithTheta.X; float tempY = pointWithTheta.Y; float tempTheta = pointWithTheta.Theta; float tempOffX = (float)((Math.Cos(tempTheta)) * tempX - (Math.Sin(tempTheta)) * tempY); float tempOffY = (float)((Math.Sin(tempTheta)) * tempX + (Math.Cos(tempTheta)) * tempY); Matrix4x4 inputMatrix = new Matrix4x4((float)Math.Cos(tempTheta), (float)-Math.Sin(tempTheta), 0, tempX, (float)Math.Sin(tempTheta), (float)Math.Cos(tempTheta), 0, tempY, 0, 0, 1, 0, 0, 0, 0, 1); Matrix4x4 outputMatrix = Matrix4x4.Multiply(transformMatrixPOROV, inputMatrix); PointWithTheta outputPoint = new PointWithTheta(outputMatrix.M14, outputMatrix.M24, (float)Math.Asin(outputMatrix.M21)); return(outputPoint); }
/// <summary> /// P_VisionCal = POROC^-1 * PGET * PBASE /// </summary> /// <param name="pointWithTheta"></param> /// <param name="transMatrixPOROV_Inv"></param> /// <param name="transMatrixBASE"></param> /// <returns></returns> private static PointWithTheta TransRobotToCam(PointWithTheta pointWithTheta, Matrix4x4 transMatrixPOROV_Inv, Matrix4x4 transMatrixBASE) { float tempX = pointWithTheta.X; float tempY = pointWithTheta.Y; float tempTheta = pointWithTheta.Theta; Matrix4x4 PGET = new Matrix4x4((float)Math.Cos(tempTheta), (float)-Math.Sin(tempTheta), 0, tempX, (float)Math.Sin(tempTheta), (float)Math.Cos(tempTheta), 0, tempY, 0, 0, 1, 0, 0, 0, 0, 1); Matrix4x4 outputMatrix = Matrix4x4.Multiply(transMatrixPOROV_Inv, PGET); outputMatrix = Matrix4x4.Multiply(outputMatrix, transMatrixBASE); PointWithTheta outputPoint = new PointWithTheta(outputMatrix.M14, outputMatrix.M24, (float)Math.Asin(outputMatrix.M21)); return(outputPoint); }
public static Matrix4x4 CalPGET_OC(PointWithTheta robotPoint1, PointWithTheta robotPoint2, PointWithTheta camPoint1, PointWithTheta camPoint2) { float x1 = camPoint1.X; float y1 = camPoint1.Y; float x2 = camPoint2.X; float y2 = camPoint2.Y; float thetaGet = robotPoint1.Theta; float thetaGet2 = robotPoint2.Theta; float xGetOC = (float)((x1 * Math.Cos((System.Double)(-2 * thetaGet2 + 2 * thetaGet)) + x2 * Math.Cos((System.Double)(-2 * thetaGet2 + 2 * thetaGet)) - 0.2e1 * x1 * Math.Cos((System.Double)(-thetaGet2 + thetaGet)) - 0.2e1 * x2 * Math.Cos((System.Double)(-thetaGet2 + thetaGet)) - 0.2e1 * x1 * Math.Cos((System.Double)(thetaGet2 + thetaGet)) - 0.2e1 * x2 * Math.Cos((System.Double)(thetaGet2 + thetaGet)) + x1 * Math.Cos((System.Double)(2 * thetaGet2)) + x2 * Math.Cos((System.Double)(2 * thetaGet2)) + x1 * Math.Cos((System.Double)(2 * thetaGet)) + x2 * Math.Cos((System.Double)(2 * thetaGet)) - Math.Sin((System.Double)(2 * thetaGet2)) * y1 + y2 * Math.Sin((System.Double)(2 * thetaGet2)) + y1 * Math.Sin((System.Double)(2 * thetaGet)) - Math.Sin((System.Double)(2 * thetaGet)) * y2 + y1 * Math.Sin((System.Double)(-2 * thetaGet2 + 2 * thetaGet)) - y2 * Math.Sin((System.Double)(-2 * thetaGet2 + 2 * thetaGet)) + x1 + x2) / (0.2e1 - 0.4e1 * Math.Cos((System.Double)(-thetaGet2 + thetaGet)) - 0.4e1 * Math.Cos((System.Double)(thetaGet2 + thetaGet)) + 0.2e1 * Math.Cos((System.Double)(-2 * thetaGet2 + 2 * thetaGet)) + 0.2e1 * Math.Cos((System.Double)(2 * thetaGet2)) + 0.2e1 * Math.Cos((System.Double)(2 * thetaGet)))); float yGetOC = (float)((y1 * Math.Cos((System.Double)(-2 * thetaGet2 + 2 * thetaGet)) + y2 * Math.Cos((System.Double)(-2 * thetaGet2 + 2 * thetaGet)) - 0.2e1 * y1 * Math.Cos((System.Double)(-thetaGet2 + thetaGet)) - 0.2e1 * y2 * Math.Cos((System.Double)(-thetaGet2 + thetaGet)) - 0.2e1 * y1 * Math.Cos((System.Double)(thetaGet2 + thetaGet)) - 0.2e1 * y2 * Math.Cos((System.Double)(thetaGet2 + thetaGet)) + Math.Cos((System.Double)(2 * thetaGet2)) * y1 + y2 * Math.Cos((System.Double)(2 * thetaGet2)) + y1 * Math.Cos((System.Double)(2 * thetaGet)) + Math.Cos((System.Double)(2 * thetaGet)) * y2 + x1 * Math.Sin((System.Double)(2 * thetaGet2)) - x2 * Math.Sin((System.Double)(2 * thetaGet2)) - x1 * Math.Sin((System.Double)(2 * thetaGet)) + x2 * Math.Sin((System.Double)(2 * thetaGet)) - x1 * Math.Sin((System.Double)(-2 * thetaGet2 + 2 * thetaGet)) + x2 * Math.Sin((System.Double)(-2 * thetaGet2 + 2 * thetaGet)) + y1 + y2) / (0.2e1 - 0.4e1 * Math.Cos((System.Double)(-thetaGet2 + thetaGet)) - 0.4e1 * Math.Cos((System.Double)(thetaGet2 + thetaGet)) + 0.2e1 * Math.Cos((System.Double)(-2 * thetaGet2 + 2 * thetaGet)) + 0.2e1 * Math.Cos((System.Double)(2 * thetaGet2)) + 0.2e1 * Math.Cos((System.Double)(2 * thetaGet)))); Matrix4x4 mPGETReturn = new Matrix4x4((float)Math.Cos(thetaGet), (float)-Math.Sin(thetaGet), 0, (float)xGetOC, (float)Math.Sin(thetaGet), (float)Math.Cos(thetaGet), 0, (float)yGetOC, 0, 0, 1, 0, 0, 0, 0, 1); return(mPGETReturn); }
internal static PointWithTheta CalTransAlignToRobot(PointWithTheta pointWithTheta, Matrix4x4 transformTT, Matrix4x4 transformMatrixPOROV) { float tempX = pointWithTheta.X; float tempY = pointWithTheta.Y; float tempTheta = pointWithTheta.Theta; Matrix4x4 PVISION = new Matrix4x4((float)Math.Cos(tempTheta), (float)-Math.Sin(tempTheta), 0, tempX, (float)Math.Sin(tempTheta), (float)Math.Cos(tempTheta), 0, tempY, 0, 0, 1, 0, 0, 0, 0, 1); Matrix4x4 PBASE_INV; Matrix4x4.Invert(transformTT, out PBASE_INV); Matrix4x4 outputMatrix = Matrix4x4.Multiply(PVISION, PBASE_INV); outputMatrix = Matrix4x4.Multiply(transformMatrixPOROV, outputMatrix); PointWithTheta outputPoint = new PointWithTheta(outputMatrix.M14, outputMatrix.M24, (float)Math.Asin(outputMatrix.M21)); return(outputPoint); }
/// <summary> /// Xử lý thêm điểm vào list từ lệnh HE nhận từ Robot /// </summary> /// <param name="tempCamPoint"></param> /// <param name="tempRobotPoint"></param> public void AddPoint(PointWithTheta tempCamPoint, PointWithTheta tempRobotPoint) { ListAutoCalibPointsRB.Add(tempRobotPoint); ListAutoCalibPointsCam.Add(tempCamPoint); NumberPoints += 1; }
/// <summary> /// Xử lý Command nhận vào từ cổng TCP /// </summary> /// <param name="cmd"></param> public string Command(string cmd) { Console.WriteLine(cmd); switch (Helper.GetCommandId(cmd)) { case "HEB": string result = CheckConditionStartAutoCalib(); if (result == "OK") { RTCAutoCalibTool.ResetReceiveData(); AutoCalibRunning = true; } else { MessageBox.Show("Không đủ điều kiện chạy!\r\n" + result); return("HE,0"); } break; case "HE": PointWithTheta tempCamPoint = GetNormalAlign(); PointWithTheta tempRobotPoint = Helper.GetRobotPointFromCmd(cmd); if (tempCamPoint != null) { RTCAutoCalibTool.AddPoint(tempCamPoint, tempRobotPoint); } else { return("HE,0"); } break; case "HEE": if (RTCAutoCalibTool.NumberPoints < 11) { MessageBox.Show("Not Enough Calib Point! N = " + RTCAutoCalibTool.NumberPoints.ToString()); return("HE,0"); } if (RTCAutoCalibTool.Calculate()) { MessageBox.Show("AutoCalib Done!"); AutoCalibRunning = false; } else { MessageBox.Show("AutoCalib Fail!"); AutoCalibRunning = false; return("HE,0"); } break; case "TT": var tempPointTT = GetNormalAlign(); if (tempPointTT != null) { RTCAutoCalibTool.CalTTTransMatrix(cmd, tempPointTT); } return("TT,1"); case "XT": string tempMessage; PointWithTheta tempPoint = CalculateAlignRB(); if (tempPoint != null) { tempMessage = Helper.CreatXTMessage(tempPoint); } else { return("XT,0"); } return(tempMessage); default: break; } return("HE,1"); }
/// <summary> /// Chuyển đổi điểm sang định dạng string trả về cho Robot /// </summary> /// <param name="point"></param> /// <returns></returns> public static string CreatXTMessage(PointWithTheta point) { return($"XT,1,{point.X.ToString("###.##")},{point.Y.ToString("###.##")},{point.Theta.ToString("###.##")}"); }