From 89d46baf43b1be8f935e9fc65a18bca678530432 Mon Sep 17 00:00:00 2001
From: Dario Sassi <dario.sassi@egalware.com>
Date: Wed, 17 Apr 2024 06:58:27 +0200
Subject: [PATCH] EgtMachKernel 2.6d4 : - aggiunta gestione riferimento
 canonico per calcolo cinematica robot comunque disposto nello spazio -
 adattamento funzioni di calcolo posizione, direzione utensile e ausiliaria da
 questi angoli - in svuotatura corretta mancata esplosione di archi in rette
 sui percorsi di ritorno quando richiesta.

---
 EgtMachKernel.rc | Bin 11774 -> 11774 bytes
 Machine.cpp      |   1 +
 Machine.h        |   1 +
 MachineCalc.cpp  |  39 +++++++++++++++++++++++++++++++++------
 Operation.cpp    |   2 +-
 Pocketing.cpp    |   8 +++++++-
 6 files changed, 43 insertions(+), 8 deletions(-)

diff --git a/EgtMachKernel.rc b/EgtMachKernel.rc
index cbfee2e7c38ffc2aa112e121d0573ed2ab86711a..2a174e2167ce6debe0d94194f84f65769d9592ff 100644
GIT binary patch
delta 236
zcmewt{V#gMI%aKGhCGIJh8%`WhGK>c1_cHUhESkLCPN8BDnkm89}HwAf_cT0IklxX
ze_&=~W;EISUhX2(<P)4ZK&GKoDdXl<+#V2NM-XMG1QHU43L&Z7%L3L5Rg_Jt^#><&
nF)y2}rOv|(a;PWRX*mp)Acsx<z?`-DjJggps?=sq?XOG#ySzRV

delta 241
zcmewt{V#gMI%aiGhCGIJh8%`ehGHOAV9;O)W+-7u1ma8}FBB-731Uz7R&U)L$1;bB
z(RlN3Hd|&!<H-*tbvLJRO<)0wTx6PjLa+cPDg+fpQs=?Ad6l#WP#;h`nqrWY@;63e
oowE6~YB8f=KG?0EV7KM~-I%!9TYV!lrtsv`ss}fp(l%iN09+nKDF6Tf

diff --git a/Machine.cpp b/Machine.cpp
index 629ce9a..63230e6 100644
--- a/Machine.cpp
+++ b/Machine.cpp
@@ -58,6 +58,7 @@ Machine::Machine( void)
    m_nHeadRotAxes = 0 ;
    m_nHeadSpecRotAxis = -1 ;
    m_frLinAx.Reset( false) ;
+   m_frRobot.Reset( false) ;
    m_nCalcChainType = KIN_CHAIN_NONE ;
    m_nMachineLook = MCH_LOOK_NONE ;
 }
diff --git a/Machine.h b/Machine.h
index 896ffdd..d1b9048 100644
--- a/Machine.h
+++ b/Machine.h
@@ -317,6 +317,7 @@ class Machine
       KINAXISVECTOR m_vCalcLinAx ;           // vettore assi lineari attivi per calcoli
       KINAXISVECTOR m_vCalcRotAx ;           // vettore assi rotanti attivi per calcoli
       Frame3d       m_frLinAx ;              // sistema di riferimento definito dagli assi lineari
+      Frame3d       m_frRobot ;              // sistema di riferimento canonico del robot
       int           m_nCalcChainType ;       // tipologia testa attiva (nulla, centro di lavoro o robot)
       Point3d       m_ptWristCen ;           // centro del polso sferico nel riferimento testa/uscita di calcolo
       Vector3d      m_vtWristRef ;           // direzione del polso sferico nel riferimento testa/uscita di calcolo
diff --git a/MachineCalc.cpp b/MachineCalc.cpp
index dc1d8c3..212e11e 100644
--- a/MachineCalc.cpp
+++ b/MachineCalc.cpp
@@ -493,6 +493,8 @@ Machine::CalculateKinematicChain( void)
    m_nHeadSpecRotAxis = -1 ;
    m_vCalcLinAx.clear() ;
    m_vCalcRotAx.clear() ;
+   m_frLinAx.Reset( false) ;
+   m_frRobot.Reset( false) ;
    m_nCalcChainType = KIN_CHAIN_NONE ;
   // recupero gli assi di tavola
    if ( m_nCalcTabId == GDB_ID_NULL)
@@ -535,6 +537,19 @@ Machine::CalculateKinematicChain( void)
       swap( m_vCalcRotAx[0], m_vCalcRotAx[5]) ;
       swap( m_vCalcRotAx[1], m_vCalcRotAx[4]) ;
       swap( m_vCalcRotAx[2], m_vCalcRotAx[3]) ;
+     // determino il riferimento canonico del robot
+      if ( ! m_frRobot.Set( m_vCalcRotAx[0].ptPos, m_vCalcRotAx[0].vtDir, m_vCalcRotAx[3].vtDir)) {
+         LOG_ERROR( GetEMkLogger(), "Robot with errors in Rotary Axes : impossible canonic frame")
+         return false ;
+      }
+     // porto tutti gli assi cinematici e i dati testa nel riferimento canonico
+      for ( int i = 0 ; i < m_nHeadRotAxes ; ++ i) {
+         m_vCalcRotAx[i].ptPos.ToLoc( m_frRobot) ;
+         m_vCalcRotAx[i].vtDir.ToLoc( m_frRobot) ;
+      }
+      m_ptCalcPos.ToLoc( m_frRobot) ;
+      m_vtCalcDir.ToLoc( m_frRobot) ;
+      m_vtCalcADir.ToLoc( m_frRobot) ;
      // direzione assi rotanti deve essere Z Y Y X Y X
       if ( ! m_vCalcRotAx[0].vtDir.IsZ()  ||
            ! m_vCalcRotAx[1].vtDir.IsY()  ||
@@ -1266,6 +1281,10 @@ Machine::GetDirection( const Vector3d& vtDir, const DBLVECTOR& vAng, Vector3d& v
          vtNew.Rotate( m_vCalcRotAx[i-1].vtDir, vAng[i-1]) ;
    }
 
+  // nel caso di robot devo passare dal riferimento canonico robot a quello di macchina
+   if ( m_nCalcChainType == KIN_CHAIN_ROBOT)
+      vtNew.ToGlob( m_frRobot) ;
+
    return true ;
 }
 
@@ -1396,6 +1415,10 @@ Machine::GetTipFromPositions( double dX, double dY, double dZ, const DBLVECTOR&
    else
       return false ;
 
+  // nel caso di robot devo passare dal riferimento canonico robot a quello di macchina
+   if ( m_nCalcChainType == KIN_CHAIN_ROBOT)
+      ptTip.ToGlob( m_frRobot) ;
+
   // Se richiesto ingombro totale o punto sotto del tip utensile
    if ( bOverall  ||  bBottom) {
      // calcolo la direzione fresa
@@ -2050,13 +2073,17 @@ Machine::GetRobotAngles( const Point3d& ptP, const Vector3d& vtDirT, const Vecto
   // pulisco il risultato
    vAng1.clear() ;
    vAng2.clear() ;
+  // porto i dati nel riferimento robot
+   Point3d ptPL = GetToLoc( ptP, m_frRobot) ;
+   Vector3d vtDirTL = GetToLoc( vtDirT, m_frRobot) ;
+   Vector3d vtDirAL = GetToLoc( vtDirA, m_frRobot) ;
   // riferimento utensile
    Frame3d frTool ;
-   frTool.Set( ORIG, vtDirT, vtDirA) ;
+   frTool.Set( ORIG, vtDirTL, vtDirAL) ;
   // deduco la posizione richiesta del centro del polso
-   Point3d ptCen = ptP + m_ptWristCen.x * frTool.VersX() +
-                         m_ptWristCen.y * frTool.VersY() +
-                         ( m_dCalcTLen + m_ptWristCen.z) * frTool.VersZ() ;
+   Point3d ptCen = ptPL + m_ptWristCen.x * frTool.VersX() +
+                          m_ptWristCen.y * frTool.VersY() +
+                          ( m_dCalcTLen + m_ptWristCen.z) * frTool.VersZ() ;
   // deduco la direzione richiesta del centro del polso
    Vector3d vtCen = m_vtWristRef.x * frTool.VersX() +
                     m_vtWristRef.y * frTool.VersY() +
@@ -2131,11 +2158,11 @@ Machine::GetRobotAngles( const Point3d& ptP, const Vector3d& vtDirT, const Vecto
    for ( int i = 4; i >= 0 ; --i)
       vtTool.Rotate( m_vCalcRotAx[i].vtDir, vAng1[i]) ;
    double dAng6 ; bool bDet6 ;
-   if ( ! vtTool.GetRotation( vtDirT, vtR6, EPS_SMALL, dAng6, bDet6)  ||  ! bDet6) {
+   if ( ! vtTool.GetRotation( vtDirTL, vtR6, EPS_SMALL, dAng6, bDet6)  ||  ! bDet6) {
       Vector3d vtAux = m_vtCalcADir ;
       for ( int i = 4; i >= 0 ; --i)
          vtAux.Rotate( m_vCalcRotAx[i].vtDir, vAng1[i]) ;
-      if ( ! vtAux.GetRotation( vtDirA, vtR6, EPS_SMALL, dAng6, bDet6)  ||  ! bDet6) {
+      if ( ! vtAux.GetRotation( vtDirAL, vtR6, EPS_SMALL, dAng6, bDet6)  ||  ! bDet6) {
          LOG_ERROR( GetEMkLogger(), "Error : R6 not calculable (robot)")
          return false ;      
       }
diff --git a/Operation.cpp b/Operation.cpp
index 3c04180..d336d83 100644
--- a/Operation.cpp
+++ b/Operation.cpp
@@ -42,7 +42,7 @@ using namespace std ;
 
 //----------------------------------------------------------------------------
 const double MAX_DIST_RAW = 200.0 ;
-const double MAX_ROBOT_G0_LEN = 300.0 ;
+const double MAX_ROBOT_G0_LEN = 299.0 ;
 
 //----------------------------------------------------------------------------
 bool
diff --git a/Pocketing.cpp b/Pocketing.cpp
index ffee199..b55c615 100644
--- a/Pocketing.cpp
+++ b/Pocketing.cpp
@@ -4223,8 +4223,14 @@ Pocketing::CalcCircleSpiral( const Point3d& ptCen, const Vector3d& vtN, double d
          return false ;
       }
       pRCrv->AddCurve( Release( pArc2)) ;
-     // inverto e eventualmente sistemo archi
+     // inverto
       pRCrv->Invert() ;
+     // se necessario, approssimo con rette
+      if ( bSplitArcs  &&  ! ApproxWithLines( pRCrv)) {
+         m_pMchMgr->SetLastError( 2419, "Error in Pocketing : Linear Approx not computable") ;
+         return false ;
+      }
+     // eventuale sistemazione archi
       VerifyArcs( pRCrv) ;
    }