// ---Start of Includes --- #include "stdafx.h" // Kompiler Definitionen #include <mfc_class.h> #include <prgdef.h> #include <standard.h> // Out-Stream WinTextView #include <afxtempl.h> #include <iostream.h> #include <STRSTREA.H> // Algebraische Vektoren und Matrizen #include <complex.h> #include "..\Tools\CString.h" #include "..\math.cpp\matrix.h" #include "..\math.cpp\CErrorPropagation.h" #include "..\math.cpp\MCmath.h" #include "..\math.cpp\MCImplizitMath.h" // Laufwerk-, Path-, FileName- und Extensions-Tools #include "..\Tools\CStringMFCTools.h" #include "..\Tools\DirControl.h" #include "..\Tools\OutStreamText.h" // Fehlerbehandlungs-Tools #include "..\Tools\CErrorTools.h" // Include Init-Program // Includes Interfaces #include "LaserTracker.h" #include "Hexapod.h" #include "HexapodV.h" #include "FTSensor.h" #include ".\FuLControl\InitPrg.h" #include "FuLData.h" #include "FuLIdent.h" #include "FuLTools.h" // --- End of Includes --- CFuLTools::CFuLTools() { } CFuLTools::~CFuLTools(void) { } void CFuLTools::Init(CHexapodV* pHexapods, CFTSensorV* pFTSensors, CFuLData* pFuLData, CFuLIdent* pFuLIdent) { m_pHexapods = pHexapods; m_pFTSensors = pFTSensors; m_pFuLData = pFuLData; } void CFuLTools::HexapodsFlat(double X, double Y, double Z) { if (AfxMessageBox("Move Hexapods to Flat Position ?",MB_YESNO) == IDNO) return; HexapodsReset(); // Define Start Posen Vektor<double> RoboterPoseFL(iDEF_DIMPosevektor); // Hexapods in Flat Position RoboterPoseFL[0] = X/1000.0; RoboterPoseFL[1] = Y/1000.0;

