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
00035 >> name
00036 >> number;
00037 tx >> n1 >> n2;
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
00061
00062
00063
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
00094
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
00269
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 }