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

Merge commit '737124b0bd167e644bb1c0131e675f515cbecb1f' into Svuotature_LeadIn/Out

Browse Source
This commit is contained in:
Riccardo Elitropi
2024-02-22 09:48:25 +01:00
parent 97a0aed32c 737124b0bd
commit ecc2c66fac
6 changed files with 85 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EgtMachKernel.rc
BIN
View File
Binary file not shown.
MachineCalc.cpp
+19 -10
View File
@@ -490,10 +490,17 @@ Machine::CalculateKinematicChain( void)
nHParId = m_pGeomDB->GetParentId( nHParId) ;
}
// se non ci sono assi, né lineari né rotanti, sicuramente errore
if ( m_nTabLinAxes == 0 && m_nHeadLinAxes == 0 &&
m_nTabRotAxes == 0 && m_nHeadRotAxes == 0) {
LOG_ERROR( GetEMkLogger(), "Errors in Axes : none have been found")
return false ;
}
// 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) {
if ( m_nTabRotAxes != 0 || m_nHeadRotAxes != 6) {
LOG_ERROR( GetEMkLogger(), "Robot with errors in Rotary Axes : number or type")
return false ;
}
@@ -501,19 +508,20 @@ Machine::CalculateKinematicChain( void)
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
// direzione assi rotanti deve essere Z Y Y X Y X
if ( ! m_vCalcRotAx[0].vtDir.IsZ() ||
! m_vCalcRotAx[1].vtDir.IsY() ||
! m_vCalcRotAx[2].vtDir.IsY() ||
! m_vCalcRotAx[3].vtDir.IsZ() ||
! m_vCalcRotAx[3].vtDir.IsX() ||
! m_vCalcRotAx[4].vtDir.IsY() ||
! m_vCalcRotAx[5].vtDir.IsZ()) {
LOG_ERROR( GetEMkLogger(), "Robot with errors in Rotary Axes : not ZYY-ZYZ")
! m_vCalcRotAx[5].vtDir.IsX()) {
LOG_ERROR( GetEMkLogger(), "Robot with errors in Rotary Axes : not ZYY-XYX")
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) {
if ( abs( m_vCalcRotAx[3].ptPos.y - m_vCalcRotAx[5].ptPos.y) > EPS_SMALL ||
abs( m_vCalcRotAx[3].ptPos.z - m_vCalcRotAx[5].ptPos.z) > EPS_SMALL ||
abs( m_vCalcRotAx[4].ptPos.z - m_vCalcRotAx[3].ptPos.z) > EPS_SMALL) {
LOG_ERROR( GetEMkLogger(), "Robot with errors in Rotary Axes : not spherical Wrist")
return false ;
}
@@ -2045,7 +2053,7 @@ Machine::GetRobotAngles( const Point3d& ptP, const Vector3d& vtDirT, const Vecto
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 dAngR3R5 = atan2( vtR3R5.z, vtR3R5.x) * 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) {
@@ -2062,11 +2070,12 @@ Machine::GetRobotAngles( const Point3d& ptP, const Vector3d& vtDirT, const Vecto
vAng1.push_back( dAng2) ;
vAng2.push_back( dAng2) ;
double dAngC = acos( dCosC) * RADTODEG ;
double dAng3 = dAngB + dAngC + dAngR3R5 ;
double dAng3 = dAngB + dAngC + dAngR3R5 - ANG_RIGHT ;
vAng1.push_back( dAng3) ;
vAng2.push_back( dAng3) ;
// calcolo i primi due assi rotanti del polso
Frame3d frWrist( m_vCalcRotAx[3].ptPos) ;
Frame3d frWrist ;
frWrist.Set( m_vCalcRotAx[3].ptPos, m_vCalcRotAx[3].vtDir, m_vCalcRotAx[4].vtDir ^ m_vCalcRotAx[3].vtDir) ;
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) ;
Operation.cpp
+2 -2
View File
@@ -4175,7 +4175,7 @@ Operation::TestCollisionAvoid( const DBLVECTOR& vAxStart, const DBLVECTOR& vAxEn
// traslo il riferimento per avere il cilindro centrato sull'asse Y in positivo
frHsol.Translate( frHsol.VersZ() * ptMin.z) ;
// verifica di collisione tra cilindro e solido
if ( pStm == nullptr || CDeCylClosedSurfTm( frHsol, max( vtDiag.x, vtDiag.y) / 2, vtDiag.z, dSafeDist, *pStm)) {
if ( pStm == nullptr || CDeCylClosedSurfTm( frHsol, max( vtDiag.x, vtDiag.y) / 2, vtDiag.z, *pStm, dSafeDist)) {
bCollide = true ;
break ;
}
@@ -4185,7 +4185,7 @@ Operation::TestCollisionAvoid( const DBLVECTOR& vAxStart, const DBLVECTOR& vAxEn
// traslo il riferimento per avere il box nel primo ottante
frHsol.Translate( frHsol.VersX() * ptMin.x + frHsol.VersY() * ptMin.y + frHsol.VersZ() * ptMin.z) ;
// verifica di collisione tra box e solido
if ( pStm == nullptr || CDeBoxClosedSurfTm( frHsol, vtDiag, dSafeDist, *pStm)) {
if ( pStm == nullptr || CDeBoxClosedSurfTm( frHsol, vtDiag, *pStm, dSafeDist)) {
bCollide = true ;
break ;
}
OutputConst.h
+2
View File
@@ -206,6 +206,8 @@ static const std::string ON_ESTIM_RAPID = "OnEstimRapid" ;
static const std::string ON_ESTIM_LINEAR = "OnEstimLinear" ;
static const std::string ON_ESTIM_ARC = "OnEstimArc" ;
// Funzioni simulazione
static const std::string ON_SIMUL_INIT = "OnSimulInit" ;
static const std::string ON_SIMUL_EXIT = "OnSimulExit" ;
static const std::string ON_SIMUL_START = "OnSimulStart" ;
static const std::string ON_SIMUL_END = "OnSimulEnd" ;
static const std::string ON_SIMUL_DISPOSITION_STARTING = "OnSimulDispositionStarting" ;
Simulator.cpp
+53 -17
View File
@@ -61,6 +61,7 @@ Simulator::Simulator( void)
m_pGeomDB = nullptr ;
m_pMachine = nullptr ;
m_pPerfCnt = nullptr ;
m_nStatus = SIS_CREATED ;
m_dStep = MID_STEP ;
m_nUiStatus = MCH_UISIM_NULL ;
m_nOpId = GDB_ID_NULL ;
@@ -92,6 +93,8 @@ Simulator::~Simulator( void)
{
// porto la macchina in posizione home
GoHome() ;
// gestione evento uscita dal simulatore
OnExit() ;
// rimuovo tavola variabili globali
m_pMachine->LuaResetGlobVar( GLOB_VAR) ;
// rimuovo performance counter
@@ -113,6 +116,7 @@ Simulator::Init( MachMgr* pMchMgr)
m_pGeomDB = m_pMchMgr->GetGeomDB() ;
m_pMachine = m_pMchMgr->GetCurrMachine() ;
m_pPerfCnt = new PerformanceCounter ;
m_nStatus = SIS_INITIALIZED ;
return true ;
}
@@ -129,8 +133,8 @@ Simulator::Start( bool bFirst)
bool bOk = true ;
// Se avvio vero
if ( bFirst) {
// Se appena entrati in simulazione
if ( m_nStatus == SIS_INITIALIZED) {
// Forzo aggiornamento attrezzaggio della macchinata
if ( ! m_pMchMgr->UpdateCurrSetup())
bOk = false ;
@@ -167,8 +171,15 @@ Simulator::Start( bool bFirst)
m_pMachine->LuaSetGlobVar( GLOB_VAR + GVAR_VER, GetEMkVer()) ;
m_pMachine->LuaSetGlobVar( GLOB_VAR + GVAR_MACHNAME, m_pMachine->GetMachineName()) ;
// Richiamo funzione su avvio simulazione
if ( ! OnStart( bFirst))
// Se appena entrati in simulazione
if ( m_nStatus == SIS_INITIALIZED) {
if ( ! OnInit())
bOk = false ;
m_nStatus = SIS_READYTOSTART ;
}
// Richiamo funzione per evento avvio simulazione
if ( ! OnProgramStart( bFirst))
bOk = false ;
// Arrivo alla preparazione per la prima lavorazione
@@ -193,6 +204,9 @@ Simulator::Start( bool bFirst)
if ( m_pPerfCnt != nullptr)
m_pPerfCnt->Start() ;
// Imposto lo stato interno
m_nStatus = ( bOk ? SIS_READYTORUN : SIS_READYTOSTART) ;
return bOk ;
}
@@ -248,7 +262,7 @@ bool
Simulator::Move( int& nStatus)
{
// Verifiche
if ( m_pGeomDB == nullptr || m_pMchMgr == nullptr || m_pMachine == nullptr) {
if ( m_pGeomDB == nullptr || m_pMchMgr == nullptr || m_pMachine == nullptr || m_nStatus != SIS_READYTORUN) {
nStatus = MCH_SIM_ERR ;
return false ;
}
@@ -292,7 +306,7 @@ Simulator::Move( int& nStatus)
return false ;
// se non ce ne sono altre, sono alla fine
if ( m_nOpId == GDB_ID_NULL) {
OnEnd() ;
OnProgramEnd() ;
nStatus = MCH_SIM_END ;
return false ;
}
@@ -1425,27 +1439,27 @@ Simulator::ExecCollisionCheck( int& nCdInd, int& nObjInd, int nMoveType)
if ( m_CollObj[j].nType == MCH_SIM_COB_BOX) {
Vector3d vtDiag( m_CollObj[j].dPar1, m_CollObj[j].dPar2, m_CollObj[j].dPar3) ;
if ( pVZM != nullptr)
bOk = pVZM->AvoidBox( frObj, vtDiag, m_dSafeDist) ;
bOk = ! pVZM->CDeBox( frObj, vtDiag, m_dSafeDist) ;
else
bOk = ! CDeBoxClosedSurfTm( frObj, vtDiag, m_dSafeDist, *pSTM) ;
bOk = ! CDeBoxClosedSurfTm( frObj, vtDiag, *pSTM, m_dSafeDist) ;
}
else if ( m_CollObj[j].nType == MCH_SIM_COB_CYL) {
if ( pVZM != nullptr)
bOk = pVZM->AvoidCylinder( frObj, m_CollObj[j].dPar1, m_CollObj[j].dPar2, m_dSafeDist) ;
bOk = ! pVZM->CDeCylinder( frObj, m_CollObj[j].dPar1, m_CollObj[j].dPar2, m_dSafeDist) ;
else
bOk = ! CDeCylClosedSurfTm( frObj, m_CollObj[j].dPar1, m_CollObj[j].dPar2, m_dSafeDist, *pSTM) ;
bOk = ! CDeCylClosedSurfTm( frObj, m_CollObj[j].dPar1, m_CollObj[j].dPar2, *pSTM, m_dSafeDist) ;
}
else if ( m_CollObj[j].nType == MCH_SIM_COB_SPHE) {
if ( pVZM != nullptr)
bOk = pVZM->AvoidSphere( frObj.Orig(), m_CollObj[j].dPar1, m_dSafeDist) ;
bOk = ! pVZM->CDeSphere( frObj.Orig(), m_CollObj[j].dPar1, m_dSafeDist) ;
else
bOk = ! CDeSpheClosedSurfTm( frObj.Orig(), m_CollObj[j].dPar1, m_dSafeDist, *pSTM) ;
bOk = ! CDeSpheClosedSurfTm( frObj.Orig(), m_CollObj[j].dPar1, *pSTM, m_dSafeDist) ;
}
else if ( m_CollObj[j].nType == MCH_SIM_COB_CONE) {
if ( pVZM != nullptr)
bOk = pVZM->AvoidConeFrustum( frObj, m_CollObj[j].dPar1, m_CollObj[j].dPar2, m_CollObj[j].dPar3, m_dSafeDist) ;
bOk = ! pVZM->CDeConeFrustum( frObj, m_CollObj[j].dPar1, m_CollObj[j].dPar2, m_CollObj[j].dPar3, m_dSafeDist) ;
else
bOk = ! CDeConeFrustumClosedSurfTm( frObj, m_CollObj[j].dPar1, m_CollObj[j].dPar2, m_CollObj[j].dPar3, m_dSafeDist, *pSTM) ;
bOk = ! CDeConeFrustumClosedSurfTm( frObj, m_CollObj[j].dPar1, m_CollObj[j].dPar2, m_CollObj[j].dPar3, *pSTM, m_dSafeDist) ;
}
}
// altrimenti poliedro
@@ -1454,7 +1468,7 @@ Simulator::ExecCollisionCheck( int& nCdInd, int& nObjInd, int nMoveType)
const ISurfTriMesh* pStmLoc = GetSurfTriMesh( SurfLoc) ;
if ( pStmLoc != nullptr) {
if ( pVZM != nullptr)
bOk = pVZM->AvoidSurfTm( *pStmLoc, m_dSafeDist) ;
bOk = ! pVZM->CDeSurfTm( *pStmLoc, m_dSafeDist) ;
else
bOk = ! CDeClosedSurfTmClosedSurfTm( *pStmLoc, *pSTM, m_dSafeDist) ;
}
@@ -1474,7 +1488,29 @@ Simulator::ExecCollisionCheck( int& nCdInd, int& nObjInd, int nMoveType)
//----------------------------------------------------------------------------
bool
Simulator::OnStart( bool bFirst)
Simulator::OnInit( void)
{
// verifico esistenza funzione
if ( ! m_pMachine->LuaExistsFunction( ON_SIMUL_INIT))
return true ;
// chiamo la funzione di ingresso nella simulazione
return m_pMachine->LuaCallFunction( ON_SIMUL_INIT) ;
}
//----------------------------------------------------------------------------
bool
Simulator::OnExit( void)
{
// verifico esistenza funzione
if ( ! m_pMachine->LuaExistsFunction( ON_SIMUL_EXIT))
return true ;
// chiamo la funzione di uscita dalla simulazione
return m_pMachine->LuaCallFunction( ON_SIMUL_EXIT) ;
}
//----------------------------------------------------------------------------
bool
Simulator::OnProgramStart( bool bFirst)
{
// assegno flag inizio simulazione
bool bOk = m_pMachine->LuaSetGlobVar( GLOB_VAR + GVAR_SIM1ST, bFirst) ;
@@ -1488,7 +1524,7 @@ Simulator::OnStart( bool bFirst)
//----------------------------------------------------------------------------
bool
Simulator::OnEnd( void)
Simulator::OnProgramEnd( void)
{
// verifico esistenza funzione
if ( ! m_pMachine->LuaExistsFunction( ON_SIMUL_END))
Simulator.h
+9 -2
View File
@@ -86,8 +86,10 @@ class Simulator
{ return ( ! m_CollObj.empty() && ! m_CdId.empty()) ; }
bool Stopped( void)
{ return ( m_nUiStatus == MCH_UISIM_STOP) ; }
bool OnStart( bool bFirst) ;
bool OnEnd( void) ;
bool OnInit( void) ;
bool OnExit( void) ;
bool OnProgramStart( bool bFirst) ;
bool OnProgramEnd( void) ;
bool OnDispositionStarting( int nOpId, int nOpInd, int nPhase,
const std::string& sTable, const Point3d& ptOri1, bool bEmpty, bool bSomeByHand) ;
bool OnDispositionStart( int nOpId, int nOpInd, int nPhase,
@@ -135,12 +137,17 @@ class Simulator
: sName( sN), sHead( sH), nExit( nE), dTdOffs( dT), dAdOffs( dA) {}
} ;
typedef std::vector<VmTool> VMTVECTOR ;
enum { SIS_CREATED = 0,
SIS_INITIALIZED = 1,
SIS_READYTOSTART = 2,
SIS_READYTORUN = 3} ;
private :
MachMgr* m_pMchMgr ; // puntatore al gestore di tutte le lavorazioni
IGeomDB* m_pGeomDB ; // puntatore al DB geometrico
Machine* m_pMachine ; // puntatore alla macchina
PerformanceCounter* m_pPerfCnt ; // timer per calcolo FPS
int m_nStatus ; // stato interno del simulatore (creato, inizializzato, pronto al movimento)
double m_dStep ; // lunghezza di riferimento per la velocità di simulazione
int m_nUiStatus ; // stato simulazione a livello utente
int m_nOpId ; // identificativo della operazione (lavoraz.) corrente