RoboterPoseFL[2] = Z/1000.0; RoboterPoseFL[3] = GradToRad(0); RoboterPoseFL[4] = GradToRad(0); RoboterPoseFL[5] = GradToRad(0); for(int iNrHexapod = 0; iNrHexapod < m_pFuLData->m_pInitPrg->m_Hexapods.m_iNumberOfHexapods; iNrHexapod++) if (m_pFuLData->HXEnabled[iNrHexapod]) (*m_pHexapods)[iNrHexapod].MoveTCPAbsAsynch(RoboterPoseFL); } void CFuLTools::HexapodsCylinderArc(double Radius, double Length, double Arc) { /*CVektor< Vektor<double> > m_Secant; //[iNrHexapod][Sec1-2,Sec2-3,Sec3-1] m_Secant.NewDim(6); double X,Y,Z; for(int iNrHexapod = 0; iNrHexapod < m_pFuLData->m_pInitPrg->m_Hexapods.m_iNumberOfHexapods; iNrHexapod++) { m_Secant[iNrHexapod].SetNewDim(3); for(int iNrPoint = 0; iNrPoint < iDEF_DIMRaumKoordinaten; iNrPoint++) { if (iNrPoint == 2) { X = m_pFuLData->m_GRLTInitPoints[iNrHexapod][iNrPoint][0] - m_pFuLData->m_GRLTInitPoints[iNrHexapod][0][0]; Y = m_pFuLData->m_GRLTInitPoints[iNrHexapod][iNrPoint][1] - m_pFuLData->m_GRLTInitPoints[iNrHexapod][0][1]; Z = m_pFuLData->m_GRLTInitPoints[iNrHexapod][iNrPoint][2] - m_pFuLData->m_GRLTInitPoints[iNrHexapod][0][2]; m_Secant[iNrHexapod][iNrPoint] = sqrt(X*X+Y*Y+Z*Z); } else { X = m_pFuLData->m_GRLTInitPoints[iNrHexapod][iNrPoint][0] - m_pFuLData->m_GRLTInitPoints[iNrHexapod][iNrPoint+1][0]; Y = m_pFuLData->m_GRLTInitPoints[iNrHexapod][iNrPoint][1] - m_pFuLData->m_GRLTInitPoints[iNrHexapod][iNrPoint+1][1]; Z = m_pFuLData->m_GRLTInitPoints[iNrHexapod][iNrPoint][2] - m_pFuLData->m_GRLTInitPoints[iNrHexapod][iNrPoint+1][2]; m_Secant[iNrHexapod][iNrPoint] = sqrt(X*X+Y*Y+Z*Z); } } }*/ if (AfxMessageBox("Move Hexapods to Cylinder Position ?",MB_YESNO) == IDNO) return; HexapodsReset(); // Construction Constants double SecGr = 500; double GrHei = SecGr/(2*tan(GradToRad(30))); double GrHei3 = GrHei/3;

// Point Position in X = Inside or Outside // PtOutsideL // PtInsideL // PtInsideR // PtOutsideR double PartEdgeHeight = 0.85+0.2+(Radius/1000); double PartEdgeLeft = (-75-GrHei3)/1000.0; // half vacuum cup + gripper height double PtOL = m_pFuLData->PartInL/1000.0; double PtIL = PtOL+GrHei/1000.0; double PtOR = (Length - m_pFuLData->PartInR)/1000.0; double PtIR = PtOR-GrHei/1000.0; // Cylinder Usefull Angle of Points double PhiGr = 2*asin(SecGr/(2*Radius)); double PhiGr2 = PhiGr/2; double PhiCupDistL = m_pFuLData->CupDistL/Radius; double PhiCupdistR = m_pFuLData->CupDistR/Radius; // Cylinder Angle of Points from Middle to Outside double Phi5 = 0; double Phi4 = PhiGr2; double Phi3 = Phi4+PhiCupDistL; double Phi2 = Phi3+PhiGr2; double Phi1 = Phi2+PhiGr2; double Phi0 = (Arc/2)/Radius; double Phi = Arc/Radius; double Phi6 = Phi4+PhiCupdistR; double Phi7 = Phi6+PhiGr2; double Phi8 = Phi7+PhiGr2; // Point Position in Y = Up or Down double P8Y = ( Radius*sin(Phi8))/1000.0; double P7Y = ( Radius*sin(Phi7))/1000.0; double P6Y = ( Radius*sin(Phi6))/1000.0; double P5Y = ( Radius*sin(Phi5))/1000.0; double P4Y = ( Radius*sin(Phi4))/1000.0; double P3Y = ( Radius*sin(Phi3))/1000.0; double P2Y = ( Radius*sin(Phi2))/1000.0; double P1Y = ( Radius*sin(Phi1))/1000.0; // Point Position in Z = Forward or Backward double P8Z = (-Radius*cos(Phi8))/1000.0; double P7Z = (-Radius*cos(Phi7))/1000.0; double P6Z = (-Radius*cos(Phi6))/1000.0; double P5Z = (-Radius*cos(Phi5))/1000.0; double P4Z = (-Radius*cos(Phi4))/1000.0; double P3Z = (-Radius*cos(Phi3))/1000.0; double P2Z = (-Radius*cos(Phi2))/1000.0; double P1Z = (-Radius*cos(Phi1))/1000.0; // Arrange Points to Homogene Matrix HMatrix<double> H1; H1.SetNewDim(iDEF_DIMHomogeneKoodinaten,3); H1[0][0] = PtOL; H1[1][0] = -P3Y; H1[2][0] = P3Z; H1[3][0] = 1.0; H1[0][1] = PtIL; H1[1][1] = -P2Y; H1[2][1] = P2Z; H1[3][1] = 1.0; H1[0][2] = PtOL;

H1[1][2] = -P1Y; H1[2][2] = P1Z; H1[3][2] = 1.0; HMatrix<double> H2; H2.SetNewDim(iDEF_DIMHomogeneKoodinaten,3); H2[0][0] = PtOR; H2[1][0] = -P3Y; H2[2][0] = P3Z; H2[3][0] = 1.0; H2[0][1] = PtIR; H2[1][1] = -P2Y; H2[2][1] = P2Z; H2[3][1] = 1.0; H2[0][2] = PtOR; H2[1][2] = -P1Y; H2[2][2] = P1Z; H2[3][2] = 1.0; HMatrix<double> H3; H3.SetNewDim(iDEF_DIMHomogeneKoodinaten,3); H3[0][0] = PtOL; H3[1][0] = P4Y; H3[2][0] = P4Z + m_pFuLData->H3G1/1000.0; H3[3][0] = 1.0; H3[0][1] = PtIL; H3[1][1] = P5Y ; H3[2][1] = P5Z + m_pFuLData->H3G2/1000.0; H3[3][1] = 1.0; H3[0][2] = PtOL; H3[1][2] = -P4Y; H3[2][2] = P4Z + m_pFuLData->H3G3/1000.0; H3[3][2] = 1.0; HMatrix<double> H4; H4.SetNewDim(iDEF_DIMHomogeneKoodinaten,3); H4[0][0] = PtOR; H4[1][0] = P4Y; H4[2][0] = P4Z; H4[3][0] = 1.0; H4[0][1] = PtIR; H4[1][1] = P5Y; H4[2][1] = P5Z; H4[3][1] = 1.0; H4[0][2] = PtOR; H4[1][2] = -P4Y; H4[2][2] = P4Z; H4[3][2] = 1.0; HMatrix<double> H5; H5.SetNewDim(iDEF_DIMHomogeneKoodinaten,3); H5[0][0] = PtOL; H5[1][0] = P1Y; H5[2][0] = P1Z; H5[3][0] = 1.0; H5[0][1] = PtIL; H5[1][1] = P2Y; H5[2][1] = P2Z; H5[3][1] = 1.0;

H5[0][2] = PtOL; H5[1][2] = P3Y; H5[2][2] = P3Z; H5[3][2] = 1.0; HMatrix<double> H6; H6.SetNewDim(iDEF_DIMHomogeneKoodinaten,3); H6[0][0] = PtOR; H6[1][0] = P1Y; H6[2][0] = P1Z; H6[3][0] = 1.0; H6[0][1] = PtIR; H6[1][1] = P2Y; H6[2][1] = P2Z; H6[3][1] = 1.0; H6[0][2] = PtOR; H6[1][2] = P3Y; H6[2][2] = P3Z; H6[3][2] = 1.0; //Pose Greifer to Part Frame HMatrix<double> HTMRH1; HTMRH1.HTMR(H1); HMatrix<double> HTMRH2; HTMRH2.HTMR(H2); HMatrix<double> HTMRH3; HTMRH3.HTMR(H3); HMatrix<double> HTMRH4; HTMRH4.HTMR(H4); HMatrix<double> HTMRH5; HTMRH5.HTMR(H5); HMatrix<double> HTMRH6; HTMRH6.HTMR(H6); //Pose Hexapod to World Frame HMatrix<double> HTBasisH1Welt; HTBasisH1Welt.RPY(m_pFuLData->m_HXWRTransf[0]); HMatrix<double> HTBasisH2Welt; HTBasisH2Welt.RPY(m_pFuLData->m_HXWRTransf[1]); HMatrix<double> HTBasisH3Welt; HTBasisH3Welt.RPY(m_pFuLData->m_HXWRTransf[2]); HMatrix<double> HTBasisH4Welt; HTBasisH4Welt.RPY(m_pFuLData->m_HXWRTransf[3]); HMatrix<double> HTBasisH5Welt; HTBasisH5Welt.RPY(m_pFuLData->m_HXWRTransf[4]); HMatrix<double> HTBasisH6Welt; HTBasisH6Welt.RPY(m_pFuLData->m_HXWRTransf[5]); //Pose Greifer to Hexapod TCP HMatrix<double> HTMTCPH1; HTMTCPH1.RPY(m_pFuLData->m_GRHXTransf[0]); HMatrix<double> HTMTCPH2; HTMTCPH2.RPY(m_pFuLData->m_GRHXTransf[1]); HMatrix<double> HTMTCPH3; HTMTCPH3.RPY(m_pFuLData->m_GRHXTransf[2]); HMatrix<double> HTMTCPH4; HTMTCPH4.RPY(m_pFuLData->m_GRHXTransf[3]); HMatrix<double> HTMTCPH5; HTMTCPH5.RPY(m_pFuLData->m_GRHXTransf[4]);

HMatrix<double> HTMTCPH6; HTMTCPH6.RPY(m_pFuLData->m_GRHXTransf[5]); //Definition Bauteil-KOS over Black-Basis-KOS Vektor<double> PoseBauteilBasisH3(iDEF_DIMPosevektor); PoseBauteilBasisH3[0] = PartEdgeLeft; PoseBauteilBasisH3[1] = 0; PoseBauteilBasisH3[2] = PartEdgeHeight; PoseBauteilBasisH3[3] = GradToRad(0); PoseBauteilBasisH3[4] = GradToRad(0); PoseBauteilBasisH3[5] = GradToRad(0); HMatrix<double> HTBauteilBasisH3; HTBauteilBasisH3.RPY(PoseBauteilBasisH3); HMatrix<double> HTBauteilWelt; HTBauteilWelt = HTBasisH3Welt * HTBauteilBasisH3; Vektor<double> PoseBauteilWelt; PoseBauteilWelt = HTBauteilWelt.IRPY(); m_pFuLData->m_PAWRTransf = HTBauteilWelt.IRPY(); cout<< "Transformation Part Frame to World Frame" << PoseBauteilWelt.Pose_mRad_To_mmGrad(); //Calculate Roboter Pose TCP zu Basis HMatrix<double> HTTCPH1; HTTCPH1 = HTBasisH1Welt.Inverse() * HTBauteilWelt * HTMRH1 * HTMTCPH1.Inverse(); Vektor<double> PoseTCPH1(6); PoseTCPH1 = HTTCPH1.IRPY(); HMatrix<double> HTTCPH2; HTTCPH2 = HTBasisH2Welt.Inverse() * HTBauteilWelt * HTMRH2 * HTMTCPH2.Inverse(); Vektor<double> PoseTCPH2(6); PoseTCPH2 = HTTCPH2.IRPY(); HMatrix<double> HTTCPH3; HTTCPH3 = HTBasisH3Welt.Inverse() * HTBauteilWelt * HTMRH3 * HTMTCPH3.Inverse(); Vektor<double> PoseTCPH3(6); PoseTCPH3 = HTTCPH3.IRPY(); HMatrix<double> HTTCPH4; HTTCPH4 = HTBasisH4Welt.Inverse() * HTBauteilWelt * HTMRH4 * HTMTCPH4.Inverse(); Vektor<double> PoseTCPH4(6); PoseTCPH4 = HTTCPH4.IRPY(); HMatrix<double> HTTCPH5; HTTCPH5 = HTBasisH5Welt.Inverse() * HTBauteilWelt * HTMRH5 * HTMTCPH5.Inverse(); Vektor<double> PoseTCPH5(6); PoseTCPH5 = HTTCPH5.IRPY(); HMatrix<double> HTTCPH6; HTTCPH6 = HTBasisH6Welt.Inverse() * HTBauteilWelt * HTMRH6 * HTMTCPH6.Inverse(); Vektor<double> PoseTCPH6(6); PoseTCPH6 = HTTCPH6.IRPY(); //Move Hexapods to Pose if(!(*m_pHexapods)[0].IsPoseReachable(PoseTCPH1)) AfxMessageBox(_T("Target pose for H1 is not reachable")); else

{ if (m_pFuLData->HXEnabled[0]) (*m_pHexapods)[0].MoveTCPAbsAsynch(PoseTCPH1); } if(!(*m_pHexapods)[2].IsPoseReachable(PoseTCPH3)) AfxMessageBox(_T("Target pose for H3 is not reachable")); else { if (m_pFuLData->HXEnabled[2]) (*m_pHexapods)[2].MoveTCPAbsAsynch(PoseTCPH3); } if(!(*m_pHexapods)[4].IsPoseReachable(PoseTCPH5)) AfxMessageBox(_T("Target pose for H5 is not reachable")); else { if (m_pFuLData->HXEnabled[4]) (*m_pHexapods)[4].MoveTCPAbsAsynch(PoseTCPH5); } if(!(*m_pHexapods)[1].IsPoseReachable(PoseTCPH2)) AfxMessageBox(_T("Target pose for H2 is not reachable")); else { if (m_pFuLData->HXEnabled[1]) (*m_pHexapods)[1].MoveTCPAbsAsynch(PoseTCPH2); } if(!(*m_pHexapods)[3].IsPoseReachable(PoseTCPH4)) AfxMessageBox(_T("Target pose for H4 is not reachable")); else { if (m_pFuLData->HXEnabled[3]) (*m_pHexapods)[3].MoveTCPAbsAsynch(PoseTCPH4); } if(!(*m_pHexapods)[5].IsPoseReachable(PoseTCPH6)) AfxMessageBox(_T("Target pose for H6 is not reachable")); else { if (m_pFuLData->HXEnabled[5]) (*m_pHexapods)[5].MoveTCPAbsAsynch(PoseTCPH6); } cout << "\n\nPose 1 " << PoseTCPH1.Pose_mRad_To_mmGrad() << flush; cout << "\n\nPose 2 " << PoseTCPH2.Pose_mRad_To_mmGrad() << flush; cout << "\n\nPose 3 " << PoseTCPH3.Pose_mRad_To_mmGrad() << flush; cout << "\n\nPose 4 " << PoseTCPH4.Pose_mRad_To_mmGrad() << flush; cout << "\n\nPose 5 " << PoseTCPH5.Pose_mRad_To_mmGrad() << flush; cout << "\n\nPose 6 " << PoseTCPH6.Pose_mRad_To_mmGrad() << flush; } void CFuLTools::HexapodsCylinderArcZahlen(double Radius, double Length, double Arc) { if (AfxMessageBox("Move Hexapods to Cylinder Position ?",MB_YESNO) == IDNO) return; HexapodsReset(); // Construction Constants double SecGr = 500; double GrHei = SecGr/(2*tan(GradToRad(30)));

double GrHei3 = GrHei/3; // Point Position in X = Inside or Outside // PtOutsideL // PtInsideL // PtInsideR // PtOutsideR double PartEdgeHeight = 0.85+0.2+Radius/1000; double PartEdgeLeft = -2*GrHei3/1000.0; double PtOL = m_pFuLData->PartInL/1000.0; double PtIL = PtOL+GrHei/1000.0; double PtOR = (Length - m_pFuLData->PartInR)/1000.0; double PtIR = PtOR-GrHei/1000.0; // Cylinder Usefull Angle of Points double PhiGr = 2*asin(SecGr/(2*Radius)); double PhiGr2 = PhiGr/2; double PhiCupDistL = m_pFuLData->CupDistL/Radius; double PhiCupdistR = m_pFuLData->CupDistR/Radius; // Cylinder Angle of Points from Middle to Outside double Phi5 = 0; double Phi4 = PhiGr2; double Phi3 = Phi4+PhiCupDistL; double Phi2 = Phi3+PhiGr2; double Phi1 = Phi2+PhiGr2; double Phi0 = (Arc/2)/Radius; double Phi = Arc/Radius; double Phi6 = Phi4+PhiCupdistR; double Phi7 = Phi6+PhiGr2; double Phi8 = Phi7+PhiGr2; // Point Position in Y = Up or Down double P8Y = ( Radius*sin(Phi8))/1000.0; double P7Y = ( Radius*sin(Phi7))/1000.0; double P6Y = ( Radius*sin(Phi6))/1000.0; double P5Y = ( Radius*sin(Phi5))/1000.0; double P4Y = ( Radius*sin(Phi4))/1000.0; double P3Y = ( Radius*sin(Phi3))/1000.0; double P2Y = ( Radius*sin(Phi2))/1000.0; double P1Y = ( Radius*sin(Phi1))/1000.0; // Point Position in Z = Forward or Backward double P8Z = (-Radius*cos(Phi8))/1000.0; double P7Z = (-Radius*cos(Phi7))/1000.0; double P6Z = (-Radius*cos(Phi6))/1000.0; double P5Z = (-Radius*cos(Phi5))/1000.0; double P4Z = (-Radius*cos(Phi4))/1000.0; double P3Z = (-Radius*cos(Phi3))/1000.0; double P2Z = (-Radius*cos(Phi2))/1000.0; double P1Z = (-Radius*cos(Phi1))/1000.0; // Arrange Points to Homogene Matrix HMatrix<double> H1; H1.SetNewDim(iDEF_DIMHomogeneKoodinaten,3); H1[0][0] = PtOL; H1[1][0] = -P2Y; H1[2][0] = P2Z; H1[3][0] = 1.0; H1[0][1] = PtIL; H1[1][1] = -P3Y; H1[2][1] = P3Z;

H1[3][1] = 1.0; H1[0][2] = PtIL; H1[1][2] = -P1Y; H1[2][2] = P1Z; H1[3][2] = 1.0; HMatrix<double> H2; H2.SetNewDim(iDEF_DIMHomogeneKoodinaten,3); H2[0][0] = PtIR; H2[1][0] = -P6Y; H2[2][0] = P6Z; H2[3][0] = 1.0; H2[0][1] = PtIR; H2[1][1] = -P8Y; H2[2][1] = P8Z; H2[3][1] = 1.0; H2[0][2] = PtOR; H2[1][2] = -P7Y; H2[2][2] = P7Z; H2[3][2] = 1.0; HMatrix<double> H3; H3.SetNewDim(iDEF_DIMHomogeneKoodinaten,3); H3[0][0] = PtOL; H3[1][0] = P4Y; H3[2][0] = P4Z + m_pFuLData->H3G1/1000.0; H3[3][0] = 1.0; H3[0][1] = PtIL; H3[1][1] = P5Y ; H3[2][1] = P5Z + m_pFuLData->H3G2/1000.0; H3[3][1] = 1.0; H3[0][2] = PtOL; H3[1][2] = -P4Y; H3[2][2] = P4Z + m_pFuLData->H3G3/1000.0; H3[3][2] = 1.0; HMatrix<double> H4; H4.SetNewDim(iDEF_DIMHomogeneKoodinaten,3); H4[0][0] = PtIR; H4[1][0] = P4Y; H4[2][0] = P4Z; H4[3][0] = 1.0; H4[0][1] = PtIR; H4[1][1] = -P4Y; H4[2][1] = P4Z; H4[3][1] = 1.0; H4[0][2] = PtOR; H4[1][2] = P5Y; H4[2][2] = P5Z; H4[3][2] = 1.0; HMatrix<double> H5; H5.SetNewDim(iDEF_DIMHomogeneKoodinaten,3); H5[0][0] = PtOL; H5[1][0] = P2Y; H5[2][0] = P2Z; H5[3][0] = 1.0; H5[0][1] = PtIL; H5[1][1] = P1Y;

H5[2][1] = P1Z; H5[3][1] = 1.0; H5[0][2] = PtIL; H5[1][2] = P3Y; H5[2][2] = P3Z; H5[3][2] = 1.0; HMatrix<double> H6; H6.SetNewDim(iDEF_DIMHomogeneKoodinaten,3); H6[0][0] = PtIR; H6[1][0] = P8Y; H6[2][0] = P8Z; H6[3][0] = 1.0; H6[0][1] = PtIR; H6[1][1] = P6Y; H6[2][1] = P6Z; H6[3][1] = 1.0; H6[0][2] = PtOR; H6[1][2] = P7Y; H6[2][2] = P7Z; H6[3][2] = 1.0; //Pose Greifer to Part Frame HMatrix<double> HTMRH1; HTMRH1.HTMR(H1); HMatrix<double> HTMRH2; HTMRH2.HTMR(H2); HMatrix<double> HTMRH3; HTMRH3.HTMR(H3); HMatrix<double> HTMRH4; HTMRH4.HTMR(H4); HMatrix<double> HTMRH5; HTMRH5.HTMR(H5); HMatrix<double> HTMRH6; HTMRH6.HTMR(H6); //Pose Hexapod to World Frame HMatrix<double> HTBasisH1Welt; HTBasisH1Welt.RPY(m_pFuLData->m_HXWRTransf[0]); HMatrix<double> HTBasisH2Welt; HTBasisH2Welt.RPY(m_pFuLData->m_HXWRTransf[1]); HMatrix<double> HTBasisH3Welt; HTBasisH3Welt.RPY(m_pFuLData->m_HXWRTransf[2]); HMatrix<double> HTBasisH4Welt; HTBasisH4Welt.RPY(m_pFuLData->m_HXWRTransf[3]); HMatrix<double> HTBasisH5Welt; HTBasisH5Welt.RPY(m_pFuLData->m_HXWRTransf[4]); HMatrix<double> HTBasisH6Welt; HTBasisH6Welt.RPY(m_pFuLData->m_HXWRTransf[5]); //Pose Greifer to Hexapod TCP HMatrix<double> HTMTCPH1; HTMTCPH1.RPY(m_pFuLData->m_GRHXTransf[0]); HMatrix<double> HTMTCPH2; HTMTCPH2.RPY(m_pFuLData->m_GRHXTransf[1]); HMatrix<double> HTMTCPH3; HTMTCPH3.RPY(m_pFuLData->m_GRHXTransf[2]); HMatrix<double> HTMTCPH4; HTMTCPH4.RPY(m_pFuLData->m_GRHXTransf[3]);

HMatrix<double> HTMTCPH5; HTMTCPH5.RPY(m_pFuLData->m_GRHXTransf[4]); HMatrix<double> HTMTCPH6; HTMTCPH6.RPY(m_pFuLData->m_GRHXTransf[5]); //Definition Bauteil-KOS over Black-Basis-KOS Vektor<double> PoseBauteilBasisH3(iDEF_DIMPosevektor); PoseBauteilBasisH3[0] = PartEdgeLeft; PoseBauteilBasisH3[1] = 0; PoseBauteilBasisH3[2] = PartEdgeHeight; PoseBauteilBasisH3[3] = GradToRad(0); PoseBauteilBasisH3[4] = GradToRad(0); PoseBauteilBasisH3[5] = GradToRad(0); HMatrix<double> HTBauteilBasisH3; HTBauteilBasisH3.RPY(PoseBauteilBasisH3); HMatrix<double> HTBauteilWelt; HTBauteilWelt = HTBasisH3Welt * HTBauteilBasisH3; Vektor<double> PoseBauteilWelt; PoseBauteilWelt = HTBauteilWelt.IRPY(); cout<< "Transformation Part Frame to World Frame" << PoseBauteilWelt.Pose_mRad_To_mmGrad(); //Calculate Roboter Pose TCP zu Basis HMatrix<double> HTTCPH1; HTTCPH1 = HTBasisH1Welt.Inverse() * HTBauteilWelt * HTMRH1 * HTMTCPH1.Inverse(); Vektor<double> PoseTCPH1(6); PoseTCPH1 = HTTCPH1.IRPY(); HMatrix<double> HTTCPH2; HTTCPH2 = HTBasisH2Welt.Inverse() * HTBauteilWelt * HTMRH2 * HTMTCPH2.Inverse(); Vektor<double> PoseTCPH2(6); PoseTCPH2 = HTTCPH2.IRPY(); HMatrix<double> HTTCPH3; HTTCPH3 = HTBasisH3Welt.Inverse() * HTBauteilWelt * HTMRH3 * HTMTCPH3.Inverse(); Vektor<double> PoseTCPH3(6); PoseTCPH3 = HTTCPH3.IRPY(); HMatrix<double> HTTCPH4; HTTCPH4 = HTBasisH4Welt.Inverse() * HTBauteilWelt * HTMRH4 * HTMTCPH4.Inverse(); Vektor<double> PoseTCPH4(6); PoseTCPH4 = HTTCPH4.IRPY(); HMatrix<double> HTTCPH5; HTTCPH5 = HTBasisH5Welt.Inverse() * HTBauteilWelt * HTMRH5 * HTMTCPH5.Inverse(); Vektor<double> PoseTCPH5(6); PoseTCPH5 = HTTCPH5.IRPY(); HMatrix<double> HTTCPH6; HTTCPH6 = HTBasisH6Welt.Inverse() * HTBauteilWelt * HTMRH6 * HTMTCPH6.Inverse(); Vektor<double> PoseTCPH6(6); PoseTCPH6 = HTTCPH6.IRPY(); //Move Hexapods to Pose if(!(*m_pHexapods)[0].IsPoseReachable(PoseTCPH1)) AfxMessageBox(_T("Target pose for H1 is not reachable"));

else { if (m_pFuLData->HXEnabled[0]) (*m_pHexapods)[0].MoveTCPAbsAsynch(PoseTCPH1); } if(!(*m_pHexapods)[2].IsPoseReachable(PoseTCPH3)) AfxMessageBox(_T("Target pose for H3 is not reachable")); else { if (m_pFuLData->HXEnabled[2]) (*m_pHexapods)[2].MoveTCPAbsAsynch(PoseTCPH3); } if(!(*m_pHexapods)[4].IsPoseReachable(PoseTCPH5)) AfxMessageBox(_T("Target pose for H5 is not reachable")); else { if (m_pFuLData->HXEnabled[4]) (*m_pHexapods)[4].MoveTCPAbsAsynch(PoseTCPH5); } if(!(*m_pHexapods)[1].IsPoseReachable(PoseTCPH2)) AfxMessageBox(_T("Target pose for H2 is not reachable")); else { if (m_pFuLData->HXEnabled[1]) (*m_pHexapods)[1].MoveTCPAbsAsynch(PoseTCPH2); } if(!(*m_pHexapods)[3].IsPoseReachable(PoseTCPH4)) AfxMessageBox(_T("Target pose for H4 is not reachable")); else { if (m_pFuLData->HXEnabled[3]) (*m_pHexapods)[3].MoveTCPAbsAsynch(PoseTCPH4); } if(!(*m_pHexapods)[5].IsPoseReachable(PoseTCPH6)) AfxMessageBox(_T("Target pose for H6 is not reachable")); else { if (m_pFuLData->HXEnabled[5]) (*m_pHexapods)[5].MoveTCPAbsAsynch(PoseTCPH6); } cout << "\n\nPose 1 " << PoseTCPH1.Pose_mRad_To_mmGrad() << flush; cout << "\n\nPose 2 " << PoseTCPH2.Pose_mRad_To_mmGrad() << flush; cout << "\n\nPose 3 " << PoseTCPH3.Pose_mRad_To_mmGrad() << flush; cout << "\n\nPose 4 " << PoseTCPH4.Pose_mRad_To_mmGrad() << flush; cout << "\n\nPose 5 " << PoseTCPH5.Pose_mRad_To_mmGrad() << flush; cout << "\n\nPose 6 " << PoseTCPH6.Pose_mRad_To_mmGrad() << flush; } void CFuLTools::HexapodsCylinderArc123123(double Radius, double Length, double Arc) { if (AfxMessageBox("Move All Hexapods Cylinder Arc?",MB_YESNO) == IDNO) return; HexapodsReset(); // Construction Constants double ArcUp = 150;

double SecGr = 500; double ArcIn = 377.5; //150 double GrHei = SecGr/(2*tan(GradToRad(30))); double PartOut = -(22.5+GrHei/3); //150 // Cylinder Angle of Points double Phi0 = Arc/Radius; double Phi1 = ArcUp/Radius; double Phi3 = 2*asin(SecGr/(2*Radius)); double Phi2 = Phi3/2; double Phi4 = 2*asin(SecGr/(2*Radius)); double Phi5 = 0; // Arrange Points to Homogene Matrix HMatrix<double> H5; H5.SetNewDim(iDEF_DIMHomogeneKoodinaten,3); H5[0][0] = (ArcIn)/1000.0; // P1X H5[1][0] = ( Radius*sin(Phi0/2-Phi1))/1000.0; // P1Y H5[2][0] = (-Radius*cos(Phi0/2-Phi1))/1000.0; // P1Z H5[3][0] = 1.0; H5[0][1] = (ArcIn+GrHei)/1000.0; // P2X H5[1][1] = ( Radius*sin(Phi0/2-Phi1-Phi2))/1000.0; // P2Y H5[2][1] = (-Radius*cos(Phi0/2-Phi1-Phi2))/1000.0; // P2Z H5[3][1] = 1.0; H5[0][2] = (ArcIn)/1000.0; // P3X H5[1][2] = ( Radius*sin(Phi0/2-Phi1-Phi3))/1000.0; // P3Y H5[2][2] = (-Radius*cos(Phi0/2-Phi1-Phi3))/1000.0; // P3Z H5[3][2] = 1; HMatrix<double> H3; H3.SetNewDim(iDEF_DIMHomogeneKoodinaten,3); H3[0][0] = (ArcIn)/1000.0; // P4X H3[1][0] = ( Radius*sin(Phi4/2))/1000.0; // P4Y H3[2][0] = (-Radius*cos(Phi4/2))/1000.0; // P4Z H3[3][0] = 1.0; H3[0][1] = (ArcIn+GrHei)/1000.0; // P5X H3[1][1] = ( Radius*sin(Phi5/2))/1000.0; // P5Y H3[2][1] = (-Radius*cos(Phi5/2))/1000.0; // P5Z H3[3][1] = 1.0; H3[0][2] = (ArcIn)/1000.0; // P6X H3[1][2] = -H3[1][0]; // P6Y H3[2][2] = H3[2][0]; // P6Z H3[3][2] = 1; HMatrix<double> H1; H1.SetNewDim(iDEF_DIMHomogeneKoodinaten,3); H1[0][0] = H5[0][2]; // P7X H1[1][0] = -H5[1][2]; // P7Y H1[2][0] = H5[2][2]; // P7Z H1[3][0] = 1.0; H1[0][1] = H5[0][1]; // P8X H1[1][1] = -H5[1][1]; // P8Y H1[2][1] = H5[2][1]; // P8Z H1[3][1] = 1.0; H1[0][2] = H5[0][0]; // P9X H1[1][2] = -H5[1][0]; // P9Y H1[2][2] = H5[2][0]; // P9Z H1[3][2] = 1.0;

HMatrix<double> H2; H2.SetNewDim(iDEF_DIMHomogeneKoodinaten,3); H2[0][0] = -H1[0][0]+Length/1000.0; H2[1][0] = H1[1][0]; H2[2][0] = H1[2][0]; H2[3][0] = 1.0; H2[0][1] = -H1[0][1]+Length/1000.0; H2[1][1] = H1[1][1]; H2[2][1] = H1[2][1]; H2[3][1] = 1.0; H2[0][2] = -H1[0][2]+Length/1000.0; H2[1][2] = H1[1][2]; H2[2][2] = H1[2][2]; H2[3][2] = 1.0; HMatrix<double> H4; H4.SetNewDim(iDEF_DIMHomogeneKoodinaten,3); H4[0][0] = -H3[0][0]+Length/1000.0; H4[1][0] = H3[1][0]; H4[2][0] = H3[2][0]; H4[3][0] = 1.0; H4[0][1] = -H3[0][1]+Length/1000.0; H4[1][1] = H3[1][1]; H4[2][1] = H3[2][1]; H4[3][1] = 1.0; H4[0][2] = -H3[0][2]+Length/1000.0; H4[1][2] = H3[1][2]; H4[2][2] = H3[2][2]; H4[3][2] = 1.0; HMatrix<double> H6; H6.SetNewDim(iDEF_DIMHomogeneKoodinaten,3); H6[0][0] = -H5[0][0]+Length/1000.0; H6[1][0] = H5[1][0]; H6[2][0] = H5[2][0]; H6[3][0] = 1.0; H6[0][1] = -H5[0][1]+Length/1000.0; H6[1][1] = H5[1][1]; H6[2][1] = H5[2][1]; H6[3][1] = 1.0; H6[0][2] = -H5[0][2]+Length/1000.0; H6[1][2] = H5[1][2]; H6[2][2] = H5[2][2]; H6[3][2] = 1.0; //Pose Greifer to Part Frame HMatrix<double> HTMRH1; HTMRH1.HTMR(H1); HMatrix<double> HTMRH2; HTMRH2.HTMR(H2); HMatrix<double> HTMRH3; HTMRH3.HTMR(H3); HMatrix<double> HTMRH4; HTMRH4.HTMR(H4); HMatrix<double> HTMRH5; HTMRH5.HTMR(H5); HMatrix<double> HTMRH6; HTMRH6.HTMR(H6);

//Pose Hexapod to World Frame HMatrix<double> HTBasisH1Welt; HTBasisH1Welt.RPY(m_pFuLData->m_HXWRTransf[0]); HMatrix<double> HTBasisH2Welt; HTBasisH2Welt.RPY(m_pFuLData->m_HXWRTransf[1]); HMatrix<double> HTBasisH3Welt; HTBasisH3Welt.RPY(m_pFuLData->m_HXWRTransf[2]); HMatrix<double> HTBasisH4Welt; HTBasisH4Welt.RPY(m_pFuLData->m_HXWRTransf[3]); HMatrix<double> HTBasisH5Welt; HTBasisH5Welt.RPY(m_pFuLData->m_HXWRTransf[4]); HMatrix<double> HTBasisH6Welt; HTBasisH6Welt.RPY(m_pFuLData->m_HXWRTransf[5]); //Pose Greifer to Hexapod TCP HMatrix<double> HTMTCPH1; HTMTCPH1.RPY(m_pFuLData->m_GRHXTransf[0]); HMatrix<double> HTMTCPH2; HTMTCPH2.RPY(m_pFuLData->m_GRHXTransf[1]); HMatrix<double> HTMTCPH3; HTMTCPH3.RPY(m_pFuLData->m_GRHXTransf[2]); HMatrix<double> HTMTCPH4; HTMTCPH4.RPY(m_pFuLData->m_GRHXTransf[3]); HMatrix<double> HTMTCPH5; HTMTCPH5.RPY(m_pFuLData->m_GRHXTransf[4]); HMatrix<double> HTMTCPH6; HTMTCPH6.RPY(m_pFuLData->m_GRHXTransf[5]); //Definition Bauteil-KOS over Black-Basis-KOS Vektor<double> PoseBauteilBasisH3(6); PoseBauteilBasisH3[0] = PartOut/1000; PoseBauteilBasisH3[1] = 0; PoseBauteilBasisH3[2] = 0.85+0.2+Radius/1000; PoseBauteilBasisH3[3] = GradToRad(0); PoseBauteilBasisH3[4] = GradToRad(0); PoseBauteilBasisH3[5] = GradToRad(0); HMatrix<double> HTBauteilBasisH3; HTBauteilBasisH3.RPY(PoseBauteilBasisH3); HMatrix<double> HTBauteilWelt; HTBauteilWelt = HTBasisH3Welt * HTBauteilBasisH3; Vektor<double> PoseBauteilWelt; PoseBauteilWelt = HTBauteilWelt.IRPY(); //Calculate Roboter Pose TCP zu Basis HMatrix<double> HTTCPH1; HTTCPH1 = HTBasisH1Welt.Inverse() * HTBauteilWelt * HTMRH1 * HTMTCPH1.Inverse(); Vektor<double> PoseTCPH1(6); PoseTCPH1 = HTTCPH1.IRPY(); HMatrix<double> HTTCPH2; HTTCPH2 = HTBasisH2Welt.Inverse() * HTBauteilWelt * HTMRH2 * HTMTCPH2.Inverse(); Vektor<double> PoseTCPH2(6); PoseTCPH2 = HTTCPH2.IRPY(); HMatrix<double> HTTCPH3; HTTCPH3 = HTBasisH3Welt.Inverse() * HTBauteilWelt * HTMRH3 * HTMTCPH3.Inverse();

Vektor<double> PoseTCPH3(6); PoseTCPH3 = HTTCPH3.IRPY(); HMatrix<double> HTTCPH4; HTTCPH4 = HTBasisH4Welt.Inverse() * HTBauteilWelt * HTMRH4 * HTMTCPH4.Inverse(); Vektor<double> PoseTCPH4(6); PoseTCPH4 = HTTCPH4.IRPY(); HMatrix<double> HTTCPH5; HTTCPH5 = HTBasisH5Welt.Inverse() * HTBauteilWelt * HTMRH5 * HTMTCPH5.Inverse(); Vektor<double> PoseTCPH5(6); PoseTCPH5 = HTTCPH5.IRPY(); HMatrix<double> HTTCPH6; HTTCPH6 = HTBasisH6Welt.Inverse() * HTBauteilWelt * HTMRH6 * HTMTCPH6.Inverse(); Vektor<double> PoseTCPH6(6); PoseTCPH6 = HTTCPH6.IRPY(); //Move Hexapods to Pose if(!(*m_pHexapods)[0].IsPoseReachable(PoseTCPH1)) AfxMessageBox(_T("Target pose for H1 is not reachable")); else (*m_pHexapods)[0].MoveTCPAbsAsynch(PoseTCPH1); if(!(*m_pHexapods)[1].IsPoseReachable(PoseTCPH2)) AfxMessageBox(_T("Target pose for H2 is not reachable")); else (*m_pHexapods)[1].MoveTCPAbsAsynch(PoseTCPH2); if(!(*m_pHexapods)[2].IsPoseReachable(PoseTCPH3)) AfxMessageBox(_T("Target pose for H3 is not reachable")); else (*m_pHexapods)[2].MoveTCPAbsAsynch(PoseTCPH3); if(!(*m_pHexapods)[3].IsPoseReachable(PoseTCPH4)) AfxMessageBox(_T("Target pose for H4 is not reachable")); else (*m_pHexapods)[3].MoveTCPAbsAsynch(PoseTCPH4); if(!(*m_pHexapods)[4].IsPoseReachable(PoseTCPH5)) AfxMessageBox(_T("Target pose for H5 is not reachable")); else (*m_pHexapods)[4].MoveTCPAbsAsynch(PoseTCPH5); if(!(*m_pHexapods)[5].IsPoseReachable(PoseTCPH6)) AfxMessageBox(_T("Target pose for H6 is not reachable")); else (*m_pHexapods)[5].MoveTCPAbsAsynch(PoseTCPH6); } void CFuLTools::HexapodsCylinderSec(double Radius, double Length, double Arc) { if (AfxMessageBox("Move All Hexapods Cylinder Sec?",MB_YESNO) == IDNO) return; HexapodsReset(); // Bauteil //CTC Shell Radius = 1978 Length = 3340 Secant 2336.89 Arc= 2530 //IFAM Shell Radius = 1978 Length = 3550 Secant 2336.89 Arc= 2550 //(Kreissehne = Secant) Secant = 2*r*sin(alpha/2)

//(Kreisbogen = Arc) Arc = r*alpha //(Winkel = Angle) Alpha = Arc / r //(Arc->Secant) Secant = 2*Radius*sin(Arc/(2*Radius)); //(Secant->Arc) Arc = 2*Radius*asin(Secant/(2*Radius)); double r = Radius; double h = Length; double S = 2*Radius*sin(Arc/(2*Radius)); // Greifer double l = 500.000; // Anschläge H1 double H1G1x,H1G2x,H1G3x; double H1G1y,H1G2y,H1G3y; double H1G1z,H1G2z,H1G3z; H1G1x = -100; //H1G1x = -200; H1G1y = -S/2+100; H1G1z = -abs(sqrt(r*r-H1G1y*H1G1y)); // Zylinder Koordinaten double phiG1 = atan2(H1G1y,-H1G1z); double phiG3 = phiG1 + 2*asin(l/(2*r)); double phiG2 = (phiG1 + phiG3)/2; H1G3x = -100; //H1G3x = -200; H1G3y = r*sin(phiG3); H1G3z = -r*cos(phiG3); //Dreieckneigung double j = 2*r*sin((phiG3-phiG1)/4); double i = sqrt(j*j -l*l/4); double beta = RadToGrad(asin(i/(l*sqrt(3)/2))); H1G2x = H1G1x - sqrt(3*l*l/4 - i*i); H1G2y = r*sin(phiG2); H1G2z = -r*cos(phiG2); double mitte = (H1G1x + H1G2x)/2; HMatrix<double> H5Anschlaege; H5Anschlaege.SetNewDim(iDEF_DIMHomogeneKoodinaten,3); H5Anschlaege[0][0] = H1G1x/1000.0; // H1G1x H5Anschlaege[1][0] = H1G1y/1000.0; // H1G1y H5Anschlaege[2][0] = H1G1z/1000.0; // H1G1z H5Anschlaege[3][0] = 1.0; H5Anschlaege[0][1] = H1G2x/1000.0; // H1G2x H5Anschlaege[1][1] = H1G2y/1000.0; // H1G2y H5Anschlaege[2][1] = H1G2z/1000.0; // H1G2z H5Anschlaege[3][1] = 1.0; H5Anschlaege[0][2] = H1G3x/1000.0; // H1G3x H5Anschlaege[1][2] = H1G3y/1000.0; // H1G3y H5Anschlaege[2][2] = H1G3z/1000.0; // H1G3z H5Anschlaege[3][2] = 1; cout << "\n H1Anschläge = " << H5Anschlaege;

HMatrix<double> H3Anschlaege; H3Anschlaege.SetNewDim(iDEF_DIMHomogeneKoodinaten,3); H3Anschlaege[0][0] = (-100)/1000.0; // H3G1x H3Anschlaege[1][0] = (-l/2)/1000.0; // H3G1y H3Anschlaege[2][0] = (-abs(sqrt(r*r - l*l/4)))/1000.0; // H3G1z H3Anschlaege[3][0] = 1.0; H3Anschlaege[0][1] = (H5Anschlaege[0][1]); // H3G2x H3Anschlaege[1][1] = (0)/1000.0; // H3G2y H3Anschlaege[2][1] = (-r)/1000.0; // H3G2z H3Anschlaege[3][1] = 1.0; H3Anschlaege[0][2] = H3Anschlaege[0][0]; // H3G3x H3Anschlaege[1][2] = -H3Anschlaege[1][0]; // H3G3y H3Anschlaege[2][2] = H3Anschlaege[2][0]; // H3G3z H3Anschlaege[3][2] = 1; cout << "\n H3Anschläge = " << H3Anschlaege; HMatrix<double> H1Anschlaege; H1Anschlaege.SetNewDim(iDEF_DIMHomogeneKoodinaten,3); H1Anschlaege[0][0] = H5Anschlaege[0][2]; // H5G3xx H1Anschlaege[1][0] = -H5Anschlaege[1][2]; // H5G3y H1Anschlaege[2][0] = H5Anschlaege[2][2]; // H5G3z H1Anschlaege[3][0] = 1.0; H1Anschlaege[0][1] = H5Anschlaege[0][1]; // H5G2x H1Anschlaege[1][1] = -H5Anschlaege[1][1]; // H5G2y H1Anschlaege[2][1] = H5Anschlaege[2][1]; // H5G2z H1Anschlaege[3][1] = 1.0; H1Anschlaege[0][2] = H5Anschlaege[0][0]; // H5G3x H1Anschlaege[1][2] = -H5Anschlaege[1][0]; // H5G1y H1Anschlaege[2][2] = H5Anschlaege[2][0]; // H5G1z H1Anschlaege[3][2] = 1.0; cout << "\n H5Anschläge = " << H1Anschlaege; HMatrix<double> H2Anschlaege; H2Anschlaege.SetNewDim(iDEF_DIMHomogeneKoodinaten,3); H2Anschlaege[0][0] = -h/1000.0 -H1Anschlaege[0][0]; // H2G1x H2Anschlaege[1][0] = H1Anschlaege[1][0]; // H2G1y H2Anschlaege[2][0] = H1Anschlaege[2][0]; // H2G1z H2Anschlaege[3][0] = 1.0; H2Anschlaege[0][1] = -h/1000.0 -H1Anschlaege[0][1]; // H2G2x H2Anschlaege[1][1] = H1Anschlaege[1][1]; // H2G2y H2Anschlaege[2][1] = H1Anschlaege[2][1]; // H2G2z H2Anschlaege[3][1] = 1.0; H2Anschlaege[0][2] = -h/1000.0 -H1Anschlaege[0][2]; // H2G3x H2Anschlaege[1][2] = H1Anschlaege[1][2]; // H2G3y H2Anschlaege[2][2] = H1Anschlaege[2][2]; // H2G3z H2Anschlaege[3][2] = 1.0; cout << "\n H2Anschläge = " << H2Anschlaege; HMatrix<double> H4Anschlaege; H4Anschlaege.SetNewDim(iDEF_DIMHomogeneKoodinaten,3); H4Anschlaege[0][0] = -h/1000.0 -H3Anschlaege[0][0]; // H4G1x H4Anschlaege[1][0] = H3Anschlaege[1][0]; // H4G1y H4Anschlaege[2][0] = H3Anschlaege[2][0]; // H4G1z H4Anschlaege[3][0] = 1; H4Anschlaege[0][1] = -h/1000.0 -H3Anschlaege[0][1]; // H4G2x H4Anschlaege[1][1] = H3Anschlaege[1][1]; // H4G2y

H4Anschlaege[2][1] = H3Anschlaege[2][1]; // H4G2z H4Anschlaege[3][1] = 1; H4Anschlaege[0][2] = -h/1000.0 -H3Anschlaege[0][2]; // H4G3x H4Anschlaege[1][2] = H3Anschlaege[1][2]; // H4G3y H4Anschlaege[2][2] = H3Anschlaege[2][2]; // H4G3z H4Anschlaege[3][2] = 1; cout << "\n H4Anschläge = " << H4Anschlaege; HMatrix<double> H6Anschlaege; H6Anschlaege.SetNewDim(iDEF_DIMHomogeneKoodinaten,3); H6Anschlaege[0][0] = -h/1000.0 -H5Anschlaege[0][0]; // H6G1x H6Anschlaege[1][0] = H5Anschlaege[1][0]; // H6G1y H6Anschlaege[2][0] = H5Anschlaege[2][0]; // H6G1z H6Anschlaege[3][0] = 1; H6Anschlaege[0][1] = -h/1000.0 -H5Anschlaege[0][1]; // H6G2x H6Anschlaege[1][1] = H5Anschlaege[1][1]; // H6G2y H6Anschlaege[2][1] = H5Anschlaege[2][1]; // H6G2z H6Anschlaege[3][1] = 1; H6Anschlaege[0][2] = -h/1000.0 -H5Anschlaege[0][2]; // H6G3x H6Anschlaege[1][2] = H5Anschlaege[1][2]; // H6G3y H6Anschlaege[2][2] = H5Anschlaege[2][2]; // H6G3z H6Anschlaege[3][2] = 1; cout << "\n H6Anschläge = " << H6Anschlaege; //Pose Greifer zu Bauteil HMatrix<double> HTMRH1; HTMRH1.HTMR(H1Anschlaege); HMatrix<double> HTMRH2; HTMRH2.HTMR(H2Anschlaege); HMatrix<double> HTMRH3; HTMRH3.HTMR(H3Anschlaege); HMatrix<double> HTMRH4; HTMRH4.HTMR(H4Anschlaege); HMatrix<double> HTMRH5; HTMRH5.HTMR(H5Anschlaege); HMatrix<double> HTMRH6; HTMRH6.HTMR(H6Anschlaege); //Read Pose of the Robot Basis zu Welt HMatrix<double> HTBasisH1Welt; HTBasisH1Welt.RPY(m_pFuLData->m_HXWRTransf[0]); HMatrix<double> HTBasisH2Welt; HTBasisH2Welt.RPY(m_pFuLData->m_HXWRTransf[1]); HMatrix<double> HTBasisH3Welt; HTBasisH3Welt.RPY(m_pFuLData->m_HXWRTransf[2]); HMatrix<double> HTBasisH4Welt; HTBasisH4Welt.RPY(m_pFuLData->m_HXWRTransf[3]); HMatrix<double> HTBasisH5Welt; HTBasisH5Welt.RPY(m_pFuLData->m_HXWRTransf[4]); HMatrix<double> HTBasisH6Welt; HTBasisH6Welt.RPY(m_pFuLData->m_HXWRTransf[5]); //Read Pose Greifer zu TCP HMatrix<double> HTMTCPH1;

HTMTCPH1.RPY(m_pFuLData->m_GRHXTransf[0]); HMatrix<double> HTMTCPH2; HTMTCPH2.RPY(m_pFuLData->m_GRHXTransf[1]); HMatrix<double> HTMTCPH3; HTMTCPH3.RPY(m_pFuLData->m_GRHXTransf[2]); HMatrix<double> HTMTCPH4; HTMTCPH4.RPY(m_pFuLData->m_GRHXTransf[3]); HMatrix<double> HTMTCPH5; HTMTCPH5.RPY(m_pFuLData->m_GRHXTransf[4]); HMatrix<double> HTMTCPH6; HTMTCPH6.RPY(m_pFuLData->m_GRHXTransf[5]); //Definition Bauteil-KOS over Black-Basis-KOS Vektor<double> PoseBauteilBasisH3(6); PoseBauteilBasisH3[0] = mitte/1000; PoseBauteilBasisH3[1] = 0; PoseBauteilBasisH3[2] = r/1000+0.85+0.2; PoseBauteilBasisH3[3] = GradToRad(0); PoseBauteilBasisH3[4] = GradToRad(0); PoseBauteilBasisH3[5] = GradToRad(180); HMatrix<double> HTBauteilBasisH3; HTBauteilBasisH3.RPY(PoseBauteilBasisH3); HMatrix<double> HTBauteilWelt; HTBauteilWelt = HTBasisH3Welt * HTBauteilBasisH3; Vektor<double> PoseBauteilWelt; PoseBauteilWelt = HTBauteilWelt.IRPY(); //Calculate Roboter Pose TCP zu Basis HMatrix<double> HTTCPH1; HTTCPH1 = HTBasisH1Welt.Inverse() * HTBauteilWelt * HTMRH1 * HTMTCPH1.Inverse(); Vektor<double> PoseTCPH1(6); PoseTCPH1 = HTTCPH1.IRPY(); HMatrix<double> HTTCPH2; HTTCPH2 = HTBasisH2Welt.Inverse() * HTBauteilWelt * HTMRH2 * HTMTCPH2.Inverse(); Vektor<double> PoseTCPH2(6); PoseTCPH2 = HTTCPH2.IRPY(); HMatrix<double> HTTCPH3; HTTCPH3 = HTBasisH3Welt.Inverse() * HTBauteilWelt * HTMRH3 * HTMTCPH3.Inverse(); Vektor<double> PoseTCPH3(6); PoseTCPH3 = HTTCPH3.IRPY(); HMatrix<double> HTTCPH4; HTTCPH4 = HTBasisH4Welt.Inverse() * HTBauteilWelt * HTMRH4 * HTMTCPH4.Inverse(); Vektor<double> PoseTCPH4(6); PoseTCPH4 = HTTCPH4.IRPY(); HMatrix<double> HTTCPH5; HTTCPH5 = HTBasisH5Welt.Inverse() * HTBauteilWelt * HTMRH5 * HTMTCPH5.Inverse(); Vektor<double> PoseTCPH5(6); PoseTCPH5 = HTTCPH5.IRPY();

HMatrix<double> HTTCPH6; HTTCPH6 = HTBasisH6Welt.Inverse() * HTBauteilWelt * HTMRH6 * HTMTCPH6.Inverse(); Vektor<double> PoseTCPH6(6); PoseTCPH6 = HTTCPH6.IRPY(); //Move Hexapods to Pose (*m_pHexapods)[0].MoveTCPAbsAsynch(PoseTCPH1); (*m_pHexapods)[1].MoveTCPAbsAsynch(PoseTCPH2); (*m_pHexapods)[2].MoveTCPAbsAsynch(PoseTCPH3); (*m_pHexapods)[3].MoveTCPAbsAsynch(PoseTCPH4); (*m_pHexapods)[4].MoveTCPAbsAsynch(PoseTCPH5); (*m_pHexapods)[5].MoveTCPAbsAsynch(PoseTCPH6); } void CFuLTools::HexapodsCylinderTeach() { if (AfxMessageBox("Move Hexapods to adjusted Radius 3.0m Position ?",MB_YESNO) == IDNO) return; HexapodsReset(); // Define Start Posen Vektor<double> RoboterPose; RoboterPose.SetNewDim(iDEF_DIMPosevektor); // Roboter Gekrümmt RoboterPose[0] = -0.075233; RoboterPose[1] = 0.078803; RoboterPose[2] = 0.984501; RoboterPose[3] = GradToRad(-16.0244); RoboterPose[4] = GradToRad(1.40449); RoboterPose[5] = GradToRad(-0.238064); (*m_pHexapods)[0].MoveTCPAbsAsynch(RoboterPose); RoboterPose[0] = -0.056841; RoboterPose[1] = 0.080039; RoboterPose[2] = 0.983405; RoboterPose[3] = GradToRad(-16.1341); RoboterPose[4] = GradToRad(-1.49949); RoboterPose[5] = GradToRad(0.180195); (*m_pHexapods)[1].MoveTCPAbsAsynch(RoboterPose); RoboterPose[0] = -0.0760516; RoboterPose[1] = -0.000739001; RoboterPose[2] = 0.884844; RoboterPose[3] = GradToRad(0.0456598); RoboterPose[4] = GradToRad(1.44508); RoboterPose[5] = GradToRad(-0.375824); (*m_pHexapods)[2].MoveTCPAbsAsynch(RoboterPose); RoboterPose[0] = -0.0554907; RoboterPose[1] = -0.00406474; RoboterPose[2] = 0.883307; RoboterPose[3] = GradToRad(0.121853); RoboterPose[4] = GradToRad(-1.4134); RoboterPose[5] = GradToRad(-0.203942); (*m_pHexapods)[3].MoveTCPAbsAsynch(RoboterPose); RoboterPose[0] = -0.073289; RoboterPose[1] = -0.081413;

RoboterPose[2] = 0.987307; RoboterPose[3] = GradToRad(16.1092); RoboterPose[4] = GradToRad(1.42185); RoboterPose[5] = GradToRad(0.426166); (*m_pHexapods)[4].MoveTCPAbsAsynch(RoboterPose); RoboterPose[0] = -0.052642; RoboterPose[1] = -0.084223; RoboterPose[2] = 0.985291; RoboterPose[3] = GradToRad(16.0974); RoboterPose[4] = GradToRad(-1.34834); RoboterPose[5] = GradToRad(-0.692763); (*m_pHexapods)[5].MoveTCPAbsAsynch(RoboterPose); } void CFuLTools::HexapodsConnect() { int iLastPoint, iBaseNr; CString strBaserAdr, StrOffsetAdr, strIPAdresse; iLastPoint = m_pFuLData->m_pInitPrg->m_Hexapods.m_strIPAddress.ReverseFind('.'); strBaserAdr = m_pFuLData->m_pInitPrg->m_Hexapods.m_strIPAddress.Left(iLastPoint); StrOffsetAdr = m_pFuLData->m_pInitPrg->m_Hexapods.m_strIPAddress.Mid(iLastPoint+1); iBaseNr = iGetStr(StrOffsetAdr); for(int iNrHexapod = 0; iNrHexapod < m_pFuLData->m_pInitPrg->m_Hexapods.m_iNumberOfHexapods; iNrHexapod++) { strIPAdresse = strBaserAdr + CString(".") + Str(iBaseNr+iNrHexapod); if (!(*m_pHexapods)[iNrHexapod].IsConnected()) { (*m_pHexapods)[iNrHexapod].Connect(strIPAdresse); (*m_pHexapods)[iNrHexapod].SetSpeed(100); (*m_pHexapods)[iNrHexapod].Reset(); if(iNrHexapod == m_pFuLData->m_pInitPrg->m_Hexapods.m_iNumberOfHexapods-1) AfxMessageBox(_T("Hexapods Connected!")); } } } void CFuLTools::HexapodsReset() { for(int iNrHexapod = 0; iNrHexapod < m_pFuLData->m_pInitPrg->m_Hexapods.m_iNumberOfHexapods; iNrHexapod++) if((*m_pHexapods)[iNrHexapod].IsConnected()) (*m_pHexapods)[iNrHexapod].Reset(); } void CFuLTools::HexapodsCoopMove(Vektor <double> PoseMaster) { // Registers in Controllers int InitPoseMaster = 100; int EndPoseMaster = 99; int InitPoseSlave = 100; int RelMasterSlave = 99; int EndPoseSlave = 98; // Transformation of Master Robot in Hexapod Basis Vektor <double> PoseTCPMasterIst(iDEF_DIMPosevektor); (*m_pHexapods)[0].GetTCPPose(PoseTCPMasterIst);

//Homogene Matrix of Master Robot in Hexapod Basis HMatrix<double> HTTCPMasterIst; HTTCPMasterIst.RPY(PoseTCPMasterIst); // Write Init-Pose of Master in Register (*m_pHexapods)[0].WritePosReg(PoseTCPMasterIst, InitPoseMaster); // Write End-Pose of Master in Register (*m_pHexapods)[0].WritePosReg(PoseMaster, EndPoseMaster); // Check if End Pose of Master is Reachable if(!(*m_pHexapods)[0].IsPoseReachable(PoseMaster)) { AfxMessageBox(_T("Destination not reachable by Master Robot")); return; } // Homogene Matrix of Master to World Frame HMatrix<double> HTBasisMasterWelt; HTBasisMasterWelt.RPY(m_pFuLData->m_HXWRTransf[0]); // Write Values to Registers of Slaves for(int iNrHexapod = 1; iNrHexapod < m_pFuLData->m_pInitPrg->m_Hexapods.m_iNumberOfHexapods; iNrHexapod++) { // Homogene Matrix fo Slave to World Frame HMatrix<double> HTBasisSlaveWelt; HTBasisSlaveWelt.RPY(m_pFuLData->m_HXWRTransf[iNrHexapod]); // Write Init-Pose of Slave in Register Vektor <double> PoseTCPSlaveIst(iDEF_DIMPosevektor); (*m_pHexapods)[iNrHexapod].GetTCPPose(PoseTCPSlaveIst); (*m_pHexapods)[iNrHexapod].WritePosReg(PoseTCPSlaveIst, InitPoseSlave); // Homogene Matrix of Slave TCP to Master TCP HMatrix<double> HTTCPSlaveIst; HTTCPSlaveIst.RPY(PoseTCPSlaveIst); HMatrix<double> HTTCPSlaveMaster; HTTCPSlaveMaster = HTTCPMasterIst.Inverse() * HTBasisMasterWelt.Inverse() * HTBasisSlaveWelt * HTTCPSlaveIst; Vektor<double> PoseTCPSlaveMaster(iDEF_DIMPosevektor); // Transformation of Slave TCP to Master TCP PoseTCPSlaveMaster = HTTCPSlaveMaster.IRPY(); // Write Transformation of Slave TCP to Master TCP in Register (*m_pHexapods)[iNrHexapod].WritePosReg(PoseTCPSlaveMaster, RelMasterSlave); // Homogene Matrix End-Pose of Slave HMatrix<double> HTTCPSlaveSoll; HMatrix<double> HTTCPMasterSoll; HTTCPMasterSoll.RPY(PoseMaster); HTTCPSlaveSoll = HTBasisSlaveWelt.Inverse() * HTBasisMasterWelt * HTTCPMasterSoll * HTTCPSlaveMaster; Vektor<double> PoseTCPSlaveSoll(6); PoseTCPSlaveSoll = HTTCPSlaveSoll.IRPY(); // Check if End Pose of Slave is Reachable if(!(*m_pHexapods)[iNrHexapod].IsPoseReachable(PoseTCPSlaveSoll)) { CString strNrHexapod; int iVal = iNrHexapod+1; strNrHexapod.Format("%i",iVal); AfxMessageBox(_T("Target pose for H"+strNrHexapod+" is not reachable")); return; } // Write End-Pose of Slave in Register (*m_pHexapods)[iNrHexapod].WritePosReg(PoseTCPSlaveSoll, EndPoseSlave);

} // Run the Programs in Controllers for Coop Movement for(int iNrHexapod = m_pFuLData->m_pInitPrg->m_Hexapods.m_iNumberOfHexapods-1; iNrHexapod >= 0 ; iNrHexapod--) (*m_pHexapods)[iNrHexapod].CallProgram(_T("MCOOP")); } void CFuLTools::HexapodsCoopMoveRel(Vektor <double> PoseMasterRel) { // Transformation of Master Robot in Hexapod Basis Vektor<double> PoseMaster(iDEF_DIMPosevektor); (*m_pHexapods)[0].GetTCPPose(PoseMaster); // Homogene Matrix of Master Robot in Hexapod Basis HMatrix <double> HTMaster; HTMaster.RPY(PoseMaster); // Homogene Matrix of Relative Movement of Master Robot HMatrix <double> HTMasterRel; HTMasterRel.RPY(PoseMasterRel); // Homogene Matrix of End Pose in Hexapod Basis HMatrix <double> HTEndPose; HTEndPose = HTMasterRel * HTMaster; // Transformation of End Pose in Hexapod Basis PoseMasterRel = HTEndPose.IRPY(); HexapodsCoopMove(PoseMasterRel); } void CFuLTools::FTSensorsConnect() { int iLastPoint, iBaseNr; CString strBaserAdr, StrOffsetAdr, strIPAdresse; iLastPoint = m_pFuLData->m_pInitPrg->m_FTSensors.m_strIPAddress.ReverseFind('.'); strBaserAdr = m_pFuLData->m_pInitPrg->m_FTSensors.m_strIPAddress.Left(iLastPoint); StrOffsetAdr = m_pFuLData->m_pInitPrg->m_FTSensors.m_strIPAddress.Mid(iLastPoint+1); iBaseNr = iGetStr(StrOffsetAdr); for(int iNrFTSensor = 0; iNrFTSensor < m_pFuLData->m_pInitPrg->m_FTSensors.m_iNumberOfFTSensors; iNrFTSensor++) { bool Connected =(*m_pFTSensors)[iNrFTSensor].IsConnected(); if(!Connected) { strIPAdresse = strBaserAdr + CString(".") + Str(iBaseNr+iNrFTSensor); (*m_pFTSensors)[iNrFTSensor].Connect(strIPAdresse); if(iNrFTSensor == m_pFuLData->m_pInitPrg->m_FTSensors.m_iNumberOfFTSensors-1) AfxMessageBox(_T("FTSensors Connected!")); } } } void CFuLTools::FTSensorsBias() { for(int iNrFTSensor = 0; iNrFTSensor < m_pFuLData->m_pInitPrg->m_FTSensors.m_iNumberOfFTSensors; iNrFTSensor++) (*m_pFTSensors)[iNrFTSensor].Bias(); } void CFuLTools::FTSensorsMeasurementFile(CString PathFilename) {

