diff --git a/GeoCalc.cpp b/GeoCalc.cpp index 2f0aa47..3055488 100644 --- a/GeoCalc.cpp +++ b/GeoCalc.cpp @@ -33,7 +33,7 @@ GetRotationComponent( const Vector3d& vtDir1, double dComp, const Vector3d& vtDi Vector3d vtU = vtPvZW ; if ( ! vtU.Normalize()) { // determino se equiversi o controversi - bool bEquiv = ( vtDir2 * vtRotAx) > 0 ; + bool bEquiv = ( vtDir2 * vtRotAx > 0) ; // se le componenti concordano, angolo indeterminato if ( abs( dT0w - ( bEquiv ? dComp : - dComp)) < 0.5 * SIN_EPS_ANG_SMALL) { bDet = false ; diff --git a/Machine.cpp b/Machine.cpp index ac0c035..6398656 100644 --- a/Machine.cpp +++ b/Machine.cpp @@ -66,6 +66,7 @@ Machine::Machine( void) m_frLinAx.Reset( false) ; m_frRobot.Reset( false) ; m_nCalcChainType = KIN_CHAIN_NONE ; + m_dPrevAngA = NAN ; m_nMachineLook = MCH_LOOK_NONE ; } diff --git a/Machine.h b/Machine.h index bd2e6f6..7648861 100644 --- a/Machine.h +++ b/Machine.h @@ -168,7 +168,9 @@ class Machine const Frame3d& GetCurrLinAxesFrame( void) const { return m_frLinAx ; } int GetCurrKinematicChainType( void) const - { return m_nCalcChainType ; } + { return m_nCalcChainType ; } + bool SetPrevAngA( double dAngA) const + { m_dPrevAngA = dAngA ; return true ; } bool GetAngles( const Vector3d& vtDirT, const Vector3d& vtDirA, int& nStat, DBLVECTOR& vAng1, DBLVECTOR& vAng2) const ; bool GetPositions( const Point3d& ptP, const DBLVECTOR& vAng, @@ -348,6 +350,7 @@ class Machine 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 double m_dPrevAngA ; // angolo precedente di riferimento per soluzione indeterminata 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 5d2bf1a..567313e 100644 --- a/MachineCalc.cpp +++ b/MachineCalc.cpp @@ -1384,6 +1384,15 @@ Machine::GetMyAngles( const Vector3d& vtDirT, const Vector3d& vtDirA, dAngA2 = dAngA1 + ( dAngA1 > EPS_ANG_ZERO ? -ANG_STRAIGHT : ANG_STRAIGHT) ; dAngB2 = -dAngB1 ; + // se soluzione indeterminata e suggerito angolo precedente valido + if ( abs( dAngB1) < EPS_ANG_SMALL / 2 && isfinite( m_dPrevAngA) && VerifyAngleOutstroke( 1, m_dPrevAngA)) { + nStat = 1 ; + dAngA1 = m_dPrevAngA ; + dAngB1 = 0 ; + } + else + nStat = 2 ; + // calcolo il terzo asse rotante del polso Vector3d vtR3 = RotAx3.vtDir ; vtR3.Rotate( RotAx2.vtDir, dAngB1) ; @@ -1404,22 +1413,25 @@ Machine::GetMyAngles( const Vector3d& vtDirT, const Vector3d& vtDirA, dAngC2 = dAngC1 + ( dAngC1 > EPS_ANG_ZERO ? -ANG_STRAIGHT : ANG_STRAIGHT) ; // verifiche dei limiti di corsa - nStat = 2 ; - // se non riesco ad aggiustare, elimino - if ( ! AdjustAngleInStroke( RotAx1.stroke, dAngA2) || - ! AdjustAngleInStroke( RotAx2.stroke, dAngB2) || - ! AdjustAngleInStroke( RotAx3.stroke, dAngC2)) - -- nStat ; - // se non riesco ad aggiustare, elimino - if ( ! AdjustAngleInStroke( RotAx1.stroke, dAngA1) || - ! AdjustAngleInStroke( RotAx2.stroke, dAngB1) || - ! AdjustAngleInStroke( RotAx3.stroke, dAngC1)) { - -- nStat ; - // riloco eventuale soluzione rimasta - if ( nStat >= 1) { - dAngA1 = dAngA2 ; - dAngB1 = dAngB2 ; - dAngC1 = dAngC2 ; + if ( nStat >= 2) { + // se non riesco ad aggiustare, elimino + if ( ! AdjustAngleInStroke( RotAx1.stroke, dAngA2) || + ! AdjustAngleInStroke( RotAx2.stroke, dAngB2) || + ! AdjustAngleInStroke( RotAx3.stroke, dAngC2)) + -- nStat ; + } + if ( nStat >= 1) { + // se non riesco ad aggiustare, elimino + if ( ! AdjustAngleInStroke( RotAx1.stroke, dAngA1) || + ! AdjustAngleInStroke( RotAx2.stroke, dAngB1) || + ! AdjustAngleInStroke( RotAx3.stroke, dAngC1)) { + -- nStat ; + // riloco eventuale soluzione rimasta + if ( nStat >= 1) { + dAngA1 = dAngA2 ; + dAngB1 = dAngB2 ; + dAngC1 = dAngC2 ; + } } } @@ -1873,7 +1885,7 @@ bool Machine::GetNearestAngleInStroke( int nInd, double dAngRef, double& dAng) const { // se angolo fittizio (non esiste l'asse rotante corrispondente), non c'è alcunchè da fare - if ( nInd < 0 || nInd >= int( m_vCalcRotAx.size())) + if ( nInd < 0 || nInd >= ssize( m_vCalcRotAx)) return true ; // cerco l'angolo più vicino stando nella corsa while ( dAng - dAngRef > ANG_STRAIGHT + EPS_ANG_ZERO && dAng - ANG_FULL >= m_vCalcRotAx[nInd].stroke.Min) @@ -1889,7 +1901,7 @@ bool Machine::LimitAngleToStroke( int nInd, double& dAng) const { // se angolo fittizio (non esiste l'asse rotante corrispondente), non c'è alcunchè da fare - if ( nInd < 0 || nInd >= int( m_vCalcRotAx.size())) + if ( nInd < 0 || nInd >= ssize( m_vCalcRotAx)) return true ; // se angolo fuori corsa, lo porto all'estremo più vicino dAng = Clamp( dAng, m_vCalcRotAx[nInd].stroke.Min, m_vCalcRotAx[nInd].stroke.Max) ; @@ -1901,7 +1913,7 @@ bool Machine::VerifyAngleOutstroke( int nInd, double dAng) const { // se angolo fittizio (non esiste l'asse rotante corrispondente), va bene - if ( nInd < 0 || nInd >= int( m_vCalcRotAx.size())) + if ( nInd < 0 || nInd >= ssize( m_vCalcRotAx)) return true ; // se angolo fuori corsa, errore if ( dAng < m_vCalcRotAx[nInd].stroke.Min) @@ -2380,13 +2392,9 @@ Machine::GetRobotAngles( const Point3d& ptP, const Vector3d& vtDirT, const Vecto return false ; } // deduco la posizione richiesta del centro del polso - Point3d ptCen = ptPL + m_ptWristCen.x * frTool.VersX() + - m_ptWristCen.y * frTool.VersY() + - ( m_dCalcTLen + m_ptWristCen.z) * frTool.VersZ() ; + Point3d ptCen = ptPL + GetToGlob( m_ptWristCen + m_dCalcTLen * Z_AX, frTool) ; // 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() ; + Vector3d vtCen = GetToGlob( m_vtWristRef, frTool) ; // calcolo il primo asse rotante Vector3d vtArm = ptCen - m_vCalcRotAx[0].ptPos ; double dAng1 ; bool bDet1 ; diff --git a/Operation.cpp b/Operation.cpp index c17c633..d4f17fd 100644 --- a/Operation.cpp +++ b/Operation.cpp @@ -2354,34 +2354,41 @@ Operation::CalculateMcentRotAxesValues( bool bFirst, const Vector3d& vtTool, con double dAngDeltaMinForHome, const DBLVECTOR& vAxRotHome, const DBLVECTOR& vAxRotPrec, DBLVECTOR& vAxRot) { + // macchina + Machine* pMachine = m_pMchMgr->GetCurrMachine() ; + if ( pMachine == nullptr) + return false ; // continuità su assi rotanti nei movimenti successivi al primo per tutte le lavorazioni, tranne disposizioni bool bRotContOnNext = ( GetType() != OPER_DISP) ; // massima variazione secondo asse rotante per continuità const double dMaxDeltaR2 = 30 ; + // imposto primo asse precedente (per caso con 3 rotanti liberi) + pMachine->SetPrevAngA( ( ! vAxRotPrec.empty() ? vAxRotPrec[0] : NAN)) ; // calcolo degli assi rotanti della macchina int nRStat ; DBLVECTOR vAng1, vAng2 ; - bool bROk = m_pMchMgr->GetCalcAngles( vtTool, vtAux, nRStat, vAng1, vAng2) ; + bool bROk = pMachine->GetAngles( vtTool, vtAux, nRStat, vAng1, vAng2) ; + pMachine->SetPrevAngA( NAN) ; if ( ! bROk || nRStat == 0) return false ; 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 - for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) { - m_pMchMgr->GetNearestAngleInStroke( i, vAxRotPrec[i], vAng1[i]) ; + for ( int i = 0 ; i < ssize( vAng1) ; ++ i) { + pMachine->GetNearestAngleInStroke( i, vAxRotPrec[i], vAng1[i]) ; if ( abs( vAng1[i] - vAxRotPrec[i]) > dAngDeltaMinForHome) - m_pMchMgr->GetNearestAngleInStroke( i, vAxRotHome[i], vAng1[i]) ; + pMachine->GetNearestAngleInStroke( i, vAxRotHome[i], vAng1[i]) ; } // se sol.ne indeterminata (sempre il primo asse libero), assegno il precedente, limitandolo alla corsa if ( nRStat < 0 && vAng1.size() >= 1) { - for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) { + for ( int i = 0 ; i < ssize( vAng1) ; ++ i) { // ignoro gli assi bloccati - if ( m_pMchMgr->IsKinematicRotAxisBlocked( i)) + if ( pMachine->IsKinematicRotAxisBlocked( i)) continue ; // assegno il precedente ed esco vAng1[i] = vAxRotPrec[i] ; - m_pMchMgr->LimitAngleToStroke( i, vAng1[i]) ; + pMachine->LimitAngleToStroke( i, vAng1[i]) ; break ; } } @@ -2389,19 +2396,19 @@ Operation::CalculateMcentRotAxesValues( bool bFirst, const Vector3d& vtTool, con // per movimenti successivi else { // scelgo gli angoli più vicini, per continuità non applico offset per stare nelle corse - for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) { + for ( int i = 0 ; i < ssize( vAng1) ; ++ i) { if ( bRotContOnNext) vAng1[i] = AngleNearAngle( vAng1[i], vAxRotPrec[i]) ; else - m_pMchMgr->GetNearestAngleInStroke( i, vAxRotPrec[i], vAng1[i]) ; + pMachine->GetNearestAngleInStroke( i, vAxRotPrec[i], vAng1[i]) ; if ( abs( vAng1[i] - vAxRotPrec[i]) > dAngDeltaMinForHome) - m_pMchMgr->GetNearestAngleInStroke( i, vAxRotHome[i], vAng1[i]) ; + pMachine->GetNearestAngleInStroke( i, vAxRotHome[i], vAng1[i]) ; } // se sol.ne indeterminata (sempre il primo asse libero), assegno il precedente if ( nRStat < 0 && vAng1.size() >= 1) { - for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) { + for ( int i = 0 ; i < ssize( vAng1) ; ++ i) { // ignoro gli assi bloccati - if ( m_pMchMgr->IsKinematicRotAxisBlocked( i)) + if ( pMachine->IsKinematicRotAxisBlocked( i)) continue ; // assegno il precedente ed esco vAng1[i] = vAxRotPrec[i] ; @@ -2416,23 +2423,23 @@ Operation::CalculateMcentRotAxesValues( bool bFirst, const Vector3d& vtTool, con // se primo movimento if ( bFirst) { // 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]) ; + for ( int i = 0 ; i < ssize( vAng1) ; ++ i) { + pMachine->GetNearestAngleInStroke( i, vAxRotPrec[i], vAng1[i]) ; if ( abs( vAng1[i] - vAxRotPrec[i]) > dAngDeltaMinForHome) - m_pMchMgr->GetNearestAngleInStroke( i, vAxRotHome[i], vAng1[i]) ; - m_pMchMgr->GetNearestAngleInStroke( i, vAxRotPrec[i], vAng2[i]) ; + pMachine->GetNearestAngleInStroke( i, vAxRotHome[i], vAng1[i]) ; + pMachine->GetNearestAngleInStroke( i, vAxRotPrec[i], vAng2[i]) ; if ( abs( vAng2[i] - vAxRotPrec[i]) > dAngDeltaMinForHome) - m_pMchMgr->GetNearestAngleInStroke( i, vAxRotHome[i], vAng2[i]) ; + pMachine->GetNearestAngleInStroke( i, vAxRotHome[i], vAng2[i]) ; } // se sol.ne indeterminata (sempre il primo asse libero), assegno il precedente, limitandolo alla corsa if ( nRStat < 0 && vAng1.size() >= 1) { - for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) { + for ( int i = 0 ; i < ssize( vAng1) ; ++ i) { // ignoro gli assi bloccati - if ( m_pMchMgr->IsKinematicRotAxisBlocked( i)) + if ( pMachine->IsKinematicRotAxisBlocked( i)) continue ; // assegno il precedente ed esco vAng1[i] = vAxRotPrec[i] ; - m_pMchMgr->LimitAngleToStroke( i, vAng1[i]) ; + pMachine->LimitAngleToStroke( i, vAng1[i]) ; vAng2[i] = vAng1[i] ; break ; } @@ -2440,25 +2447,25 @@ Operation::CalculateMcentRotAxesValues( bool bFirst, const Vector3d& vtTool, con } else { // scelgo gli angoli più vicini, per continuità non applico offset per stare nelle corse - for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) { + for ( int i = 0 ; i < ssize( vAng1) ; ++ i) { if ( bRotContOnNext) vAng1[i] = AngleNearAngle( vAng1[i], vAxRotPrec[i]) ; else - m_pMchMgr->GetNearestAngleInStroke( i, vAxRotPrec[i], vAng1[i]) ; + pMachine->GetNearestAngleInStroke( i, vAxRotPrec[i], vAng1[i]) ; if ( abs( vAng1[i] - vAxRotPrec[i]) > dAngDeltaMinForHome) - m_pMchMgr->GetNearestAngleInStroke( i, vAxRotHome[i], vAng1[i]) ; + pMachine->GetNearestAngleInStroke( i, vAxRotHome[i], vAng1[i]) ; if ( bRotContOnNext) vAng2[i] = AngleNearAngle( vAng2[i], vAxRotPrec[i]) ; else - m_pMchMgr->GetNearestAngleInStroke( i, vAxRotPrec[i], vAng2[i]) ; + pMachine->GetNearestAngleInStroke( i, vAxRotPrec[i], vAng2[i]) ; if ( abs( vAng2[i] - vAxRotPrec[i]) > dAngDeltaMinForHome) - m_pMchMgr->GetNearestAngleInStroke( i, vAxRotHome[i], vAng2[i]) ; + pMachine->GetNearestAngleInStroke( i, vAxRotHome[i], vAng2[i]) ; } // se sol.ne indeterminata (sempre il primo asse libero), assegno il precedente if ( nRStat < 0 && vAng1.size() >= 1) { - for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) { + for ( int i = 0 ; i < ssize( vAng1) ; ++ i) { // ignoro gli assi bloccati - if ( m_pMchMgr->IsKinematicRotAxisBlocked( i)) + if ( pMachine->IsKinematicRotAxisBlocked( i)) continue ; // assegno il precedente ed esco vAng1[i] = vAxRotPrec[i] ; @@ -2467,29 +2474,29 @@ Operation::CalculateMcentRotAxesValues( bool bFirst, const Vector3d& vtTool, con } } // verifico che le soluzioni siano nelle corse - for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) { - if ( ! m_pMchMgr->VerifyAngleOutstroke( i, vAng1[i])) + for ( int i = 0 ; i < ssize( vAng1) ; ++ i) { + if ( ! pMachine->VerifyAngleOutstroke( i, vAng1[i])) bAng1 = false ; - if ( ! m_pMchMgr->VerifyAngleOutstroke( i, vAng2[i])) + if ( ! pMachine->VerifyAngleOutstroke( i, vAng2[i])) bAng2 = false ; } } // scelgo la soluzione più vicina alla precedente double dDelta1 = 0 ; double dDelta2 = 0 ; - for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) { + for ( int i = 0 ; i < ssize( vAng1) ; ++ i) { // ignoro gli assi bloccati - if ( m_pMchMgr->IsKinematicRotAxisBlocked( i)) + if ( pMachine->IsKinematicRotAxisBlocked( i)) continue ; // calcolo i delta asse con eventuale peso bool bFirst = ( i == 0) ; - dDelta1 += abs( vAng1[i] - vAxRotPrec[i]) * ( bFirst ? m_pMchMgr->GetCalcRot1W() : 1) ; - dDelta2 += abs( vAng2[i] - vAxRotPrec[i]) * ( bFirst ? m_pMchMgr->GetCalcRot1W() : 1) ; + dDelta1 += abs( vAng1[i] - vAxRotPrec[i]) * ( bFirst ? pMachine->GetCurrRot1W() : 1) ; + dDelta2 += abs( vAng2[i] - vAxRotPrec[i]) * ( bFirst ? pMachine->GetCurrRot1W() : 1) ; } if ( bAng2 && ( dDelta2 < dDelta1 - EPS_ANG_SMALL || ! bAng1)) swap( vAng1, vAng2) ; // se imposto limite su variazione di secondo asse rotante - if ( ( m_pMchMgr->GetCalcMaxDeltaR2OnFirst() || ! bFirst) && dMaxDeltaR2 > EPS_ANG_SMALL && vAng1.size() >= 2) { + if ( ( pMachine->GetCurrCalcMaxDeltaR2OnFirst() || ! bFirst) && dMaxDeltaR2 > EPS_ANG_SMALL && vAng1.size() >= 2) { double dR2Diff1 = vAng1[1] - vAxRotPrec[1] ; double dR2Diff2 = vAng2[1] - vAxRotPrec[1] ; if ( abs( dR2Diff1) > abs( dR2Diff2) && abs( dR2Diff1) > dMaxDeltaR2) @@ -4943,6 +4950,10 @@ Operation::AddRobotClimb( int nEntId, double dDeltaZ) int nNewEntId = m_pGeomDB->Copy( nEntId, GDB_ID_NULL, nEntId, GDB_BEFORE) ; if ( nNewEntId == GDB_ID_NULL) return false ; + // aggiorno il CamData + pCdEnt = GetCamData( m_pGeomDB->GetUserObj( nNewEntId)) ; + if ( pCdEnt == nullptr) + return false ; // modifico quella originale (è la prima del percorso) m_pGeomDB->SetName( nNewEntId, MCH_CL_CLIMB) ; m_pGeomDB->RemoveInfo( nNewEntId, KEY_CL_DOUBLE) ; @@ -4954,7 +4965,7 @@ Operation::AddRobotClimb( int nEntId, double dDeltaZ) 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) ; + int nNewId = m_pGeomDB->Copy( nNewEntId, GDB_ID_NULL, nNewEntId, GDB_AFTER) ; if ( nNewId == GDB_ID_NULL) return false ; // calcolo la posizione sopra il punto a Zmax