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

EgtMachKernel :

Browse Source 
- aggiunta la gestione delle collisioni tra oggetti e grezzi dinamici in simulazione.
This commit is contained in:
Dario Sassi
2019-08-06 09:32:37 +00:00
parent 36c161d03a
commit 5816a62373
7 changed files with 190 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Simulator.cpp
+121 -32
View File
@@ -23,6 +23,7 @@
#include "DllMain.h"
#include "/EgtDev/Include/EGkVolZmap.h"
#include "/EgtDev/Include/EGkStringUtils3d.h"
#include "/EgtDev/Include/EXeConst.h"
#include "/EgtDev/Include/EMkToolConst.h"
#include "/EgtDev/Include/EMkOperationConst.h"
#include "/EgtDev/Include/EgtPerfCounter.h"
@@ -38,6 +39,7 @@ static const double MIN_STEP = 1.0 ;
static const double MAX_STEP = 100.0 ;
static const double MID_STEP = 50.0 ;
static const double COEFF_LIM = 0.999 ;
static const double SAFEDIST_STD = 5.0 ;
//----------------------------------------------------------------------------
Simulator::Simulator( void)
@@ -61,6 +63,7 @@ Simulator::Simulator( void)
m_nAuxEInd = 0 ;
m_dVmTdOffs = 0 ;
m_dVmAdOffs = 0 ;
m_dSafeDist = SAFEDIST_STD ;
m_bEnabAxes = true ;
m_AxesName.reserve( 8) ;
m_AxesToken.reserve( 8) ;
@@ -191,6 +194,7 @@ Simulator::ResetInterpolation( void)
m_VmId.clear() ;
m_dVmTdOffs = 0 ;
m_dVmAdOffs = 0 ;
m_dSafeDist = SAFEDIST_STD ;
m_bEnabAxes = true ;
return true ;
}
@@ -208,7 +212,7 @@ Simulator::Move( int& nStatus)
// Recupero tempo impiegato (calcolo + visualizzazione precedenti)
if ( m_pPerfCnt != nullptr && ExeGetDebugLevel() >= 5) {
double dElapsed = m_pPerfCnt->Stop() ;
string sOut = "Elapsed=" + ToString( dElapsed, 0) ;
string sOut = "Elapsed=" + ToString( dElapsed, 1) ;
LOG_DBG_INFO( GetEMkLogger(), sOut.c_str())
m_pPerfCnt->Start() ;
}
@@ -935,9 +939,11 @@ Simulator::ManageSingleMove( int& nStatus, double& dMove)
m_pMachine->SetAxisPos( m_AxesName[i], dVal) ;
}
// eseguo eventuale Vmill
Point3d ptNoseF ; Vector3d vtDirF ; Vector3d vtAuxF ;
bool bOkF = GetHeadCurrPosDirAux( ptNoseF, vtDirF, vtAuxF) ;
for ( int j = 0 ; j < int( m_VmId.size()) ; ++ j) {
Point3d ptNoseF ; Vector3d vtDirF ; Vector3d vtAuxF ; Frame3d frVzmF ;
bool bOkF = GetHeadCurrPosDirAux( ptNoseF, vtDirF, vtAuxF) && m_pGeomDB->GetGlobFrame( m_VmId[j], frVzmF) ; ;
Frame3d frVzmF ;
bOkF = m_pGeomDB->GetGlobFrame( m_VmId[j], frVzmF) && bOkF ;
ExecLineVmill( m_VmId[j], ptNoseI, vtDirI, vtAuxI, vFrVzmI[j], ptNoseF, vtDirF, vtAuxF, frVzmF) ;
}
}
@@ -1016,16 +1022,20 @@ Simulator::ManageSingleMove( int& nStatus, double& dMove)
m_pMachine->SetAxisPos( m_AuxAxesName[i], dVal) ;
}
// Richiamo gestione evento movimento in corso
int nErr ;
if ( ! OnMove( nErr)) {
nStatus = ( Stopped() ? MCH_SIM_STOP : ( nErr == 1 ? MCH_SIM_COLLISION : MCH_SIM_ERR)) ;
// Se arrivato a fine interpolazione movimento, salvo posizioni
if ( m_dCoeff > COEFF_LIM && m_bEnabAxes) {
for ( size_t i = 0 ; i < m_AxesName.size() ; ++ i)
m_AxesVal[i] = AxesEnd[i] ;
// Eseguo eventuale verifica di collisione
int nVmInd, nObjInd ;
if ( ! ExecCollisionCheck( nVmInd, nObjInd)) {
// Richiamo funzione di convalida collisione
int nErr ;
if ( ! OnCollision( nVmInd, nObjInd, nErr)) {
nStatus = ( Stopped() ? MCH_SIM_STOP : ( nErr == 1 ? MCH_SIM_COLLISION : MCH_SIM_ERR)) ;
// Se arrivato a fine interpolazione movimento, salvo posizioni
if ( m_dCoeff > COEFF_LIM && m_bEnabAxes) {
for ( size_t i = 0 ; i < m_AxesName.size() ; ++ i)
m_AxesVal[i] = AxesEnd[i] ;
}
return false ;
}
return false ;
}
// Se arrivato a fine interpolazione movimento, salvo posizioni e segnalo
@@ -1099,6 +1109,48 @@ Simulator::ExecLineVmill( int nVmId, const Point3d& ptHi, const Vector3d& vtHi,
return pVZM->MillingStep( ptHiL, vtHiL, vtAiL, ptHfL, vtHfL, vtAfL) ;
}
//----------------------------------------------------------------------------
bool
Simulator::ExecCollisionCheck( int& nVmInd, int& nObjInd)
{
// se non ci sono oggetti da controllare o non c'è il grezzo lavorato, tutto bene
if ( m_CollObj.empty() || m_VmId.empty())
return true ;
// ciclo sui grezzi
for ( int i = 0 ; i < int( m_VmId.size()) ; ++ i) {
const IVolZmap* pVZM = GetVolZmap( m_pGeomDB->GetGeoObj( m_VmId[i])) ;
if ( pVZM == nullptr)
continue ;
Frame3d frZM ; m_pGeomDB->GetGlobFrame( m_VmId[i], frZM) ;
// ciclo sugli oggetti da verificare
for ( int j = 0 ; j < int( m_CollObj.size()) ; ++ j) {
bool bOk = true ;
const IGeoFrame3d* pGeoFrame = GetGeoFrame3d( m_pGeomDB->GetGeoObj( m_CollObj[j].nFrameId)) ;
if ( pGeoFrame == nullptr)
continue ;
Frame3d frObj = pGeoFrame->GetFrame() ;
Frame3d frParent ; m_pGeomDB->GetGlobFrame( m_CollObj[j].nFrameId, frParent) ;
frObj.LocToLoc( frParent, frZM) ;
if ( m_CollObj[j].nType == MCH_SIM_COB_BOX) {
Vector3d vtDiag( m_CollObj[j].dPar1, m_CollObj[j].dPar2, m_CollObj[j].dPar3) ;
bOk = pVZM->AvoidBox( frObj, vtDiag, m_dSafeDist) ;
}
else if ( m_CollObj[j].nType == MCH_SIM_COB_CYL) {
bOk = pVZM->AvoidCylinder( frObj, m_CollObj[j].dPar1, m_CollObj[j].dPar2, m_dSafeDist) ;
}
else if ( m_CollObj[j].nType == MCH_SIM_COB_SPHE) {
bOk = pVZM->AvoidSphere( frObj.Orig(), m_CollObj[j].dPar1, m_dSafeDist) ;
}
if ( ! bOk) {
nVmInd = i ;
nObjInd = j ;
return false ;
}
}
}
return true ;
}
//----------------------------------------------------------------------------
bool
Simulator::OnStart( bool bFirst)
@@ -1200,6 +1252,8 @@ bool
Simulator::OnToolSelect( const string& sTool, const string& sHead, int nExit, const string& sTcPos,
bool bFirst)
{
// reset oggetti per verifica collisione con grezzo
m_CollObj.clear() ;
// assegno il nome dell'utensile, la testa e l'uscita
if ( ! m_pMachine->LuaSetGlobVar( GLOB_VAR + GVAR_TOOL, sTool) ||
! m_pMachine->LuaSetGlobVar( GLOB_VAR + GVAR_HEAD, sHead) ||
@@ -1230,6 +1284,8 @@ Simulator::OnToolSelect( const string& sTool, const string& sHead, int nExit, co
bool bOk = m_pMachine->LuaCallFunction( ON_SIMUL_TOOL_SELECT) ;
// forzo aggiornamento posizione assi (possono essere stati mossi nello script)
UpdateAxesPos() ;
// recupero distanza di sicurezza per verifica collisione
m_pMachine->LuaGetGlobVar( GLOB_VAR + GVAR_SAFEDIST, m_dSafeDist) ;
// recupero i dati di ritorno per assi ausiliari
ResetAuxAxes() ;
int nNumAuxAxes = 0 ;
@@ -1511,26 +1567,6 @@ Simulator::OnMoveStart( const CamData* pCamData, int& nErr)
return bOk ;
}
//----------------------------------------------------------------------------
bool
Simulator::OnMove( int& nErr)
{
// reset stato di errore da script
nErr = 0 ;
// verifico esistenza funzione
if ( ! m_pMachine->LuaExistsFunction( ON_SIMUL_MOVE))
return true ;
// reset stato di errore
if ( ! m_pMachine->LuaSetGlobVar( GLOB_VAR + GVAR_ERR, nErr))
return false ;
// chiamo la funzione
if ( ! m_pMachine->LuaCallFunction( ON_SIMUL_MOVE))
return false ;
// verifico codice di errore
m_pMachine->LuaGetGlobVar( GLOB_VAR + GVAR_ERR, nErr) ;
return ( nErr == 0) ;
}
//----------------------------------------------------------------------------
bool
Simulator::OnMoveEnd( void)
@@ -1545,6 +1581,31 @@ Simulator::OnMoveEnd( void)
return bOk ;
}
//----------------------------------------------------------------------------
bool
Simulator::OnCollision( int nVmInd, int nObjInd, int& nErr)
{
// reset stato di errore da script
nErr = 0 ;
// verifico esistenza funzione
if ( ! m_pMachine->LuaExistsFunction( ON_SIMUL_COLLISION))
return true ;
// reset stato di errore
if ( ! m_pMachine->LuaSetGlobVar( GLOB_VAR + GVAR_ERR, nErr))
return false ;
// assegno identificativi grezzo e oggetto in collisione
if ( ! m_pMachine->LuaSetGlobVar( GLOB_VAR + GVAR_SIMVMID, m_VmId[nVmInd]))
return false ;
if ( ! m_pMachine->LuaSetGlobVar( GLOB_VAR + GVAR_SIMCOBIND, m_CollObj[nObjInd].nInd))
return false ;
// chiamo la funzione
if ( ! m_pMachine->LuaCallFunction( ON_SIMUL_COLLISION))
return false ;
// verifico codice di errore
m_pMachine->LuaGetGlobVar( GLOB_VAR + GVAR_ERR, nErr) ;
return ( nErr == 0) ;
}
//----------------------------------------------------------------------------
bool
Simulator::OnResetMachine( void)
@@ -1555,3 +1616,31 @@ Simulator::OnResetMachine( void)
// eseguo reset macchina
return m_pMachine->LuaCallFunction( ON_RESET_MACHINE) ;
}
//----------------------------------------------------------------------------
bool
Simulator::AddCollisionObj( int nInd, int nFrameId, int nType, double dPar1, double dPar2, double dPar3)
{
// verifiche sui parametri
if ( nInd <= 0 || nFrameId <= GDB_ID_NULL)
return false ;
switch ( nType) {
case MCH_SIM_COB_BOX :
if ( abs( dPar1) < EPS_SMALL || abs( dPar2) < EPS_SMALL || abs( dPar3) < EPS_SMALL)
return false ;
break ;
case MCH_SIM_COB_CYL :
if ( abs( dPar1) < EPS_SMALL || abs( dPar2) < EPS_SMALL)
return false ;
break ;
case MCH_SIM_COB_SPHE :
if ( abs( dPar1) < EPS_SMALL)
return false ;
break ;
default :
return false ;
}
// aggiungo al vettore
m_CollObj.emplace_back( nInd, nFrameId, nType, dPar1, dPar2, dPar3) ;
return true ;
}