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

EgtMachKernel 2.6d4 :

Browse Source 
- aggiunta gestione riferimento canonico per calcolo cinematica robot comunque disposto nello spazio
- adattamento funzioni di calcolo posizione, direzione utensile e ausiliaria da questi angoli
- in svuotatura corretta mancata esplosione di archi in rette sui percorsi di ritorno quando richiesta.
This commit is contained in:
Dario Sassi
2024-04-17 06:58:27 +02:00
parent 1f57693da8
commit 89d46baf43
6 changed files with 43 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MachineCalc.cpp
+33 -6
View File
@@ -493,6 +493,8 @@ Machine::CalculateKinematicChain( void)
m_nHeadSpecRotAxis = -1 ;
m_vCalcLinAx.clear() ;
m_vCalcRotAx.clear() ;
m_frLinAx.Reset( false) ;
m_frRobot.Reset( false) ;
m_nCalcChainType = KIN_CHAIN_NONE ;
// recupero gli assi di tavola
if ( m_nCalcTabId == GDB_ID_NULL)
@@ -535,6 +537,19 @@ Machine::CalculateKinematicChain( void)
swap( m_vCalcRotAx[0], m_vCalcRotAx[5]) ;
swap( m_vCalcRotAx[1], m_vCalcRotAx[4]) ;
swap( m_vCalcRotAx[2], m_vCalcRotAx[3]) ;
// determino il riferimento canonico del robot
if ( ! m_frRobot.Set( m_vCalcRotAx[0].ptPos, m_vCalcRotAx[0].vtDir, m_vCalcRotAx[3].vtDir)) {
LOG_ERROR( GetEMkLogger(), "Robot with errors in Rotary Axes : impossible canonic frame")
return false ;
}
// porto tutti gli assi cinematici e i dati testa nel riferimento canonico
for ( int i = 0 ; i < m_nHeadRotAxes ; ++ i) {
m_vCalcRotAx[i].ptPos.ToLoc( m_frRobot) ;
m_vCalcRotAx[i].vtDir.ToLoc( m_frRobot) ;
}
m_ptCalcPos.ToLoc( m_frRobot) ;
m_vtCalcDir.ToLoc( m_frRobot) ;
m_vtCalcADir.ToLoc( m_frRobot) ;
// direzione assi rotanti deve essere Z Y Y X Y X
if ( ! m_vCalcRotAx[0].vtDir.IsZ() ||
! m_vCalcRotAx[1].vtDir.IsY() ||
@@ -1266,6 +1281,10 @@ Machine::GetDirection( const Vector3d& vtDir, const DBLVECTOR& vAng, Vector3d& v
vtNew.Rotate( m_vCalcRotAx[i-1].vtDir, vAng[i-1]) ;
}
// nel caso di robot devo passare dal riferimento canonico robot a quello di macchina
if ( m_nCalcChainType == KIN_CHAIN_ROBOT)
vtNew.ToGlob( m_frRobot) ;
return true ;
}
@@ -1396,6 +1415,10 @@ Machine::GetTipFromPositions( double dX, double dY, double dZ, const DBLVECTOR&
else
return false ;
// nel caso di robot devo passare dal riferimento canonico robot a quello di macchina
if ( m_nCalcChainType == KIN_CHAIN_ROBOT)
ptTip.ToGlob( m_frRobot) ;
// Se richiesto ingombro totale o punto sotto del tip utensile
if ( bOverall || bBottom) {
// calcolo la direzione fresa
@@ -2050,13 +2073,17 @@ Machine::GetRobotAngles( const Point3d& ptP, const Vector3d& vtDirT, const Vecto
// pulisco il risultato
vAng1.clear() ;
vAng2.clear() ;
// porto i dati nel riferimento robot
Point3d ptPL = GetToLoc( ptP, m_frRobot) ;
Vector3d vtDirTL = GetToLoc( vtDirT, m_frRobot) ;
Vector3d vtDirAL = GetToLoc( vtDirA, m_frRobot) ;
// riferimento utensile
Frame3d frTool ;
frTool.Set( ORIG, vtDirT, vtDirA) ;
frTool.Set( ORIG, vtDirTL, vtDirAL) ;
// 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() ;
Point3d ptCen = ptPL + 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() +
@@ -2131,11 +2158,11 @@ Machine::GetRobotAngles( const Point3d& ptP, const Vector3d& vtDirT, const Vecto
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) {
if ( ! vtTool.GetRotation( vtDirTL, 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) {
if ( ! vtAux.GetRotation( vtDirAL, vtR6, EPS_SMALL, dAng6, bDet6) || ! bDet6) {
LOG_ERROR( GetEMkLogger(), "Error : R6 not calculable (robot)")
return false ;
}