From e92a37aa4fa582025c02f3a5d808a7b0cd54d42f Mon Sep 17 00:00:00 2001 From: Dario Sassi Date: Mon, 11 Aug 2025 18:48:08 +0200 Subject: [PATCH] EgtMachKernel : - nel calcolo assi di robot ora si gestiscono anche MCH_SCC_ADIR_NEAR e MCH_SCC_ADIR_FAR. --- MachineCalc.cpp | 158 ++++++++++++++++++++++++++++++++---------------- Operation.cpp | 19 ++++-- 2 files changed, 120 insertions(+), 57 deletions(-) diff --git a/MachineCalc.cpp b/MachineCalc.cpp index a598dd0..b0b215f 100644 --- a/MachineCalc.cpp +++ b/MachineCalc.cpp @@ -547,60 +547,113 @@ Machine::CalculateKinematicChain( void) return false ; } - // se nessun asse lineare deve essere un robot (in futuro va permesso un lineare di tavola) + // se nessun asse lineare deve essere un robot (in futuro andrà permesso un lineare di testa o di tavola) if ( m_nTabLinAxes == 0 && m_nHeadLinAxes == 0) { - // verifico ci siano 6 assi rotanti tutti di testa (in futuro va permesso un rotante di tavola) - if ( m_nTabRotAxes != 0 || m_nHeadRotAxes != 6) { + // se 6 assi rotanti tutti di testa + if ( m_nTabRotAxes == 0 && m_nHeadRotAxes == 6) { + // riordino gli assi rotanti + swap( m_vCalcRotAx[0], m_vCalcRotAx[5]) ; + swap( m_vCalcRotAx[1], m_vCalcRotAx[4]) ; + swap( m_vCalcRotAx[2], m_vCalcRotAx[3]) ; + // determino il riferimento canonico del robot + if ( ! m_frRobot.Set( m_vCalcRotAx[0].ptPos, m_vCalcRotAx[0].vtDir, m_vCalcRotAx[3].vtDir)) { + LOG_ERROR( GetEMkLogger(), "Robot with errors in Rotary Axes : impossible canonic frame") + return false ; + } + // porto tutti gli assi cinematici e i dati testa nel riferimento canonico + for ( int i = 0 ; i < m_nHeadRotAxes ; ++ i) { + m_vCalcRotAx[i].ptPos.ToLoc( m_frRobot) ; + m_vCalcRotAx[i].vtDir.ToLoc( m_frRobot) ; + } + m_ptCalcPos.ToLoc( m_frRobot) ; + m_vtCalcDir.ToLoc( m_frRobot) ; + m_vtCalcADir.ToLoc( m_frRobot) ; + // direzione assi rotanti deve essere Z Y Y X Y X + if ( ! m_vCalcRotAx[0].vtDir.IsZ() || + ! m_vCalcRotAx[1].vtDir.IsY() || + ! m_vCalcRotAx[2].vtDir.IsY() || + ! m_vCalcRotAx[3].vtDir.IsX() || + ! m_vCalcRotAx[4].vtDir.IsY() || + ! m_vCalcRotAx[5].vtDir.IsX()) { + LOG_ERROR( GetEMkLogger(), "Robot with errors in Rotary Axes : not ZYY-XYX") + return false ; + } + // verifico che gli ultimi 3 assi formino un polso sferico (ovvero passino per uno stesso punto) + if ( abs( m_vCalcRotAx[3].ptPos.y - m_vCalcRotAx[5].ptPos.y) > EPS_SMALL || + abs( m_vCalcRotAx[3].ptPos.z - m_vCalcRotAx[5].ptPos.z) > EPS_SMALL || + abs( m_vCalcRotAx[4].ptPos.z - m_vCalcRotAx[3].ptPos.z) > EPS_SMALL) { + LOG_ERROR( GetEMkLogger(), "Robot with errors in Rotary Axes : not spherical Wrist") + return false ; + } + // calcolo il centro del polso in coordinate globali (R5.x, R6.y, R5.z) + Point3d ptCenG( m_vCalcRotAx[4].ptPos.x, m_vCalcRotAx[5].ptPos.y, m_vCalcRotAx[4].ptPos.z) ; + // recupero il riferimento dell'uscita (da posizione, direzione utensile e direzione ausiliaria) + Frame3d frExit ; + if ( ! frExit.Set( m_ptCalcPos, m_vtCalcDir, m_vtCalcADir)) + return false ; + // calcolo il centro del polso in locale a questo riferimento + m_ptWristCen = GetToLoc( ptCenG, frExit) ; + // calcolo la direzione di riferimento del polso in locale a questo riferimento + m_vtWristRef = GetToLoc( m_vCalcRotAx[5].vtDir, frExit) ; + // dichiaro tipo robot + m_nCalcChainType = KIN_CHAIN_ROBOT ; + return true ; + } + // se altrimenti 6 assi rotanti tutti di tavola + else if ( m_nTabRotAxes == 6 && m_nHeadRotAxes == 0) { + // riordino gli assi rotanti + swap( m_vCalcRotAx[0], m_vCalcRotAx[5]) ; + swap( m_vCalcRotAx[1], m_vCalcRotAx[4]) ; + swap( m_vCalcRotAx[2], m_vCalcRotAx[3]) ; + // determino il riferimento canonico del robot + if ( ! m_frRobot.Set( m_vCalcRotAx[0].ptPos, m_vCalcRotAx[0].vtDir, m_vCalcRotAx[3].vtDir)) { + LOG_ERROR( GetEMkLogger(), "Robot with errors in Rotary Axes : impossible canonic frame") + return false ; + } + // porto tutti gli assi cinematici e i dati testa nel riferimento canonico + for ( int i = 0 ; i < m_nTabRotAxes ; ++ i) { + m_vCalcRotAx[i].ptPos.ToLoc( m_frRobot) ; + m_vCalcRotAx[i].vtDir.ToLoc( m_frRobot) ; + } + m_ptCalcPos.ToLoc( m_frRobot) ; + m_vtCalcDir.ToLoc( m_frRobot) ; + m_vtCalcADir.ToLoc( m_frRobot) ; + // direzione assi rotanti deve essere Z Y Y X Y X + if ( ! m_vCalcRotAx[0].vtDir.IsZ() || + ! m_vCalcRotAx[1].vtDir.IsY() || + ! m_vCalcRotAx[2].vtDir.IsY() || + ! m_vCalcRotAx[3].vtDir.IsX() || + ! m_vCalcRotAx[4].vtDir.IsY() || + ! m_vCalcRotAx[5].vtDir.IsX()) { + LOG_ERROR( GetEMkLogger(), "Robot with errors in Rotary Axes : not ZYY-XYX") + return false ; + } + // verifico che gli ultimi 3 assi formino un polso sferico (ovvero passino per uno stesso punto) + if ( abs( m_vCalcRotAx[3].ptPos.y - m_vCalcRotAx[5].ptPos.y) > EPS_SMALL || + abs( m_vCalcRotAx[3].ptPos.z - m_vCalcRotAx[5].ptPos.z) > EPS_SMALL || + abs( m_vCalcRotAx[4].ptPos.z - m_vCalcRotAx[3].ptPos.z) > EPS_SMALL) { + LOG_ERROR( GetEMkLogger(), "Robot with errors in Rotary Axes : not spherical Wrist") + return false ; + } + // calcolo il centro del polso in coordinate globali (R5.x, R6.y, R5.z) + Point3d ptCenG( m_vCalcRotAx[4].ptPos.x, m_vCalcRotAx[5].ptPos.y, m_vCalcRotAx[4].ptPos.z) ; + // recupero il riferimento dell'uscita (da posizione, direzione utensile e direzione ausiliaria) + Frame3d frExit ; + if ( ! frExit.Set( m_ptCalcPos, m_vtCalcDir, m_vtCalcADir)) + return false ; + // calcolo il centro del polso in locale a questo riferimento + m_ptWristCen = GetToLoc( ptCenG, frExit) ; + // calcolo la direzione di riferimento del polso in locale a questo riferimento + m_vtWristRef = GetToLoc( m_vCalcRotAx[5].vtDir, frExit) ; + // dichiaro tipo robot + m_nCalcChainType = KIN_CHAIN_ROBOT ; + return true ; + } + // altrimenti errore + else { LOG_ERROR( GetEMkLogger(), "Robot with errors in Rotary Axes : number or type") return false ; } - // riordino gli assi rotanti - swap( m_vCalcRotAx[0], m_vCalcRotAx[5]) ; - swap( m_vCalcRotAx[1], m_vCalcRotAx[4]) ; - swap( m_vCalcRotAx[2], m_vCalcRotAx[3]) ; - // determino il riferimento canonico del robot - if ( ! m_frRobot.Set( m_vCalcRotAx[0].ptPos, m_vCalcRotAx[0].vtDir, m_vCalcRotAx[3].vtDir)) { - LOG_ERROR( GetEMkLogger(), "Robot with errors in Rotary Axes : impossible canonic frame") - return false ; - } - // porto tutti gli assi cinematici e i dati testa nel riferimento canonico - for ( int i = 0 ; i < m_nHeadRotAxes ; ++ i) { - m_vCalcRotAx[i].ptPos.ToLoc( m_frRobot) ; - m_vCalcRotAx[i].vtDir.ToLoc( m_frRobot) ; - } - m_ptCalcPos.ToLoc( m_frRobot) ; - m_vtCalcDir.ToLoc( m_frRobot) ; - m_vtCalcADir.ToLoc( m_frRobot) ; - // direzione assi rotanti deve essere Z Y Y X Y X - if ( ! m_vCalcRotAx[0].vtDir.IsZ() || - ! m_vCalcRotAx[1].vtDir.IsY() || - ! m_vCalcRotAx[2].vtDir.IsY() || - ! m_vCalcRotAx[3].vtDir.IsX() || - ! m_vCalcRotAx[4].vtDir.IsY() || - ! m_vCalcRotAx[5].vtDir.IsX()) { - LOG_ERROR( GetEMkLogger(), "Robot with errors in Rotary Axes : not ZYY-XYX") - return false ; - } - // verifico che gli ultimi 3 assi formino un polso sferico (ovvero passino per uno stesso punto) - if ( abs( m_vCalcRotAx[3].ptPos.y - m_vCalcRotAx[5].ptPos.y) > EPS_SMALL || - abs( m_vCalcRotAx[3].ptPos.z - m_vCalcRotAx[5].ptPos.z) > EPS_SMALL || - abs( m_vCalcRotAx[4].ptPos.z - m_vCalcRotAx[3].ptPos.z) > EPS_SMALL) { - LOG_ERROR( GetEMkLogger(), "Robot with errors in Rotary Axes : not spherical Wrist") - return false ; - } - // calcolo il centro del polso in coordinate globali (R5.x, R6.y, R5.z) - Point3d ptCenG( m_vCalcRotAx[4].ptPos.x, m_vCalcRotAx[5].ptPos.y, m_vCalcRotAx[4].ptPos.z) ; - // recupero il riferimento dell'uscita (da posizione, direzione utensile e direzione ausiliaria) - Frame3d frExit ; - if ( ! frExit.Set( m_ptCalcPos, m_vtCalcDir, m_vtCalcADir)) - return false ; - // calcolo il centro del polso in locale a questo riferimento - m_ptWristCen = GetToLoc( ptCenG, frExit) ; - // calcolo la direzione di riferimento del polso in locale a questo riferimento - m_vtWristRef = GetToLoc( m_vCalcRotAx[5].vtDir, frExit) ; - // dichiaro tipo robot - m_nCalcChainType = KIN_CHAIN_ROBOT ; - return true ; } // verifiche sugli assi lineari : @@ -2153,7 +2206,10 @@ Machine::GetRobotAngles( const Point3d& ptP, const Vector3d& vtDirT, const Vecto Vector3d vtDirAL = GetToLoc( vtDirA, m_frRobot) ; // riferimento utensile Frame3d frTool ; - frTool.Set( ORIG, vtDirTL, vtDirAL) ; + if ( ! frTool.Set( ORIG, vtDirTL, vtDirAL)) { + LOG_ERROR( GetEMkLogger(), "Error : tool reference not defined (robot)") + return false ; + } // deduco la posizione richiesta del centro del polso Point3d ptCen = ptPL + m_ptWristCen.x * frTool.VersX() + m_ptWristCen.y * frTool.VersY() + diff --git a/Operation.cpp b/Operation.cpp index d09bec3..eb5ef88 100644 --- a/Operation.cpp +++ b/Operation.cpp @@ -2610,9 +2610,10 @@ Operation::CalculateClPathRobotAxesValues( int nClPathId, int nLinAxes, int nRot // cancello eventuali punti aggiunti EraseAddedPoints( nClPathId) ; // eventuale direzione Aux preferenziale + int nSolCh = GetSolCh() ; bool bUseRefAux = true ; - Vector3d vtRefAux = Y_AX ; - switch ( GetSolCh()) { + Vector3d vtRefAux = V_NULL ; + switch ( nSolCh) { case MCH_SCC_ADIR_XP : vtRefAux = X_AX ; break ; case MCH_SCC_ADIR_XM : vtRefAux = -X_AX ; break ; case MCH_SCC_ADIR_YP : vtRefAux = Y_AX ; break ; @@ -2654,10 +2655,16 @@ Operation::CalculateClPathRobotAxesValues( int nClPathId, int nLinAxes, int nRot Vector3d vtDir = pCamData->GetToolDir() ; Vector3d vtAux ; if ( ! bUseRefAux) { - Frame3d frTool ; - frTool.Set( ORIG, vtDir) ; - frTool.Rotate( frTool.Orig(), frTool.VersZ(), 0, 1) ; - vtAux = frTool.VersX() ; + if ( nSolCh == MCH_SCC_ADIR_NEAR || nSolCh == MCH_SCC_ADIR_FAR) { + vtAux = OrthoCompo( pCamData->GetAuxDir(), vtDir) ; + vtAux.Normalize() ; + } + else { + Frame3d frTool ; + frTool.Set( ORIG, vtDir) ; + frTool.Rotate( frTool.Orig(), frTool.VersZ(), 0, 1) ; + vtAux = frTool.VersX() ; + } } else { vtAux = OrthoCompo( vtRefAux, vtDir) ;