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
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
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 }