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

EgtMachKernel :

Browse Source 
- aggiunta GetTipFromPositions (cinematica inversa).
This commit is contained in:
Dario Sassi
2015-11-26 16:49:08 +00:00
parent 9588b2de83
commit 5eb5cb387f
4 changed files with 87 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MachineCalc.cpp
+69 -2
View File
@@ -78,6 +78,7 @@ Machine::SetCurrTool( const string& sTool, const string& sHead, int nExit)
m_dCalcRot1W = 1 ;
m_nCalcSolCh = MCH_SCC_NONE ;
m_dCalcTLen = 0 ;
m_dCalcTRad = 0 ;
// recupero il gruppo della testa
int nHeadId = GetGroup( sHead) ;
// recupero i dati della testa
@@ -98,7 +99,10 @@ Machine::SetCurrTool( const string& sTool, const string& sHead, int nExit)
if ( nToolId == GDB_ID_NULL || m_pGeomDB->GetGdbType( nToolId) != GDB_TY_GROUP)
return false ;
double dTLen = 0 ;
if ( ! m_pMchMgr->TdbSetCurrTool( sTool) || ! m_pMchMgr->TdbGetCurrToolParam( TPA_LEN, dTLen))
double dTDiam = 0 ;
if ( ! m_pMchMgr->TdbSetCurrTool( sTool) ||
! m_pMchMgr->TdbGetCurrToolParam( TPA_LEN, dTLen) ||
! m_pMchMgr->TdbGetCurrToolParam( TPA_DIAM, dTDiam))
return false ;
// assegno tutti i dati
m_nCalcHeadId = nHeadId ;
@@ -110,6 +114,7 @@ Machine::SetCurrTool( const string& sTool, const string& sHead, int nExit)
m_vtCalcDir = pExit->GetTDir() ;
m_vtCalcADir = pHead->GetADir() ;
m_dCalcTLen = dTLen ;
m_dCalcTRad = dTDiam / 2 ;
// determino la catena cinematica
return CalculateKinematicChain() ;
}
@@ -433,7 +438,7 @@ Machine::GetPositions( const Point3d& ptP, double dAngA, double dAngB,
int& nStat, double& dX, double& dY, double& dZ)
{
// la posizione deve essere espressa rispetto allo ZERO MACCHINA
// per ora gestisco solo gli assi di testa
// per ora gestisco solo gli assi rotanti di testa
// posizione e direzione fresa su testa a riposo
Point3d ptPosH = m_ptCalcPos ;
@@ -478,6 +483,68 @@ Machine::GetPositions( const Point3d& ptP, double dAngA, double dAngB,
return true ;
}
//----------------------------------------------------------------------------
bool
Machine::GetDirection( const Vector3d& vtDir, double dAngA, double dAngB, Vector3d& vtNew)
{
// per ora gestisco solo gli assi rotanti di testa
// direzione a riposo
vtNew = vtDir ;
// se c'è secondo asse rotante di testa
if ( m_vCalcRotAx.size() >= 2 && m_vCalcRotAx[1].bHead) {
// posizione e direzione primo asse rotante
Point3d ptAx2 = m_vCalcRotAx[1].ptPos ;
Vector3d vtAx2 = m_vCalcRotAx[1].vtDir ;
// ruoto dati a riposo
vtNew.Rotate( vtAx2, dAngB) ;
}
// se c'è primo asse rotante di testa
if ( m_vCalcRotAx.size() >= 1 && m_vCalcRotAx[0].bHead) {
// posizione e direzione primo asse rotante
Point3d ptAx1 = m_vCalcRotAx[0].ptPos ;
Vector3d vtAx1 = m_vCalcRotAx[0].vtDir ;
// ruoto dati a riposo
vtNew.Rotate( vtAx1, dAngA) ;
}
return true ;
}
//----------------------------------------------------------------------------
bool
Machine::GetTipFromPositions( double dX, double dY, double dZ, double dAngA, double dAngB,
bool bBottom, Point3d& ptTip)
{
// la posizione deve essere espressa rispetto allo ZERO MACCHINA
// per ora gestisco solo gli assi rotanti di testa
// Calcoli recuperi della testa e lunghezza utensile per orientamento
int nStat ;
double dRecX, dRecY, dRecZ ;
if ( ! GetPositions( ORIG, dAngA, dAngB, nStat, dRecX, dRecY, dRecZ))
return false ;
ptTip.Set( dX - dRecX, dY - dRecY, dZ - dRecZ) ;
// Se richiesto punto sotto del tip utensile
if ( bBottom) {
// calcolo la direzione fresa
Vector3d vtDirT ;
if ( ! GetDirection( m_vtCalcDir, dAngA, dAngB, vtDirT))
return false ;
// se direzione fresa non esattamente verticale, esiste un punto più basso del'utensile
if ( ! vtDirT.IsZplus() && ! vtDirT.IsZminus()) {
// calcolo la direzione perpendicolare più verticale possibile
Vector3d vtCorr = FromUprightOrtho( vtDirT) ;
// correggo il tip
ptTip -= vtCorr * m_dCalcTRad ;
}
}
return true ;
}
//----------------------------------------------------------------------------
bool
Machine::VerifyScc( const Vector3d& vtDirI, const Vector3d& vtDirA, int nSolCh)