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

EgtMachKernel :

Browse Source 
- aggiunta gestione macchine di tipo Robot, le altre sono ora definite di tipo Center.
This commit is contained in:
Dario Sassi
2024-02-13 11:45:05 +01:00
parent 1179ad7e84
commit 028a5b7bba
10 changed files with 1144 additions and 491 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MachineCalc.cpp
+230 -49
View File
@@ -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 ;
}