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

SIG_Link.cpp

00001 #include "SIGEL_Robot/SIG_Link.h"
00002 
00003 #include "SIGEL_Robot/SIG_RobotExceptions.h"
00004 #include "SIGEL_Robot/SIG_TranslationalJoint.h"
00005 #include "SIGEL_Robot/SIG_RotationalJoint.h"
00006 #include "SIGEL_Robot/SIG_WrongKinematicsException.h"
00007 #include "SIGEL_Robot/IFunctions.h"
00008 #include "SIGEL_Tools/SIG_TypeConverter.h"
00009 #include "SIGEL_Tools/SIG_IO.h"
00010 
00011 #include <cmath>
00012 #include <cstdio>
00013 
00014 #include "SIGEL_Simulation/SIG_DynaSystem.h"
00015 
00016 using namespace SIGEL_Tools;
00017 
00018 namespace SIGEL_Robot {
00019         SIG_Link::SIG_Link(SIG_Robot *par, QString n, int nr)
00020                 : parent (par),
00021                   name (n),
00022                   number (nr),
00023                   body (0),
00024                   geometry (0),
00025                   mirtich (0),
00026                   material (0),
00027                   adjacentJoints (),
00028                   noCollide (),
00029                   initialLocation (0.0, 0.0, 0.0),
00030                   initiated (false), mdh_visited (false)
00031         {
00032                 initialOrientation.makeone ();
00033         }
00034 
00035         SIG_Link::SIG_Link (SIG_Robot *par, QTextStream & tx)
00036                 : parent (par),
00037                   geometry (0),
00038                   mirtich (0)
00039         {
00040                 QString tmpstr;
00041                 int anum;
00042                 
00043                 // "Link" wurde bereits von SIG_Robot gelesen.
00044                 tx >> name >> number;
00045 
00046                 tx >> anum;
00047                 for (int i = 0; i < anum; i++) {
00048                         tx >> tmpstr;
00049                         DL_vector *dl = new DL_vector
00050                                 (SIG_Robot::streamToVector (tx));
00051                         points.insert (tmpstr, dl);
00052                 }
00053 
00054                 // To understand this, please read my paper. Holger.
00055                 tx >> anum;
00056                 for (int i = 0; i < anum; i++) {
00057                         tx >> tmpstr;
00058                         SIG_Link *l = parent->lookupLink (tmpstr);
00059                         if (l)
00060                                 addNoCollide (l);
00061                 }
00062                 
00063                 initialLocation = SIG_Robot::streamToVector (tx);
00064                 initialOrientation = SIG_Robot::streamToMatrix (tx);
00065                 tx >> tmpstr;
00066                 initiated = (tmpstr == "y");
00067                 tx >> tmpstr;
00068                 mdh_visited = (tmpstr == "y");
00069 
00070                 tx >> tmpstr;
00071                 body = parent->lookupBody (tmpstr);
00072                 tx >> tmpstr;
00073                 material = parent->lookupMaterial (tmpstr);
00074 
00075                 tx >> tmpstr;
00076                 if (tmpstr == "y") {
00077                         geometry = new SIG_Geometry (tx);
00078                         mirtich=new SIG_Mirtich (geometry, name + "(" + body->getName () + ")");
00079                 }
00080         }
00081 
00082         SIG_Link::~SIG_Link()
00083         {
00084                 points.setAutoDelete (TRUE);
00085                 adjacentJoints.setAutoDelete (FALSE);
00086                 noCollide.setAutoDelete (FALSE);
00087 
00088                 if (geometry) {
00089                         delete mirtich;
00090                         delete geometry;
00091                 }
00092         }
00093 
00094         QString SIG_Link::getName (void) const
00095         {
00096                 return name;
00097         }
00098         
00099         int SIG_Link::getNumber (void) const
00100         {
00101                 return number;
00102         }
00103 
00104         bool SIG_Link::isRootLink (void) const
00105         {
00106                 return parent->getRootLink () == this;
00107         }
00108         
00109         void SIG_Link::setBody (SIG_Body *b)
00110         {
00111                 body = b;
00112         }
00113         
00114         SIG_Body const *SIG_Link::getBody (void) const
00115         {
00116                 return body;
00117         }
00118         
00119         void SIG_Link::setMaterial (SIG_Material *mtrl)
00120         {
00121                 material = mtrl;
00122         }
00123         
00124         SIG_Material const *SIG_Link::getMaterial (void) const
00125         {
00126                 return material;
00127         }
00128 
00129         void SIG_Link::addPoint (QString pointname, DL_vector point)
00130         {
00131                 points.insert (pointname, new DL_vector (&point));
00132         }
00133         
00134         DL_vector SIG_Link::getPoint (QString id) const
00135         {
00136                 DL_vector tmp;
00137                 DL_vector *t;
00138                 t = points.find (id);
00139                 if (t)
00140                         tmp = *t;
00141                 return tmp;
00142         }
00143 
00144         bool SIG_Link::hasPoint (QString id) const
00145         {
00146                 return (points.find (id) != 0);
00147         }
00148 
00149         QDictIterator<DL_vector> SIG_Link::getPointIter (void) const
00150         {
00151                 return QDictIterator<DL_vector> (points);
00152         }
00153 
00154         int SIG_Link::getNrOfPoints (void) const
00155         {
00156                 return points.count ();
00157         }
00158 
00159         void SIG_Link::instantiateGeometry (void)
00160         {
00161                 if (geometry) {
00162                         delete mirtich;
00163                         delete geometry;
00164                 }
00165                 
00166                 geometry = new SIG_Geometry (body->getGeometry ());
00167                 mirtich = new SIG_Mirtich (geometry, name + "(" + body->getName () + ")");
00168         }
00169 
00170         void SIG_Link::transformToDynaMo (void)
00171         {
00172                 DL_vector v;
00173                 DL_matrix m;
00174                 mirtich->moveToOriginAndMajorAxes (v, m);
00175                 transformPoints (v, m);
00176         }
00177 
00178         void SIG_Link::transformPoints (DL_vector mov, DL_matrix rot)
00179         {
00180                 QDictIterator<DL_vector> pit (points);
00181                 DL_vector *pt;
00182                 while (pt = pit.current ()) {
00183                         pt->plusis (&mov);
00184                         DL_vector v (pt);
00185                         rot.times (&v, pt);
00186                         ++pit;
00187                 }
00188 
00189                 QListIterator<SIG_Joint> jit (adjacentJoints);
00190                 SIG_Joint *j;
00191                 while (j = jit.current ()) {
00192                         j->transformPoints (this, mov, rot);
00193                         ++jit;
00194                 }
00195         }
00196         
00197         void SIG_Link::addNoCollide (SIG_Link *link, bool negotiate = true)
00198         {
00199                 if (noCollide.containsRef (link) == 0) {
00200                         noCollide.append (link);
00201                         if (negotiate)
00202                                 link->addNoCollide (this, false);
00203                 }
00204         }
00205         
00206         QList<SIG_Link> SIG_Link::getNoCollides () const
00207         {
00208                 return noCollide;
00209         }
00210         
00211         void SIG_Link::addJoint (SIG_Joint *joint)
00212         {
00213                 adjacentJoints.append (joint);
00214         }
00215         
00216         QList<SIG_Joint> SIG_Link::getJoints () const
00217         {
00218                 return adjacentJoints;
00219         }
00220 
00221         SIG_Geometry const *SIG_Link::getGeometry (void) const
00222         {
00223                 return geometry;
00224         }
00225 
00226         void SIG_Link::getPhysics (DL_Scalar & m,
00227                                    DL_vector & com,
00228                                    DL_matrix & it)
00229         {
00230                 mirtich->computePhysics (material->getDensity (), m, com, it);
00231         }
00232 
00233         void SIG_Link::propagateInitialLocation (SIG_Link *comingfrom)
00234         {
00235                 // It is very imported, that the following assignment
00236                 // is done first within this procedure. This is due
00237                 // to recursion.
00238                 initiated = true;
00239 
00240                 SIG_Joint *j;
00241                 SIG_Link *l;
00242                 QListIterator<SIG_Joint> li (adjacentJoints);
00243                 while (j = li.current ()) {
00244                         DL_vector transla, fglobtransla;
00245                         DL_matrix rota, fglobrota;
00246 
00247                         l = j->otherSide (this);
00248                         if (l != comingfrom) {
00249                                 j->getGeomRelation (transla, rota, this);
00250 
00251                                 initialOrientation.times (&rota, &fglobrota);
00252                                 initialOrientation.times (&transla, &fglobtransla);
00253                                 fglobtransla.plusis (&initialLocation);
00254 
00255                                 l->setInitialLocation (fglobtransla, fglobrota, this);
00256                         }
00257 
00258                         ++li;
00259                 }
00260         }
00261         
00262         void SIG_Link::setInitialLocation (DL_vector p, DL_matrix o, SIG_Link *comingfrom = 0)
00263         {
00264                 if (!initiated) {
00265                         initialLocation = p;
00266                         initialOrientation = o;
00267                         
00268                         propagateInitialLocation (comingfrom);
00269                 } else {
00270                         DL_Scalar const ILnull = 0.000001;
00271                         for (int i = 0; i < 3; i++) {
00272                                 if (fabs (p.get (i) - initialLocation.get (i)) > ILnull)
00273                                         throw SIG_InitialLocationError
00274                                                 (__FILE__, __LINE__,
00275                                                  "Ambiguous values for position in link \"" +
00276                                                  name + "\".");
00277                                 for (int j = 0; j < 3; j++)
00278                                         if (fabs (o.get (i,j) - initialOrientation.get (i,j)) > ILnull)
00279                                                 throw SIG_InitialLocationError
00280                                                         (__FILE__, __LINE__,
00281                                                          "Ambiguous values for orientation in link \"" +
00282                                                          name + "\".");
00283                         }
00284                 }
00285         }
00286         
00287         void SIG_Link::getInitialLocation (DL_vector & p, DL_matrix & o) const
00288         {
00289                 p = initialLocation;
00290                 o = initialOrientation;
00291         }
00292         
00293         bool SIG_Link::isInitiated (void) const
00294         {
00295                 return initiated;
00296         }
00297 
00298         void SIG_Link::writeToFileTransfer (QTextStream & tx) const
00299         {
00300                 QDictIterator<DL_vector> piter (points);
00301                 QListIterator<SIG_Link> nciter (noCollide);
00302                 tx << "Link "
00303                    << getName () << ' '
00304                    << getNumber () << ' '
00305                    << points.count () << ' ';
00306                 while (piter.current ()) {
00307                         tx << piter.currentKey () << ' ';
00308                         SIG_Robot::vectorToStream (tx, *piter.current ());
00309                         ++piter;
00310                 }
00311                 tx << noCollide.count () << ' ';
00312                 while (nciter.current ()) {
00313                         tx << nciter.current ()->getName () << ' ';
00314                         ++nciter;
00315                 }
00316                 SIG_Robot::vectorToStream (tx, initialLocation);
00317                 SIG_Robot::matrixToStream (tx, initialOrientation);
00318                 tx << (initiated ? "y " : "n ");
00319                 tx << (mdh_visited ? "y " : "n ");
00320                 tx << body->getName () << ' '
00321                    << material->getName () << ' ';
00322                 if (geometry) {
00323                         tx << "y\n";
00324                         geometry->writeToFileTransfer (tx);
00325                 } else
00326                         tx << "n\n";
00327         }
00328 
00329         bool SIG_Link::isMDHVisited (void) const
00330         {
00331           return mdh_visited;
00332         }
00333 
00334         void SIG_Link::transformToDynaMechs ( SIG_Joint *predecessor,
00335                                               DL_vector predBase,
00336                                               DL_vector predDir,
00337                                               DL_vector predHand )
00338         {
00339 #ifdef SIG_DEBUG
00340           SIGEL_Tools::SIG_IO::cerr << "Transforming link "
00341                                     << getName()
00342                                     <<" to DynaMechs!\n";
00343 #endif
00344 
00345           mdh_visited = true;
00346 
00347           QList< SIG_Joint > successors;
00348 
00349           SIG_Joint *actAdjacentJoint = adjacentJoints.first();
00350 
00351           while (actAdjacentJoint)
00352             {
00353               if (actAdjacentJoint->continuable( this ))
00354                 successors.append( actAdjacentJoint );
00355 
00356               actAdjacentJoint = adjacentJoints.next();
00357             };
00358 
00359           SIG_Joint *realSuccessor = successors.first();
00360 
00361           bool transformZ = predecessor;
00362           bool transformX = realSuccessor;
00363 
00364           DL_vector realNewXAxis;
00365           DL_vector realNewYAxis;
00366           DL_vector realNewZAxis;
00367 
00368           DL_vector realNewOrigin;
00369 
00370           SIG_Joint *actSuccessor = successors.first();
00371 
00372           do
00373             {
00374 #ifdef SIG_DEBUG
00375               if (actSuccessor)
00376                 SIGEL_Tools::SIG_IO::cerr << "Transforming link "
00377                                           << getName()
00378                                           << " accordingly to successor "
00379                                           << actSuccessor->getName()
00380                                           << ".\n";
00381               else
00382                 SIGEL_Tools::SIG_IO::cerr << "Tansforming link "
00383                                           << getName()
00384                                           << " without successor.\n";
00385 #endif
00386 
00387               bool processingRealSuccessor = (actSuccessor == realSuccessor);
00388 
00389               DL_vector newXAxis(1, 0, 0);
00390               DL_vector newZAxis(0, 0, 1);
00391 
00392               DL_vector newOrigin(0, 0, 0);
00393 
00394               if (transformZ)
00395                 {
00396                   newZAxis = predDir;
00397                   newZAxis.minusis( &predBase );
00398                   newZAxis.normalize();
00399 
00400 #ifdef SIG_DEBUG
00401                   SIGEL_Tools::SIG_IO::cerr << "Z-Axis transformed:";
00402                   for (int i=0; i<3; i++)
00403                     SIGEL_Tools::SIG_IO::cerr << " " << newZAxis.get( i );
00404                   SIGEL_Tools::SIG_IO::cerr << "\n";
00405 #endif
00406 
00407                 };
00408 
00409               if (transformX)
00410                 {
00411                   bool successorsLeftLink = ( this == actSuccessor->getLeftLink() );
00412 
00413                   DL_vector succBase;
00414                   DL_vector succDir;
00415                   DL_vector succHand;
00416 
00417                   switch (actSuccessor->getJointType())
00418                     {
00419                     case SIG_Joint::tTranslationalJoint:
00420                       {
00421                         SIG_TranslationalJoint *translationalJoint = static_cast< SIG_TranslationalJoint * >(actSuccessor);
00422 
00423                         succBase = ( successorsLeftLink ) ? translationalJoint->getLeftBase() : translationalJoint->getRightBase();
00424                         succDir = ( successorsLeftLink ) ? translationalJoint->getLeftDir() : translationalJoint->getRightDir();
00425                         succHand = ( successorsLeftLink ) ? translationalJoint->getLeftFix() : translationalJoint->getRightFix();
00426                       };
00427                       break;
00428                     case SIG_Joint::tRotationalJoint:
00429                       {
00430                         SIG_RotationalJoint *rotationalJoint = static_cast< SIG_RotationalJoint * >(actSuccessor);
00431 
00432                         succBase = ( successorsLeftLink ) ? rotationalJoint->getLeftBase() : rotationalJoint->getRightBase();
00433                         succDir = ( successorsLeftLink ) ? rotationalJoint->getLeftDir() : rotationalJoint->getRightDir();
00434                         succHand = ( successorsLeftLink ) ? rotationalJoint->getLeftHand() : rotationalJoint->getRightHand();
00435                       };
00436                       break;
00437                     };
00438 
00439                   DL_vector otherAxis = succDir;
00440                   otherAxis.minusis( &succBase );
00441                   otherAxis.normalize();
00442 
00443                   calculateCommonNormal( predBase,
00444                                          newZAxis,
00445                                          succBase,
00446                                          otherAxis,
00447                                          newOrigin,
00448                                          newXAxis );
00449 
00450 #ifdef SIG_DEBUG
00451                   SIGEL_Tools::SIG_IO::cerr << "X-Axis transformed:";
00452                   for (int i=0; i<3; i++)
00453                     SIGEL_Tools::SIG_IO::cerr << " " << newXAxis.get( i );
00454                   SIGEL_Tools::SIG_IO::cerr << "\n";
00455 
00456                   SIGEL_Tools::SIG_IO::cerr << "Origin transformed:";
00457                   for (int i=0; i<3; i++)
00458                     SIGEL_Tools::SIG_IO::cerr << " " << newOrigin.get( i );
00459                   SIGEL_Tools::SIG_IO::cerr << "\n";
00460 #endif
00461 
00462                 }
00463               else if (transformZ)
00464                 {
00465                   int firstIndex;             
00466 
00467                   for (int i=0; i<3; i++)
00468                     if (newZAxis.get( i )!=0)
00469                       {
00470                         firstIndex = i;
00471                         break;
00472                       };
00473 
00474                   int secondIndex = (firstIndex + 1) % 3;
00475 
00476                   int thirdIndex = (secondIndex + 1) % 3;
00477 
00478                   newXAxis.set( firstIndex, - newZAxis.get( secondIndex ) );
00479                   newXAxis.set( secondIndex, newZAxis.get( firstIndex ) );
00480                   newXAxis.set( thirdIndex, 0 );
00481 
00482                   newOrigin = predBase;
00483 
00484 #ifdef SIG_DEBUG
00485                   SIGEL_Tools::SIG_IO::cerr << "X-Axis transformed:";
00486                   for (int i=0; i<3; i++)
00487                     SIGEL_Tools::SIG_IO::cerr << " " << newXAxis.get( i );
00488                   SIGEL_Tools::SIG_IO::cerr << "\n";
00489 
00490                   SIGEL_Tools::SIG_IO::cerr << "Origin transformed:";
00491                   for (int i=0; i<3; i++)
00492                     SIGEL_Tools::SIG_IO::cerr << " " << newOrigin.get( i );
00493                   SIGEL_Tools::SIG_IO::cerr << "\n";
00494 #endif
00495 
00496                 };
00497 
00498               newXAxis.normalize();
00499 
00500               double screwD = 0;
00501               double screwTheta = 0;
00502 
00503               if (processingRealSuccessor)
00504                 {
00505                   realNewXAxis = newXAxis;
00506                   realNewZAxis = newZAxis;
00507 
00508                   realNewOrigin = newOrigin;
00509                 }
00510               else
00511                 {
00512                   DL_vector originDistanceVector = newOrigin;
00513                   originDistanceVector.minusis( &realNewOrigin );
00514 
00515                   screwD = originDistanceVector.norm();
00516 
00517                   double const minimalDistanceMeasure = 0.00001;
00518 
00519                   if (screwD > minimalDistanceMeasure)
00520                     {
00521                       DL_vector translationDirectionVector = originDistanceVector;
00522 
00523                       translationDirectionVector.normalize();
00524 
00525                       if (translationDirectionVector.inprod( &newZAxis ) < 0)
00526                         screwD *= -1;
00527                     };
00528 
00529                   screwTheta = tolerantACos( realNewXAxis.inprod( &newXAxis ) );
00530 
00531                   double const pi = std::atan( 1 ) * 4;
00532 
00533                   double const maximalParallelityMeasure = 0.00001;
00534 
00535                   DL_vector xxNormalVector;
00536                   realNewXAxis.crossprod( &newXAxis, &xxNormalVector );
00537 
00538                   if (xxNormalVector.norm() > maximalParallelityMeasure)
00539                     {
00540                       xxNormalVector.normalize();
00541 
00542                       if (newZAxis.inprod( &xxNormalVector ) < 0)
00543                         screwTheta = 2 * pi - screwTheta;
00544                     };
00545 
00546 #ifdef SIG_DEBUG
00547                   SIGEL_Tools::SIG_IO::cerr << "screwD: " << screwD
00548                                             << " screwTheta: " << screwTheta
00549                                             << "\n";
00550 
00551                   DL_vector debugScrewTranslationVector = newZAxis;
00552                   debugScrewTranslationVector.timesis( screwD );
00553 
00554                   DL_vector debugTranslatedOrigin = realNewOrigin;
00555                   debugTranslatedOrigin.plusis( &debugScrewTranslationVector );
00556 
00557                   DL_matrix debugScrewRotationMatrix = rotationMatrix( newZAxis,
00558                                                                        screwTheta );
00559 
00560                   DL_vector debugHelpXAxis = realNewXAxis;
00561                   DL_vector debugRotatedXAxis;
00562 
00563                   debugScrewRotationMatrix.times( &debugHelpXAxis, &debugRotatedXAxis );
00564 
00565                   realNewZAxis.crossprod( &realNewXAxis, &realNewYAxis );
00566                   realNewYAxis.normalize();
00567 
00568                   DL_vector debugNewYAxis;
00569                   newZAxis.crossprod( &newXAxis, &debugNewYAxis );
00570                   debugNewYAxis.normalize();
00571 
00572                   DL_vector debugHelpYAxis = realNewYAxis;
00573                   DL_vector debugRotatedYAxis;
00574 
00575                   debugScrewRotationMatrix.times( &debugHelpYAxis, &debugRotatedYAxis );
00576 
00577                   SIGEL_Tools::SIG_IO::cerr << "New origin:";
00578                   for (int i=0; i<3; i++)
00579                     SIGEL_Tools::SIG_IO::cerr << " " << newOrigin.get( i );
00580                   SIGEL_Tools::SIG_IO::cerr << "\n";
00581 
00582                   SIGEL_Tools::SIG_IO::cerr << "Translated real new origin:";
00583                   for (int i=0; i<3; i++)
00584                     SIGEL_Tools::SIG_IO::cerr << " " << debugTranslatedOrigin.get( i );
00585                   SIGEL_Tools::SIG_IO::cerr << "\n";
00586 
00587                   SIGEL_Tools::SIG_IO::cerr << "New X-Axis:";
00588                   for (int i=0; i<3; i++)
00589                     SIGEL_Tools::SIG_IO::cerr << " " << newXAxis.get( i );
00590                   SIGEL_Tools::SIG_IO::cerr << "\n";
00591 
00592                   SIGEL_Tools::SIG_IO::cerr << "Rotated real new X-Axis:";
00593                   for (int i=0; i<3; i++)
00594                     SIGEL_Tools::SIG_IO::cerr << " " << debugRotatedXAxis.get( i );
00595                   SIGEL_Tools::SIG_IO::cerr << "\n";
00596 
00597                   SIGEL_Tools::SIG_IO::cerr << "New Y-Axis:";
00598                   for (int i=0; i<3; i++)
00599                     SIGEL_Tools::SIG_IO::cerr << " " << debugNewYAxis.get( i );
00600                   SIGEL_Tools::SIG_IO::cerr << "\n";
00601 
00602                   SIGEL_Tools::SIG_IO::cerr << "Rotated real new Y-Axis:";
00603                   for (int i=0; i<3; i++)
00604                     SIGEL_Tools::SIG_IO::cerr << " " << debugRotatedYAxis.get( i );
00605                   SIGEL_Tools::SIG_IO::cerr << "\n";
00606 #endif
00607 
00608                 };
00609 
00610               if (actSuccessor)
00611                 {
00612                   actSuccessor->transformToDynaMechs( this,
00613                                                       screwD,
00614                                                       screwTheta );
00615 
00616                   actSuccessor = successors.next();
00617                 };
00618             }
00619           while (actSuccessor);
00620 
00621           if (transformX || transformZ)
00622             {
00623               realNewZAxis.crossprod( &realNewXAxis, &realNewYAxis );
00624               realNewYAxis.normalize();
00625 
00626 #ifdef SIG_DEBUG
00627               SIGEL_Tools::SIG_IO::cerr << "Y-Axis transformed:";
00628               for (int i=0; i<3; i++)
00629                 SIGEL_Tools::SIG_IO::cerr << " " << realNewYAxis.get( i );
00630               SIGEL_Tools::SIG_IO::cerr << "\n";
00631 #endif
00632 
00633               NEWMAT::Matrix newCoordinates( 3, 3 );
00634               newCoordinates.Column( 1 ) = SIG_TypeConverter::toColumnVector( realNewXAxis );
00635               newCoordinates.Column( 2 ) = SIG_TypeConverter::toColumnVector( realNewYAxis );
00636               newCoordinates.Column( 3 ) = SIG_TypeConverter::toColumnVector( realNewZAxis );
00637 
00638               NEWMAT::Matrix rotation = newCoordinates.i();
00639 
00640               NEWMAT::ColumnVector translation = SIG_TypeConverter::toColumnVector( realNewOrigin ) * -1;
00641 
00642               if (this->isRootLink())
00643                 {
00644                   parent->initialLocation = SIG_TypeConverter::toDL_vector( translation * -1 );
00645                   parent->initialOrientation = SIG_TypeConverter::toDL_matrix( rotation.i() );
00646 
00647 #ifdef SIG_DEBUG
00648                   SIGEL_Tools::SIG_IO::cerr << "Initial robot location: ";
00649                   for (int i=0; i<3; i++)
00650                     SIGEL_Tools::SIG_IO::cerr << " " << parent->initialLocation.get( i );
00651                   SIGEL_Tools::SIG_IO::cerr << "\n";
00652                   SIGEL_Tools::SIG_IO::cerr << "Initial robot orientation:\n";
00653                   for (int i=0; i<3; i++)
00654                     {
00655                       for (int j=0; j<3; j++)
00656                         SIGEL_Tools::SIG_IO::cerr << parent->initialOrientation.get( i, j ) << " ";
00657                       SIGEL_Tools::SIG_IO::cerr << "\n";
00658                     };
00659 #endif
00660 
00661                 };
00662 
00663               geometry->translate( SIG_TypeConverter::toDL_vector( translation ) );
00664               geometry->rotate( SIG_TypeConverter::toDL_matrix( rotation ) );
00665 
00666               transformPoints( SIG_TypeConverter::toDL_vector( translation ),
00667                                SIG_TypeConverter::toDL_matrix( rotation ) );
00668 
00669 #ifdef SIG_DEBUG
00670               SIGEL_Tools::SIG_IO::cerr << "Rotation matrix:\n";
00671               for (int i=1; i<=3; i++)
00672                 {
00673                   for (int j=1; j<=3; j++)
00674                     SIGEL_Tools::SIG_IO::cerr << rotation( i, j ) << " ";
00675                   SIGEL_Tools::SIG_IO::cerr << "\n";
00676                 };
00677               SIGEL_Tools::SIG_IO::cerr << "Translation vector:";
00678               for (int i=1; i<=3; i++)
00679                 SIGEL_Tools::SIG_IO::cerr << " " << translation( i );
00680               SIGEL_Tools::SIG_IO::cerr << "\n";
00681               SIGEL_Tools::SIG_IO::cerr << "Transformed points:\n";
00682               QDictIterator< DL_vector > pit( points );
00683               pit.toFirst();
00684               while (pit.current())
00685                 {
00686                   for (int i=0; i<3; i++)
00687                     SIGEL_Tools::SIG_IO::cerr << " " << pit.current()->get( i );
00688                   SIGEL_Tools::SIG_IO::cerr << "\n";
00689                   ++pit;
00690                 };
00691 #endif        
00692 
00693               mirtich->computeAgain();
00694             };
00695 
00696 #ifdef SIG_DEBUG
00697                   SIGEL_Tools::SIG_IO::cerr << "Link "
00698                                             << getName()
00699                                             << " transformed.\n";
00700 #endif
00701         }
00702 
00703         void SIG_Link::calculateCommonNormal( DL_vector a,
00704                                               DL_vector u,
00705                                               DL_vector b,
00706                                               DL_vector v,
00707                                               DL_vector &c,
00708                                               DL_vector &w )
00709         {
00710           u.normalize();
00711           v.normalize();
00712 
00713 #ifdef SIG_DEBUG
00714           SIGEL_Tools::SIG_IO::cerr << "Calculating common normal:\n"
00715                                     << "a:";
00716           for (int i=0; i<3; i++)
00717             SIGEL_Tools::SIG_IO::cerr << " " << a.get( i );
00718           SIGEL_Tools::SIG_IO::cerr << "\n";
00719           SIGEL_Tools::SIG_IO::cerr << "u:";
00720           for (int i=0; i<3; i++)
00721             SIGEL_Tools::SIG_IO::cerr << " " << u.get( i );
00722           SIGEL_Tools::SIG_IO::cerr << "\n";
00723           SIGEL_Tools::SIG_IO::cerr << "b:";
00724           for (int i=0; i<3; i++)
00725             SIGEL_Tools::SIG_IO::cerr << " " << b.get( i );
00726           SIGEL_Tools::SIG_IO::cerr << "\n";
00727           SIGEL_Tools::SIG_IO::cerr << "v:";
00728           for (int i=0; i<3; i++)
00729             SIGEL_Tools::SIG_IO::cerr << " " << v.get( i );
00730           SIGEL_Tools::SIG_IO::cerr << "\n";
00731 #endif
00732 
00733           c.init(0, 0, 0);
00734           w.init(0, 0, 0);
00735 
00736           double const minimalDistanceMeasure = 0.000001;
00737 
00738           DL_vector uvNormal;
00739           u.crossprod( &v, &uvNormal );
00740           double parallelityMeasure = uvNormal.norm();
00741 
00742           double const maximalParallelityMeasure = 0.000001; // Let's try other values!
00743 
00744           bool uvParallel = ( parallelityMeasure <= maximalParallelityMeasure );
00745 
00746           if (uvParallel)
00747             {
00748 #ifdef SIG_DEBUG
00749               SIGEL_Tools::SIG_IO::cerr << "u and v are parallel.\n";
00750 #endif
00751 
00752               c = a;
00753 
00754               double t0 = ( u.inprod( &c ) - u.inprod( &b ) ) / u.inprod( &v );
00755               DL_vector p = b;
00756               DL_vector dir = v;
00757               dir.timesis( t0 );
00758               p.plusis( &dir );
00759 
00760               w = p;
00761               w.minusis( &c );
00762 
00763               if (w.norm() < minimalDistanceMeasure)
00764                 {
00765                   QString message = "Axes of adjacent joints meeting in link " + getName() + " are overlapping!";
00766                   throw SIG_WrongKinematicsException( __FILE__,
00767                                                       __LINE__,
00768                                                       message );
00769                 };
00770 
00771               w.normalize();
00772             }
00773           else
00774             {
00775 #ifdef SIG_DEBUG
00776               SIGEL_Tools::SIG_IO::cerr << "u and v are not parallel.\n";
00777 #endif
00778 
00779               u.crossprod( &v, &w );
00780               w.normalize();
00781 
00782               NEWMAT::Matrix linEqSystem( 3, 3 );
00783               linEqSystem.Column( 1 ) = SIG_TypeConverter::toColumnVector( u );
00784               linEqSystem.Column( 2 ) = SIG_TypeConverter::toColumnVector( w );
00785               linEqSystem.Column( 3 ) = SIG_TypeConverter::toColumnVector( v ) * -1;
00786 
00787               NEWMAT::ColumnVector rightSide = SIG_TypeConverter::toColumnVector( b ) - SIG_TypeConverter::toColumnVector( a );
00788 
00789               NEWMAT::ColumnVector solutions = linEqSystem.i() * rightSide;
00790 
00791 #ifdef SIG_DEBUG
00792               SIGEL_Tools::SIG_IO::cerr << "Linear equation system:\n";
00793               for (int i=1; i<=3; i++)
00794                 {
00795                   for (int j=1; j<=3; j++)
00796                     SIGEL_Tools::SIG_IO::cerr << linEqSystem( i, j ) << " ";
00797                   SIGEL_Tools::SIG_IO::cerr << "\n";
00798                 };
00799 
00800               SIGEL_Tools::SIG_IO::cerr << "Right side";
00801               for (int i=1; i<=3; i++)
00802                 SIGEL_Tools::SIG_IO::cerr << " " << rightSide( i );
00803               SIGEL_Tools::SIG_IO::cerr << "\n";
00804 
00805               SIGEL_Tools::SIG_IO::cerr << "Solution vector (t0, t1, t2)^T:";
00806               for (int i=1; i<=3; i++)
00807                 SIGEL_Tools::SIG_IO::cerr << " " << solutions( i );
00808               SIGEL_Tools::SIG_IO::cerr << "\n";
00809 #endif
00810 
00811               c = u;
00812               c.timesis( solutions( 1 ) );
00813               c.plusis( &a );
00814 
00815               DL_vector cut = w;
00816               cut.timesis( solutions( 2 ) );
00817               cut.plusis( &c );
00818 
00819               DL_vector hDir = cut;
00820               hDir.minusis( &c );
00821 
00822               if (hDir.norm() < minimalDistanceMeasure)
00823                 {
00824                   QString message = "Axes of adjacent joints meeting in link " + getName() + " cut!";
00825                   throw SIG_WrongKinematicsException( __FILE__,
00826                                                       __LINE__,
00827                                                       message );
00828                 };
00829 
00830               hDir.normalize();
00831 
00832               if (w.inprod( &hDir ) == -1)
00833                 w.timesis( -1 );
00834 
00835               w.normalize();
00836             };
00837 
00838 #ifdef SIG_DEBUG
00839               SIGEL_Tools::SIG_IO::cerr << "c:";
00840               for (int i=0; i<3; i++)
00841                 SIGEL_Tools::SIG_IO::cerr << " " << c.get( i );
00842               SIGEL_Tools::SIG_IO::cerr << "\n";
00843 
00844               SIGEL_Tools::SIG_IO::cerr << "w:";
00845               for (int i=0; i<3; i++)
00846                 SIGEL_Tools::SIG_IO::cerr << " " << w.get( i );
00847               SIGEL_Tools::SIG_IO::cerr << "\n";
00848 #endif
00849         }
00850 }

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