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
EgtMachKernel.rc
BIN
View File
Binary file not shown.
Machine.cpp
+1
View File
@@ -58,6 +58,7 @@ Machine::Machine( void)
m_nHeadRotAxes = 0 ;
m_nHeadSpecRotAxis = -1 ;
m_frLinAx.Reset( false) ;
m_frRobot.Reset( false) ;
m_nCalcChainType = KIN_CHAIN_NONE ;
m_nMachineLook = MCH_LOOK_NONE ;
}
Machine.h
+1
View File
@@ -317,6 +317,7 @@ class Machine
KINAXISVECTOR m_vCalcLinAx ; // vettore assi lineari attivi per calcoli
KINAXISVECTOR m_vCalcRotAx ; // vettore assi rotanti attivi per calcoli
Frame3d m_frLinAx ; // sistema di riferimento definito dagli assi lineari
Frame3d m_frRobot ; // sistema di riferimento canonico del robot
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
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 ;
}
Operation.cpp
+1 -1
View File
@@ -42,7 +42,7 @@ using namespace std ;
//----------------------------------------------------------------------------
const double MAX_DIST_RAW = 200.0 ;
const double MAX_ROBOT_G0_LEN = 300.0 ;
const double MAX_ROBOT_G0_LEN = 299.0 ;
//----------------------------------------------------------------------------
bool
Pocketing.cpp
+7 -1
View File
@@ -4223,8 +4223,14 @@ Pocketing::CalcCircleSpiral( const Point3d& ptCen, const Vector3d& vtN, double d
return false ;
}
pRCrv->AddCurve( Release( pArc2)) ;
// inverto e eventualmente sistemo archi
// inverto
pRCrv->Invert() ;
// se necessario, approssimo con rette
if ( bSplitArcs && ! ApproxWithLines( pRCrv)) {
m_pMchMgr->SetLastError( 2419, "Error in Pocketing : Linear Approx not computable") ;
return false ;
}
// eventuale sistemazione archi
VerifyArcs( pRCrv) ;
}