Main Page   Namespace List   Class Hierarchy   Compound List   File List   Namespace Members   Compound Members  

SIG_Joint.cpp

00001 #include "SIGEL_Robot/SIG_Joint.h"
00002 
00003 #include "SIGEL_Robot/SIG_TranslationalJoint.h"
00004 #include "SIGEL_Robot/SIG_RotationalJoint.h"
00005 #include "SIGEL_Tools/SIG_TypeConverter.h"
00006 #include "SIGEL_Tools/SIG_IO.h"
00007 #include "SIGEL_Robot/IFunctions.h"
00008 
00009 #include <cmath>
00010 
00011 using namespace SIGEL_Tools;
00012 
00013 namespace SIGEL_Robot {
00014 
00015         SIG_Joint::SIG_Joint (SIG_Robot *par, QString n, int nr)
00016                 : name (n),
00017                   parent (par),
00018                   number (nr),
00019                   leftLink (0),
00020                   rightLink (0),
00021                   mdh_a (0), mdh_alpha (0), mdh_d (0), mdh_theta (0),
00022                   mdh_screw_d(0), mdh_screw_theta(0), mdh_predecessor_is_left (-1),
00023                   mechsMinPos(0), mechsMaxPos(0)
00024         { }
00025 
00026         SIG_Joint::SIG_Joint (SIG_Robot *par, QTextStream & tx)
00027                 : parent (par),
00028                   number (-1),
00029                   leftLink (0),
00030                   rightLink (0)
00031         {
00032                 QString n1, n2;
00033 
00034                 tx >> n1  // Read away the "Joint" keyword.
00035                    >> name
00036                    >> number;
00037                 tx >> n1 >> n2; // read da names o' da links.
00038                 setLeftLink (parent->lookupLink (n1));
00039                 setRightLink (parent->lookupLink (n2));
00040                 tx >> mdh_a >> mdh_alpha >> mdh_d >> mdh_theta
00041                    >> mdh_screw_d >> mdh_screw_theta
00042                    >> mechsMinPos >> mechsMaxPos
00043                    >> mdh_predecessor_is_left;
00044         }
00045 
00046         SIG_Joint::~SIG_Joint (void)
00047         { }
00048 
00049         QString SIG_Joint::getName (void) const
00050         {
00051                 return name;
00052         }
00053 
00054         int SIG_Joint::getNumber (void) const
00055         {
00056                 return number;
00057         }
00058 
00059         //
00060         // SIG_Joint::JointType SIG_Joint::getJointType () const
00061         // { /* Bitte noch implementieren! */ }
00062         // Da hat wohl jemand ausserhalb nicht kapiert, dass diese
00063         // Methode ABSTRAKT ist. *kicher*
00064         //
00065 
00066         void SIG_Joint::setLeftLink (SIG_Link *theLink)
00067         {
00068                 leftLink = theLink;
00069                 leftLink->addJoint (this);
00070         }
00071 
00072         void SIG_Joint::setRightLink (SIG_Link *theLink)
00073         {
00074                 rightLink = theLink;
00075                 rightLink->addJoint (this);
00076         }
00077 
00078         SIG_Link const *SIG_Joint::getLeftLink (void) const
00079         {
00080                 return leftLink;
00081         }
00082 
00083         SIG_Link const *SIG_Joint::getRightLink (void) const
00084         {
00085                 return rightLink;
00086         }
00087 
00088         SIG_Link *SIG_Joint::otherSide (SIG_Link *myself) const
00089         {
00090                 return myself == leftLink ? rightLink : leftLink;
00091         }
00092 
00093         // auch getGeomRelation ist ABSTRAKT!
00094         // Gott sei Dank hat noch keiner nach gefragt.
00095 
00096         void SIG_Joint::getMDH (SIG_Link * & predecessor,
00097                                 double & a, double & alpha,
00098                                 double & d, double & theta,
00099                                 double & screwD, double & screwTheta)
00100         {
00101                 throw SIG_Exception (__FILE__, __LINE__,
00102                         QString ("Joint ") + name +
00103                         " has no MDH parameters.");
00104         }
00105 
00106         void SIG_Joint::writeToFileTransfer (QTextStream & tx)
00107         {
00108                 tx << "Joint "
00109                    << getName () << ' '
00110                    << getNumber () << ' '
00111                    << getLeftLink ()->getName () << ' '
00112                    << getRightLink ()->getName () << ' '
00113                    << mdh_a << ' ' << mdh_alpha << ' '
00114                    << mdh_d << ' ' << mdh_theta << ' '
00115                    << mdh_screw_d << ' ' << mdh_screw_theta << ' '
00116                    << mechsMinPos << ' ' << mechsMaxPos << ' '
00117                    << mdh_predecessor_is_left << '\n';
00118         }
00119 
00120         void SIG_Joint::tfap (DL_vector mov, DL_matrix rot, DL_vector *p)
00121         {
00122                 p->plusis (&mov);
00123                 DL_vector v (p);
00124                 rot.times (&v, p);
00125         }
00126 
00127         double SIG_Joint::getMechsMinPos() const
00128         {
00129           return mechsMinPos;
00130         };
00131 
00132         double SIG_Joint::getMechsMaxPos() const
00133         {
00134           return mechsMaxPos;
00135         };
00136 
00137         void SIG_Joint::transformToDynaMechs( SIG_Link *predecessor,
00138                                               double screwD,
00139                                               double screwTheta )
00140         {
00141           mdh_screw_d = screwD;
00142           mdh_screw_theta = screwTheta;
00143 
00144           SIG_Link *successor;
00145 
00146           bool successorIsLeftLink;
00147 
00148           if (predecessor == leftLink)
00149             {
00150               successor = rightLink;
00151               mdh_predecessor_is_left = 1;
00152               successorIsLeftLink = false;
00153             }
00154           else
00155             {
00156               successor = leftLink;
00157               mdh_predecessor_is_left = 0;
00158               successorIsLeftLink = true;
00159             };
00160 
00161           DL_vector base;
00162           DL_vector dir;
00163           DL_vector hand;
00164 
00165           switch (getJointType())
00166                 {
00167                 case tTranslationalJoint:
00168                   {
00169                     SIG_TranslationalJoint *translationalJoint = static_cast< SIG_TranslationalJoint* >(this);
00170 
00171                     base = ( successorIsLeftLink ) ? translationalJoint->getLeftBase() : translationalJoint->getRightBase();
00172                     dir = ( successorIsLeftLink ) ? translationalJoint->getLeftDir() : translationalJoint->getRightDir();
00173                     hand = ( successorIsLeftLink ) ? translationalJoint->getLeftFix() : translationalJoint->getRightFix();
00174                   };
00175                   break;
00176                 case tRotationalJoint:
00177                   {
00178                     SIG_RotationalJoint *rotationalJoint = static_cast< SIG_RotationalJoint* >(this);
00179 
00180                     base = ( successorIsLeftLink ) ? rotationalJoint->getLeftBase() : rotationalJoint->getRightBase();
00181                     dir = ( successorIsLeftLink ) ? rotationalJoint->getLeftDir() : rotationalJoint->getRightDir();
00182                     hand = ( successorIsLeftLink ) ? rotationalJoint->getLeftHand() : rotationalJoint->getRightHand();
00183                   };
00184                   break;
00185                 };
00186 
00187           successor->transformToDynaMechs( this,
00188                                            base,
00189                                            dir,
00190                                            hand );
00191         }
00192 
00193         bool SIG_Joint::continuable( SIG_Link *predecessor ) const
00194         {
00195           SIG_Link *successor = ( predecessor == leftLink ) ? rightLink : leftLink;
00196 
00197           return ( !((mdh_predecessor_is_left==0) || (mdh_predecessor_is_left==1)) && !successor->isMDHVisited());
00198         };
00199 
00200        void SIG_Joint::calculateMDH( SIG_Link *caller )
00201        {
00202          SIG_Link *predecessor;
00203 
00204          switch (mdh_predecessor_is_left)
00205            {
00206            case 0:
00207              predecessor = rightLink;
00208              break;
00209            case 1:
00210              predecessor = leftLink;
00211              break;
00212            default:
00213              predecessor = 0;
00214            };
00215 
00216          if (predecessor!=caller)
00217            return;
00218 
00219          SIG_Link *successor = ( leftLink == predecessor ) ? rightLink : leftLink;
00220 
00221          DL_vector predecessorsInitialPosition;
00222          DL_matrix predecessorsInitialRotation;
00223 
00224          predecessor->getInitialLocation( predecessorsInitialPosition,
00225                                           predecessorsInitialRotation );
00226 
00227          DL_vector predOrigin = predecessorsInitialPosition;
00228 
00229          DL_vector successorsInitialPosition;
00230          DL_matrix successorsInitialRotation;
00231 
00232          successor->getInitialLocation( successorsInitialPosition,
00233                                         successorsInitialRotation );
00234 
00235          DL_vector succOrigin = successorsInitialPosition;
00236 
00237 #ifdef SIG_DEBUG
00238          SIGEL_Tools::SIG_IO::cerr << "Initial rotation of link "
00239                                    << successor->getName()
00240                                    << ":\n";
00241          for (int i=0; i<3; i++)
00242            {
00243              for (int j=0; j<3; j++)
00244                SIGEL_Tools::SIG_IO::cerr << successorsInitialRotation.get( i, j ) << " ";
00245              SIGEL_Tools::SIG_IO::cerr << "\n";
00246            };
00247 #endif
00248 
00249          DL_vector predXAxis = predecessorsInitialRotation.c0;
00250          DL_vector predZAxis = predecessorsInitialRotation.c2;
00251 
00252          DL_vector succXAxis = successorsInitialRotation.c0;
00253          DL_vector succYAxis = successorsInitialRotation.c1;
00254          DL_vector succZAxis = successorsInitialRotation.c2;
00255 
00256          DL_vector screwTranslationVector = predZAxis;
00257          screwTranslationVector.timesis( mdh_screw_d );
00258 
00259          predOrigin.plusis( &screwTranslationVector );
00260 
00261          DL_matrix screwRotationMatrix = rotationMatrix( predZAxis,
00262                                                          mdh_screw_theta );
00263 
00264          DL_vector helpPredXAxis = predXAxis;
00265 
00266          screwRotationMatrix.times( &helpPredXAxis, &predXAxis );
00267 
00268          // The predecessor's X-Axis and the successors Z-Axis must not
00269          // be parallel!
00270 
00271          DL_vector predXSuccZNormal;
00272          predXAxis.crossprod( &succZAxis, &predXSuccZNormal );
00273          predXSuccZNormal.normalize();
00274 
00275          DL_vector cut;
00276 
00277          calculateCut( succOrigin,
00278                        succZAxis,
00279                        predXSuccZNormal,
00280                        predOrigin,
00281                        predXAxis,
00282                        cut );
00283 
00284          DL_vector h = cut;
00285          h.minusis( &predOrigin );
00286 
00287 #ifdef SIG_DEBUG
00288          SIGEL_Tools::SIG_IO::cerr << "succOrigin: ";
00289          for (int i=0; i<3; i++)
00290            SIGEL_Tools::SIG_IO::cerr << " " << succOrigin.get( i );
00291          SIGEL_Tools::SIG_IO::cerr << "\n";
00292 
00293          SIGEL_Tools::SIG_IO::cerr << "succZAxis: ";
00294          for (int i=0; i<3; i++)
00295            SIGEL_Tools::SIG_IO::cerr << " " << succZAxis.get( i );
00296          SIGEL_Tools::SIG_IO::cerr << "\n";
00297 
00298          SIGEL_Tools::SIG_IO::cerr << "predXSuccZNormal: ";
00299          for (int i=0; i<3; i++)
00300            SIGEL_Tools::SIG_IO::cerr << " " << predXSuccZNormal.get( i );
00301          SIGEL_Tools::SIG_IO::cerr << "\n";
00302 
00303          SIGEL_Tools::SIG_IO::cerr << "predOrigin: ";
00304          for (int i=0; i<3; i++)
00305            SIGEL_Tools::SIG_IO::cerr << " " << predOrigin.get( i );
00306          SIGEL_Tools::SIG_IO::cerr << "\n";
00307 
00308          SIGEL_Tools::SIG_IO::cerr << "predXAxis: ";
00309          for (int i=0; i<3; i++)
00310            SIGEL_Tools::SIG_IO::cerr << " " << predXAxis.get( i );
00311          SIGEL_Tools::SIG_IO::cerr << "\n";
00312 
00313          SIGEL_Tools::SIG_IO::cerr << "Cut: ";
00314          for (int i=0; i<3; i++)
00315            SIGEL_Tools::SIG_IO::cerr << " " << cut.get( i );
00316          SIGEL_Tools::SIG_IO::cerr << "\n";
00317 #endif
00318 
00319          mdh_a = h.norm();
00320 
00321          double const pi = 4 * std::atan( 1 );
00322 
00323 #ifdef SIG_DEBUG
00324          SIGEL_Tools::SIG_IO::cerr << "predZAxis:";
00325          for (int i=0; i<3; i++)
00326            SIGEL_Tools::SIG_IO::cerr << " " << predZAxis.get( i );
00327          SIGEL_Tools::SIG_IO::cerr << "\n";
00328 
00329          SIGEL_Tools::SIG_IO::cerr << "succZAxis:";
00330          for (int i=0; i<3; i++)
00331            SIGEL_Tools::SIG_IO::cerr << " " << succZAxis.get( i );
00332          SIGEL_Tools::SIG_IO::cerr << "\n";
00333 #endif
00334 
00335          mdh_alpha = tolerantACos( predZAxis.inprod( &succZAxis ) );
00336 
00337          double const maximalParallelityMeasure = 0.00001;
00338 
00339          DL_vector rotationDir = h;
00340          rotationDir.normalize();
00341 
00342          DL_vector zzNormalVector;
00343          predZAxis.crossprod( &succZAxis, &zzNormalVector );
00344 
00345          if (zzNormalVector.norm() > maximalParallelityMeasure)
00346            {
00347              zzNormalVector.normalize();
00348 
00349              if (rotationDir.inprod( &zzNormalVector ) < 0)
00350                  mdh_alpha = 2 * pi - mdh_alpha;
00351            };
00352 
00353          DL_vector distanceVector = succOrigin;
00354 
00355          distanceVector.minusis( &cut );
00356 
00357          mdh_d = distanceVector.norm();
00358 
00359          double const minimalDistanceMeasure = 0.00001;
00360 
00361          if (mdh_d > minimalDistanceMeasure)
00362            {
00363              distanceVector.normalize();
00364 
00365              if (distanceVector.inprod( &succZAxis ) < 0)
00366                mdh_d *= -1;
00367            };
00368 
00369 #ifdef SIG_DEBUG
00370          SIGEL_Tools::SIG_IO::cerr << "Predecessor's X-Axis:";
00371          for (int i=0; i<3; i++)
00372            SIGEL_Tools::SIG_IO::cerr << " " << predXAxis.get( i );
00373          SIGEL_Tools::SIG_IO::cerr << "\n";
00374 
00375          SIGEL_Tools::SIG_IO::cerr << "Successor's X-Axis:";
00376          for (int i=0; i<3; i++)
00377            SIGEL_Tools::SIG_IO::cerr << " " << succXAxis.get( i );
00378          SIGEL_Tools::SIG_IO::cerr << "\n";
00379 #endif
00380 
00381          mdh_theta = tolerantACos( predXAxis.inprod( &succXAxis ) );
00382 
00383          rotationDir = succZAxis;
00384 
00385          DL_vector xxNormalVector;
00386          predXAxis.crossprod( &succXAxis, &xxNormalVector );
00387 
00388          if (xxNormalVector.norm() > maximalParallelityMeasure)
00389            {
00390              xxNormalVector.normalize();
00391 
00392              if (rotationDir.inprod( &xxNormalVector) < 0)
00393                mdh_theta = 2 * pi - mdh_theta;
00394            };
00395 
00396          switch (getJointType())
00397            {
00398            case tTranslationalJoint:
00399              {
00400                SIG_TranslationalJoint *translationalJoint = static_cast< SIG_TranslationalJoint* >(this);
00401 
00402                if (translationalJoint->getMax()==translationalJoint->getMin())
00403                  {
00404                    mechsMinPos = mechsMaxPos = 0;
00405                  }
00406                else
00407                  {
00408                    DL_vector localPredBase;
00409                    DL_vector localSuccBase;
00410 
00411                    double sigelMin;
00412                    double sigelMax;
00413 
00414                    if (mdh_predecessor_is_left==1)
00415                      {
00416                        localPredBase = translationalJoint->getLeftBase();
00417                        localSuccBase = translationalJoint->getRightBase();
00418 
00419                        sigelMin = translationalJoint->getMin();
00420                        sigelMax = translationalJoint->getMax();
00421                      }
00422                    else
00423                      {
00424                        localPredBase = translationalJoint->getRightBase();
00425                        localSuccBase = translationalJoint->getLeftBase();
00426 
00427                        sigelMin = - translationalJoint->getMax();
00428                        sigelMax = - translationalJoint->getMin();
00429                      };
00430 
00431                    DL_vector predBase;
00432                    DL_vector succBase;
00433 
00434                    predecessorsInitialRotation.times( &localPredBase, &predBase );
00435                    predBase.plusis( &predecessorsInitialPosition );
00436 
00437                    successorsInitialRotation.times( &localSuccBase, &succBase );
00438                    succBase.plusis( &successorsInitialPosition );
00439 
00440                    DL_vector predBaseOffset = cut;
00441                    predBaseOffset.minusis( &predBase );
00442 
00443                    DL_vector succBaseOffset = succOrigin;
00444                    succBaseOffset.minusis( &succBase );
00445 
00446                    double b = predBaseOffset.norm();
00447 
00448                    if (b > minimalDistanceMeasure)
00449                      {
00450                        predBaseOffset.normalize();
00451 
00452                        if (succZAxis.inprod( &predBaseOffset ) < 0)
00453                          b *= -1;
00454                      };
00455 
00456                    double c = succBaseOffset.norm();
00457 
00458                    if (c > minimalDistanceMeasure)
00459                      {
00460                        succBaseOffset.normalize();
00461 
00462                        if (succZAxis.inprod( &succBaseOffset ) < 0)
00463                          c *= -1;
00464                      };
00465 
00466                    mechsMinPos = sigelMin - b + c;
00467                    mechsMaxPos = sigelMax - b + c;
00468                  };
00469              };
00470              break;
00471            case tRotationalJoint:
00472              {
00473                SIG_RotationalJoint *rotationalJoint = static_cast< SIG_RotationalJoint* >(this);
00474 
00475 
00476                if (rotationalJoint->getMax()==rotationalJoint->getMin())
00477                  {
00478                    mechsMinPos = mechsMaxPos = 0;
00479                  }
00480                else
00481                  {
00482 
00483                    DL_vector localPredHand;
00484                    DL_vector localSuccHand;
00485 
00486                    double sigelMax;
00487                    double sigelMin;
00488 
00489                    if (mdh_predecessor_is_left==1)
00490                      {
00491                        localPredHand = rotationalJoint->getLeftHand();
00492                        localSuccHand = rotationalJoint->getRightHand();
00493 
00494                        sigelMin = rotationalJoint->getMin();
00495                        sigelMax = rotationalJoint->getMax();
00496                      }
00497                    else
00498                      {
00499                        localPredHand = rotationalJoint->getRightHand();
00500                        localSuccHand = rotationalJoint->getLeftHand();
00501 
00502                        sigelMin = - rotationalJoint->getMax();
00503                        sigelMax = - rotationalJoint->getMin();
00504                      };
00505 
00506                    DL_vector predHand;
00507                    DL_vector succHand;
00508 
00509                    predecessorsInitialRotation.times( &localPredHand, &predHand );
00510                    predHand.plusis( &predecessorsInitialPosition );
00511 
00512                    successorsInitialRotation.times( &localSuccHand, &succHand );
00513                    succHand.plusis( &successorsInitialPosition );
00514 
00515                    DL_vector predHandProjected;
00516 
00517                    calculateCut( succOrigin,
00518                                  succXAxis,
00519                                  succYAxis,
00520                                  predHand,
00521                                  succZAxis,
00522                                  predHandProjected );
00523 
00524                    DL_vector predHandProjectedDir = predHandProjected;
00525                    predHandProjectedDir.minusis( &succOrigin );
00526                    predHandProjectedDir.normalize();
00527 
00528                    DL_vector succHandProjected;
00529 
00530                    calculateCut( succOrigin,
00531                                  succXAxis,
00532                                  succYAxis,
00533                                  succHand,
00534                                  succZAxis,
00535                                  succHandProjected );
00536 
00537                    DL_vector succHandProjectedDir = succHandProjected;
00538                    succHandProjectedDir.minusis( &succOrigin );
00539                    succHandProjectedDir.normalize();
00540 
00541 #ifdef SIG_DEBUG
00542                    SIGEL_Tools::SIG_IO::cerr << "predHand:";
00543                    for (int i=0; i<3; i++)
00544                      SIGEL_Tools::SIG_IO::cerr << " " << predHand.get( i );
00545                    SIGEL_Tools::SIG_IO::cerr << "\n";
00546 
00547                    SIGEL_Tools::SIG_IO::cerr << "predHandProjected:";
00548                    for (int i=0; i<3; i++)
00549                      SIGEL_Tools::SIG_IO::cerr << " " << predHandProjected.get( i );
00550                    SIGEL_Tools::SIG_IO::cerr << "\n";
00551 
00552                    SIGEL_Tools::SIG_IO::cerr << "predHandProjectedDir:";
00553                    for (int i=0; i<3; i++)
00554                      SIGEL_Tools::SIG_IO::cerr << " " << predHandProjectedDir.get( i );
00555                    SIGEL_Tools::SIG_IO::cerr << "\n";
00556 
00557                    SIGEL_Tools::SIG_IO::cerr << "Predecessors X-Axis:";
00558                    for (int i=0; i<3; i++)
00559                      SIGEL_Tools::SIG_IO::cerr << " " << predXAxis.get( i );
00560                    SIGEL_Tools::SIG_IO::cerr << "\n";
00561 
00562                    SIGEL_Tools::SIG_IO::cerr << "succHand:";
00563                    for (int i=0; i<3; i++)
00564                      SIGEL_Tools::SIG_IO::cerr << " " << succHand.get( i );
00565                    SIGEL_Tools::SIG_IO::cerr << "\n";
00566 
00567                    SIGEL_Tools::SIG_IO::cerr << "succHandProjected:";
00568                    for (int i=0; i<3; i++)
00569                      SIGEL_Tools::SIG_IO::cerr << " " << succHandProjected.get( i );
00570                    SIGEL_Tools::SIG_IO::cerr << "\n";
00571 
00572                    SIGEL_Tools::SIG_IO::cerr << "succHandProjectedDir:";
00573                    for (int i=0; i<3; i++)
00574                      SIGEL_Tools::SIG_IO::cerr << " " << succHandProjectedDir.get( i );
00575                    SIGEL_Tools::SIG_IO::cerr << "\n";
00576 
00577                    SIGEL_Tools::SIG_IO::cerr << "Successor's X-Axis:";
00578                    for (int i=0; i<3; i++)
00579                      SIGEL_Tools::SIG_IO::cerr << " " << succXAxis.get( i );
00580                    SIGEL_Tools::SIG_IO::cerr << "\n";
00581 #endif
00582 
00583                    double beta = tolerantACos( predHandProjectedDir.inprod( &predXAxis ) );
00584 
00585                    rotationDir = succZAxis;
00586 
00587                    DL_vector predHandProjectedDirPredXAxisNormalVector;
00588                    predHandProjectedDir.crossprod( &predXAxis, &predHandProjectedDirPredXAxisNormalVector );
00589 
00590                    if (predHandProjectedDirPredXAxisNormalVector.norm() > maximalParallelityMeasure)
00591                      {
00592                        predHandProjectedDirPredXAxisNormalVector.normalize();
00593 
00594                        if (rotationDir.inprod( &predHandProjectedDirPredXAxisNormalVector ) < 0)
00595                          beta = (2 * pi) - beta;
00596                      };
00597 
00598                    double gamma = tolerantACos( succHandProjectedDir.inprod( &succXAxis ) );
00599 
00600                    DL_vector succHandProjectedDirSuccXAxisNormalVector;
00601                    succHandProjectedDir.crossprod( &succXAxis, &succHandProjectedDirSuccXAxisNormalVector );
00602 
00603                    if (succHandProjectedDirSuccXAxisNormalVector.norm() > maximalParallelityMeasure)
00604                      {
00605                        succHandProjectedDirSuccXAxisNormalVector.normalize();
00606 
00607                        if (rotationDir.inprod( &succHandProjectedDirSuccXAxisNormalVector ) < 0)
00608                          gamma = (2 * pi) - gamma;
00609                      };
00610 
00611                    if (rotationalJoint->getMax()==rotationalJoint->getMin())
00612                      {
00613                        mechsMinPos = mechsMaxPos = 0;
00614                      }
00615                    else
00616                      {
00617                        if (sigelMax < 0)
00618                          sigelMax += 360;
00619                        else if (sigelMax >= 360)
00620                          sigelMax -= 360;
00621 
00622                        if (sigelMin < 0)
00623                          sigelMin += 360;
00624                        else if (sigelMin >= 360)
00625                          sigelMin -= 360;
00626 
00627 #ifdef SIG_DEBUG
00628                        SIGEL_Tools::SIG_IO::cerr << "sigelMax: "
00629                                                  << sigelMax
00630                                                  << ", sigelMin: "
00631                                                  << sigelMin
00632                                                  << "\n";
00633 #endif
00634 
00635                        mechsMinPos = (2 * pi) - ( (sigelMax / 360) * 2 * pi );
00636                        mechsMaxPos = (2 * pi) - ( (sigelMin / 360) * 2 * pi );
00637 
00638 #ifdef SIG_DEBUG
00639                        SIGEL_Tools::SIG_IO::cerr << "mechsMinPos: "
00640                                                  << mechsMinPos
00641                                                  << ", mechsMaxPos: "
00642                                                  << mechsMaxPos
00643                                                  << "\n"
00644                                                  << "beta: "
00645                                                  << beta
00646                                                  << "\n";
00647 #endif
00648 
00649                        mechsMinPos -= beta;
00650                        mechsMaxPos -= beta;
00651 
00652 #ifdef SIG_DEBUG
00653                        SIGEL_Tools::SIG_IO::cerr << "New mechsMinPos: "
00654                                                  << mechsMinPos
00655                                                  << ", new mechsMaxPos: "
00656                                                  << mechsMaxPos
00657                                                  << "\n";
00658 #endif
00659 
00660                        mechsMinPos = normalizeRadAngle( mechsMinPos );
00661                        mechsMaxPos = normalizeRadAngle( mechsMaxPos );
00662 
00663 #ifdef SIG_DEBUG
00664                        SIGEL_Tools::SIG_IO::cerr << "New mechsMinPos: "
00665                                                  << mechsMinPos
00666                                                  << ", new mechsMaxPos: "
00667                                                  << mechsMaxPos
00668                                                  << "\n"
00669                                                  << "gamma: "
00670                                                  << gamma
00671                                                  << "\n";
00672 #endif
00673 
00674                        mechsMinPos += gamma;
00675                        mechsMaxPos += gamma;
00676 
00677 #ifdef SIG_DEBUG
00678                        SIGEL_Tools::SIG_IO::cerr << "New mechsMinPos: "
00679                                                  << mechsMinPos
00680                                                  << ", new mechsMaxPos: "
00681                                                  << mechsMaxPos
00682                                                  << "\n";
00683 #endif
00684 
00685                        mechsMinPos = normalizeRadAngle( mechsMinPos );
00686                        mechsMaxPos = normalizeRadAngle( mechsMaxPos );
00687 
00688 #ifdef SIG_DEBUG
00689                        SIGEL_Tools::SIG_IO::cerr << "New mechsMinPos: "
00690                                                  << mechsMinPos
00691                                                  << ", new mechsMaxPos: "
00692                                                  << mechsMaxPos
00693                                                  << "\n";
00694 #endif
00695 
00696                        double const minimalAngleDistance = 0.00001;
00697 
00698                        if ( std::abs( mdh_theta - mechsMinPos ) <= minimalAngleDistance )
00699                          mdh_theta = mechsMinPos;
00700                        else if ( std::abs( mdh_theta - mechsMaxPos ) <= minimalAngleDistance )
00701                          mdh_theta = mechsMaxPos;
00702 
00703                        if ( mechsMaxPos < mechsMinPos )
00704                          if ( mdh_theta <= mechsMaxPos )
00705                            mechsMinPos -= 2 * pi;
00706                          else
00707                            mechsMaxPos += 2 * pi;
00708 
00709 #ifdef SIG_DEBUG
00710                        SIGEL_Tools::SIG_IO::cerr << "New mechsMinPos: "
00711                                                  << mechsMinPos
00712                                                  << ", new mechsMaxPos: "
00713                                                  << mechsMaxPos
00714                                                  << "\n";
00715 #endif
00716                      };
00717                  };
00718              };
00719              break;
00720            };
00721 
00722          QList< SIG_Joint > joints = successor->getJoints();
00723 
00724          SIG_Joint *actJoint = joints.first();
00725 
00726          while (actJoint)
00727            {
00728              actJoint->calculateMDH( successor );
00729 
00730              actJoint = joints.next();
00731            };
00732        };
00733 
00734        void SIG_Joint::calculateCut( DL_vector a,
00735                                      DL_vector u,
00736                                      DL_vector v,
00737                                      DL_vector b,
00738                                      DL_vector w,
00739                                      DL_vector &cut )
00740        {
00741          NEWMAT::Matrix linEqSystem( 3, 3 );
00742 
00743          linEqSystem.Column( 1 ) = SIG_TypeConverter::toColumnVector( u );
00744          linEqSystem.Column( 2 ) = SIG_TypeConverter::toColumnVector( v );
00745          linEqSystem.Column( 3 ) = SIG_TypeConverter::toColumnVector( w ) * -1;
00746 
00747          NEWMAT::ColumnVector rightSide =   SIG_TypeConverter::toColumnVector( b )
00748                                           - SIG_TypeConverter::toColumnVector( a );
00749 
00750          NEWMAT::ColumnVector solution = linEqSystem.i() * rightSide;
00751 
00752          cut = w;
00753          cut.timesis( solution( 3 ) );
00754          cut.plusis( &b );
00755        };
00756 
00757        double SIG_Joint::normalizeRadAngle( double input )
00758        {
00759          static double const pi = std::atan( 1 ) * 4;
00760 
00761          if (input < 0)
00762            return input + (2 * pi);
00763          else if (input >= (2*pi))
00764            return input - (2 * pi);
00765 
00766          return input;
00767        };
00768 }

Generated at Mon Sep 3 01:32:26 2001 for PG 368 - SIGEL by doxygen1.2.3 written by Dimitri van Heesch, © 1997-2000