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

EgtMacheKernel :

Browse Source 
- aggiunta gestione calcolo angoli di macchine con tre assi rotanti liberi (di tipo ZYZ o equivalenti) purchè fornito versore ausiliario
- aggiunta possibilità di impostare versore ausiliario tramite UserNotes (VtAuxDir=Vx,Vy,Vz) in fresature, svuotature e forature con punta singola.
This commit is contained in:
Dario Sassi
2026-04-07 17:50:13 +02:00
parent b577dfe86b
commit 83c83fb5f0
7 changed files with 217 additions and 71 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MachineCalc.cpp
+173 -65
View File
@@ -660,7 +660,7 @@ Machine::CalculateKinematicChain( void)
// 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) {
for ( int i = 0 ; i < ssize( m_vCalcLinAx) ; ++ i) {
if ( m_vCalcLinAx[i].bHead)
m_vCalcLinAx[i].nInd += m_nHeadLinAxes + m_nHeadRotAxes + 1 ;
else
@@ -891,7 +891,7 @@ Machine::BlockKinematicRotAxis( int nId, double dVal)
if ( nId == GDB_ID_NULL)
return false ;
// cerco l'asse rotante di calcolo con questo identificativo
for ( size_t i = 0 ; i < m_vCalcRotAx.size() ; ++ i) {
for ( int i = 0 ; i < ssize( m_vCalcRotAx) ; ++ i) {
if ( m_vCalcRotAx[i].nGrpId == nId) {
if ( dVal < m_vCalcRotAx[i].stroke.Min || dVal > m_vCalcRotAx[i].stroke.Max) {
m_vCalcRotAx[i].bFixed = false ;
@@ -920,7 +920,7 @@ Machine::FreeKinematicRotAxis( int nId)
if ( nId == GDB_ID_NULL)
return false ;
// cerco l'asse rotante di calcolo con questo identificativo
for ( size_t i = 0 ; i < m_vCalcRotAx.size() ; ++ i) {
for ( int i = 0 ; i < ssize( m_vCalcRotAx) ; ++ i) {
if ( m_vCalcRotAx[i].nGrpId == nId) {
m_vCalcRotAx[i].bFixed = false ;
return true ;
@@ -994,31 +994,6 @@ Machine::SetSolCh( int nScc, bool bExact)
return false ;
}
//----------------------------------------------------------------------------
bool
Machine::GetAngles( const Vector3d& vtDirT, const Vector3d& vtDirA,
int& nStat, double& dAngA1, double& dAngB1, double& dAngA2, double& dAngB2) const
{
// assegno dati
Vector3d vtDirH = m_vtCalcDir ;
Vector3d vtDirI = m_vtCalcADir ;
int nNumRotAx = 0 ;
KinAxis RotAx1 ;
if ( m_vCalcRotAx.size() >= 1) {
++ nNumRotAx ;
RotAx1 = m_vCalcRotAx[0] ;
}
KinAxis RotAx2 ;
if ( m_vCalcRotAx.size() >= 2) {
++ nNumRotAx ;
RotAx2 = m_vCalcRotAx[1] ;
}
// eseguo calcolo
return GetMyAngles( vtDirT, vtDirA, vtDirH, vtDirI, nNumRotAx, RotAx1, RotAx2,
nStat, dAngA1, dAngB1, dAngA2, dAngB2) ;
}
//----------------------------------------------------------------------------
bool
Machine::GetAngles( const Vector3d& vtDirT, const Vector3d& vtDirA,
@@ -1030,19 +1005,19 @@ Machine::GetAngles( const Vector3d& vtDirT, const Vector3d& vtDirA,
Vector3d vtDirH = m_vtCalcDir ;
Vector3d vtDirI = m_vtCalcADir ;
int nNumRotAx = 0 ;
KinAxis RotAx[2] ;
KinAxis RotAx[3] ;
INTVECTOR vBloHeAx ;
for ( size_t i = 0 ; i < m_vCalcRotAx.size() ; ++ i) {
for ( int i = 0 ; i < ssize( m_vCalcRotAx) ; ++ i) {
// se asse libero
if ( ! m_vCalcRotAx[i].bFixed) {
// verifico di non superare il limite
if ( nNumRotAx >= 2)
if ( nNumRotAx >= 3)
return false ;
// assegno l'asse
RotAx[nNumRotAx] = m_vCalcRotAx[i] ;
// se asse di testa, lo aggiorno con precedenti bloccati di testa (applicati in ordine contrario)
if ( RotAx[nNumRotAx].bHead) {
for ( size_t k = vBloHeAx.size() ; k >= 1 ; -- k) {
for ( int k = ssize( vBloHeAx) ; k >= 1 ; -- k) {
int nA = vBloHeAx[k-1] ;
RotAx[nNumRotAx].ptPos.Rotate( m_vCalcRotAx[nA].ptPos, m_vCalcRotAx[nA].vtDir, m_vCalcRotAx[nA].dFixVal) ;
RotAx[nNumRotAx].vtDir.Rotate( m_vCalcRotAx[nA].vtDir, m_vCalcRotAx[nA].dFixVal) ;
@@ -1059,7 +1034,7 @@ Machine::GetAngles( const Vector3d& vtDirT, const Vector3d& vtDirA,
vtDirTm.Rotate( m_vCalcRotAx[i].vtDir, m_vCalcRotAx[i].dFixVal) ;
vtDirAm.Rotate( m_vCalcRotAx[i].vtDir, m_vCalcRotAx[i].dFixVal) ;
// aggiorno eventuali assi già inseriti (sicuramente di tavola)
for ( size_t j = 0 ; int( j) < nNumRotAx ; ++ j) {
for ( int j = 0 ; j < nNumRotAx ; ++ j) {
RotAx[j].ptPos.Rotate( m_vCalcRotAx[i].ptPos, m_vCalcRotAx[i].vtDir, m_vCalcRotAx[i].dFixVal) ;
RotAx[j].vtDir.Rotate( m_vCalcRotAx[i].vtDir, m_vCalcRotAx[i].dFixVal) ;
}
@@ -1070,42 +1045,83 @@ Machine::GetAngles( const Vector3d& vtDirT, const Vector3d& vtDirA,
vBloHeAx.push_back( int( i)) ;
}
}
}
// aggiorno direzioni utensile e ausiliaria su testa con eventuali assi bloccati di testa (applico in ordine contrario)
for ( size_t k = vBloHeAx.size() ; k >= 1 ; -- k) {
for ( int k = ssize( vBloHeAx) ; k >= 1 ; -- k) {
int nA = vBloHeAx[k-1] ;
vtDirH.Rotate( m_vCalcRotAx[nA].vtDir, m_vCalcRotAx[nA].dFixVal) ;
vtDirI.Rotate( m_vCalcRotAx[nA].vtDir, m_vCalcRotAx[nA].dFixVal) ;
}
// eseguo calcolo
double dAngA1, dAngB1, dAngA2, dAngB2 ;
if ( ! GetMyAngles( vtDirTm, vtDirAm, vtDirH, vtDirI, nNumRotAx, RotAx[0], RotAx[1],
nStat, dAngA1, dAngB1, dAngA2, dAngB2))
return false ;
// assegno gli angoli
int nRotAxInd = 1 ;
for ( size_t i = 0 ; i < m_vCalcRotAx.size() ; ++i) {
if ( m_vCalcRotAx[i].bFixed) {
vAng1.push_back( m_vCalcRotAx[i].dFixVal) ;
vAng2.push_back( m_vCalcRotAx[i].dFixVal) ;
}
else {
if ( nRotAxInd == 1) {
vAng1.push_back( dAngA1) ;
vAng2.push_back( dAngA2) ;
// fino a due assi rotanti liberi
if ( nNumRotAx <= 2) {
// eseguo calcolo
double dAngA1, dAngB1, dAngA2, dAngB2 ;
if ( ! GetMyAngles( vtDirTm, vtDirAm, vtDirH, vtDirI, nNumRotAx, RotAx[0], RotAx[1],
nStat, dAngA1, dAngB1, dAngA2, dAngB2))
return false ;
// assegno gli angoli
int nRotAxInd = 1 ;
for ( int i = 0 ; i < ssize( m_vCalcRotAx) ; ++i) {
if ( m_vCalcRotAx[i].bFixed) {
vAng1.push_back( m_vCalcRotAx[i].dFixVal) ;
vAng2.push_back( m_vCalcRotAx[i].dFixVal) ;
}
else if ( nRotAxInd == 2) {
vAng1.push_back( dAngB1) ;
vAng2.push_back( dAngB2) ;
else {
if ( nRotAxInd == 1) {
vAng1.push_back( dAngA1) ;
vAng2.push_back( dAngA2) ;
}
else if ( nRotAxInd == 2) {
vAng1.push_back( dAngB1) ;
vAng2.push_back( dAngB2) ;
}
else
return false ;
++ nRotAxInd ;
}
else
return false ;
++ nRotAxInd ;
}
}
return true ;
}
// altrimenti tre assi rotanti liberi
else {
// verifico siano del tipo XYX (primo e terzo paralleli, secondo ortogonale a questi)
if ( ! AreSameOrOppositeVectorApprox( RotAx[0].vtDir, RotAx[2].vtDir) ||
! AreOrthoApprox( RotAx[0].vtDir, RotAx[1].vtDir))
return false ;
// eseguo calcolo
double dAngA1, dAngB1, dAngC1, dAngA2, dAngB2, dAngC2 ;
if ( ! GetMyAngles( vtDirTm, vtDirAm, vtDirH, vtDirI, RotAx[0], RotAx[1], RotAx[2],
nStat, dAngA1, dAngB1, dAngC1, dAngA2, dAngB2, dAngC2))
return false ;
// assegno gli angoli
int nRotAxInd = 1 ;
for ( int i = 0 ; i < ssize( m_vCalcRotAx) ; ++i) {
if ( m_vCalcRotAx[i].bFixed) {
vAng1.push_back( m_vCalcRotAx[i].dFixVal) ;
vAng2.push_back( m_vCalcRotAx[i].dFixVal) ;
}
else {
if ( nRotAxInd == 1) {
vAng1.push_back( dAngA1) ;
vAng2.push_back( dAngA2) ;
}
else if ( nRotAxInd == 2) {
vAng1.push_back( dAngB1) ;
vAng2.push_back( dAngB2) ;
}
else if ( nRotAxInd == 3) {
vAng1.push_back( dAngC1) ;
vAng2.push_back( dAngC2) ;
}
else
return false ;
++ nRotAxInd ;
}
}
return true ;
}
return true ;
}
//----------------------------------------------------------------------------
@@ -1318,6 +1334,98 @@ Machine::GetMyAngles( const Vector3d& vtDirT, const Vector3d& vtDirA,
return true ;
}
//----------------------------------------------------------------------------
bool
Machine::GetMyAngles( const Vector3d& vtDirT, const Vector3d& vtDirA,
const Vector3d& vtDirH, const Vector3d& vtDirI,
const KinAxis& RotAx1, const KinAxis& RotAx2, const KinAxis& RotAx3,
int& nStat, double& dAngA1, double& dAngB1, double& dAngC1, double& dAngA2, double& dAngB2, double& dAngC2) const
{
// annullo tutti gli angoli
nStat = 0 ; dAngA1 = 0 ; dAngB1 = 0 ; dAngC1 = 0 ; dAngA2 = 0 ; dAngB2 = 0 ; dAngC2 = 0 ;
// direzione fresa normalizzata
Vector3d vtDirTn = vtDirT ;
if ( ! vtDirTn.Normalize())
return false ;
// direzione ausiliaria normalizzata
Vector3d vtDirAn = vtDirA ;
if ( ! vtDirAn.Normalize())
return false ;
// riferimento fresa su testa a riposo
Frame3d frTzero ;
frTzero.Set( ORIG, vtDirH, vtDirI) ;
// riferimento fresa richiesto
Frame3d frTool ;
frTool.Set( ORIG, vtDirTn, vtDirAn) ;
// riferimento intrinseco primi due assi rotanti
Frame3d frWrist ;
frWrist.Set( ORIG, RotAx1.vtDir, RotAx2.vtDir ^ RotAx1.vtDir) ;
// direzione terzo asse rotante con testa a riposo
Vector3d vtCzero = GetToLoc( RotAx3.vtDir, frTzero) ;
// direzione richiesta terzo asse rotante
Vector3d vtCen = GetToGlob( vtCzero, frTool) ;
// calcolo primi due assi rotanti
Vector3d vtCenL = GetToLoc( vtCen, frWrist) ;
vtCenL.ToSpherical( nullptr, &dAngB1, &dAngA1) ;
if ( dAngA1 > ANG_STRAIGHT)
dAngA1 -= ANG_FULL ;
if ( dAngA1 > ANG_RIGHT) {
dAngA1 -= ANG_STRAIGHT ;
dAngB1 = -dAngB1 ;
}
else if ( dAngA1 < -ANG_RIGHT) {
dAngA1 += ANG_STRAIGHT ;
dAngB1 = -dAngB1 ;
}
dAngA2 = dAngA1 + ( dAngA1 > EPS_ANG_ZERO ? -ANG_STRAIGHT : ANG_STRAIGHT) ;
dAngB2 = -dAngB1 ;
// calcolo il terzo asse rotante del polso
Vector3d vtR3 = RotAx3.vtDir ;
vtR3.Rotate( RotAx2.vtDir, dAngB1) ;
vtR3.Rotate( RotAx1.vtDir, dAngA1) ;
Vector3d vtTool = vtDirH ;
vtTool.Rotate( RotAx2.vtDir, dAngB1) ;
vtTool.Rotate( RotAx1.vtDir, dAngA1) ;
bool bDetC1 ;
if ( ! vtTool.GetRotation( vtDirTn, vtR3, EPS_SMALL, dAngC1, bDetC1) || ! bDetC1) {
Vector3d vtAux = vtDirI ;
vtAux.Rotate( RotAx2.vtDir, dAngB1) ;
vtAux.Rotate( RotAx1.vtDir, dAngA1) ;
if ( ! vtAux.GetRotation( vtDirAn, vtR3, EPS_SMALL, dAngC1, bDetC1) || ! bDetC1) {
LOG_ERROR( GetEMkLogger(), "Error : R3 not calculable (machine)")
return false ;
}
}
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 ;
}
}
return true ;
}
//----------------------------------------------------------------------------
bool
Machine::GetPositions( const Point3d& ptP, const DBLVECTOR& vAng,
@@ -1399,7 +1507,7 @@ Machine::GetDirection( const Vector3d& vtDir, const DBLVECTOR& vAng, Vector3d& v
vtNew = vtDir ;
// ciclo sugli assi di testa dall'ultimo (prima assi di tavola, poi di testa)
for ( size_t i = m_vCalcRotAx.size() ; i >= 1 ; -- i) {
for ( int i = ssize( m_vCalcRotAx) ; i >= 1 ; -- i) {
// se asse di testa
if ( m_vCalcRotAx[i-1].bHead)
vtNew.Rotate( m_vCalcRotAx[i-1].vtDir, vAng[i-1]) ;
@@ -1426,7 +1534,7 @@ Machine::GetBackDirection( const Vector3d& vtDir, const DBLVECTOR& vAng, Vector3
vtNew = vtDir ;
// ciclo sugli assi dall'ultimo (prima assi di tavola, poi di testa)
for ( size_t i = m_vCalcRotAx.size() ; i >= 1 ; -- i) {
for ( int i = ssize( m_vCalcRotAx) ; i >= 1 ; -- i) {
// se asse di testa
if ( m_vCalcRotAx[i-1].bHead)
vtNew.Rotate( m_vCalcRotAx[i-1].vtDir, vAng[i-1]) ;
@@ -1617,7 +1725,7 @@ Machine::GetPartDirFromAngles( const Vector3d& vtPart, const DBLVECTOR& vAng, Ve
vtDir = vtPart ;
// ciclo sugli assi dal primo (solo assi di tavola)
for ( size_t i = 1 ; i <= m_vCalcRotAx.size() ; ++ i) {
for ( int i = 1 ; i <= ssize( m_vCalcRotAx) ; ++ i) {
// se asse di tavola
if ( ! m_vCalcRotAx[i-1].bHead)
vtDir.Rotate( m_vCalcRotAx[i-1].vtDir, vAng[i-1]) ;
@@ -1625,7 +1733,7 @@ Machine::GetPartDirFromAngles( const Vector3d& vtPart, const DBLVECTOR& vAng, Ve
// eseguo rotazione inversa eventuale asse rotante speciale di testa
if ( m_nHeadSpecRotAxis != -1) {
if ( m_nHeadSpecRotAxis < 0 || m_nHeadSpecRotAxis >= int( m_vCalcRotAx.size()))
if ( m_nHeadSpecRotAxis < 0 || m_nHeadSpecRotAxis >= ssize( m_vCalcRotAx))
return false ;
int i = m_nHeadSpecRotAxis ;
vtDir.Rotate( m_vCalcRotAx[i].vtDir, -vAng[i]) ;
@@ -1814,7 +1922,7 @@ Machine::VerifyOutstroke( double dX, double dY, double dZ, const DBLVECTOR& vAng
m_OutstrokeInfo.Clear() ;
// verifica degli assi lineari
DBLVECTOR vLin( 3) ; vLin[0] = dX ; vLin[1] = dY ; vLin[2] = dZ ;
for ( size_t i = 0 ; i < m_vCalcLinAx.size() && i < vLin.size() ; ++ i) {
for ( int i = 0 ; i < ssize( m_vCalcLinAx) && i < ssize( vLin) ; ++ i) {
if ( vLin[i] < m_vCalcLinAx[i].stroke.Min) {
if ( nStat == 0) {
m_OutstrokeInfo.sAxName = GetAxis( m_vCalcLinAx[i].nGrpId)->GetName() ;
@@ -1837,7 +1945,7 @@ Machine::VerifyOutstroke( double dX, double dY, double dZ, const DBLVECTOR& vAng
}
}
// verifica degli assi rotanti
for ( size_t i = 0 ; i < m_vCalcRotAx.size() && i < vAng.size() ; ++ i) {
for ( int i = 0 ; i < ssize(m_vCalcRotAx) && i < ssize( vAng) ; ++ i) {
if ( vAng[i] < m_vCalcRotAx[i].stroke.Min) {
if ( nStat == 0) {
m_OutstrokeInfo.sAxName = GetAxis( m_vCalcRotAx[i].nGrpId)->GetName() ;