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

EgtMachKernel :

Browse Source 
- corretta AddRobotClimb (aggiungeva entità in posizione non corretta)
- aggiunta gestione soluzione indeterminata con tre assi rotanti liberi.
This commit is contained in:
Dario Sassi
2026-04-09 20:17:50 +02:00
parent 40f1e051a4
commit 932700d4b2
5 changed files with 86 additions and 63 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MachineCalc.cpp
+33 -25
View File
@@ -1384,6 +1384,15 @@ Machine::GetMyAngles( const Vector3d& vtDirT, const Vector3d& vtDirA,
dAngA2 = dAngA1 + ( dAngA1 > EPS_ANG_ZERO ? -ANG_STRAIGHT : ANG_STRAIGHT) ;
dAngB2 = -dAngB1 ;
// se soluzione indeterminata e suggerito angolo precedente valido
if ( abs( dAngB1) < EPS_ANG_SMALL / 2 && isfinite( m_dPrevAngA) && VerifyAngleOutstroke( 1, m_dPrevAngA)) {
nStat = 1 ;
dAngA1 = m_dPrevAngA ;
dAngB1 = 0 ;
}
else
nStat = 2 ;
// calcolo il terzo asse rotante del polso
Vector3d vtR3 = RotAx3.vtDir ;
vtR3.Rotate( RotAx2.vtDir, dAngB1) ;
@@ -1404,22 +1413,25 @@ Machine::GetMyAngles( const Vector3d& vtDirT, const Vector3d& vtDirA,
dAngC2 = dAngC1 + ( dAngC1 > EPS_ANG_ZERO ? -ANG_STRAIGHT : ANG_STRAIGHT) ;
// verifiche dei limiti di corsa
nStat = 2 ;
// se non riesco ad aggiustare, elimino
if ( ! AdjustAngleInStroke( RotAx1.stroke, dAngA2) ||
! AdjustAngleInStroke( RotAx2.stroke, dAngB2) ||
! AdjustAngleInStroke( RotAx3.stroke, dAngC2))
-- nStat ;
// se non riesco ad aggiustare, elimino
if ( ! AdjustAngleInStroke( RotAx1.stroke, dAngA1) ||
! AdjustAngleInStroke( RotAx2.stroke, dAngB1) ||
! AdjustAngleInStroke( RotAx3.stroke, dAngC1)) {
-- nStat ;
// riloco eventuale soluzione rimasta
if ( nStat >= 1) {
dAngA1 = dAngA2 ;
dAngB1 = dAngB2 ;
dAngC1 = dAngC2 ;
if ( nStat >= 2) {
// se non riesco ad aggiustare, elimino
if ( ! AdjustAngleInStroke( RotAx1.stroke, dAngA2) ||
! AdjustAngleInStroke( RotAx2.stroke, dAngB2) ||
! AdjustAngleInStroke( RotAx3.stroke, dAngC2))
-- nStat ;
}
if ( nStat >= 1) {
// se non riesco ad aggiustare, elimino
if ( ! AdjustAngleInStroke( RotAx1.stroke, dAngA1) ||
! AdjustAngleInStroke( RotAx2.stroke, dAngB1) ||
! AdjustAngleInStroke( RotAx3.stroke, dAngC1)) {
-- nStat ;
// riloco eventuale soluzione rimasta
if ( nStat >= 1) {
dAngA1 = dAngA2 ;
dAngB1 = dAngB2 ;
dAngC1 = dAngC2 ;
}
}
}
@@ -1873,7 +1885,7 @@ bool
Machine::GetNearestAngleInStroke( int nInd, double dAngRef, double& dAng) const
{
// se angolo fittizio (non esiste l'asse rotante corrispondente), non c'è alcunchè da fare
if ( nInd < 0 || nInd >= int( m_vCalcRotAx.size()))
if ( nInd < 0 || nInd >= ssize( m_vCalcRotAx))
return true ;
// cerco l'angolo più vicino stando nella corsa
while ( dAng - dAngRef > ANG_STRAIGHT + EPS_ANG_ZERO && dAng - ANG_FULL >= m_vCalcRotAx[nInd].stroke.Min)
@@ -1889,7 +1901,7 @@ bool
Machine::LimitAngleToStroke( int nInd, double& dAng) const
{
// se angolo fittizio (non esiste l'asse rotante corrispondente), non c'è alcunchè da fare
if ( nInd < 0 || nInd >= int( m_vCalcRotAx.size()))
if ( nInd < 0 || nInd >= ssize( m_vCalcRotAx))
return true ;
// se angolo fuori corsa, lo porto all'estremo più vicino
dAng = Clamp( dAng, m_vCalcRotAx[nInd].stroke.Min, m_vCalcRotAx[nInd].stroke.Max) ;
@@ -1901,7 +1913,7 @@ bool
Machine::VerifyAngleOutstroke( int nInd, double dAng) const
{
// se angolo fittizio (non esiste l'asse rotante corrispondente), va bene
if ( nInd < 0 || nInd >= int( m_vCalcRotAx.size()))
if ( nInd < 0 || nInd >= ssize( m_vCalcRotAx))
return true ;
// se angolo fuori corsa, errore
if ( dAng < m_vCalcRotAx[nInd].stroke.Min)
@@ -2380,13 +2392,9 @@ Machine::GetRobotAngles( const Point3d& ptP, const Vector3d& vtDirT, const Vecto
return false ;
}
// deduco la posizione richiesta del centro del polso
Point3d ptCen = ptPL + m_ptWristCen.x * frTool.VersX() +
m_ptWristCen.y * frTool.VersY() +
( m_dCalcTLen + m_ptWristCen.z) * frTool.VersZ() ;
Point3d ptCen = ptPL + GetToGlob( m_ptWristCen + m_dCalcTLen * Z_AX, frTool) ;
// deduco la direzione richiesta del centro del polso
Vector3d vtCen = m_vtWristRef.x * frTool.VersX() +
m_vtWristRef.y * frTool.VersY() +
m_vtWristRef.z * frTool.VersZ() ;
Vector3d vtCen = GetToGlob( m_vtWristRef, frTool) ;
// calcolo il primo asse rotante
Vector3d vtArm = ptCen - m_vCalcRotAx[0].ptPos ;
double dAng1 ; bool bDet1 ;