From 737124b0bd167e644bb1c0131e675f515cbecb1f Mon Sep 17 00:00:00 2001
From: Dario Sassi <dario.sassi@egalware.com>
Date: Tue, 20 Feb 2024 10:52:23 +0100
Subject: [PATCH] EgtMachKernel : - modifiche per configurazione canonica robot
 da ZYY-ZYZ a ZYY-XYX.

---
 MachineCalc.cpp | 20 +++++++++++---------
 1 file changed, 11 insertions(+), 9 deletions(-)

diff --git a/MachineCalc.cpp b/MachineCalc.cpp
index c9e1289..167f998 100644
--- a/MachineCalc.cpp
+++ b/MachineCalc.cpp
@@ -508,19 +508,20 @@ Machine::CalculateKinematicChain( void)
       swap( m_vCalcRotAx[0], m_vCalcRotAx[5]) ;
       swap( m_vCalcRotAx[1], m_vCalcRotAx[4]) ;
       swap( m_vCalcRotAx[2], m_vCalcRotAx[3]) ;
-     // direzione assi rotanti deve essere Z Y Y Z Y Z
+     // direzione assi rotanti deve essere Z Y Y X Y X
       if ( ! m_vCalcRotAx[0].vtDir.IsZ()  ||
            ! m_vCalcRotAx[1].vtDir.IsY()  ||
            ! m_vCalcRotAx[2].vtDir.IsY()  ||
-           ! m_vCalcRotAx[3].vtDir.IsZ()  ||
+           ! m_vCalcRotAx[3].vtDir.IsX()  ||
            ! m_vCalcRotAx[4].vtDir.IsY()  ||
-           ! m_vCalcRotAx[5].vtDir.IsZ()) {
-         LOG_ERROR( GetEMkLogger(), "Robot with errors in Rotary Axes : not ZYY-ZYZ")
+           ! m_vCalcRotAx[5].vtDir.IsX()) {
+         LOG_ERROR( GetEMkLogger(), "Robot with errors in Rotary Axes : not ZYY-XYX")
          return false ;
       }
      // verifico che gli ultimi 3 assi formino un polso sferico (ovvero passino per uno stesso punto)
-      if ( ! AreSamePointXYApprox( m_vCalcRotAx[3].ptPos, m_vCalcRotAx[5].ptPos)  ||
-           abs( m_vCalcRotAx[4].ptPos.x - m_vCalcRotAx[3].ptPos.x) > EPS_SMALL) {
+      if ( abs( m_vCalcRotAx[3].ptPos.y - m_vCalcRotAx[5].ptPos.y) > EPS_SMALL  ||
+           abs( m_vCalcRotAx[3].ptPos.z - m_vCalcRotAx[5].ptPos.z) > EPS_SMALL  ||
+           abs( m_vCalcRotAx[4].ptPos.z - m_vCalcRotAx[3].ptPos.z) > EPS_SMALL) {
          LOG_ERROR( GetEMkLogger(), "Robot with errors in Rotary Axes : not spherical Wrist")
          return false ;
       }
@@ -2052,7 +2053,7 @@ Machine::GetRobotAngles( const Point3d& ptP, const Vector3d& vtDirT, const Vecto
    double dDistR2R3 = m_vCalcRotAx[2].ptPos.z - m_vCalcRotAx[1].ptPos.z ;
    Vector3d vtR3R5 = m_vCalcRotAx[4].ptPos - m_vCalcRotAx[2].ptPos ; vtR3R5.y = 0 ;
    double dDistR3R5 = vtR3R5.Len() ;
-   double dAngR3R5 = atan2( -vtR3R5.x, vtR3R5.z) * RADTODEG ;
+   double dAngR3R5 = atan2( vtR3R5.z, vtR3R5.x) * RADTODEG ;
    double dCosB = ( dDistR2R3 * dDistR2R3 + dDistR2Cen * dDistR2Cen - dDistR3R5 * dDistR3R5) / ( 2 * dDistR2R3 * dDistR2Cen) ;
    double dCosC = ( dDistR3R5 * dDistR3R5 + dDistR2Cen * dDistR2Cen - dDistR2R3 * dDistR2R3) / ( 2 * dDistR3R5 * dDistR2Cen) ;
    if ( abs( dCosB) > 1  ||  abs( dCosC) > 1) {
@@ -2069,11 +2070,12 @@ Machine::GetRobotAngles( const Point3d& ptP, const Vector3d& vtDirT, const Vecto
    vAng1.push_back( dAng2) ;
    vAng2.push_back( dAng2) ;
    double dAngC = acos( dCosC) * RADTODEG ;
-   double dAng3 = dAngB + dAngC + dAngR3R5 ;
+   double dAng3 = dAngB + dAngC + dAngR3R5 - ANG_RIGHT ;
    vAng1.push_back( dAng3) ;
    vAng2.push_back( dAng3) ;
   // calcolo i primi due assi rotanti del polso
-   Frame3d frWrist( m_vCalcRotAx[3].ptPos) ;
+   Frame3d frWrist ;
+   frWrist.Set( m_vCalcRotAx[3].ptPos, m_vCalcRotAx[3].vtDir, m_vCalcRotAx[4].vtDir ^ m_vCalcRotAx[3].vtDir) ;
    frWrist.Rotate( m_vCalcRotAx[2].ptPos, m_vCalcRotAx[2].vtDir, dAng3) ;
    frWrist.Rotate( m_vCalcRotAx[1].ptPos, m_vCalcRotAx[1].vtDir, dAng2) ;
    frWrist.Rotate( m_vCalcRotAx[0].ptPos, m_vCalcRotAx[0].vtDir, dAng1) ;