public override XYUPoint GetObjPonit(XYUPoint objVisoionpoint, XYUPoint SanpMachinePoint)
{
XYUPoint ObjPoint = new XYUPoint();
HTuple qx, qy;
HOperatorSet.AffineTransPoint2d(m_Hom2Dutr, objVisoionpoint.x, objVisoionpoint.y, out qx, out qy);
ObjPoint.x = SanpMachinePoint.x - qx[0].D;
ObjPoint.y = SanpMachinePoint.y - qy[0].D;
return(ObjPoint);
}
public XYUPoint GetDispOrLaserPoint(XYUPoint OldMachineModlePos, XYUPoint NowMachineModlePos, XYUPoint OldPinPos)
{
HOperatorSet.VectorAngleToRigid(OldMachineModlePos.x, OldMachineModlePos.y, 0, NowMachineModlePos.x, NowMachineModlePos.y, NowMachineModlePos.u, out HTuple hom2d);
HTuple qx, qy;
HOperatorSet.AffineTransPoint2d(hom2d, OldPinPos.x, OldPinPos.y, out qx, out qy);
XYUPoint NowPoint = new XYUPoint(qx[0].D, qy[0].D, NowMachineModlePos.u);
return(NowPoint);
}
public override XYUPoint GetObjPonit(XYUPoint objVisoionpoint, XYUPoint SanpMachinePoint)
{
HTuple qx, qy;
HOperatorSet.AffineTransPoint2d(m_Hom2Dutr, objVisoionpoint.x, objVisoionpoint.y, out qx, out qy);
XYUPoint dstpoint = new XYUPoint();
dstpoint.x = SanpMachinePoint.x - qx + AidPoint.x;
dstpoint.y = SanpMachinePoint.y - qy + AidPoint.y;
return(dstpoint);
}
public override XYUPoint GetObjPonit(XYUPoint objVisoionpoint, XYUPoint SanpMachinePoint)
{
XYUPoint objMachinePos = new XYUPoint();
HTuple hTuplex, hTupley;
HTuple qx, qy;
HOperatorSet.AffineTransPoint2d(m_Hom2Ddtr, objVisoionpoint.x, objVisoionpoint.y, out qx, out qy);
objMachinePos.x = qx[0].D;
objMachinePos.y = qy[0].D;
return(objMachinePos);
}
public void Init()
{
offSetX = 0;
offSetY = 0;
offSetU = 0;
highZ = 0;
NozzleCalibVisionCoor = new Point2d(0, 0);
//吸嘴标定Robot坐标
NozzleCalibRobotCoor = new XYUPoint(0, 0, 0);
IsCounterclockwise_Rotation = true;
u0degModle = 0;
}
public override XYUPoint GetDstPonit(XYUPoint dstVisoionpoint, XYUPoint SanpMachinePoint)
{
XYUPoint dstMachinePos = new XYUPoint();
HTuple hTuplex, hTupley;
HTuple qx, qy;
HOperatorSet.AffineTransPoint2d(m_Hom2Dutd, dstVisoionpoint.x, dstVisoionpoint.y, out hTuplex, out hTupley);
HOperatorSet.AffineTransPoint2d(m_Hom2Ddtr, hTuplex, hTupley, out qx, out qy);
XYUPoint vec = new XYUPoint();
vec.x = SanpMachinePoint.x - CalibPoint.x;
vec.y = SanpMachinePoint.y - CalibPoint.y;
dstMachinePos.x = qx[0].D + vec.x;
dstMachinePos.y = qy[0].D + vec.y;
return(dstMachinePos);
}
/// <summary>
/// 获取对应Laser点和Pin的 现况下的位置
/// </summary>
/// <param name="OldVisoionModlePos">模板视觉坐标位置</param>
/// <param name="NowVisionModlePos">现在匹配模板视觉坐标位置</param>
/// <param name="OldSnapModlePos">模板拍照机械位置</param>
/// <param name="OldPinMachinePos">开始设置的Pin机械位置</param>
/// <param name=""></param>
/// <returns></returns>
public XYUPoint GetDispOrLaserPoint2(XYUPoint OldVisoionModlePos, XYUPoint NowVisionModlePos, XYUPoint OldSnapModlePos, XYUPoint OldPinMachinePos, XYUPoint NowSnapMchinePos)
{
HOperatorSet.VectorAngleToRigid(OldVisoionModlePos.x, OldVisoionModlePos.y, OldVisoionModlePos.u, NowVisionModlePos.x, NowVisionModlePos.y, NowVisionModlePos.u, out HTuple hom2d);
HTuple vx, vy;
XYUPoint machineXY = new XYUPoint(OldSnapModlePos.x - OldPinMachinePos.x, OldSnapModlePos.y - OldPinMachinePos.y, 0);
HOperatorSet.AffineTransPoint2d(m_Hom2Drtu, machineXY.x, machineXY.y, out vx, out vy);
HTuple qvx, qvy;
HOperatorSet.AffineTransPixel(hom2d, vx, vy, out qvx, out qvy);
XYUPoint NowPinMachinePoint = GetDstPonit(new XYUPoint(qvx, qvy, 0), NowSnapMchinePos);
return(NowPinMachinePoint);
}
/// <summary>
/// 获取 吸嘴中新对准点
/// </summary>
/// <param name="snappos"></param>
/// <param name="u0degModle"></param>
/// <param name="Point2dVisionCoor"></param>
/// <returns></returns>
public XYUPoint GetPosWhenUpCameraSnap(int nIndex, XYUPoint snappos, XYUPoint Point2dVisionCoor)
{
XYUPoint posull = new XYUPoint(double.MaxValue, double.MaxValue, double.MaxValue);
if (nIndex > resultCalibMulitNozzle.nozzleSturts.Count || nIndex < 0)
{
return(posull);
}
XYUPoint NozzleCalibRobotCoor = resultCalibMulitNozzle.nozzleSturts[nIndex].NozzleCalibRobotCoor;
Point2d NozzleCalibVisionCoor = resultCalibMulitNozzle.nozzleSturts[nIndex].NozzleCalibVisionCoor;
double u0degModle = resultCalibMulitNozzle.nozzleSturts[nIndex].u0degModle;
bool IsCounterclockwise_Rotation = resultCalibMulitNozzle.nozzleSturts[nIndex].IsCounterclockwise_Rotation;
// u0degModle 0 度模板（做模板） u的角度
HTuple qx, qy;
HTuple dsqx, dsqy;
HTuple dcqx, dcqy;
HOperatorSet.AffineTransPoint2d(m_Hom2Drtd, snappos.x, snappos.y, out dsqx, out dsqy);
HOperatorSet.AffineTransPoint2d(m_Hom2Drtd, CalibPos.x, CalibPos.y, out dcqx, out dcqy);
HTuple calbx, calby;
HOperatorSet.AffineTransPoint2d(m_Hom2Ddtr, dcqx, dcqy, out calbx, out calby);
HOperatorSet.AffineTransPoint2d(m_Hom2Dutd, Point2dVisionCoor.x, Point2dVisionCoor.y, out qx, out qy);
HTuple vecx, vecy;
vecx = dsqx - dcqx;
vecy = dsqy - dcqy;
//mark 坐标
HTuple dmarkx, dmarky;
dmarkx = qx + vecx;
dmarky = qy + vecy;
HTuple rmarkx, rmarky;
HOperatorSet.AffineTransPoint2d(m_Hom2Ddtr, dmarkx, dmarky, out rmarkx, out rmarky);
if (Point2dVisionCoor.u > Math.PI)
{
Point2dVisionCoor.u = Point2dVisionCoor.u - 2 * Math.PI;
}
else if (Point2dVisionCoor.u < -Math.PI)
{
Point2dVisionCoor.u = Point2dVisionCoor.u + 2 * Math.PI;
}
//旋转轴的物理 角度
HTuple tupleAngle = 0;
if (!IsCounterclockwise_Rotation)// 旋转轴 正向 逆时针 IsCounterclockwise_Rotation=ture
{
tupleAngle = u0degModle - 180.000 * Point2dVisionCoor.u / Math.PI;
}
else
{
tupleAngle = u0degModle + 180.000 * Point2dVisionCoor.u / Math.PI;
}
HTuple rqx, rqy;
HOperatorSet.AffineTransPoint2d(m_Hom2Ddtr, qx, qy, out rqx, out rqy);
XYUPoint post = new XYUPoint();
post.x = rqx + snappos.x - CalibPos.x;
post.y = rqy + snappos.y - CalibPos.y;
HTuple vecNx, vecNy;
HTuple dcnqx, dcnqy;
HOperatorSet.AffineTransPoint2d(m_Hom2Drtd, NozzleCalibRobotCoor.x, NozzleCalibRobotCoor.y, out dcnqx, out dcnqy);
vecNx = NozzleCalibVisionCoor.x - dcnqx;
vecNy = NozzleCalibVisionCoor.y - dcnqy;
HTuple lens = Math.Sqrt(vecNx.D * vecNx.D + vecNy.D * vecNy.D);
//需要旋转的角度
HTuple AngleRote = tupleAngle - snappos.u;
HTuple tupleAnglecalib = 0;
HOperatorSet.AngleLx(dcnqy, dcnqx, NozzleCalibVisionCoor.y, NozzleCalibVisionCoor.x, out tupleAnglecalib);
// HOperatorSet.AngleLl(0,0,0,100,dcnqy, dcnqx, NozzleCalibVisionCoor.y, NozzleCalibVisionCoor.x, out tupleAnglecalib);
HTuple destangleandR = 0;
if (!IsCounterclockwise_Rotation)
{
destangleandR = tupleAnglecalib - Math.PI * (tupleAngle - NozzleCalibRobotCoor.u) / 180.0000;
}
else
{
destangleandR = tupleAnglecalib + Math.PI * (tupleAngle - NozzleCalibRobotCoor.u) / 180.0000;
}
HTuple Destnozzledx = dsqx + Math.Cos(destangleandR.D) * lens;
HTuple Destnozzledy = dsqy - Math.Sin(destangleandR.D) * lens;
HTuple rnozzlex, rnozzley;
HOperatorSet.AffineTransPoint2d(m_Hom2Ddtr, Destnozzledx, Destnozzledy, out rnozzlex, out rnozzley);
HTuple offsetx = rmarkx - rnozzlex;
HTuple offsety = rmarky - rnozzley;
XYUPoint pos = new XYUPoint(snappos.x + offsetx.D, snappos.y + offsety.D, tupleAngle);
return(pos);
}
/// <summary>
/// 上相机看目标 下相机看对象，机械手拿料移动组装
/// </summary>
/// <param name="snappos"></param>
/// <param name="Point2dVisionCoorDst"></param>
/// <param name="Point2dVisionCoorObj"></param>
/// <returns></returns>
public XYUPoint GetPosWhenUpDownSnap(XYUPoint snappos, XYUPoint Point2dVisionCoorDst, XYUPoint Point2dVisionCoorObj)
{
XYUPoint pos = new XYUPoint(snappos.x, snappos.y, 0);
return(pos);
}
/// <summary>
/// 获取 吸嘴中新对准点 上相机拍照 获取吸嘴对应的点视觉坐标 转换得出 机械坐标点（带旋转的机械手 适用 上相机 拍照拾取）
/// </summary>带旋转的机械手 适用 上相机 拍照拾取
/// <param name="snappos"> 拍照点 机械手坐标</param>
/// <param name="u0degModle"> 0 度模板对应的 机械手 机械坐标</param>
/// <param name="Point2dVisionCoor">视觉mark的视觉坐标</param>
/// <returns></returns>
public XYUPoint GetPosWhenUpCameraSnap(XYUPoint snappos, double u0degModle, XYUPoint Point2dVisionCoor)
{
// u0degModle 0 度模板（做模板） u的角度
HTuple qx, qy;
HTuple dsqx, dsqy;
HTuple dcqx, dcqy;
HOperatorSet.AffineTransPoint2d(m_Hom2Drtd, snappos.x, snappos.y, out dsqx, out dsqy);
HOperatorSet.AffineTransPoint2d(m_Hom2Drtd, CalibPos.x, CalibPos.y, out dcqx, out dcqy);
HTuple calbx, calby;
HOperatorSet.AffineTransPoint2d(m_Hom2Ddtr, dcqx, dcqy, out calbx, out calby);
HOperatorSet.AffineTransPoint2d(m_Hom2Dutd, Point2dVisionCoor.x, Point2dVisionCoor.y, out qx, out qy);
HTuple vecx, vecy;
vecx = dsqx - dcqx;
vecy = dsqy - dcqy;
//mark 坐标
HTuple dmarkx, dmarky;
dmarkx = qx + vecx;
dmarky = qy + vecy;
HTuple rmarkx, rmarky;
HOperatorSet.AffineTransPoint2d(m_Hom2Ddtr, dmarkx, dmarky, out rmarkx, out rmarky);
if (Point2dVisionCoor.u > Math.PI)
{
Point2dVisionCoor.u = Point2dVisionCoor.u - 2 * Math.PI;
}
else if (Point2dVisionCoor.u < -Math.PI)
{
Point2dVisionCoor.u = Point2dVisionCoor.u + 2 * Math.PI;
}
HTuple tupleAngle = u0degModle - 180.000 * Point2dVisionCoor.u / Math.PI;
HTuple rqx, rqy;
//HOperatorSet.AffineTransPoint2d(m_Hom2Ddtr, qx, qy, out rqx, out rqy);
//XYUPoint post = new XYUPoint();
//post.x = rqx + snappos.x - CalibPos.x;
//post.y = rqy + snappos.y - CalibPos.y;
HTuple vecNx, vecNy;
HTuple dcnqx, dcnqy;
HOperatorSet.AffineTransPoint2d(m_Hom2Drtd, NozzleCalibRobotCoor.x, NozzleCalibRobotCoor.y, out dcnqx, out dcnqy);
vecNx = NozzleCalibVisionCoor.x - dcnqx;
vecNy = NozzleCalibVisionCoor.y - dcnqy;
HTuple lens = Math.Sqrt(vecNx.D * vecNx.D + vecNy.D * vecNy.D);
////旋转后到的角度
// HTuple tupleAngle = u0degModle+180.000* Point2dVisionCoor.u/Math.PI;
//需要旋转的角度
HTuple AngleRote = tupleAngle - snappos.u;
HTuple tupleAnglecalib = 0;
HOperatorSet.AngleLx(dcnqy, dcnqx, NozzleCalibVisionCoor.y, NozzleCalibVisionCoor.x, out tupleAnglecalib);
// HOperatorSet.AngleLl(0,0,0,100,dcnqy, dcnqx, NozzleCalibVisionCoor.y, NozzleCalibVisionCoor.x, out tupleAnglecalib);
HTuple destangleandR = tupleAnglecalib - Math.PI * (tupleAngle - NozzleCalibRobotCoor.u) / 180.0000;
HTuple Destnozzledx = dsqx + Math.Cos(destangleandR.D) * lens;
HTuple Destnozzledy = dsqy - Math.Sin(destangleandR.D) * lens;
HTuple rnozzlex, rnozzley;
HOperatorSet.AffineTransPoint2d(m_Hom2Ddtr, Destnozzledx, Destnozzledy, out rnozzlex, out rnozzley);
HTuple offsetx = rmarkx - rnozzlex;
HTuple offsety = rmarky - rnozzley;
XYUPoint pos = new XYUPoint(snappos.x + offsetx.D, snappos.y + offsety.D, tupleAngle);
return(pos);
}
public XYUPoint GetModleMachiePos(XYUPoint dstVisoionpoint, XYUPoint SanpMachinePoint)
{
return(GetDstPonit(dstVisoionpoint, SanpMachinePoint));
}
/// <summary> /// 获取对象的点位 /// </summary> /// <param name="dstVisoionpoint"> 对象视觉坐标</param> /// <param name="SanpPoint">对象的拍照机械位置</param> /// <returns>返回对象机械坐标</returns> public abstract XYUPoint GetObjPonit(XYUPoint objVisoionpoint, XYUPoint SanpMachinePoint);
/// <summary> /// 获取目标的点位 /// </summary> /// <param name="dstVisoionpoint"> 目标视觉坐标</param> /// <param name="SanpPoint">目标的拍照机械位置</param> /// <returns>返回目标机械坐标</returns> public abstract XYUPoint GetDstPonit(XYUPoint dstVisoionpoint, XYUPoint SanpMachinePoint);