From 028a5b7bbaab65b2d1111aeb67560a6ea01e0f39 Mon Sep 17 00:00:00 2001 From: Dario Sassi Date: Tue, 13 Feb 2024 11:45:05 +0100 Subject: [PATCH] EgtMachKernel : - aggiunta gestione macchine di tipo Robot, le altre sono ora definite di tipo Center. --- MachMgr.h | 2 + MachMgrMachines.cpp | 20 + Machine.cpp | 3 + Machine.h | 9 +- MachineCalc.cpp | 279 ++++++++-- MachineStruConst.h | 6 + Milling.cpp | 9 +- Operation.cpp | 1290 ++++++++++++++++++++++++++++--------------- Operation.h | 11 +- Simulator.cpp | 6 +- 10 files changed, 1144 insertions(+), 491 deletions(-) diff --git a/MachMgr.h b/MachMgr.h index 6a9c7a4..212c9c1 100644 --- a/MachMgr.h +++ b/MachMgr.h @@ -440,6 +440,8 @@ class MachMgr : public IMachMgr bool GetAllCurrAxesHomePos( DBLVECTOR& vAxHomeVal) const ; bool GetCurrAxisHomePos( int nInd, double& dHome) const ; const Frame3d& GetCurrLinAxesFrame( void) const ; + bool GetCurrIsCenter( void) const ; + bool GetCurrIsRobot( void) const ; bool ApplyRotAxisBlock( void) ; void ClearRotAxisBlock( void) { m_vAxisBlock.clear() ; } diff --git a/MachMgrMachines.cpp b/MachMgrMachines.cpp index f9ac352..7c5f4cb 100644 --- a/MachMgrMachines.cpp +++ b/MachMgrMachines.cpp @@ -683,6 +683,26 @@ MachMgr::GetCurrLinAxesFrame( void) const return pMch->GetCurrLinAxesFrame() ; } +//---------------------------------------------------------------------------- +bool +MachMgr::GetCurrIsCenter( void) const +{ + Machine* pMch = GetCurrMachine() ; + if ( pMch == nullptr) + return false ; + return ( pMch->GetCurrKinematicChainType() == KIN_CHAIN_CENTER) ; +} + +//---------------------------------------------------------------------------- +bool +MachMgr::GetCurrIsRobot( void) const +{ + Machine* pMch = GetCurrMachine() ; + if ( pMch == nullptr) + return false ; + return ( pMch->GetCurrKinematicChainType() == KIN_CHAIN_ROBOT) ; +} + //---------------------------------------------------------------------------- bool MachMgr::GetCalcAngles( const Vector3d& vtDirT, const Vector3d& vtDirA, diff --git a/Machine.cpp b/Machine.cpp index 7e33bb0..db7b123 100644 --- a/Machine.cpp +++ b/Machine.cpp @@ -58,6 +58,7 @@ Machine::Machine( void) m_nHeadRotAxes = 0 ; m_nHeadSpecRotAxis = -1 ; m_frLinAx.Reset( false) ; + m_nCalcChainType = KIN_CHAIN_NONE ; m_nMachineLook = MCH_LOOK_NONE ; } @@ -141,6 +142,8 @@ Machine::Init( const string& sMachineName, const string& sMachineDir, MachMgr* p m_nMachineLook = ( bOk ? MCH_LOOK_ALL : MCH_LOOK_NONE) ; // metto tutti gli assi in posizione home bOk = bOk && ResetAllAxesPos() ; + // reset catena cinematica corrente + m_nCalcChainType = KIN_CHAIN_NONE ; return bOk ; } diff --git a/Machine.h b/Machine.h index 7159d24..b2ac0d4 100644 --- a/Machine.h +++ b/Machine.h @@ -53,7 +53,7 @@ class Machine int GetHeadId( const std::string& sHead) const { int nId = GetGroup( sHead) ; return ( IsHeadGroup( nId) ? nId : GDB_ID_NULL) ; } - int GetExitId( const std::string& sHead, int nExit) const + int GetExitId( const std::string& sHead, int nExit) const { int nHeadId = GetHeadId( sHead) ; int nId = ( m_pGeomDB != nullptr ? m_pGeomDB->GetFirstNameInGroup( nHeadId, MCH_EXIT + ToString( nExit)) : GDB_ID_NULL) ; return ( IsExitGroup( nId) ? nId : GDB_ID_NULL) ; } @@ -132,12 +132,16 @@ class Machine bool GetAllCurrAxesHomePos( DBLVECTOR& vAxHomeVal) const ; const Frame3d& GetCurrLinAxesFrame( void) const { return m_frLinAx ; } + int GetCurrKinematicChainType( void) const + { return m_nCalcChainType ; } bool GetAngles( const Vector3d& vtDirT, const Vector3d& vtDirA, int& nStat, double& dAngA1, double& dAngB1, double& dAngA2, double& dAngB2) const ; bool GetAngles( const Vector3d& vtDirT, const Vector3d& vtDirA, int& nStat, DBLVECTOR& vAng1, DBLVECTOR& vAng2) const ; bool GetPositions( const Point3d& ptP, const DBLVECTOR& vAng, int& nStat, double& dX, double& dY, double& dZ) const ; + bool GetRobotAngles( const Point3d& ptP, const Vector3d& vtDirT, const Vector3d& vtDirA, + DBLVECTOR& vAng1, DBLVECTOR& vAng2) const ; bool GetNoseFromPositions( double dX, double dY, double dZ, const DBLVECTOR& vAng, Point3d& ptNose) const ; bool GetTipFromPositions( double dX, double dY, double dZ, const DBLVECTOR& vAng, @@ -311,6 +315,9 @@ class Machine KINAXISVECTOR m_vCalcLinAx ; // vettore assi lineari attivi per calcoli KINAXISVECTOR m_vCalcRotAx ; // vettore assi rotanti attivi per calcoli Frame3d m_frLinAx ; // sistema di riferimento definito dagli assi lineari + int m_nCalcChainType ; // tipologia testa attiva (nulla, centro di lavoro o robot) + Point3d m_ptWristCen ; // centro del polso sferico nel riferimento testa/uscita di calcolo + Vector3d m_vtWristRef ; // direzione del polso sferico nel riferimento testa/uscita di calcolo mutable OutStroke m_OutstrokeInfo ; // informazioni su ultima extra corsa // stato di visualizzazione int m_nMachineLook ; // stato di visualizzazione della macchina diff --git a/MachineCalc.cpp b/MachineCalc.cpp index 9969cd7..c64028d 100644 --- a/MachineCalc.cpp +++ b/MachineCalc.cpp @@ -466,14 +466,13 @@ Machine::CalculateKinematicChain( void) m_nHeadSpecRotAxis = -1 ; m_vCalcLinAx.clear() ; m_vCalcRotAx.clear() ; + m_nCalcChainType = KIN_CHAIN_NONE ; // recupero gli assi di tavola if ( m_nCalcTabId == GDB_ID_NULL) return false ; int nTParId = m_pGeomDB->GetParentId( m_nCalcTabId) ; if ( nTParId == GDB_ID_NULL) return false ; - m_nTabLinAxes = 0 ; - m_nTabRotAxes = 0 ; while ( IsAxisGroup( nTParId)) { if ( ! AddKinematicAxis( false, nTParId)) return false ; @@ -485,14 +484,54 @@ Machine::CalculateKinematicChain( void) int nHParId = m_pGeomDB->GetParentId( m_nCalcHeadId) ; if ( nHParId == GDB_ID_NULL) return false ; - m_nHeadLinAxes = 0 ; - m_nHeadRotAxes = 0 ; while ( IsAxisGroup( nHParId)) { if ( ! AddKinematicAxis( true, nHParId)) return false ; nHParId = m_pGeomDB->GetParentId( nHParId) ; } + // se nessun asse lineare deve essere un robot (in futuro va permesso un lineare 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) { + 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]) ; + // direzione assi rotanti deve essere Z Y Y Z Y Z + if ( ! m_vCalcRotAx[0].vtDir.IsZ() || + ! m_vCalcRotAx[1].vtDir.IsY() || + ! m_vCalcRotAx[2].vtDir.IsY() || + ! m_vCalcRotAx[3].vtDir.IsZ() || + ! m_vCalcRotAx[4].vtDir.IsY() || + ! m_vCalcRotAx[5].vtDir.IsZ()) { + LOG_ERROR( GetEMkLogger(), "Robot with errors in Rotary Axes : not ZYY-ZYZ") + return false ; + } + // verifico che gli ultimi 3 assi formino un polso sferico (ovvero passino per uno stesso punto) + if ( ! AreSamePointXYApprox( m_vCalcRotAx[3].ptPos, m_vCalcRotAx[5].ptPos) || + abs( m_vCalcRotAx[4].ptPos.x - m_vCalcRotAx[3].ptPos.x) > 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 : // aggiusto gli indici di ordine sulla sua catena cinematica (1-based) for ( int i = 0 ; i < int( m_vCalcLinAx.size()) ; ++ i) { @@ -531,7 +570,6 @@ Machine::CalculateKinematicChain( void) } // verifiche sugli assi rotanti : - bool bOk = false ; // aggiusto gli indici di ordine sulla sua catena cinematica (1-based) for ( int i = 0 ; i < int( m_vCalcRotAx.size()) ; ++ i) { if ( m_vCalcRotAx[i].bHead) @@ -541,7 +579,7 @@ Machine::CalculateKinematicChain( void) } // se 0 o 1 va bene if ( m_vCalcRotAx.size() <= 1) - bOk = true ; + ; // se 2 va bene else if ( m_vCalcRotAx.size() == 2) { // se entrambi di testa devo invertirne l'ordine @@ -555,7 +593,6 @@ Machine::CalculateKinematicChain( void) m_vCalcRotAx[1].stroke.Max = min( m_vCalcRotAx[1].stroke.Max, pHead->GetRot2Stroke().Max) ; } } - bOk = true ; } // se 3 va bene ( uno dovrà poi avere valore assegnato) else if ( m_vCalcRotAx.size() == 3) { @@ -578,10 +615,13 @@ Machine::CalculateKinematicChain( void) m_vCalcRotAx[n2ndHeadRotAx].stroke.Max = min( m_vCalcRotAx[n2ndHeadRotAx].stroke.Max, pHead->GetRot2Stroke().Max) ; } } - bOk = true ; } - if ( ! bOk) + // se più di 3 + else { + // altrimenti non ancora gestito, quindi errore + LOG_ERROR( GetEMkLogger(), "Rotary Axes not manageable") return false ; + } // verifico esistenza eventuale asse rotante speciale di testa if ( m_nHeadRotAxes > 0 && m_nHeadLinAxes > 0) { // indice di posizione primo asse di testa @@ -617,11 +657,9 @@ Machine::CalculateKinematicChain( void) } } } + // dichiaro tipo centro di lavoro + m_nCalcChainType = KIN_CHAIN_CENTER ; return true ; - - // altrimenti non ancora gestito, quindi errore - LOG_ERROR( GetEMkLogger(), "Rotary Axes not manageable") - return false ; } //---------------------------------------------------------------------------- @@ -1230,26 +1268,46 @@ Machine::GetNoseFromPositions( double dX, double dY, double dZ, const DBLVECTOR& // la posizione deve essere espressa rispetto allo ZERO MACCHINA // è espressa nel riferimento di macchina (tiene conto delle sole traslazioni e rotazioni di testa) - // aggiorno posizione testa a riposo mediante ciclo inverso sugli assi rotanti di testa - ptNose = m_ptCalcPos ; - for ( int i = int( m_vCalcRotAx.size()) - 1 ; i >= 0 ; -- i) { - // se asse di testa non speciale - if ( m_vCalcRotAx[i].bHead && i != m_nHeadSpecRotAxis) - ptNose.Rotate( m_vCalcRotAx[i].ptPos, m_vCalcRotAx[i].vtDir, vAng[i]) ; + // verifico dimensione vettore angoli rispetto al numero di assi rotanti + if ( vAng.size() < m_vCalcRotAx.size()) + return false ; + + // se centro di lavoro + if ( m_nCalcChainType == KIN_CHAIN_CENTER) { + // aggiorno posizione testa a riposo mediante ciclo inverso sugli assi rotanti di testa + ptNose = m_ptCalcPos ; + for ( int i = int( m_vCalcRotAx.size()) - 1 ; i >= 0 ; -- i) { + // se asse di testa non speciale + if ( m_vCalcRotAx[i].bHead && i != m_nHeadSpecRotAxis) + ptNose.Rotate( m_vCalcRotAx[i].ptPos, m_vCalcRotAx[i].vtDir, vAng[i]) ; + } + // aggiorno posizione testa con assi lineari di testa + DBLVECTOR vMov( {dX, dY, dZ}) ; + for ( int i = 0 ; i < int( m_vCalcLinAx.size()) ; ++ i) { + if ( m_vCalcLinAx[i].bHead) + ptNose += m_vCalcLinAx[i].vtDir * vMov[i] ; + } + // eseguo rotazione eventuale asse rotante speciale di testa + if ( m_nHeadSpecRotAxis != -1) { + if ( m_nHeadSpecRotAxis < 0 || m_nHeadSpecRotAxis >= int( m_vCalcRotAx.size())) + return false ; + int i = m_nHeadSpecRotAxis ; + ptNose.Rotate( m_vCalcRotAx[i].ptPos, m_vCalcRotAx[i].vtDir, vAng[i]) ; + } } - // aggiorno posizione testa con assi lineari di testa - DBLVECTOR vMov( {dX, dY, dZ}) ; - for ( int i = 0 ; i < int( m_vCalcLinAx.size()) ; ++ i) { - if ( m_vCalcLinAx[i].bHead) - ptNose += m_vCalcLinAx[i].vtDir * vMov[i] ; - } - // eseguo rotazione eventuale asse rotante speciale di testa - if ( m_nHeadSpecRotAxis != -1) { - if ( m_nHeadSpecRotAxis < 0 || m_nHeadSpecRotAxis >= int( m_vCalcRotAx.size())) - return false ; - int i = m_nHeadSpecRotAxis ; - ptNose.Rotate( m_vCalcRotAx[i].ptPos, m_vCalcRotAx[i].vtDir, vAng[i]) ; + // se robot + else if ( m_nCalcChainType == KIN_CHAIN_ROBOT) { + // aggiorno posizione testa a riposo mediante ciclo inverso sugli assi rotanti di testa + ptNose = m_ptCalcPos ; + for ( int i = int( m_vCalcRotAx.size()) - 1 ; i >= 0 ; -- i) { + // se asse di testa + if ( m_vCalcRotAx[i].bHead) + ptNose.Rotate( m_vCalcRotAx[i].ptPos, m_vCalcRotAx[i].vtDir, vAng[i]) ; + } } + // altrimenti errore + else + return false ; return true ; } @@ -1262,26 +1320,46 @@ Machine::GetTipFromPositions( double dX, double dY, double dZ, const DBLVECTOR& // la posizione deve essere espressa rispetto allo ZERO MACCHINA // è espressa nel riferimento di macchina (tiene conto delle sole rotazioni di testa) - // aggiorno posizione tip utensile a riposo mediante ciclo inverso sugli assi rotanti di testa - ptTip = m_ptCalcPos - m_vtCalcDir * m_dCalcTLen ; - for ( int i = int( m_vCalcRotAx.size()) - 1 ; i >= 0 ; -- i) { - // se asse di testa non speciale - if ( m_vCalcRotAx[i].bHead && i != m_nHeadSpecRotAxis) - ptTip.Rotate( m_vCalcRotAx[i].ptPos, m_vCalcRotAx[i].vtDir, vAng[i]) ; + // verifico dimensione vettore angoli rispetto al numero di assi rotanti + if ( vAng.size() < m_vCalcRotAx.size()) + return false ; + + // se centro di lavoro + if ( m_nCalcChainType == KIN_CHAIN_CENTER) { + // aggiorno posizione tip utensile a riposo mediante ciclo inverso sugli assi rotanti di testa + ptTip = m_ptCalcPos - m_vtCalcDir * m_dCalcTLen ; + for ( int i = int( m_vCalcRotAx.size()) - 1 ; i >= 0 ; -- i) { + // se asse di testa non speciale + if ( m_vCalcRotAx[i].bHead && i != m_nHeadSpecRotAxis) + ptTip.Rotate( m_vCalcRotAx[i].ptPos, m_vCalcRotAx[i].vtDir, vAng[i]) ; + } + // aggiorno posizione tip utensile con assi lineari di testa + DBLVECTOR vMov( {dX, dY, dZ}) ; + for ( int i = 0 ; i < int( m_vCalcLinAx.size()) ; ++ i) { + if ( m_vCalcLinAx[i].bHead) + ptTip += m_vCalcLinAx[i].vtDir * vMov[i] ; + } + // eseguo rotazione eventuale asse rotante speciale di testa + if ( m_nHeadSpecRotAxis != -1) { + if ( m_nHeadSpecRotAxis < 0 || m_nHeadSpecRotAxis >= int( m_vCalcRotAx.size())) + return false ; + int i = m_nHeadSpecRotAxis ; + ptTip.Rotate( m_vCalcRotAx[i].ptPos, m_vCalcRotAx[i].vtDir, vAng[i]) ; + } } - // aggiorno posizione tip utensile con assi lineari di testa - DBLVECTOR vMov( {dX, dY, dZ}) ; - for ( int i = 0 ; i < int( m_vCalcLinAx.size()) ; ++ i) { - if ( m_vCalcLinAx[i].bHead) - ptTip += m_vCalcLinAx[i].vtDir * vMov[i] ; - } - // eseguo rotazione eventuale asse rotante speciale di testa - if ( m_nHeadSpecRotAxis != -1) { - if ( m_nHeadSpecRotAxis < 0 || m_nHeadSpecRotAxis >= int( m_vCalcRotAx.size())) - return false ; - int i = m_nHeadSpecRotAxis ; - ptTip.Rotate( m_vCalcRotAx[i].ptPos, m_vCalcRotAx[i].vtDir, vAng[i]) ; + // se robot + else if ( m_nCalcChainType == KIN_CHAIN_ROBOT) { + // aggiorno posizione tip utensile a riposo mediante ciclo inverso sugli assi rotanti di testa + ptTip = m_ptCalcPos - m_vtCalcDir * m_dCalcTLen ; + for ( int i = int( m_vCalcRotAx.size()) - 1 ; i >= 0 ; -- i) { + // se asse di testa + if ( m_vCalcRotAx[i].bHead) + ptTip.Rotate( m_vCalcRotAx[i].ptPos, m_vCalcRotAx[i].vtDir, vAng[i]) ; + } } + // altrimenti errore + else + return false ; // Se richiesto ingombro totale o punto sotto del tip utensile if ( bOverall || bBottom) { @@ -1928,3 +2006,106 @@ Machine::GetAllCurrAxesHomePos( DBLVECTOR& vAxHomeVal) const } return bOk ; } + +//---------------------------------------------------------------------------- +bool +Machine::GetRobotAngles( const Point3d& ptP, const Vector3d& vtDirT, const Vector3d& vtDirA, + DBLVECTOR& vAng1, DBLVECTOR& vAng2) const +{ + // pulisco il risultato + vAng1.clear() ; + vAng2.clear() ; + // riferimento utensile + Frame3d frTool ; + frTool.Set( ORIG, vtDirT, vtDirA) ; + // deduco la posizione richiesta del centro del polso + Point3d ptCen = ptP + m_ptWristCen.x * frTool.VersX() + + m_ptWristCen.y * frTool.VersY() + + ( m_dCalcTLen + m_ptWristCen.z) * frTool.VersZ() ; + // deduco la direzione richiesta del centro del polso + Vector3d vtCen = m_vtWristRef.x * frTool.VersX() + + m_vtWristRef.y * frTool.VersY() + + m_vtWristRef.z * frTool.VersZ() ; + // calcolo il primo asse rotante + Vector3d vtArm = ptCen - m_vCalcRotAx[0].ptPos ; + double dAng1 ; bool bDet1 ; + if ( ! X_AX.GetRotation( vtArm, m_vCalcRotAx[0].vtDir, EPS_SMALL, dAng1, bDet1) || ! bDet1) { + LOG_ERROR( GetEMkLogger(), "Error : direction unreachable (robot)") + return false ; + } + vAng1.push_back( dAng1) ; + vAng2.push_back( dAng1) ; + // calcolo secondo e terzo asse rotante + Point3d ptR2 = m_vCalcRotAx[1].ptPos ; + ptR2.Rotate( m_vCalcRotAx[0].ptPos, m_vCalcRotAx[0].vtDir, dAng1) ; + Vector3d vtR2 = m_vCalcRotAx[1].vtDir ; + vtR2.Rotate( m_vCalcRotAx[0].vtDir, dAng1) ; + Vector3d vtR2Cen = ptCen - ( ptR2 + ( ptCen - ptR2) * vtR2 * vtR2) ; + double dDistR2Cen = vtR2Cen.Len() ; + double dDistR2R3 = m_vCalcRotAx[2].ptPos.z - m_vCalcRotAx[1].ptPos.z ; + Vector3d vtR3R5 = m_vCalcRotAx[4].ptPos - m_vCalcRotAx[2].ptPos ; vtR3R5.y = 0 ; + double dDistR3R5 = vtR3R5.Len() ; + double dAngR3R5 = atan2( -vtR3R5.x, vtR3R5.z) * RADTODEG ; + double dCosB = ( dDistR2R3 * dDistR2R3 + dDistR2Cen * dDistR2Cen - dDistR3R5 * dDistR3R5) / ( 2 * dDistR2R3 * dDistR2Cen) ; + double dCosC = ( dDistR3R5 * dDistR3R5 + dDistR2Cen * dDistR2Cen - dDistR2R3 * dDistR2R3) / ( 2 * dDistR3R5 * dDistR2Cen) ; + if ( abs( dCosB) > 1 || abs( dCosC) > 1) { + LOG_ERROR( GetEMkLogger(), "Error : position unreachable (robot)") + return false ; + } + double dAngB = acos( dCosB) * RADTODEG ; + double dAng2 ; bool bDet2 ; + if ( ! Z_AX.GetRotation( vtR2Cen, vtR2, EPS_SMALL, dAng2, bDet2) || ! bDet2) { + LOG_ERROR( GetEMkLogger(), "Error : R2 not calculable (robot)") + return false ; + } + dAng2 -= dAngB ; + vAng1.push_back( dAng2) ; + vAng2.push_back( dAng2) ; + double dAngC = acos( dCosC) * RADTODEG ; + double dAng3 = dAngB + dAngC + dAngR3R5 ; + vAng1.push_back( dAng3) ; + vAng2.push_back( dAng3) ; + // calcolo i primi due assi rotanti del polso + Frame3d frWrist( m_vCalcRotAx[3].ptPos) ; + frWrist.Rotate( m_vCalcRotAx[2].ptPos, m_vCalcRotAx[2].vtDir, dAng3) ; + frWrist.Rotate( m_vCalcRotAx[1].ptPos, m_vCalcRotAx[1].vtDir, dAng2) ; + frWrist.Rotate( m_vCalcRotAx[0].ptPos, m_vCalcRotAx[0].vtDir, dAng1) ; + Vector3d vtCenL = GetToLoc( vtCen, frWrist) ; + double dAng4, dAng5 ; + vtCenL.ToSpherical( nullptr, &dAng5, &dAng4) ; + if ( dAng4 > ANG_STRAIGHT) + dAng4 -= ANG_FULL ; + if ( dAng4 > ANG_RIGHT) { + dAng4 -= ANG_STRAIGHT ; + dAng5 = -dAng5 ; + } + else if ( dAng4 < -ANG_RIGHT) { + dAng4 += ANG_STRAIGHT ; + dAng5 = -dAng5 ; + } + vAng1.push_back( dAng4) ; + vAng1.push_back( dAng5) ; + vAng2.push_back( dAng4 + ( dAng4 > EPS_ANG_ZERO ? -ANG_STRAIGHT : ANG_STRAIGHT)) ; + vAng2.push_back( -dAng5) ; + // calcolo il terzo asse rotante del polso + Vector3d vtR6 = m_vCalcRotAx[5].vtDir ; + for ( int i = 4; i >= 0 ; --i) + vtR6.Rotate( m_vCalcRotAx[i].vtDir, vAng1[i]) ; + Vector3d vtTool = m_vtCalcDir ; + for ( int i = 4; i >= 0 ; --i) + vtTool.Rotate( m_vCalcRotAx[i].vtDir, vAng1[i]) ; + double dAng6 ; bool bDet6 ; + if ( ! vtTool.GetRotation( vtDirT, vtR6, EPS_SMALL, dAng6, bDet6) || ! bDet6) { + Vector3d vtAux = m_vtCalcADir ; + for ( int i = 4; i >= 0 ; --i) + vtAux.Rotate( m_vCalcRotAx[i].vtDir, vAng1[i]) ; + if ( ! vtAux.GetRotation( vtDirA, vtR6, EPS_SMALL, dAng6, bDet6) || ! bDet6) { + LOG_ERROR( GetEMkLogger(), "Error : R6 not calculable (robot)") + return false ; + } + } + vAng1.push_back( dAng6) ; + vAng2.push_back( dAng6 + ( dAng6 > EPS_ANG_ZERO ? -ANG_STRAIGHT : ANG_STRAIGHT)) ; + + return true ; +} diff --git a/MachineStruConst.h b/MachineStruConst.h index f35562d..881ac25 100644 --- a/MachineStruConst.h +++ b/MachineStruConst.h @@ -94,6 +94,12 @@ enum MchSelType { MCH_SLT_FIXEDEXITS = 0, MCH_SLT_ONEEXIT = 1, MCH_SLT_MULTIEXITS = 2} ; +//---------------------------------------------------------------------------- +// Tipo della catena cinematica +enum KinChainType { KIN_CHAIN_NONE = 0, + KIN_CHAIN_CENTER = 1, + KIN_CHAIN_ROBOT = 2} ; + //---------------------------------------------------------------------------- // Identificativo iniziale riferimenti di tavola const std::string MCH_TREF = "R" ; diff --git a/Milling.cpp b/Milling.cpp index 46eefa7..1c07b62 100644 --- a/Milling.cpp +++ b/Milling.cpp @@ -1792,9 +1792,12 @@ Milling::ProcessPath( int nPathId, int nPvId, int nClId) bool bSplitArcs = ( nSplitArcs == SPLAR_ALWAYS || ( nSplitArcs == SPLAR_NO_XY_PLANE && ! vtTool.IsZplus()) || ( nSplitArcs == SPLAR_GEN_PLANE && vtTool.IsGeneric())) ; - if ( bSplitArcs && ! ApproxWithLines( pCompo)) { - m_pMchMgr->SetLastError( 2313, "Error in Milling : Linear Approx not computable") ; - return false ; + if ( bSplitArcs) { + double dMaxLen = ( m_pMchMgr->GetCurrIsRobot() ? 5 : 0) ; + if ( ! ApproxWithLines( pCompo, dMaxLen)) { + m_pMchMgr->SetLastError( 2313, "Error in Milling : Linear Approx not computable") ; + return false ; + } } // verifiche sull'ampiezza dell'angolo al centro degli eventuali archi diff --git a/Operation.cpp b/Operation.cpp index 9096b5a..8454740 100644 --- a/Operation.cpp +++ b/Operation.cpp @@ -42,6 +42,7 @@ using namespace std ; //---------------------------------------------------------------------------- const double MAX_DIST_RAW = 200.0 ; +const double MAX_ROBOT_G0_LEN = 300.0 ; //---------------------------------------------------------------------------- bool @@ -230,7 +231,7 @@ Operation::GetElevation( int nPhase, const Point3d& ptP, if ( IntersLineSurfTm( ptPL, vtDirL, RAY_LEN, *pStm, vInfo)) { for ( int i = 0 ; i < int( vInfo.size()) ; ++ i) { const IntLinStmInfo& Info = vInfo[i] ; - // se tratto di intersezione coincidente, considero la posizione più lontana, ovvero 2 + // se tratto di intersezione coincidente, considero la posizione più lontana, ovvero 2 if ( Info.nILTT == ILTT_SEGM || Info.nILTT == ILTT_SEGM_ON_EDGE) { // se prosegue un tratto precedente, non devo controllare sia minimo if ( abs( dElev - Info.dU) < EPS_SMALL) { @@ -249,7 +250,7 @@ Operation::GetElevation( int nPhase, const Point3d& ptP, nTria = Info.nT ; } } - // se altrimenti è la prima ed entra + // se altrimenti è la prima ed entra else if ( i == 0 && Info.dCosDN < - COS_ORTO_ANG_ZERO) { // se una sola o distante salto tutto if ( vInfo.size() == 1 || Info.dU > ( dElev > INFINITO - 1 ? 0 : dElev) + MAX_DIST_RAW) @@ -260,7 +261,7 @@ Operation::GetElevation( int nPhase, const Point3d& ptP, } } } - // se c'è triangolo di intersezione, ne recupero la normale e salvo Id del grezzo + // se c'è triangolo di intersezione, ne recupero la normale e salvo Id del grezzo if ( nTria != SVT_NULL) { Triangle3d Tria ; if ( pStm->GetTriangle( nTria, Tria)) @@ -326,7 +327,7 @@ Operation::GetElevation( int nPhase, const Point3d& ptP1, const Point3d& ptP2, if ( IntersLineSurfTm( ptPL, vtDirL, RAY_LEN, *pStm, vInfo)) { for ( int i = 0 ; i < int( vInfo.size()) ; ++ i) { const IntLinStmInfo& Info = vInfo[i] ; - // se tratto di intersezione coincidente, considero la posizione più lontana, ovvero 2 + // se tratto di intersezione coincidente, considero la posizione più lontana, ovvero 2 if ( Info.nILTT == ILTT_SEGM || Info.nILTT == ILTT_SEGM_ON_EDGE) { // se prosegue un tratto precedente, non devo controllare sia minimo if ( abs( dElevP - Info.dU) < EPS_SMALL) @@ -338,7 +339,7 @@ Operation::GetElevation( int nPhase, const Point3d& ptP1, const Point3d& ptP2, else if ( Info.dCosDN > - COS_ORTO_ANG_ZERO) { dElevP = min( dElevP, Info.dU) ; } - // se altrimenti è la prima, distante ed entra, salto tutto + // se altrimenti è la prima, distante ed entra, salto tutto else if ( i == 0 && Info.dU > MAX_DIST_RAW && Info.dCosDN < - COS_ORTO_ANG_ZERO) break ; } @@ -422,14 +423,14 @@ Operation::GetElevation( int nPhase, const Point3d& ptP, const Vector3d& vtTool, pCAvTlStm->SetStdTool( dLen, dRad, 0) ; pCAvTlStm->SetSurfTm( *pStm) ; double dDist = 0 ; - // non è ammesso un angolo oltre 90deg tra direzione utensile e vettore movimento + // non è ammesso un angolo oltre 90deg tra direzione utensile e vettore movimento if ( vtToolL * vtDirL < 0) { - // il tip utensile è qui il naso mandrino per Collision Avoid + // il tip utensile è qui il naso mandrino per Collision Avoid if ( ! pCAvTlStm->TestPosition( ptPL, -vtToolL, vtDirL, dDist)) return false ; } else { - // per Collision Avoid il punto di riferimento non è il tip dell'utensile ma il naso mandrino + // per Collision Avoid il punto di riferimento non è il tip dell'utensile ma il naso mandrino if ( ! pCAvTlStm->TestPosition( ptPL + dLen * vtToolL, vtToolL, vtDirL, dDist)) return false ; } @@ -508,7 +509,7 @@ bool Operation::GetAhPointUnderRaw( const Point3d& ptP, const Vector3d& vtTool, double dToolRad, double dToolRadForElev, double dToolLen, bool bIsSaw, double dSafeZ, const Vector3d& vtDir, double& dElev) const { - // punto a metà lunghezza utensile + // punto a metà lunghezza utensile Point3d ptQ = ptP + vtTool * dToolLen / 2 ; // box di ingombro dell'utensile BBox3d b3Tool ; @@ -539,18 +540,18 @@ Operation::GetAhPointUnderRaw( const Point3d& ptP, const Vector3d& vtTool, doubl double dRawCentZ = ( b3Raw.GetMin().z + b3Raw.GetMax().z) / 2 ; double dRawDimZ = b3Raw.GetMax().z - b3Raw.GetMin().z ; double dToolDimZ = b3Tool.GetMax().z - b3Tool.GetMin().z ; - // determino elevazione del punto rispetto al grezzo a metà altezza + // determino elevazione del punto rispetto al grezzo a metà altezza Point3d ptTest( ptP.x, ptP.y, dRawCentZ) ; double dNewElev ; if ( GetElevation( m_nPhase, ptTest, vtTool, dToolRadForElev, vtMyDir, dNewElev)) dElev = max( dElev, dNewElev) ; // se direzione di fuga quasi orizzontale (max 30 deg) e ingombro utensile rilevante rispetto ad altezza grezzo if ( vtMyDir.z < 0.5 && dToolDimZ > dRawDimZ / 2) { - // determino elevazione del punto rispetto al grezzo a metà altezza più metà ingombro utensile + // determino elevazione del punto rispetto al grezzo a metà altezza più metà ingombro utensile ptTest = Point3d( ptP.x, ptP.y, dRawCentZ + dToolDimZ / 2) ; if ( GetElevation( m_nPhase, ptTest, vtTool, dToolRadForElev, vtMyDir, dNewElev)) dElev = max( dElev, dNewElev) ; - // determino elevazione del punto rispetto al grezzo a metà altezza meno metà ingombro utensile + // determino elevazione del punto rispetto al grezzo a metà altezza meno metà ingombro utensile ptTest = Point3d( ptP.x, ptP.y, dRawCentZ - dToolDimZ / 2) ; if ( GetElevation( m_nPhase, ptTest, vtTool, dToolRadForElev, vtMyDir, dNewElev)) dElev = max( dElev, dNewElev) ; @@ -568,7 +569,7 @@ bool Operation::GetUhPointAboveRaw( const Point3d& ptP, const Vector3d& vtTool, double dToolRad, double dToolRadForElev, double dToolLen, bool bIsSaw, double dSafeZ, const Vector3d& vtDir, double& dElev) const { - // punto a metà lunghezza utensile + // punto a metà lunghezza utensile Point3d ptQ = ptP + vtTool * dToolLen / 2 ; // box di ingombro dell'utensile BBox3d b3Tool ; @@ -599,18 +600,18 @@ Operation::GetUhPointAboveRaw( const Point3d& ptP, const Vector3d& vtTool, doubl double dRawCentZ = ( b3Raw.GetMin().z + b3Raw.GetMax().z) / 2 ; double dRawDimZ = b3Raw.GetMax().z - b3Raw.GetMin().z ; double dToolDimZ = b3Tool.GetMax().z - b3Tool.GetMin().z ; - // determino elevazione del punto rispetto al grezzo a metà altezza + // determino elevazione del punto rispetto al grezzo a metà altezza Point3d ptTest( ptP.x, ptP.y, dRawCentZ) ; double dNewElev ; if ( GetElevation( m_nPhase, ptTest, vtTool, dToolRadForElev, vtMyDir, dNewElev)) dElev = max( dElev, dNewElev) ; // se direzione di fuga quasi orizzontale (max 30 deg) e ingombro utensile rilevante rispetto ad altezza grezzo if ( vtMyDir.z > -0.5 && dToolDimZ > dRawDimZ / 2) { - // determino elevazione del punto rispetto al grezzo a metà altezza più metà ingombro utensile + // determino elevazione del punto rispetto al grezzo a metà altezza più metà ingombro utensile ptTest = Point3d( ptP.x, ptP.y, dRawCentZ + dToolDimZ / 2) ; if ( GetElevation( m_nPhase, ptTest, vtTool, dToolRadForElev, vtMyDir, dNewElev)) dElev = max( dElev, dNewElev) ; - // determino elevazione del punto rispetto al grezzo a metà altezza meno metà ingombro utensile + // determino elevazione del punto rispetto al grezzo a metà altezza meno metà ingombro utensile ptTest = Point3d( ptP.x, ptP.y, dRawCentZ - dToolDimZ / 2) ; if ( GetElevation( m_nPhase, ptTest, vtTool, dToolRadForElev, vtMyDir, dNewElev)) dElev = max( dElev, dNewElev) ; @@ -629,7 +630,7 @@ Operation::GetDistanceFromRawSide( int nPhase, const Point3d& ptP, const Vector3 { if ( m_pMchMgr == nullptr || m_pGeomDB == nullptr) return false ; - // recupero il grezzo più vicino al punto (entro 100 mm) + // recupero il grezzo più vicino al punto (entro 100 mm) const double EXPAND_LEN = 100 ; BBox3d b3Pnt( ptP) ; b3Pnt.Expand( EXPAND_LEN, EXPAND_LEN, 0) ; @@ -643,7 +644,7 @@ Operation::GetDistanceFromRawSide( int nPhase, const Point3d& ptP, const Vector3 BBox3d b3Raw ; int nRawSolidId = m_pGeomDB->GetFirstNameInGroup( nCurrRawId, MACH_RAW_SOLID) ; if ( m_pGeomDB->GetGlobalBBox( nRawSolidId, b3Raw) && b3Pnt.OverlapsXY( b3Raw)) { - // porto il punto nel riferimento del grezzo (a Z=0 perchè ivi è il contorno) + // porto il punto nel riferimento del grezzo (a Z=0 perchè ivi è il contorno) Frame3d frRaw ; m_pGeomDB->GetGroupGlobFrame( nCurrRawId, frRaw) ; Point3d ptPL = ptP ; @@ -673,7 +674,7 @@ Operation::GetDistanceFromRawSide( int nPhase, const Point3d& ptP, const Vector3 // passo al grezzo successivo nCurrRawId = m_pMchMgr->GetNextRawPart( nCurrRawId) ; } - // se il punto non è interno ad alcun grezzo, distanza nulla + // se il punto non è interno ad alcun grezzo, distanza nulla if ( nRawId == GDB_ID_NULL) { dDist = 0 ; return true ; @@ -716,7 +717,7 @@ Operation::GetMinDistanceFromRawSide( int nPhase, const Point3d& ptP, double dEx { if ( m_pMchMgr == nullptr || m_pGeomDB == nullptr) return false ; - // recupero il grezzo in cui è incluso il punto + // recupero il grezzo in cui è incluso il punto BBox3d b3Pnt( ptP) ; double dTotExpand = dExpand + 10 ; b3Pnt.Expand( dTotExpand, dTotExpand, 0) ; @@ -728,7 +729,7 @@ Operation::GetMinDistanceFromRawSide( int nPhase, const Point3d& ptP, double dEx BBox3d b3Raw ; int nRawSolidId = m_pGeomDB->GetFirstNameInGroup( nRawId, MACH_RAW_SOLID) ; if ( m_pGeomDB->GetGlobalBBox( nRawSolidId, b3Raw) && b3Pnt.OverlapsXY( b3Raw)) { - // porto il punto nel riferimento del grezzo (a Z=0 perchè ivi è il contorno) + // porto il punto nel riferimento del grezzo (a Z=0 perchè ivi è il contorno) Frame3d frRaw ; m_pGeomDB->GetGroupGlobFrame( nRawId, frRaw) ; Point3d ptPL = ptP ; @@ -786,7 +787,7 @@ Operation::GetMinDistanceFromRawSide( int nPhase, const Point3d& ptP, double dEx // passo al grezzo successivo nRawId = m_pMchMgr->GetNextRawPart( nRawId) ; } - // se il punto non è interno ad alcun grezzo, distanza nulla + // se il punto non è interno ad alcun grezzo, distanza nulla if ( nRawId == GDB_ID_NULL) dDist = 0 ; @@ -933,7 +934,7 @@ GetDirInFacePerpSide( int nFaceUse, const Vector3d& vtN, ICurveComposite* pCrvCo vtRef -= vtRef * vtN * vtN ; if ( ! vtRef.Normalize()) vtRef = V_NULL ; - // sostituisco questa direzione con quella perpendicolare alla curva più adatta + // sostituisco questa direzione con quella perpendicolare alla curva più adatta double dCosMax = -1 ; Vector3d vtPerpMax ; const ICurve* pCrv = pCrvCompo->GetFirstCurve() ; @@ -947,7 +948,7 @@ GetDirInFacePerpSide( int nFaceUse, const Vector3d& vtN, ICurveComposite* pCrvCo } pCrv = pCrvCompo->GetNextCurve() ; } - // richiedo non più di 85deg di deviazione + // richiedo non più di 85deg di deviazione if ( dCosMax > 0.087) vtRef = vtPerpMax ; return vtRef ; @@ -973,7 +974,7 @@ Operation::AdjustCurveFromSurf( ICurveComposite* pCrvCompo, int nToolDir, int nF // se richiesto contorno if ( nFaceUse == FACE_CONT) { - // cerco l'estremo più alto e lo imposto come inizio + // cerco l'estremo più alto e lo imposto come inizio double dU = 0 ; double dUmax = 0 ; double dZmax = - INFINITO ; @@ -1074,7 +1075,7 @@ Operation::AdjustCurveFromSurf( ICurveComposite* pCrvCompo, int nToolDir, int nF vtExtr = vtEe ; if ( nToolDir == TOOL_PAR_SLANT) { Vector3d vtDirPN = -vtDirPrev + vtDirNext ; - // se le direzioni prima e dopo non sono troppo diverse tra loro (15deg) e la loro media è normalizzabile + // se le direzioni prima e dopo non sono troppo diverse tra loro (15deg) e la loro media è normalizzabile if ( ( -vtDirPrev * vtDirNext) > cos( 15 * DEGTORAD) && vtDirPN.Normalize()) { if ( vtDirPN * vtExtr > 0) vtExtr = vtDirPN ; @@ -1159,9 +1160,9 @@ Operation::ApproxWithArcsIfUseful( ICurveComposite* pCompo, bool bCareTempProp) BBox3d b3Crv ; if ( ! pCompo->GetBBox( frInvRef, b3Crv) || abs( b3Crv.GetMax().z - b3Crv.GetMin().z - dThick) > 100 * EPS_SMALL) return true ; - // porto la curva nel suo piano perchè ApproxWithArcsEx funziona bene solo nel piano XY (all'uscita va ripristinato il rif. originale) + // porto la curva nel suo piano perchè ApproxWithArcsEx funziona bene solo nel piano XY (all'uscita va ripristinato il rif. originale) pCompo->ToLoc( frRef) ; - // se posso ignorare la proprietà temporanea + // se posso ignorare la proprietà temporanea if ( ! bCareTempProp) { // calcolo approssimazione con archi (se possibile) PolyArc PA ; @@ -1176,7 +1177,7 @@ Operation::ApproxWithArcsIfUseful( ICurveComposite* pCompo, bool bCareTempProp) return false ; } } - // altrimenti divido la curva in parti con la stessa proprietà e approssimo separatamente ciascuna parte + // altrimenti divido la curva in parti con la stessa proprietà e approssimo separatamente ciascuna parte else { // determino gli intervalli in cui dividere la curva INTINTVECTOR vInt ; @@ -1216,14 +1217,14 @@ Operation::ApproxWithArcsIfUseful( ICurveComposite* pCompo, bool bCareTempProp) pCompo->ToGlob( frRef) ; return false ; } - // assegno la proprietà originale ad ogni curva semplice della composita + // assegno la proprietà originale ad ogni curva semplice della composita for ( int j = 0 ; j < pCC->GetCurveCount() ; ++ j) pCC->SetCurveTempProp( j, vInt[i].second) ; // reinserisco nel vettore di curve vpCrvs.back().Set( pCC) ; } } - // riassemblo la curva e assegno le opportune proprietà + // riassemblo la curva e assegno le opportune proprietà pCompo->Clear() ; for ( int i = 0 ; i < int( vpCrvs.size()) ; ++ i) { pCompo->AddCurve( Release( vpCrvs[i])) ; @@ -1241,7 +1242,7 @@ Operation::ApproxWithArcsIfUseful( ICurveComposite* pCompo, bool bCareTempProp) //---------------------------------------------------------------------------- bool -Operation::ApproxWithLines( ICurveComposite* pCompo) const +Operation::ApproxWithLines( ICurveComposite* pCompo, double dMaxLen) const { // recupero estrusione e spessore Vector3d vtExtr = Z_AX ; @@ -1252,6 +1253,9 @@ Operation::ApproxWithLines( ICurveComposite* pCompo) const PolyLine PL ; if ( ! pCompo->ApproxWithLines( 50 * EPS_SMALL, ANG_TOL_MAX_DEG, ICurve::APL_SPECIAL, PL)) return false ; + // se prevista massima lunghezza + if ( dMaxLen > 100 * EPS_SMALL) + PL.AdjustForMaxSegmentLen( dMaxLen) ; // sostituisco le linee alle curve originali pCompo->Clear() ; pCompo->FromPolyLine( PL) ; @@ -1269,7 +1273,7 @@ Operation::VerifyArcs( ICurveComposite* pCompo, double dMaxAngCen) const // verifiche sull'ampiezza dell'angolo al centro degli eventuali archi int nMaxInd = pCompo->GetCurveCount() - 1 ; for ( int i = 0 ; i <= nMaxInd ; ) { - // se arco con angolo al centro oltre il limite, lo divido a metà + // se arco con angolo al centro oltre il limite, lo divido a metà const ICurveArc* pArc = GetCurveArc( pCompo->GetCurve( i)) ; if ( pArc != nullptr && abs( pArc->GetAngCenter()) > dMaxAngCen) { pCompo->AddJoint( i + 0.5) ; @@ -1296,7 +1300,7 @@ Operation::CalcAndSetBBox( int nClId) for ( int nEntId = m_pGeomDB->GetFirstInGroup( nClPathId) ; nEntId != GDB_ID_NULL ; nEntId = m_pGeomDB->GetNext( nEntId)) { - // recupero i dati Cam dell'entità + // recupero i dati Cam dell'entità CamData* pCamData = GetCamData( m_pGeomDB->GetUserObj( nEntId)) ; if ( pCamData == nullptr) continue ; @@ -1363,7 +1367,7 @@ Operation::CalcAndSetAxesBBox( void) for ( int nEntId = m_pGeomDB->GetFirstInGroup( nClPathId) ; nEntId != GDB_ID_NULL ; nEntId = m_pGeomDB->GetNext( nEntId)) { - // recupero i dati Cam dell'entità + // recupero i dati Cam dell'entità CamData* pCamData = GetCamData( m_pGeomDB->GetUserObj( nEntId)) ; if ( pCamData == nullptr) continue ; @@ -1557,7 +1561,7 @@ Operation::GetInitialCamData( bool bSkipClimb) const return nullptr ; // recupero il primo percorso CL int nClPathId = m_pGeomDB->GetFirstGroupInGroup( nClId) ; - // recupero i dati Cam della prima entità del percorso + // recupero i dati Cam della prima entità del percorso return GetClPathInitialCamData( nClPathId, bSkipClimb) ; } @@ -1567,9 +1571,9 @@ Operation::GetClPathInitialCamData( int nClPathId, bool bSkipClimb) const { if ( m_pMchMgr == nullptr || m_pGeomDB == nullptr) return nullptr ; - // recupero la prima entità di questo percorso + // recupero la prima entità di questo percorso int nEntId = m_pGeomDB->GetFirstInGroup( nClPathId) ; - // se richiesto di ignorare le entità CLIMB + // se richiesto di ignorare le entità CLIMB if ( bSkipClimb) { while ( nEntId != GDB_ID_NULL) { string sName ; @@ -1578,7 +1582,7 @@ Operation::GetClPathInitialCamData( int nClPathId, bool bSkipClimb) const nEntId = m_pGeomDB->GetNext( nEntId) ; } } - // recupero i dati Cam dell'entità + // recupero i dati Cam dell'entità return GetCamData( m_pGeomDB->GetUserObj( nEntId)) ; } @@ -1594,7 +1598,7 @@ Operation::GetFinalCamData( bool bSkipRise) const return nullptr ; // recupero l'ultimo percorso CL int nClPathId = m_pGeomDB->GetLastGroupInGroup( nClId) ; - // recupero i dati Cam dell'ultima entità del percorso + // recupero i dati Cam dell'ultima entità del percorso return GetClPathFinalCamData( nClPathId, bSkipRise) ; } @@ -1604,9 +1608,9 @@ Operation::GetClPathFinalCamData( int nClPathId, bool bSkipRise) const { if ( m_pMchMgr == nullptr || m_pGeomDB == nullptr) return nullptr ; - // recupero l'ultima entità di questo percorso + // recupero l'ultima entità di questo percorso int nEntId = m_pGeomDB->GetLastInGroup( nClPathId) ; - // se richiesto di ignorare le entità RISE + // se richiesto di ignorare le entità RISE if ( bSkipRise) { while ( nEntId != GDB_ID_NULL) { string sName ; @@ -1615,7 +1619,7 @@ Operation::GetClPathFinalCamData( int nClPathId, bool bSkipRise) const nEntId = m_pGeomDB->GetPrev( nEntId) ; } } - // recupero i dati Cam dell'entità + // recupero i dati Cam dell'entità return GetCamData( m_pGeomDB->GetUserObj( nEntId)) ; } @@ -1709,7 +1713,7 @@ Operation::CalculateAxesValues( const string& sHint, bool bRotContOnNext, bool b if ( ! m_pMchMgr->SetCalcTool( GetToolName(), GetHeadName(), GetExitNbr())) return false ; - // rimuovo posizionamento home da lavorazione precedente se non richiesto (non può più essere l'ultima) + // rimuovo posizionamento home da lavorazione precedente se non richiesto (non può più essere l'ultima) if ( pPrevOp != nullptr && ! NeedPrevHome()) pPrevOp->RemoveHome() ; @@ -1789,36 +1793,60 @@ Operation::CalculateAxesValues( const string& sHint, bool bRotContOnNext, bool b // calcolo il valore degli assi macchina di tutti i movimenti bool bOk = true ; - int nClPathId = m_pGeomDB->GetFirstGroupInGroup( nClId) ; - while ( nClPathId != GDB_ID_NULL) { - DBLVECTOR vAxRotPrecOri = vAxRotPrec ; - int nOutStrC = 0 ; - if ( ! CalculateClPathAxesValues( nClPathId, nLinAxes, nRotAxes, dRot1W, bMaxDeltaR2OnFirst, - bRotContOnNext, dAngDeltaMinForHome, vAxRotHome, vAxRotPrec, nOutStrC)) { - // se attivata scelta angolo iniziale vicino ad home ed extra corsa C, provo a rifarlo disattivando - if ( dAngDeltaMinForHome < ANG_FULL && nOutStrC != 0) { - m_pMchMgr->GetCurrMachine()->ResetOutstrokeInfo() ; - CalculateClPathAxesValues( nClPathId, nLinAxes, nRotAxes, dRot1W, bMaxDeltaR2OnFirst, - bRotContOnNext, INFINITO, vAxRotHome, vAxRotPrec, nOutStrC) ; - } - // se extracorsa dell'asse C, provo a precaricarlo al contrario - if ( nOutStrC != 0) { - vAxRotPrec = vAxRotPrecOri ; - vAxRotPrec[0] = vAxRotPrecOri[0] + ( nOutStrC > 0 ? - ANG_FULL : ANG_FULL) ; - m_pMchMgr->GetCurrMachine()->ResetOutstrokeInfo() ; - if ( ! CalculateClPathAxesValues( nClPathId, nLinAxes, nRotAxes, dRot1W, bMaxDeltaR2OnFirst, - bRotContOnNext, INFINITO, vAxRotHome, vAxRotPrec, nOutStrC)) - bOk = false ; - } - else + if ( m_pMchMgr->GetCurrIsCenter()) { + int nClPathId = m_pGeomDB->GetFirstGroupInGroup( nClId) ; + while ( nClPathId != GDB_ID_NULL) { + if ( ! CalculateClPathCenterAxesValues( nClPathId, nLinAxes, nRotAxes, dRot1W, bMaxDeltaR2OnFirst, + bRotContOnNext, dAngDeltaMinForHome, vAxRotHome, vAxRotPrec)) bOk = false ; + nClPathId = m_pGeomDB->GetNextGroup( nClPathId) ; + } + } + else if ( m_pMchMgr->GetCurrIsRobot()) { + int nClPathId = m_pGeomDB->GetFirstGroupInGroup( nClId) ; + while ( nClPathId != GDB_ID_NULL) { + if ( ! CalculateClPathRobotAxesValues( nClPathId, nLinAxes, nRotAxes, dRot1W, bMaxDeltaR2OnFirst, + bRotContOnNext, dAngDeltaMinForHome, vAxRotHome, vAxRotPrec)) + bOk = false ; + nClPathId = m_pGeomDB->GetNextGroup( nClPathId) ; } - nClPathId = m_pGeomDB->GetNextGroup( nClPathId) ; } return bOk ; } +//---------------------------------------------------------------------------- +bool +Operation::CalculateClPathCenterAxesValues( int nClPathId, int nLinAxes, int nRotAxes, double dRot1W, + bool bMaxDeltaR2OnFirst, bool bRotContOnNext, double dAngDeltaMinForHome, + const DBLVECTOR& vAxRotHome, DBLVECTOR& vAxRotPrec) +{ + DBLVECTOR vAxRotPrecOri = vAxRotPrec ; + int nOutStrC = 0 ; + if ( ! CalculateClPathAxesValues( nClPathId, nLinAxes, nRotAxes, dRot1W, bMaxDeltaR2OnFirst, + bRotContOnNext, dAngDeltaMinForHome, vAxRotHome, vAxRotPrec, nOutStrC)) { + // se attivata scelta angolo iniziale vicino ad home ed extra corsa C, provo a rifarlo disattivando + if ( dAngDeltaMinForHome < ANG_FULL && nOutStrC != 0) { + m_pMchMgr->GetCurrMachine()->ResetOutstrokeInfo() ; + if ( CalculateClPathAxesValues( nClPathId, nLinAxes, nRotAxes, dRot1W, bMaxDeltaR2OnFirst, + bRotContOnNext, INFINITO, vAxRotHome, vAxRotPrec, nOutStrC)) + return true ; + } + // se extracorsa dell'asse C, provo a precaricarlo al contrario + if ( nOutStrC != 0) { + vAxRotPrec = vAxRotPrecOri ; + vAxRotPrec[0] = vAxRotPrecOri[0] + ( nOutStrC > 0 ? - ANG_FULL : ANG_FULL) ; + m_pMchMgr->GetCurrMachine()->ResetOutstrokeInfo() ; + if ( ! CalculateClPathAxesValues( nClPathId, nLinAxes, nRotAxes, dRot1W, bMaxDeltaR2OnFirst, + bRotContOnNext, INFINITO, vAxRotHome, vAxRotPrec, nOutStrC)) + return false ; + } + else + return false ; + } + return true ; +} + //---------------------------------------------------------------------------- bool Operation::CalculateClPathAxesValues( int nClPathId, int nLinAxes, int nRotAxes, double dRot1W, @@ -1827,21 +1855,21 @@ Operation::CalculateClPathAxesValues( int nClPathId, int nLinAxes, int nRotAxes, { if ( m_pMchMgr == nullptr || m_pGeomDB == nullptr) return false ; - // massima variazione secondo asse rotante per continuità + // massima variazione secondo asse rotante per continuità double dMaxDeltaR2 = 30 ; // predispongo variabile per valori assi DBLVECTOR vAxVal ; vAxVal.reserve( 8) ; // variabili per coordinate lineari precedenti double dXprec, dYprec, dZprec ; - // ciclo su tutte le entità del percorso CL + // ciclo su tutte le entità del percorso CL nOutStrC = 0 ; bool bOk = true ; bool bFirst = true ; for ( int nEntId = m_pGeomDB->GetFirstInGroup( nClPathId) ; nEntId != GDB_ID_NULL ; nEntId = m_pGeomDB->GetNext( nEntId)) { - // recupero i dati Cam dell'entità + // recupero i dati Cam dell'entità CamData* pCamData = GetCamData( m_pGeomDB->GetUserObj( nEntId)) ; if ( pCamData == nullptr) continue ; @@ -1863,7 +1891,7 @@ Operation::CalculateClPathAxesValues( int nClPathId, int nLinAxes, int nRotAxes, if ( abs( nRStat) == 1) { // se primo movimento if ( bFirst) { - // porto gli angoli ai valori più vicini ai precedenti con offset di uno o più giri + // porto gli angoli ai valori più vicini ai precedenti con offset di uno o più giri for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) { m_pMchMgr->GetNearestAngleInStroke( i, vAxRotPrec[i], vAng1[i]) ; if ( abs( vAng1[i] - vAxRotPrec[i]) > dAngDeltaMinForHome) @@ -1884,7 +1912,7 @@ Operation::CalculateClPathAxesValues( int nClPathId, int nLinAxes, int nRotAxes, } // per movimenti successivi else { - // scelgo gli angoli più vicini, per continuità non applico offset per stare nelle corse + // scelgo gli angoli più vicini, per continuità non applico offset per stare nelle corse for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) { if ( bRotContOnNext) vAng1[i] = AngleNearAngle( vAng1[i], vAxRotPrec[i]) ; @@ -1911,7 +1939,7 @@ Operation::CalculateClPathAxesValues( int nClPathId, int nLinAxes, int nRotAxes, bool bAng2 = true ; // se primo movimento if ( bFirst) { - // porto gli angoli ai valori più vicini ai precedenti con offset di uno o più giri + // porto gli angoli ai valori più vicini ai precedenti con offset di uno o più giri for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) { m_pMchMgr->GetNearestAngleInStroke( i, vAxRotPrec[i], vAng1[i]) ; if ( abs( vAng1[i] - vAxRotPrec[i]) > dAngDeltaMinForHome) @@ -1935,7 +1963,7 @@ Operation::CalculateClPathAxesValues( int nClPathId, int nLinAxes, int nRotAxes, } } else { - // scelgo gli angoli più vicini, per continuità non applico offset per stare nelle corse + // scelgo gli angoli più vicini, per continuità non applico offset per stare nelle corse for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) { if ( bRotContOnNext) vAng1[i] = AngleNearAngle( vAng1[i], vAxRotPrec[i]) ; @@ -1970,7 +1998,7 @@ Operation::CalculateClPathAxesValues( int nClPathId, int nLinAxes, int nRotAxes, bAng2 = false ; } } - // scelgo la soluzione più vicina alla precedente + // scelgo la soluzione più vicina alla precedente double dDelta1 = 0 ; double dDelta2 = 0 ; for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) { @@ -2045,9 +2073,9 @@ Operation::CalculateClPathAxesValues( int nClPathId, int nLinAxes, int nRotAxes, Vector3d vtBackAux ; m_pMchMgr->GetCurrMachine()->GetBackAuxDirFromAngles( vAng1, vtBackAux) ; pCamData->SetBackAuxDir( vtBackAux) ; - // se esiste precedente, corrente è linea ed esistono aree protette, devo verificare anche i punti interni + // se esiste precedente, corrente è linea ed esistono aree protette, devo verificare anche i punti intermedi if ( ! bFirst && pCamData->IsLine() && m_pMchMgr->ExistProtectedAreas()) { - // verifico i limiti di corsa dei punti lungo l'arco + // verifico i limiti di corsa dei punti lungo la linea const int NUM_VERIF_STEP = 16 ; Vector3d vtN = pCamData->GetAxesNormDir() ; Point3d ptPrec( dXprec, dYprec, dZprec) ; @@ -2118,7 +2146,7 @@ Operation::CalculateClPathAxesValues( int nClPathId, int nLinAxes, int nRotAxes, pCamData->SetAxesNormDir( V_NULL) ; // il movimento diventa lineare pCamData->SetMoveType( 1) ; - // non è necessario verificare i limiti di corsa degli assi + // non è necessario verificare i limiti di corsa degli assi } // altrimenti calcolo l'arco cui appartengono else { @@ -2177,7 +2205,7 @@ Operation::CalculateClPathAxesValues( int nClPathId, int nLinAxes, int nRotAxes, pCamData->SetAxesNormDir( V_NULL) ; // il movimento diventa lineare pCamData->SetMoveType( 1) ; - // non è necessario verificare i limiti di corsa degli assi + // non è necessario verificare i limiti di corsa degli assi } } } @@ -2193,6 +2221,103 @@ Operation::CalculateClPathAxesValues( int nClPathId, int nLinAxes, int nRotAxes, return bOk ; } +//---------------------------------------------------------------------------- +bool +Operation::CalculateClPathRobotAxesValues( int nClPathId, int nLinAxes, int nRotAxes, double dRot1W, + bool bMaxDeltaR2OnFirst, bool bRotContOnNext, double dAngDeltaMinForHome, + const DBLVECTOR& vAxRotHome, DBLVECTOR& vAxRotPrec) +{ + if ( m_pMchMgr == nullptr || m_pGeomDB == nullptr) + return false ; + // massima variazione assi rotanti per continuità + double dMaxDeltaR = 30 ; + // eventuale direzione Aux preferenziale + bool bUseRefAux = true ; + Vector3d vtRefAux = Y_AX ; + switch ( GetSolCh()) { + 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 ; + case MCH_SCC_ADIR_YM : vtRefAux = -Y_AX ; break ; + case MCH_SCC_ADIR_ZP : vtRefAux = Z_AX ; break ; + case MCH_SCC_ADIR_ZM : vtRefAux = -Z_AX ; break ; + default : bUseRefAux = false ; break ; + } + // predispongo variabile per valori assi + DBLVECTOR vAxVal ; + vAxVal.reserve( 8) ; + // ciclo su tutte le entità del percorso CL + bool bOk = true ; + bool bFirst = true ; + for ( int nEntId = m_pGeomDB->GetFirstInGroup( nClPathId) ; + nEntId != GDB_ID_NULL ; + nEntId = m_pGeomDB->GetNext( nEntId)) { + // recupero i dati Cam dell'entità + CamData* pCamData = GetCamData( m_pGeomDB->GetUserObj( nEntId)) ; + if ( pCamData == nullptr) + continue ; + // verifico che il movimento non sia un arco + if ( bFirst && pCamData->IsArc()) { + pCamData->ResetAxes() ; + LOG_ERROR( GetEMkLogger(), "Error : move in ClPath for Robot is an arc") + return false ; + } + // ricavo posizione con eventuali modifiche dipendenti dalla lavorazione + Point3d ptP = pCamData->GetEndPoint() ; + AdjustEndPointForAxesCalc( pCamData, ptP) ; + // calcolo gli assi del robot + Vector3d vtTool = pCamData->GetToolDir() ; + Vector3d vtAux ; + if ( ! bUseRefAux) { + Frame3d frTool ; + frTool.Set( ORIG, vtTool) ; + frTool.Rotate( frTool.Orig(), frTool.VersZ(), 0, 1) ; + vtAux = frTool.VersX() ; + } + else { + vtAux = OrthoCompo( vtRefAux, vtTool) ; + vtAux.Normalize() ; + } + pCamData->SetAuxDir( vtAux) ; + DBLVECTOR vAng1, vAng2 ; + bool bRaOk = m_pMchMgr->GetCurrMachine()->GetRobotAngles( ptP, vtTool, vtAux, vAng1, vAng2) ; + if ( ! bRaOk) { + pCamData->SetAxes( CamData::AS_ERR, vAxVal) ; + return false ; + } + // se inizio, scelgo la prima soluzione + if ( bFirst) { + vAxVal = vAng1 ; + bFirst = false ; + } + // altrimenti, scelgo la soluzione più vicina + else { + // minimizzo rotazione polso + if ( abs( vAng1[3] - vAxRotPrec[3]) < abs( vAng2[3] - vAxRotPrec[3]) + 10 * EPS_ANG_SMALL) + vAxVal = vAng1 ; + else + vAxVal = vAng2 ; + // controllo continuità di R6 dalla posizione precedente + m_pMchMgr->GetNearestAngleInStroke( 5, vAxRotPrec[5], vAxVal[5]) ; + } + // verifico di essere nelle corse degli assi + int nStat ; + bool bOsOk = m_pMchMgr->VerifyOutstroke( 0, 0, 0, vAxVal, false, nStat) ; + // salvo il risultato + if ( ! bOsOk || nStat != 0) { + bOk = false ; + pCamData->SetAxes( CamData::AS_OUTSTROKE, vAxVal) ; + } + else { + pCamData->SetAxes( CamData::AS_OK, vAxVal) ; + } + // aggiorno valori precedenti + vAxRotPrec = vAxVal ; + } + + return bOk ; +} + //---------------------------------------------------------------------------- bool Operation::AdjustStartEndMovements( bool bVerifyPreviousLink) @@ -2239,140 +2364,93 @@ Operation::AdjustStartEndMovements( bool bVerifyPreviousLink) // imposto l'utensile per i calcoli macchina if ( ! m_pMchMgr->SetCalcTool( GetToolName(), GetHeadName(), GetExitNbr())) return false ; - // cancello risalita lavorazione precedente - pPrevOp->RemoveRise() ; - // recupero posizione finale lavorazione precedente - if ( ! pPrevOp->GetFinalAxesValues( false, vAxVal)) - return false ; // se richiesta verifica collegamento con lavorazione precedente if ( bVerifyPreviousLink) { - // recupero se ZHome è in basso - bool bZHomeDown = GetZHomeDown() ; - // recupero quota di home in Z - double dHomeZ ; - if ( ! m_pMchMgr->GetCurrAxisHomePos( 2, dHomeZ)) + // cancello eventuale risalita finale lavorazione precedente + pPrevOp->RemoveRise() ; + // recupero posizione finale lavorazione precedente + if ( ! pPrevOp->GetFinalAxesValues( false, vAxVal)) return false ; - // elimino eventuali CLIMB già presenti + // elimino eventuali CLIMB già presenti RemoveClimb( m_pGeomDB->GetFirstGroupInGroup( nClId)) ; // recupero posizione iniziale lavorazione DBLVECTOR vAxIni ; if ( ! GetInitialAxesValues( false, vAxIni)) return false ; - // se necessaria, aggiungo risalita parziale - double dDeltaZ = ( vAxIni[2] - vAxVal[2]) ; - if ( ( ! bZHomeDown && dDeltaZ > 100 * EPS_SMALL && vAxIni[2] <= dHomeZ) || - ( bZHomeDown && dDeltaZ < -100 * EPS_SMALL && vAxIni[2] >= dHomeZ)) { - if ( ! pPrevOp->AddRise( vAxVal, dDeltaZ, GDB_ID_NULL, bZHomeDown)) + // se centro di lavoro + if ( m_pMchMgr->GetCurrIsCenter()) { + // recupero se ZHome è in basso + bool bZHomeDown = GetZHomeDown() ; + // recupero quota di home in Z + double dHomeZ ; + if ( ! m_pMchMgr->GetCurrAxisHomePos( 2, dHomeZ)) return false ; - // aggiorno quota in Z della posizione iniziale ( anche se verrà aggiornata solo in seguito) - vAxIni[2] = vAxVal[2] ; - } - // Verifico non ci sia collisione a HomeZ - bool bToZmax = false ; - bool bToMyHomeZ = false ; - DBLVECTOR vAxVal2 = vAxVal ; vAxVal2[2] = dHomeZ ; - DBLVECTOR vAxIni2 = vAxIni ; vAxIni2[2] = dHomeZ ; - if ( ! TestCollisionAvoid( vAxVal2, vAxIni2)) { - // se ammessa risalita aggiuntiva - Vector3d vtTprev ; pPrevOp->GetFinalToolDir( false, vtTprev) ; - Vector3d vtTcurr ; GetInitialToolDir( false, vtTcurr) ; - double dExtraZ ; - if ( GetExtraZ( vAxVal2, vtTprev, vAxIni2, vtTcurr, dHomeZ, dExtraZ)) { - if ( abs( dExtraZ) > EPS_SMALL) { - double dMyHomeZ = dHomeZ + dExtraZ ; - vAxVal2[2] = dMyHomeZ ; - vAxIni2[2] = dMyHomeZ ; - if ( TestCollisionAvoid( vAxVal2, vAxIni2)) { - dHomeZ = dMyHomeZ ; - bToMyHomeZ = true ; + // se necessaria, aggiungo risalita parziale + double dDeltaZ = ( vAxIni[2] - vAxVal[2]) ; + if ( ( ! bZHomeDown && dDeltaZ > 100 * EPS_SMALL && vAxIni[2] <= dHomeZ) || + ( bZHomeDown && dDeltaZ < -100 * EPS_SMALL && vAxIni[2] >= dHomeZ)) { + if ( ! pPrevOp->AddRise( vAxVal, dDeltaZ, GDB_ID_NULL, bZHomeDown)) + return false ; + // aggiorno quota in Z della posizione iniziale ( anche se verrà aggiornata solo in seguito) + vAxIni[2] = vAxVal[2] ; + } + // Verifico non ci sia collisione a HomeZ + bool bToZmax = false ; + bool bToMyHomeZ = false ; + DBLVECTOR vAxVal2 = vAxVal ; vAxVal2[2] = dHomeZ ; + DBLVECTOR vAxIni2 = vAxIni ; vAxIni2[2] = dHomeZ ; + if ( ! TestCollisionAvoid( vAxVal2, vAxIni2)) { + // se ammessa risalita aggiuntiva + Vector3d vtTprev ; pPrevOp->GetFinalToolDir( false, vtTprev) ; + Vector3d vtTcurr ; GetInitialToolDir( false, vtTcurr) ; + double dExtraZ ; + if ( GetExtraZ( vAxVal2, vtTprev, vAxIni2, vtTcurr, dHomeZ, dExtraZ)) { + if ( abs( dExtraZ) > EPS_SMALL) { + double dMyHomeZ = dHomeZ + dExtraZ ; + vAxVal2[2] = dMyHomeZ ; + vAxIni2[2] = dMyHomeZ ; + if ( TestCollisionAvoid( vAxVal2, vAxIni2)) { + dHomeZ = dMyHomeZ ; + bToMyHomeZ = true ; + } + else + bToZmax = true ; } else bToZmax = true ; } - else - bToZmax = true ; - } - // se altrimenti ammessa rotazione a Zmax e richiesta rotazione - else if ( GetRotationAtZmax() && - (( vAxVal2.size() >= 4 && abs( vAxVal2[3] - vAxIni2[3]) > 5) || - ( vAxVal2.size() >= 5 && abs( vAxVal2[4] - vAxIni2[4]) > 5))) { - // verifico se va bene rotazione assi iniziale e poi movimento - DBLVECTOR vAxMid2 = vAxVal2 ; - vAxMid2[3] = vAxIni2[3] ; - if ( vAxVal2.size() >= 5) - vAxMid2[4] = vAxIni2[4] ; - if ( TestCollisionAvoid( vAxVal2, vAxMid2) && - TestCollisionAvoid( vAxMid2, vAxIni2) && - pPrevOp->AddRise( vAxVal2) && - pPrevOp->AddSpecialRise( vAxMid2, true, GDB_ID_NULL, 5)) { - vAxVal = vAxMid2 ; - bMaxZ = true ; + // se altrimenti ammessa rotazione a Zmax e richiesta rotazione + else if ( GetRotationAtZmax() && + (( vAxVal2.size() >= 4 && abs( vAxVal2[3] - vAxIni2[3]) > 5) || + ( vAxVal2.size() >= 5 && abs( vAxVal2[4] - vAxIni2[4]) > 5))) { + // verifico se va bene rotazione assi iniziale e poi movimento + DBLVECTOR vAxMid2 = vAxVal2 ; + vAxMid2[3] = vAxIni2[3] ; + if ( vAxVal2.size() >= 5) + vAxMid2[4] = vAxIni2[4] ; + if ( TestCollisionAvoid( vAxVal2, vAxMid2) && + TestCollisionAvoid( vAxMid2, vAxIni2) && + pPrevOp->AddRise( vAxVal2) && + pPrevOp->AddSpecialRise( vAxMid2, true, GDB_ID_NULL, 5)) { + vAxVal = vAxMid2 ; + bMaxZ = true ; + } + else + return false ; } + // altrimenti impossibile else return false ; } - // altrimenti impossibile - else - return false ; - } - // Recupero box complessivo dei grezzi attivi - BBox3d b3Raws ; - GetCurrRawsGlobBox( b3Raws) ; - // Se già a Zmax - if ( bMaxZ) { - // non devo fare alcunché - } - // se altrimenti richiesta risalita a Zmax - else if ( bToZmax || ForcedZmax( vAxVal, vAxIni, b3Raws)) { - // cancello eventuale risalita parziale della lavorazione precedente - pPrevOp->RemoveRise() ; - // aggiungo risalita a Zmax - if ( ! pPrevOp->AddRise( vAxVal)) - return false ; - // si parte da Z massima - bMaxZ = true ; - } - // altrimenti, verifico se la testa interferisce con i pezzi o i bloccaggi sulla tavola - else if ( bToMyHomeZ || ! TestCollisionAvoid( vAxVal, vAxIni)) { - // riprovo con risalita parziale - bool bPartRise = false ; - double dSafeZ = dHomeZ ; - double dUnsafeZ = ( bZHomeDown ? min( vAxVal[2], vAxIni[2]) : max( vAxVal[2], vAxIni[2])) ; - if ( ( ! bZHomeDown && dUnsafeZ < dSafeZ) || ( bZHomeDown && dUnsafeZ > dSafeZ)) { - DBLVECTOR vAxVal2 = vAxVal ; - DBLVECTOR vAxIni2 = vAxIni ; - vAxVal2[2] = dUnsafeZ ; - vAxIni2[2] = dUnsafeZ ; - if ( TestCollisionAvoid( vAxVal2, vAxIni2)) { - bPartRise = true ; - dSafeZ = dUnsafeZ ; - } - else { - for ( int i = 1 ; i <= 3 ; ++ i) { - double dTestSafeZ = ( dUnsafeZ + dSafeZ) / 2 ; - vAxVal2[2] = dTestSafeZ ; - vAxIni2[2] = dTestSafeZ ; - if ( TestCollisionAvoid( vAxVal2, vAxIni2)) { - bPartRise = true ; - dSafeZ = dTestSafeZ ; - } - else { - dUnsafeZ = dTestSafeZ ; - } - } - } - // se necessario, riprovo appena prima di Zmax - if ( ! bPartRise) { - double dOffsZ = 1 * ( bZHomeDown ? 1 : -1) ; - vAxVal2[2] = dHomeZ + dOffsZ ; - vAxIni2[2] = dHomeZ + dOffsZ ; - if ( TestCollisionAvoid( vAxVal2, vAxIni2)) { - bPartRise = true ; - dSafeZ = dHomeZ + dOffsZ ; - } - } + // Recupero box complessivo dei grezzi attivi + BBox3d b3Raws ; + GetCurrRawsGlobBox( b3Raws) ; + // Se già a Zmax + if ( bMaxZ) { + // non devo fare alcunché } - if ( ! bPartRise) { + // se altrimenti richiesta risalita a Zmax + else if ( bToZmax || ForcedZmax( vAxVal, vAxIni, b3Raws)) { // cancello eventuale risalita parziale della lavorazione precedente pPrevOp->RemoveRise() ; // aggiungo risalita a Zmax @@ -2381,49 +2459,146 @@ Operation::AdjustStartEndMovements( bool bVerifyPreviousLink) // si parte da Z massima bMaxZ = true ; } - else { - // cancello eventuale risalita parziale della lavorazione precedente - pPrevOp->RemoveRise() ; - // recupero le nuove quote finali (per calcolare il corretto delta) - pPrevOp->GetFinalAxesValues( false, vAxVal) ; - // aggiungo risalita a Zsafe - double dDelta = ( ! bZHomeDown ? max( dSafeZ - vAxVal[2], 0.) : min( dSafeZ - vAxVal[2], 0.)) ; - if ( ! pPrevOp->AddRise( vAxVal, dDelta, GDB_ID_NULL, bZHomeDown)) - return false ; - // aggiorno quota iniziale in Z - vAxIni[2] = dSafeZ ; - } - } - // se ci sono aree protette... - if ( m_pMchMgr->GetCurrMachine()->ExistProtectedAreas()) { - // verifico se il collegamento le attraversa - const double PA_VERIF_LEN = 10 ; - Point3d ptStart( vAxVal[0], vAxVal[1], vAxVal[2]) ; - Point3d ptEnd( vAxIni[0], vAxIni[1], vAxIni[2]) ; - double dLen = DistXY( ptStart, ptEnd) ; - int nStep = int( dLen / PA_VERIF_LEN + 1) ; - bool bOutstroke = false ; - for ( int i = 1 ; i < nStep ; ++ i) { - Point3d ptCurr = Media( ptStart, ptEnd, double( i) / nStep) ; - int nStat = 0 ; - if ( ! m_pMchMgr->GetCurrMachine()->VerifyProtectedAreas( ptCurr.x, ptCurr.y, ptCurr.z, {}, nStat) || nStat != 0) { - bOutstroke = true ; - break ; + // altrimenti, verifico se la testa interferisce con i pezzi o i bloccaggi sulla tavola + else if ( bToMyHomeZ || ! TestCollisionAvoid( vAxVal, vAxIni)) { + // riprovo con risalita parziale + bool bPartRise = false ; + double dSafeZ = dHomeZ ; + double dUnsafeZ = ( bZHomeDown ? min( vAxVal[2], vAxIni[2]) : max( vAxVal[2], vAxIni[2])) ; + if ( ( ! bZHomeDown && dUnsafeZ < dSafeZ) || ( bZHomeDown && dUnsafeZ > dSafeZ)) { + DBLVECTOR vAxVal2 = vAxVal ; + DBLVECTOR vAxIni2 = vAxIni ; + vAxVal2[2] = dUnsafeZ ; + vAxIni2[2] = dUnsafeZ ; + if ( TestCollisionAvoid( vAxVal2, vAxIni2)) { + bPartRise = true ; + dSafeZ = dUnsafeZ ; + } + else { + for ( int i = 1 ; i <= 3 ; ++ i) { + double dTestSafeZ = ( dUnsafeZ + dSafeZ) / 2 ; + vAxVal2[2] = dTestSafeZ ; + vAxIni2[2] = dTestSafeZ ; + if ( TestCollisionAvoid( vAxVal2, vAxIni2)) { + bPartRise = true ; + dSafeZ = dTestSafeZ ; + } + else { + dUnsafeZ = dTestSafeZ ; + } + } + } + // se necessario, riprovo appena prima di Zmax + if ( ! bPartRise) { + double dOffsZ = 1 * ( bZHomeDown ? 1 : -1) ; + vAxVal2[2] = dHomeZ + dOffsZ ; + vAxIni2[2] = dHomeZ + dOffsZ ; + if ( TestCollisionAvoid( vAxVal2, vAxIni2)) { + bPartRise = true ; + dSafeZ = dHomeZ + dOffsZ ; + } + } + } + if ( ! bPartRise) { + // cancello eventuale risalita parziale della lavorazione precedente + pPrevOp->RemoveRise() ; + // aggiungo risalita a Zmax + if ( ! pPrevOp->AddRise( vAxVal)) + return false ; + // si parte da Z massima + bMaxZ = true ; + } + else { + // cancello eventuale risalita parziale della lavorazione precedente + pPrevOp->RemoveRise() ; + // recupero le nuove quote finali (per calcolare il corretto delta) + pPrevOp->GetFinalAxesValues( false, vAxVal) ; + // aggiungo risalita a Zsafe + double dDelta = ( ! bZHomeDown ? max( dSafeZ - vAxVal[2], 0.) : min( dSafeZ - vAxVal[2], 0.)) ; + if ( ! pPrevOp->AddRise( vAxVal, dDelta, GDB_ID_NULL, bZHomeDown)) + return false ; + // aggiorno quota iniziale in Z + vAxIni[2] = dSafeZ ; } } - // in caso di attraversamento dell'area proibita - if ( bOutstroke) { - // chiamo funzione script OnSpecialRapidMove per avere nuovo punto intermedio - DBLVECTOR vAxNew ; - bool bModif = false ; - bool bOk = ( SpecialMoveRapid( vAxVal, vAxIni, vAxNew, bModif) && bModif) ; - // inserisco il nuovo punto alla fine della lavorazione precedente - if ( bOk) - vAxVal = vAxNew ; - if ( ! pPrevOp->AddSpecialRise( vAxVal, bOk)) - return false ; + // se ci sono aree protette... + if ( m_pMchMgr->GetCurrMachine()->ExistProtectedAreas()) { + // verifico se il collegamento le attraversa + const double PA_VERIF_LEN = 10 ; + Point3d ptStart( vAxVal[0], vAxVal[1], vAxVal[2]) ; + Point3d ptEnd( vAxIni[0], vAxIni[1], vAxIni[2]) ; + double dLen = DistXY( ptStart, ptEnd) ; + int nStep = int( dLen / PA_VERIF_LEN + 1) ; + bool bOutstroke = false ; + for ( int i = 1 ; i < nStep ; ++ i) { + Point3d ptCurr = Media( ptStart, ptEnd, double( i) / nStep) ; + int nStat = 0 ; + if ( ! m_pMchMgr->GetCurrMachine()->VerifyProtectedAreas( ptCurr.x, ptCurr.y, ptCurr.z, {}, nStat) || nStat != 0) { + bOutstroke = true ; + break ; + } + } + // in caso di attraversamento dell'area proibita + if ( bOutstroke) { + // chiamo funzione script OnSpecialRapidMove per avere nuovo punto intermedio + DBLVECTOR vAxNew ; + bool bModif = false ; + bool bOk = ( SpecialMoveRapid( vAxVal, vAxIni, vAxNew, bModif) && bModif) ; + // inserisco il nuovo punto alla fine della lavorazione precedente + if ( bOk) + vAxVal = vAxNew ; + if ( ! pPrevOp->AddSpecialRise( vAxVal, bOk)) + return false ; + } } } + // se robot + else if ( m_pMchMgr->GetCurrIsRobot()) { + // recupero dati posizione finale lavorazione precedente + const CamData* pCdEnd = pPrevOp->GetFinalCamData( false) ; + const Point3d& ptPEnd = pCdEnd->GetEndPoint() ; + const Vector3d& vtDirEnd = pCdEnd->GetToolDir() ; + const Vector3d& vtAuxEnd = pCdEnd->GetAuxDir() ; + // recupero dati posizione iniziale lavorazione corrente + const CamData* pCdStart = GetInitialCamData( false) ; + const Point3d& ptPStart = pCdStart->GetEndPoint() ; + const Vector3d& vtDirStart = pCdStart->GetToolDir() ; + const Vector3d& vtAuxStart = pCdStart->GetAuxDir() ; + // verifico se collegamento diretto tra le due posizioni (muovendosi alla Z maggiore) è possibile senza collisioni + int nLowerPos = 0 ; + DBLVECTOR vAxEnd = vAxVal ; + DBLVECTOR vAxStart = vAxIni ; + if ( ptPEnd.z < ptPStart.z - 100 * EPS_SMALL) { + nLowerPos = - 1 ; + if ( ! CalcRobotAxesAbovePos( ptPEnd, vtDirEnd, vtAuxEnd, ptPStart.z - ptPEnd.z, vAxEnd)) + return false ; + } + else if ( ptPEnd.z > ptPStart.z + 100 * EPS_SMALL) { + nLowerPos = 1 ; + if ( ! CalcRobotAxesAbovePos( ptPStart, vtDirStart, vtAuxStart, ptPEnd.z - ptPStart.z, vAxStart)) + return false ; + } + if ( TestCollisionAvoid( vAxEnd, vAxStart)) { + if ( nLowerPos == -1) { + // aggiungo risalita a Z opportuna + if ( ! pPrevOp->AddRise( vAxVal, ptPStart.z - ptPEnd.z)) + return false ; + } + } + // altrimenti aggiungo risalita a Zmax + else { + if ( ! pPrevOp->AddRise( vAxVal)) + return false ; + // si parte da Z massima + bMaxZ = true ; + } + } + } + // altrimenti non richiesta verifica collegamento con lavorazione precedente + else { + // recupero posizione finale lavorazione precedente + if ( ! pPrevOp->GetFinalAxesValues( false, vAxVal)) + return false ; } } // altrimenti aggiungo uscita per cambio utensile alla lavorazione precedente e parto da questa @@ -2433,7 +2608,7 @@ Operation::AdjustStartEndMovements( bool bVerifyPreviousLink) // imposto l'utensile precedente per i calcoli macchina if ( ! m_pMchMgr->SetCalcTool( pPrevOp->GetToolName(), pPrevOp->GetHeadName(), pPrevOp->GetExitNbr())) return false ; - // ricalcolo risalita di lavorazione precedente (se già calcolata) + // ricalcolo risalita di lavorazione precedente (se già calcolata) if ( pPrevOp->RemoveRise()) { if ( ! pPrevOp->AddRise( vAxVal)) return false ; @@ -2491,140 +2666,43 @@ Operation::AdjustOneStartMovement( int nClPathId, int nPrevClPathId, Operation* { if ( m_pMchMgr == nullptr || m_pGeomDB == nullptr) return false ; - // elimino eventuali CLIMB già presenti + // elimino eventuali CLIMB già presenti RemoveClimb( nClPathId) ; - // recupero la prima entità di questo percorso + // recupero la prima entità di questo percorso int nEntId = m_pGeomDB->GetFirstInGroup( nClPathId) ; - // recupero i dati Cam dell'entità + // recupero i dati Cam dell'entità CamData* pCamData = GetCamData( m_pGeomDB->GetUserObj( nEntId)) ; if ( pCamData == nullptr) return false ; // recupero i valori degli assi - DBLVECTOR vAxCurr = pCamData->GetAxesVal() ; + const DBLVECTOR vAxCurr = pCamData->GetAxesVal() ; // se provengo da Z massima if ( bMaxZ) { - // copio l'entità - if ( m_pGeomDB->Copy( nEntId, GDB_ID_NULL, nEntId, GDB_AFTER) == GDB_ID_NULL) - return false ; - // recupero HomeZ - double dHomeZ ; - if ( ! m_pMchMgr->GetCurrAxisHomePos( 2, dHomeZ)) - return false ; - // aggiungo eventuale ExtraZ - const CamData* pCamDataPrev = GetClPathFinalCamData( nPrevClPathId, true) ; - const DBLVECTOR& vAxPrev = ( pCamDataPrev != nullptr ? pCamDataPrev->GetAxesVal() : DBLVECTOR()) ; - const Vector3d& vtTprev = ( pCamDataPrev != nullptr ? pCamDataPrev->GetToolDir() : V_NULL) ; - const Vector3d& vtTcurr = pCamData->GetToolDir() ; - double dExtraZ ; - if ( GetExtraZ( vAxPrev, vtTprev, vAxCurr, vtTcurr, dHomeZ, dExtraZ)) { - if ( abs( dExtraZ) > EPS_SMALL) - dHomeZ += dExtraZ ; - } - // modifico quella originale (è la prima del percorso) - m_pGeomDB->SetName( nEntId, MCH_CL_CLIMB) ; - DBLVECTOR vAxNew = vAxCurr ; - vAxNew[2] = dHomeZ ; - int nFlagNew = 2 ; - int nFlag2New = 1 ; - // eventuali aggiustamenti speciali dipendenti dalla macchina - DBLVECTOR vAxTmp = vAxNew ; - Vector3d vtTool = pCamData->GetToolDir() ; - int nFlagTmp = nFlagNew ; - int nFlag2Tmp = nFlag2New ; - bool bModif ; - if ( ! SpecialMoveZup( vAxTmp, vtTool, nFlagTmp, nFlag2Tmp, bModif)) - return false ; - if ( bModif) { - vAxNew = vAxTmp ; - pCamData->SetToolDir( vtTool) ; - Vector3d vtAux = pCamData->GetAuxDir() ; - if ( ! vtAux.IsSmall()) { - DBLVECTOR vAng( vAxNew.begin() + 3, vAxNew.end()) ; - if ( m_pMchMgr->GetCalcAuxDirFromAngles( vAng, vtAux)) - pCamData->SetAuxDir( vtAux) ; - } - nFlagNew = nFlagTmp ; - nFlag2New = nFlag2Tmp ; - } - // applico le modifiche (non forzo emissione della Z che è già massima) - pCamData->SetAxes( CamData::AS_OK, vAxNew) ; - pCamData->ChangeAxesMask( CamData::MSK_L1 | CamData::MSK_L2 | CamData::MSK_R1 | CamData::MSK_R2 | CamData::MSK_R3) ; - pCamData->SetFlag( nFlagNew) ; - pCamData->SetFlag2( nFlag2New) ; - } - // verifico se la testa interferisce con i pezzi o i bloccaggi sulla tavola - else { - // Recupero box complessivo dei grezzi attivi - BBox3d b3Raws ; - GetCurrRawsGlobBox( b3Raws) ; - // recupero se ZHome è in basso - bool bZHomeDown = GetZHomeDown() ; - // determino la Z più alta tra le due posizioni - double dTopZ = ( bZHomeDown ? min( vAxPrev[2], vAxCurr[2]) : max( vAxPrev[2], vAxCurr[2])) ; - // per il test uso posizioni temporanee con questa Z - DBLVECTOR vAxPrevTmp = vAxPrev ; vAxPrevTmp[2] = dTopZ ; - DBLVECTOR vAxCurrTmp = vAxCurr ; vAxCurrTmp[2] = dTopZ ; - // verifico se forzata risalita a Zmax - bool bForcedZMax = ForcedZmax( vAxPrevTmp, vAxCurrTmp, b3Raws) ; - // se interferisce - if ( bForcedZMax || ! TestCollisionAvoid( vAxPrevTmp, vAxCurrTmp)) { + // se centro di lavoro + if ( m_pMchMgr->GetCurrIsCenter()) { + // copio l'entità + if ( m_pGeomDB->Copy( nEntId, GDB_ID_NULL, nEntId, GDB_AFTER) == GDB_ID_NULL) + return false ; // recupero HomeZ double dHomeZ ; if ( ! m_pMchMgr->GetCurrAxisHomePos( 2, dHomeZ)) return false ; - bool bPartRise = false ; - double dSafeZ = dHomeZ ; - // se non forzata Zmax - if ( ! bForcedZMax) { - // riprovo con una Z intermedia - double dUnsafeZ = dTopZ ; - for ( int i = 1 ; i <= 3 ; ++ i) { - double dTestSafeZ = ( dUnsafeZ + dSafeZ) / 2 ; - vAxPrevTmp[2] = dTestSafeZ ; - vAxCurrTmp[2] = dTestSafeZ ; - if ( TestCollisionAvoid( vAxPrevTmp, vAxCurrTmp)) { - bPartRise = true ; - dSafeZ = dTestSafeZ ; - } - else - dUnsafeZ = dTestSafeZ ; - } - // se necessario, riprovo appena prima di Zmax - if ( ! bPartRise) { - double dOffsZ = 5 * ( bZHomeDown ? 1 : -1) ; - vAxPrevTmp[2] = dHomeZ + dOffsZ ; - vAxCurrTmp[2] = dHomeZ + dOffsZ ; - if ( TestCollisionAvoid( vAxPrevTmp, vAxCurrTmp)) { - bPartRise = true ; - dSafeZ = dHomeZ + dOffsZ ; - } - } + // aggiungo eventuale ExtraZ + const CamData* pCamDataPrev = GetClPathFinalCamData( nPrevClPathId, true) ; + const DBLVECTOR& vAxPrev = ( pCamDataPrev != nullptr ? pCamDataPrev->GetAxesVal() : DBLVECTOR()) ; + const Vector3d& vtTprev = ( pCamDataPrev != nullptr ? pCamDataPrev->GetToolDir() : V_NULL) ; + const Vector3d& vtTcurr = pCamData->GetToolDir() ; + double dExtraZ ; + if ( GetExtraZ( vAxPrev, vtTprev, vAxCurr, vtTcurr, dHomeZ, dExtraZ)) { + if ( abs( dExtraZ) > EPS_SMALL) + dHomeZ += dExtraZ ; } - // risalita sopra fine percorso precedente - DBLVECTOR vAxNew1 ; - if ( nPrevClPathId == GDB_ID_NULL) { - if ( pPrevOp == nullptr || ! pPrevOp->RemoveRise()) - return false ; - DBLVECTOR vAxPrevNoRise ; - pPrevOp->GetFinalAxesValues( false, vAxPrevNoRise) ; - double dDelta = ( ! bZHomeDown ? max( dSafeZ - vAxPrevNoRise[2], 0.) : min( dSafeZ - vAxPrevNoRise[2], 0.)) ; - if ( ! pPrevOp->AddRise( vAxNew1, dDelta, GDB_ID_NULL, bZHomeDown)) - return false ; - } - else { - double dDelta = ( ! bZHomeDown ? max( dSafeZ - vAxPrev[2], 0.) : min( dSafeZ - vAxPrev[2], 0.)) ; - if ( ! AddRise( vAxNew1, dDelta, nPrevClPathId, bZHomeDown)) - return false ; - } - // aggiungo posizione elevata prima dell'inizio del percorso corrente - if ( m_pGeomDB->Copy( nEntId, GDB_ID_NULL, nEntId, GDB_AFTER) == GDB_ID_NULL) - return false ; - // modifico l'entità originale (è la prima del percorso) + // modifico quella originale (è la prima del percorso) m_pGeomDB->SetName( nEntId, MCH_CL_CLIMB) ; DBLVECTOR vAxNew = vAxCurr ; - vAxNew[2] = vAxNew1[2] ; + vAxNew[2] = dHomeZ ; int nFlagNew = 2 ; - int nFlag2New = ( bPartRise ? 0 : 1) ; + int nFlag2New = 1 ; // eventuali aggiustamenti speciali dipendenti dalla macchina DBLVECTOR vAxTmp = vAxNew ; Vector3d vtTool = pCamData->GetToolDir() ; @@ -2645,40 +2723,94 @@ Operation::AdjustOneStartMovement( int nClPathId, int nPrevClPathId, Operation* nFlagNew = nFlagTmp ; nFlag2New = nFlag2Tmp ; } - // applico le modifiche (non forzo emissione della Z che è già alta) + // applico le modifiche (non forzo emissione della Z che è già massima) pCamData->SetAxes( CamData::AS_OK, vAxNew) ; pCamData->ChangeAxesMask( CamData::MSK_L1 | CamData::MSK_L2 | CamData::MSK_R1 | CamData::MSK_R2 | CamData::MSK_R3) ; pCamData->SetFlag( nFlagNew) ; pCamData->SetFlag2( nFlag2New) ; } - // altrimenti - else { - // se Z pressochè uguali non devo fare alcunché - if ( abs( vAxCurr[2] - vAxPrev[2]) <= 100 * EPS_SMALL) - return true ; - // se Z corrente maggiore della precedente o viceversa se HomeZ sotto - else if ( ( ! bZHomeDown && vAxCurr[2] > vAxPrev[2]) || - ( bZHomeDown && vAxCurr[2] < vAxPrev[2]) ) { - // se vengo da precedente lavorazione non devo fare alcunché - if ( nPrevClPathId == GDB_ID_NULL) - return true ; - // aggiungo risalita alla fine del precedente percorso - DBLVECTOR vAxNew ; - double dDelta = ( ! bZHomeDown ? max( vAxCurr[2] - vAxPrev[2], 0.) : min( vAxCurr[2] - vAxPrev[2], 0.)) ; - if ( ! AddRise( vAxNew, dDelta, nPrevClPathId, bZHomeDown)) + // se altrimenti robot + else if ( m_pMchMgr->GetCurrIsRobot()) { + // inserisco la posizione iniziale a Zmax + if ( ! AddRobotClimb( nEntId, -1)) + return false ; + } + } + // verifico se la testa interferisce con i pezzi o i bloccaggi sulla tavola + else { + // se centro di lavoro + if ( m_pMchMgr->GetCurrIsCenter()) { + // Recupero box complessivo dei grezzi attivi + BBox3d b3Raws ; + GetCurrRawsGlobBox( b3Raws) ; + // recupero se ZHome è in basso + bool bZHomeDown = GetZHomeDown() ; + // determino la Z più alta tra le due posizioni + double dTopZ = ( bZHomeDown ? min( vAxPrev[2], vAxCurr[2]) : max( vAxPrev[2], vAxCurr[2])) ; + // per il test uso posizioni temporanee con questa Z + DBLVECTOR vAxPrevTmp = vAxPrev ; vAxPrevTmp[2] = dTopZ ; + DBLVECTOR vAxCurrTmp = vAxCurr ; vAxCurrTmp[2] = dTopZ ; + // verifico se forzata risalita a Zmax + bool bForcedZMax = ForcedZmax( vAxPrevTmp, vAxCurrTmp, b3Raws) ; + // se interferisce + if ( bForcedZMax || ! TestCollisionAvoid( vAxPrevTmp, vAxCurrTmp)) { + // recupero HomeZ + double dHomeZ ; + if ( ! m_pMchMgr->GetCurrAxisHomePos( 2, dHomeZ)) return false ; - } - // altrimenti Z corrente minore della precedente - else { + bool bPartRise = false ; + double dSafeZ = dHomeZ ; + // se non forzata Zmax + if ( ! bForcedZMax) { + // riprovo con una Z intermedia + double dUnsafeZ = dTopZ ; + for ( int i = 1 ; i <= 3 ; ++ i) { + double dTestSafeZ = ( dUnsafeZ + dSafeZ) / 2 ; + vAxPrevTmp[2] = dTestSafeZ ; + vAxCurrTmp[2] = dTestSafeZ ; + if ( TestCollisionAvoid( vAxPrevTmp, vAxCurrTmp)) { + bPartRise = true ; + dSafeZ = dTestSafeZ ; + } + else + dUnsafeZ = dTestSafeZ ; + } + // se necessario, riprovo appena prima di Zmax + if ( ! bPartRise) { + double dOffsZ = 5 * ( bZHomeDown ? 1 : -1) ; + vAxPrevTmp[2] = dHomeZ + dOffsZ ; + vAxCurrTmp[2] = dHomeZ + dOffsZ ; + if ( TestCollisionAvoid( vAxPrevTmp, vAxCurrTmp)) { + bPartRise = true ; + dSafeZ = dHomeZ + dOffsZ ; + } + } + } + // risalita sopra fine percorso precedente + DBLVECTOR vAxNew1 ; + if ( nPrevClPathId == GDB_ID_NULL) { + if ( pPrevOp == nullptr || ! pPrevOp->RemoveRise()) + return false ; + DBLVECTOR vAxPrevNoRise ; + pPrevOp->GetFinalAxesValues( false, vAxPrevNoRise) ; + double dDelta = ( ! bZHomeDown ? max( dSafeZ - vAxPrevNoRise[2], 0.) : min( dSafeZ - vAxPrevNoRise[2], 0.)) ; + if ( ! pPrevOp->AddRise( vAxNew1, dDelta, GDB_ID_NULL, bZHomeDown)) + return false ; + } + else { + double dDelta = ( ! bZHomeDown ? max( dSafeZ - vAxPrev[2], 0.) : min( dSafeZ - vAxPrev[2], 0.)) ; + if ( ! AddRise( vAxNew1, dDelta, nPrevClPathId, bZHomeDown)) + return false ; + } // aggiungo posizione elevata prima dell'inizio del percorso corrente if ( m_pGeomDB->Copy( nEntId, GDB_ID_NULL, nEntId, GDB_AFTER) == GDB_ID_NULL) return false ; - // modifico l'entità originale (è la prima del percorso) + // modifico l'entità originale (è la prima del percorso) m_pGeomDB->SetName( nEntId, MCH_CL_CLIMB) ; DBLVECTOR vAxNew = vAxCurr ; - vAxNew[2] = vAxPrev[2] ; + vAxNew[2] = vAxNew1[2] ; int nFlagNew = 2 ; - int nFlag2New = 0 ; + int nFlag2New = ( bPartRise ? 0 : 1) ; // eventuali aggiustamenti speciali dipendenti dalla macchina DBLVECTOR vAxTmp = vAxNew ; Vector3d vtTool = pCamData->GetToolDir() ; @@ -2699,12 +2831,151 @@ Operation::AdjustOneStartMovement( int nClPathId, int nPrevClPathId, Operation* nFlagNew = nFlagTmp ; nFlag2New = nFlag2Tmp ; } - // applico le modifiche (non forzo emissione della Z che è già alta) + // applico le modifiche (non forzo emissione della Z che è già alta) pCamData->SetAxes( CamData::AS_OK, vAxNew) ; pCamData->ChangeAxesMask( CamData::MSK_L1 | CamData::MSK_L2 | CamData::MSK_R1 | CamData::MSK_R2 | CamData::MSK_R3) ; pCamData->SetFlag( nFlagNew) ; pCamData->SetFlag2( nFlag2New) ; } + // altrimenti + else { + // se Z pressochè uguali non devo fare alcunché + if ( abs( vAxCurr[2] - vAxPrev[2]) <= 100 * EPS_SMALL) + return true ; + // se Z corrente maggiore della precedente o viceversa se HomeZ sotto + else if ( ( ! bZHomeDown && vAxCurr[2] > vAxPrev[2]) || + ( bZHomeDown && vAxCurr[2] < vAxPrev[2]) ) { + // se vengo da precedente lavorazione non devo fare alcunché + if ( nPrevClPathId == GDB_ID_NULL) + return true ; + // aggiungo risalita alla fine del precedente percorso + DBLVECTOR vAxNew ; + double dDelta = ( ! bZHomeDown ? max( vAxCurr[2] - vAxPrev[2], 0.) : min( vAxCurr[2] - vAxPrev[2], 0.)) ; + if ( ! AddRise( vAxNew, dDelta, nPrevClPathId, bZHomeDown)) + return false ; + } + // altrimenti Z corrente minore della precedente + else { + // aggiungo posizione elevata prima dell'inizio del percorso corrente + if ( m_pGeomDB->Copy( nEntId, GDB_ID_NULL, nEntId, GDB_AFTER) == GDB_ID_NULL) + return false ; + // modifico l'entità originale (è la prima del percorso) + m_pGeomDB->SetName( nEntId, MCH_CL_CLIMB) ; + DBLVECTOR vAxNew = vAxCurr ; + vAxNew[2] = vAxPrev[2] ; + int nFlagNew = 2 ; + int nFlag2New = 0 ; + // eventuali aggiustamenti speciali dipendenti dalla macchina + DBLVECTOR vAxTmp = vAxNew ; + Vector3d vtTool = pCamData->GetToolDir() ; + int nFlagTmp = nFlagNew ; + int nFlag2Tmp = nFlag2New ; + bool bModif ; + if ( ! SpecialMoveZup( vAxTmp, vtTool, nFlagTmp, nFlag2Tmp, bModif)) + return false ; + if ( bModif) { + vAxNew = vAxTmp ; + pCamData->SetToolDir( vtTool) ; + Vector3d vtAux = pCamData->GetAuxDir() ; + if ( ! vtAux.IsSmall()) { + DBLVECTOR vAng( vAxNew.begin() + 3, vAxNew.end()) ; + if ( m_pMchMgr->GetCalcAuxDirFromAngles( vAng, vtAux)) + pCamData->SetAuxDir( vtAux) ; + } + nFlagNew = nFlagTmp ; + nFlag2New = nFlag2Tmp ; + } + // applico le modifiche (non forzo emissione della Z che è già alta) + pCamData->SetAxes( CamData::AS_OK, vAxNew) ; + pCamData->ChangeAxesMask( CamData::MSK_L1 | CamData::MSK_L2 | CamData::MSK_R1 | CamData::MSK_R2 | CamData::MSK_R3) ; + pCamData->SetFlag( nFlagNew) ; + pCamData->SetFlag2( nFlag2New) ; + } + } + } + // se robot + else if ( m_pMchMgr->GetCurrIsRobot()) { + // recupero dati posizione finale lavorazione precedente + const CamData* pCamDataPrev = GetClPathFinalCamData( nPrevClPathId, true) ; + if ( nPrevClPathId == GDB_ID_NULL && pPrevOp != nullptr) + pCamDataPrev = pPrevOp->GetFinalCamData( true) ; + if ( pCamDataPrev == nullptr) + return false ; + const DBLVECTOR& vAxPrev = pCamDataPrev->GetAxesVal() ; + const Point3d& ptPEnd = pCamDataPrev->GetEndPoint() ; + const Vector3d& vtDirEnd = pCamDataPrev->GetToolDir() ; + const Vector3d& vtAuxEnd = pCamDataPrev->GetAuxDir() ; + // recupero dati posizione iniziale lavorazione corrente + const CamData* CdStart = GetInitialCamData( false) ; + // vAxCurr già recuperati sopra + const Point3d& ptPStart = pCamData->GetEndPoint() ; + const Vector3d& vtDirStart = pCamData->GetToolDir() ; + const Vector3d& vtAuxStart = pCamData->GetAuxDir() ; + // verifico se collegamento diretto tra le due posizioni (muovendosi alla Z maggiore) è possibile senza collisioni + int nLowerPos = 0 ; + DBLVECTOR vAxEnd = vAxPrev ; + DBLVECTOR vAxStart = vAxCurr ; + if ( ptPEnd.z < ptPStart.z - 100 * EPS_SMALL) { + nLowerPos = - 1 ; + if ( ! CalcRobotAxesAbovePos( ptPEnd, vtDirEnd, vtAuxEnd, ptPStart.z - ptPEnd.z, vAxEnd)) + return false ; + } + else if ( ptPEnd.z > ptPStart.z + 100 * EPS_SMALL) { + nLowerPos = 1 ; + if ( ! CalcRobotAxesAbovePos( ptPStart, vtDirStart, vtAuxStart, ptPEnd.z - ptPStart.z, vAxStart)) + return false ; + } + if ( TestCollisionAvoid( vAxEnd, vAxStart)) { + // se precedente più basso + if ( nLowerPos == -1) { + // se precedente appartiene ad altra operazione + if ( nPrevClPathId == GDB_ID_NULL) { + // elimino eventuali risalite finali + if ( pPrevOp == nullptr || ! pPrevOp->RemoveRise()) + return false ; + // aggiungo risalita a Z opportuna alla fine del percorso precedente + DBLVECTOR vAxNew = vAxPrev ; + if ( ! pPrevOp->AddRise( vAxNew, ptPStart.z - ptPEnd.z)) + return false ; + } + // altrimenti precedente appartiene alla stessa operazione + else { + // eventuali risalite finali già eliminate dal chiamante + // aggiungo risalita a Z opportuna alla fine del percorso precedente + DBLVECTOR vAxNew = vAxPrev ; + if ( ! AddRise( vAxNew, ptPStart.z - ptPEnd.z, nPrevClPathId)) + return false ; + } + } + // se corrente più basso + else if ( nLowerPos == 1) { + // inserisco la posizione a Z alta + if ( ! AddRobotClimb( nEntId, ptPEnd.z - ptPStart.z)) + return false ; + } + // altrimenti non devo fare alcunché + } + // altrimenti aggiungo risalita a Zmax + else { + // se precedente appartiene ad altra operazione + if ( nPrevClPathId == GDB_ID_NULL) { + // elimino eventuali risalite finali + if ( pPrevOp == nullptr || ! pPrevOp->RemoveRise()) + return false ; + // aggiungo risalita a Z max alla fine del percorso precedente + DBLVECTOR vAxNew = vAxPrev ; + if ( ! pPrevOp->AddRise( vAxNew)) + return false ; + } + // altrimenti precedente appartiene alla stessa operazione + else { + // eventuali risalite finali già eliminate dal chiamante + // aggiungo risalita a Z opportuna alla fine del percorso precedente + DBLVECTOR vAxNew = vAxPrev ; + if ( ! AddRise( vAxNew, -1, nPrevClPathId)) + return false ; + } + } } } @@ -2736,7 +3007,7 @@ Operation::RemoveClimb( int nClPathId) { if ( m_pMchMgr == nullptr || m_pGeomDB == nullptr) return false ; - // elimino tutte le entità CLIMB all'inizio del percorso + // elimino tutte le entità CLIMB all'inizio del percorso int nId = m_pGeomDB->GetFirstNameInGroup( nClPathId, MCH_CL_CLIMB) ; while ( nId != GDB_ID_NULL) { m_pGeomDB->Erase( nId) ; @@ -2767,7 +3038,7 @@ Operation::AddRise( DBLVECTOR& vAxVal, double dDelta, int nClPathId, bool bZHome if ( nClPathId == GDB_ID_NULL) return false ; } - // recupero l'ultima entità del percorso + // recupero l'ultima entità del percorso int nEntId = m_pGeomDB->GetLastInGroup( nClPathId) ; if ( nEntId == GDB_ID_NULL) return false ; @@ -2798,60 +3069,107 @@ Operation::AddRise( DBLVECTOR& vAxVal, double dDelta, int nClPathId, bool bZHome return false ; // recupero i valori degli assi vAxVal = pCam->GetAxesVal() ; - if ( vAxVal.size() < 3) - return false ; // flag per tipo di movimento in rapido int nFlag = 0 ; int nFlag2 = 0 ; - // se delta positivo lo uso come incremento di Z - if ( dDelta > 0 || bZHomeDown) { - vAxVal[2] += dDelta ; - nFlag = 0 ; // movimento standard - } - // altrimenti uso la Z home - else { - // recupero posizione home - DBLVECTOR vAxHome ; - m_pMchMgr->GetAllCurrAxesHomePos( vAxHome) ; - vAxVal[2] = vAxHome[2] ; - nFlag = 3 ; // movimento a Zmax - nFlag2 = 1 ; - } - // eventuale aggiustamenti speciali dipendenti dalla macchina - DBLVECTOR vAxTmp = vAxVal ; - Vector3d vtTool = pCam->GetToolDir() ; - int nFlagTmp = nFlag ; - int nFlag2Tmp = nFlag2 ; - bool bModif ; - if ( ! SpecialMoveZup( vAxTmp, vtTool, nFlagTmp, nFlag2Tmp, bModif)) - return false ; - if ( bModif) { - vAxVal = vAxTmp ; - pCam->SetToolDir( vtTool) ; - Vector3d vtAux = pCam->GetAuxDir() ; - if ( ! vtAux.IsSmall()) { - DBLVECTOR vAng( vAxVal.begin() + 3, vAxVal.end()) ; - if ( m_pMchMgr->GetCalcAuxDirFromAngles( vAng, vtAux)) - pCam->SetAuxDir( vtAux) ; + bool bOk = false ; + // se centro di lavoro + if ( m_pMchMgr->GetCurrIsCenter()) { + // devono esserci almeno i tre assi lineari + if ( vAxVal.size() < 3) + return false ; + // se delta positivo lo uso come incremento di Z + if ( dDelta > 0 || bZHomeDown) { + vAxVal[2] += dDelta ; + nFlag = 0 ; // movimento standard + } + // altrimenti uso la Z home + else { + // recupero posizione home + DBLVECTOR vAxHome ; + m_pMchMgr->GetAllCurrAxesHomePos( vAxHome) ; + vAxVal[2] = vAxHome[2] ; + nFlag = 3 ; // movimento a Zmax + nFlag2 = 1 ; + } + // eventuale aggiustamenti speciali dipendenti dalla macchina + DBLVECTOR vAxTmp = vAxVal ; + Vector3d vtTool = pCam->GetToolDir() ; + int nFlagTmp = nFlag ; + int nFlag2Tmp = nFlag2 ; + bool bModif ; + if ( ! SpecialMoveZup( vAxTmp, vtTool, nFlagTmp, nFlag2Tmp, bModif)) + return false ; + if ( bModif) { + vAxVal = vAxTmp ; + pCam->SetToolDir( vtTool) ; + Vector3d vtAux = pCam->GetAuxDir() ; + if ( ! vtAux.IsSmall()) { + DBLVECTOR vAng( vAxVal.begin() + 3, vAxVal.end()) ; + if ( m_pMchMgr->GetCalcAuxDirFromAngles( vAng, vtAux)) + pCam->SetAuxDir( vtAux) ; + } + nFlag = nFlagTmp ; + nFlag2 = nFlag2Tmp ; + } + // verifico extra-corsa asse Z + double dL1 = vAxVal[0] ; + double dL2 = vAxVal[1] ; + double dL3 = vAxVal[2] ; + DBLVECTOR vAng( vAxVal.begin() + 3, vAxVal.end()) ; + int nStat ; + bOk = ( m_pMchMgr->VerifyOutstroke( dL1, dL2, dL3, vAng, false, nStat) && nStat == 0) ; + // assegno i dati + pCam->SetAxes( ( bOk ? CamData::AS_OK : CamData::AS_OUTSTROKE), vAxVal) ; + pCam->SetMoveType( 0) ; + pCam->SetFeed( 0) ; + pCam->SetFlag( nFlag) ; + pCam->SetFlag2( nFlag2) ; + // associo questo oggetto a quello geometrico + m_pGeomDB->SetUserObj( nId, Release( pCam)) ; + } + // se altrimenti robot + else if ( m_pMchMgr->GetCurrIsRobot()) { + // devono esserci i sei assi rotanti + if ( vAxVal.size() < 6) + return false ; + // verifico se richiesta Zmax + bool bZmax = ( dDelta < 0) ; + // calcolo gli assi (compresa verifica corse) + double dNewDeltaZ ; + Point3d ptEnd = pCamData->GetEndPoint() ; + Vector3d vtTool = pCamData->GetToolDir() ; + Vector3d vtAux = pCamData->GetAuxDir() ; + bOk = CalcRobotAxesAbovePos( ptEnd, vtTool, vtAux, dDelta, vAxVal, &dNewDeltaZ) ; + // assegno i dati + pCam->SetAxes( ( bOk ? CamData::AS_OK : CamData::AS_OUTSTROKE), vAxVal) ; + pCam->SetMoveType( 0) ; + pCam->SetFeed( 0) ; + pCam->SetFlag( ( bZmax ? 3 : 0)) ; + pCam->SetFlag2( ( bZmax ? 1 : 0)) ; + // associo questo oggetto a quello geometrico + m_pGeomDB->SetUserObj( nId, Release( pCam)) ; + // aggiungo eventuali posizioni intermedie per garantire movimento lineare entro tolleranze + int nStep = 1 + int( dNewDeltaZ / MAX_ROBOT_G0_LEN) ; + double dStep = dNewDeltaZ / nStep ; + for ( int i = 1 ; i < nStep ; ++ i) { + // copio l'entità + int nNewId = m_pGeomDB->Copy( nId, GDB_ID_NULL, nId, GDB_BEFORE) ; + if ( nNewId == GDB_ID_NULL) + return false ; + // calcolo la posizione sopra il punto a Zmax + DBLVECTOR vAxNew = vAxVal ; + if ( ! CalcRobotAxesAbovePos( ptEnd, vtTool, vtAux, i * dStep, vAxNew)) + return false ; + // modifico l'entità copiata + CamData* pCdNew = GetCamData( m_pGeomDB->GetUserObj( nNewId)) ; + if ( pCdNew == nullptr) + return false ; + pCdNew->SetAxes( CamData::AS_OK, vAxNew) ; + pCdNew->SetFlag( 0) ; + pCdNew->SetFlag2( 0) ; } - nFlag = nFlagTmp ; - nFlag2 = nFlag2Tmp ; } - // verifico extra-corsa dell'asse Z - double dL1 = vAxVal[0] ; - double dL2 = vAxVal[1] ; - double dL3 = vAxVal[2] ; - DBLVECTOR vAng( vAxVal.begin() + 3, vAxVal.end()) ; - int nStat ; - bool bOk = ( m_pMchMgr->VerifyOutstroke( dL1, dL2, dL3, vAng, false, nStat) && nStat == 0) ; - // assegno i dati - pCam->SetAxes( ( bOk ? CamData::AS_OK : CamData::AS_OUTSTROKE), vAxVal) ; - pCam->SetMoveType( 0) ; - pCam->SetFeed( 0) ; - pCam->SetFlag( nFlag) ; - pCam->SetFlag2( nFlag2) ; - // associo questo oggetto a quello geometrico - m_pGeomDB->SetUserObj( nId, Release( pCam)) ; return bOk ; } @@ -2877,7 +3195,7 @@ Operation::AddSpecialRise( const DBLVECTOR& vAxVal, bool bOk, int nClPathId, int if ( nClPathId == GDB_ID_NULL) return false ; } - // recupero l'ultima entità del percorso + // recupero l'ultima entità del percorso int nEntId = m_pGeomDB->GetLastInGroup( nClPathId) ; if ( nEntId == GDB_ID_NULL) return false ; @@ -2933,13 +3251,13 @@ Operation::RemoveRise( int nClPathId) if ( nClPathId == GDB_ID_NULL) return false ; } - // elimino tutte le entità RISE alla fine del percorso + // elimino tutte le entità RISE alla fine del percorso int nId = m_pGeomDB->GetFirstNameInGroup( nClPathId, MCH_CL_RISE) ; while ( nId != GDB_ID_NULL) { m_pGeomDB->Erase( nId) ; nId = m_pGeomDB->GetFirstNameInGroup( nClPathId, MCH_CL_RISE) ; } - // elimino a maggior ragione eventuali entità HOME + // elimino a maggior ragione eventuali entità HOME nId = m_pGeomDB->GetFirstNameInGroup( nClPathId, MCH_CL_HOME) ; while ( nId != GDB_ID_NULL) { m_pGeomDB->Erase( nId) ; @@ -2961,7 +3279,7 @@ Operation::AddHome( void) return false ; // recupero l'ultimo percorso int nLastPxClId = m_pGeomDB->GetLastGroupInGroup( nClId) ; - // recupero l'ultima entità del percorso di nome RISE + // recupero l'ultima entità del percorso di nome RISE int nEntId = m_pGeomDB->GetLastNameInGroup( nLastPxClId, MCH_CL_RISE) ; if ( nEntId == GDB_ID_NULL) return false ; @@ -3012,7 +3330,7 @@ Operation::RemoveHome( void) return false ; // recupero l'ultimo percorso CL int nClPathId = m_pGeomDB->GetLastGroupInGroup( nClId) ; - // elimino tutte le entità HOME alla fine del percorso + // elimino tutte le entità HOME alla fine del percorso int nId = m_pGeomDB->GetFirstNameInGroup( nClPathId, MCH_CL_HOME) ; while ( nId != GDB_ID_NULL) { m_pGeomDB->Erase( nId) ; @@ -3026,7 +3344,7 @@ Operation::RemoveHome( void) bool Operation::RemoveClimbRiseHome( void) { - // elimino le entità CLIMB, RISE e HOME delle diverse path dell'operazione + // elimino le entità CLIMB, RISE e HOME delle diverse path dell'operazione int nClId = m_pGeomDB->GetFirstNameInGroup( m_nOwnerId, MCH_CL) ; int nClPathId = m_pGeomDB->GetFirstGroupInGroup( nClId) ; while ( nClPathId != GDB_ID_NULL) { @@ -3037,6 +3355,110 @@ Operation::RemoveClimbRiseHome( void) return ( nClId != GDB_ID_NULL) ; } +//---------------------------------------------------------------------------- +bool +Operation::AddRobotClimb( int nEntId, double dDeltaZ) +{ + // recupero i dati Cam dell'entità + CamData* pCdEnt = GetCamData( m_pGeomDB->GetUserObj( nEntId)) ; + if ( pCdEnt == nullptr) + return false ; + DBLVECTOR vAxVal = pCdEnt->GetAxesVal() ; + // verifico se richiesta Zmax + bool bZmax = ( dDeltaZ < 0) ; + // calcolo la posizione sopra il punto a Z richiesta + DBLVECTOR vAxClimb = vAxVal ; + double dNewDeltaZ ; + if ( ! CalcRobotAxesAbovePos( pCdEnt->GetEndPoint(), pCdEnt->GetToolDir(), pCdEnt->GetAuxDir(), dDeltaZ, vAxClimb, &dNewDeltaZ)) + return false ; + // aggiungo l'entità solo se necessario + if ( dNewDeltaZ < 100 * EPS_SMALL) + return true ; + // copio l'entità + if ( m_pGeomDB->Copy( nEntId, GDB_ID_NULL, nEntId, GDB_AFTER) == GDB_ID_NULL) + return false ; + // modifico quella originale (è la prima del percorso) + m_pGeomDB->SetName( nEntId, MCH_CL_CLIMB) ; + pCdEnt->SetAxes( CamData::AS_OK, vAxClimb) ; + pCdEnt->SetFlag( 2) ; + pCdEnt->SetFlag2( ( bZmax ? 1 : 0)) ; + // aggiungo eventuali posizioni intermedie per garantire movimento lineare entro tolleranze + int nStep = 1 + int( dNewDeltaZ / MAX_ROBOT_G0_LEN) ; + double dStep = dNewDeltaZ / nStep ; + for ( int i = 1 ; i < nStep ; ++ i) { + // copio l'entità + int nNewId = m_pGeomDB->Copy( nEntId, GDB_ID_NULL, nEntId, GDB_AFTER) ; + if ( nNewId == GDB_ID_NULL) + return false ; + // calcolo la posizione sopra il punto a Zmax + DBLVECTOR vAxNew = vAxVal ; + if ( ! CalcRobotAxesAbovePos( pCdEnt->GetEndPoint(), pCdEnt->GetToolDir(), pCdEnt->GetAuxDir(), i * dStep, vAxNew)) + return false ; + // modifico l'entità copiata + CamData* pCdNew = GetCamData( m_pGeomDB->GetUserObj( nNewId)) ; + if ( pCdNew == nullptr) + return false ; + pCdNew->SetAxes( CamData::AS_OK, vAxNew) ; + pCdNew->SetFlag( 0) ; + pCdNew->SetFlag2( 0) ; + } + return true ; +} + +//---------------------------------------------------------------------------- +bool +Operation::CalcRobotAxesAbovePos( const Point3d& ptP, const Vector3d& vtT, const Vector3d& vtA, double dDeltaZ, + DBLVECTOR& vAx, double* pdNewDeltaZ) const +{ + // Recupero macchina corrente + Machine* pMch = ( m_pMchMgr != nullptr ? m_pMchMgr->GetCurrMachine() : nullptr) ; + if ( pMch == nullptr) { + if ( pdNewDeltaZ != nullptr) + *pdNewDeltaZ = 0 ; + return false ; + } + // se DeltaZ non specificata vado a Zmax + if ( dDeltaZ < 0 ) { + // recupero quota tavola + Point3d ptRef1 ; + pMch->GetCurrTableRef1( ptRef1) ; + // recupero Zmax sopra tavola + double dMaxOnTabZ ; + if ( ! m_pGeomDB->GetInfo( pMch->GetCurrTable(), "MaxOnTabZ", dMaxOnTabZ)) + dMaxOnTabZ = 600 ; + // assegno la variazione di Z + dDeltaZ = ptRef1.z + dMaxOnTabZ - ptP.z ; + } + if ( pdNewDeltaZ != nullptr) + *pdNewDeltaZ = dDeltaZ ; + // calcolo assi macchina + Point3d ptNew( ptP.x, ptP.y, ptP.z + dDeltaZ) ; + DBLVECTOR vAng1, vAng2 ; + if ( ! m_pMchMgr->GetCurrMachine()->GetRobotAngles( ptNew, vtT, vtA, vAng1, vAng2)) + return false ; + // se non definita posizione precedente, scelgo la prima soluzione + if ( vAx.size() < 6) { + vAx = vAng1 ; + } + // altrimenti, scelgo la soluzione più vicina + else { + // salvo precedente R6 + double dPrevR6 = vAx[5] ; + // minimizzo rotazione polso + if ( abs( vAng1[3] - vAx[3]) < abs( vAng2[3] - vAx[3]) + 10 * EPS_ANG_SMALL) + vAx = vAng1 ; + else + vAx = vAng2 ; + // controllo continuità di R6 dalla posizione precedente + m_pMchMgr->GetNearestAngleInStroke( 5, dPrevR6, vAx[5]) ; + } + // verifico di essere nelle corse degli assi + int nStat ; + if ( ! m_pMchMgr->VerifyOutstroke( 0, 0, 0, vAx, false, nStat) || nStat != 0) + return false ; + return true ; +} + //---------------------------------------------------------------------------- bool Operation::CalcDeltaZForHeadRotation( const DBLVECTOR& vAxStart, const DBLVECTOR& vAxEnd, double& dDeltaZ) const @@ -3544,7 +3966,7 @@ Operation::SpecialTestCollisionAvoid( const DBLVECTOR& vAxStart, const DBLVECTOR if ( pMch == nullptr) return false ; - // assi già verificati + // assi già verificati // costanti static const string EMC_VAR = "EMC" ; // tabella variabili locali per calcolo diff --git a/Operation.h b/Operation.h index bb8dc25..9af8cfe 100644 --- a/Operation.h +++ b/Operation.h @@ -105,7 +105,7 @@ class Operation : public IUserObj bool AdjustCurveFromSurf( ICurveComposite* pCrvCompo, int nToolDir, int nFaceUse, double dToolThick, int nGrade = 3) ; bool ApproxWithArcsIfUseful( ICurveComposite* pCompo, bool bCareTempProp = false) const ; - bool ApproxWithLines( ICurveComposite* pCompo) const ; + bool ApproxWithLines( ICurveComposite* pCompo, double dMaxLen = 0) const ; bool VerifyArcs( ICurveComposite* pCompo, double dMaxAngCen = MAX_ANG_CEN) const ; bool CalcAndSetBBox( int nClId) ; @@ -129,9 +129,15 @@ class Operation : public IUserObj std::string ExtractHint( const std::string& sNotes) const ; bool SetBlockedRotAxis( const std::string& sBlockedAxis) const ; bool CalculateAxesValues( const std::string& sHint, bool bRotContOnNext = true, bool bSolChExact = false) ; + bool CalculateClPathCenterAxesValues( int nClPathId, int nLinAxes, int nRotAxes, double dRot1W, + bool bMaxDeltaR2OnFirst, bool bRotContOnNext, double dAngDeltaMinForHome, + const DBLVECTOR& vAxRotHome, DBLVECTOR& vAxRotPrec) ; bool CalculateClPathAxesValues( int nClPathId, int nLinAxes, int nRotAxes, double dRot1W, bool bMaxDeltaR2OnFirst, bool bRotContOnNext, double dAngDeltaMinForHome, const DBLVECTOR& vAxRotHome, DBLVECTOR& vAxRotPrec, int& nOutStrC) ; + bool CalculateClPathRobotAxesValues( int nClPathId, int nLinAxes, int nRotAxes, double dRot1W, + bool bMaxDeltaR2OnFirst, bool bRotContOnNext, double dAngDeltaMinForHome, + const DBLVECTOR& vAxRotHome, DBLVECTOR& vAxRotPrec) ; bool AdjustStartEndMovements( bool bVerifyPreviousLink = true) ; bool AdjustOneStartMovement( int nClPathId, int nPrevClPathId, Operation* pPrevOp, const DBLVECTOR& vAxPrev, bool bMaxZ) ; bool ToolChangeNeeded( const Operation& Op1, const Operation& Op2) const ; @@ -141,6 +147,9 @@ class Operation : public IUserObj bool RemoveRise( int nClPathId = GDB_ID_NULL) ; bool AddHome( void) ; bool RemoveClimbRiseHome( void) ; + bool AddRobotClimb( int nEntId, double dDeltaZ) ; + bool CalcRobotAxesAbovePos( const Point3d& ptP, const Vector3d& vtT, const Vector3d& vtA, double dDeltaZ, + DBLVECTOR& vAx, double* pdNewDeltaZ = nullptr) const ; bool CalcDeltaZForHeadRotation( const DBLVECTOR& vAxStart, const DBLVECTOR& vAxEnd, double& dDeltaZ) const ; bool GetExtraZ( const DBLVECTOR& vAx1, const Vector3d& vtTool1, const DBLVECTOR& vAx2, const Vector3d& vtTool2, diff --git a/Simulator.cpp b/Simulator.cpp index 4a17e19..0e5d872 100644 --- a/Simulator.cpp +++ b/Simulator.cpp @@ -568,7 +568,7 @@ Simulator::UpdateAxes( void) if ( ! pMachine->GetAllCurrAxesName( m_AxesName) || ! pMachine->GetAllCurrAxesToken( m_AxesToken)) return false ; - // Aggiorno flag invertito, il tipo e la posizione corrente degli assi macchina attivi + // Aggiorno flag di invertito, offset, tipo e posizione corrente degli assi macchina attivi for ( size_t i = 0 ; i < m_AxesName.size() ; ++ i) { bool bInvert ; m_pMachine->GetAxisInvert( m_AxesName[i], bInvert) ; @@ -1012,8 +1012,8 @@ Simulator::ManageSingleMove( int& nStatus, double& dMove) // Calcolo distanza di movimento double dSqDist = 0 ; for ( size_t i = 0 ; i < m_AxesName.size() ; ++ i) { - // coefficiente moltiplicativo per differenziare assi lineari (primi 3) e rotanti (altri) - double dSqCoeff = (( i < 3) ? 1 : SQ_COEFF_ROT_MOVE) ; + // coefficiente moltiplicativo per differenziare assi lineari e rotanti + double dSqCoeff = ( m_AxesLinear[i] ? 1 : SQ_COEFF_ROT_MOVE) ; dSqDist += dSqCoeff * ( AxesEnd[i] - m_AxesVal[i]) * ( AxesEnd[i] - m_AxesVal[i]) ; } // Calcolo distanza di movimento eventuali assi ausiliari