Egalware/EgtMachKernel
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

EgtMachKernel :

Browse Source 
- corretta AddRobotClimb (aggiungeva entità in posizione non corretta)
- aggiunta gestione soluzione indeterminata con tre assi rotanti liberi.
This commit is contained in:
Dario Sassi
2026-04-09 20:17:50 +02:00
parent 40f1e051a4
commit 932700d4b2
5 changed files with 86 additions and 63 deletions
Download Patch File Download Diff File Expand all files Collapse all files
GeoCalc.cpp
+1 -1
View File
@@ -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 ;
Machine.cpp
+1
View File
@@ -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 ;
}
Machine.h
+4 -1
View File
@@ -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
MachineCalc.cpp
+33 -25
View File
@@ -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 ;
Operation.cpp
+47 -36
View File
@@ -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