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

EgtMachKernel 2.7g5 :

Browse Source 
- modifiche per controllo punto medio nel calcolo assi robot.
This commit is contained in:
Dario Sassi
2025-07-30 07:47:12 +02:00
parent 517a1021b5
commit 21459f66d7
7 changed files with 333 additions and 107 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EgtMachKernel.rc
BIN
View File
Binary file not shown.
MachMgr.h
+1 -1
View File
@@ -461,7 +461,7 @@ class MachMgr : public IMachMgr
bool GetCurrAxisMax( int nInd, double& dHome) const ;
bool GetCurrAxisMin( int nInd, double& dHome) const ;
const Frame3d& GetCurrLinAxesFrame( void) const ;
bool GetCurrIsCenter( void) const ;
bool GetCurrIsMcent( void) const ;
bool GetCurrIsRobot( void) const ;
bool ApplyRotAxisBlock( void) ;
void ClearRotAxisBlock( void)
MachMgrMachines.cpp
+2 -2
View File
@@ -741,12 +741,12 @@ MachMgr::GetCurrLinAxesFrame( void) const
//----------------------------------------------------------------------------
bool
MachMgr::GetCurrIsCenter( void) const
MachMgr::GetCurrIsMcent( void) const
{
Machine* pMch = GetCurrMachine() ;
if ( pMch == nullptr)
return false ;
return ( pMch->GetCurrKinematicChainType() == KIN_CHAIN_CENTER) ;
return ( pMch->GetCurrKinematicChainType() == KIN_CHAIN_MCENT) ;
}
//----------------------------------------------------------------------------
MachineCalc.cpp
+3 -3
View File
@@ -729,7 +729,7 @@ Machine::CalculateKinematicChain( void)
}
}
// dichiaro tipo centro di lavoro
m_nCalcChainType = KIN_CHAIN_CENTER ;
m_nCalcChainType = KIN_CHAIN_MCENT ;
return true ;
}
@@ -1348,7 +1348,7 @@ Machine::GetNoseFromPositions( double dX, double dY, double dZ, const DBLVECTOR&
return false ;
// se centro di lavoro
if ( m_nCalcChainType == KIN_CHAIN_CENTER) {
if ( m_nCalcChainType == KIN_CHAIN_MCENT) {
// 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) {
@@ -1401,7 +1401,7 @@ Machine::GetTipFromPositions( double dX, double dY, double dZ, const DBLVECTOR&
return false ;
// se centro di lavoro
if ( m_nCalcChainType == KIN_CHAIN_CENTER) {
if ( m_nCalcChainType == KIN_CHAIN_MCENT) {
// 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) {
MachineStruConst.h
+1 -1
View File
@@ -104,7 +104,7 @@ enum MchSelType { MCH_SLT_FIXEDEXITS = 0,
//----------------------------------------------------------------------------
// Tipo della catena cinematica
enum KinChainType { KIN_CHAIN_NONE = 0,
KIN_CHAIN_CENTER = 1,
KIN_CHAIN_MCENT = 1,
KIN_CHAIN_ROBOT = 2} ;
//----------------------------------------------------------------------------
Operation.cpp
+308 -88
View File
@@ -1916,11 +1916,11 @@ Operation::CalculateAxesValues( const string& sHint, bool bRotContOnNext, bool b
// calcolo il valore degli assi macchina di tutti i movimenti
bool bOk = true ;
if ( m_pMchMgr->GetCurrIsCenter()) {
if ( m_pMchMgr->GetCurrIsMcent()) {
int nClPathId = m_pGeomDB->GetFirstGroupInGroup( nClId) ;
while ( nClPathId != GDB_ID_NULL) {
if ( ! CalculateClPathCenterAxesValues( nClPathId, nLinAxes, nRotAxes, dRot1W, bMaxDeltaR2OnFirst,
bRotContOnNext, dAngDeltaMinForHome, vAxRotHome, vAxRotPrec))
if ( ! CalculateClPathMcentAxesValues( nClPathId, nLinAxes, nRotAxes, dRot1W, bMaxDeltaR2OnFirst,
bRotContOnNext, dAngDeltaMinForHome, vAxRotHome, vAxRotPrec))
bOk = false ;
nClPathId = m_pGeomDB->GetNextGroup( nClPathId) ;
}
@@ -1940,25 +1940,25 @@ Operation::CalculateAxesValues( const string& sHint, bool bRotContOnNext, bool b
//----------------------------------------------------------------------------
bool
Operation::CalculateClPathCenterAxesValues( int nClPathId, int nLinAxes, int nRotAxes, double dRot1W,
bool bMaxDeltaR2OnFirst, bool bRotContOnNext, double dAngDeltaMinForHome,
const DBLVECTOR& vAxRotHome, DBLVECTOR& vAxRotPrec)
Operation::CalculateClPathMcentAxesValues( int nClPathId, int nLinAxes, int nRotAxes, double dRot1W,
bool bMaxDeltaR2OnFirst, bool bRotContOnNext, double dAngDeltaMinForHome,
const DBLVECTOR& vAxRotHome, DBLVECTOR& vAxRotPrec)
{
DBLVECTOR vAxRotPrecOri = vAxRotPrec ;
// cancello eventuali punti aggiunti
EraseAddedPoints( nClPathId) ;
// calcolo
int nOutStrC = 0 ;
if ( ! CalculateClPathAxesValues( nClPathId, nLinAxes, nRotAxes, dRot1W, bMaxDeltaR2OnFirst,
bRotContOnNext, dAngDeltaMinForHome, vAxRotHome, vAxRotPrec, nOutStrC)) {
if ( ! MyCalculateClPathMcentAxesValues( nClPathId, nLinAxes, nRotAxes, dRot1W, bMaxDeltaR2OnFirst,
bRotContOnNext, dAngDeltaMinForHome, vAxRotHome, vAxRotPrec, nOutStrC)) {
// se attivata scelta angolo iniziale vicino ad home ed extra corsa C, provo a rifarlo disattivando
if ( dAngDeltaMinForHome < ANG_FULL && nOutStrC != 0) {
// cancello eventuali punti aggiunti
EraseAddedPoints( nClPathId) ;
// ricalcolo
m_pMchMgr->GetCurrMachine()->ResetOutstrokeInfo() ;
if ( CalculateClPathAxesValues( nClPathId, nLinAxes, nRotAxes, dRot1W, bMaxDeltaR2OnFirst,
bRotContOnNext, INFINITO, vAxRotHome, vAxRotPrec, nOutStrC))
if ( MyCalculateClPathMcentAxesValues( nClPathId, nLinAxes, nRotAxes, dRot1W, bMaxDeltaR2OnFirst,
bRotContOnNext, INFINITO, vAxRotHome, vAxRotPrec, nOutStrC))
return true ;
}
// se extracorsa dell'asse C, provo a precaricarlo al contrario
@@ -1969,8 +1969,8 @@ Operation::CalculateClPathCenterAxesValues( int nClPathId, int nLinAxes, int nRo
vAxRotPrec = vAxRotPrecOri ;
vAxRotPrec[0] = vAxRotPrecOri[0] + ( nOutStrC > 0 ? - ANG_FULL : ANG_FULL) ;
m_pMchMgr->GetCurrMachine()->ResetOutstrokeInfo() ;
if ( ! CalculateClPathAxesValues( nClPathId, nLinAxes, nRotAxes, dRot1W, bMaxDeltaR2OnFirst,
bRotContOnNext, INFINITO, vAxRotHome, vAxRotPrec, nOutStrC))
if ( ! MyCalculateClPathMcentAxesValues( nClPathId, nLinAxes, nRotAxes, dRot1W, bMaxDeltaR2OnFirst,
bRotContOnNext, INFINITO, vAxRotHome, vAxRotPrec, nOutStrC))
return false ;
}
else
@@ -1999,9 +1999,9 @@ Operation::EraseAddedPoints( int nClPathId)
//----------------------------------------------------------------------------
bool
Operation::CalculateClPathAxesValues( int nClPathId, int nLinAxes, int nRotAxes, double dRot1W,
bool bMaxDeltaR2OnFirst, bool bRotContOnNext, double dAngDeltaMinForHome,
const DBLVECTOR& vAxRotHome, DBLVECTOR& vAxRotPrec, int& nOutStrC)
Operation::MyCalculateClPathMcentAxesValues( int nClPathId, int nLinAxes, int nRotAxes, double dRot1W,
bool bMaxDeltaR2OnFirst, bool bRotContOnNext, double dAngDeltaMinForHome,
const DBLVECTOR& vAxRotHome, DBLVECTOR& vAxRotPrec, int& nOutStrC)
{
if ( m_pMchMgr == nullptr || m_pGeomDB == nullptr)
return false ;
@@ -2039,8 +2039,8 @@ Operation::CalculateClPathAxesValues( int nClPathId, int nLinAxes, int nRotAxes,
Vector3d vtAux = pCamData->GetAuxDir() ;
Vector3d vtCorr = pCamData->GetCorrDir() ;
DBLVECTOR vAxRot ;
if ( ! CalculateRotAxesValues( bFirst, vtDir, vtAux, dRot1W,
bMaxDeltaR2OnFirst, bRotContOnNext, dAngDeltaMinForHome, vAxRotHome, vAxRotPrec, vAxRot)) {
if ( ! CalculateMcentRotAxesValues( bFirst, vtDir, vtAux, dRot1W,
bMaxDeltaR2OnFirst, bRotContOnNext, dAngDeltaMinForHome, vAxRotHome, vAxRotPrec, vAxRot)) {
pCamData->SetAxes( CamData::AS_DIR_ERR, vAxVal) ;
LOG_ERROR( GetEMkLogger(), "Error : tool direction unreachable")
return false ;
@@ -2112,10 +2112,10 @@ Operation::CalculateClPathAxesValues( int nClPathId, int nLinAxes, int nRotAxes,
bMidAdded = true ;
else {
bool bAxesError ;
if ( ! VerifyLineMidPoint( ptPrec, vtDirPrec, vtAuxPrec, vtCorrPrec, vAxPrec,
ptP, vtDir, vtAux, vtCorr, vAxVal,
1, nEntId, dRot1W, nMoveType, bToolShowPrec && pCamData->GetToolShow(),
bMidAdded, bAxesError)) {
if ( ! VerifyMcentLineMidPoint( ptPrec, vtDirPrec, vtAuxPrec, vtCorrPrec, vAxPrec,
ptP, vtDir, vtAux, vtCorr, vAxVal,
1, nEntId, dRot1W, nMoveType, bToolShowPrec && pCamData->GetToolShow(),
bMidAdded, bAxesError)) {
string sOut = "Error : VerifyLineMidPoint of CalculateClPathAxesValues failed (EntId=" + ToString( nEntId) + ")" ;
LOG_ERROR( GetEMkLogger(), sOut.c_str())
return false ;
@@ -2277,9 +2277,9 @@ Operation::CalculateClPathAxesValues( int nClPathId, int nLinAxes, int nRotAxes,
//----------------------------------------------------------------------------
bool
Operation::CalculateRotAxesValues( bool bFirst, const Vector3d& vtTool, const Vector3d& vtAux,
double dRot1W, bool bMaxDeltaR2OnFirst, bool bRotContOnNext, double dAngDeltaMinForHome,
const DBLVECTOR& vAxRotHome, const DBLVECTOR& vAxRotPrec, DBLVECTOR& vAxRot)
Operation::CalculateMcentRotAxesValues( bool bFirst, const Vector3d& vtTool, const Vector3d& vtAux,
double dRot1W, bool bMaxDeltaR2OnFirst, bool bRotContOnNext, double dAngDeltaMinForHome,
const DBLVECTOR& vAxRotHome, const DBLVECTOR& vAxRotPrec, DBLVECTOR& vAxRot)
{
// massima variazione secondo asse rotante per continuità
const double dMaxDeltaR2 = 30 ;
@@ -2427,9 +2427,9 @@ Operation::CalculateRotAxesValues( bool bFirst, const Vector3d& vtTool, const Ve
//----------------------------------------------------------------------------
bool
Operation::VerifyLineMidPoint( const Point3d& ptPrec, const Vector3d& vtDirPrec, const Vector3d& vtAuxPrec, const Vector3d& vtCorrPrec, const DBLVECTOR& vAxPrec,
const Point3d& ptP, const Vector3d& vtDir, const Vector3d& vtAux, const Vector3d& vtCorr, const DBLVECTOR& vAxVal,
int nCnt, int nEntId, double dRot1W, int nMoveType, bool bToolShow, bool& bAdded, bool& bAxError)
Operation::VerifyMcentLineMidPoint( const Point3d& ptPrec, const Vector3d& vtDirPrec, const Vector3d& vtAuxPrec, const Vector3d& vtCorrPrec, const DBLVECTOR& vAxPrec,
const Point3d& ptP, const Vector3d& vtDir, const Vector3d& vtAux, const Vector3d& vtCorr, const DBLVECTOR& vAxVal,
int nCnt, int nEntId, double dRot1W, int nMoveType, bool bToolShow, bool& bAdded, bool& bAxError)
{
// impostazioni
bAdded = false ;
@@ -2470,13 +2470,20 @@ Operation::VerifyLineMidPoint( const Point3d& ptPrec, const Vector3d& vtDirPrec,
double dDist ;
if ( ! DistPointLine( ptMoveMid, ptPrec, ptP).GetDist( dDist))
return false ;
// LOG_INFO( GetEMkLogger(), ( "Midpoint dist =" + ToString( dDist, 3)).c_str())
double dLinTol = GetApproxLinTol() ;
if ( nMoveType == 0)
dLinTol = max( 10 * dLinTol, LIN_TOL_RAW) ;
else
dLinTol = max( dLinTol, 2 * EPS_SMALL) ;
bool bInTol = ( dDist <= dLinTol) ;
if ( ExeGetDebugLevel() >= 5) {
string sOut = "Midpoint dist = " + ToString( dDist, -4) +
( nMoveType == 0 ? " (G0" : " (G1") + " Cnt=" + ToString( nCnt) + ") " + ( bInTol ? "Ok" : "Split") ;
LOG_INFO( GetEMkLogger(), sOut.c_str())
}
// Se posizione in tolleranza, non devo fare alcunché
if ( dDist <= dLinTol)
if ( bInTol)
return true ;
// Va inserito il punto medio
@@ -2510,7 +2517,7 @@ Operation::VerifyLineMidPoint( const Point3d& ptPrec, const Vector3d& vtDirPrec,
if ( IsNull( pGP))
return false ;
// assegno le coordinate del punto
pGP->Set( ptP) ;
pGP->Set( ptMid) ;
// inserisco l'oggetto nel DB geometrico
int nMidEntId = m_pGeomDB->InsertGeoObj( GDB_ID_NULL, nEntId, GDB_BEFORE, Release( pGP)) ;
if ( nMidEntId == GDB_ID_NULL)
@@ -2539,7 +2546,7 @@ Operation::VerifyLineMidPoint( const Point3d& ptPrec, const Vector3d& vtDirPrec,
DBLVECTOR vAxRotPrec( int( vAxVal.size() - 3)) ;
for ( int i = 3 ; i < int( vAxVal.size()) ; ++ i)
vAxRotPrec[i-3] = vAxVal[i] ;
if ( ! CalculateRotAxesValues(false, vtDirMid, vtAuxMid, dRot1W, false, true, INFINITO, vAxRotHome, vAxRotPrec, vAxRotMid)) {
if ( ! CalculateMcentRotAxesValues( false, vtDirMid, vtAuxMid, dRot1W, false, true, INFINITO, vAxRotHome, vAxRotPrec, vAxRotMid)) {
pMidCamData->SetAxes( CamData::AS_DIR_ERR, vAxVal) ;
return false ;
}
@@ -2574,17 +2581,17 @@ Operation::VerifyLineMidPoint( const Point3d& ptPrec, const Vector3d& vtDirPrec,
// devo verificare le due nuove parti
++ nCnt ;
bool bAdded1, bAxError1, bAdded2, bAxError2 ;
if ( ! VerifyLineMidPoint( ptPrec, vtDirPrec, vtAuxPrec, vtCorrPrec, vAxPrec,
ptMid, vtDirMid, vtAuxMid, vtCorrMid, vAxMid,
nCnt, nMidEntId, dRot1W, nMoveType, bToolShow, bAdded1, bAxError1))
if ( ! VerifyMcentLineMidPoint( ptPrec, vtDirPrec, vtAuxPrec, vtCorrPrec, vAxPrec,
ptMid, vtDirMid, vtAuxMid, vtCorrMid, vAxMid,
nCnt, nMidEntId, dRot1W, nMoveType, bToolShow, bAdded1, bAxError1))
return false ;
if ( bAxError1) {
bAxError = true ;
return true ;
}
if ( ! VerifyLineMidPoint( ptMid, vtDirMid, vtAuxMid, vtCorrMid, vAxMid,
ptP, vtDir, vtAux, vtCorr, vAxVal,
nCnt, nEntId, dRot1W, nMoveType, bToolShow, bAdded2, bAxError2))
if ( ! VerifyMcentLineMidPoint( ptMid, vtDirMid, vtAuxMid, vtCorrMid, vAxMid,
ptP, vtDir, vtAux, vtCorr, vAxVal,
nCnt, nEntId, dRot1W, nMoveType, bToolShow, bAdded2, bAxError2))
return false ;
if ( bAxError2) {
bAxError = true ;
@@ -2602,6 +2609,8 @@ Operation::CalculateClPathRobotAxesValues( int nClPathId, int nLinAxes, int nRot
{
if ( m_pMchMgr == nullptr || m_pGeomDB == nullptr)
return false ;
// cancello eventuali punti aggiunti
EraseAddedPoints( nClPathId) ;
// eventuale direzione Aux preferenziale
bool bUseRefAux = true ;
Vector3d vtRefAux = Y_AX ;
@@ -2614,6 +2623,13 @@ Operation::CalculateClPathRobotAxesValues( int nClPathId, int nLinAxes, int nRot
case MCH_SCC_ADIR_ZM : vtRefAux = -Z_AX ; break ;
default : bUseRefAux = false ; break ;
}
// valori del precedente
Point3d ptPrec ;
Vector3d vtDirPrec ;
Vector3d vtAuxPrec ;
Vector3d vtCorrPrec ;
int nFlag2Prec = 0 ;
bool bToolShowPrec = true ;
// predispongo variabile per valori assi
DBLVECTOR vAxVal ;
vAxVal.reserve( 8) ;
@@ -2636,69 +2652,30 @@ Operation::CalculateClPathRobotAxesValues( int nClPathId, int nLinAxes, int nRot
// ricavo posizione con eventuali modifiche dipendenti dalla lavorazione
Point3d ptP = pCamData->GetEndPoint() ;
AdjustEndPointForAxesCalc( pCamData, ptP) ;
// calcolo gli assi del robot
Vector3d vtTool = pCamData->GetToolDir() ;
// recupero direzioni
Vector3d vtDir = pCamData->GetToolDir() ;
Vector3d vtAux ;
if ( ! bUseRefAux) {
Frame3d frTool ;
frTool.Set( ORIG, vtTool) ;
frTool.Set( ORIG, vtDir) ;
frTool.Rotate( frTool.Orig(), frTool.VersZ(), 0, 1) ;
vtAux = frTool.VersX() ;
}
else {
vtAux = OrthoCompo( vtRefAux, vtTool) ;
vtAux = OrthoCompo( vtRefAux, vtDir) ;
vtAux.Normalize() ;
}
pCamData->SetAuxDir( vtAux) ;
DBLVECTOR vAng1, vAng2 ;
bool bRaOk = m_pMchMgr->GetCurrMachine()->GetRobotAngles( ptP, vtTool, vtAux, vAng1, vAng2) ;
if ( ! bRaOk) {
Vector3d vtCorr = pCamData->GetCorrDir() ;
// calcolo gli assi del robot
if ( ! CalculateRobotAxesValues( bFirst, ptP, vtDir, vtAux,
bRotContOnNext, dAngDeltaMinForHome, vAxRotHome, vAxRotPrec, vAxVal)) {
pCamData->SetAxes( CamData::AS_ERR, vAxVal) ;
return false ;
}
// se inizio
if ( bFirst) {
// porto gli angoli ai valori più vicini ai precedenti con offset di uno o più giri
for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) {
m_pMchMgr->GetNearestAngleInStroke( i, vAxRotPrec[i], vAng1[i]) ;
if ( abs( vAng1[i] - vAxRotPrec[i]) > dAngDeltaMinForHome)
m_pMchMgr->GetNearestAngleInStroke( i, vAxRotHome[i], vAng1[i]) ;
}
for ( int i = 0 ; i < int( vAng2.size()) ; ++ i) {
m_pMchMgr->GetNearestAngleInStroke( i, vAxRotPrec[i], vAng2[i]) ;
if ( abs( vAng2[i] - vAxRotPrec[i]) > dAngDeltaMinForHome)
m_pMchMgr->GetNearestAngleInStroke( i, vAxRotHome[i], vAng2[i]) ;
}
bFirst = false ;
}
// altrimenti movimenti successivi
else {
// porto gli angoli ai valori più vicini ai precedenti con offset di uno o più giri
for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) {
if ( bRotContOnNext)
vAng1[i] = AngleNearAngle( vAng1[i], vAxRotPrec[i]) ;
else
m_pMchMgr->GetNearestAngleInStroke( i, vAxRotPrec[i], vAng1[i]) ;
}
for ( int i = 0 ; i < int( vAng2.size()) ; ++ i) {
if ( bRotContOnNext)
vAng2[i] = AngleNearAngle( vAng2[i], vAxRotPrec[i]) ;
else
m_pMchMgr->GetNearestAngleInStroke( i, vAxRotPrec[i], vAng2[i]) ;
}
}
// Scelgo la soluzione più vicina
// minimizzo rotazione polso
if ( vAng2.empty() || abs( vAng1[3] - vAxRotPrec[3]) < abs( vAng2[3] - vAxRotPrec[3]) + 10 * EPS_ANG_SMALL)
vAxVal = vAng1 ;
else
vAxVal = vAng2 ;
// controllo continuità di R6 dalla posizione precedente
m_pMchMgr->GetNearestAngleInStroke( 5, vAxRotPrec[5], vAxVal[5]) ;
// verifico di essere nelle corse degli assi
int nStat ;
bool bOsOk = m_pMchMgr->VerifyOutstroke( 0, 0, 0, vAxVal, false, nStat) ;
// salvo il risultato
if ( ! bOsOk || nStat != 0) {
bOk = false ;
pCamData->SetAxes( CamData::AS_OUTSTROKE, vAxVal) ;
@@ -2706,13 +2683,256 @@ Operation::CalculateClPathRobotAxesValues( int nClPathId, int nLinAxes, int nRot
else {
pCamData->SetAxes( CamData::AS_OK, vAxVal) ;
}
// tipo di movimento
int nMoveType = pCamData->GetMoveType() ;
// se non è l'inizio, verifico il punto medio
if ( ! bFirst) {
// Verifica ricorsiva del punto medio (se non già fatta)
bool bMidAdded = false ;
if ( nFlag2Prec == -1 || pCamData->GetFlag2() == -1)
bMidAdded = true ;
else {
bool bAxesError ;
if ( ! VerifyRobotLineMidPoint( ptPrec, vtDirPrec, vtAuxPrec, vtCorrPrec, vAxRotPrec,
ptP, vtDir, vtAux, vtCorr, vAxVal,
1, nEntId, nMoveType, bToolShowPrec && pCamData->GetToolShow(), bMidAdded, bAxesError)) {
string sOut = "Error : VerifyLineMidPoint of CalculateClPathAxesValues failed (EntId=" + ToString( nEntId) + ")" ;
LOG_ERROR( GetEMkLogger(), sOut.c_str())
return false ;
}
if ( bAxesError) {
bOk = false ;
continue ;
}
}
}
// aggiorno valori precedenti
bFirst = false ;
ptPrec = ptP ;
vtDirPrec = vtDir ;
vtAuxPrec = vtAux ;
vtCorrPrec = vtCorr ;
vAxRotPrec = vAxVal ;
nFlag2Prec = pCamData->GetFlag2() ;
bToolShowPrec = pCamData->GetToolShow() ;
}
return bOk ;
}
//----------------------------------------------------------------------------
bool
Operation::CalculateRobotAxesValues( bool bFirst, const Point3d& ptP, const Vector3d& vtTool, const Vector3d& vtAux,
bool bRotContOnNext, double dAngDeltaMinForHome,
const DBLVECTOR& vAxRotHome, const DBLVECTOR& vAxRotPrec, DBLVECTOR& vAxVal)
{
// calcolo angoli dei giunti
DBLVECTOR vAng1, vAng2 ;
bool bRaOk = m_pMchMgr->GetCurrMachine()->GetRobotAngles( ptP, vtTool, vtAux, vAng1, vAng2) ;
if ( ! bRaOk)
return false ;
// se inizio
if ( bFirst) {
// porto gli angoli ai valori più vicini ai precedenti con offset di uno o più giri
for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) {
m_pMchMgr->GetNearestAngleInStroke( i, vAxRotPrec[i], vAng1[i]) ;
if ( abs( vAng1[i] - vAxRotPrec[i]) > dAngDeltaMinForHome)
m_pMchMgr->GetNearestAngleInStroke( i, vAxRotHome[i], vAng1[i]) ;
}
for ( int i = 0 ; i < int( vAng2.size()) ; ++ i) {
m_pMchMgr->GetNearestAngleInStroke( i, vAxRotPrec[i], vAng2[i]) ;
if ( abs( vAng2[i] - vAxRotPrec[i]) > dAngDeltaMinForHome)
m_pMchMgr->GetNearestAngleInStroke( i, vAxRotHome[i], vAng2[i]) ;
}
bFirst = false ;
}
// altrimenti movimenti successivi
else {
// porto gli angoli ai valori più vicini ai precedenti con offset di uno o più giri
for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) {
if ( bRotContOnNext)
vAng1[i] = AngleNearAngle( vAng1[i], vAxRotPrec[i]) ;
else
m_pMchMgr->GetNearestAngleInStroke( i, vAxRotPrec[i], vAng1[i]) ;
}
for ( int i = 0 ; i < int( vAng2.size()) ; ++ i) {
if ( bRotContOnNext)
vAng2[i] = AngleNearAngle( vAng2[i], vAxRotPrec[i]) ;
else
m_pMchMgr->GetNearestAngleInStroke( i, vAxRotPrec[i], vAng2[i]) ;
}
}
// Scelgo la soluzione più vicina
// minimizzo rotazione polso
if ( vAng2.empty() || abs( vAng1[3] - vAxRotPrec[3]) < abs( vAng2[3] - vAxRotPrec[3]) + 10 * EPS_ANG_SMALL)
vAxVal = vAng1 ;
else
vAxVal = vAng2 ;
// controllo continuità di R6 dalla posizione precedente
m_pMchMgr->GetNearestAngleInStroke( 5, vAxRotPrec[5], vAxVal[5]) ;
return true ;
}
//----------------------------------------------------------------------------
bool
Operation::VerifyRobotLineMidPoint( const Point3d& ptPrec, const Vector3d& vtDirPrec, const Vector3d& vtAuxPrec, const Vector3d& vtCorrPrec, const DBLVECTOR& vAxPrec,
const Point3d& ptP, const Vector3d& vtDir, const Vector3d& vtAux, const Vector3d& vtCorr, const DBLVECTOR& vAxVal,
int nCnt, int nEntId, int nMoveType, bool bToolShow, bool& bAdded, bool& bAxError)
{
// impostazioni
bAdded = false ;
bAxError = false ;
Machine* pMachine = m_pMchMgr->GetCurrMachine() ;
// se disposizione non vanno fatti controlli
if ( GetType() == OPER_DISP)
return true ;
// se superato il limite di ricursioni, non devo fare alcunché
const int MAX_RECURSION = 8 ;
if ( nCnt > MAX_RECURSION)
return true ;
// calcolo variazione assi
double dDeltaAng = 0 ;
for ( int i = 0 ; i < int( vAxVal.size()) ; ++i)
dDeltaAng += abs( vAxVal[i] - vAxPrec[i]) ;
// Se non cambia la direzione utensile, non devo fare alcunché
const double MAX_DELTA_ANG = 0.01 ;
if ( dDeltaAng <= MAX_DELTA_ANG)
return true ;
// Determino i valori degli assi al punto medio
DBLVECTOR vAxMid( vAxVal.size()) ;
for ( int i = 0 ; i < int( vAxVal.size()) ; ++ i)
vAxMid[i] = ( vAxPrec[i] + vAxVal[i]) / 2 ;
// ricavo il punto medio del movimento
Point3d ptMoveMid ;
if ( ! pMachine->GetTipFromPositions( 0, 0, 0, vAxMid, false, false, true, ptMoveMid))
return false ;
// verifico le differenze
double dDist ;
if ( ! DistPointLine( ptMoveMid, ptPrec, ptP).GetDist( dDist))
return false ;
double dLinTol = GetApproxLinTol() ;
if ( nMoveType == 0)
dLinTol = max( 10 * dLinTol, LIN_TOL_RAW) ;
else
dLinTol = max( dLinTol, 2 * EPS_SMALL) ;
bool bInTol = ( dDist <= dLinTol) ;
if ( ExeGetDebugLevel() >= 5) {
string sOut = "Midpoint dist = " + ToString( dDist, -4) +
( nMoveType == 0 ? " (G0" : " (G1") + " Cnt=" + ToString( nCnt) + ") " + ( bInTol ? "Ok" : "Split") ;
LOG_INFO( GetEMkLogger(), sOut.c_str())
}
// Se posizione in tolleranza, non devo fare alcunché
if ( bInTol)
return true ;
// Va inserito il punto medio
Point3d ptMid = Media( ptPrec, ptP) ;
Vector3d vtDirMid = Media( vtDirPrec, vtDir) ;
if ( ! vtDirMid.Normalize()) {
if ( ! pMachine->GetBackToolDirFromAngles( vAxMid, vtDirMid))
return ( nMoveType == 0) ;
}
Vector3d vtAuxMid = Media( vtAuxPrec, vtAux) ;
if ( ! vtAuxPrec.IsSmall() && ! vtAux.IsSmall() && ! vtAuxMid.Normalize()) {
if ( ! pMachine->GetBackAuxDirFromAngles( vAxMid, vtAuxMid))
return ( nMoveType == 0) ;
}
Vector3d vtCorrMid = Media( vtCorrPrec, vtCorr) ;
if ( ! vtCorrPrec.IsSmall() && ! vtCorr.IsSmall() && ! vtCorrMid.Normalize()) {
// assegno il versore correzione precedente
vtCorrMid = vtCorrPrec ;
// se possibile, cerco di migliorarlo considerando i rif. prec. e sul medio da direzioni fresa (Z) e ausiliaria (X)
Frame3d refPrec, refMid ;
if ( refPrec.Set( ORIG, vtDirPrec, vtAuxPrec) &&
refMid.Set( ORIG, vtDirMid, vtAuxMid)) {
// versore correzione in locale al precente coincide con quello in locale al corrente
vtCorrMid.ToLoc( refPrec) ;
vtCorrMid.ToGlob( refMid) ;
}
}
// inserisco nuova entità punto (per evitare problemi di lunghezza linea)
// creo oggetto punto per DB geometrico
PtrOwner<IGeoPoint3d> pGP( CreateGeoPoint3d()) ;
if ( IsNull( pGP))
return false ;
// assegno le coordinate del punto
pGP->Set( ptMid) ;
// inserisco l'oggetto nel DB geometrico
int nMidEntId = m_pGeomDB->InsertGeoObj( GDB_ID_NULL, nEntId, GDB_BEFORE, Release( pGP)) ;
if ( nMidEntId == GDB_ID_NULL)
return false ;
// creo oggetto dati Cam
const CamData* pEntCam = GetCamData( m_pGeomDB->GetUserObj( nEntId)) ;
if ( pEntCam == nullptr)
return false ;
PtrOwner<CamData> pCam( pEntCam->Clone()) ;
if ( IsNull( pCam))
return false ;
// associo questo oggetto a quello geometrico
m_pGeomDB->SetUserObj( nMidEntId, Release( pCam)) ;
// assegno valori
CamData* pMidCamData = GetCamData( m_pGeomDB->GetUserObj( nMidEntId)) ;
if ( pMidCamData == nullptr)
return false ;
pMidCamData->SetEndPoint( ptMid) ;
pMidCamData->SetToolDir( vtDirMid) ;
pMidCamData->SetAuxDir( vtAuxMid) ;
pMidCamData->SetCorrDir( vtCorrMid) ;
pMidCamData->SetFlag2( -1) ;
pMidCamData->SetToolShow( bToolShow && nCnt < 4) ;
// calcolo gli assi per il punto medio
DBLVECTOR vAxRotHome( vAxVal.size()) ;
DBLVECTOR vAxRotPrec( vAxVal.size()) ;
for ( int i = 0 ; i < int( vAxVal.size()) ; ++ i)
vAxRotPrec[i] = vAxVal[i] ;
if ( ! CalculateRobotAxesValues( false, ptMid, vtDirMid, vtAuxMid,
true, INFINITO, vAxRotHome, vAxRotPrec, vAxMid)) {
pMidCamData->SetAxes( CamData::AS_ERR, vAxVal) ;
return false ;
}
// verifico di essere nelle corse degli assi
int nStat ;
bool bOsOk = m_pMchMgr->VerifyOutstroke( 0, 0, 0, vAxMid, false, nStat) ;
if ( ! bOsOk || nStat != 0) {
pMidCamData->SetAxes( CamData::AS_OUTSTROKE, vAxMid) ;
bAxError = true ;
return true ;
}
// assegno i valori degli assi
pMidCamData->SetAxes( CamData::AS_OK, vAxMid) ;
// calcolo e salvo il valore dell'asse ausiliario di testa riportato sul grezzo
Vector3d vtBackAux ; pMachine->GetBackAuxDirFromAngles( vAxMid, vtBackAux) ;
pMidCamData->SetBackAuxDir( vtBackAux) ;
// devo verificare le due nuove parti
++ nCnt ;
bool bAdded1, bAxError1, bAdded2, bAxError2 ;
if ( ! VerifyRobotLineMidPoint( ptPrec, vtDirPrec, vtAuxPrec, vtCorrPrec, vAxPrec,
ptMid, vtDirMid, vtAuxMid, vtCorrMid, vAxMid,
nCnt, nMidEntId, nMoveType, bToolShow, bAdded1, bAxError1))
return false ;
if ( bAxError1) {
bAxError = true ;
return true ;
}
if ( ! VerifyRobotLineMidPoint( ptMid, vtDirMid, vtAuxMid, vtCorrMid, vAxMid,
ptP, vtDir, vtAux, vtCorr, vAxVal,
nCnt, nEntId, nMoveType, bToolShow, bAdded2, bAxError2))
return false ;
if ( bAxError2) {
bAxError = true ;
return true ;
}
return true ;
}
//----------------------------------------------------------------------------
bool
Operation::AdjustStartEndMovements( bool bVerifyPreviousLink)
@@ -3232,7 +3452,7 @@ Operation::AdjustOneStartEndMovement( int nClPathId, int nPrevClPathId, Operatio
// se provengo da Z massima
if ( bMaxZ) {
// se centro di lavoro
if ( m_pMchMgr->GetCurrIsCenter()) {
if ( m_pMchMgr->GetCurrIsMcent()) {
// copio l'entità
if ( m_pGeomDB->Copy( nEntId, GDB_ID_NULL, nEntId, GDB_AFTER) == GDB_ID_NULL)
return false ;
@@ -3292,7 +3512,7 @@ Operation::AdjustOneStartEndMovement( int nClPathId, int nPrevClPathId, Operatio
// verifico se la testa interferisce con i pezzi o i bloccaggi sulla tavola
else {
// se centro di lavoro
if ( m_pMchMgr->GetCurrIsCenter()) {
if ( m_pMchMgr->GetCurrIsMcent()) {
// Recupero box complessivo dei grezzi attivi
BBox3d b3Raws ;
GetCurrRawsGlobBox( b3Raws) ;
@@ -3760,7 +3980,7 @@ Operation::AddRise( DBLVECTOR& vAxVal, double dDelta, int nClPathId, int nToMinM
int nFlag2 = 0 ;
bool bOk = false ;
// se centro di lavoro
if ( m_pMchMgr->GetCurrIsCenter()) {
if ( m_pMchMgr->GetCurrIsMcent()) {
// devono esserci almeno i tre assi lineari
if ( vAxVal.size() < 3)
return false ;
@@ -4545,7 +4765,7 @@ Operation::TestCollisionAvoid( const DBLVECTOR& vAxStart, const DBLVECTOR& vAxEn
}
// Verifica
int nLinkAxisOrder = ( m_pMchMgr->GetCurrIsCenter() ? pMch->GetLinkAxesMoveOrder() : LKAMO_INTERP) ;
int nLinkAxisOrder = ( m_pMchMgr->GetCurrIsMcent() ? pMch->GetLinkAxesMoveOrder() : LKAMO_INTERP) ;
// se ordine dipende da variazione angolare, verifico quale ordine effettivo
if ( nLinkAxisOrder == LKAMO_HEAD_BEFORE_IF_SAME_ANG || nLinkAxisOrder == LKAMO_HEAD_AFTER_IF_SAME_ANG) {
// cerco massima variazione degli assi rotanti
Operation.h
+18 -12
View File
@@ -211,22 +211,28 @@ class Operation : public IUserObj
const CamData* GetClPathInitialCamData( int nClPathId, bool bSkipClimb) const ;
const CamData* GetFinalCamData( bool bSkipRise) const ;
const CamData* GetClPathFinalCamData( int nClPathId, bool bSkipRise) const ;
bool CalculateClPathCenterAxesValues( int nClPathId, int nLinAxes, int nRotAxes, double dRot1W,
bool bMaxDeltaR2OnFirst, bool bRotContOnNext, double dAngDeltaMinForHome,
const DBLVECTOR& vAxRotHome, DBLVECTOR& vAxRotPrec) ;
bool CalculateClPathMcentAxesValues( int nClPathId, int nLinAxes, int nRotAxes, double dRot1W,
bool bMaxDeltaR2OnFirst, bool bRotContOnNext, double dAngDeltaMinForHome,
const DBLVECTOR& vAxRotHome, DBLVECTOR& vAxRotPrec) ;
bool EraseAddedPoints( int nClPathId) ;
bool CalculateClPathAxesValues( int nClPathId, int nLinAxes, int nRotAxes, double dRot1W,
bool bMaxDeltaR2OnFirst, bool bRotContOnNext, double dAngDeltaMinForHome,
const DBLVECTOR& vAxRotHome, DBLVECTOR& vAxRotPrec, int& nOutStrC) ;
bool CalculateRotAxesValues( bool bFirst, const Vector3d& vtTool, const Vector3d& vtAux,
double dRot1W, bool bMaxDeltaR2OnFirst, bool bRotContOnNext, double dAngDeltaMinForHome,
const DBLVECTOR& vAxRotHome, const DBLVECTOR& vAxRotPrec, DBLVECTOR& vAxRot) ;
bool VerifyLineMidPoint( const Point3d& ptPrec, const Vector3d& vtDirPrec, const Vector3d& vtAuxPrec, const Vector3d& vtCorrPrec, const DBLVECTOR& vAxPrec,
const Point3d& ptP, const Vector3d& vtDir, const Vector3d& vtAux, const Vector3d& vtCorr, const DBLVECTOR& vAxVal,
int nCnt, int nEntId, double dRot1W, int nMoveType, bool bToolShow, bool& bAdded, bool& bAxError) ;
bool MyCalculateClPathMcentAxesValues( int nClPathId, int nLinAxes, int nRotAxes, double dRot1W,
bool bMaxDeltaR2OnFirst, bool bRotContOnNext, double dAngDeltaMinForHome,
const DBLVECTOR& vAxRotHome, DBLVECTOR& vAxRotPrec, int& nOutStrC) ;
bool CalculateMcentRotAxesValues( bool bFirst, const Vector3d& vtTool, const Vector3d& vtAux,
double dRot1W, bool bMaxDeltaR2OnFirst, bool bRotContOnNext, double dAngDeltaMinForHome,
const DBLVECTOR& vAxRotHome, const DBLVECTOR& vAxRotPrec, DBLVECTOR& vAxRot) ;
bool VerifyMcentLineMidPoint( const Point3d& ptPrec, const Vector3d& vtDirPrec, const Vector3d& vtAuxPrec, const Vector3d& vtCorrPrec, const DBLVECTOR& vAxPrec,
const Point3d& ptP, const Vector3d& vtDir, const Vector3d& vtAux, const Vector3d& vtCorr, const DBLVECTOR& vAxVal,
int nCnt, int nEntId, double dRot1W, int nMoveType, bool bToolShow, bool& bAdded, bool& bAxError) ;
bool CalculateClPathRobotAxesValues( int nClPathId, int nLinAxes, int nRotAxes, double dRot1W,
bool bMaxDeltaR2OnFirst, bool bRotContOnNext, double dAngDeltaMinForHome,
const DBLVECTOR& vAxRotHome, DBLVECTOR& vAxRotPrec) ;
bool CalculateRobotAxesValues( bool bFirst, const Point3d& ptP, const Vector3d& vtTool, const Vector3d& vtAux,
bool bRotContOnNext, double dAngDeltaMinForHome,
const DBLVECTOR& vAxRotHome, const DBLVECTOR& vAxRotPrec, DBLVECTOR& vAxRot) ;
bool VerifyRobotLineMidPoint( const Point3d& ptPrec, const Vector3d& vtDirPrec, const Vector3d& vtAuxPrec, const Vector3d& vtCorrPrec, const DBLVECTOR& vAxPrec,
const Point3d& ptP, const Vector3d& vtDir, const Vector3d& vtAux, const Vector3d& vtCorr, const DBLVECTOR& vAxVal,
int nCnt, int nEntId, int nMoveType, bool bToolShow, bool& bAdded, bool& bAxError) ;
bool GetAxisMidForTestCollisionAvoid( const DBLVECTOR& vAxStart, const DBLVECTOR& vAxEnd, int nAxisOrder, Machine* pMch,
DBLVECTOR& vAxMid) const ;
bool OneMoveTestCollisionAvoid( const STRVECTOR& vAxName, const DBLVECTOR& vAxStart, const DBLVECTOR& vAxEnd,