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

IFunctions.cpp

00001 #include "SIGEL_Robot/IFunctions.h"
00002 #include "SIGEL_Tools/SIG_IO.h"
00003 #include "SIGEL_Robot/SIG_RobotExceptions.h"
00004 #include "SIGEL_Robot/SIG_Joint.h"
00005 #include "SIGEL_Tools/SIG_TypeConverter.h"
00006 
00007 #include <dm.h>
00008 #include <cmath>
00009 #include <newmat.h>
00010 
00011 using namespace SIGEL_Tools;
00012 
00013 namespace SIGEL_Robot {
00014 
00015         double tolerantACos( double cosInput )
00016         {
00017           if (cosInput > 1)
00018             cosInput= 1;
00019           else if (cosInput < -1)
00020             cosInput = -1;
00021 
00022           return std::acos( cosInput );
00023         };
00024 
00025         DL_vector orthogonalVector( DL_vector input )
00026         {
00027           int firstIndex;
00028 
00029           for (int i=0; i<3; i++)
00030             if (input.get( i )!=0)
00031               {
00032                 firstIndex = i;
00033                 break;
00034               };
00035 
00036           int secondIndex = (firstIndex + 1) % 3;
00037 
00038           int thirdIndex = (secondIndex + 1) % 3;
00039 
00040           DL_vector result;
00041           result.set( firstIndex, - input.get( secondIndex ) );
00042           result.set( secondIndex, input.get( firstIndex ) );
00043           result.set( thirdIndex, 0 );
00044 
00045           result.normalize();
00046 
00047           return result;
00048         };
00049 
00050         DL_matrix rotationMatrix(DL_vector v, DL_Scalar phi)
00051         {
00052           double sinPhi = std::sin( phi / 2 );
00053           double cosPhi = std::cos( phi / 2 );
00054 
00055           Quaternion q;
00056           q[0] = v.x * sinPhi;
00057           q[1] = v.y * sinPhi;
00058           q[2] = v.z * sinPhi;
00059           q[3] = cosPhi;
00060 
00061           normalizeQuat( q );
00062 
00063           RotationMatrix result;
00064 
00065           buildRotMat( q, result );
00066 
00067           return SIG_TypeConverter::toDL_matrix( result );
00068         }
00069 
00070         NEWMAT::Matrix phatRockingUpStylinVectorBendingAngleSwingingMasterFunction( DL_vector A,
00071                                                                                     DL_vector B,
00072                                                                                     DL_vector C,
00073                                                                                     DL_vector otherA,
00074                                                                                     DL_vector otherB,
00075                                                                                     DL_vector otherC )
00076         {
00077           DL_vector firstTranslationVector = A;
00078           firstTranslationVector.minusis( &otherA );
00079 
00080           otherB.plusis( &firstTranslationVector );
00081           otherC.plusis( &firstTranslationVector );
00082 
00083           B.minusis( &A );
00084           C.minusis( &A );
00085           otherB.minusis( &A );
00086           otherC.minusis( &A );
00087 
00088           B.normalize();
00089           C.normalize();
00090           otherB.normalize();
00091           otherC.normalize();
00092 
00093 #ifdef SIG_DEBUG
00094           SIGEL_Tools::SIG_IO::cerr << "B:";
00095           for (int i=0; i<3; i++)
00096             SIGEL_Tools::SIG_IO::cerr << " " << B.get( i );
00097           SIGEL_Tools::SIG_IO::cerr << "\n";
00098 
00099           SIGEL_Tools::SIG_IO::cerr << "C:";
00100           for (int i=0; i<3; i++)
00101             SIGEL_Tools::SIG_IO::cerr << " " << C.get( i );
00102           SIGEL_Tools::SIG_IO::cerr << "\n";
00103 
00104           SIGEL_Tools::SIG_IO::cerr << "otherB:";
00105           for (int i=0; i<3; i++)
00106             SIGEL_Tools::SIG_IO::cerr << " " << otherB.get( i );
00107           SIGEL_Tools::SIG_IO::cerr << "\n";
00108 
00109           SIGEL_Tools::SIG_IO::cerr << "otherC:";
00110           for (int i=0; i<3; i++)
00111             SIGEL_Tools::SIG_IO::cerr << " " << otherC.get( i );
00112           SIGEL_Tools::SIG_IO::cerr << "\n";
00113 #endif
00114 
00115           DL_matrix firstRotation;
00116 
00117           double const minimalAngleMeasure = 0.00001;
00118 
00119           double const pi = std::atan( 1 ) * 4;
00120 
00121           double bsAngle = tolerantACos( B.inprod( &otherB ) );
00122 
00123           if (bsAngle > minimalAngleMeasure)
00124             {
00125               if (std::abs( pi - bsAngle ) > minimalAngleMeasure)
00126                 {
00127                   DL_vector bsNormal;
00128 
00129                   otherB.crossprod( &B, &bsNormal );
00130                   bsNormal.normalize();
00131 
00132                   firstRotation = rotationMatrix( bsNormal, bsAngle );
00133                 }
00134               else
00135                 {
00136                   DL_vector rotationAxis = orthogonalVector( B );
00137 
00138                   firstRotation = rotationMatrix( rotationAxis, bsAngle );
00139                 };
00140             }
00141           else
00142             firstRotation.makeone();
00143 
00144           DL_vector bufferVector = otherB;
00145           firstRotation.times( &bufferVector, &otherB );
00146           bufferVector = otherC;
00147           firstRotation.times( &bufferVector, &otherC );
00148 
00149 #ifdef SIG_DEBUG
00150           SIGEL_Tools::SIG_IO::cerr << "otherB and otherC have been rotated.\n";
00151           SIGEL_Tools::SIG_IO::cerr << "B:";
00152           for (int i=0; i<3; i++)
00153             SIGEL_Tools::SIG_IO::cerr << " " << B.get( i );
00154           SIGEL_Tools::SIG_IO::cerr << "\n";
00155 
00156           SIGEL_Tools::SIG_IO::cerr << "C:";
00157           for (int i=0; i<3; i++)
00158             SIGEL_Tools::SIG_IO::cerr << " " << C.get( i );
00159           SIGEL_Tools::SIG_IO::cerr << "\n";
00160 
00161           SIGEL_Tools::SIG_IO::cerr << "otherB:";
00162           for (int i=0; i<3; i++)
00163             SIGEL_Tools::SIG_IO::cerr << " " << otherB.get( i );
00164           SIGEL_Tools::SIG_IO::cerr << "\n";
00165 
00166           SIGEL_Tools::SIG_IO::cerr << "otherC:";
00167           for (int i=0; i<3; i++)
00168             SIGEL_Tools::SIG_IO::cerr << " " << otherC.get( i );
00169           SIGEL_Tools::SIG_IO::cerr << "\n";
00170 #endif
00171 
00172           DL_matrix secondRotation;
00173 
00174           DL_vector u = orthogonalVector( B );
00175 
00176           DL_vector v;
00177           B.crossprod( &u, &v );
00178           v.normalize();
00179 
00180           DL_vector cProjected;
00181 
00182           SIG_Joint::calculateCut( DL_vector(0, 0, 0),
00183                                    u,
00184                                    v,
00185                                    C,
00186                                    B,
00187                                    cProjected );
00188           cProjected.normalize();
00189 
00190           DL_vector otherCProjected;
00191 
00192           SIG_Joint::calculateCut( DL_vector(0, 0, 0),
00193                                    u,
00194                                    v,
00195                                    otherC,
00196                                    B,
00197                                    otherCProjected );
00198           otherCProjected.normalize();
00199 
00200           double csAngle = tolerantACos( otherCProjected.inprod( &cProjected ) );
00201 
00202           if (csAngle > minimalAngleMeasure)
00203             {
00204               if (std::abs( pi - csAngle ) > minimalAngleMeasure)
00205                 {
00206                   DL_vector csNormal;
00207 
00208                   otherCProjected.crossprod( &cProjected, &csNormal );
00209                   csNormal.normalize();
00210 
00211                   if (B.inprod( &csNormal ) < 0)
00212                     csAngle = 2 * pi - csAngle;
00213                 };
00214 
00215               secondRotation = rotationMatrix( B, csAngle );
00216             }
00217           else
00218             secondRotation.makeone();
00219 
00220 #ifdef SIG_DEBUG
00221           bufferVector = otherB;
00222           secondRotation.times( &bufferVector, &otherB );
00223           bufferVector = otherC;
00224           secondRotation.times( &bufferVector, &otherC );
00225 
00226           SIGEL_Tools::SIG_IO::cerr << "otherB and otherC have been rotated.\n";
00227           SIGEL_Tools::SIG_IO::cerr << "B:";
00228           for (int i=0; i<3; i++)
00229             SIGEL_Tools::SIG_IO::cerr << " " << B.get( i );
00230           SIGEL_Tools::SIG_IO::cerr << "\n";
00231 
00232           SIGEL_Tools::SIG_IO::cerr << "C:";
00233           for (int i=0; i<3; i++)
00234             SIGEL_Tools::SIG_IO::cerr << " " << C.get( i );
00235           SIGEL_Tools::SIG_IO::cerr << "\n";
00236 
00237           SIGEL_Tools::SIG_IO::cerr << "otherB:";
00238           for (int i=0; i<3; i++)
00239             SIGEL_Tools::SIG_IO::cerr << " " << otherB.get( i );
00240           SIGEL_Tools::SIG_IO::cerr << "\n";
00241 
00242           SIGEL_Tools::SIG_IO::cerr << "otherC:";
00243           for (int i=0; i<3; i++)
00244             SIGEL_Tools::SIG_IO::cerr << " " << otherC.get( i );
00245           SIGEL_Tools::SIG_IO::cerr << "\n";
00246 #endif
00247 
00248           NEWMAT::Matrix transformation( 4, 4 );
00249 
00250           transformation << 1 << 0 << 0 << 0
00251                          << 0 << 1 << 0 << 0
00252                          << 0 << 0 << 1 << 0
00253                          << 0 << 0 << 0 << 1;
00254 
00255           NEWMAT::Matrix firstTransformation = transformation;
00256           NEWMAT::Matrix secondTransformation = transformation;
00257           NEWMAT::Matrix thirdTransformation = transformation;
00258           NEWMAT::Matrix fourthTransformation = transformation;
00259 
00260           firstTransformation.SubMatrix( 1, 3, 4, 4 ) = - SIG_TypeConverter::toColumnVector( otherA );
00261 
00262           secondTransformation.SubMatrix( 1, 3, 1, 3 ) = SIG_TypeConverter::toMatrix( firstRotation );
00263 
00264           thirdTransformation.SubMatrix( 1, 3, 1, 3 ) = SIG_TypeConverter::toMatrix( secondRotation );
00265 
00266           fourthTransformation.SubMatrix( 1, 3, 4, 4 ) = SIG_TypeConverter::toColumnVector( A );
00267 
00268           transformation =   fourthTransformation
00269                            * thirdTransformation
00270                            * secondTransformation
00271                            * firstTransformation;
00272 
00273           return transformation;
00274         };
00275 
00276         void calculateAnyJoint (DL_vector A, DL_vector B, DL_vector C,
00277                                 DL_vector D, DL_vector E, DL_vector F,
00278                                 double winkel, double verschiebung,
00279                                 DL_matrix & o, DL_vector & t,
00280                                 QString someIdentifier)
00281         {
00282                 // Punkte abdeckbar machen.
00283                 DL_vector r, zw;
00284                 DL_Scalar h1;
00285                 
00286                 r.assign (&B);
00287                 r.minusis (&A);
00288                 r.normalize ();
00289                 B.assign (&A);
00290                 B.plusis (&r);
00291 
00292                 zw.assign (&C);
00293                 zw.minusis (&A);
00294                 h1 = zw.inprod (&r);
00295                 r.timesis (h1);
00296                 zw.minusis (&r);
00297                 zw.normalize ();
00298                 C.assign (&A);
00299                 C.plusis (&zw);
00300                 
00301                 r.assign (&E);
00302                 r.minusis (&D);
00303                 r.normalize ();
00304                 E.assign (&D);
00305                 E.plusis (&r);
00306 
00307                 zw.assign (&F);
00308                 zw.minusis (&D);
00309                 h1 = zw.inprod (&r);
00310                 r.timesis (h1);
00311                 zw.minusis (&r);
00312                 zw.normalize ();
00313                 F.assign (&D);
00314                 F.plusis (&zw);
00315                 // Ende der Abdeckbarmachung
00316 
00317 #ifdef SIG_DEBUG
00318                 SIGEL_Tools::SIG_IO::cerr << "Phat processing of "
00319                                           << someIdentifier
00320                                           << "\n";
00321 #endif
00322 
00323 
00324                 NEWMAT::Matrix transformation = phatRockingUpStylinVectorBendingAngleSwingingMasterFunction( A,
00325                                                                                                              B,
00326                                                                                                              C,
00327                                                                                                              D,
00328                                                                                                              E,
00329                                                                                                              F );
00330 
00331                 o = SIG_TypeConverter::toDL_matrix( transformation.SubMatrix( 1, 3, 1, 3 ) );
00332                 t = SIG_TypeConverter::toDL_vector( transformation.SubMatrix( 1, 3, 4, 4 ) );
00333 
00334                 DL_vector v (&B);
00335                 v.minusis (&A);
00336 
00337                 DL_vector schiebung (&v);
00338                 schiebung.timesis (verschiebung);
00339                 t.plusis (&schiebung);
00340 
00341                 DL_Scalar phi = (winkel / 180.0) * M_PI;
00342 
00343                 DL_matrix drehmatrix;
00344 
00345                 drehmatrix=rotationMatrix(v,-phi);
00346                 t.minusis (&A);
00347                 DL_vector stflorianhilf;
00348                 drehmatrix.times (&t, &stflorianhilf);
00349                 t.assign (&stflorianhilf);
00350                 t.plusis (&A);
00351 
00352                 DL_matrix hilf;
00353                 drehmatrix.times (&o, &hilf);
00354                 o.assign (&hilf);
00355         }
00356 } // namespace

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