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
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
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
00236
00237
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;
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 }