//CString PathFilename; //PathFilename = "MeasureFT.dat"; CFileTools FileTools; ofstream OutputFile; if(!FileTools.OpenStreamDlg(OutputFile, PathFilename, TRUE)) return; Vektor <double> FTMeasure(9); for(int iNrFTSensor = 0; iNrFTSensor < m_pFuLData->m_pInitPrg->m_FTSensors.m_iNumberOfFTSensors; iNrFTSensor++) { (*m_pFTSensors)[iNrFTSensor].SingleMeasurement(FTMeasure); OutputFile << "\n\nHX"<<(iNrFTSensor+1) <<"\n"; OutputFile << (int) (FTMeasure[0]*10.0)/10.0 <<"\n"; OutputFile << (int) (FTMeasure[1]*10.0)/10.0 <<"\n"; OutputFile << (int) (FTMeasure[2]*10.0)/10.0 <<"\n"; OutputFile << (int) (FTMeasure[3]*10.0)/10.0 <<"\n"; OutputFile << (int) (FTMeasure[4]*10.0)/10.0 <<"\n"; OutputFile << (int) (FTMeasure[5]*10.0)/10.0 <<"\n"; } OutputFile << "\n\n\n\n" << "----------------------------------------------------------------------------------\n" << "Static Measurement of Forces and Torques (Fx,Fx,Fz,Tx,Ty,Tz)\n" << "1.-RED.\n" << "2.-BLUE.\n" << "3.-BLACK.\n" << "4.-GREEN.\n" << "5.-WHITE.\n" << "6.-PINK.\n" << "Date :" <<AktualTimeDate() << "\n" << "----------------------------------------------------------------------------------\n"; OutputFile.close(); } void CFuLTools::MeasureConfiguration() { for(int iNrHexapod = 0; iNrHexapod < m_pFuLData->m_pInitPrg->m_Hexapods.m_iNumberOfHexapods; iNrHexapod++) m_pFuLData->HXEnabled[iNrHexapod] = false; FTSensorsConnect(); m_pFuLData->HXEnabled[2] = true; //--- Start Measurement --- if (AfxMessageBox("Attention!: Move Robots To Def = 0.0?",MB_YESNO) == IDYES) { m_pFuLData->H3G2 = 0; HexapodsCylinderArc(m_pFuLData->Radius, m_pFuLData->Length, m_pFuLData->Arc); if (AfxMessageBox("Attention!: Save Measurement Def = 0.0?",MB_YESNO) == IDYES) { FTSensorsBias(); FTSensorsMeasurementFile("FTMesaure_0.0mm.dat"); } } if (AfxMessageBox("Attention!: Move Robots To Def = 0.3?",MB_YESNO) == IDYES) { m_pFuLData->H3G2 = 0.3;

