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

EgtMachKernel :

Browse Source 
- modifiche per configurazione canonica robot da ZYY-ZYZ a ZYY-XYX.
This commit is contained in:
Dario Sassi
2024-02-20 10:52:23 +01:00
parent 09bdd4852d
commit 737124b0bd
1 changed files with 11 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MachineCalc.cpp
+11 -9
View File
@@ -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) ;