kinematics.cpp
Go to the documentation of this file.
00001  /**************************************************************************
00002 * kinematics.cpp -
00003 * Kinematics class for Katana4XX Robots using kinematics lib
00004 * Copyright (C) 2007-2008 Neuronics AG
00005 * PKE/UKE 2007, JHA 2008
00006  **************************************************************************/
00007  
00008 #include "kinematics.h"
00009 
00011 KinematicsLib::KinematicsLib(){
00012         initializeMembers();
00013 }
00015 KinematicsLib::KinematicsLib(int type){
00016         initializeMembers();
00017         setType(type);
00018         init();
00019 }
00021 KinematicsLib::~KinematicsLib(){
00022         delete _anaGuess;
00023 }
00025 
00026 
00027 int KinematicsLib::sign(int value) {
00028         return (value > 0) - (value < 0);
00029 }
00030 int KinematicsLib::sign(double value) {
00031         return (value > 0) - (value < 0);
00032 }
00033 #ifdef WIN32
00034 double KinematicsLib::round( double x)
00035 // Copyright (C) 2001 Tor M. Aamodt, University of Toronto
00036 // Permisssion to use for all purposes commercial and otherwise granted.
00037 // THIS MATERIAL IS PROVIDED "AS IS" WITHOUT WARRANTY, OR ANY CONDITION OR
00038 // OTHER TERM OF ANY KIND INCLUDING, WITHOUT LIMITATION, ANY WARRANTY
00039 // OF MERCHANTABILITY, SATISFACTORY QUALITY, OR FITNESS FOR A PARTICULAR
00040 // PURPOSE.
00041 {
00042    if( x > 0 ) {
00043        __int64 xint = (__int64) (x+0.5);
00044        if( xint % 2 ) {
00045            // then we might have an even number...
00046            double diff = x - (double)xint;
00047            if( diff == -0.5 )
00048                return double(xint-1);
00049        }
00050        return double(xint);
00051    } else {
00052        __int64 xint = (__int64) (x-0.5);
00053        if( xint % 2 ) {
00054            // then we might have an even number...
00055            double diff = x - (double)xint;
00056            if( diff == 0.5 )
00057                return double(xint+1);
00058        }
00059        return double(xint);
00060    }
00061 }
00062 #endif // ifdef WIN32
00063 int KinematicsLib::initializeMembers() {
00064         _type = -1;
00065         _matrixInit = false;
00066         _dof = -1;
00067         _dom = -1;
00068         _angOffInit = false;
00069         _angRanInit = false;
00070         _immobile = 0;
00071         _thetaimmobile = 0.0;
00072         _initialized = false;
00073         for (int i = 0; i < 4; ++i) {
00074                 _tcpOffset[i] = 0.0;
00075         }
00076         return 1;
00077 }
00078 int KinematicsLib::setAngleMinMax() {
00079         int dir;
00080         for (int i = 0; i < _dof; i++) {
00081                 dir = sign(_encoderOffset[i]) * _rotDir[i];
00082                 if (dir < 0) {
00083                         _angleMin[i] = _angleOffset[i];
00084                         _angleMax[i] = _angleOffset[i] + _angleRange[i];
00085                 } else {
00086                         _angleMax[i] = _angleOffset[i];
00087                         _angleMin[i] = _angleOffset[i] - _angleRange[i];
00088                 }
00089                 _data(i + 1, 6) = _angleMin[i];
00090                 _data(i + 1, 7) = _angleMax[i];
00091         }
00092 
00093         return 1;
00094 }
00095 int KinematicsLib::initDofMat(int dof) {
00096         _dof = dof;
00097         _dom = _dof;
00098         _data = Matrix(_dof, 23);
00099         _data = 0.0;
00100         _matrixInit = true;
00101 
00102         return 1;
00103 }
00104 int KinematicsLib::angleArrMDH2vecK4D(const double arr[], std::vector<double>* angleK4D) {
00105         if (_type < 0)
00106                 return -1;
00107         std::vector<double> angleMDH;
00108         for (int i = 0; i < _dom; ++i) {
00109                 angleMDH.push_back(arr[i]);
00110         }
00111         angleK4D->clear();
00112         mDH2K4DAng(angleMDH, *angleK4D);
00113         return 1;
00114 }
00115 
00116 
00118 int KinematicsLib::setType(int type) {
00119         std::vector<double> angOff;
00120         double angStopArr[MaxDof];
00121         std::vector<double> angStop;
00122         std::vector<double> lengths;
00123         switch(type) {
00124         case K_6M90A_F: // 0
00125                 _type = type;
00126                 initDofMat(Dof_90);
00127                 _data << Katana6M90A_F_data;
00128                 for (int i = 0; i < _dof; i++) {
00129                         _data(i + 1, 3) *= LENGTH_MULTIPLIER;
00130                         _data(i + 1, 4) *= LENGTH_MULTIPLIER;
00131                 }
00132                 for (int i = 0; i < _dof; i++) {
00133                         _epc[i] = Encoder_per_cycle[i];
00134                         _encoderOffset[i] = Encoder_offset[i];
00135                         _rotDir[i] = Rotation_direction[i];
00136                         _angleOffset[i] = Angle_offset_90A_F[i];
00137                         _angleRange[i] = Angle_range_90A_F[i];
00138                 }
00139                 _angOffInit = true;
00140                 _angRanInit = true;
00141                 setAngleMinMax();
00142                 for (int i = 0; i < 4; i++) {
00143                         _linkLength[i] = Link_length_90A_F[i];
00144                 }
00145                 // analytical guess
00146                 _anaGuess = (AnaGuess::Kinematics*) new AnaGuess::Kinematics6M90T();
00147                 angleArrMDH2vecK4D(_angleOffset, &angOff);
00148                 _anaGuess->setAngOff(angOff);
00149                 for (int i = 0; i < _dom; ++i) {
00150                         angStopArr[i] = _angleOffset[i] - sign(_encoderOffset[i]) * _rotDir[i] *
00151                                 _angleRange[i];
00152                 }
00153                 angleArrMDH2vecK4D(angStopArr, &angStop);
00154                 _anaGuess->setAngStop(angStop);
00155                 for (int i = 0; i < 4; ++i) {
00156                         lengths.push_back(_linkLength[i] * 1000);
00157                 }
00158                 _anaGuess->setLinkLength(lengths);
00159                 break;
00160         case K_6M90A_G: // 1
00161                 _type = type;
00162                 initDofMat(Dof_90);
00163                 _data << Katana6M90A_G_data;
00164                 for (int i = 0; i < _dof; i++) {
00165                         _data(i + 1, 3) *= LENGTH_MULTIPLIER;
00166                         _data(i + 1, 4) *= LENGTH_MULTIPLIER;
00167                 }
00168                 _dom = Dof_90 - 1;
00169                 _immobile = true;
00170                 _thetaimmobile = _data(_dof, 2);
00171                 for (int i = 0; i < _dof; i++) {
00172                         _epc[i] = Encoder_per_cycle[i];
00173                         _encoderOffset[i] = Encoder_offset[i];
00174                         _rotDir[i] = Rotation_direction[i];
00175                         _angleOffset[i] = Angle_offset_90A_G[i];
00176                         _angleRange[i] = Angle_range_90A_G[i];
00177                 }
00178                 _angOffInit = true;
00179                 _angRanInit = true;
00180                 setAngleMinMax();
00181                 for (int i = 0; i < 4; i++) {
00182                         _linkLength[i] = Link_length_90A_G[i];
00183                 }
00184                 // analytical guess
00185                 _anaGuess = (AnaGuess::Kinematics*) new AnaGuess::Kinematics6M90G();
00186                 angleArrMDH2vecK4D(_angleOffset, &angOff);
00187                 angOff.push_back(-2.150246); // immobile angle in mDH
00188                 _anaGuess->setAngOff(angOff);
00189                 for (int i = 0; i < _dom; ++i) {
00190                         angStopArr[i] = _angleOffset[i] - sign(_encoderOffset[i]) * _rotDir[i] *
00191                                 _angleRange[i];
00192                 }
00193                 angleArrMDH2vecK4D(angStopArr, &angStop);
00194                 angStop.push_back(3.731514); // immobile angle in mDH
00195                 _anaGuess->setAngStop(angStop);
00196                 for (int i = 0; i < 4; ++i) {
00197                         lengths.push_back(_linkLength[i] * 1000);
00198                 }
00199                 _anaGuess->setLinkLength(lengths);
00200                 break;
00201         case K_6M180: // 2
00202                 _type = type;
00203                 initDofMat(Dof_180);
00204                 _data << Katana6M180_data;
00205                 for (int i = 0; i < _dof; i++) {
00206                         _data(i + 1, 3) *= LENGTH_MULTIPLIER;
00207                         _data(i + 1, 4) *= LENGTH_MULTIPLIER;
00208                 }
00209                 for (int i = 0; i < _dof; i++) {
00210                         _epc[i] = Encoder_per_cycle[i];
00211                         _encoderOffset[i] = Encoder_offset[i];
00212                         _rotDir[i] = Rotation_direction[i];
00213                         _angleOffset[i] = Angle_offset_180[i];
00214                         _angleRange[i] = Angle_range_180[i];
00215                 }
00216                 _angOffInit = true;
00217                 _angRanInit = true;
00218                 setAngleMinMax();
00219                 for (int i = 0; i < 4; i++) {
00220                         _linkLength[i] = Link_length_180[i];
00221                 }
00222                 // analytical guess
00223                 _anaGuess = (AnaGuess::Kinematics*) new AnaGuess::Kinematics6M180();
00224                 angleArrMDH2vecK4D(_angleOffset, &angOff);
00225                 angOff.push_back(-2.150246); // angle not present in mDH
00226                 _anaGuess->setAngOff(angOff);
00227                 for (int i = 0; i < _dom; ++i) {
00228                         angStopArr[i] = _angleOffset[i] - sign(_encoderOffset[i]) * _rotDir[i] *
00229                                 _angleRange[i];
00230                 }
00231                 angleArrMDH2vecK4D(angStopArr, &angStop);
00232                 angStop.push_back(3.731514); // angle not present in mDH
00233                 _anaGuess->setAngStop(angStop);
00234                 for (int i = 0; i < 4; ++i) {
00235                         lengths.push_back(_linkLength[i] * 1000);
00236                 }
00237                 _anaGuess->setLinkLength(lengths);
00238                 break;
00239         case K_6M90B_F: // 3
00240                 _type = type;
00241                 initDofMat(Dof_90);
00242                 _data << Katana6M90B_F_data;
00243                 for (int i = 0; i < _dof; i++) {
00244                         _data(i + 1, 3) *= LENGTH_MULTIPLIER;
00245                         _data(i + 1, 4) *= LENGTH_MULTIPLIER;
00246                 }
00247                 for (int i = 0; i < _dof; i++) {
00248                         _epc[i] = Encoder_per_cycle[i];
00249                         _encoderOffset[i] = Encoder_offset[i];
00250                         _rotDir[i] = Rotation_direction[i];
00251                         _angleOffset[i] = Angle_offset_90B_F[i];
00252                         _angleRange[i] = Angle_range_90B_F[i];
00253                 }
00254                 _angOffInit = true;
00255                 _angRanInit = true;
00256                 setAngleMinMax();
00257                 for (int i = 0; i < 4; i++) {
00258                         _linkLength[i] = Link_length_90B_F[i];
00259                 }
00260                 // analytical guess
00261                 _anaGuess = (AnaGuess::Kinematics*) new AnaGuess::Kinematics6M90T();
00262                 angleArrMDH2vecK4D(_angleOffset, &angOff);
00263                 _anaGuess->setAngOff(angOff);
00264                 for (int i = 0; i < _dom; ++i) {
00265                         angStopArr[i] = _angleOffset[i] - sign(_encoderOffset[i]) * _rotDir[i] *
00266                                 _angleRange[i];
00267                 }
00268                 angleArrMDH2vecK4D(angStopArr, &angStop);
00269                 _anaGuess->setAngStop(angStop);
00270                 for (int i = 0; i < 4; ++i) {
00271                         lengths.push_back(_linkLength[i] * 1000);
00272                 }
00273                 _anaGuess->setLinkLength(lengths);
00274                 break;
00275         case K_6M90B_G: // 4
00276                 _type = type;
00277                 initDofMat(Dof_90);
00278                 _data << Katana6M90B_G_data;
00279                 for (int i = 0; i < _dof; i++) {
00280                         _data(i + 1, 3) *= LENGTH_MULTIPLIER;
00281                         _data(i + 1, 4) *= LENGTH_MULTIPLIER;
00282                 }
00283                 _dom = Dof_90 - 1;
00284                 _immobile = true;
00285                 _thetaimmobile = _data(_dof, 2);
00286                 for (int i = 0; i < _dof; i++) {
00287                         _epc[i] = Encoder_per_cycle[i];
00288                         _encoderOffset[i] = Encoder_offset[i];
00289                         _rotDir[i] = Rotation_direction[i];
00290                         _angleOffset[i] = Angle_offset_90B_G[i];
00291                         _angleRange[i] = Angle_range_90B_G[i];
00292                 }
00293                 _angOffInit = true;
00294                 _angRanInit = true;
00295                 setAngleMinMax();
00296                 for (int i = 0; i < 4; i++) {
00297                         _linkLength[i] = Link_length_90B_G[i];
00298                 }
00299                 // analytical guess
00300                 _anaGuess = (AnaGuess::Kinematics*) new AnaGuess::Kinematics6M90G();
00301                 angleArrMDH2vecK4D(_angleOffset, &angOff);
00302                 angOff.push_back(-2.150246); // immobile angle in mDH
00303                 _anaGuess->setAngOff(angOff);
00304                 for (int i = 0; i < _dom; ++i) {
00305                         angStopArr[i] = _angleOffset[i] - sign(_encoderOffset[i]) * _rotDir[i] *
00306                                 _angleRange[i];
00307                 }
00308                 angleArrMDH2vecK4D(angStopArr, &angStop);
00309                 angStop.push_back(3.731514); // immobile angle in mDH
00310                 _anaGuess->setAngStop(angStop);
00311                 for (int i = 0; i < 4; ++i) {
00312                         lengths.push_back(_linkLength[i] * 1000);
00313                 }
00314                 _anaGuess->setLinkLength(lengths);
00315                 break;
00316         default:
00317                 return -1;
00318         }
00319 
00320         return 1;
00321 }
00323 int KinematicsLib::setMDH(std::vector<double> theta, std::vector<double> d,
00324                 std::vector<double> a, std::vector<double> alpha, int typeNr) {
00325         // check vector sizes
00326         if (_dof == -1) {
00327                 if ((int)theta.size() > MaxDof)
00328                         return -1;
00329                 initDofMat(theta.size());
00330         }
00331 
00332         if ((int)theta.size() != _dof || (int)d.size() != _dof ||
00333                         (int)a.size() != _dof || (int)alpha.size() != _dof) {
00334                 return -1;
00335         }
00336 
00337         if (typeNr >= 0)
00338                 typeNr = -2;
00339 
00340         for (int i = 0; i < _dof; ++i) {
00341                 _data(i + 1, 2) = theta.at(i);
00342                 _data(i + 1, 3) = d.at(i) * LENGTH_MULTIPLIER;
00343                 _data(i + 1, 4) = a.at(i) * LENGTH_MULTIPLIER;
00344                 _data(i + 1, 5) = alpha.at(i);
00345                 _data(i + 1, 23) = 0;
00346         }
00347 
00348         _dom = _dof;
00349         _immobile = false;
00350         _type = typeNr;
00351 
00352         return 1;
00353 }
00355 int KinematicsLib::setLinkLen(std::vector<double> links) {
00356         if ((_dof == -1) || ((int)links.size() != 4))
00357                 return -1;
00358 
00359         switch (_type) {
00360         case K_6M90A_F: // 0
00361         case K_6M90A_G: // 1
00362         case K_6M90B_F: // 3
00363         case K_6M90B_G: // 4
00364                 _data(3, 4) = links.at(0) * LENGTH_MULTIPLIER;
00365                 _data(4, 4) = links.at(1) * LENGTH_MULTIPLIER;
00366                 _data(5, 3) = links.at(2) * LENGTH_MULTIPLIER;
00367                 _data(6, 3) = links.at(3) * LENGTH_MULTIPLIER;
00368                 break;
00369         case K_6M180: // 2
00370                 _data(3, 4) = links.at(0) * LENGTH_MULTIPLIER;
00371                 _data(4, 4) = links.at(1) * LENGTH_MULTIPLIER;
00372                 _data(5, 3) = (links.at(2) + links.at(3)) * LENGTH_MULTIPLIER;
00373                 break;
00374         default:
00375                 return -1;
00376         }
00377 
00378         // store in _linkLength
00379         for (int i = 0; i < 4; ++i) {
00380                 _linkLength[i] = links.at(i);
00381         }
00382 
00383         // set in AnalyticalGuess
00384         std::vector<double> lengths;
00385         for (int i = 0; i < 4; ++i) {
00386                 lengths.push_back(_linkLength[i] * 1000);
00387         }
00388         _anaGuess->setLinkLength(lengths);
00389 
00390         return 1;
00391 }
00393 int KinematicsLib::setImmob(int immob) {
00394         if (_dof == -1 || immob < 0 || immob > 1) {
00395                 return -1;
00396         }
00397 
00398         _data(_dof, 23) = immob;
00399         _immobile = immob;
00400         if (immob) {
00401                 _dom = _dof - 1;
00402                 _thetaimmobile = _data(_dof, 2);
00403         } else {
00404                 _dom = _dof;
00405         }
00406 
00407         return 1;
00408 }
00410 int KinematicsLib::setEPC(std::vector<int> epc) {
00411         if ((int)epc.size() < _dom) {
00412                 return -1;
00413         }
00414 
00415         for (int i = 0; i < _dom; ++i) {
00416                 _epc[i] = epc.at(i);
00417         }
00418 
00419         return 1;
00420 }
00422 int KinematicsLib::setEncOff(std::vector<int> encOffset) {
00423         if ((int)encOffset.size() < _dom) {
00424                 return -1;
00425         }
00426 
00427         for (int i = 0; i < _dom; ++i) {
00428                 _encoderOffset[i] = encOffset.at(i);
00429         }
00430 
00431         return 1;
00432 }
00434 int KinematicsLib::setRotDir(std::vector<int> rotDir) {
00435         if ((int)rotDir.size() < _dom) {
00436                 return -1;
00437         }
00438 
00439         for (int i = 0; i < _dom; ++i) {
00440                 if (rotDir.at(i) < 0) {
00441                         _rotDir[i] = -1;
00442                 } else {
00443                         _rotDir[i] = 1;
00444                 }
00445         }
00446 
00447         return 1;
00448 }
00450 int KinematicsLib::setAngOff(std::vector<double> angleOffset) {
00451         if ((int)angleOffset.size() < _dom) {
00452                 return -1;
00453         }
00454 
00455         for (int i = 0; i < _dom; ++i) {
00456                 _angleOffset[i] = angleOffset.at(i);
00457         }
00458         _angOffInit = true;
00459         if (_angRanInit)
00460                 setAngleMinMax();
00461 
00462         // analytical guess
00463         std::vector<double> angOff;
00464         double angStopArr[MaxDof];
00465         std::vector<double> angStop;
00466         switch(_type) {
00467         case K_6M90A_F: // 0
00468         case K_6M90B_F: // 3
00469                 angleArrMDH2vecK4D(_angleOffset, &angOff);
00470                 _anaGuess->setAngOff(angOff);
00471                 for (int i = 0; i < _dom; ++i) {
00472                         angStopArr[i] = _angleOffset[i] - sign(_encoderOffset[i]) * _rotDir[i] *
00473                                 _angleRange[i];
00474                 }
00475                 angleArrMDH2vecK4D(angStopArr, &angStop);
00476                 _anaGuess->setAngStop(angStop);
00477                 break;
00478         case K_6M90A_G: // 1
00479         case K_6M90B_G: // 4
00480                 angleArrMDH2vecK4D(_angleOffset, &angOff);
00481                 angOff.push_back(-2.150246); // immobile angle in mDH
00482                 _anaGuess->setAngOff(angOff);
00483                 for (int i = 0; i < _dom; ++i) {
00484                         angStopArr[i] = _angleOffset[i] - sign(_encoderOffset[i]) * _rotDir[i] *
00485                                 _angleRange[i];
00486                 }
00487                 angleArrMDH2vecK4D(angStopArr, &angStop);
00488                 angStop.push_back(3.731514); // immobile angle in mDH
00489                 _anaGuess->setAngStop(angStop);
00490                 break;
00491         case K_6M180: // 2
00492                 angleArrMDH2vecK4D(_angleOffset, &angOff);
00493                 angOff.push_back(-2.150246); // angle not present in mDH
00494                 _anaGuess->setAngOff(angOff);
00495                 for (int i = 0; i < _dom; ++i) {
00496                         angStopArr[i] = _angleOffset[i] - sign(_encoderOffset[i]) * _rotDir[i] *
00497                                 _angleRange[i];
00498                 }
00499                 angleArrMDH2vecK4D(angStopArr, &angStop);
00500                 angStop.push_back(3.731514); // angle not present in mDH
00501                 _anaGuess->setAngStop(angStop);
00502                 break;
00503         default:
00504                 break;
00505         }
00506 
00507         return 1;
00508 }
00510 int KinematicsLib::setAngRan(std::vector<double> angleRange) {
00511         if ((int)angleRange.size() < _dom) {
00512                 return -1;
00513         }
00514 
00515         for (int i = 0; i < _dom; ++i) {
00516                 _angleRange[i] = angleRange.at(i);
00517         }
00518         _angRanInit = true;
00519         if (_angOffInit)
00520                 setAngleMinMax();
00521 
00522         // analytical guess
00523         double angStopArr[MaxDof];
00524         std::vector<double> angStop;
00525         switch(_type) {
00526         case K_6M90A_F: // 0
00527         case K_6M90B_F: // 3
00528                 for (int i = 0; i < _dom; ++i) {
00529                         angStopArr[i] = _angleOffset[i] - sign(_encoderOffset[i]) * _rotDir[i] *
00530                                 _angleRange[i];
00531                 }
00532                 angleArrMDH2vecK4D(angStopArr, &angStop);
00533                 _anaGuess->setAngStop(angStop);
00534                 break;
00535         case K_6M90A_G: // 1
00536         case K_6M90B_G: // 4
00537                 for (int i = 0; i < _dom; ++i) {
00538                         angStopArr[i] = _angleOffset[i] - sign(_encoderOffset[i]) * _rotDir[i] *
00539                                 _angleRange[i];
00540                 }
00541                 angleArrMDH2vecK4D(angStopArr, &angStop);
00542                 angStop.push_back(3.731514); // immobile angle in mDH
00543                 _anaGuess->setAngStop(angStop);
00544                 break;
00545         case K_6M180: // 2
00546                 for (int i = 0; i < _dom; ++i) {
00547                         angStopArr[i] = _angleOffset[i] - sign(_encoderOffset[i]) * _rotDir[i] *
00548                                 _angleRange[i];
00549                 }
00550                 angleArrMDH2vecK4D(angStopArr, &angStop);
00551                 angStop.push_back(3.731514); // angle not present in mDH
00552                 _anaGuess->setAngStop(angStop);
00553                 break;
00554         default:
00555                 break;
00556         }
00557 
00558         return 1;
00559 }
00561 int KinematicsLib::setTcpOff(std::vector<double> tcpOffset) {
00562         if ((int)tcpOffset.size() < 4) {
00563                 return -1;
00564         }
00565 
00566         for (int i = 0; i < 4; ++i) {
00567                 _tcpOffset[i] = tcpOffset.at(i);
00568         }
00569 
00570         return 1;
00571 }
00573 int KinematicsLib::getType() {
00574         return _type;
00575 }
00577 int KinematicsLib::getMaxDOF() {
00578         return MaxDof;
00579 }
00581 int KinematicsLib::getDOF() {
00582         return _dof;
00583 }
00585 int KinematicsLib::getDOM() {
00586         return _dom;
00587 }
00589 int KinematicsLib::getMDH(std::vector<double>& theta, std::vector<double>& d,
00590                 std::vector<double>& a, std::vector<double>& alpha) {
00591         if (_dof == -1)
00592                 return -1;
00593         theta.clear();
00594         d.clear();
00595         a.clear();
00596         alpha.clear();
00597         for (int i = 0; i < _dof; ++i) {
00598                 theta.push_back(_data(i + 1, 2));
00599                 d.push_back(_data(i + 1, 3) / LENGTH_MULTIPLIER);
00600                 a.push_back(_data(i + 1, 4) / LENGTH_MULTIPLIER);
00601                 alpha.push_back(_data(i + 1, 5));
00602         }
00603 
00604         return 1;
00605 }
00607 int KinematicsLib::getImmob() {
00608         return _immobile;
00609 }
00611 int KinematicsLib::getEPC(std::vector<int>& epc) {
00612         if (_dof == -1)
00613                 return -1;
00614         epc.clear();
00615         for (int i = 0; i < _dom; ++i) {
00616                 epc.push_back(_epc[i]);
00617         }
00618 
00619         return 1;
00620 }
00622 int KinematicsLib::getEncOff(std::vector<int>& encOffset) {
00623         if (_dof == -1)
00624                 return -1;
00625         encOffset.clear();
00626         for (int i = 0; i < _dom; ++i) {
00627                 encOffset.push_back(_encoderOffset[i]);
00628         }
00629 
00630         return 1;
00631 }
00633 int KinematicsLib::getRotDir(std::vector<int>& rotDir) {
00634         if (_dof == -1)
00635                 return -1;
00636         rotDir.clear();
00637         for (int i = 0; i < _dom; ++i) {
00638                 rotDir.push_back(_rotDir[i]);
00639         }
00640 
00641         return 1;
00642 }
00644 int KinematicsLib::getAngOff(std::vector<double>& angleOffset) {
00645         if (_dof == -1)
00646                 return -1;
00647         angleOffset.clear();
00648         for (int i = 0; i < _dom; ++i) {
00649                 angleOffset.push_back(_angleOffset[i]);
00650         }
00651 
00652         return 1;
00653 }
00655 int KinematicsLib::getAngRan(std::vector<double>& angleRange) {
00656         if (_dof == -1)
00657                 return -1;
00658         angleRange.clear();
00659         for (int i = 0; i < _dom; ++i) {
00660                 angleRange.push_back(_angleRange[i]);
00661         }
00662 
00663         return 1;
00664 }
00666 int KinematicsLib::getAngStop(std::vector<double>& angleStop) {
00667         std::vector<double> angoff;
00668         int okcount = getAngOff(angoff);
00669         std::vector<int> encoff;
00670         okcount += getEncOff(encoff);
00671         std::vector<int> rotdir;
00672         okcount += getRotDir(rotdir);
00673         std::vector<double> angran;
00674         okcount += getAngRan(angran);
00675         angleStop.clear();
00676         for (int i = 0; i < _dom; i++) {
00677                 angleStop.push_back(angoff.at(i) - (sign(encoff.at(i)) * rotdir.at(i))
00678                                 * (angran.at(i)));
00679         }
00680         int ok = (okcount == 4) ? 1 : 0;
00681         return ok;
00682 }
00684 int KinematicsLib::getAngMin(std::vector<double>& angleMin) {
00685         std::vector<double> angoff;
00686         int okcount = getAngOff(angoff);
00687         std::vector<double> angstop;
00688         okcount += getAngStop(angstop);
00689         angleMin.clear();
00690         for (int i = 0; i < _dom; ++i) {
00691                 angleMin.push_back(min(angoff.at(i), angstop.at(i)));
00692         }
00693         int ok = (okcount == 2) ? 1 : 0;
00694         return ok;
00695 }
00697 int KinematicsLib::getAngMax(std::vector<double>& angleMax) {
00698         std::vector<double> angoff;
00699         int okcount = getAngOff(angoff);
00700         std::vector<double> angstop;
00701         okcount += getAngStop(angstop);
00702         angleMax.clear();
00703         for (int i = 0; i < _dom; ++i) {
00704                 angleMax.push_back(max(angoff.at(i), angstop.at(i)));
00705         }
00706         int ok = (okcount == 2) ? 1 : 0;
00707         return ok;
00708 }
00710 int KinematicsLib::getTcpOff(std::vector<double>& tcpOffset) {
00711         if (_dof == -1)
00712                 return -1;
00713         tcpOffset.clear();
00714         for (int i = 0; i < 4; ++i) {
00715                 tcpOffset.push_back(_tcpOffset[i]);
00716         }
00717 
00718         return 1;
00719 }
00721 int KinematicsLib::getVersion(std::vector<int>& version) {
00722         version.clear();
00723         version.push_back(KINLIB_VERSION_MAJOR);
00724         version.push_back(KINLIB_VERSION_MINOR);
00725         version.push_back(KINLIB_VERSION_REVISION);
00726 
00727         return 1;
00728 }
00730 int KinematicsLib::init() {
00731         if (!_matrixInit || !_angOffInit || !_angRanInit)
00732                 return -1;
00733 
00734         _robot = mRobot(_data);
00735         _initialized = true;
00736 
00737         return 1;
00738 }
00740 int KinematicsLib::K4D2mDHAng(std::vector<double> angleK4D, std::vector<double>& angleMDH) {
00741         if (_type == -1)
00742                 return -1;
00743 
00744         if ((int)angleK4D.size() < _dom)
00745                 return -1;
00746 
00747         angleMDH.clear();
00748         
00749         // for all models the same
00750         angleMDH.push_back(angleK4D.at(0) - mPi);
00751         angleMDH.push_back(angleK4D.at(1));
00752         angleMDH.push_back(angleK4D.at(2) - mPi);
00753         angleMDH.push_back(mPi / 2.0 - angleK4D.at(3));
00754 
00755         // model specific
00756         switch (_type) {
00757         case K_6M90A_F: // 0
00758         case K_6M90B_F: // 3
00759                 angleMDH.push_back(mPi / 2.0 - angleK4D.at(4));
00760                 angleMDH.push_back(mPi / 2.0 - angleK4D.at(5));
00761                 break;
00762         case K_6M90A_G: // 1
00763         case K_6M90B_G: // 4
00764                 angleMDH.push_back(mPi / 2.0 - angleK4D.at(4));
00765                 break;
00766         case K_6M180: // 2
00767                 angleMDH.push_back(-1.0 * angleK4D.at(4) + 3.0 * mPi / 2.0);
00768                 break;
00769         default:
00770                 return -1;
00771         }
00772 
00773         return 1;
00774 }
00776 int KinematicsLib::mDH2K4DAng(std::vector<double> angleMDH, std::vector<double>& angleK4D) {
00777         if (_type == -1)
00778                 return -1;
00779 
00780         if ((int)angleMDH.size() < _dom)
00781                 return -1;
00782 
00783         angleK4D.clear();
00784         
00785         // for all models the same
00786         angleK4D.push_back(angleMDH.at(0) + mPi);
00787         angleK4D.push_back(angleMDH.at(1));
00788         angleK4D.push_back(angleMDH.at(2) + mPi);
00789         angleK4D.push_back(mPi / 2.0 - angleMDH.at(3));
00790 
00791         // model specific
00792         switch (_type) {
00793         case K_6M90A_F: // 0
00794         case K_6M90B_F: // 3
00795                 angleK4D.push_back(mPi / 2.0 - angleMDH.at(4));
00796                 angleK4D.push_back(mPi / 2.0 - angleMDH.at(5));
00797                 break;
00798         case K_6M90A_G: // 1
00799         case K_6M90B_G: // 4
00800                 angleK4D.push_back(mPi / 2.0 - angleMDH.at(4));
00801                 break;
00802         case K_6M180: // 2
00803                 angleK4D.push_back(-1.0 * angleMDH.at(4) + 3.0 * mPi / 2.0);
00804                 break;
00805         default:
00806                 return -1;
00807         }
00808 
00809         return 1;
00810 }
00812 int KinematicsLib::enc2rad(std::vector<int> encoders,
00813                 std::vector<double>& angles) {
00814         if ((int)encoders.size() < _dom)
00815                 return -1;
00816 
00817         angles.clear();
00818         for(int i = 0; i < _dom; ++i) {
00819                 angles.push_back(_angleOffset[i] + _rotDir[i] * (encoders.at(i) -
00820                         _encoderOffset[i]) * 2.0 * mPi / ((double) _epc[i]));
00821         }
00822 
00823         return 1;
00824 }
00826 int KinematicsLib::rad2enc(std::vector<double> angles,
00827                 std::vector<int>& encoders) {
00828         if ((int)angles.size() < _dom)
00829                 return -1;
00830 
00831         encoders.clear();
00832         for(int i = 0; i < _dom; ++i){
00833                 encoders.push_back((int) round(_encoderOffset[i] + _rotDir[i] * (angles.at(i) -
00834                         _angleOffset[i]) * _epc[i] / (2 * mPi)));
00835         }
00836 
00837         return 1;
00838 }
00840 int KinematicsLib::directKinematics(std::vector<double> angles,
00841                 std::vector<double>& pose) {
00842         if (!_initialized || (int)angles.size() < _dom)
00843                 return -1;
00844         
00845         //Angles in ColumnVector
00846         ColumnVector qr = ColumnVector(_dof);
00847         for(int k = 0; k < _dof; ++k){
00848                 if (k == _dom) {
00849                         // immobile = 1: _dom == _dof - 1
00850                         qr(k + 1) = (float) _thetaimmobile;
00851                 } else {
00852                         qr(k + 1) = angles.at(k);
00853                 }
00854         }
00855         _robot.set_q(qr);
00856 
00857         //Calculate kinematics
00858         Matrix TCP_Position = _robot.kine();
00859         TCP_Position(1,4) /= LENGTH_MULTIPLIER;
00860         TCP_Position(2,4) /= LENGTH_MULTIPLIER;
00861         TCP_Position(3,4) /= LENGTH_MULTIPLIER;
00862 
00863         // adjust TCP offset
00864         // transformation from flange (kinematics tcp) to effective tcp (with offset)
00865         Matrix TCP_Offset(4, 4);
00866         TCP_Offset.row(1) << 1.0 << 0.0 << 0.0 << _tcpOffset[0];
00867         TCP_Offset.row(2) << 0.0 << cos(_tcpOffset[3]) << -sin(_tcpOffset[3]) << _tcpOffset[1];
00868         TCP_Offset.row(3) << 0.0 << sin(_tcpOffset[3]) << cos(_tcpOffset[3]) << _tcpOffset[2];
00869         TCP_Offset.row(4) << 0.0 << 0.0 << 0.0 << 1.0;
00870         TCP_Position = TCP_Position * TCP_Offset;
00871 
00872         pose.clear();
00873         
00874         //x-Position
00875         pose.push_back(TCP_Position(1,4));
00876         //y-Position
00877         pose.push_back(TCP_Position(2,4));
00878         //z-Position
00879         pose.push_back(TCP_Position(3,4));
00880 
00881         //Calculate euler angles
00882         ColumnVector eul = ieulzxz(TCP_Position);
00883 
00884         //Phi
00885         pose.push_back(eul(1));
00886         //Theta
00887         pose.push_back(eul(2));
00888         //Psi
00889         pose.push_back(eul(3));
00890 
00891         return 1;
00892 }
00893 
00895 
00896 // single inverse kinematics
00897 int KinematicsLib::invKin(std::vector<double> pose, std::vector<double> prev,
00898                 std::vector<double>& angle) {
00899         if ((int)pose.size() < 6 || (int)prev.size() < _dof)
00900                 return -1;
00901 
00902         // IK algorithm type to use
00903         const int ikalgo = 0; // based on Jacobian (faster)
00904         //const int ikalgo = 1; // based on derivative of T (converge more often)
00905 
00906         //ReturnMatrix eulzxz(const ColumnVector & a);
00907         ColumnVector v(3);
00908         v(1) = pose.at(3);
00909         v(2) = pose.at(4);
00910         v(3) = pose.at(5);
00911         Matrix Pos = eulzxz(v);
00912         Pos(1,4) = pose.at(0) * LENGTH_MULTIPLIER;
00913         Pos(2,4) = pose.at(1) * LENGTH_MULTIPLIER;
00914         Pos(3,4) = pose.at(2) * LENGTH_MULTIPLIER;
00915 
00916         //Set previous angles
00917         ColumnVector qs = ColumnVector(_dof);
00918         for (int j = 0; j < _dof; ++j) {
00919                 qs(j+1) = prev.at(j);
00920         }
00921         _robot.set_q(qs);
00922 
00923         bool converge = false;
00924         ColumnVector q0 = _robot.inv_kin(Pos, ikalgo, _dof, converge);
00925 
00926         angle.clear();
00927         for (int j = 0; j < _dom; ++j) {
00928                 angle.push_back(q0(j + 1));
00929         }
00930         if (_immobile == 1)
00931                 angle.push_back(_thetaimmobile);
00932 
00933         int ok = 1;
00934         if (!converge)
00935                 ok = -1;
00936         return ok;
00937 }
00938 
00939 // inverse kinematics using bisection if no solution found
00940 int KinematicsLib::invKin_bisec(std::vector<double> pose, std::vector<double> prev,
00941                 std::vector<double>& conf, int maxBisection) {
00942         if ((int)pose.size() < 6 || (int)prev.size() < _dof || maxBisection < 0)
00943                 return -1;
00944 
00945         int ok = invKin(pose, prev, conf);
00946 
00947         // Orig 3D                                                      pose
00948         // Orig JS      prev                                    angle
00949         // Bisec 3D     prev1pose       prev2pose       pose
00950         // Bisec JS     prev            prev2conf       conf
00951         if (ok < 0 && maxBisection > 0) {
00952                 // prev1pose
00953                 std::vector<double> prev1pose;
00954                 directKinematics(prev, prev1pose);
00955 
00956                 // prev2pose
00957                 std::vector<double> prev2pose;
00958                 for (int i = 0; i < 6; ++i)
00959                         prev2pose.push_back(prev1pose.at(i) + pose.at(i) / 2.0);
00960 
00961                 // prev2conf (IK on first part)
00962                 std::vector<double> prev2conf;
00963                 ok = inverseKinematics(prev2pose, prev, prev2conf, maxBisection-1);
00964 
00965                 if (ok == 1) {
00966                         // conf (IK on second part, only if first part successful)
00967                         ok = inverseKinematics(pose, prev2conf, conf, maxBisection-1);
00968                 }
00969         }
00970 
00971         return ok;
00972 }
00973 
00974 // analytical guess
00975 int KinematicsLib::anaGuess(std::vector<double> pose, std::vector<double> prev,
00976                 std::vector<double>& angle) {
00977         if (_type < 0 || (int)pose.size() < 6 || (int)prev.size() < _dof)
00978                 return -1;
00979 
00980         std::vector<double> positions;
00981         for (int i = 0; i < 6; ++i) {
00982                 if (i < 3)
00983                         positions.push_back(pose.at(i) * 1000);
00984                 else
00985                         positions.push_back(pose.at(i));
00986         }
00987 
00988         std::vector<double> previous_angles;
00989         std::vector<double> prevK4D;
00990         mDH2K4DAng(prev, prevK4D);
00991         for (int i = 0; i < _dom; ++i) {
00992                 previous_angles.push_back(prevK4D.at(i));
00993         }
00994         if (_dom == 5) {
00995                 // give 6 previous angles to the analytical kinematics
00996                 previous_angles.push_back(0.0);
00997         }
00998 
00999         std::vector<double> anglesK4D;
01000 
01001         int error;
01002         try{
01003                 error = ((int) _anaGuess->inverseKinematics(anglesK4D, positions,
01004                         previous_angles)) - 1;
01005                 K4D2mDHAng(anglesK4D, angle);
01006                 if (_immobile)
01007                         angle.push_back(_thetaimmobile);
01008         } catch (AnaGuess::NoSolutionException nse) {
01009                 error = -2;
01010         } catch (AnaGuess::Exception e) {
01011                 error = e.errorNumber() - 2;
01012         }
01013 
01014         if (error != 0) {
01015                 angle.clear();
01016                 for (int i = 0; i < _dof; ++i)
01017                         angle.push_back(prev.at(i));
01018         }
01019 
01020         int ok = (error == 0) ? 1 : -1;
01021         return ok;
01022 }
01023 
01024 bool KinematicsLib::checkConfig(std::vector<double> config, std::vector<double> pose,
01025                 double tol) {
01026         std::vector<double> configpose;
01027         directKinematics(config, configpose);
01028         double posdiff = 0.0;
01029         for (int i = 0; i < 6; ++i) {
01030                 posdiff += abs(pose.at(i) - configpose.at(i));
01031         }
01032         bool ok;
01033         if (posdiff > tol)
01034                 ok = false;
01035         else
01036                 ok = true;
01037         return ok;
01038 }
01039 
01041 
01043 int KinematicsLib::inverseKinematics(std::vector<double> pose,
01044                 std::vector<double> prev, std::vector<double>& angles, int maxBisection) {
01045         if (!_initialized || (int)pose.size() < 6 || (int)prev.size() < _dom ||
01046                         maxBisection < 0)
01047                 return -1;
01048 
01049         // tolerance for configuration check (sum of 3 * meter + 3 * radian)
01050         double tol = 0.0001;
01051         
01052         // adjust TCP offset
01053         // pose to matrix
01054         ColumnVector v(3);
01055         v(1) = pose.at(3);
01056         v(2) = pose.at(4);
01057         v(3) = pose.at(5);
01058         Matrix Pos = eulzxz(v);
01059         Pos(1,4) = pose.at(0);
01060         Pos(2,4) = pose.at(1);
01061         Pos(3,4) = pose.at(2);
01062         // transformation from effective tcp (with offset) to flange (kinematics tcp)
01063         Matrix TCP_Offset(4, 4);
01064         TCP_Offset.row(1) << 1.0 << 0.0 << 0.0 << -_tcpOffset[0];
01065         TCP_Offset.row(2) << 0.0 << cos(_tcpOffset[3]) << sin(_tcpOffset[3]) << (-_tcpOffset[1] * cos(_tcpOffset[3]) - _tcpOffset[2] * sin(_tcpOffset[3]));
01066         TCP_Offset.row(3) << 0.0 << -sin(_tcpOffset[3]) << cos(_tcpOffset[3]) << (_tcpOffset[1] * sin(_tcpOffset[3]) - _tcpOffset[2] * cos(_tcpOffset[3]));
01067         TCP_Offset.row(4) << 0.0 << 0.0 << 0.0 << 1.0;
01068         Pos = Pos * TCP_Offset;
01069         // matrix to pose
01070         std::vector<double> flangepose;
01071         flangepose.push_back(Pos(1,4));
01072         flangepose.push_back(Pos(2,4));
01073         flangepose.push_back(Pos(3,4));
01074         ColumnVector eul = ieulzxz(Pos);
01075         if (_type != K_6M180) {
01076                 flangepose.push_back(eul(1));
01077         } else {
01078                 // calculate phi from X and Y with K_6M180
01079                 // own impl. of atan w/ 2 args for K4D compatibility --JHA
01080                 double phi_calc = atan1(Pos(1,4),Pos(2,4))+mPi/2.0;
01081                 if (Pos(1,4)*eul(1) < 0) {
01082                         // X and given phi different sign -> tool point inwards
01083                         phi_calc += mPi;
01084                 }
01085                 // bring phi in range [-mPi,mPi)
01086                 phi_calc = fmod(phi_calc+mPi,2.0*mPi)-mPi;
01087                 // bring phi in range (-mPi,mPi]
01088                 if(phi_calc==-mPi) phi_calc+=2.0*mPi;
01089                 // store calculated phi
01090                 flangepose.push_back(phi_calc);
01091         }
01092         flangepose.push_back(eul(2));
01093         flangepose.push_back(eul(3));
01094 
01095         // Copy and complete prev
01096         std::vector<double> prev1conf;
01097         for(int i = 0; i < _dof; ++i){
01098                 if (i == _dom) {
01099                         // immobile = 1: _dom == _dof - 1
01100                         prev1conf.push_back(_thetaimmobile);
01101                 } else {
01102                         prev1conf.push_back(prev.at(i));
01103                 }
01104         }
01105 
01106         std::vector<double> conf;
01107 
01108         // calculate inverse kinematics (roboop)
01109         int ok = invKin_bisec(flangepose, prev1conf, conf, maxBisection);
01110 
01111         // on fail try turning phi about pi (switch inward / outward) with K_6M180
01112         if (ok<0 && _type == K_6M180) {
01113                 if (flangepose[3] > 0)
01114                         flangepose[3] -= mPi;
01115                 else
01116                         flangepose[3] += mPi;
01117                 ok = invKin_bisec(flangepose, prev1conf, conf, maxBisection);
01118         }
01119 
01120         // if no solution found and type is 6M robot, get analytical guess
01121         if (ok < 0 && _type >= 0) {
01122                 std::vector<double> guess;
01123                 ok = anaGuess(flangepose, prev1conf, guess);
01124 
01125                 // if analytical guess found, calculate inverse kinematics using guess
01126                 if (ok == 1) {
01127                         ok = invKin_bisec(flangepose, guess, conf, maxBisection);
01128                         // check solution
01129                         if (checkConfig(conf, pose, tol) == false) // use effective pose!
01130                                 ok = -1; // wrong solution
01131                 }
01132 
01133                 // if no guess or solution found, get analytical guess of near pose
01134                 if (ok < 0) {
01135                         std::vector<double> nearpose;
01136                         nearpose.push_back(flangepose.at(0) - sign(flangepose.at(0)) * 0.05);
01137                         nearpose.push_back(flangepose.at(1) - sign(flangepose.at(1)) * 0.05);
01138                         nearpose.push_back(flangepose.at(2) - sign(flangepose.at(2)) * 0.05);
01139                         nearpose.push_back(flangepose.at(3) - sign(flangepose.at(3)) * 0.2);
01140                         nearpose.push_back(flangepose.at(4) - sign(flangepose.at(4) - mPi / 2.0) * 0.2);
01141                         nearpose.push_back(flangepose.at(5) - sign(flangepose.at(5)) * 0.2);
01142                         ok = anaGuess(nearpose, prev1conf, guess);
01143                         // if analytical guess found, calculate inverse kinematics using guess
01144                         if (ok == 1) {
01145                                 ok = invKin_bisec(flangepose, guess, conf, maxBisection);
01146                                 // check solution
01147                                 if (checkConfig(conf, pose, tol) == false)
01148                                         ok = -1; // wrong solution
01149                         }
01150                 }
01151         }
01152 
01153         // set angle if solution found
01154         angles.clear();
01155         if (ok == 1) {
01156                 for (int i = 0; i < _dom; ++i)
01157                         angles.push_back(conf.at(i));
01158         } else {
01159                 // set prev if no solution found
01160                 for (int i = 0; i < _dom; ++i)
01161                         angles.push_back(prev.at(i));
01162         }
01163 
01164         return ok;
01165 }
01166 
01167 
01168 
01169 
01170 


kni
Author(s): Neuronics AG (see AUTHORS.txt); ROS wrapper by Martin Günther
autogenerated on Mon Oct 6 2014 10:45:32