HexapodsCylinderArc(m_pFuLData->Radius, m_pFuLData->Length, m_pFuLData->Arc); if (AfxMessageBox("Attention!: Save Measurement Def = 0.3?",MB_YESNO) == IDYES) FTSensorsMeasurementFile("FTMesaure_0.3mm.dat"); } if (AfxMessageBox("Attention!: Move Robots To Def = 1.3?",MB_YESNO) == IDYES) { m_pFuLData->H3G2 = 1.3; HexapodsCylinderArc(m_pFuLData->Radius, m_pFuLData->Length, m_pFuLData->Arc); if (AfxMessageBox("Attention!: Save Measurement Def = 1.3?",MB_YESNO) == IDYES) FTSensorsMeasurementFile("FTMesaure_1.3mm.dat"); } if (AfxMessageBox("Attention!: Move Robots To Def = 2.3?",MB_YESNO) == IDYES) { m_pFuLData->H3G2 = 2.3; HexapodsCylinderArc(m_pFuLData->Radius, m_pFuLData->Length, m_pFuLData->Arc); if (AfxMessageBox("Attention!: Save Measurement Def = 2.3?",MB_YESNO) == IDYES) FTSensorsMeasurementFile("FTMesaure_2.3mm.dat"); } if (AfxMessageBox("Attention!: Move Robots To Def = 3.5?",MB_YESNO) == IDYES) { m_pFuLData->H3G2 = 3.5; HexapodsCylinderArc(m_pFuLData->Radius, m_pFuLData->Length, m_pFuLData->Arc); if (AfxMessageBox("Attention!: Save Measurement Def = 3.5?",MB_YESNO) == IDYES) FTSensorsMeasurementFile("FTMesaure_3.5mm.dat"); } if (AfxMessageBox("Attention!: Move Robots To Def = 0.00?",MB_YESNO) == IDYES) { m_pFuLData->H3G2 = 0; HexapodsCylinderArc(m_pFuLData->Radius, m_pFuLData->Length, m_pFuLData->Arc); if (AfxMessageBox("Attention!: Save Measurement Def = 0.00?",MB_YESNO) == IDYES) FTSensorsMeasurementFile("FTMesaure_0.00mm.dat"); FTSensorsBias(); } if (AfxMessageBox("Attention!: Move Robots To Def = -0.3?",MB_YESNO) == IDYES) { m_pFuLData->H3G2 = -0.3; HexapodsCylinderArc(m_pFuLData->Radius, m_pFuLData->Length, m_pFuLData->Arc); if (AfxMessageBox("Attention!: Save Measurement Def = -0.3?",MB_YESNO) == IDYES) FTSensorsMeasurementFile("FTMesaure_-0.3mm.dat"); } if (AfxMessageBox("Attention!: Move Robots To Def = -1.3?",MB_YESNO) == IDYES) { m_pFuLData->H3G2 = -1.3; HexapodsCylinderArc(m_pFuLData->Radius, m_pFuLData->Length, m_pFuLData->Arc); if (AfxMessageBox("Attention!: Save Measurement Def = -1.3?",MB_YESNO) == IDYES)

FTSensorsMeasurementFile("FTMesaure_-1.3mm.dat"); } if (AfxMessageBox("Attention!: Move Robots To Def = -2.3?",MB_YESNO) == IDYES) { m_pFuLData->H3G2 = -2.3; HexapodsCylinderArc(m_pFuLData->Radius, m_pFuLData->Length, m_pFuLData->Arc); if (AfxMessageBox("Attention!: Save Measurement Def = -2.3?",MB_YESNO) == IDYES) FTSensorsMeasurementFile("FTMesaure_-2.3mm.dat"); } if (AfxMessageBox("Attention!: Move Robots To Def = -3.5?",MB_YESNO) == IDYES) { m_pFuLData->H3G2 = -3.5; HexapodsCylinderArc(m_pFuLData->Radius, m_pFuLData->Length, m_pFuLData->Arc); if (AfxMessageBox("Attention!: Save Measurement Def = -3.5?",MB_YESNO) == IDYES) FTSensorsMeasurementFile("FTMesaure_-3.5mm.dat"); } if (AfxMessageBox("Attention!: Move Robots To Def = 0.000?",MB_YESNO) == IDYES) { m_pFuLData->H3G2 = 0; HexapodsCylinderArc(m_pFuLData->Radius, m_pFuLData->Length, m_pFuLData->Arc); if (AfxMessageBox("Attention!: Save Measurement Def = 0.000?",MB_YESNO) == IDYES) FTSensorsMeasurementFile("FTMesaure_0.000mm.dat"); FTSensorsBias(); } //--- End Measurement --- m_pFuLData->HXEnabled[2] = false; }