Egalware/EgtMachKernel
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
Preview
EgtMachKernel/MachineCalc.cpp
T
Dario Sassi 2e5caf2def EgtMachKernel : 
- da GetPositions di Machine eliminato parametro nStat inutile
- da GetCalcPositions di MachMgr eliminato lo stesso parametro
- nei robot ora si assegna sempre BackAuxDir
- migliorato calcolo angoli nei robot.
2025-09-14 23:07:30 +02:00

2371 lines
88 KiB
C++
Raw Permalink Blame History

//----------------------------------------------------------------------------
// EgalTech 2015-2015
//----------------------------------------------------------------------------
// File : MachineCalc.cpp Data : 12.05.15 Versione : 1.6e3
// Contenuto : Implementazione gestione macchina : funzioni di calcolo.
//
//
//
// Modifiche : 12.05.15 DS Creazione modulo.
//
//
//----------------------------------------------------------------------------
//--------------------------- Include ----------------------------------------
#include "stdafx.h"
#include "MachMgr.h"
#include "GeoCalc.h"
#include "DllMain.h"
#include "Table.h"
#include "Axis.h"
#include "Head.h"
#include "Exit.h"
#include "/EgtDev/Include/EMkToolConst.h"
#include "/EgtDev/Include/EGkGeoVector3d.h"
#include "/EgtDev/Include/EGnStringUtils.h"
#include "/EgtDev/Include/EGnFileUtils.h"
#include "/EgtDev/Include/EgtNumUtils.h"
using namespace std ;
//----------------------------------------------------------------------------
static const string EMC_VAR = "EMC" ; // tabella variabili locali per calcolo
static const string EVAR_VER = ".VER" ; // (string) versione della Dll
static const string EVAR_TABNAME = ".TABNAME" ; // (string) nome della tavola macchina
static const string EVAR_HEAD = ".HEAD" ; // (string) nome della testa
static const string EVAR_EXIT = ".EXIT" ; // (int) numero dell'uscita
static const string EVAR_TOOL = ".TOOL" ; // (string) nome dell'utensile
static const string EVAR_TOTDIAM = ".TOTDIAM" ; // (num) diametro di ingombro dell'utensile
static const string EVAR_TOTLEN = ".TOTLEN" ; // (num) lunghezza di ingombro dell'utensile
static const string EVAR_DIST = ".DIST" ; // (num) distanza dell'utensile (per seghe a catena)
static const string EVAR_EXITPOS = ".EXITPOS" ; // (point) posizione attuale dell'uscita
static const string EVAR_USERNOTES = ".USERNOTES" ; // (string) note utente dell'utensile
static const string EVAR_TCPOS = ".TCPOS" ; // (string) posizione nell'attrezzaggio
static const string EVAR_L1 = ".L1" ; // (num) valore del primo asse lineare
static const string EVAR_L2 = ".L2" ; // (num) valore del secondo asse lineare
static const string EVAR_L3 = ".L3" ; // (num) valore del terzo asse lineare
static const string EVAR_R1 = ".R1" ; // (num) valore del primo asse rotante
static const string EVAR_R2 = ".R2" ; // (num) valore del secondo asse rotante
static const string EVAR_R3 = ".R3" ; // (num) valore del terzo asse rotante
static const string EVAR_R4 = ".R4" ; // (num) valore del quarto asse rotante
static const string EVAR_ERROR = ".ERR" ; // OUT (int) codice di errore ( 0 = ok)
static const string EVAR_STAT = ".STAT" ; // OUT (int) codice di stato ( 0 = ok)
static const string EVAR_AUXINFO = ".AUXINFO" ; // OUT (string) stringa con info ausiliarie
static const string EMC_VAR_BACKUP = "QQQ_EMC" ; // nome del backup della tabella sopra indicata
static const string AXIS_NAME_PROTECTEDAREAS = "PRA" ;
static const string ON_SET_TABLE = "OnSetTable" ;
static const string ON_SET_HEAD = "OnSetHead" ;
static const string ON_VERIFY_PROTECTEDAREAS = "OnVerifyProtectedAreas" ;
//----------------------------------------------------------------------------
bool
Machine::SetCurrTable( const string& sTable)
{
// controllo GeomDB
if ( m_pGeomDB == nullptr)
return false ;
// recupero il gruppo della tavola
m_nCalcTabId = GetGroup( sTable) ;
if ( m_nCalcTabId == GDB_ID_NULL || ! IsTableGroup( m_nCalcTabId)) {
m_nCalcTabId = GDB_ID_NULL ;
return false ;
}
// il gruppo tavola corrente deve essere sempre visibile
m_pGeomDB->SetStatus( m_nCalcTabId, GDB_ST_ON) ;
// lancio eventuale funzione lua di personalizzazione
if ( LuaExistsFunction( ON_SET_TABLE)) {
// salvo eventuale variabile EMC_VAR già presente
bool bOldEMC = LuaChangeNameGlobVar( EMC_VAR, EMC_VAR_BACKUP) ;
// definisco variabili
bool bOk = LuaCreateGlobTable( EMC_VAR) ;
bOk = bOk && LuaSetGlobVar( EMC_VAR + EVAR_VER, GetEMkVer()) ;
bOk = bOk && LuaSetGlobVar( EMC_VAR + EVAR_TABNAME, sTable) ;
// chiamo funzione
bOk = bOk && LuaCallFunction( ON_SET_TABLE) ;
// reset variabili
bOk = bOk && LuaResetGlobVar( EMC_VAR) ;
// ripristino eventuale variabile EMC_VAR già presente
if ( bOldEMC)
LuaChangeNameGlobVar( EMC_VAR_BACKUP, EMC_VAR) ;
// restituisco risultato
return bOk ;
}
else
return true ;
}
//----------------------------------------------------------------------------
bool
Machine::ResetCurrTable( void)
{
m_nCalcTabId = GDB_ID_NULL ;
return true ;
}
//----------------------------------------------------------------------------
int
Machine::GetCurrTable( void) const
{
// controllo GeomDB
if ( m_pGeomDB == nullptr)
return GDB_ID_NULL ;
// recupero identificativo della tavola corrente
return m_nCalcTabId ;
}
//----------------------------------------------------------------------------
bool
Machine::GetCurrTable( string& sTable) const
{
// controllo GeomDB
if ( m_pGeomDB == nullptr)
return false ;
// recupero nome della tavola corrente
return m_pGeomDB->GetName( m_nCalcTabId, sTable) ;
}
//----------------------------------------------------------------------------
bool
Machine::GetCurrTableRef1( Point3d& ptRef1) const
{
Table* pTab = GetTable( m_nCalcTabId) ;
if ( pTab == nullptr)
return false ;
ptRef1 = pTab->GetRef1() ;
return true ;
}
//----------------------------------------------------------------------------
bool
Machine::GetCurrTableArea1( BBox3d& b3Area1) const
{
Table* pTab = GetTable( m_nCalcTabId) ;
if ( pTab == nullptr)
return false ;
b3Area1 = pTab->GetArea1() ;
return true ;
}
//----------------------------------------------------------------------------
bool
Machine::GetCurrTableDeltaRef1( Vector3d& vtDelta1) const
{
Table* pTab = GetTable( m_nCalcTabId) ;
if ( pTab == nullptr)
return false ;
// recupero la posizione corrente del riferimento 1 della tavola
// riferimento globale del gruppo tavola
Frame3d frTable ;
m_pGeomDB->GetGroupGlobFrame( m_nCalcTabId, frTable) ;
// recupero il primo riferimento della tavola
int nRef1 = m_pGeomDB->GetFirstNameInGroup( m_nCalcTabId, MCH_TREF + "1") ;
if ( nRef1 == GDB_ID_NULL || m_pGeomDB->GetGeoType( nRef1) != GEO_FRAME3D)
return false ;
// recupero frame
const Frame3d& frFrame = GetGeoFrame3d( m_pGeomDB->GetGeoObj( nRef1))->GetFrame() ;
// ne calcolo l'origine in globale
Point3d ptPos = frFrame.Orig() ;
ptPos.ToGlob( frTable) ;
// calcolo il delta
vtDelta1 = ptPos - pTab->GetRef1() ;
return true ;
}
//----------------------------------------------------------------------------
bool
Machine::GetCurrTableIsTilting( bool& bTilting, Vector3d& vtTiltingAx) const
{
// default
bTilting = false ;
vtTiltingAx = V_NULL ;
// verifico esistenza tavola
if ( m_nCalcTabId == GDB_ID_NULL)
return false ;
// verifico se presente flag che lo forza come tale
int nTiltingLike ;
if ( m_pGeomDB->GetInfo( m_nCalcTabId, MCH_TILTINGLIKE, nTiltingLike) && nTiltingLike != 0) {
bTilting = true ;
switch ( abs( nTiltingLike)) {
default : vtTiltingAx = X_AX ; break ;
case 2 : vtTiltingAx = Y_AX ; break ;
case 3 : vtTiltingAx = Z_AX ; break ;
}
return true ;
}
// recupero gli eventuali assi rotanti della tavola
int nTParId = m_pGeomDB->GetParentId( m_nCalcTabId) ;
if ( nTParId == GDB_ID_NULL)
return false ;
while ( IsAxisGroup( nTParId)) {
// recupero il gestore dell'asse
Axis* pAx = GetAxis( nTParId) ;
if ( pAx == nullptr)
return false ;
// verifico se asse rotante orizzontale
if ( pAx->GetType() == MCH_AT_ROTARY && abs( pAx->GetDir().z) < EPS_SMALL) {
bTilting = true ;
vtTiltingAx = pAx->GetDir() ;
return true ;
}
// risalgo lungo la catena
nTParId = m_pGeomDB->GetParentId( nTParId) ;
}
return true ;
}
//----------------------------------------------------------------------------
bool
Machine::GetCurrTableCollGroups( INTVECTOR& vIds) const
{
// controllo GeomDB
if ( m_pGeomDB == nullptr)
return false ;
// recupero la tavola corrente
Table* pTab = GetTable( m_nCalcTabId) ;
if ( pTab == nullptr)
return false ;
// recupero stringhe con gruppi ausiliari di collisione
const STRVECTOR& vsColl = pTab->GetCollGroups() ;
for ( const auto& sColl : vsColl) {
string sGrp, sSub ;
Split( sColl, "/", true, sGrp, sSub) ;
int nId = GetGroup( sGrp) ;
if ( ! sSub.empty() && nId != GDB_ID_NULL)
nId = m_pGeomDB->GetFirstNameInGroup( nId, sSub) ;
if ( nId != GDB_ID_NULL)
vIds.push_back( nId) ;
}
return true ;
}
//----------------------------------------------------------------------------
bool
Machine::SetCurrTool( const string& sTool, const string& sHead, int nExit)
{
// azzero tutto
ResetCurrTool() ;
// controllo GeomDB
if ( m_pGeomDB == nullptr)
return false ;
// recupero il gruppo della testa
int nHeadId = GetGroup( sHead) ;
// recupero i dati della testa
Head* pHead = GetHead( nHeadId) ;
if ( pHead == nullptr) {
string sOutHead = ( IsEmptyOrSpaces( sHead) ? "???" : sHead) ;
string sOutTool = ( IsEmptyOrSpaces( sTool) ? "???" : sTool) ;
string sOut = "Missing head " + sOutHead + " for tool " + sOutTool ;
LOG_ERROR( GetEMkLogger(), sOut.c_str())
return false ;
}
// recupero il gruppo dell'uscita
string sExit = MCH_EXIT + ToString( nExit) ;
int nExitId = m_pGeomDB->GetFirstNameInGroup( nHeadId, sExit) ;
// recupero i dati dell'uscita
Exit* pExit = GetExit( nExitId) ;
if ( pExit == nullptr) {
string sOut = "Missing exit " + ToString( nExit) + " on head " + sHead ;
LOG_ERROR( GetEMkLogger(), sOut.c_str())
return false ;
}
// recupero i dati dell'utensile
int nToolId = GDB_ID_NULL ;
double dTLen = 0 ;
double dTDiam = 0 ;
double dTOvLen = 0 ;
double dTOvDiam = 0 ;
double dTDist = 0 ;
string sTUserNotes = "" ;
string sTcPos = "" ;
// se definito
if ( ! sTool.empty()) {
// carico anche gli utensili su eventuali altre uscite della testa
LoadTools( sHead, nExit) ;
// carico l'utensile (si sostituisce ad altro già presente sulla stessa uscita)
if ( ! LoadTool( sHead, nExit, sTool)) {
string sOut = "Missing tool " + sTool ;
LOG_ERROR( GetEMkLogger(), sOut.c_str())
return false ;
}
nToolId = m_pGeomDB->GetFirstNameInGroup( nExitId, sTool) ;
if ( nToolId == GDB_ID_NULL || m_pGeomDB->GetGdbType( nToolId) != GDB_TY_GROUP)
return false ;
if ( ! m_pMchMgr->TdbSetCurrTool( sTool) ||
! m_pMchMgr->TdbGetCurrToolParam( TPA_LEN, dTLen) ||
! m_pMchMgr->TdbGetCurrToolParam( TPA_DIAM, dTDiam) ||
! m_pMchMgr->TdbGetCurrToolParam( TPA_TOTLEN, dTOvLen) ||
! m_pMchMgr->TdbGetCurrToolParam( TPA_TOTDIAM, dTOvDiam) ||
! m_pMchMgr->TdbGetCurrToolParam( TPA_USERNOTES, sTUserNotes))
return false ;
m_pMchMgr->TdbGetCurrToolParam( TPA_DIST, dTDist) ; // opzionale
if ( ! m_pMchMgr->GetCurrSetupMgr().GetToolSetupPos( sTool, sTcPos))
m_pMchMgr->TdbGetCurrToolParam( TPA_TCPOS, sTcPos) ;
}
// altrimenti casi speciali senza utensile
else {
// tolgo eventuale utensile dalla testa
if ( ! ResetHeadSet( sHead))
return false ;
nToolId = GDB_ID_NULL ;
dTLen = 0 ;
dTDiam = 0 ;
dTOvLen = 0 ;
dTOvDiam = 0 ;
dTDist = 0 ;
sTcPos = "" ;
m_pMchMgr->TdbSetCurrTool( "") ;
}
// assegno tutti i dati
m_nCalcHeadId = nHeadId ;
m_nCalcExitId = nExitId ;
m_nCalcToolId = nToolId ;
m_dCalcRot1W = pHead->GetRot1W() ;
m_bCalcMaxDeltaR2On1 = pHead->GetMaxDeltaR2On1() ;
m_nCalcSolCh = pHead->GetSolCh() ;
m_ptCalcPos = pExit->GetPos() ;
m_vtCalcDir = pExit->GetTDir() ;
m_vtCalcADir = pHead->GetADir() ;
m_dCalcTLen = dTLen ;
m_dCalcTRad = dTDiam / 2 ;
m_dCalcTOvLen = dTOvLen ;
m_dCalcTOvRad = dTOvDiam / 2 ;
// lancio eventuale funzione lua di personalizzazione
if ( LuaExistsFunction( ON_SET_HEAD)) {
// salvo eventuale variabile EMC_VAR già presente
bool bOldEMC = LuaChangeNameGlobVar( EMC_VAR, EMC_VAR_BACKUP) ;
// definisco variabili
bool bOk = LuaCreateGlobTable( EMC_VAR) ;
bOk = bOk && LuaSetGlobVar( EMC_VAR + EVAR_VER, GetEMkVer()) ;
bOk = bOk && LuaSetGlobVar( EMC_VAR + EVAR_HEAD, sHead) ;
bOk = bOk && LuaSetGlobVar( EMC_VAR + EVAR_EXIT, nExit) ;
bOk = bOk && LuaSetGlobVar( EMC_VAR + EVAR_TOOL, sTool) ;
bOk = bOk && LuaSetGlobVar( EMC_VAR + EVAR_EXITPOS, m_ptCalcPos) ;
bOk = bOk && LuaSetGlobVar( EMC_VAR + EVAR_TOTDIAM, dTOvDiam) ;
bOk = bOk && LuaSetGlobVar( EMC_VAR + EVAR_TOTLEN, dTOvLen) ;
bOk = bOk && LuaSetGlobVar( EMC_VAR + EVAR_DIST, dTDist) ;
bOk = bOk && LuaSetGlobVar( EMC_VAR + EVAR_USERNOTES, sTUserNotes) ;
bOk = bOk && LuaSetGlobVar( EMC_VAR + EVAR_TCPOS, sTcPos) ;
// chiamo funzione
bOk = bOk && LuaCallFunction( ON_SET_HEAD) ;
// reset variabili
bOk = bOk && LuaResetGlobVar( EMC_VAR) ;
// ripristino eventuale variabile EMC_VAR già presente
if ( bOldEMC)
LuaChangeNameGlobVar( EMC_VAR_BACKUP, EMC_VAR) ;
// in caso di errore esco
if ( ! bOk)
return false ;
// aggiorno
m_ptCalcPos = pExit->GetPos() ;
}
// determino la catena cinematica
return CalculateKinematicChain() ;
}
//----------------------------------------------------------------------------
bool
Machine::ResetCurrTool( void)
{
// azzero tutto
m_nCalcHeadId = GDB_ID_NULL ;
m_nCalcExitId = GDB_ID_NULL ;
m_nCalcToolId = GDB_ID_NULL ;
m_dCalcRot1W = ROT1_WEIGHT_DFLT ;
m_dSingConeAng = SING_CONE_ANG_DFLT ;
m_bCalcMaxDeltaR2On1 = true ;
m_nCalcSolCh = MCH_SCC_NONE ;
m_dCalcTLen = 0 ;
m_dCalcTRad = 0 ;
m_dCalcTOvLen = 0 ;
m_dCalcTOvRad = 0 ;
// ritorno
return true ;
}
//----------------------------------------------------------------------------
int
Machine::GetCurrTool( void) const
{
// controllo GeomDB
if ( m_pGeomDB == nullptr)
return GDB_ID_NULL ;
// recupero identificativo dell'utensile
return m_nCalcToolId ;
}
//----------------------------------------------------------------------------
bool
Machine::GetCurrTool( string& sTool) const
{
// controllo GeomDB
if ( m_pGeomDB == nullptr)
return false ;
// recupero nome gruppo dell'utensile
return m_pGeomDB->GetName( m_nCalcToolId, sTool) ;
}
//----------------------------------------------------------------------------
int
Machine::GetCurrHead( void) const
{
// controllo GeomDB
if ( m_pGeomDB == nullptr)
return GDB_ID_NULL ;
// recupero identificativo della testa
return m_nCalcHeadId ;
}
//----------------------------------------------------------------------------
bool
Machine::GetCurrHead( string& sHead) const
{
// controllo GeomDB
if ( m_pGeomDB == nullptr)
return false ;
// recupero nome gruppo della testa
return m_pGeomDB->GetName( m_nCalcHeadId, sHead) ;
}
//----------------------------------------------------------------------------
int
Machine::GetCurrExit( void) const
{
// controllo GeomDB
if ( m_pGeomDB == nullptr)
return GDB_ID_NULL ;
// recupero identificativo dell'uscita
return m_nCalcExitId ;
}
//----------------------------------------------------------------------------
bool
Machine::GetCurrExit( int& nExit) const
{
// controllo GeomDB
if ( m_pGeomDB == nullptr)
return GDB_ID_NULL ;
// recupero nome gruppo dell'uscita
string sExit ;
if ( ! m_pGeomDB->GetName( m_nCalcExitId, sExit))
return false ;
// recupero indice dell'uscita
TrimLeft( sExit, MCH_EXIT.c_str()) ;
return FromString( sExit, nExit) ;
}
//----------------------------------------------------------------------------
bool
Machine::GetCurrHeadCollGroups( INTVECTOR& vIds) const
{
// controllo GeomDB
if ( m_pGeomDB == nullptr)
return false ;
// recupero la testa corrente
Head* pHead = GetHead( m_nCalcHeadId) ;
if ( pHead == nullptr)
return false ;
vIds.push_back( m_nCalcHeadId) ;
// recupero stringhe con gruppi ausiliari di collisione
const STRVECTOR& vsOthColl = pHead->GetOtherCollGroups() ;
for ( const auto& sOthColl : vsOthColl) {
string sGrp, sSub ;
Split( sOthColl, "/", true, sGrp, sSub) ;
int nId = GetGroup( sGrp) ;
if ( ! sSub.empty() && nId != GDB_ID_NULL)
nId = m_pGeomDB->GetFirstNameInGroup( nId, sSub) ;
if ( nId != GDB_ID_NULL)
vIds.push_back( nId) ;
}
return true ;
}
//----------------------------------------------------------------------------
bool
Machine::IsCurrToolFloating( void) const
{
// controllo GeomDB
if ( m_pGeomDB == nullptr)
return false ;
// leggo info con tipo
string sType ;
return ( m_pGeomDB->GetInfo( m_nCalcToolId, TTH_TYPE, sType) && sType == TTH_TYPE_FLOAT) ;
}
//----------------------------------------------------------------------------
bool
Machine::ClearKinematicChain( void)
{
// azzero tutti gli assi della catena cinematica
m_nTabLinAxes = 0 ;
m_nTabRotAxes = 0 ;
m_nHeadLinAxes = 0 ;
m_nHeadRotAxes = 0 ;
m_nHeadSpecRotAxis = -1 ;
m_vCalcLinAx.clear() ;
m_vCalcRotAx.clear() ;
m_frLinAx.Reset( false) ;
m_frRobot.Reset( false) ;
m_nCalcChainType = KIN_CHAIN_NONE ;
return true ;
}
//----------------------------------------------------------------------------
bool
Machine::CalculateKinematicChain( void)
{
// controllo GeomDB
if ( m_pGeomDB == nullptr)
return false ;
// azzero tutti gli assi della catena cinematica
ClearKinematicChain() ;
// recupero gli assi di tavola
if ( m_nCalcTabId == GDB_ID_NULL)
return false ;
int nTParId = m_pGeomDB->GetParentId( m_nCalcTabId) ;
if ( nTParId == GDB_ID_NULL)
return false ;
while ( IsAxisGroup( nTParId)) {
if ( ! AddKinematicAxis( false, nTParId))
return false ;
nTParId = m_pGeomDB->GetParentId( nTParId) ;
}
// recupero gli assi di testa
if ( m_nCalcHeadId == GDB_ID_NULL)
return false ;
int nHParId = m_pGeomDB->GetParentId( m_nCalcHeadId) ;
if ( nHParId == GDB_ID_NULL)
return false ;
while ( IsAxisGroup( nHParId)) {
if ( ! AddKinematicAxis( true, nHParId))
return false ;
nHParId = m_pGeomDB->GetParentId( nHParId) ;
}
// se non ci sono assi, né lineari né rotanti, sicuramente errore
if ( m_nTabLinAxes == 0 && m_nHeadLinAxes == 0 &&
m_nTabRotAxes == 0 && m_nHeadRotAxes == 0) {
LOG_ERROR( GetEMkLogger(), "Errors in Axes : none have been found")
return false ;
}
// se nessun asse lineare deve essere un robot (in futuro andrà permesso un lineare di testa o di tavola)
if ( m_nTabLinAxes == 0 && m_nHeadLinAxes == 0) {
// se 6 assi rotanti tutti di testa
if ( m_nTabRotAxes == 0 && m_nHeadRotAxes == 6) {
// riordino gli assi rotanti
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() ||
! m_vCalcRotAx[2].vtDir.IsY() ||
! m_vCalcRotAx[3].vtDir.IsX() ||
! m_vCalcRotAx[4].vtDir.IsY() ||
! 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 ( 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 ;
}
// calcolo il centro del polso in coordinate globali (R5.x, R6.y, R5.z)
Point3d ptCenG( m_vCalcRotAx[4].ptPos.x, m_vCalcRotAx[5].ptPos.y, m_vCalcRotAx[4].ptPos.z) ;
// recupero il riferimento dell'uscita (da posizione, direzione utensile e direzione ausiliaria)
Frame3d frExit ;
if ( ! frExit.Set( m_ptCalcPos, m_vtCalcDir, m_vtCalcADir))
return false ;
// calcolo il centro del polso in locale a questo riferimento
m_ptWristCen = GetToLoc( ptCenG, frExit) ;
// calcolo la direzione di riferimento del polso in locale a questo riferimento
m_vtWristRef = GetToLoc( m_vCalcRotAx[5].vtDir, frExit) ;
// dichiaro tipo robot
m_nCalcChainType = KIN_CHAIN_ROBOT ;
return true ;
}
// se altrimenti 6 assi rotanti tutti di tavola
else if ( m_nTabRotAxes == 6 && m_nHeadRotAxes == 0) {
// riordino gli assi rotanti
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_nTabRotAxes ; ++ 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() ||
! m_vCalcRotAx[2].vtDir.IsY() ||
! m_vCalcRotAx[3].vtDir.IsX() ||
! m_vCalcRotAx[4].vtDir.IsY() ||
! 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 ( 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 ;
}
// calcolo il centro del polso in coordinate globali (R5.x, R6.y, R5.z)
Point3d ptCenG( m_vCalcRotAx[4].ptPos.x, m_vCalcRotAx[5].ptPos.y, m_vCalcRotAx[4].ptPos.z) ;
// recupero il riferimento dell'uscita (da posizione, direzione utensile e direzione ausiliaria)
Frame3d frExit ;
if ( ! frExit.Set( m_ptCalcPos, m_vtCalcDir, m_vtCalcADir))
return false ;
// calcolo il centro del polso in locale a questo riferimento
m_ptWristCen = GetToLoc( ptCenG, frExit) ;
// calcolo la direzione di riferimento del polso in locale a questo riferimento
m_vtWristRef = GetToLoc( m_vCalcRotAx[5].vtDir, frExit) ;
// dichiaro tipo robot
m_nCalcChainType = KIN_CHAIN_ROBOT ;
return true ;
}
// altrimenti errore
else {
LOG_ERROR( GetEMkLogger(), "Robot with errors in Rotary Axes : number or type")
return false ;
}
}
// verifiche sugli assi lineari :
// aggiusto gli indici di ordine sulla sua catena cinematica (1-based)
for ( int i = 0 ; i < int( m_vCalcLinAx.size()) ; ++ i) {
if ( m_vCalcLinAx[i].bHead)
m_vCalcLinAx[i].nInd += m_nHeadLinAxes + m_nHeadRotAxes + 1 ;
else
m_vCalcLinAx[i].nInd *= -1 ;
}
// devono essere 3
if ( m_vCalcLinAx.size() != 3) {
LOG_ERROR( GetEMkLogger(), "Linear Axes are not 3 in number")
return false ;
}
// devono essere ordinabili come XYZ
if ( ! m_vCalcLinAx[0].vtDir.IsX()) {
if ( m_vCalcLinAx[1].vtDir.IsX())
swap( m_vCalcLinAx[0], m_vCalcLinAx[1]) ;
else if ( m_vCalcLinAx[2].vtDir.IsX())
swap( m_vCalcLinAx[0], m_vCalcLinAx[2]) ;
else {
LOG_ERROR( GetEMkLogger(), "Linear Axes are not aligned with Global XYZ")
return false ;
}
}
if ( ! m_vCalcLinAx[1].vtDir.IsY()) {
if ( m_vCalcLinAx[2].vtDir.IsY())
swap( m_vCalcLinAx[1], m_vCalcLinAx[2]) ;
else {
LOG_ERROR( GetEMkLogger(), "Linear Axes are not aligned with Global XYZ")
return false ;
}
}
if ( ! m_frLinAx.Set( ORIG, m_vCalcLinAx[0].vtDir, m_vCalcLinAx[1].vtDir, m_vCalcLinAx[2].vtDir)) {
LOG_ERROR( GetEMkLogger(), "Linear Axes are not a Rigth-handed Frame")
return false ;
}
// verifiche sugli assi rotanti :
// aggiusto gli indici di ordine sulla sua catena cinematica (1-based)
for ( int i = 0 ; i < int( m_vCalcRotAx.size()) ; ++ i) {
if ( m_vCalcRotAx[i].bHead)
m_vCalcRotAx[i].nInd += m_nHeadLinAxes + m_nHeadRotAxes + 1 ;
else
m_vCalcRotAx[i].nInd *= -1 ;
}
// se 0 o 1 va bene
if ( m_vCalcRotAx.size() <= 1)
;
// se 2 va bene
else if ( m_vCalcRotAx.size() == 2) {
// se entrambi di testa devo invertirne l'ordine
if ( m_vCalcRotAx[0].bHead && m_vCalcRotAx[1].bHead)
swap( m_vCalcRotAx[0], m_vCalcRotAx[1]) ;
// impongo limiti di corsa sul secondo asse rotante di testa
if ( m_vCalcRotAx[1].bHead) {
Head* pHead = GetHead( m_nCalcHeadId) ;
if ( pHead != nullptr) {
m_vCalcRotAx[1].stroke.Min = max( m_vCalcRotAx[1].stroke.Min, pHead->GetRot2Stroke().Min) ;
m_vCalcRotAx[1].stroke.Max = min( m_vCalcRotAx[1].stroke.Max, pHead->GetRot2Stroke().Max) ;
}
}
}
// se 3 va bene ( uno dovrà poi avere valore assegnato)
else if ( m_vCalcRotAx.size() == 3) {
int n2ndHeadRotAx = - 1 ;
// se tutti e tre di testa, devo invertire il primo con il terzo
if ( m_vCalcRotAx[0].bHead && m_vCalcRotAx[1].bHead && m_vCalcRotAx[2].bHead) {
swap( m_vCalcRotAx[0], m_vCalcRotAx[2]) ;
n2ndHeadRotAx = 1 ;
}
// se altrimenti ultimi due di testa, devo invertirne l'ordine
else if ( m_vCalcRotAx[1].bHead && m_vCalcRotAx[2].bHead) {
swap( m_vCalcRotAx[1], m_vCalcRotAx[2]) ;
n2ndHeadRotAx = 2 ;
}
// impongo limiti di corsa sul secondo asse rotante di testa
if ( n2ndHeadRotAx > 0) {
Head* pHead = GetHead( m_nCalcHeadId) ;
if ( pHead != nullptr) {
m_vCalcRotAx[n2ndHeadRotAx].stroke.Min = max( m_vCalcRotAx[n2ndHeadRotAx].stroke.Min, pHead->GetRot2Stroke().Min) ;
m_vCalcRotAx[n2ndHeadRotAx].stroke.Max = min( m_vCalcRotAx[n2ndHeadRotAx].stroke.Max, pHead->GetRot2Stroke().Max) ;
}
}
}
// se più di 3
else {
// altrimenti non ancora gestito, quindi errore
LOG_ERROR( GetEMkLogger(), "Rotary Axes not manageable")
return false ;
}
// verifico esistenza eventuale asse rotante speciale di testa
if ( m_nHeadRotAxes > 0 && m_nHeadLinAxes > 0) {
// indice di posizione primo asse di testa
int nHeadFirst = 1 ;
// ricerco sui rotanti
for ( int i = 0 ; i < int( m_vCalcRotAx.size()) ; ++ i) {
// se asse di testa
if ( m_vCalcRotAx[i].bHead && m_vCalcRotAx[i].nInd <= nHeadFirst) {
// non sono ammessi due assi di questo tipo
if ( m_nHeadSpecRotAxis != -1)
return false ;
// la tavola non deve avere più di un asse lineare
if ( m_nTabLinAxes > 1)
return false ;
// se ha un asse lineare deve essere allineato con il rotante speciale
else if ( m_nTabLinAxes == 1) {
// ne recupero la direzione
Vector3d vtTabLinDir ;
for ( int j = 0 ; j < int( m_vCalcLinAx.size()) ; ++ j) {
if ( ! m_vCalcLinAx[i].bHead) {
vtTabLinDir = m_vCalcLinAx[i].vtDir ;
break ;
}
}
// la confronto con quella dell'asse rotante speciale
if ( ! AreSameOrOppositeVectorApprox( vtTabLinDir, m_vCalcRotAx[i].vtDir))
return false ;
}
// ne salvo l'indice
m_nHeadSpecRotAxis = i ;
// incremento indice di posizione posibile primo asse lineare di testa
++ nHeadFirst ;
}
}
}
// dichiaro tipo centro di lavoro
m_nCalcChainType = KIN_CHAIN_MCENT ;
return true ;
}
//----------------------------------------------------------------------------
bool
Machine::AddKinematicAxis( bool bOnHead, int nId)
{
// controllo GeomDB
if ( m_pGeomDB == nullptr)
return false ;
// recupero il gestore dell'asse
Axis* pAx = GetAxis( nId) ;
if ( pAx == nullptr)
return false ;
// ne recupero i dati
KinAxis kAx ;
kAx.nGrpId = nId ;
kAx.bLinear = ( pAx->GetType() != MCH_AT_ROTARY) ;
kAx.bHead = bOnHead ; // catena cinematica di appartenenza (testa o tavola)
kAx.ptPos = pAx->GetPos() ;
kAx.vtDir = pAx->GetDir() ;
kAx.stroke = pAx->GetStroke() ;
kAx.dHomeVal = pAx->GetHomeVal() ;
// ne determino l'indice di posizione nella sua catena cinematica (assegno valore negato perchè provvisorio)
if ( kAx.bHead) {
( kAx.bLinear ? ++ m_nHeadLinAxes : ++ m_nHeadRotAxes) ;
kAx.nInd = - ( m_nHeadLinAxes + m_nHeadRotAxes) ;
}
else {
( kAx.bLinear ? ++ m_nTabLinAxes : ++ m_nTabRotAxes) ;
kAx.nInd = - ( m_nTabLinAxes + m_nTabRotAxes) ;
}
// se lineare di tavola, devo invertirlo
if ( kAx.bLinear && ! kAx.bHead)
kAx.vtDir.Invert() ;
// lo inserisco nella opportuna lista degli assi
if ( kAx.bLinear)
m_vCalcLinAx.emplace_back( kAx) ;
else
m_vCalcRotAx.emplace_back( kAx) ;
return true ;
}
//----------------------------------------------------------------------------
string
Machine::GetKinematicAxis( int nInd) const
{
// controllo GeomDB
if ( m_pGeomDB == nullptr)
return "" ;
// controllo indice
int nLinAxTot = int( m_vCalcLinAx.size()) ;
int nRotAxTot = int( m_vCalcRotAx.size()) ;
if ( nInd < 0 || nInd >= nLinAxTot + nRotAxTot)
return "" ;
// recupero nome
string sName ;
if ( nInd < nLinAxTot)
m_pGeomDB->GetName( m_vCalcLinAx[nInd].nGrpId, sName) ;
else
m_pGeomDB->GetName( m_vCalcRotAx[nInd-nLinAxTot].nGrpId, sName) ;
return sName ;
}
//----------------------------------------------------------------------------
bool
Machine::BlockKinematicRotAxis( const string& sName, double dVal)
{
return BlockKinematicRotAxis( GetAxisId( sName), dVal) ;
}
//----------------------------------------------------------------------------
bool
Machine::BlockKinematicRotAxis( int nId, double dVal)
{
// verifico identificativo
if ( nId == GDB_ID_NULL)
return false ;
// cerco l'asse rotante di calcolo con questo identificativo
for ( size_t i = 0 ; i < m_vCalcRotAx.size() ; ++ i) {
if ( m_vCalcRotAx[i].nGrpId == nId) {
if ( dVal < m_vCalcRotAx[i].stroke.Min || dVal > m_vCalcRotAx[i].stroke.Max) {
m_vCalcRotAx[i].bFixed = false ;
return false ;
}
m_vCalcRotAx[i].bFixed = true ;
m_vCalcRotAx[i].dFixVal = dVal ;
return true ;
}
}
return false ;
}
//----------------------------------------------------------------------------
bool
Machine::FreeKinematicRotAxis( const string& sName)
{
return FreeKinematicRotAxis( GetAxisId( sName)) ;
}
//----------------------------------------------------------------------------
bool
Machine::FreeKinematicRotAxis( int nId)
{
// verifico identificativo
if ( nId == GDB_ID_NULL)
return false ;
// cerco l'asse rotante di calcolo con questo identificativo
for ( size_t i = 0 ; i < m_vCalcRotAx.size() ; ++ i) {
if ( m_vCalcRotAx[i].nGrpId == nId) {
m_vCalcRotAx[i].bFixed = false ;
return true ;
}
}
return false ;
}
//----------------------------------------------------------------------------
bool
Machine::IsKinematicRotAxisBlocked( int nInd) const
{
int nRotAxTot = int( m_vCalcRotAx.size()) ;
if ( nInd < 0 || nInd >= nRotAxTot)
return false ;
return m_vCalcRotAx[nInd].bFixed ;
}
//----------------------------------------------------------------------------
bool
Machine::GetKinematicRotAxisBlocked( int nInd, string& sName, double& dVal) const
{
int nRotAxTot = int( m_vCalcRotAx.size()) ;
if ( nInd < 0 || nInd >= nRotAxTot || ! m_vCalcRotAx[nInd].bFixed)
return false ;
m_pGeomDB->GetName( m_vCalcRotAx[nInd].nGrpId, sName) ;
dVal = m_vCalcRotAx[nInd].dFixVal ;
return true ;
}
//----------------------------------------------------------------------------
bool
Machine::SetSolCh( int nScc, bool bExact)
{
// imposto se richiesta verifica esatta
m_bSolChExact = bExact ;
// se standard o nullo o suo opposto
if ( nScc == MCH_SCC_STD || nScc == MCH_SCC_NONE || nScc == MCH_SCC_OPPOSITE) {
// recupero i dati della testa
Head* pHead = GetHead( m_nCalcHeadId) ;
if ( pHead == nullptr) {
LOG_ERROR( GetEMkLogger(), "Missing head in SetSolCh")
return false ;
}
// assegno il criterio definito nella testa (standard)
m_nCalcSolCh = pHead->GetSolCh() ;
if ( nScc == MCH_SCC_STD || nScc == MCH_SCC_NONE)
return true ;
// essendo opposto, devo invertirlo
switch ( m_nCalcSolCh) {
case MCH_SCC_NONE : break ;
case MCH_SCC_ADIR_XP : m_nCalcSolCh = MCH_SCC_ADIR_XM ; break ;
case MCH_SCC_ADIR_XM : m_nCalcSolCh = MCH_SCC_ADIR_XP ; break ;
case MCH_SCC_ADIR_YP : m_nCalcSolCh = MCH_SCC_ADIR_YM ; break ;
case MCH_SCC_ADIR_YM : m_nCalcSolCh = MCH_SCC_ADIR_YP ; break ;
case MCH_SCC_ADIR_ZP : m_nCalcSolCh = MCH_SCC_ADIR_ZM ; break ;
case MCH_SCC_ADIR_ZM : m_nCalcSolCh = MCH_SCC_ADIR_ZP ; break ;
case MCH_SCC_ADIR_NEAR : m_nCalcSolCh = MCH_SCC_ADIR_FAR ; break ;
case MCH_SCC_ADIR_FAR : m_nCalcSolCh = MCH_SCC_ADIR_NEAR ; break ;
default : m_nCalcSolCh = MCH_SCC_NONE ; break ;
}
return true ;
}
// altri casi
if ( IsValidHeadScc( nScc)) {
m_nCalcSolCh = nScc ;
return true ;
}
// non previsto
m_nCalcSolCh = MCH_SCC_NONE ;
return false ;
}
//----------------------------------------------------------------------------
bool
Machine::GetAngles( const Vector3d& vtDirT, const Vector3d& vtDirA,
int& nStat, double& dAngA1, double& dAngB1, double& dAngA2, double& dAngB2) const
{
// assegno dati
Vector3d vtDirH = m_vtCalcDir ;
Vector3d vtDirI = m_vtCalcADir ;
int nNumRotAx = 0 ;
KinAxis RotAx1 ;
if ( m_vCalcRotAx.size() >= 1) {
++ nNumRotAx ;
RotAx1 = m_vCalcRotAx[0] ;
}
KinAxis RotAx2 ;
if ( m_vCalcRotAx.size() >= 2) {
++ nNumRotAx ;
RotAx2 = m_vCalcRotAx[1] ;
}
// eseguo calcolo
return GetMyAngles( vtDirT, vtDirA, vtDirH, vtDirI, nNumRotAx, RotAx1, RotAx2,
nStat, dAngA1, dAngB1, dAngA2, dAngB2) ;
}
//----------------------------------------------------------------------------
bool
Machine::GetAngles( const Vector3d& vtDirT, const Vector3d& vtDirA,
int& nStat, DBLVECTOR& vAng1, DBLVECTOR& vAng2) const
{
// verifiche e aggiustamenti degli assi rotanti
Vector3d vtDirTm = vtDirT ;
Vector3d vtDirAm = vtDirA ;
Vector3d vtDirH = m_vtCalcDir ;
Vector3d vtDirI = m_vtCalcADir ;
int nNumRotAx = 0 ;
KinAxis RotAx[2] ;
INTVECTOR vBloHeAx ;
for ( size_t i = 0 ; i < m_vCalcRotAx.size() ; ++ i) {
// se asse libero
if ( ! m_vCalcRotAx[i].bFixed) {
// verifico di non superare il limite
if ( nNumRotAx >= 2)
return false ;
// assegno l'asse
RotAx[nNumRotAx] = m_vCalcRotAx[i] ;
// se asse di testa, lo aggiorno con precedenti bloccati di testa (applicati in ordine contrario)
if ( RotAx[nNumRotAx].bHead) {
for ( size_t k = vBloHeAx.size() ; k >= 1 ; -- k) {
int nA = vBloHeAx[k-1] ;
RotAx[nNumRotAx].ptPos.Rotate( m_vCalcRotAx[nA].ptPos, m_vCalcRotAx[nA].vtDir, m_vCalcRotAx[nA].dFixVal) ;
RotAx[nNumRotAx].vtDir.Rotate( m_vCalcRotAx[nA].vtDir, m_vCalcRotAx[nA].dFixVal) ;
}
}
// incremento contatore
++ nNumRotAx ;
}
// altrimenti asse bloccato
else {
// se asse di tavola
if ( ! m_vCalcRotAx[i].bHead) {
// aggiorno direzioni utensile e ausiliaria richieste
vtDirTm.Rotate( m_vCalcRotAx[i].vtDir, m_vCalcRotAx[i].dFixVal) ;
vtDirAm.Rotate( m_vCalcRotAx[i].vtDir, m_vCalcRotAx[i].dFixVal) ;
// aggiorno eventuali assi già inseriti (sicuramente di tavola)
for ( size_t j = 0 ; int( j) < nNumRotAx ; ++ j) {
RotAx[j].ptPos.Rotate( m_vCalcRotAx[i].ptPos, m_vCalcRotAx[i].vtDir, m_vCalcRotAx[i].dFixVal) ;
RotAx[j].vtDir.Rotate( m_vCalcRotAx[i].vtDir, m_vCalcRotAx[i].dFixVal) ;
}
}
// altrimenti asse di testa
else {
// inserisco in lista assi di testa bloccati
vBloHeAx.push_back( int( i)) ;
}
}
}
// aggiorno direzioni utensile e ausiliaria su testa con eventuali assi bloccati di testa (applico in ordine contrario)
for ( size_t k = vBloHeAx.size() ; k >= 1 ; -- k) {
int nA = vBloHeAx[k-1] ;
vtDirH.Rotate( m_vCalcRotAx[nA].vtDir, m_vCalcRotAx[nA].dFixVal) ;
vtDirI.Rotate( m_vCalcRotAx[nA].vtDir, m_vCalcRotAx[nA].dFixVal) ;
}
// eseguo calcolo
double dAngA1, dAngB1, dAngA2, dAngB2 ;
if ( ! GetMyAngles( vtDirTm, vtDirAm, vtDirH, vtDirI, nNumRotAx, RotAx[0], RotAx[1],
nStat, dAngA1, dAngB1, dAngA2, dAngB2))
return false ;
// assegno gli angoli
int nRotAxInd = 1 ;
for ( size_t i = 0 ; i < m_vCalcRotAx.size() ; ++i) {
if ( m_vCalcRotAx[i].bFixed) {
vAng1.push_back( m_vCalcRotAx[i].dFixVal) ;
vAng2.push_back( m_vCalcRotAx[i].dFixVal) ;
}
else {
if ( nRotAxInd == 1) {
vAng1.push_back( dAngA1) ;
vAng2.push_back( dAngA2) ;
}
else if ( nRotAxInd == 2) {
vAng1.push_back( dAngB1) ;
vAng2.push_back( dAngB2) ;
}
else
return false ;
++ nRotAxInd ;
}
}
return true ;
}
//----------------------------------------------------------------------------
bool
Machine::GetMyAngles( const Vector3d& vtDirT, const Vector3d& vtDirA,
const Vector3d& vtDirH, const Vector3d& vtDirI,
int nNumRotAx, const KinAxis& RotAx1, const KinAxis& RotAx2,
int& nStat, double& dAngA1, double& dAngB1, double& dAngA2, double& dAngB2) const
{
// annullo tutti gli angoli
nStat = 0 ; dAngA1 = 0 ; dAngB1 = 0 ; dAngA2 = 0 ; dAngB2 = 0 ;
// direzione fresa normalizzata
Vector3d vtDirTn = vtDirT ;
if ( ! vtDirTn.Normalize())
return false ;
// direzione ausiliaria normalizzata
Vector3d vtDirAn = vtDirA ;
vtDirAn.Normalize() ;
// direzione fresa su testa a riposo
Vector3d vtDirHn = vtDirH ;
// direzione ausiliaria su testa a riposo
Vector3d vtDirIn = vtDirI ;
// se nessun asse rotante, devo solo verificare corrispondenza tra direzione richiesta e fresa
if ( nNumRotAx == 0) {
if ( AreSameVectorApprox( vtDirTn, vtDirHn))
nStat = 1 ;
else
nStat = 0 ;
return true ;
}
// direzione primo asse rotante
Vector3d vtAx1 = RotAx1.vtDir ;
// se asse di tavola, ne inverto la direzione
if ( ! RotAx1.bHead)
vtAx1.Invert() ;
// componente versore fresa desiderato su direzione primo asse rotante
double dCompTSuAxR1 = vtDirTn * vtAx1 ;
// se c'è secondo asse rotante, si calcola angolo per avere il componente appena calcolato
bool bDet = true ;
Vector3d vtDirH1, vtDirH2 ;
Vector3d vtDirI1, vtDirI2 ;
if ( nNumRotAx == 2) {
// direzione secondo asse rotante
Vector3d vtAx2 = RotAx2.vtDir ;
// se asse di tavola, ne inverto la direzione
if ( ! RotAx2.bHead)
vtAx2.Invert() ;
// calcolo secondo angolo di rotazione
nStat = GetRotationComponent( vtDirHn, dCompTSuAxR1, vtAx1, vtAx2, dAngB1, dAngB2, bDet) ;
// se indeterminato, provo ricalcolo con direzione aux
if ( nStat >= 1 && ! bDet) {
// componente versore ausiliario desiderato su direzione primo asse rotante
Vector3d vtSccDir ;
if ( GetSccDir( m_nCalcSolCh, vtDirAn, vtSccDir)) {
double dCompASuAxR1 = vtSccDir * vtAx1 ;
// ricalcolo secondo angolo di rotazione
nStat = GetRotationComponent( vtDirIn, dCompASuAxR1, vtAx1, vtAx2, dAngB1, dAngB2, bDet) ;
}
}
// aggiornamento direzioni fresa e ausiliaria su testa
if ( nStat >= 1) {
// se indeterminato lo azzero
if ( ! bDet)
dAngB1 = 0 ;
// eseguo aggiornamento
vtDirH1 = vtDirHn ;
vtDirH1.Rotate( vtAx2, dAngB1) ;
vtDirI1 = vtDirIn ;
vtDirI1.Rotate( vtAx2, dAngB1) ;
}
if ( nStat == 2) {
vtDirH2 = vtDirHn ;
vtDirH2.Rotate( vtAx2, dAngB2) ;
vtDirI2 = vtDirIn ;
vtDirI2.Rotate( vtAx2, dAngB2) ;
}
}
// altrimenti verifico se compatibili
else {
// componente versore utensile su direzione primo asse
double dCompHSuAxR1 = vtDirHn * vtAx1 ;
// componenti versori fresa e utensile perpendicolari direzione primo asse
double dTemp = 1 - dCompTSuAxR1 * dCompTSuAxR1 ;
double dCompTOrtAxR1 = ( ( dTemp > EPS_ZERO) ? sqrt( dTemp) : 0) ;
dTemp = 1 - dCompHSuAxR1 * dCompHSuAxR1 ;
double dCompHOrtAxR1 = ( ( dTemp > EPS_ZERO) ? sqrt( dTemp) : 0) ;
// verifica ( controllo verso e max delta angolare < 0.002 deg)
if ( abs( dCompTSuAxR1 - dCompHSuAxR1) < EPS_SMALL &&
abs( dCompTOrtAxR1 * dCompHSuAxR1 - dCompHOrtAxR1 * dCompTSuAxR1) < 2 * SIN_EPS_ANG_SMALL) {
nStat = 1 ;
vtDirH1 = vtDirHn ;
vtDirI1 = vtDirIn ;
// reset secondo angolo
dAngB1 = 0 ;
}
}
// calcolo primo angolo di rotazione per seconda soluzione
bool bDet2 = true ;
if ( nStat == 2) {
if ( ! vtDirH2.GetRotation( vtDirTn, vtAx1, SIN_EPS_ANG_SMALL, dAngA2, bDet2))
nStat = 1 ;
else {
// se indeterminato, provo a determinarlo con la direzione ausiliaria
if ( ! bDet2) {
bool bDetX ;
Vector3d vtSccDir ;
bool bSccDir = GetSccDir( m_nCalcSolCh, vtDirAn, vtSccDir) ;
vtDirI2.GetRotation( vtSccDir, vtAx1, dAngA2, bDetX) ;
if ( bDetX)
bDet2 = bSccDir ;
else
dAngA2 = 0 ;
}
}
// aggiornamento direzioni fresa e ausiliaria su testa
vtDirH2.Rotate( vtAx1, dAngA2) ;
vtDirI2.Rotate( vtAx1, dAngA2) ;
}
// calcolo primo angolo di rotazione per prima soluzione
bool bDet1 = true ;
if ( nStat >= 1) {
if ( ! vtDirH1.GetRotation( vtDirTn, vtAx1, SIN_EPS_ANG_SMALL, dAngA1, bDet1))
nStat = 0 ;
else {
// se indeterminato, provo a determinarlo con la direzione ausiliaria
if ( ! bDet1) {
bool bDetX ;
Vector3d vtSccDir ;
bool bSccDir = GetSccDir( m_nCalcSolCh, vtDirAn, vtSccDir) ;
vtDirI1.GetRotation( vtSccDir, vtAx1, dAngA1, bDetX) ;
if ( bDetX)
bDet1 = bSccDir ;
else
dAngA1 = 0 ;
}
}
// aggiornamento direzioni fresa e ausiliaria su testa
vtDirH1.Rotate( vtAx1, dAngA1) ;
vtDirI1.Rotate( vtAx1, dAngA1) ;
}
// verifica per direzione singolare (direzione fresa molto vicina a direzione primo asse rotante)
if ( nNumRotAx == 2 && nStat == 2 && abs( dAngB2 - dAngB1) < 2 * m_dSingConeAng) {
nStat = 1 ;
bDet1 = false ;
dAngB1 = ( dAngB1 + dAngB2) / 2 ;
// ripristino direzioni fresa e ausiliaria su testa
vtDirH1.Rotate( vtAx1, -dAngA1) ;
vtDirI1.Rotate( vtAx1, -dAngA1) ;
// provo a determinare primo angolo con la direzione ausiliaria
bool bDetX ;
Vector3d vtSccDir ;
bool bSccDir = GetSccDir( m_nCalcSolCh, vtDirAn, vtSccDir) ;
vtDirI1.GetRotation( vtSccDir, vtAx1, dAngA1, bDetX) ;
if ( bDetX)
bDet1 = bSccDir ;
else
dAngA1 = 0 ;
// aggiornamento direzioni fresa e ausiliaria su testa
vtDirH1.Rotate( vtAx1, dAngA1) ;
vtDirI1.Rotate( vtAx1, dAngA1) ;
}
// verifiche per criterio scelta soluzione
if ( nStat == 2) {
if ( ! VerifyScc( vtDirI2, vtDirAn, m_nCalcSolCh, m_bSolChExact))
-- nStat ;
}
if ( nStat >= 1) {
if ( ! VerifyScc( vtDirI1, vtDirAn, m_nCalcSolCh, m_bSolChExact)) {
-- nStat ;
// riloco eventuale soluzione rimasta
if ( nStat >= 1) {
dAngA1 = dAngA2 ;
dAngB1 = dAngB2 ;
}
}
}
// verifiche dei limiti di corsa
if ( nStat == 2) {
// se non riesco ad aggiustare, elimino
if ( ! AdjustAngleInStroke( RotAx1.stroke, dAngA2) ||
( nNumRotAx == 2 && ! AdjustAngleInStroke( RotAx2.stroke, dAngB2)))
-- nStat ;
}
if ( nStat >= 1) {
// se non riesco ad aggiustare, elimino
if ( ( bDet1 && ! AdjustAngleInStroke( RotAx1.stroke, dAngA1)) ||
( nNumRotAx == 2 && ! AdjustAngleInStroke( RotAx2.stroke, dAngB1))) {
-- nStat ;
// riloco eventuale soluzione rimasta
if ( nStat >= 1) {
dAngA1 = dAngA2 ;
dAngB1 = dAngB2 ;
}
}
}
// modifico stato per angolo indeterminato
if ( ( nStat == 2 && ! bDet2) || ( nStat >= 1 && ! bDet1))
nStat = - nStat ;
return true ;
}
//----------------------------------------------------------------------------
bool
Machine::GetPositions( const Point3d& ptP, const DBLVECTOR& vAng,
double& dX, double& dY, double& dZ) const
{
// la posizione deve essere espressa rispetto allo ZERO MACCHINA
// il punto è dato rispetto alla posizione home della tavola
// verifico che siano stati assegnati gli angoli necessari, altrimenti errore
if ( vAng.size() < m_vCalcRotAx.size())
return false ;
// aggiorno punto di lavoro mediante ciclo diretto sugli assi di tavola
Point3d ptW = ptP ;
// annullo la posizione home degli assi lineari
for ( int i = 0 ; i < int( m_vCalcLinAx.size()) ; ++ i) {
// se asse di tavola
if ( ! m_vCalcLinAx[i].bHead)
ptW.Translate( - m_vCalcLinAx[i].dHomeVal * ( - m_vCalcLinAx[i].vtDir)) ;
}
// effettuo rotazione diminuita della posizione home degli assi rotanti
for ( int i = 0 ; i < int( m_vCalcRotAx.size()) ; ++ i) {
// se asse di tavola
if ( ! m_vCalcRotAx[i].bHead)
ptW.Rotate( m_vCalcRotAx[i].ptPos, m_vCalcRotAx[i].vtDir, vAng[i] - m_vCalcRotAx[i].dHomeVal) ;
}
// effettuo rotazione inversa per asse rotante di testa speciale
if ( m_nHeadSpecRotAxis != -1) {
if ( m_nHeadSpecRotAxis < 0 || m_nHeadSpecRotAxis >= int( m_vCalcRotAx.size()))
return false ;
int i = m_nHeadSpecRotAxis ;
ptW.Rotate( m_vCalcRotAx[i].ptPos, m_vCalcRotAx[i].vtDir, -vAng[i]) ;
}
// aggiorno posizione e direzione fresa su testa a riposo mediante ciclo inverso sugli assi di testa
Point3d ptPosH = m_ptCalcPos ;
Vector3d vtDirH = m_vtCalcDir ;
for ( int i = int( m_vCalcRotAx.size()) - 1 ; i >= 0 ; -- i) {
// se asse di testa non speciale
if ( m_vCalcRotAx[i].bHead && i != m_nHeadSpecRotAxis) {
ptPosH.Rotate( m_vCalcRotAx[i].ptPos, m_vCalcRotAx[i].vtDir, vAng[i]) ;
vtDirH.Rotate( m_vCalcRotAx[i].vtDir, vAng[i]) ;
}
}
// assegno l'offset testa
Vector3d vtDtHe = ORIG - m_ptCalcPos ;
// calcolo il recupero degli assi : è l'opposto dello spostamento della posizione
Vector3d vtDtAx = m_ptCalcPos - ptPosH ;
// calcolo il recupero di lunghezza utensile
Vector3d vtDtTL = vtDirH * m_dCalcTLen ;
// calcolo i valori degli assi lineari (posizioni)
Point3d ptPos( ptW.x + vtDtHe.x + vtDtAx.x + vtDtTL.x,
ptW.y + vtDtHe.y + vtDtAx.y + vtDtTL.y,
ptW.z + vtDtHe.z + vtDtAx.z + vtDtTL.z) ;
ptPos.ToLoc( m_frLinAx) ;
dX = ptPos.x ;
dY = ptPos.y ;
dZ = ptPos.z ;
// tutto ok
return true ;
}
//----------------------------------------------------------------------------
bool
Machine::GetDirection( const Vector3d& vtDir, const DBLVECTOR& vAng, Vector3d& vtNew) const
{
// è espressa nel riferimento di macchina (tiene conto delle sole rotazioni di testa)
// verifico dimensione vettore angoli rispetto al numero di assi rotanti
if ( vAng.size() < m_vCalcRotAx.size())
return false ;
// direzione a riposo
vtNew = vtDir ;
// ciclo sugli assi di testa dall'ultimo (prima assi di tavola, poi di testa)
for ( size_t i = m_vCalcRotAx.size() ; i >= 1 ; -- i) {
// se asse di testa
if ( m_vCalcRotAx[i-1].bHead)
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 ;
}
//----------------------------------------------------------------------------
bool
Machine::GetBackDirection( const Vector3d& vtDir, const DBLVECTOR& vAng, Vector3d& vtNew) const
{
// è espressa nel riferimento del pezzo (tiene conto delle rotazioni di testa e al contrario di quelle di tavola)
// verifico dimensione vettore angoli rispetto al numero di assi rotanti
if ( vAng.size() < m_vCalcRotAx.size())
return false ;
// direzione a riposo
vtNew = vtDir ;
// ciclo sugli assi dall'ultimo (prima assi di tavola, poi di testa)
for ( size_t i = m_vCalcRotAx.size() ; i >= 1 ; -- i) {
// se asse di testa
if ( m_vCalcRotAx[i-1].bHead)
vtNew.Rotate( m_vCalcRotAx[i-1].vtDir, vAng[i-1]) ;
// altrimenti di tavola
else
vtNew.Rotate( m_vCalcRotAx[i-1].vtDir, - vAng[i-1]) ;
}
return true ;
}
//----------------------------------------------------------------------------
bool
Machine::GetNoseFromPositions( double dX, double dY, double dZ, const DBLVECTOR& vAng,
Point3d& ptNose) const
{
// la posizione deve essere espressa rispetto allo ZERO MACCHINA
// è espressa nel riferimento di macchina (tiene conto delle sole traslazioni e rotazioni di testa)
// verifico dimensione vettore angoli rispetto al numero di assi rotanti
if ( vAng.size() < m_vCalcRotAx.size())
return false ;
// se centro di lavoro
if ( m_nCalcChainType == KIN_CHAIN_MCENT) {
// aggiorno posizione testa a riposo mediante ciclo inverso sugli assi rotanti di testa
ptNose = m_ptCalcPos ;
for ( int i = int( m_vCalcRotAx.size()) - 1 ; i >= 0 ; -- i) {
// se asse di testa non speciale
if ( m_vCalcRotAx[i].bHead && i != m_nHeadSpecRotAxis)
ptNose.Rotate( m_vCalcRotAx[i].ptPos, m_vCalcRotAx[i].vtDir, vAng[i]) ;
}
// aggiorno posizione testa con assi lineari di testa
DBLVECTOR vMov( {dX, dY, dZ}) ;
for ( int i = 0 ; i < int( m_vCalcLinAx.size()) ; ++ i) {
if ( m_vCalcLinAx[i].bHead)
ptNose += m_vCalcLinAx[i].vtDir * vMov[i] ;
}
// eseguo rotazione eventuale asse rotante speciale di testa
if ( m_nHeadSpecRotAxis != -1) {
if ( m_nHeadSpecRotAxis < 0 || m_nHeadSpecRotAxis >= int( m_vCalcRotAx.size()))
return false ;
int i = m_nHeadSpecRotAxis ;
ptNose.Rotate( m_vCalcRotAx[i].ptPos, m_vCalcRotAx[i].vtDir, vAng[i]) ;
}
}
// se robot
else if ( m_nCalcChainType == KIN_CHAIN_ROBOT) {
// aggiorno posizione testa a riposo mediante ciclo inverso sugli assi rotanti di testa
ptNose = m_ptCalcPos ;
for ( int i = int( m_vCalcRotAx.size()) - 1 ; i >= 0 ; -- i) {
// se asse di testa
if ( m_vCalcRotAx[i].bHead)
ptNose.Rotate( m_vCalcRotAx[i].ptPos, m_vCalcRotAx[i].vtDir, vAng[i]) ;
}
}
// altrimenti errore
else
return false ;
return true ;
}
//----------------------------------------------------------------------------
bool
Machine::GetTipFromPositions( double dX, double dY, double dZ, const DBLVECTOR& vAng,
bool bOverall, bool bBottom, bool bBack, Point3d& ptTip) const
{
// la posizione deve essere espressa rispetto allo ZERO MACCHINA
// è espressa nel riferimento di macchina (tiene conto delle sole rotazioni di testa)
// se bBack vero, allora è nel riferimento pezzo
// verifico dimensione vettore angoli rispetto al numero di assi rotanti
if ( vAng.size() < m_vCalcRotAx.size())
return false ;
// se centro di lavoro
if ( m_nCalcChainType == KIN_CHAIN_MCENT) {
// aggiorno posizione tip utensile a riposo mediante ciclo inverso sugli assi rotanti di testa
ptTip = m_ptCalcPos - m_vtCalcDir * m_dCalcTLen ;
for ( int i = int( m_vCalcRotAx.size()) - 1 ; i >= 0 ; -- i) {
// se asse di testa non speciale
if ( m_vCalcRotAx[i].bHead && i != m_nHeadSpecRotAxis)
ptTip.Rotate( m_vCalcRotAx[i].ptPos, m_vCalcRotAx[i].vtDir, vAng[i]) ;
}
// aggiorno posizione tip utensile con assi lineari di testa
DBLVECTOR vMov( {dX, dY, dZ}) ;
for ( int i = 0 ; i < int( m_vCalcLinAx.size()) ; ++ i) {
if ( m_vCalcLinAx[i].bHead)
ptTip += m_vCalcLinAx[i].vtDir * vMov[i] ;
}
// eseguo rotazione eventuale asse rotante speciale di testa
if ( m_nHeadSpecRotAxis != -1) {
if ( m_nHeadSpecRotAxis < 0 || m_nHeadSpecRotAxis >= int( m_vCalcRotAx.size()))
return false ;
int i = m_nHeadSpecRotAxis ;
ptTip.Rotate( m_vCalcRotAx[i].ptPos, m_vCalcRotAx[i].vtDir, vAng[i]) ;
}
}
// se robot
else if ( m_nCalcChainType == KIN_CHAIN_ROBOT) {
// aggiorno posizione tip utensile a riposo mediante ciclo inverso sugli assi rotanti di testa
ptTip = m_ptCalcPos - m_vtCalcDir * m_dCalcTLen ;
for ( int i = int( m_vCalcRotAx.size()) - 1 ; i >= 0 ; -- i) {
// se asse di testa
if ( m_vCalcRotAx[i].bHead)
ptTip.Rotate( m_vCalcRotAx[i].ptPos, m_vCalcRotAx[i].vtDir, vAng[i]) ;
}
}
// altrimenti errore
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 nel riferimento pezzo
if ( bBack) {
// ciclo sugli assi lineari di tavola all'indietro
DBLVECTOR vMov( {dX, dY, dZ}) ;
for ( int i = int( m_vCalcLinAx.size()) ; i > 0 ; -- i) {
if ( ! m_vCalcLinAx[i-1].bHead)
ptTip += m_vCalcLinAx[i-1].vtDir * ( vMov[i-1] - m_vCalcLinAx[i-1].dHomeVal) ;
}
// ciclo sugli assi rotanti di tavola all'indietro !!! NON VERIFICATO !!!
for ( int i = int( m_vCalcRotAx.size()) ; i > 0 ; -- i) {
if ( ! m_vCalcRotAx[i-1].bHead)
ptTip.Rotate( m_vCalcRotAx[i-1].ptPos, m_vCalcRotAx[i-1].vtDir, -vAng[i-1]) ;
}
}
// Se richiesto ingombro totale o punto sotto del tip utensile
if ( bOverall || bBottom) {
// calcolo la direzione fresa
Vector3d vtDirT ;
if ( bBack) {
if ( ! GetBackDirection( m_vtCalcDir, vAng, vtDirT))
return false ;
}
else {
if ( ! GetDirection( m_vtCalcDir, vAng, vtDirT))
return false ;
}
// se richiesto ingombro totale
if ( bOverall)
ptTip -= vtDirT * max( m_dCalcTOvLen - m_dCalcTLen, 0.) ;
// se richiesto punto più basso e direzione fresa non esattamente verticale
if ( bBottom && ! vtDirT.IsZplus() && ! vtDirT.IsZminus()) {
// calcolo la direzione perpendicolare più verticale possibile
Vector3d vtCorr = FromUprightOrtho( vtDirT) ;
// correggo il tip
if ( bOverall)
ptTip -= vtCorr * max( m_dCalcTOvRad, m_dCalcTRad) ;
else
ptTip -= vtCorr * m_dCalcTRad ;
}
}
return true ;
}
//----------------------------------------------------------------------------
bool
Machine::GetToolDirFromAngles( const DBLVECTOR& vAng, Vector3d& vtDir) const
{
return GetDirection( m_vtCalcDir, vAng, vtDir) ;
}
//----------------------------------------------------------------------------
bool
Machine::GetAuxDirFromAngles( const DBLVECTOR& vAng, Vector3d& vtDir) const
{
return GetDirection( m_vtCalcADir, vAng, vtDir) ;
}
//----------------------------------------------------------------------------
bool
Machine::GetPartDirFromAngles( const Vector3d& vtPart, const DBLVECTOR& vAng, Vector3d& vtDir) const
{
// è espressa nel riferimento di macchina (tiene conto delle sole rotazioni di tavola e asse rotante speciale di testa)
// verifico dimensione vettore angoli rispetto al numero di assi rotanti
if ( vAng.size() < m_vCalcRotAx.size())
return false ;
// direzione a riposo
vtDir = vtPart ;
// ciclo sugli assi dal primo (solo assi di tavola)
for ( size_t i = 1 ; i <= m_vCalcRotAx.size() ; ++ i) {
// se asse di tavola
if ( ! m_vCalcRotAx[i-1].bHead)
vtDir.Rotate( m_vCalcRotAx[i-1].vtDir, vAng[i-1]) ;
}
// eseguo rotazione inversa eventuale asse rotante speciale di testa
if ( m_nHeadSpecRotAxis != -1) {
if ( m_nHeadSpecRotAxis < 0 || m_nHeadSpecRotAxis >= int( m_vCalcRotAx.size()))
return false ;
int i = m_nHeadSpecRotAxis ;
vtDir.Rotate( m_vCalcRotAx[i].vtDir, -vAng[i]) ;
}
return true ;
}
//----------------------------------------------------------------------------
bool
Machine::GetBackToolDirFromAngles( const DBLVECTOR& vAng, Vector3d& vtDir) const
{
return GetBackDirection( m_vtCalcDir, vAng, vtDir) ;
}
//----------------------------------------------------------------------------
bool
Machine::GetBackAuxDirFromAngles( const DBLVECTOR& vAng, Vector3d& vtDir) const
{
return GetBackDirection( m_vtCalcADir, vAng, vtDir) ;
}
//----------------------------------------------------------------------------
bool
Machine::GetSccDir( int nSolCh, const Vector3d& vtDirA, Vector3d& vtDirScc) const
{
switch ( nSolCh) {
case MCH_SCC_ADIR_XP : vtDirScc = X_AX ; return true ;
case MCH_SCC_ADIR_XM : vtDirScc = - X_AX ; return true ;
case MCH_SCC_ADIR_YP : vtDirScc = Y_AX ; return true ;
case MCH_SCC_ADIR_YM : vtDirScc = - Y_AX ; return true ;
case MCH_SCC_ADIR_ZP : vtDirScc = Z_AX ; return true ;
case MCH_SCC_ADIR_ZM : vtDirScc = - Z_AX ; return true ;
case MCH_SCC_ADIR_NEAR : vtDirScc = vtDirA ; return true ;
case MCH_SCC_ADIR_FAR : vtDirScc = - vtDirA ; return true ;
}
vtDirScc = vtDirA ;
return false ;
}
//----------------------------------------------------------------------------
bool
Machine::VerifyScc( const Vector3d& vtDirI, const Vector3d& vtDirA, int nSolCh, bool bExact) const
{
switch ( nSolCh) {
default :
return true ;
case MCH_SCC_ADIR_XP :
if ( bExact)
return vtDirI.IsXplus() ;
else
return ( vtDirI.x > - EPS_ZERO) ;
case MCH_SCC_ADIR_XM :
if ( bExact)
return vtDirI.IsXminus() ;
else
return ( vtDirI.x < EPS_ZERO) ;
case MCH_SCC_ADIR_YP :
if ( bExact)
return vtDirI.IsYplus() ;
else
return ( vtDirI.y > - EPS_ZERO) ;
case MCH_SCC_ADIR_YM :
if ( bExact)
return vtDirI.IsYminus() ;
else
return ( vtDirI.y < EPS_ZERO) ;
case MCH_SCC_ADIR_ZP :
if ( bExact)
return vtDirI.IsZplus() ;
else
return ( vtDirI.z > - EPS_ZERO) ;
case MCH_SCC_ADIR_ZM :
if ( bExact)
return vtDirI.IsZminus() ;
else
return ( vtDirI.z < EPS_ZERO) ;
case MCH_SCC_ADIR_NEAR :
if ( bExact)
return ( ! vtDirA.IsSmall() && ( vtDirI * vtDirA) > cos( 0.1 * DEGTORAD)) ;
else
return ( vtDirA.IsSmall() || ( vtDirI * vtDirA) > cos( 60 * DEGTORAD)) ;
case MCH_SCC_ADIR_FAR :
if ( bExact)
return ( ! vtDirA.IsSmall() && ( vtDirI * vtDirA) < cos( 179.1 * DEGTORAD)) ;
else
return ( vtDirA.IsSmall() || ( vtDirI * vtDirA) < cos( 120 * DEGTORAD)) ;
}
}
//----------------------------------------------------------------------------
bool
Machine::AdjustAngleInStroke( const STROKE& Stroke, double& dAng) const
{
// se asse rotante vero
if ( Stroke.Max - Stroke.Min > EPS_ANG_SMALL) {
// eseguo gli aggiustamenti
while ( dAng < Stroke.Min)
dAng += ANG_FULL ;
while ( dAng > Stroke.Max)
dAng -= ANG_FULL ;
// se nei limiti, tutto bene
if ( dAng >= Stroke.Min && dAng <= Stroke.Max)
return true ;
// altrimenti, determino il valore con il minimo outstroke
if ( dAng < Stroke.Min) {
if ( abs( dAng - Stroke.Min) > abs( dAng + ANG_FULL - Stroke.Max))
dAng += ANG_FULL ;
}
else {
if ( abs( dAng - Stroke.Max) > abs( dAng - ANG_FULL - Stroke.Min))
dAng -= ANG_FULL ;
}
return false ;
}
// altrimenti asse rotante senza corsa
else {
// angolo di riferimento
double dStrokeMid = ( Stroke.Min + Stroke.Max) / 2 ;
// eseguo gli aggiustamenti
while ( dAng < dStrokeMid - EPS_ANG_SMALL)
dAng += ANG_FULL ;
while ( dAng > dStrokeMid + EPS_ANG_SMALL)
dAng -= ANG_FULL ;
// verifico
if ( abs( dAng - dStrokeMid) < EPS_ANG_SMALL) {
dAng = dStrokeMid ;
return true ;
}
else
return false ;
}
}
//----------------------------------------------------------------------------
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()))
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)
dAng -= ANG_FULL ;
while ( dAng - dAngRef < -ANG_STRAIGHT + EPS_ANG_ZERO && dAng + ANG_FULL <= m_vCalcRotAx[nInd].stroke.Max)
dAng += ANG_FULL ;
return ( dAng >= m_vCalcRotAx[nInd].stroke.Min &&
dAng <= m_vCalcRotAx[nInd].stroke.Max) ;
}
//----------------------------------------------------------------------------
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()))
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) ;
return true ;
}
//----------------------------------------------------------------------------
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()))
return true ;
// se angolo fuori corsa, errore
if ( dAng < m_vCalcRotAx[nInd].stroke.Min)
return false ;
else if ( dAng > m_vCalcRotAx[nInd].stroke.Max)
return false ;
// va bene
return true ;
}
//----------------------------------------------------------------------------
bool
Machine::VerifyOutstroke( double dX, double dY, double dZ, const DBLVECTOR& vAng, bool bClear, int& nStat) const
{
// default tutto ok
nStat = 0 ;
if ( bClear)
m_OutstrokeInfo.Clear() ;
// verifica degli assi lineari
DBLVECTOR vLin( 3) ; vLin[0] = dX ; vLin[1] = dY ; vLin[2] = dZ ;
for ( size_t i = 0 ; i < m_vCalcLinAx.size() && i < vLin.size() ; ++ i) {
if ( vLin[i] < m_vCalcLinAx[i].stroke.Min) {
if ( nStat == 0) {
m_OutstrokeInfo.sAxName = GetAxis( m_vCalcLinAx[i].nGrpId)->GetName() ;
m_OutstrokeInfo.sAxToken = GetAxis( m_vCalcLinAx[i].nGrpId)->GetToken() ;
m_OutstrokeInfo.bLinear = true ;
m_OutstrokeInfo.dExtra = vLin[i] - m_vCalcLinAx[i].stroke.Min ;
m_OutstrokeInfo.sAuxInfo = " (L" + ToString( int(i) + 1) + "-) " ;
}
nStat += ( 1 << ( 2 * i)) ;
}
else if ( vLin[i] > m_vCalcLinAx[i].stroke.Max) {
if ( nStat == 0) {
m_OutstrokeInfo.sAxName = GetAxis( m_vCalcLinAx[i].nGrpId)->GetName() ;
m_OutstrokeInfo.sAxToken = GetAxis( m_vCalcLinAx[i].nGrpId)->GetToken() ;
m_OutstrokeInfo.bLinear = true ;
m_OutstrokeInfo.dExtra = vLin[i] - m_vCalcLinAx[i].stroke.Max ;
m_OutstrokeInfo.sAuxInfo = " (L" + ToString( int(i) + 1) + "+) " ;
}
nStat += ( 2 << ( 2 * i)) ;
}
}
// verifica degli assi rotanti
for ( size_t i = 0 ; i < m_vCalcRotAx.size() && i < vAng.size() ; ++ i) {
if ( vAng[i] < m_vCalcRotAx[i].stroke.Min) {
if ( nStat == 0) {
m_OutstrokeInfo.sAxName = GetAxis( m_vCalcRotAx[i].nGrpId)->GetName() ;
m_OutstrokeInfo.sAxToken = GetAxis( m_vCalcRotAx[i].nGrpId)->GetToken() ;
m_OutstrokeInfo.bLinear = false ;
m_OutstrokeInfo.dExtra = vAng[i] - m_vCalcRotAx[i].stroke.Min ;
m_OutstrokeInfo.sAuxInfo = " (R" + ToString( int(i) + 1) + "-) " ;
}
nStat += ( 64 << ( 2 * i)) ;
}
else if ( vAng[i] > m_vCalcRotAx[i].stroke.Max) {
if ( nStat == 0) {
m_OutstrokeInfo.sAxName = GetAxis( m_vCalcRotAx[i].nGrpId)->GetName() ;
m_OutstrokeInfo.sAxToken = GetAxis( m_vCalcRotAx[i].nGrpId)->GetToken() ;
m_OutstrokeInfo.bLinear = false ;
m_OutstrokeInfo.dExtra = vAng[i] - m_vCalcRotAx[i].stroke.Max ;
m_OutstrokeInfo.sAuxInfo = " (R" + ToString( int(i) + 1) + "+) " ;
}
nStat += ( 128 << ( 2 * i)) ;
}
}
// verifica delle aree protette
if ( nStat == 0)
return const_cast<Machine*>( this)->VerifyProtectedAreas( dX, dY, dZ, vAng, nStat) ;
return true ;
}
//----------------------------------------------------------------------------
bool
Machine::ExistProtectedAreas(void) const
{
return LuaExistsFunction( ON_VERIFY_PROTECTEDAREAS) ;
}
//----------------------------------------------------------------------------
bool
Machine::VerifyProtectedAreas( double dX, double dY, double dZ, const DBLVECTOR& vAng, int& nStat)
{
// se non esiste funzione gestione aree protette, non devo fare alcunchè
if ( ! LuaExistsFunction( ON_VERIFY_PROTECTEDAREAS))
return true ;
// default
bool bOk = true ;
int nErr = 99 ;
// salvo eventuale variabile EMC_VAR già presente
bool bOldEMC = LuaChangeNameGlobVar( EMC_VAR, EMC_VAR_BACKUP) ;
// definisco variabili (nelle disposizioni l'utensile non è definito e non ha nome)
string sTool, sHead ; int nExit ;
bOk = bOk && LuaCreateGlobTable( EMC_VAR) ;
bOk = bOk && LuaSetGlobVar( EMC_VAR + EVAR_VER, GetEMkVer()) ;
bOk = bOk && GetCurrHead( sHead) && LuaSetGlobVar( EMC_VAR + EVAR_HEAD, sHead) ;
bOk = bOk && GetCurrExit( nExit) && LuaSetGlobVar( EMC_VAR + EVAR_EXIT, nExit) ;
bOk = bOk && ( GetCurrTool( sTool) || true) && LuaSetGlobVar( EMC_VAR + EVAR_TOOL, sTool) ;
bOk = bOk && LuaSetGlobVar( EMC_VAR + EVAR_L1, dX) ;
bOk = bOk && LuaSetGlobVar( EMC_VAR + EVAR_L2, dY) ;
bOk = bOk && LuaSetGlobVar( EMC_VAR + EVAR_L3, dZ) ;
if ( vAng.size() >= 1)
bOk = bOk && LuaSetGlobVar( EMC_VAR + EVAR_R1, vAng[0]) ;
if ( vAng.size() >= 2)
bOk = bOk && LuaSetGlobVar( EMC_VAR + EVAR_R2, vAng[1]) ;
if ( vAng.size() >= 3)
bOk = bOk && LuaSetGlobVar( EMC_VAR + EVAR_R3, vAng[2]) ;
if ( vAng.size() >= 4)
bOk = bOk && LuaSetGlobVar( EMC_VAR + EVAR_R4, vAng[3]) ;
// chiamo funzione
bOk = bOk && LuaCallFunction( ON_VERIFY_PROTECTEDAREAS) ;
// recupero il risultato
bOk = bOk && LuaGetGlobVar( EMC_VAR + EVAR_ERROR, nErr) ;
if ( nErr != 0) {
bOk = false ;
string sOut = " Error in " + ON_VERIFY_PROTECTEDAREAS + " (" + ToString( nErr) + ")" ;
LOG_INFO( GetEMkLogger(), sOut.c_str())
}
int nMyStat = 0 ;
bOk = bOk && LuaGetGlobVar( EMC_VAR + EVAR_STAT, nMyStat) ;
if ( nMyStat != 0) {
nStat += ( nMyStat << 14) ;
m_OutstrokeInfo.sAxName = AXIS_NAME_PROTECTEDAREAS ;
m_OutstrokeInfo.sAxToken = "" ;
m_OutstrokeInfo.bLinear = false ;
m_OutstrokeInfo.dExtra = 0 ;
bOk = bOk && LuaGetGlobVar( EMC_VAR + EVAR_AUXINFO, m_OutstrokeInfo.sAuxInfo) ;
}
// reset variabili
bOk = LuaResetGlobVar( EMC_VAR) && bOk ;
// ripristino eventuale variabile EMC_VAR già presente
if ( bOldEMC)
LuaChangeNameGlobVar( EMC_VAR_BACKUP, EMC_VAR) ;
// esco
return bOk ;
}
//----------------------------------------------------------------------------
bool
Machine::VerifyOutstroke( const string& sAxName, double dVal) const
{
// recupero l'asse
Axis* pAx = GetAxis( GetAxisId( sAxName)) ;
if ( pAx == nullptr) {
m_OutstrokeInfo.sAxName = sAxName ;
m_OutstrokeInfo.sAxToken = sAxName ;
m_OutstrokeInfo.bLinear = false ;
m_OutstrokeInfo.dExtra = 0 ;
m_OutstrokeInfo.sAuxInfo = " (Unknown) " ;
return false ;
}
// verifica con l'estremo inferiore
if ( dVal < pAx->GetStroke().Min) {
m_OutstrokeInfo.sAxName = pAx->GetName() ;
m_OutstrokeInfo.sAxToken = pAx->GetToken() ;
m_OutstrokeInfo.bLinear = ( pAx->GetType() == MCH_AT_LINEAR) ;
m_OutstrokeInfo.dExtra = dVal - pAx->GetStroke().Min ;
m_OutstrokeInfo.sAuxInfo = " (Ax-) " ;
return false ;
}
// verifica con l'estremo superiore
if ( dVal > pAx->GetStroke().Max) {
m_OutstrokeInfo.sAxName = pAx->GetName() ;
m_OutstrokeInfo.sAxToken = pAx->GetToken() ;
m_OutstrokeInfo.bLinear = ( pAx->GetType() == MCH_AT_LINEAR) ;
m_OutstrokeInfo.dExtra = dVal - pAx->GetStroke().Max ;
m_OutstrokeInfo.sAuxInfo = " (Ax+) " ;
return false ;
}
// va bene
return true ;
}
//----------------------------------------------------------------------------
string
Machine::GetOutstrokeInfo( bool bMM) const
{
// se non c'è extracorsa, ritorno stringa vuota
if ( m_OutstrokeInfo.sAxName.empty())
return "" ;
// creo stringa con info opportune
if ( m_OutstrokeInfo.sAxName == AXIS_NAME_PROTECTEDAREAS)
return m_OutstrokeInfo.sAuxInfo ;
else if ( bMM || ! m_OutstrokeInfo.bLinear)
return m_OutstrokeInfo.sAxToken + "=" + ToString( m_OutstrokeInfo.dExtra, 2) + m_OutstrokeInfo.sAuxInfo ;
else
return m_OutstrokeInfo.sAxToken + "=" + ToString( m_OutstrokeInfo.dExtra / ONEINCH, 3) + m_OutstrokeInfo.sAuxInfo ;
}
//----------------------------------------------------------------------------
int
Machine::GetCurrLinAxes( void) const
{
return int( m_vCalcLinAx.size()) ;
}
//----------------------------------------------------------------------------
int
Machine::GetCurrRotAxes( void) const
{
return int( m_vCalcRotAx.size()) ;
}
//----------------------------------------------------------------------------
bool
Machine::GetCurrAxisName( int nInd, string& sAxName) const
{
int nLinAxes = int( m_vCalcLinAx.size()) ;
int nRotAxes = int( m_vCalcRotAx.size()) ;
if ( nInd >= 0 && nInd < nLinAxes) {
Axis* pAx = GetAxis( m_vCalcLinAx[nInd].nGrpId) ;
if ( pAx == nullptr)
return false ;
sAxName = pAx->GetName() ;
return true ;
}
else if ( nInd >= nLinAxes && nInd < nLinAxes + nRotAxes) {
Axis* pAx = GetAxis( m_vCalcRotAx[nInd-nLinAxes].nGrpId) ;
if ( pAx == nullptr)
return false ;
sAxName = pAx->GetName() ;
return true ;
}
return false ;
}
//----------------------------------------------------------------------------
bool
Machine::GetAllCurrAxesNames( STRVECTOR& vAxName) const
{
vAxName.clear() ;
bool bOk = true ;
// ciclo sugli assi lineari correnti
for ( auto& CalcLinAx : m_vCalcLinAx) {
Axis* pAx = GetAxis( CalcLinAx.nGrpId) ;
if ( pAx != nullptr)
vAxName.emplace_back( pAx->GetName()) ;
else
bOk = false ;
}
// ciclo sugli assi rotanti correnti
for ( auto& CalcRotAx : m_vCalcRotAx) {
Axis* pAx = GetAxis( CalcRotAx.nGrpId) ;
if ( pAx != nullptr)
vAxName.emplace_back( pAx->GetName()) ;
else
bOk = false ;
}
return bOk ;
}
//----------------------------------------------------------------------------
bool
Machine::GetCurrAxisToken( int nInd, string& sAxToken) const
{
int nLinAxes = int( m_vCalcLinAx.size()) ;
int nRotAxes = int( m_vCalcRotAx.size()) ;
if ( nInd >= 0 && nInd < nLinAxes) {
Axis* pAx = GetAxis( m_vCalcLinAx[nInd].nGrpId) ;
if ( pAx == nullptr)
return false ;
sAxToken = pAx->GetToken() ;
return true ;
}
else if ( nInd >= nLinAxes && nInd < nLinAxes + nRotAxes) {
Axis* pAx = GetAxis( m_vCalcRotAx[nInd-nLinAxes].nGrpId) ;
if ( pAx == nullptr)
return false ;
sAxToken = pAx->GetToken() ;
return true ;
}
return false ;
}
//----------------------------------------------------------------------------
bool
Machine::GetAllCurrAxesTokens( STRVECTOR& vAxToken) const
{
vAxToken.clear() ;
bool bOk = true ;
// ciclo sugli assi lineari correnti
for ( auto& CalcLinAx : m_vCalcLinAx) {
Axis* pAx = GetAxis( CalcLinAx.nGrpId) ;
if ( pAx != nullptr)
vAxToken.emplace_back( pAx->GetToken()) ;
else
bOk = false ;
}
// ciclo sugli assi rotanti correnti
for ( auto& CalcRotAx : m_vCalcRotAx) {
Axis* pAx = GetAxis( CalcRotAx.nGrpId) ;
if ( pAx != nullptr)
vAxToken.emplace_back( pAx->GetToken()) ;
else
bOk = false ;
}
return bOk ;
}
//----------------------------------------------------------------------------
bool
Machine::GetCurrAxisType( int nInd, bool& bLinear, bool& bHead) const
{
int nLinAxes = int( m_vCalcLinAx.size()) ;
int nRotAxes = int( m_vCalcRotAx.size()) ;
if ( nInd >= 0 && nInd < nLinAxes) {
bLinear = true ;
bHead = m_vCalcLinAx[nInd].bHead ;
return true ;
}
else if ( nInd >= nLinAxes && nInd < nLinAxes + nRotAxes) {
bLinear = false ;
bHead = m_vCalcRotAx[nInd-nLinAxes].bHead ;
return true ;
}
return false ;
}
//----------------------------------------------------------------------------
bool
Machine::GetCurrAxisMin( int nInd, double& dMin) const
{
int nLinAxes = int( m_vCalcLinAx.size()) ;
int nRotAxes = int( m_vCalcRotAx.size()) ;
if ( nInd >= 0 && nInd < nLinAxes) {
Axis* pAx = GetAxis( m_vCalcLinAx[nInd].nGrpId) ;
if ( pAx == nullptr)
return false ;
dMin = pAx->GetStroke().Min ;
return true ;
}
else if ( nInd >= nLinAxes && nInd < nLinAxes + nRotAxes) {
Axis* pAx = GetAxis( m_vCalcRotAx[nInd-nLinAxes].nGrpId) ;
if ( pAx == nullptr)
return false ;
dMin = pAx->GetStroke().Min ;
return true ;
}
return false ;
}
//----------------------------------------------------------------------------
bool
Machine::GetCurrAxisMax( int nInd, double& dMax) const
{
int nLinAxes = int( m_vCalcLinAx.size()) ;
int nRotAxes = int( m_vCalcRotAx.size()) ;
if ( nInd >= 0 && nInd < nLinAxes) {
Axis* pAx = GetAxis( m_vCalcLinAx[nInd].nGrpId) ;
if ( pAx == nullptr)
return false ;
dMax = pAx->GetStroke().Max ;
return true ;
}
else if ( nInd >= nLinAxes && nInd < nLinAxes + nRotAxes) {
Axis* pAx = GetAxis( m_vCalcRotAx[nInd-nLinAxes].nGrpId) ;
if ( pAx == nullptr)
return false ;
dMax = pAx->GetStroke().Max ;
return true ;
}
return false ;
}
//----------------------------------------------------------------------------
bool
Machine::GetCurrAxisOffset( int nInd, double& dOffset) const
{
int nLinAxes = int( m_vCalcLinAx.size()) ;
int nRotAxes = int( m_vCalcRotAx.size()) ;
if ( nInd >= 0 && nInd < nLinAxes) {
Axis* pAx = GetAxis( m_vCalcLinAx[nInd].nGrpId) ;
if ( pAx == nullptr)
return false ;
dOffset = pAx->GetOffset() ;
return true ;
}
else if ( nInd >= nLinAxes && nInd < nLinAxes + nRotAxes) {
Axis* pAx = GetAxis( m_vCalcRotAx[nInd-nLinAxes].nGrpId) ;
if ( pAx == nullptr)
return false ;
dOffset = pAx->GetOffset() ;
return true ;
}
return false ;
}
//----------------------------------------------------------------------------
bool
Machine::GetCurrAxisInvert( int nInd, bool& bInvert) const
{
int nLinAxes = int( m_vCalcLinAx.size()) ;
int nRotAxes = int( m_vCalcRotAx.size()) ;
if ( nInd >= 0 && nInd < nLinAxes) {
Axis* pAx = GetAxis( m_vCalcLinAx[nInd].nGrpId) ;
if ( pAx == nullptr)
return false ;
bInvert = pAx->GetInvert() ;
return true ;
}
else if ( nInd >= nLinAxes && nInd < nLinAxes + nRotAxes) {
Axis* pAx = GetAxis( m_vCalcRotAx[nInd-nLinAxes].nGrpId) ;
if ( pAx == nullptr)
return false ;
bInvert = pAx->GetInvert() ;
return true ;
}
return false ;
}
//----------------------------------------------------------------------------
bool
Machine::GetCurrAxisHomePos( int nInd, double& dHome) const
{
int nLinAxes = int( m_vCalcLinAx.size()) ;
int nRotAxes = int( m_vCalcRotAx.size()) ;
if ( nInd >= 0 && nInd < nLinAxes) {
Axis* pAx = GetAxis( m_vCalcLinAx[nInd].nGrpId) ;
if ( pAx == nullptr)
return false ;
dHome = pAx->GetHomeVal() ;
return true ;
}
else if ( nInd >= nLinAxes && nInd < nLinAxes + nRotAxes) {
Axis* pAx = GetAxis( m_vCalcRotAx[nInd-nLinAxes].nGrpId) ;
if ( pAx == nullptr)
return false ;
dHome = pAx->GetHomeVal() ;
return true ;
}
return false ;
}
//----------------------------------------------------------------------------
bool
Machine::GetAllCurrAxesHomePos( DBLVECTOR& vAxHomeVal) const
{
vAxHomeVal.clear() ;
bool bOk = true ;
// ciclo sugi assi lineari correnti
for ( auto& CalcLinAx : m_vCalcLinAx) {
Axis* pAx = GetAxis( CalcLinAx.nGrpId) ;
if ( pAx != nullptr)
vAxHomeVal.emplace_back( pAx->GetHomeVal()) ;
else
bOk = false ;
}
// ciclo sugi assi rotanti correnti
for ( auto& CalcRotAx : m_vCalcRotAx) {
Axis* pAx = GetAxis( CalcRotAx.nGrpId) ;
if ( pAx != nullptr)
vAxHomeVal.emplace_back( pAx->GetHomeVal()) ;
else
bOk = false ;
}
return bOk ;
}
//----------------------------------------------------------------------------
bool
Machine::GetRobotAngles( const Point3d& ptP, const Vector3d& vtDirT, const Vector3d& vtDirA,
DBLVECTOR& vAng1, DBLVECTOR& vAng2) const
{
// 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 ;
if ( ! frTool.Set( ORIG, vtDirTL, vtDirAL)) {
LOG_ERROR( GetEMkLogger(), "Error : tool reference not defined (robot)")
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() ;
// 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() ;
// calcolo il primo asse rotante
Vector3d vtArm = ptCen - m_vCalcRotAx[0].ptPos ;
double dAng1 ; bool bDet1 ;
if ( ! X_AX.GetRotation( vtArm, m_vCalcRotAx[0].vtDir, EPS_SMALL, dAng1, bDet1) || ! bDet1) {
LOG_ERROR( GetEMkLogger(), "Error : direction unreachable (robot)")
return false ;
}
vAng1.push_back( dAng1) ;
vAng2.push_back( dAng1) ;
// calcolo secondo e terzo asse rotante
Point3d ptR2 = m_vCalcRotAx[1].ptPos ;
ptR2.Rotate( m_vCalcRotAx[0].ptPos, m_vCalcRotAx[0].vtDir, dAng1) ;
Vector3d vtR2 = m_vCalcRotAx[1].vtDir ;
vtR2.Rotate( m_vCalcRotAx[0].vtDir, dAng1) ;
Vector3d vtR2Cen = ptCen - ( ptR2 + ( ptCen - ptR2) * vtR2 * vtR2) ;
double dDistR2Cen = vtR2Cen.Len() ;
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.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) {
double dDeltaOut = dDistR2Cen - ( dDistR2R3 + dDistR3R5) ;
double dDeltaIn = dDistR2Cen - abs( dDistR2R3 - dDistR3R5) ;
string sErr = "Error : position for the robot unreachable" ;
if ( dDeltaOut > 0)
sErr += " (Out=" + ToString( dDeltaOut, 3) + ")" ;
else if ( dDeltaIn < 0)
sErr += " (In=" + ToString( dDeltaIn, 3) + ")" ;
LOG_ERROR( GetEMkLogger(), sErr.c_str())
return false ;
}
double dAngB = acos( dCosB) * RADTODEG ;
double dAng2 ; bool bDet2 ;
if ( ! Z_AX.GetRotation( vtR2Cen, vtR2, EPS_SMALL, dAng2, bDet2) || ! bDet2) {
LOG_ERROR( GetEMkLogger(), "Error : R2 not calculable (robot)")
return false ;
}
dAng2 -= dAngB ;
vAng1.push_back( dAng2) ;
vAng2.push_back( dAng2) ;
double dAngC = acos( dCosC) * RADTODEG ;
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 ;
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) ;
Vector3d vtCenL = GetToLoc( vtCen, frWrist) ;
double dAng4, dAng5 ;
vtCenL.ToSpherical( nullptr, &dAng5, &dAng4) ;
if ( dAng4 > ANG_STRAIGHT)
dAng4 -= ANG_FULL ;
if ( dAng4 > ANG_RIGHT) {
dAng4 -= ANG_STRAIGHT ;
dAng5 = -dAng5 ;
}
else if ( dAng4 < -ANG_RIGHT) {
dAng4 += ANG_STRAIGHT ;
dAng5 = -dAng5 ;
}
vAng1.push_back( dAng4) ;
vAng1.push_back( dAng5) ;
vAng2.push_back( dAng4 + ( dAng4 > EPS_ANG_ZERO ? -ANG_STRAIGHT : ANG_STRAIGHT)) ;
vAng2.push_back( -dAng5) ;
// calcolo il terzo asse rotante del polso
Vector3d vtR6 = m_vCalcRotAx[5].vtDir ;
for ( int i = 4; i >= 0 ; --i)
vtR6.Rotate( m_vCalcRotAx[i].vtDir, vAng1[i]) ;
Vector3d vtTool = m_vtCalcDir ;
for ( int i = 4; i >= 0 ; --i)
vtTool.Rotate( m_vCalcRotAx[i].vtDir, vAng1[i]) ;
double dAng6 ; bool 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( vtDirAL, vtR6, EPS_SMALL, dAng6, bDet6) || ! bDet6) {
LOG_ERROR( GetEMkLogger(), "Error : R6 not calculable (robot)")
return false ;
}
}
vAng1.push_back( dAng6) ;
vAng2.push_back( dAng6 + ( dAng6 > EPS_ANG_ZERO ? -ANG_STRAIGHT : ANG_STRAIGHT)) ;
// verifiche dei limiti di corsa
bool bAng1 = true, bAng2 = true ;
for ( int i = 0 ; i < int( vAng1.size()) ; ++ i) {
if ( ! AdjustAngleInStroke( m_vCalcRotAx[i].stroke, vAng1[i]))
bAng1 = false ;
if ( ! AdjustAngleInStroke( m_vCalcRotAx[i].stroke, vAng2[i]))
bAng2 = false ;
}
if ( bAng2) {
if ( ! bAng1) {
vAng1 = vAng2 ;
vAng2.clear() ;
swap( bAng1, bAng2) ;
}
}
else {
vAng2.clear() ;
}
if ( ! bAng1) {
int nStat ;
VerifyOutstroke( 0, 0, 0, vAng1, true, nStat) ;
LOG_ERROR( GetEMkLogger(), GetOutstrokeInfo().c_str())
return false ;
}
return true ;
}
Reference in New Issue View Git Blame Copy Permalink